diff options
203 files changed, 11152 insertions, 2623 deletions
diff --git a/.gitignore b/.gitignore index 8ea2698e7..1c3f8fe04 100644 --- a/.gitignore +++ b/.gitignore @@ -17,6 +17,7 @@ .~lock.* Archives/* Build/* +build/* core cscope.out Firmware.sublime-workspace @@ -38,6 +39,9 @@ tags .pydevproject .ropeproject *.orig +src/modules/uORB/topics/* +src/platforms/nuttx/px4_messages/* +src/platforms/ros/px4_messages/* Firmware.zip unittests/build *.generated.h 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..f76dbbf41 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,265 @@ +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 +) + +## 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 +) + +## 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 +) + +## 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) @@ -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: nuttxpatches $(NUTTX_ARCHIVES) # We cannot build these parallel; note that we also force -j1 for the # sub-make invocations. @@ -183,6 +183,29 @@ $(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) $(Q) $(COPY) $(NUTTX_SRC)nuttx-export.zip $@ $(Q) (cd $(NUTTX_SRC)/configs && $(RMDIR) $(board)) +NUTTX_PATCHES := $(wildcard $(PX4_NUTTX_PATCH_DIR)*.patch) +NUTTX_PATCHED = $(NUTTX_SRC).patchedpx4common + +.PHONY: nuttxpatches +nuttxpatches: + $(Q) -if [ ! -f $(NUTTX_PATCHED) ]; then \ + for patch in $(NUTTX_PATCHES); \ + do \ + $(PATCH) -p0 -N < $$patch >/dev/null; \ + done \ + fi + $(Q) $(TOUCH) $(NUTTX_PATCHED) + +.PHONY: cleannuttxpatches +cleannuttxpatches: + $(Q) -if [ -f $(NUTTX_PATCHED) ]; then \ + for patch in $(NUTTX_PATCHES); \ + do \ + $(PATCH) -p0 -N -R -r - < $$patch >/dev/null; \ + done \ + fi + $(Q) $(REMOVE) $(NUTTX_PATCHED) + # # The user can run the NuttX 'menuconfig' tool for a single board configuration with # make BOARDS=<boardname> menuconfig @@ -224,6 +247,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) $(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) $(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 # @@ -235,12 +281,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 @@ -250,7 +296,7 @@ clean: $(Q) $(REMOVE) $(IMAGE_DIR)*.px4 .PHONY: distclean -distclean: clean +distclean: cleannuttxpatches clean @echo > /dev/null $(Q) $(REMOVE) $(ARCHIVE_DIR)*.export $(Q) $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) distclean diff --git a/NuttX b/NuttX -Subproject e4c914e261d2647e44d05222afa7aa3cc90d3c6 +Subproject 69283d36b665ba41085021eb36066bfa074c688 diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index 7fde482df..b5be9bb21 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -13,21 +13,21 @@ then if [ $MIXER_AUX == none ] then - set MIXER_AUX $MIXER.aux + set MIXER_AUX ${MIXER}.aux fi # Use the mixer file from the SD-card if it exists - if [ -f $SDCARD_MIXERS_PATH/$MIXER.main.mix ] + if [ -f ${SDCARD_MIXERS_PATH}/${MIXER}.main.mix ] then - set MIXER_FILE $SDCARD_MIXERS_PATH/$MIXER.main.mix + set MIXER_FILE ${SDCARD_MIXERS_PATH}/${MIXER}.main.mix # Try out the old convention, for backward compatibility else - if [ -f $SDCARD_MIXERS_PATH/$MIXER.mix ] + if [ -f ${SDCARD_MIXERS_PATH}/${MIXER}.mix ] then - set MIXER_FILE $SDCARD_MIXERS_PATH/$MIXER.mix + set MIXER_FILE ${SDCARD_MIXERS_PATH}/${MIXER}.mix else - set MIXER_FILE /etc/mixers/$MIXER.main.mix + set MIXER_FILE /etc/mixers/${MIXER}.main.mix fi fi @@ -104,14 +104,14 @@ then set MIXER_AUX_FILE none - if [ -f $SDCARD_MIXERS_PATH/$MIXER_AUX.mix ] + if [ -f ${SDCARD_MIXERS_PATH}/${MIXER_AUX}.mix ] then - set MIXER_AUX_FILE $SDCARD_MIXERS_PATH/$MIXER_AUX.mix + set MIXER_AUX_FILE ${SDCARD_MIXERS_PATH}/${MIXER_AUX}.mix else - if [ -f /etc/mixers/$MIXER_AUX.mix ] + if [ -f /etc/mixers/${MIXER_AUX}.mix ] then - set MIXER_AUX_FILE /etc/mixers/$MIXER_AUX.mix + set MIXER_AUX_FILE /etc/mixers/${MIXER_AUX}.mix fi fi diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 033b3b640..7b29fb3a7 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -8,7 +8,12 @@ attitude_estimator_ekf start #ekf_att_pos_estimator start position_estimator_inav start -mc_att_control start +if mc_att_control start +then +else + # try the multiplatform version + mc_att_control_m start +fi mc_pos_control start # diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 4d337171a..d14d6e48d 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -198,7 +198,7 @@ then if px4io forceupdate 14662 $IO_FILE then - usleep 500000 + usleep 10000 if px4io checkcrc $IO_FILE then echo "PX4IO CRC OK after updating" >> $LOG_FILE 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..4bcab4d54 --- /dev/null +++ b/Tools/px_generate_uorb_topic_headers.py @@ -0,0 +1,159 @@ +#!/usr/bin/env python +############################################################################# +# +# Copyright (C) 2013-2014 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################# + +""" +px_generate_uorb_topic_headers.py +Generates c/cpp header files for uorb topics from .msg (ROS syntax) +message files +""" +from __future__ import print_function +import os +import shutil +import filecmp +import argparse +import genmsg.template_tools + +__author__ = "Thomas Gubler" +__copyright__ = "Copyright (C) 2013-2014 PX4 Development Team." +__license__ = "BSD" +__email__ = "thomasgubler@gmail.com" + + +msg_template_map = {'msg.h.template': '@NAME@.h'} +srv_template_map = {} +incl_default = ['std_msgs:./msg/std_msgs'] +package = 'px4' + + +def convert_file(filename, outputdir, templatedir, includepath): + """ + Converts a single .msg file to a uorb header + """ + print("Generating 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/px4_ros_installation_ubuntu.sh b/Tools/ros/px4_ros_installation_ubuntu.sh new file mode 100755 index 000000000..72b4f9468 --- /dev/null +++ b/Tools/ros/px4_ros_installation_ubuntu.sh @@ -0,0 +1,40 @@ +#!/bin/sh + +# 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 + +## 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..4055f7320 --- /dev/null +++ b/Tools/ros/px4_workspace_create.sh @@ -0,0 +1,8 @@ +#!/bin/sh +# this script creates a catkin_ws in the current folder + +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..1675e54f3 --- /dev/null +++ b/Tools/ros/px4_workspace_setup.sh @@ -0,0 +1,31 @@ +#!/bin/bash +# 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/launch/ardrone.launch b/launch/ardrone.launch new file mode 100644 index 000000000..495388be5 --- /dev/null +++ b/launch/ardrone.launch @@ -0,0 +1,19 @@ +<launch> + +<include file="$(find px4)/launch/multicopter_x.launch" /> + +<group ns="px4_multicopter"> + <param name="MC_ROLL_P" type="double" value="6.0" /> + <param name="MC_PITCH_P" type="double" value="6.0" /> + <param name="MC_ROLLRATE_P" type="double" value="0.05" /> + <param name="MC_ROLLRATE_D" type="double" value="0.0" /> + <param name="MC_PITCHRATE_P" type="double" value="0.05" /> + <param name="MC_PITCHRATE_D" type="double" value="0.0" /> + <param name="MC_YAW_FF" type="double" value="0" /> + <param name="MC_YAW_P" type="double" value="5.0" /> + <param name="MC_YAWRATE_P" type="double" value="0.5" /> + <param name="MC_MAN_R_MAX" type="double" value="10.0" /> + <param name="MC_MAN_P_MAX" type="double" value="10.0" /> +</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_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..44a0ae034 --- /dev/null +++ b/launch/iris.launch @@ -0,0 +1,20 @@ +<launch> + +<include file="$(find px4)/launch/multicopter_w.launch" /> + +<group ns="px4_multicopter"> + <param name="MC_ROLL_P" type="double" value="6.0" /> + <param name="MC_PITCH_P" type="double" value="6.0" /> + <param name="MC_ROLLRATE_P" type="double" value="0.05" /> + <param name="MC_ROLLRATE_D" type="double" value="0.0" /> + <param name="MC_PITCHRATE_P" type="double" value="0.05" /> + <param name="MC_PITCHRATE_D" type="double" value="0.0" /> + <param name="MC_YAW_FF" type="double" value="0" /> + <param name="MC_YAW_P" type="double" value="3.0" /> + <param name="MC_YAWRATE_P" type="double" value="0.5" /> + <param name="MC_MAN_R_MAX" type="double" value="10.0" /> + <param name="MC_MAN_P_MAX" type="double" value="10.0" /> + <param name="mixer" type="string" value="i" /> +</group> + +</launch> diff --git a/launch/multicopter.launch b/launch/multicopter.launch new file mode 100644 index 000000000..96ff3ad99 --- /dev/null +++ b/launch/multicopter.launch @@ -0,0 +1,12 @@ +<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="mc_att_control" type="mc_att_control"/> +</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..e3bbe3a03 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -111,6 +111,7 @@ MODULES += lib/geo MODULES += lib/geo_lookup MODULES += lib/conversion MODULES += lib/launchdetection +MODULES += platforms/nuttx # # Demo apps @@ -153,4 +154,5 @@ endef # command priority stack entrypoint BUILTIN_COMMANDS := \ $(call _B, sercon, , 2048, sercon_main ) \ - $(call _B, serdis, , 2048, serdis_main ) + $(call _B, serdis, , 2048, serdis_main ) \ + $(call _B, cu, , 2048, cu_main ) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 3abebd82f..a48b3115e 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 @@ -163,4 +163,5 @@ endef # command priority stack entrypoint BUILTIN_COMMANDS := \ $(call _B, sercon, , 2048, sercon_main ) \ - $(call _B, serdis, , 2048, serdis_main ) + $(call _B, serdis, , 2048, serdis_main ) \ + $(call _B, cu, , 2048, cu_main ) diff --git a/makefiles/config_px4fmu-v2_multiplatform.mk b/makefiles/config_px4fmu-v2_multiplatform.mk new file mode 100644 index 000000000..12411cde1 --- /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/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 ) \ + $(call _B, cu, , 2048, cu_main ) diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index bc6723e5f..313fb130a 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -32,11 +32,34 @@ MODULES += systemcmds/tests MODULES += systemcmds/nshterm MODULES += systemcmds/mtd MODULES += systemcmds/ver +MODULES += systemcmds/param +MODULES += systemcmds/top # -# Testing modules +# System commands +# +MODULES += systemcmds/bl_update +MODULES += systemcmds/boardinfo +MODULES += systemcmds/mixer +MODULES += systemcmds/param +MODULES += systemcmds/perf +MODULES += systemcmds/preflight_check +MODULES += systemcmds/pwm +MODULES += systemcmds/esc_calib +MODULES += systemcmds/reboot +MODULES += systemcmds/top +MODULES += systemcmds/config +MODULES += systemcmds/nshterm +MODULES += systemcmds/mtd +MODULES += systemcmds/dumpfile +MODULES += systemcmds/ver + +# +# Example modules # MODULES += examples/matlab_csv_serial +MODULES += examples/subscriber +MODULES += examples/publisher # # Library modules @@ -44,10 +67,11 @@ MODULES += examples/matlab_csv_serial MODULES += modules/systemlib MODULES += modules/systemlib/mixer MODULES += modules/uORB -LIBRARIES += lib/mathlib/CMSIS MODULES += lib/mathlib MODULES += lib/mathlib/math/filter MODULES += lib/conversion +LIBRARIES += lib/mathlib/CMSIS +MODULES += platforms/nuttx # # Example modules to test-build @@ -69,7 +93,6 @@ MODULES += drivers/pca8574 # # Tests # - MODULES += modules/unit_test MODULES += modules/mavlink/mavlink_tests MODULES += modules/commander/commander_tests @@ -89,4 +112,5 @@ endef # command priority stack entrypoint BUILTIN_COMMANDS := \ $(call _B, sercon, , 2048, sercon_main ) \ - $(call _B, serdis, , 2048, serdis_main ) + $(call _B, serdis, , 2048, serdis_main ) \ + $(call _B, cu, , 2048, cu_main ) diff --git a/makefiles/setup.mk b/makefiles/setup.mk index c932a6758..5a4c68b96 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)/ @@ -54,6 +55,7 @@ export ROMFS_SRC = $(abspath $(PX4_BASE)/ROMFS)/ export IMAGE_DIR = $(abspath $(PX4_BASE)/Images)/ export BUILD_DIR = $(abspath $(PX4_BASE)/Build)/ export ARCHIVE_DIR = $(abspath $(PX4_BASE)/Archives)/ +export PX4_NUTTX_PATCH_DIR = $(abspath $(PX4_BASE)/nuttx-patches)/ # # Default include paths @@ -61,25 +63,27 @@ 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 # -export MKFW = $(PX4_BASE)/Tools/px_mkfw.py +export MKFW = $(PX4_BASE)/Tools/px_mkfw.py export UPLOADER = $(PX4_BASE)/Tools/px_uploader.py -export COPY = cp +export COPY = cp export COPYDIR = cp -Rf export REMOVE = rm -f export RMDIR = rm -rf export GENROMFS = genromfs export TOUCH = touch export MKDIR = mkdir -export FIND = find -export ECHO = echo +export FIND = find +export ECHO = echo export UNZIP_CMD = unzip export PYTHON = python export OPENOCD = openocd +export PATCH = patch export GREP = grep # diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index d4d73fb84..56138ff34 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -80,12 +80,23 @@ ARCHCPUFLAGS_CORTEXM3 = -mcpu=cortex-m3 \ -march=armv7-m \ -mfloat-abi=soft +# +# Tool to test a Nuttx Config value from config.h +# + +NUTTX_CONFIG_H=$(WORK_DIR)nuttx-export/include/nuttx/config.h +define check_nuttx_config +$(strip $(shell $(GREP) -q $1 $2;echo -n $$?;)) +endef +nuttx_config_true:="0" +nuttx_config_2true:="0 0" + +# # Enabling stack checks if OS was build with them # -TEST_FILE_STACKCHECK=$(WORK_DIR)nuttx-export/include/nuttx/config.h -TEST_VALUE_STACKCHECK=CONFIG_ARMV7M_STACKCHECK\ 1 -ENABLE_STACK_CHECKS=$(shell $(GREP) -q "$(TEST_VALUE_STACKCHECK)" $(TEST_FILE_STACKCHECK); echo $$?;) -ifeq ("$(ENABLE_STACK_CHECKS)","0") + +ENABLE_STACK_CHECKS=$(call check_nuttx_config ,"CONFIG_ARMV7M_STACKCHECK 1", $(NUTTX_CONFIG_H)) +ifeq ("$(ENABLE_STACK_CHECKS)",$(nuttx_config_true)) ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -finstrument-functions -ffixed-r10 ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -finstrument-functions -ffixed-r10 ARCHINSTRUMENTATIONDEFINES_CORTEXM3 = @@ -128,7 +139,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 # @@ -325,3 +336,4 @@ define BIN_TO_OBJ --rename-section .data=.rodata $(Q) $(REMOVE) $2.c $2.c.o endef + 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/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/msg.h.template b/msg/templates/msg.h.template new file mode 100644 index 000000000..cc128c1ea --- /dev/null +++ b/msg/templates/msg.h.template @@ -0,0 +1,159 @@ +@############################################### +@# +@# PX4 ROS compatible message source code +@# generation for C++ +@# +@# EmPy template for generating <msg>.h files +@# Based on the original template for ROS +@# +@############################################### +@# Start of Template +@# +@# Context: +@# - file_name_in (String) Source file +@# - spec (msggen.MsgSpec) Parsed specification of the .msg file +@# - md5sum (String) MD5Sum of the .msg specification +@############################################### +/**************************************************************************** + * + * Copyright (C) 2013-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + /* Auto-generated by genmsg_cpp from file @file_name_in */ + +@{ +import genmsg.msgs +import gencpp + +cpp_namespace = '::%s::'%(spec.package) # TODO handle nested namespace +cpp_class = '%s_'%spec.short_name +cpp_full_name = '%s%s'%(cpp_namespace,cpp_class) +cpp_full_name_with_alloc = '%s<ContainerAllocator>'%(cpp_full_name) +cpp_msg_definition = gencpp.escape_message_definition(msg_definition) +}@ + +#pragma once + +@############################## +@# Generic Includes +@############################## +#include <stdint.h> +#include "../uORB.h" + +@############################## +@# Includes for dependencies +@############################## +@{ +for field in spec.parsed_fields(): + if (not field.is_builtin): + if (not field.is_header): + (package, name) = genmsg.names.package_resource_name(field.base_type) + package = package or spec.package # convert '' to package + print('#include <uORB/topics/%s.h>'%(name)) + +}@ +@# Constants +@[for constant in spec.constants]@ +#define @(constant.name) @(int(constant.val)) +@[end for] + +/** + * @@addtogroup topics + * @@{ + */ + +@############################## +@# Main struct of message +@############################## +@{ + +type_map = {'int8': 'int8_t', + 'int16': 'int16_t', + 'int32': 'int32_t', + 'int64': 'int64_t', + 'uint8': 'uint8_t', + 'uint16': 'uint16_t', + 'uint32': 'uint32_t', + 'uint64': 'uint64_t', + 'float32': 'float', + 'bool': 'bool', + 'fence_vertex': 'fence_vertex'} + +# 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 @(spec.short_name)_s { +struct __EXPORT @(spec.short_name)_s { +@#public: +#else +struct @(spec.short_name)_s { +#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) +}@ +}; + +/** + * @@} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(@(spec.short_name)); 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..622641617 --- /dev/null +++ b/msg/templates/uorb/msg.h.template @@ -0,0 +1,156 @@ +@############################################### +@# +@# 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 +@[for constant in spec.constants]@ +#define @(constant.name) @(int(constant.val)) +@[end for] + +/** + * @@addtogroup topics + * @@{ + */ + +@############################## +@# Main struct of message +@############################## +@{ + +type_map = {'int8': 'int8_t', + 'int16': 'int16_t', + 'int32': 'int32_t', + 'int64': 'int64_t', + 'uint8': 'uint8_t', + 'uint16': 'uint16_t', + 'uint32': 'uint32_t', + 'uint64': 'uint64_t', + 'float32': 'float', + 'bool': 'bool', + 'fence_vertex': 'fence_vertex'} + +# 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) +}@ +}; + +/** + * @@} + */ + +/* 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_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/nuttx-configs/aerocore/nsh/Make.defs b/nuttx-configs/aerocore/nsh/Make.defs index 3808fc1cf..10cd651f5 100644 --- a/nuttx-configs/aerocore/nsh/Make.defs +++ b/nuttx-configs/aerocore/nsh/Make.defs @@ -35,6 +35,7 @@ include ${TOPDIR}/.config include ${TOPDIR}/tools/Config.mk +include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs # # We only support building with the ARM bare-metal toolchain from diff --git a/nuttx-configs/aerocore/nsh/defconfig b/nuttx-configs/aerocore/nsh/defconfig index c44b074f3..dcb132e01 100644 --- a/nuttx-configs/aerocore/nsh/defconfig +++ b/nuttx-configs/aerocore/nsh/defconfig @@ -2,7 +2,6 @@ # Automatically generated file; DO NOT EDIT. # Nuttx/ Configuration # -CONFIG_NUTTX_NEWCONFIG=y # # Build Setup @@ -93,7 +92,7 @@ CONFIG_ARCH_HAVE_MPU=y # CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set # CONFIG_ARMV7M_TOOLCHAIN_CODEREDL is not set # CONFIG_ARMV7M_TOOLCHAIN_CODESOURCERYL is not set -CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y +CONFIG_ARMV7M_TOOLCHAIN_GNU_EABIL=y CONFIG_ARMV7M_STACKCHECK=n CONFIG_SERIAL_TERMIOS=y @@ -293,7 +292,7 @@ CONFIG_STM32_USART_SINGLEWIRE=y # CONFIG_STM32_I2C_DYNTIMEO is not set CONFIG_STM32_I2CTIMEOSEC=0 CONFIG_STM32_I2CTIMEOMS=10 -CONFIG_STM32_I2CTIMEOTICKS=500 +CONFIG_STM32_I2CTIMEOTICKS=10 # CONFIG_STM32_I2C_DUTY16_9 is not set # @@ -319,6 +318,7 @@ CONFIG_ARCH_DMA=y # CONFIG_ADDRENV is not set CONFIG_ARCH_HAVE_VFORK=y CONFIG_ARCH_STACKDUMP=y +CONFIG_STACK_COLORATION=y # CONFIG_ENDIAN_BIG is not set # CONFIG_ARCH_HAVE_RAMFUNCS is not set CONFIG_ARCH_HAVE_RAMVECTORS=y @@ -332,7 +332,7 @@ CONFIG_BOARD_LOOPSPERMSEC=16717 CONFIG_DRAM_START=0x20000000 CONFIG_DRAM_SIZE=262144 CONFIG_ARCH_HAVE_INTERRUPTSTACK=y -CONFIG_ARCH_INTERRUPTSTACK=4096 +CONFIG_ARCH_INTERRUPTSTACK=2048 # # Boot options @@ -346,8 +346,9 @@ CONFIG_BOOT_RUNFROMFLASH=y # # Board Selection # -CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD="" +CONFIG_ARCH_BOARD_AEROCORE=y +# CONFIG_ARCH_BOARD_CUSTOM is not set +CONFIG_ARCH_BOARD="aerocore" # # Common Board Options @@ -414,8 +415,8 @@ CONFIG_PREALLOC_TIMERS=50 # # Stack and heap information # -CONFIG_IDLETHREAD_STACKSIZE=6000 -CONFIG_USERMAIN_STACKSIZE=4096 +CONFIG_IDLETHREAD_STACKSIZE=3500 +CONFIG_USERMAIN_STACKSIZE=2600 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_PTHREAD_STACK_DEFAULT=2048 @@ -466,6 +467,8 @@ CONFIG_MTD_BYTE_WRITE=y # CONFIG_MTD_M25P is not set # CONFIG_MTD_SMART is not set CONFIG_MTD_RAMTRON=y +CONFIG_RAMTRON_WRITEWAIT=y +CONFIG_RAMTRON_SETSPEED=y CONFIG_RAMTRON_FUJITSU=y # CONFIG_MTD_SST25 is not set # CONFIG_MTD_SST39FV is not set @@ -862,7 +865,9 @@ CONFIG_NSH_CODECS_BUFSIZE=128 CONFIG_NSH_FILEIOSIZE=512 CONFIG_NSH_STRERROR=y CONFIG_NSH_LINELEN=128 +CONFIG_NSH_CMDPARMS=y CONFIG_NSH_MAXARGUMENTS=12 +CONFIG_NSH_ARGCAT=y CONFIG_NSH_NESTDEPTH=8 # CONFIG_NSH_DISABLESCRIPT is not set # CONFIG_NSH_DISABLEBG is not set diff --git a/nuttx-configs/px4fmu-v1/nsh/Make.defs b/nuttx-configs/px4fmu-v1/nsh/Make.defs index 4e08d28a2..0d3fbe546 100644 --- a/nuttx-configs/px4fmu-v1/nsh/Make.defs +++ b/nuttx-configs/px4fmu-v1/nsh/Make.defs @@ -35,6 +35,7 @@ include ${TOPDIR}/.config include ${TOPDIR}/tools/Config.mk +include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs # # We only support building with the ARM bare-metal toolchain from diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index a467fa605..eae796a9c 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -2,14 +2,14 @@ # Automatically generated file; DO NOT EDIT. # Nuttx/ Configuration # -CONFIG_NUTTX_NEWCONFIG=y # # Build Setup # # CONFIG_EXPERIMENTAL is not set -# CONFIG_HOST_LINUX is not set -CONFIG_HOST_OSX=y +# CONFIG_DEFAULT_SMALL is not set +CONFIG_HOST_LINUX=y +# CONFIG_HOST_OSX is not set # CONFIG_HOST_WINDOWS is not set # CONFIG_HOST_OTHER is not set @@ -17,6 +17,7 @@ CONFIG_HOST_OSX=y # Build Configuration # CONFIG_APPS_DIR="../apps" +CONFIG_BUILD_FLAT=y # CONFIG_BUILD_2PASS is not set # @@ -26,10 +27,12 @@ CONFIG_APPS_DIR="../apps" # CONFIG_INTELHEX_BINARY is not set # CONFIG_MOTOROLA_SREC is not set CONFIG_RAW_BINARY=y +# CONFIG_UBOOT_UIMAGE is not set # # Customize Header Files # +# CONFIG_ARCH_STDINT_H is not set # CONFIG_ARCH_STDBOOL_H is not set CONFIG_ARCH_MATH_H=y # CONFIG_ARCH_FLOAT_H is not set @@ -39,12 +42,17 @@ CONFIG_ARCH_MATH_H=y # Debug Options # # CONFIG_DEBUG is not set +CONFIG_ARCH_HAVE_STACKCHECK=y +CONFIG_ARCH_HAVE_HEAPCHECK=y CONFIG_DEBUG_SYMBOLS=y +CONFIG_ARCH_HAVE_CUSTOMOPT=y +CONFIG_DEBUG_NOOPT=y +# CONFIG_DEBUG_CUSTOMOPT is not set +# CONFIG_DEBUG_FULLOPT is not set # # System Type # -# CONFIG_ARCH_8051 is not set CONFIG_ARCH_ARM=y # CONFIG_ARCH_AVR is not set # CONFIG_ARCH_HC is not set @@ -60,23 +68,35 @@ CONFIG_ARCH="arm" # # ARM Options # +# CONFIG_ARCH_CHIP_A1X is not set # CONFIG_ARCH_CHIP_C5471 is not set # CONFIG_ARCH_CHIP_CALYPSO is not set # CONFIG_ARCH_CHIP_DM320 is not set +# CONFIG_ARCH_CHIP_EFM32 is not set # CONFIG_ARCH_CHIP_IMX is not set # CONFIG_ARCH_CHIP_KINETIS is not set # CONFIG_ARCH_CHIP_KL is not set # CONFIG_ARCH_CHIP_LM is not set +# CONFIG_ARCH_CHIP_TIVA is not set # CONFIG_ARCH_CHIP_LPC17XX is not set # CONFIG_ARCH_CHIP_LPC214X is not set # CONFIG_ARCH_CHIP_LPC2378 is not set # CONFIG_ARCH_CHIP_LPC31XX is not set # CONFIG_ARCH_CHIP_LPC43XX is not set # CONFIG_ARCH_CHIP_NUC1XX is not set +# CONFIG_ARCH_CHIP_SAMA5 is not set +# CONFIG_ARCH_CHIP_SAMD is not set # CONFIG_ARCH_CHIP_SAM34 is not set CONFIG_ARCH_CHIP_STM32=y # CONFIG_ARCH_CHIP_STR71X is not set +# CONFIG_ARCH_ARM7TDMI is not set +# CONFIG_ARCH_ARM926EJS is not set +# CONFIG_ARCH_ARM920T is not set +# CONFIG_ARCH_CORTEXM0 is not set +# CONFIG_ARCH_CORTEXM3 is not set CONFIG_ARCH_CORTEXM4=y +# CONFIG_ARCH_CORTEXA5 is not set +# CONFIG_ARCH_CORTEXA8 is not set CONFIG_ARCH_FAMILY="armv7-m" CONFIG_ARCH_CHIP="stm32" CONFIG_ARMV7M_USEBASEPRI=y @@ -84,16 +104,17 @@ CONFIG_ARCH_HAVE_CMNVECTOR=y CONFIG_ARMV7M_CMNVECTOR=y CONFIG_ARCH_HAVE_FPU=y CONFIG_ARCH_FPU=y -CONFIG_ARCH_HAVE_MPU=y # CONFIG_ARMV7M_MPU is not set # # ARMV7M Configuration Options # # CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set -CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y +# CONFIG_ARMV7M_TOOLCHAIN_CODEREDL is not set +# CONFIG_ARMV7M_TOOLCHAIN_CODESOURCERYL is not set +CONFIG_ARMV7M_TOOLCHAIN_GNU_EABIL=y CONFIG_ARMV7M_STACKCHECK=y -CONFIG_SERIAL_TERMIOS=y +# CONFIG_ARMV7M_ITMSYSLOG is not set # # STM32 Configuration Options @@ -116,6 +137,7 @@ CONFIG_SERIAL_TERMIOS=y # CONFIG_ARCH_CHIP_STM32L152V6 is not set # CONFIG_ARCH_CHIP_STM32L152V8 is not set # CONFIG_ARCH_CHIP_STM32L152VB is not set +# CONFIG_ARCH_CHIP_STM32L162ZD is not set # CONFIG_ARCH_CHIP_STM32F100C8 is not set # CONFIG_ARCH_CHIP_STM32F100CB is not set # CONFIG_ARCH_CHIP_STM32F100R8 is not set @@ -128,15 +150,27 @@ CONFIG_SERIAL_TERMIOS=y # CONFIG_ARCH_CHIP_STM32F100VC is not set # CONFIG_ARCH_CHIP_STM32F100VD is not set # CONFIG_ARCH_CHIP_STM32F100VE is not set +# CONFIG_ARCH_CHIP_STM32F102CB is not set +# CONFIG_ARCH_CHIP_STM32F103T8 is not set +# CONFIG_ARCH_CHIP_STM32F103TB is not set # CONFIG_ARCH_CHIP_STM32F103C4 is not set # CONFIG_ARCH_CHIP_STM32F103C8 is not set -# CONFIG_ARCH_CHIP_STM32F103RET6 is not set -# CONFIG_ARCH_CHIP_STM32F103VCT6 is not set -# CONFIG_ARCH_CHIP_STM32F103VET6 is not set -# CONFIG_ARCH_CHIP_STM32F103ZET6 is not set -# CONFIG_ARCH_CHIP_STM32F105VBT7 is not set +# CONFIG_ARCH_CHIP_STM32F103CB is not set +# CONFIG_ARCH_CHIP_STM32F103R8 is not set +# CONFIG_ARCH_CHIP_STM32F103RB is not set +# CONFIG_ARCH_CHIP_STM32F103RC is not set +# CONFIG_ARCH_CHIP_STM32F103RD is not set +# CONFIG_ARCH_CHIP_STM32F103RE is not set +# CONFIG_ARCH_CHIP_STM32F103RG is not set +# CONFIG_ARCH_CHIP_STM32F103V8 is not set +# CONFIG_ARCH_CHIP_STM32F103VB is not set +# CONFIG_ARCH_CHIP_STM32F103VC is not set +# CONFIG_ARCH_CHIP_STM32F103VE is not set +# CONFIG_ARCH_CHIP_STM32F103ZE is not set +# CONFIG_ARCH_CHIP_STM32F105VB is not set # CONFIG_ARCH_CHIP_STM32F107VC is not set # CONFIG_ARCH_CHIP_STM32F207IG is not set +# CONFIG_ARCH_CHIP_STM32F207ZE is not set # CONFIG_ARCH_CHIP_STM32F302CB is not set # CONFIG_ARCH_CHIP_STM32F302CC is not set # CONFIG_ARCH_CHIP_STM32F302RB is not set @@ -149,6 +183,8 @@ CONFIG_SERIAL_TERMIOS=y # CONFIG_ARCH_CHIP_STM32F303RC is not set # CONFIG_ARCH_CHIP_STM32F303VB is not set # CONFIG_ARCH_CHIP_STM32F303VC is not set +# CONFIG_ARCH_CHIP_STM32F401RE is not set +# CONFIG_ARCH_CHIP_STM32F411RE is not set CONFIG_ARCH_CHIP_STM32F405RG=y # CONFIG_ARCH_CHIP_STM32F405VG is not set # CONFIG_ARCH_CHIP_STM32F405ZG is not set @@ -161,23 +197,71 @@ CONFIG_ARCH_CHIP_STM32F405RG=y # CONFIG_ARCH_CHIP_STM32F427V is not set # CONFIG_ARCH_CHIP_STM32F427Z is not set # CONFIG_ARCH_CHIP_STM32F427I is not set +# CONFIG_ARCH_CHIP_STM32F429V is not set +# CONFIG_ARCH_CHIP_STM32F429Z is not set +# CONFIG_ARCH_CHIP_STM32F429I is not set +# CONFIG_ARCH_CHIP_STM32F429B is not set +# CONFIG_ARCH_CHIP_STM32F429N is not set # CONFIG_STM32_STM32L15XX is not set # CONFIG_STM32_ENERGYLITE is not set # CONFIG_STM32_STM32F10XX is not set # CONFIG_STM32_VALUELINE is not set # CONFIG_STM32_CONNECTIVITYLINE is not set # CONFIG_STM32_PERFORMANCELINE is not set +# CONFIG_STM32_USBACCESSLINE is not set # CONFIG_STM32_HIGHDENSITY is not set # CONFIG_STM32_MEDIUMDENSITY is not set # CONFIG_STM32_LOWDENSITY is not set # CONFIG_STM32_STM32F20XX is not set +# CONFIG_STM32_STM32F207 is not set # CONFIG_STM32_STM32F30XX is not set CONFIG_STM32_STM32F40XX=y +# CONFIG_STM32_STM32F401 is not set +# CONFIG_STM32_STM32F411 is not set +CONFIG_STM32_STM32F405=y +# CONFIG_STM32_STM32F407 is not set +# CONFIG_STM32_STM32F427 is not set +# CONFIG_STM32_STM32F429 is not set # CONFIG_STM32_DFU is not set # # STM32 Peripheral Support # +CONFIG_STM32_HAVE_CCM=y +# CONFIG_STM32_HAVE_USBDEV is not set +CONFIG_STM32_HAVE_OTGFS=y +CONFIG_STM32_HAVE_FSMC=y +CONFIG_STM32_HAVE_USART3=y +CONFIG_STM32_HAVE_UART4=y +CONFIG_STM32_HAVE_UART5=y +CONFIG_STM32_HAVE_USART6=y +# CONFIG_STM32_HAVE_UART7 is not set +# CONFIG_STM32_HAVE_UART8 is not set +CONFIG_STM32_HAVE_TIM1=y +CONFIG_STM32_HAVE_TIM5=y +CONFIG_STM32_HAVE_TIM6=y +CONFIG_STM32_HAVE_TIM7=y +CONFIG_STM32_HAVE_TIM8=y +CONFIG_STM32_HAVE_TIM9=y +CONFIG_STM32_HAVE_TIM10=y +CONFIG_STM32_HAVE_TIM11=y +CONFIG_STM32_HAVE_TIM12=y +CONFIG_STM32_HAVE_TIM13=y +CONFIG_STM32_HAVE_TIM14=y +# CONFIG_STM32_HAVE_TIM15 is not set +# CONFIG_STM32_HAVE_TIM16 is not set +# CONFIG_STM32_HAVE_TIM17 is not set +CONFIG_STM32_HAVE_ADC2=y +CONFIG_STM32_HAVE_ADC3=y +# CONFIG_STM32_HAVE_ADC4 is not set +CONFIG_STM32_HAVE_CAN1=y +CONFIG_STM32_HAVE_CAN2=y +CONFIG_STM32_HAVE_RNG=y +# CONFIG_STM32_HAVE_ETHMAC is not set +CONFIG_STM32_HAVE_SPI2=y +CONFIG_STM32_HAVE_SPI3=y +# CONFIG_STM32_HAVE_SPI4 is not set +# CONFIG_STM32_HAVE_SPI5 is not set CONFIG_STM32_ADC1=y # CONFIG_STM32_ADC2 is not set # CONFIG_STM32_ADC3 is not set @@ -192,7 +276,6 @@ CONFIG_STM32_DMA2=y # CONFIG_STM32_DAC1 is not set # CONFIG_STM32_DAC2 is not set # CONFIG_STM32_DCMI is not set -# CONFIG_STM32_ETHMAC is not set # CONFIG_STM32_FSMC is not set # CONFIG_STM32_HASH is not set CONFIG_STM32_I2C1=y @@ -265,15 +348,12 @@ CONFIG_STM32_USART=y # CONFIG_USART1_RXDMA is not set # CONFIG_USART2_RS485 is not set CONFIG_USART2_RXDMA=y -# CONFIG_USART3_RXDMA is not set -# CONFIG_UART4_RXDMA is not set # CONFIG_UART5_RS485 is not set CONFIG_UART5_RXDMA=y # CONFIG_USART6_RS485 is not set CONFIG_USART6_RXDMA=y -# CONFIG_USART7_RXDMA is not set -# CONFIG_USART8_RXDMA is not set CONFIG_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32_FLOWCONTROL_BROKEN=y CONFIG_STM32_USART_SINGLEWIRE=y # @@ -285,21 +365,27 @@ CONFIG_STM32_USART_SINGLEWIRE=y # # I2C Configuration # +# CONFIG_STM32_I2C_ALT is not set # CONFIG_STM32_I2C_DYNTIMEO is not set CONFIG_STM32_I2CTIMEOSEC=0 CONFIG_STM32_I2CTIMEOMS=10 +CONFIG_STM32_I2CTIMEOTICKS=10 # CONFIG_STM32_I2C_DUTY16_9 is not set # -# USB Host Configuration +# USB FS Host Configuration # # -# USB Device Configuration +# USB HS Host Configuration +# + +# +# USB Host Debug Configuration # # -# External Memory Configuration +# USB Device Configuration # # @@ -308,12 +394,22 @@ CONFIG_STM32_I2CTIMEOMS=10 # CONFIG_ARCH_NOINTC is not set # CONFIG_ARCH_VECNOTIRQ is not set CONFIG_ARCH_DMA=y -# CONFIG_ARCH_IRQPRIO is not set -# CONFIG_CUSTOM_STACK is not set -# CONFIG_ADDRENV is not set +CONFIG_ARCH_HAVE_IRQPRIO=y +# CONFIG_ARCH_L2CACHE is not set +# CONFIG_ARCH_HAVE_COHERENT_DCACHE is not set +# CONFIG_ARCH_HAVE_ADDRENV is not set +# CONFIG_ARCH_NEED_ADDRENV_MAPPING is not set CONFIG_ARCH_HAVE_VFORK=y +# CONFIG_ARCH_HAVE_MMU is not set +CONFIG_ARCH_HAVE_MPU=y +# CONFIG_ARCH_NAND_HWECC is not set +# CONFIG_ARCH_HAVE_EXTCLK is not set +# CONFIG_ARCH_USE_MPU is not set +# CONFIG_ARCH_IRQPRIO is not set CONFIG_ARCH_STACKDUMP=y +CONFIG_STACK_COLORATION=y # CONFIG_ENDIAN_BIG is not set +# CONFIG_ARCH_IDLE_CUSTOM is not set # CONFIG_ARCH_HAVE_RAMFUNCS is not set CONFIG_ARCH_HAVE_RAMVECTORS=y # CONFIG_ARCH_RAMVECTORS is not set @@ -323,10 +419,14 @@ CONFIG_ARCH_HAVE_RAMVECTORS=y # CONFIG_BOARD_LOOPSPERMSEC=16717 # CONFIG_ARCH_CALIBRATION is not set -CONFIG_DRAM_START=0x20000000 -CONFIG_DRAM_SIZE=196608 + +# +# Interrupt options +# CONFIG_ARCH_HAVE_INTERRUPTSTACK=y -CONFIG_ARCH_INTERRUPTSTACK=4096 +CONFIG_ARCH_INTERRUPTSTACK=2048 +CONFIG_ARCH_HAVE_HIPRI_INTERRUPT=y +# CONFIG_ARCH_HIPRI_INTERRUPT is not set # # Boot options @@ -338,10 +438,25 @@ CONFIG_BOOT_RUNFROMFLASH=y # CONFIG_BOOT_COPYTORAM is not set # +# Boot Memory Configuration +# +CONFIG_RAM_START=0x20000000 +CONFIG_RAM_SIZE=196608 +# CONFIG_ARCH_HAVE_SDRAM is not set + +# # Board Selection # -CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD="" +CONFIG_ARCH_BOARD_PX4FMU_V1=y +# CONFIG_ARCH_BOARD_CUSTOM is not set +CONFIG_ARCH_BOARD="px4fmu-v1" + +# +# Custom Board Configuration +# +CONFIG_ARCH_BOARD_CUSTOM_DIR="" +# CONFIG_BOARD_CUSTOM_LEDS is not set +# CONFIG_BOARD_CUSTOM_BUTTONS is not set # # Common Board Options @@ -357,37 +472,75 @@ CONFIG_NSH_MMCSDSPIPORTNO=3 # # RTOS Features # -# CONFIG_BOARD_INITIALIZE is not set -CONFIG_MSEC_PER_TICK=1 -CONFIG_RR_INTERVAL=0 -CONFIG_SCHED_INSTRUMENTATION=y -CONFIG_TASK_NAME_SIZE=24 -# CONFIG_SCHED_HAVE_PARENT is not set +CONFIG_DISABLE_OS_API=y +# CONFIG_DISABLE_POSIX_TIMERS is not set +# CONFIG_DISABLE_PTHREAD is not set +# CONFIG_DISABLE_SIGNALS is not set +# CONFIG_DISABLE_MQUEUE is not set +# CONFIG_DISABLE_ENVIRON is not set + +# +# Clocks and Timers +# +CONFIG_USEC_PER_TICK=1000 +# CONFIG_SYSTEM_TIME64 is not set +# CONFIG_CLOCK_MONOTONIC is not set # CONFIG_JULIAN_TIME is not set CONFIG_START_YEAR=1970 CONFIG_START_MONTH=1 CONFIG_START_DAY=1 -CONFIG_DEV_CONSOLE=y +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=50 +CONFIG_WDOG_INTRESERVE=4 +CONFIG_PREALLOC_TIMERS=50 + +# +# Tasks and Scheduling +# +# CONFIG_INIT_NONE is not set +CONFIG_INIT_ENTRYPOINT=y +# CONFIG_INIT_FILEPATH is not set +CONFIG_USER_ENTRYPOINT="nsh_main" +CONFIG_RR_INTERVAL=0 +CONFIG_TASK_NAME_SIZE=24 +CONFIG_MAX_TASKS=32 +# CONFIG_SCHED_HAVE_PARENT is not set +CONFIG_SCHED_WAITPID=y + +# +# Pthread Options +# # CONFIG_MUTEX_TYPES is not set -CONFIG_PRIORITY_INHERITANCE=y -CONFIG_SEM_PREALLOCHOLDERS=0 -CONFIG_SEM_NNESTPRIO=8 +CONFIG_NPTHREAD_KEYS=4 + +# +# Performance Monitoring +# +# CONFIG_SCHED_CPULOAD is not set +CONFIG_SCHED_INSTRUMENTATION=y + +# +# Files and I/O +# +CONFIG_DEV_CONSOLE=y # CONFIG_FDCLONE_DISABLE is not set CONFIG_FDCLONE_STDIO=y CONFIG_SDCLONE_DISABLE=y -CONFIG_SCHED_WAITPID=y +CONFIG_NFILE_DESCRIPTORS=42 +CONFIG_NFILE_STREAMS=8 +CONFIG_NAME_MAX=32 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_NNESTPRIO=8 + +# +# RTOS hooks +# +# CONFIG_BOARD_INITIALIZE is not set # CONFIG_SCHED_STARTHOOK is not set CONFIG_SCHED_ATEXIT=y CONFIG_SCHED_ATEXIT_MAX=1 # CONFIG_SCHED_ONEXIT is not set -CONFIG_USER_ENTRYPOINT="nsh_main" -CONFIG_DISABLE_OS_API=y -# CONFIG_DISABLE_CLOCK is not set -# CONFIG_DISABLE_POSIX_TIMERS is not set -# CONFIG_DISABLE_PTHREAD is not set -# CONFIG_DISABLE_SIGNALS is not set -# CONFIG_DISABLE_MQUEUE is not set -# CONFIG_DISABLE_ENVIRON is not set # # Signal Numbers @@ -399,19 +552,25 @@ CONFIG_SIG_SIGCONDTIMEDOUT=16 CONFIG_SIG_SIGWORK=4 # -# Sizes of configurable things (0 disables) +# POSIX Message Queue Options # -CONFIG_MAX_TASKS=32 -CONFIG_MAX_TASK_ARGS=10 -CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=42 -CONFIG_NFILE_STREAMS=8 -CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 CONFIG_MQ_MAXMSGSIZE=32 -CONFIG_MAX_WDOGPARMS=2 -CONFIG_PREALLOC_WDOGS=50 -CONFIG_PREALLOC_TIMERS=50 + +# +# Work Queue Support +# +CONFIG_SCHED_WORKQUEUE=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=192 +CONFIG_SCHED_HPWORKPERIOD=5000 +CONFIG_SCHED_HPWORKSTACKSIZE=1800 +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPNTHREADS=1 +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKPRIOMAX=176 +CONFIG_SCHED_LPWORKPERIOD=50000 +CONFIG_SCHED_LPWORKSTACKSIZE=1800 # # Stack and heap information @@ -420,6 +579,7 @@ CONFIG_IDLETHREAD_STACKSIZE=3500 CONFIG_USERMAIN_STACKSIZE=2600 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_PTHREAD_STACK_DEFAULT=2048 +# CONFIG_LIB_SYSCALL is not set # # Device Drivers @@ -428,25 +588,37 @@ CONFIG_PTHREAD_STACK_DEFAULT=2048 CONFIG_DEV_NULL=y # CONFIG_DEV_ZERO is not set # CONFIG_LOOP is not set + +# +# Buffering +# +# CONFIG_DRVR_WRITEBUFFER is not set +# CONFIG_DRVR_READAHEAD is not set # CONFIG_RAMDISK is not set # CONFIG_CAN is not set +# CONFIG_ARCH_HAVE_PWM_PULSECOUNT is not set # CONFIG_PWM is not set +CONFIG_ARCH_HAVE_I2CRESET=y CONFIG_I2C=y # CONFIG_I2C_SLAVE is not set CONFIG_I2C_TRANSFER=y # CONFIG_I2C_WRITEREAD is not set # CONFIG_I2C_POLLED is not set # CONFIG_I2C_TRACE is not set -CONFIG_ARCH_HAVE_I2CRESET=y CONFIG_I2C_RESET=y CONFIG_SPI=y # CONFIG_SPI_OWNBUS is not set CONFIG_SPI_EXCHANGE=y # CONFIG_SPI_CMDDATA is not set +# CONFIG_SPI_BITBANG is not set +# CONFIG_I2S is not set # CONFIG_RTC is not set CONFIG_WATCHDOG=y +CONFIG_WATCHDOG_DEVPATH="/dev/watchdog0" +# CONFIG_TIMER is not set # CONFIG_ANALOG is not set # CONFIG_AUDIO_DEVICES is not set +# CONFIG_VIDEO_DEVICES is not set # CONFIG_BCH is not set # CONFIG_INPUT is not set # CONFIG_LCD is not set @@ -458,49 +630,91 @@ CONFIG_MMCSD_NSLOTS=1 # CONFIG_MMCSD_HAVECARDDETECT is not set CONFIG_MMCSD_SPI=y CONFIG_MMCSD_SPICLOCK=24000000 -# CONFIG_MMCSD_SDIO is not set +CONFIG_MMCSD_SPIMODE=0 +# CONFIG_ARCH_HAVE_SDIO is not set CONFIG_MTD=y + +# +# MTD Configuration +# +CONFIG_MTD_PARTITION=y +# CONFIG_MTD_SECT512 is not set +CONFIG_MTD_BYTE_WRITE=y +# CONFIG_MTD_CONFIG is not set + +# +# MTD Device Drivers +# +# CONFIG_MTD_NAND is not set +# CONFIG_RAMMTD is not set +# CONFIG_MTD_AT24XX is not set +# CONFIG_MTD_AT25 is not set +# CONFIG_MTD_AT45DB is not set +# CONFIG_MTD_M25P is not set +# CONFIG_MTD_SMART is not set +# CONFIG_MTD_RAMTRON is not set +# CONFIG_MTD_SST25 is not set +# CONFIG_MTD_SST25XX is not set +# CONFIG_MTD_SST39FV is not set +# CONFIG_MTD_W25 is not set +CONFIG_EEPROM=y +# CONFIG_SPI_EE_25XX is not set CONFIG_PIPES=y # CONFIG_PM is not set # CONFIG_POWER is not set # CONFIG_SENSORS is not set +# CONFIG_SERCOMM_CONSOLE is not set CONFIG_SERIAL=y # CONFIG_DEV_LOWCONSOLE is not set CONFIG_SERIAL_REMOVABLE=y # CONFIG_16550_UART is not set +# CONFIG_ARCH_HAVE_UART is not set +# CONFIG_ARCH_HAVE_UART0 is not set +# CONFIG_ARCH_HAVE_UART1 is not set +# CONFIG_ARCH_HAVE_UART2 is not set +# CONFIG_ARCH_HAVE_UART3 is not set +# CONFIG_ARCH_HAVE_UART4 is not set CONFIG_ARCH_HAVE_UART5=y +# CONFIG_ARCH_HAVE_UART6 is not set +# CONFIG_ARCH_HAVE_UART7 is not set +# CONFIG_ARCH_HAVE_UART8 is not set +# CONFIG_ARCH_HAVE_SCI0 is not set +# CONFIG_ARCH_HAVE_SCI1 is not set +# CONFIG_ARCH_HAVE_USART0 is not set CONFIG_ARCH_HAVE_USART1=y CONFIG_ARCH_HAVE_USART2=y +# CONFIG_ARCH_HAVE_USART3 is not set +# CONFIG_ARCH_HAVE_USART4 is not set +# CONFIG_ARCH_HAVE_USART5 is not set CONFIG_ARCH_HAVE_USART6=y +# CONFIG_ARCH_HAVE_USART7 is not set +# CONFIG_ARCH_HAVE_USART8 is not set +# CONFIG_ARCH_HAVE_OTHER_UART is not set + +# +# USART Configuration +# +CONFIG_USART1_ISUART=y +CONFIG_USART2_ISUART=y +CONFIG_USART6_ISUART=y CONFIG_MCU_SERIAL=y CONFIG_STANDARD_SERIAL=y CONFIG_SERIAL_NPOLLWAITERS=2 +CONFIG_SERIAL_IFLOWCONTROL=y +CONFIG_SERIAL_OFLOWCONTROL=y +CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y +CONFIG_SERIAL_IFLOWCONTROL_LOWER_WATERMARK=10 +CONFIG_SERIAL_IFLOWCONTROL_UPPER_WATERMARK=90 +CONFIG_ARCH_HAVE_SERIAL_TERMIOS=y +CONFIG_SERIAL_TERMIOS=y CONFIG_USART1_SERIAL_CONSOLE=y # CONFIG_USART2_SERIAL_CONSOLE is not set # CONFIG_UART5_SERIAL_CONSOLE is not set # CONFIG_USART6_SERIAL_CONSOLE is not set +# CONFIG_OTHER_SERIAL_CONSOLE is not set # CONFIG_NO_SERIAL_CONSOLE is not set # -# MTD Configuration -# -CONFIG_MTD_PARTITION=y -CONFIG_MTD_BYTE_WRITE=y - -# -# MTD Device Drivers -# -# CONFIG_RAMMTD is not set -# CONFIG_MTD_AT24XX is not set -# CONFIG_MTD_AT45DB is not set -# CONFIG_MTD_M25P is not set -# CONFIG_MTD_SMART is not set -# CONFIG_MTD_RAMTRON is not set -# CONFIG_MTD_SST25 is not set -# CONFIG_MTD_SST39FV is not set -# CONFIG_MTD_W25 is not set - -# # USART1 Configuration # CONFIG_USART1_RXBUFSIZE=300 @@ -547,8 +761,6 @@ CONFIG_USART6_PARITY=0 CONFIG_USART6_2STOP=0 # CONFIG_USART6_IFLOWCONTROL is not set # CONFIG_USART6_OFLOWCONTROL is not set -CONFIG_SERIAL_IFLOWCONTROL=y -CONFIG_SERIAL_OFLOWCONTROL=y CONFIG_USBDEV=y # @@ -559,8 +771,8 @@ CONFIG_USBDEV=y # CONFIG_USBDEV_SELFPOWERED is not set CONFIG_USBDEV_BUSPOWERED=y CONFIG_USBDEV_MAXPOWER=500 -# CONFIG_USBDEV_REMOTEWAKEUP is not set # CONFIG_USBDEV_DMA is not set +# CONFIG_ARCH_USBDEV_STALLQUEUE is not set # CONFIG_USBDEV_TRACE is not set # @@ -569,7 +781,7 @@ CONFIG_USBDEV_MAXPOWER=500 # CONFIG_USBDEV_COMPOSITE is not set # CONFIG_PL2303 is not set CONFIG_CDCACM=y -CONFIG_CDCACM_CONSOLE=n +# CONFIG_CDCACM_CONSOLE is not set CONFIG_CDCACM_EP0MAXPACKET=64 CONFIG_CDCACM_EPINTIN=1 CONFIG_CDCACM_EPINTIN_FSSIZE=64 @@ -605,9 +817,16 @@ CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v1.x" # # Networking Support # +# CONFIG_ARCH_HAVE_NET is not set +# CONFIG_ARCH_HAVE_PHY is not set # CONFIG_NET is not set # +# Crypto API +# +# CONFIG_CRYPTO is not set + +# # File Systems # @@ -615,6 +834,13 @@ CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v1.x" # File system configuration # # CONFIG_DISABLE_MOUNTPOINT is not set +# CONFIG_FS_AUTOMOUNTER is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +CONFIG_FS_READABLE=y +CONFIG_FS_WRITABLE=y +# CONFIG_FS_AIO is not set +# CONFIG_FS_NAMED_SEMAPHORES is not set +CONFIG_FS_MQUEUE_MPATH="/var/mqueue" # CONFIG_FS_RAMMAP is not set CONFIG_FS_FAT=y CONFIG_FAT_LCNAMES=y @@ -623,6 +849,7 @@ CONFIG_FAT_MAXFNAME=32 CONFIG_FS_FATTIME=y # CONFIG_FAT_DMAMEMORY is not set CONFIG_FS_NXFFS=y +# CONFIG_NXFFS_SCAN_VOLUME is not set CONFIG_NXFFS_PREALLOCATED=y CONFIG_NXFFS_ERASEDSTATE=0xff CONFIG_NXFFS_PACKTHRESHOLD=32 @@ -631,12 +858,13 @@ CONFIG_NXFFS_TAILTHRESHOLD=2048 CONFIG_FS_ROMFS=y # CONFIG_FS_SMARTFS is not set CONFIG_FS_BINFS=y +# CONFIG_FS_PROCFS is not set # # System Logging # -# CONFIG_SYSLOG_ENABLE is not set CONFIG_SYSLOG=y +# CONFIG_SYSLOG_TIMESTAMP is not set CONFIG_SYSLOG_CHAR=y CONFIG_SYSLOG_DEVPATH="/dev/ttyS0" @@ -648,9 +876,9 @@ CONFIG_SYSLOG_DEVPATH="/dev/ttyS0" # # Memory Management # -# CONFIG_MM_MULTIHEAP is not set # CONFIG_MM_SMALL is not set CONFIG_MM_REGIONS=2 +# CONFIG_ARCH_HAVE_HEAP2 is not set CONFIG_GRAN=y CONFIG_GRAN_SINGLE=y CONFIG_GRAN_INTR=y @@ -661,7 +889,7 @@ CONFIG_GRAN_INTR=y # CONFIG_AUDIO is not set # -# Binary Formats +# Binary Loader # # CONFIG_BINFMT_DISABLE is not set # CONFIG_BINFMT_EXEPATH is not set @@ -684,6 +912,7 @@ CONFIG_NUNGET_CHARS=2 CONFIG_LIB_HOMEDIR="/" # CONFIG_NOPRINTF_FIELDWIDTH is not set CONFIG_LIBC_FLOATINGPOINT=y +# CONFIG_LIBC_IOCTL_VARIADIC is not set CONFIG_LIB_RAND_ORDER=1 # CONFIG_EOL_IS_CR is not set # CONFIG_EOL_IS_LF is not set @@ -695,7 +924,10 @@ CONFIG_TASK_SPAWN_DEFAULT_STACKSIZE=2048 CONFIG_LIBC_STRERROR=y # CONFIG_LIBC_STRERROR_SHORT is not set # CONFIG_LIBC_PERROR_STDOUT is not set +CONFIG_LIBC_TMPDIR="/tmp" +CONFIG_LIBC_MAX_TMPFILE=32 CONFIG_ARCH_LOWPUTC=y +# CONFIG_LIBC_LOCALTIME is not set CONFIG_LIB_SENDFILE_BUFSIZE=512 # CONFIG_ARCH_ROMGETC is not set CONFIG_ARCH_OPTIMIZED_FUNCTIONS=y @@ -715,15 +947,6 @@ CONFIG_ARCH_MEMCPY=y # # Non-standard Library Support # -CONFIG_SCHED_WORKQUEUE=y -CONFIG_SCHED_HPWORK=y -CONFIG_SCHED_WORKPRIORITY=192 -CONFIG_SCHED_WORKPERIOD=5000 -CONFIG_SCHED_WORKSTACKSIZE=1800 -CONFIG_SCHED_LPWORK=y -CONFIG_SCHED_LPWORKPRIORITY=50 -CONFIG_SCHED_LPWORKPERIOD=50000 -CONFIG_SCHED_LPWORKSTACKSIZE=1800 # CONFIG_LIB_KBDCODEC is not set # CONFIG_LIB_SLCDCODEC is not set @@ -754,8 +977,8 @@ CONFIG_BUILTIN_PROXY_STACKSIZE=1024 # # CONFIG_EXAMPLES_BUTTONS is not set # CONFIG_EXAMPLES_CAN is not set -CONFIG_EXAMPLES_CDCACM=y -# CONFIG_EXAMPLES_COMPOSITE is not set +# CONFIG_EXAMPLES_CONFIGDATA is not set +# CONFIG_EXAMPLES_CPUHOG is not set # CONFIG_EXAMPLES_CXXTEST is not set # CONFIG_EXAMPLES_DHCPD is not set # CONFIG_EXAMPLES_ELF is not set @@ -767,15 +990,20 @@ CONFIG_EXAMPLES_CDCACM=y # CONFIG_EXAMPLES_HIDKBD is not set # CONFIG_EXAMPLES_KEYPADTEST is not set # CONFIG_EXAMPLES_IGMP is not set -# CONFIG_EXAMPLES_LCDRW is not set # CONFIG_EXAMPLES_MM is not set # CONFIG_EXAMPLES_MODBUS is not set CONFIG_EXAMPLES_MOUNT=y +# CONFIG_EXAMPLES_MOUNT_BLOCKDEVICE is not set +CONFIG_EXAMPLES_MOUNT_NSECTORS=2048 +CONFIG_EXAMPLES_MOUNT_SECTORSIZE=512 +CONFIG_EXAMPLES_MOUNT_RAMDEVNO=0 +# CONFIG_EXAMPLES_MTDPART is not set # CONFIG_EXAMPLES_NRF24L01TERM is not set CONFIG_EXAMPLES_NSH=y +# CONFIG_EXAMPLES_NSH_CXXINITIALIZE is not set # CONFIG_EXAMPLES_NULL is not set # CONFIG_EXAMPLES_NX is not set -# CONFIG_EXAMPLES_NXCONSOLE is not set +# CONFIG_EXAMPLES_NXTERM is not set # CONFIG_EXAMPLES_NXFFS is not set # CONFIG_EXAMPLES_NXFLAT is not set # CONFIG_EXAMPLES_NXHELLO is not set @@ -783,13 +1011,14 @@ CONFIG_EXAMPLES_NSH=y # CONFIG_EXAMPLES_NXLINES is not set # CONFIG_EXAMPLES_NXTEXT is not set # CONFIG_EXAMPLES_OSTEST is not set -# CONFIG_EXAMPLES_PASHELLO is not set # CONFIG_EXAMPLES_PIPE is not set # CONFIG_EXAMPLES_POSIXSPAWN is not set # CONFIG_EXAMPLES_QENCODER is not set # CONFIG_EXAMPLES_RGMP is not set # CONFIG_EXAMPLES_ROMFS is not set # CONFIG_EXAMPLES_SENDMAIL is not set +# CONFIG_EXAMPLES_SERIALBLASTER is not set +# CONFIG_EXAMPLES_SERIALRX is not set # CONFIG_EXAMPLES_SERLOOP is not set # CONFIG_EXAMPLES_SLCD is not set # CONFIG_EXAMPLES_SMART_TEST is not set @@ -800,9 +1029,8 @@ CONFIG_EXAMPLES_NSH=y # CONFIG_EXAMPLES_TIFF is not set # CONFIG_EXAMPLES_TOUCHSCREEN is not set # CONFIG_EXAMPLES_UDP is not set -# CONFIG_EXAMPLES_UIP is not set +# CONFIG_EXAMPLES_WEBSERVER is not set # CONFIG_EXAMPLES_USBSERIAL is not set -# CONFIG_EXAMPLES_USBMSC is not set # CONFIG_EXAMPLES_USBTERM is not set # CONFIG_EXAMPLES_WATCHDOG is not set @@ -810,11 +1038,13 @@ CONFIG_EXAMPLES_NSH=y # Graphics Support # # CONFIG_TIFF is not set +# CONFIG_GRAPHICS_TRAVELER is not set # # Interpreters # # CONFIG_INTERPRETERS_FICL is not set +# CONFIG_INTERPRETERS_BAS is not set # CONFIG_INTERPRETERS_PCODE is not set # @@ -825,17 +1055,14 @@ CONFIG_EXAMPLES_NSH=y # Networking Utilities # # CONFIG_NETUTILS_CODECS is not set -# CONFIG_NETUTILS_DHCPC is not set # CONFIG_NETUTILS_DHCPD is not set # CONFIG_NETUTILS_FTPC is not set # CONFIG_NETUTILS_FTPD is not set # CONFIG_NETUTILS_JSON is not set -# CONFIG_NETUTILS_RESOLV is not set # CONFIG_NETUTILS_SMTP is not set -# CONFIG_NETUTILS_TELNETD is not set # CONFIG_NETUTILS_TFTPC is not set # CONFIG_NETUTILS_THTTPD is not set -# CONFIG_NETUTILS_UIPLIB is not set +# CONFIG_NETUTILS_NETLIB is not set # CONFIG_NETUTILS_WEBCLIENT is not set # @@ -847,15 +1074,32 @@ CONFIG_EXAMPLES_NSH=y # NSH Library # CONFIG_NSH_LIBRARY=y + +# +# Command Line Configuration +# +CONFIG_NSH_READLINE=y +# CONFIG_NSH_CLE is not set +CONFIG_NSH_LINELEN=128 +# CONFIG_NSH_DISABLE_SEMICOLON is not set +CONFIG_NSH_CMDPARMS=y +CONFIG_NSH_MAXARGUMENTS=12 +CONFIG_NSH_ARGCAT=y +CONFIG_NSH_NESTDEPTH=8 +# CONFIG_NSH_DISABLEBG is not set CONFIG_NSH_BUILTIN_APPS=y # # Disable Individual commands # +# CONFIG_NSH_DISABLE_ADDROUTE is not set # CONFIG_NSH_DISABLE_CAT is not set # CONFIG_NSH_DISABLE_CD is not set # CONFIG_NSH_DISABLE_CP is not set +# CONFIG_NSH_DISABLE_CMP is not set # CONFIG_NSH_DISABLE_DD is not set +# CONFIG_NSH_DISABLE_DF is not set +# CONFIG_NSH_DISABLE_DELROUTE is not set # CONFIG_NSH_DISABLE_ECHO is not set # CONFIG_NSH_DISABLE_EXEC is not set # CONFIG_NSH_DISABLE_EXIT is not set @@ -875,9 +1119,7 @@ CONFIG_NSH_BUILTIN_APPS=y # CONFIG_NSH_DISABLE_MH is not set # CONFIG_NSH_DISABLE_MOUNT is not set # CONFIG_NSH_DISABLE_MW is not set -# CONFIG_NSH_DISABLE_NSFMOUNT is not set # CONFIG_NSH_DISABLE_PS is not set -# CONFIG_NSH_DISABLE_PING is not set # CONFIG_NSH_DISABLE_PUT is not set # CONFIG_NSH_DISABLE_PWD is not set # CONFIG_NSH_DISABLE_RM is not set @@ -897,31 +1139,36 @@ CONFIG_NSH_BUILTIN_APPS=y # # CONFIG_NSH_CMDOPT_DF_H is not set CONFIG_NSH_CODECS_BUFSIZE=128 +CONFIG_NSH_CMDOPT_HEXDUMP=y CONFIG_NSH_FILEIOSIZE=512 CONFIG_NSH_STRERROR=y -CONFIG_NSH_LINELEN=128 -CONFIG_NSH_MAXARGUMENTS=12 -CONFIG_NSH_NESTDEPTH=8 + +# +# Scripting Support +# # CONFIG_NSH_DISABLESCRIPT is not set -# CONFIG_NSH_DISABLEBG is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_LOOPS is not set CONFIG_NSH_ROMFSETC=y # CONFIG_NSH_ROMFSRC is not set CONFIG_NSH_ROMFSMOUNTPT="/etc" CONFIG_NSH_INITSCRIPT="init.d/rcS" CONFIG_NSH_ROMFSDEVNO=0 CONFIG_NSH_ROMFSSECTSIZE=128 +# CONFIG_NSH_DEFAULTROMFS is not set CONFIG_NSH_ARCHROMFS=y +# CONFIG_NSH_CUSTOMROMFS is not set CONFIG_NSH_FATDEVNO=1 CONFIG_NSH_FATSECTSIZE=512 CONFIG_NSH_FATNSECTORS=1024 CONFIG_NSH_FATMOUNTPT="/tmp" -CONFIG_NSH_CONSOLE=y -# CONFIG_NSH_USBCONSOLE is not set # -# USB Trace Support +# Console Configuration # -# CONFIG_NSH_CONDEV is not set +CONFIG_NSH_CONSOLE=y +# CONFIG_NSH_USBCONSOLE is not set +# CONFIG_NSH_ALTCONDEV is not set CONFIG_NSH_ARCHINIT=y # @@ -929,7 +1176,12 @@ CONFIG_NSH_ARCHINIT=y # # -# System NSH Add-Ons +# Platform-specific Support +# +# CONFIG_PLATFORM_CONFIGDATA is not set + +# +# System Libraries and NSH Add-Ons # # @@ -938,9 +1190,18 @@ CONFIG_NSH_ARCHINIT=y # CONFIG_SYSTEM_FREE is not set # -# I2C tool +# EMACS-like Command Line Editor # -# CONFIG_SYSTEM_I2CTOOL is not set +# CONFIG_SYSTEM_CLE is not set + +# +# CU Minimal Terminal +# +CONFIG_SYSTEM_CUTERM=y +CONFIG_SYSTEM_CUTERM_DEFAULT_DEVICE="/dev/ttyS0" +CONFIG_SYSTEM_CUTERM_DEFAULT_BAUD=57600 +CONFIG_SYSTEM_CUTERM_STACKSIZE=2048 +CONFIG_SYSTEM_CUTERM_PRIORITY=100 # # FLASH Program Installation @@ -950,6 +1211,31 @@ CONFIG_NSH_ARCHINIT=y # # FLASH Erase-all Command # +# CONFIG_SYSTEM_FLASH_ERASEALL is not set + +# +# Intel HEX to binary conversion +# +# CONFIG_SYSTEM_HEX2BIN is not set + +# +# I2C tool +# +# CONFIG_SYSTEM_I2CTOOL is not set + +# +# INI File Parser +# +# CONFIG_SYSTEM_INIFILE is not set + +# +# NxPlayer media player library / command Line +# + +# +# RAM test +# +# CONFIG_SYSTEM_RAMTEST is not set # # readline() @@ -958,6 +1244,14 @@ CONFIG_SYSTEM_READLINE=y CONFIG_READLINE_ECHO=y # +# P-Code Support +# + +# +# PHY Tool +# + +# # Power Off # # CONFIG_SYSTEM_POWEROFF is not set @@ -973,10 +1267,44 @@ CONFIG_READLINE_ECHO=y # CONFIG_SYSTEM_SDCARD is not set # +# Sudoku +# +# CONFIG_SYSTEM_SUDOKU is not set + +# # Sysinfo # CONFIG_SYSTEM_SYSINFO=y +CONFIG_SYSTEM_SYSINFO_STACKSIZE=1024 + +# +# VI Work-Alike Editor +# +# CONFIG_SYSTEM_VI is not set + +# +# Stack Monitor +# + +# +# USB CDC/ACM Device Commands +# +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_CDCACM_DEVMINOR=0 + +# +# USB Composite Device Commands +# + +# +# USB Mass Storage Device Commands +# # # USB Monitor # + +# +# Zmodem Commands +# +# CONFIG_SYSTEM_ZMODEM is not set diff --git a/nuttx-configs/px4fmu-v2/Kconfig b/nuttx-configs/px4fmu-v2/Kconfig new file mode 100644 index 000000000..f9f0c5e6a --- /dev/null +++ b/nuttx-configs/px4fmu-v2/Kconfig @@ -0,0 +1,22 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +if ARCH_BOARD_PX4FMU_V2 + +config BOARD_HAS_PROBES + bool "Board provides GPIO or other Hardware for signaling to timing analyze." + default y + ---help--- + This board provides GPIO FMU-CH1-6 as PROBE_1-6 to provide timing signals from selected drivers. + +config BOARD_USE_PROBES + bool "Enable the use the board provided GPIO FMU-CH1-6 as PROBE_1-6 to provide timing signals from selected drivers" + default n + depends on BOARD_HAS_PROBES + + ---help--- + Select to use GPIO FMU-CH1-6 to provide timing signals from selected drivers. + +endif diff --git a/nuttx-configs/px4fmu-v2/include/board.h b/nuttx-configs/px4fmu-v2/include/board.h index 3b3c6fa70..3c8671f53 100755 --- a/nuttx-configs/px4fmu-v2/include/board.h +++ b/nuttx-configs/px4fmu-v2/include/board.h @@ -1,5 +1,5 @@ /************************************************************************************ - * configs/px4fmu/include/board.h + * configs/px4fmu-v2_upstream/include/board.h * include/arch/board/board.h * * Copyright (C) 2009 Gregory Nutt. All rights reserved. @@ -34,8 +34,8 @@ * ************************************************************************************/ -#ifndef __ARCH_BOARD_BOARD_H -#define __ARCH_BOARD_BOARD_H +#ifndef __CONFIGS_PX4FMU_V2_UPSTREAM_INCLUDE_BOARD_H +#define __CONFIGS_PX4FMU_V2_UPSTREAM_INCLUDE_BOARD_H /************************************************************************************ * Included Files @@ -141,26 +141,26 @@ #define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) #define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) -#define STM32_APB2_TIM9_CLKIN (2*STM32_PCLK2_FREQUENCY) -#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK2_FREQUENCY) -#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM9_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK1_FREQUENCY) /* Timer Frequencies, if APBx is set to 1, frequency is same to APBx - * otherwise frequency is 2xAPBx. + * otherwise frequency is 2xAPBx. * Note: TIM1,8 are on APB2, others on APB1 */ #define STM32_TIM18_FREQUENCY (2*STM32_PCLK2_FREQUENCY) #define STM32_TIM27_FREQUENCY (2*STM32_PCLK1_FREQUENCY) -/* SDIO dividers. Note that slower clocking is required when DMA is disabled +/* SDIO dividers. Note that slower clocking is required when DMA is disabled * in order to avoid RX overrun/TX underrun errors due to delayed responses * to service FIFOs in interrupt driven mode. These values have not been * tuned!!! * * HCLK=72MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(178+2)=400 KHz */ - + #define SDIO_INIT_CLKDIV (178 << SDIO_CLKCR_CLKDIV_SHIFT) /* DMA ON: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(2+2)=18 MHz @@ -168,9 +168,9 @@ */ #ifdef CONFIG_SDIO_DMA -# define SDIO_MMCXFR_CLKDIV (2 << SDIO_CLKCR_CLKDIV_SHIFT) +# define SDIO_MMCXFR_CLKDIV (2 << SDIO_CLKCR_CLKDIV_SHIFT) #else -# define SDIO_MMCXFR_CLKDIV (3 << SDIO_CLKCR_CLKDIV_SHIFT) +# define SDIO_MMCXFR_CLKDIV (3 << SDIO_CLKCR_CLKDIV_SHIFT) #endif /* DMA ON: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(1+2)=24 MHz @@ -196,82 +196,124 @@ /* Alternate function pin selections ************************************************/ -/* - * UARTs. - */ -#define GPIO_USART1_RX GPIO_USART1_RX_1 /* console in from IO */ -#define GPIO_USART1_TX 0 /* USART1 is RX-only */ +/* UARTs */ + +#define GPIO_USART1_RX GPIO_USART1_RX_1 /* Console in from IO */ +#define GPIO_USART1_TX 0 /* USART1 is RX-only */ -#define GPIO_USART2_RX GPIO_USART2_RX_2 -#define GPIO_USART2_TX GPIO_USART2_TX_2 -#define GPIO_USART2_RTS GPIO_USART2_RTS_2 -#define GPIO_USART2_CTS GPIO_USART2_CTS_2 +#define GPIO_USART2_RX GPIO_USART2_RX_2 +#define GPIO_USART2_TX GPIO_USART2_TX_2 +#define GPIO_USART2_RTS GPIO_USART2_RTS_2 +#define GPIO_USART2_CTS GPIO_USART2_CTS_2 -#define GPIO_USART3_RX GPIO_USART3_RX_3 -#define GPIO_USART3_TX GPIO_USART3_TX_3 -#define GPIO_USART3_RTS GPIO_USART3_RTS_2 -#define GPIO_USART3_CTS GPIO_USART3_CTS_2 +#define GPIO_USART3_RX GPIO_USART3_RX_3 +#define GPIO_USART3_TX GPIO_USART3_TX_3 +#define GPIO_USART3_RTS GPIO_USART3_RTS_2 +#define GPIO_USART3_CTS GPIO_USART3_CTS_2 -#define GPIO_UART4_RX GPIO_UART4_RX_1 -#define GPIO_UART4_TX GPIO_UART4_TX_1 +#define GPIO_UART4_RX GPIO_UART4_RX_1 +#define GPIO_UART4_TX GPIO_UART4_TX_1 -#define GPIO_USART6_RX GPIO_USART6_RX_1 -#define GPIO_USART6_TX GPIO_USART6_TX_1 +#define GPIO_USART6_RX GPIO_USART6_RX_1 +#define GPIO_USART6_TX GPIO_USART6_TX_1 -#define GPIO_UART7_RX GPIO_UART7_RX_1 -#define GPIO_UART7_TX GPIO_UART7_TX_1 +#define GPIO_UART7_RX GPIO_UART7_RX_1 +#define GPIO_UART7_TX GPIO_UART7_TX_1 /* UART8 has no alternate pin config */ /* UART RX DMA configurations */ + #define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 #define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 -/* - * CAN +/* CAN * - * CAN1 is routed to the onboard transceiver. + * CAN1 is routed to the onboard transceiver. * CAN2 is routed to the expansion connector. */ -#define GPIO_CAN1_RX GPIO_CAN1_RX_3 -#define GPIO_CAN1_TX GPIO_CAN1_TX_3 -#define GPIO_CAN2_RX GPIO_CAN2_RX_1 -#define GPIO_CAN2_TX GPIO_CAN2_TX_2 -/* - * I2C +#define GPIO_CAN1_RX GPIO_CAN1_RX_3 +#define GPIO_CAN1_TX GPIO_CAN1_TX_3 +#define GPIO_CAN2_RX GPIO_CAN2_RX_1 +#define GPIO_CAN2_TX GPIO_CAN2_TX_2 + +/* I2C * * The optional _GPIO configurations allow the I2C driver to manually * reset the bus to clear stuck slaves. They match the pin configuration, * but are normally-high GPIOs. */ -#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 -#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 -#define GPIO_I2C1_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN8) -#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9) - -#define GPIO_I2C2_SCL GPIO_I2C2_SCL_1 -#define GPIO_I2C2_SDA GPIO_I2C2_SDA_1 -#define GPIO_I2C2_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN10) -#define GPIO_I2C2_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11) - -/* - * SPI + +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 +#define GPIO_I2C1_SCL_GPIO \ + (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN8) +#define GPIO_I2C1_SDA_GPIO \ + (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9) + +#define GPIO_I2C2_SCL GPIO_I2C2_SCL_1 +#define GPIO_I2C2_SDA GPIO_I2C2_SDA_1 +#define GPIO_I2C2_SCL_GPIO \ + (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN10) +#define GPIO_I2C2_SDA_GPIO \ + (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11) + +/* SPI * * There are sensors on SPI1, and SPI2 is connected to the FRAM. */ -#define GPIO_SPI1_MISO (GPIO_SPI1_MISO_1|GPIO_SPEED_50MHz) -#define GPIO_SPI1_MOSI (GPIO_SPI1_MOSI_1|GPIO_SPEED_50MHz) -#define GPIO_SPI1_SCK (GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz) -#define GPIO_SPI2_MISO (GPIO_SPI2_MISO_1|GPIO_SPEED_50MHz) -#define GPIO_SPI2_MOSI (GPIO_SPI2_MOSI_1|GPIO_SPEED_50MHz) -#define GPIO_SPI2_SCK (GPIO_SPI2_SCK_2|GPIO_SPEED_50MHz) +#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 +#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 +#define GPIO_SPI1_SCK GPIO_SPI1_SCK_1 + +#define GPIO_SPI2_MISO GPIO_SPI2_MISO_1 +#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_1 +#define GPIO_SPI2_SCK GPIO_SPI2_SCK_2 #define GPIO_SPI4_MISO (GPIO_SPI4_MISO_1|GPIO_SPEED_50MHz) #define GPIO_SPI4_MOSI (GPIO_SPI4_MOSI_1|GPIO_SPEED_50MHz) #define GPIO_SPI4_SCK (GPIO_SPI4_SCK_1|GPIO_SPEED_50MHz) +/* LED Definitions. Needed if CONFIG_ARCH_LEDs is defined */ + +#define LED_STARTED 0 +#define LED_HEAPALLOCATE 0 +#define LED_IRQSENABLED 0 +#define LED_STACKCREATED 1 +#define LED_INIRQ 1 +#define LED_SIGNAL 1 +#define LED_ASSERTION 1 +#define LED_PANIC 1 + +/* Board provides GPIO or other Hardware for signaling to timing analyzer */ + +#if defined(CONFIG_BOARD_USE_PROBES) +# define PROBE_N(n) (1<<((n)-1)) +# define PROBE_1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN14) +# define PROBE_2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN13) +# define PROBE_3 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11) +# define PROBE_4 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN9) +# define PROBE_5 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13) +# define PROBE_6 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) + +# define PROBE_INIT(mask) \ + do { \ + if ((mask)& PROBE_N(1)) { stm32_configgpio(PROBE_1); } \ + if ((mask)& PROBE_N(2)) { stm32_configgpio(PROBE_2); } \ + if ((mask)& PROBE_N(3)) { stm32_configgpio(PROBE_3); } \ + if ((mask)& PROBE_N(4)) { stm32_configgpio(PROBE_4); } \ + if ((mask)& PROBE_N(5)) { stm32_configgpio(PROBE_5); } \ + if ((mask)& PROBE_N(6)) { stm32_configgpio(PROBE_6); } \ + } while(0) + +# define PROBE(n,s) do {stm32_gpiowrite(PROBE_##n,(s));}while(0) +# define PROBE_MARK(n) PROBE(n,false);PROBE(n,true) +#endif + + + /************************************************************************************ * Public Data ************************************************************************************/ @@ -281,7 +323,8 @@ #undef EXTERN #if defined(__cplusplus) #define EXTERN extern "C" -extern "C" { +extern "C" +{ #else #define EXTERN extern #endif @@ -299,7 +342,7 @@ extern "C" { * ************************************************************************************/ -EXTERN void stm32_boardinitialize(void); +void stm32_boardinitialize(void); #undef EXTERN #if defined(__cplusplus) @@ -307,4 +350,4 @@ EXTERN void stm32_boardinitialize(void); #endif #endif /* __ASSEMBLY__ */ -#endif /* __ARCH_BOARD_BOARD_H */ +#endif /* __CONFIGS_PX4FMU_V2_UPSTREAM_INCLUDE_BOARD_H */ diff --git a/nuttx-configs/px4fmu-v2/nsh/Make.defs b/nuttx-configs/px4fmu-v2/nsh/Make.defs index 5a1d5af2c..02e2db428 100644 --- a/nuttx-configs/px4fmu-v2/nsh/Make.defs +++ b/nuttx-configs/px4fmu-v2/nsh/Make.defs @@ -1,5 +1,5 @@ ############################################################################ -# configs/px4fmu-v2/nsh/Make.defs +# configs/px4fmu-v2_updstream/nsh/Make.defs # # Copyright (C) 2011 Gregory Nutt. All rights reserved. # Author: Gregory Nutt <gnutt@nuttx.org> @@ -35,146 +35,145 @@ include ${TOPDIR}/.config include ${TOPDIR}/tools/Config.mk - -# -# We only support building with the ARM bare-metal toolchain from -# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. -# -CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI - include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs -CC = $(CROSSDEV)gcc -CXX = $(CROSSDEV)g++ -CPP = $(CROSSDEV)gcc -E -LD = $(CROSSDEV)ld -AR = $(CROSSDEV)ar rcs -NM = $(CROSSDEV)nm -OBJCOPY = $(CROSSDEV)objcopy -OBJDUMP = $(CROSSDEV)objdump +CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI + +CC = $(CROSSDEV)gcc +CXX = $(CROSSDEV)g++ +CPP = $(CROSSDEV)gcc -E +LD = $(CROSSDEV)ld +AR = $(CROSSDEV)ar rcs +NM = $(CROSSDEV)nm +OBJCOPY = $(CROSSDEV)objcopy +OBJDUMP = $(CROSSDEV)objdump MAXOPTIMIZATION = -Os -ARCHCPUFLAGS = -mcpu=cortex-m4 \ - -mthumb \ - -march=armv7e-m \ - -mfpu=fpv4-sp-d16 \ - -mfloat-abi=hard +ARCHCPUFLAGS = -mcpu=cortex-m4 \ + -mthumb \ + -march=armv7e-m \ + -mfpu=fpv4-sp-d16 \ + -mfloat-abi=hard -# enable precise stack overflow tracking ifeq ($(CONFIG_ARMV7M_STACKCHECK),y) -INSTRUMENTATIONDEFINES = -finstrument-functions -ffixed-r10 -endif +# enable precise stack overflow tracking +INSTRUMENTATIONDEFINES = -finstrument-functions \ + -ffixed-r10 +endif +# Pull in *just* libm from the toolchain ... this is grody -# pull in *just* libm from the toolchain ... this is grody -LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" -EXTRA_LIBS += $(LIBM) +LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" +EXTRA_LIBS += $(LIBM) -# use our linker script -LDSCRIPT = ld.script +# Use our linker script + +LDSCRIPT = ld.script ifeq ($(WINTOOL),y) # Windows-native toolchains - DIRLINK = $(TOPDIR)/tools/copydir.sh - DIRUNLINK = $(TOPDIR)/tools/unlink.sh - MKDEP = $(TOPDIR)/tools/mknulldeps.sh - ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" - ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" - ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}" + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" + ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" + ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}" else ifeq ($(PX4_WINTOOL),y) # Windows-native toolchains (MSYS) - DIRLINK = $(TOPDIR)/tools/copydir.sh - DIRUNLINK = $(TOPDIR)/tools/unlink.sh - MKDEP = $(TOPDIR)/tools/mknulldeps.sh - ARCHINCLUDES = -I. -isystem $(TOPDIR)/include - ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx - ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) else - # Linux/Cygwin-native toolchain - MKDEP = $(TOPDIR)/tools/mkdeps.sh - ARCHINCLUDES = -I. -isystem $(TOPDIR)/include - ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx - ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + # Linux/Cygwin-native toolchain + MKDEP = $(TOPDIR)/tools/mkdeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) endif endif -# tool versions -ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} -ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} +# Tool versions + +ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} +ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} -# optimisation flags -ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ - -fno-strict-aliasing \ - -fno-strength-reduce \ - -fomit-frame-pointer \ - -funsafe-math-optimizations \ - -fno-builtin-printf \ - -ffunction-sections \ - -fdata-sections +# Optimization flags + +ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ + -fno-strict-aliasing \ + -fno-strength-reduce \ + -fomit-frame-pointer \ + -funsafe-math-optimizations \ + -fno-builtin-printf \ + -ffunction-sections \ + -fdata-sections ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") -ARCHOPTIMIZATION += -g +ARCHOPTIMIZATION += -g endif -ARCHCFLAGS = -std=gnu99 -ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -ARCHWARNINGS = -Wall \ - -Wno-sign-compare \ - -Wextra \ - -Wdouble-promotion \ - -Wshadow \ - -Wfloat-equal \ - -Wframe-larger-than=1024 \ - -Wpointer-arith \ - -Wlogical-op \ - -Wmissing-declarations \ - -Wpacked \ - -Wno-unused-parameter +ARCHCFLAGS = -std=gnu99 +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x +ARCHWARNINGS = -Wall \ + -Wno-sign-compare \ + -Wextra \ + -Wdouble-promotion \ + -Wshadow \ + -Wfloat-equal \ + -Wframe-larger-than=1024 \ + -Wpointer-arith \ + -Wlogical-op \ + -Wmissing-declarations \ + -Wpacked \ + -Wno-unused-parameter # -Wcast-qual - generates spurious noreturn attribute warnings, try again later # -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code # -Wcast-align - would help catch bad casts in some cases, but generates too many false positives -ARCHCWARNINGS = $(ARCHWARNINGS) \ - -Wbad-function-cast \ - -Wstrict-prototypes \ - -Wold-style-declaration \ - -Wmissing-parameter-type \ - -Wmissing-prototypes \ - -Wnested-externs -ARCHWARNINGSXX = $(ARCHWARNINGS) \ - -Wno-psabi -ARCHDEFINES = -ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 - -# this seems to be the only way to add linker flags -EXTRA_LIBS += --warn-common \ - --gc-sections - -CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common -CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) -CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) -CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -AFLAGS = $(CFLAGS) -D__ASSEMBLY__ - -NXFLATLDFLAGS1 = -r -d -warn-common -NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections -LDNXFLATFLAGS = -e main -s 2048 - -OBJEXT = .o -LIBEXT = .a -EXEEXT = - - -# produce partially-linked $1 from files in $2 +ARCHCWARNINGS = $(ARCHWARNINGS) \ + -Wbad-function-cast \ + -Wstrict-prototypes \ + -Wold-style-declaration \ + -Wmissing-parameter-type \ + -Wmissing-prototypes \ + -Wnested-externs +ARCHWARNINGSXX = $(ARCHWARNINGS) \ + -Wno-psabi +ARCHDEFINES = +ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 + +# This seems to be the only way to add linker flags + +EXTRA_LIBS += --warn-common \ + --gc-sections + +CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common +CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) +CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe +CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) +CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ + +NXFLATLDFLAGS1 = -r -d -warn-common +NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections +LDNXFLATFLAGS = -e main -s 2048 + +OBJEXT = .o +LIBEXT = .a +EXEEXT = + +# Produce partially-linked $1 from files in $2 + define PRELINK - @echo "PRELINK: $1" - $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 + @echo "PRELINK: $1" + $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 endef -HOSTCC = gcc -HOSTINCLUDES = -I. -HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe -HOSTLDFLAGS = - +HOSTCC = gcc +HOSTINCLUDES = -I. +HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe +HOSTLDFLAGS = diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index dedebdfa0..ba39a3125 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -2,14 +2,14 @@ # Automatically generated file; DO NOT EDIT. # Nuttx/ Configuration # -CONFIG_NUTTX_NEWCONFIG=y # # Build Setup # # CONFIG_EXPERIMENTAL is not set -# CONFIG_HOST_LINUX is not set -CONFIG_HOST_OSX=y +# CONFIG_DEFAULT_SMALL is not set +CONFIG_HOST_LINUX=y +# CONFIG_HOST_OSX is not set # CONFIG_HOST_WINDOWS is not set # CONFIG_HOST_OTHER is not set @@ -17,6 +17,7 @@ CONFIG_HOST_OSX=y # Build Configuration # CONFIG_APPS_DIR="../apps" +CONFIG_BUILD_FLAT=y # CONFIG_BUILD_2PASS is not set # @@ -26,10 +27,12 @@ CONFIG_APPS_DIR="../apps" # CONFIG_INTELHEX_BINARY is not set # CONFIG_MOTOROLA_SREC is not set CONFIG_RAW_BINARY=y +# CONFIG_UBOOT_UIMAGE is not set # # Customize Header Files # +# CONFIG_ARCH_STDINT_H is not set # CONFIG_ARCH_STDBOOL_H is not set CONFIG_ARCH_MATH_H=y # CONFIG_ARCH_FLOAT_H is not set @@ -38,37 +41,19 @@ CONFIG_ARCH_MATH_H=y # # Debug Options # -CONFIG_DEBUG=n -CONFIG_DEBUG_VERBOSE=n - -# -# Subsystem Debug Options -# -# CONFIG_DEBUG_MM is not set -# CONFIG_DEBUG_SCHED is not set -# CONFIG_DEBUG_USB is not set -CONFIG_DEBUG_FS=y -# CONFIG_DEBUG_LIB is not set -# CONFIG_DEBUG_BINFMT is not set -# CONFIG_DEBUG_GRAPHICS is not set - -# -# Driver Debug Options -# -# CONFIG_DEBUG_ANALOG is not set -# CONFIG_DEBUG_I2C is not set -# CONFIG_DEBUG_SPI is not set -# CONFIG_DEBUG_SDIO is not set -# CONFIG_DEBUG_GPIO is not set -CONFIG_DEBUG_DMA=y -# CONFIG_DEBUG_WATCHDOG is not set -# CONFIG_DEBUG_AUDIO is not set +# CONFIG_DEBUG is not set +CONFIG_ARCH_HAVE_STACKCHECK=y +CONFIG_ARCH_HAVE_HEAPCHECK=y +CONFIG_STACK_COLORATION=y CONFIG_DEBUG_SYMBOLS=y +CONFIG_ARCH_HAVE_CUSTOMOPT=y +CONFIG_DEBUG_NOOPT=y +# CONFIG_DEBUG_CUSTOMOPT is not set +# CONFIG_DEBUG_FULLOPT is not set # # System Type # -# CONFIG_ARCH_8051 is not set CONFIG_ARCH_ARM=y # CONFIG_ARCH_AVR is not set # CONFIG_ARCH_HC is not set @@ -84,23 +69,35 @@ CONFIG_ARCH="arm" # # ARM Options # +# CONFIG_ARCH_CHIP_A1X is not set # CONFIG_ARCH_CHIP_C5471 is not set # CONFIG_ARCH_CHIP_CALYPSO is not set # CONFIG_ARCH_CHIP_DM320 is not set +# CONFIG_ARCH_CHIP_EFM32 is not set # CONFIG_ARCH_CHIP_IMX is not set # CONFIG_ARCH_CHIP_KINETIS is not set # CONFIG_ARCH_CHIP_KL is not set # CONFIG_ARCH_CHIP_LM is not set +# CONFIG_ARCH_CHIP_TIVA is not set # CONFIG_ARCH_CHIP_LPC17XX is not set # CONFIG_ARCH_CHIP_LPC214X is not set # CONFIG_ARCH_CHIP_LPC2378 is not set # CONFIG_ARCH_CHIP_LPC31XX is not set # CONFIG_ARCH_CHIP_LPC43XX is not set # CONFIG_ARCH_CHIP_NUC1XX is not set +# CONFIG_ARCH_CHIP_SAMA5 is not set +# CONFIG_ARCH_CHIP_SAMD is not set # CONFIG_ARCH_CHIP_SAM34 is not set CONFIG_ARCH_CHIP_STM32=y # CONFIG_ARCH_CHIP_STR71X is not set +# CONFIG_ARCH_ARM7TDMI is not set +# CONFIG_ARCH_ARM926EJS is not set +# CONFIG_ARCH_ARM920T is not set +# CONFIG_ARCH_CORTEXM0 is not set +# CONFIG_ARCH_CORTEXM3 is not set CONFIG_ARCH_CORTEXM4=y +# CONFIG_ARCH_CORTEXA5 is not set +# CONFIG_ARCH_CORTEXA8 is not set CONFIG_ARCH_FAMILY="armv7-m" CONFIG_ARCH_CHIP="stm32" CONFIG_ARMV7M_USEBASEPRI=y @@ -108,17 +105,17 @@ CONFIG_ARCH_HAVE_CMNVECTOR=y CONFIG_ARMV7M_CMNVECTOR=y CONFIG_ARCH_HAVE_FPU=y CONFIG_ARCH_FPU=y -CONFIG_ARCH_HAVE_MPU=y # CONFIG_ARMV7M_MPU is not set -# CONFIG_DEBUG_HARDFAULT is not set # # ARMV7M Configuration Options # # CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set -CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y -CONFIG_ARMV7M_STACKCHECK=n -CONFIG_SERIAL_TERMIOS=y +# CONFIG_ARMV7M_TOOLCHAIN_CODEREDL is not set +# CONFIG_ARMV7M_TOOLCHAIN_CODESOURCERYL is not set +CONFIG_ARMV7M_TOOLCHAIN_GNU_EABIL=y +# CONFIG_ARMV7M_STACKCHECK is not set +# CONFIG_ARMV7M_ITMSYSLOG is not set CONFIG_SDIO_DMA=y CONFIG_SDIO_DMAPRIO=0x00010000 # CONFIG_SDIO_WIDTH_D1_ONLY is not set @@ -144,6 +141,7 @@ CONFIG_SDIO_DMAPRIO=0x00010000 # CONFIG_ARCH_CHIP_STM32L152V6 is not set # CONFIG_ARCH_CHIP_STM32L152V8 is not set # CONFIG_ARCH_CHIP_STM32L152VB is not set +# CONFIG_ARCH_CHIP_STM32L162ZD is not set # CONFIG_ARCH_CHIP_STM32F100C8 is not set # CONFIG_ARCH_CHIP_STM32F100CB is not set # CONFIG_ARCH_CHIP_STM32F100R8 is not set @@ -156,15 +154,27 @@ CONFIG_SDIO_DMAPRIO=0x00010000 # CONFIG_ARCH_CHIP_STM32F100VC is not set # CONFIG_ARCH_CHIP_STM32F100VD is not set # CONFIG_ARCH_CHIP_STM32F100VE is not set +# CONFIG_ARCH_CHIP_STM32F102CB is not set +# CONFIG_ARCH_CHIP_STM32F103T8 is not set +# CONFIG_ARCH_CHIP_STM32F103TB is not set # CONFIG_ARCH_CHIP_STM32F103C4 is not set # CONFIG_ARCH_CHIP_STM32F103C8 is not set -# CONFIG_ARCH_CHIP_STM32F103RET6 is not set -# CONFIG_ARCH_CHIP_STM32F103VCT6 is not set -# CONFIG_ARCH_CHIP_STM32F103VET6 is not set -# CONFIG_ARCH_CHIP_STM32F103ZET6 is not set -# CONFIG_ARCH_CHIP_STM32F105VBT7 is not set +# CONFIG_ARCH_CHIP_STM32F103CB is not set +# CONFIG_ARCH_CHIP_STM32F103R8 is not set +# CONFIG_ARCH_CHIP_STM32F103RB is not set +# CONFIG_ARCH_CHIP_STM32F103RC is not set +# CONFIG_ARCH_CHIP_STM32F103RD is not set +# CONFIG_ARCH_CHIP_STM32F103RE is not set +# CONFIG_ARCH_CHIP_STM32F103RG is not set +# CONFIG_ARCH_CHIP_STM32F103V8 is not set +# CONFIG_ARCH_CHIP_STM32F103VB is not set +# CONFIG_ARCH_CHIP_STM32F103VC is not set +# CONFIG_ARCH_CHIP_STM32F103VE is not set +# CONFIG_ARCH_CHIP_STM32F103ZE is not set +# CONFIG_ARCH_CHIP_STM32F105VB is not set # CONFIG_ARCH_CHIP_STM32F107VC is not set # CONFIG_ARCH_CHIP_STM32F207IG is not set +# CONFIG_ARCH_CHIP_STM32F207ZE is not set # CONFIG_ARCH_CHIP_STM32F302CB is not set # CONFIG_ARCH_CHIP_STM32F302CC is not set # CONFIG_ARCH_CHIP_STM32F302RB is not set @@ -177,6 +187,8 @@ CONFIG_SDIO_DMAPRIO=0x00010000 # CONFIG_ARCH_CHIP_STM32F303RC is not set # CONFIG_ARCH_CHIP_STM32F303VB is not set # CONFIG_ARCH_CHIP_STM32F303VC is not set +# CONFIG_ARCH_CHIP_STM32F401RE is not set +# CONFIG_ARCH_CHIP_STM32F411RE is not set # CONFIG_ARCH_CHIP_STM32F405RG is not set # CONFIG_ARCH_CHIP_STM32F405VG is not set # CONFIG_ARCH_CHIP_STM32F405ZG is not set @@ -189,24 +201,71 @@ CONFIG_SDIO_DMAPRIO=0x00010000 CONFIG_ARCH_CHIP_STM32F427V=y # CONFIG_ARCH_CHIP_STM32F427Z is not set # CONFIG_ARCH_CHIP_STM32F427I is not set +# CONFIG_ARCH_CHIP_STM32F429V is not set +# CONFIG_ARCH_CHIP_STM32F429Z is not set +# CONFIG_ARCH_CHIP_STM32F429I is not set +# CONFIG_ARCH_CHIP_STM32F429B is not set +# CONFIG_ARCH_CHIP_STM32F429N is not set # CONFIG_STM32_STM32L15XX is not set # CONFIG_STM32_ENERGYLITE is not set # CONFIG_STM32_STM32F10XX is not set # CONFIG_STM32_VALUELINE is not set # CONFIG_STM32_CONNECTIVITYLINE is not set # CONFIG_STM32_PERFORMANCELINE is not set +# CONFIG_STM32_USBACCESSLINE is not set # CONFIG_STM32_HIGHDENSITY is not set # CONFIG_STM32_MEDIUMDENSITY is not set # CONFIG_STM32_LOWDENSITY is not set # CONFIG_STM32_STM32F20XX is not set +# CONFIG_STM32_STM32F207 is not set # CONFIG_STM32_STM32F30XX is not set CONFIG_STM32_STM32F40XX=y +# CONFIG_STM32_STM32F401 is not set +# CONFIG_STM32_STM32F411 is not set +# CONFIG_STM32_STM32F405 is not set +# CONFIG_STM32_STM32F407 is not set CONFIG_STM32_STM32F427=y +# CONFIG_STM32_STM32F429 is not set # CONFIG_STM32_DFU is not set # # STM32 Peripheral Support # +CONFIG_STM32_HAVE_CCM=y +# CONFIG_STM32_HAVE_USBDEV is not set +CONFIG_STM32_HAVE_OTGFS=y +CONFIG_STM32_HAVE_FSMC=y +CONFIG_STM32_HAVE_USART3=y +CONFIG_STM32_HAVE_UART4=y +CONFIG_STM32_HAVE_UART5=y +CONFIG_STM32_HAVE_USART6=y +CONFIG_STM32_HAVE_UART7=y +CONFIG_STM32_HAVE_UART8=y +CONFIG_STM32_HAVE_TIM1=y +CONFIG_STM32_HAVE_TIM5=y +CONFIG_STM32_HAVE_TIM6=y +CONFIG_STM32_HAVE_TIM7=y +CONFIG_STM32_HAVE_TIM8=y +CONFIG_STM32_HAVE_TIM9=y +CONFIG_STM32_HAVE_TIM10=y +CONFIG_STM32_HAVE_TIM11=y +CONFIG_STM32_HAVE_TIM12=y +CONFIG_STM32_HAVE_TIM13=y +CONFIG_STM32_HAVE_TIM14=y +# CONFIG_STM32_HAVE_TIM15 is not set +# CONFIG_STM32_HAVE_TIM16 is not set +# CONFIG_STM32_HAVE_TIM17 is not set +CONFIG_STM32_HAVE_ADC2=y +CONFIG_STM32_HAVE_ADC3=y +# CONFIG_STM32_HAVE_ADC4 is not set +CONFIG_STM32_HAVE_CAN1=y +CONFIG_STM32_HAVE_CAN2=y +CONFIG_STM32_HAVE_RNG=y +CONFIG_STM32_HAVE_ETHMAC=y +CONFIG_STM32_HAVE_SPI2=y +CONFIG_STM32_HAVE_SPI3=y +CONFIG_STM32_HAVE_SPI4=y +CONFIG_STM32_HAVE_SPI5=y CONFIG_STM32_ADC1=y # CONFIG_STM32_ADC2 is not set # CONFIG_STM32_ADC3 is not set @@ -302,7 +361,6 @@ CONFIG_USART2_RXDMA=y CONFIG_USART3_RXDMA=y # CONFIG_UART4_RS485 is not set CONFIG_UART4_RXDMA=y -CONFIG_UART5_RXDMA=y # CONFIG_USART6_RS485 is not set CONFIG_USART6_RXDMA=y # CONFIG_UART7_RS485 is not set @@ -310,6 +368,7 @@ CONFIG_UART7_RXDMA=y # CONFIG_UART8_RS485 is not set CONFIG_UART8_RXDMA=y CONFIG_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32_FLOWCONTROL_BROKEN=y CONFIG_STM32_USART_SINGLEWIRE=y # @@ -321,26 +380,34 @@ CONFIG_STM32_USART_SINGLEWIRE=y # # I2C Configuration # +# CONFIG_STM32_I2C_ALT is not set # CONFIG_STM32_I2C_DYNTIMEO is not set CONFIG_STM32_I2CTIMEOSEC=0 CONFIG_STM32_I2CTIMEOMS=10 +CONFIG_STM32_I2CTIMEOTICKS=10 # CONFIG_STM32_I2C_DUTY16_9 is not set # # SDIO Configuration # -CONFIG_SDIO_PRI=128 +# CONFIG_RTC_LSECLOCK is not set +# CONFIG_RTC_LSICLOCK is not set +CONFIG_RTC_HSECLOCK=y # -# USB Host Configuration +# USB FS Host Configuration # # -# USB Device Configuration +# USB HS Host Configuration # # -# External Memory Configuration +# USB Host Debug Configuration +# + +# +# USB Device Configuration # # @@ -349,12 +416,21 @@ CONFIG_SDIO_PRI=128 # CONFIG_ARCH_NOINTC is not set # CONFIG_ARCH_VECNOTIRQ is not set CONFIG_ARCH_DMA=y -# CONFIG_ARCH_IRQPRIO is not set -# CONFIG_CUSTOM_STACK is not set -# CONFIG_ADDRENV is not set +CONFIG_ARCH_HAVE_IRQPRIO=y +# CONFIG_ARCH_L2CACHE is not set +# CONFIG_ARCH_HAVE_COHERENT_DCACHE is not set +# CONFIG_ARCH_HAVE_ADDRENV is not set +# CONFIG_ARCH_NEED_ADDRENV_MAPPING is not set CONFIG_ARCH_HAVE_VFORK=y +# CONFIG_ARCH_HAVE_MMU is not set +CONFIG_ARCH_HAVE_MPU=y +# CONFIG_ARCH_NAND_HWECC is not set +# CONFIG_ARCH_HAVE_EXTCLK is not set +# CONFIG_ARCH_USE_MPU is not set +# CONFIG_ARCH_IRQPRIO is not set CONFIG_ARCH_STACKDUMP=y # CONFIG_ENDIAN_BIG is not set +# CONFIG_ARCH_IDLE_CUSTOM is not set # CONFIG_ARCH_HAVE_RAMFUNCS is not set CONFIG_ARCH_HAVE_RAMVECTORS=y # CONFIG_ARCH_RAMVECTORS is not set @@ -364,10 +440,14 @@ CONFIG_ARCH_HAVE_RAMVECTORS=y # CONFIG_BOARD_LOOPSPERMSEC=16717 # CONFIG_ARCH_CALIBRATION is not set -CONFIG_DRAM_START=0x20000000 -CONFIG_DRAM_SIZE=262144 + +# +# Interrupt options +# CONFIG_ARCH_HAVE_INTERRUPTSTACK=y -CONFIG_ARCH_INTERRUPTSTACK=4096 +CONFIG_ARCH_INTERRUPTSTACK=2048 +CONFIG_ARCH_HAVE_HIPRI_INTERRUPT=y +# CONFIG_ARCH_HIPRI_INTERRUPT is not set # # Boot options @@ -379,49 +459,95 @@ CONFIG_BOOT_RUNFROMFLASH=y # CONFIG_BOOT_COPYTORAM is not set # +# Boot Memory Configuration +# +CONFIG_RAM_START=0x20000000 +CONFIG_RAM_SIZE=262144 +# CONFIG_ARCH_HAVE_SDRAM is not set + +# # Board Selection # -CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD="" +CONFIG_ARCH_BOARD_PX4FMU_V2=y +# CONFIG_ARCH_BOARD_CUSTOM is not set +CONFIG_ARCH_BOARD="px4fmu-v2" # # Common Board Options # CONFIG_NSH_MMCSDMINOR=0 CONFIG_NSH_MMCSDSLOTNO=0 -CONFIG_MMCSD_HAVE_SDIOWAIT_WRCOMPLETE=y + # # Board-Specific Options # +CONFIG_BOARD_HAS_PROBES=y +# CONFIG_BOARD_USE_PROBES is not set # # RTOS Features # -# CONFIG_BOARD_INITIALIZE is not set -CONFIG_MSEC_PER_TICK=1 +# CONFIG_DISABLE_OS_API is not set + +# +# Clocks and Timers +# +CONFIG_USEC_PER_TICK=1000 +# CONFIG_SYSTEM_TIME64 is not set +# CONFIG_CLOCK_MONOTONIC is not set +# CONFIG_JULIAN_TIME is not set +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=50 +CONFIG_WDOG_INTRESERVE=4 +CONFIG_PREALLOC_TIMERS=50 + +# +# Tasks and Scheduling +# +# CONFIG_INIT_NONE is not set +CONFIG_INIT_ENTRYPOINT=y +# CONFIG_INIT_FILEPATH is not set +CONFIG_USER_ENTRYPOINT="nsh_main" CONFIG_RR_INTERVAL=0 -CONFIG_SCHED_INSTRUMENTATION=y CONFIG_TASK_NAME_SIZE=24 +CONFIG_MAX_TASKS=32 # CONFIG_SCHED_HAVE_PARENT is not set -# CONFIG_JULIAN_TIME is not set -CONFIG_START_YEAR=1970 -CONFIG_START_MONTH=1 -CONFIG_START_DAY=1 -CONFIG_DEV_CONSOLE=y +CONFIG_SCHED_WAITPID=y + +# +# Pthread Options +# # CONFIG_MUTEX_TYPES is not set -CONFIG_PRIORITY_INHERITANCE=y -CONFIG_SEM_PREALLOCHOLDERS=0 -CONFIG_SEM_NNESTPRIO=8 +CONFIG_NPTHREAD_KEYS=4 + +# +# Performance Monitoring +# +# CONFIG_SCHED_CPULOAD is not set +CONFIG_SCHED_INSTRUMENTATION=y + +# +# Files and I/O +# +CONFIG_DEV_CONSOLE=y # CONFIG_FDCLONE_DISABLE is not set CONFIG_FDCLONE_STDIO=y CONFIG_SDCLONE_DISABLE=y -CONFIG_SCHED_WAITPID=y +CONFIG_NFILE_DESCRIPTORS=42 +CONFIG_NFILE_STREAMS=8 +CONFIG_NAME_MAX=32 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_NNESTPRIO=8 + +# +# RTOS hooks +# +# CONFIG_BOARD_INITIALIZE is not set # CONFIG_SCHED_STARTHOOK is not set CONFIG_SCHED_ATEXIT=y CONFIG_SCHED_ATEXIT_MAX=1 # CONFIG_SCHED_ONEXIT is not set -CONFIG_USER_ENTRYPOINT="nsh_main" -# CONFIG_DISABLE_OS_API is not set # # Signal Numbers @@ -433,19 +559,25 @@ CONFIG_SIG_SIGCONDTIMEDOUT=16 CONFIG_SIG_SIGWORK=4 # -# Sizes of configurable things (0 disables) +# POSIX Message Queue Options # -CONFIG_MAX_TASKS=32 -CONFIG_MAX_TASK_ARGS=10 -CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=42 -CONFIG_NFILE_STREAMS=8 -CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 CONFIG_MQ_MAXMSGSIZE=32 -CONFIG_MAX_WDOGPARMS=2 -CONFIG_PREALLOC_WDOGS=50 -CONFIG_PREALLOC_TIMERS=50 + +# +# Work Queue Support +# +CONFIG_SCHED_WORKQUEUE=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=192 +CONFIG_SCHED_HPWORKPERIOD=5000 +CONFIG_SCHED_HPWORKSTACKSIZE=1800 +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPNTHREADS=1 +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKPRIOMAX=176 +CONFIG_SCHED_LPWORKPERIOD=50000 +CONFIG_SCHED_LPWORKSTACKSIZE=1800 # # Stack and heap information @@ -454,6 +586,7 @@ CONFIG_IDLETHREAD_STACKSIZE=3500 CONFIG_USERMAIN_STACKSIZE=2600 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_PTHREAD_STACK_DEFAULT=2048 +# CONFIG_LIB_SYSCALL is not set # # Device Drivers @@ -462,27 +595,39 @@ CONFIG_PTHREAD_STACK_DEFAULT=2048 CONFIG_DEV_NULL=y # CONFIG_DEV_ZERO is not set # CONFIG_LOOP is not set + +# +# Buffering +# +# CONFIG_DRVR_WRITEBUFFER is not set +# CONFIG_DRVR_READAHEAD is not set # CONFIG_RAMDISK is not set # CONFIG_CAN is not set +# CONFIG_ARCH_HAVE_PWM_PULSECOUNT is not set # CONFIG_PWM is not set +CONFIG_ARCH_HAVE_I2CRESET=y CONFIG_I2C=y # CONFIG_I2C_SLAVE is not set CONFIG_I2C_TRANSFER=y # CONFIG_I2C_WRITEREAD is not set # CONFIG_I2C_POLLED is not set # CONFIG_I2C_TRACE is not set -CONFIG_ARCH_HAVE_I2CRESET=y CONFIG_I2C_RESET=y CONFIG_SPI=y # CONFIG_SPI_OWNBUS is not set CONFIG_SPI_EXCHANGE=y # CONFIG_SPI_CMDDATA is not set +# CONFIG_SPI_BITBANG is not set +# CONFIG_I2S is not set CONFIG_RTC=y CONFIG_RTC_DATETIME=y -CONFIG_RTC_HSECLOCK=y +# CONFIG_RTC_ALARM is not set CONFIG_WATCHDOG=y +CONFIG_WATCHDOG_DEVPATH="/dev/watchdog0" +# CONFIG_TIMER is not set # CONFIG_ANALOG is not set # CONFIG_AUDIO_DEVICES is not set +# CONFIG_VIDEO_DEVICES is not set # CONFIG_BCH is not set # CONFIG_INPUT is not set # CONFIG_LCD is not set @@ -493,8 +638,12 @@ CONFIG_MMCSD_MULTIBLOCK_DISABLE=y # CONFIG_MMCSD_MMCSUPPORT is not set # CONFIG_MMCSD_HAVECARDDETECT is not set # CONFIG_MMCSD_SPI is not set +CONFIG_ARCH_HAVE_SDIO=y +CONFIG_ARCH_HAVE_SDIOWAIT_WRCOMPLETE=y CONFIG_MMCSD_SDIO=y +CONFIG_SDIO_PREFLIGHT=y # CONFIG_SDIO_MUXBUS is not set +CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y # CONFIG_SDIO_BLOCKSETUP is not set CONFIG_MTD=y @@ -502,39 +651,77 @@ CONFIG_MTD=y # MTD Configuration # CONFIG_MTD_PARTITION=y +# CONFIG_MTD_SECT512 is not set CONFIG_MTD_BYTE_WRITE=y +# CONFIG_MTD_CONFIG is not set # # MTD Device Drivers # +# CONFIG_MTD_NAND is not set # CONFIG_RAMMTD is not set # CONFIG_MTD_AT24XX is not set +# CONFIG_MTD_AT25 is not set # CONFIG_MTD_AT45DB is not set # CONFIG_MTD_M25P is not set # CONFIG_MTD_SMART is not set CONFIG_MTD_RAMTRON=y +CONFIG_RAMTRON_WRITEWAIT=y +CONFIG_RAMTRON_SETSPEED=y # CONFIG_MTD_SST25 is not set +# CONFIG_MTD_SST25XX is not set # CONFIG_MTD_SST39FV is not set # CONFIG_MTD_W25 is not set +# CONFIG_EEPROM is not set CONFIG_PIPES=y # CONFIG_PM is not set # CONFIG_POWER is not set # CONFIG_SENSORS is not set +# CONFIG_SERCOMM_CONSOLE is not set CONFIG_SERIAL=y # CONFIG_DEV_LOWCONSOLE is not set CONFIG_SERIAL_REMOVABLE=y # CONFIG_16550_UART is not set +# CONFIG_ARCH_HAVE_UART is not set +# CONFIG_ARCH_HAVE_UART0 is not set +# CONFIG_ARCH_HAVE_UART1 is not set +# CONFIG_ARCH_HAVE_UART2 is not set +# CONFIG_ARCH_HAVE_UART3 is not set CONFIG_ARCH_HAVE_UART4=y +# CONFIG_ARCH_HAVE_UART5 is not set +# CONFIG_ARCH_HAVE_UART6 is not set CONFIG_ARCH_HAVE_UART7=y CONFIG_ARCH_HAVE_UART8=y +# CONFIG_ARCH_HAVE_SCI0 is not set +# CONFIG_ARCH_HAVE_SCI1 is not set +# CONFIG_ARCH_HAVE_USART0 is not set CONFIG_ARCH_HAVE_USART1=y CONFIG_ARCH_HAVE_USART2=y CONFIG_ARCH_HAVE_USART3=y +# CONFIG_ARCH_HAVE_USART4 is not set +# CONFIG_ARCH_HAVE_USART5 is not set CONFIG_ARCH_HAVE_USART6=y +# CONFIG_ARCH_HAVE_USART7 is not set +# CONFIG_ARCH_HAVE_USART8 is not set +# CONFIG_ARCH_HAVE_OTHER_UART is not set + +# +# USART Configuration +# +CONFIG_USART1_ISUART=y +CONFIG_USART2_ISUART=y +CONFIG_USART3_ISUART=y +CONFIG_USART6_ISUART=y CONFIG_MCU_SERIAL=y CONFIG_STANDARD_SERIAL=y CONFIG_SERIAL_NPOLLWAITERS=2 -# CONFIG_SERIAL_TIOCSERGSTRUCT is not set +CONFIG_SERIAL_IFLOWCONTROL=y +CONFIG_SERIAL_OFLOWCONTROL=y +CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y +CONFIG_SERIAL_IFLOWCONTROL_LOWER_WATERMARK=10 +CONFIG_SERIAL_IFLOWCONTROL_UPPER_WATERMARK=90 +CONFIG_ARCH_HAVE_SERIAL_TERMIOS=y +CONFIG_SERIAL_TERMIOS=y # CONFIG_USART1_SERIAL_CONSOLE is not set # CONFIG_USART2_SERIAL_CONSOLE is not set # CONFIG_USART3_SERIAL_CONSOLE is not set @@ -542,6 +729,7 @@ CONFIG_SERIAL_NPOLLWAITERS=2 # CONFIG_USART6_SERIAL_CONSOLE is not set CONFIG_UART7_SERIAL_CONSOLE=y # CONFIG_UART8_SERIAL_CONSOLE is not set +# CONFIG_OTHER_SERIAL_CONSOLE is not set # CONFIG_NO_SERIAL_CONSOLE is not set # @@ -627,8 +815,6 @@ CONFIG_UART8_PARITY=0 CONFIG_UART8_2STOP=0 # CONFIG_UART8_IFLOWCONTROL is not set # CONFIG_UART8_OFLOWCONTROL is not set -CONFIG_SERIAL_IFLOWCONTROL=y -CONFIG_SERIAL_OFLOWCONTROL=y CONFIG_USBDEV=y # @@ -640,6 +826,7 @@ CONFIG_USBDEV=y CONFIG_USBDEV_BUSPOWERED=y CONFIG_USBDEV_MAXPOWER=500 # CONFIG_USBDEV_DMA is not set +# CONFIG_ARCH_USBDEV_STALLQUEUE is not set # CONFIG_USBDEV_TRACE is not set # @@ -684,9 +871,16 @@ CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v2.x" # # Networking Support # +# CONFIG_ARCH_HAVE_NET is not set +# CONFIG_ARCH_HAVE_PHY is not set # CONFIG_NET is not set # +# Crypto API +# +# CONFIG_CRYPTO is not set + +# # File Systems # @@ -694,6 +888,13 @@ CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v2.x" # File system configuration # # CONFIG_DISABLE_MOUNTPOINT is not set +# CONFIG_FS_AUTOMOUNTER is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +CONFIG_FS_READABLE=y +CONFIG_FS_WRITABLE=y +# CONFIG_FS_AIO is not set +# CONFIG_FS_NAMED_SEMAPHORES is not set +CONFIG_FS_MQUEUE_MPATH="/var/mqueue" # CONFIG_FS_RAMMAP is not set CONFIG_FS_FAT=y CONFIG_FAT_LCNAMES=y @@ -702,6 +903,7 @@ CONFIG_FAT_MAXFNAME=32 CONFIG_FS_FATTIME=y CONFIG_FAT_DMAMEMORY=y CONFIG_FS_NXFFS=y +# CONFIG_NXFFS_SCAN_VOLUME is not set CONFIG_NXFFS_PREALLOCATED=y CONFIG_NXFFS_ERASEDSTATE=0xff CONFIG_NXFFS_PACKTHRESHOLD=32 @@ -710,12 +912,13 @@ CONFIG_NXFFS_TAILTHRESHOLD=2048 CONFIG_FS_ROMFS=y # CONFIG_FS_SMARTFS is not set CONFIG_FS_BINFS=y +# CONFIG_FS_PROCFS is not set # # System Logging # -# CONFIG_SYSLOG_ENABLE is not set # CONFIG_SYSLOG is not set +# CONFIG_SYSLOG_TIMESTAMP is not set # # Graphics Support @@ -725,13 +928,12 @@ CONFIG_FS_BINFS=y # # Memory Management # -# CONFIG_MM_MULTIHEAP is not set # CONFIG_MM_SMALL is not set CONFIG_MM_REGIONS=2 +# CONFIG_ARCH_HAVE_HEAP2 is not set CONFIG_GRAN=y # CONFIG_GRAN_SINGLE is not set -# CONFIG_GRAN_INTR is not set -# CONFIG_DEBUG_GRAN is not set +CONFIG_GRAN_INTR=y # # Audio Support @@ -739,7 +941,7 @@ CONFIG_GRAN=y # CONFIG_AUDIO is not set # -# Binary Formats +# Binary Loader # # CONFIG_BINFMT_DISABLE is not set # CONFIG_BINFMT_EXEPATH is not set @@ -762,6 +964,7 @@ CONFIG_NUNGET_CHARS=2 CONFIG_LIB_HOMEDIR="/" # CONFIG_NOPRINTF_FIELDWIDTH is not set CONFIG_LIBC_FLOATINGPOINT=y +# CONFIG_LIBC_IOCTL_VARIADIC is not set CONFIG_LIB_RAND_ORDER=1 # CONFIG_EOL_IS_CR is not set # CONFIG_EOL_IS_LF is not set @@ -773,7 +976,10 @@ CONFIG_TASK_SPAWN_DEFAULT_STACKSIZE=2048 CONFIG_LIBC_STRERROR=y # CONFIG_LIBC_STRERROR_SHORT is not set # CONFIG_LIBC_PERROR_STDOUT is not set +CONFIG_LIBC_TMPDIR="/tmp" +CONFIG_LIBC_MAX_TMPFILE=32 CONFIG_ARCH_LOWPUTC=y +# CONFIG_LIBC_LOCALTIME is not set CONFIG_LIB_SENDFILE_BUFSIZE=512 # CONFIG_ARCH_ROMGETC is not set CONFIG_ARCH_OPTIMIZED_FUNCTIONS=y @@ -793,15 +999,6 @@ CONFIG_ARCH_MEMCPY=y # # Non-standard Library Support # -CONFIG_SCHED_WORKQUEUE=y -CONFIG_SCHED_HPWORK=y -CONFIG_SCHED_WORKPRIORITY=192 -CONFIG_SCHED_WORKPERIOD=5000 -CONFIG_SCHED_WORKSTACKSIZE=1800 -CONFIG_SCHED_LPWORK=y -CONFIG_SCHED_LPWORKPRIORITY=50 -CONFIG_SCHED_LPWORKPERIOD=50000 -CONFIG_SCHED_LPWORKSTACKSIZE=1800 # CONFIG_LIB_KBDCODEC is not set # CONFIG_LIB_SLCDCODEC is not set @@ -832,8 +1029,8 @@ CONFIG_BUILTIN_PROXY_STACKSIZE=1024 # # CONFIG_EXAMPLES_BUTTONS is not set # CONFIG_EXAMPLES_CAN is not set -CONFIG_EXAMPLES_CDCACM=y -# CONFIG_EXAMPLES_COMPOSITE is not set +# CONFIG_EXAMPLES_CONFIGDATA is not set +# CONFIG_EXAMPLES_CPUHOG is not set # CONFIG_EXAMPLES_CXXTEST is not set # CONFIG_EXAMPLES_DHCPD is not set # CONFIG_EXAMPLES_ELF is not set @@ -845,15 +1042,20 @@ CONFIG_EXAMPLES_CDCACM=y # CONFIG_EXAMPLES_HIDKBD is not set # CONFIG_EXAMPLES_KEYPADTEST is not set # CONFIG_EXAMPLES_IGMP is not set -# CONFIG_EXAMPLES_LCDRW is not set # CONFIG_EXAMPLES_MM is not set # CONFIG_EXAMPLES_MODBUS is not set CONFIG_EXAMPLES_MOUNT=y +# CONFIG_EXAMPLES_MOUNT_BLOCKDEVICE is not set +CONFIG_EXAMPLES_MOUNT_NSECTORS=2048 +CONFIG_EXAMPLES_MOUNT_SECTORSIZE=512 +CONFIG_EXAMPLES_MOUNT_RAMDEVNO=0 +# CONFIG_EXAMPLES_MTDPART is not set # CONFIG_EXAMPLES_NRF24L01TERM is not set CONFIG_EXAMPLES_NSH=y +# CONFIG_EXAMPLES_NSH_CXXINITIALIZE is not set # CONFIG_EXAMPLES_NULL is not set # CONFIG_EXAMPLES_NX is not set -# CONFIG_EXAMPLES_NXCONSOLE is not set +# CONFIG_EXAMPLES_NXTERM is not set # CONFIG_EXAMPLES_NXFFS is not set # CONFIG_EXAMPLES_NXFLAT is not set # CONFIG_EXAMPLES_NXHELLO is not set @@ -861,13 +1063,14 @@ CONFIG_EXAMPLES_NSH=y # CONFIG_EXAMPLES_NXLINES is not set # CONFIG_EXAMPLES_NXTEXT is not set # CONFIG_EXAMPLES_OSTEST is not set -# CONFIG_EXAMPLES_PASHELLO is not set # CONFIG_EXAMPLES_PIPE is not set # CONFIG_EXAMPLES_POSIXSPAWN is not set # CONFIG_EXAMPLES_QENCODER is not set # CONFIG_EXAMPLES_RGMP is not set # CONFIG_EXAMPLES_ROMFS is not set # CONFIG_EXAMPLES_SENDMAIL is not set +# CONFIG_EXAMPLES_SERIALBLASTER is not set +# CONFIG_EXAMPLES_SERIALRX is not set # CONFIG_EXAMPLES_SERLOOP is not set # CONFIG_EXAMPLES_SLCD is not set # CONFIG_EXAMPLES_SMART_TEST is not set @@ -877,10 +1080,8 @@ CONFIG_EXAMPLES_NSH=y # CONFIG_EXAMPLES_THTTPD is not set # CONFIG_EXAMPLES_TIFF is not set # CONFIG_EXAMPLES_TOUCHSCREEN is not set -# CONFIG_EXAMPLES_UDP is not set -# CONFIG_EXAMPLES_UIP is not set +# CONFIG_EXAMPLES_WEBSERVER is not set # CONFIG_EXAMPLES_USBSERIAL is not set -# CONFIG_EXAMPLES_USBMSC is not set # CONFIG_EXAMPLES_USBTERM is not set # CONFIG_EXAMPLES_WATCHDOG is not set @@ -888,12 +1089,15 @@ CONFIG_EXAMPLES_NSH=y # Graphics Support # # CONFIG_TIFF is not set +# CONFIG_GRAPHICS_TRAVELER is not set # # Interpreters # +# CONFIG_INTERPRETERS_BAS is not set # CONFIG_INTERPRETERS_FICL is not set # CONFIG_INTERPRETERS_PCODE is not set +# CONFIG_INTERPRETERS_MICROPYTHON is not set # # Network Utilities @@ -903,17 +1107,14 @@ CONFIG_EXAMPLES_NSH=y # Networking Utilities # # CONFIG_NETUTILS_CODECS is not set -# CONFIG_NETUTILS_DHCPC is not set # CONFIG_NETUTILS_DHCPD is not set # CONFIG_NETUTILS_FTPC is not set # CONFIG_NETUTILS_FTPD is not set # CONFIG_NETUTILS_JSON is not set -# CONFIG_NETUTILS_RESOLV is not set # CONFIG_NETUTILS_SMTP is not set -# CONFIG_NETUTILS_TELNETD is not set # CONFIG_NETUTILS_TFTPC is not set # CONFIG_NETUTILS_THTTPD is not set -# CONFIG_NETUTILS_UIPLIB is not set +# CONFIG_NETUTILS_NETLIB is not set # CONFIG_NETUTILS_WEBCLIENT is not set # @@ -925,15 +1126,32 @@ CONFIG_EXAMPLES_NSH=y # NSH Library # CONFIG_NSH_LIBRARY=y + +# +# Command Line Configuration +# +CONFIG_NSH_READLINE=y +# CONFIG_NSH_CLE is not set +CONFIG_NSH_LINELEN=128 +# CONFIG_NSH_DISABLE_SEMICOLON is not set +CONFIG_NSH_CMDPARMS=y +CONFIG_NSH_MAXARGUMENTS=12 +CONFIG_NSH_ARGCAT=y +CONFIG_NSH_NESTDEPTH=8 +# CONFIG_NSH_DISABLEBG is not set CONFIG_NSH_BUILTIN_APPS=y # # Disable Individual commands # +# CONFIG_NSH_DISABLE_ADDROUTE is not set # CONFIG_NSH_DISABLE_CAT is not set # CONFIG_NSH_DISABLE_CD is not set # CONFIG_NSH_DISABLE_CP is not set +# CONFIG_NSH_DISABLE_CMP is not set # CONFIG_NSH_DISABLE_DD is not set +# CONFIG_NSH_DISABLE_DF is not set +# CONFIG_NSH_DISABLE_DELROUTE is not set # CONFIG_NSH_DISABLE_ECHO is not set # CONFIG_NSH_DISABLE_EXEC is not set # CONFIG_NSH_DISABLE_EXIT is not set @@ -953,9 +1171,7 @@ CONFIG_NSH_BUILTIN_APPS=y # CONFIG_NSH_DISABLE_MH is not set # CONFIG_NSH_DISABLE_MOUNT is not set # CONFIG_NSH_DISABLE_MW is not set -# CONFIG_NSH_DISABLE_NSFMOUNT is not set # CONFIG_NSH_DISABLE_PS is not set -# CONFIG_NSH_DISABLE_PING is not set # CONFIG_NSH_DISABLE_PUT is not set # CONFIG_NSH_DISABLE_PWD is not set # CONFIG_NSH_DISABLE_RM is not set @@ -975,32 +1191,36 @@ CONFIG_NSH_BUILTIN_APPS=y # # CONFIG_NSH_CMDOPT_DF_H is not set CONFIG_NSH_CODECS_BUFSIZE=128 +# CONFIG_NSH_CMDOPT_HEXDUMP is not set CONFIG_NSH_FILEIOSIZE=512 CONFIG_NSH_STRERROR=y -CONFIG_NSH_LINELEN=128 -CONFIG_NSH_MAXARGUMENTS=12 -CONFIG_NSH_NESTDEPTH=8 + +# +# Scripting Support +# # CONFIG_NSH_DISABLESCRIPT is not set -# CONFIG_NSH_DISABLEBG is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_LOOPS is not set CONFIG_NSH_ROMFSETC=y # CONFIG_NSH_ROMFSRC is not set CONFIG_NSH_ROMFSMOUNTPT="/etc" CONFIG_NSH_INITSCRIPT="init.d/rcS" CONFIG_NSH_ROMFSDEVNO=0 CONFIG_NSH_ROMFSSECTSIZE=128 +# CONFIG_NSH_DEFAULTROMFS is not set CONFIG_NSH_ARCHROMFS=y +# CONFIG_NSH_CUSTOMROMFS is not set CONFIG_NSH_FATDEVNO=1 CONFIG_NSH_FATSECTSIZE=512 CONFIG_NSH_FATNSECTORS=1024 CONFIG_NSH_FATMOUNTPT="/tmp" -CONFIG_NSH_CONSOLE=y -# CONFIG_NSH_USBCONSOLE is not set # -# USB Trace Support +# Console Configuration # -# CONFIG_NSH_USBDEV_TRACE is not set -# CONFIG_NSH_CONDEV is not set +CONFIG_NSH_CONSOLE=y +# CONFIG_NSH_USBCONSOLE is not set +# CONFIG_NSH_ALTCONDEV is not set CONFIG_NSH_ARCHINIT=y # @@ -1008,7 +1228,12 @@ CONFIG_NSH_ARCHINIT=y # # -# System NSH Add-Ons +# Platform-specific Support +# +# CONFIG_PLATFORM_CONFIGDATA is not set + +# +# System Libraries and NSH Add-Ons # # @@ -1017,9 +1242,18 @@ CONFIG_NSH_ARCHINIT=y # CONFIG_SYSTEM_FREE is not set # -# I2C tool +# EMACS-like Command Line Editor # -# CONFIG_SYSTEM_I2CTOOL is not set +# CONFIG_SYSTEM_CLE is not set + +# +# CU Minimal Terminal +# +CONFIG_SYSTEM_CUTERM=y +CONFIG_SYSTEM_CUTERM_DEFAULT_DEVICE="/dev/ttyS0" +CONFIG_SYSTEM_CUTERM_DEFAULT_BAUD=57600 +CONFIG_SYSTEM_CUTERM_STACKSIZE=2048 +CONFIG_SYSTEM_CUTERM_PRIORITY=100 # # FLASH Program Installation @@ -1032,12 +1266,44 @@ CONFIG_NSH_ARCHINIT=y # CONFIG_SYSTEM_FLASH_ERASEALL is not set # +# Intel HEX to binary conversion +# +# CONFIG_SYSTEM_HEX2BIN is not set + +# +# I2C tool +# +# CONFIG_SYSTEM_I2CTOOL is not set + +# +# INI File Parser +# +# CONFIG_SYSTEM_INIFILE is not set + +# +# NxPlayer media player library / command Line +# + +# +# RAM test +# +# CONFIG_SYSTEM_RAMTEST is not set + +# # readline() # CONFIG_SYSTEM_READLINE=y CONFIG_READLINE_ECHO=y # +# P-Code Support +# + +# +# PHY Tool +# + +# # Power Off # # CONFIG_SYSTEM_POWEROFF is not set @@ -1053,10 +1319,48 @@ CONFIG_READLINE_ECHO=y # CONFIG_SYSTEM_SDCARD is not set # +# Sudoku +# +# CONFIG_SYSTEM_SUDOKU is not set + +# # Sysinfo # CONFIG_SYSTEM_SYSINFO=y +CONFIG_SYSTEM_SYSINFO_STACKSIZE=1024 + +# +# Temperature +# + +# +# VI Work-Alike Editor +# +# CONFIG_SYSTEM_VI is not set + +# +# Stack Monitor +# + +# +# USB CDC/ACM Device Commands +# +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_CDCACM_DEVMINOR=0 + +# +# USB Composite Device Commands +# + +# +# USB Mass Storage Device Commands +# # # USB Monitor # + +# +# Zmodem Commands +# +# CONFIG_SYSTEM_ZMODEM is not set diff --git a/nuttx-configs/px4fmu-v2/nsh/setenv.sh b/nuttx-configs/px4fmu-v2/nsh/setenv.sh index 265520997..01f62bbe1 100755 --- a/nuttx-configs/px4fmu-v2/nsh/setenv.sh +++ b/nuttx-configs/px4fmu-v2/nsh/setenv.sh @@ -1,7 +1,7 @@ #!/bin/bash -# configs/stm3240g-eval/nsh/setenv.sh +# configs/px4fmu-v2_upstream/nsh/setenv.sh # -# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Copyright (C) 2014 Gregory Nutt. All rights reserved. # Author: Gregory Nutt <gnutt@nuttx.org> # # Redistribution and use in source and binary forms, with or without @@ -47,15 +47,11 @@ if [ -z "${PATH_ORIG}" ]; then export PATH_ORIG="${PATH}" fi -# This the Cygwin path to the location where I installed the RIDE -# toolchain under windows. You will also have to edit this if you install -# the RIDE toolchain in any other location -#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/Raisonance/Ride/arm-gcc/bin" - # This the Cygwin path to the location where I installed the CodeSourcery # toolchain under windows. You will also have to edit this if you install # the CodeSourcery toolchain in any other location -export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery G++ Lite/bin" +#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery G++ Lite/bin" +export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery_CodeBench_Lite_for_ARM_EABI/bin # This the Cygwin path to the location where I build the buildroot # toolchain. diff --git a/nuttx-configs/px4fmu-v2/scripts/ld.script b/nuttx-configs/px4fmu-v2/scripts/ld.script index bec896d1c..c78db47e2 100644 --- a/nuttx-configs/px4fmu-v2/scripts/ld.script +++ b/nuttx-configs/px4fmu-v2/scripts/ld.script @@ -1,5 +1,5 @@ /**************************************************************************** - * configs/px4fmu/common/ld.script + * configs/px4fmu-v2_upstream/common/ld.script * * Copyright (C) 2011 Gregory Nutt. All rights reserved. * Author: Gregory Nutt <gnutt@nuttx.org> @@ -61,7 +61,7 @@ OUTPUT_ARCH(arm) ENTRY(__start) /* treat __start as the anchor for dead code stripping */ EXTERN(_vectors) /* force the vectors to be included in the output */ -/* +/* * Ensure that abort() is present in the final object. The exception handling * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). */ @@ -72,17 +72,17 @@ SECTIONS .text : { _stext = ABSOLUTE(.); *(.vectors) - *(.text .text.*) + *(.text .text.*) *(.fixup) *(.gnu.warning) - *(.rodata .rodata.*) + *(.rodata .rodata.*) *(.gnu.linkonce.t.*) *(.got) *(.gcc_except_table) *(.gnu.linkonce.r.*) _etext = ABSOLUTE(.); - /* + /* * This is a hack to make the newlib libm __errno() call * use the NuttX get_errno_ptr() function. */ diff --git a/nuttx-configs/px4fmu-v2/src/Makefile b/nuttx-configs/px4fmu-v2/src/Makefile index d4276f7fc..35ad52a2d 100644 --- a/nuttx-configs/px4fmu-v2/src/Makefile +++ b/nuttx-configs/px4fmu-v2/src/Makefile @@ -1,5 +1,5 @@ ############################################################################ -# configs/px4fmu/src/Makefile +# configs/px4fmu-v2_upstream/src/Makefile # # Copyright (C) 2011 Gregory Nutt. All rights reserved. # Author: Gregory Nutt <gnutt@nuttx.org> @@ -35,24 +35,24 @@ -include $(TOPDIR)/Make.defs -CFLAGS += -I$(TOPDIR)/sched +CFLAGS += -I$(TOPDIR)/sched -ASRCS = -AOBJS = $(ASRCS:.S=$(OBJEXT)) +ASRCS = +AOBJS = $(ASRCS:.S=$(OBJEXT)) -CSRCS = empty.c -COBJS = $(CSRCS:.c=$(OBJEXT)) +CSRCS = empty.c +COBJS = $(CSRCS:.c=$(OBJEXT)) -SRCS = $(ASRCS) $(CSRCS) -OBJS = $(AOBJS) $(COBJS) +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) -ARCH_SRCDIR = $(TOPDIR)/arch/$(CONFIG_ARCH)/src +ARCH_SRCDIR = $(TOPDIR)/arch/$(CONFIG_ARCH)/src ifeq ($(WINTOOL),y) - CFLAGS += -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \ - -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \ - -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}" + CFLAGS += -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}" else - CFLAGS += -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m + CFLAGS += -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m endif all: libboard$(LIBEXT) @@ -81,4 +81,3 @@ distclean: clean $(call DELFILE, .depend) -include Make.dep - diff --git a/nuttx-configs/px4io-v1/nsh/Make.defs b/nuttx-configs/px4io-v1/nsh/Make.defs index 74f38c0cb..09a36ea6d 100644 --- a/nuttx-configs/px4io-v1/nsh/Make.defs +++ b/nuttx-configs/px4io-v1/nsh/Make.defs @@ -35,6 +35,7 @@ include ${TOPDIR}/.config include ${TOPDIR}/tools/Config.mk +include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs # # We only support building with the ARM bare-metal toolchain from diff --git a/nuttx-configs/px4io-v1/nsh/defconfig b/nuttx-configs/px4io-v1/nsh/defconfig index 7c76be7ec..10a278801 100755 --- a/nuttx-configs/px4io-v1/nsh/defconfig +++ b/nuttx-configs/px4io-v1/nsh/defconfig @@ -1,538 +1,979 @@ -############################################################################ -# configs/px4io-v1/nsh/defconfig -# -# Copyright (C) 2012 PX4 Development Team. All rights reserved. -# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt <gnutt@nuttx.org> -# -# 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 NuttX 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. -# -############################################################################ -# -# architecture selection -# -# CONFIG_ARCH - identifies the arch subdirectory and, hence, the -# processor architecture. -# CONFIG_ARCH_family - for use in C code. This identifies the -# particular chip family that the architecture is implemented -# in. -# CONFIG_ARCH_architecture - for use in C code. This identifies the -# specific architecture within the chip family. -# CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory -# CONFIG_ARCH_CHIP_name - For use in C code -# CONFIG_ARCH_BOARD - identifies the configs subdirectory and, hence, -# the board that supports the particular chip or SoC. -# CONFIG_ARCH_BOARD_name - for use in C code -# CONFIG_ENDIAN_BIG - define if big endian (default is little endian) -# CONFIG_BOARD_LOOPSPERMSEC - for delay loops -# CONFIG_DRAM_SIZE - Describes the installed DRAM. -# CONFIG_DRAM_START - The start address of DRAM (physical) -# CONFIG_ARCH_IRQPRIO - The ST32F100CB supports interrupt prioritization -# CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt -# stack. If defined, this symbol is the size of the interrupt -# stack in bytes. If not defined, the user task stacks will be -# used during interrupt handling. -# CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions -# CONFIG_ARCH_BOOTLOADER - Set if you are using a bootloader. -# CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture. -# CONFIG_ARCH_BUTTONS - Enable support for buttons. Unique to board architecture. -# CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that -# cause a 100 second delay during boot-up. This 100 second delay -# serves no purpose other than it allows you to calibrate -# CONFIG_BOARD_LOOPSPERMSEC. You simply use a stop watch to measure -# the 100 second delay then adjust CONFIG_BOARD_LOOPSPERMSEC until -# the delay actually is 100 seconds. -# CONFIG_ARCH_DMA - Support DMA initialization # -CONFIG_ARCH="arm" -CONFIG_ARCH_ARM=y -CONFIG_ARCH_CORTEXM3=y -CONFIG_ARCH_CHIP="stm32" -CONFIG_ARCH_CHIP_STM32F100C8=y +# Automatically generated file; DO NOT EDIT. +# Nuttx/ Configuration # -# Board Selection + # -CONFIG_ARCH_BOARD_PX4IO_V1=y -# CONFIG_ARCH_BOARD_CUSTOM is not set -CONFIG_ARCH_BOARD="px4io-v1" -CONFIG_BOARD_LOOPSPERMSEC=2000 -CONFIG_DRAM_SIZE=0x00002000 -CONFIG_DRAM_START=0x20000000 -CONFIG_ARCH_INTERRUPTSTACK=n -CONFIG_ARCH_STACKDUMP=y -CONFIG_ARCH_BOOTLOADER=n -CONFIG_ARCH_LEDS=n -CONFIG_ARCH_BUTTONS=n -CONFIG_ARCH_CALIBRATION=n -CONFIG_ARCH_DMA=y +# Build Setup +# +# CONFIG_EXPERIMENTAL is not set +# CONFIG_DEFAULT_SMALL is not set +CONFIG_HOST_LINUX=y +# CONFIG_HOST_OSX is not set +# CONFIG_HOST_WINDOWS is not set +# CONFIG_HOST_OTHER is not set + +# +# Build Configuration +# +CONFIG_APPS_DIR="../apps" +CONFIG_BUILD_FLAT=y +# CONFIG_BUILD_2PASS is not set + +# +# Binary Output Formats +# +# CONFIG_RRLOAD_BINARY is not set +# CONFIG_INTELHEX_BINARY is not set +# CONFIG_MOTOROLA_SREC is not set +CONFIG_RAW_BINARY=y +# CONFIG_UBOOT_UIMAGE is not set + +# +# Customize Header Files +# +# CONFIG_ARCH_STDINT_H is not set +# CONFIG_ARCH_STDBOOL_H is not set CONFIG_ARCH_MATH_H=y +# CONFIG_ARCH_FLOAT_H is not set +# CONFIG_ARCH_STDARG_H is not set -CONFIG_ARMV7M_CMNVECTOR=y -# CONFIG_ARMV7M_STACKCHECK is not set +# +# Debug Options +# +# CONFIG_DEBUG is not set +CONFIG_ARCH_HAVE_STACKCHECK=y +CONFIG_ARCH_HAVE_HEAPCHECK=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_ARCH_HAVE_CUSTOMOPT=y +# CONFIG_DEBUG_NOOPT is not set +# CONFIG_DEBUG_CUSTOMOPT is not set +CONFIG_DEBUG_FULLOPT=y # -# JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): +# System Type # -# CONFIG_STM32_DFU - Use the DFU bootloader, not JTAG +CONFIG_ARCH_ARM=y +# CONFIG_ARCH_AVR is not set +# CONFIG_ARCH_HC is not set +# CONFIG_ARCH_MIPS is not set +# CONFIG_ARCH_RGMP is not set +# CONFIG_ARCH_SH is not set +# CONFIG_ARCH_SIM is not set +# CONFIG_ARCH_X86 is not set +# CONFIG_ARCH_Z16 is not set +# CONFIG_ARCH_Z80 is not set +CONFIG_ARCH="arm" + # -# JTAG Enable options: +# ARM Options +# +# CONFIG_ARCH_CHIP_A1X is not set +# CONFIG_ARCH_CHIP_C5471 is not set +# CONFIG_ARCH_CHIP_CALYPSO is not set +# CONFIG_ARCH_CHIP_DM320 is not set +# CONFIG_ARCH_CHIP_EFM32 is not set +# CONFIG_ARCH_CHIP_IMX is not set +# CONFIG_ARCH_CHIP_KINETIS is not set +# CONFIG_ARCH_CHIP_KL is not set +# CONFIG_ARCH_CHIP_LM is not set +# CONFIG_ARCH_CHIP_TIVA is not set +# CONFIG_ARCH_CHIP_LPC17XX is not set +# CONFIG_ARCH_CHIP_LPC214X is not set +# CONFIG_ARCH_CHIP_LPC2378 is not set +# CONFIG_ARCH_CHIP_LPC31XX is not set +# CONFIG_ARCH_CHIP_LPC43XX is not set +# CONFIG_ARCH_CHIP_NUC1XX is not set +# CONFIG_ARCH_CHIP_SAMA5 is not set +# CONFIG_ARCH_CHIP_SAMD is not set +# CONFIG_ARCH_CHIP_SAM34 is not set +CONFIG_ARCH_CHIP_STM32=y +# CONFIG_ARCH_CHIP_STR71X is not set +# CONFIG_ARCH_ARM7TDMI is not set +# CONFIG_ARCH_ARM926EJS is not set +# CONFIG_ARCH_ARM920T is not set +# CONFIG_ARCH_CORTEXM0 is not set +CONFIG_ARCH_CORTEXM3=y +# CONFIG_ARCH_CORTEXM4 is not set +# CONFIG_ARCH_CORTEXA5 is not set +# CONFIG_ARCH_CORTEXA8 is not set +CONFIG_ARCH_FAMILY="armv7-m" +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARCH_HAVE_CMNVECTOR=y +CONFIG_ARMV7M_CMNVECTOR=y +# CONFIG_ARCH_HAVE_FPU is not set +# CONFIG_ARMV7M_MPU is not set + # -# CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) -# CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) -# but without JNTRST. -# CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled +# ARMV7M Configuration Options # -CONFIG_STM32_DFU=n -CONFIG_STM32_JTAG_FULL_ENABLE=y -CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n -CONFIG_STM32_JTAG_SW_ENABLE=n +# CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set +# CONFIG_ARMV7M_TOOLCHAIN_CODEREDL is not set +# CONFIG_ARMV7M_TOOLCHAIN_CODESOURCERYL is not set +CONFIG_ARMV7M_TOOLCHAIN_GNU_EABIL=y +# CONFIG_ARMV7M_STACKCHECK is not set +# CONFIG_ARMV7M_ITMSYSLOG is not set # -# Individual subsystems can be enabled: +# STM32 Configuration Options +# +# CONFIG_ARCH_CHIP_STM32L151C6 is not set +# CONFIG_ARCH_CHIP_STM32L151C8 is not set +# CONFIG_ARCH_CHIP_STM32L151CB is not set +# CONFIG_ARCH_CHIP_STM32L151R6 is not set +# CONFIG_ARCH_CHIP_STM32L151R8 is not set +# CONFIG_ARCH_CHIP_STM32L151RB is not set +# CONFIG_ARCH_CHIP_STM32L151V6 is not set +# CONFIG_ARCH_CHIP_STM32L151V8 is not set +# CONFIG_ARCH_CHIP_STM32L151VB is not set +# CONFIG_ARCH_CHIP_STM32L152C6 is not set +# CONFIG_ARCH_CHIP_STM32L152C8 is not set +# CONFIG_ARCH_CHIP_STM32L152CB is not set +# CONFIG_ARCH_CHIP_STM32L152R6 is not set +# CONFIG_ARCH_CHIP_STM32L152R8 is not set +# CONFIG_ARCH_CHIP_STM32L152RB is not set +# CONFIG_ARCH_CHIP_STM32L152V6 is not set +# CONFIG_ARCH_CHIP_STM32L152V8 is not set +# CONFIG_ARCH_CHIP_STM32L152VB is not set +# CONFIG_ARCH_CHIP_STM32L162ZD is not set +CONFIG_ARCH_CHIP_STM32F100C8=y +# CONFIG_ARCH_CHIP_STM32F100CB is not set +# CONFIG_ARCH_CHIP_STM32F100R8 is not set +# CONFIG_ARCH_CHIP_STM32F100RB is not set +# CONFIG_ARCH_CHIP_STM32F100RC is not set +# CONFIG_ARCH_CHIP_STM32F100RD is not set +# CONFIG_ARCH_CHIP_STM32F100RE is not set +# CONFIG_ARCH_CHIP_STM32F100V8 is not set +# CONFIG_ARCH_CHIP_STM32F100VB is not set +# CONFIG_ARCH_CHIP_STM32F100VC is not set +# CONFIG_ARCH_CHIP_STM32F100VD is not set +# CONFIG_ARCH_CHIP_STM32F100VE is not set +# CONFIG_ARCH_CHIP_STM32F102CB is not set +# CONFIG_ARCH_CHIP_STM32F103T8 is not set +# CONFIG_ARCH_CHIP_STM32F103TB is not set +# CONFIG_ARCH_CHIP_STM32F103C4 is not set +# CONFIG_ARCH_CHIP_STM32F103C8 is not set +# CONFIG_ARCH_CHIP_STM32F103CB is not set +# CONFIG_ARCH_CHIP_STM32F103R8 is not set +# CONFIG_ARCH_CHIP_STM32F103RB is not set +# CONFIG_ARCH_CHIP_STM32F103RC is not set +# CONFIG_ARCH_CHIP_STM32F103RD is not set +# CONFIG_ARCH_CHIP_STM32F103RE is not set +# CONFIG_ARCH_CHIP_STM32F103RG is not set +# CONFIG_ARCH_CHIP_STM32F103V8 is not set +# CONFIG_ARCH_CHIP_STM32F103VB is not set +# CONFIG_ARCH_CHIP_STM32F103VC is not set +# CONFIG_ARCH_CHIP_STM32F103VE is not set +# CONFIG_ARCH_CHIP_STM32F103ZE is not set +# CONFIG_ARCH_CHIP_STM32F105VB is not set +# CONFIG_ARCH_CHIP_STM32F107VC is not set +# CONFIG_ARCH_CHIP_STM32F207IG is not set +# CONFIG_ARCH_CHIP_STM32F207ZE is not set +# CONFIG_ARCH_CHIP_STM32F302CB is not set +# CONFIG_ARCH_CHIP_STM32F302CC is not set +# CONFIG_ARCH_CHIP_STM32F302RB is not set +# CONFIG_ARCH_CHIP_STM32F302RC is not set +# CONFIG_ARCH_CHIP_STM32F302VB is not set +# CONFIG_ARCH_CHIP_STM32F302VC is not set +# CONFIG_ARCH_CHIP_STM32F303CB is not set +# CONFIG_ARCH_CHIP_STM32F303CC is not set +# CONFIG_ARCH_CHIP_STM32F303RB is not set +# CONFIG_ARCH_CHIP_STM32F303RC is not set +# CONFIG_ARCH_CHIP_STM32F303VB is not set +# CONFIG_ARCH_CHIP_STM32F303VC is not set +# CONFIG_ARCH_CHIP_STM32F401RE is not set +# CONFIG_ARCH_CHIP_STM32F411RE is not set +# CONFIG_ARCH_CHIP_STM32F405RG is not set +# CONFIG_ARCH_CHIP_STM32F405VG is not set +# CONFIG_ARCH_CHIP_STM32F405ZG is not set +# CONFIG_ARCH_CHIP_STM32F407VE is not set +# CONFIG_ARCH_CHIP_STM32F407VG is not set +# CONFIG_ARCH_CHIP_STM32F407ZE is not set +# CONFIG_ARCH_CHIP_STM32F407ZG is not set +# CONFIG_ARCH_CHIP_STM32F407IE is not set +# CONFIG_ARCH_CHIP_STM32F407IG is not set +# CONFIG_ARCH_CHIP_STM32F427V is not set +# CONFIG_ARCH_CHIP_STM32F427Z is not set +# CONFIG_ARCH_CHIP_STM32F427I is not set +# CONFIG_ARCH_CHIP_STM32F429V is not set +# CONFIG_ARCH_CHIP_STM32F429Z is not set +# CONFIG_ARCH_CHIP_STM32F429I is not set +# CONFIG_ARCH_CHIP_STM32F429B is not set +# CONFIG_ARCH_CHIP_STM32F429N is not set +# CONFIG_STM32_STM32L15XX is not set +# CONFIG_STM32_ENERGYLITE is not set +CONFIG_STM32_STM32F10XX=y +CONFIG_STM32_VALUELINE=y +# CONFIG_STM32_CONNECTIVITYLINE is not set +# CONFIG_STM32_PERFORMANCELINE is not set +# CONFIG_STM32_USBACCESSLINE is not set +# CONFIG_STM32_HIGHDENSITY is not set +# CONFIG_STM32_MEDIUMDENSITY is not set +# CONFIG_STM32_LOWDENSITY is not set +# CONFIG_STM32_STM32F20XX is not set +# CONFIG_STM32_STM32F207 is not set +# CONFIG_STM32_STM32F30XX is not set +# CONFIG_STM32_STM32F40XX is not set +# CONFIG_STM32_STM32F401 is not set +# CONFIG_STM32_STM32F411 is not set +# CONFIG_STM32_STM32F405 is not set +# CONFIG_STM32_STM32F407 is not set +# CONFIG_STM32_STM32F427 is not set +# CONFIG_STM32_STM32F429 is not set + # -# AHB: +# STM32 Peripheral Support +# +# CONFIG_STM32_HAVE_CCM is not set +# CONFIG_STM32_HAVE_USBDEV is not set +# CONFIG_STM32_HAVE_OTGFS is not set +# CONFIG_STM32_HAVE_FSMC is not set +CONFIG_STM32_HAVE_USART3=y +CONFIG_STM32_HAVE_UART4=y +CONFIG_STM32_HAVE_UART5=y +# CONFIG_STM32_HAVE_USART6 is not set +# CONFIG_STM32_HAVE_UART7 is not set +# CONFIG_STM32_HAVE_UART8 is not set +CONFIG_STM32_HAVE_TIM1=y +CONFIG_STM32_HAVE_TIM5=y +CONFIG_STM32_HAVE_TIM6=y +CONFIG_STM32_HAVE_TIM7=y +# CONFIG_STM32_HAVE_TIM8 is not set +# CONFIG_STM32_HAVE_TIM9 is not set +# CONFIG_STM32_HAVE_TIM10 is not set +# CONFIG_STM32_HAVE_TIM11 is not set +CONFIG_STM32_HAVE_TIM12=y +CONFIG_STM32_HAVE_TIM13=y +CONFIG_STM32_HAVE_TIM14=y +CONFIG_STM32_HAVE_TIM15=y +CONFIG_STM32_HAVE_TIM16=y +CONFIG_STM32_HAVE_TIM17=y +CONFIG_STM32_HAVE_ADC2=y +# CONFIG_STM32_HAVE_ADC3 is not set +# CONFIG_STM32_HAVE_ADC4 is not set +# CONFIG_STM32_HAVE_CAN1 is not set +# CONFIG_STM32_HAVE_CAN2 is not set +# CONFIG_STM32_HAVE_RNG is not set +# CONFIG_STM32_HAVE_ETHMAC is not set +# CONFIG_STM32_HAVE_SPI2 is not set +# CONFIG_STM32_HAVE_SPI3 is not set +# CONFIG_STM32_HAVE_SPI4 is not set +# CONFIG_STM32_HAVE_SPI5 is not set +CONFIG_STM32_ADC1=y +# CONFIG_STM32_ADC2 is not set +# CONFIG_STM32_BKP is not set +# CONFIG_STM32_CEC is not set +# CONFIG_STM32_CRC is not set CONFIG_STM32_DMA1=y -CONFIG_STM32_DMA2=n -CONFIG_STM32_CRC=n -# APB1: -# Timers 2,3 and 4 are owned by the PWM driver -CONFIG_STM32_TIM2=n -CONFIG_STM32_TIM3=n -CONFIG_STM32_TIM4=n -CONFIG_STM32_TIM5=n -CONFIG_STM32_TIM6=n -CONFIG_STM32_TIM7=n -CONFIG_STM32_WWDG=n -CONFIG_STM32_SPI2=n +# CONFIG_STM32_DAC1 is not set +# CONFIG_STM32_DAC2 is not set +CONFIG_STM32_I2C1=y +# CONFIG_STM32_I2C2 is not set +# CONFIG_STM32_PWR is not set +# CONFIG_STM32_SPI1 is not set +# CONFIG_STM32_TIM1 is not set +# CONFIG_STM32_TIM2 is not set +# CONFIG_STM32_TIM3 is not set +# CONFIG_STM32_TIM4 is not set +# CONFIG_STM32_TIM5 is not set +# CONFIG_STM32_TIM6 is not set +# CONFIG_STM32_TIM7 is not set +# CONFIG_STM32_TIM12 is not set +# CONFIG_STM32_TIM13 is not set +# CONFIG_STM32_TIM14 is not set +# CONFIG_STM32_TIM15 is not set +# CONFIG_STM32_TIM16 is not set +# CONFIG_STM32_TIM17 is not set +CONFIG_STM32_USART1=y CONFIG_STM32_USART2=y CONFIG_STM32_USART3=y -CONFIG_STM32_I2C1=y -CONFIG_STM32_I2C2=n +# CONFIG_STM32_UART4 is not set +# CONFIG_STM32_UART5 is not set +# CONFIG_STM32_IWDG is not set +# CONFIG_STM32_WWDG is not set +CONFIG_STM32_ADC=y +CONFIG_STM32_I2C=y + +# +# Alternate Pin Mapping +# +# CONFIG_STM32_I2C1_REMAP is not set +# CONFIG_STM32_USART1_REMAP is not set +# CONFIG_STM32_USART2_REMAP is not set +CONFIG_STM32_USART3_NO_REMAP=y +# CONFIG_STM32_USART3_FULL_REMAP is not set +# CONFIG_STM32_USART3_PARTIAL_REMAP is not set +# CONFIG_STM32_JTAG_DISABLE is not set +# CONFIG_STM32_JTAG_FULL_ENABLE is not set +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set +CONFIG_STM32_JTAG_SW_ENABLE=y +CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y +# CONFIG_STM32_FORCEPOWER is not set +# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set +# CONFIG_STM32_DMACAPABLE is not set +CONFIG_STM32_USART=y + +# +# U[S]ART Configuration +# +# CONFIG_USART1_RS485 is not set +CONFIG_USART1_RXDMA=y +# CONFIG_USART2_RS485 is not set +# CONFIG_USART2_RXDMA is not set +# CONFIG_USART3_RS485 is not set +CONFIG_USART3_RXDMA=y +# CONFIG_SERIAL_DISABLE_REORDERING is not set +# CONFIG_STM32_FLOWCONTROL_BROKEN is not set +CONFIG_STM32_USART_SINGLEWIRE=y + +# +# I2C Configuration +# +# CONFIG_STM32_I2C_ALT is not set +# CONFIG_STM32_I2C_DYNTIMEO is not set CONFIG_STM32_I2CTIMEOSEC=0 CONFIG_STM32_I2CTIMEOMS=1 -CONFIG_STM32_BKP=n -CONFIG_STM32_PWR=n -CONFIG_STM32_DAC=n -# APB2: -# We use our own ADC driver, but leave this on for clocking purposes. -CONFIG_STM32_ADC1=y -CONFIG_STM32_ADC2=n -# TIM1 is owned by the HRT -CONFIG_STM32_TIM1=n -CONFIG_STM32_SPI1=n -CONFIG_STM32_TIM8=n -CONFIG_STM32_USART1=y -CONFIG_STM32_ADC3=n +CONFIG_STM32_I2CTIMEOTICKS=1 +# CONFIG_STM32_I2C_DUTY16_9 is not set +# +# USB FS Host Configuration +# # -# STM32F100 specific serial device driver settings +# USB HS Host Configuration # -# CONFIG_USARTn_SERIAL_CONSOLE - selects the USARTn for the -# console and ttys0 (default is the USART1). -# CONFIG_USARTn_RXBUFSIZE - Characters are buffered as received. -# This specific the size of the receive buffer -# CONFIG_USARTn_TXBUFSIZE - Characters are buffered before -# being sent. This specific the size of the transmit buffer -# CONFIG_USARTn_BAUD - The configure BAUD of the UART. Must be -# CONFIG_USARTn_BITS - The number of bits. Must be either 7 or 8. -# CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity -# CONFIG_USARTn_2STOP - Two stop bits + +# +# USB Host Debug Configuration # -CONFIG_SERIAL_TERMIOS=y -CONFIG_STANDARD_SERIAL=y -CONFIG_USART1_SERIAL_CONSOLE=y -CONFIG_USART2_SERIAL_CONSOLE=n -CONFIG_USART3_SERIAL_CONSOLE=n +# +# USB Device Configuration +# -CONFIG_USART1_TXBUFSIZE=64 -CONFIG_USART2_TXBUFSIZE=64 -CONFIG_USART3_TXBUFSIZE=64 +# +# Architecture Options +# +# CONFIG_ARCH_NOINTC is not set +# CONFIG_ARCH_VECNOTIRQ is not set +CONFIG_ARCH_DMA=y +CONFIG_ARCH_HAVE_IRQPRIO=y +# CONFIG_ARCH_L2CACHE is not set +# CONFIG_ARCH_HAVE_COHERENT_DCACHE is not set +# CONFIG_ARCH_HAVE_ADDRENV is not set +# CONFIG_ARCH_NEED_ADDRENV_MAPPING is not set +CONFIG_ARCH_HAVE_VFORK=y +# CONFIG_ARCH_HAVE_MMU is not set +CONFIG_ARCH_HAVE_MPU=y +# CONFIG_ARCH_NAND_HWECC is not set +# CONFIG_ARCH_HAVE_EXTCLK is not set +# CONFIG_ARCH_USE_MPU is not set +# CONFIG_ARCH_IRQPRIO is not set +CONFIG_ARCH_STACKDUMP=y +CONFIG_STACK_COLORATION=y +# CONFIG_ENDIAN_BIG is not set +# CONFIG_ARCH_IDLE_CUSTOM is not set +# CONFIG_ARCH_HAVE_RAMFUNCS is not set +CONFIG_ARCH_HAVE_RAMVECTORS=y +# CONFIG_ARCH_RAMVECTORS is not set -CONFIG_USART1_RXBUFSIZE=64 -CONFIG_USART2_RXBUFSIZE=64 -CONFIG_USART3_RXBUFSIZE=64 +# +# Board Settings +# +CONFIG_BOARD_LOOPSPERMSEC=2000 +# CONFIG_ARCH_CALIBRATION is not set -CONFIG_USART1_BAUD=115200 -CONFIG_USART2_BAUD=115200 -CONFIG_USART3_BAUD=115200 +# +# Interrupt options +# +CONFIG_ARCH_HAVE_INTERRUPTSTACK=y +CONFIG_ARCH_INTERRUPTSTACK=0 +CONFIG_ARCH_HAVE_HIPRI_INTERRUPT=y +# CONFIG_ARCH_HIPRI_INTERRUPT is not set -CONFIG_USART1_BITS=8 -CONFIG_USART2_BITS=8 -CONFIG_USART3_BITS=8 +# +# Boot options +# +# CONFIG_BOOT_RUNFROMEXTSRAM is not set +CONFIG_BOOT_RUNFROMFLASH=y +# CONFIG_BOOT_RUNFROMISRAM is not set +# CONFIG_BOOT_RUNFROMSDRAM is not set +# CONFIG_BOOT_COPYTORAM is not set -CONFIG_USART1_PARITY=0 -CONFIG_USART2_PARITY=0 -CONFIG_USART3_PARITY=0 +# +# Boot Memory Configuration +# +CONFIG_RAM_START=0x20000000 +CONFIG_RAM_SIZE=8192 +# CONFIG_ARCH_HAVE_SDRAM is not set -CONFIG_USART1_2STOP=0 -CONFIG_USART2_2STOP=0 -CONFIG_USART3_2STOP=0 +# +# Board Selection +# +CONFIG_ARCH_BOARD_PX4IO_V1=y +# CONFIG_ARCH_BOARD_CUSTOM is not set +CONFIG_ARCH_BOARD="px4io-v1" -CONFIG_USART1_RXDMA=y -SERIAL_HAVE_CONSOLE_DMA=y -# Conflicts with I2C1 DMA -CONFIG_USART2_RXDMA=n -CONFIG_USART3_RXDMA=y +# +# Custom Board Configuration +# +CONFIG_ARCH_BOARD_CUSTOM_DIR="" +# CONFIG_BOARD_CUSTOM_LEDS is not set +# CONFIG_BOARD_CUSTOM_BUTTONS is not set # -# General build options -# -# CONFIG_RRLOAD_BINARY - make the rrload binary format used with -# BSPs from www.ridgerun.com using the tools/mkimage.sh script -# CONFIG_INTELHEX_BINARY - make the Intel HEX binary format -# used with many different loaders using the GNU objcopy program -# Should not be selected if you are not using the GNU toolchain. -# CONFIG_MOTOROLA_SREC - make the Motorola S-Record binary format -# used with many different loaders using the GNU objcopy program -# Should not be selected if you are not using the GNU toolchain. -# CONFIG_RAW_BINARY - make a raw binary format file used with many -# different loaders using the GNU objcopy program. This option -# should not be selected if you are not using the GNU toolchain. -# CONFIG_HAVE_LIBM - toolchain supports libm.a -# -CONFIG_RRLOAD_BINARY=n -CONFIG_INTELHEX_BINARY=n -CONFIG_MOTOROLA_SREC=n -CONFIG_RAW_BINARY=y -CONFIG_HAVE_LIBM=n - -# -# General OS setup -# -# CONFIG_APPS_DIR - Identifies the relative path to the directory -# that builds the application to link with NuttX. Default: ../apps -# CONFIG_DEBUG - enables built-in debug options -# CONFIG_DEBUG_VERBOSE - enables verbose debug output -# CONFIG_DEBUG_SYMBOLS - build without optimization and with -# debug symbols (needed for use with a debugger). -# CONFIG_HAVE_CXX - Enable support for C++ -# CONFIG_HAVE_CXXINITIALIZE - The platform-specific logic includes support -# for initialization of static C++ instances for this architecture -# and for the selected toolchain (via up_cxxinitialize()). -# CONFIG_MM_REGIONS - If the architecture includes multiple -# regions of memory to allocate from, this specifies the -# number of memory regions that the memory manager must -# handle and enables the API mm_addregion(start, end); -# CONFIG_ARCH_LOWPUTC - architecture supports low-level, boot -# time console output -# CONFIG_MSEC_PER_TICK - The default system timer is 100Hz -# or MSEC_PER_TICK=10. This setting may be defined to -# inform NuttX that the processor hardware is providing -# system timer interrupts at some interrupt interval other -# than 10 msec. -# CONFIG_RR_INTERVAL - The round robin timeslice will be set -# this number of milliseconds; Round robin scheduling can -# be disabled by setting this value to zero. -# CONFIG_SCHED_INSTRUMENTATION - enables instrumentation in -# scheduler to monitor system performance -# CONFIG_TASK_NAME_SIZE - Spcifies that maximum size of a -# task name to save in the TCB. Useful if scheduler -# instrumentation is selected. Set to zero to disable. -# CONFIG_START_YEAR, CONFIG_START_MONTH, CONFIG_START_DAY - -# Used to initialize the internal time logic. -# CONFIG_GREGORIAN_TIME - Enables Gregorian time conversions. -# You would only need this if you are concerned about accurate -# time conversions in the past or in the distant future. -# CONFIG_JULIAN_TIME - Enables Julian time conversions. You -# would only need this if you are concerned about accurate -# time conversion in the distand past. You must also define -# CONFIG_GREGORIAN_TIME in order to use Julian time. -# CONFIG_DEV_CONSOLE - Set if architecture-specific logic -# provides /dev/console. Enables stdout, stderr, stdin. -# CONFIG_DEV_LOWCONSOLE - Use the simple, low-level serial console -# driver (minimul support) -# CONFIG_MUTEX_TYPES: Set to enable support for recursive and -# errorcheck mutexes. Enables pthread_mutexattr_settype(). -# CONFIG_PRIORITY_INHERITANCE : Set to enable support for priority -# inheritance on mutexes and semaphores. -# CONFIG_SEM_PREALLOCHOLDERS: This setting is only used if priority -# inheritance is enabled. It defines the maximum number of -# different threads (minus one) that can take counts on a -# semaphore with priority inheritance support. This may be -# set to zero if priority inheritance is disabled OR if you -# are only using semaphores as mutexes (only one holder) OR -# if no more than two threads participate using a counting -# semaphore. -# CONFIG_SEM_NNESTPRIO. If priority inheritance is enabled, -# then this setting is the maximum number of higher priority -# threads (minus 1) than can be waiting for another thread -# to release a count on a semaphore. This value may be set -# to zero if no more than one thread is expected to wait for -# a semaphore. -# CONFIG_FDCLONE_DISABLE. Disable cloning of all file descriptors -# by task_create() when a new task is started. If set, all -# files/drivers will appear to be closed in the new task. -# CONFIG_FDCLONE_STDIO. Disable cloning of all but the first -# three file descriptors (stdin, stdout, stderr) by task_create() -# when a new task is started. If set, all files/drivers will -# appear to be closed in the new task except for stdin, stdout, -# and stderr. -# CONFIG_SDCLONE_DISABLE. Disable cloning of all socket -# desciptors by task_create() when a new task is started. If -# set, all sockets will appear to be closed in the new task. -# CONFIG_SCHED_WORKQUEUE. Create a dedicated "worker" thread to -# handle delayed processing from interrupt handlers. This feature -# is required for some drivers but, if there are not complaints, -# can be safely disabled. The worker thread also performs -# garbage collection -- completing any delayed memory deallocations -# from interrupt handlers. If the worker thread is disabled, -# then that clean will be performed by the IDLE thread instead -# (which runs at the lowest of priority and may not be appropriate -# if memory reclamation is of high priority). If CONFIG_SCHED_WORKQUEUE -# is enabled, then the following options can also be used: -# CONFIG_SCHED_WORKPRIORITY - The execution priority of the worker -# thread. Default: 50 -# CONFIG_SCHED_WORKPERIOD - How often the worker thread checks for -# work in units of microseconds. Default: 50*1000 (50 MS). -# CONFIG_SCHED_WORKSTACKSIZE - The stack size allocated for the worker -# thread. Default: CONFIG_IDLETHREAD_STACKSIZE. -# CONFIG_SIG_SIGWORK - The signal number that will be used to wake-up -# the worker thread. Default: 4 -# CONFIG_SCHED_WAITPID - Enable the waitpid() API -# CONFIG_SCHED_ATEXIT - Enabled the atexit() API +# Common Board Options # -CONFIG_USER_ENTRYPOINT="user_start" -#CONFIG_APPS_DIR= -CONFIG_DEBUG=n -CONFIG_DEBUG_VERBOSE=n -CONFIG_DEBUG_SYMBOLS=y -CONFIG_DEBUG_FS=n -CONFIG_DEBUG_GRAPHICS=n -CONFIG_DEBUG_LCD=n -CONFIG_DEBUG_USB=n -CONFIG_DEBUG_NET=n -CONFIG_DEBUG_RTC=n -CONFIG_DEBUG_ANALOG=n -CONFIG_DEBUG_PWM=n -CONFIG_DEBUG_CAN=n -CONFIG_DEBUG_I2C=n -CONFIG_DEBUG_INPUT=n - -CONFIG_MSEC_PER_TICK=1 -CONFIG_HAVE_CXX=y -CONFIG_HAVE_CXXINITIALIZE=y -CONFIG_MM_REGIONS=1 -CONFIG_MM_SMALL=y -CONFIG_ARCH_LOWPUTC=y -CONFIG_RR_INTERVAL=0 -CONFIG_SCHED_INSTRUMENTATION=n -CONFIG_TASK_NAME_SIZE=8 -CONFIG_START_YEAR=1970 -CONFIG_START_MONTH=1 -CONFIG_START_DAY=1 -CONFIG_GREGORIAN_TIME=n -CONFIG_JULIAN_TIME=n -# this eats ~1KiB of RAM ... work out why -CONFIG_DEV_CONSOLE=y -CONFIG_DEV_LOWCONSOLE=n -CONFIG_MUTEX_TYPES=n -CONFIG_PRIORITY_INHERITANCE=n -CONFIG_SEM_PREALLOCHOLDERS=0 -CONFIG_SEM_NNESTPRIO=0 -CONFIG_FDCLONE_DISABLE=y -CONFIG_FDCLONE_STDIO=y -CONFIG_SDCLONE_DISABLE=y -CONFIG_SCHED_WORKQUEUE=n -CONFIG_SCHED_WORKPRIORITY=50 -CONFIG_SCHED_WORKPERIOD=50000 -CONFIG_SCHED_WORKSTACKSIZE=1024 -CONFIG_SIG_SIGWORK=4 -CONFIG_SCHED_WAITPID=n -CONFIG_SCHED_ATEXIT=n - -# -# The following can be used to disable categories of -# APIs supported by the OS. If the compiler supports -# weak functions, then it should not be necessary to -# disable functions unless you want to restrict usage -# of those APIs. -# -# There are certain dependency relationships in these -# features. -# -# o mq_notify logic depends on signals to awaken tasks -# waiting for queues to become full or empty. -# o pthread_condtimedwait() depends on signals to wake -# up waiting tasks. -# -CONFIG_DISABLE_CLOCK=n + +# +# Board-Specific Options +# + +# +# RTOS Features +# +CONFIG_DISABLE_OS_API=y CONFIG_DISABLE_POSIX_TIMERS=y CONFIG_DISABLE_PTHREAD=y CONFIG_DISABLE_SIGNALS=y CONFIG_DISABLE_MQUEUE=y -CONFIG_DISABLE_MOUNTPOINT=y CONFIG_DISABLE_ENVIRON=y -CONFIG_DISABLE_POLL=y # -# Misc libc settings -# -# CONFIG_NOPRINTF_FIELDWIDTH - sprintf-related logic is a -# little smaller if we do not support fieldwidthes -# -CONFIG_NOPRINTF_FIELDWIDTH=n - -# -# Allow for architecture optimized implementations -# -# The architecture can provide optimized versions of the -# following to improve system performance -# -CONFIG_ARCH_MEMCPY=n -CONFIG_ARCH_MEMCMP=n -CONFIG_ARCH_MEMMOVE=n -CONFIG_ARCH_MEMSET=n -CONFIG_ARCH_STRCMP=n -CONFIG_ARCH_STRCPY=n -CONFIG_ARCH_STRNCPY=n -CONFIG_ARCH_STRLEN=n -CONFIG_ARCH_STRNLEN=n -CONFIG_ARCH_BZERO=n - -# -# Sizes of configurable things (0 disables) -# -# CONFIG_MAX_TASKS - The maximum number of simultaneously -# active tasks. This value must be a power of two. -# CONFIG_MAX_TASK_ARGS - This controls the maximum number of -# of parameters that a task may receive (i.e., maxmum value -# of 'argc') -# CONFIG_NPTHREAD_KEYS - The number of items of thread- -# specific data that can be retained -# CONFIG_NFILE_DESCRIPTORS - The maximum number of file -# descriptors (one for each open) -# CONFIG_NFILE_STREAMS - The maximum number of streams that -# can be fopen'ed -# CONFIG_NAME_MAX - The maximum size of a file name. -# CONFIG_STDIO_BUFFER_SIZE - Size of the buffer to allocate -# on fopen. (Only if CONFIG_NFILE_STREAMS > 0) -# CONFIG_STDIO_LINEBUFFER - If standard C buffered I/O is enabled -# (CONFIG_STDIO_BUFFER_SIZE > 0), then this option may be added -# to force automatic, line-oriented flushing the output buffer -# for putc(), fputc(), putchar(), puts(), fputs(), printf(), -# fprintf(), and vfprintf(). When a newline is encountered in -# the output string, the output buffer will be flushed. This -# (slightly) increases the NuttX footprint but supports the kind -# of behavior that people expect for printf(). -# CONFIG_NUNGET_CHARS - Number of characters that can be -# buffered by ungetc() (Only if CONFIG_NFILE_STREAMS > 0) -# CONFIG_PREALLOC_MQ_MSGS - The number of pre-allocated message -# structures. The system manages a pool of preallocated -# message structures to minimize dynamic allocations -# CONFIG_MQ_MAXMSGSIZE - Message structures are allocated with -# a fixed payload size given by this settin (does not include -# other message structure overhead. -# CONFIG_MAX_WDOGPARMS - Maximum number of parameters that -# can be passed to a watchdog handler -# CONFIG_PREALLOC_WDOGS - The number of pre-allocated watchdog -# structures. The system manages a pool of preallocated -# watchdog structures to minimize dynamic allocations -# CONFIG_PREALLOC_TIMERS - The number of pre-allocated POSIX -# timer structures. The system manages a pool of preallocated -# timer structures to minimize dynamic allocations. Set to -# zero for all dynamic allocations. +# Clocks and Timers # -CONFIG_MAX_TASKS=4 -CONFIG_MAX_TASK_ARGS=4 -CONFIG_NPTHREAD_KEYS=2 -CONFIG_NFILE_DESCRIPTORS=8 -CONFIG_NFILE_STREAMS=0 -CONFIG_NAME_MAX=12 -CONFIG_STDIO_BUFFER_SIZE=32 -CONFIG_STDIO_LINEBUFFER=n -CONFIG_NUNGET_CHARS=2 -CONFIG_PREALLOC_MQ_MSGS=4 -CONFIG_MQ_MAXMSGSIZE=32 +CONFIG_USEC_PER_TICK=1000 +# CONFIG_SYSTEM_TIME64 is not set +# CONFIG_CLOCK_MONOTONIC is not set +# CONFIG_JULIAN_TIME is not set +CONFIG_START_YEAR=1970 +CONFIG_START_MONTH=1 +CONFIG_START_DAY=1 CONFIG_MAX_WDOGPARMS=2 -CONFIG_PREALLOC_WDOGS=4 +CONFIG_PREALLOC_WDOGS=6 +CONFIG_WDOG_INTRESERVE=2 CONFIG_PREALLOC_TIMERS=0 +# +# Tasks and Scheduling +# +# CONFIG_INIT_NONE is not set +CONFIG_INIT_ENTRYPOINT=y +# CONFIG_INIT_FILEPATH is not set +CONFIG_USER_ENTRYPOINT="user_start" +CONFIG_RR_INTERVAL=0 +CONFIG_TASK_NAME_SIZE=8 +CONFIG_MAX_TASKS=4 +# CONFIG_SCHED_HAVE_PARENT is not set +# CONFIG_SCHED_WAITPID is not set # -# Settings for apps/nshlib +# Performance Monitoring +# +# CONFIG_SCHED_CPULOAD is not set +# CONFIG_SCHED_INSTRUMENTATION is not set + # -# CONFIG_NSH_BUILTIN_APPS - Support external registered, -# "named" applications that can be executed from the NSH -# command line (see apps/README.txt for more information). -# CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer -# CONFIG_NSH_STRERROR - Use strerror(errno) -# CONFIG_NSH_LINELEN - Maximum length of one command line -# CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi -# CONFIG_NSH_DISABLESCRIPT - Disable scripting support -# CONFIG_NSH_DISABLEBG - Disable background commands -# CONFIG_NSH_ROMFSETC - Use startup script in /etc -# CONFIG_NSH_CONSOLE - Use serial console front end -# CONFIG_NSH_TELNET - Use telnetd console front end -# CONFIG_NSH_ARCHINIT - Platform provides architecture -# specific initialization (nsh_archinitialize()). +# Files and I/O # +CONFIG_DEV_CONSOLE=y +CONFIG_FDCLONE_DISABLE=y +CONFIG_FDCLONE_STDIO=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_NFILE_DESCRIPTORS=8 +CONFIG_NFILE_STREAMS=0 +CONFIG_NAME_MAX=12 +# CONFIG_PRIORITY_INHERITANCE is not set -# Disable NSH completely -CONFIG_NSH_CONSOLE=n +# +# RTOS hooks +# +# CONFIG_BOARD_INITIALIZE is not set +# CONFIG_SCHED_STARTHOOK is not set +# CONFIG_SCHED_ATEXIT is not set +# CONFIG_SCHED_ONEXIT is not set + +# +# Work Queue Support +# # # Stack and heap information # -# CONFIG_BOOT_RUNFROMFLASH - Some configurations support XIP -# operation from FLASH but must copy initialized .data sections to RAM. -# (should also be =n for the STM3210E-EVAL which always runs from flash) -# CONFIG_BOOT_COPYTORAM - Some configurations boot in FLASH -# but copy themselves entirely into RAM for better performance. -# CONFIG_CUSTOM_STACK - The up_ implementation will handle -# all stack operations outside of the nuttx model. -# CONFIG_STACK_POINTER - The initial stack pointer (arm7tdmi only) -# CONFIG_IDLETHREAD_STACKSIZE - The size of the initial stack. -# This is the thread that (1) performs the inital boot of the system up -# to the point where user_start() is spawned, and (2) there after is the -# IDLE thread that executes only when there is no other thread ready to -# run. -# CONFIG_USERMAIN_STACKSIZE - The size of the stack to allocate -# for the main user thread that begins at the user_start() entry point. -# CONFIG_PTHREAD_STACK_MIN - Minimum pthread stack size -# CONFIG_PTHREAD_STACK_DEFAULT - Default pthread stack size -# CONFIG_HEAP_BASE - The beginning of the heap -# CONFIG_HEAP_SIZE - The size of the heap -# -CONFIG_BOOT_RUNFROMFLASH=n -CONFIG_BOOT_COPYTORAM=n -CONFIG_CUSTOM_STACK=n -CONFIG_STACK_POINTER= CONFIG_IDLETHREAD_STACKSIZE=1024 CONFIG_USERMAIN_STACKSIZE=1200 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_PTHREAD_STACK_DEFAULT=1024 -CONFIG_HEAP_BASE= -CONFIG_HEAP_SIZE= +# CONFIG_LIB_SYSCALL is not set + +# +# Device Drivers +# +CONFIG_DISABLE_POLL=y +# CONFIG_DEV_NULL is not set +# CONFIG_DEV_ZERO is not set +# CONFIG_LOOP is not set + +# +# Buffering +# +# CONFIG_DRVR_WRITEBUFFER is not set +# CONFIG_DRVR_READAHEAD is not set +# CONFIG_RAMDISK is not set +# CONFIG_CAN is not set +# CONFIG_ARCH_HAVE_PWM_PULSECOUNT is not set +# CONFIG_PWM is not set +CONFIG_ARCH_HAVE_I2CRESET=y +# CONFIG_I2C is not set +# CONFIG_SPI is not set +# CONFIG_I2S is not set +# CONFIG_RTC is not set +# CONFIG_WATCHDOG is not set +# CONFIG_TIMER is not set +# CONFIG_ANALOG is not set +# CONFIG_AUDIO_DEVICES is not set +# CONFIG_VIDEO_DEVICES is not set +# CONFIG_BCH is not set +# CONFIG_INPUT is not set +# CONFIG_LCD is not set +# CONFIG_MMCSD is not set +# CONFIG_MTD is not set +# CONFIG_EEPROM is not set +# CONFIG_PIPES is not set +# CONFIG_PM is not set +# CONFIG_POWER is not set +# CONFIG_SENSORS is not set +# CONFIG_SERCOMM_CONSOLE is not set +CONFIG_SERIAL=y +# CONFIG_DEV_LOWCONSOLE is not set +# CONFIG_16550_UART is not set +# CONFIG_ARCH_HAVE_UART is not set +# CONFIG_ARCH_HAVE_UART0 is not set +# CONFIG_ARCH_HAVE_UART1 is not set +# CONFIG_ARCH_HAVE_UART2 is not set +# CONFIG_ARCH_HAVE_UART3 is not set +# CONFIG_ARCH_HAVE_UART4 is not set +# CONFIG_ARCH_HAVE_UART5 is not set +# CONFIG_ARCH_HAVE_UART6 is not set +# CONFIG_ARCH_HAVE_UART7 is not set +# CONFIG_ARCH_HAVE_UART8 is not set +# CONFIG_ARCH_HAVE_SCI0 is not set +# CONFIG_ARCH_HAVE_SCI1 is not set +# CONFIG_ARCH_HAVE_USART0 is not set +CONFIG_ARCH_HAVE_USART1=y +CONFIG_ARCH_HAVE_USART2=y +CONFIG_ARCH_HAVE_USART3=y +# CONFIG_ARCH_HAVE_USART4 is not set +# CONFIG_ARCH_HAVE_USART5 is not set +# CONFIG_ARCH_HAVE_USART6 is not set +# CONFIG_ARCH_HAVE_USART7 is not set +# CONFIG_ARCH_HAVE_USART8 is not set +# CONFIG_ARCH_HAVE_OTHER_UART is not set + +# +# USART Configuration +# +CONFIG_USART1_ISUART=y +CONFIG_USART2_ISUART=y +CONFIG_USART3_ISUART=y +CONFIG_MCU_SERIAL=y +CONFIG_STANDARD_SERIAL=y +# CONFIG_SERIAL_IFLOWCONTROL is not set +# CONFIG_SERIAL_OFLOWCONTROL is not set +CONFIG_ARCH_HAVE_SERIAL_TERMIOS=y +CONFIG_SERIAL_TERMIOS=y +CONFIG_USART1_SERIAL_CONSOLE=y +# CONFIG_USART2_SERIAL_CONSOLE is not set +# CONFIG_USART3_SERIAL_CONSOLE is not set +# CONFIG_OTHER_SERIAL_CONSOLE is not set +# CONFIG_NO_SERIAL_CONSOLE is not set + +# +# USART1 Configuration +# +CONFIG_USART1_RXBUFSIZE=64 +CONFIG_USART1_TXBUFSIZE=64 +CONFIG_USART1_BAUD=115200 +CONFIG_USART1_BITS=8 +CONFIG_USART1_PARITY=0 +CONFIG_USART1_2STOP=0 +# CONFIG_USART1_IFLOWCONTROL is not set +# CONFIG_USART1_OFLOWCONTROL is not set + +# +# USART2 Configuration +# +CONFIG_USART2_RXBUFSIZE=64 +CONFIG_USART2_TXBUFSIZE=64 +CONFIG_USART2_BAUD=115200 +CONFIG_USART2_BITS=8 +CONFIG_USART2_PARITY=0 +CONFIG_USART2_2STOP=0 +# CONFIG_USART2_IFLOWCONTROL is not set +# CONFIG_USART2_OFLOWCONTROL is not set + +# +# USART3 Configuration +# +CONFIG_USART3_RXBUFSIZE=64 +CONFIG_USART3_TXBUFSIZE=64 +CONFIG_USART3_BAUD=115200 +CONFIG_USART3_BITS=8 +CONFIG_USART3_PARITY=0 +CONFIG_USART3_2STOP=0 +# CONFIG_USART3_IFLOWCONTROL is not set +# CONFIG_USART3_OFLOWCONTROL is not set +# CONFIG_USBDEV is not set +# CONFIG_USBHOST is not set +# CONFIG_WIRELESS is not set + +# +# System Logging Device Options +# + +# +# System Logging +# +# CONFIG_RAMLOG is not set + +# +# Networking Support +# +# CONFIG_ARCH_HAVE_NET is not set +# CONFIG_ARCH_HAVE_PHY is not set +# CONFIG_NET is not set + +# +# Crypto API +# +# CONFIG_CRYPTO is not set + +# +# File Systems +# + +# +# File system configuration +# +CONFIG_DISABLE_MOUNTPOINT=y +CONFIG_DISABLE_PSEUDOFS_OPERATIONS=y +# CONFIG_FS_READABLE is not set +# CONFIG_FS_WRITABLE is not set +# CONFIG_FS_NAMED_SEMAPHORES is not set +# CONFIG_FS_RAMMAP is not set +# CONFIG_FS_PROCFS is not set + +# +# System Logging +# +# CONFIG_SYSLOG is not set +# CONFIG_SYSLOG_TIMESTAMP is not set + +# +# Graphics Support +# +# CONFIG_NX is not set + +# +# Memory Management +# +CONFIG_MM_SMALL=y +CONFIG_MM_REGIONS=1 +# CONFIG_ARCH_HAVE_HEAP2 is not set +# CONFIG_GRAN is not set + +# +# Audio Support +# +# CONFIG_AUDIO is not set + +# +# Binary Loader +# +# CONFIG_BINFMT_DISABLE is not set +# CONFIG_NXFLAT is not set +# CONFIG_ELF is not set +# CONFIG_BUILTIN is not set +# CONFIG_PIC is not set +# CONFIG_SYMTAB_ORDEREDBYNAME is not set + +# +# Library Routines +# + +# +# Standard C Library Options +# +CONFIG_STDIO_BUFFER_SIZE=32 +# CONFIG_STDIO_LINEBUFFER is not set +CONFIG_NUNGET_CHARS=2 +# CONFIG_NOPRINTF_FIELDWIDTH is not set +# CONFIG_LIBC_FLOATINGPOINT is not set +# CONFIG_LIBC_IOCTL_VARIADIC is not set +CONFIG_LIB_RAND_ORDER=1 +# CONFIG_EOL_IS_CR is not set +# CONFIG_EOL_IS_LF is not set +# CONFIG_EOL_IS_BOTH_CRLF is not set +CONFIG_EOL_IS_EITHER_CRLF=y +# CONFIG_LIBC_EXECFUNCS is not set +CONFIG_POSIX_SPAWN_PROXY_STACKSIZE=1024 +CONFIG_TASK_SPAWN_DEFAULT_STACKSIZE=1024 +# CONFIG_LIBC_STRERROR is not set +# CONFIG_LIBC_PERROR_STDOUT is not set +CONFIG_ARCH_LOWPUTC=y +CONFIG_LIB_SENDFILE_BUFSIZE=512 +# CONFIG_ARCH_ROMGETC is not set +# CONFIG_ARCH_OPTIMIZED_FUNCTIONS is not set + +# +# Non-standard Library Support +# +# CONFIG_LIB_KBDCODEC is not set +# CONFIG_LIB_SLCDCODEC is not set + +# +# Basic CXX Support +# +CONFIG_C99_BOOL8=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +# CONFIG_CXX_NEWLONG is not set + +# +# uClibc++ Standard C++ Library +# +# CONFIG_UCLIBCXX is not set + +# +# Application Configuration +# + +# +# Built-In Applications +# + +# +# Examples +# +# CONFIG_EXAMPLES_BUTTONS is not set +# CONFIG_EXAMPLES_CAN is not set +# CONFIG_EXAMPLES_CONFIGDATA is not set +# CONFIG_EXAMPLES_CPUHOG is not set +# CONFIG_EXAMPLES_CXXTEST is not set +# CONFIG_EXAMPLES_DHCPD is not set +# CONFIG_EXAMPLES_ELF is not set +# CONFIG_EXAMPLES_FTPC is not set +# CONFIG_EXAMPLES_FTPD is not set +# CONFIG_EXAMPLES_HELLO is not set +# CONFIG_EXAMPLES_HELLOXX is not set +# CONFIG_EXAMPLES_JSON is not set +# CONFIG_EXAMPLES_HIDKBD is not set +# CONFIG_EXAMPLES_KEYPADTEST is not set +# CONFIG_EXAMPLES_IGMP is not set +# CONFIG_EXAMPLES_MM is not set +# CONFIG_EXAMPLES_MODBUS is not set +# CONFIG_EXAMPLES_MOUNT is not set +# CONFIG_EXAMPLES_NRF24L01TERM is not set +# CONFIG_EXAMPLES_NSH is not set +# CONFIG_EXAMPLES_NULL is not set +# CONFIG_EXAMPLES_NX is not set +# CONFIG_EXAMPLES_NXTERM is not set +# CONFIG_EXAMPLES_NXFFS is not set +# CONFIG_EXAMPLES_NXFLAT is not set +# CONFIG_EXAMPLES_NXHELLO is not set +# CONFIG_EXAMPLES_NXIMAGE is not set +# CONFIG_EXAMPLES_NXLINES is not set +# CONFIG_EXAMPLES_NXTEXT is not set +# CONFIG_EXAMPLES_OSTEST is not set +# CONFIG_EXAMPLES_PIPE is not set +# CONFIG_EXAMPLES_POLL is not set +# CONFIG_EXAMPLES_POSIXSPAWN is not set +# CONFIG_EXAMPLES_QENCODER is not set +# CONFIG_EXAMPLES_RGMP is not set +# CONFIG_EXAMPLES_ROMFS is not set +# CONFIG_EXAMPLES_SENDMAIL is not set +# CONFIG_EXAMPLES_SERIALBLASTER is not set +# CONFIG_EXAMPLES_SERIALRX is not set +# CONFIG_EXAMPLES_SERLOOP is not set +# CONFIG_EXAMPLES_SLCD is not set +# CONFIG_EXAMPLES_SMART is not set +# CONFIG_EXAMPLES_TCPECHO is not set +# CONFIG_EXAMPLES_TELNETD is not set +# CONFIG_EXAMPLES_THTTPD is not set +# CONFIG_EXAMPLES_TIFF is not set +# CONFIG_EXAMPLES_TOUCHSCREEN is not set +# CONFIG_EXAMPLES_UDP is not set +# CONFIG_EXAMPLES_WEBSERVER is not set +# CONFIG_EXAMPLES_USBSERIAL is not set +# CONFIG_EXAMPLES_USBTERM is not set +# CONFIG_EXAMPLES_WATCHDOG is not set + +# +# Graphics Support +# +# CONFIG_TIFF is not set +# CONFIG_GRAPHICS_TRAVELER is not set + +# +# Interpreters +# +# CONFIG_INTERPRETERS_FICL is not set +# CONFIG_INTERPRETERS_PCODE is not set + +# +# Network Utilities +# + +# +# Networking Utilities +# +# CONFIG_NETUTILS_CODECS is not set +# CONFIG_NETUTILS_DHCPD is not set +# CONFIG_NETUTILS_FTPC is not set +# CONFIG_NETUTILS_FTPD is not set +# CONFIG_NETUTILS_JSON is not set +# CONFIG_NETUTILS_SMTP is not set +# CONFIG_NETUTILS_TFTPC is not set +# CONFIG_NETUTILS_THTTPD is not set +# CONFIG_NETUTILS_NETLIB is not set +# CONFIG_NETUTILS_WEBCLIENT is not set + +# +# FreeModBus +# +# CONFIG_MODBUS is not set # # NSH Library # # CONFIG_NSH_LIBRARY is not set -# CONFIG_NSH_BUILTIN_APPS is not set + +# +# NxWidgets/NxWM +# + +# +# Platform-specific Support +# +# CONFIG_PLATFORM_CONFIGDATA is not set + +# +# System Libraries and NSH Add-Ons +# + +# +# Custom Free Memory Command +# +# CONFIG_SYSTEM_FREE is not set + +# +# EMACS-like Command Line Editor +# +# CONFIG_SYSTEM_CLE is not set + +# +# CU Minimal Terminal +# +# CONFIG_SYSTEM_CUTERM is not set + +# +# FLASH Program Installation +# +# CONFIG_SYSTEM_INSTALL is not set + +# +# FLASH Erase-all Command +# + +# +# Intel HEX to binary conversion +# +# CONFIG_SYSTEM_HEX2BIN is not set + +# +# I2C tool +# + +# +# INI File Parser +# +# CONFIG_SYSTEM_INIFILE is not set + +# +# NxPlayer media player library / command Line +# + +# +# RAM test +# +# CONFIG_SYSTEM_RAMTEST is not set + +# +# readline() +# +# CONFIG_SYSTEM_READLINE is not set + +# +# P-Code Support +# + +# +# PHY Tool +# + +# +# Power Off +# +# CONFIG_SYSTEM_POWEROFF is not set + +# +# RAMTRON +# +# CONFIG_SYSTEM_RAMTRON is not set + +# +# SD Card +# +# CONFIG_SYSTEM_SDCARD is not set + +# +# Sudoku +# +# CONFIG_SYSTEM_SUDOKU is not set + +# +# Sysinfo +# +# CONFIG_SYSTEM_SYSINFO is not set + +# +# VI Work-Alike Editor +# +# CONFIG_SYSTEM_VI is not set + +# +# Stack Monitor +# + +# +# USB CDC/ACM Device Commands +# + +# +# USB Composite Device Commands +# + +# +# USB Mass Storage Device Commands +# + +# +# USB Monitor +# + +# +# Zmodem Commands +# +# CONFIG_SYSTEM_ZMODEM is not set diff --git a/nuttx-configs/px4io-v2/nsh/Make.defs b/nuttx-configs/px4io-v2/nsh/Make.defs index 287466db6..8705ed578 100644 --- a/nuttx-configs/px4io-v2/nsh/Make.defs +++ b/nuttx-configs/px4io-v2/nsh/Make.defs @@ -35,6 +35,7 @@ include ${TOPDIR}/.config include ${TOPDIR}/tools/Config.mk +include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs # # We only support building with the ARM bare-metal toolchain from diff --git a/nuttx-configs/px4io-v2/nsh/defconfig b/nuttx-configs/px4io-v2/nsh/defconfig index 02b51e3d7..bfd6f9ba9 100755 --- a/nuttx-configs/px4io-v2/nsh/defconfig +++ b/nuttx-configs/px4io-v2/nsh/defconfig @@ -1,547 +1,979 @@ -############################################################################ -# configs/px4io/nsh/defconfig -# -# Copyright (C) 2012 PX4 Development Team. All rights reserved. -# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt <gnutt@nuttx.org> -# -# 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 NuttX 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. -# -############################################################################ -# -# architecture selection -# -# CONFIG_ARCH - identifies the arch subdirectory and, hence, the -# processor architecture. -# CONFIG_ARCH_family - for use in C code. This identifies the -# particular chip family that the architecture is implemented -# in. -# CONFIG_ARCH_architecture - for use in C code. This identifies the -# specific architecture within the chip family. -# CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory -# CONFIG_ARCH_CHIP_name - For use in C code -# CONFIG_ARCH_BOARD - identifies the configs subdirectory and, hence, -# the board that supports the particular chip or SoC. -# CONFIG_ARCH_BOARD_name - for use in C code -# CONFIG_ENDIAN_BIG - define if big endian (default is little endian) -# CONFIG_BOARD_LOOPSPERMSEC - for delay loops -# CONFIG_DRAM_SIZE - Describes the installed DRAM. -# CONFIG_DRAM_START - The start address of DRAM (physical) -# CONFIG_ARCH_IRQPRIO - The ST32F100CB supports interrupt prioritization -# CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt -# stack. If defined, this symbol is the size of the interrupt -# stack in bytes. If not defined, the user task stacks will be -# used during interrupt handling. -# CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions -# CONFIG_ARCH_BOOTLOADER - Set if you are using a bootloader. -# CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture. -# CONFIG_ARCH_BUTTONS - Enable support for buttons. Unique to board architecture. -# CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that -# cause a 100 second delay during boot-up. This 100 second delay -# serves no purpose other than it allows you to calibrate -# CONFIG_BOARD_LOOPSPERMSEC. You simply use a stop watch to measure -# the 100 second delay then adjust CONFIG_BOARD_LOOPSPERMSEC until -# the delay actually is 100 seconds. -# CONFIG_ARCH_DMA - Support DMA initialization # -CONFIG_ARCH="arm" -CONFIG_ARCH_ARM=y -CONFIG_ARCH_CORTEXM3=y -CONFIG_ARCH_CHIP="stm32" -CONFIG_ARCH_CHIP_STM32F100C8=y -CONFIG_ARCH_BOARD="px4io-v2" -CONFIG_ARCH_BOARD_PX4IO_V2=y -CONFIG_BOARD_LOOPSPERMSEC=2000 -CONFIG_DRAM_SIZE=0x00002000 -CONFIG_DRAM_START=0x20000000 -CONFIG_ARCH_INTERRUPTSTACK=n -CONFIG_ARCH_STACKDUMP=y -CONFIG_ARCH_BOOTLOADER=n -CONFIG_ARCH_LEDS=n -CONFIG_ARCH_BUTTONS=n -CONFIG_ARCH_CALIBRATION=n -CONFIG_ARCH_DMA=y +# Automatically generated file; DO NOT EDIT. +# Nuttx/ Configuration +# + +# +# Build Setup +# +# CONFIG_EXPERIMENTAL is not set +# CONFIG_DEFAULT_SMALL is not set +CONFIG_HOST_LINUX=y +# CONFIG_HOST_OSX is not set +# CONFIG_HOST_WINDOWS is not set +# CONFIG_HOST_OTHER is not set + +# +# Build Configuration +# +CONFIG_APPS_DIR="../apps" +CONFIG_BUILD_FLAT=y +# CONFIG_BUILD_2PASS is not set + +# +# Binary Output Formats +# +# CONFIG_RRLOAD_BINARY is not set +# CONFIG_INTELHEX_BINARY is not set +# CONFIG_MOTOROLA_SREC is not set +CONFIG_RAW_BINARY=y +# CONFIG_UBOOT_UIMAGE is not set + +# +# Customize Header Files +# +# CONFIG_ARCH_STDINT_H is not set +# CONFIG_ARCH_STDBOOL_H is not set CONFIG_ARCH_MATH_H=y +# CONFIG_ARCH_FLOAT_H is not set +# CONFIG_ARCH_STDARG_H is not set -CONFIG_ARMV7M_CMNVECTOR=y +# +# Debug Options +# +# CONFIG_DEBUG is not set +CONFIG_ARCH_HAVE_STACKCHECK=y +CONFIG_STACK_COLORATION=y +CONFIG_ARCH_HAVE_HEAPCHECK=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_ARCH_HAVE_CUSTOMOPT=y +# CONFIG_DEBUG_NOOPT is not set +# CONFIG_DEBUG_CUSTOMOPT is not set +CONFIG_DEBUG_FULLOPT=y # -# JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): +# System Type # -# CONFIG_STM32_DFU - Use the DFU bootloader, not JTAG +CONFIG_ARCH_ARM=y +# CONFIG_ARCH_AVR is not set +# CONFIG_ARCH_HC is not set +# CONFIG_ARCH_MIPS is not set +# CONFIG_ARCH_RGMP is not set +# CONFIG_ARCH_SH is not set +# CONFIG_ARCH_SIM is not set +# CONFIG_ARCH_X86 is not set +# CONFIG_ARCH_Z16 is not set +# CONFIG_ARCH_Z80 is not set +CONFIG_ARCH="arm" + # -# JTAG Enable options: +# ARM Options +# +# CONFIG_ARCH_CHIP_A1X is not set +# CONFIG_ARCH_CHIP_C5471 is not set +# CONFIG_ARCH_CHIP_CALYPSO is not set +# CONFIG_ARCH_CHIP_DM320 is not set +# CONFIG_ARCH_CHIP_EFM32 is not set +# CONFIG_ARCH_CHIP_IMX is not set +# CONFIG_ARCH_CHIP_KINETIS is not set +# CONFIG_ARCH_CHIP_KL is not set +# CONFIG_ARCH_CHIP_LM is not set +# CONFIG_ARCH_CHIP_TIVA is not set +# CONFIG_ARCH_CHIP_LPC17XX is not set +# CONFIG_ARCH_CHIP_LPC214X is not set +# CONFIG_ARCH_CHIP_LPC2378 is not set +# CONFIG_ARCH_CHIP_LPC31XX is not set +# CONFIG_ARCH_CHIP_LPC43XX is not set +# CONFIG_ARCH_CHIP_NUC1XX is not set +# CONFIG_ARCH_CHIP_SAMA5 is not set +# CONFIG_ARCH_CHIP_SAMD is not set +# CONFIG_ARCH_CHIP_SAM34 is not set +CONFIG_ARCH_CHIP_STM32=y +# CONFIG_ARCH_CHIP_STR71X is not set +# CONFIG_ARCH_ARM7TDMI is not set +# CONFIG_ARCH_ARM926EJS is not set +# CONFIG_ARCH_ARM920T is not set +# CONFIG_ARCH_CORTEXM0 is not set +CONFIG_ARCH_CORTEXM3=y +# CONFIG_ARCH_CORTEXM4 is not set +# CONFIG_ARCH_CORTEXA5 is not set +# CONFIG_ARCH_CORTEXA8 is not set +CONFIG_ARCH_FAMILY="armv7-m" +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARCH_HAVE_CMNVECTOR=y +CONFIG_ARMV7M_CMNVECTOR=y +# CONFIG_ARCH_HAVE_FPU is not set +# CONFIG_ARMV7M_MPU is not set + # -# CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) -# CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) -# but without JNTRST. -# CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled +# ARMV7M Configuration Options # -CONFIG_STM32_DFU=n -CONFIG_STM32_JTAG_FULL_ENABLE=n -CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n -CONFIG_STM32_JTAG_SW_ENABLE=y +# CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set +# CONFIG_ARMV7M_TOOLCHAIN_CODEREDL is not set +# CONFIG_ARMV7M_TOOLCHAIN_CODESOURCERYL is not set +CONFIG_ARMV7M_TOOLCHAIN_GNU_EABIL=y +# CONFIG_ARMV7M_STACKCHECK is not set +# CONFIG_ARMV7M_ITMSYSLOG is not set # -# Individual subsystems can be enabled: +# STM32 Configuration Options +# +# CONFIG_ARCH_CHIP_STM32L151C6 is not set +# CONFIG_ARCH_CHIP_STM32L151C8 is not set +# CONFIG_ARCH_CHIP_STM32L151CB is not set +# CONFIG_ARCH_CHIP_STM32L151R6 is not set +# CONFIG_ARCH_CHIP_STM32L151R8 is not set +# CONFIG_ARCH_CHIP_STM32L151RB is not set +# CONFIG_ARCH_CHIP_STM32L151V6 is not set +# CONFIG_ARCH_CHIP_STM32L151V8 is not set +# CONFIG_ARCH_CHIP_STM32L151VB is not set +# CONFIG_ARCH_CHIP_STM32L152C6 is not set +# CONFIG_ARCH_CHIP_STM32L152C8 is not set +# CONFIG_ARCH_CHIP_STM32L152CB is not set +# CONFIG_ARCH_CHIP_STM32L152R6 is not set +# CONFIG_ARCH_CHIP_STM32L152R8 is not set +# CONFIG_ARCH_CHIP_STM32L152RB is not set +# CONFIG_ARCH_CHIP_STM32L152V6 is not set +# CONFIG_ARCH_CHIP_STM32L152V8 is not set +# CONFIG_ARCH_CHIP_STM32L152VB is not set +# CONFIG_ARCH_CHIP_STM32L162ZD is not set +CONFIG_ARCH_CHIP_STM32F100C8=y +# CONFIG_ARCH_CHIP_STM32F100CB is not set +# CONFIG_ARCH_CHIP_STM32F100R8 is not set +# CONFIG_ARCH_CHIP_STM32F100RB is not set +# CONFIG_ARCH_CHIP_STM32F100RC is not set +# CONFIG_ARCH_CHIP_STM32F100RD is not set +# CONFIG_ARCH_CHIP_STM32F100RE is not set +# CONFIG_ARCH_CHIP_STM32F100V8 is not set +# CONFIG_ARCH_CHIP_STM32F100VB is not set +# CONFIG_ARCH_CHIP_STM32F100VC is not set +# CONFIG_ARCH_CHIP_STM32F100VD is not set +# CONFIG_ARCH_CHIP_STM32F100VE is not set +# CONFIG_ARCH_CHIP_STM32F102CB is not set +# CONFIG_ARCH_CHIP_STM32F103T8 is not set +# CONFIG_ARCH_CHIP_STM32F103TB is not set +# CONFIG_ARCH_CHIP_STM32F103C4 is not set +# CONFIG_ARCH_CHIP_STM32F103C8 is not set +# CONFIG_ARCH_CHIP_STM32F103CB is not set +# CONFIG_ARCH_CHIP_STM32F103R8 is not set +# CONFIG_ARCH_CHIP_STM32F103RB is not set +# CONFIG_ARCH_CHIP_STM32F103RC is not set +# CONFIG_ARCH_CHIP_STM32F103RD is not set +# CONFIG_ARCH_CHIP_STM32F103RE is not set +# CONFIG_ARCH_CHIP_STM32F103RG is not set +# CONFIG_ARCH_CHIP_STM32F103V8 is not set +# CONFIG_ARCH_CHIP_STM32F103VB is not set +# CONFIG_ARCH_CHIP_STM32F103VC is not set +# CONFIG_ARCH_CHIP_STM32F103VE is not set +# CONFIG_ARCH_CHIP_STM32F103ZE is not set +# CONFIG_ARCH_CHIP_STM32F105VB is not set +# CONFIG_ARCH_CHIP_STM32F107VC is not set +# CONFIG_ARCH_CHIP_STM32F207IG is not set +# CONFIG_ARCH_CHIP_STM32F207ZE is not set +# CONFIG_ARCH_CHIP_STM32F302CB is not set +# CONFIG_ARCH_CHIP_STM32F302CC is not set +# CONFIG_ARCH_CHIP_STM32F302RB is not set +# CONFIG_ARCH_CHIP_STM32F302RC is not set +# CONFIG_ARCH_CHIP_STM32F302VB is not set +# CONFIG_ARCH_CHIP_STM32F302VC is not set +# CONFIG_ARCH_CHIP_STM32F303CB is not set +# CONFIG_ARCH_CHIP_STM32F303CC is not set +# CONFIG_ARCH_CHIP_STM32F303RB is not set +# CONFIG_ARCH_CHIP_STM32F303RC is not set +# CONFIG_ARCH_CHIP_STM32F303VB is not set +# CONFIG_ARCH_CHIP_STM32F303VC is not set +# CONFIG_ARCH_CHIP_STM32F401RE is not set +# CONFIG_ARCH_CHIP_STM32F411RE is not set +# CONFIG_ARCH_CHIP_STM32F405RG is not set +# CONFIG_ARCH_CHIP_STM32F405VG is not set +# CONFIG_ARCH_CHIP_STM32F405ZG is not set +# CONFIG_ARCH_CHIP_STM32F407VE is not set +# CONFIG_ARCH_CHIP_STM32F407VG is not set +# CONFIG_ARCH_CHIP_STM32F407ZE is not set +# CONFIG_ARCH_CHIP_STM32F407ZG is not set +# CONFIG_ARCH_CHIP_STM32F407IE is not set +# CONFIG_ARCH_CHIP_STM32F407IG is not set +# CONFIG_ARCH_CHIP_STM32F427V is not set +# CONFIG_ARCH_CHIP_STM32F427Z is not set +# CONFIG_ARCH_CHIP_STM32F427I is not set +# CONFIG_ARCH_CHIP_STM32F429V is not set +# CONFIG_ARCH_CHIP_STM32F429Z is not set +# CONFIG_ARCH_CHIP_STM32F429I is not set +# CONFIG_ARCH_CHIP_STM32F429B is not set +# CONFIG_ARCH_CHIP_STM32F429N is not set +# CONFIG_STM32_STM32L15XX is not set +# CONFIG_STM32_ENERGYLITE is not set +CONFIG_STM32_STM32F10XX=y +CONFIG_STM32_VALUELINE=y +# CONFIG_STM32_CONNECTIVITYLINE is not set +# CONFIG_STM32_PERFORMANCELINE is not set +# CONFIG_STM32_USBACCESSLINE is not set +# CONFIG_STM32_HIGHDENSITY is not set +# CONFIG_STM32_MEDIUMDENSITY is not set +# CONFIG_STM32_LOWDENSITY is not set +# CONFIG_STM32_STM32F20XX is not set +# CONFIG_STM32_STM32F207 is not set +# CONFIG_STM32_STM32F30XX is not set +# CONFIG_STM32_STM32F40XX is not set +# CONFIG_STM32_STM32F401 is not set +# CONFIG_STM32_STM32F411 is not set +# CONFIG_STM32_STM32F405 is not set +# CONFIG_STM32_STM32F407 is not set +# CONFIG_STM32_STM32F427 is not set +# CONFIG_STM32_STM32F429 is not set + # -# AHB: +# STM32 Peripheral Support +# +# CONFIG_STM32_HAVE_CCM is not set +# CONFIG_STM32_HAVE_USBDEV is not set +# CONFIG_STM32_HAVE_OTGFS is not set +# CONFIG_STM32_HAVE_FSMC is not set +CONFIG_STM32_HAVE_USART3=y +CONFIG_STM32_HAVE_UART4=y +CONFIG_STM32_HAVE_UART5=y +# CONFIG_STM32_HAVE_USART6 is not set +# CONFIG_STM32_HAVE_UART7 is not set +# CONFIG_STM32_HAVE_UART8 is not set +CONFIG_STM32_HAVE_TIM1=y +CONFIG_STM32_HAVE_TIM5=y +CONFIG_STM32_HAVE_TIM6=y +CONFIG_STM32_HAVE_TIM7=y +# CONFIG_STM32_HAVE_TIM8 is not set +# CONFIG_STM32_HAVE_TIM9 is not set +# CONFIG_STM32_HAVE_TIM10 is not set +# CONFIG_STM32_HAVE_TIM11 is not set +CONFIG_STM32_HAVE_TIM12=y +CONFIG_STM32_HAVE_TIM13=y +CONFIG_STM32_HAVE_TIM14=y +CONFIG_STM32_HAVE_TIM15=y +CONFIG_STM32_HAVE_TIM16=y +CONFIG_STM32_HAVE_TIM17=y +CONFIG_STM32_HAVE_ADC2=y +# CONFIG_STM32_HAVE_ADC3 is not set +# CONFIG_STM32_HAVE_ADC4 is not set +# CONFIG_STM32_HAVE_CAN1 is not set +# CONFIG_STM32_HAVE_CAN2 is not set +# CONFIG_STM32_HAVE_RNG is not set +# CONFIG_STM32_HAVE_ETHMAC is not set +# CONFIG_STM32_HAVE_SPI2 is not set +# CONFIG_STM32_HAVE_SPI3 is not set +# CONFIG_STM32_HAVE_SPI4 is not set +# CONFIG_STM32_HAVE_SPI5 is not set +CONFIG_STM32_ADC1=y +# CONFIG_STM32_ADC2 is not set +# CONFIG_STM32_BKP is not set +# CONFIG_STM32_CEC is not set +# CONFIG_STM32_CRC is not set CONFIG_STM32_DMA1=y -CONFIG_STM32_DMA2=n -CONFIG_STM32_CRC=n -# APB1: -# Timers 2,3 and 4 are owned by the PWM driver -CONFIG_STM32_TIM2=n -CONFIG_STM32_TIM3=n -CONFIG_STM32_TIM4=n -CONFIG_STM32_TIM5=n -CONFIG_STM32_TIM6=n -CONFIG_STM32_TIM7=n -CONFIG_STM32_WWDG=n -CONFIG_STM32_SPI2=n +# CONFIG_STM32_DAC1 is not set +# CONFIG_STM32_DAC2 is not set +CONFIG_STM32_I2C1=y +# CONFIG_STM32_I2C2 is not set +# CONFIG_STM32_PWR is not set +# CONFIG_STM32_SPI1 is not set +# CONFIG_STM32_TIM1 is not set +# CONFIG_STM32_TIM2 is not set +# CONFIG_STM32_TIM3 is not set +# CONFIG_STM32_TIM4 is not set +# CONFIG_STM32_TIM5 is not set +# CONFIG_STM32_TIM6 is not set +# CONFIG_STM32_TIM7 is not set +# CONFIG_STM32_TIM12 is not set +# CONFIG_STM32_TIM13 is not set +# CONFIG_STM32_TIM14 is not set +# CONFIG_STM32_TIM15 is not set +# CONFIG_STM32_TIM16 is not set +# CONFIG_STM32_TIM17 is not set +CONFIG_STM32_USART1=y CONFIG_STM32_USART2=y CONFIG_STM32_USART3=y -CONFIG_STM32_I2C1=n -CONFIG_STM32_I2C2=n -CONFIG_STM32_BKP=n -CONFIG_STM32_PWR=n -CONFIG_STM32_DAC=n -# APB2: -# We use our own ADC driver, but leave this on for clocking purposes. -CONFIG_STM32_ADC1=y -CONFIG_STM32_ADC2=n -# TIM1 is owned by the HRT -CONFIG_STM32_TIM1=n -CONFIG_STM32_SPI1=n -CONFIG_STM32_TIM8=n -CONFIG_STM32_USART1=y -CONFIG_STM32_ADC3=n +# CONFIG_STM32_UART4 is not set +# CONFIG_STM32_UART5 is not set +# CONFIG_STM32_IWDG is not set +# CONFIG_STM32_WWDG is not set +CONFIG_STM32_ADC=y +CONFIG_STM32_I2C=y +# +# Alternate Pin Mapping +# +# CONFIG_STM32_I2C1_REMAP is not set +# CONFIG_STM32_USART1_REMAP is not set +# CONFIG_STM32_USART2_REMAP is not set +CONFIG_STM32_USART3_NO_REMAP=y +# CONFIG_STM32_USART3_FULL_REMAP is not set +# CONFIG_STM32_USART3_PARTIAL_REMAP is not set +# CONFIG_STM32_JTAG_DISABLE is not set +# CONFIG_STM32_JTAG_FULL_ENABLE is not set +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set +CONFIG_STM32_JTAG_SW_ENABLE=y +CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y +# CONFIG_STM32_FORCEPOWER is not set +# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set +# CONFIG_STM32_DMACAPABLE is not set +CONFIG_STM32_USART=y # -# STM32F100 specific serial device driver settings +# U[S]ART Configuration +# +# CONFIG_USART1_RS485 is not set +CONFIG_USART1_RXDMA=y +# CONFIG_USART2_RS485 is not set +# CONFIG_USART2_RXDMA is not set +# CONFIG_USART3_RS485 is not set +CONFIG_USART3_RXDMA=y +# CONFIG_SERIAL_DISABLE_REORDERING is not set +# CONFIG_STM32_FLOWCONTROL_BROKEN is not set +CONFIG_STM32_USART_SINGLEWIRE=y + # -# CONFIG_USARTn_SERIAL_CONSOLE - selects the USARTn for the -# console and ttys0 (default is the USART1). -# CONFIG_USARTn_RXBUFSIZE - Characters are buffered as received. -# This specific the size of the receive buffer -# CONFIG_USARTn_TXBUFSIZE - Characters are buffered before -# being sent. This specific the size of the transmit buffer -# CONFIG_USARTn_BAUD - The configure BAUD of the UART. Must be -# CONFIG_USARTn_BITS - The number of bits. Must be either 7 or 8. -# CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity -# CONFIG_USARTn_2STOP - Two stop bits +# I2C Configuration # -CONFIG_SERIAL_TERMIOS=y -CONFIG_STANDARD_SERIAL=y +# CONFIG_STM32_I2C_ALT is not set +# CONFIG_STM32_I2C_DYNTIMEO is not set +CONFIG_STM32_I2CTIMEOSEC=0 +CONFIG_STM32_I2CTIMEOMS=1 +CONFIG_STM32_I2CTIMEOTICKS=1 +# CONFIG_STM32_I2C_DUTY16_9 is not set -CONFIG_USART1_SERIAL_CONSOLE=y -CONFIG_USART2_SERIAL_CONSOLE=n -CONFIG_USART3_SERIAL_CONSOLE=n +# +# USB FS Host Configuration +# -CONFIG_USART1_TXBUFSIZE=64 -CONFIG_USART2_TXBUFSIZE=64 -CONFIG_USART3_TXBUFSIZE=64 +# +# USB HS Host Configuration +# -CONFIG_USART1_RXBUFSIZE=64 -CONFIG_USART2_RXBUFSIZE=64 -CONFIG_USART3_RXBUFSIZE=64 +# +# USB Host Debug Configuration +# -CONFIG_USART1_BAUD=115200 -CONFIG_USART2_BAUD=115200 -CONFIG_USART3_BAUD=115200 +# +# USB Device Configuration +# -CONFIG_USART1_BITS=8 -CONFIG_USART2_BITS=8 -CONFIG_USART3_BITS=8 +# +# Architecture Options +# +# CONFIG_ARCH_NOINTC is not set +# CONFIG_ARCH_VECNOTIRQ is not set +CONFIG_ARCH_DMA=y +CONFIG_ARCH_HAVE_IRQPRIO=y +# CONFIG_ARCH_L2CACHE is not set +# CONFIG_ARCH_HAVE_COHERENT_DCACHE is not set +# CONFIG_ARCH_HAVE_ADDRENV is not set +# CONFIG_ARCH_NEED_ADDRENV_MAPPING is not set +CONFIG_ARCH_HAVE_VFORK=y +# CONFIG_ARCH_HAVE_MMU is not set +CONFIG_ARCH_HAVE_MPU=y +# CONFIG_ARCH_NAND_HWECC is not set +# CONFIG_ARCH_HAVE_EXTCLK is not set +# CONFIG_ARCH_USE_MPU is not set +# CONFIG_ARCH_IRQPRIO is not set +CONFIG_ARCH_STACKDUMP=y +# CONFIG_ENDIAN_BIG is not set +# CONFIG_ARCH_IDLE_CUSTOM is not set +# CONFIG_ARCH_HAVE_RAMFUNCS is not set +CONFIG_ARCH_HAVE_RAMVECTORS=y +# CONFIG_ARCH_RAMVECTORS is not set -CONFIG_USART1_PARITY=0 -CONFIG_USART2_PARITY=0 -CONFIG_USART3_PARITY=0 +# +# Board Settings +# +CONFIG_BOARD_LOOPSPERMSEC=2000 +# CONFIG_ARCH_CALIBRATION is not set -CONFIG_USART1_2STOP=0 -CONFIG_USART2_2STOP=0 -CONFIG_USART3_2STOP=0 +# +# Interrupt options +# +CONFIG_ARCH_HAVE_INTERRUPTSTACK=y +CONFIG_ARCH_INTERRUPTSTACK=0 +CONFIG_ARCH_HAVE_HIPRI_INTERRUPT=y +# CONFIG_ARCH_HIPRI_INTERRUPT is not set -CONFIG_USART1_RXDMA=y -SERIAL_HAVE_CONSOLE_DMA=y -# Conflicts with I2C1 DMA -CONFIG_USART2_RXDMA=n -CONFIG_USART3_RXDMA=y +# +# Boot options +# +# CONFIG_BOOT_RUNFROMEXTSRAM is not set +CONFIG_BOOT_RUNFROMFLASH=y +# CONFIG_BOOT_RUNFROMISRAM is not set +# CONFIG_BOOT_RUNFROMSDRAM is not set +# CONFIG_BOOT_COPYTORAM is not set # -# PX4IO specific driver settings -# -# CONFIG_HRT_TIMER -# Enables the high-resolution timer. The board definition must -# set HRT_TIMER and HRT_TIMER_CHANNEL to the timer and capture/ -# compare channels to be used. -# CONFIG_HRT_PPM -# Enables R/C PPM input using the HRT. The board definition must -# set HRT_PPM_CHANNEL to the timer capture/compare channel to be -# used, and define GPIO_PPM_IN to configure the appropriate timer -# GPIO. -# CONFIG_PWM_SERVO -# Enables the PWM servo driver. The driver configuration must be -# supplied by the board support at initialisation time. -# Note that USART2 must be disabled on the PX4 board for this to -# be available. -# -# -CONFIG_HRT_TIMER=y -CONFIG_HRT_PPM=y - -# -# General build options -# -# CONFIG_RRLOAD_BINARY - make the rrload binary format used with -# BSPs from www.ridgerun.com using the tools/mkimage.sh script -# CONFIG_INTELHEX_BINARY - make the Intel HEX binary format -# used with many different loaders using the GNU objcopy program -# Should not be selected if you are not using the GNU toolchain. -# CONFIG_MOTOROLA_SREC - make the Motorola S-Record binary format -# used with many different loaders using the GNU objcopy program -# Should not be selected if you are not using the GNU toolchain. -# CONFIG_RAW_BINARY - make a raw binary format file used with many -# different loaders using the GNU objcopy program. This option -# should not be selected if you are not using the GNU toolchain. -# CONFIG_HAVE_LIBM - toolchain supports libm.a -# -CONFIG_RRLOAD_BINARY=n -CONFIG_INTELHEX_BINARY=n -CONFIG_MOTOROLA_SREC=n -CONFIG_RAW_BINARY=y -CONFIG_HAVE_LIBM=n - -# -# General OS setup -# -# CONFIG_APPS_DIR - Identifies the relative path to the directory -# that builds the application to link with NuttX. Default: ../apps -# CONFIG_DEBUG - enables built-in debug options -# CONFIG_DEBUG_VERBOSE - enables verbose debug output -# CONFIG_DEBUG_SYMBOLS - build without optimization and with -# debug symbols (needed for use with a debugger). -# CONFIG_HAVE_CXX - Enable support for C++ -# CONFIG_HAVE_CXXINITIALIZE - The platform-specific logic includes support -# for initialization of static C++ instances for this architecture -# and for the selected toolchain (via up_cxxinitialize()). -# CONFIG_MM_REGIONS - If the architecture includes multiple -# regions of memory to allocate from, this specifies the -# number of memory regions that the memory manager must -# handle and enables the API mm_addregion(start, end); -# CONFIG_ARCH_LOWPUTC - architecture supports low-level, boot -# time console output -# CONFIG_MSEC_PER_TICK - The default system timer is 100Hz -# or MSEC_PER_TICK=10. This setting may be defined to -# inform NuttX that the processor hardware is providing -# system timer interrupts at some interrupt interval other -# than 10 msec. -# CONFIG_RR_INTERVAL - The round robin timeslice will be set -# this number of milliseconds; Round robin scheduling can -# be disabled by setting this value to zero. -# CONFIG_SCHED_INSTRUMENTATION - enables instrumentation in -# scheduler to monitor system performance -# CONFIG_TASK_NAME_SIZE - Spcifies that maximum size of a -# task name to save in the TCB. Useful if scheduler -# instrumentation is selected. Set to zero to disable. -# CONFIG_START_YEAR, CONFIG_START_MONTH, CONFIG_START_DAY - -# Used to initialize the internal time logic. -# CONFIG_GREGORIAN_TIME - Enables Gregorian time conversions. -# You would only need this if you are concerned about accurate -# time conversions in the past or in the distant future. -# CONFIG_JULIAN_TIME - Enables Julian time conversions. You -# would only need this if you are concerned about accurate -# time conversion in the distand past. You must also define -# CONFIG_GREGORIAN_TIME in order to use Julian time. -# CONFIG_DEV_CONSOLE - Set if architecture-specific logic -# provides /dev/console. Enables stdout, stderr, stdin. -# CONFIG_DEV_LOWCONSOLE - Use the simple, low-level serial console -# driver (minimul support) -# CONFIG_MUTEX_TYPES: Set to enable support for recursive and -# errorcheck mutexes. Enables pthread_mutexattr_settype(). -# CONFIG_PRIORITY_INHERITANCE : Set to enable support for priority -# inheritance on mutexes and semaphores. -# CONFIG_SEM_PREALLOCHOLDERS: This setting is only used if priority -# inheritance is enabled. It defines the maximum number of -# different threads (minus one) that can take counts on a -# semaphore with priority inheritance support. This may be -# set to zero if priority inheritance is disabled OR if you -# are only using semaphores as mutexes (only one holder) OR -# if no more than two threads participate using a counting -# semaphore. -# CONFIG_SEM_NNESTPRIO. If priority inheritance is enabled, -# then this setting is the maximum number of higher priority -# threads (minus 1) than can be waiting for another thread -# to release a count on a semaphore. This value may be set -# to zero if no more than one thread is expected to wait for -# a semaphore. -# CONFIG_FDCLONE_DISABLE. Disable cloning of all file descriptors -# by task_create() when a new task is started. If set, all -# files/drivers will appear to be closed in the new task. -# CONFIG_FDCLONE_STDIO. Disable cloning of all but the first -# three file descriptors (stdin, stdout, stderr) by task_create() -# when a new task is started. If set, all files/drivers will -# appear to be closed in the new task except for stdin, stdout, -# and stderr. -# CONFIG_SDCLONE_DISABLE. Disable cloning of all socket -# desciptors by task_create() when a new task is started. If -# set, all sockets will appear to be closed in the new task. -# CONFIG_SCHED_WORKQUEUE. Create a dedicated "worker" thread to -# handle delayed processing from interrupt handlers. This feature -# is required for some drivers but, if there are not complaints, -# can be safely disabled. The worker thread also performs -# garbage collection -- completing any delayed memory deallocations -# from interrupt handlers. If the worker thread is disabled, -# then that clean will be performed by the IDLE thread instead -# (which runs at the lowest of priority and may not be appropriate -# if memory reclamation is of high priority). If CONFIG_SCHED_WORKQUEUE -# is enabled, then the following options can also be used: -# CONFIG_SCHED_WORKPRIORITY - The execution priority of the worker -# thread. Default: 50 -# CONFIG_SCHED_WORKPERIOD - How often the worker thread checks for -# work in units of microseconds. Default: 50*1000 (50 MS). -# CONFIG_SCHED_WORKSTACKSIZE - The stack size allocated for the worker -# thread. Default: CONFIG_IDLETHREAD_STACKSIZE. -# CONFIG_SIG_SIGWORK - The signal number that will be used to wake-up -# the worker thread. Default: 4 -# CONFIG_SCHED_WAITPID - Enable the waitpid() API -# CONFIG_SCHED_ATEXIT - Enabled the atexit() API +# Boot Memory Configuration # -CONFIG_USER_ENTRYPOINT="user_start" -#CONFIG_APPS_DIR= -CONFIG_DEBUG=n -CONFIG_DEBUG_VERBOSE=n -CONFIG_DEBUG_SYMBOLS=y -CONFIG_DEBUG_FS=n -CONFIG_DEBUG_GRAPHICS=n -CONFIG_DEBUG_LCD=n -CONFIG_DEBUG_USB=n -CONFIG_DEBUG_NET=n -CONFIG_DEBUG_RTC=n -CONFIG_DEBUG_ANALOG=n -CONFIG_DEBUG_PWM=n -CONFIG_DEBUG_CAN=n -CONFIG_DEBUG_I2C=n -CONFIG_DEBUG_INPUT=n - -CONFIG_MSEC_PER_TICK=1 -CONFIG_HAVE_CXX=y -CONFIG_HAVE_CXXINITIALIZE=y -CONFIG_MM_REGIONS=1 -CONFIG_MM_SMALL=y -CONFIG_ARCH_LOWPUTC=y -CONFIG_RR_INTERVAL=0 -CONFIG_SCHED_INSTRUMENTATION=n -CONFIG_TASK_NAME_SIZE=8 -CONFIG_START_YEAR=1970 -CONFIG_START_MONTH=1 -CONFIG_START_DAY=1 -CONFIG_GREGORIAN_TIME=n -CONFIG_JULIAN_TIME=n -# this eats ~1KiB of RAM ... work out why -CONFIG_DEV_CONSOLE=y -CONFIG_DEV_LOWCONSOLE=n -CONFIG_MUTEX_TYPES=n -CONFIG_PRIORITY_INHERITANCE=n -CONFIG_SEM_PREALLOCHOLDERS=0 -CONFIG_SEM_NNESTPRIO=0 -CONFIG_FDCLONE_DISABLE=y -CONFIG_FDCLONE_STDIO=y -CONFIG_SDCLONE_DISABLE=y -CONFIG_SCHED_WORKQUEUE=n -CONFIG_SCHED_WORKPRIORITY=50 -CONFIG_SCHED_WORKPERIOD=50000 -CONFIG_SCHED_WORKSTACKSIZE=1024 -CONFIG_SIG_SIGWORK=4 -CONFIG_SCHED_WAITPID=n -CONFIG_SCHED_ATEXIT=n - -# -# The following can be used to disable categories of -# APIs supported by the OS. If the compiler supports -# weak functions, then it should not be necessary to -# disable functions unless you want to restrict usage -# of those APIs. -# -# There are certain dependency relationships in these -# features. -# -# o mq_notify logic depends on signals to awaken tasks -# waiting for queues to become full or empty. -# o pthread_condtimedwait() depends on signals to wake -# up waiting tasks. -# -CONFIG_DISABLE_CLOCK=n +CONFIG_RAM_START=0x20000000 +CONFIG_RAM_SIZE=8192 +# CONFIG_ARCH_HAVE_SDRAM is not set + +# +# Board Selection +# +CONFIG_ARCH_BOARD_PX4IO_V2=y +# CONFIG_ARCH_BOARD_CUSTOM is not set +CONFIG_ARCH_BOARD="px4io-v2" + +# +# Custom Board Configuration +# +CONFIG_ARCH_BOARD_CUSTOM_DIR="" +# CONFIG_BOARD_CUSTOM_LEDS is not set +# CONFIG_BOARD_CUSTOM_BUTTONS is not set + +# +# Common Board Options +# + +# +# Board-Specific Options +# + +# +# RTOS Features +# +CONFIG_DISABLE_OS_API=y CONFIG_DISABLE_POSIX_TIMERS=y CONFIG_DISABLE_PTHREAD=y CONFIG_DISABLE_SIGNALS=y CONFIG_DISABLE_MQUEUE=y -CONFIG_DISABLE_MOUNTPOINT=y CONFIG_DISABLE_ENVIRON=y -CONFIG_DISABLE_POLL=y # -# Misc libc settings -# -# CONFIG_NOPRINTF_FIELDWIDTH - sprintf-related logic is a -# little smaller if we do not support fieldwidthes -# -CONFIG_NOPRINTF_FIELDWIDTH=n - -# -# Allow for architecture optimized implementations -# -# The architecture can provide optimized versions of the -# following to improve system performance -# -CONFIG_ARCH_MEMCPY=n -CONFIG_ARCH_MEMCMP=n -CONFIG_ARCH_MEMMOVE=n -CONFIG_ARCH_MEMSET=n -CONFIG_ARCH_STRCMP=n -CONFIG_ARCH_STRCPY=n -CONFIG_ARCH_STRNCPY=n -CONFIG_ARCH_STRLEN=n -CONFIG_ARCH_STRNLEN=n -CONFIG_ARCH_BZERO=n - -# -# Sizes of configurable things (0 disables) -# -# CONFIG_MAX_TASKS - The maximum number of simultaneously -# active tasks. This value must be a power of two. -# CONFIG_MAX_TASK_ARGS - This controls the maximum number of -# of parameters that a task may receive (i.e., maxmum value -# of 'argc') -# CONFIG_NPTHREAD_KEYS - The number of items of thread- -# specific data that can be retained -# CONFIG_NFILE_DESCRIPTORS - The maximum number of file -# descriptors (one for each open) -# CONFIG_NFILE_STREAMS - The maximum number of streams that -# can be fopen'ed -# CONFIG_NAME_MAX - The maximum size of a file name. -# CONFIG_STDIO_BUFFER_SIZE - Size of the buffer to allocate -# on fopen. (Only if CONFIG_NFILE_STREAMS > 0) -# CONFIG_STDIO_LINEBUFFER - If standard C buffered I/O is enabled -# (CONFIG_STDIO_BUFFER_SIZE > 0), then this option may be added -# to force automatic, line-oriented flushing the output buffer -# for putc(), fputc(), putchar(), puts(), fputs(), printf(), -# fprintf(), and vfprintf(). When a newline is encountered in -# the output string, the output buffer will be flushed. This -# (slightly) increases the NuttX footprint but supports the kind -# of behavior that people expect for printf(). -# CONFIG_NUNGET_CHARS - Number of characters that can be -# buffered by ungetc() (Only if CONFIG_NFILE_STREAMS > 0) -# CONFIG_PREALLOC_MQ_MSGS - The number of pre-allocated message -# structures. The system manages a pool of preallocated -# message structures to minimize dynamic allocations -# CONFIG_MQ_MAXMSGSIZE - Message structures are allocated with -# a fixed payload size given by this settin (does not include -# other message structure overhead. -# CONFIG_MAX_WDOGPARMS - Maximum number of parameters that -# can be passed to a watchdog handler -# CONFIG_PREALLOC_WDOGS - The number of pre-allocated watchdog -# structures. The system manages a pool of preallocated -# watchdog structures to minimize dynamic allocations -# CONFIG_PREALLOC_TIMERS - The number of pre-allocated POSIX -# timer structures. The system manages a pool of preallocated -# timer structures to minimize dynamic allocations. Set to -# zero for all dynamic allocations. +# Clocks and Timers # -CONFIG_MAX_TASKS=4 -CONFIG_MAX_TASK_ARGS=4 -CONFIG_NPTHREAD_KEYS=2 -CONFIG_NFILE_DESCRIPTORS=8 -CONFIG_NFILE_STREAMS=0 -CONFIG_NAME_MAX=12 -CONFIG_STDIO_BUFFER_SIZE=32 -CONFIG_STDIO_LINEBUFFER=n -CONFIG_NUNGET_CHARS=2 -CONFIG_PREALLOC_MQ_MSGS=4 -CONFIG_MQ_MAXMSGSIZE=32 +CONFIG_USEC_PER_TICK=1000 +# CONFIG_SYSTEM_TIME64 is not set +# CONFIG_CLOCK_MONOTONIC is not set +# CONFIG_JULIAN_TIME is not set +CONFIG_START_YEAR=1970 +CONFIG_START_MONTH=1 +CONFIG_START_DAY=1 CONFIG_MAX_WDOGPARMS=2 -CONFIG_PREALLOC_WDOGS=4 +CONFIG_PREALLOC_WDOGS=6 +CONFIG_WDOG_INTRESERVE=2 CONFIG_PREALLOC_TIMERS=0 +# +# Tasks and Scheduling +# +# CONFIG_INIT_NONE is not set +CONFIG_INIT_ENTRYPOINT=y +# CONFIG_INIT_FILEPATH is not set +CONFIG_USER_ENTRYPOINT="user_start" +CONFIG_RR_INTERVAL=0 +CONFIG_TASK_NAME_SIZE=8 +CONFIG_MAX_TASKS=4 +# CONFIG_SCHED_HAVE_PARENT is not set +# CONFIG_SCHED_WAITPID is not set # -# Settings for apps/nshlib +# Performance Monitoring # -# CONFIG_NSH_BUILTIN_APPS - Support external registered, -# "named" applications that can be executed from the NSH -# command line (see apps/README.txt for more information). -# CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer -# CONFIG_NSH_STRERROR - Use strerror(errno) -# CONFIG_NSH_LINELEN - Maximum length of one command line -# CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi -# CONFIG_NSH_DISABLESCRIPT - Disable scripting support -# CONFIG_NSH_DISABLEBG - Disable background commands -# CONFIG_NSH_ROMFSETC - Use startup script in /etc -# CONFIG_NSH_CONSOLE - Use serial console front end -# CONFIG_NSH_TELNET - Use telnetd console front end -# CONFIG_NSH_ARCHINIT - Platform provides architecture -# specific initialization (nsh_archinitialize()). +# CONFIG_SCHED_CPULOAD is not set +# CONFIG_SCHED_INSTRUMENTATION is not set + +# +# Files and I/O # +CONFIG_DEV_CONSOLE=y +CONFIG_FDCLONE_DISABLE=y +CONFIG_FDCLONE_STDIO=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_NFILE_DESCRIPTORS=8 +CONFIG_NFILE_STREAMS=0 +CONFIG_NAME_MAX=12 +# CONFIG_PRIORITY_INHERITANCE is not set -# Disable NSH completely -CONFIG_NSH_CONSOLE=n +# +# RTOS hooks +# +# CONFIG_BOARD_INITIALIZE is not set +# CONFIG_SCHED_STARTHOOK is not set +# CONFIG_SCHED_ATEXIT is not set +# CONFIG_SCHED_ONEXIT is not set + +# +# Work Queue Support +# # # Stack and heap information # -# CONFIG_BOOT_RUNFROMFLASH - Some configurations support XIP -# operation from FLASH but must copy initialized .data sections to RAM. -# (should also be =n for the STM3210E-EVAL which always runs from flash) -# CONFIG_BOOT_COPYTORAM - Some configurations boot in FLASH -# but copy themselves entirely into RAM for better performance. -# CONFIG_CUSTOM_STACK - The up_ implementation will handle -# all stack operations outside of the nuttx model. -# CONFIG_STACK_POINTER - The initial stack pointer (arm7tdmi only) -# CONFIG_IDLETHREAD_STACKSIZE - The size of the initial stack. -# This is the thread that (1) performs the inital boot of the system up -# to the point where user_start() is spawned, and (2) there after is the -# IDLE thread that executes only when there is no other thread ready to -# run. -# CONFIG_USERMAIN_STACKSIZE - The size of the stack to allocate -# for the main user thread that begins at the user_start() entry point. -# CONFIG_PTHREAD_STACK_MIN - Minimum pthread stack size -# CONFIG_PTHREAD_STACK_DEFAULT - Default pthread stack size -# CONFIG_HEAP_BASE - The beginning of the heap -# CONFIG_HEAP_SIZE - The size of the heap -# -CONFIG_BOOT_RUNFROMFLASH=n -CONFIG_BOOT_COPYTORAM=n -CONFIG_CUSTOM_STACK=n -CONFIG_STACK_POINTER= CONFIG_IDLETHREAD_STACKSIZE=1024 CONFIG_USERMAIN_STACKSIZE=1200 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_PTHREAD_STACK_DEFAULT=1024 -CONFIG_HEAP_BASE= -CONFIG_HEAP_SIZE= +# CONFIG_LIB_SYSCALL is not set + +# +# Device Drivers +# +CONFIG_DISABLE_POLL=y +# CONFIG_DEV_NULL is not set +# CONFIG_DEV_ZERO is not set +# CONFIG_LOOP is not set + +# +# Buffering +# +# CONFIG_DRVR_WRITEBUFFER is not set +# CONFIG_DRVR_READAHEAD is not set +# CONFIG_RAMDISK is not set +# CONFIG_CAN is not set +# CONFIG_ARCH_HAVE_PWM_PULSECOUNT is not set +# CONFIG_PWM is not set +CONFIG_ARCH_HAVE_I2CRESET=y +# CONFIG_I2C is not set +# CONFIG_SPI is not set +# CONFIG_I2S is not set +# CONFIG_RTC is not set +# CONFIG_WATCHDOG is not set +# CONFIG_TIMER is not set +# CONFIG_ANALOG is not set +# CONFIG_AUDIO_DEVICES is not set +# CONFIG_VIDEO_DEVICES is not set +# CONFIG_BCH is not set +# CONFIG_INPUT is not set +# CONFIG_LCD is not set +# CONFIG_MMCSD is not set +# CONFIG_MTD is not set +# CONFIG_EEPROM is not set +# CONFIG_PIPES is not set +# CONFIG_PM is not set +# CONFIG_POWER is not set +# CONFIG_SENSORS is not set +# CONFIG_SERCOMM_CONSOLE is not set +CONFIG_SERIAL=y +# CONFIG_DEV_LOWCONSOLE is not set +# CONFIG_16550_UART is not set +# CONFIG_ARCH_HAVE_UART is not set +# CONFIG_ARCH_HAVE_UART0 is not set +# CONFIG_ARCH_HAVE_UART1 is not set +# CONFIG_ARCH_HAVE_UART2 is not set +# CONFIG_ARCH_HAVE_UART3 is not set +# CONFIG_ARCH_HAVE_UART4 is not set +# CONFIG_ARCH_HAVE_UART5 is not set +# CONFIG_ARCH_HAVE_UART6 is not set +# CONFIG_ARCH_HAVE_UART7 is not set +# CONFIG_ARCH_HAVE_UART8 is not set +# CONFIG_ARCH_HAVE_SCI0 is not set +# CONFIG_ARCH_HAVE_SCI1 is not set +# CONFIG_ARCH_HAVE_USART0 is not set +CONFIG_ARCH_HAVE_USART1=y +CONFIG_ARCH_HAVE_USART2=y +CONFIG_ARCH_HAVE_USART3=y +# CONFIG_ARCH_HAVE_USART4 is not set +# CONFIG_ARCH_HAVE_USART5 is not set +# CONFIG_ARCH_HAVE_USART6 is not set +# CONFIG_ARCH_HAVE_USART7 is not set +# CONFIG_ARCH_HAVE_USART8 is not set +# CONFIG_ARCH_HAVE_OTHER_UART is not set + +# +# USART Configuration +# +CONFIG_USART1_ISUART=y +CONFIG_USART2_ISUART=y +CONFIG_USART3_ISUART=y +CONFIG_MCU_SERIAL=y +CONFIG_STANDARD_SERIAL=y +# CONFIG_SERIAL_IFLOWCONTROL is not set +# CONFIG_SERIAL_OFLOWCONTROL is not set +CONFIG_ARCH_HAVE_SERIAL_TERMIOS=y +CONFIG_SERIAL_TERMIOS=y +CONFIG_USART1_SERIAL_CONSOLE=y +# CONFIG_USART2_SERIAL_CONSOLE is not set +# CONFIG_USART3_SERIAL_CONSOLE is not set +# CONFIG_OTHER_SERIAL_CONSOLE is not set +# CONFIG_NO_SERIAL_CONSOLE is not set + +# +# USART1 Configuration +# +CONFIG_USART1_RXBUFSIZE=64 +CONFIG_USART1_TXBUFSIZE=64 +CONFIG_USART1_BAUD=115200 +CONFIG_USART1_BITS=8 +CONFIG_USART1_PARITY=0 +CONFIG_USART1_2STOP=0 +# CONFIG_USART1_IFLOWCONTROL is not set +# CONFIG_USART1_OFLOWCONTROL is not set + +# +# USART2 Configuration +# +CONFIG_USART2_RXBUFSIZE=64 +CONFIG_USART2_TXBUFSIZE=64 +CONFIG_USART2_BAUD=115200 +CONFIG_USART2_BITS=8 +CONFIG_USART2_PARITY=0 +CONFIG_USART2_2STOP=0 +# CONFIG_USART2_IFLOWCONTROL is not set +# CONFIG_USART2_OFLOWCONTROL is not set + +# +# USART3 Configuration +# +CONFIG_USART3_RXBUFSIZE=64 +CONFIG_USART3_TXBUFSIZE=64 +CONFIG_USART3_BAUD=115200 +CONFIG_USART3_BITS=8 +CONFIG_USART3_PARITY=0 +CONFIG_USART3_2STOP=0 +# CONFIG_USART3_IFLOWCONTROL is not set +# CONFIG_USART3_OFLOWCONTROL is not set +# CONFIG_USBDEV is not set +# CONFIG_USBHOST is not set +# CONFIG_WIRELESS is not set + +# +# System Logging Device Options +# + +# +# System Logging +# +# CONFIG_RAMLOG is not set + +# +# Networking Support +# +# CONFIG_ARCH_HAVE_NET is not set +# CONFIG_ARCH_HAVE_PHY is not set +# CONFIG_NET is not set + +# +# Crypto API +# +# CONFIG_CRYPTO is not set + +# +# File Systems +# + +# +# File system configuration +# +CONFIG_DISABLE_MOUNTPOINT=y +CONFIG_DISABLE_PSEUDOFS_OPERATIONS=y +# CONFIG_FS_READABLE is not set +# CONFIG_FS_WRITABLE is not set +# CONFIG_FS_NAMED_SEMAPHORES is not set +# CONFIG_FS_RAMMAP is not set +# CONFIG_FS_PROCFS is not set + +# +# System Logging +# +# CONFIG_SYSLOG is not set +# CONFIG_SYSLOG_TIMESTAMP is not set + +# +# Graphics Support +# +# CONFIG_NX is not set + +# +# Memory Management +# +CONFIG_MM_SMALL=y +CONFIG_MM_REGIONS=1 +# CONFIG_ARCH_HAVE_HEAP2 is not set +# CONFIG_GRAN is not set + +# +# Audio Support +# +# CONFIG_AUDIO is not set + +# +# Binary Loader +# +# CONFIG_BINFMT_DISABLE is not set +# CONFIG_NXFLAT is not set +# CONFIG_ELF is not set +# CONFIG_BUILTIN is not set +# CONFIG_PIC is not set +# CONFIG_SYMTAB_ORDEREDBYNAME is not set + +# +# Library Routines +# + +# +# Standard C Library Options +# +CONFIG_STDIO_BUFFER_SIZE=32 +# CONFIG_STDIO_LINEBUFFER is not set +CONFIG_NUNGET_CHARS=2 +# CONFIG_NOPRINTF_FIELDWIDTH is not set +# CONFIG_LIBC_FLOATINGPOINT is not set +# CONFIG_LIBC_IOCTL_VARIADIC is not set +CONFIG_LIB_RAND_ORDER=1 +# CONFIG_EOL_IS_CR is not set +# CONFIG_EOL_IS_LF is not set +# CONFIG_EOL_IS_BOTH_CRLF is not set +CONFIG_EOL_IS_EITHER_CRLF=y +# CONFIG_LIBC_EXECFUNCS is not set +CONFIG_POSIX_SPAWN_PROXY_STACKSIZE=1024 +CONFIG_TASK_SPAWN_DEFAULT_STACKSIZE=1024 +# CONFIG_LIBC_STRERROR is not set +# CONFIG_LIBC_PERROR_STDOUT is not set +CONFIG_ARCH_LOWPUTC=y +CONFIG_LIB_SENDFILE_BUFSIZE=512 +# CONFIG_ARCH_ROMGETC is not set +# CONFIG_ARCH_OPTIMIZED_FUNCTIONS is not set + +# +# Non-standard Library Support +# +# CONFIG_LIB_KBDCODEC is not set +# CONFIG_LIB_SLCDCODEC is not set + +# +# Basic CXX Support +# +CONFIG_C99_BOOL8=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +# CONFIG_CXX_NEWLONG is not set + +# +# uClibc++ Standard C++ Library +# +# CONFIG_UCLIBCXX is not set + +# +# Application Configuration +# + +# +# Built-In Applications +# + +# +# Examples +# +# CONFIG_EXAMPLES_BUTTONS is not set +# CONFIG_EXAMPLES_CAN is not set +# CONFIG_EXAMPLES_CONFIGDATA is not set +# CONFIG_EXAMPLES_CPUHOG is not set +# CONFIG_EXAMPLES_CXXTEST is not set +# CONFIG_EXAMPLES_DHCPD is not set +# CONFIG_EXAMPLES_ELF is not set +# CONFIG_EXAMPLES_FTPC is not set +# CONFIG_EXAMPLES_FTPD is not set +# CONFIG_EXAMPLES_HELLO is not set +# CONFIG_EXAMPLES_HELLOXX is not set +# CONFIG_EXAMPLES_JSON is not set +# CONFIG_EXAMPLES_HIDKBD is not set +# CONFIG_EXAMPLES_KEYPADTEST is not set +# CONFIG_EXAMPLES_IGMP is not set +# CONFIG_EXAMPLES_MM is not set +# CONFIG_EXAMPLES_MODBUS is not set +# CONFIG_EXAMPLES_MOUNT is not set +# CONFIG_EXAMPLES_NRF24L01TERM is not set +# CONFIG_EXAMPLES_NSH is not set +# CONFIG_EXAMPLES_NULL is not set +# CONFIG_EXAMPLES_NX is not set +# CONFIG_EXAMPLES_NXTERM is not set +# CONFIG_EXAMPLES_NXFFS is not set +# CONFIG_EXAMPLES_NXFLAT is not set +# CONFIG_EXAMPLES_NXHELLO is not set +# CONFIG_EXAMPLES_NXIMAGE is not set +# CONFIG_EXAMPLES_NXLINES is not set +# CONFIG_EXAMPLES_NXTEXT is not set +# CONFIG_EXAMPLES_OSTEST is not set +# CONFIG_EXAMPLES_PIPE is not set +# CONFIG_EXAMPLES_POLL is not set +# CONFIG_EXAMPLES_POSIXSPAWN is not set +# CONFIG_EXAMPLES_QENCODER is not set +# CONFIG_EXAMPLES_RGMP is not set +# CONFIG_EXAMPLES_ROMFS is not set +# CONFIG_EXAMPLES_SENDMAIL is not set +# CONFIG_EXAMPLES_SERIALBLASTER is not set +# CONFIG_EXAMPLES_SERIALRX is not set +# CONFIG_EXAMPLES_SERLOOP is not set +# CONFIG_EXAMPLES_SLCD is not set +# CONFIG_EXAMPLES_SMART is not set +# CONFIG_EXAMPLES_TCPECHO is not set +# CONFIG_EXAMPLES_TELNETD is not set +# CONFIG_EXAMPLES_THTTPD is not set +# CONFIG_EXAMPLES_TIFF is not set +# CONFIG_EXAMPLES_TOUCHSCREEN is not set +# CONFIG_EXAMPLES_UDP is not set +# CONFIG_EXAMPLES_WEBSERVER is not set +# CONFIG_EXAMPLES_USBSERIAL is not set +# CONFIG_EXAMPLES_USBTERM is not set +# CONFIG_EXAMPLES_WATCHDOG is not set + +# +# Graphics Support +# +# CONFIG_TIFF is not set +# CONFIG_GRAPHICS_TRAVELER is not set + +# +# Interpreters +# +# CONFIG_INTERPRETERS_FICL is not set +# CONFIG_INTERPRETERS_PCODE is not set + +# +# Network Utilities +# + +# +# Networking Utilities +# +# CONFIG_NETUTILS_CODECS is not set +# CONFIG_NETUTILS_DHCPD is not set +# CONFIG_NETUTILS_FTPC is not set +# CONFIG_NETUTILS_FTPD is not set +# CONFIG_NETUTILS_JSON is not set +# CONFIG_NETUTILS_SMTP is not set +# CONFIG_NETUTILS_TFTPC is not set +# CONFIG_NETUTILS_THTTPD is not set +# CONFIG_NETUTILS_NETLIB is not set +# CONFIG_NETUTILS_WEBCLIENT is not set + +# +# FreeModBus +# +# CONFIG_MODBUS is not set + +# +# NSH Library +# +# CONFIG_NSH_LIBRARY is not set + +# +# NxWidgets/NxWM +# + +# +# Platform-specific Support +# +# CONFIG_PLATFORM_CONFIGDATA is not set + +# +# System Libraries and NSH Add-Ons +# + +# +# Custom Free Memory Command +# +# CONFIG_SYSTEM_FREE is not set + +# +# EMACS-like Command Line Editor +# +# CONFIG_SYSTEM_CLE is not set + +# +# CU Minimal Terminal +# +# CONFIG_SYSTEM_CUTERM is not set + +# +# FLASH Program Installation +# +# CONFIG_SYSTEM_INSTALL is not set + +# +# FLASH Erase-all Command +# + +# +# Intel HEX to binary conversion +# +# CONFIG_SYSTEM_HEX2BIN is not set + +# +# I2C tool +# + +# +# INI File Parser +# +# CONFIG_SYSTEM_INIFILE is not set + +# +# NxPlayer media player library / command Line +# + +# +# RAM test +# +# CONFIG_SYSTEM_RAMTEST is not set + +# +# readline() +# +# CONFIG_SYSTEM_READLINE is not set + +# +# P-Code Support +# + +# +# PHY Tool +# + +# +# Power Off +# +# CONFIG_SYSTEM_POWEROFF is not set + +# +# RAMTRON +# +# CONFIG_SYSTEM_RAMTRON is not set + +# +# SD Card +# +# CONFIG_SYSTEM_SDCARD is not set + +# +# Sudoku +# +# CONFIG_SYSTEM_SUDOKU is not set + +# +# Sysinfo +# +# CONFIG_SYSTEM_SYSINFO is not set + +# +# VI Work-Alike Editor +# +# CONFIG_SYSTEM_VI is not set + +# +# Stack Monitor +# + +# +# USB CDC/ACM Device Commands +# + +# +# USB Composite Device Commands +# + +# +# USB Mass Storage Device Commands +# + +# +# USB Monitor +# + +# +# Zmodem Commands +# +# CONFIG_SYSTEM_ZMODEM is not set diff --git a/nuttx-patches/Fixed-Shadow-wanings.patch b/nuttx-patches/Fixed-Shadow-wanings.patch new file mode 100755 index 000000000..3c0ab5f7c --- /dev/null +++ b/nuttx-patches/Fixed-Shadow-wanings.patch @@ -0,0 +1,62 @@ +diff --git NuttX/nuttx/include/signal.h NuttX/nuttx/include/signal.h +index ffb77e0..cca6a15 100644 +--- NuttX/nuttx/include/signal.h ++++ NuttX/nuttx/include/signal.h +@@ -260,8 +260,11 @@ int sigfillset(FAR sigset_t *set); + int sigaddset(FAR sigset_t *set, int signo); + int sigdelset(FAR sigset_t *set, int signo); + int sigismember(FAR const sigset_t *set, int signo); ++#pragma GCC diagnostic push ++#pragma GCC diagnostic ignored "-Wshadow" + int sigaction(int sig, FAR const struct sigaction *act, + FAR struct sigaction *oact); ++#pragma GCC diagnostic pop + int sigprocmask(int how, FAR const sigset_t *set, FAR sigset_t *oset); + int sigpending(FAR sigset_t *set); + int sigsuspend(FAR const sigset_t *sigmask); +diff --git NuttX/nuttx/include/stdio.h NuttX/nuttx/include/stdio.h +index cb16366..6ff12bb 100644 +--- NuttX/nuttx/include/stdio.h ++++ NuttX/nuttx/include/stdio.h +@@ -180,7 +180,11 @@ int vdprintf(int fd, FAR const char *fmt, va_list ap); + + /* Operations on paths */ + ++#pragma GCC diagnostic push ++#pragma GCC diagnostic ignored "-Wshadow" ++struct statfs; /* Forward Decleration */ + int statfs(FAR const char *path, FAR struct statfs *buf); ++#pragma GCC diagnostic pop + FAR char *tmpnam(FAR char *s); + FAR char *tempnam(FAR const char *dir, FAR const char *pfx); + +diff --git NuttX/nuttx/include/stdlib.h NuttX/nuttx/include/stdlib.h +index 16a3f93..dbf94df 100644 +--- NuttX/nuttx/include/stdlib.h ++++ NuttX/nuttx/include/stdlib.h +@@ -191,7 +191,10 @@ void qsort(void *base, size_t nmemb, size_t size, + int(*compar)(const void *, const void *)); + + #ifdef CONFIG_CAN_PASS_STRUCTS ++#pragma GCC diagnostic push ++#pragma GCC diagnostic ignored "-Wshadow" + struct mallinfo mallinfo(void); ++#pragma GCC diagnostic pop + #else + int mallinfo(struct mallinfo *info); + #endif +diff --git NuttX/nuttx/include/sys/stat.h NuttX/nuttx/include/sys/stat.h +index 4fe9f5c..b66b9a0 100644 +--- NuttX/nuttx/include/sys/stat.h ++++ NuttX/nuttx/include/sys/stat.h +@@ -130,7 +130,10 @@ extern "C" + + int mkdir(FAR const char *pathname, mode_t mode); + int mkfifo(FAR const char *pathname, mode_t mode); ++#pragma GCC diagnostic push ++#pragma GCC diagnostic ignored "-Wshadow" + int stat(const char *path, FAR struct stat *buf); ++#pragma GCC diagnostic pop + int fstat(int fd, FAR struct stat *buf); + + #undef EXTERN diff --git a/nuttx-patches/math.h.patch b/nuttx-patches/math.h.patch new file mode 100644 index 000000000..18650027e --- /dev/null +++ b/nuttx-patches/math.h.patch @@ -0,0 +1,583 @@ +diff -ruN NuttX/nuttx/arch/arm/include/math.h NuttX/nuttx/arch/arm/include/math.h +--- NuttX/nuttx/arch/arm/include/math.h 1969-12-31 14:00:00.000000000 -1000 ++++ NuttX/nuttx/arch/arm/include/math.h 2014-12-25 17:33:53.404950574 -1000 +@@ -0,0 +1,579 @@ ++#ifndef _MATH_H_ ++ ++#define _MATH_H_ ++ ++#include <machine/ieeefp.h> ++#include "_ansi.h" ++ ++_BEGIN_STD_C ++ ++/* Natural log of 2 */ ++#define _M_LN2 0.693147180559945309417 ++ ++#if defined(__GNUC__) && \ ++ ( (__GNUC__ >= 4) || \ ++ ( (__GNUC__ >= 3) && defined(__GNUC_MINOR__) && (__GNUC_MINOR__ >= 3) ) ) ++ ++ /* gcc >= 3.3 implicitly defines builtins for HUGE_VALx values. */ ++ ++# ifndef HUGE_VAL ++# define HUGE_VAL (__builtin_huge_val()) ++# endif ++ ++# ifndef HUGE_VALF ++# define HUGE_VALF (__builtin_huge_valf()) ++# endif ++ ++# ifndef HUGE_VALL ++# define HUGE_VALL (__builtin_huge_vall()) ++# endif ++ ++# ifndef INFINITY ++# define INFINITY (__builtin_inff()) ++# endif ++ ++# ifndef NAN ++# define NAN (__builtin_nanf("")) ++# endif ++ ++#else /* !gcc >= 3.3 */ ++ ++ /* No builtins. Use fixed defines instead. (All 3 HUGE plus the INFINITY ++ * * and NAN macros are required to be constant expressions. Using a variable-- ++ * * even a static const--does not meet this requirement, as it cannot be ++ * * evaluated at translation time.) ++ * * The infinities are done using numbers that are far in excess of ++ * * something that would be expected to be encountered in a floating-point ++ * * implementation. (A more certain way uses values from float.h, but that is ++ * * avoided because system includes are not supposed to include each other.) ++ * * This method might produce warnings from some compilers. (It does in ++ * * newer GCCs, but not for ones that would hit this #else.) If this happens, ++ * * please report details to the Newlib mailing list. */ ++ ++ #ifndef HUGE_VAL ++ #define HUGE_VAL (1.0e999999999) ++ #endif ++ ++ #ifndef HUGE_VALF ++ #define HUGE_VALF (1.0e999999999F) ++ #endif ++ ++ #if !defined(HUGE_VALL) && defined(_HAVE_LONG_DOUBLE) ++ #define HUGE_VALL (1.0e999999999L) ++ #endif ++ ++ #if !defined(INFINITY) ++ #define INFINITY (HUGE_VALF) ++ #endif ++ ++ #if !defined(NAN) ++ #if defined(__GNUC__) && defined(__cplusplus) ++ /* Exception: older g++ versions warn about the divide by 0 used in the ++ * * normal case (even though older gccs do not). This trick suppresses the ++ * * warning, but causes errors for plain gcc, so is only used in the one ++ * * special case. */ ++ static const union { __ULong __i[1]; float __d; } __Nanf = {0x7FC00000}; ++ #define NAN (__Nanf.__d) ++ #else ++ #define NAN (0.0F/0.0F) ++ #endif ++ #endif ++ ++#endif /* !gcc >= 3.3 */ ++ ++/* Reentrant ANSI C functions. */ ++ ++#ifndef __math_68881 ++extern double atan _PARAMS((double)); ++extern double cos _PARAMS((double)); ++extern double sin _PARAMS((double)); ++extern double tan _PARAMS((double)); ++extern double tanh _PARAMS((double)); ++extern double frexp _PARAMS((double, int *)); ++extern double modf _PARAMS((double, double *)); ++extern double ceil _PARAMS((double)); ++extern double fabs _PARAMS((double)); ++extern double floor _PARAMS((double)); ++#endif /* ! defined (__math_68881) */ ++ ++/* Non reentrant ANSI C functions. */ ++ ++#ifndef _REENT_ONLY ++#ifndef __math_68881 ++extern double acos _PARAMS((double)); ++extern double asin _PARAMS((double)); ++extern double atan2 _PARAMS((double, double)); ++extern double cosh _PARAMS((double)); ++extern double sinh _PARAMS((double)); ++extern double exp _PARAMS((double)); ++extern double ldexp _PARAMS((double, int)); ++extern double log _PARAMS((double)); ++extern double log10 _PARAMS((double)); ++extern double pow _PARAMS((double, double)); ++extern double sqrt _PARAMS((double)); ++extern double fmod _PARAMS((double, double)); ++#endif /* ! defined (__math_68881) */ ++#endif /* ! defined (_REENT_ONLY) */ ++ ++#if !defined(__STRICT_ANSI__) || defined(__cplusplus) || __STDC_VERSION__ >= 199901L ++ ++/* ISO C99 types and macros. */ ++ ++#ifndef FLT_EVAL_METHOD ++#define FLT_EVAL_METHOD 0 ++typedef float float_t; ++typedef double double_t; ++#endif /* FLT_EVAL_METHOD */ ++ ++#define FP_NAN 0 ++#define FP_INFINITE 1 ++#define FP_ZERO 2 ++#define FP_SUBNORMAL 3 ++#define FP_NORMAL 4 ++ ++#ifndef FP_ILOGB0 ++# define FP_ILOGB0 (-INT_MAX) ++#endif ++#ifndef FP_ILOGBNAN ++# define FP_ILOGBNAN INT_MAX ++#endif ++ ++#ifndef MATH_ERRNO ++# define MATH_ERRNO 1 ++#endif ++#ifndef MATH_ERREXCEPT ++# define MATH_ERREXCEPT 2 ++#endif ++#ifndef math_errhandling ++# define math_errhandling MATH_ERRNO ++#endif ++ ++extern int __isinff (float x); ++extern int __isinfd (double x); ++extern int __isnanf (float x); ++extern int __isnand (double x); ++extern int __fpclassifyf (float x); ++extern int __fpclassifyd (double x); ++extern int __signbitf (float x); ++extern int __signbitd (double x); ++ ++#define fpclassify(__x) \ ++ ((sizeof(__x) == sizeof(float)) ? __fpclassifyf(__x) : \ ++ __fpclassifyd(__x)) ++ ++#ifndef isfinite ++ #define isfinite(__y) \ ++ (__extension__ ({int __cy = fpclassify(__y); \ ++ __cy != FP_INFINITE && __cy != FP_NAN;})) ++#endif ++ ++/* Note: isinf and isnan were once functions in newlib that took double ++ * * arguments. C99 specifies that these names are reserved for macros ++ * * supporting multiple floating point types. Thus, they are ++ * * now defined as macros. Implementations of the old functions ++ * * taking double arguments still exist for compatibility purposes ++ * * (prototypes for them are in <ieeefp.h>). */ ++#ifndef isinf ++ #define isinf(y) (fpclassify(y) == FP_INFINITE) ++#endif ++ ++#ifndef isnan ++ #define isnan(y) (fpclassify(y) == FP_NAN) ++#endif ++ ++#define isnormal(y) (fpclassify(y) == FP_NORMAL) ++#define signbit(__x) \ ++ ((sizeof(__x) == sizeof(float)) ? __signbitf(__x) : \ ++ __signbitd(__x)) ++ ++#define isgreater(x,y) \ ++ (__extension__ ({__typeof__(x) __x = (x); __typeof__(y) __y = (y); \ ++ !isunordered(__x,__y) && (__x > __y);})) ++#define isgreaterequal(x,y) \ ++ (__extension__ ({__typeof__(x) __x = (x); __typeof__(y) __y = (y); \ ++ !isunordered(__x,__y) && (__x >= __y);})) ++#define isless(x,y) \ ++ (__extension__ ({__typeof__(x) __x = (x); __typeof__(y) __y = (y); \ ++ !isunordered(__x,__y) && (__x < __y);})) ++#define islessequal(x,y) \ ++ (__extension__ ({__typeof__(x) __x = (x); __typeof__(y) __y = (y); \ ++ !isunordered(__x,__y) && (__x <= __y);})) ++#define islessgreater(x,y) \ ++ (__extension__ ({__typeof__(x) __x = (x); __typeof__(y) __y = (y); \ ++ !isunordered(__x,__y) && (__x < __y || __x > __y);})) ++ ++#define isunordered(a,b) \ ++ (__extension__ ({__typeof__(a) __a = (a); __typeof__(b) __b = (b); \ ++ fpclassify(__a) == FP_NAN || fpclassify(__b) == FP_NAN;})) ++ ++/* Non ANSI double precision functions. */ ++ ++extern double infinity _PARAMS((void)); ++extern double nan _PARAMS((const char *)); ++extern int finite _PARAMS((double)); ++extern double copysign _PARAMS((double, double)); ++extern double logb _PARAMS((double)); ++extern int ilogb _PARAMS((double)); ++ ++extern double asinh _PARAMS((double)); ++extern double cbrt _PARAMS((double)); ++extern double nextafter _PARAMS((double, double)); ++extern double rint _PARAMS((double)); ++extern double scalbn _PARAMS((double, int)); ++ ++extern double exp2 _PARAMS((double)); ++extern double scalbln _PARAMS((double, long int)); ++extern double tgamma _PARAMS((double)); ++extern double nearbyint _PARAMS((double)); ++extern long int lrint _PARAMS((double)); ++extern long long int llrint _PARAMS((double)); ++extern double round _PARAMS((double)); ++extern long int lround _PARAMS((double)); ++extern long long int llround _PARAMS((double)); ++extern double trunc _PARAMS((double)); ++extern double remquo _PARAMS((double, double, int *)); ++extern double fdim _PARAMS((double, double)); ++extern double fmax _PARAMS((double, double)); ++extern double fmin _PARAMS((double, double)); ++extern double fma _PARAMS((double, double, double)); ++ ++#ifndef __math_68881 ++extern double log1p _PARAMS((double)); ++extern double expm1 _PARAMS((double)); ++#endif /* ! defined (__math_68881) */ ++ ++#ifndef _REENT_ONLY ++extern double acosh _PARAMS((double)); ++extern double atanh _PARAMS((double)); ++extern double remainder _PARAMS((double, double)); ++extern double gamma _PARAMS((double)); ++extern double lgamma _PARAMS((double)); ++extern double erf _PARAMS((double)); ++extern double erfc _PARAMS((double)); ++extern double log2 _PARAMS((double)); ++ ++#ifndef __math_68881 ++extern double hypot _PARAMS((double, double)); ++#endif ++ ++#endif /* ! defined (_REENT_ONLY) */ ++ ++/* Single precision versions of ANSI functions. */ ++ ++extern float atanf _PARAMS((float)); ++extern float cosf _PARAMS((float)); ++extern float sinf _PARAMS((float)); ++extern float tanf _PARAMS((float)); ++extern float tanhf _PARAMS((float)); ++extern float frexpf _PARAMS((float, int *)); ++extern float modff _PARAMS((float, float *)); ++extern float ceilf _PARAMS((float)); ++extern float fabsf _PARAMS((float)); ++extern float floorf _PARAMS((float)); ++ ++#ifndef _REENT_ONLY ++extern float acosf _PARAMS((float)); ++extern float asinf _PARAMS((float)); ++extern float atan2f _PARAMS((float, float)); ++extern float coshf _PARAMS((float)); ++extern float sinhf _PARAMS((float)); ++extern float expf _PARAMS((float)); ++extern float ldexpf _PARAMS((float, int)); ++extern float logf _PARAMS((float)); ++extern float log10f _PARAMS((float)); ++extern float powf _PARAMS((float, float)); ++extern float sqrtf _PARAMS((float)); ++extern float fmodf _PARAMS((float, float)); ++#endif /* ! defined (_REENT_ONLY) */ ++ ++/* Other single precision functions. */ ++ ++extern float exp2f _PARAMS((float)); ++extern float scalblnf _PARAMS((float, long int)); ++extern float tgammaf _PARAMS((float)); ++extern float nearbyintf _PARAMS((float)); ++extern long int lrintf _PARAMS((float)); ++extern long long llrintf _PARAMS((float)); ++extern float roundf _PARAMS((float)); ++extern long int lroundf _PARAMS((float)); ++extern long long int llroundf _PARAMS((float)); ++extern float truncf _PARAMS((float)); ++extern float remquof _PARAMS((float, float, int *)); ++extern float fdimf _PARAMS((float, float)); ++extern float fmaxf _PARAMS((float, float)); ++extern float fminf _PARAMS((float, float)); ++extern float fmaf _PARAMS((float, float, float)); ++ ++extern float infinityf _PARAMS((void)); ++extern float nanf _PARAMS((const char *)); ++extern int finitef _PARAMS((float)); ++extern float copysignf _PARAMS((float, float)); ++extern float logbf _PARAMS((float)); ++extern int ilogbf _PARAMS((float)); ++ ++extern float asinhf _PARAMS((float)); ++extern float cbrtf _PARAMS((float)); ++extern float nextafterf _PARAMS((float, float)); ++extern float rintf _PARAMS((float)); ++extern float scalbnf _PARAMS((float, int)); ++extern float log1pf _PARAMS((float)); ++extern float expm1f _PARAMS((float)); ++ ++#ifndef _REENT_ONLY ++extern float acoshf _PARAMS((float)); ++extern float atanhf _PARAMS((float)); ++extern float remainderf _PARAMS((float, float)); ++extern float gammaf _PARAMS((float)); ++extern float lgammaf _PARAMS((float)); ++extern float erff _PARAMS((float)); ++extern float erfcf _PARAMS((float)); ++extern float log2f _PARAMS((float)); ++extern float hypotf _PARAMS((float, float)); ++#endif /* ! defined (_REENT_ONLY) */ ++ ++/* On platforms where long double equals double. */ ++#ifdef _LDBL_EQ_DBL ++/* Reentrant ANSI C functions. */ ++#ifndef __math_68881 ++extern long double atanl _PARAMS((long double)); ++extern long double cosl _PARAMS((long double)); ++extern long double sinl _PARAMS((long double)); ++extern long double tanl _PARAMS((long double)); ++extern long double tanhl _PARAMS((long double)); ++extern long double frexpl _PARAMS((long double value, int *)); ++extern long double modfl _PARAMS((long double, long double *)); ++extern long double ceill _PARAMS((long double)); ++extern long double fabsl _PARAMS((long double)); ++extern long double floorl _PARAMS((long double)); ++extern long double log1pl _PARAMS((long double)); ++extern long double expm1l _PARAMS((long double)); ++#endif /* ! defined (__math_68881) */ ++/* Non reentrant ANSI C functions. */ ++#ifndef _REENT_ONLY ++#ifndef __math_68881 ++extern long double acosl _PARAMS((long double)); ++extern long double asinl _PARAMS((long double)); ++extern long double atan2l _PARAMS((long double, long double)); ++extern long double coshl _PARAMS((long double)); ++extern long double sinhl _PARAMS((long double)); ++extern long double expl _PARAMS((long double)); ++extern long double ldexpl _PARAMS((long double, int)); ++extern long double logl _PARAMS((long double)); ++extern long double log2l _PARAMS((long double)); ++extern long double log10l _PARAMS((long double)); ++extern long double powl _PARAMS((long double, long double)); ++extern long double sqrtl _PARAMS((long double)); ++extern long double fmodl _PARAMS((long double, long double)); ++extern long double hypotl _PARAMS((long double, long double)); ++#endif /* ! defined (__math_68881) */ ++#endif /* ! defined (_REENT_ONLY) */ ++extern long double copysignl _PARAMS((long double, long double)); ++extern long double nanl _PARAMS((const char *)); ++extern int ilogbl _PARAMS((long double)); ++extern long double asinhl _PARAMS((long double)); ++extern long double cbrtl _PARAMS((long double)); ++extern long double nextafterl _PARAMS((long double, long double)); ++extern long double rintl _PARAMS((long double)); ++extern long double scalbnl _PARAMS((long double, int)); ++extern long double exp2l _PARAMS((long double)); ++extern long double scalblnl _PARAMS((long double, long)); ++extern long double tgammal _PARAMS((long double)); ++extern long double nearbyintl _PARAMS((long double)); ++extern long int lrintl _PARAMS((long double)); ++extern long long int llrintl _PARAMS((long double)); ++extern long double roundl _PARAMS((long double)); ++extern long lroundl _PARAMS((long double)); ++extern long long int llroundl _PARAMS((long double)); ++extern long double truncl _PARAMS((long double)); ++extern long double remquol _PARAMS((long double, long double, int *)); ++extern long double fdiml _PARAMS((long double, long double)); ++extern long double fmaxl _PARAMS((long double, long double)); ++extern long double fminl _PARAMS((long double, long double)); ++extern long double fmal _PARAMS((long double, long double, long double)); ++#ifndef _REENT_ONLY ++extern long double acoshl _PARAMS((long double)); ++extern long double atanhl _PARAMS((long double)); ++extern long double remainderl _PARAMS((long double, long double)); ++extern long double lgammal _PARAMS((long double)); ++extern long double erfl _PARAMS((long double)); ++extern long double erfcl _PARAMS((long double)); ++#endif /* ! defined (_REENT_ONLY) */ ++#else /* !_LDBL_EQ_DBL */ ++#ifdef __i386__ ++/* Other long double precision functions. */ ++extern _LONG_DOUBLE rintl _PARAMS((_LONG_DOUBLE)); ++extern long int lrintl _PARAMS((_LONG_DOUBLE)); ++extern long long llrintl _PARAMS((_LONG_DOUBLE)); ++#endif /* __i386__ */ ++#endif /* !_LDBL_EQ_DBL */ ++ ++#endif /* !defined (__STRICT_ANSI__) || defined(__cplusplus) || __STDC_VERSION__ >= 199901L */ ++ ++#if !defined (__STRICT_ANSI__) || defined(__cplusplus) ++ ++extern double drem _PARAMS((double, double)); ++extern void sincos _PARAMS((double, double *, double *)); ++extern double gamma_r _PARAMS((double, int *)); ++extern double lgamma_r _PARAMS((double, int *)); ++ ++extern double y0 _PARAMS((double)); ++extern double y1 _PARAMS((double)); ++extern double yn _PARAMS((int, double)); ++extern double j0 _PARAMS((double)); ++extern double j1 _PARAMS((double)); ++extern double jn _PARAMS((int, double)); ++ ++extern float dremf _PARAMS((float, float)); ++extern void sincosf _PARAMS((float, float *, float *)); ++extern float gammaf_r _PARAMS((float, int *)); ++extern float lgammaf_r _PARAMS((float, int *)); ++ ++extern float y0f _PARAMS((float)); ++extern float y1f _PARAMS((float)); ++extern float ynf _PARAMS((int, float)); ++extern float j0f _PARAMS((float)); ++extern float j1f _PARAMS((float)); ++extern float jnf _PARAMS((int, float)); ++ ++/* GNU extensions */ ++# ifndef exp10 ++extern double exp10 _PARAMS((double)); ++# endif ++# ifndef pow10 ++extern double pow10 _PARAMS((double)); ++# endif ++# ifndef exp10f ++extern float exp10f _PARAMS((float)); ++# endif ++# ifndef pow10f ++extern float pow10f _PARAMS((float)); ++# endif ++ ++#endif /* !defined (__STRICT_ANSI__) || defined(__cplusplus) */ ++ ++#ifndef __STRICT_ANSI__ ++ ++/* The gamma functions use a global variable, signgam. */ ++#ifndef _REENT_ONLY ++#define signgam (*__signgam()) ++extern int *__signgam _PARAMS((void)); ++#endif /* ! defined (_REENT_ONLY) */ ++ ++#define __signgam_r(ptr) _REENT_SIGNGAM(ptr) ++ ++/* The exception structure passed to the matherr routine. */ ++/* We have a problem when using C++ since `exception' is a reserved ++ * name in C++. */ ++#ifdef __cplusplus ++struct __exception ++#else ++struct exception ++#endif ++{ ++ int type; ++ char *name; ++ double arg1; ++ double arg2; ++ double retval; ++ int err; ++}; ++ ++#ifdef __cplusplus ++extern int matherr _PARAMS((struct __exception *e)); ++#else ++extern int matherr _PARAMS((struct exception *e)); ++#endif ++ ++/* Values for the type field of struct exception. */ ++ ++#define DOMAIN 1 ++#define SING 2 ++#define OVERFLOW 3 ++#define UNDERFLOW 4 ++#define TLOSS 5 ++#define PLOSS 6 ++ ++/* Useful constants. */ ++ ++#define MAXFLOAT 3.40282347e+38F ++ ++#define M_E 2.7182818284590452354 ++#define M_LOG2E 1.4426950408889634074 ++#define M_LOG10E 0.43429448190325182765 ++#define M_LN2 _M_LN2 ++#define M_LN10 2.30258509299404568402 ++#define M_PI 3.14159265358979323846 ++#define M_TWOPI (M_PI * 2.0) ++#define M_PI_2 1.57079632679489661923 ++#define M_PI_4 0.78539816339744830962 ++#define M_3PI_4 2.3561944901923448370E0 ++#define M_SQRTPI 1.77245385090551602792981 ++#define M_1_PI 0.31830988618379067154 ++#define M_2_PI 0.63661977236758134308 ++#define M_2_SQRTPI 1.12837916709551257390 ++#define M_DEG_TO_RAD 0.01745329251994 ++#define M_RAD_TO_DEG 57.2957795130823 ++#define M_SQRT2 1.41421356237309504880 ++#define M_SQRT1_2 0.70710678118654752440 ++#define M_LN2LO 1.9082149292705877000E-10 ++#define M_LN2HI 6.9314718036912381649E-1 ++#define M_SQRT3 1.73205080756887719000 ++#define M_IVLN10 0.43429448190325182765 /* 1 / log(10) */ ++#define M_LOG2_E _M_LN2 ++#define M_INVLN2 1.4426950408889633870E0 /* 1 / log(2) */ ++ ++ ++#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) */ ++ ++/* Global control over fdlibm error handling. */ ++ ++enum __fdlibm_version ++{ ++ __fdlibm_ieee = -1, ++ __fdlibm_svid, ++ __fdlibm_xopen, ++ __fdlibm_posix ++}; ++ ++#define _LIB_VERSION_TYPE enum __fdlibm_version ++#define _LIB_VERSION __fdlib_version ++ ++extern __IMPORT _LIB_VERSION_TYPE _LIB_VERSION; ++ ++#define _IEEE_ __fdlibm_ieee ++#define _SVID_ __fdlibm_svid ++#define _XOPEN_ __fdlibm_xopen ++#define _POSIX_ __fdlibm_posix ++ ++#endif /* ! defined (__STRICT_ANSI__) */ ++ ++_END_STD_C ++ ++#ifdef __FAST_MATH__ ++#include <machine/fastmath.h> ++#endif ++ ++#endif /* _MATH_H_ */ 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/boards/aerocore/aerocore_init.c b/src/drivers/boards/aerocore/aerocore_init.c index 1ce235da8..91dbe4b81 100644 --- a/src/drivers/boards/aerocore/aerocore_init.c +++ b/src/drivers/boards/aerocore/aerocore_init.c @@ -53,11 +53,11 @@ #include <errno.h> #include <nuttx/arch.h> -#include <nuttx/spi.h> +#include <nuttx/spi/spi.h> #include <nuttx/i2c.h> #include <nuttx/mmcsd.h> #include <nuttx/analog/adc.h> -#include <nuttx/gran.h> +#include <nuttx/mm/gran.h> #include <stm32.h> #include "board_config.h" @@ -191,7 +191,7 @@ stm32_boardinitialize(void) stm32_spiinitialize(); /* configure LEDs */ - up_ledinit(); + board_led_initialize(); } /**************************************************************************** @@ -228,6 +228,21 @@ __EXPORT int nsh_archinitialize(void) stm32_configgpio(GPIO_ADC1_IN12); /* J1 breakout */ stm32_configgpio(GPIO_ADC1_IN13); /* J1 breakout */ +#if defined(CONFIG_HAVE_CXX) && defined(CONFIG_HAVE_CXXINITIALIZE) + + /* run C++ ctors before we go any further */ + + up_cxxinitialize(); + +# if defined(CONFIG_EXAMPLES_NSH_CXXINITIALIZE) +# error CONFIG_EXAMPLES_NSH_CXXINITIALIZE Must not be defined! Use CONFIG_HAVE_CXX and CONFIG_HAVE_CXXINITIALIZE. +# endif + +#else +# error platform is dependent on c++ both CONFIG_HAVE_CXX and CONFIG_HAVE_CXXINITIALIZE must be defined. +#endif + + /* configure the high-resolution time/callout interface */ hrt_init(); @@ -264,7 +279,7 @@ __EXPORT int nsh_archinitialize(void) spi3 = up_spiinitialize(3); if (!spi3) { message("[boot] FAILED to initialize SPI port 3\n"); - up_ledon(LED_AMBER); + board_led_on(LED_AMBER); return -ENODEV; } /* Default: 1MHz, 8 bits, Mode 3 */ @@ -281,7 +296,7 @@ __EXPORT int nsh_archinitialize(void) spi4 = up_spiinitialize(4); if (!spi4) { message("[boot] FAILED to initialize SPI port 4\n"); - up_ledon(LED_AMBER); + board_led_on(LED_AMBER); return -ENODEV; } /* Default: ~10MHz, 8 bits, Mode 3 */ diff --git a/src/drivers/boards/aerocore/aerocore_spi.c b/src/drivers/boards/aerocore/aerocore_spi.c index e329bd9d1..8577d4938 100644 --- a/src/drivers/boards/aerocore/aerocore_spi.c +++ b/src/drivers/boards/aerocore/aerocore_spi.c @@ -47,7 +47,7 @@ #include <stdbool.h> #include <debug.h> -#include <nuttx/spi.h> +#include <nuttx/spi/spi.h> #include <arch/board/board.h> #include <up_arch.h> diff --git a/src/drivers/boards/aerocore/module.mk b/src/drivers/boards/aerocore/module.mk index 0a2d91009..05e3d878d 100644 --- a/src/drivers/boards/aerocore/module.mk +++ b/src/drivers/boards/aerocore/module.mk @@ -7,4 +7,10 @@ SRCS = aerocore_init.c \ aerocore_spi.c \ aerocore_led.c +ENABLE_CXXINITIALIZE=$(call check_nuttx_config ,"CONFIG_HAVE_CXX 1", $(NUTTX_CONFIG_H)) +ENABLE_CXXINITIALIZE+=$(call check_nuttx_config ,"CONFIG_HAVE_CXXINITIALIZE 1", $(NUTTX_CONFIG_H)) +ifeq ("$(ENABLE_CXXINITIALIZE)",$(nuttx_config_2true)) +SRCS += ../../../modules/systemlib/up_cxxinitialize.c +endif + MAXOPTIMIZATION = -Os diff --git a/src/drivers/boards/px4fmu-v1/module.mk b/src/drivers/boards/px4fmu-v1/module.mk index 5e1a27d5a..ff717fe92 100644 --- a/src/drivers/boards/px4fmu-v1/module.mk +++ b/src/drivers/boards/px4fmu-v1/module.mk @@ -9,4 +9,10 @@ SRCS = px4fmu_can.c \ px4fmu_usb.c \ px4fmu_led.c +ENABLE_CXXINITIALIZE=$(call check_nuttx_config ,"CONFIG_HAVE_CXX 1", $(NUTTX_CONFIG_H)) +ENABLE_CXXINITIALIZE+=$(call check_nuttx_config ,"CONFIG_HAVE_CXXINITIALIZE 1", $(NUTTX_CONFIG_H)) +ifeq ("$(ENABLE_CXXINITIALIZE)",$(nuttx_config_2true)) +SRCS += ../../../modules/systemlib/up_cxxinitialize.c +endif + MAXOPTIMIZATION = -Os diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_init.c b/src/drivers/boards/px4fmu-v1/px4fmu_init.c index 293021f8b..c9d4cce1e 100644 --- a/src/drivers/boards/px4fmu-v1/px4fmu_init.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_init.c @@ -53,7 +53,7 @@ #include <errno.h> #include <nuttx/arch.h> -#include <nuttx/spi.h> +#include <nuttx/spi/spi.h> #include <nuttx/i2c.h> #include <nuttx/mmcsd.h> #include <nuttx/analog/adc.h> @@ -69,6 +69,12 @@ #include <systemlib/cpuload.h> +/* todo: This is constant but not proper */ +__BEGIN_DECLS +extern void led_off(int led); +__END_DECLS + + /**************************************************************************** * Pre-Processor Definitions ****************************************************************************/ @@ -77,33 +83,6 @@ /* Debug ********************************************************************/ -#ifdef CONFIG_CPP_HAVE_VARARGS -# ifdef CONFIG_DEBUG -# define message(...) lowsyslog(__VA_ARGS__) -# else -# define message(...) printf(__VA_ARGS__) -# endif -#else -# ifdef CONFIG_DEBUG -# define message lowsyslog -# else -# define message printf -# endif -#endif - -/* - * Ideally we'd be able to get these from up_internal.h, - * but since we want to be able to disable the NuttX use - * of leds for system indication at will and there is no - * separate switch, we need to build independent of the - * CONFIG_ARCH_LEDS configuration switch. - */ -__BEGIN_DECLS -extern void led_init(void); -extern void led_on(int led); -extern void led_off(int led); -__END_DECLS - /**************************************************************************** * Protected Functions ****************************************************************************/ @@ -127,8 +106,8 @@ __EXPORT void stm32_boardinitialize(void) /* configure SPI interfaces */ stm32_spiinitialize(); - /* configure LEDs (empty call to NuttX' ledinit) */ - up_ledinit(); + /* configure LEDs */ + board_led_initialize(); } /**************************************************************************** @@ -166,6 +145,20 @@ __EXPORT int nsh_archinitialize(void) stm32_configgpio(GPIO_ADC1_IN11); /* IN12 and IN13 further below */ +#if defined(CONFIG_HAVE_CXX) && defined(CONFIG_HAVE_CXXINITIALIZE) + + /* run C++ ctors before we go any further */ + + up_cxxinitialize(); + +# if defined(CONFIG_EXAMPLES_NSH_CXXINITIALIZE) +# error CONFIG_EXAMPLES_NSH_CXXINITIALIZE Must not be defined! Use CONFIG_HAVE_CXX and CONFIG_HAVE_CXXINITIALIZE. +# endif + +#else +# error platform is dependent on c++ both CONFIG_HAVE_CXX and CONFIG_HAVE_CXXINITIALIZE must be defined. +#endif + /* configure the high-resolution time/callout interface */ hrt_init(); @@ -202,8 +195,8 @@ __EXPORT int nsh_archinitialize(void) spi1 = up_spiinitialize(1); if (!spi1) { - message("[boot] FAILED to initialize SPI port 1\r\n"); - up_ledon(LED_AMBER); + syslog(LOG_ERR, "[boot] FAILED to initialize SPI port 1\r\n"); + board_led_on(LED_AMBER); return -ENODEV; } @@ -216,7 +209,7 @@ __EXPORT int nsh_archinitialize(void) SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); up_udelay(20); - message("[boot] Successfully initialized SPI port 1\r\n"); + syslog(LOG_INFO, "[boot] Successfully initialized SPI port 1\r\n"); /* * If SPI2 is enabled in the defconfig, we loose some ADC pins as chip selects. @@ -232,10 +225,10 @@ __EXPORT int nsh_archinitialize(void) SPI_SELECT(spi2, PX4_SPIDEV_GYRO, false); SPI_SELECT(spi2, PX4_SPIDEV_ACCEL_MAG, false); - message("[boot] Initialized SPI port2 (ADC IN12/13 blocked)\n"); + syslog(LOG_INFO, "[boot] Initialized SPI port2 (ADC IN12/13 blocked)\n"); #else spi2 = NULL; - message("[boot] Enabling IN12/13 instead of SPI2\n"); + syslog(LOG_INFO, "[boot] Enabling IN12/13 instead of SPI2\n"); /* no SPI2, use pins for ADC */ stm32_configgpio(GPIO_ADC1_IN12); stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards @@ -243,27 +236,27 @@ __EXPORT int nsh_archinitialize(void) /* Get the SPI port for the microSD slot */ - message("[boot] Initializing SPI port 3\n"); + syslog(LOG_INFO, "[boot] Initializing SPI port 3\n"); spi3 = up_spiinitialize(3); if (!spi3) { - message("[boot] FAILED to initialize SPI port 3\n"); - up_ledon(LED_AMBER); + syslog(LOG_ERR, "[boot] FAILED to initialize SPI port 3\n"); + board_led_on(LED_AMBER); return -ENODEV; } - message("[boot] Successfully initialized SPI port 3\n"); + syslog(LOG_INFO, "[boot] Successfully initialized SPI port 3\n"); /* Now bind the SPI interface to the MMCSD driver */ result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3); if (result != OK) { - message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\n"); - up_ledon(LED_AMBER); + syslog(LOG_ERR, "[boot] FAILED to bind SPI port 3 to the MMCSD driver\n"); + board_led_on(LED_AMBER); return -ENODEV; } - message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n"); + syslog(LOG_INFO, "[boot] Successfully bound SPI port 3 to the MMCSD driver\n"); return OK; } diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_spi.c b/src/drivers/boards/px4fmu-v1/px4fmu_spi.c index 17e6862f7..a47498e95 100644 --- a/src/drivers/boards/px4fmu-v1/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_spi.c @@ -47,7 +47,7 @@ #include <stdbool.h> #include <debug.h> -#include <nuttx/spi.h> +#include <nuttx/spi/spi.h> #include <arch/board/board.h> #include "up_arch.h" @@ -121,6 +121,8 @@ __EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devi return SPI_STATUS_PRESENT; } + +#ifdef CONFIG_STM32_SPI2 __EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) { /* SPI select is active low, so write !selected to select the device */ @@ -138,6 +140,7 @@ __EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devi { return SPI_STATUS_PRESENT; } +#endif __EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) diff --git a/src/drivers/boards/px4fmu-v2/module.mk b/src/drivers/boards/px4fmu-v2/module.mk index 103232b0c..e62e48e99 100644 --- a/src/drivers/boards/px4fmu-v2/module.mk +++ b/src/drivers/boards/px4fmu-v2/module.mk @@ -9,4 +9,11 @@ SRCS = px4fmu_can.c \ px4fmu_usb.c \ px4fmu2_led.c + +ENABLE_CXXINITIALIZE=$(call check_nuttx_config ,"CONFIG_HAVE_CXX 1", $(NUTTX_CONFIG_H)) +ENABLE_CXXINITIALIZE+=$(call check_nuttx_config ,"CONFIG_HAVE_CXXINITIALIZE 1", $(NUTTX_CONFIG_H)) +ifeq ("$(ENABLE_CXXINITIALIZE)",$(nuttx_config_2true)) +SRCS += ../../../modules/systemlib/up_cxxinitialize.c +endif + MAXOPTIMIZATION = -Os diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c index 9b25c574a..e32c18759 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -53,12 +53,12 @@ #include <errno.h> #include <nuttx/arch.h> -#include <nuttx/spi.h> +#include <nuttx/spi/spi.h> #include <nuttx/i2c.h> #include <nuttx/sdio.h> #include <nuttx/mmcsd.h> #include <nuttx/analog/adc.h> -#include <nuttx/gran.h> +#include <nuttx/mm/gran.h> #include <stm32.h> #include "board_config.h" @@ -72,6 +72,11 @@ #include <systemlib/cpuload.h> #include <systemlib/perf_counter.h> +/* todo: This is constant but not proper */ +__BEGIN_DECLS +extern void led_off(int led); +__END_DECLS + /**************************************************************************** * Pre-Processor Definitions ****************************************************************************/ @@ -94,19 +99,6 @@ # endif #endif -/* - * Ideally we'd be able to get these from up_internal.h, - * but since we want to be able to disable the NuttX use - * of leds for system indication at will and there is no - * separate switch, we need to build independent of the - * CONFIG_ARCH_LEDS configuration switch. - */ -__BEGIN_DECLS -extern void led_init(void); -extern void led_on(int led); -extern void led_off(int led); -__END_DECLS - /**************************************************************************** * Protected Functions ****************************************************************************/ @@ -116,6 +108,8 @@ __END_DECLS # error microSD DMA support requires CONFIG_GRAN # endif +#ifdef CONFIG_FAT_DMAMEMORY + static GRAN_HANDLE dma_allocator; /* @@ -130,6 +124,7 @@ static GRAN_HANDLE dma_allocator; */ static uint8_t g_dma_heap[8192] __attribute__((aligned(64))); static perf_counter_t g_dma_perf; +#endif static void dma_alloc_init(void) @@ -139,7 +134,7 @@ dma_alloc_init(void) 7, /* 128B granule - must be > alignment (XXX bug?) */ 6); /* 64B alignment */ if (dma_allocator == NULL) { - message("[boot] DMA allocator setup FAILED"); + syslog(LOG_ERR, "[boot] DMA allocator setup FAILED"); } else { g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations"); } @@ -192,7 +187,7 @@ stm32_boardinitialize(void) stm32_spiinitialize(); /* configure LEDs */ - up_ledinit(); + board_led_initialize(); } /**************************************************************************** @@ -244,6 +239,20 @@ __EXPORT int nsh_archinitialize(void) stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC); stm32_configgpio(GPIO_VDD_5V_PERIPH_OC); +#if defined(CONFIG_HAVE_CXX) && defined(CONFIG_HAVE_CXXINITIALIZE) + + /* run C++ ctors before we go any further */ + + up_cxxinitialize(); + +# if defined(CONFIG_EXAMPLES_NSH_CXXINITIALIZE) +# error CONFIG_EXAMPLES_NSH_CXXINITIALIZE Must not be defined! Use CONFIG_HAVE_CXX and CONFIG_HAVE_CXXINITIALIZE. +# endif + +#else +# error platform is dependent on c++ both CONFIG_HAVE_CXX and CONFIG_HAVE_CXXINITIALIZE must be defined. +#endif + /* configure the high-resolution time/callout interface */ hrt_init(); @@ -281,8 +290,8 @@ __EXPORT int nsh_archinitialize(void) spi1 = up_spiinitialize(1); if (!spi1) { - message("[boot] FAILED to initialize SPI port 1\n"); - up_ledon(LED_AMBER); + syslog(LOG_ERR, "[boot] FAILED to initialize SPI port 1\n"); + board_led_on(LED_AMBER); return -ENODEV; } @@ -296,15 +305,15 @@ __EXPORT int nsh_archinitialize(void) SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); up_udelay(20); - message("[boot] Initialized SPI port 1 (SENSORS)\n"); + syslog(LOG_INFO, "[boot] Initialized SPI port 1 (SENSORS)\n"); /* Get the SPI port for the FRAM */ spi2 = up_spiinitialize(2); if (!spi2) { - message("[boot] FAILED to initialize SPI port 2\n"); - up_ledon(LED_AMBER); + syslog(LOG_ERR, "[boot] FAILED to initialize SPI port 2\n"); + board_led_on(LED_AMBER); return -ENODEV; } @@ -317,7 +326,7 @@ __EXPORT int nsh_archinitialize(void) SPI_SETMODE(spi2, SPIDEV_MODE3); SPI_SELECT(spi2, SPIDEV_FLASH, false); - message("[boot] Initialized SPI port 2 (RAMTRON FRAM)\n"); + syslog(LOG_INFO, "[boot] Initialized SPI port 2 (RAMTRON FRAM)\n"); spi4 = up_spiinitialize(4); @@ -335,7 +344,7 @@ __EXPORT int nsh_archinitialize(void) sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO); if (!sdio) { - message("[boot] Failed to initialize SDIO slot %d\n", + syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", CONFIG_NSH_MMCSDSLOTNO); return -ENODEV; } @@ -343,14 +352,14 @@ __EXPORT int nsh_archinitialize(void) /* Now bind the SDIO interface to the MMC/SD driver */ int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio); if (ret != OK) { - message("[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret); + syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret); return ret; } /* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */ sdio_mediachange(sdio, true); - message("[boot] Initialized SDIO\n"); + syslog(LOG_INFO, "[boot] Initialized SDIO\n"); #endif return OK; diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c index 27f193513..b98cd999b 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c @@ -47,7 +47,7 @@ #include <stdbool.h> #include <debug.h> -#include <nuttx/spi.h> +#include <nuttx/spi/spi.h> #include <arch/board/board.h> #include <up_arch.h> diff --git a/src/drivers/boards/px4io-v1/module.mk b/src/drivers/boards/px4io-v1/module.mk index a7a14dd07..52f17f3f2 100644 --- a/src/drivers/boards/px4io-v1/module.mk +++ b/src/drivers/boards/px4io-v1/module.mk @@ -5,4 +5,10 @@ SRCS = px4io_init.c \ px4io_pwm_servo.c +ENABLE_CXXINITIALIZE=$(call check_nuttx_config ,"CONFIG_HAVE_CXX 1", $(NUTTX_CONFIG_H)) +ENABLE_CXXINITIALIZE+=$(call check_nuttx_config ,"CONFIG_HAVE_CXXINITIALIZE 1", $(NUTTX_CONFIG_H)) +ifeq ("$(ENABLE_CXXINITIALIZE)",$(nuttx_config_2true)) +SRCS += ../../../modules/systemlib/up_cxxinitialize.c +endif + MAXOPTIMIZATION = -Os diff --git a/src/drivers/boards/px4io-v2/module.mk b/src/drivers/boards/px4io-v2/module.mk index 3f0e9a0b3..5613ad405 100644 --- a/src/drivers/boards/px4io-v2/module.mk +++ b/src/drivers/boards/px4io-v2/module.mk @@ -5,4 +5,10 @@ SRCS = px4iov2_init.c \ px4iov2_pwm_servo.c +ENABLE_CXXINITIALIZE=$(call check_nuttx_config ,"CONFIG_HAVE_CXX 1", $(NUTTX_CONFIG_H)) +ENABLE_CXXINITIALIZE+=$(call check_nuttx_config ,"CONFIG_HAVE_CXXINITIALIZE 1", $(NUTTX_CONFIG_H)) +ifeq ("$(ENABLE_CXXINITIALIZE)",$(nuttx_config_2true)) +SRCS += ../../../modules/systemlib/up_cxxinitialize.c +endif + MAXOPTIMIZATION = -Os diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h index 1d9837689..95b06cd1a 100644 --- a/src/drivers/device/spi.h +++ b/src/drivers/device/spi.h @@ -42,7 +42,7 @@ #include "device.h" -#include <nuttx/spi.h> +#include <nuttx/spi/spi.h> namespace device __EXPORT { diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index 9b5c8133b..6ffa8252e 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 */ @@ -513,12 +515,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): @@ -659,7 +661,7 @@ int hil_new_mode(PortMode new_mode) { // uint32_t gpio_bits; - + // /* reset to all-inputs */ // g_hil->ioctl(0, GPIO_RESET, 0); @@ -691,17 +693,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/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 1055487cb..a4505fe6f 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/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 112c01513..2552e7e6a 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> @@ -1144,7 +1148,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 556eebab6..ffcb1c907 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> @@ -618,8 +622,15 @@ PX4IO::init() return ret; /* get some parameters */ - unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); + unsigned protocol; + hrt_abstime start_try_time = hrt_absolute_time(); + do { + usleep(2000); + protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); + } while (protocol == _io_reg_get_error && (hrt_elapsed_time(&start_try_time) < 700U * 1000U)); + + /* if the error still persists after timing out, we give up */ if (protocol == _io_reg_get_error) { log("failed to communicate with IO"); mavlink_log_emergency(_mavlink_fd, "[IO] failed to communicate with IO, abort."); @@ -1199,7 +1210,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; } @@ -2590,7 +2601,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) { diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp index d227e15d5..91f11eea1 100644 --- a/src/drivers/px4io/px4io_serial.cpp +++ b/src/drivers/px4io/px4io_serial.cpp @@ -217,6 +217,9 @@ PX4IO_serial::~PX4IO_serial() stm32_unconfiggpio(PX4IO_SERIAL_TX_GPIO); stm32_unconfiggpio(PX4IO_SERIAL_RX_GPIO); + /* Disable APB clock for the USART peripheral */ + modifyreg32(STM32_RCC_APB2ENR, RCC_APB2ENR_USART6EN, 0); + /* and kill our semaphores */ sem_destroy(&_completion_semaphore); sem_destroy(&_bus_semaphore); @@ -239,12 +242,17 @@ PX4IO_serial::~PX4IO_serial() int PX4IO_serial::init() { + /* allocate DMA */ _tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP); _rx_dma = stm32_dmachannel(PX4IO_SERIAL_RX_DMAMAP); if ((_tx_dma == nullptr) || (_rx_dma == nullptr)) return -1; + + /* Enable the APB clock for the USART peripheral */ + modifyreg32(STM32_RCC_APB2ENR, 0, RCC_APB2ENR_USART6EN); + /* configure pins for serial use */ stm32_configgpio(PX4IO_SERIAL_TX_GPIO); stm32_configgpio(PX4IO_SERIAL_RX_GPIO); @@ -258,6 +266,7 @@ PX4IO_serial::init() (void)rSR; (void)rDR; + /* configure line speed */ uint32_t usartdiv32 = PX4IO_SERIAL_CLOCK / (PX4IO_SERIAL_BITRATE / 2); uint32_t mantissa = usartdiv32 >> 5; @@ -292,7 +301,7 @@ PX4IO_serial::ioctl(unsigned operation, unsigned &arg) case 1: /* XXX magic number - test operation */ switch (arg) { case 0: - lowsyslog("test 0\n"); + lowsyslog(LOG_INFO, "test 0\n"); /* kill DMA, this is a PIO test */ stm32_dmastop(_tx_dma); @@ -316,7 +325,7 @@ PX4IO_serial::ioctl(unsigned operation, unsigned &arg) fails++; if (count >= 5000) { - lowsyslog("==== test 1 : %u failures ====\n", fails); + lowsyslog(LOG_INFO, "==== test 1 : %u failures ====\n", fails); perf_print_counter(_pc_txns); perf_print_counter(_pc_dmasetup); perf_print_counter(_pc_retries); @@ -333,7 +342,7 @@ PX4IO_serial::ioctl(unsigned operation, unsigned &arg) return 0; } case 2: - lowsyslog("test 2\n"); + lowsyslog(LOG_INFO, "test 2\n"); return 0; } default: @@ -555,7 +564,7 @@ PX4IO_serial::_wait_complete() } /* we might? see this for EINTR */ - lowsyslog("unexpected ret %d/%d\n", ret, errno); + lowsyslog(LOG_ERR, "unexpected ret %d/%d\n", ret, errno); } /* reset DMA status */ 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/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index 8f523b390..13796935b 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -672,7 +672,7 @@ ToneAlarm::next_note() // tune looks bad (unexpected EOF, bad character, etc.) tune_error: - lowsyslog("tune error\n"); + lowsyslog(LOG_ERR, "tune error\n"); _repeat = false; // don't loop on error // stop (and potentially restart) the tune 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/nuttx-configs/px4io-v2/nsh/appconfig b/src/examples/publisher/module.mk index 48a41bcdb..62a5f8dd1 100644 --- a/nuttx-configs/px4io-v2/nsh/appconfig +++ b/src/examples/publisher/module.mk @@ -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 @@ -30,3 +30,15 @@ # 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/nuttx-configs/aerocore/nsh/appconfig b/src/examples/subscriber/module.mk index 2850dac06..0e02c65b4 100644 --- a/nuttx-configs/aerocore/nsh/appconfig +++ b/src/examples/subscriber/module.mk @@ -1,8 +1,6 @@ ############################################################################ -# configs/aerocore/nsh/appconfig # -# Copyright (C) 2011 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt <gnutt@nuttx.org> +# 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 @@ -14,7 +12,7 @@ # notice, this list of conditions and the following disclaimer in # the documentation and/or other materials provided with the # distribution. -# 3. Neither the name NuttX nor the names of its contributors may be +# 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. # @@ -33,10 +31,15 @@ # ############################################################################ -# Path to example in apps/examples containing the user_start entry point +# +# Subscriber Example Application +# + +MODULE_COMMAND = subscriber -CONFIGURED_APPS += examples/nsh +SRCS = subscriber_main.cpp \ + subscriber_start_nuttx.cpp \ + subscriber_example.cpp \ + subscriber_params.c -# The NSH application library -CONFIGURED_APPS += nshlib -CONFIGURED_APPS += system/readline +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/actuator_armed.h b/src/examples/subscriber/subscriber_params.c index 0f6c9aca1..a43817b5b 100644 --- a/src/modules/uORB/topics/actuator_armed.h +++ b/src/examples/subscriber/subscriber_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * 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 @@ -32,38 +32,25 @@ ****************************************************************************/ /** - * @file actuator_armed.h - * - * Actuator armed topic + * @file subscriber_params.c + * Parameters for the subscriber example * + * @author Thomas Gubler <thomasgubler@gmail.com> */ -#ifndef TOPIC_ACTUATOR_ARMED_H -#define TOPIC_ACTUATOR_ARMED_H - -#include <stdint.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 */ - -/** global 'actuator output is live' control. */ -struct actuator_armed_s { - - 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 */ -}; +PX4_PARAM_DEFINE_INT32(SUB_INTERV); /** - * @} + * Float Demonstration Parameter in the Example + * + * @group Subscriber Example */ - -/* register this as object request broker structure */ -ORB_DECLARE(actuator_armed); - -#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/containers/List.hpp b/src/include/containers/List.hpp index 13cbda938..5c0ba59fd 100644 --- a/src/include/containers/List.hpp +++ b/src/include/containers/List.hpp @@ -38,6 +38,7 @@ */ #pragma once +#include <cstddef> template<class T> class __EXPORT ListNode diff --git a/src/include/px4.h b/src/include/px4.h new file mode 100644 index 000000000..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/commander.cpp b/src/modules/commander/commander.cpp index 247a2c5b8..a1c4e205d 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> 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 923aa2861..d51075b8c 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" @@ -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 6f225bb48..88918333d 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..74249c9c5 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); } } 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_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e5a21651d..2e28a6d2c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -799,7 +799,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)); 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 a094ed2c6..562033db1 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -63,7 +63,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> @@ -615,14 +619,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..fcfee28dc --- /dev/null +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp @@ -0,0 +1,308 @@ +/* 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 */ + memcpy(&_v_att_sp_mod, &_v_att_sp->data(), sizeof(_v_att_sp_mod)); + } + + 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 setpint 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 */ + memcpy(&_v_att_sp_mod, &_v_att_sp->data(), sizeof(_v_att_sp_mod)); + + /* 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/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp new file mode 100644 index 000000000..c2b847075 --- /dev/null +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_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 mc_att_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_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}"); + } + + 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_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"); + + } else { + warnx("not started"); + } + + exit(0); + } + + warnx("unrecognized command"); + return 1; +} diff --git a/nuttx-configs/px4fmu-v2/nsh/appconfig b/src/modules/mc_att_control_multiplatform/module.mk index 0e18aa8ef..959f6492b 100644 --- a/nuttx-configs/px4fmu-v2/nsh/appconfig +++ b/src/modules/mc_att_control_multiplatform/module.mk @@ -1,8 +1,6 @@ ############################################################################ -# configs/px4fmu/nsh/appconfig # -# Copyright (C) 2011 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt <gnutt@nuttx.org> +# 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 @@ -14,7 +12,7 @@ # notice, this list of conditions and the following disclaimer in # the documentation and/or other materials provided with the # distribution. -# 3. Neither the name NuttX nor the names of its contributors may be +# 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. # @@ -33,20 +31,14 @@ # ############################################################################ -# Path to example in apps/examples containing the user_start entry point - -CONFIGURED_APPS += examples/nsh - -# The NSH application library -CONFIGURED_APPS += nshlib -CONFIGURED_APPS += system/readline +# +# Multirotor attitude controller (vector based, no Euler singularities) +# -ifeq ($(CONFIG_CAN),y) -#CONFIGURED_APPS += examples/can -endif +MODULE_COMMAND = mc_att_control_m -#ifeq ($(CONFIG_USBDEV),y) -#ifeq ($(CONFIG_CDCACM),y) -CONFIGURED_APPS += examples/cdcacm -#endif -#endif +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 962dc6d4a..a00f29bbc 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -43,8 +43,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> @@ -54,8 +55,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> @@ -67,12 +67,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 @@ -531,9 +532,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)); } @@ -994,7 +995,7 @@ MulticopterPositionControl::task_main() if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == 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; @@ -1155,11 +1156,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 { @@ -1267,7 +1268,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 */ @@ -1282,7 +1283,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/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/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 14ee9cb40..a2f8965bd 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -226,9 +226,20 @@ calculate_fw_crc(void) int user_start(int argc, char *argv[]) { +#if defined(CONFIG_HAVE_CXX) && defined(CONFIG_HAVE_CXXINITIALIZE) + /* run C++ ctors before we go any further */ + up_cxxinitialize(); +# if defined(CONFIG_EXAMPLES_NSH_CXXINITIALIZE) +# error CONFIG_EXAMPLES_NSH_CXXINITIALIZE Must not be defined! Use CONFIG_HAVE_CXX and CONFIG_HAVE_CXXINITIALIZE. +# endif + +#else +# error platform is dependent on c++ both CONFIG_HAVE_CXX and CONFIG_HAVE_CXXINITIALIZE must be defined. +#endif + /* reset all to zero */ memset(&system_state, 0, sizeof(system_state)); @@ -247,7 +258,7 @@ user_start(int argc, char *argv[]) #endif /* print some startup info */ - lowsyslog("\nPX4IO: starting\n"); + lowsyslog(LOG_INFO, "\nPX4IO: starting\n"); /* default all the LEDs to off while we start */ LED_AMBER(false); @@ -292,7 +303,7 @@ user_start(int argc, char *argv[]) perf_counter_t loop_perf = perf_alloc(PC_INTERVAL, "loop"); struct mallinfo minfo = mallinfo(); - lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks); + lowsyslog(LOG_INFO, "MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks); /* initialize PWM limit lib */ pwm_limit_init(&pwm_limit); @@ -312,7 +323,7 @@ user_start(int argc, char *argv[]) */ if (minfo.mxordblk < 600) { - lowsyslog("ERR: not enough MEM"); + lowsyslog(LOG_ERR, "ERR: not enough MEM"); bool phase = false; while (true) { diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 93a33490f..04fa94ae9 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -63,7 +63,7 @@ #ifdef DEBUG # include <debug.h> -# define debug(fmt, args...) lowsyslog(fmt "\n", ##args) +# define debug(fmt, args...) lowsyslog(LOG_DEBUG,fmt "\n", ##args) #else # define debug(fmt, args...) do {} while(0) #endif diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index d76ec55f0..fee9d656d 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -112,10 +112,8 @@ sbus_init(const char *device) partial_frame_count = 0; last_rx_time = hrt_absolute_time(); - debug("S.Bus: ready"); - } else { - debug("S.Bus: open failed"); + dbg("S.Bus: open failed"); } return sbus_fd; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index a3407e42c..b0127d02c 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -56,6 +56,7 @@ #include <string.h> #include <ctype.h> #include <systemlib/err.h> +#include <systemlib/px4_macros.h> #include <unistd.h> #include <drivers/drv_hrt.h> #include <math.h> @@ -70,6 +71,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> @@ -1088,6 +1093,11 @@ int sdlog2_thread_main(int argc, char *argv[]) int encoders_sub; } subs; + /* Make sure we have enough file descriptors to support this many subscriptions + * taking into account stdin,stdout and std err + */ + CCASSERT((sizeof(subs) / sizeof (int)) < CONFIG_NFILE_DESCRIPTORS-3); + subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); subs.status_sub = orb_subscribe(ORB_ID(vehicle_status)); subs.vtol_status_sub = orb_subscribe(ORB_ID(vtol_vehicle_status)); @@ -1117,7 +1127,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/sensors.cpp b/src/modules/sensors/sensors.cpp index 793b7c2b6..6aa6b6bbd 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 @@ -813,28 +813,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_FUNCTION_THROTTLE] = _parameters.rc_map_throttle - 1; + _rc.function[RC_CHANNELS_FUNCTION_ROLL] = _parameters.rc_map_roll - 1; + _rc.function[RC_CHANNELS_FUNCTION_PITCH] = _parameters.rc_map_pitch - 1; + _rc.function[RC_CHANNELS_FUNCTION_YAW] = _parameters.rc_map_yaw - 1; + + _rc.function[RC_CHANNELS_FUNCTION_MODE] = _parameters.rc_map_mode_sw - 1; + _rc.function[RC_CHANNELS_FUNCTION_RETURN] = _parameters.rc_map_return_sw - 1; + _rc.function[RC_CHANNELS_FUNCTION_POSCTL] = _parameters.rc_map_posctl_sw - 1; + _rc.function[RC_CHANNELS_FUNCTION_LOITER] = _parameters.rc_map_loiter_sw - 1; + _rc.function[RC_CHANNELS_FUNCTION_ACRO] = _parameters.rc_map_acro_sw - 1; + _rc.function[RC_CHANNELS_FUNCTION_OFFBOARD] = _parameters.rc_map_offboard_sw - 1; + + _rc.function[RC_CHANNELS_FUNCTION_FLAPS] = _parameters.rc_map_flaps - 1; + + _rc.function[RC_CHANNELS_FUNCTION_AUX_1] = _parameters.rc_map_aux1 - 1; + _rc.function[RC_CHANNELS_FUNCTION_AUX_2] = _parameters.rc_map_aux2 - 1; + _rc.function[RC_CHANNELS_FUNCTION_AUX_3] = _parameters.rc_map_aux3 - 1; + _rc.function[RC_CHANNELS_FUNCTION_AUX_4] = _parameters.rc_map_aux4 - 1; + _rc.function[RC_CHANNELS_FUNCTION_AUX_5] = _parameters.rc_map_aux5 - 1; 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_FUNCTION_PARAM_1 + i] = _parameters.rc_map_param[i] - 1; } /* gyro offsets */ @@ -891,8 +891,8 @@ Sensors::parameters_update() 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); @@ -1498,7 +1498,7 @@ Sensors::rc_parameter_map_poll(bool forced) 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_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 */ @@ -1646,7 +1646,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,7 +1667,7 @@ Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max } switch_pos_t -Sensors::get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv, float mid_th, bool mid_inv) +Sensors::get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv) { if (_rc.function[func] >= 0) { float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f; @@ -1688,7 +1688,7 @@ Sensors::get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, boo } switch_pos_t -Sensors::get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv) +Sensors::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv) { if (_rc.function[func] >= 0) { float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f; @@ -1710,14 +1710,14 @@ 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_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_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) { @@ -1847,24 +1847,24 @@ Sensors::rc_poll() manual.timestamp = rc_input.timestamp_last_signal; /* limit controls */ - manual.y = get_rc_value(ROLL, -1.0, 1.0); - manual.x = get_rc_value(PITCH, -1.0, 1.0); - manual.r = get_rc_value(YAW, -1.0, 1.0); - manual.z = get_rc_value(THROTTLE, 0.0, 1.0); - manual.flaps = get_rc_value(FLAPS, -1.0, 1.0); - manual.aux1 = get_rc_value(AUX_1, -1.0, 1.0); - manual.aux2 = get_rc_value(AUX_2, -1.0, 1.0); - manual.aux3 = get_rc_value(AUX_3, -1.0, 1.0); - manual.aux4 = get_rc_value(AUX_4, -1.0, 1.0); - manual.aux5 = get_rc_value(AUX_5, -1.0, 1.0); + manual.y = get_rc_value(RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0); + manual.x = get_rc_value(RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0); + manual.r = get_rc_value(RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0); + manual.z = get_rc_value(RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0); + manual.flaps = get_rc_value(RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0); + manual.aux1 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0); + manual.aux2 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0); + manual.aux3 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0); + manual.aux4 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0); + manual.aux5 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0); /* mode switches */ - manual.mode_switch = get_rc_sw3pos_position(MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv); - manual.posctl_switch = get_rc_sw2pos_position(POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv); - manual.return_switch = get_rc_sw2pos_position(RETURN, _parameters.rc_return_th, _parameters.rc_return_inv); - manual.loiter_switch = get_rc_sw2pos_position(LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv); - manual.acro_switch = get_rc_sw2pos_position(ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv); - manual.offboard_switch = get_rc_sw2pos_position(OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv); + manual.mode_switch = get_rc_sw3pos_position(RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv); + manual.posctl_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv); + manual.return_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th, _parameters.rc_return_inv); + manual.loiter_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv); + manual.acro_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv); + manual.offboard_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv); /* publish manual_control_setpoint topic */ if (_manual_control_pub > 0) { diff --git a/src/modules/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/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/topics/rc_channels.h b/src/modules/systemlib/px4_macros.h index 2fde5d424..956355b2c 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/systemlib/px4_macros.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,70 @@ ****************************************************************************/ /** - * @file rc_channels.h - * Definition of the rc_channels uORB topic. + * @file px4_macros.h * - * @deprecated DO NOT USE FOR NEW CODE + * A set of useful macros for enhanced runtime and compile time + * error detection and warning suppression. + * + * Define NO_BLOAT to reduce bloat from file name inclusion. + * + * The arraySize() will compute the size of an array regardless + * it's type + * + * INVALID_CASE(c) should be used is case statements to ferret out + * unintended behavior + * + * UNUSED(var) will suppress compile time warnings of unused + * variables + * + * CCASSERT(predicate) Will generate a compile time error it the + * predicate is false */ +#include <assert.h> -#ifndef RC_CHANNELS_H_ -#define RC_CHANNELS_H_ +#ifndef _PX4_MACROS_H +#define _PX4_MACROS_H -#include <stdint.h> -#include "../uORB.h" -/** - * This defines the mapping of the RC functions. - * The value assigned to the specific function corresponds to the entry of - * the channel array channels[]. - */ -enum RC_CHANNELS_FUNCTION { - THROTTLE = 0, - ROLL, - PITCH, - YAW, - MODE, - RETURN, - POSCTL, - LOITER, - OFFBOARD, - ACRO, - FLAPS, - AUX_1, - AUX_2, - AUX_3, - AUX_4, - AUX_5, - PARAM_1, - PARAM_2, - PARAM_3 -}; - -// MAXIMUM FUNCTIONS IS != MAXIMUM RC INPUT CHANNELS +#if !defined(arraySize) +#define arraySize(a) (sizeof((a))/sizeof((a[0]))) +#endif -#define RC_CHANNELS_FUNCTION_MAX 19 +#if !defined(NO_BLOAT) +#if defined(__BASE_FILE__) +#define _FILE_NAME_ __BASE_FILE__ +#else +#define _FILE_NAME_ __FILE__ +#endif +#else +#define _FILE_NAME_ "" +#endif -/** - * @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. */ +#if !defined(INVALID_CASE) +#define INVALID_CASE(c) printf("Invalid Case %d, %s:%d",(c),__BASE_FILE__,__LINE__) /* todo use PANIC */ +#endif -/** - * @} - */ +#if !defined(UNUSED) +#define UNUSED(var) (void)(var) +#endif -/* register this as object request broker structure */ -ORB_DECLARE(rc_channels); +#if !defined(CAT) +#if !defined(_CAT) +#define _CAT(a, b) a ## b +#endif +#define CAT(a, b) _CAT(a, b) +#endif +#if !defined(CCASSERT) +#if defined(static_assert) +# define CCASSERT(predicate) static_assert(predicate) +# else +# define CCASSERT(predicate) _x_CCASSERT_LINE(predicate, __LINE__) +# if !defined(_x_CCASSERT_LINE) +# define _x_CCASSERT_LINE(predicate, line) typedef char CAT(constraint_violated_on_line_,line)[2*((predicate)!=0)-1] __attribute__ ((unused)) ; +# endif +# endif #endif + + +#endif /* _PX4_MACROS_H */ diff --git a/src/modules/systemlib/up_cxxinitialize.c b/src/modules/systemlib/up_cxxinitialize.c index c78f29570..4028945e5 100644 --- a/src/modules/systemlib/up_cxxinitialize.c +++ b/src/modules/systemlib/up_cxxinitialize.c @@ -1,8 +1,8 @@ /************************************************************************************ - * configs/stm32f4discovery/src/up_cxxinitialize.c + * src/modules/systemlib/up_cxxinitialize.c * arch/arm/src/board/up_cxxinitialize.c * - * Copyright (C) 2012 Gregory Nutt. All rights reserved. + * Copyright (C) 2013 Gregory Nutt. All rights reserved. * Author: Gregory Nutt <gnutt@nuttx.org> * * Redistribution and use in source and binary forms, with or without @@ -44,9 +44,10 @@ #include <nuttx/arch.h> -//#include <arch/stm32/chip.h> -//#include "chip.h" +#include <arch/stm32/chip.h> +#include "chip.h" +#if defined(CONFIG_HAVE_CXX) && defined(CONFIG_HAVE_CXXINITIALIZE) /************************************************************************************ * Definitions ************************************************************************************/ @@ -122,7 +123,7 @@ extern uint32_t _etext; * ***************************************************************************/ -__EXPORT void up_cxxinitialize(void) +void up_cxxinitialize(void) { initializer_t *initp; @@ -148,3 +149,6 @@ __EXPORT void up_cxxinitialize(void) } } } + +#endif /* CONFIG_HAVE_CXX && CONFIG_HAVE_CXXINITIALIZE */ + diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index 71757e1f4..41a866968 100644 --- a/src/modules/uORB/Publication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -49,15 +49,16 @@ #include "topics/actuator_direct.h" #include "topics/encoders.h" #include "topics/tecs_status.h" +#include "topics/rc_channels.h" namespace uORB { template<class T> Publication<T>::Publication( - List<PublicationBase *> * list, - const struct orb_metadata *meta) : + const struct orb_metadata *meta, + List<PublicationNode *> * list) : T(), // initialize data structure to zero - PublicationBase(list, meta) { + PublicationNode(meta, list) { } template<class T> @@ -80,5 +81,6 @@ template class __EXPORT Publication<actuator_outputs_s>; template class __EXPORT Publication<actuator_direct_s>; template class __EXPORT Publication<encoders_s>; template class __EXPORT Publication<tecs_status_s>; +template class __EXPORT Publication<rc_channels_s>; } diff --git a/src/modules/uORB/Publication.hpp b/src/modules/uORB/Publication.hpp index 8650b3df8..fd1ee4dec 100644 --- a/src/modules/uORB/Publication.hpp +++ b/src/modules/uORB/Publication.hpp @@ -38,6 +38,8 @@ #pragma once +#include <assert.h> + #include <uORB/uORB.h> #include <containers/List.hpp> @@ -49,55 +51,112 @@ namespace uORB * Base publication warapper class, used in list traversal * of various publications. */ -class __EXPORT PublicationBase : public ListNode<uORB::PublicationBase *> +class __EXPORT PublicationBase { public: - PublicationBase( - List<PublicationBase *> * list, - const struct orb_metadata *meta) : + /** + * Constructor + * + * + * @param meta The uORB metadata (usually from the ORB_ID() + * macro) for the topic. + */ + PublicationBase(const struct orb_metadata *meta) : _meta(meta), _handle(-1) { - if (list != NULL) list->add(this); } - void update() { + + /** + * Update the struct + * @param data The uORB message struct we are updating. + */ + void update(void * data) { if (_handle > 0) { - orb_publish(getMeta(), getHandle(), getDataVoidPtr()); + orb_publish(getMeta(), getHandle(), data); } else { - setHandle(orb_advertise(getMeta(), getDataVoidPtr())); + setHandle(orb_advertise(getMeta(), data)); } } - virtual void *getDataVoidPtr() = 0; + + /** + * Deconstructor + */ virtual ~PublicationBase() { orb_unsubscribe(getHandle()); } +// accessors const struct orb_metadata *getMeta() { return _meta; } int getHandle() { return _handle; } protected: +// accessors void setHandle(orb_advert_t handle) { _handle = handle; } +// attributes const struct orb_metadata *_meta; orb_advert_t _handle; }; /** + * alias class name so it is clear that the base class + * can be used by itself if desired + */ +typedef PublicationBase PublicationTiny; + +/** + * The publication base class as a list node. + */ +class __EXPORT PublicationNode : + public PublicationBase, + public ListNode<PublicationNode *> +{ +public: + /** + * Constructor + * + * + * @param meta The uORB metadata (usually from the ORB_ID() + * macro) for the topic. + * @param list A pointer to a list of subscriptions + * that this should be appended to. + */ + PublicationNode(const struct orb_metadata *meta, + List<PublicationNode *> * list=nullptr) : + PublicationBase(meta) { + if (list != nullptr) list->add(this); + } + + /** + * This function is the callback for list traversal + * updates, a child class must implement it. + */ + virtual void update() = 0; +}; + +/** * Publication wrapper class */ template<class T> class Publication : public T, // this must be first! - public PublicationBase + public PublicationNode { public: /** * Constructor * - * @param list A list interface for adding to list during construction - * @param meta The uORB metadata (usually from the ORB_ID() macro) - * for the topic. + * @param meta The uORB metadata (usually from + * the ORB_ID() macro) for the topic. + * @param list A list interface for adding to + * list during construction */ - Publication(List<PublicationBase *> * list, - const struct orb_metadata *meta); + Publication(const struct orb_metadata *meta, + List<PublicationNode *> * list=nullptr); + + /** + * Deconstructor + **/ virtual ~Publication(); + /* * XXX * This function gets the T struct, assuming @@ -106,6 +165,13 @@ public: * seem to be available */ void *getDataVoidPtr(); + + /** + * Create an update function that uses the embedded struct. + */ + void update() { + PublicationBase::update(getDataVoidPtr()); + } }; } // namespace uORB diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index 44b6debc7..fa0594c2e 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 34e9a83e0..f82586285 100644 --- a/src/modules/uORB/Subscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -38,10 +38,11 @@ #pragma once +#include <assert.h> + #include <uORB/uORB.h> #include <containers/List.hpp> - namespace uORB { @@ -49,8 +50,7 @@ namespace uORB * Base subscription warapper class, used in list traversal * of various subscriptions. */ -class __EXPORT SubscriptionBase : - public ListNode<SubscriptionBase *> +class __EXPORT SubscriptionBase { public: // methods @@ -58,23 +58,42 @@ public: /** * Constructor * - * @param meta The uORB metadata (usually from the ORB_ID() macro) - * for the topic. + * @param meta The uORB metadata (usually from the ORB_ID() + * macro) for the topic. + * + * @param interval The minimum interval in milliseconds + * between updates */ - SubscriptionBase( - List<SubscriptionBase *> * list, - const struct orb_metadata *meta) : + SubscriptionBase(const struct orb_metadata *meta, + unsigned interval=0) : _meta(meta), _handle() { - if (list != NULL) list->add(this); + setHandle(orb_subscribe(getMeta())); + orb_set_interval(getHandle(), interval); } - bool updated(); - void update() { + + /** + * Check if there is a new update. + * */ + bool updated() { + bool isUpdated = false; + orb_check(_handle, &isUpdated); + return isUpdated; + } + + /** + * Update the struct + * @param data The uORB message struct we are updating. + */ + void update(void * data) { if (updated()) { - orb_copy(_meta, _handle, getDataVoidPtr()); + orb_copy(_meta, _handle, data); } } - virtual void *getDataVoidPtr() = 0; + + /** + * Deconstructor + */ virtual ~SubscriptionBase() { orb_unsubscribe(_handle); } @@ -90,30 +109,86 @@ protected: }; /** + * alias class name so it is clear that the base class + */ +typedef SubscriptionBase SubscriptionTiny; + +/** + * The publication base class as a list node. + */ +class __EXPORT SubscriptionNode : + + public SubscriptionBase, + public ListNode<SubscriptionNode *> +{ +public: + /** + * Constructor + * + * + * @param meta The uORB metadata (usually from the ORB_ID() + * macro) for the topic. + * @param interval The minimum interval in milliseconds + * between updates + * @param list A pointer to a list of subscriptions + * that this should be appended to. + */ + SubscriptionNode(const struct orb_metadata *meta, + unsigned interval=0, + List<SubscriptionNode *> * list=nullptr) : + SubscriptionBase(meta, interval), + _interval(interval) { + if (list != nullptr) list->add(this); + } + + /** + * This function is the callback for list traversal + * updates, a child class must implement it. + */ + virtual void update() = 0; +// accessors + unsigned getInterval() { return _interval; } +protected: +// attributes + unsigned _interval; + +}; + +/** * Subscription wrapper class */ template<class T> class __EXPORT Subscription : public T, // this must be first! - public SubscriptionBase + public SubscriptionNode { public: /** * Constructor * - * @param list A list interface for adding to list during construction - * @param meta The uORB metadata (usually from the ORB_ID() macro) - * for the topic. - * @param interval The minimum interval in milliseconds between updates + * @param meta The uORB metadata (usually from + * the ORB_ID() macro) for the topic. + * @param interval The minimum interval in milliseconds + * between updates + * @param list A list interface for adding to + * list during construction */ - Subscription( - List<SubscriptionBase *> * list, - const struct orb_metadata *meta, unsigned interval); + Subscription(const struct orb_metadata *meta, + unsigned interval=0, + List<SubscriptionNode *> * list=nullptr); /** * Deconstructor */ virtual ~Subscription(); + + /** + * Create an update function that uses the embedded struct. + */ + void update() { + SubscriptionBase::update(getDataVoidPtr()); + } + /* * XXX * This function gets the T struct, assuming diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 78fdf4de7..204a114e1 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -121,8 +121,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); @@ -192,13 +194,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/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_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_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.h b/src/modules/uORB/uORB.h index beb23f61d..672f8d8d1 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -152,7 +152,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. @@ -288,4 +288,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/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/nuttx-configs/px4io-v1/nsh/appconfig b/src/platforms/nuttx/module.mk index 48a41bcdb..4a2aff824 100644 --- a/nuttx-configs/px4io-v1/nsh/appconfig +++ b/src/platforms/nuttx/module.mk @@ -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 @@ -30,3 +30,11 @@ # 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..c8e2cf290 --- /dev/null +++ b/src/platforms/px4_defines.h @@ -0,0 +1,204 @@ +/**************************************************************************** + * + * 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) */ + +#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..1bd4509ca --- /dev/null +++ b/src/platforms/px4_includes.h @@ -0,0 +1,93 @@ +/**************************************************************************** + * + * 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> +#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> +#endif +#include <systemlib/err.h> +#include <systemlib/param/param.h> +#include <systemlib/systemlib.h> + +#endif diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/platforms/px4_message.h index 668f8f164..bff7aa313 100644 --- a/src/modules/uORB/topics/actuator_controls.h +++ b/src/platforms/px4_message.h @@ -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,46 @@ ****************************************************************************/ /** - * @file actuator_controls.h + * @file px4_message.h * - * 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 + * Defines the message base types */ +#pragma once -#ifndef TOPIC_ACTUATOR_CONTROLS_H -#define TOPIC_ACTUATOR_CONTROLS_H - -#include <stdint.h> -#include "../uORB.h" - -#define NUM_ACTUATOR_CONTROLS 8 -#define NUM_ACTUATOR_CONTROL_GROUPS 4 /**< for sanity checking */ +#if defined(__PX4_ROS) +typedef const char* PX4TopicHandle; +#else +#include <uORB/uORB.h> +typedef orb_id_t PX4TopicHandle; +#endif -/* control sets with pre-defined applications */ -#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) +namespace px4 +{ -/** - * @addtogroup topics - * @{ - */ +template <typename M> +class __EXPORT PX4Message +{ + // friend class NodeHandle; +// #if defined(__PX4_ROS) + // template<typename T> + // friend class SubscriberROS; +// #endif -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]; -}; +public: + PX4Message() : + _data() + {} -/** - * @} - */ + PX4Message(M msg) : + _data(msg) + {} -/* 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); + virtual ~PX4Message() {}; + 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/modules/uORB/topics/vehicle_control_mode.h b/src/platforms/ros/geo.cpp index ca7705456..6fad681c9 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/platforms/ros/geo.cpp @@ -32,64 +32,142 @@ ****************************************************************************/ /** - * @file vehicle_control_mode.h - * Definition of the vehicle_control_mode uORB topic. + * @file geo.c * - * All control apps should depend their actions based on the flags set here. + * Geo / math functions to perform geodesic calculations * - * @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> + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> */ -#ifndef VEHICLE_CONTROL_MODE -#define VEHICLE_CONTROL_MODE - -#include <stdint.h> +#include <geo/geo.h> +#include <px4.h> +#include <unistd.h> +#include <pthread.h> +#include <stdio.h> +#include <math.h> #include <stdbool.h> -#include "../uORB.h" -#include "vehicle_status.h" +#include <string.h> +#include <float.h> -/** - * @addtogroup topics @{ - */ +__EXPORT float _wrap_pi(float bearing) +{ + /* value is inf or NaN */ + if (!isfinite(bearing)) { + return bearing; + } + int c = 0; -/** - * state machine / state of vehicle. - * - * Encodes the complete system state and is set by the commander app. - */ + while (bearing >= M_PI_F) { + bearing -= M_TWOPI_F; -struct vehicle_control_mode_s { - uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ + if (c++ > 3) { + return NAN; + } + } - bool flag_armed; + c = 0; - bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ + while (bearing < -M_PI_F) { + bearing += M_TWOPI_F; - // XXX needs yet to be set by state machine helper - bool flag_system_hil_enabled; + if (c++ > 3) { + return NAN; + } + } - 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 */ -}; + 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; -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_control_mode); + if (c++ > 3) { + return NAN; + } + } -#endif + return bearing; +} 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..bcee0b479 --- /dev/null +++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp @@ -0,0 +1,149 @@ +/**************************************************************************** + * + * 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> + +AttitudeEstimator::AttitudeEstimator() : + _n(), + // _sub_modelstates(_n.subscribe("/gazebo/model_states", 1, &AttitudeEstimator::ModelStatesCallback, this)), + _sub_imu(_n.subscribe("/px4_multicopter/imu", 1, &AttitudeEstimator::ImuCallback, this)), + _vehicle_attitude_pub(_n.advertise<px4::vehicle_attitude>("vehicle_attitude", 1)) +{ +} + +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; + + _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; + + _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..b9fc296f9 --- /dev/null +++ b/src/platforms/ros/nodes/commander/commander.cpp @@ -0,0 +1,116 @@ +/**************************************************************************** + * + * 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 <px4/manual_control_setpoint.h> +#include <px4/vehicle_control_mode.h> +#include <px4/vehicle_status.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() +{ +} + +void Commander::ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg) +{ + px4::vehicle_control_mode msg_vehicle_control_mode; + px4::vehicle_status msg_vehicle_status; + + /* fill vehicle control mode */ + //XXX hardcoded + msg_vehicle_control_mode.timestamp = px4::get_time_micros(); + 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; + + /* 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; + } + + } else { + /* Check for arm */ + if (msg->r > arm_th && msg->z < (1 - arm_th)) { + _msg_actuator_armed.armed = true; + } + } + + /* fill vehicle status */ + //XXX hardcoded + msg_vehicle_status.timestamp = px4::get_time_micros(); + msg_vehicle_status.main_state = msg_vehicle_status.MAIN_STATE_MANUAL; + msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_MANUAL; + msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_ARMED; + 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); + } +} + +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..bd4092b3a --- /dev/null +++ b/src/platforms/ros/nodes/commander/commander.h @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * 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/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); + + 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; + +}; 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..688df50e0 --- /dev/null +++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * 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 <px4/manual_control_setpoint.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)) +{ + 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], dz_default); + + _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); + +} + +void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr &msg) +{ + px4::manual_control_setpoint msg_out; + + /* 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_out.x); + MapAxis(msg, _param_axes_map[1], _param_axes_scale[1], _param_axes_offset[1], _param_axes_dz[1], msg_out.y); + MapAxis(msg, _param_axes_map[2], _param_axes_scale[2], _param_axes_offset[2], _param_axes_dz[2], msg_out.z); + MapAxis(msg, _param_axes_map[3], _param_axes_scale[3], _param_axes_offset[3], _param_axes_dz[3], msg_out.r); + //ROS_INFO("x: %1.4f y: %1.4f z: %1.4f r: %1.4f", msg_out.x, msg_out.y, msg_out.z, msg_out.r); + + /* Map buttons to switches */ + //XXX todo + /* for now just publish switches in position for manual flight */ + msg_out.mode_switch = 0; + msg_out.return_switch = 0; + msg_out.posctl_switch = 0; + msg_out.loiter_switch = 0; + msg_out.acro_switch = 0; + msg_out.offboard_switch = 0; + + msg_out.timestamp = px4::get_time_micros(); + + _man_ctrl_sp_pub.publish(msg_out); +} + +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; + } +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "manual_input"); + ManualInput m; + ros::spin(); + return 0; +} diff --git a/src/platforms/ros/nodes/manual_input/manual_input.h b/src/platforms/ros/nodes/manual_input/manual_input.h new file mode 100644 index 000000000..93e0abe64 --- /dev/null +++ b/src/platforms/ros/nodes/manual_input/manual_input.h @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * 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.h + * Reads from the ros joystick topic and publishes to the px4 manual control setpoint topic. + * + * @author Thomas Gubler <thomasgubler@gmail.com> +*/ + +#include "ros/ros.h" +#include <sensor_msgs/Joy.h> + +class ManualInput +{ +public: + ManualInput(); + + ~ManualInput() {} + +protected: + /** + * Takes ROS joystick message and converts/publishes to px4 manual control setpoint topic + */ + void JoyCallback(const sensor_msgs::JoyConstPtr &msg); + + /** + * Helper function to map and scale joystick input + */ + void MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, double scale, double offset, double deadzone, + float &out); + + ros::NodeHandle _n; + ros::Subscriber _joy_sub; + ros::Publisher _man_ctrl_sp_pub; + + /* Parameters */ + int _param_axes_map[4]; + double _param_axes_scale[4]; + double _param_axes_offset[4]; + double _param_axes_dz[4]; + + +}; 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..54f5fa78b --- /dev/null +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -0,0 +1,276 @@ +/**************************************************************************** + * + * 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]; + ROS_WARN("using x"); + } 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]; + ROS_WARN("using w"); + } else if (mixer_name == "i") { + _rotors = _config_index[4]; + ROS_WARN("using i"); + } + ROS_WARN("mixer_name %s", mixer_name.c_str()); + + // 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/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/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c index 7d80af307..0935bf44a 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> @@ -51,9 +52,6 @@ #include <sys/ioctl.h> #include <sys/stat.h> -#include <nuttx/i2c.h> -#include <nuttx/mtd.h> -#include <nuttx/fs/nxffs.h> #include <nuttx/fs/ioctl.h> #include <arch/board/board.h> @@ -63,6 +61,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/mtd/24xxxx_mtd.c b/src/systemcmds/mtd/24xxxx_mtd.c index f85ed8e2d..d0407f5aa 100644 --- a/src/systemcmds/mtd/24xxxx_mtd.c +++ b/src/systemcmds/mtd/24xxxx_mtd.c @@ -64,7 +64,7 @@ #include <nuttx/kmalloc.h> #include <nuttx/fs/ioctl.h> #include <nuttx/i2c.h> -#include <nuttx/mtd.h> +#include <nuttx/mtd/mtd.h> #include "systemlib/perf_counter.h" diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c index a925cdd40..16bfb294f 100644 --- a/src/systemcmds/mtd/mtd.c +++ b/src/systemcmds/mtd/mtd.c @@ -51,8 +51,8 @@ #include <sys/ioctl.h> #include <sys/stat.h> -#include <nuttx/spi.h> -#include <nuttx/mtd.h> +#include <nuttx/spi/spi.h> +#include <nuttx/mtd/mtd.h> #include <nuttx/fs/nxffs.h> #include <nuttx/fs/ioctl.h> diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index eeba89fa8..4bc1a727b 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -46,13 +46,9 @@ #include <unistd.h> #include <fcntl.h> #include <poll.h> -#include <sys/mount.h> #include <sys/ioctl.h> #include <sys/stat.h> -#include <nuttx/i2c.h> -#include <nuttx/mtd.h> -#include <nuttx/fs/nxffs.h> #include <nuttx/fs/ioctl.h> #include <arch/board/board.h> diff --git a/src/systemcmds/tests/test_adc.c b/src/systemcmds/tests/test_adc.c index 03391b851..82f39439d 100644 --- a/src/systemcmds/tests/test_adc.c +++ b/src/systemcmds/tests/test_adc.c @@ -48,8 +48,6 @@ #include <errno.h> #include <debug.h> -#include <nuttx/spi.h> - #include "tests.h" #include <nuttx/analog/adc.h> diff --git a/src/systemcmds/tests/test_hrt.c b/src/systemcmds/tests/test_hrt.c index 5690997a9..52b703bd2 100644 --- a/src/systemcmds/tests/test_hrt.c +++ b/src/systemcmds/tests/test_hrt.c @@ -54,8 +54,6 @@ #include <drivers/drv_hrt.h> #include <drivers/drv_tone_alarm.h> -#include <nuttx/spi.h> - #include "tests.h" /**************************************************************************** diff --git a/src/systemcmds/tests/test_jig_voltages.c b/src/systemcmds/tests/test_jig_voltages.c index 10c93b264..073474620 100644 --- a/src/systemcmds/tests/test_jig_voltages.c +++ b/src/systemcmds/tests/test_jig_voltages.c @@ -47,8 +47,6 @@ #include <errno.h> #include <debug.h> -#include <nuttx/spi.h> - #include "tests.h" #include <nuttx/analog/adc.h> diff --git a/src/systemcmds/tests/test_sensors.c b/src/systemcmds/tests/test_sensors.c index e005bf9c1..d87551bd9 100644 --- a/src/systemcmds/tests/test_sensors.c +++ b/src/systemcmds/tests/test_sensors.c @@ -53,8 +53,6 @@ #include <arch/board/board.h> -#include <nuttx/spi.h> - #include "tests.h" #include <drivers/drv_gyro.h> diff --git a/src/systemcmds/tests/test_servo.c b/src/systemcmds/tests/test_servo.c index 9c6951ca2..9545e7dc9 100644 --- a/src/systemcmds/tests/test_servo.c +++ b/src/systemcmds/tests/test_servo.c @@ -53,8 +53,6 @@ #include <drivers/drv_pwm_output.h> #include <systemlib/err.h> -#include <nuttx/spi.h> - #include "tests.h" int test_servo(int argc, char *argv[]) diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 0f56704e6..16fa3469f 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -52,8 +52,6 @@ #include <arch/board/board.h> -#include <nuttx/spi.h> - #include <systemlib/perf_counter.h> #include "tests.h" diff --git a/src/systemcmds/top/top.c b/src/systemcmds/top/top.c index 37e913040..03a049386 100644 --- a/src/systemcmds/top/top.c +++ b/src/systemcmds/top/top.c @@ -41,6 +41,7 @@ */ #include <nuttx/config.h> +#include <nuttx/arch.h> #include <stdio.h> #include <fcntl.h> #include <stdbool.h> @@ -217,18 +218,20 @@ top_main(void) ); } - unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr - - (uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr; - unsigned stack_free = 0; - uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr; - while (stack_free < stack_size) { - if (*stack_sweeper++ != 0xff) - break; + size_t stack_size = system_load.tasks[i].tcb->adj_stack_size; + ssize_t stack_free = 0; - stack_free++; +#if CONFIG_ARCH_INTERRUPTSTACK > 3 + if (system_load.tasks[i].tcb->pid == 0) { + stack_size = (CONFIG_ARCH_INTERRUPTSTACK & ~3); + stack_free = up_check_intstack_remain(); + } else { +#endif + stack_free = up_check_tcbstack_remain(system_load.tasks[i].tcb); +#if CONFIG_ARCH_INTERRUPTSTACK > 3 } - +#endif printf(CL "%4d %*-s %8lld %2d.%03d %5u/%5u %3u (%3u) ", system_load.tasks[i].tcb->pid, CONFIG_TASK_NAME_SIZE, system_load.tasks[i].tcb->name, diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index 666570a71..ee99b5a41 100644 --- a/unittests/CMakeLists.txt +++ b/unittests/CMakeLists.txt @@ -25,6 +25,9 @@ include_directories(${PX_SRC}/modules) include_directories(${PX_SRC}/lib) add_definitions(-D__EXPORT=) +add_definitions(-D__PX4_TESTS) +add_definitions(-Dnoreturn_function=) +add_definitions(-Dmain_t=int) # check add_custom_target(unittests COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure) |