diff --git a/.gitignore b/.gitignore index 4a12b5f3bc..8b1b934563 100644 --- a/.gitignore +++ b/.gitignore @@ -64,7 +64,6 @@ CMakeLists.txt.user GPATH GRTAGS GTAGS -*.config *.creator *.creator.user *.files diff --git a/.gitmodules b/.gitmodules index dc9e183976..513501ab5c 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,12 +1,15 @@ [submodule "mavlink/include/mavlink/v1.0"] path = mavlink/include/mavlink/v1.0 - url = git://github.com/mavlink/c_library.git + url = https://github.com/mavlink/c_library_v1.git +[submodule "mavlink/include/mavlink/v2.0"] + path = mavlink/include/mavlink/v2.0 + url = https://github.com/mavlink/c_library_v2.git [submodule "NuttX"] path = NuttX - url = git://github.com/PX4/NuttX.git + url = https://github.com/PX4/NuttX.git [submodule "src/modules/uavcan/libuavcan"] path = src/modules/uavcan/libuavcan - url = git://github.com/UAVCAN/libuavcan.git + url = https://github.com/UAVCAN/libuavcan.git [submodule "Tools/genmsg"] path = Tools/genmsg url = https://github.com/ros/genmsg.git diff --git a/.travis.yml b/.travis.yml index 1ebc7fe7b2..f0abcb58b1 100644 --- a/.travis.yml +++ b/.travis.yml @@ -7,129 +7,117 @@ matrix: fast_finish: true include: - os: linux - sudo: false - env: GCC_VER=4.8 - - os: linux - sudo: false - env: GCC_VER=4.9 + sudo: required + env: GCC_VER=4.8 DOCKER_REPO="px4io/px4-dev-base:2016-07-14" + services: + - docker - os: osx - osx_image: xcode7 sudo: true + osx_image: xcode7.3 + env: CCACHE_CPP2=1 cache: + ccache: true + pip: true directories: - - $HOME/.ccache - -addons: - apt: - sources: - - kubuntu-backports - - ubuntu-toolchain-r-test - - george-edison55-precise-backports - packages: - - build-essential - - ccache - - clang-3.5 - - cmake - - g++-4.9 - - gcc-4.9 - - genromfs - - libc6-i386 - - libncurses5-dev - - ninja-build - - python-argparse - - python-empy - - s3cmd - - texinfo - - zlib1g-dev + - $HOME/.pip/cache/ + - $HOME/Library/Caches/pip before_install: - if [ "${TRAVIS_OS_NAME}" = "linux" ]; then - pushd . - && cd ~ && mkdir gcc && cd gcc - && if [ "$GCC_VER" = "4.8" ]; then GCC_URL="https://launchpadlibrarian.net/186124160/gcc-arm-none-eabi-4_8-2014q3-20140805-linux.tar.bz2" ; fi - && if [ "$GCC_VER" = "4.9" ]; then GCC_URL="https://launchpad.net/gcc-arm-embedded/4.9/4.9-2014-q4-major/+download/gcc-arm-none-eabi-4_9-2014q4-20141203-linux.tar.bz2" ; fi - && wget -O gcc.tar.bz2 ${GCC_URL} - && tar -jxf gcc.tar.bz2 --strip 1 - && exportline="export PATH=$HOME/gcc/bin:\$PATH" - && if grep -Fxq "$exportline" ~/.profile; then echo nothing to do ; else echo $exportline >> ~/.profile; fi - && . ~/.profile - && popd - && git clone git://github.com/PX4/CI-Tools.git - && ./CI-Tools/s3cmd-configure - && mkdir -p ~/bin - && wget -O ~/bin/astyle https://github.com/PX4/astyle/releases/download/2.05.1/astyle-linux && chmod +x ~/bin/astyle - && astyle --version - && if [ "$CXX" = "g++" ]; then export CXX="g++-4.9" CC="gcc-4.9"; fi + cd ${TRAVIS_BUILD_DIR} + && git fetch --unshallow && git fetch --all --tags + && docker pull ${DOCKER_REPO} ; elif [ "${TRAVIS_OS_NAME}" = "osx" ]; then - brew tap PX4/homebrew-px4 - && brew update; brew update - && brew install cmake ninja - && brew install genromfs - && sudo easy_install pip - && sudo pip install empy + sudo -H easy_install pip + && sudo -H pip install empy + && wget https://s3.amazonaws.com/px4-travis/toolchain/macos/ccache + && sudo mv ccache /usr/local/bin + && chmod +x /usr/local/bin/ccache + && mkdir -p ~/bin + && sudo ln -s /usr/local/bin/ccache ~/bin/c++ + && sudo ln -s /usr/local/bin/ccache ~/bin/cc + && sudo ln -s /usr/local/bin/ccache ~/bin/clang + && sudo ln -s /usr/local/bin/ccache ~/bin/clang++ + && sudo ln -s /usr/local/bin/ccache ~/bin/g++ + && sudo ln -s /usr/local/bin/ccache ~/bin/gcc + && export PATH=~/bin:$PATH + && wget https://s3.amazonaws.com/px4-travis/toolchain/macos/ninja + && sudo mv ninja /usr/local/bin + && chmod +x /usr/local/bin/ninja ; fi -before_script: -# setup ccache - - mkdir -p ~/bin - - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-g++ - - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-gcc - - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-size - - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-objcopy - - ln -s /usr/bin/ccache ~/bin/clang++ - - ln -s /usr/bin/ccache ~/bin/clang++-3.4 - - ln -s /usr/bin/ccache ~/bin/clang++-3.5 - - ln -s /usr/bin/ccache ~/bin/clang - - ln -s /usr/bin/ccache ~/bin/clang-3.4 - - ln -s /usr/bin/ccache ~/bin/clang-3.5 - - export PATH=~/bin:$PATH - env: global: - - NINJA_BUILD=1 # AWS KEY: $PX4_AWS_KEY - secure: "XknnZHWBbpHbN4f3fuAVwUztdLIu8ej4keC3aQSDofo3uw8AFEzojfsQsN9u77ShWSIV4iYJWh9C9ALkCx7TocJ+xYjiboo10YhM9lH/8u+EXjYWG6GHS8ua0wkir+cViSxoLNaMtmcb/rPTicJecAGANxLsIHyBAgTL3fkbLSA=" # AWS SECRET: $PX4_AWS_SECRET - secure: "h6oajlW68dWIr+wZhO58Dv6e68dZHrBLVA6lPXZmheFQBW6Xam1HuLGA0LOW6cL9TnrAsOZ8g4goB58eMQnMEijFZKi3mhRwZhd/Xjq/ZGJOWBUrLoQHZUw2dQk5ja5vmUlKEoQnFZjDuMjx8KfX5ZMNy8A3yssWZtJYHD8c+bk=" - PX4_AWS_BUCKET=px4-travis + - GIT_SUBMODULES_ARE_EVIL=1 script: - - git submodule update --quiet --init --recursive + - ccache -M 1GB; ccache -z - if [ "${TRAVIS_OS_NAME}" = "linux" ]; then - arm-none-eabi-gcc --version && make check VECTORCONTROL=1; + docker run --rm -v `pwd`:`pwd`:rw -v $HOME/.ccache:$HOME/.ccache:rw -e CCACHE_DIR=$HOME/.ccache -e GIT_SUBMODULES_ARE_EVIL=1 -w=`pwd` --user=$UID -it ${DOCKER_REPO} /bin/bash -c "make check_qgc_firmware VECTORCONTROL=1"; elif [ "${TRAVIS_OS_NAME}" = "osx" ]; then make check_posix_sitl_default; fi + - ccache -s after_success: - - if [[ "${TRAVIS_OS_NAME}" = "linux" && "${GCC_VER}" = "4.8" ]]; then - make package_firmware - && find . -name \*.px4 -exec cp "{}" . \; - && find . -maxdepth 1 -mindepth 1 -type f -name 'nuttx-*-default.px4' | sed 's/.\/nuttx-//' | sed 's/-default.px4//' | xargs -I{} mv nuttx-{}-default.px4 {}_default.px4 - && ./CI-Tools/s3cmd-put `find . -maxdepth 1 -mindepth 1 -type f -name '*_default.px4'` build_px4fmu-v2_default/parameters.xml build_px4fmu-v2_default/airframes.xml CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/ - && ./CI-Tools/s3cmd-put Firmware.zip archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/ - && ./CI-Tools/s3cmd-put CI-Tools/directory/index.html archives/Firmware/$TRAVIS_BRANCH/ - && ./CI-Tools/s3cmd-put CI-Tools/index.html index.html - && ./CI-Tools/s3cmd-put CI-Tools/timestamp.html timestamp.html - && echo "" - && echo "Binaries have been posted to:" https://px4-travis.s3.amazonaws.com/archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/Firmware.zip - ; + - make package_firmware && mkdir s3deploy-archive && cp Firmware.zip s3deploy-archive/ +# find all px4 firmware (*.px4) and rename + - find . -type f -name 'nuttx-*-default.px4' -exec cp "{}" . \; + - find . -maxdepth 1 -mindepth 1 -type f -name 'nuttx-*-default.px4' | sed 's/.\/nuttx-//' | sed 's/-default.px4//' | xargs -I{} mv nuttx-{}-default.px4 {}_default.px4 + - mkdir s3deploy-branch && mv *_default.px4 build_px4fmu-v2_default/parameters.xml build_px4fmu-v2_default/airframes.xml s3deploy-branch/ +# only deploy GCC 4.8 builds on master/beta/stable + - if [[ "$GCC_VER" == "4.8" && ( "$TRAVIS_BRANCH" == "master" || "$TRAVIS_BRANCH" == "beta" || "$TRAVIS_BRANCH" == "stable" ) ]]; then + export PX4_S3_DEPLOY=1; fi deploy: - provider: releases - api_key: - secure: cdHWLRBxA5UlYpOS0Sp891QK7PFmMgQ5ZWs1aPt+sw0rIrowyWMHCwXNBEdUqaExHYNYgXCUDI0EzNgfB7ZcR63Qv1MQeoyamV4jsxlyAqDqmxNtWO82S6RhHGeMLk26VgFKzynVcEk1IYlQP2nqzMQLdu+jTrngERuAIrCdRuc= - file: "Firmware.zip" - skip_cleanup: true - on: - tags: true - all_branches: true - repo: PX4/Firmware - condition: $GCC_VER = 4.8 +# deploy *.px4 to S3 px4-travis/Firmware/$TRAVIS_BRANCH + - provider: s3 + access_key_id: $PX4_AWS_KEY + secret_access_key: + secure: $PX4_AWS_SECRET + bucket: px4-travis + local_dir: s3deploy-branch + upload-dir: Firmware/$TRAVIS_BRANCH + acl: public_read + skip_cleanup: true + on: + all_branches: true + condition: $PX4_S3_DEPLOY = 1 + +# deploy Firmware.zip to S3 px4-travis/archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID + - provider: s3 + access_key_id: $PX4_AWS_KEY + secret_access_key: + secure: $PX4_AWS_SECRET + bucket: px4-travis + local_dir: s3deploy-archive + upload-dir: archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID + acl: public_read + skip_cleanup: true + on: + all_branches: true + condition: $PX4_S3_DEPLOY = 1 + +# on tags deploy Firmware.zip to Github releases + - provider: releases + api_key: + secure: cdHWLRBxA5UlYpOS0Sp891QK7PFmMgQ5ZWs1aPt+sw0rIrowyWMHCwXNBEdUqaExHYNYgXCUDI0EzNgfB7ZcR63Qv1MQeoyamV4jsxlyAqDqmxNtWO82S6RhHGeMLk26VgFKzynVcEk1IYlQP2nqzMQLdu+jTrngERuAIrCdRuc= + file: "Firmware.zip" + skip_cleanup: true + on: + tags: true + all_branches: true + repo: PX4/Firmware + condition: $GCC_VER = 4.8 notifications: webhooks: diff --git a/CMakeLists.txt b/CMakeLists.txt index 019984d9ba..9397b87c99 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -132,7 +132,7 @@ cmake_minimum_required(VERSION 2.8 FATAL_ERROR) set(CMAKE_BUILD_TYPE "" CACHE STRING "build type") set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS ";Debug;Release;RelWithDebInfo;MinSizeRel") -set(CONFIG "nuttx_px4fmu-v2_default" CACHE STRING "desired configuration") +set(CONFIG "posix_sitl_default" CACHE STRING "desired configuration") file(GLOB_RECURSE configs RELATIVE cmake/configs "cmake/configs/*.cmake") set_property(CACHE CONFIG PROPERTY STRINGS ${configs}) set(THREADS "4" CACHE STRING @@ -152,6 +152,17 @@ set(target_name "${OS}-${BOARD}-${LABEL}") message(STATUS "${target_name}") +# Define GNU standard installation directories +include(GNUInstallDirs) + +# Setup install paths +if(NOT CMAKE_INSTALL_PREFIX) + if (${OS} STREQUAL "posix") + set(CMAKE_INSTALL_PREFIX "/usr" CACHE PATH "Install path prefix" FORCE) + endif() +endif() +message(STATUS "CMAKE_INSTALL_PREFIX: ${CMAKE_INSTALL_PREFIX}") + # switch to ros CMake file if building ros if (${OS} STREQUAL "ros") include("cmake/ros-CMakeLists.txt") @@ -202,7 +213,7 @@ if (NOT ${CMAKE_VERSION} VERSION_LESS 3.1.0) endif() set(version_major 1) -set(version_minor 0) +set(version_minor 4) set(version_patch 1) set(version "${version_major}.${version_minor}.${version_patch}") set(package-contact "px4users@googlegroups.com") @@ -235,7 +246,7 @@ endforeach() px4_add_git_submodule(TARGET git_genmsg PATH "Tools/genmsg") px4_add_git_submodule(TARGET git_gencpp PATH "Tools/gencpp") px4_add_git_submodule(TARGET git_mavlink PATH "mavlink/include/mavlink/v1.0") -px4_add_git_submodule(TARGET git_gtest PATH "unittets/gtest") +px4_add_git_submodule(TARGET git_gtest PATH "unittests/gtest") px4_add_git_submodule(TARGET git_uavcan PATH "src/modules/uavcan/libuavcan") px4_add_git_submodule(TARGET git_nuttx PATH "NuttX") px4_add_git_submodule(TARGET git_driverframework PATH "src/lib/DriverFramework") @@ -295,11 +306,11 @@ add_definitions(${definitions}) #============================================================================= # source code generation # -file(GLOB_RECURSE msg_files msg/*.msg) +add_subdirectory(msg) px4_generate_messages(TARGET msg_gen MSG_FILES ${msg_files} OS ${OS} - DEPENDS git_genmsg git_gencpp + DEPENDS git_genmsg git_gencpp prebuild_targets ) px4_generate_parameters_xml(OUT parameters.xml BOARD ${BOARD}) px4_generate_airframes_xml(OUT airframes.xml BOARD ${BOARD}) @@ -348,6 +359,9 @@ foreach(module ${config_module_list}) #message(STATUS "adding module: ${module}") endforeach() +# Keep track of external shared libs required for modules +set(module_external_libraries "${module_external_libraries}" CACHE INTERNAL "module_external_libraries") + add_subdirectory(src/firmware/${OS}) #add_dependencies(df_driver_framework nuttx_export_${CONFIG}.stamp) @@ -368,10 +382,27 @@ px4_create_git_hash_header(HEADER ${CMAKE_BINARY_DIR}/build_git_version.h) # # Important to having packaging at end of cmake file. # +set(CPACK_PACKAGE_NAME ${PROJECT_NAME}-${CONFIG}) set(CPACK_PACKAGE_VERSION ${version}) -set(CPACK_PACKAGE_CONTACT ${package_contact}) +set(CPACK_PACKAGE_CONTACT ${package-contact}) +set(CPACK_DEBIAN_PACKAGE_SHLIBDEPS ON) +set(CPACK_DEBIAN_PACKAGE_SECTION "devel") +set(CPACK_DEBIAN_PACKAGE_PRIORITY "optional") +set(short-description "The px4 autopilot.") +set(CPACK_DEBIAN_PACKAGE_DESCRIPTION ${short-description}) set(CPACK_GENERATOR "ZIP") -set(CPACK_SOURCE_GENERATOR "ZIP") +set(CPACK_PACKAGE_FILE_NAME "${PROJECT_NAME}-${CONFIG}-${version}") +set(CPACK_SOURCE_PACKAGE_FILE_NAME "${PROJECT_NAME}-${version}") +set(CPACK_SOURCE_GENERATOR "ZIP;TBZ2") +if ("${CMAKE_SYSTEM}" MATCHES "Linux") + find_program(DPKG_PROGRAM dpkg) + if (EXISTS ${DPKG_PROGRAM}) + list (APPEND CPACK_GENERATOR "DEB") + endif() +endif() +if (${OS} STREQUAL "posix") + set(CPACK_SET_DESTDIR "ON") +endif() include(CPack) endif() # ros alternative endif diff --git a/Documentation/px4_hil/UserGuide.md b/Documentation/px4_hil/UserGuide.md index 28483498da..30da6d453a 100644 --- a/Documentation/px4_hil/UserGuide.md +++ b/Documentation/px4_hil/UserGuide.md @@ -66,8 +66,8 @@ Linux or Eagle with a working IP interface (?? does this need further instructio > adb shell # bash root@linaro-developer:/# cd ??? -root@linaro-developer:/# ./mainapp -App name: mainapp +root@linaro-developer:/# ./px4 +App name: px4 Enter a command and its args: uorb start muorb start diff --git a/Images/mindpx-v2.prototype b/Images/mindpx-v2.prototype index 1a5ebb8f9d..c99c883b89 100644 --- a/Images/mindpx-v2.prototype +++ b/Images/mindpx-v2.prototype @@ -1,10 +1,10 @@ { - "board_id": 9, - "magic": "PX4FWv2", - "description": "Firmware for the MindPx-V2 board", + "board_id": 88, + "magic": "MindPX", + "description": "Firmware for the MindPXFMUv2 board", "image": "", "build_time": 0, - "summary": "MindPx-v2", + "summary": "MindPXFMUv2", "version": "2.1", "image_size": 0, "git_identity": "", diff --git a/Images/tap-v1.prototype b/Images/tap-v1.prototype new file mode 100644 index 0000000000..8876ec5e88 --- /dev/null +++ b/Images/tap-v1.prototype @@ -0,0 +1,12 @@ +{ + "board_id": 78, + "magic": "PX4FWv1", + "description": "Firmware for the TAPv1 board", + "image": "", + "build_time": 0, + "summary": "TAPv1", + "version": "0.1", + "image_size": 0, + "git_identity": "", + "board_revision": 0 +} diff --git a/Makefile b/Makefile index b46f619f4a..f80a24399c 100644 --- a/Makefile +++ b/Makefile @@ -50,11 +50,11 @@ ifneq ($(CMAKE_VER),0) $(warning sudo apt-get install cmake) $(warning ) $(warning Official website:) - $(warning wget https://cmake.org/files/v3.3/cmake-3.3.2-Linux-x86_64.sh) - $(warning chmod +x cmake-3.3.2-Linux-x86_64.sh) - $(warning sudo mkdir /opt/cmake-3.3.2) - $(warning sudo ./cmake-3.3.2-Linux-x86_64.sh --prefix=/opt/cmake-3.3.2 --exclude-subdir) - $(warning export PATH=/opt/cmake-3.3.2/bin:$$PATH) + $(warning wget https://cmake.org/files/v3.4/cmake-3.4.3-Linux-x86_64.sh) + $(warning chmod +x cmake-3.4.3-Linux-x86_64.sh) + $(warning sudo mkdir /opt/cmake-3.4.3) + $(warning sudo ./cmake-3.4.3-Linux-x86_64.sh --prefix=/opt/cmake-3.4.3 --exclude-subdir) + $(warning export PATH=/opt/cmake-3.4.3/bin:$$PATH) $(warning ) $(error Fatal) endif @@ -77,7 +77,7 @@ endif # in that directory with the target upload. # explicity set default build target -all: px4fmu-v2_default +all: posix_sitl_default # Parsing # -------------------------------------------------------------------- @@ -104,14 +104,27 @@ endif PX4_MAKE_ARGS = -j$(j) --no-print-directory endif +# check if replay env variable is set & set build dir accordingly +ifdef replay + BUILD_DIR_SUFFIX := _replay +else + BUILD_DIR_SUFFIX := +endif + # Functions # -------------------------------------------------------------------- # describe how to build a cmake config define cmake-build -+@if [ $(PX4_CMAKE_GENERATOR) = "Ninja" ] && [ -e $(PWD)/build_$@/Makefile ]; then rm -rf $(PWD)/build_$@; fi -+@if [ ! -e $(PWD)/build_$@/CMakeCache.txt ]; then Tools/check_submodules.sh && mkdir -p $(PWD)/build_$@ && cd $(PWD)/build_$@ && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1) || (cd .. && rm -rf $(PWD)/build_$@); fi ++@if [ $(PX4_CMAKE_GENERATOR) = "Ninja" ] && [ -e ./build_$@$(BUILD_DIR_SUFFIX)/Makefile ]; then rm -rf ./build_$@$(BUILD_DIR_SUFFIX); fi ++@if [ ! -e ./build_$@$(BUILD_DIR_SUFFIX)/CMakeCache.txt ]; then Tools/check_submodules.sh && mkdir -p ./build_$@$(BUILD_DIR_SUFFIX) && cd ./build_$@$(BUILD_DIR_SUFFIX) && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1) || (cd .. && rm -rf ./build_$@$(BUILD_DIR_SUFFIX)); fi +@Tools/check_submodules.sh -+@(echo "PX4 CONFIG: $@" && cd $(PWD)/build_$@ && $(PX4_MAKE) $(PX4_MAKE_ARGS) $(ARGS)) ++@(echo "PX4 CONFIG: $@$(BUILD_DIR_SUFFIX)" && cd ./build_$@$(BUILD_DIR_SUFFIX) && $(PX4_MAKE) $(PX4_MAKE_ARGS) $(ARGS)) +endef + +define cmake-build-other ++@if [ $(PX4_CMAKE_GENERATOR) = "Ninja" ] && [ -e ./build_$@/Makefile ]; then rm -rf ./build_$@; fi ++@if [ ! -e ./build_$@/CMakeCache.txt ]; then Tools/check_submodules.sh && mkdir -p ./build_$@ && cd ./build_$@ && cmake $(2) -G$(PX4_CMAKE_GENERATOR) || (cd .. && rm -rf ./build_$@); fi ++@(cd ./build_$@ && $(PX4_MAKE) $(PX4_MAKE_ARGS) $(ARGS)) endef # create empty targets to avoid msgs for targets passed to cmake @@ -131,12 +144,18 @@ endef # -------------------------------------------------------------------- # Do not put any spaces between function arguments. +tap-v1_default: + $(call cmake-build,nuttx_tap-v1_default) + px4fmu-v1_default: $(call cmake-build,nuttx_px4fmu-v1_default) px4fmu-v2_default: $(call cmake-build,nuttx_px4fmu-v2_default) +px4fmu-v2_test: + $(call cmake-build,nuttx_px4fmu-v2_test) + px4fmu-v4_default: $(call cmake-build,nuttx_px4fmu-v4_default) @@ -155,9 +174,6 @@ posix_sitl_default: posix_sitl_lpe: $(call cmake-build,$@) -posix_sitl_ekf2: - $(call cmake-build,$@) - posix_sitl_replay: $(call cmake-build,$@) @@ -177,12 +193,13 @@ posix_eagle_default: $(call cmake-build,$@) eagle_default: posix_eagle_default qurt_eagle_default +eagle_legacy_default: posix_eagle_legacy_driver_default qurt_eagle_legacy_driver_default qurt_eagle_legacy_driver_default: - $(call cmake-build,$@) - + $(call cmake-build,$@) + posix_eagle_legacy_driver_default: - $(call cmake-build,$@) + $(call cmake-build,$@) qurt_excelsior_default: $(call cmake-build,$@) @@ -192,10 +209,13 @@ posix_excelsior_default: excelsior_default: posix_excelsior_default qurt_excelsior_default -posix_rpi2_default: +posix_rpi_native: $(call cmake-build,$@) -posix_rpi2_release: +posix_rpi_cross: + $(call cmake-build,$@) + +posix_bebop_default: $(call cmake-build,$@) posix: posix_sitl_default @@ -213,8 +233,14 @@ run_sitl_ros: sitl_deprecation # Other targets # -------------------------------------------------------------------- -.PHONY: uavcan_firmware check check_format unittest tests package_firmware clean submodulesclean distclean -.NOTPARALLEL: uavcan_firmware check check_format unittest tests package_firmware clean submodulesclean distclean +.PHONY: gazebo_build uavcan_firmware check check_format unittest tests qgc_firmware package_firmware clean submodulesclean distclean +.NOTPARALLEL: gazebo_build uavcan_firmware check check_format unittest tests qgc_firmware package_firmware clean submodulesclean distclean + +gazebo_build: + @mkdir -p build_gazebo + @if [ ! -e ./build_gazebo/CMakeCache.txt ];then cd build_gazebo && cmake -Wno-dev -G$(PX4_CMAKE_GENERATOR) ../Tools/sitl_gazebo; fi + @cd build_gazebo && $(PX4_MAKE) $(PX4_MAKE_ARGS) + @cd build_gazebo && $(PX4_MAKE) $(PX4_MAKE_ARGS) sdf uavcan_firmware: ifeq ($(VECTORCONTROL),1) @@ -222,12 +248,39 @@ ifeq ($(VECTORCONTROL),1) @(rm -rf vectorcontrol && git clone --quiet --depth 1 https://github.com/thiemar/vectorcontrol.git && cd vectorcontrol && BOARD=s2740vc_1_0 make --silent --no-print-directory && BOARD=px4esc_1_6 make --silent --no-print-directory && ../Tools/uavcan_copy.sh) endif -check: check_px4fmu-v1_default check_px4fmu-v2_default check_px4fmu-v4_default_and_uavcan check_mindpx-v2_default check_px4-stm32f4discovery_default check_posix_sitl_default check_unittest check_format +checks_defaults: \ + check_px4fmu-v1_default \ + check_px4fmu-v2_default \ + check_mindpx-v2_default \ + check_px4-stm32f4discovery_default \ + check_tap-v1_default \ + +checks_bootloaders: \ + + +checks_tests: \ + check_px4fmu-v2_test + +checks_alts: \ + check_px4fmu-v2_ekf2 \ + +checks_uavcan: \ + check_px4fmu-v4_default_and_uavcan + +checks_sitls: \ + check_posix_sitl_default + +checks_last: \ + check_tests \ + check_format \ + +check: checks_defaults checks_tests checks_alts checks_uavcan checks_bootloaders checks_last +quick_check: check_px4fmu-v2_default check_px4fmu-v4_default check_tests check_format check_format: $(call colorecho,"Checking formatting with astyle") @./Tools/fix_code_style.sh - @./Tools/check_code_style.sh + @./Tools/check_code_style_all.sh check_%: @echo @@ -245,7 +298,30 @@ ifeq ($(VECTORCONTROL),1) endif unittest: posix_sitl_default - @(cd unittests && cmake -G$(PX4_CMAKE_GENERATOR) && $(PX4_MAKE) $(PX4_MAKE_ARGS) && ctest -j2 --output-on-failure) + $(call cmake-build-other,unittest, ../unittests) + @(cd build_unittest && ctest -j2 --output-on-failure) + +run_tests_posix: posix_sitl_default + @mkdir -p build_posix_sitl_default/src/firmware/posix/rootfs/fs/microsd + @mkdir -p build_posix_sitl_default/src/firmware/posix/rootfs/eeprom + @touch build_posix_sitl_default/src/firmware/posix/rootfs/eeprom/parameters + @(cd build_posix_sitl_default/src/firmware/posix && ./px4 -d ../../../../posix-configs/SITL/init/rcS_tests | tee test_output) + @(cd build_posix_sitl_default/src/firmware/posix && grep --color=always "All tests passed" test_output) + +tests: check_unittest run_tests_posix + +# QGroundControl flashable firmware +qgc_firmware: \ + check_px4fmu-v1_default \ + check_px4fmu-v2_default \ + check_mindpx-v2_default \ + check_px4fmu-v4_default_and_uavcan \ + check_format + +extra_firmware: \ + check_px4-stm32f4discovery_default \ + check_px4fmu-v2_test \ + check_px4fmu-v2_ekf2 package_firmware: @zip --junk-paths Firmware.zip `find . -name \*.px4` @@ -255,18 +331,18 @@ clean: @(cd NuttX/nuttx && make clean) submodulesclean: - @git submodule sync + @git submodule sync --recursive @git submodule deinit -f . @git submodule update --init --recursive --force distclean: submodulesclean - @git clean -ff -x -d + @git clean -ff -x -d -e ".project" -e ".cproject" # targets handled by cmake -cmake_targets = test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak \ +cmake_targets = install test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak \ run_cmake_config config gazebo gazebo_gdb gazebo_lldb jmavsim replay \ jmavsim_gdb jmavsim_lldb gazebo_gdb_iris gazebo_lldb_tailsitter gazebo_iris gazebo_iris_opt_flow gazebo_tailsitter \ - gazebo_gdb_standard_vtol gazebo_lldb_standard_vtol gazebo_standard_vtol gazebo_plane + gazebo_gdb_standard_vtol gazebo_lldb_standard_vtol gazebo_standard_vtol gazebo_plane gazebo_solo gazebo_typhoon_h480 $(foreach targ,$(cmake_targets),$(eval $(call cmake-targ,$(targ)))) .PHONY: clean diff --git a/NuttX b/NuttX index f0f4bdc872..55e8d557ec 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit f0f4bdc872d324f64e9d93d6f8989d3c1dfa2633 +Subproject commit 55e8d557ecba65c82f7ce8fd326575470f0e5acc diff --git a/README.md b/README.md index 70a2c6a84b..8b71befe58 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ ## PX4 Pro Drone Autopilot ## -[![Build Status](https://travis-ci.org/PX4/Firmware.svg?branch=master)](https://travis-ci.org/PX4/Firmware) [![Coverity Scan](https://scan.coverity.com/projects/3966/badge.svg?flat=1)](https://scan.coverity.com/projects/3966?tab=overview) +[![Releases](https://img.shields.io/github/release/PX4/Firmware.svg)](https://github.com/PX4/Firmware/releases) [![DOI](https://zenodo.org/badge/22634/PX4/Firmware.svg)](https://zenodo.org/badge/latestdoi/22634/PX4/Firmware) [![Build Status](https://travis-ci.org/PX4/Firmware.svg?branch=master)](https://travis-ci.org/PX4/Firmware) [![Coverity Scan](https://scan.coverity.com/projects/3966/badge.svg?flat=1)](https://scan.coverity.com/projects/3966?tab=overview) [![Gitter](https://badges.gitter.im/Join%20Chat.svg)](https://gitter.im/PX4/Firmware?utm_source=badge&utm_medium=badge&utm_campaign=pr-badge&utm_content=badge) diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index 30af86730e..a9f65a6479 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -25,8 +25,8 @@ then param set MC_YAWRATE_I 0.25 param set MC_YAWRATE_D 0.0 - param set BAT_V_SCALING 0.00989 - param set BAT_C_SCALING 0.0124 + param set BAT_V_DIV 12.27559 + param set BAT_A_PER_V 15.39103 fi set MIXER quad_w diff --git a/ROMFS/px4fmu_common/init.d/10020_3dr_quad b/ROMFS/px4fmu_common/init.d/10020_3dr_quad index a1b2e90270..1917df18dd 100644 --- a/ROMFS/px4fmu_common/init.d/10020_3dr_quad +++ b/ROMFS/px4fmu_common/init.d/10020_3dr_quad @@ -7,7 +7,7 @@ # @maintainer Lorenz Meier # -sh /etc/init.d/rc.mc_defaults +sh /etc/init.d/4001_quad_x if [ $AUTOCNF == yes ] then @@ -21,10 +21,3 @@ then param set MC_PITCHRATE_D 0.004 param set MC_YAW_P 4 fi - -set MIXER quad_x - -set PWM_OUT 1234 - -set PWM_MIN 1100 -set PWM_MAX 1950 diff --git a/ROMFS/px4fmu_common/init.d/13006_vtol_standard_delta b/ROMFS/px4fmu_common/init.d/13006_vtol_standard_delta index deb9aa3049..c3395eed0d 100644 --- a/ROMFS/px4fmu_common/init.d/13006_vtol_standard_delta +++ b/ROMFS/px4fmu_common/init.d/13006_vtol_standard_delta @@ -15,22 +15,28 @@ then param set VT_MOT_COUNT 4 param set VT_TRANS_THR 0.75 - param set MC_ROLL_P 7.0 + param set MC_ROLL_P 6.5 param set MC_ROLLRATE_P 0.15 - param set MC_ROLLRATE_I 0.002 + param set MC_ROLLRATE_I 0.01 param set MC_ROLLRATE_D 0.003 param set MC_ROLLRATE_FF 0.0 - param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.12 - param set MC_PITCHRATE_I 0.002 + param set MC_PITCH_P 6.5 + param set MC_PITCHRATE_P 0.15 + param set MC_PITCHRATE_I 0.01 param set MC_PITCHRATE_D 0.003 param set MC_PITCHRATE_FF 0.0 - param set MC_YAW_P 2.8 + param set MC_YAW_P 3.5 param set MC_YAW_FF 0.5 - param set MC_YAWRATE_P 0.22 - param set MC_YAWRATE_I 0.02 + param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_I 0.1 param set MC_YAWRATE_D 0.0 param set MC_YAWRATE_FF 0.0 + param set MC_YAWRATE_MAX 20 + param set MC_YAWRAUTO_MAX 20 + + param set MPC_XY_P 0.8 + param set MPC_XY_VEL_P 0.1 + param set MPC_ACC_HOR_MAX 2.0 param set VT_MOT_COUNT 4 param set VT_IDLE_PWM_MC 1080 diff --git a/ROMFS/px4fmu_common/init.d/13010_claire b/ROMFS/px4fmu_common/init.d/13010_claire new file mode 100644 index 0000000000..21d7b9ff09 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/13010_claire @@ -0,0 +1,36 @@ +#!nsh +# +# @name CruiseAder Claire +# +# @type VTOL Tiltrotor +# +# @maintainer Samay Siga +# + +sh /etc/init.d/rc.vtol_defaults + +if [ $AUTOCNF == yes ] +then +param set VT_TYPE 1 +param set VT_TILT_MC 0.08 +param set VT_TILT_TRANS 0.5 +param set VT_TILT_FW 0.9 + +param set VT_MOT_COUNT 4 +param set VT_IDLE_PWM_MC 1080 +param set VT_TYPE 1 +fi + +set MIXER claire +set PWM_OUT 1234 +set PWM_RATE 400 +set PWM_MAX 2000 + +set MIXER_AUX claire +set PWM_AUX_RATE 50 +set PWM_AUX_RATE 123 +set PWM_AUX_MIN 1000 +set PWM_AUX_MAX 2000 +set PWM_AUX_DISARMED 1000 + +set MAV_TYPE 21 diff --git a/ROMFS/px4fmu_common/init.d/15001_coax_heli b/ROMFS/px4fmu_common/init.d/15001_coax_heli index 97513b7049..fd10afffed 100644 --- a/ROMFS/px4fmu_common/init.d/15001_coax_heli +++ b/ROMFS/px4fmu_common/init.d/15001_coax_heli @@ -1,6 +1,6 @@ #!nsh # -# @name Coaxial Helicopter (such as Esky Lama v4 or Esky Big Lama) +# @name Esky (Big) Lama v4 # # @type Coaxial Helicopter # @@ -12,7 +12,8 @@ # @maintainer Emmanuel Roussel # -set VEHICLE_TYPE mc +sh /etc/init.d/rc.mc_defaults +set MIXER coax if [ $AUTOCNF == yes ] then @@ -47,7 +48,6 @@ then param set MC_YAWRATE_FF 0 fi -set MIXER coax # use PWM parameters for throttle channel set PWM_OUT 34 set PWM_RATE 400 @@ -62,4 +62,3 @@ set PWM_AUX_OUT 1234 set PWM_AUX_DISARMED 1500 set PWM_AUX_MIN 1000 set PWM_AUX_MAX 2000 - diff --git a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha index 780efd9807..f9c63b18ed 100644 --- a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha +++ b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha @@ -11,31 +11,44 @@ sh /etc/init.d/rc.fw_defaults if [ $AUTOCNF == yes ] then - param set FW_AIRSPD_MAX 20 - param set FW_AIRSPD_MIN 12 - param set FW_AIRSPD_TRIM 16 - param set FW_R_TC 0.3 - param set FW_P_TC 0.3 - param set FW_L1_DAMPING 0.74 - param set FW_L1_PERIOD 16 + param set FW_AIRSPD_MAX 25 + param set FW_AIRSPD_MIN 12.5 + param set FW_AIRSPD_TRIM 16.5 + param set LNDFW_AIRSPD_MAX 6 + param set LNDFW_VELI_MAX 4 + param set LNDFW_VEL_XY_MAX 3 + param set LNDFW_VEL_Z_MAX 5 + param set FW_R_TC 0.4 + param set FW_P_TC 0.4 + param set FW_THR_CRUISE 0.55 + param set FW_L1_DAMPING 0.75 + param set FW_L1_PERIOD 15 param set FW_LND_ANG 15 - param set FW_LND_FLALT 5 + param set FW_LND_FLALT 8 param set FW_LND_HHDIST 15 param set FW_LND_HVIRT 13 - param set FW_LND_TLALT 5 + param set FW_LND_TLALT 10 param set FW_THR_LND_MAX 0 - param set FW_PR_FF 0.35 + param set FW_P_LIM_MAX 20 + param set FW_P_LIM_MIN -30 + param set FW_R_LIM 45 + param set FW_PR_FF 0.45 param set FW_PR_IMAX 0.4 - param set FW_PR_P 0.08 - param set FW_RR_FF 0.6 + param set FW_PR_P 0.005 + param set FW_RR_FF 0.45 param set FW_RR_IMAX 0.2 - param set FW_RR_P 0.04 + param set FW_RR_P 0.013 + param set FW_P_RMAX_NEG 70 + param set FW_P_RMAX_POS 70 + param set FW_R_RMAX 70 param set SYS_COMPANION 157600 - param set PWM_MAIN_REV0 1 param set PWM_MAIN_REV1 1 + param set PWM_MAIN_REV2 1 param set PWM_DISARMED 0 param set PWM_MIN 900 param set PWM_MAX 2100 + param set MIS_TAKEOFF_ALT 50 + param set NAV_LOITER_RAD 30 fi set PWM_DISARMED p:PWM_DISARMED diff --git a/ROMFS/px4fmu_common/init.d/4002_qavr5 b/ROMFS/px4fmu_common/init.d/4002_qavr5 new file mode 100644 index 0000000000..142982bf4b --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/4002_qavr5 @@ -0,0 +1,34 @@ +#!nsh +# +# @name Lumenier QAV-R (raceblade) 5" arms +# +# @type Quadrotor x +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer James Goppert +# + +sh /etc/init.d/4001_quad_x + +if [ $AUTOCNF == yes ] +then + param set MC_ROLL_P 8.0 + param set MC_ROLLRATE_P 0.08 + param set MC_ROLLRATE_I 0.05 + param set MC_ROLLRATE_D 0.003 + param set MC_PITCH_P 8.0 + param set MC_PITCHRATE_P 0.1 + param set MC_PITCHRATE_I 0.05 + param set MC_PITCHRATE_D 0.003 + param set MC_YAW_P 2.8 + param set MC_YAWRATE_P 0.15 + param set MC_YAWRATE_I 0.1 + param set MC_YAWRATE_D 0.0 + param set MC_YAW_FF 0.5 + param set PWM_MIN 1075 + param set MPC_THR_MIN 0.06 + param set MPC_MANTHR_MIN 0.06 +fi diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone index 9d3e4427be..6849e1e827 100644 --- a/ROMFS/px4fmu_common/init.d/4008_ardrone +++ b/ROMFS/px4fmu_common/init.d/4008_ardrone @@ -29,7 +29,7 @@ then param set MC_YAWRATE_D 0.0 param set MC_YAW_FF 0.8 - param set BAT_V_SCALING 0.00838095238 + param set BAT_V_DIV 34.32838 fi set OUTPUT_MODE ardrone diff --git a/ROMFS/px4fmu_common/init.d/4030_solo b/ROMFS/px4fmu_common/init.d/4030_solo index c2462ef4e6..77661c8596 100644 --- a/ROMFS/px4fmu_common/init.d/4030_solo +++ b/ROMFS/px4fmu_common/init.d/4030_solo @@ -79,7 +79,7 @@ then param set RC5_TRIM 1500 fi -set MIXER solo +set MIXER quad_x set PWM_OUT 1234 set MIXER_AUX none diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index 9e2d1f6ffe..04a82d506f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -6,7 +6,7 @@ # # Start the attitude and position estimator # -ekf_att_pos_estimator start +ekf2 start # # Start attitude controller diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index f876353263..a7206200c2 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -11,7 +11,12 @@ then param set RTL_DESCEND_ALT 100 param set RTL_LAND_DELAY -1 - param set NAV_ACC_RAD 50 + # FW uses L1 distance for acceptance radius + # set a smaller NAV_ACC_RAD for vertical acceptance distance + param set NAV_ACC_RAD 10 + + param set MIS_LTRMIN_ALT 25 + param set MIS_TAKEOFF_ALT 25 param set PE_VELNE_NOISE 0.3 param set PE_VELD_NOISE 0.35 diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index a8c17ad2c0..3ec0cd0d95 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -45,10 +45,10 @@ then if mixer load $OUTPUT_DEV $MIXER_FILE then - echo "[i] Mixer: $MIXER_FILE on $OUTPUT_DEV" + echo "INFO [init] Mixer: $MIXER_FILE on $OUTPUT_DEV" else - echo "[i] Error loading mixer: $MIXER_FILE" - echo "ERROR: Could not load mixer: $MIXER_FILE" >> $LOG_FILE + echo "ERROR [init] Error loading mixer: $MIXER_FILE" + echo "ERROR:[init] Could not load mixer: $MIXER_FILE" >> $LOG_FILE tone_alarm $TUNE_ERR fi @@ -56,8 +56,8 @@ then else if [ $MIXER != skip ] then - echo "[i] Mixer not defined" - echo "ERROR: Mixer not defined" >> $LOG_FILE + echo "ERROR [init] Mixer not defined" + echo "ERROR [init] Mixer not defined" >> $LOG_FILE tone_alarm $TUNE_ERR fi fi @@ -137,10 +137,10 @@ then then if mixer load $OUTPUT_AUX_DEV $MIXER_AUX_FILE then - echo "[i] Mixer: $MIXER_AUX_FILE on $OUTPUT_AUX_DEV" + echo "INFO [init] Mixer: $MIXER_AUX_FILE on $OUTPUT_AUX_DEV" else - echo "[i] Error loading mixer: $MIXER_AUX_FILE" - echo "ERROR: Could not load mixer: $MIXER_AUX_FILE" >> $LOG_FILE + echo "ERROR [init] Error loading mixer: $MIXER_AUX_FILE" + echo "ERROR [init] Could not load mixer: $MIXER_AUX_FILE" >> $LOG_FILE fi else set PWM_AUX_OUT none diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 56a349457f..399f4398ac 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -29,19 +29,9 @@ then fi #--------------------------------------- -if mc_att_control start -then -else - # try the multiplatform version - mc_att_control_m start -fi +mc_att_control start -if mc_pos_control start -then -else - # try the multiplatform version - mc_pos_control_m start -fi +mc_pos_control start # # Start Land Detector diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 9a4d12192e..3fc86b0873 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -14,6 +14,17 @@ else fmu i2c 1 100000 fmu i2c 2 100000 + if ver hwcmp PX4FMU_V4 + then + # We know there are sketchy boards out there + # as chinese companies produce Pixracers without + # fully understanding the critical parts of the + # schematic and BOM, leading to sensor brownouts + # on boot. Original Pixracers following the + # open hardware design do not require this. + fmu sensor_reset 20 + fi + if ms5611 -s start then fi @@ -190,7 +201,7 @@ else fi fi -if sf10a start +if sf1xx start then fi diff --git a/ROMFS/px4fmu_common/init.d/rc.uavcan b/ROMFS/px4fmu_common/init.d/rc.uavcan deleted file mode 100644 index ebecea2de4..0000000000 --- a/ROMFS/px4fmu_common/init.d/rc.uavcan +++ /dev/null @@ -1,29 +0,0 @@ -#!nsh -# -# UAVCAN initialization script. -# - -# -# Starting stuff according to UAVCAN_ENABLE value -# -if param greater UAVCAN_ENABLE 0 -then - if uavcan start - then - echo "[i] UAVCAN started" - else - echo "[i] ERROR: Could not start UAVCAN" - tone_alarm $TUNE_ERR - fi -fi - -if param greater UAVCAN_ENABLE 1 -then - if uavcan start fw - then - echo "[i] UAVCAN servers started" - else - echo "[i] ERROR: Could not start UAVCAN servers" - tone_alarm $TUNE_ERR - fi -fi diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb deleted file mode 100644 index 4a157a683f..0000000000 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ /dev/null @@ -1,9 +0,0 @@ -#!nsh -# -# USB MAVLink start -# - -mavlink start -r 800000 -d /dev/ttyACM0 -m config -x - -# Exit shell to make it available to MAVLink -exit diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 3786a8ec98..c89b4bf84d 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -32,9 +32,9 @@ else then if mount -t vfat /dev/mmcsd0 /fs/microsd then - echo "[i] microSD card formatted" + echo "INFO [init] MicroSD card formatted" else - echo "[i] format failed" + echo "ERROR [init] Format failed" tone_alarm MNBG set LOG_FILE /dev/null fi @@ -50,7 +50,7 @@ fi set FRC /fs/microsd/etc/rc.txt if [ -f $FRC ] then - echo "[i] Executing script: $FRC" + echo "INFO [init] Executing script: $FRC" sh $FRC set MODE custom fi @@ -109,6 +109,16 @@ then set AUTOCNF yes else set AUTOCNF no + + # + # Release 1.4.0 transitional support: + # set to old default if unconfigured. + # this preserves the previous behaviour + # + if param compare BAT_N_CELLS 0 + then + param set BAT_N_CELLS 3 + fi fi # @@ -173,7 +183,7 @@ then # if param compare SYS_AUTOSTART 0 then - echo "[i] No autostart" + echo "INFO [init] No autostart" else sh /etc/init.d/rc.autostart fi @@ -192,7 +202,7 @@ then set FCONFIG /fs/microsd/etc/config.txt if [ -f $FCONFIG ] then - echo "[i] Custom: $FCONFIG" + echo "INFO [init] Custom: $FCONFIG" sh $FCONFIG fi unset FCONFIG @@ -223,7 +233,7 @@ then if px4io checkcrc ${IO_FILE} then - echo "PX4IO CRC OK" >> $LOG_FILE + echo "INFO [init] PX4IO CRC OK" >> $LOG_FILE set IO_PRESENT yes else @@ -246,16 +256,16 @@ then usleep 500000 if px4io checkcrc $IO_FILE then - echo "PX4IO CRC OK after updating" >> $LOG_FILE + echo "INFO [init] PX4IO CRC OK after updating" >> $LOG_FILE tone_alarm MLL8CDE set IO_PRESENT yes else - echo "PX4IO update failed" >> $LOG_FILE + echo "ERROR [init] PX4IO update failed" >> $LOG_FILE tone_alarm $TUNE_ERR fi else - echo "PX4IO update failed" >> $LOG_FILE + echo "ERROR [init] PX4IO update failed" >> $LOG_FILE tone_alarm $TUNE_ERR fi fi @@ -263,7 +273,7 @@ then if [ $IO_PRESENT == no ] then - echo "ERROR: PX4IO not found" >> $LOG_FILE + echo "ERROR [init] PX4IO not found" >> $LOG_FILE tone_alarm $TUNE_ERR fi fi @@ -330,6 +340,11 @@ then commander start fi + # + # Start CPU load monitor + # + load_mon start + # # Start primary output # @@ -350,7 +365,7 @@ then then if param compare UAVCAN_ENABLE 0 then - echo "[i] OVERRIDING UAVCAN_ENABLE = 1" + echo "INFO [init] OVERRIDING UAVCAN_ENABLE = 1" >> $LOG_FILE param set UAVCAN_ENABLE 1 fi fi @@ -361,7 +376,7 @@ then then sh /etc/init.d/rc.io else - echo "PX4IO start failed" >> $LOG_FILE + echo "INFO [init] PX4IO start failed" >> $LOG_FILE tone_alarm $TUNE_ERR fi fi @@ -371,7 +386,7 @@ then if fmu mode_$FMU_MODE then else - echo "FMU start failed" >> $LOG_FILE + echo "ERR [init] FMU start failed" >> $LOG_FILE tone_alarm $TUNE_ERR fi @@ -403,7 +418,7 @@ then if mkblctrl $MKBLCTRL_ARG then else - echo "ERROR: MK start failed" >> $LOG_FILE + echo "ERROR [init] MK start failed" >> $LOG_FILE tone_alarm $TUNE_ERR fi unset MKBLCTRL_ARG @@ -415,7 +430,7 @@ then if pwm_out_sim mode_port2_pwm8 then else - echo "PWM SIM start failed" >> $LOG_FILE + echo "ERROR [init] PWM SIM start failed" >> $LOG_FILE tone_alarm $TUNE_ERR fi fi @@ -431,7 +446,7 @@ then then sh /etc/init.d/rc.io else - echo "PX4IO start failed" >> $LOG_FILE + echo "ERROR [init] PX4IO start failed" >> $LOG_FILE tone_alarm $TUNE_ERR fi fi @@ -441,7 +456,7 @@ then if fmu mode_$FMU_MODE then else - echo "FMU mode_$FMU_MODE start failed" >> $LOG_FILE + echo "ERROR [init] FMU mode_$FMU_MODE start failed" >> $LOG_FILE tone_alarm $TUNE_ERR fi @@ -525,25 +540,17 @@ then # clear pins 5 and 6 if param compare SENS_EN_LL40LS 1 then + set FMU_MODE pwm4 set AUX_MODE pwm4 fi if param greater TRIG_MODE 0 then - # Get FMU driver out of the way - set MIXER_AUX none - set AUX_MODE none + set FMU_MODE pwm4 + set AUX_MODE pwm4 camera_trigger start fi fi - # Transitional support: Disable safety on all Pixracer boards - if ver hwcmp PX4FMU_V4 - then - if param set CBRK_IO_SAFETY 22027 - then - fi - fi - # # Starting stuff according to UAVCAN_ENABLE value # @@ -586,6 +593,12 @@ then sf0x start fi + # mb12xx sonar sensor + if param compare SENS_EN_MB12XX 1 + then + mb12xx start + fi + if ver hwcmp PX4FMU_V4 then frsky_telemetry start -d /dev/ttyS6 @@ -612,26 +625,36 @@ then mavlink start -r 800000 -d /dev/ttyACM0 -m config -x fi - # # Logging # - if [ -d /fs/microsd ] + if ver hwcmp PX4FMU_V1 then - if ver hwcmp PX4FMU_V1 + if sdlog2 start -r 30 -a -b 2 -t then - if sdlog2 start -r 30 -a -b 2 -t + fi + else + # check if we should increase logging rate for ekf2 replay message logging + if param greater EKF2_REC_RPL 0 + then + if param compare SYS_LOGGER 0 then - fi - else - # check if we should increase logging rate for ekf2 replay message logging - if param greater EKF2_REC_RPL 0 - then - if sdlog2 start -r 500 -e -b 20 -t + if sdlog2 start -r 500 -e -b 18 -t then fi else - if sdlog2 start -r 100 -a -b 12 -t + if logger start -r 500 + then + fi + fi + else + if param compare SYS_LOGGER 0 + then + if sdlog2 start -r 100 -a -b 9 -t + then + fi + else + if logger start -b 12 -t then fi fi @@ -651,7 +674,7 @@ then # if [ $VEHICLE_TYPE == fw ] then - echo "FIXED WING" + echo "INFO [init] Fixedwing" if [ $MIXER == none ] then @@ -679,11 +702,11 @@ then # if [ $VEHICLE_TYPE == mc ] then - echo "MULTICOPTER" + echo "INFO [init] Multicopter" if [ $MIXER == none ] then - echo "Mixer undefined" + echo "INFO [init] Mixer undefined" fi if [ $MAV_TYPE == none ] @@ -721,12 +744,16 @@ then then set MAV_TYPE 14 fi + if [ $MIXER == coax ] + then + set MAV_TYPE 3 + fi fi # Still no MAV_TYPE found if [ $MAV_TYPE == none ] then - echo "Unknown MAV_TYPE" + echo "WARN [init] Unknown MAV_TYPE" param set MAV_TYPE 2 else param set MAV_TYPE $MAV_TYPE @@ -744,11 +771,11 @@ then # if [ $VEHICLE_TYPE == vtol ] then - echo "VTOL" + echo "INFO [init] VTOL" if [ $MIXER == none ] then - echo "VTOL mixer undefined" + echo "WARN [init] VTOL mixer undefined" fi if [ $MAV_TYPE == none ] @@ -771,7 +798,7 @@ then # Still no MAV_TYPE found if [ $MAV_TYPE == none ] then - echo "Unknown MAV_TYPE" + echo "WARN [init] Unknown MAV_TYPE" param set MAV_TYPE 19 else param set MAV_TYPE $MAV_TYPE @@ -855,14 +882,14 @@ then # if [ $VEHICLE_TYPE == none ] then - echo "[i] No autostart ID found" + echo "WARN [init] No autostart ID found" fi # Start any custom addons set FEXTRAS /fs/microsd/etc/extras.txt if [ -f $FEXTRAS ] then - echo "[i] Addons script: $FEXTRAS" + echo "INFO [init] Addons script: $FEXTRAS" sh $FEXTRAS fi unset FEXTRAS @@ -886,7 +913,7 @@ mavlink boot_complete if [ $EXIT_ON_END == yes ] then - echo "NSH EXIT" + echo "INFO [init] NSH exit" exit fi unset EXIT_ON_END diff --git a/ROMFS/px4fmu_common/logging/conv.zip b/ROMFS/px4fmu_common/logging/conv.zip deleted file mode 100644 index 7f485184c6..0000000000 Binary files a/ROMFS/px4fmu_common/logging/conv.zip and /dev/null differ diff --git a/ROMFS/px4fmu_common/mixers/claire.aux.mix b/ROMFS/px4fmu_common/mixers/claire.aux.mix new file mode 100644 index 0000000000..54b2708294 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/claire.aux.mix @@ -0,0 +1,38 @@ +# mixer for the CruiseAder Claire tilt mechansim servo, aileron and elevator + +======================================================================= + + +Tilt mechanism servo mixer + +--------------------------- + +M: 1 + +O: 10000 10000 0 -10000 10000 + +S: 1 4 10000 10000 0 -10000 10000 + + + +Aileron mixers + +------------- + +M: 1 + +O: 10000 10000 0 -10000 10000 + +S: 1 0 10000 10000 0 -10000 10000 + + + +Elevator mixers + +------------- + +M: 1 + +O: 10000 10000 0 -10000 10000 + +S: 1 1 -10000 -10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/claire.main.mix b/ROMFS/px4fmu_common/mixers/claire.main.mix new file mode 100644 index 0000000000..baad77d3f8 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/claire.main.mix @@ -0,0 +1,7 @@ +# CruiseAder Claire Main Multirotor mixer for PX4FMU + +# + +#=========================== + +R: 4x 10000 10000 10000 0 diff --git a/ROMFS/px4fmu_common/mixers/plane_sitl.main.mix b/ROMFS/px4fmu_common/mixers/plane_sitl.main.mix deleted file mode 100644 index 387fbe10c6..0000000000 --- a/ROMFS/px4fmu_common/mixers/plane_sitl.main.mix +++ /dev/null @@ -1,26 +0,0 @@ -Mixer for SITL plane, using the VTOL airframe for now -========================================================= - -Z: - -Z: - -Z: - -Z: - -# mixer for the elevons -M: 2 -O: 10000 10000 0 -10000 10000 -S: 0 0 5000 5000 0 -10000 10000 -S: 0 1 -5000 -5000 0 -10000 10000 - -M: 2 -O: 10000 10000 0 -10000 10000 -S: 0 0 5000 5000 0 -10000 10000 -S: 0 1 5000 5000 0 -10000 10000 - -# mixer for the pusher/puller throttle -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 3 0 20000 -10000 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/quad_v.main.mix b/ROMFS/px4fmu_common/mixers/quad_v.main.mix deleted file mode 100644 index d14074694d..0000000000 --- a/ROMFS/px4fmu_common/mixers/quad_v.main.mix +++ /dev/null @@ -1,18 +0,0 @@ -Multirotor mixer for PX4FMU -=========================== - -This file defines a single mixer for a quadrotor in the V configuration. All controls -are mixed 100%. - -R: 4v 10000 10000 10000 0 - -Gimbal / payload mixer for last two channels ------------------------------------------------------ - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 3 5 10000 10000 0 -10000 10000 - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 3 6 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix b/ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix index bd332a2688..12bd885dce 100644 --- a/ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix +++ b/ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix @@ -6,13 +6,21 @@ are mixed 100%. R: 4x 10000 10000 10000 0 -#mixer for the elevons +Z: + +# left elevon M: 2 O: 10000 10000 0 -10000 10000 S: 1 0 5000 5000 0 -10000 10000 S: 1 1 5000 5000 0 -10000 10000 +# right elevon M: 2 O: 10000 10000 0 -10000 10000 S: 1 0 5000 5000 0 -10000 10000 S: 1 1 -5000 -5000 0 -10000 10000 + +# mixer for the virtual elevator +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 1 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/solo.main.mix b/ROMFS/px4fmu_common/mixers/solo.main.mix deleted file mode 100644 index 12a3bee20c..0000000000 --- a/ROMFS/px4fmu_common/mixers/solo.main.mix +++ /dev/null @@ -1,7 +0,0 @@ -Multirotor mixer for PX4FMU -=========================== - -This file defines a single mixer for a quadrotor in the X configuration. All controls -are mixed 100%. - -R: 4x 10000 10000 10000 0 diff --git a/ROMFS/px4fmu_common/mixers/vtol_delta.aux.mix b/ROMFS/px4fmu_common/mixers/vtol_delta.aux.mix index ae3bbf9385..6b4990220b 100644 --- a/ROMFS/px4fmu_common/mixers/vtol_delta.aux.mix +++ b/ROMFS/px4fmu_common/mixers/vtol_delta.aux.mix @@ -19,13 +19,13 @@ input is inverted between the two servos. M: 2 O: 10000 10000 0 -10000 10000 -S: 1 0 -6000 -6000 0 -10000 10000 -S: 1 1 6500 6500 0 -10000 10000 +S: 1 0 -8000 -8000 0 -10000 10000 +S: 1 1 8000 8000 0 -10000 10000 M: 2 O: 10000 10000 0 -10000 10000 -S: 1 0 -6000 -6000 0 -10000 10000 -S: 1 1 -6500 -6500 0 -10000 10000 +S: 1 0 -8000 -8000 0 -10000 10000 +S: 1 1 -8000 -8000 0 -10000 10000 Motor speed mixer ----------------- diff --git a/ROMFS/px4fmu_test/init.d/rc.sensors b/ROMFS/px4fmu_test/init.d/rc.sensors new file mode 100644 index 0000000000..a39bc4edba --- /dev/null +++ b/ROMFS/px4fmu_test/init.d/rc.sensors @@ -0,0 +1,201 @@ +#!nsh +# +# Standard startup script for PX4FMU v1, v2, v3, v4 onboard sensor drivers. +# + +if ver hwcmp PX4FMU_V1 +then + if ms5611 start + then + fi +else + # Configure all I2C buses to 100 KHz as they + # are all external or slow + fmu i2c 1 100000 + fmu i2c 2 100000 + + if ms5611 -s start + then + fi + + # Blacksheep telemetry + if bst start + then + fi +fi + +if adc start +then +fi + +if ver hwcmp PX4FMU_V2 +then + # External I2C bus + if hmc5883 -C -T -X start + then + fi + + # External I2C bus + if lis3mdl -X start + then + fi + + # Internal I2C bus + if hmc5883 -C -T -I -R 4 start + then + fi + + # external MPU6K is rotated 180 degrees yaw + if mpu6000 -X -R 4 start + then + set BOARD_FMUV3 true + else + set BOARD_FMUV3 false + fi + + if [ $BOARD_FMUV3 == true ] + then + # external L3GD20H is rotated 180 degrees yaw + if l3gd20 -X -R 4 start + then + fi + + # external LSM303D is rotated 270 degrees yaw + if lsm303d -X -R 6 start + then + fi + + # internal MPU6000 is rotated 180 deg roll, 270 deg yaw + if mpu6000 -R 14 start + then + fi + + if hmc5883 -C -T -S -R 8 start + then + fi + + if meas_airspeed start -b 2 + then + fi + + else + # FMUv2 + if mpu6000 start + then + fi + + if l3gd20 start + then + fi + + if lsm303d start + then + fi + fi +fi + +if ver hwcmp PX4FMU_V4 +then + # External I2C bus + if hmc5883 -C -T -X start + then + fi + + if lis3mdl -R 2 start + then + fi + + # Internal SPI bus is rotated 90 deg yaw + if hmc5883 -C -T -S -R 2 start + then + fi + + # Internal SPI bus ICM-20608-G is rotated 90 deg yaw + if mpu6000 -R 2 -T 20608 start + then + fi + + # Internal SPI bus mpu9250 is rotated 90 deg yaw + if mpu9250 -R 2 start + then + fi +fi + +if ver hwcmp PX4FMU_V1 +then + # FMUv1 + if mpu6000 start + then + fi + + if l3gd20 start + then + fi + + # MAG selection + if param compare SENS_EXT_MAG 2 + then + if hmc5883 -C -I start + then + fi + else + # Use only external as primary + if param compare SENS_EXT_MAG 1 + then + if hmc5883 -C -X start + then + fi + else + # auto-detect the primary, prefer external + if hmc5883 start + then + fi + fi + fi +fi + +if ver hwcmp MINDPX_V2 +then + if mpu6500 start + then + fi + + if lsm303d start + then + fi + + if l3gd20 start + then + fi + + # External I2C bus + if hmc5883 -C -T -X start + then + fi + + if lis3mdl -R 2 start + then + fi +fi + +if meas_airspeed start +then +else + if ets_airspeed start + then + else + if ets_airspeed start -b 1 + then + fi + fi +fi + +if sf1xx start +then +fi + +# Wait 20 ms for sensors (because we need to wait for the HRT and work queue callbacks to fire) +usleep 20000 +if sensors start +then +fi diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS index 0df6c29071..53f7524be0 100644 --- a/ROMFS/px4fmu_test/init.d/rcS +++ b/ROMFS/px4fmu_test/init.d/rcS @@ -38,6 +38,11 @@ fi # Start a minimal system # +# +# Start the ORB (first app to start) +# +uorb start + # # Load parameters # @@ -104,131 +109,24 @@ then tests mount fi +sh /etc/init.d/rc.sensors + +ver all + # # Run unit tests at board boot, reporting failure as needed. # Add new unit tests using the same pattern as below. # -if mavlink_tests -then -else - set unit_test_failure 1 - set unit_test_failure_list "${unit_test_failure_list} mavlink_tests" -fi - -if commander_tests -then -else - set unit_test_failure 1 - set unit_test_failure_list "${unit_test_failure_list} commander_tests" -fi - -if uorb test -then -else - set unit_test_failure 1 - set unit_test_failure_list "${unit_test_failure_list} uorb_tests" -fi - -# Start all sensors on all boards -ms5611 start -adc start - -if mpu6000 -X start -then -fi - -if mpu6000 start -then -fi - -if l3gd20 -X start -then -fi - -if l3gd20 start -then -fi - -# MAG selection -if param compare SENS_EXT_MAG 2 -then - if hmc5883 -I start - then - fi -else - # Use only external as primary - if param compare SENS_EXT_MAG 1 - then - if hmc5883 -X start - then - fi - else - # auto-detect the primary, prefer external - if hmc5883 start - then - fi - fi -fi - -if ver hwcmp PX4FMU_V2 -then - if lsm303d -X start - then - fi - - if lsm303d start - then - fi - - if ms5611 -X start - then - fi -fi - -if meas_airspeed start -then -else - if ets_airspeed start - then - else - if ets_airspeed start -b 1 - then - fi - fi -fi - -if sensors start -then -fi - -# Check for flow sensor -if px4flow start -then -fi - -if ll40ls start -then -fi - if tests all -then -else - set unit_test_failure 1 - set unit_test_failure_list "${unit_test_failure_list} system_tests" -fi - -if [ $unit_test_failure == 0 ] then echo echo "All Unit Tests PASSED" rgbled rgb 20 255 20 else echo - echo "Some Unit Tests FAILED:${unit_test_failure_list}" + echo "Some Unit Tests FAILED" rgbled rgb 255 20 20 fi -ver all - free diff --git a/ROMFS/px4fmu_common/mixers/quad_test.mix b/ROMFS/px4fmu_test/mixers/quad_test.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/quad_test.mix rename to ROMFS/px4fmu_test/mixers/quad_test.mix diff --git a/ROMFS/sitl/mixers/plane_sitl.main.mix b/ROMFS/sitl/mixers/plane_sitl.main.mix new file mode 100644 index 0000000000..f694702765 --- /dev/null +++ b/ROMFS/sitl/mixers/plane_sitl.main.mix @@ -0,0 +1,36 @@ +Mixer for SITL plane +========================================================= + +Z: + +Z: + +# mixer for the rudder +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 2 10000 10000 0 -10000 10000 + +# mixer for the flaps +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +# mixer for the pusher/puller throttle +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 + +# mixer for the left aileron +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 0 -10000 -10000 0 -10000 10000 + +# mixer for the right aileron +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 0 10000 10000 0 -10000 10000 + +# mixer for the elevator +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 1 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/standard_vtol_sitl.main.mix b/ROMFS/sitl/mixers/standard_vtol_sitl.main.mix similarity index 50% rename from ROMFS/px4fmu_common/mixers/standard_vtol_sitl.main.mix rename to ROMFS/sitl/mixers/standard_vtol_sitl.main.mix index ffa5a024bc..6e84c2814b 100644 --- a/ROMFS/px4fmu_common/mixers/standard_vtol_sitl.main.mix +++ b/ROMFS/sitl/mixers/standard_vtol_sitl.main.mix @@ -1,24 +1,26 @@ Mixer for standard vtol plane (SITL) with motor x configuration ========================================================= -This file defines a single mixer for a standard vtol plane (SITL gazebo) with motors in X configuration. -The plane has two ailerons and one elevator. The ailerons and elevator are treated as elevons -in order to make the standard vtol simulation compatible with the tailsitter simulation. +This file defines a single mixer for a standard vtol plane (SITL gazebo) with motors in X configuration. The plane has two ailerons and one elevator. R: 4x 10000 10000 10000 0 -# mixer for the elevons -M: 2 -O: 10000 10000 0 -10000 10000 -S: 1 0 5000 5000 0 -10000 10000 -S: 1 1 -5000 -5000 0 -10000 10000 - -M: 2 -O: 10000 10000 0 -10000 10000 -S: 1 0 5000 5000 0 -10000 10000 -S: 1 1 5000 5000 0 -10000 10000 - # mixer for the pusher/puller throttle M: 1 O: 10000 10000 0 -10000 10000 S: 1 3 0 20000 -10000 -10000 10000 + +# mixer for the left aileron +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 0 10000 10000 0 -10000 10000 + +# mixer for the right aileron +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 0 -10000 -10000 0 -10000 10000 + +# mixer for the elevator +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 1 10000 10000 0 -10000 10000 diff --git a/ROMFS/tap_common/init.d/4001_quad_x b/ROMFS/tap_common/init.d/4001_quad_x new file mode 100644 index 0000000000..8f7c8da356 --- /dev/null +++ b/ROMFS/tap_common/init.d/4001_quad_x @@ -0,0 +1,18 @@ +#!nsh +# +# @name Generic Quadrotor X config +# +# @type Quadrotor x +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Lorenz Meier +# + +sh /etc/init.d/rc.mc_defaults + +set MIXER quad_x + +set PWM_OUT 1234 diff --git a/ROMFS/tap_common/init.d/6001_hexa_x b/ROMFS/tap_common/init.d/6001_hexa_x new file mode 100644 index 0000000000..584c4a381c --- /dev/null +++ b/ROMFS/tap_common/init.d/6001_hexa_x @@ -0,0 +1,19 @@ +#!nsh +# +# @name Generic Hexarotor x geometry +# +# @type Hexarotor x +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Anton Babushkin +# + +sh /etc/init.d/rc.mc_defaults + +set MIXER hexa_x + +# Need to set all 8 channels +set PWM_OUT 12345678 diff --git a/ROMFS/tap_common/init.d/rc.fw_apps b/ROMFS/tap_common/init.d/rc.fw_apps new file mode 100644 index 0000000000..040bcbde03 --- /dev/null +++ b/ROMFS/tap_common/init.d/rc.fw_apps @@ -0,0 +1,20 @@ +#!nsh +# +# Standard apps for fixed wing +# + +# +# Start the attitude and position estimator +# +ekf2 start + +# +# Start attitude controller +# +fw_att_control start +fw_pos_control_l1 start + +# +# Start Land Detector +# +land_detector start fixedwing diff --git a/ROMFS/tap_common/init.d/rc.fw_defaults b/ROMFS/tap_common/init.d/rc.fw_defaults new file mode 100644 index 0000000000..f876353263 --- /dev/null +++ b/ROMFS/tap_common/init.d/rc.fw_defaults @@ -0,0 +1,28 @@ +#!nsh + +set VEHICLE_TYPE fw + +if [ $AUTOCNF == yes ] +then +# +# Default parameters for FW +# + param set RTL_RETURN_ALT 100 + param set RTL_DESCEND_ALT 100 + param set RTL_LAND_DELAY -1 + + param set NAV_ACC_RAD 50 + + param set PE_VELNE_NOISE 0.3 + param set PE_VELD_NOISE 0.35 + param set PE_POSNE_NOISE 0.5 + param set PE_POSD_NOISE 1.0 +fi + +# This is the gimbal pass mixer +set MIXER_AUX pass +set PWM_AUX_RATE 50 +set PWM_AUX_OUT 1234 +set PWM_AUX_DISARMED 1500 +set PWM_AUX_MIN 1000 +set PWM_AUX_MAX 2000 diff --git a/ROMFS/tap_common/init.d/rc.interface b/ROMFS/tap_common/init.d/rc.interface new file mode 100644 index 0000000000..bfa105b537 --- /dev/null +++ b/ROMFS/tap_common/init.d/rc.interface @@ -0,0 +1,83 @@ +#!nsh +# +# Script to configure control interface +# + +set SDCARD_MIXERS_PATH /fs/microsd/etc/mixers + +if [ $MIXER != none -a $MIXER != skip ] +then + # + # Load main mixer + # + + # Use the mixer file + set MIXER_FILE /etc/mixers/$MIXER.main.mix + + set OUTPUT_DEV /dev/pwm_output0 + + if [ $OUTPUT_MODE == uavcan_esc ] + then + set OUTPUT_DEV /dev/uavcan/esc + fi + + if mixer load $OUTPUT_DEV $MIXER_FILE + then + echo "INFO [init] Mixer: $MIXER_FILE on $OUTPUT_DEV" + else + echo "ERROR [init] Error loading mixer: $MIXER_FILE" + echo "ERROR:[init] Could not load mixer: $MIXER_FILE" >> $LOG_FILE + tone_alarm $TUNE_ERR + fi + + unset MIXER_FILE +else + if [ $MIXER != skip ] + then + echo "ERROR [init] Mixer not defined" + echo "ERROR [init] Mixer not defined" >> $LOG_FILE + tone_alarm $TUNE_ERR + fi +fi + +if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == io ] +then + if [ $PWM_OUT != none ] + then + # + # Set PWM output frequency + # + if [ $PWM_RATE != none ] + then + pwm rate -c $PWM_OUT -r $PWM_RATE + fi + + # + # Set disarmed, min and max PWM values + # + if [ $PWM_DISARMED != none ] + then + pwm disarmed -c $PWM_OUT -p $PWM_DISARMED + fi + if [ $PWM_MIN != none ] + then + pwm min -c $PWM_OUT -p $PWM_MIN + fi + if [ $PWM_MAX != none ] + then + pwm max -c $PWM_OUT -p $PWM_MAX + fi + fi + + if [ $FAILSAFE != none ] + then + pwm failsafe -d $OUTPUT_DEV $FAILSAFE + fi +fi + +unset PWM_OUT +unset PWM_RATE +unset PWM_MIN +unset PWM_MAX +unset FAILSAFE +unset OUTPUT_DEV diff --git a/ROMFS/tap_common/init.d/rc.mc_apps b/ROMFS/tap_common/init.d/rc.mc_apps new file mode 100644 index 0000000000..607dffd5b0 --- /dev/null +++ b/ROMFS/tap_common/init.d/rc.mc_apps @@ -0,0 +1,17 @@ +#!nsh +# +# Standard apps for multirotors: +# att & pos estimator, att & pos control. +# + + +ekf2 start + +mc_att_control start + +mc_pos_control start + +# +# Start Land Detector +# +land_detector start multicopter diff --git a/ROMFS/tap_common/init.d/rc.mc_defaults b/ROMFS/tap_common/init.d/rc.mc_defaults new file mode 100644 index 0000000000..0bf41dcac6 --- /dev/null +++ b/ROMFS/tap_common/init.d/rc.mc_defaults @@ -0,0 +1,49 @@ +#!nsh + +set VEHICLE_TYPE mc + +if [ $AUTOCNF == yes ] +then + param set PE_VELNE_NOISE 0.5 + param set PE_VELD_NOISE 0.35 + param set PE_POSNE_NOISE 0.5 + param set PE_POSD_NOISE 1.25 + + param set NAV_ACC_RAD 2.0 + param set RTL_RETURN_ALT 30.0 + param set RTL_DESCEND_ALT 10.0 + param set PWM_DISARMED 900 + param set PWM_MIN 1075 + param set PWM_MAX 1950 + + param set RTL_LAND_DELAY 0 +fi + +# set environment variables (!= parameters) +set PWM_RATE 400 +# tell the mixer to use parameters for these instead +set PWM_DISARMED p:PWM_DISARMED +set PWM_MIN p:PWM_MIN +set PWM_MAX p:PWM_MAX + +# This is the gimbal pass mixer +set MIXER_AUX pass +set PWM_AUX_RATE 50 +set PWM_AUX_OUT 1234 +set PWM_AUX_DISARMED 1500 +set PWM_AUX_MIN 1000 +set PWM_AUX_MAX 2000 + +# Transitional support: ensure suitable PWM min/max param values +if param compare PWM_MIN 1000 +then + param set PWM_MIN 1075 +fi +if param compare PWM_MAX 2000 +then + param set PWM_MAX 1950 +fi +if param compare PWM_DISARMED 0 +then + param set PWM_DISARMED 900 +fi diff --git a/ROMFS/tap_common/init.d/rc.sensors b/ROMFS/tap_common/init.d/rc.sensors new file mode 100644 index 0000000000..4e767bdc6c --- /dev/null +++ b/ROMFS/tap_common/init.d/rc.sensors @@ -0,0 +1,46 @@ +#!nsh +# +# Standard startup script for TAP v1 onboard sensor drivers. +# + +if adc start +then +fi + +# External I2C bus +if hmc5883 -C -T -X start +then +fi + +if lis3mdl -R 2 start +then +fi + +# Internal SPI bus is rotated 90 deg yaw +if hmc5883 -C -T -S -R 2 start +then +fi + +# Internal SPI bus ICM-20608-G is rotated 90 deg yaw +if mpu6000 -R 2 -T 20608 start +then +fi + +# Internal SPI bus mpu9250 is rotated 90 deg yaw +if mpu9250 -R 2 start +then +fi + +if meas_airspeed start +then +fi + +if sf1xx start +then +fi + +# Wait 20 ms for sensors (because we need to wait for the HRT and work queue callbacks to fire) +usleep 20000 +if sensors start +then +fi diff --git a/ROMFS/tap_common/init.d/rc.vtol_apps b/ROMFS/tap_common/init.d/rc.vtol_apps new file mode 100644 index 0000000000..0ae3f28950 --- /dev/null +++ b/ROMFS/tap_common/init.d/rc.vtol_apps @@ -0,0 +1,24 @@ +#!nsh +# +# Standard apps for vtol: +# att & pos estimator, att & pos control. +# +# + + +#--------------------------------------- +# Estimator group selction +# +ekf2 start + +vtol_att_control start +mc_att_control start +mc_pos_control start +fw_att_control start +fw_pos_control_l1 start + +# +# Start Land Detector +# Multicopter for now until we have something for VTOL +# +land_detector start vtol diff --git a/ROMFS/tap_common/init.d/rc.vtol_defaults b/ROMFS/tap_common/init.d/rc.vtol_defaults new file mode 100644 index 0000000000..d8dab16cf0 --- /dev/null +++ b/ROMFS/tap_common/init.d/rc.vtol_defaults @@ -0,0 +1,49 @@ +#!nsh + +set VEHICLE_TYPE vtol + +if [ $AUTOCNF == yes ] +then + param set MC_ROLL_P 6.0 + param set MC_PITCH_P 6.0 + param set MC_YAW_P 4 + + param set PE_VELNE_NOISE 0.5 + param set PE_VELD_NOISE 0.3 + param set PE_POSNE_NOISE 0.5 + param set PE_POSD_NOISE 1.25 + param set PE_ABIAS_PNOISE 0.0001 + + # + # Default parameters for mission and position handling + # + param set NAV_ACC_RAD 3 + param set MPC_TKO_SPEED 1.0 + param set MPC_LAND_SPEED 0.7 + param set MPC_Z_VEL_MAX 1.5 + param set MPC_XY_VEL_MAX 4.0 + param set MIS_YAW_TMT 10 + param set MPC_ACC_HOR_MAX 2.0 + param set RTL_LAND_DELAY 0 +fi + +# set environment variables (!= parameters) +set PWM_RATE 400 +# tell the mixer to use parameters for these instead +set PWM_DISARMED p:PWM_DISARMED +set PWM_MIN p:PWM_MIN +set PWM_MAX p:PWM_MAX + +# Transitional support: ensure suitable PWM min/max param values +if param compare PWM_MIN 1000 +then + param set PWM_MIN 1075 +fi +if param compare PWM_MAX 2000 +then + param set PWM_MAX 1950 +fi +if param compare PWM_DISARMED 0 +then + param set PWM_DISARMED 900 +fi diff --git a/ROMFS/tap_common/init.d/rcS b/ROMFS/tap_common/init.d/rcS new file mode 100644 index 0000000000..a68c78e6fd --- /dev/null +++ b/ROMFS/tap_common/init.d/rcS @@ -0,0 +1,486 @@ +#!nsh +# +# PX4FMU startup script. +# +# NOTE: COMMENT LINES ARE REMOVED BEFORE STORED IN ROMFS. +# + +# +# Start CDC/ACM serial driver +# +sercon + +# +# Default to auto-start mode. +# +set MODE autostart + +set TUNE_ERR ML<> $LOG_FILE + param set UAVCAN_ENABLE 1 + fi + fi + + if [ $OUTPUT_MODE == fmu ] + then + if fmu mode_$FMU_MODE + then + else + echo "ERR [init] FMU start failed" >> $LOG_FILE + tone_alarm $TUNE_ERR + fi + fi + + if fmu mode_pwm4 + then + else + echo "ERROR [init] FMU mode_$FMU_MODE start failed" >> $LOG_FILE + tone_alarm $TUNE_ERR + fi + fi + + mavlink start -r 1200 -d /dev/ttyS1 + + # + # Starting stuff according to UAVCAN_ENABLE value + # + if param greater UAVCAN_ENABLE 0 + then + if uavcan start + then + else + tone_alarm $TUNE_ERR + fi + fi + + if param greater UAVCAN_ENABLE 1 + then + if uavcan start fw + then + else + tone_alarm $TUNE_ERR + fi + fi + + # + # Optional drivers + # + + # Sensors on the PWM interface bank + if param compare SENS_EN_LL40LS 1 + then + if pwm_input start + then + if ll40ls start pwm + then + fi + fi + fi + + # sf0x lidar sensor + if param compare SENS_EN_SF0X 1 + then + sf0x start + fi + + # Start USB shell if no microSD present, MAVLink else + if [ $LOG_FILE == /dev/null ] + then + # Try to get an USB console + nshterm /dev/ttyACM0 & + else + mavlink start -r 800000 -d /dev/ttyACM0 -m config -x + fi + + # + # Logging + # + if param compare SYS_LOGGER 0 + then + if sdlog2 start -r 100 -a -b 9 -t + then + fi + else + if logger start -b 12 -t + then + fi + fi + + # + # Fixed wing setup + # + if [ $VEHICLE_TYPE == fw ] + then + echo "INFO [init] Fixedwing" + + if [ $MIXER == none ] + then + # Set default mixer for fixed wing if not defined + set MIXER AERT + fi + + if [ $MAV_TYPE == none ] + then + # Use MAV_TYPE = 1 (fixed wing) if not defined + set MAV_TYPE 1 + fi + + param set MAV_TYPE $MAV_TYPE + + # Load mixer and configure outputs + sh /etc/init.d/rc.interface + + # Start standard fixedwing apps + sh /etc/init.d/rc.fw_apps + fi + + # + # Multicopters setup + # + if [ $VEHICLE_TYPE == mc ] + then + echo "INFO [init] Multicopter" + + if [ $MIXER == none ] + then + echo "INFO [init] Mixer undefined" + fi + + if [ $MAV_TYPE == none ] + then + # Use mixer to detect vehicle type + if [ $MIXER == quad_x -o $MIXER == quad_+ ] + then + set MAV_TYPE 2 + fi + if [ $MIXER == quad_w ] + then + set MAV_TYPE 2 + fi + if [ $MIXER == quad_h ] + then + set MAV_TYPE 2 + fi + if [ $MIXER == tri_y_yaw- -o $MIXER == tri_y_yaw+ ] + then + set MAV_TYPE 15 + fi + if [ $MIXER == hexa_x -o $MIXER == hexa_+ ] + then + set MAV_TYPE 13 + fi + if [ $MIXER == hexa_cox ] + then + set MAV_TYPE 13 + fi + if [ $MIXER == octo_x -o $MIXER == octo_+ ] + then + set MAV_TYPE 14 + fi + fi + + # Still no MAV_TYPE found + if [ $MAV_TYPE == none ] + then + echo "WARN [init] Unknown MAV_TYPE" + param set MAV_TYPE 2 + else + param set MAV_TYPE $MAV_TYPE + fi + + # Load mixer and configure outputs + sh /etc/init.d/rc.interface + + # Start standard multicopter apps + sh /etc/init.d/rc.mc_apps + fi + + # + # VTOL setup + # + if [ $VEHICLE_TYPE == vtol ] + then + echo "INFO [init] VTOL" + + if [ $MIXER == none ] + then + echo "WARN [init] VTOL mixer undefined" + fi + + if [ $MAV_TYPE == none ] + then + # Use mixer to detect vehicle type + if [ $MIXER == caipirinha_vtol ] + then + set MAV_TYPE 19 + fi + if [ $MIXER == firefly6 ] + then + set MAV_TYPE 21 + fi + if [ $MIXER == quad_x_pusher_vtol ] + then + set MAV_TYPE 22 + fi + fi + + # Still no MAV_TYPE found + if [ $MAV_TYPE == none ] + then + echo "WARN [init] Unknown MAV_TYPE" + param set MAV_TYPE 19 + else + param set MAV_TYPE $MAV_TYPE + fi + + # Load mixer and configure outputs + sh /etc/init.d/rc.interface + + # Start standard vtol apps + sh /etc/init.d/rc.vtol_apps + fi + + # + # Rover setup + # + if [ $VEHICLE_TYPE == rover ] + then + # 10 is MAV_TYPE_GROUND_ROVER + set MAV_TYPE 10 + + # Load mixer and configure outputs + sh /etc/init.d/rc.interface + + # Start standard rover apps + sh /etc/init.d/rc.axialracing_ax10_apps + + param set MAV_TYPE 10 + fi + + unset MIXER + unset MAV_TYPE + unset OUTPUT_MODE + + # + # Start the navigator + # + navigator start + + # + # Generic setup (autostart ID not found) + # + if [ $VEHICLE_TYPE == none ] + then + echo "WARN [init] No autostart ID found" + fi + + # Start any custom addons + set FEXTRAS /fs/microsd/etc/extras.txt + if [ -f $FEXTRAS ] + then + echo "INFO [init] Addons script: $FEXTRAS" + sh $FEXTRAS + fi + unset FEXTRAS + + # Run no SD alarm + if [ $LOG_FILE == /dev/null ] + then + # Play SOS + tone_alarm error + fi + +# End of autostart +fi + +# There is no further script processing, so we can free some RAM +# XXX potentially unset all script variables. +unset TUNE_ERR + +# Boot is complete, inform MAVLink app(s) that the system is now fully up and running +mavlink boot_complete + +if [ $EXIT_ON_END == yes ] +then + echo "INFO [init] NSH exit" + exit +fi +unset EXIT_ON_END diff --git a/ROMFS/tap_common/mixers/README.md b/ROMFS/tap_common/mixers/README.md new file mode 100644 index 0000000000..0e591ca288 --- /dev/null +++ b/ROMFS/tap_common/mixers/README.md @@ -0,0 +1,105 @@ +## PX4 mixer definitions ## + +Files in this directory implement example mixers that can be used as a basis +for customisation, or for general testing purposes. + +For a detailed description of the mixing architecture and examples see: +http://px4.io/dev/mixing + +### Syntax ### + +Mixer definitions are text files; lines beginning with a single capital letter +followed by a colon are significant. All other lines are ignored, meaning that +explanatory text can be freely mixed with the definitions. + +Each file may define more than one mixer; the allocation of mixers to actuators +is specific to the device reading the mixer definition, and the number of +actuator outputs generated by a mixer is specific to the mixer. + +For example: each simple or null mixer is assigned to outputs 1 to x +in the order they appear in the mixer file. + +A mixer begins with a line of the form + + : + +The tag selects the mixer type; 'M' for a simple summing mixer, 'R' for a +multirotor mixer, etc. + +#### Null Mixer #### + +A null mixer consumes no controls and generates a single actuator output whose +value is always zero. Typically a null mixer is used as a placeholder in a +collection of mixers in order to achieve a specific pattern of actuator outputs. + +The null mixer definition has the form: + + Z: + +#### Simple Mixer #### + +A simple mixer combines zero or more control inputs into a single actuator +output. Inputs are scaled, and the mixing function sums the result before +applying an output scaler. + +A simple mixer definition begins with: + + M: + O: <-ve scale> <+ve scale> + +If is zero, the sum is effectively zero and the mixer will +output a fixed value that is constrained by and . + +The second line defines the output scaler with scaler parameters as discussed +above. Whilst the calculations are performed as floating-point operations, the +values stored in the definition file are scaled by a factor of 10000; i.e. an +offset of -0.5 is encoded as -5000. + +The definition continues with entries describing the control +inputs and their scaling, in the form: + + S: <-ve scale> <+ve scale> + +The value identifies the control group from which the scaler will read, +and the value an offset within that group. These values are specific to +the device reading the mixer definition. + +When used to mix vehicle controls, mixer group zero is the vehicle attitude +control group, and index values zero through three are normally roll, pitch, +yaw and thrust respectively. + +The remaining fields on the line configure the control scaler with parameters as +discussed above. Whilst the calculations are performed as floating-point +operations, the values stored in the definition file are scaled by a factor of +10000; i.e. an offset of -0.5 is encoded as -5000. + +#### Multirotor Mixer #### + +The multirotor mixer combines four control inputs (roll, pitch, yaw, thrust) +into a set of actuator outputs intended to drive motor speed controllers. + +The mixer definition is a single line of the form: + + R: + +The supported geometries include: + + * 4x - quadrotor in X configuration + * 4+ - quadrotor in + configuration + * 6x - hexcopter in X configuration + * 6+ - hexcopter in + configuration + * 8x - octocopter in X configuration + * 8+ - octocopter in + configuration + +Each of the roll, pitch and yaw scale values determine scaling of the roll, +pitch and yaw controls relative to the thrust control. Whilst the calculations +are performed as floating-point operations, the values stored in the definition +file are scaled by a factor of 10000; i.e. an factor of 0.5 is encoded as 5000. + +Roll, pitch and yaw inputs are expected to range from -1.0 to 1.0, whilst the +thrust input ranges from 0.0 to 1.0. Output for each actuator is in the +range -1.0 to 1.0. + +In the case where an actuator saturates, all actuator values are rescaled so that +the saturating actuator is limited to 1.0. diff --git a/ROMFS/tap_common/mixers/hexa_x.main.mix b/ROMFS/tap_common/mixers/hexa_x.main.mix new file mode 100644 index 0000000000..16e6e22f90 --- /dev/null +++ b/ROMFS/tap_common/mixers/hexa_x.main.mix @@ -0,0 +1,3 @@ +# Hexa X + +R: 6x 10000 10000 10000 0 diff --git a/ROMFS/tap_common/mixers/quad_x.main.mix b/ROMFS/tap_common/mixers/quad_x.main.mix new file mode 100644 index 0000000000..bf034e7c1f --- /dev/null +++ b/ROMFS/tap_common/mixers/quad_x.main.mix @@ -0,0 +1,7 @@ +R: 4x 10000 10000 10000 0 +M: 1 +O: 10000 10000 0 -10000 10000 +S: 3 5 10000 10000 0 -10000 10000 +M: 1 +O: 10000 10000 0 -10000 10000 +S: 3 6 10000 10000 0 -10000 10000 diff --git a/Tools/CI/VTOLmission.txt b/Tools/CI/VTOLmission.txt deleted file mode 100644 index 65bb78aea2..0000000000 --- a/Tools/CI/VTOLmission.txt +++ /dev/null @@ -1,9 +0,0 @@ -QGC WPL 110 -0 1 0 16 0 0 0 0 47.3979949951 8.54559612274 25.0 1 -1 0 2 3000 4.0 0.0 0.0 0.0 47.3980331421 8.54578971863 25.0 1 -2 0 3 16 0.0 0.0 -0.0 0.0 47.399269104 8.54557228088 25.0 1 -3 0 3 16 0.0 0.0 -0.0 0.0 47.3992652893 8.54230213165 25.0 1 -4 0 3 16 0.0 0.0 -0.0 0.0 47.3974761963 8.54239082336 25.0 1 -5 0 3 16 0.0 0.0 -0.0 0.0 47.3976669312 8.54509449005 25.0 1 -6 0 2 3000 3.0 0.0 0.0 0.0 47.3977851868 8.54526233673 25.0 1 -7 0 3 21 25.0 0.0 -0.0 0.0 47.3979797363 8.54460906982 25.0 1 diff --git a/Tools/adb_upload.sh b/Tools/adb_upload.sh index 166aa29716..b4e4f649cb 100755 --- a/Tools/adb_upload.sh +++ b/Tools/adb_upload.sh @@ -23,3 +23,7 @@ do adb push $arg $last ((i+=1)) done + +# Make sure they are synced to the file system +echo "Syncing FS..." +adb shell sync diff --git a/Tools/adb_upload_to_bebop.sh b/Tools/adb_upload_to_bebop.sh new file mode 100755 index 0000000000..ca3eb84837 --- /dev/null +++ b/Tools/adb_upload_to_bebop.sh @@ -0,0 +1,39 @@ +#!/bin/bash + +if [ -z ${BEBOP_IP+x} ]; then + ip=192.168.42.1 + echo "\$BEBOP_IP is not set (use default: $ip)" +else + ip=$BEBOP_IP + echo "\$BEBOP_IP is set to $ip" +fi +port=9050 + +echo "Connecting to bebop: $ip:$port" + +# adb returns also 0 as exit status if the connection fails +adb_return=$(adb connect $ip:$port) +adb_status=$(echo $adb_return | cut -f 1 -d " ") + +if [[ $adb_status == "unable" ]]; then + + echo "" + echo "Connection with Parrot Bebop could not be established:" + echo " Make sure you are connected with the Bebop's WiFi and" + echo " enable access to the board by pressing the power button 4 times." + echo "" + exit 50 + +fi + +echo "Connection successfully established" + +sleep 1 + +adb shell mount -o remount,rw / +adb shell touch /home/root/parameters + +../Tools/adb_upload.sh $@ + +echo "Disconnecting from bebop" +adb disconnect diff --git a/Tools/check_code_style.sh b/Tools/check_code_style.sh index 2f12ce5bc7..898f9d1c38 100755 --- a/Tools/check_code_style.sh +++ b/Tools/check_code_style.sh @@ -1,70 +1,23 @@ #!/usr/bin/env bash -set -eu -failed=0 -for fn in $(find src/examples \ - src/systemcmds \ - src/include \ - src/drivers \ - src/platforms \ - src/firmware \ - src/lib/launchdetection \ - src/lib/geo \ - src/lib/geo_lookup \ - src/lib/conversion \ - src/lib/rc \ - src/lib/version \ - src/modules/attitude_estimator_q \ - src/modules/fw_att_control \ - src/modules/gpio_led \ - src/modules/land_detector \ - src/modules/muorb \ - src/modules/px4iofirmware \ - src/modules/param \ - src/modules/sensors \ - src/modules/simulator \ - src/modules/uORB \ - src/modules/bottle_drop \ - src/modules/dataman \ - src/modules/segway \ - src/modules/local_position_estimator \ - src/modules/unit_test \ - src/modules/systemlib \ - src/lib/controllib \ - -path './Build' -prune -o \ - -path './mavlink' -prune -o \ - -path './NuttX' -prune -o \ - -path './src/lib/eigen' -prune -o \ - -path './src/modules/uavcan/libuavcan' -prune -o \ - -path './src/modules/attitude_estimator_ekf/codegen' -prune -o \ - -path './src/modules/ekf_att_pos_estimator' -prune -o \ - -path './src/modules/sdlog2' -prune -o \ - -path './src/modules/uORB' -prune -o \ - -path './src/modules/vtol_att_control' -prune -o \ - -path './unittests/build' -prune -o \ - -path './unittests/gtest' -prune -o \ - -name '*.c' -o -name '*.cpp' -o -name '*.hpp' -o -name '*.h' \ - -not -name '*generated*' \ - -not -name '*uthash.h' \ - -not -name '*utstring.h' \ - -not -name '*utlist.h' \ - -not -name '*utarray.h' \ - -type f); do - if [ -f "$fn" ]; - then - ./Tools/fix_code_style.sh --quiet < $fn > $fn.pretty - diffsize=$(diff -y --suppress-common-lines $fn $fn.pretty | wc -l) - rm -f $fn.pretty - if [ $diffsize -ne 0 ]; then - failed=1 - echo $fn 'bad formatting, please run "./Tools/fix_code_style.sh' $fn'"' - fi - fi -done -if [ $failed -eq 0 ]; then - echo "Format checks passed" - exit 0 -else - echo "Format checks failed" - exit 1 +file=$1 + +DIR=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd ) + +if [ -f "$file" ]; +then + ${DIR}/fix_code_style.sh --dry-run $file | grep --quiet Formatted + if [[ $? -eq 0 ]] + then + ${DIR}/fix_code_style.sh --quiet < $file > $file.pretty + + echo + git --no-pager diff --no-index --minimal --histogram --color=always $file $file.pretty + echo + + rm -f $file.pretty + echo $file 'bad formatting, please run "./Tools/fix_code_style.sh' $file'"' + exit 1 + fi fi + diff --git a/Tools/check_code_style_all.sh b/Tools/check_code_style_all.sh new file mode 100755 index 0000000000..d39e8ea0b9 --- /dev/null +++ b/Tools/check_code_style_all.sh @@ -0,0 +1,57 @@ +#!/usr/bin/env bash +set -eu +failed=0 + +DIR=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd ) + +find \ + src/drivers \ + src/examples \ + src/firmware \ + src/include \ + src/lib/controllib \ + src/lib/conversion \ + src/lib/geo \ + src/lib/geo_lookup \ + src/lib/launchdetection \ + src/lib/rc \ + src/lib/tailsitter_recovery \ + src/lib/terrain_estimation \ + src/lib/version \ + src/modules/attitude_estimator_q \ + src/modules/bottle_drop \ + src/modules/controllib_test \ + src/modules/dataman \ + src/modules/fw_att_control \ + src/modules/fw_pos_control_l1 \ + src/modules/gpio_led \ + src/modules/land_detector \ + src/modules/local_position_estimator \ + src/modules/logger \ + src/modules/mavlink/mavlink_tests \ + src/modules/muorb \ + src/modules/param \ + src/modules/px4iofirmware \ + src/modules/replay \ + src/modules/segway \ + src/modules/sensors \ + src/modules/simulator \ + src/modules/systemlib \ + src/modules/unit_test \ + src/modules/uORB \ + src/modules/vtol_att_control \ + src/platforms \ + src/systemcmds \ + -type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) \ + -not -name '*generated.h' \ + -not -name '*uthash.h' \ + -not -name '*utstring.h' \ + -not -name '*utlist.h' \ + -not -name '*utarray.h' \ + -print0 | xargs -0 -n 1 -P 8 -I % ${DIR}/check_code_style.sh % + + +if [ $? -eq 0 ]; then + echo "Format checks passed" + exit 0 +fi diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index c32ec83a78..624213097d 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -69,6 +69,7 @@ check_git_submodule Tools/jMAVSim check_git_submodule Tools/sitl_gazebo check_git_submodule cmake/cmake_hexagon check_git_submodule mavlink/include/mavlink/v1.0 +check_git_submodule mavlink/include/mavlink/v2.0 check_git_submodule src/lib/DriverFramework check_git_submodule src/lib/DriverFramework/cmake/cmake_hexagon check_git_submodule src/lib/DriverFramework/dspal diff --git a/Tools/decode_backtrace.py b/Tools/decode_backtrace.py index 62a9c16b46..64f8a8ea30 100755 --- a/Tools/decode_backtrace.py +++ b/Tools/decode_backtrace.py @@ -43,10 +43,10 @@ import subprocess # If the following lines were pasted into the shell after running decode_backtrace.py # # INFO Backtrace: 10 -# INFO ./mainapp(px4_backtrace+0x27) [0x42b212] -# INFO ./mainapp() [0x42d608] -# INFO ./mainapp() [0x42d57e] -# INFO ./mainapp() [0x4ba48d] +# INFO ./px4(px4_backtrace+0x27) [0x42b212] +# INFO ./px4() [0x42d608] +# INFO ./px4() [0x42d57e] +# INFO ./px4() [0x4ba48d] # # The output would be: # @@ -63,7 +63,7 @@ def usage(): msg = """ Usage: Tools/decode_backtrace.py -This will load the symbols for /src/firmware/posix/mainapp +This will load the symbols for /src/firmware/posix/px4 The user just needs to copy and paste the backtrace into the terminal where decode_backtrace.py is running. @@ -75,7 +75,7 @@ func = [] # Load the symbols from the binary def load_symbol_map(): - output = subprocess.check_output(["nm", "-p", "-C", os.sys.argv[1]+"/src/firmware/posix/mainapp"]) + output = subprocess.check_output(["nm", "-p", "-C", os.sys.argv[1]+"/src/firmware/posix/px4"]) data = output.split("\n") data.sort() diff --git a/Tools/generate_listener.py b/Tools/generate_listener.py index 4e428d3409..59cd79c7bd 100755 --- a/Tools/generate_listener.py +++ b/Tools/generate_listener.py @@ -115,7 +115,6 @@ print(""" #include #include #include -#define __STDC_FORMAT_MACROS #include #ifndef PRIu64 @@ -173,6 +172,7 @@ for index,m in enumerate(messages[1:]): print("\t\t\ti++;") print("\t\t\tprintf(\"\\nTOPIC: %s #%%d\\n\", i);" % m) print("\t\t\torb_copy(ID,sub,&container);") + print("\t\t\tprintf(\"timestamp: %\" PRIu64 \"\\n\", container.timestamp);") for item in message_elements[index+1]: if item[0] == "float": print("\t\t\tprintf(\"%s: %%8.4f\\n\",(double)container.%s);" % (item[1], item[1])) diff --git a/Tools/jMAVSim b/Tools/jMAVSim index 9155c8f955..a5b0f1f089 160000 --- a/Tools/jMAVSim +++ b/Tools/jMAVSim @@ -1 +1 @@ -Subproject commit 9155c8f955e05c969bcd932b705b98d98845f228 +Subproject commit a5b0f1f0896c9372c62222e88117c6ced2c6282b diff --git a/Tools/mavlink_shell.py b/Tools/mavlink_shell.py new file mode 100755 index 0000000000..64ce2bdf14 --- /dev/null +++ b/Tools/mavlink_shell.py @@ -0,0 +1,206 @@ +#!/usr/bin/env python + +""" +Open a shell over MAVLink. + +@author: Beat Kueng (beat-kueng@gmx.net) +""" + + +from __future__ import print_function +import sys, select +import termios + +try: + from pymavlink import mavutil + import serial +except: + print("Failed to import pymavlink.") + print("You may need to install it with 'pip install pymavlink pyserial'") + exit(-1) +from argparse import ArgumentParser + + +class MavlinkSerialPort(): + '''an object that looks like a serial port, but + transmits using mavlink SERIAL_CONTROL packets''' + def __init__(self, portname, baudrate, devnum=0, debug=0): + self.baudrate = 0 + self._debug = debug + self.buf = '' + self.port = devnum + self.debug("Connecting with MAVLink to %s ..." % portname) + self.mav = mavutil.mavlink_connection(portname, autoreconnect=True, baud=baudrate) + self.mav.wait_heartbeat() + self.debug("HEARTBEAT OK\n") + self.debug("Locked serial device\n") + + def debug(self, s, level=1): + '''write some debug text''' + if self._debug >= level: + print(s) + + def write(self, b): + '''write some bytes''' + self.debug("sending '%s' (0x%02x) of len %u\n" % (b, ord(b[0]), len(b)), 2) + while len(b) > 0: + n = len(b) + if n > 70: + n = 70 + buf = [ord(x) for x in b[:n]] + buf.extend([0]*(70-len(buf))) + self.mav.mav.serial_control_send(self.port, + mavutil.mavlink.SERIAL_CONTROL_FLAG_EXCLUSIVE | + mavutil.mavlink.SERIAL_CONTROL_FLAG_RESPOND, + 0, + 0, + n, + buf) + b = b[n:] + + def close(self): + self.mav.mav.serial_control_send(self.port, 0, 0, 0, 0, [0]*70) + + def _recv(self): + '''read some bytes into self.buf''' + m = self.mav.recv_match(condition='SERIAL_CONTROL.count!=0', + type='SERIAL_CONTROL', blocking=True, + timeout=0.03) + if m is not None: + if self._debug > 2: + print(m) + data = m.data[:m.count] + self.buf += ''.join(str(chr(x)) for x in data) + + def read(self, n): + '''read some bytes''' + if len(self.buf) == 0: + self._recv() + if len(self.buf) > 0: + if n > len(self.buf): + n = len(self.buf) + ret = self.buf[:n] + self.buf = self.buf[n:] + if self._debug >= 2: + for b in ret: + self.debug("read 0x%x" % ord(b), 2) + return ret + return '' + + +def main(): + parser = ArgumentParser(description=__doc__) + parser.add_argument('port', metavar='PORT', nargs='?', default = None, + help='Mavlink port name: serial: DEVICE[,BAUD], udp: IP:PORT, tcp: tcp:IP:PORT. Eg: \ +/dev/ttyUSB0 or 0.0.0.0:14550. Auto-detect serial if not given.') + parser.add_argument("--baudrate", "-b", dest="baudrate", type=int, + help="Mavlink port baud rate (default=115200)", default=115200) + args = parser.parse_args() + + + if args.port == None: + serial_list = mavutil.auto_detect_serial(preferred_list=['*FTDI*', + "*Arduino_Mega_2560*", "*3D_Robotics*", "*USB_to_UART*", '*PX4*', '*FMU*']) + + if len(serial_list) == 0: + print("Error: no serial connection found") + return + + if len(serial_list) > 1: + print('Auto-detected serial ports are:') + for port in serial_list: + print(" {:}".format(port)) + print('Using port {:}'.format(serial_list[0])) + args.port = serial_list[0].device + + + print("Connecting to MAVLINK...") + mav_serialport = MavlinkSerialPort(args.port, args.baudrate, devnum=10) + + mav_serialport.write('\n') # make sure the shell is started + + # setup the console, so we can read one char at a time + fd_in = sys.stdin.fileno() + old_attr = termios.tcgetattr(fd_in) + new_attr = termios.tcgetattr(fd_in) + new_attr[3] = new_attr[3] & ~termios.ECHO # lflags + new_attr[3] = new_attr[3] & ~termios.ICANON + + try: + termios.tcsetattr(fd_in, termios.TCSANOW, new_attr) + cur_line = '' + command_history = [] + cur_history_index = 0 + + def erase_last_n_chars(N): + if N == 0: return + CURSOR_BACK_N = '\x1b['+str(N)+'D' + ERASE_END_LINE = '\x1b[K' + sys.stdout.write(CURSOR_BACK_N + ERASE_END_LINE) + + while True: + while True: + i, o, e = select.select([sys.stdin], [], [], 0) + if not i: break + ch = sys.stdin.read(1) + + # provide a simple shell with command history + if ch == '\n': + if len(cur_line) > 0: + # erase current text (mavlink shell will echo it as well) + erase_last_n_chars(len(cur_line)) + + # add to history + if len(command_history) == 0 or command_history[-1] != cur_line: + command_history.append(cur_line) + if len(command_history) > 50: + del command_history[0] + cur_history_index = len(command_history) + mav_serialport.write(cur_line+'\n') + cur_line = '' + elif ord(ch) == 127: # backslash + if len(cur_line) > 0: + erase_last_n_chars(1) + cur_line = cur_line[:-1] + sys.stdout.write(ch) + elif ord(ch) == 033: + ch = sys.stdin.read(1) # skip one + ch = sys.stdin.read(1) + if ch == 'A': # arrow up + if cur_history_index > 0: + cur_history_index -= 1 + elif ch == 'B': # arrow down + if cur_history_index < len(command_history): + cur_history_index += 1 + # TODO: else: support line editing + + erase_last_n_chars(len(cur_line)) + if cur_history_index == len(command_history): + cur_line = '' + else: + cur_line = command_history[cur_history_index] + sys.stdout.write(cur_line) + + elif ord(ch) > 3: + cur_line += ch + sys.stdout.write(ch) + sys.stdout.flush() + + data = mav_serialport.read(4096) + if data and len(data) > 0: + sys.stdout.write(data) + sys.stdout.flush() + + except serial.serialutil.SerialException as e: + print(e) + + except KeyboardInterrupt: + mav_serialport.close() + + finally: + termios.tcsetattr(fd_in, termios.TCSADRAIN, old_attr) + + +if __name__ == '__main__': + main() + diff --git a/Tools/px4airframes/xmlout.py b/Tools/px4airframes/xmlout.py index 378c057f70..758343f8c8 100644 --- a/Tools/px4airframes/xmlout.py +++ b/Tools/px4airframes/xmlout.py @@ -69,6 +69,8 @@ class XMLOutput(): xml_group.attrib["image"] = "VTOLTiltRotor" elif (group.GetName() == "Coaxial Helicopter"): xml_group.attrib["image"] = "HelicopterCoaxial" + elif (group.GetName() == "Helicopter"): + xml_group.attrib["image"] = "Helicopter" elif (group.GetName() == "Hexarotor Coaxial"): xml_group.attrib["image"] = "Y6A" elif (group.GetName() == "Y6B"): diff --git a/Tools/px4params/srcparser.py b/Tools/px4params/srcparser.py index c4f1b54e43..183c0cb3c9 100644 --- a/Tools/px4params/srcparser.py +++ b/Tools/px4params/srcparser.py @@ -1,5 +1,7 @@ import sys import re +import math + global default_var default_var = {} @@ -39,7 +41,7 @@ class Parameter(object): # Define sorting order of the fields priority = { - "board": 9, + "board": 9, "short_desc": 8, "long_desc": 7, "min": 5, @@ -52,6 +54,7 @@ class Parameter(object): def __init__(self, name, type, default = ""): self.fields = {} self.values = {} + self.bitmask = {} self.name = name self.type = type self.default = default @@ -77,6 +80,12 @@ class Parameter(object): """ self.values[code] = value + def SetBitmaskBit(self, index, bit): + """ + Set named enum value + """ + self.bitmask[index] = bit + def GetFieldCodes(self): """ Return list of existing field codes in convenient order @@ -115,6 +124,24 @@ class Parameter(object): return "" return fv + def GetBitmaskList(self): + """ + Return list of existing bitmask codes in convenient order + """ + keys = self.bitmask.keys() + keys.sort(key=float) + return keys + + def GetBitmaskBit(self, index): + """ + Return value of the given bitmask code or None if not found. + """ + fv = self.bitmask.get(index) + if not fv: + # required because python 3 sorted does not accept None + return "" + return fv + class SourceParser(object): """ Parses provided data and stores all found parameters internally. @@ -133,7 +160,7 @@ class SourceParser(object): re_remove_dots = re.compile(r'\.+$') re_remove_carriage_return = re.compile('\n+') - valid_tags = set(["group", "board", "min", "max", "unit", "decimal", "increment", "reboot_required", "value", "boolean"]) + valid_tags = set(["group", "board", "min", "max", "unit", "decimal", "increment", "reboot_required", "value", "boolean", "bit"]) # Order of parameter groups priority = { @@ -164,6 +191,7 @@ class SourceParser(object): long_desc = None tags = {} def_values = {} + def_bitmask = {} elif state is not None and state != "comment-processed": m = self.re_comment_end.search(line) if m: @@ -187,6 +215,10 @@ class SourceParser(object): # Take the meta info string and split the code and description metainfo = desc.split(" ", 1) def_values[metainfo[0]] = metainfo[1] + elif (tag == "bit"): + # Take the meta info string and split the code and description + metainfo = desc.split(" ", 1) + def_bitmask[metainfo[0]] = metainfo[1] else: tags[tag] = desc current_tag = tag @@ -262,6 +294,8 @@ class SourceParser(object): param.SetField(tag, tags[tag]) for def_value in def_values: param.SetEnumValue(def_value, def_values[def_value]) + for def_bit in def_bitmask: + param.SetBitmaskBit(def_bit, def_bitmask[def_bit]) # Store the parameter if group not in self.param_groups: self.param_groups[group] = ParameterGroup(group) @@ -284,6 +318,9 @@ class SourceParser(object): for group in self.GetParamGroups(): for param in group.GetParams(): name = param.GetName() + if len(name) > 16: + sys.stderr.write("Parameter Name {0} is too long (Limit is 16)\n".format(name)) + return False board = param.GetFieldValue("board") # Check for duplicates name_plus_board = name + "+" + board @@ -321,6 +358,16 @@ class SourceParser(object): if param.GetEnumValue(code) == "": sys.stderr.write("Description for enum value is empty: {0} {1}\n".format(name, code)) return False + for index in param.GetBitmaskList(): + if not self.IsNumber(index): + sys.stderr.write("bit value not number: {0} {1}\n".format(name, index)) + return False + if not int(min) <= math.pow(2, int(index)) <= int(max): + sys.stderr.write("Bitmask bit must be between {0} and {1}: {2} {3}\n".format(min, max, name, math.pow(2, int(index)))) + return False + if param.GetBitmaskBit(index) == "": + sys.stderr.write("Description for bitmask bit is empty: {0} {1}\n".format(name, index)) + return False return True def GetParamGroups(self): diff --git a/Tools/px4params/srcscanner.py b/Tools/px4params/srcscanner.py index 739a067c9a..085c956139 100644 --- a/Tools/px4params/srcscanner.py +++ b/Tools/px4params/srcscanner.py @@ -33,8 +33,9 @@ class SourceScanner(object): Scans provided file and passes its contents to the parser using parser.Parse method. """ - prefix = ".." + os.path.sep + "src" + os.path.sep - scope = re.sub(prefix, '', os.path.dirname(os.path.relpath(path))) + prefix = "^.*" + os.path.sep + "src" + os.path.sep + scope = re.sub(prefix.replace("\\", "/"), "", os.path.dirname(os.path.relpath(path)).replace("\\", "/")) + with codecs.open(path, 'r', 'utf-8') as f: try: contents = f.read() diff --git a/Tools/px4params/xmlout.py b/Tools/px4params/xmlout.py index 0c041a6fc8..fbee44f405 100644 --- a/Tools/px4params/xmlout.py +++ b/Tools/px4params/xmlout.py @@ -63,6 +63,14 @@ class XMLOutput(): xml_value = ET.SubElement(xml_values, "value") xml_value.attrib["code"] = code; xml_value.text = param.GetEnumValue(code) + + if len(param.GetBitmaskList()) > 0: + xml_values = ET.SubElement(xml_param, "bitmask") + for index in param.GetBitmaskList(): + xml_value = ET.SubElement(xml_values, "bit") + xml_value.attrib["index"] = index; + xml_value.text = param.GetBitmaskBit(index) + indent(xml_parameters) self.xml_document = ET.ElementTree(xml_parameters) diff --git a/Tools/px_generate_uorb_topic_headers.py b/Tools/px_generate_uorb_topic_files.py similarity index 53% rename from Tools/px_generate_uorb_topic_headers.py rename to Tools/px_generate_uorb_topic_files.py index 7ed0b3af19..4bf1df6014 100755 --- a/Tools/px_generate_uorb_topic_headers.py +++ b/Tools/px_generate_uorb_topic_files.py @@ -1,7 +1,7 @@ #!/usr/bin/env python ############################################################################# # -# Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. +# Copyright (C) 2013-2016 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,8 +33,8 @@ ############################################################################# """ -px_generate_uorb_topic_headers.py -Generates c/cpp header files for uorb topics from .msg (ROS syntax) +px_generate_uorb_topics.py +Generates c/cpp header/source files for uorb topics from .msg (ROS syntax) message files """ from __future__ import print_function @@ -49,6 +49,7 @@ sys.path.append(px4_tools_dir + "/genmsg/src") sys.path.append(px4_tools_dir + "/gencpp/src") try: + import em import genmsg.template_tools except ImportError as e: print("python import error: ", e) @@ -68,35 +69,99 @@ On Windows please run: ''') exit(1) -__author__ = "Thomas Gubler" -__copyright__ = "Copyright (C) 2013-2014 PX4 Development Team." +__author__ = "Sergey Belash, Thomas Gubler, Beat Kueng" +__copyright__ = "Copyright (C) 2013-2016 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' +TEMPLATE_FILE = ['msg.h.template', 'msg.cpp.template'] +TOPICS_LIST_TEMPLATE_FILE = 'uORBTopics.cpp.template' +OUTPUT_FILE_EXT = ['.h', '.cpp'] +INCL_DEFAULT = ['std_msgs:./msg/std_msgs'] +PACKAGE = 'px4' +TOPICS_TOKEN = '# TOPICS ' -def convert_file(filename, outputdir, templatedir, includepath): +def get_multi_topics(filename): """ - Converts a single .msg file to a uorb header + Get TOPICS names from a "# TOPICS" line """ - #print("Generating headers from {0}".format(filename)) - genmsg.template_tools.generate_from_file(filename, - package, - outputdir, - templatedir, - includepath, - msg_template_map, - srv_template_map) + ofile = open(filename, 'r') + text = ofile.read() + result = [] + for each_line in text.split('\n'): + if each_line.startswith (TOPICS_TOKEN): + topic_names_str = each_line.strip() + topic_names_str = topic_names_str.replace(TOPICS_TOKEN, "") + result.extend(topic_names_str.split(" ")) + ofile.close() + return result + +def get_msgs_list(msgdir): + """ + Makes list of msg files in the given directory + """ + return [fn for fn in os.listdir(msgdir) if fn.endswith(".msg")] -def convert_dir(inputdir, outputdir, templatedir): +def generate_output_from_file(format_idx, filename, outputdir, templatedir, includepath): """ - Converts all .msg files in inputdir to uORB header files + Converts a single .msg file to an uorb header/source file + """ + msg_context = genmsg.msg_loader.MsgContext.create_default() + full_type_name = genmsg.gentools.compute_full_type_name(PACKAGE, os.path.basename(filename)) + spec = genmsg.msg_loader.load_msg_from_file(msg_context, filename, full_type_name) + topics = get_multi_topics(filename) + if includepath: + search_path = genmsg.command_line.includepath_to_dict(includepath) + else: + search_path = {} + genmsg.msg_loader.load_depends(msg_context, spec, search_path) + md5sum = genmsg.gentools.compute_md5(msg_context, spec) + if len(topics) == 0: + topics.append(spec.short_name) + em_globals = { + "file_name_in": filename, + "md5sum": md5sum, + "search_path": search_path, + "msg_context": msg_context, + "spec": spec, + "topics": topics + } + + # Make sure output directory exists: + if not os.path.isdir(outputdir): + os.makedirs(outputdir) + + template_file = os.path.join(templatedir, TEMPLATE_FILE[format_idx]) + output_file = os.path.join(outputdir, spec.short_name + + OUTPUT_FILE_EXT[format_idx]) + + return generate_by_template(output_file, template_file, em_globals) + + +def generate_by_template(output_file, template_file, em_globals): + """ + Invokes empy intepreter to geneate output_file by the + given template_file and predefined em_globals dict + """ + ofile = open(output_file, 'w') + # todo, reuse interpreter + interpreter = em.Interpreter(output=ofile, globals=em_globals, options={em.RAW_OPT:True,em.BUFFERED_OPT:True}) + if not os.path.isfile(template_file): + ofile.close() + os.remove(output_file) + raise RuntimeError("Template file %s not found" % (template_file)) + interpreter.file(open(template_file)) #todo try + interpreter.shutdown() + ofile.close() + return True + + +def convert_dir(format_idx, inputdir, outputdir, templatedir): + """ + Converts all .msg files in inputdir to uORB header/source files """ # Find the most recent modification time in input dir @@ -122,7 +187,7 @@ def convert_dir(inputdir, outputdir, templatedir): if (maxinputtime != 0 and maxouttime != 0 and maxinputtime < maxouttime): return False - includepath = incl_default + [':'.join([package, inputdir])] + includepath = INCL_DEFAULT + [':'.join([PACKAGE, inputdir])] for f in os.listdir(inputdir): # Ignore hidden files if f.startswith("."): @@ -133,11 +198,10 @@ def convert_dir(inputdir, outputdir, templatedir): if not os.path.isfile(fn): continue - convert_file(fn, - outputdir, - templatedir, - includepath) + if fn[-4:].lower() != '.msg': + continue + generate_output_from_file(format_idx, fn, outputdir, templatedir, includepath) return True @@ -151,15 +215,15 @@ def copy_changed(inputdir, outputdir, prefix='', quiet=False): if not os.path.isdir(outputdir): os.makedirs(outputdir) - for f in os.listdir(inputdir): - fni = os.path.join(inputdir, f) + for input_file in os.listdir(inputdir): + fni = os.path.join(inputdir, input_file) if os.path.isfile(fni): - # Check if f exists in outpoutdir, copy the file if not - fno = os.path.join(outputdir, prefix + f) + # Check if input_file exists in outpoutdir, copy the file if not + fno = os.path.join(outputdir, prefix + input_file) if not os.path.isfile(fno): shutil.copy(fni, fno) if not quiet: - print("{0}: new header file".format(f)) + print("{0}: new header file".format(fno)) continue if os.path.getmtime(fni) > os.path.getmtime(fno): @@ -168,26 +232,39 @@ def copy_changed(inputdir, outputdir, prefix='', quiet=False): if not filecmp.cmp(fni, fno): shutil.copy(fni, fno) if not quiet: - print("{0}: updated".format(f)) + print("{0}: updated".format(input_file)) continue if not quiet: - print("{0}: unchanged".format(f)) + print("{0}: unchanged".format(input_file)) -def convert_dir_save(inputdir, outputdir, templatedir, temporarydir, prefix, quiet=False): +def convert_dir_save(format_idx, inputdir, outputdir, templatedir, temporarydir, prefix, quiet=False): """ 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) + convert_dir(format_idx, inputdir, temporarydir, templatedir) + if generate_idx == 1: + generate_topics_list_file(inputdir, temporarydir, templatedir) # Copy changed headers from temporary dir to output dir copy_changed(temporarydir, outputdir, prefix, quiet) +def generate_topics_list_file(msgdir, outputdir, templatedir): + # generate cpp file with topics list + tl_globals = {"msgs" : get_msgs_list(msgdir)} + tl_template_file = os.path.join(templatedir, TOPICS_LIST_TEMPLATE_FILE) + tl_out_file = os.path.join(outputdir, TOPICS_LIST_TEMPLATE_FILE.replace(".template", "")) + generate_by_template(tl_out_file, tl_template_file, tl_globals) + if __name__ == "__main__": parser = argparse.ArgumentParser( - description='Convert msg files to uorb headers') + description='Convert msg files to uorb headers/sources') + parser.add_argument('--headers', help='Generate header files', + action='store_true') + parser.add_argument('--sources', help='Generate source files', + action='store_true') 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)", @@ -206,15 +283,22 @@ if __name__ == "__main__": ' name when converting directories') args = parser.parse_args() + if args.headers: + generate_idx = 0 + elif args.sources: + generate_idx = 1 + else: + print('Error: either --headers or --sources must be specified') + exit(-1) + if args.file is not None: - for f in args.file: - convert_file( - f, - args.outputdir, - args.templatedir, - incl_default) + for f in args.file: + generate_output_from_file(generate_idx, f, args.outputdir, args.templatedir, INCL_DEFAULT) + if generate_idx == 1: + generate_topics_list_file(args.dir, args.outputdir, args.templatedir) elif args.dir is not None: - convert_dir_save( + convert_dir_save( + generate_idx, args.dir, args.outputdir, args.templatedir, diff --git a/Tools/px_generate_uorb_topic_helper.py b/Tools/px_generate_uorb_topic_helper.py new file mode 100644 index 0000000000..9a28b8a2eb --- /dev/null +++ b/Tools/px_generate_uorb_topic_helper.py @@ -0,0 +1,171 @@ + +# helper methods & common code for the uorb message templates msg.{cpp,h}.template + +# Another positive effect of having the code here, is that this file will get +# precompiled and thus message generation will be much faster + + +import genmsg.msgs +import gencpp + +type_map = { + 'int8': 'int8_t', + 'int16': 'int16_t', + 'int32': 'int32_t', + 'int64': 'int64_t', + 'uint8': 'uint8_t', + 'uint16': 'uint16_t', + 'uint32': 'uint32_t', + 'uint64': 'uint64_t', + 'float32': 'float', + 'float64': 'double', + 'bool': 'bool', + 'char': 'char', +} + +msgtype_size_map = { + 'int8': 1, + 'int16': 2, + 'int32': 4, + 'int64': 8, + 'uint8': 1, + 'uint16': 2, + 'uint32': 4, + 'uint64': 8, + 'float32': 4, + 'float64': 8, + 'bool': 1, + 'char': 1, +} + + +def bare_name(msg_type): + """ + Get bare_name from /[x] format + """ + bare = msg_type + if '/' in msg_type: + # removing prefix + bare = (msg_type.split('/'))[1] + # removing suffix + return bare.split('[')[0] + + +def sizeof_field_type(field): + """ + Get size of a field, used for sorting + """ + bare_name_str = bare_name(field.type) + if bare_name_str in msgtype_size_map: + return msgtype_size_map[bare_name_str] + return 0 # this is for non-builtin types: sort them at the end + +def get_children_fields(base_type, search_path): + (package, name) = genmsg.names.package_resource_name(base_type) + tmp_msg_context = genmsg.msg_loader.MsgContext.create_default() + spec_temp = genmsg.msg_loader.load_msg_by_type(tmp_msg_context, '%s/%s' %(package, name), search_path) + sorted_fields = sorted(spec_temp.parsed_fields(), key=sizeof_field_type, reverse=True) + return sorted_fields + +def add_padding_bytes(fields, search_path): + """ + Add padding fields before the embedded types, at the end and calculate the + struct size + returns a tuple with the struct size and padding at the end + """ + struct_size = 8 # account for the timestamp + align_to = 8 # this is always 8, because of the 64bit timestamp + i = 0 + padding_idx = 0 + while i < len(fields): + field = fields[i] + if not field.is_header: + a_pos = field.type.find('[') + array_size = 1 + if field.is_array: + array_size = field.array_len + if field.is_builtin: + field.sizeof_field_type = sizeof_field_type(field) + else: + # embedded type: may need to add padding + num_padding_bytes = align_to - (struct_size % align_to) + if num_padding_bytes != align_to: + padding_field = genmsg.Field('_padding'+str(padding_idx), + 'uint8['+str(num_padding_bytes)+']') + padding_idx += 1 + padding_field.sizeof_field_type = 1 + struct_size += num_padding_bytes + fields.insert(i, padding_field) + i += 1 + children_fields = get_children_fields(field.base_type, search_path) + field.sizeof_field_type, unused = add_padding_bytes(children_fields, + search_path) + struct_size += field.sizeof_field_type * array_size + i += 1 + + # add padding at the end (necessary for embedded types) + num_padding_bytes = align_to - (struct_size % align_to) + if num_padding_bytes == align_to: + num_padding_bytes = 0 + else: + padding_field = genmsg.Field('_padding'+str(padding_idx), + 'uint8['+str(num_padding_bytes)+']') + padding_idx += 1 + padding_field.sizeof_field_type = 1 + struct_size += num_padding_bytes + fields.append(padding_field) + return (struct_size, num_padding_bytes) + + +def convert_type(spec_type): + """ + Convert from msg type to C type + """ + bare_type = spec_type + if '/' in spec_type: + # removing prefix + bare_type = (spec_type.split('/'))[1] + + msg_type, is_array, array_length = genmsg.msgs.parse_type(bare_type) + c_type = msg_type + if msg_type in type_map: + c_type = type_map[msg_type] + if is_array: + return c_type + "[" + str(array_length) + "]" + return c_type + + +def print_field_def(field): + """ + Print the C type from a field + """ + type_name = field.type + # detect embedded types + sl_pos = type_name.find('/') + type_appendix = '' + type_prefix = '' + if (sl_pos >= 0): + type_name = type_name[sl_pos + 1:] + type_prefix = 'struct ' + type_appendix = '_s' + + # detect arrays + a_pos = type_name.find('[') + array_size = '' + if (a_pos >= 0): + # field is array + array_size = type_name[a_pos:] + type_name = type_name[:a_pos] + + if type_name in type_map: + # need to add _t: int8 --> int8_t + type_px4 = type_map[type_name] + else: + type_px4 = type_name + + comment = '' + if field.name.startswith('_padding'): + comment = ' // required for logger' + + print('\t%s%s%s %s%s;%s'%(type_prefix, type_px4, type_appendix, field.name, + array_size, comment)) diff --git a/Tools/px_process_airframes.py b/Tools/px_process_airframes.py index aa7e57a8c8..05f9c85f2a 100755 --- a/Tools/px_process_airframes.py +++ b/Tools/px_process_airframes.py @@ -71,7 +71,7 @@ def main(): const="", metavar="BOARD", help="Board to create airframes xml for") - parser.add_argument("-v", "--verbose", help="verbose output") + parser.add_argument('-v', '--verbose', action='store_true', help="verbose output") args = parser.parse_args() # Check for valid command diff --git a/Tools/px_process_params.py b/Tools/px_process_params.py index 29206275bf..7bac8834d3 100644 --- a/Tools/px_process_params.py +++ b/Tools/px_process_params.py @@ -107,7 +107,7 @@ def main(): metavar="SUMMARY", default="Automagically updated parameter documentation from code.", help="DokuWiki page edit summary") - parser.add_argument("-v", "--verbose", help="verbose output") + parser.add_argument('-v', '--verbose', action='store_true', help="verbose output") args = parser.parse_args() # Check for valid command diff --git a/Tools/px_romfs_pruner.py b/Tools/px_romfs_pruner.py index 5ac8fc2801..d6481e03ed 100644 --- a/Tools/px_romfs_pruner.py +++ b/Tools/px_romfs_pruner.py @@ -1,7 +1,7 @@ #!/usr/bin/env python ############################################################################ # -# Copyright (C) 2014-2015 PX4 Development Team. All rights reserved. +# Copyright (C) 2014-2016 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 @@ -35,7 +35,12 @@ """ px_romfs_pruner.py: -Delete all comments and newlines before ROMFS is converted to an image +Try to keep size of ROMFS minimal. + +This script goes through the temporarily copied ROMFS data and deletes all +comments, empty lines and leading whitespace. +It also deletes hidden files such as auto-saved backups that a text editor +might have left in the tree. @author: Julian Oes """ @@ -53,18 +58,25 @@ def main(): help="ROMFS scratch folder.") args = parser.parse_args() - #print("Pruning ROMFS files.") - - # go through + # go through temp folder for (root, dirs, files) in os.walk(args.folder): for file in files: - # only prune text files - if ".zip" in file or ".bin" in file or ".swp" in file \ - or ".data" in file or ".DS_Store" in file \ - or file.startswith("."): + file_path = os.path.join(root, file) + + # delete hidden files + if file.startswith("."): + os.remove(file_path) continue - file_path = os.path.join(root, file) + # delete documentation + if file.startswith("README"): + os.remove(file_path) + continue + + # only prune text files + if ".zip" in file or ".bin" in file or ".swp" in file \ + or ".data" in file or ".DS_Store" in file: + continue # read file line by line pruned_content = "" @@ -77,7 +89,7 @@ def main(): else: if not line.isspace() \ and not line.strip().startswith("#"): - pruned_content += line + pruned_content += line.strip() + "\n" # overwrite old scratch file with open(file_path, "wb") as f: pruned_content = re.sub("\r\n", "\n", pruned_content) diff --git a/Tools/scp_upload.sh b/Tools/scp_upload.sh new file mode 100755 index 0000000000..2659af37cd --- /dev/null +++ b/Tools/scp_upload.sh @@ -0,0 +1,32 @@ +#!/bin/bash + +if [[ "$#" < 2 ]]; then + echo "usage: scp_upload.sh SRC1 [SRC2 ...] DEST" + exit +fi + +if [ -z ${AUTOPILOT_HOST+x} ]; then + host=px4autopilot + echo "\$AUTOPILOT_HOST is not set (use default: $host)" +else + host=$AUTOPILOT_HOST + echo "\$AUTOPILOT_HOST is set to $host" +fi + +echo "Uploading..." + +# Get last argument +for last; do true; done + +# Go through source files and push them one by one. +i=0 +for arg +do + if [[ $((i+1)) == "$#" ]]; then + break + fi + # echo "Pushing $arg to $last" + #adb push $arg $last + scp $arg pi@$host:$last + ((i+=1)) +done diff --git a/Tools/sdlog2/README.txt b/Tools/sdlog2/README.txt index abeb9a4c71..e407c0936f 100644 --- a/Tools/sdlog2/README.txt +++ b/Tools/sdlog2/README.txt @@ -8,6 +8,14 @@ logconv.m: This is a MATLAB script which will automatically convert and display sdlog2_dump.py: This is a Python script (compatible with v2 and v3) which converts the self-describing binary log format to a CSV file. To export a CSV file from within a shell (Windows CMD or BASH on Linux / Mac OS), run: -python sdlog2_dump.py log001.bin -f "export.csv" -t "TIME" -d "," -n "" + python sdlog2_dump.py log001.bin -f "export.csv" -t "TIME" -d "," -n "" -Python can be downloaded from http://python.org, but is available as default on Mac OS and Linux. \ No newline at end of file +geo_tag_images.py: Use this script to geotag a set of images. It uses GPS time and file creation date to synchronize the images, so it needs that the images have a valid creation date. + + python geo_tag_images.py --logfile=mylog.bin --input=images/ --output=tagged/ + +geotagging.py: Use this script to geotag a set of images. It uses the CAM trigger data from the log file for image association. + + python geotagging.py --logfile=mylog.bin --input=images/ --output=tagged/ + +Python can be downloaded from http://python.org, but is available as default on Mac OS and Linux. diff --git a/Tools/sdlog2/geo_tag_images.py b/Tools/sdlog2/geo_tag_images.py new file mode 100644 index 0000000000..c934c26e8b --- /dev/null +++ b/Tools/sdlog2/geo_tag_images.py @@ -0,0 +1,375 @@ +#!/usr/bin/env python +# +# Tag the images recorded during a flight with geo location extracted from +# a PX4 binary log file. +# +# This file accepts *.jpg format images and reads position information +# from a *.bin file +# +# Example Syntax: +# python geotag.py --logfile=log001.bin --input=images/ --output=imagesWithTag/ --offset=-0.4 -v +# +# Author: Hector Azpurua +# Based on the script of Andreas Bircher + +import os +import re +import sys +import csv +import bisect +import pyexiv2 +import argparse +from lxml import etree +import datetime, calendar +from shutil import copyfile +from pykml.factory import KML_ElementMaker as KML +from pykml.factory import GX_ElementMaker as GX + + +class GpsPosition(object): + def __init__(self, timestamp, lat, lon, alt): + self.timestamp = timestamp + self.lat = float(lat) + self.lon = float(lon) + self.alt = float(alt) + + +class Main: + def __init__(self): + """ + + :param logfile: + :param input: + :param output: + :param offset: + :param verbose: + :return: + """ + args = self.get_arg() + + self.logfile = args['logfile'] + self.input = args['input'] + self.output = args['output'] + self.klm = args['klm'] + self.verbose = args['verbose'] + self.offset = args['offset'] + self.time_tresh = args['treshold'] + + self.tdiff_list = [] + self.non_processed_files = [] + self.tagged_gps = [] + self.gps_list = self.load_gps_from_log(self.logfile, self.offset) + self.img_list = self.load_image_list(self.input) + + if len(self.img_list) <= 0: + print '[ERROR] Cannot load JPG images from input folder, please check filename extensions.' + sys.exit(1) + + if not os.path.exists(self.output): + os.makedirs(self.output) + + if not self.output.endswith(os.path.sep): + self.output += os.path.sep + + self.tag_images() + + if self.klm and len(self.tdiff_list) > 0: + self.gen_klm() + + if len(self.non_processed_files) > 0: + print '[WARNING] Some images werent processed:' + for elem in self.non_processed_files: + print '\t', elem + + @staticmethod + def to_degree(value, loc): + """ + Convert a lat or lon value to degrees/minutes/seconds + :param value: the latitude or longitude value + :param loc: could be ["S", "N"] or ["W", "E"] + :return: + """ + if value < 0: + loc_value = loc[0] + elif value > 0: + loc_value = loc[1] + else: + loc_value = "" + + absolute_value = abs(value) + deg = int(absolute_value) + t1 = (absolute_value-deg) * 60 + minute = int(t1) + sec = round((t1 - minute) * 60, 5) + + return deg, minute, sec, loc_value + + @staticmethod + def gps_week_seconds_to_datetime(gpsweek, gpsmillis, leapmillis=0): + """ + Convert GPS week and seconds to datetime object, using leap milliseconds if necessary + :param gpsweek: + :param gpsmillis: + :param leapmillis: + :return: + """ + datetimeformat = "%Y-%m-%d %H:%M:%S.%f" + epoch = datetime.datetime.strptime("1980-01-06 00:00:00.000", datetimeformat) + elapsed = datetime.timedelta(days=(gpsweek * 7), milliseconds=(gpsmillis + leapmillis)) + + return Main.utc_to_local(epoch + elapsed) + + @staticmethod + def utc_to_local(utc_dt): + """ + Convert UTC time in local time + :param utc_dt: + :return: + """ + timestamp = calendar.timegm(utc_dt.timetuple()) # use integer timestamp to avoid precision lost + local_dt = datetime.datetime.fromtimestamp(timestamp) + assert utc_dt.resolution >= datetime.timedelta(microseconds=1) + + return local_dt.replace(microsecond=utc_dt.microsecond) + + def gen_klm(self): + """ + Generate a KML file with keypoints on the locations of the pictures, including height + :return: + """ + style_dot = "sn_shaded_dot" + style_path = "red_path" + + doc = KML.kml( + KML.Document( + KML.Name("GPS of the images"), + KML.Style( + KML.IconStyle( + KML.scale(0.4), + KML.Icon( + KML.href("http://maps.google.com/mapfiles/kml/shapes/shaded_dot.png") + ), + ), + id=style_dot, + ), + KML.Style( + KML.LineStyle( + KML.color('7f0000ff'), + KML.width(6), + GX.labelVisibility('1'), + ), + id=style_path + ) + ) + ) + + # create points + for i, gps in enumerate(self.tagged_gps): + ii = i + 1 + doc.Document.append( + KML.Placemark( + KML.styleUrl('#{0}'.format(style_dot)), + KML.Point( + KML.extrude(True), + KML.altitudeMode('relativeToGround'), + KML.coordinates("{},{},{}".format(gps.lon, gps.lat, gps.alt)) + ), + KML.name(str(ii)) if ii % 5 == 0 or ii == 1 else KML.name() + ) + ) + + # create the path + doc.Document.append( + KML.Placemark( + KML.styleUrl('#{0}'.format(style_path)), + KML.LineString( + KML.altitudeMode('relativeToGround'), + KML.coordinates( + ' '.join(["{},{},{}".format(gps.lon, gps.lat, gps.alt) for gps in self.tagged_gps]) + ) + ) + ) + ) + + s = etree.tostring(doc) + + file_path = self.output + 'GoogleEarth_points.kml' + f = open(file_path,'w') + f.write(s) + f.close() + + print '[INFO] KML file generated on:', file_path + + def get_closest_datetime_index(self, datetime_list, elem): + """ + Get the closest element between a list of datetime objects and a date + :param datetime_list: + :param elem: + :return: + """ + i = bisect.bisect_left(datetime_list, elem) + date = datetime_list[i] + diff = (date - elem).total_seconds() + + if diff > self.time_tresh: + return -1, diff + + return i, diff + + def set_gps_location(self, file_name, lat, lng, alt): + """ + Add the GPS tag and altitude to a image file + :param file_name: + :param lat: + :param lng: + :param alt: + :return: + """ + lat_deg = self.to_degree(lat, ["S", "N"]) + lng_deg = self.to_degree(lng, ["W", "E"]) + + exiv_lat = (pyexiv2.Rational(lat_deg[0] * 60 + lat_deg[1], 60), + pyexiv2.Rational(lat_deg[2] * 100, 6000), pyexiv2.Rational(0, 1)) + exiv_lng = (pyexiv2.Rational(lng_deg[0] * 60 + lng_deg[1], 60), + pyexiv2.Rational(lng_deg[2] * 100, 6000), pyexiv2.Rational(0, 1)) + + try: + exiv_image = pyexiv2.ImageMetadata(file_name) + exiv_image.read() + + exiv_image["Exif.GPSInfo.GPSLatitude"] = exiv_lat + exiv_image["Exif.GPSInfo.GPSLatitudeRef"] = lat_deg[3] + exiv_image["Exif.GPSInfo.GPSLongitude"] = exiv_lng + exiv_image["Exif.GPSInfo.GPSLongitudeRef"] = lng_deg[3] + exiv_image["Exif.GPSInfo.GPSAltitude"] = pyexiv2.Rational(alt, 1) + exiv_image["Exif.GPSInfo.GPSAltitudeRef"] = '0' + exiv_image["Exif.Image.GPSTag"] = 654 + exiv_image["Exif.GPSInfo.GPSMapDatum"] = "WGS-84" + exiv_image["Exif.GPSInfo.GPSVersionID"] = '2 0 0 0' + + exiv_image.write(True) + except Exception as e: + print '[ERROR]', e + + def load_gps_from_log(self, log_file, offset): + """ + Load gps list from PX4 binary log + :param log_file: + :param offset: + :return: + """ + os.system('python sdlog2_dump.py ' + log_file + ' -f log.csv') + f = open('log.csv', 'rb') + reader = csv.reader(f) + headers = reader.next() + line = {} + for h in headers: + line[h] = [] + + for row in reader: + for h, v in zip(headers, row): + line[h].append(v) + + gps_list = [] + for seq in range(0, len(line['GPS_Lat']) - 1): + gps_time = int(line['GPS_TimeMS'][seq + 1]) + gps_week = int(line['GPS_Week'][seq + 1]) + gps_lat = float(line['GPS_Lat'][seq + 1]) + gps_lon = float(line['GPS_Lng'][seq + 1]) + gps_alt = float(line['GPS_RelAlt'][seq + 1]) + + date = self.gps_week_seconds_to_datetime(gps_week, gps_time, leapmillis=offset) + print date + gps_list.append(GpsPosition(date, gps_lat, gps_lon, gps_alt)) + + return gps_list + + def load_image_list(self, input_folder, file_type='jpg'): + """ + Load image list from a folder given a file type + :param input_folder: + :param file_type: + :return: + """ + self.img_list = [input_folder + filename for filename in os.listdir(input_folder) + if re.search(r'\.'+file_type+'$', filename, re.IGNORECASE)] + self.img_list = sorted(self.img_list) + return self.img_list + + def tag_images(self): + """ + Tag the image list using the GPS loaded from the LOG file + :return: + """ + tagged_gps = [] + img_size = len(self.img_list) + print '[INFO] Number of images:', img_size + + dt_list = [x.timestamp for x in self.gps_list] + + img_seq = 1 + + for i in xrange(img_size): + base_path, filename = os.path.split(self.img_list[i]) + cdate = datetime.datetime.fromtimestamp(os.path.getmtime(self.img_list[i])) + gps_i, img_tdiff = self.get_closest_datetime_index(dt_list, cdate) + + if gps_i == -1: + self.non_processed_files.append(filename) + continue + + closest_gps = self.gps_list[gps_i] + self.tdiff_list.append(img_tdiff) + + if self.verbose: + msg = "[DEBUG] %s/%s) %s\n\timg %s -> gps %s (%ss)\n\tlat:%s, lon:%s, alt:%s".ljust(60) %\ + (i+1, img_size, filename, cdate, closest_gps.timestamp, img_tdiff, closest_gps.lat, closest_gps.lon, closest_gps.alt) + print msg + + copyfile(self.img_list[i], self.output + str(img_seq) + filename) + self.set_gps_location(self.output + str(img_seq) + filename, closest_gps.lat, closest_gps.lon, closest_gps.alt) + self.tagged_gps.append(closest_gps) + img_seq += 1 + + if len(self.tdiff_list) > 0: + print '[INFO] Mean diff in seconds:', sum(self.tdiff_list) / float(len(self.tdiff_list)) + + @staticmethod + def get_arg(): + parser = argparse.ArgumentParser( + description='Geotag script to add GPS info to pictures from PX4 binary log files.'\ + 'It uses synchronized time to allocate GPS positions.' + ) + + parser.add_argument( + '-l', '--logfile', help='PX4 log file containing recorded positions.', required=True + ) + parser.add_argument( + '-i', '--input', help='Input folder containing untagged images.', required=True + ) + parser.add_argument( + '-o', '--output', help='Output folder to contain tagged images.', required=True + ) + parser.add_argument( + '-t', '--treshold', help='Time treshold between the GPS time and the local image time.', + default=1, required=False, type=float + ) + parser.add_argument( + '-of', '--offset', help='Time offset in MILLISECONDS between the GPS time and the local time.', + default=-17000, required=False, type=float + ) + parser.add_argument( + '-klm', '--klm', help='Save the in KML format the information of all tagged images.', + required=False, action='store_true' + ) + parser.add_argument( + '-v', '--verbose', help='Prints lots of information.', + required=False, action='store_true' + ) + + args = vars(parser.parse_args()) + return args + + +if __name__ == "__main__": + m = Main() \ No newline at end of file diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index 9353834b2f..00dcabb1c2 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit 9353834b2f89d08beaa055fc97321e1871bc2b3a +Subproject commit 00dcabb1c2c001b40713903da1a4a7f71d01ef27 diff --git a/Tools/sitl_multiple_run.sh b/Tools/sitl_multiple_run.sh new file mode 100755 index 0000000000..f306679edd --- /dev/null +++ b/Tools/sitl_multiple_run.sh @@ -0,0 +1,52 @@ +#!/bin/bash + +sitl_num=2 + +sim_port=15019 +mav_port=15010 +mav_port2=15011 + +mav_oport=15015 +mav_oport2=15016 + +port_step=10 + +src_path=`pwd` + +rc_script="posix-configs/SITL/init/rcS_multiple" +build_path=${src_path}/build_posix_sitl_default + +pkill px4 +sleep 2 + +cd $build_path/src/firmware/posix + +user=`whoami` +n=1 +while [ $n -le $sitl_num ]; do + if [ ! -d $n ]; then + mkdir -p $n + cd $n + + mkdir -p rootfs/fs/microsd + mkdir -p rootfs/eeprom + touch rootfs/eeprom/parameters + + cp ${src_path}/ROMFS/px4fmu_common/mixers/quad_w.main.mix ./ + cat ${src_path}/${rc_script}_gazebo_iris | sed s/_SIMPORT_/${sim_port}/ | sed s/_MAVPORT_/${mav_port}/g | sed s/_MAVOPORT_/${mav_oport}/ | sed s/_MAVPORT2_/${mav_port2}/ | sed s/_MAVOPORT2_/${mav_oport2}/ > rcS + cd ../ + fi + + cd $n + + sudo -b -u $user ../px4 -d rcS >out.log 2>err.log + + cd ../ + + n=$(($n + 1)) + sim_port=$(($sim_port + $port_step)) + mav_port=$(($mav_port + $port_step)) + mav_port2=$(($mav_port2 + $port_step)) + mav_oport=$(($mav_oport + $port_step)) + mav_oport2=$(($mav_oport2 + $port_step)) +done diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index aa5bd9ae3f..6e467c31fd 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -43,7 +43,7 @@ fi # kill process names that might stil # be running from last time pkill gazebo -pkill mainapp +pkill px4 jmavsim_pid=`jps | grep Simulator | cut -d" " -f1` if [ -n "$jmavsim_pid" ] then @@ -58,7 +58,7 @@ cp Tools/posix.gdbinit $build_path/src/firmware/posix/.gdbinit SIM_PID=0 -if [ "$program" == "jmavsim" ] && [ "$no_sim" == "" ] +if [ "$program" == "jmavsim" ] && [ ! -n "$no_sim" ] then cd Tools/jMAVSim ant create_run_jar copy_res @@ -66,31 +66,33 @@ then java -Djava.ext.dirs= -jar jmavsim_run.jar -udp 127.0.0.1:14560 & SIM_PID=`echo $!` cd ../.. -elif [ "$program" == "gazebo" ] && [ "$no_sim" == "" ] +elif [ "$program" == "gazebo" ] && [ ! -n "$no_sim" ] then if [ -x "$(command -v gazebo)" ] then # Set the plugin path so Gazebo finds our model and sim - export GAZEBO_PLUGIN_PATH=$curr_dir/Tools/sitl_gazebo/Build:${GAZEBO_PLUGIN_PATH} + export GAZEBO_PLUGIN_PATH=$curr_dir/build_gazebo:${GAZEBO_PLUGIN_PATH} # Set the model path so Gazebo finds the airframes export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:$curr_dir/Tools/sitl_gazebo/models # The next line would disable online model lookup, can be commented in, in case of unstable behaviour. # export GAZEBO_MODEL_DATABASE_URI="" export SITL_GAZEBO_PATH=$curr_dir/Tools/sitl_gazebo - mkdir -p Tools/sitl_gazebo/Build - cd Tools/sitl_gazebo/Build - cmake -Wno-dev .. - make -j4 - make sdf - gzserver --verbose ../worlds/${model}.world & + make --no-print-directory gazebo_build + + gzserver --verbose $curr_dir/Tools/sitl_gazebo/worlds/${model}.world & SIM_PID=`echo $!` - gzclient --verbose & - GUI_PID=`echo $!` + + if [[ -n "$HEADLESS" ]]; then + echo "not running gazebo gui" + else + gzclient --verbose & + GUI_PID=`echo $!` + fi else echo "You need to have gazebo simulator installed!" exit 1 fi -elif [ "$program" == "replay" ] && [ "$no_sim" == "" ] +elif [ "$program" == "replay" ] && [ ! -n "$no_sim" ] then echo "Replaying logfile: $logfile" # This is not a simulator, but a log file to replay @@ -115,18 +117,18 @@ set +e # Start Java simulator if [ "$debugger" == "lldb" ] then - lldb -- mainapp ../../../../${rc_script}_${program}_${model} + lldb -- px4 ../../../../${rc_script}_${program}_${model} elif [ "$debugger" == "gdb" ] then - gdb --args mainapp ../../../../${rc_script}_${program}_${model} + gdb --args px4 ../../../../${rc_script}_${program}_${model} elif [ "$debugger" == "ddd" ] then - ddd --debugger gdb --args mainapp ../../../../${rc_script}_${program}_${model} + ddd --debugger gdb --args px4 ../../../../${rc_script}_${program}_${model} elif [ "$debugger" == "valgrind" ] then - valgrind ./mainapp ../../../../${rc_script}_${program}_${model} + valgrind ./px4 ../../../../${rc_script}_${program}_${model} else - $sudo_enabled ./mainapp $chroot_enabled ../../../../${rc_script}_${program}_${model} + $sudo_enabled ./px4 $chroot_enabled ../../../../${rc_script}_${program}_${model} fi if [ "$program" == "jmavsim" ] @@ -135,5 +137,7 @@ then elif [ "$program" == "gazebo" ] then kill -9 $SIM_PID - kill -9 $GUI_PID + if [[ ! -n "$HEADLESS" ]]; then + kill -9 $GUI_PID + fi fi diff --git a/Tools/upload.sh b/Tools/upload.sh index d78abc3d23..4a2e463008 100755 --- a/Tools/upload.sh +++ b/Tools/upload.sh @@ -17,7 +17,7 @@ fi if [ $SYSTYPE = "Linux" ]; then -SERIAL_PORTS="/dev/serial/by-id/usb-3D_Robotics*" +SERIAL_PORTS="/dev/serial/by-id/usb-3D_Robotics*,/dev/serial/by-id/usb-The_Autopilot*" fi if [ $SYSTYPE = "" ]; @@ -25,4 +25,4 @@ then SERIAL_PORTS="COM32,COM31,COM30,COM29,COM28,COM27,COM26,COM25,COM24,COM23,COM22,COM21,COM20,COM19,COM18,COM17,COM16,COM15,COM14,COM13,COM12,COM11,COM10,COM9,COM8,COM7,COM6,COM5,COM4,COM3,COM2,COM1,COM0" fi -python $BASEDIR/px_uploader.py --port $SERIAL_PORTS $1 \ No newline at end of file +python $BASEDIR/px_uploader.py --port $SERIAL_PORTS $1 diff --git a/circle.yml b/circle.yml new file mode 100644 index 0000000000..492719d00b --- /dev/null +++ b/circle.yml @@ -0,0 +1,20 @@ +machine: + services: + - docker + +checkout: + post: + - git submodule sync --recursive + - git submodule update --init --recursive + +## Customize dependencies +dependencies: + cache_directories: + - "~/.ccache" + pre: + - docker pull px4io/px4-dev-nuttx-gcc4.9 + +test: + override: + - docker run --rm -v `pwd`:`pwd`:rw -w=`pwd` -v $HOME/.ccache:$HOME/.ccache:rw -e CCACHE_DIR=$HOME/.ccache --user=$UID -it px4io/px4-dev-nuttx-gcc4.9 /bin/bash -c "ccache -z; make px4fmu-v4_default; ccache -s" + diff --git a/cmake/cmake_hexagon b/cmake/cmake_hexagon index 77d487dc07..eb1d242e57 160000 --- a/cmake/cmake_hexagon +++ b/cmake/cmake_hexagon @@ -1 +1 @@ -Subproject commit 77d487dc076a8519f81a335a5e4a2040799790de +Subproject commit eb1d242e57b34a28ba4b8ab4c04901bd85540d07 diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake index 5650e0aa2a..ef05520375 100644 --- a/cmake/common/px4_base.cmake +++ b/cmake/common/px4_base.cmake @@ -303,6 +303,9 @@ function(px4_add_module) if(MAIN) set_target_properties(${MODULE} PROPERTIES COMPILE_DEFINITIONS PX4_MAIN=${MAIN}_app_main) + add_definitions(-DMODULE_NAME="${MAIN}") + else() + add_definitions(-DMODULE_NAME="${MODULE}") endif() if(INCLUDES) @@ -359,13 +362,15 @@ function(px4_generate_messages) NAME px4_generate_messages OPTIONS VERBOSE ONE_VALUE OS TARGET - MULTI_VALUE MSG_FILES DEPENDS + MULTI_VALUE MSG_FILES DEPENDS INCLUDES REQUIRED MSG_FILES OS TARGET ARGN ${ARGN}) set(QUIET) if(NOT VERBOSE) set(QUIET "-q") endif() + + # headers set(msg_out_path ${CMAKE_BINARY_DIR}/src/modules/uORB/topics) set(msg_list) foreach(msg_file ${MSG_FILES}) @@ -378,18 +383,48 @@ function(px4_generate_messages) endforeach() add_custom_command(OUTPUT ${msg_files_out} COMMAND ${PYTHON_EXECUTABLE} - Tools/px_generate_uorb_topic_headers.py + Tools/px_generate_uorb_topic_files.py + --headers ${QUIET} -d msg -o ${msg_out_path} -e msg/templates/uorb - -t ${CMAKE_BINARY_DIR}/topics_temporary + -t ${CMAKE_BINARY_DIR}/topics_temporary_header DEPENDS ${DEPENDS} ${MSG_FILES} WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} COMMENT "Generating uORB topic headers" VERBATIM ) + # !sources + set(msg_source_out_path ${CMAKE_BINARY_DIR}/topics_sources) + set(msg_source_files_out ${msg_source_out_path}/uORBTopics.cpp) + foreach(msg ${msg_list}) + list(APPEND msg_source_files_out ${msg_source_out_path}/${msg}.cpp) + endforeach() + add_custom_command(OUTPUT ${msg_source_files_out} + COMMAND ${PYTHON_EXECUTABLE} + Tools/px_generate_uorb_topic_files.py + --sources + ${QUIET} + -d msg + -o ${msg_source_out_path} + -e msg/templates/uorb + -t ${CMAKE_BINARY_DIR}/topics_temporary_sources + DEPENDS ${DEPENDS} ${MSG_FILES} + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + COMMENT "Generating uORB topic sources" + VERBATIM + ) + set_source_files_properties(${msg_source_files_out} PROPERTIES GENERATED TRUE) + + # We remove uORBTopics.cpp to make sure the generator is re-run, which is + # necessary when a .msg file is removed and because uORBTopics.cpp depends + # on all topics. + execute_process(COMMAND rm uORBTopics.cpp + WORKING_DIRECTORY ${msg_source_out_path} + ERROR_QUIET) + # multi messages for target OS set(msg_multi_out_path ${CMAKE_BINARY_DIR}/src/platforms/${OS}/px4_messages) @@ -399,7 +434,8 @@ function(px4_generate_messages) endforeach() add_custom_command(OUTPUT ${msg_multi_files_out} COMMAND ${PYTHON_EXECUTABLE} - Tools/px_generate_uorb_topic_headers.py + Tools/px_generate_uorb_topic_files.py + --headers ${QUIET} -d msg -o ${msg_multi_out_path} @@ -411,8 +447,13 @@ function(px4_generate_messages) COMMENT "Generating uORB topic multi headers for ${OS}" VERBATIM ) - add_custom_target(${TARGET} - DEPENDS ${msg_multi_files_out} ${msg_files_out}) + + add_library(${TARGET} + ${msg_source_files_out} + ${msg_multi_files_out} + ${msg_files_out} + ) + endfunction() #============================================================================= @@ -447,6 +488,7 @@ function(px4_add_upload) if(${CMAKE_HOST_SYSTEM_NAME} STREQUAL "Linux") list(APPEND serial_ports /dev/serial/by-id/usb-3D_Robotics* + /dev/serial/by-id/usb-The_Autopilot* /dev/serial/by-id/pci-3D_Robotics* ) elseif(${CMAKE_HOST_SYSTEM_NAME} STREQUAL "Darwin") @@ -490,6 +532,42 @@ function(px4_add_adb_push) ) endfunction() +function(px4_add_adb_push_to_bebop) + px4_parse_function_args( + NAME px4_add_upload_to_bebop + ONE_VALUE OS BOARD OUT DEST + MULTI_VALUE FILES DEPENDS + REQUIRED OS BOARD OUT FILES DEPENDS DEST + ARGN ${ARGN}) + + add_custom_target(${OUT} + COMMAND ${CMAKE_SOURCE_DIR}/Tools/adb_upload_to_bebop.sh ${FILES} ${DEST} + DEPENDS ${DEPENDS} + WORKING_DIRECTORY ${CMAKE_BINARY_DIR} + COMMENT "uploading ${BUNDLE}" + VERBATIM + USES_TERMINAL + ) +endfunction() + +function(px4_add_scp_push) + px4_parse_function_args( + NAME px4_add_upload + ONE_VALUE OS BOARD OUT DEST + MULTI_VALUE FILES DEPENDS + REQUIRED OS BOARD OUT FILES DEPENDS DEST + ARGN ${ARGN}) + + add_custom_target(${OUT} + COMMAND ${CMAKE_SOURCE_DIR}/Tools/scp_upload.sh ${FILES} ${DEST} + DEPENDS ${DEPENDS} + WORKING_DIRECTORY ${CMAKE_BINARY_DIR} + COMMENT "uploading ${BUNDLE}" + VERBATIM + USES_TERMINAL + ) +endfunction() + #============================================================================= # @@ -541,7 +619,6 @@ function(px4_add_common_flags) -Wall -Werror -Wextra - -Wpacked -Wno-sign-compare -Wshadow -Wfloat-equal @@ -583,7 +660,8 @@ function(px4_add_common_flags) endif() if ($ENV{MEMORY_DEBUG} MATCHES "1") - set(max_optimization -O0) + message(STATUS "address sanitizer enabled") + set(max_optimization -Os) set(optimization_flags -fno-strict-aliasing @@ -591,7 +669,7 @@ function(px4_add_common_flags) -funsafe-math-optimizations -ffunction-sections -fdata-sections - -g -fsanitize=address + -g3 -fsanitize=address ) else() set(max_optimization -Os) @@ -710,6 +788,7 @@ function(px4_add_common_flags) string(REPLACE "-" "_" board_config ${board_upper}) set(added_definitions -DCONFIG_ARCH_BOARD_${board_config} + -D__STDC_FORMAT_MACROS ) if (NOT (APPLE AND (${CMAKE_C_COMPILER_ID} MATCHES ".*Clang.*"))) @@ -776,21 +855,21 @@ function(px4_create_git_hash_header) REQUIRED HEADER ARGN ${ARGN}) execute_process( - COMMAND git describe --tags + COMMAND git describe --always --tags OUTPUT_VARIABLE git_tag OUTPUT_STRIP_TRAILING_WHITESPACE WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} ) - #message(STATUS "GIT_TAG = ${git_tag}") + message(STATUS "GIT_TAG = ${git_tag}") execute_process( - COMMAND git rev-parse HEAD - OUTPUT_VARIABLE git_desc + COMMAND git rev-parse --verify HEAD + OUTPUT_VARIABLE git_version OUTPUT_STRIP_TRAILING_WHITESPACE WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} ) - #message(STATUS "GIT_DESC = ${git_desc}") - set(git_desc_short) - string(SUBSTRING ${git_desc} 1 16 git_desc_short) + #message(STATUS "GIT_VERSION = ${git_version}") + set(git_version_short) + string(SUBSTRING ${git_version} 1 16 git_version_short) configure_file(${CMAKE_SOURCE_DIR}/cmake/templates/build_git_version.h.in ${HEADER} @ONLY) endfunction() diff --git a/cmake/configs/nuttx_mindpx-v2_default.cmake b/cmake/configs/nuttx_mindpx-v2_default.cmake index 9f0626e5b7..41c9d32df8 100644 --- a/cmake/configs/nuttx_mindpx-v2_default.cmake +++ b/cmake/configs/nuttx_mindpx-v2_default.cmake @@ -15,7 +15,7 @@ set(config_module_list drivers/led drivers/px4fmu #drivers/px4io - drivers/test_ppm + #drivers/test_ppm drivers/boards/mindpx-v2 #drivers/rgbled drivers/rgbled_pwm @@ -64,18 +64,33 @@ set(config_module_list systemcmds/pwm systemcmds/esc_calib systemcmds/reboot - #systemcmds/topic_listener + systemcmds/topic_listener systemcmds/top systemcmds/config systemcmds/nshterm systemcmds/mtd systemcmds/dumpfile systemcmds/ver + systemcmds/sd_bench + systemcmds/motor_ramp + + # + # Tests + # + drivers/sf0x/sf0x_tests + drivers/test_ppm + modules/commander/commander_tests + modules/controllib_test + modules/mavlink/mavlink_tests + modules/unit_test + modules/uORB/uORB_tests + systemcmds/tests # # General system control # modules/commander + modules/load_mon modules/navigator modules/mavlink modules/gpio_led @@ -85,16 +100,15 @@ set(config_module_list # # Estimation modules (EKF/ SO3 / other filters) # - # Too high RAM usage due to static allocations - # modules/attitude_estimator_ekf modules/attitude_estimator_q modules/ekf_att_pos_estimator modules/position_estimator_inav + modules/local_position_estimator + modules/ekf2 # # Vehicle Control # - # modules/segway # XXX Needs GCC 4.7 fix modules/fw_pos_control_l1 modules/fw_att_control modules/mc_att_control @@ -105,6 +119,7 @@ set(config_module_list # Logging # modules/sdlog2 + modules/logger # # Library modules @@ -134,7 +149,7 @@ set(config_module_list platforms/nuttx # had to add for cmake, not sure why wasn't in original config - platforms/common + platforms/common platforms/nuttx/px4_layer # @@ -145,7 +160,7 @@ set(config_module_list # # Rover apps # - examples/rover_steering_control + #examples/rover_steering_control # # Demo apps @@ -153,7 +168,7 @@ set(config_module_list #examples/math_demo # Tutorial code from # https://px4.io/dev/px4_simple_app - examples/px4_simple_app + #examples/px4_simple_app # Tutorial code from # https://px4.io/dev/daemon @@ -176,10 +191,6 @@ set(config_extra_builtin_cmds sercon ) -set(config_io_board - px4io-v2 - ) - set(config_extra_libs uavcan uavcan_stm32_driver @@ -191,9 +202,11 @@ set(config_io_extra_libs add_custom_target(sercon) set_target_properties(sercon PROPERTIES PRIORITY "SCHED_PRIORITY_DEFAULT" - MAIN "sercon" STACK_MAIN "2048") + MAIN "sercon" + STACK_MAIN "2048") add_custom_target(serdis) set_target_properties(serdis PROPERTIES PRIORITY "SCHED_PRIORITY_DEFAULT" - MAIN "serdis" STACK_MAIN "2048") + MAIN "serdis" + STACK_MAIN "2048") diff --git a/cmake/configs/nuttx_px4fmu-v1_default.cmake b/cmake/configs/nuttx_px4fmu-v1_default.cmake index 85c7e2f347..75f8d054ed 100644 --- a/cmake/configs/nuttx_px4fmu-v1_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v1_default.cmake @@ -61,6 +61,7 @@ set(config_module_list # General system control # modules/commander + modules/load_mon modules/navigator modules/mavlink modules/gpio_led @@ -90,6 +91,7 @@ set(config_module_list # Logging # modules/sdlog2 + modules/logger # # Library modules diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake index 97f9126004..c38d726702 100644 --- a/cmake/configs/nuttx_px4fmu-v2_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake @@ -48,33 +48,48 @@ set(config_module_list drivers/bst drivers/snapdragon_rc_pwm drivers/lis3mdl - drivers/bmi160 # # System commands # systemcmds/bl_update + systemcmds/config + systemcmds/dumpfile + #systemcmds/esc_calib systemcmds/mixer + #systemcmds/motor_ramp + systemcmds/mtd + systemcmds/nshterm systemcmds/param systemcmds/perf systemcmds/pwm - systemcmds/esc_calib systemcmds/reboot - #systemcmds/topic_listener + #systemcmds/sd_bench systemcmds/top - systemcmds/config - systemcmds/nshterm - systemcmds/mtd - systemcmds/dumpfile + #systemcmds/topic_listener systemcmds/ver + # + # Testing + # + #drivers/sf0x/sf0x_tests + #drivers/test_ppm + #lib/rc/rc_tests + #modules/commander/commander_tests + #modules/controllib_test + #modules/mavlink/mavlink_tests + #modules/unit_test + #modules/uORB/uORB_tests + #systemcmds/tests + # # General system control # modules/commander + modules/load_mon modules/navigator modules/mavlink - modules/gpio_led + #modules/gpio_led modules/uavcan modules/land_detector @@ -82,14 +97,14 @@ set(config_module_list # Estimation modules (EKF/ SO3 / other filters) # modules/attitude_estimator_q - modules/ekf_att_pos_estimator + #modules/ekf_att_pos_estimator modules/position_estimator_inav modules/local_position_estimator + modules/ekf2 # # Vehicle Control # - # modules/segway # XXX Needs GCC 4.7 fix modules/fw_pos_control_l1 modules/fw_att_control modules/mc_att_control @@ -99,6 +114,7 @@ set(config_module_list # # Logging # + #modules/logger modules/sdlog2 # diff --git a/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake b/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake index 6cc8c2d262..f43b782045 100644 --- a/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake @@ -41,11 +41,13 @@ set(config_module_list modules/sensors #drivers/mkblctrl drivers/px4flow - drivers/oreoled + #drivers/oreoled drivers/gimbal drivers/pwm_input drivers/camera_trigger drivers/bst + #drivers/snapdragon_rc_pwm + #drivers/lis3mdl # # System commands @@ -64,25 +66,25 @@ set(config_module_list systemcmds/mtd systemcmds/dumpfile systemcmds/ver + #systemcmds/sd_bench + #systemcmds/tests + systemcmds/motor_ramp # # General system control # modules/commander + modules/load_mon modules/navigator modules/mavlink - modules/gpio_led + #modules/gpio_led modules/uavcan modules/land_detector # # Estimation modules (EKF/ SO3 / other filters) # - # Too high RAM usage due to static allocations - # modules/attitude_estimator_ekf - modules/attitude_estimator_q modules/ekf2 - modules/position_estimator_inav # # Vehicle Control @@ -97,6 +99,7 @@ set(config_module_list # # Logging # + modules/logger modules/sdlog2 # @@ -138,7 +141,7 @@ set(config_module_list # # Rover apps # - examples/rover_steering_control + #examples/rover_steering_control # # Demo apps diff --git a/cmake/configs/nuttx_px4fmu-v2_test.cmake b/cmake/configs/nuttx_px4fmu-v2_test.cmake new file mode 100644 index 0000000000..b72c5a8190 --- /dev/null +++ b/cmake/configs/nuttx_px4fmu-v2_test.cmake @@ -0,0 +1,210 @@ +include(nuttx/px4_impl_nuttx) + +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake) + +set(config_uavcan_num_ifaces 2) + +set(config_module_list + # + # Board support modules + # + drivers/device + drivers/stm32 + drivers/stm32/adc + drivers/stm32/tone_alarm + drivers/led + drivers/px4fmu + drivers/px4io + drivers/boards/px4fmu-v2 + drivers/rgbled + drivers/mpu6000 + drivers/mpu9250 + drivers/lsm303d + drivers/l3gd20 + drivers/hmc5883 + drivers/ms5611 + #drivers/mb12xx + #drivers/srf02 + #drivers/sf0x + #drivers/ll40ls + #drivers/trone + drivers/gps + #drivers/pwm_out_sim + #drivers/hott + #drivers/hott/hott_telemetry + #drivers/hott/hott_sensors + drivers/blinkm + drivers/airspeed + drivers/ets_airspeed + drivers/meas_airspeed + drivers/frsky_telemetry + modules/sensors + #drivers/mkblctrl + drivers/px4flow + #drivers/oreoled + drivers/gimbal + drivers/pwm_input + drivers/camera_trigger + #drivers/bst + #drivers/snapdragon_rc_pwm + #drivers/lis3mdl + + # + # System commands + # + systemcmds/bl_update + systemcmds/config + systemcmds/dumpfile + #systemcmds/esc_calib + systemcmds/mixer + systemcmds/motor_ramp + systemcmds/mtd + systemcmds/nshterm + systemcmds/param + systemcmds/perf + systemcmds/pwm + systemcmds/reboot + #systemcmds/sd_bench + systemcmds/top + #systemcmds/topic_listener + systemcmds/ver + + # + # Testing + # + drivers/sf0x/sf0x_tests + drivers/test_ppm + #lib/rc/rc_tests + modules/commander/commander_tests + modules/controllib_test + modules/mavlink/mavlink_tests + modules/unit_test + modules/uORB/uORB_tests + systemcmds/tests + + # + # General system control + # + modules/commander + modules/load_mon + modules/navigator + modules/mavlink + #modules/gpio_led + modules/uavcan + modules/land_detector + + # + # Estimation modules (EKF/ SO3 / other filters) + # + modules/attitude_estimator_q + #modules/ekf_att_pos_estimator + modules/position_estimator_inav + modules/local_position_estimator + modules/ekf2 + + # + # Vehicle Control + # + modules/fw_pos_control_l1 + modules/fw_att_control + modules/mc_att_control + modules/mc_pos_control + modules/vtol_att_control + + # + # Logging + # + #modules/logger + #modules/sdlog2 + + # + # Library modules + # + modules/param + modules/systemlib + modules/systemlib/mixer + modules/uORB + modules/dataman + + # + # Libraries + # + lib/controllib + lib/mathlib + lib/mathlib/math/filter + lib/ecl + lib/external_lgpl + lib/geo + lib/geo_lookup + lib/conversion + lib/launchdetection + lib/terrain_estimation + lib/runway_takeoff + lib/tailsitter_recovery + lib/DriverFramework/framework + platforms/nuttx + + # had to add for cmake, not sure why wasn't in original config + platforms/common + platforms/nuttx/px4_layer + + # + # OBC challenge + # + #modules/bottle_drop + + # + # Rover apps + # + #examples/rover_steering_control + + # + # Demo apps + # + #examples/math_demo + # Tutorial code from + # https://px4.io/dev/px4_simple_app + #examples/px4_simple_app + + # Tutorial code from + # https://px4.io/dev/daemon + #examples/px4_daemon_app + + # Tutorial code from + # https://px4.io/dev/debug_values + #examples/px4_mavlink_debug + + # Tutorial code from + # https://px4.io/dev/example_fixedwing_control + #examples/fixedwing_control + + # Hardware test + #examples/hwtest +) + +set(config_extra_builtin_cmds + serdis + sercon + ) + +set(config_io_board + px4io-v2 + ) + +set(config_extra_libs + uavcan + uavcan_stm32_driver + ) + +set(config_io_extra_libs + ) + +add_custom_target(sercon) +set_target_properties(sercon PROPERTIES + PRIORITY "SCHED_PRIORITY_DEFAULT" + MAIN "sercon" STACK_MAIN "2048") + +add_custom_target(serdis) +set_target_properties(serdis PROPERTIES + PRIORITY "SCHED_PRIORITY_DEFAULT" + MAIN "serdis" STACK_MAIN "2048") diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake index 0db31f82d6..866e19dc0d 100644 --- a/cmake/configs/nuttx_px4fmu-v4_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake @@ -23,6 +23,7 @@ set(config_module_list drivers/mb12xx drivers/srf02 drivers/sf0x + drivers/sf1xx drivers/ll40ls drivers/trone drivers/gps @@ -45,6 +46,10 @@ set(config_module_list drivers/bst drivers/snapdragon_rc_pwm drivers/lis3mdl + drivers/bmp280 + drivers/bma180 + drivers/bmi160 + drivers/tap_esc # # System commands @@ -63,12 +68,26 @@ set(config_module_list systemcmds/mtd systemcmds/dumpfile systemcmds/ver + systemcmds/sd_bench + systemcmds/motor_ramp + + # + # Testing + # + drivers/sf0x/sf0x_tests + drivers/test_ppm + modules/commander/commander_tests + modules/controllib_test + modules/mavlink/mavlink_tests + modules/unit_test + modules/uORB/uORB_tests systemcmds/tests # # General system control # modules/commander + modules/load_mon modules/navigator modules/mavlink modules/gpio_led @@ -78,12 +97,11 @@ set(config_module_list # # Estimation modules (EKF/ SO3 / other filters) # - # Too high RAM usage due to static allocations - # modules/attitude_estimator_ekf modules/attitude_estimator_q modules/ekf_att_pos_estimator modules/position_estimator_inav modules/ekf2 + modules/local_position_estimator # # Vehicle Control @@ -99,6 +117,7 @@ set(config_module_list # Logging # modules/sdlog2 + modules/logger # # Library modules diff --git a/cmake/configs/nuttx_tap-v1_default.cmake b/cmake/configs/nuttx_tap-v1_default.cmake new file mode 100644 index 0000000000..2dad824901 --- /dev/null +++ b/cmake/configs/nuttx_tap-v1_default.cmake @@ -0,0 +1,129 @@ +include(nuttx/px4_impl_nuttx) + +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake) + +set(config_module_list + # + # Board support modules + # + drivers/device + drivers/stm32 + drivers/stm32/adc + drivers/stm32/tone_alarm + drivers/led + drivers/px4fmu + drivers/boards/tap-v1 + drivers/rgbled_pwm + drivers/tap_esc + #drivers/mpu6500 + drivers/ms5611 + drivers/hmc5883 + drivers/gps + drivers/airspeed + drivers/meas_airspeed + modules/sensors + drivers/camera_trigger + + # + # System commands + # + systemcmds/bl_update + systemcmds/mixer + systemcmds/param + systemcmds/perf + systemcmds/pwm + systemcmds/motor_test + systemcmds/reboot + systemcmds/top + systemcmds/config + systemcmds/nshterm + systemcmds/mtd + systemcmds/dumpfile + systemcmds/ver + systemcmds/topic_listener + + # + # General system control + # + modules/commander + modules/load_mon + modules/navigator + modules/mavlink + modules/land_detector + + # + # Estimation modules (EKF/ SO3 / other filters) + # + modules/ekf2 + + # + # Vehicle Control + # + modules/fw_pos_control_l1 + modules/fw_att_control + modules/mc_att_control + modules/mc_pos_control + modules/vtol_att_control + + # + # Logging + # + modules/logger + + # + # Library modules + # + modules/param + modules/systemlib + modules/systemlib/mixer + modules/uORB + modules/dataman + + # + # Libraries + # + lib/controllib + lib/mathlib + lib/mathlib/math/filter + lib/ecl + lib/external_lgpl + lib/geo + lib/geo_lookup + lib/conversion + lib/launchdetection + lib/terrain_estimation + lib/runway_takeoff + lib/tailsitter_recovery + lib/DriverFramework/framework + platforms/nuttx + + # had to add for cmake, not sure why wasn't in original config + platforms/common + platforms/nuttx/px4_layer +) + +set(config_extra_builtin_cmds + serdis + sercon + ) + +set(config_io_board + ) + +set(config_extra_libs + ) + +set(config_io_extra_libs + ) + +add_custom_target(sercon) +set_target_properties(sercon PROPERTIES + PRIORITY "SCHED_PRIORITY_DEFAULT" + MAIN "sercon" + STACK_MAIN "2048") + +add_custom_target(serdis) +set_target_properties(serdis PROPERTIES + PRIORITY "SCHED_PRIORITY_DEFAULT" + MAIN "serdis" + STACK_MAIN "2048") diff --git a/cmake/configs/posix_rpi2_release.cmake b/cmake/configs/posix_bebop_default.cmake similarity index 60% rename from cmake/configs/posix_rpi2_release.cmake rename to cmake/configs/posix_bebop_default.cmake index e42a1b8fd3..5538ffe35a 100644 --- a/cmake/configs/posix_rpi2_release.cmake +++ b/cmake/configs/posix_bebop_default.cmake @@ -1,57 +1,106 @@ include(posix/px4_impl_posix) -if ("${RPI_TOOLCHAIN_DIR}" STREQUAL "") - set(RPI_TOOLCHAIN_DIR /opt/rpi_toolchain) -endif() +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf-raspbian.cmake) -set(CMAKE_PROGRAM_PATH +add_definitions( + -D__PX4_POSIX_BEBOP + -D__LINUX + -D__BEBOP + ) + +set(CMAKE_PROGRAM_PATH "${RPI_TOOLCHAIN_DIR}/gcc-linaro-arm-linux-gnueabihf-raspbian/bin" ${CMAKE_PROGRAM_PATH} ) -set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake) - set(config_module_list + + # examples/px4_simple_app + + # + # Board support modules + # drivers/device - platforms/common - platforms/posix/px4_layer - platforms/posix/work_queue + modules/sensors + platforms/posix/drivers/df_ms5607_wrapper + platforms/posix/drivers/df_mpu6050_wrapper + platforms/posix/drivers/df_ak8963_wrapper + + # + # System commands + # systemcmds/param systemcmds/mixer systemcmds/ver systemcmds/esc_calib - systemcmds/reboot systemcmds/topic_listener systemcmds/perf - modules/uORB + + # + # Estimation modules (EKF/ SO3 / other filters) + # + #modules/attitude_estimator_ekf + modules/ekf_att_pos_estimator + modules/attitude_estimator_q + modules/position_estimator_inav + modules/local_position_estimator + modules/ekf2 + + # + # Vehicle Control + # + modules/mc_att_control + modules/mc_pos_control + modules/fw_att_control + modules/fw_pos_control_l1 + modules/vtol_att_control + + # + # Library modules + # + modules/sdlog2 + modules/logger + modules/commander modules/param modules/systemlib modules/systemlib/mixer - modules/sensors - modules/mavlink - modules/attitude_estimator_q - modules/position_estimator_inav - modules/navigator - modules/vtol_att_control - modules/mc_pos_control - modules/mc_att_control - modules/land_detector - modules/fw_att_control - modules/fw_pos_control_l1 + modules/uORB modules/dataman - modules/sdlog2 - modules/commander + modules/land_detector + modules/navigator + modules/mavlink + + # + # PX4 drivers + # + + # + # Libraries + # lib/controllib lib/mathlib lib/mathlib/math/filter - lib/conversion - lib/ecl - lib/external_lgpl lib/geo + lib/ecl lib/geo_lookup lib/launchdetection + lib/external_lgpl + lib/conversion lib/terrain_estimation lib/runway_takeoff lib/tailsitter_recovery lib/DriverFramework/framework + + # + # POSIX + # + platforms/common + platforms/posix/px4_layer + platforms/posix/work_queue +) + +set(config_df_driver_list + ms5607 + mpu6050 + ak8963 ) diff --git a/cmake/configs/posix_eagle_hil.cmake b/cmake/configs/posix_eagle_hil.cmake index 252de9718c..909bdeecb1 100644 --- a/cmake/configs/posix_eagle_hil.cmake +++ b/cmake/configs/posix_eagle_hil.cmake @@ -27,6 +27,7 @@ set(config_module_list modules/sensors modules/dataman modules/sdlog2 + modules/logger modules/simulator modules/commander diff --git a/cmake/configs/posix_eagle_legacy_driver_default.cmake b/cmake/configs/posix_eagle_legacy_driver_default.cmake index 8b82f52613..ec172e7602 100644 --- a/cmake/configs/posix_eagle_legacy_driver_default.cmake +++ b/cmake/configs/posix_eagle_legacy_driver_default.cmake @@ -1,6 +1,6 @@ include(posix/px4_impl_posix) -set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake) +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-arm-linux-gnueabihf.cmake) set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon") @@ -12,7 +12,7 @@ set(CONFIG_SHMEM "1") # or if it is for the Snapdragon. add_definitions( -D__PX4_POSIX_EAGLE - -D__USING_SNAPDRAGON_LEGACY_DRIVER + -D__USING_SNAPDRAGON_LEGACY_DRIVER ) set(config_module_list @@ -45,8 +45,10 @@ set(config_module_list modules/sensors modules/dataman modules/sdlog2 + modules/logger modules/simulator modules/commander + modules/navigator lib/controllib lib/mathlib diff --git a/cmake/configs/posix_rpi2_default.cmake b/cmake/configs/posix_rpi2_default.cmake deleted file mode 100644 index 071466b224..0000000000 --- a/cmake/configs/posix_rpi2_default.cmake +++ /dev/null @@ -1,48 +0,0 @@ -include(posix/px4_impl_posix) - -set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake) - -set(config_module_list - drivers/device - platforms/common - platforms/posix/px4_layer - platforms/posix/work_queue - systemcmds/param - systemcmds/mixer - systemcmds/ver - systemcmds/esc_calib - systemcmds/reboot - systemcmds/topic_listener - systemcmds/perf - modules/uORB - modules/param - modules/systemlib - modules/systemlib/mixer - modules/sensors - modules/mavlink - modules/attitude_estimator_q - modules/position_estimator_inav - modules/navigator - modules/vtol_att_control - modules/mc_pos_control - modules/mc_att_control - modules/land_detector - modules/fw_att_control - modules/fw_pos_control_l1 - modules/dataman - modules/sdlog2 - modules/commander - lib/controllib - lib/mathlib - lib/mathlib/math/filter - lib/conversion - lib/ecl - lib/external_lgpl - lib/geo - lib/geo_lookup - lib/launchdetection - lib/terrain_estimation - lib/runway_takeoff - lib/tailsitter_recovery - lib/DriverFramework/framework -) diff --git a/cmake/configs/posix_rpi_common.cmake b/cmake/configs/posix_rpi_common.cmake new file mode 100644 index 0000000000..b7f1dd0aa5 --- /dev/null +++ b/cmake/configs/posix_rpi_common.cmake @@ -0,0 +1,114 @@ +# This file is shared between posix_rpi_native.cmake +# and posix_rpi_cross.cmake. + +include(posix/px4_impl_posix) + +# This definition allows to differentiate if this just the usual POSIX build +# or if it is for the RPi. +add_definitions( + -D__PX4_POSIX_RPI + -D__LINUX +) + + +set(config_module_list + # + # Board support modules + # + drivers/device + modules/sensors + platforms/posix/drivers/df_mpu9250_wrapper + platforms/posix/drivers/df_lsm9ds1_wrapper + platforms/posix/drivers/df_ms5611_wrapper + platforms/posix/drivers/df_hmc5883_wrapper + platforms/posix/drivers/df_trone_wrapper + platforms/posix/drivers/df_isl29501_wrapper + + # + # System commands + # + systemcmds/param + systemcmds/mixer + systemcmds/ver + systemcmds/esc_calib + systemcmds/reboot + systemcmds/topic_listener + systemcmds/perf + + # + # Estimation modules (EKF/ SO3 / other filters) + # + #modules/attitude_estimator_ekf + modules/ekf_att_pos_estimator + modules/attitude_estimator_q + modules/position_estimator_inav + modules/local_position_estimator + modules/ekf2 + + # + # Vehicle Control + # + modules/mc_att_control + modules/mc_pos_control + modules/fw_att_control + modules/fw_pos_control_l1 + modules/vtol_att_control + + # + # Library modules + # + modules/sdlog2 + modules/logger + modules/commander + modules/param + modules/systemlib + modules/systemlib/mixer + modules/uORB + modules/dataman + modules/land_detector + modules/navigator + modules/mavlink + + # + # PX4 drivers + # + drivers/gps + drivers/navio_sysfs_rc_in + drivers/navio_sysfs_pwm_out + + # + # Libraries + # + lib/controllib + lib/mathlib + lib/mathlib/math/filter + lib/geo + lib/ecl + lib/geo_lookup + lib/launchdetection + lib/external_lgpl + lib/conversion + lib/terrain_estimation + lib/runway_takeoff + lib/tailsitter_recovery + lib/DriverFramework/framework + + # + # POSIX + # + platforms/common + platforms/posix/px4_layer + platforms/posix/work_queue +) + +# +# DriverFramework driver +# +set(config_df_driver_list + mpu9250 + lsm9ds1 + ms5611 + hmc5883 + trone + isl29501 +) diff --git a/cmake/configs/posix_rpi_cross.cmake b/cmake/configs/posix_rpi_cross.cmake new file mode 100644 index 0000000000..46243fc361 --- /dev/null +++ b/cmake/configs/posix_rpi_cross.cmake @@ -0,0 +1,8 @@ +include(configs/posix_rpi_common) + +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf-raspbian.cmake) + +set(CMAKE_PROGRAM_PATH + "${RPI_TOOLCHAIN_DIR}/gcc-linaro-arm-linux-gnueabihf-raspbian/bin" + ${CMAKE_PROGRAM_PATH} +) diff --git a/cmake/configs/posix_rpi_native.cmake b/cmake/configs/posix_rpi_native.cmake new file mode 100644 index 0000000000..e58f370282 --- /dev/null +++ b/cmake/configs/posix_rpi_native.cmake @@ -0,0 +1,3 @@ +include(configs/posix_rpi_common) + +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake) diff --git a/cmake/configs/posix_sdflight_default.cmake b/cmake/configs/posix_sdflight_default.cmake index da51797a59..c43e51974c 100644 --- a/cmake/configs/posix_sdflight_default.cmake +++ b/cmake/configs/posix_sdflight_default.cmake @@ -38,8 +38,10 @@ set(config_module_list modules/sensors modules/dataman modules/sdlog2 + modules/logger modules/simulator modules/commander + modules/navigator lib/controllib lib/mathlib diff --git a/cmake/configs/posix_sitl_broadcast.cmake b/cmake/configs/posix_sitl_broadcast.cmake index 2e92a2f08d..9a9029f1c0 100644 --- a/cmake/configs/posix_sitl_broadcast.cmake +++ b/cmake/configs/posix_sitl_broadcast.cmake @@ -49,6 +49,7 @@ set(config_module_list modules/fw_pos_control_l1 modules/dataman modules/sdlog2 + modules/logger modules/commander lib/controllib lib/mathlib diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake index 2e92a2f08d..6fa6d9c0b7 100644 --- a/cmake/configs/posix_sitl_default.cmake +++ b/cmake/configs/posix_sitl_default.cmake @@ -3,67 +3,92 @@ include(posix/px4_impl_posix) set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake) set(config_module_list - drivers/device drivers/boards/sitl - drivers/pwm_out_sim - platforms/common - platforms/posix/px4_layer - platforms/posix/work_queue - platforms/posix/drivers/adcsim - platforms/posix/drivers/gpssim + drivers/device drivers/gps - platforms/posix/drivers/tonealrmsim + drivers/pwm_out_sim + + platforms/common platforms/posix/drivers/accelsim + platforms/posix/drivers/adcsim platforms/posix/drivers/airspeedsim platforms/posix/drivers/barosim + platforms/posix/drivers/gpssim platforms/posix/drivers/gyrosim - platforms/posix/drivers/rgbledsim platforms/posix/drivers/ledsim - systemcmds/param - systemcmds/mixer - systemcmds/ver + platforms/posix/drivers/rgbledsim + platforms/posix/drivers/tonealrmsim + platforms/posix/px4_layer + platforms/posix/work_queue + systemcmds/esc_calib - systemcmds/reboot - systemcmds/topic_listener + systemcmds/mixer + systemcmds/param systemcmds/perf - modules/uORB - modules/param - modules/systemlib - modules/systemlib/mixer - modules/sensors - modules/simulator - modules/mavlink + systemcmds/reboot + systemcmds/sd_bench + systemcmds/topic_listener + systemcmds/ver + systemcmds/top + systemcmds/motor_ramp + modules/attitude_estimator_ekf modules/attitude_estimator_q + modules/commander + modules/dataman modules/ekf2 modules/ekf_att_pos_estimator - modules/position_estimator_inav - modules/navigator - modules/vtol_att_control - modules/mc_pos_control - modules/mc_att_control - modules/mc_pos_control_multiplatform - modules/mc_att_control_multiplatform - modules/land_detector modules/fw_att_control modules/fw_pos_control_l1 - modules/dataman + modules/land_detector + modules/logger + modules/mavlink + modules/mc_att_control + modules/mc_att_control_multiplatform + modules/mc_pos_control + modules/mc_pos_control_multiplatform + modules/navigator + modules/param + modules/position_estimator_inav + modules/local_position_estimator + modules/replay modules/sdlog2 - modules/commander + modules/sensors + modules/simulator + modules/systemlib + modules/systemlib/mixer + modules/uORB + modules/vtol_att_control + lib/controllib - lib/mathlib - lib/mathlib/math/filter lib/conversion + lib/DriverFramework/framework lib/ecl lib/external_lgpl lib/geo lib/geo_lookup lib/launchdetection - lib/terrain_estimation + lib/mathlib + lib/mathlib/math/filter + lib/rc lib/runway_takeoff lib/tailsitter_recovery - lib/DriverFramework/framework + lib/terrain_estimation + examples/px4_simple_app + + # + # Testing + # + drivers/sf0x/sf0x_tests + lib/rc/rc_tests + modules/commander/commander_tests + modules/controllib_test + #modules/mavlink/mavlink_tests #TODO: fix mavlink_tests + modules/unit_test + modules/uORB/uORB_tests + systemcmds/tests + ) set(config_extra_builtin_cmds @@ -89,3 +114,12 @@ set(config_sitl_debugger ) set_property(CACHE config_sitl_debugger PROPERTY STRINGS "disable;gdb;lldb") + +# If the environment variable 'replay' is defined, we are building with replay +# support. In this case, we enable the orb publisher rules. +set(REPLAY_FILE "$ENV{replay}") +if(REPLAY_FILE) + message("Building with uorb publisher rules support") + add_definitions(-DORB_USE_PUBLISHER_RULES) +endif() + diff --git a/cmake/configs/posix_sitl_ekf2.cmake b/cmake/configs/posix_sitl_ekf2.cmake deleted file mode 100644 index 5cb53b3c22..0000000000 --- a/cmake/configs/posix_sitl_ekf2.cmake +++ /dev/null @@ -1,90 +0,0 @@ -include(posix/px4_impl_posix) - -set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake) - -set(config_module_list - drivers/device - drivers/boards/sitl - drivers/pwm_out_sim - platforms/common - platforms/posix/px4_layer - platforms/posix/work_queue - platforms/posix/drivers/adcsim - platforms/posix/drivers/gpssim - platforms/posix/drivers/tonealrmsim - platforms/posix/drivers/accelsim - platforms/posix/drivers/airspeedsim - platforms/posix/drivers/barosim - platforms/posix/drivers/gyrosim - platforms/posix/drivers/rgbledsim - platforms/posix/drivers/ledsim - systemcmds/param - systemcmds/mixer - systemcmds/ver - systemcmds/esc_calib - systemcmds/reboot - systemcmds/topic_listener - systemcmds/perf - modules/uORB - modules/param - modules/systemlib - modules/systemlib/mixer - modules/sensors - modules/simulator - modules/mavlink - modules/attitude_estimator_ekf - modules/attitude_estimator_q - modules/ekf_att_pos_estimator - modules/ekf2 - modules/position_estimator_inav - modules/navigator - modules/vtol_att_control - modules/mc_pos_control - modules/mc_att_control - modules/mc_pos_control_multiplatform - modules/mc_att_control_multiplatform - modules/land_detector - modules/fw_att_control - modules/fw_pos_control_l1 - modules/dataman - modules/sdlog2 - modules/commander - lib/controllib - lib/mathlib - lib/mathlib/math/filter - lib/conversion - lib/ecl - lib/external_lgpl - lib/geo - lib/geo_lookup - lib/launchdetection - lib/terrain_estimation - lib/runway_takeoff - lib/tailsitter_recovery - lib/DriverFramework/framework - examples/px4_simple_app - ) - -set(config_extra_builtin_cmds - serdis - sercon - ) - -set(config_sitl_rcS - posix-configs/SITL/init/rcS_ekf2 - CACHE FILEPATH "init script for sitl" - ) - -set(config_sitl_viewer - jmavsim - CACHE STRING "viewer for sitl" - ) -set_property(CACHE config_sitl_viewer - PROPERTY STRINGS "jmavsim;none") - -set(config_sitl_debugger - disable - CACHE STRING "debugger for sitl" - ) -set_property(CACHE config_sitl_debugger - PROPERTY STRINGS "disable;gdb;lldb") diff --git a/cmake/configs/posix_sitl_lpe.cmake b/cmake/configs/posix_sitl_lpe.cmake index c264466f9d..c098b72bf0 100644 --- a/cmake/configs/posix_sitl_lpe.cmake +++ b/cmake/configs/posix_sitl_lpe.cmake @@ -1,9 +1,5 @@ include(cmake/configs/posix_sitl_default.cmake) -list(APPEND config_module_list - modules/local_position_estimator - ) - set(config_sitl_rcS posix-configs/SITL/init/rcS_lpe ) diff --git a/cmake/configs/posix_sitl_replay.cmake b/cmake/configs/posix_sitl_replay.cmake index 959e389bdb..3c99cba7a7 100644 --- a/cmake/configs/posix_sitl_replay.cmake +++ b/cmake/configs/posix_sitl_replay.cmake @@ -17,6 +17,7 @@ set(config_module_list modules/ekf2 modules/ekf2_replay modules/sdlog2 + modules/logger lib/controllib lib/mathlib lib/mathlib/math/filter diff --git a/cmake/configs/qurt_eagle_legacy_driver_default.cmake b/cmake/configs/qurt_eagle_legacy_driver_default.cmake index 223f3b8a4f..f0fdb2dee5 100644 --- a/cmake/configs/qurt_eagle_legacy_driver_default.cmake +++ b/cmake/configs/qurt_eagle_legacy_driver_default.cmake @@ -16,6 +16,8 @@ set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexa add_definitions( -D__USING_SNAPDRAGON_LEGACY_DRIVER + -D__PX4_QURT + -D__PX4_QURT_EAGLE ) set(config_module_list @@ -26,8 +28,6 @@ set(config_module_list modules/sensors platforms/posix/drivers/df_mpu9250_wrapper platforms/posix/drivers/df_bmp280_wrapper - platforms/posix/drivers/df_hmc5883_wrapper - platforms/posix/drivers/df_trone_wrapper # # System commands @@ -37,7 +37,6 @@ set(config_module_list # # Estimation modules (EKF/ SO3 / other filters) # - #modules/attitude_estimator_ekf modules/ekf_att_pos_estimator modules/attitude_estimator_q modules/position_estimator_inav @@ -63,9 +62,15 @@ set(config_module_list # PX4 drivers # drivers/gps - drivers/uart_esc + drivers/pwm_out_rc_in drivers/qshell/qurt + # + # FC_ADDON drivers + # + platforms/qurt/fc_addon/rc_receiver + platforms/qurt/fc_addon/uart_esc + # # Libraries # @@ -97,6 +102,4 @@ set(config_module_list set(config_df_driver_list mpu9250 bmp280 - hmc5883 - trone ) diff --git a/cmake/configs/qurt_eagle_travis.cmake b/cmake/configs/qurt_eagle_travis.cmake index 01b87f6bb2..16aa788edc 100644 --- a/cmake/configs/qurt_eagle_travis.cmake +++ b/cmake/configs/qurt_eagle_travis.cmake @@ -8,6 +8,7 @@ set(QURT_ENABLE_STUBS "1") set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake) set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon") +include(hexagon_sdk) if ("$ENV{HEXAGON_SDK_ROOT}" STREQUAL "") message(FATAL_ERROR "Enviroment variable HEXAGON_SDK_ROOT must be set") @@ -15,7 +16,7 @@ else() set(HEXAGON_SDK_ROOT $ENV{HEXAGON_SDK_ROOT}) endif() -include_directories(${HEXAGON_SDK_ROOT}/lib/common/qurt/ADSPv5MP/include) +include_directories(${HEXAGON_8074_INCLUDES}) set(config_generate_parameters_scope ALL) diff --git a/cmake/configs/qurt_sdflight_default.cmake b/cmake/configs/qurt_sdflight_default.cmake index 3ea8ad37b4..195219f7af 100644 --- a/cmake/configs/qurt_sdflight_default.cmake +++ b/cmake/configs/qurt_sdflight_default.cmake @@ -60,7 +60,7 @@ set(config_module_list # PX4 drivers # drivers/gps - drivers/uart_esc + drivers/pwm_out_rc_in drivers/qshell/qurt # diff --git a/cmake/nuttx/px4_impl_nuttx.cmake b/cmake/nuttx/px4_impl_nuttx.cmake index 51e8e5e6a9..c969263be1 100644 --- a/cmake/nuttx/px4_impl_nuttx.cmake +++ b/cmake/nuttx/px4_impl_nuttx.cmake @@ -202,21 +202,32 @@ function(px4_nuttx_add_export) add_dependencies(nuttx_patch nuttx_patch_${patch_name}) endforeach() + # Read defconfig to see if CONFIG_ARMV7M_STACKCHECK is yes + # note: CONFIG will be BOARD in the future evaluation of ${hw_stack_check_${CONFIG} + file(STRINGS "${CMAKE_SOURCE_DIR}/nuttx-configs/${CONFIG}/nsh/defconfig" + hw_stack_check_${CONFIG} + REGEX "CONFIG_ARMV7M_STACKCHECK=y" + ) + if ("${hw_stack_check_${CONFIG}}" STREQUAL "CONFIG_ARMV7M_STACKCHECK=y") + set(config_nuttx_hw_stack_check_${CONFIG} y CACHE INTERNAL "" FORCE) + endif() + # copy and export + file(RELATIVE_PATH nuttx_cp_src ${CMAKE_BINARY_DIR} ${CMAKE_SOURCE_DIR}/NuttX) file(GLOB_RECURSE config_files ${CMAKE_SOURCE_DIR}/nuttx-configs/${CONFIG}/*) add_custom_command(OUTPUT ${CMAKE_BINARY_DIR}/${CONFIG}.export COMMAND ${MKDIR} -p ${nuttx_src} - COMMAND ${CP} -a ${CMAKE_SOURCE_DIR}/NuttX/. ${nuttx_src}/ - COMMAND ${RM} -rf ${nuttx_src}/.git + COMMAND rsync -a --delete --exclude=.git ${nuttx_cp_src}/ ${CONFIG}/NuttX/ #COMMAND ${ECHO} Configuring NuttX for ${CONFIG} COMMAND ${MAKE} --no-print-directory -C${nuttx_src}/nuttx -r --quiet distclean COMMAND ${CP} -r ${CMAKE_SOURCE_DIR}/nuttx-configs/PX4_Warnings.mk ${nuttx_src}/nuttx/ COMMAND ${CP} -r ${CMAKE_SOURCE_DIR}/nuttx-configs/${CONFIG} ${nuttx_src}/nuttx/configs - COMMAND cd ${nuttx_src}/nuttx/tools && ./configure.sh ${CONFIG}/nsh + COMMAND cd ${nuttx_src}/nuttx/tools && ./configure.sh ${CONFIG}/nsh && cd .. #COMMAND ${ECHO} Exporting NuttX for ${CONFIG} COMMAND ${MAKE} --no-print-directory --quiet -C ${nuttx_src}/nuttx -j${THREADS} -r CONFIG_ARCH_BOARD=${CONFIG} export > nuttx_build.log COMMAND ${CP} -r ${nuttx_src}/nuttx/nuttx-export.zip ${CMAKE_BINARY_DIR}/${CONFIG}.export DEPENDS ${config_files} ${DEPENDS} + WORKING_DIRECTORY ${CMAKE_BINARY_DIR} COMMENT "Building NuttX for ${CONFIG}") # extract @@ -428,56 +439,19 @@ function(px4_os_add_flags) set(added_exe_linker_flags) # none currently + set(instrument_flags) + if ("${config_nuttx_hw_stack_check_${BOARD}}" STREQUAL "y") + set(instrument_flags + -finstrument-functions + -ffixed-r10 + ) + list(APPEND c_flags ${instrument_flags}) + list(APPEND cxx_flags ${instrument_flags}) + endif() + set(cpu_flags) - if (${BOARD} STREQUAL "px4fmu-v1") - set(cpu_flags - -mcpu=cortex-m4 - -mthumb - -march=armv7e-m - -mfpu=fpv4-sp-d16 - -mfloat-abi=hard - ) - elseif (${BOARD} STREQUAL "px4fmu-v2") - set(cpu_flags - -mcpu=cortex-m4 - -mthumb - -march=armv7e-m - -mfpu=fpv4-sp-d16 - -mfloat-abi=hard - ) - elseif (${BOARD} STREQUAL "px4fmu-v4") - set(cpu_flags - -mcpu=cortex-m4 - -mthumb - -march=armv7e-m - -mfpu=fpv4-sp-d16 - -mfloat-abi=hard - ) - elseif (${BOARD} STREQUAL "px4-stm32f4discovery") - set(cpu_flags - -mcpu=cortex-m4 - -mthumb - -march=armv7e-m - -mfpu=fpv4-sp-d16 - -mfloat-abi=hard - ) - elseif (${BOARD} STREQUAL "aerocore") - set(cpu_flags - -mcpu=cortex-m4 - -mthumb - -march=armv7e-m - -mfpu=fpv4-sp-d16 - -mfloat-abi=hard - ) - elseif (${BOARD} STREQUAL "mindpx-v2") - set(cpu_flags - -mcpu=cortex-m4 - -mthumb - -march=armv7e-m - -mfpu=fpv4-sp-d16 - -mfloat-abi=hard - ) - elseif (${BOARD} STREQUAL "px4io-v1") + # Handle non-F4 boards specifically here + if (${BOARD} STREQUAL "px4io-v1") set(cpu_flags -mcpu=cortex-m3 -mthumb @@ -489,6 +463,14 @@ function(px4_os_add_flags) -mthumb -march=armv7-m ) + else () + set(cpu_flags + -mcpu=cortex-m4 + -mthumb + -march=armv7e-m + -mfpu=fpv4-sp-d16 + -mfloat-abi=hard + ) endif() list(APPEND c_flags ${cpu_flags}) list(APPEND cxx_flags ${cpu_flags}) diff --git a/cmake/posix/px4_impl_posix.cmake b/cmake/posix/px4_impl_posix.cmake index 51afab20de..d661bfa5eb 100644 --- a/cmake/posix/px4_impl_posix.cmake +++ b/cmake/posix/px4_impl_posix.cmake @@ -166,6 +166,15 @@ function(px4_os_add_flags) mavlink/include/mavlink ) +# Use the pthread instead of lpthread if the firmware is build for the parrot +# bebop. This resolves some linker errors in DriverFramework, when building a +# static target. +if ("${BOARD}" STREQUAL "bebop") + set(PX4_PTHREAD_BUILD "-pthread") +else() + set(PX4_PTHREAD_BUILD "-lpthread") +endif() + if(UNIX AND APPLE) set(added_definitions -D__PX4_POSIX @@ -177,7 +186,7 @@ if(UNIX AND APPLE) ) set(added_exe_linker_flags - -lpthread + ${PX4_PTHREAD_BUILD} ) else() @@ -192,7 +201,7 @@ else() ) set(added_exe_linker_flags - -lpthread -lrt + ${PX4_PTHREAD_BUILD} -lrt ) endif() diff --git a/cmake/ros-CMakeLists.txt b/cmake/ros-CMakeLists.txt index a6df11a3f6..67870b1e2c 100644 --- a/cmake/ros-CMakeLists.txt +++ b/cmake/ros-CMakeLists.txt @@ -59,30 +59,29 @@ find_package(Eigen REQUIRED) ## 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 + actuator_controls.msg commander_state.msg - vehicle_local_position.msg + control_state.msg + distance_sensor.msg + manual_control_setpoint.msg + mc_virtual_rates_setpoint.msg + offboard_control_mode.msg + parameter_update.msg position_setpoint.msg position_setpoint_triplet.msg - vehicle_local_position_setpoint.msg - vehicle_global_velocity_setpoint.msg - offboard_control_mode.msg + rc_channels.msg + ros/actuator_controls_0.msg + ros/actuator_controls_virtual_mc.msg + vehicle_attitude.msg + vehicle_attitude_setpoint.msg + vehicle_control_mode.msg vehicle_force_setpoint.msg - distance_sensor.msg - control_state.msg + vehicle_global_velocity_setpoint.msg + vehicle_local_position.msg + vehicle_local_position_setpoint.msg + vehicle_rates_setpoint.msg + vehicle_status.msg ) ## Generate services in the 'srv' folder @@ -148,8 +147,8 @@ set(MULTIPLATFORM_HEADER_DIR ${CMAKE_CURRENT_SOURCE_DIR}/src/platforms/ros/px4_m 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} +add_custom_target(multiplatform_message_headers ALL ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/Tools/px_generate_uorb_topic_files.py + --headers -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 diff --git a/cmake/scripts/convert_modules_to_cmake.py b/cmake/scripts/convert_modules_to_cmake.py deleted file mode 100755 index 7cd8768c65..0000000000 --- a/cmake/scripts/convert_modules_to_cmake.py +++ /dev/null @@ -1,91 +0,0 @@ -#!/usr/bin/env python - -from __future__ import print_function - -import argparse -import os -import sys -import fnmatch -import re -import shutil -import jinja2 - -src_path = os.path.join(os.path.curdir, 'src') - -parser = argparse.ArgumentParser('converts module.mk to CMakeList.txt, run in root of repo') -parser.add_argument('path', help='directory of modules to convert') -parser.add_argument('--overwrite', help='overwrite existing files', dest='overwrite', action='store_true') -parser.add_argument('--backup', help='create backup of existing files if overwriting', dest='backup', action='store_true') -parser.set_defaults(overwrite=False, backup=False) -args = parser.parse_args() - -cmake_template = jinja2.Template(open('cmake/scripts/cmake_lists.jinja', 'r').read()) - -module_files = [] -for root, dirnames, filenames in os.walk(args.path): - for filename in fnmatch.filter(filenames, 'module.mk'): - module_files.append(os.path.join(root, filename)) - - -search_data = [ - # name # re string - ('command', r'.*MODULE_COMMAND\s*[\+]?=\s*([^\n]+)'), - ('stacksize', r'.*MODULE_STACKSIZE\s*[\+]?=([^\n]+)'), - ('extracxxflags', r'.*EXTRACXXFLAGS\s*[\+]?=([^\n]+)'), - ('extracflags', r'.*EXTRACFLAGS\s*[\+]?=\s*([^\s]+)\s*'), - ('priority', r'.*MODULE_PRIORITY\s*[\+]?=\s*([^\s]+)\s*'), - ('maxoptimization', r'.*MAXOPTIMIZATION\s*[\+]?=\s*([^\s]+)\s*'), - ('srcs', '.*SRCS\s*[\+]?=([^\n\\\]*([\\\]\s*\n[^\n\\\]*)*)'), - ('include_dirs', '.*INCLUDE_DIRS\s*[\+]?=([^\n\\\]*([\\\]\s*\n[^\n\\\]*)*)'), - ] - -progs = {} -for name, re_str in search_data: - progs[name] = re.compile(re_str) - -for module_file in module_files: - - data = {} - with open(module_file, 'r') as f: - module_text = f.read() - data['text'] = module_text - module_dir = os.path.dirname(module_file) - data['module'] = os.path.relpath(module_dir, src_path).replace( - os.sep, '__').split('.')[0] - #print(module_text) - for name, re_str in search_data: - result = progs[name].search(module_text) - if result is not None: - d = result.group(1).strip() - if name in ['srcs', 'extracxxflags', 'extracflags']: - d_store = d.replace('\\', '').split() - elif name == 'include_dirs': - d_store = d.replace('(', '{').replace(')', '}').split() - else: - d_store = d - data[name] = d_store - else: - data[name] = '' - - cmake_file = os.path.join(os.path.dirname(module_file), 'CMakeLists.txt') - cmake_file_backup = cmake_file + '.backup' - - if os.path.exists(cmake_file): - if args.backup: - if os.path.exists(cmake_file_backup): - print('error: file already exists:', cmake_file_backup) - continue - else: - shutil.copyfile(cmake_file, cmake_file_backup) - if args.overwrite: - print('overwriting', cmake_file) - else: - print('error: file already exists:', cmake_file) - continue - - with open(cmake_file, 'w') as f: - data_rendered = cmake_template.render(data=data) - f.write(data_rendered) - - -# vim: set et fenc= ff=unix sts=4 sw=4 ts=4 : diff --git a/cmake/scripts/test_compare.py b/cmake/scripts/test_compare.py deleted file mode 100755 index fb2b4745a4..0000000000 --- a/cmake/scripts/test_compare.py +++ /dev/null @@ -1,50 +0,0 @@ -#!/usr/bin/env python -""" -This runs a command and compares output to a known file over -a given line range. -""" -from __future__ import print_function -import subprocess -import argparse -import os - - -#pylint: disable=invalid-name -parser = argparse.ArgumentParser(description='Process some integers.') -parser.add_argument('--command', required=True) -parser.add_argument('--stdin', required=True) -parser.add_argument('--stdout', required=True) -parser.add_argument('--check', required=True) -parser.add_argument('--start', default=0) -parser.add_argument('--stop', default=-1) -args = parser.parse_args() - -d = os.path.dirname(args.stdout) -if not os.path.exists(d): - os.makedirs(d) - -with open(args.stdout, 'w') as outfile: - with open(args.stdin, 'r') as infile: - proc = subprocess.Popen( - args.command, stdout=outfile, stdin=infile) -proc.communicate() - -i_start = int(args.start) -i_stop = int(args.stop) - -with open(args.stdout, 'r') as outfile: - out_contents = file.readlines(outfile) -out_contents = "".join(out_contents[i_start:i_stop]) - -with open(args.check, 'r') as checkfile: - check_contents = file.readlines(checkfile) -check_contents = "".join(check_contents[i_start:i_stop]) - -if (out_contents != check_contents): - print("output:\n", out_contents) - print("check:\n", check_contents) - exit(1) - -exit(0) - -# vim: set et ft=python fenc= ff=unix sts=4 sw=4 ts=4 : diff --git a/cmake/templates/build_git_version.h.in b/cmake/templates/build_git_version.h.in index 99b3551743..36e4e74e3e 100644 --- a/cmake/templates/build_git_version.h.in +++ b/cmake/templates/build_git_version.h.in @@ -1,5 +1,5 @@ /* Auto Magically Generated file */ /* Do not edit! */ -#define PX4_GIT_VERSION_STR "@git_desc@" -#define PX4_GIT_VERSION_BINARY 0x@git_desc_short@ +#define PX4_GIT_VERSION_STR "@git_version@" +#define PX4_GIT_VERSION_BINARY 0x@git_version_short@ #define PX4_GIT_TAG_STR "@git_tag@" diff --git a/cmake/test/px4_simple_app_correct.txt b/cmake/test/px4_simple_app_correct.txt index 317d4f5982..9504a8e27a 100644 --- a/cmake/test/px4_simple_app_correct.txt +++ b/cmake/test/px4_simple_app_correct.txt @@ -2,7 +2,7 @@ INFO Shell id is 47996278451456 WARN 1 starting task wkr_high (file /home/jgoppert/git/px4/cmake-Firmware/src/platforms/posix/px4_layer/px4_posix_tasks.cpp line 146) WARN 54 starting task wkr_low (file /home/jgoppert/git/px4/cmake-Firmware/src/platforms/posix/px4_layer/px4_posix_tasks.cpp line 146) WARN 100 starting task wkr_hrt (file /home/jgoppert/git/px4/cmake-Firmware/src/platforms/posix/px4_layer/px4_posix_tasks.cpp line 146) -App name: mainapp +App name: px4 Enter a command and its args: ---------------------------------- Running: uorb diff --git a/cmake/toolchains/Toolchain-arm-linux-gnueabihf-raspbian.cmake b/cmake/toolchains/Toolchain-arm-linux-gnueabihf-raspbian.cmake new file mode 100644 index 0000000000..4806222740 --- /dev/null +++ b/cmake/toolchains/Toolchain-arm-linux-gnueabihf-raspbian.cmake @@ -0,0 +1,89 @@ +# defines: +# +# NM +# OBJCOPY +# LD +# CXX_COMPILER +# C_COMPILER +# CMAKE_SYSTEM_NAME +# CMAKE_SYSTEM_VERSION +# LINKER_FLAGS +# CMAKE_EXE_LINKER_FLAGS +# CMAKE_FIND_ROOT_PATH +# CMAKE_FIND_ROOT_PATH_MODE_PROGRAM +# CMAKE_FIND_ROOT_PATH_MODE_LIBRARY +# CMAKE_FIND_ROOT_PATH_MODE_INCLUDE + +include(CMakeForceCompiler) + +if ("$ENV{RPI_TOOLCHAIN_DIR}" STREQUAL "") + message(FATAL_ERROR "RPI_TOOLCHAIN_DIR not set") +else() + set(RPI_TOOLCHAIN_DIR $ENV{RPI_TOOLCHAIN_DIR}) +endif() + +# this one is important +set(CMAKE_SYSTEM_NAME Generic) + +#this one not so much +set(CMAKE_SYSTEM_VERSION 1) + +# specify the cross compiler +find_program(C_COMPILER arm-linux-gnueabihf-gcc + PATHS ${RPI_TOOLCHAIN_DIR}/gcc-linaro-arm-linux-gnueabihf-raspbian/bin + NO_DEFAULT_PATH + ) + +if(NOT C_COMPILER) + message(FATAL_ERROR "could not find arm-linux-gnueabihf-gcc compiler") +endif() +cmake_force_c_compiler(${C_COMPILER} GNU) + +find_program(CXX_COMPILER arm-linux-gnueabihf-g++ + PATHS ${RPI_TOOLCHAIN_DIR}/gcc-linaro-arm-linux-gnueabihf-raspbian/bin + NO_DEFAULT_PATH + ) + +if(NOT CXX_COMPILER) + message(FATAL_ERROR "could not find arm-linux-gnueabihf-g++ compiler") +endif() +cmake_force_cxx_compiler(${CXX_COMPILER} GNU) + +# compiler tools +foreach(tool objcopy nm ld) + string(TOUPPER ${tool} TOOL) + find_program(${TOOL} arm-linux-gnueabihf-${tool} + PATHS ${RPI_TOOLCHAIN_DIR}/gcc-linaro-arm-linux-gnueabihf-raspbian/bin + NO_DEFAULT_PATH + ) + if(NOT ${TOOL}) + message(FATAL_ERROR "could not find arm-linux-gnueabihf-${tool}") + endif() +endforeach() + +# os tools +foreach(tool echo grep rm mkdir nm cp touch make unzip) + string(TOUPPER ${tool} TOOL) + find_program(${TOOL} ${tool}) + if(NOT ${TOOL}) + message(FATAL_ERROR "could not find ${TOOL}") + endif() +endforeach() + +add_definitions( + -D __RPI + ) + +set(LINKER_FLAGS "-Wl,-gc-sections") +set(CMAKE_EXE_LINKER_FLAGS ${LINKER_FLAGS}) +set(CMAKE_C_FLAGS ${C_FLAGS}) +set(CMAKE_CXX_LINKER_FLAGS ${C_FLAGS}) + +# where is the target environment +set(CMAKE_FIND_ROOT_PATH get_file_component(${C_COMPILER} PATH)) + +# search for programs in the build host directories +set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER) +# for libraries and headers in the target directories +set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) +set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) diff --git a/cmake/toolchains/Toolchain-native.cmake b/cmake/toolchains/Toolchain-native.cmake index 888c73a6d8..2899671043 100644 --- a/cmake/toolchains/Toolchain-native.cmake +++ b/cmake/toolchains/Toolchain-native.cmake @@ -8,7 +8,7 @@ foreach(tool nm ld) endforeach() # os tools -foreach(tool echo patch grep rm mkdir nm genromfs cp touch make unzip) +foreach(tool echo grep rm mkdir nm cp touch make unzip) string(TOUPPER ${tool} TOOL) find_program(${TOOL} ${tool}) if(NOT ${TOOL}) diff --git a/template_.cproject b/eclipse.cproject similarity index 98% rename from template_.cproject rename to eclipse.cproject index d6fb4019f1..c7bec6ea52 100644 --- a/template_.cproject +++ b/eclipse.cproject @@ -5,12 +5,12 @@ + - @@ -105,23 +105,32 @@ + + make + + px4fmu-v1_default + true + true + true + make + px4fmu-v2_default true true true - + make - - px4fmu-v2_default upload + px4fmu-v4_default true true true make + all true true @@ -129,54 +138,46 @@ make - posix_sitl_default true true true - + make - px4fmu-v1_default + check true true true make + clean true true true - make - distclean - true - true - true - - make - px4fmu-v4_default + distclean true true true make - tests true true true - + make - check - true + submodulesclean + false true true diff --git a/template_.project b/eclipse.project similarity index 100% rename from template_.project rename to eclipse.project diff --git a/Tools/CI/MissionCheck.py b/integrationtests/python_src/px4_it/dronekit/MissionCheck.py similarity index 97% rename from Tools/CI/MissionCheck.py rename to integrationtests/python_src/px4_it/dronekit/MissionCheck.py index a7421250da..444a0b1483 100755 --- a/Tools/CI/MissionCheck.py +++ b/integrationtests/python_src/px4_it/dronekit/MissionCheck.py @@ -17,9 +17,9 @@ connection_string = '127.0.0.1:14540' -import_mission_filename = 'VTOLmission.txt' -max_execution_time = 250 -alt_acceptance_radius = 10 +import_mission_filename = 'VTOL_TAKEOFF.mission' +max_execution_time = 200 +alt_acceptance_radius = 5 ################################################################################################ # Init @@ -150,14 +150,14 @@ def save_mission(aFileName): Save a mission in the Waypoint file format (http://qgroundcontrol.org/mavlink/waypoint_protocol#waypoint_file_format). """ - print "\nSave mission from Vehicle to file: %s" % export_mission_filename + print "\nSave mission from Vehicle to file: %s" % aFileName #Download mission from vehicle missionlist = download_mission() #Add file-format information output='QGC WPL 110\n' #Add home location as 0th waypoint home = vehicle.home_location - output+="%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\n" % (0,1,0,16,0,0,0,0,home.lat,home.lon,home.alt,1) + output+="%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\n\n\n" % (0,1,0,16,0,0,0,0,home.lat,home.lon,home.alt,1) #Add commands for cmd in missionlist: commandline="%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\n" % (cmd.seq,cmd.current,cmd.frame,cmd.command,cmd.param1,cmd.param2,cmd.param3,cmd.param4,cmd.x,cmd.y,cmd.z,cmd.autocontinue) diff --git a/integrationtests/python_src/px4_it/dronekit/VTOL_TAKEOFF.mission b/integrationtests/python_src/px4_it/dronekit/VTOL_TAKEOFF.mission new file mode 100644 index 0000000000..0c0d6592ba --- /dev/null +++ b/integrationtests/python_src/px4_it/dronekit/VTOL_TAKEOFF.mission @@ -0,0 +1,5 @@ +QGC WPL 110 +1 0 3 84 0.0 0.0 -0.0 0.0 47.3975105286 8.55026626587 10.0 1 +1 0 3 16 0.0 0.0 -0.0 0.0 47.395450592 8.55018424988 10.0 1 +2 0 3 16 0.0 0.0 -0.0 0.0 47.3954582214 8.54566764832 10.0 1 +3 0 3 85 0.0 0.0 -0.0 0.0 47.3977355957 8.54561138153 10.0 1 diff --git a/integrationtests/python_src/px4_it/mavros/mission_test.py b/integrationtests/python_src/px4_it/mavros/mission_test.py new file mode 100755 index 0000000000..5b3fbd2e53 --- /dev/null +++ b/integrationtests/python_src/px4_it/mavros/mission_test.py @@ -0,0 +1,301 @@ +#!/usr/bin/env python +#*************************************************************************** +# +# Copyright (c) 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. +# +#***************************************************************************/ + +# +# @author Andreas Antener +# +PKG = 'px4' + +import unittest +import rospy +import math +import rosbag +import sys +import os + +import mavros +from pymavlink import mavutil +from mavros import mavlink + +from geometry_msgs.msg import PoseStamped +from mavros_msgs.srv import CommandLong, WaypointPush +from mavros_msgs.msg import Mavlink, Waypoint, ExtendedState +from sensor_msgs.msg import NavSatFix +from mavros.mission import QGroundControlWP +#from px4_test_helper import PX4TestHelper + +class MavrosMissionTest(unittest.TestCase): + """ + Run a mission + """ + + def setUp(self): + rospy.init_node('test_node', anonymous=True) + + self.rate = rospy.Rate(10) # 10hz + self.has_global_pos = False + self.local_position = PoseStamped() + self.global_position = NavSatFix() + self.extended_state = ExtendedState() + self.home_alt = 0 + self.mc_rad = 5 + self.fw_rad = 60 + self.fw_alt_rad = 10 + self.last_alt_d = 9999 + self.last_pos_d = 9999 + self.mission_name = "" + + # need to simulate heartbeat for datalink loss detection + rospy.Timer(rospy.Duration(0.5), self.send_heartbeat) + + rospy.wait_for_service('mavros/cmd/command', 30) + self.pub_mavlink = rospy.Publisher('mavlink/to', Mavlink, queue_size=1) + self._srv_cmd_long = rospy.ServiceProxy('mavros/cmd/command', CommandLong, persistent=True) + self._srv_wp_push = rospy.ServiceProxy('mavros/mission/push', WaypointPush, persistent=True) + + rospy.Subscriber("mavros/local_position/pose", PoseStamped, self.position_callback) + rospy.Subscriber("mavros/global_position/global", NavSatFix, self.global_position_callback) + rospy.Subscriber("mavros/extended_state", ExtendedState, self.extended_state_callback) + + def tearDown(self): + #self.helper.tearDown() + pass + + # + # General callback functions used in tests + # + def position_callback(self, data): + self.local_position = data + + def global_position_callback(self, data): + self.global_position = data + + if not self.has_global_pos: + self.home_alt = data.altitude + self.has_global_pos = True + + def extended_state_callback(self, data): + self.extended_state = data + + # + # Helper methods + # + def is_at_position(self, lat, lon, alt, xy_offset, z_offset): + R = 6371000 # metres + rlat1 = math.radians(lat) + rlat2 = math.radians(self.global_position.latitude) + + rlat_d = math.radians(self.global_position.latitude - lat) + rlon_d = math.radians(self.global_position.longitude - lon) + + a = (math.sin(rlat_d / 2) * math.sin(rlat_d / 2) + + math.cos(rlat1) * math.cos(rlat2) * + math.sin(rlon_d / 2) * math.sin(rlon_d / 2)) + c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a)) + + d = R * c + alt_d = abs(alt - self.global_position.altitude) + + #rospy.loginfo("d: %f, alt_d: %f", d, alt_d) + + # remember best distances + if self.last_pos_d > d: + self.last_pos_d = d + if self.last_alt_d > alt_d: + self.last_alt_d = alt_d + + return d < xy_offset and alt_d < z_offset + + def reach_position(self, lat, lon, alt, timeout, index): + # reset best distances + self.last_alt_d = 9999 + self.last_pos_d = 9999 + + rospy.loginfo("trying to reach waypoint " + + "lat: %f, lon: %f, alt: %f, timeout: %d, index: %d" % + (lat, lon, alt, timeout, index)) + + # does it reach the position in X seconds? + count = 0 + while count < timeout: + # use MC radius by default + # FIXME: also check MAV_TYPE from system status, otherwise pure fixed-wing won't work + xy_radius = self.mc_rad + z_radius = self.mc_rad + + # use FW radius if in FW or in transition + if (self.extended_state.vtol_state == ExtendedState.VTOL_STATE_FW or + self.extended_state.vtol_state == ExtendedState.VTOL_STATE_TRANSITION_TO_MC or + self.extended_state.vtol_state == ExtendedState.VTOL_STATE_TRANSITION_TO_FW): + xy_radius = self.fw_rad + z_radius = self.fw_alt_rad + + if self.is_at_position(lat, lon, alt, xy_radius, z_radius): + rospy.loginfo("position reached, index: %d, count: %d, pos_d: %f, alt_d: %f" % + (index, count, self.last_pos_d, self.last_alt_d)) + break + + count = count + 1 + self.rate.sleep() + + self.assertTrue(count < timeout, (("(%s) took too long to get to position " + + "lat: %f, lon: %f, alt: %f, xy off: %f, z off: %f, timeout: %d, index: %d, pos_d: %f, alt_d: %f") % + (self.mission_name, lat, lon, alt, xy_radius, z_radius, timeout, index, self.last_pos_d, self.last_alt_d))) + + def run_mission(self): + """switch mode: armed | auto""" + self._srv_cmd_long(False, 176, False, + # arm | custom, auto, mission + 128 | 1, 4, 4, 0, 0, 0, 0) + + def wait_until_ready(self): + """FIXME: hack to wait for simulation to be ready""" + while not self.has_global_pos: + self.rate.sleep() + + def wait_on_landing(self, timeout, index): + """Wait for landed state""" + + rospy.loginfo("waiting for landing " + + "timeout: %d, index: %d" % + (timeout, index)) + + count = 0 + while count < timeout: + if self.extended_state.landed_state == ExtendedState.LANDED_STATE_ON_GROUND: + break + + count = count + 1 + self.rate.sleep() + + self.assertTrue(count < timeout, ("(%s) landing not detected after landing WP " + + "timeout: %d, index: %d") % + (self.mission_name, timeout, index)) + + def wait_on_transition(self, transition, timeout, index): + """Wait for VTOL transition""" + + rospy.loginfo("waiting for VTOL transition " + + "transition: %d, timeout: %d, index: %d" % + (transition, timeout, index)) + + count = 0 + while count < timeout: + # transition to MC + if (transition == ExtendedState.VTOL_STATE_MC and + self.extended_state.vtol_state == ExtendedState.VTOL_STATE_MC): + break + + # transition to FW + if (transition == ExtendedState.VTOL_STATE_FW and + self.extended_state.vtol_state == ExtendedState.VTOL_STATE_FW): + break + + count = count + 1 + self.rate.sleep() + + self.assertTrue(count < timeout, ("(%s) transition not detected " + + "timeout: %d, index: %d") % + (self.mission_name, timeout, index)) + + def send_heartbeat(self, event=None): + # mav type gcs + mavmsg = mavutil.mavlink.MAVLink_heartbeat_message(6, 0, 0, 0, 0, 0) + # XXX: hack: using header object to set mav properties + mavmsg.pack(mavutil.mavlink.MAVLink_header(0, 0, 0, 2, 1)) + rosmsg = mavlink.convert_to_rosmsg(mavmsg) + self.pub_mavlink.publish(rosmsg) + + def test_mission(self): + """Test mission""" + + if len(sys.argv) < 2: + self.fail("usage: mission_test.py mission_file") + return + + self.mission_name = sys.argv[1] + mission_file = os.path.dirname(os.path.realpath(__file__)) + "/" + sys.argv[1] + + rospy.loginfo("reading mission %s", mission_file) + mission = QGroundControlWP() + wps = [] + for waypoint in mission.read(open(mission_file, 'r')): + wps.append(waypoint) + rospy.logdebug(waypoint) + + rospy.loginfo("wait until ready") + self.wait_until_ready() + + rospy.loginfo("send mission") + res = self._srv_wp_push(wps) + rospy.loginfo(res) + self.assertTrue(res.success, "(%s) mission could not be transfered" % self.mission_name) + + rospy.loginfo("run mission") + self.run_mission() + + index = 0 + for waypoint in wps: + # only check position for waypoints where this makes sense + if waypoint.frame == Waypoint.FRAME_GLOBAL_REL_ALT or waypoint.frame == Waypoint.FRAME_GLOBAL: + alt = waypoint.z_alt + if waypoint.frame == Waypoint.FRAME_GLOBAL_REL_ALT: + alt += self.home_alt + self.reach_position(waypoint.x_lat, waypoint.y_long, alt, 600, index) + + # check if VTOL transition happens if applicable + if waypoint.command == 84 or waypoint.command == 85 or waypoint.command == 3000: + transition = waypoint.param1 + + if waypoint.command == 84: # VTOL takeoff implies transition to FW + transition = ExtendedState.VTOL_STATE_FW + + if waypoint.command == 85: # VTOL takeoff implies transition to MC + transition = ExtendedState.VTOL_STATE_MC + + self.wait_on_transition(transition, 600, index) + + # after reaching position, wait for landing detection if applicable + if waypoint.command == 85 or waypoint.command == 21: + self.wait_on_landing(600, index) + + index += 1 + + +if __name__ == '__main__': + import rostest + name = "mavros_mission_test" + if len(sys.argv) > 1: + name += "-%s" % sys.argv[1] + rostest.rosrun(PKG, name, MavrosMissionTest) diff --git a/integrationtests/python_src/px4_it/mavros/vtol_new_1.txt b/integrationtests/python_src/px4_it/mavros/vtol_new_1.txt new file mode 100644 index 0000000000..527bd5f5ad --- /dev/null +++ b/integrationtests/python_src/px4_it/mavros/vtol_new_1.txt @@ -0,0 +1,4 @@ +# New style transitions, takeoff not at home +QGC WPL 110 +0 1 3 84 15.0 0 0 0 47.397731862593744 8.543269295853861 12.0 1 +1 0 3 85 0.0 0.0 -0.0 0.0 47.3990753922183 8.5432371088360526 0.0 1 diff --git a/integrationtests/python_src/px4_it/mavros/vtol_new_2.txt b/integrationtests/python_src/px4_it/mavros/vtol_new_2.txt new file mode 100644 index 0000000000..7d6908a986 --- /dev/null +++ b/integrationtests/python_src/px4_it/mavros/vtol_new_2.txt @@ -0,0 +1,5 @@ +# New style transitions, takeoff at home +QGC WPL 110 +0 1 3 84 15.0 0 0 0 47.397746387399621 8.5455920888607579 12.0 1 +1 0 3 16 0.0 0.0 0.0 0.0 47.399093548895856 8.5455438069836305 12.0 1 +2 0 3 85 0.0 0.0 -0.0 0.0 47.39903545 8.5432263800000001 0.0 1 diff --git a/integrationtests/python_src/px4_it/mavros/vtol_old_1.txt b/integrationtests/python_src/px4_it/mavros/vtol_old_1.txt new file mode 100644 index 0000000000..123423751d --- /dev/null +++ b/integrationtests/python_src/px4_it/mavros/vtol_old_1.txt @@ -0,0 +1,8 @@ +# Old style transitions, takeoff and landing WPs away from home and last WP +QGC WPL 110 +0 1 3 22 15.0 0 0 0 47.398046406687619 8.5458366721115908 12.0 1 +1 0 2 3000 4 0.0 0.0 0.0 47.398033142089844 8.5457897186279297 12.0 1 +2 0 3 16 0 0.0 0.0 0.0 47.399269104003906 8.5455722808837891 12.0 1 +3 0 3 16 0 0.0 0.0 0.0 47.399281145681528 8.547910568913295 12.0 1 +4 0 2 3000 3 0.0 0.0 0.0 47.397785186767578 8.545262336730957 12.0 1 +5 0 3 21 25.0 0.0 0.0 0.0 47.398884865088675 8.547918116539563 12.0 1 diff --git a/integrationtests/python_src/px4_it/mavros/vtol_old_2.txt b/integrationtests/python_src/px4_it/mavros/vtol_old_2.txt new file mode 100644 index 0000000000..d021eb9ddd --- /dev/null +++ b/integrationtests/python_src/px4_it/mavros/vtol_old_2.txt @@ -0,0 +1,8 @@ +# Old style transitions, takeoff WP at home location, landing WP at last WP location +QGC WPL 110 +0 1 3 22 15.0 0 0 0 47.397740722711738 8.5455944102696719 12.0 1 +1 0 2 3000 4 0.0 0.0 0.0 47.397732380000001 8.5458224099999995 12.0 1 +2 0 3 16 0 0.0 0.0 0.0 47.397749067235559 8.5429755110031351 12.0 1 +3 0 3 16 0 0.0 0.0 0.0 47.399213119631852 8.5430124879018479 12.0 1 +4 0 2 3000 3 0.0 0.0 0.0 47.399158900000003 8.5429077299999996 12.0 1 +5 0 3 21 25.0 0.0 0.0 0.0 47.399211883544922 8.5430123448444419 12.0 1 diff --git a/integrationtests/python_src/px4_it/mavros/vtol_old_3.txt b/integrationtests/python_src/px4_it/mavros/vtol_old_3.txt new file mode 100644 index 0000000000..ab7814c9c9 --- /dev/null +++ b/integrationtests/python_src/px4_it/mavros/vtol_old_3.txt @@ -0,0 +1,5 @@ +# Old style transitions, takeoff with normal WP +QGC WPL 110 +0 1 3 16 15.0 0 0 0 47.398157669127094 8.5460336317115377 12.0 1 +1 0 2 3000 4 0.0 0.0 0.0 47.398033142089844 8.5457897186279297 12.0 1 +2 0 3 16 0 0.0 0.0 0.0 47.399269104003906 8.5455722808837891 12.0 1 diff --git a/integrationtests/run_tests.bash b/integrationtests/run_tests.bash index 6707af90aa..2a3a4c2a0d 100755 --- a/integrationtests/run_tests.bash +++ b/integrationtests/run_tests.bash @@ -25,7 +25,16 @@ TEST_RESULT_TARGET_DIR=$JOB_DIR/test_results # EXPORT_CHARTS=/sitl/testing/export_charts.py # source ROS env -source /opt/ros/indigo/setup.bash +if [ -f /opt/ros/indigo/setup.bash ] +then + source /opt/ros/indigo/setup.bash +elif [ -f /opt/ros/kinetic/setup.bash ] +then + source /opt/ros/kinetic/setup.bash +else + echo "could not find /opt/ros/{ros-distro}/setup.bash" + exit 1 +fi source $SRC_DIR/integrationtests/setup_gazebo_ros.bash $SRC_DIR echo "deleting previous test results ($TEST_RESULT_TARGET_DIR)" @@ -43,17 +52,14 @@ ln -s ${SRC_DIR} /root/Firmware echo "=====> compile ($SRC_DIR)" cd $SRC_DIR make ${BUILD} -mkdir -p Tools/sitl_gazebo/Build -cd Tools/sitl_gazebo/Build -cmake -Wno-dev .. -make -j4 -make sdf +make --no-print-directory gazebo_build echo "<=====" # don't exit on error anymore from here on (because single tests or exports might fail) set +e echo "=====> run tests" rostest px4 mavros_posix_tests_iris.launch +rostest px4 mavros_posix_tests_standard_vtol.launch TEST_RESULT=$? echo "<=====" diff --git a/integrationtests/setup_gazebo_ros.bash b/integrationtests/setup_gazebo_ros.bash index 7655fec9a7..6c22bbe192 100755 --- a/integrationtests/setup_gazebo_ros.bash +++ b/integrationtests/setup_gazebo_ros.bash @@ -15,6 +15,8 @@ SRC_DIR=$1 # setup Gazebo env and update package path export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:${SRC_DIR}/Tools/sitl_gazebo/models -export GAZEBO_PLUGIN_PATH=${SRC_DIR}/Tools/sitl_gazebo/Build/:${GAZEBO_PLUGIN_PATH} +export GAZEBO_PLUGIN_PATH=${SRC_DIR}/build_gazebo:${GAZEBO_PLUGIN_PATH} export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:${SRC_DIR}/Tools/sitl_gazebo/Build/msgs/ export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:${SRC_DIR} +export GAZEBO_MODEL_DATABASE_URI="" +export SITL_GAZEBO_PATH=$SRC_DIR/Tools/sitl_gazebo diff --git a/launch/mavros_posix_sitl.launch b/launch/mavros_posix_sitl.launch index 905e0a2ff1..558f3bbadb 100644 --- a/launch/mavros_posix_sitl.launch +++ b/launch/mavros_posix_sitl.launch @@ -6,6 +6,7 @@ + @@ -17,6 +18,7 @@ - + + diff --git a/launch/mavros_posix_tests_standard_vtol.launch b/launch/mavros_posix_tests_standard_vtol.launch new file mode 100644 index 0000000000..d5aa9e285e --- /dev/null +++ b/launch/mavros_posix_tests_standard_vtol.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/posix_sitl.launch b/launch/posix_sitl.launch index 4e3b71f70c..9895ee9d43 100644 --- a/launch/posix_sitl.launch +++ b/launch/posix_sitl.launch @@ -7,7 +7,7 @@ - + diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 4e9682c6e8..6adda4dbc1 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 4e9682c6e8130825f69b0c2c6392a9d8154fffbe +Subproject commit 6adda4dbc1b68a29b8729c6e7ab34c9a1851c4df diff --git a/mavlink/include/mavlink/v2.0 b/mavlink/include/mavlink/v2.0 new file mode 160000 index 0000000000..17231c3777 --- /dev/null +++ b/mavlink/include/mavlink/v2.0 @@ -0,0 +1 @@ +Subproject commit 17231c377749da84cffdf16cda74e15dd1a48f7e diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt new file mode 100644 index 0000000000..278874c089 --- /dev/null +++ b/msg/CMakeLists.txt @@ -0,0 +1,130 @@ +############################################################################ +# +# Copyright (c) 2016 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. +# +############################################################################ + +set(msg_file_names + actuator_armed.msg + actuator_controls.msg + actuator_direct.msg + actuator_outputs.msg + adc_report.msg + airspeed.msg + att_pos_mocap.msg + battery_status.msg + camera_trigger.msg + commander_state.msg + control_state.msg + cpuload.msg + debug_key_value.msg + differential_pressure.msg + distance_sensor.msg + ekf2_innovations.msg + ekf2_replay.msg + esc_report.msg + esc_status.msg + estimator_status.msg + fence.msg + fence_vertex.msg + filtered_bottom_flow.msg + follow_target.msg + fw_pos_ctrl_status.msg + fw_virtual_attitude_setpoint.msg + fw_virtual_rates_setpoint.msg + geofence_result.msg + gps_dump.msg + gps_inject_data.msg + hil_sensor.msg + home_position.msg + input_rc.msg + manual_control_setpoint.msg + mavlink_log.msg + mc_att_ctrl_status.msg + mc_virtual_attitude_setpoint.msg + mc_virtual_rates_setpoint.msg + mission.msg + mission_result.msg + multirotor_motor_limits.msg + offboard_control_mode.msg + optical_flow.msg + output_pwm.msg + parameter_update.msg + position_setpoint.msg + position_setpoint_triplet.msg + pwm_input.msg + qshell_req.msg + rc_channels.msg + rc_parameter_map.msg + safety.msg + satellite_info.msg + sensor_accel.msg + sensor_baro.msg + sensor_combined.msg + sensor_gyro.msg + sensor_mag.msg + servorail_status.msg + subsystem_info.msg + system_power.msg + tecs_status.msg + telemetry_status.msg + test_motor.msg + time_offset.msg + transponder_report.msg + uavcan_parameter_request.msg + uavcan_parameter_value.msg + vehicle_attitude.msg + vehicle_attitude_setpoint.msg + vehicle_command_ack.msg + vehicle_command.msg + vehicle_control_mode.msg + vehicle_force_setpoint.msg + vehicle_global_position.msg + vehicle_global_velocity_setpoint.msg + vehicle_gps_position.msg + vehicle_land_detected.msg + vehicle_local_position.msg + vehicle_local_position_setpoint.msg + vehicle_rates_setpoint.msg + vehicle_status.msg + vision_position_estimate.msg + vtol_vehicle_status.msg + wind_estimate.msg + ) + +# Get absolute paths +set(msg_files) +foreach(msg_file ${msg_file_names}) + list(APPEND msg_files ${CMAKE_CURRENT_SOURCE_DIR}/${msg_file}) +endforeach() + +set(msg_files ${msg_files} PARENT_SCOPE) + +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/msg/actuator_armed.msg b/msg/actuator_armed.msg index f307e366ae..51a7ab116c 100644 --- a/msg/actuator_armed.msg +++ b/msg/actuator_armed.msg @@ -1,5 +1,4 @@ -uint64 timestamp # Microseconds since system boot bool armed # Set to true if system is armed bool prearmed # Set to true if the actuator safety is disabled but motors are not armed bool ready_to_arm # Set to true if system is ready to be armed diff --git a/msg/actuator_controls.msg b/msg/actuator_controls.msg index a802f1502a..46bf35ebff 100644 --- a/msg/actuator_controls.msg +++ b/msg/actuator_controls.msg @@ -10,6 +10,9 @@ uint8 INDEX_AIRBRAKES = 6 uint8 INDEX_LANDING_GEAR = 7 uint8 GROUP_INDEX_ATTITUDE = 0 uint8 GROUP_INDEX_ATTITUDE_ALTERNATE = 1 -uint64 timestamp uint64 timestamp_sample # the timestamp the data this control response is based on was sampled float32[8] control + +# TOPICS actuator_controls actuator_controls_0 actuator_controls_1 actuator_controls_2 actuator_controls_3 +# TOPICS actuator_controls_virtual_fw actuator_controls_virtual_mc + diff --git a/msg/actuator_controls_1.msg b/msg/actuator_controls_1.msg deleted file mode 100644 index 414eb06ddb..0000000000 --- a/msg/actuator_controls_1.msg +++ /dev/null @@ -1,5 +0,0 @@ -uint8 NUM_ACTUATOR_CONTROLS = 8 -uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 -uint64 timestamp -uint64 timestamp_sample # the timestamp the data this control response is based on was sampled -float32[8] control diff --git a/msg/actuator_controls_2.msg b/msg/actuator_controls_2.msg deleted file mode 100644 index 414eb06ddb..0000000000 --- a/msg/actuator_controls_2.msg +++ /dev/null @@ -1,5 +0,0 @@ -uint8 NUM_ACTUATOR_CONTROLS = 8 -uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 -uint64 timestamp -uint64 timestamp_sample # the timestamp the data this control response is based on was sampled -float32[8] control diff --git a/msg/actuator_controls_3.msg b/msg/actuator_controls_3.msg deleted file mode 100644 index 414eb06ddb..0000000000 --- a/msg/actuator_controls_3.msg +++ /dev/null @@ -1,5 +0,0 @@ -uint8 NUM_ACTUATOR_CONTROLS = 8 -uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 -uint64 timestamp -uint64 timestamp_sample # the timestamp the data this control response is based on was sampled -float32[8] control diff --git a/msg/actuator_controls_virtual_fw.msg b/msg/actuator_controls_virtual_fw.msg deleted file mode 100644 index 414eb06ddb..0000000000 --- a/msg/actuator_controls_virtual_fw.msg +++ /dev/null @@ -1,5 +0,0 @@ -uint8 NUM_ACTUATOR_CONTROLS = 8 -uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 -uint64 timestamp -uint64 timestamp_sample # the timestamp the data this control response is based on was sampled -float32[8] control diff --git a/msg/actuator_direct.msg b/msg/actuator_direct.msg index c798f5101f..7d22f79e75 100644 --- a/msg/actuator_direct.msg +++ b/msg/actuator_direct.msg @@ -1,4 +1,3 @@ uint8 NUM_ACTUATORS_DIRECT = 16 -uint64 timestamp # timestamp in us since system boot uint32 nvalues # number of valid values float32[16] values # actuator values, from -1 to 1 diff --git a/msg/actuator_outputs.msg b/msg/actuator_outputs.msg index 00a3c35b79..269255340d 100644 --- a/msg/actuator_outputs.msg +++ b/msg/actuator_outputs.msg @@ -1,5 +1,4 @@ uint8 NUM_ACTUATOR_OUTPUTS = 16 uint8 NUM_ACTUATOR_OUTPUT_GROUPS = 4 # for sanity checking -uint64 timestamp # output timestamp in us since system boot uint32 noutputs # valid outputs float32[16] output # output data, in natural output units diff --git a/msg/adc_report.msg b/msg/adc_report.msg index bfa093a202..a93ff2ac76 100644 --- a/msg/adc_report.msg +++ b/msg/adc_report.msg @@ -1,3 +1,2 @@ -uint64 timestamp # Timestamp in microseconds since boot int16[8] channel_id # ADC channel IDs, negative for non-existent float32[8] channel_value # ADC channel value in volt, valid if channel ID is positive diff --git a/msg/airspeed.msg b/msg/airspeed.msg index f0a109a7e7..744f9d8830 100644 --- a/msg/airspeed.msg +++ b/msg/airspeed.msg @@ -1,4 +1,3 @@ -uint64 timestamp # microseconds since system boot, needed to integrate float32 indicated_airspeed_m_s # indicated airspeed in meters per second, -1 if unknown float32 true_airspeed_m_s # true filtered airspeed in meters per second, -1 if unknown float32 true_airspeed_unfiltered_m_s # true airspeed in meters per second, -1 if unknown diff --git a/msg/att_pos_mocap.msg b/msg/att_pos_mocap.msg index 52bc04b5aa..6c007a6393 100644 --- a/msg/att_pos_mocap.msg +++ b/msg/att_pos_mocap.msg @@ -1,7 +1,6 @@ uint32 id # ID of the estimator, commonly the component ID of the incoming message -uint64 timestamp_boot # time of this estimate, in microseconds since system start -uint64 timestamp_computer # timestamp provided by the companion computer, in us +uint64 timestamp_received # timestamp when the estimate was received float32[4] q # Estimated attitude as quaternion diff --git a/msg/battery_status.msg b/msg/battery_status.msg index dc4c90e9c8..351b31d035 100644 --- a/msg/battery_status.msg +++ b/msg/battery_status.msg @@ -1,4 +1,3 @@ -uint64 timestamp # microseconds since system boot, needed to integrate float32 voltage_v # Battery voltage in volts, 0 if unknown float32 voltage_filtered_v # Battery voltage in volts, filtered, 0 if unknown float32 current_a # Battery current in amperes, -1 if unknown diff --git a/msg/camera_trigger.msg b/msg/camera_trigger.msg index b4dcfe8ef3..384e9aa61c 100644 --- a/msg/camera_trigger.msg +++ b/msg/camera_trigger.msg @@ -1,4 +1,3 @@ -uint64 timestamp # Timestamp when camera was triggered uint32 seq # Image sequence diff --git a/msg/commander_state.msg b/msg/commander_state.msg index 0c6a896dc7..a9c92ebb5e 100644 --- a/msg/commander_state.msg +++ b/msg/commander_state.msg @@ -14,6 +14,5 @@ uint8 MAIN_STATE_AUTO_LAND = 11 uint8 MAIN_STATE_AUTO_FOLLOW_TARGET = 12 uint8 MAIN_STATE_MAX = 13 -uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data uint8 main_state # main state machine diff --git a/msg/control_state.msg b/msg/control_state.msg index 82af7aeda6..96efea0f81 100644 --- a/msg/control_state.msg +++ b/msg/control_state.msg @@ -1,5 +1,8 @@ # This is similar to the mavlink message CONTROL_SYSTEM_STATE, but for onboard use */ -uint64 timestamp # in microseconds since system start +uint8 AIRSPD_MODE_MEAS = 0 # airspeed is measured airspeed from sensor +uint8 AIRSPD_MODE_EST = 1 # airspeed is estimated by body velocity +uint8 AIRSPD_MODE_DISABLED = 2 # airspeed is disabled + float32 x_acc # X acceleration in body frame float32 y_acc # Y acceleration in body frame float32 z_acc # Z acceleration in body frame diff --git a/msg/cpuload.msg b/msg/cpuload.msg new file mode 100644 index 0000000000..ef85fa58a3 --- /dev/null +++ b/msg/cpuload.msg @@ -0,0 +1 @@ +float32 load # processor load from 0 to 1 diff --git a/msg/differential_pressure.msg b/msg/differential_pressure.msg index bb31449d6c..10fb9eb88e 100644 --- a/msg/differential_pressure.msg +++ b/msg/differential_pressure.msg @@ -1,4 +1,3 @@ -uint64 timestamp # Microseconds since system boot, needed to integrate uint64 error_count # Number of errors detected by driver float32 differential_pressure_raw_pa # Raw differential pressure reading (may be negative) float32 differential_pressure_filtered_pa # Low pass filtered differential pressure reading diff --git a/msg/distance_sensor.msg b/msg/distance_sensor.msg index ad01ce63e0..c2e1a171f2 100644 --- a/msg/distance_sensor.msg +++ b/msg/distance_sensor.msg @@ -1,6 +1,5 @@ # DISTANCE_SENSOR message data -uint64 timestamp # Microseconds since system boot float32 min_distance # Minimum distance the sensor can measure (in m) float32 max_distance # Maximum distance the sensor can measure (in m) diff --git a/msg/ekf2_innovations.msg b/msg/ekf2_innovations.msg index 0822a473b5..225922b8b6 100644 --- a/msg/ekf2_innovations.msg +++ b/msg/ekf2_innovations.msg @@ -1,4 +1,3 @@ -uint64 timestamp # Timestamp in microseconds since boot float32[6] vel_pos_innov # velocity and position innovations float32[3] mag_innov # earth magnetic field innovations float32 heading_innov # heading innovation diff --git a/msg/ekf2_replay.msg b/msg/ekf2_replay.msg index e3127ef0e9..61f0d1d27d 100644 --- a/msg/ekf2_replay.msg +++ b/msg/ekf2_replay.msg @@ -1,15 +1,16 @@ uint64 time_ref # ekf2 reference time. This is a timestamp passed to the # estimator which it uses a absolute reference. -uint64 gyro_integral_dt # gyro integration period in us -uint64 accelerometer_integral_dt # accelerometer integration period in us +float32 gyro_integral_dt # gyro integration period in s +float32 accelerometer_integral_dt # accelerometer integration period in s uint64 magnetometer_timestamp # timestamp of magnetometer measurement in us uint64 baro_timestamp # timestamp of barometer measurement in us uint64 rng_timestamp # timestamp of range finder measurement in us uint64 flow_timestamp # timestamp of optical flow measurement in us uint64 asp_timestamp # timestamp of airspeed measurement in us +uint64 ev_timestamp # timestamp of external vision measurement in us -float32[3] gyro_integral_rad # integrated gyro vector in rad -float32[3] accelerometer_integral_m_s # integrated accelerometer vector in m/s +float32[3] gyro_rad # gyro vector in rad +float32[3] accelerometer_m_s2 # accelerometer vector in m/s^2 float32[3] magnetometer_ga # magnetometer measurement vector (body fixed NED) in ga float32 baro_alt_meter # barometer altitude measurement in m @@ -42,7 +43,9 @@ uint8 flow_quality # Quality of accumulated optical flow data (0 - 255) # airspeed float32 indicated_airspeed_m_s # indicated airspeed in meters per second, -1 if unknown float32 true_airspeed_m_s # true filtered airspeed in meters per second, -1 if unknown -float32 true_airspeed_unfiltered_m_s # true airspeed in meters per second, -1 if unknown -float32 air_temperature_celsius # air temperature in degrees celsius, -1000 if unknown -float32 confidence # confidence value from 0 to 1 for this sensor +# external vision measurements +float32[3] pos_ev # position in earth (NED) frame (metres) +float32[4] quat_ev # quaternion rotation from earth (NED) to body (XYZ) frame +float32 pos_err # position error 1-std for each axis (metres) +float32 ang_err # angular error 1-std for each axis (rad) diff --git a/msg/encoders.msg b/msg/encoders.msg deleted file mode 100644 index 505365c57e..0000000000 --- a/msg/encoders.msg +++ /dev/null @@ -1,5 +0,0 @@ -uint8 NUM_ENCODERS = 4 - -uint64 timestamp -int64[4] counts # counts of encoder -float32[4] velocity # counts of encoder/ second diff --git a/msg/esc_status.msg b/msg/esc_status.msg index b54131756b..5148642459 100644 --- a/msg/esc_status.msg +++ b/msg/esc_status.msg @@ -3,6 +3,7 @@ uint8 CONNECTED_ESC_MAX = 8 # The number of ESCs supported. Current (Q2/2013) uint8 ESC_VENDOR_GENERIC = 0 # generic ESC uint8 ESC_VENDOR_MIKROKOPTER = 1 # Mikrokopter uint8 ESC_VENDOR_GRAUPNER_HOTT = 2 # Graupner HoTT ESC +uint8 ESC_VENDOR_TAP = 3 # TAP ESC uint8 ESC_CONNECTION_TYPE_PPM = 0 # Traditional PPM ESC uint8 ESC_CONNECTION_TYPE_SERIAL = 1 # Serial Bus connected ESC @@ -11,7 +12,6 @@ uint8 ESC_CONNECTION_TYPE_I2C = 3 # I2C uint8 ESC_CONNECTION_TYPE_CAN = 4 # CAN-Bus uint16 counter # incremented by the writing thread everytime new data is stored -uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data uint8 esc_count # number of connected ESCs uint8 esc_connectiontype # how ESCs connected to the system diff --git a/msg/estimator_status.msg b/msg/estimator_status.msg index cda4844089..9dea01893d 100644 --- a/msg/estimator_status.msg +++ b/msg/estimator_status.msg @@ -1,4 +1,3 @@ -uint64 timestamp # Timestamp in microseconds since boot float32[32] states # Internal filter states float32 n_states # Number of states effectively used float32[3] vibe # Vibration levels in X, Y and Z @@ -23,12 +22,26 @@ uint16 control_mode_flags # Bitmask to indicate EKF logic state # 2 - true if GPS measurements are being fused # 3 - true if optical flow measurements are being fused # 4 - true if a simple magnetic yaw heading is being fused -# 5 - true if the horizontal projection of magnetometer data is being fused -# 6 - true if 3-axis magnetometer measurement are being fused -# 7 - true if synthetic magnetic declination measurements are being fused -# 8 - true when the vehicle is airborne -# 9 - true when the vehicle motors are armed -# 10 - true when wind velocity is being estimated -# 11 - true when baro height is being fused as a primary height reference -# 12 - true when range finder height is being fused as a primary height reference -# 15 - true when range finder height is being fused as a primary height reference +# 5 - true if 3-axis magnetometer measurement are being fused +# 6 - true if synthetic magnetic declination measurements are being fused +# 7 - true when the vehicle is airborne +# 8 - true when wind velocity is being estimated +# 9 - true when baro height is being fused as a primary height reference +# 10 - true when range finder height is being fused as a primary height reference +# 11 - true when range finder height is being fused as a primary height reference +uint16 filter_fault_flags # Bitmask to indicate EKF internal faults +# 0 - true if the fusion of the magnetometer X-axis has encountered a numerical error +# 1 - true if the fusion of the magnetometer Y-axis has encountered a numerical error +# 2 - true if the fusion of the magnetometer Z-axis has encountered a numerical error +# 3 - true if the fusion of the magnetic heading has encountered a numerical error +# 4 - true if the fusion of the magnetic declination has encountered a numerical error +# 5 - true if fusion of the airspeed has encountered a numerical error +# 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error +# 7 - true if fusion of the optical flow X axis has encountered a numerical error +# 8 - true if fusion of the optical flow Y axis has encountered a numerical error +# 9 - true if fusion of the North velocity has encountered a numerical error +# 10 - true if fusion of the East velocity has encountered a numerical error +# 11 - true if fusion of the Down velocity has encountered a numerical error +# 12 - true if fusion of the North position has encountered a numerical error +# 13 - true if fusion of the East position has encountered a numerical error +# 14 - true if fusion of the Down position has encountered a numerical error diff --git a/msg/filtered_bottom_flow.msg b/msg/filtered_bottom_flow.msg index 815a38414d..1de919ee92 100644 --- a/msg/filtered_bottom_flow.msg +++ b/msg/filtered_bottom_flow.msg @@ -1,5 +1,4 @@ # Filtered bottom flow in bodyframe. -uint64 timestamp # time of this estimate, in microseconds since system start float32 sumx # Integrated bodyframe x flow in meters float32 sumy # Integrated bodyframe y flow in meters diff --git a/msg/follow_target.msg b/msg/follow_target.msg index 51ed91a28f..75f124e2ee 100644 --- a/msg/follow_target.msg +++ b/msg/follow_target.msg @@ -1,4 +1,7 @@ -uint64 timestamp # timestamp, UNIX epoch (GPS synced) float64 lat # target position (deg * 1e7) float64 lon # target position (deg * 1e7) -float32 alt # target position +float32 alt # target position +float32 vy # target vel in y +float32 vx # target vel in x +float32 vz # target vel in z +uint8 est_cap # target reporting capabilities diff --git a/msg/navigation_capabilities.msg b/msg/fw_pos_ctrl_status.msg similarity index 64% rename from msg/navigation_capabilities.msg rename to msg/fw_pos_ctrl_status.msg index 6d12aaaed9..0af57b1bf5 100644 --- a/msg/navigation_capabilities.msg +++ b/msg/fw_pos_ctrl_status.msg @@ -1,5 +1,12 @@ -uint64 timestamp # timestamp this topic was emitted +float32 nav_roll +float32 nav_pitch +float32 nav_bearing + +float32 target_bearing +float32 wp_dist +float32 xtrack_error float32 turn_distance # the optimal distance to a waypoint to switch to the next + float32 landing_horizontal_slope_displacement float32 landing_slope_angle_rad float32 landing_flare_length diff --git a/msg/fw_virtual_attitude_setpoint.msg b/msg/fw_virtual_attitude_setpoint.msg index 6c12b225c8..a379477bce 100644 --- a/msg/fw_virtual_attitude_setpoint.msg +++ b/msg/fw_virtual_attitude_setpoint.msg @@ -8,7 +8,6 @@ # ############################################################################################### -uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data float32 roll_body # body angle in NED frame float32 pitch_body # body angle in NED frame @@ -35,3 +34,6 @@ bool fw_control_yaw # control heading with rudder (used for auto takeoff on bool disable_mc_yaw_control # control yaw for mc (used for vtol weather-vane mode) bool apply_flaps + +# WAS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint +# TOPICS fw_virtual_attitude_setpoint diff --git a/msg/fw_virtual_rates_setpoint.msg b/msg/fw_virtual_rates_setpoint.msg index 4fa09b563f..369b68270a 100644 --- a/msg/fw_virtual_rates_setpoint.msg +++ b/msg/fw_virtual_rates_setpoint.msg @@ -8,7 +8,6 @@ # ############################################################################################### -uint64 timestamp # in microseconds since system start float32 roll # body angular rates in NED frame float32 pitch # body angular rates in NED frame diff --git a/msg/gps_dump.msg b/msg/gps_dump.msg new file mode 100644 index 0000000000..8a91149d46 --- /dev/null +++ b/msg/gps_dump.msg @@ -0,0 +1,6 @@ +# This message is used to dump the raw gps communication to the log. +# Set the parameter GPS_DUMP_COMM to 1 to use this. + +uint8 len # length of data, MSB bit set = message to the gps device, + # clear = message from the device +uint8[79] data # data to write to the log diff --git a/msg/gps_inject_data.msg b/msg/gps_inject_data.msg index 4ba4ebc341..ac44d02dbe 100644 --- a/msg/gps_inject_data.msg +++ b/msg/gps_inject_data.msg @@ -1,2 +1,3 @@ uint8 len # length of data -uint8[110] data # data to write to GPS device +uint8 flags # LSB: 1=fragmented +uint8[182] data # data to write to GPS device (RTCM message) diff --git a/msg/hil_sensor.msg b/msg/hil_sensor.msg index 9317722db4..e1debf8f6f 100644 --- a/msg/hil_sensor.msg +++ b/msg/hil_sensor.msg @@ -17,7 +17,6 @@ int32 MAGNETOMETER_MODE_NEGATIVE_BIAS = 2 # # NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only -uint64 timestamp # Timestamp in microseconds since boot, from gyro # int16[3] gyro_raw # Raw sensor values of angular velocity float32[3] gyro_rad_s # Angular velocity in radian per seconds diff --git a/msg/home_position.msg b/msg/home_position.msg index 7135c07b18..19c9d44868 100644 --- a/msg/home_position.msg +++ b/msg/home_position.msg @@ -1,5 +1,4 @@ # GPS home position in WGS84 coordinates. -uint64 timestamp # Timestamp (microseconds since system boot) float64 lat # Latitude in degrees float64 lon # Longitude in degrees diff --git a/msg/manual_control_setpoint.msg b/msg/manual_control_setpoint.msg index 81d77635ee..599e63d5a9 100644 --- a/msg/manual_control_setpoint.msg +++ b/msg/manual_control_setpoint.msg @@ -12,8 +12,6 @@ int8 MODE_SLOT_5 = 4 # mode slot 5 selected int8 MODE_SLOT_6 = 5 # mode slot 6 selected int8 MODE_SLOT_MAX = 6 # number of slots plus one -uint64 timestamp - # Any of the channels may not be available and be set to NaN # to indicate that it does not contain valid data. # The variable names follow the definition of the @@ -53,4 +51,5 @@ 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 uint8 kill_switch # throttle kill: _NORMAL_, KILL +uint8 transition_switch # VTOL transition switch: _HOVER, FORWARD_FLIGHT int8 mode_slot # the slot a specific model selector is in diff --git a/msg/mavlink_log.msg b/msg/mavlink_log.msg index 1161a25543..274596adde 100644 --- a/msg/mavlink_log.msg +++ b/msg/mavlink_log.msg @@ -1,4 +1,3 @@ -uint64 timestamp # Timestamp in microseconds since boot uint8[50] text -uint8 severity +uint8 severity # log level (same as in the linux kernel, starting with 0) diff --git a/msg/mc_att_ctrl_status.msg b/msg/mc_att_ctrl_status.msg index 114e843b0e..c2979c3f00 100644 --- a/msg/mc_att_ctrl_status.msg +++ b/msg/mc_att_ctrl_status.msg @@ -1,4 +1,3 @@ -uint64 timestamp # in microseconds since system start float32 roll_rate_integ # roll rate inegrator float32 pitch_rate_integ # pitch rate integrator diff --git a/msg/mc_virtual_attitude_setpoint.msg b/msg/mc_virtual_attitude_setpoint.msg index 6c12b225c8..f2fe44da86 100644 --- a/msg/mc_virtual_attitude_setpoint.msg +++ b/msg/mc_virtual_attitude_setpoint.msg @@ -8,7 +8,6 @@ # ############################################################################################### -uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data float32 roll_body # body angle in NED frame float32 pitch_body # body angle in NED frame @@ -35,3 +34,6 @@ bool fw_control_yaw # control heading with rudder (used for auto takeoff on bool disable_mc_yaw_control # control yaw for mc (used for vtol weather-vane mode) bool apply_flaps + +# WAS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint +# TOPICS mc_virtual_attitude_setpoint diff --git a/msg/mc_virtual_rates_setpoint.msg b/msg/mc_virtual_rates_setpoint.msg index 4fa09b563f..369b68270a 100644 --- a/msg/mc_virtual_rates_setpoint.msg +++ b/msg/mc_virtual_rates_setpoint.msg @@ -8,7 +8,6 @@ # ############################################################################################### -uint64 timestamp # in microseconds since system start float32 roll # body angular rates in NED frame float32 pitch # body angular rates in NED frame diff --git a/msg/mission.msg b/msg/mission.msg index ac135a4e09..b25940c07a 100644 --- a/msg/mission.msg +++ b/msg/mission.msg @@ -1,3 +1,5 @@ int32 dataman_id # default 0, there are two offboard storage places in the dataman: 0 or 1 uint32 count # count of the missions stored in the dataman int32 current_seq # default -1, start at the one changed latest + +# TOPICS mission offboard_mission onboard_mission diff --git a/msg/offboard_control_mode.msg b/msg/offboard_control_mode.msg index e2d06963b6..927c8e6503 100644 --- a/msg/offboard_control_mode.msg +++ b/msg/offboard_control_mode.msg @@ -1,5 +1,4 @@ # Off-board control mode -uint64 timestamp bool ignore_thrust bool ignore_attitude diff --git a/msg/optical_flow.msg b/msg/optical_flow.msg index d34e32b8fd..a890df68a4 100644 --- a/msg/optical_flow.msg +++ b/msg/optical_flow.msg @@ -1,7 +1,6 @@ # Optical flow in NED body frame in SI units. # @see http://en.wikipedia.org/wiki/International_System_of_Units -uint64 timestamp # in microseconds since system start uint8 sensor_id # id of the sensor emitting the flow value float32 pixel_flow_x_integral # accumulated optical flow in radians around x axis float32 pixel_flow_y_integral # accumulated optical flow in radians around y axis diff --git a/msg/parameter_update.msg b/msg/parameter_update.msg index 78cebf7f4f..3fa04a7657 100644 --- a/msg/parameter_update.msg +++ b/msg/parameter_update.msg @@ -1,2 +1 @@ -uint64 timestamp # time at which the latest parameter was updated bool saved # wether the change has already been saved to disk diff --git a/msg/position_setpoint.msg b/msg/position_setpoint.msg index 394885933b..eba8ed9160 100644 --- a/msg/position_setpoint.msg +++ b/msg/position_setpoint.msg @@ -37,3 +37,4 @@ bool acceleration_valid # true if acceleration setpoint is valid/should be used bool acceleration_is_force # interprete acceleration as force float32 acceptance_radius # navigation acceptance_radius if we're doing waypoint navigation float32 cruising_speed # the generally desired cruising speed (not a hard constraint) +float32 cruising_throttle # the generally desired cruising throttle (not a hard constraint) diff --git a/msg/pwm_input.msg b/msg/pwm_input.msg index beb82021d2..8b91883532 100644 --- a/msg/pwm_input.msg +++ b/msg/pwm_input.msg @@ -1,5 +1,4 @@ -uint64 timestamp # Microseconds since system boot uint64 error_count uint32 pulse_width # Pulse width, timer counts uint32 period # Period, timer counts diff --git a/msg/qshell_req.msg b/msg/qshell_req.msg index bad96a1cec..fa75390edd 100644 --- a/msg/qshell_req.msg +++ b/msg/qshell_req.msg @@ -1,4 +1,3 @@ -uint64 timestamp # Microseconds since system boot int32[100] string uint64 MAX_STRLEN = 100 uint64 strlen diff --git a/msg/rc_channels.msg b/msg/rc_channels.msg index b8bcc5f536..52d89df5fb 100644 --- a/msg/rc_channels.msg +++ b/msg/rc_channels.msg @@ -20,11 +20,12 @@ uint8 RC_CHANNELS_FUNCTION_PARAM_2=17 uint8 RC_CHANNELS_FUNCTION_PARAM_3_5=18 uint8 RC_CHANNELS_FUNCTION_RATTITUDE=19 uint8 RC_CHANNELS_FUNCTION_KILLSWITCH=20 -uint64 timestamp # Timestamp in microseconds since boot time +uint8 RC_CHANNELS_FUNCTION_TRANSITION=21 + uint64 timestamp_last_valid # Timestamp of last valid RC signal float32[18] channels # Scaled to -1..1 (throttle: 0..1) uint8 channel_count # Number of valid channels -int8[21] function # Functions mapping +int8[22] function # Functions mapping uint8 rssi # Receive signal strength indicator (0-100) bool signal_lost # Control signal lost, should be checked together with topic timeout uint32 frame_drop_count # Number of dropped frames diff --git a/msg/rc_parameter_map.msg b/msg/rc_parameter_map.msg index 4a94498108..4a5cb5cfd1 100644 --- a/msg/rc_parameter_map.msg +++ b/msg/rc_parameter_map.msg @@ -1,7 +1,6 @@ uint8 RC_PARAM_MAP_NCHAN = 3 # This limit is also hardcoded in the enum RC_CHANNELS_FUNCTION in rc_channels.h uint8 PARAM_ID_LEN = 16 # corresponds to MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN -uint64 timestamp # time at which the map was updated bool[3] valid #true for RC-Param channels which are mapped to a param int32[3] param_index # corresponding param index, this field is ignored if set to -1, in this case param_id will be used char[51] param_id # MAP_NCHAN * (ID_LEN + 1) chars, corresponding param id, null terminated diff --git a/msg/actuator_controls_0.msg b/msg/ros/actuator_controls_0.msg similarity index 91% rename from msg/actuator_controls_0.msg rename to msg/ros/actuator_controls_0.msg index 414eb06ddb..4424f197f8 100644 --- a/msg/actuator_controls_0.msg +++ b/msg/ros/actuator_controls_0.msg @@ -1,5 +1,4 @@ uint8 NUM_ACTUATOR_CONTROLS = 8 uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 -uint64 timestamp uint64 timestamp_sample # the timestamp the data this control response is based on was sampled float32[8] control diff --git a/msg/actuator_controls_virtual_mc.msg b/msg/ros/actuator_controls_virtual_mc.msg similarity index 91% rename from msg/actuator_controls_virtual_mc.msg rename to msg/ros/actuator_controls_virtual_mc.msg index 414eb06ddb..4424f197f8 100644 --- a/msg/actuator_controls_virtual_mc.msg +++ b/msg/ros/actuator_controls_virtual_mc.msg @@ -1,5 +1,4 @@ uint8 NUM_ACTUATOR_CONTROLS = 8 uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 -uint64 timestamp uint64 timestamp_sample # the timestamp the data this control response is based on was sampled float32[8] control diff --git a/msg/safety.msg b/msg/safety.msg index be0fadad1e..34d66d12cc 100644 --- a/msg/safety.msg +++ b/msg/safety.msg @@ -1,3 +1,2 @@ -uint64 timestamp bool safety_switch_available # Set to true if a safety switch is connected bool safety_off # Set to true if safety is off diff --git a/msg/satellite_info.msg b/msg/satellite_info.msg index 24cb03facc..0efa030a87 100644 --- a/msg/satellite_info.msg +++ b/msg/satellite_info.msg @@ -1,6 +1,5 @@ uint8 SAT_INFO_MAX_SATELLITES = 20 -uint64 timestamp # Timestamp of satellite info uint8 count # Number of satellites in satellite info uint8[20] svid # Space vehicle ID [1..255], see scheme below uint8[20] used # 0: Satellite not used, 1: used for navigation diff --git a/msg/sensor_accel.msg b/msg/sensor_accel.msg index 0ed71957ef..9dcefa6b87 100644 --- a/msg/sensor_accel.msg +++ b/msg/sensor_accel.msg @@ -1,4 +1,3 @@ -uint64 timestamp uint64 integral_dt # integration time uint64 error_count float32 x # acceleration in the NED X board axis in m/s^2 diff --git a/msg/sensor_baro.msg b/msg/sensor_baro.msg index 315d5a56ec..9fe621f1c8 100644 --- a/msg/sensor_baro.msg +++ b/msg/sensor_baro.msg @@ -1,5 +1,4 @@ float32 pressure float32 altitude float32 temperature -uint64 timestamp uint64 error_count diff --git a/msg/sensor_combined.msg b/msg/sensor_combined.msg index ec58e6f923..86682c0f11 100644 --- a/msg/sensor_combined.msg +++ b/msg/sensor_combined.msg @@ -1,74 +1,25 @@ -# Definition of the sensor_combined uORB topic. - -int32 MAGNETOMETER_MODE_NORMAL = 0 -int32 MAGNETOMETER_MODE_POSITIVE_BIAS = 1 -int32 MAGNETOMETER_MODE_NEGATIVE_BIAS = 2 - -uint32 SENSOR_PRIO_MIN = 0 -uint32 SENSOR_PRIO_VERY_LOW = 25 -uint32 SENSOR_PRIO_LOW = 50 -uint32 SENSOR_PRIO_DEFAULT = 75 -uint32 SENSOR_PRIO_HIGH = 100 -uint32 SENSOR_PRIO_VERY_HIGH = 125 -uint32 SENSOR_PRIO_MAX = 255 - -# Sensor readings in raw and SI-unit form. # -# These values are read from the sensors. Raw values are in sensor-specific units, -# the scaled values are in SI-units, as visible from the ending of the variable -# or the comments. The use of the SI fields is in general advised, as these fields -# are scaled and offset-compensated where possible and do not change with board -# revisions and sensor updates. +# Sensor readings in SI-unit form. # -# Actual data, this is specific to the type of data which is stored in this struct -# A line containing L0GME will be added by the Python logging code generator to the logged dataset. +# These fields are scaled and offset-compensated where possible and do not +# change with board revisions and sensor updates. # -# NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only -uint64 timestamp # Timestamp in microseconds since boot, from gyro -uint64[3] gyro_timestamp # Gyro timestamps -int16[9] gyro_raw # Raw sensor values of angular velocity -float32[9] gyro_rad_s # Angular velocity in radian per seconds -uint32[3] gyro_priority # Sensor priority -float32[9] gyro_integral_rad # delta angle in radians -uint64[3] gyro_integral_dt # delta time for gyro integral in us -uint32[3] gyro_errcount # Error counter for gyro 0 -float32[3] gyro_temp # Temperature of gyro 0 +int32 RELATIVE_TIMESTAMP_INVALID = 2147483647 # (0x7fffffff) If one of the relative timestamps is set to this value, it means the associated sensor values are invalid -int16[9] accelerometer_raw # Raw acceleration in NED body frame -float32[9] accelerometer_m_s2 # Acceleration in NED body frame, in m/s^2 -float32[9] accelerometer_integral_m_s # velocity in NED body frame, in m/s^2 -uint64[3] accelerometer_integral_dt # delta time for accel integral in us -int16[3] accelerometer_mode # Accelerometer measurement mode -float32[3] accelerometer_range_m_s2 # Accelerometer measurement range in m/s^2 -uint64[3] accelerometer_timestamp # Accelerometer timestamp -uint32[3] accelerometer_priority # Sensor priority -uint32[3] accelerometer_errcount # Error counter for accel 0 -float32[3] accelerometer_temp # Temperature of accel 0 -int16[9] magnetometer_raw # Raw magnetic field in NED body frame -float32[9] magnetometer_ga # Magnetic field in NED body frame, in Gauss -int16[3] magnetometer_mode # Magnetometer measurement mode -float32[3] magnetometer_range_ga # measurement range in Gauss -float32[3] magnetometer_cuttoff_freq_hz # Internal analog low pass frequency of sensor -uint64[3] magnetometer_timestamp # Magnetometer timestamp -uint32[3] magnetometer_priority # Sensor priority -uint32[3] magnetometer_errcount # Error counter for mag 0 -float32[3] magnetometer_temp # Temperature of mag 0 +# gyro timstamp is equal to the timestamp of the message +float32[3] gyro_rad # delta angle in the NED body frame in rad +float32 gyro_integral_dt # delta time for gyro integral in s -float32[3] baro_pres_mbar # Barometric pressure, already temp. comp. -float32[3] baro_alt_meter # Altitude, already temp. comp. -float32[3] baro_temp_celcius # Temperature in degrees celsius -uint64[3] baro_timestamp # Barometer timestamp -uint32[3] baro_priority # Sensor priority -uint32[3] baro_errcount # Error count in communication +int32 accelerometer_timestamp_relative # timestamp + accelerometer_timestamp_relative = Accelerometer timestamp +float32[3] accelerometer_m_s2 # velocity in NED body frame, in m/s^2 +float32 accelerometer_integral_dt # delta time for accel integral in s -float32[10] adc_voltage_v # ADC voltages of ADC Chan 10/11/12/13 or -1 -uint16[10] adc_mapping # Channel indices of each of these values -float32 mcu_temp_celcius # Internal temperature measurement of MCU +int32 magnetometer_timestamp_relative # timestamp + magnetometer_timestamp_relative = Magnetometer timestamp +float32[3] magnetometer_ga # Magnetic field in NED body frame, in Gauss + +int32 baro_timestamp_relative # timestamp + baro_timestamp_relative = Barometer timestamp +float32 baro_alt_meter # Altitude, already temp. comp. +float32 baro_temp_celcius # Temperature in degrees celsius -float32[3] differential_pressure_pa # Airspeed sensor differential pressure -uint64[3] differential_pressure_timestamp # Last measurement timestamp -float32[3] differential_pressure_filtered_pa # Low pass filtered airspeed sensor differential pressure reading -uint32[3] differential_pressure_priority # Sensor priority -uint32[3] differential_pressure_errcount # Error count in communication diff --git a/msg/sensor_gyro.msg b/msg/sensor_gyro.msg index badaec0eb5..31636d148a 100644 --- a/msg/sensor_gyro.msg +++ b/msg/sensor_gyro.msg @@ -1,4 +1,3 @@ -uint64 timestamp uint64 integral_dt # integration time uint64 error_count float32 x # angular velocity in the NED X board axis in rad/s diff --git a/msg/sensor_mag.msg b/msg/sensor_mag.msg index 8ea03d1fc3..662ebdbb79 100644 --- a/msg/sensor_mag.msg +++ b/msg/sensor_mag.msg @@ -1,4 +1,3 @@ -uint64 timestamp uint64 error_count float32 x float32 y diff --git a/msg/servorail_status.msg b/msg/servorail_status.msg index f473cc1e28..39edf80e4f 100644 --- a/msg/servorail_status.msg +++ b/msg/servorail_status.msg @@ -1,3 +1,2 @@ -uint64 timestamp # microseconds since system boot float32 voltage_v # Servo rail voltage in volts float32 rssi_v # RSSI pin voltage in volts diff --git a/msg/system_power.msg b/msg/system_power.msg index cb2a7c0eb0..e6648ceec7 100644 --- a/msg/system_power.msg +++ b/msg/system_power.msg @@ -1,4 +1,3 @@ -uint64 timestamp # microseconds since system boot float32 voltage5V_v # peripheral 5V rail voltage uint8 usb_connected # USB is connected when 1 uint8 brick_valid # brick power is good when 1 diff --git a/msg/tecs_status.msg b/msg/tecs_status.msg index 4dcd16b2ca..7576112c57 100644 --- a/msg/tecs_status.msg +++ b/msg/tecs_status.msg @@ -6,7 +6,6 @@ uint8 TECS_MODE_LAND_THROTTLELIM = 4 uint8 TECS_MODE_BAD_DESCENT = 5 uint8 TECS_MODE_CLIMBOUT = 6 -uint64 timestamp # timestamp, in microseconds since system start */ float32 altitudeSp float32 altitude_filtered diff --git a/msg/telemetry_status.msg b/msg/telemetry_status.msg index cdcef931af..e7c653c6f5 100644 --- a/msg/telemetry_status.msg +++ b/msg/telemetry_status.msg @@ -4,7 +4,6 @@ uint8 TELEMETRY_STATUS_RADIO_TYPE_UBIQUITY_BULLET = 2 uint8 TELEMETRY_STATUS_RADIO_TYPE_WIRE = 3 uint8 TELEMETRY_STATUS_RADIO_TYPE_USB = 4 -uint64 timestamp uint64 heartbeat_time # Time of last received heartbeat from remote system uint64 telem_time # Time of last received telemetry status packet, 0 for none uint8 type # type of the radio hardware diff --git a/msg/templates/px4/uorb/msg.h.template b/msg/templates/px4/uorb/msg.h.template index 2d42511074..1707dfc015 100644 --- a/msg/templates/px4/uorb/msg.h.template +++ b/msg/templates/px4/uorb/msg.h.template @@ -55,7 +55,6 @@ import genmsg.msgs import gencpp -cpp_class = 'px4_%s'%spec.short_name native_type = '%s_s'%spec.short_name topic_name = spec.short_name }@ @@ -74,6 +73,11 @@ topic_name = spec.short_name namespace px4 { +@[for multi_topic in topics]@ +@{ +cpp_class = 'px4_%s'%multi_topic +}@ + class __EXPORT @(cpp_class) : public PX4Message<@(native_type)> @@ -89,7 +93,8 @@ public: ~@(cpp_class)() {} - static PX4TopicHandle handle() {return ORB_ID(@(topic_name));} + static PX4TopicHandle handle() {return ORB_ID(@(multi_topic));} }; +@[end for] }; diff --git a/msg/templates/uorb/msg.cpp.template b/msg/templates/uorb/msg.cpp.template new file mode 100644 index 0000000000..67789a8152 --- /dev/null +++ b/msg/templates/uorb/msg.cpp.template @@ -0,0 +1,79 @@ +@############################################### +@# +@# PX4 ROS compatible message source code +@# generation for C++ +@# +@# EmPy template for generating .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 +@# - search_path (dict) search paths for genmsg +@# - topics (List of String) multi-topic names +@############################################### +/**************************************************************************** + * + * Copyright (C) 2013-2016 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 +from px_generate_uorb_topic_helper import * # this is in Tools/ + +uorb_struct = '%s_s'%spec.short_name +topic_name = spec.short_name + +sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True) +struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path) +topic_fields = ["uint64_t timestamp"]+["%s %s" % (convert_type(field.type), field.name) for field in sorted_fields] +}@ + +#include +#include +#include + + +@# join all msg files in one line e.g: "float[3] position;float[3] velocity;bool armed" +@# This is used for the logger +const char *__orb_@(topic_name)_fields = "@( ";".join(topic_fields) );"; + +@[for multi_topic in topics]@ +ORB_DEFINE(@multi_topic, struct @uorb_struct, @(struct_size-padding_end_size), + __orb_@(topic_name)_fields); +@[end for] diff --git a/msg/templates/uorb/msg.h.template b/msg/templates/uorb/msg.h.template index 78e1992e06..b890bf88b5 100644 --- a/msg/templates/uorb/msg.h.template +++ b/msg/templates/uorb/msg.h.template @@ -13,10 +13,12 @@ @# - file_name_in (String) Source file @# - spec (msggen.MsgSpec) Parsed specification of the .msg file @# - md5sum (String) MD5Sum of the .msg specification +@# - search_path (dict) search paths for genmsg +@# - topics (List of String) multi-topic names @############################################### /**************************************************************************** * - * Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-2016 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 @@ -52,6 +54,7 @@ @{ import genmsg.msgs import gencpp +from px_generate_uorb_topic_helper import * # this is in Tools/ uorb_struct = '%s_s'%spec.short_name topic_name = spec.short_name @@ -63,6 +66,12 @@ topic_name = spec.short_name @# Generic Includes @############################## #include +#ifdef __cplusplus +#include +#else +#include +#endif + #include @############################## @@ -70,12 +79,11 @@ topic_name = spec.short_name @############################## @{ 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 '%(name)) - + 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 '%(name)) }@ @# Constants c style @@ -85,61 +93,21 @@ for field in spec.parsed_fields(): @[end for] #endif -/** - * @@addtogroup topics - * @@{ - */ - @############################## @# Main struct of message @############################## @{ -type_map = {'int8': 'int8_t', - 'int16': 'int16_t', - 'int32': 'int32_t', - 'int64': 'int64_t', - 'uint8': 'uint8_t', - 'uint16': 'uint16_t', - 'uint32': 'uint32_t', - 'uint64': 'uint64_t', - 'float32': 'float', - 'float64': 'double', - 'bool': 'bool', - 'char': 'char', - 'fence_vertex': 'fence_vertex', - 'position_setpoint': 'position_setpoint', - 'esc_report': 'esc_report'} +def print_parsed_fields(): + # sort fields (using a stable sort) + sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True) + struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path) + # loop over all fields and print the type and name + for field in sorted_fields: + if (not field.is_header): + print_field_def(field) +}@ -# 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) { @@ -147,31 +115,26 @@ struct __EXPORT @(uorb_struct) { #else struct @(uorb_struct) { #endif -@{ -# loop over all fields and print the type and name -for field in spec.parsed_fields(): - if (not field.is_header): - print_field_def(field) -}@ +@# timestamp at the beginning of each topic is required for logger + uint64_t timestamp; // required for logger +@print_parsed_fields() #ifdef __cplusplus @# Constants again c++-ified @{ for constant in spec.constants: - type = constant.type - if type in type_map: - # 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)) + type_name = constant.type + if type_name in type_map: + # need to add _t: int8 --> int8_t + type_px4 = type_map[type_name] + else: + raise Exception("Type {0} not supported, add to to template file!".format(type_name)) - print('\tstatic const %s %s = %s;'%(type_px4, constant.name, int(constant.val))) + print('\tstatic const %s %s = %s;'%(type_px4, constant.name, int(constant.val))) } #endif }; -/** - * @@} - */ - /* register this as object request broker structure */ -ORB_DECLARE(@(topic_name)); +@[for multi_topic in topics]@ +ORB_DECLARE(@multi_topic); +@[end for] diff --git a/msg/templates/uorb/uORBTopics.cpp.template b/msg/templates/uorb/uORBTopics.cpp.template new file mode 100644 index 0000000000..91a3a7e640 --- /dev/null +++ b/msg/templates/uorb/uORBTopics.cpp.template @@ -0,0 +1,70 @@ +@############################################### +@# +@# EmPy template for generating uORBTopics.cpp file +@# for logging purposes +@# +@############################################### +@# Start of Template +@# +@# Context: +@# - msgs (List) list of all msg files +@############################################### +/**************************************************************************** + * + * Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +@{ +msgs_count = len(msgs) +msg_names = [mn.replace(".msg", "") for mn in msgs] +}@ +@[for msg_name in msg_names]@ +#include +@[end for] + +const size_t _uorb_topics_count = @(msgs_count); +const struct orb_metadata* _uorb_topics_list[_uorb_topics_count] = { +@[for idx, msg_name in enumerate(msg_names, 1)]@ + ORB_ID(@(msg_name))@[if idx != msgs_count],@[end if] +@[end for] +}; + +size_t orb_topics_count() +{ + return _uorb_topics_count; +} + +const struct orb_metadata **orb_get_topics() +{ + return _uorb_topics_list; +} diff --git a/msg/test_motor.msg b/msg/test_motor.msg index ec06e64fd6..2805fa64f5 100644 --- a/msg/test_motor.msg +++ b/msg/test_motor.msg @@ -1,5 +1,4 @@ uint8 NUM_MOTOR_OUTPUTS = 8 -uint64 timestamp # output timestamp in us since system boot uint32 motor_number # number of motor to spin float32 value # output power, range [0..1] diff --git a/msg/transponder_report.msg b/msg/transponder_report.msg index eac37d713f..c02fb8cc15 100644 --- a/msg/transponder_report.msg +++ b/msg/transponder_report.msg @@ -1,4 +1,3 @@ -uint64 timestamp # Timestamp of this position report uint32 ICAO_address # ICAO address float64 lat # Latitude, expressed as degrees float64 lon # Longitude, expressed as degrees diff --git a/msg/vehicle_attitude.msg b/msg/vehicle_attitude.msg index 0ee90cd61d..2c64c32e13 100644 --- a/msg/vehicle_attitude.msg +++ b/msg/vehicle_attitude.msg @@ -1,5 +1,4 @@ # This is similar to the mavlink message ATTITUDE, but for onboard use */ -uint64 timestamp # in microseconds since system start # @warning roll, pitch and yaw have always to be valid, the rotation matrix and quaternion are optional float32 roll # Roll angle (rad, Tait-Bryan, NED) float32 pitch # Pitch angle (rad, Tait-Bryan, NED) diff --git a/msg/vehicle_attitude_setpoint.msg b/msg/vehicle_attitude_setpoint.msg index 6c12b225c8..2bbbe748fd 100644 --- a/msg/vehicle_attitude_setpoint.msg +++ b/msg/vehicle_attitude_setpoint.msg @@ -8,7 +8,6 @@ # ############################################################################################### -uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data float32 roll_body # body angle in NED frame float32 pitch_body # body angle in NED frame @@ -35,3 +34,6 @@ bool fw_control_yaw # control heading with rudder (used for auto takeoff on bool disable_mc_yaw_control # control yaw for mc (used for vtol weather-vane mode) bool apply_flaps + +# WAS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint +# TOPICS vehicle_attitude_setpoint diff --git a/msg/vehicle_control_mode.msg b/msg/vehicle_control_mode.msg index 5ad1bbb9b2..32ec589061 100644 --- a/msg/vehicle_control_mode.msg +++ b/msg/vehicle_control_mode.msg @@ -1,5 +1,4 @@ -uint64 timestamp # in microseconds since system start # is set whenever the writing thread stores new data bool flag_armed diff --git a/msg/vehicle_global_position.msg b/msg/vehicle_global_position.msg index 8aa7917df0..39b815015a 100644 --- a/msg/vehicle_global_position.msg +++ b/msg/vehicle_global_position.msg @@ -4,7 +4,6 @@ # estimator, which will take more sources of information into account than just GPS, # e.g. control inputs of the vehicle in a Kalman-filter implementation. # -uint64 timestamp # Time of this estimate since system start, (microseconds) uint64 time_utc_usec # GPS UTC timestamp, (microseconds) float64 lat # Latitude, (degrees) float64 lon # Longitude, (degrees) diff --git a/msg/vehicle_gps_position.msg b/msg/vehicle_gps_position.msg index ab8a92873f..c57a078033 100644 --- a/msg/vehicle_gps_position.msg +++ b/msg/vehicle_gps_position.msg @@ -1,11 +1,10 @@ # GPS position in WGS84 coordinates. -uint64 timestamp_position # Time of the position estimates since system start, (microseconds) +# the auto-generated field 'timestamp' is for the position & velocity (microseconds) int32 lat # Latitude in 1E-7 degrees int32 lon # Longitude in 1E-7 degrees int32 alt # Altitude in 1E-3 meters above MSL, (millimetres) int32 alt_ellipsoid # Altitude in 1E-3 meters bove Ellipsoid, (millimetres) -uint64 timestamp_variance # Time of the accuracy estimates since system start, (microseconds) float32 s_variance_m_s # GPS speed accuracy estimate, (metres/sec) float32 c_variance_rad # GPS course accuracy estimate, (radians) uint8 fix_type # 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic, float, 6: Real-Time Kinematic, fixed, 8: Extrapolated. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. @@ -19,7 +18,6 @@ float32 vdop # Vertical dilution of precision int32 noise_per_ms # GPS noise per millisecond int32 jamming_indicator # indicates jamming is occurring -uint64 timestamp_velocity # Time of the velocity estimates since system start, (microseconds) float32 vel_m_s # GPS ground speed, (metres/sec) float32 vel_n_m_s # GPS North velocity, (metres/sec) float32 vel_e_m_s # GPS East velocity, (metres/sec) @@ -27,7 +25,7 @@ float32 vel_d_m_s # GPS Down velocity, (metres/sec) float32 cog_rad # Course over ground (NOT heading, but direction of movement), -PI..PI, (radians) bool vel_ned_valid # True if NED velocity is valid -uint64 timestamp_time # Time of the UTC timestamp since system start, (microseconds) +int32 timestamp_time_relative # timestamp + timestamp_time_relative = Time of the UTC timestamp since system start, (microseconds) uint64 time_utc_usec # Timestamp (microseconds, UTC), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0 uint8 satellites_used # Number of satellites used diff --git a/msg/vehicle_land_detected.msg b/msg/vehicle_land_detected.msg index d886d31b05..e7f741b8bf 100644 --- a/msg/vehicle_land_detected.msg +++ b/msg/vehicle_land_detected.msg @@ -1,3 +1,2 @@ -uint64 timestamp # timestamp of the setpoint bool landed # true if vehicle is currently landed on the ground bool freefall # true if vehicle is currently in free-fall diff --git a/msg/vehicle_local_position.msg b/msg/vehicle_local_position.msg index 89c141c417..bf107056e2 100644 --- a/msg/vehicle_local_position.msg +++ b/msg/vehicle_local_position.msg @@ -1,6 +1,5 @@ # Fused local position in NED. -uint64 timestamp # Time of this estimate since system start, (microseconds) bool xy_valid # true if x and y are valid bool z_valid # true if z is valid bool v_xy_valid # true if vy and vy are valid diff --git a/msg/vehicle_local_position_setpoint.msg b/msg/vehicle_local_position_setpoint.msg index 927e9ab5ad..a5407519bb 100644 --- a/msg/vehicle_local_position_setpoint.msg +++ b/msg/vehicle_local_position_setpoint.msg @@ -1,6 +1,5 @@ # Local position setpoint in NED frame -uint64 timestamp # timestamp of the setpoint float32 x # in meters NED float32 y # in meters NED float32 z # in meters NED diff --git a/msg/vehicle_rates_setpoint.msg b/msg/vehicle_rates_setpoint.msg index 4fa09b563f..369b68270a 100644 --- a/msg/vehicle_rates_setpoint.msg +++ b/msg/vehicle_rates_setpoint.msg @@ -8,7 +8,6 @@ # ############################################################################################### -uint64 timestamp # in microseconds since system start float32 roll # body angular rates in NED frame float32 pitch # body angular rates in NED frame diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index f5b1666085..0444a577e1 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -41,7 +41,6 @@ uint8 RC_IN_MODE_GENERATED = 2 # state machine / state of vehicle. # Encodes the complete system state and is set by the commander app. -uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data uint8 nav_state # set navigation state machine to specified value uint8 arming_state # current arming state @@ -70,6 +69,3 @@ bool mission_failure # Set to true if mission could not continue/finish uint32 onboard_control_sensors_present uint32 onboard_control_sensors_enabled uint32 onboard_control_sensors_health - -float32 load # processor load from 0 to 1 - diff --git a/msg/vision_position_estimate.msg b/msg/vision_position_estimate.msg index 53e2b9c0f0..7e3ee14766 100644 --- a/msg/vision_position_estimate.msg +++ b/msg/vision_position_estimate.msg @@ -1,7 +1,6 @@ uint32 id # ID of the estimator, commonly the component ID of the incoming message -uint64 timestamp_boot # time of this estimate, in microseconds since system start -uint64 timestamp_computer # timestamp provided by the companion computer, in us +uint64 timestamp_received # timestamp when the estimate was received float32 x # X position in meters in NED earth-fixed frame float32 y # Y position in meters in NED earth-fixed frame @@ -12,3 +11,6 @@ float32 vy # Y velocity in meters per second in NED earth-fixed frame float32 vz # Z velocity in meters per second in NED earth-fixed frame float32[4] q # Estimated attitude as quaternion + +float32 pos_err # position error 1-std for each axis (metres) +float32 ang_err # angular error 1-std for each axis (rad) diff --git a/msg/vtol_vehicle_status.msg b/msg/vtol_vehicle_status.msg index 27b804c437..5334e291ca 100644 --- a/msg/vtol_vehicle_status.msg +++ b/msg/vtol_vehicle_status.msg @@ -5,7 +5,6 @@ uint8 VEHICLE_VTOL_STATE_TRANSITION_TO_MC = 2 uint8 VEHICLE_VTOL_STATE_MC = 3 uint8 VEHICLE_VTOL_STATE_FW = 4 -uint64 timestamp # Microseconds since system boot bool vtol_in_rw_mode # true: vtol vehicle is in rotating wing mode bool vtol_in_trans_mode bool vtol_transition_failsafe # vtol in transition failsafe mode diff --git a/msg/wind_estimate.msg b/msg/wind_estimate.msg index fbd7b638b8..a61df3252a 100644 --- a/msg/wind_estimate.msg +++ b/msg/wind_estimate.msg @@ -1,4 +1,3 @@ -uint64 timestamp # Microseconds since system boot float32 windspeed_north # Wind component in north / X direction float32 windspeed_east # Wind component in east / Y direction float32 covariance_north # Uncertainty - set to zero (no uncertainty) if not estimated diff --git a/nuttx-configs/aerocore/nsh/defconfig b/nuttx-configs/aerocore/nsh/defconfig index 655dd1406e..6be8f217ba 100644 --- a/nuttx-configs/aerocore/nsh/defconfig +++ b/nuttx-configs/aerocore/nsh/defconfig @@ -589,12 +589,7 @@ CONFIG_FAT_LFN=y CONFIG_FAT_MAXFNAME=32 CONFIG_FS_FATTIME=y CONFIG_FAT_DMAMEMORY=y -CONFIG_FS_NXFFS=y -CONFIG_NXFFS_PREALLOCATED=y -CONFIG_NXFFS_ERASEDSTATE=0xff -CONFIG_NXFFS_PACKTHRESHOLD=32 -CONFIG_NXFFS_MAXNAMLEN=32 -CONFIG_NXFFS_TAILTHRESHOLD=2048 +# CONFIG_FS_NXFFS is not set CONFIG_FS_ROMFS=y # CONFIG_FS_SMARTFS is not set CONFIG_FS_BINFS=y diff --git a/nuttx-configs/mindpx-v2/nsh/defconfig b/nuttx-configs/mindpx-v2/nsh/defconfig index 251609002e..9d97d4543f 100644 --- a/nuttx-configs/mindpx-v2/nsh/defconfig +++ b/nuttx-configs/mindpx-v2/nsh/defconfig @@ -440,7 +440,7 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=42 +CONFIG_NFILE_DESCRIPTORS=51 CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 @@ -703,12 +703,7 @@ CONFIG_FAT_LFN=y CONFIG_FAT_MAXFNAME=32 CONFIG_FS_FATTIME=y CONFIG_FAT_DMAMEMORY=y -CONFIG_FS_NXFFS=y -CONFIG_NXFFS_PREALLOCATED=y -CONFIG_NXFFS_ERASEDSTATE=0xff -CONFIG_NXFFS_PACKTHRESHOLD=32 -CONFIG_NXFFS_MAXNAMLEN=32 -CONFIG_NXFFS_TAILTHRESHOLD=2048 +# CONFIG_FS_NXFFS is not set CONFIG_FS_ROMFS=y # CONFIG_FS_SMARTFS is not set CONFIG_FS_BINFS=y diff --git a/nuttx-configs/mindpx-v2/scripts/ld.script b/nuttx-configs/mindpx-v2/scripts/ld.script index b04ad89a6c..ab66d2dc61 100644 --- a/nuttx-configs/mindpx-v2/scripts/ld.script +++ b/nuttx-configs/mindpx-v2/scripts/ld.script @@ -50,8 +50,7 @@ MEMORY { - /* disabled due to silicon errata flash (rx) : ORIGIN = 0x08004000, LENGTH = 2032K */ - flash (rx) : ORIGIN = 0x08004000, LENGTH = 1008K + flash (rx) : ORIGIN = 0x08004000, LENGTH = 2032K sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K } diff --git a/nuttx-configs/px4-stm32f4discovery/nsh/defconfig b/nuttx-configs/px4-stm32f4discovery/nsh/defconfig index 5285fae077..b81038bd90 100644 --- a/nuttx-configs/px4-stm32f4discovery/nsh/defconfig +++ b/nuttx-configs/px4-stm32f4discovery/nsh/defconfig @@ -555,12 +555,7 @@ CONFIG_FS_FAT=y # CONFIG_FAT_LFN is not set # CONFIG_FS_FATTIME is not set # CONFIG_FAT_DMAMEMORY is not set -CONFIG_FS_NXFFS=y -CONFIG_NXFFS_PREALLOCATED=y -CONFIG_NXFFS_ERASEDSTATE=0xff -CONFIG_NXFFS_PACKTHRESHOLD=32 -CONFIG_NXFFS_MAXNAMLEN=32 -CONFIG_NXFFS_TAILTHRESHOLD=2048 +# CONFIG_FS_NXFFS is not set CONFIG_FS_ROMFS=y # CONFIG_FS_SMARTFS is not set CONFIG_FS_BINFS=y diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index b89b4039d8..e172c3bcfb 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -625,12 +625,7 @@ CONFIG_FAT_LFN=y CONFIG_FAT_MAXFNAME=32 CONFIG_FS_FATTIME=y # CONFIG_FAT_DMAMEMORY is not set -CONFIG_FS_NXFFS=y -CONFIG_NXFFS_PREALLOCATED=y -CONFIG_NXFFS_ERASEDSTATE=0xff -CONFIG_NXFFS_PACKTHRESHOLD=32 -CONFIG_NXFFS_MAXNAMLEN=32 -CONFIG_NXFFS_TAILTHRESHOLD=2048 +# CONFIG_FS_NXFFS is not set CONFIG_FS_ROMFS=y # CONFIG_FS_SMARTFS is not set CONFIG_FS_BINFS=y diff --git a/nuttx-configs/px4fmu-v2/include/board.h b/nuttx-configs/px4fmu-v2/include/board.h index 52668cacd2..e84a5cb517 100755 --- a/nuttx-configs/px4fmu-v2/include/board.h +++ b/nuttx-configs/px4fmu-v2/include/board.h @@ -294,7 +294,7 @@ extern "C" { * * Description: * All STM32 architectures must provide the following entry point. This entry point - * is called early in the intitialization -- after all memory has been configured + * is called early in the initialization -- after all memory has been configured * and mapped but before any devices have been initialized. * ************************************************************************************/ diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 950f17b907..edc4c2c9c5 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -442,7 +442,7 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=42 +CONFIG_NFILE_DESCRIPTORS=51 CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 @@ -705,12 +705,7 @@ CONFIG_FAT_LFN=y CONFIG_FAT_MAXFNAME=32 CONFIG_FS_FATTIME=y CONFIG_FAT_DMAMEMORY=y -CONFIG_FS_NXFFS=y -CONFIG_NXFFS_PREALLOCATED=y -CONFIG_NXFFS_ERASEDSTATE=0xff -CONFIG_NXFFS_PACKTHRESHOLD=32 -CONFIG_NXFFS_MAXNAMLEN=32 -CONFIG_NXFFS_TAILTHRESHOLD=2048 +# CONFIG_FS_NXFFS is not set CONFIG_FS_ROMFS=y # CONFIG_FS_SMARTFS is not set CONFIG_FS_BINFS=y diff --git a/nuttx-configs/px4fmu-v4/nsh/defconfig b/nuttx-configs/px4fmu-v4/nsh/defconfig index bb7e077056..2520f708fb 100644 --- a/nuttx-configs/px4fmu-v4/nsh/defconfig +++ b/nuttx-configs/px4fmu-v4/nsh/defconfig @@ -442,7 +442,7 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=42 +CONFIG_NFILE_DESCRIPTORS=51 CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 @@ -705,12 +705,7 @@ CONFIG_FAT_LFN=y CONFIG_FAT_MAXFNAME=32 CONFIG_FS_FATTIME=y CONFIG_FAT_DMAMEMORY=y -CONFIG_FS_NXFFS=y -CONFIG_NXFFS_PREALLOCATED=y -CONFIG_NXFFS_ERASEDSTATE=0xff -CONFIG_NXFFS_PACKTHRESHOLD=32 -CONFIG_NXFFS_MAXNAMLEN=32 -CONFIG_NXFFS_TAILTHRESHOLD=2048 +# CONFIG_FS_NXFFS is not set CONFIG_FS_ROMFS=y # CONFIG_FS_SMARTFS is not set CONFIG_FS_BINFS=y diff --git a/nuttx-configs/tap-v1/include/board.h b/nuttx-configs/tap-v1/include/board.h new file mode 100644 index 0000000000..5c7467f9d0 --- /dev/null +++ b/nuttx-configs/tap-v1/include/board.h @@ -0,0 +1,432 @@ +/************************************************************************************ + * configs/tap-v1/include/board.h + * include/arch/board/board.h + * + * Copyright (C) 2012-2016 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * David Sidrane + * + * 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. + * + ************************************************************************************/ + +#ifndef __CONFIG_TAP_V1_INCLUDE_BOARD_H +#define __CONFIG_TAP_V1_INCLUDE_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#ifndef __ASSEMBLY__ +# include +#endif + +#include "stm32_rcc.h" +#include "stm32_sdio.h" +#include "stm32.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ +/* The TAP V1 uses a 16MHz crystal connected to the HSE. + * + * This is the canonical configuration: + * System Clock source : PLL (HSE) + * SYSCLK(Hz) : 168000000 Determined by PLL configuration + * HCLK(Hz) : 168000000 (STM32_RCC_CFGR_HPRE) + * AHB Prescaler : 1 (STM32_RCC_CFGR_HPRE) + * APB1 Prescaler : 4 (STM32_RCC_CFGR_PPRE1) + * APB2 Prescaler : 2 (STM32_RCC_CFGR_PPRE2) + * HSE Frequency(Hz) : 16000000 (STM32_BOARD_XTAL) + * PLLM : 8 (STM32_PLLCFG_PLLM) + * PLLN : 168 (STM32_PLLCFG_PLLN) + * PLLP : 2 (STM32_PLLCFG_PLLP) + * PLLQ : 7 (STM32_PLLCFG_PLLQ) + * Main regulator output voltage : Scale1 mode Needed for high speed SYSCLK + * Flash Latency(WS) : 5 + * Prefetch Buffer : OFF + * Instruction cache : ON + * Data cache : ON + * Require 48MHz for USB OTG FS, : Enabled + * SDIO and RNG clock + */ + +/* HSI - 16 MHz RC factory-trimmed + * LSI - 32 KHz RC + * HSE - On-board crystal frequency is 16MHz + * LSE - not installed + */ + +#define STM32_BOARD_XTAL 16000000ul + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL + +/* Main PLL Configuration. + * + * PLL source is HSE + * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * = (16,000,000 / 8) * 168 + * = 336,000,000 + * SYSCLK = PLL_VCO / PLLP + * = 336,000,000 / 2 = 168,000,000 + * USB OTG FS, SDIO and RNG Clock + * = PLL_VCO / PLLQ + * = 336,000,000 / 7 + * = 48,000,000 + */ + +#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(8) +#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(168) +#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_2 +#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(7) + +#define STM32_SYSCLK_FREQUENCY 168000000ul + +/* AHB clock (HCLK) is SYSCLK (168MHz) */ + +#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK /* HCLK = SYSCLK / 1 */ +#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/4 (42MHz) */ + +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd4 /* PCLK1 = HCLK / 4 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/4) + +/* Timers driven from APB1 will be twice PCLK1 */ + +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* APB2 clock (PCLK2) is HCLK/2 (84MHz) */ + +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timers driven from APB2 will be twice PCLK2 */ + +#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) + +/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx + * 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) + +/* LED definitions ******************************************************************/ +/* If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in any + * way. The following definitions are used to access individual LEDs. + */ + +/* LED index values for use with stm32_setled() + * + * PC4 BLUE_LED D4 Blue LED cathode + * PC5 RED_LED D5 Red LED cathode +*/ +#define BOARD_LED1 0 +#define BOARD_LED2 1 +#define BOARD_NLEDS 2 + +#define BOARD_LED_BLUE BOARD_LED1 +#define BOARD_LED_RED BOARD_LED2 + +/* LED bits for use with stm32_setleds() */ + +#define BOARD_LED1_BIT (1 << BOARD_LED1) +#define BOARD_LED2_BIT (1 << BOARD_LED2) + +/* If CONFIG_ARCH_LEDs is defined, then NuttX will control the 2 LEDs on board + * the tap-v1. The following definitions describe how NuttX controls + * the LEDs: + */ + +#define LED_STARTED 0 /* BLUE */ +#define LED_HEAPALLOCATE 1 /* LED2 */ +#define LED_IRQSENABLED 2 /* BLUE */ +#define LED_STACKCREATED 3 /* BLUE + RED */ +#define LED_INIRQ 4 /* BLUE */ +#define LED_SIGNAL 5 /* RED */ +#define LED_ASSERTION 6 /* BLUE + RED */ +#define LED_PANIC 7 /* BLUE + RED */ + +/* Alternate function pin selections ************************************************/ + +/* + * USARTs. + * + * + * Peripheral Port Signal Name CONN + * USART1_TX PB6 GPS_USART1_TX JP1-15,16 + * USART1_RX PB7 GPS_USART1_RX JP1-13,14 + * USART2_TX PA2 GB_USART2_TX JP2-19,20 + * USART2_RX PA3 GB_USART2_RX JP2-21,22 + * USART3_TX PC10 RF2_USART3_TX J3-2 + * USART3_RX PC11 RF2_USART3_RX J3-1 + * USART6_TX PC6 RF_USART6_TX JP2-15,16 + * USART6_RX PC7 RF_USART6_RX JP2-17,18 +*/ + +#define GPIO_USART1_TX GPIO_USART1_TX_2 +#define GPIO_USART1_RX GPIO_USART1_RX_2 + +#define GPIO_USART2_TX GPIO_USART2_TX_1 +#define GPIO_USART2_RX GPIO_USART2_RX_1 + +#define GPIO_USART3_TX GPIO_USART3_TX_2 +#define GPIO_USART3_RX GPIO_USART3_RX_2 + +#define GPIO_USART6_TX GPIO_USART6_TX_1 +#define GPIO_USART6_RX GPIO_USART6_RX_1 + +/* USART DMA configuration for USART 1 and 6 */ + +#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 +#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 + +/* + * UARTs. + * + * N.B. The 's' is here to match the wrong labeling on Schematic + * + * Peripheral Port Signal Name CONN + * UART4_TX PA0 OFS_UsART4_TX JP1-19,20 + * UART4_RX PA1 OFS_UsART4_RX JP1-17,18 + * UART5_TX PC12 ESC_UsART5_TX U7-HCT244 etal ESC + * UART5_RX PD2 ESC_UsART5_RX U8-5 74HCT151 + * + * Note that UART5 has no optional pinout, so it is not listed here. + * +*/ + +#define GPIO_UART4_TX GPIO_UART4_TX_1 +#define GPIO_UART4_RX GPIO_UART4_RX_1 + +/* + * I2C + * + * Peripheral Port Signal Name CONN + * I2C1_SDA PB9 I2C1_SDA J2-4,9,16,21 mpu6050, U4 MS6507 + * I2C1_SDL PB8 I2C1_SCL J2-3,10,15,22 mpu6050, U4 MS6507 + * I2C2_SDA PB11 Sonar Echo/I2C_SDA JP2-31,32 + * I2C2_SDL PB10 Sonar Trig/I2C_SCL JP2-29,30 + * I2C3_SDA PC9 COMPASS_I2C3_SDA JP1-27,28 + * I2C3_SDL PA8 COMPASS_I2C3_SCL JP1-25,26 + * + * 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_SDA GPIO_I2C1_SDA_2 +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 +#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9) +#define GPIO_I2C1_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN8) + +#define GPIO_I2C2_SDA GPIO_I2C2_SDA_1 +#define GPIO_I2C2_SCL GPIO_I2C2_SCL_1 +#define GPIO_I2C2_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11) +#define GPIO_I2C2_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN10) + +#define GPIO_I2C3_SDA GPIO_I2C3_SDA_1 +#define GPIO_I2C3_SCL GPIO_I2C3_SCL_1 +#define GPIO_I2C3_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN9) +#define GPIO_I2C3_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN8) + +/* + * SPI + * + * Peripheral Port Signal Name CONN + * SPI2_NSS PB12 SD_SPI2_NSS SD-2 CS + * SPI2_SCK PB13 SD_SPI2_SCK SD-5 CLK + * SPI2_MISO PB14 SD_SPI2_MISO SD-7 D0 + * SPI2_MOSI PB15 SD_SPI2_MOSI SD-3 DI + * + */ + +#define GPIO_SPI2_NSS (GPIO_SPI2_NSS_1 | GPIO_SPEED_50MHz) +#define GPIO_SPI2_SCK (GPIO_SPI2_SCK_2 | 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) + +/* SPI DMA configuration for SPI2 (microSD) */ +#define DMACHAN_SPI2_RX DMAMAP_SPI2_RX +#define DMACHAN_SPI2_TX DMAMAP_SPI2_TX + +/* The following Pin Mapping is just for completeness */ +/* + * JTAG + * + * We will only enable SW-DP, JTAG-DP will be disabled + * + * Function Port Signal Name CONN + * SWDIO PA13 DAT J10-3,J7 + * SWCLK PA14 CLK J10-4,J8 + * + */ + +/* + * BOOT + * + * Function Port Signal Name CONN + * BOOT0 NA BOOT0 GND via 10 K + * BOOT1 PB2 BOOT1 V3.3 - 10 K + * + * As jumpered the device can only boot from FLASH. + * + * It can be booted to: + * + * SRAM if BOOT0 is pulled High with 1K. + * System memory if: + * BOOT0 is pulled High with 1K and + * BOOT1 is pulled Low with 1K +*/ + +/* + * Timer PWM + * + * Peripheral Port Signal Name CONN + * TIM3_CH1 PA6 LED_R JP2-23,24 + * TIM3_CH2 PA7 LED_G JP2-25,26 + * TIM3_CH3 PB0 LED_B JP2-27,28 + * TIM3_CH4 PB1 nPWM_1 AUX1(Landing Gear) JP1-21,22 + */ + +/* + * GPIO + * + * Port Signal Name CONN + * PA4 POWER JP1-23, - Must be held High to run w/o USB + * PB4 TEMP_CONT J2-2,11,14,23 - Gyro Heater + * PC0 VOLTAGE JP2-13,14 - 1.84 @16.66 1.67 @15.12 Scale 0.1105 + * PC1 KEY_AD JP1-31,32 - Low when Power button is depressed + * PC2 SD_SW SD-9 SW - Card Present + * PC3 PCON_RADIO JP1-29,30 + * PC13 S2 U8-9 74HCT151 + * PC14 S1 U8-10 74HCT151 + * PC15 S0 U8-11 74HCT151 + */ + +/* + * USB + * + * Port Signal Name CONN + * PA9 OTG_FS_VBUS J1-1 + * PA10 OTG_FS_ID J1-4 + * PA11 OTG_FS_DM J1-2 + * PA12 OTG_FS_DP J1-3 + */ + +/* + * UNUSED PINS - In an idle world - these would have been tied to pads to + * facilitate debugging probs. + * Port + * PA15 + * PB3 + * PB5 + * PC8 +*/ + +/* 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_PORTA|GPIO_PIN15) +# define PROBE_2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN3) +# define PROBE_3 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN5) +# define PROBE_4 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN8) + +# 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); } \ + } while(0) + +# define PROBE(n,s) do {stm32_gpiowrite(PROBE_##n,(s));}while(0) +# define PROBE_MARK(n) PROBE(n,false);PROBE(n,true) +#else +# define PROBE_INIT(mask) +# define PROBE(n,s) +# define PROBE_MARK(n) +#endif + +/************************************************************************************ + * Public Data + ************************************************************************************/ + +#ifndef __ASSEMBLY__ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +/************************************************************************************ + * Public Function Prototypes + ************************************************************************************/ +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the initialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +EXTERN void stm32_boardinitialize(void); + +#undef EXTERN +#if defined(__cplusplus) +} +#endif + +#endif /* __ASSEMBLY__ */ +#endif /* __CONFIG_TAP_V1_INCLUDE_BOARD_H */ diff --git a/nuttx-configs/tap-v1/include/nsh_romfsimg.h b/nuttx-configs/tap-v1/include/nsh_romfsimg.h new file mode 100644 index 0000000000..beb1a924cf --- /dev/null +++ b/nuttx-configs/tap-v1/include/nsh_romfsimg.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * Copyright (C) 2016 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. + * + ****************************************************************************/ + +/** + * nsh_romfsetc.h + * + * This file is a stub for 'make export' purposes; the actual ROMFS + * must be supplied by the library client. + */ + +extern unsigned char romfs_img[]; +extern unsigned int romfs_img_len; diff --git a/nuttx-configs/tap-v1/nsh/Make.defs b/nuttx-configs/tap-v1/nsh/Make.defs new file mode 100644 index 0000000000..7601fcc095 --- /dev/null +++ b/nuttx-configs/tap-v1/nsh/Make.defs @@ -0,0 +1,160 @@ +############################################################################ +# configs/tap-v1/nsh/Make.defs +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# +############################################################################ + +include ${TOPDIR}/.config +include ${TOPDIR}/tools/Config.mk + +include $(TOPDIR)/PX4_Warnings.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 + +MAXOPTIMIZATION = -Os +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 + +# pull in *just* libm from the toolchain ... this is grody +LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" +EXTRA_LIBS += $(LIBM) + +# 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)}" +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) + 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) + 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} + +# optimisation 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 +endif + +ARCHCFLAGS = -std=gnu99 +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x +ARCHWARNINGS = $(PX4_ARCHWARNINGS) +ARCHCWARNINGS = $(PX4_ARCHWARNINGS) $(PX4_ARCHCWARNINGS) +ARCHWARNINGSXX = $(ARCHWARNINGS) $(PX4_ARCHWARNINGSXX) +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 +endef + +HOSTCC = gcc +HOSTINCLUDES = -I. +HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe +HOSTLDFLAGS = + diff --git a/nuttx-configs/tap-v1/nsh/defconfig b/nuttx-configs/tap-v1/nsh/defconfig new file mode 100644 index 0000000000..d1076b2aa4 --- /dev/null +++ b/nuttx-configs/tap-v1/nsh/defconfig @@ -0,0 +1,982 @@ +# +# Automatically generated file; DO NOT EDIT. +# Nuttx/ Configuration +# +CONFIG_NUTTX_NEWCONFIG=y + +# +# Build Setup +# +# CONFIG_EXPERIMENTAL 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_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 + +# +# Customize Header Files +# +# 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 + +# +# Debug Options +# +# CONFIG_DEBUG is not set +CONFIG_DEBUG_SYMBOLS=y + +# +# System Type +# +# CONFIG_ARCH_8051 is not set +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" + +# +# ARM Options +# +# CONFIG_ARCH_CHIP_C5471 is not set +# CONFIG_ARCH_CHIP_CALYPSO is not set +# CONFIG_ARCH_CHIP_DM320 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_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_SAM34 is not set +CONFIG_ARCH_CHIP_STM32=y +# CONFIG_ARCH_CHIP_STR71X is not set +CONFIG_ARCH_CORTEXM4=y +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=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_CODEREDL is not set +# CONFIG_ARMV7M_TOOLCHAIN_CODESOURCERYL is not set +CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y +# CONFIG_ARMV7M_STACKCHECK is not set +CONFIG_SERIAL_TERMIOS=y + +# +# 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_STM32F100C8 is not set +# 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_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_STM32F107VC is not set +# CONFIG_ARCH_CHIP_STM32F207IG 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_STM32F405RG=y +# 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_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_HIGHDENSITY is not set +# CONFIG_STM32_MEDIUMDENSITY is not set +# CONFIG_STM32_LOWDENSITY is not set +# CONFIG_STM32_STM32F20XX is not set +# CONFIG_STM32_STM32F30XX is not set +CONFIG_STM32_STM32F40XX=y +# CONFIG_STM32_DFU is not set + +# +# STM32 Peripheral Support +# +CONFIG_STM32_ADC1=y +# CONFIG_STM32_ADC2 is not set +# CONFIG_STM32_ADC3 is not set +CONFIG_STM32_BKPSRAM=y +# CONFIG_STM32_CAN1 is not set +# CONFIG_STM32_CAN2 is not set +CONFIG_STM32_CCMDATARAM=y +# CONFIG_STM32_CRC is not set +# CONFIG_STM32_CRYP is not set +CONFIG_STM32_DMA1=y +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 +CONFIG_STM32_I2C2=y +CONFIG_STM32_I2C3=y +CONFIG_STM32_OTGFS=y +# CONFIG_STM32_OTGHS is not set +CONFIG_STM32_PWR=y +# CONFIG_STM32_RNG is not set +# CONFIG_STM32_SDIO is not set +# CONFIG_STM32_SPI1 is not set +CONFIG_STM32_SPI2=y +# CONFIG_STM32_SPI3 is not set +CONFIG_STM32_SYSCFG=y +# CONFIG_STM32_TIM1 is not set +# CONFIG_STM32_TIM2 is not set +# CONFIG_STM32_TIM3 is not set +CONFIG_STM32_TIM4=y +CONFIG_STM32_TIM5=y +CONFIG_STM32_TIM6=y +CONFIG_STM32_TIM7=y +# CONFIG_STM32_TIM8 is not set +CONFIG_STM32_TIM9=y +CONFIG_STM32_TIM10=y +CONFIG_STM32_TIM11=y +CONFIG_STM32_TIM12=y +CONFIG_STM32_TIM13=y +CONFIG_STM32_TIM14=y +CONFIG_STM32_USART1=y +CONFIG_STM32_USART2=y +CONFIG_STM32_USART3=y +CONFIG_STM32_UART4=y +CONFIG_STM32_UART5=y +CONFIG_STM32_USART6=y +# CONFIG_STM32_IWDG is not set +CONFIG_STM32_WWDG=y +CONFIG_STM32_ADC=y +CONFIG_STM32_SPI=y +CONFIG_STM32_I2C=y + +# +# Alternate Pin Mapping +# +CONFIG_STM32_FLASH_PREFETCH=y +# 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_CCMEXCLUDE is not set +CONFIG_STM32_DMACAPABLE=y +# CONFIG_STM32_TIM4_PWM is not set +# CONFIG_STM32_TIM5_PWM is not set +# CONFIG_STM32_TIM9_PWM is not set +# CONFIG_STM32_TIM10_PWM is not set +# CONFIG_STM32_TIM11_PWM is not set +# CONFIG_STM32_TIM12_PWM is not set +# CONFIG_STM32_TIM13_PWM is not set +# CONFIG_STM32_TIM14_PWM is not set +# CONFIG_STM32_TIM4_ADC is not set +# CONFIG_STM32_TIM5_ADC 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=y +# CONFIG_USART3_RS485 is not set +CONFIG_USART3_RXDMA=y +# CONFIG_UART4_RS485 is not set +CONFIG_UART4_RXDMA=y +# CONFIG_UART5_RS485 is not set +CONFIG_UART5_RXDMA=y +# CONFIG_USART6_RS485 is not set +CONFIG_USART6_RXDMA=y +CONFIG_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32_USART_SINGLEWIRE=y + +# +# SPI Configuration +# +# CONFIG_STM32_SPI_INTERRUPTS is not set +# CONFIG_STM32_SPI_DMA is not set + +# +# I2C Configuration +# +# CONFIG_STM32_I2C_DYNTIMEO is not set +CONFIG_STM32_I2CTIMEOSEC=0 +CONFIG_STM32_I2CTIMEOMS=10 +# CONFIG_STM32_I2CTIMEOTICKS is not set (PX4 Codbase has this) +# CONFIG_STM32_I2C_DUTY16_9 is not set + +# +# USB Host Configuration +# + +# +# USB Device Configuration +# + +# +# External Memory Configuration +# + +# +# Architecture Options +# +# CONFIG_ARCH_NOINTC is not set +# CONFIG_ARCH_VECNOTIRQ is not set +CONFIG_ARCH_DMA=y +CONFIG_ARCH_IRQPRIO=y +# CONFIG_CUSTOM_STACK is not set +# CONFIG_ADDRENV is not set +CONFIG_ARCH_HAVE_VFORK=y +CONFIG_ARCH_STACKDUMP=y +# CONFIG_ENDIAN_BIG is not set +# CONFIG_ARCH_HAVE_RAMFUNCS is not set +CONFIG_ARCH_HAVE_RAMVECTORS=y +# CONFIG_ARCH_RAMVECTORS is not set + +# +# Board Settings +# +CONFIG_BOARD_LOOPSPERMSEC=16717 +# CONFIG_ARCH_CALIBRATION is not set +CONFIG_DRAM_START=0x20000000 +CONFIG_DRAM_SIZE=196608 +CONFIG_ARCH_HAVE_INTERRUPTSTACK=y +CONFIG_ARCH_INTERRUPTSTACK=750 + +# +# 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 + +# +# Board Selection +# +CONFIG_ARCH_BOARD_TAP_V1=y +CONFIG_ARCH_BOARD="tap-v1" + +# +# Common Board Options +# +CONFIG_NSH_MMCSDMINOR=0 +CONFIG_NSH_MMCSDSLOTNO=0 +CONFIG_NSH_MMCSDSPIPORTNO=2 + +# +# Board-Specific Options +# + +# +# 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_JULIAN_TIME is not set +CONFIG_START_YEAR=1970 +CONFIG_START_MONTH=1 +CONFIG_START_DAY=1 +CONFIG_DEV_CONSOLE=y +# CONFIG_MUTEX_TYPES is not set +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_NNESTPRIO=8 +# CONFIG_FDCLONE_DISABLE is not set +CONFIG_FDCLONE_STDIO=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SCHED_WAITPID=y +# 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 +# +CONFIG_SIG_SIGUSR1=1 +CONFIG_SIG_SIGUSR2=2 +CONFIG_SIG_SIGALARM=3 +CONFIG_SIG_SIGCONDTIMEDOUT=16 +CONFIG_SIG_SIGWORK=4 + +# +# Sizes of configurable things (0 disables) +# +CONFIG_MAX_TASKS=32 +CONFIG_MAX_TASK_ARGS=10 +CONFIG_NPTHREAD_KEYS=4 +CONFIG_NFILE_DESCRIPTORS=46 +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 + +# +# Stack and heap information +# +CONFIG_IDLETHREAD_STACKSIZE=1000 +CONFIG_USERMAIN_STACKSIZE=2500 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_PTHREAD_STACK_DEFAULT=2048 + +# +# Device Drivers +# +# CONFIG_DISABLE_POLL is not set +CONFIG_DEV_NULL=y +# CONFIG_DEV_ZERO is not set +# CONFIG_LOOP is not set +# CONFIG_RAMDISK is not set +# CONFIG_CAN is not set +# CONFIG_PWM is not set +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_RTC is not set +CONFIG_WATCHDOG=y +# CONFIG_ANALOG is not set +# CONFIG_AUDIO_DEVICES is not set +# CONFIG_BCH is not set +# CONFIG_INPUT is not set +# CONFIG_LCD is not set +CONFIG_MMCSD=y +CONFIG_MMCSD_NSLOTS=1 +# CONFIG_MMCSD_READONLY is not set +# CONFIG_MMCSD_MULTIBLOCK_DISABLE is not set +# CONFIG_MMCSD_MMCSUPPORT is not set +# CONFIG_MMCSD_HAVECARDDETECT is not set +# CONFIG_MMCSD_HAVE_SDIOWAIT_WRCOMPLETE is not set +CONFIG_MMCSD_SPI=y +CONFIG_MMCSD_SPICLOCK=24000000 +# CONFIG_MMCSD_SDIO is not set +# CONFIG_MTD is not set +CONFIG_PIPES=y +# CONFIG_PM is not set +# CONFIG_POWER is not set +# CONFIG_SENSORS 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_UART4=y +CONFIG_ARCH_HAVE_UART5=y +CONFIG_ARCH_HAVE_USART1=y +CONFIG_ARCH_HAVE_USART2=y +CONFIG_ARCH_HAVE_USART3=y +CONFIG_ARCH_HAVE_USART6=y +CONFIG_MCU_SERIAL=y +CONFIG_STANDARD_SERIAL=y +CONFIG_SERIAL_NPOLLWAITERS=2 +# CONFIG_USART1_SERIAL_CONSOLE is not set +# CONFIG_USART2_SERIAL_CONSOLE is not set +CONFIG_USART3_SERIAL_CONSOLE=y +# CONFIG_UART4_SERIAL_CONSOLE is not set +# CONFIG_UART5_SERIAL_CONSOLE is not set +# CONFIG_USART6_SERIAL_CONSOLE is not set +# CONFIG_NO_SERIAL_CONSOLE is not set + +# +# USART1 Configuration +# +CONFIG_USART1_RXBUFSIZE=128 +CONFIG_USART1_TXBUFSIZE=128 +CONFIG_USART1_BAUD=57600 +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=300 +CONFIG_USART2_TXBUFSIZE=300 +CONFIG_USART2_BAUD=57600 +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=256 +CONFIG_USART3_TXBUFSIZE=256 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_BITS=8 +CONFIG_USART3_PARITY=0 +CONFIG_USART3_2STOP=0 +# CONFIG_USART3_IFLOWCONTROL is not set +# CONFIG_USART3_OFLOWCONTROL is not set + +# +# UART4 Configuration +# +CONFIG_UART4_RXBUFSIZE=256 +CONFIG_UART4_TXBUFSIZE=256 +CONFIG_UART4_BAUD=115200 +CONFIG_UART4_BITS=8 +CONFIG_UART4_PARITY=0 +CONFIG_UART4_2STOP=0 +# CONFIG_UART4_IFLOWCONTROL is not set +# CONFIG_UART4_OFLOWCONTROL is not set + +# +# UART5 Configuration +# +CONFIG_UART5_RXBUFSIZE=300 +CONFIG_UART5_TXBUFSIZE=300 +CONFIG_UART5_BAUD=57600 +CONFIG_UART5_BITS=8 +CONFIG_UART5_PARITY=0 +CONFIG_UART5_2STOP=0 +# CONFIG_UART5_IFLOWCONTROL is not set +# CONFIG_UART5_OFLOWCONTROL is not set + +# +# USART6 Configuration +# +CONFIG_USART6_RXBUFSIZE=128 +CONFIG_USART6_TXBUFSIZE=64 +CONFIG_USART6_BAUD=57600 +CONFIG_USART6_BITS=8 +CONFIG_USART6_PARITY=0 +CONFIG_USART6_2STOP=0 +# CONFIG_USART6_IFLOWCONTROL is not set +# CONFIG_USART6_OFLOWCONTROL is not set +# CONFIG_SERIAL_IFLOWCONTROL is not set +# CONFIG_SERIAL_OFLOWCONTROL is not set +CONFIG_USBDEV=y + +# +# USB Device Controller Driver Options +# +# CONFIG_USBDEV_ISOCHRONOUS is not set +# CONFIG_USBDEV_DUALSPEED is not set +# CONFIG_USBDEV_SELFPOWERED is not set +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +# CONFIG_USBDEV_DMA is not set +# CONFIG_USBDEV_TRACE is not set + +# +# USB Device Class Driver Options +# +# CONFIG_USBDEV_COMPOSITE is not set +# CONFIG_PL2303 is not set +CONFIG_CDCACM=y +# CONFIG_CDCACM_CONSOLE is not set +CONFIG_CDCACM_EP0MAXPACKET=64 +CONFIG_CDCACM_EPINTIN=1 +CONFIG_CDCACM_EPINTIN_FSSIZE=64 +CONFIG_CDCACM_EPINTIN_HSSIZE=64 +CONFIG_CDCACM_EPBULKOUT=3 +CONFIG_CDCACM_EPBULKOUT_FSSIZE=64 +CONFIG_CDCACM_EPBULKOUT_HSSIZE=512 +CONFIG_CDCACM_EPBULKIN=2 +CONFIG_CDCACM_EPBULKIN_FSSIZE=64 +CONFIG_CDCACM_EPBULKIN_HSSIZE=512 +CONFIG_CDCACM_NWRREQS=4 +CONFIG_CDCACM_NRDREQS=4 +CONFIG_CDCACM_BULKIN_REQLEN=96 +CONFIG_CDCACM_RXBUFSIZE=300 +CONFIG_CDCACM_TXBUFSIZE=1000 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_PRODUCTID=0x0040 +CONFIG_CDCACM_VENDORSTR="The Autopilot" +CONFIG_CDCACM_PRODUCTSTR="PX4 TAP v1.x" +# CONFIG_USBMSC 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_NET is not set + +# +# File Systems +# + +# +# File system configuration +# +# CONFIG_DISABLE_MOUNTPOINT is not set +# CONFIG_FS_RAMMAP is not set +CONFIG_FS_FAT=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_MAXFNAME=32 +CONFIG_FS_FATTIME=y +# CONFIG_FAT_DMAMEMORY is not set +# CONFIG_FS_NXFFS is not set +CONFIG_FS_ROMFS=y +# CONFIG_FS_SMARTFS is not set +CONFIG_FS_BINFS=y + +# +# System Logging +# +# CONFIG_SYSLOG_ENABLE is not set +# CONFIG_SYSLOG is not set + +# +# Graphics Support +# +# CONFIG_NX is not set + +# +# Memory Management +# +# CONFIG_MM_MULTIHEAP is not set +# CONFIG_MM_SMALL is not set +CONFIG_MM_REGIONS=2 +CONFIG_GRAN=y +# CONFIG_GRAN_SINGLE is not set +# CONFIG_GRAN_INTR is not set + +# +# Audio Support +# +# CONFIG_AUDIO is not set + +# +# Binary Formats +# +# CONFIG_BINFMT_DISABLE is not set +# CONFIG_BINFMT_EXEPATH is not set +# CONFIG_NXFLAT is not set +# CONFIG_ELF is not set +CONFIG_BUILTIN=y +# CONFIG_PIC is not set +# CONFIG_SYMTAB_ORDEREDBYNAME is not set + +# +# Library Routines +# + +# +# Standard C Library Options +# +CONFIG_STDIO_BUFFER_SIZE=180 +CONFIG_STDIO_LINEBUFFER=y +CONFIG_NUNGET_CHARS=2 +CONFIG_LIB_HOMEDIR="/" +# CONFIG_NOPRINTF_FIELDWIDTH is not set +CONFIG_LIBC_FLOATINGPOINT=y +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=2048 +CONFIG_LIBC_STRERROR=y +# CONFIG_LIBC_STRERROR_SHORT 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=y +CONFIG_ARCH_MEMCPY=y +# CONFIG_ARCH_MEMCMP is not set +# CONFIG_ARCH_MEMMOVE is not set +# CONFIG_ARCH_MEMSET is not set +# CONFIG_MEMSET_OPTSPEED is not set +# CONFIG_ARCH_STRCHR is not set +# CONFIG_ARCH_STRCMP is not set +# CONFIG_ARCH_STRCPY is not set +# CONFIG_ARCH_STRNCPY is not set +# CONFIG_ARCH_STRLEN is not set +# CONFIG_ARCH_STRNLEN is not set +# CONFIG_ARCH_BZERO is not set + +# +# Non-standard Library Support +# +CONFIG_SCHED_WORKQUEUE=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_WORKPRIORITY=192 +CONFIG_SCHED_WORKPERIOD=5000 +CONFIG_SCHED_WORKSTACKSIZE=1600 +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKPERIOD=50000 +CONFIG_SCHED_LPWORKSTACKSIZE=1600 +# 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 +# +CONFIG_BUILTIN_PROXY_STACKSIZE=1024 + +# +# Examples +# +# CONFIG_EXAMPLES_BUTTONS is not set +# CONFIG_EXAMPLES_CAN is not set +CONFIG_EXAMPLES_CDCACM=y +# CONFIG_EXAMPLES_COMPOSITE 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_LCDRW is not set +# CONFIG_EXAMPLES_MM is not set +# CONFIG_EXAMPLES_MODBUS is not set +CONFIG_EXAMPLES_MOUNT=y +# CONFIG_EXAMPLES_NRF24L01TERM is not set +CONFIG_EXAMPLES_NSH=y +# CONFIG_EXAMPLES_NULL is not set +# CONFIG_EXAMPLES_NX is not set +# CONFIG_EXAMPLES_NXCONSOLE 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_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_SERLOOP is not set +# CONFIG_EXAMPLES_SLCD is not set +# CONFIG_EXAMPLES_SMART_TEST 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_UIP 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 + +# +# Graphics Support +# +# CONFIG_TIFF 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_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_WEBCLIENT is not set + +# +# FreeModBus +# +# CONFIG_MODBUS is not set + +# +# NSH Library +# +CONFIG_NSH_LIBRARY=y +CONFIG_NSH_BUILTIN_APPS=y + +# +# Disable Individual commands +# +# CONFIG_NSH_DISABLE_CAT is not set +# CONFIG_NSH_DISABLE_CD is not set +# CONFIG_NSH_DISABLE_CP is not set +# CONFIG_NSH_DISABLE_DD is not set +# CONFIG_NSH_DISABLE_ECHO is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_FREE is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_HELP is not set +# CONFIG_NSH_DISABLE_HEXDUMP is not set +CONFIG_NSH_DISABLE_IFCONFIG=y +# CONFIG_NSH_DISABLE_KILL is not set +CONFIG_NSH_DISABLE_LOSETUP=y +# CONFIG_NSH_DISABLE_LS is not set +# CONFIG_NSH_DISABLE_MB is not set +# CONFIG_NSH_DISABLE_MKDIR is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_MKFIFO is not set +# CONFIG_NSH_DISABLE_MKRD is not set +# 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=y +# CONFIG_NSH_DISABLE_PS is not set +CONFIG_NSH_DISABLE_PING=y +CONFIG_NSH_DISABLE_PUT=y +# CONFIG_NSH_DISABLE_PWD is not set +# CONFIG_NSH_DISABLE_RM is not set +# CONFIG_NSH_DISABLE_RMDIR is not set +# CONFIG_NSH_DISABLE_SET is not set +# CONFIG_NSH_DISABLE_SH is not set +# CONFIG_NSH_DISABLE_SLEEP is not set +# CONFIG_NSH_DISABLE_TEST is not set +# CONFIG_NSH_DISABLE_UMOUNT is not set +# CONFIG_NSH_DISABLE_UNSET is not set +# CONFIG_NSH_DISABLE_USLEEP is not set +CONFIG_NSH_DISABLE_WGET=y +# CONFIG_NSH_DISABLE_XD is not set + +# +# Configure Command Options +# +# CONFIG_NSH_CMDOPT_DF_H is not set +CONFIG_NSH_CODECS_BUFSIZE=128 +CONFIG_NSH_FILEIOSIZE=512 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=12 +CONFIG_NSH_NESTDEPTH=8 +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLEBG 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_ARCHROMFS=y +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 +# +# CONFIG_NSH_CONDEV is not set +CONFIG_NSH_ARCHINIT=y + +# +# NxWidgets/NxWM +# + +# +# System NSH Add-Ons +# + +# +# Custom Free Memory Command +# +# CONFIG_SYSTEM_FREE is not set + +# +# I2C tool +# +# CONFIG_SYSTEM_I2CTOOL is not set + +# +# FLASH Program Installation +# +# CONFIG_SYSTEM_INSTALL is not set + +# +# FLASH Erase-all Command +# + +# +# readline() +# +CONFIG_SYSTEM_READLINE=y +CONFIG_READLINE_ECHO=y + +# +# Power Off +# +# CONFIG_SYSTEM_POWEROFF is not set + +# +# RAMTRON +# +# CONFIG_SYSTEM_RAMTRON is not set + +# +# SD Card +# +# CONFIG_SYSTEM_SDCARD is not set + +# +# Sysinfo +# +CONFIG_SYSTEM_SYSINFO=y + +# +# USB Monitor +# diff --git a/nuttx-configs/tap-v1/nsh/setenv.sh b/nuttx-configs/tap-v1/nsh/setenv.sh new file mode 100755 index 0000000000..db372217cd --- /dev/null +++ b/nuttx-configs/tap-v1/nsh/setenv.sh @@ -0,0 +1,75 @@ +#!/bin/bash +# configs/px4fmu-v1/usbnsh/setenv.sh +# +# Copyright (C) 2013 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# + +if [ "$_" = "$0" ] ; then + echo "You must source this script, not run it!" 1>&2 + exit 1 +fi + +WD=`pwd` +if [ ! -x "setenv.sh" ]; then + echo "This script must be executed from the top-level NuttX build directory" + exit 1 +fi + +if [ -z "${PATH_ORIG}" ]; then + export PATH_ORIG="${PATH}" +fi + +# This is 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 is 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" + +# These are the Cygwin paths to the locations where I installed the Atollic +# toolchain under windows. You will also have to edit this if you install +# the Atollic toolchain in any other location. /usr/bin is added before +# the Atollic bin path because there is are binaries named gcc.exe and g++.exe +# at those locations as well. +#export TOOLCHAIN_BIN="/usr/bin:/cygdrive/c/Program Files (x86)/Atollic/TrueSTUDIO for ARM Pro 2.3.0/ARMTools/bin" +#export TOOLCHAIN_BIN="/usr/bin:/cygdrive/c/Program Files (x86)/Atollic/TrueSTUDIO for STMicroelectronics STM32 Lite 2.3.0/ARMTools/bin" + +# This is the Cygwin path to the location where I build the buildroot +# toolchain. +#export TOOLCHAIN_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" + +# Add the path to the toolchain to the PATH varialble +export PATH="${TOOLCHAIN_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" + +echo "PATH : ${PATH}" diff --git a/nuttx-configs/tap-v1/scripts/ld.script b/nuttx-configs/tap-v1/scripts/ld.script new file mode 100644 index 0000000000..c1ded8f3df --- /dev/null +++ b/nuttx-configs/tap-v1/scripts/ld.script @@ -0,0 +1,150 @@ +/**************************************************************************** + * configs/tap-v1/scripts/ld.script + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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. + * + ****************************************************************************/ + +/* The STM32F405RG has 1024Kb of FLASH beginning at address 0x0800:0000 and + * 192Kb of SRAM. SRAM is split up into three blocks: + * + * 1) 112Kb of SRAM beginning at address 0x2000:0000 + * 2) 16Kb of SRAM beginning at address 0x2001:c000 + * 3) 64Kb of CCM SRAM beginning at address 0x1000:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + * + * The first 16 KiB of flash is reserved for the bootloader. + * Paramater storage will use the next 2 16KiB Sectors. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x0800C000, LENGTH = 976K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 128K + ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K +} + +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). + */ +EXTERN(abort) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.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. + */ + __errno = get_errno_ptr; + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + /* + * Construction data for parameters. + */ + __param ALIGN(4): { + __param_start = ABSOLUTE(.); + KEEP(*(__param*)) + __param_end = ABSOLUTE(.); + } > flash + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/src/systemcmds/mixer/module.mk b/nuttx-configs/tap-v1/src/Makefile similarity index 56% rename from src/systemcmds/mixer/module.mk rename to nuttx-configs/tap-v1/src/Makefile index 5d5cf84857..8f86d0ada1 100644 --- a/src/systemcmds/mixer/module.mk +++ b/nuttx-configs/tap-v1/src/Makefile @@ -1,6 +1,8 @@ ############################################################################ +# configs/tap-v1/src/Makefile # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# Copyright (C) 2013 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -12,7 +14,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 PX4 nor the names of its contributors may be +# 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. # @@ -31,18 +33,52 @@ # ############################################################################ -# -# Build the mixer tool. -# +-include $(TOPDIR)/Make.defs -MODULE_COMMAND = mixer -SRCS = mixer.cpp +CFLAGS += -I$(TOPDIR)/sched -MODULE_STACKSIZE = 4096 +ASRCS = +AOBJS = $(ASRCS:.S=$(OBJEXT)) -MAXOPTIMIZATION = -Os +CSRCS = empty.c +COBJS = $(CSRCS:.c=$(OBJEXT)) -ifdef ($(PX4_TARGET_OS),nuttx) -EXTRACXXFLAGS = -Wframe-larger-than=2048 +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +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}" +else + CFLAGS += -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m endif +all: libboard$(LIBEXT) + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +libboard$(LIBEXT): $(OBJS) + $(call ARCHIVE, $@, $(OBJS)) + +.depend: Makefile $(SRCS) + $(Q) $(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + $(Q) touch $@ + +depend: .depend + +clean: + $(call DELFILE, libboard$(LIBEXT)) + $(call CLEAN) + +distclean: clean + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) + +-include Make.dep + diff --git a/nuttx-configs/tap-v1/src/empty.c b/nuttx-configs/tap-v1/src/empty.c new file mode 100644 index 0000000000..5de10699fb --- /dev/null +++ b/nuttx-configs/tap-v1/src/empty.c @@ -0,0 +1,4 @@ +/* + * There are no source files here, but libboard.a can't be empty, so + * we have this empty source file to keep it company. + */ diff --git a/posix-configs/SITL/README.md b/posix-configs/SITL/README.md index 20ed19fd4a..2588c4b09b 100644 --- a/posix-configs/SITL/README.md +++ b/posix-configs/SITL/README.md @@ -39,12 +39,12 @@ NOTE: This is only necessary if you are not using the instructions above. 1. Launch PX4 from the SITL build directory "`./Firmware/Build/posix_sitl.build/`" using the command below. The optional `` can be used to cause a set of commands to be entered into the PX4 shell at startup. ``` -> ./mainapp +> ./px4 ``` There is a sample startup script at `posix_configs/SITL/init/rcS`. ``` -> ./mainapp ../../posix-configs/SITL/init/rcS +> ./px4 ../../posix-configs/SITL/init/rcS ``` Without the ``, the commands can be entered at the shell prompt one at a time. An example startup file is given below. This example shows that the "mavlink" module has selected port 14556 for its socket server (as shown in the SITL diagram) and will listen for connections on this port. diff --git a/posix-configs/SITL/init/rc.fixed_wing b/posix-configs/SITL/init/rc.fixed_wing deleted file mode 100644 index bc8a314e2f..0000000000 --- a/posix-configs/SITL/init/rc.fixed_wing +++ /dev/null @@ -1,51 +0,0 @@ -uorb start -param load -param set MAV_TYPE 1 -param set SYS_AUTOSTART 3033 -param set SYS_RESTART_TYPE 2 -dataman start -param set CAL_GYRO0_ID 2293768 -param set CAL_ACC0_ID 1376264 -param set CAL_ACC1_ID 1310728 -param set CAL_MAG0_ID 196616 -param set CAL_GYRO0_XOFF 0.01 -param set CAL_ACC0_XOFF 0.01 -param set CAL_ACC0_YOFF -0.01 -param set CAL_ACC0_ZOFF 0.01 -param set CAL_ACC0_XSCALE 1.01 -param set CAL_ACC0_YSCALE 1.01 -param set CAL_ACC0_ZSCALE 1.01 -param set CAL_ACC1_XOFF 0.01 -param set CAL_MAG0_XOFF 0.01 -param set MPC_XY_P 0.4 -param set MPC_XY_VEL_P 0.2 -param set MPC_XY_VEL_D 0.005 -param set COM_RC_IN_MODE 1 -param set NAV_DLL_ACT 2 -simulator start -s -rgbled start -tone_alarm start -gyrosim start -accelsim start -barosim start -adcsim start -gpssim start -measairspeedsim start -pwm_out_sim mode_pwm -sleep 1 -sensors start -commander start -land_detector start fixedwing -navigator start -ekf_att_pos_estimator start -fw_att_control start -fw_pos_control_l1 start -mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/IO_pass.main.mix -mavlink start -u 14556 -r 60000 -mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556 -mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14556 -mavlink stream -r 50 -s ATTITUDE -u 14556 -mavlink stream -r 80 -s ATTITUDE_QUATERNION -u 14556 -mavlink stream -r 50 -s ATTITUDE_TARGET -u 14556 -mavlink boot_complete -sdlog2 start -r 100 -e -t -a diff --git a/posix-configs/SITL/init/rcS_gazebo_iris b/posix-configs/SITL/init/rcS_gazebo_iris index dd27b3f4c2..d985929601 100644 --- a/posix-configs/SITL/init/rcS_gazebo_iris +++ b/posix-configs/SITL/init/rcS_gazebo_iris @@ -4,6 +4,7 @@ param set MAV_TYPE 2 param set SYS_AUTOSTART 4010 param set SYS_RESTART_TYPE 2 dataman start +param set BAT_N_CELLS 3 param set CAL_GYRO0_ID 2293768 param set CAL_ACC0_ID 1376264 param set CAL_ACC1_ID 1310728 @@ -23,16 +24,25 @@ param set COM_RC_IN_MODE 1 param set NAV_DLL_ACT 2 param set COM_DISARM_LAND 3 param set NAV_ACC_RAD 2.0 +param set COM_OF_LOSS_T 5 +param set COM_OBL_ACT 2 +param set COM_OBL_RC_ACT 0 param set RTL_RETURN_ALT 30.0 -param set RTL_DESCEND_ALT 10.0 -param set RTL_LAND_DELAY 0 +param set RTL_DESCEND_ALT 5.0 +param set RTL_LAND_DELAY 5 param set MIS_TAKEOFF_ALT 2.5 -param set MC_ROLLRATE_P 0.25 -param set MC_PITCHRATE_P 0.25 -param set MC_PITCH_P 5 -param set MC_ROLL_P 5 -param set MPC_Z_VEL_P 0.8 +param set MC_ROLLRATE_P 0.2 +param set MC_PITCHRATE_P 0.2 +param set MC_PITCH_P 6 +param set MC_ROLL_P 6 +param set MPC_HOLD_MAX_Z 2.0 +param set MPC_HOLD_XY_DZ 0.1 +param set MPC_Z_VEL_P 0.6 param set MPC_Z_VEL_I 0.15 +param set EKF2_GBIAS_INIT 0.01 +param set EKF2_ANGERR_INIT 0.01 +param set EKF2_MAG_TYPE 1 +replay tryapplyparams simulator start -s rgbledsim start tone_alarm start @@ -51,8 +61,8 @@ ekf2 start mc_pos_control start mc_att_control start mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_w.main.mix -mavlink start -u 14556 -r 4000000 -t 127.0.0.1 -mavlink start -u 14557 -r 4000000 -m onboard -o 14540 -t 127.0.0.1 +mavlink start -u 14556 -r 4000000 +mavlink start -u 14557 -r 4000000 -m onboard -o 14540 mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556 mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14556 mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 14556 @@ -63,5 +73,6 @@ mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 14556 mavlink stream -r 20 -s RC_CHANNELS -u 14556 mavlink stream -r 250 -s HIGHRES_IMU -u 14556 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 -mavlink boot_complete sdlog2 start -r 100 -e -t -a +mavlink boot_complete +replay trystart diff --git a/posix-configs/SITL/init/rcS_gazebo_iris_opt_flow b/posix-configs/SITL/init/rcS_gazebo_iris_opt_flow index 271a9bc9e4..2be4900b81 100644 --- a/posix-configs/SITL/init/rcS_gazebo_iris_opt_flow +++ b/posix-configs/SITL/init/rcS_gazebo_iris_opt_flow @@ -4,6 +4,7 @@ param set MAV_TYPE 2 param set SYS_AUTOSTART 4010 param set SYS_RESTART_TYPE 2 dataman start +param set BAT_N_CELLS 3 param set CAL_GYRO0_ID 2293768 param set CAL_ACC0_ID 1376264 param set CAL_ACC1_ID 1310728 @@ -27,12 +28,19 @@ param set RTL_RETURN_ALT 30.0 param set RTL_DESCEND_ALT 10.0 param set RTL_LAND_DELAY 0 param set MIS_TAKEOFF_ALT 2.5 -param set MC_ROLLRATE_P 0.25 -param set MC_PITCHRATE_P 0.25 -param set MC_PITCH_P 5 -param set MC_ROLL_P 5 +param set MC_ROLLRATE_P 0.3 +param set MC_PITCHRATE_P 0.3 +param set MC_PITCH_P 5.5 +param set MC_ROLL_P 5.5 +param set MC_ROLLRATE_I 0.1 +param set MC_PITCHRATE_I 0.1 +param set MPC_HOLD_MAX_Z 2.0 +param set MPC_HOLD_XY_DZ 0.1 param set MPC_Z_VEL_P 0.8 param set MPC_Z_VEL_I 0.15 +param set MPC_XY_VEL_P 0.15 +param set MPC_XY_VEL_I 0.2 +replay tryapplyparams simulator start -s rgbledsim start tone_alarm start @@ -52,8 +60,8 @@ position_estimator_inav start mc_pos_control start mc_att_control start mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_w.main.mix -mavlink start -u 14556 -r 2000000 -t 127.0.0.1 -mavlink start -u 14557 -r 2000000 -m onboard -o 14540 -t 127.0.0.1 +mavlink start -u 14556 -r 2000000 +mavlink start -u 14557 -r 2000000 -m onboard -o 14540 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556 mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556 mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556 @@ -63,5 +71,6 @@ mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556 mavlink stream -r 20 -s RC_CHANNELS -u 14556 mavlink stream -r 250 -s HIGHRES_IMU -u 14556 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 -mavlink boot_complete sdlog2 start -r 100 -e -t -a +mavlink boot_complete +replay trystart diff --git a/posix-configs/SITL/init/rcS_gazebo_plane b/posix-configs/SITL/init/rcS_gazebo_plane index ffe21a36ac..b4f40ce2dd 100644 --- a/posix-configs/SITL/init/rcS_gazebo_plane +++ b/posix-configs/SITL/init/rcS_gazebo_plane @@ -4,6 +4,7 @@ param set MAV_TYPE 1 param set SYS_AUTOSTART 3033 param set SYS_RESTART_TYPE 2 dataman start +param set BAT_N_CELLS 3 param set CAL_GYRO0_ID 2293768 param set CAL_ACC0_ID 1376264 param set CAL_ACC1_ID 1310728 @@ -24,6 +25,10 @@ param set COM_RC_IN_MODE 1 param set NAV_DLL_ACT 2 param set NAV_ACC_RAD 15.0 param set FW_THR_IDLE 0.8 +param set EKF2_GBIAS_INIT 0.01 +param set EKF2_ANGERR_INIT 0.01 +param set EKF2_MAG_TYPE 1 +replay tryapplyparams simulator start -s rgbledsim start tone_alarm start @@ -37,14 +42,14 @@ pwm_out_sim mode_pwm sleep 1 sensors start commander start -land_detector start fixedwing navigator start ekf2 start fw_pos_control_l1 start fw_att_control start -mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/plane_sitl.main.mix -mavlink start -u 14556 -r 2000000 -t 127.0.0.1 -mavlink start -u 14557 -r 2000000 -m onboard -o 14540 -t 127.0.0.1 +land_detector start fixedwing +mixer load /dev/pwm_output0 ../../../../ROMFS/sitl/mixers/plane_sitl.main.mix +mavlink start -u 14556 -r 2000000 +mavlink start -u 14557 -r 2000000 -m onboard -o 14540 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556 mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556 mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556 @@ -54,5 +59,6 @@ mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556 mavlink stream -r 20 -s RC_CHANNELS -u 14556 mavlink stream -r 250 -s HIGHRES_IMU -u 14556 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 -mavlink boot_complete sdlog2 start -r 200 -e -t -a +mavlink boot_complete +replay trystart diff --git a/posix-configs/SITL/init/rcS_ekf2_jmavsim_iris b/posix-configs/SITL/init/rcS_gazebo_solo similarity index 57% rename from posix-configs/SITL/init/rcS_ekf2_jmavsim_iris rename to posix-configs/SITL/init/rcS_gazebo_solo index 2504e9f4e3..008261b6ec 100644 --- a/posix-configs/SITL/init/rcS_ekf2_jmavsim_iris +++ b/posix-configs/SITL/init/rcS_gazebo_solo @@ -1,15 +1,10 @@ uorb start param load param set MAV_TYPE 2 -param set MC_PITCHRATE_P 0.15 -param set MC_PITCH_P 7 -param set MC_ROLL_P 7 -param set MC_ROLLRATE_P 0.15 -param set MC_YAW_P 2.8 -param set MC_YAWRATE_P 0.35 param set SYS_AUTOSTART 4010 param set SYS_RESTART_TYPE 2 dataman start +param set BAT_N_CELLS 3 param set CAL_GYRO0_ID 2293768 param set CAL_ACC0_ID 1376264 param set CAL_ACC1_ID 1310728 @@ -23,16 +18,28 @@ param set CAL_ACC0_YSCALE 1.01 param set CAL_ACC0_ZSCALE 1.01 param set CAL_ACC1_XOFF 0.01 param set CAL_MAG0_XOFF 0.01 -param set MPC_XY_P 0.4 -param set MPC_XY_VEL_P 0.2 -param set MPC_XY_VEL_D 0.005 -param set SENS_BOARD_ROT 0 +param set SENS_BOARD_ROT 8 param set SENS_BOARD_X_OFF 0.000001 param set COM_RC_IN_MODE 1 param set NAV_DLL_ACT 2 +param set COM_DISARM_LAND 3 param set NAV_ACC_RAD 2.0 param set RTL_RETURN_ALT 30.0 param set RTL_DESCEND_ALT 10.0 +param set RTL_LAND_DELAY 0 +param set MIS_TAKEOFF_ALT 2.5 +param set MC_ROLLRATE_P 0.2 +param set MC_PITCHRATE_P 0.2 +param set MC_PITCH_P 6 +param set MC_ROLL_P 6 +param set MPC_HOLD_MAX_Z 2.0 +param set MPC_HOLD_XY_DZ 0.1 +param set MPC_Z_VEL_P 0.6 +param set MPC_Z_VEL_I 0.15 +param set EKF2_GBIAS_INIT 0.01 +param set EKF2_ANGERR_INIT 0.01 +param set EKF2_MAG_TYPE 1 +replay tryapplyparams simulator start -s rgbledsim start tone_alarm start @@ -47,19 +54,22 @@ sensors start commander start land_detector start multicopter navigator start +ekf2 start mc_pos_control start mc_att_control start mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix -mavlink start -u 14556 -r 2000000 -mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556 -mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556 -mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556 -mavlink stream -r 80 -s ATTITUDE -u 14556 -mavlink stream -r 80 -s ATTITUDE_QUATERNION -u 14556 -mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556 +mavlink start -u 14556 -r 4000000 +mavlink start -u 14557 -r 4000000 -m onboard -o 14540 +mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556 +mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14556 +mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 14556 +mavlink stream -r 50 -s ATTITUDE -u 14556 +mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 14556 +mavlink stream -r 50 -s ATTITUDE_TARGET -u 14556 +mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 14556 mavlink stream -r 20 -s RC_CHANNELS -u 14556 mavlink stream -r 250 -s HIGHRES_IMU -u 14556 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 -mavlink boot_complete sdlog2 start -r 100 -e -t -a -ekf2 start +mavlink boot_complete +replay trystart diff --git a/posix-configs/SITL/init/rcS_gazebo_standard_vtol b/posix-configs/SITL/init/rcS_gazebo_standard_vtol index c9b0296a86..a1f448bb58 100644 --- a/posix-configs/SITL/init/rcS_gazebo_standard_vtol +++ b/posix-configs/SITL/init/rcS_gazebo_standard_vtol @@ -1,14 +1,11 @@ uorb start param load param set MAV_TYPE 20 -param set MC_PITCHRATE_P 0.3 -param set MC_ROLLRATE_P 0.3 -param set MC_YAW_P 2.0 -param set MC_YAWRATE_P 0.35 param set VT_TYPE 2 param set SYS_AUTOSTART 4010 param set SYS_RESTART_TYPE 2 dataman start +param set BAT_N_CELLS 3 param set CAL_GYRO0_ID 2293768 param set CAL_ACC0_ID 1376264 param set CAL_ACC1_ID 1310728 @@ -22,12 +19,16 @@ param set CAL_ACC0_YSCALE 1.01 param set CAL_ACC0_ZSCALE 1.01 param set CAL_ACC1_XOFF 0.01 param set CAL_MAG0_XOFF 0.01 +param set MC_ROLLRATE_P 0.2 +param set MC_PITCHRATE_P 0.2 +param set MC_PITCH_P 6 +param set MC_ROLL_P 6 param set MPC_XY_P 0.15 param set MPC_XY_VEL_P 0.05 param set MPC_XY_VEL_D 0.005 param set MPC_XY_FF 0.1 param set MPC_Z_VEL_MAX 1.5 -param set MPC_Z_VEL_P 0.8 +param set MPC_Z_VEL_P 0.6 param set MPC_Z_VEL_I 0.15 param set SENS_BOARD_ROT 8 param set SENS_DPRES_OFF 0.001 @@ -37,7 +38,17 @@ param set NAV_DLL_ACT 2 param set NAV_ACC_RAD 3.0 param set MPC_TKO_SPEED 1.0 param set MIS_YAW_TMT 10 -simulator start -s +param set RTL_RETURN_ALT 30.0 +param set RTL_DESCEND_ALT 10.0 +param set RTL_LAND_DELAY 0 +param set COM_DISARM_LAND 5 +param set COM_DL_LOSS_EN 1 +param set MPC_ACC_HOR_MAX 2 +param set EKF2_GBIAS_INIT 0.01 +param set EKF2_ANGERR_INIT 0.01 +param set EKF2_MAG_TYPE 1 +replay tryapplyparams +simulator start -s rgbledsim start tone_alarm start gyrosim start @@ -58,9 +69,9 @@ mc_pos_control start mc_att_control start fw_pos_control_l1 start fw_att_control start -mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/standard_vtol_sitl.main.mix -mavlink start -u 14556 -r 2000000 -t 127.0.0.1 -mavlink start -u 14557 -r 2000000 -m onboard -o 14540 -t 127.0.0.1 +mixer load /dev/pwm_output0 ../../../../ROMFS/sitl/mixers/standard_vtol_sitl.main.mix +mavlink start -u 14556 -r 2000000 +mavlink start -u 14557 -r 2000000 -m onboard -o 14540 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556 mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556 mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556 @@ -70,5 +81,6 @@ mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556 mavlink stream -r 20 -s RC_CHANNELS -u 14556 mavlink stream -r 250 -s HIGHRES_IMU -u 14556 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 -mavlink boot_complete sdlog2 start -r 200 -e -t -a +mavlink boot_complete +replay trystart diff --git a/posix-configs/SITL/init/rcS_gazebo_tailsitter b/posix-configs/SITL/init/rcS_gazebo_tailsitter index 57367ce3ff..2ba72c5b8c 100644 --- a/posix-configs/SITL/init/rcS_gazebo_tailsitter +++ b/posix-configs/SITL/init/rcS_gazebo_tailsitter @@ -1,14 +1,11 @@ uorb start param load param set MAV_TYPE 20 -param set MC_PITCHRATE_P 0.3 -param set MC_ROLLRATE_P 0.3 -param set MC_YAW_P 2.0 -param set MC_YAWRATE_P 0.35 param set VT_TYPE 0 param set SYS_AUTOSTART 4010 param set SYS_RESTART_TYPE 2 dataman start +param set BAT_N_CELLS 3 param set CAL_GYRO0_ID 2293768 param set CAL_ACC0_ID 1376264 param set CAL_ACC1_ID 1310728 @@ -22,6 +19,10 @@ param set CAL_ACC0_YSCALE 1.01 param set CAL_ACC0_ZSCALE 1.01 param set CAL_ACC1_XOFF 0.01 param set CAL_MAG0_XOFF 0.01 +param set MC_ROLLRATE_P 0.2 +param set MC_PITCHRATE_P 0.2 +param set MC_PITCH_P 6 +param set MC_ROLL_P 6 param set MPC_XY_P 0.15 param set MPC_XY_VEL_P 0.05 param set MPC_XY_VEL_D 0.005 @@ -34,6 +35,10 @@ param set SENS_DPRES_OFF 0.001 param set SENS_BOARD_X_OFF 0.000001 param set COM_RC_IN_MODE 1 param set NAV_DLL_ACT 2 +param set EKF2_GBIAS_INIT 0.01 +param set EKF2_ANGERR_INIT 0.01 +param set EKF2_MAG_TYPE 1 +replay tryapplyparams simulator start -s rgbledsim start tone_alarm start @@ -56,8 +61,8 @@ mc_att_control start fw_pos_control_l1 start fw_att_control start mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix -mavlink start -u 14556 -r 2000000 -t 127.0.0.1 -mavlink start -u 14557 -r 2000000 -m onboard -o 14540 -t 127.0.0.1 +mavlink start -u 14556 -r 2000000 +mavlink start -u 14557 -r 2000000 -m onboard -o 14540 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556 mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556 mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556 @@ -67,5 +72,6 @@ mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556 mavlink stream -r 20 -s RC_CHANNELS -u 14556 mavlink stream -r 250 -s HIGHRES_IMU -u 14556 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 -mavlink boot_complete sdlog2 start -r 200 -e -t -a +mavlink boot_complete +replay trystart diff --git a/posix-configs/SITL/init/rcS_gazebo_typhoon_h480 b/posix-configs/SITL/init/rcS_gazebo_typhoon_h480 new file mode 100644 index 0000000000..87c620c750 --- /dev/null +++ b/posix-configs/SITL/init/rcS_gazebo_typhoon_h480 @@ -0,0 +1,77 @@ +uorb start +param load +param set MAV_TYPE 13 +param set SYS_AUTOSTART 6001 +param set SYS_RESTART_TYPE 2 +dataman start +param set BAT_N_CELLS 3 +param set CAL_GYRO0_ID 2293768 +param set CAL_ACC0_ID 1376264 +param set CAL_ACC1_ID 1310728 +param set CAL_MAG0_ID 196616 +param set CAL_GYRO0_XOFF 0.01 +param set CAL_ACC0_XOFF 0.01 +param set CAL_ACC0_YOFF -0.01 +param set CAL_ACC0_ZOFF 0.01 +param set CAL_ACC0_XSCALE 1.01 +param set CAL_ACC0_YSCALE 1.01 +param set CAL_ACC0_ZSCALE 1.01 +param set CAL_ACC1_XOFF 0.01 +param set CAL_MAG0_XOFF 0.01 +param set SENS_BOARD_ROT 8 +param set SENS_BOARD_X_OFF 0.000001 +param set COM_RC_IN_MODE 1 +param set NAV_DLL_ACT 2 +param set COM_DISARM_LAND 3 +param set NAV_ACC_RAD 2.0 +param set RTL_RETURN_ALT 30.0 +param set RTL_DESCEND_ALT 10.0 +param set RTL_LAND_DELAY 0 +param set MIS_TAKEOFF_ALT 2.5 +param set MC_ROLLRATE_P 0.2 +param set MC_PITCHRATE_P 0.2 +param set MC_PITCH_P 6 +param set MC_ROLL_P 6 +param set MPC_HOLD_MAX_Z 2.0 +param set MPC_HOLD_XY_DZ 0.1 +param set MPC_Z_VEL_P 0.6 +param set MPC_Z_VEL_I 0.15 +param set MPC_XY_VEL_P 0.15 +param set MPC_XY_VEL_I 0.2 +param set EKF2_GBIAS_INIT 0.01 +param set EKF2_ANGERR_INIT 0.01 +param set EKF2_MAG_TYPE 1 +replay tryapplyparams +simulator start -s +rgbledsim start +tone_alarm start +gyrosim start +accelsim start +barosim start +adcsim start +gpssim start +pwm_out_sim mode_pwm +sleep 1 +sensors start +commander start +land_detector start multicopter +navigator start +ekf2 start +mc_pos_control start +mc_att_control start +mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/hexa_x.main.mix +mavlink start -u 14556 -r 4000000 +mavlink start -u 14557 -r 4000000 -m onboard -o 14540 +mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556 +mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14556 +mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 14556 +mavlink stream -r 50 -s ATTITUDE -u 14556 +mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 14556 +mavlink stream -r 50 -s ATTITUDE_TARGET -u 14556 +mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 14556 +mavlink stream -r 20 -s RC_CHANNELS -u 14556 +mavlink stream -r 250 -s HIGHRES_IMU -u 14556 +mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 +sdlog2 start -r 100 -e -t -a +mavlink boot_complete +replay trystart diff --git a/posix-configs/SITL/init/rcS_jmavsim_iris b/posix-configs/SITL/init/rcS_jmavsim_iris index ab0e045afc..26db22d8f0 100644 --- a/posix-configs/SITL/init/rcS_jmavsim_iris +++ b/posix-configs/SITL/init/rcS_jmavsim_iris @@ -1,15 +1,10 @@ uorb start param load param set MAV_TYPE 2 -param set MC_PITCHRATE_P 0.15 -param set MC_PITCH_P 7 -param set MC_ROLL_P 7 -param set MC_ROLLRATE_P 0.15 -param set MC_YAW_P 2.8 -param set MC_YAWRATE_P 0.35 param set SYS_AUTOSTART 4010 param set SYS_RESTART_TYPE 2 dataman start +param set BAT_N_CELLS 3 param set CAL_GYRO0_ID 2293768 param set CAL_ACC0_ID 1376264 param set CAL_ACC1_ID 1310728 @@ -36,10 +31,18 @@ param set RTL_RETURN_ALT 30.0 param set RTL_DESCEND_ALT 10.0 param set RTL_LAND_DELAY 0 param set MIS_TAKEOFF_ALT 2.5 +param set MC_ROLLRATE_P 0.2 +param set MC_PITCHRATE_P 0.2 +param set MC_PITCH_P 6 +param set MC_ROLL_P 6 param set MPC_HOLD_MAX_Z 2.0 param set MPC_HOLD_XY_DZ 0.1 param set MPC_Z_VEL_MAX 2.0 param set MPC_Z_VEL_P 0.4 +param set EKF2_GBIAS_INIT 0.01 +param set EKF2_ANGERR_INIT 0.01 +param set EKF2_MAG_TYPE 1 +replay tryapplyparams simulator start -s rgbledsim start tone_alarm start @@ -48,6 +51,7 @@ accelsim start barosim start adcsim start gpssim start +#gps start -d /dev/ttyACM0 -s pwm_out_sim mode_pwm sleep 1 sensors start @@ -58,8 +62,8 @@ ekf2 start mc_pos_control start mc_att_control start mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix -mavlink start -u 14556 -r 2000000 -t 127.0.0.1 -mavlink start -u 14557 -r 2000000 -m onboard -o 14540 -t 127.0.0.1 +mavlink start -u 14556 -r 2000000 +mavlink start -u 14557 -r 2000000 -m onboard -o 14540 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556 mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556 mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556 @@ -70,5 +74,7 @@ mavlink stream -r 20 -s RC_CHANNELS -u 14556 mavlink stream -r 250 -s HIGHRES_IMU -u 14556 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 mavlink stream -r 20 -s MANUAL_CONTROL -u 14556 -mavlink boot_complete sdlog2 start -r 100 -e -t -a +logger start -e -t +mavlink boot_complete +replay trystart diff --git a/posix-configs/SITL/init/rcS_lpe_gazebo_iris b/posix-configs/SITL/init/rcS_lpe_gazebo_iris index 1de001b619..9c84018e98 100644 --- a/posix-configs/SITL/init/rcS_lpe_gazebo_iris +++ b/posix-configs/SITL/init/rcS_lpe_gazebo_iris @@ -1,39 +1,49 @@ uorb start param load param set MAV_TYPE 2 -param set MC_PITCHRATE_P 0.3 -param set MC_ROLLRATE_P 0.3 -param set MC_YAW_P 2.0 -param set MC_YAWRATE_P 0.35 param set SYS_AUTOSTART 4010 param set SYS_RESTART_TYPE 2 dataman start +param set BAT_N_CELLS 3 param set CAL_GYRO0_ID 2293768 param set CAL_ACC0_ID 1376264 param set CAL_ACC1_ID 1310728 param set CAL_MAG0_ID 196616 param set CAL_GYRO0_XOFF 0.01 -param set CAL_ACC0_XOFF 1 -param set CAL_ACC0_YOFF 2 -param set CAL_ACC0_ZOFF 3 +param set CAL_ACC0_XOFF 0.01 +param set CAL_ACC0_YOFF -0.01 +param set CAL_ACC0_ZOFF 0.01 param set CAL_ACC0_XSCALE 1.01 param set CAL_ACC0_YSCALE 1.01 param set CAL_ACC0_ZSCALE 1.01 param set CAL_ACC1_XOFF 0.01 param set CAL_MAG0_XOFF 0.01 -param set MPC_XY_P 0.15 -param set MPC_XY_VEL_P 0.05 -param set MPC_XY_VEL_D 0.005 -param set MPC_XY_FF 0.1 param set SENS_BOARD_ROT 8 param set SENS_BOARD_X_OFF 0.000001 - -# changes for LPE param set COM_RC_IN_MODE 1 param set NAV_DLL_ACT 2 -param set LPE_BETA_MAX 10000 +param set COM_DISARM_LAND 3 +param set NAV_ACC_RAD 2.0 +param set COM_OF_LOSS_T 5 +param set COM_OBL_ACT 2 +param set COM_OBL_RC_ACT 0 +param set RTL_RETURN_ALT 30.0 +param set RTL_DESCEND_ALT 5.0 +param set RTL_LAND_DELAY 5 +param set MIS_TAKEOFF_ALT 2.5 +param set MC_ROLLRATE_P 0.2 +param set MC_PITCHRATE_P 0.2 +param set MC_PITCH_P 6 +param set MC_ROLL_P 6 +param set MPC_HOLD_MAX_Z 2.0 +param set MPC_HOLD_XY_DZ 0.1 +param set MPC_Z_VEL_P 0.6 +param set MPC_Z_VEL_I 0.15 +param set EKF2_GBIAS_INIT 0.01 +param set EKF2_ANGERR_INIT 0.01 +replay tryapplyparams simulator start -s -rgbled start +rgbledsim start tone_alarm start gyrosim start accelsim start @@ -47,23 +57,22 @@ commander start land_detector start multicopter navigator start attitude_estimator_q start +local_position_estimator start mc_pos_control start mc_att_control start -mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_w.main.mix -mavlink start -u 14556 -r 2000000 -t 127.0.0.1 -mavlink start -u 14557 -r 2000000 -m onboard -o 14540 -t 127.0.0.1 -mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556 -mavlink stream -r 80 -s LOCAL_POSITION_NED_COV -u 14556 -mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556 -mavlink stream -r 80 -s ATTITUDE -u 14556 -mavlink stream -r 80 -s ATTITUDE_QUATERNION -u 14556 -mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556 +mixer load /dev/pwm_output0 /usr/share/px4/ROMFS/px4fmu_common/mixers/quad_w.main.mix +mavlink start -u 14556 -r 4000000 +mavlink start -u 14557 -r 4000000 -m onboard -o 14540 +mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556 +mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14556 +mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 14556 +mavlink stream -r 50 -s ATTITUDE -u 14556 +mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 14556 +mavlink stream -r 50 -s ATTITUDE_TARGET -u 14556 +mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 14556 mavlink stream -r 20 -s RC_CHANNELS -u 14556 mavlink stream -r 250 -s HIGHRES_IMU -u 14556 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 -mavlink boot_complete sdlog2 start -r 100 -e -t -a - -# start LPE at end, when we know it is ok to init sensors -sleep 5 -local_position_estimator start +mavlink boot_complete +replay trystart diff --git a/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow b/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow index 85a3ce7617..c1a6a2eae5 100644 --- a/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow +++ b/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow @@ -1,13 +1,10 @@ uorb start param load param set MAV_TYPE 2 -param set MC_PITCHRATE_P 0.3 -param set MC_ROLLRATE_P 0.3 -param set MC_YAW_P 2.0 -param set MC_YAWRATE_P 0.35 param set SYS_AUTOSTART 4010 param set SYS_RESTART_TYPE 2 dataman start +param set BAT_N_CELLS 3 param set CAL_GYRO0_ID 2293768 param set CAL_ACC0_ID 1376264 param set CAL_ACC1_ID 1310728 @@ -21,21 +18,39 @@ param set CAL_ACC0_YSCALE 1.01 param set CAL_ACC0_ZSCALE 1.01 param set CAL_ACC1_XOFF 0.01 param set CAL_MAG0_XOFF 0.01 -param set MPC_XY_P 0.15 -param set MPC_XY_VEL_P 0.05 -param set MPC_XY_VEL_D 0.005 -param set MPC_XY_FF 0.1 param set SENS_BOARD_ROT 8 param set SENS_BOARD_X_OFF 0.000001 param set COM_RC_IN_MODE 1 -param set NAV_ACC_RAD 2.0 +param set NAV_DLL_ACT 2 +param set COM_DISARM_LAND 3 +param set NAV_ACC_RAD 12.0 param set RTL_RETURN_ALT 30.0 param set RTL_DESCEND_ALT 10.0 +param set RTL_LAND_DELAY 0 +param set MIS_TAKEOFF_ALT 2.5 +param set MC_ROLLRATE_P 0.3 +param set MC_PITCHRATE_P 0.3 +param set MC_PITCH_P 5.5 +param set MC_ROLL_P 5.5 +param set MC_ROLLRATE_I 0.1 +param set MC_PITCHRATE_I 0.1 +param set MPC_HOLD_MAX_Z 2.0 +param set MPC_HOLD_XY_DZ 0.1 +param set MPC_Z_VEL_P 0.8 +param set MPC_Z_VEL_I 0.15 +param set MPC_XY_VEL_P 0.15 +param set MPC_XY_VEL_I 0.2 -# changes for LPE -param set COM_RC_IN_MODE 1 -param set NAV_DLL_ACT 2 -param set LPE_BETA_MAX 10000 +# changes for LPE indoor flight +param set LPE_GPS_ON 0 +param set MPC_ALT_MODE 1 +param set LPE_T_MAX_GRADE 0 +param set MPC_XY_VEL_MAX 2 +param set MPC_Z_VEL_MAX 2 +param set MPC_XY_P 0.5 +param set MIS_TAKEOFF_ALT 2 + +replay tryapplyparams simulator start -s rgbledsim start tone_alarm start @@ -51,13 +66,14 @@ commander start land_detector start multicopter navigator start attitude_estimator_q start +local_position_estimator start mc_pos_control start mc_att_control start -mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_w.main.mix -mavlink start -u 14556 -r 2000000 -t 127.0.0.1 -mavlink start -u 14557 -r 2000000 -m onboard -o 14540 -t 127.0.0.1 +mixer load /dev/pwm_output0 /usr/share/px4/ROMFS/px4fmu_common/mixers/quad_w.main.mix +mavlink start -u 14556 -r 2000000 +mavlink start -u 14557 -r 2000000 -m onboard -o 14540 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556 -mavlink stream -r 80 -s LOCAL_POSITION_NED_COV -u 14556 +mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556 mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556 mavlink stream -r 80 -s ATTITUDE -u 14556 mavlink stream -r 80 -s ATTITUDE_QUATERNION -u 14556 @@ -65,9 +81,6 @@ mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556 mavlink stream -r 20 -s RC_CHANNELS -u 14556 mavlink stream -r 250 -s HIGHRES_IMU -u 14556 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 -mavlink boot_complete sdlog2 start -r 100 -e -t -a - -# start LPE at end, when we know it is ok to init sensors -sleep 5 -local_position_estimator start +mavlink boot_complete +replay trystart diff --git a/posix-configs/SITL/init/rcS_lpe_jmavsim_iris b/posix-configs/SITL/init/rcS_lpe_jmavsim_iris index a12d2e6f47..106d032275 100644 --- a/posix-configs/SITL/init/rcS_lpe_jmavsim_iris +++ b/posix-configs/SITL/init/rcS_lpe_jmavsim_iris @@ -1,15 +1,10 @@ uorb start param load param set MAV_TYPE 2 -param set MC_PITCHRATE_P 0.15 -param set MC_PITCH_P 7 -param set MC_ROLL_P 7 -param set MC_ROLLRATE_P 0.15 -param set MC_YAW_P 2.8 -param set MC_YAWRATE_P 0.35 param set SYS_AUTOSTART 4010 param set SYS_RESTART_TYPE 2 dataman start +param set BAT_N_CELLS 3 param set CAL_GYRO0_ID 2293768 param set CAL_ACC0_ID 1376264 param set CAL_ACC1_ID 1310728 @@ -29,11 +24,24 @@ param set MPC_XY_VEL_D 0.005 param set SENS_BOARD_ROT 0 param set SENS_BOARD_X_OFF 0.000001 param set COM_RC_IN_MODE 1 +param set NAV_DLL_ACT 2 +param set COM_DISARM_LAND 3 param set NAV_ACC_RAD 2.0 param set RTL_RETURN_ALT 30.0 param set RTL_DESCEND_ALT 10.0 -param set COM_RC_IN_MODE 1 -param set NAV_DLL_ACT 2 +param set RTL_LAND_DELAY 0 +param set MIS_TAKEOFF_ALT 2.5 +param set MC_ROLLRATE_P 0.2 +param set MC_PITCHRATE_P 0.2 +param set MC_PITCH_P 6 +param set MC_ROLL_P 6 +param set MPC_HOLD_MAX_Z 2.0 +param set MPC_HOLD_XY_DZ 0.1 +param set MPC_Z_VEL_MAX 2.0 +param set MPC_Z_VEL_P 0.4 +param set EKF2_GBIAS_INIT 0.01 +param set EKF2_ANGERR_INIT 0.01 +replay tryapplyparams simulator start -s rgbledsim start tone_alarm start @@ -42,18 +50,22 @@ accelsim start barosim start adcsim start gpssim start +#gps start -d /dev/ttyACM0 -s pwm_out_sim mode_pwm sleep 1 sensors start commander start land_detector start multicopter navigator start +attitude_estimator_q start +local_position_estimator start mc_pos_control start mc_att_control start -mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix -mavlink start -u 14556 -r 2000000 -t 127.0.0.1 +mixer load /dev/pwm_output0 /usr/share/px4/ROMFS/px4fmu_common/mixers/quad_x.main.mix +mavlink start -u 14556 -r 2000000 +mavlink start -u 14557 -r 2000000 -m onboard -o 14540 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556 -mavlink stream -r 80 -s LOCAL_POSITION_NED_COV -u 14556 +mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556 mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556 mavlink stream -r 80 -s ATTITUDE -u 14556 mavlink stream -r 80 -s ATTITUDE_QUATERNION -u 14556 @@ -61,7 +73,8 @@ mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556 mavlink stream -r 20 -s RC_CHANNELS -u 14556 mavlink stream -r 250 -s HIGHRES_IMU -u 14556 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 -mavlink boot_complete +mavlink stream -r 20 -s MANUAL_CONTROL -u 14556 sdlog2 start -r 100 -e -t -a -attitude_estimator_q start -local_position_estimator start +logger start -e -t +mavlink boot_complete +replay trystart diff --git a/posix-configs/SITL/init/rcS_multiple_gazebo_iris b/posix-configs/SITL/init/rcS_multiple_gazebo_iris new file mode 100644 index 0000000000..d1ee2acc1e --- /dev/null +++ b/posix-configs/SITL/init/rcS_multiple_gazebo_iris @@ -0,0 +1,68 @@ +uorb start +simulator start -s -u _SIMPORT_ +param load +param set MAV_TYPE 2 +param set SYS_AUTOSTART 4010 +param set SYS_RESTART_TYPE 2 +dataman start +param set BAT_N_CELLS 3 +param set CAL_GYRO0_ID 2293768 +param set CAL_ACC0_ID 1376264 +param set CAL_ACC1_ID 1310728 +param set CAL_MAG0_ID 196616 +param set CAL_GYRO0_XOFF 0.01 +param set CAL_ACC0_XOFF 0.01 +param set CAL_ACC0_YOFF -0.01 +param set CAL_ACC0_ZOFF 0.01 +param set CAL_ACC0_XSCALE 1.01 +param set CAL_ACC0_YSCALE 1.01 +param set CAL_ACC0_ZSCALE 1.01 +param set CAL_ACC1_XOFF 0.01 +param set CAL_MAG0_XOFF 0.01 +param set MC_ROLLRATE_P 0.2 +param set MC_PITCHRATE_P 0.2 +param set MC_PITCH_P 6 +param set MC_ROLL_P 6 +param set MPC_XY_P 0.15 +param set MPC_XY_VEL_P 0.05 +param set MPC_XY_VEL_D 0.005 +param set MPC_XY_FF 0.1 +param set SENS_BOARD_ROT 8 +param set COM_RC_IN_MODE 1 +param set NAV_ACC_RAD 2.0 +param set RTL_RETURN_ALT 30.0 +param set RTL_DESCEND_ALT 10.0 +param set EKF2_GBIAS_INIT 0.01 +param set EKF2_ANGERR_INIT 0.01 +param set EKF2_MAG_TYPE 1 +replay tryapplyparams +rgbledsim start +tone_alarm start +gyrosim start +accelsim start +barosim start +adcsim start +gpssim start +pwm_out_sim mode_pwm +sleep 1 +sensors start +commander start +land_detector start multicopter +navigator start +ekf2 start +mc_pos_control start +mc_att_control start +mixer load /dev/pwm_output0 quad_w.main.mix +mavlink start -u _MAVPORT_ -r 2000000 -o _MAVOPORT_ +mavlink start -u _MAVPORT2_ -r 2000000 -m onboard -o _MAVOPORT2_ +mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u _MAVPORT_ +mavlink stream -r 80 -s LOCAL_POSITION_NED -u _MAVPORT_ +mavlink stream -r 80 -s GLOBAL_POSITION_INT -u _MAVPORT_ +mavlink stream -r 80 -s ATTITUDE -u _MAVPORT_ +mavlink stream -r 80 -s ATTITUDE_TARGET -u _MAVPORT_ +mavlink stream -r 20 -s RC_CHANNELS -u _MAVPORT_ +mavlink stream -r 250 -s HIGHRES_IMU -u _MAVPORT_ +mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u _MAVPORT_ +sdlog2 start -r 100 -e -t -a +mavlink boot_complete +replay trystart diff --git a/posix-configs/SITL/init/rcS_tests b/posix-configs/SITL/init/rcS_tests new file mode 100644 index 0000000000..c41a301e2b --- /dev/null +++ b/posix-configs/SITL/init/rcS_tests @@ -0,0 +1,12 @@ +uorb start +param load +param set SYS_RESTART_TYPE 0 +dataman start + +ver all + +sleep 1 + +tests all + +shutdown diff --git a/posix-configs/bebop/px4.config b/posix-configs/bebop/px4.config new file mode 100644 index 0000000000..df9e4f9472 --- /dev/null +++ b/posix-configs/bebop/px4.config @@ -0,0 +1,18 @@ +uorb start +param select /home/root/parameters +param load +param set SYS_AUTOSTART 4001 +param set MAV_BROADCAST 1 +df_ms5607_wrapper start +df_mpu6050_wrapper start -R 8 +df_ak8963_wrapper start -R 4 +sensors start +attitude_estimator_q start +position_estimator_inav start +commander start +sleep 1 +mavlink start -u 14556 -r 1000000 +sleep 1 +mavlink stream -u 14556 -s HIGHRES_IMU -r 50 +mavlink stream -u 14556 -s ATTITUDE -r 50 +mavlink boot_complete diff --git a/posix-configs/eagle/200qx/mainapp-flight.config b/posix-configs/eagle/200qx/mainapp.config similarity index 88% rename from posix-configs/eagle/200qx/mainapp-flight.config rename to posix-configs/eagle/200qx/mainapp.config index fcad72bfe5..60fe035f7d 100644 --- a/posix-configs/eagle/200qx/mainapp-flight.config +++ b/posix-configs/eagle/200qx/mainapp.config @@ -1,5 +1,6 @@ uorb start muorb start +param set MAV_BROADCAST 1 mavlink start -u 14556 sleep 1 mavlink stream -u 14556 -s HIGHRES_IMU -r 50 diff --git a/posix-configs/eagle/200qx/px4-calib.config b/posix-configs/eagle/200qx/px4-calib.config deleted file mode 100644 index 5cda1c9323..0000000000 --- a/posix-configs/eagle/200qx/px4-calib.config +++ /dev/null @@ -1,72 +0,0 @@ -uorb start -param set CAL_GYRO0_ID 2293760 -param set CAL_ACC0_ID 1310720 -param set CAL_ACC1_ID 1376256 -param set CAL_MAG0_ID 196608 -param set RC_RECEIVER_TYPE 1 -rc_receiver start -D /dev/tty-1 -attitude_estimator_q start -position_estimator_inav start -mc_pos_control start -mc_att_control start -sleep 1 -param set SYS_AUTOSTART 4001 -param set SYS_AUTOCONFIG 1 -param set MAV_TYPE 2​ -param set RC_MAP_THROTTLE 1 -param set RC_MAP_ROLL 2 -param set RC_MAP_PITCH 3 -param set RC_MAP_YAW 4 -param set RC_MAP_MODE_SW 5 -param set RC_MAP_POSCTL_SW 6 -param set RC1_MAX 1900 -param set RC1_MIN 1099 -param set RC1_TRIM 1099 -param set RC1_REV 1 -param set RC2_MAX 1900 -param set RC2_MIN 1099 -param set RC2_TRIM 1500 -param set RC2_REV -1 -param set RC3_MAX 1900 -param set RC3_MIN 1099 -param set RC3_TRIM 1500 -param set RC3_REV 1 -param set RC4_MAX 1900 -param set RC4_MIN 1099 -param set RC4_TRIM 1500 -param set RC4_REV -1 -param set RC5_MAX 1900 -param set RC5_MIN 1099 -param set RC5_TRIM 1500 -param set RC5_REV 1 -param set RC6_MAX 1900 -param set RC6_MIN 1099 -param set RC6_TRIM 1099 -param set RC6_REV 1 -sensors start -param set MC_YAW_P 7.0 -param set MC_YAWRATE_P 0.08 -param set MC_YAWRATE_I 0.0 -param set MC_YAWRATE_D 0 -param set MC_PITCH_P 7.0 -param set MC_PITCHRATE_P 0.08 -param set MC_PITCHRATE_I 0.0 -param set MC_PITCHRATE_D 0.0001 -param set MC_ROLL_P 7.0 -param set MC_ROLLRATE_P 0.08 -param set MC_ROLLRATE_I 0.0 -param set MC_ROLLRATE_D 0.0001 -param set ATT_W_MAG 0.00 -param set PE_MAG_NOISE 1.0f -param set SENS_BOARD_ROT 0 -param set MPU_GYRO_LPF_ENUM 4 -param set MPU_ACC_LPF_ENUM 4 -param set MPU_SAMPLE_RATE_ENUM 2 -sleep 1 -mpu9x50 start -D /dev/spi-1 -uart_esc start -D /dev/tty-2 -csr_gps start -D /dev/tty-3 -list_devices -list_files -list_tasks -list_topics diff --git a/posix-configs/eagle/200qx/px4-flight-v1-board.config b/posix-configs/eagle/200qx/px4-flight-v1-board.config deleted file mode 100644 index 61120ad5e0..0000000000 --- a/posix-configs/eagle/200qx/px4-flight-v1-board.config +++ /dev/null @@ -1,80 +0,0 @@ -uorb start -param set CAL_GYRO0_ID 2293760 -param set CAL_ACC0_ID 1310720 -param set CAL_ACC1_ID 1376256 -param set CAL_MAG0_ID 196608 -param set SYS_AUTOSTART 4001 -param set SYS_AUTOCONFIG 1 -param set MAV_TYPE 2 -param set MC_YAW_P 7.0 -param set MC_YAWRATE_P 0.08 -param set MC_YAWRATE_I 0.0 -param set MC_YAWRATE_D 0 -param set MC_PITCH_P 7.0 -param set MC_PITCHRATE_P 0.08 -param set MC_PITCHRATE_I 0.0 -param set MC_PITCHRATE_D 0.0001 -param set MC_ROLL_P 7.0 -param set MC_ROLLRATE_P 0.08 -param set MC_ROLLRATE_I 0.0 -param set MC_ROLLRATE_D 0.0001 -param set RC_MAP_THROTTLE 1 -param set RC_MAP_ROLL 2 -param set RC_MAP_PITCH 3 -param set RC_MAP_YAW 4 -param set RC_MAP_MODE_SW 5 -param set RC_MAP_POSCTL_SW 6 -param set RC1_MAX 1900 -param set RC1_MIN 1099 -param set RC1_TRIM 1099 -param set RC1_REV 1 -param set RC2_MAX 1900 -param set RC2_MIN 1099 -param set RC2_TRIM 1500 -param set RC2_REV -1 -param set RC3_MAX 1900 -param set RC3_MIN 1099 -param set RC3_TRIM 1500 -param set RC3_REV 1 -param set RC4_MAX 1900 -param set RC4_MIN 1099 -param set RC4_TRIM 1500 -param set RC4_REV -1 -param set RC5_MAX 1900 -param set RC5_MIN 1099 -param set RC5_TRIM 1500 -param set RC5_REV 1 -param set RC6_MAX 1900 -param set RC6_MIN 1099 -param set RC6_TRIM 1099 -param set RC6_REV 1 -param set ATT_W_MAG 0.00 -param set PE_MAG_NOISE 1.0f -param set SENS_BOARD_ROT 6 -param set MPU_GYRO_LPF_ENUM 4 -param set MPU_ACC_LPF_ENUM 4 -param set MPU_SAMPLE_RATE_ENUM 2 -param set UART_ESC_MODEL 0 -param set UART_ESC_BAUDRATE 250000 -param set UART_ESC_PX4MOTOR1 2 -param set UART_ESC_PX4MOTOR2 4 -param set UART_ESC_PX4MOTOR3 1 -param set UART_ESC_PX4MOTOR4 3 -sleep 1 -commander start -rc_receiver start -D /dev/tty-1 -sleep 1 -mpu9x50 start -D /dev/spi-1 -df_bmp280_wrapper start -sensors start -attitude_estimator_q start -position_estimator_inav start -mc_pos_control start -mc_att_control start -land_detector start multicopter -sleep 1 -uart_esc start -D /dev/tty-2 -list_devices -list_files -list_tasks -list_topics diff --git a/posix-configs/eagle/200qx/px4-flight-v2-board.config b/posix-configs/eagle/200qx/px4-flight-v2-board.config deleted file mode 100644 index 11072428e5..0000000000 --- a/posix-configs/eagle/200qx/px4-flight-v2-board.config +++ /dev/null @@ -1,80 +0,0 @@ -uorb start -param set CAL_GYRO0_ID 2293760 -param set CAL_ACC0_ID 1310720 -param set CAL_ACC1_ID 1376256 -param set CAL_MAG0_ID 196608 -param set SYS_AUTOSTART 4001 -param set SYS_AUTOCONFIG 1 -param set MAV_TYPE 2 -param set MC_YAW_P 7.0 -param set MC_YAWRATE_P 0.08 -param set MC_YAWRATE_I 0.0 -param set MC_YAWRATE_D 0 -param set MC_PITCH_P 7.0 -param set MC_PITCHRATE_P 0.08 -param set MC_PITCHRATE_I 0.0 -param set MC_PITCHRATE_D 0.0001 -param set MC_ROLL_P 7.0 -param set MC_ROLLRATE_P 0.08 -param set MC_ROLLRATE_I 0.0 -param set MC_ROLLRATE_D 0.0001 -param set RC_MAP_THROTTLE 1 -param set RC_MAP_ROLL 2 -param set RC_MAP_PITCH 3 -param set RC_MAP_YAW 4 -param set RC_MAP_MODE_SW 5 -param set RC_MAP_POSCTL_SW 6 -param set RC1_MAX 1900 -param set RC1_MIN 1099 -param set RC1_TRIM 1099 -param set RC1_REV 1 -param set RC2_MAX 1900 -param set RC2_MIN 1099 -param set RC2_TRIM 1500 -param set RC2_REV -1 -param set RC3_MAX 1900 -param set RC3_MIN 1099 -param set RC3_TRIM 1500 -param set RC3_REV 1 -param set RC4_MAX 1900 -param set RC4_MIN 1099 -param set RC4_TRIM 1500 -param set RC4_REV -1 -param set RC5_MAX 1900 -param set RC5_MIN 1099 -param set RC5_TRIM 1500 -param set RC5_REV 1 -param set RC6_MAX 1900 -param set RC6_MIN 1099 -param set RC6_TRIM 1099 -param set RC6_REV 1 -param set ATT_W_MAG 0.00 -param set PE_MAG_NOISE 1.0f -param set SENS_BOARD_ROT 0 -param set MPU_GYRO_LPF_ENUM 4 -param set MPU_ACC_LPF_ENUM 4 -param set MPU_SAMPLE_RATE_ENUM 2 -param set UART_ESC_MODEL 0 -param set UART_ESC_BAUDRATE 250000 -param set UART_ESC_PX4MOTOR1 2 -param set UART_ESC_PX4MOTOR2 4 -param set UART_ESC_PX4MOTOR3 1 -param set UART_ESC_PX4MOTOR4 3 -sleep 1 -commander start -rc_receiver start -D /dev/tty-1 -sleep 1 -mpu9x50 start -D /dev/spi-1 -df_bmp280_wrapper start -sensors start -attitude_estimator_q start -position_estimator_inav start -mc_pos_control start -mc_att_control start -land_detector start multicopter -sleep 1 -uart_esc start -D /dev/tty-2 -list_devices -list_files -list_tasks -list_topics diff --git a/posix-configs/eagle/210qc/px4-calib.config b/posix-configs/eagle/200qx/px4.config similarity index 77% rename from posix-configs/eagle/210qc/px4-calib.config rename to posix-configs/eagle/200qx/px4.config index 5cda1c9323..5bbd3c0703 100644 --- a/posix-configs/eagle/210qc/px4-calib.config +++ b/posix-configs/eagle/200qx/px4.config @@ -1,18 +1,22 @@ uorb start -param set CAL_GYRO0_ID 2293760 -param set CAL_ACC0_ID 1310720 -param set CAL_ACC1_ID 1376256 -param set CAL_MAG0_ID 196608 -param set RC_RECEIVER_TYPE 1 -rc_receiver start -D /dev/tty-1 -attitude_estimator_q start -position_estimator_inav start -mc_pos_control start -mc_att_control start -sleep 1 +qshell start +param set CAL_GYRO0_ID 100 +param set CAL_ACC0_ID 100 +param set CAL_MAG0_ID 101 param set SYS_AUTOSTART 4001 -param set SYS_AUTOCONFIG 1 -param set MAV_TYPE 2​ +param set MAV_TYPE 2 +param set MC_YAW_P 7.0 +param set MC_YAWRATE_P 0.08 +param set MC_YAWRATE_I 0.0 +param set MC_YAWRATE_D 0 +param set MC_PITCH_P 7.0 +param set MC_PITCHRATE_P 0.08 +param set MC_PITCHRATE_I 0.0 +param set MC_PITCHRATE_D 0.0001 +param set MC_ROLL_P 7.0 +param set MC_ROLLRATE_P 0.08 +param set MC_ROLLRATE_I 0.0 +param set MC_ROLLRATE_D 0.0001 param set RC_MAP_THROTTLE 1 param set RC_MAP_ROLL 2 param set RC_MAP_PITCH 3 @@ -43,29 +47,28 @@ param set RC6_MAX 1900 param set RC6_MIN 1099 param set RC6_TRIM 1099 param set RC6_REV 1 -sensors start -param set MC_YAW_P 7.0 -param set MC_YAWRATE_P 0.08 -param set MC_YAWRATE_I 0.0 -param set MC_YAWRATE_D 0 -param set MC_PITCH_P 7.0 -param set MC_PITCHRATE_P 0.08 -param set MC_PITCHRATE_I 0.0 -param set MC_PITCHRATE_D 0.0001 -param set MC_ROLL_P 7.0 -param set MC_ROLLRATE_P 0.08 -param set MC_ROLLRATE_I 0.0 -param set MC_ROLLRATE_D 0.0001 param set ATT_W_MAG 0.00 param set PE_MAG_NOISE 1.0f param set SENS_BOARD_ROT 0 -param set MPU_GYRO_LPF_ENUM 4 -param set MPU_ACC_LPF_ENUM 4 -param set MPU_SAMPLE_RATE_ENUM 2 +param set RC_RECEIVER_TYPE 1 +param set UART_ESC_MODEL 2 +param set UART_ESC_BAUDRTE 250000 +param set UART_ESC_MOTOR1 4 +param set UART_ESC_MOTOR2 2 +param set UART_ESC_MOTOR3 1 +param set UART_ESC_MOTOR4 3 sleep 1 -mpu9x50 start -D /dev/spi-1 +df_mpu9250_wrapper start +df_bmp280_wrapper start +sensors start +commander start +ekf2 start +land_detector start multicopter +mc_pos_control start +mc_att_control start uart_esc start -D /dev/tty-2 -csr_gps start -D /dev/tty-3 +rc_receiver start -D /dev/tty-1 +sleep 1 list_devices list_files list_tasks diff --git a/posix-configs/eagle/210qc/mainapp-calib.config b/posix-configs/eagle/210qc/mainapp-calib.config deleted file mode 100644 index 7130d85ded..0000000000 --- a/posix-configs/eagle/210qc/mainapp-calib.config +++ /dev/null @@ -1,8 +0,0 @@ -uorb start -muorb start -mavlink start -u 14556 -sleep 1 -mavlink stream -u 14556 -s HIGHRES_IMU -r 50 -mavlink stream -u 14556 -s ATTITUDE -r 50 -mavlink boot_complete -commander start diff --git a/posix-configs/eagle/210qc/mainapp-flight.config b/posix-configs/eagle/210qc/mainapp-flight.config deleted file mode 100644 index b511bb1121..0000000000 --- a/posix-configs/eagle/210qc/mainapp-flight.config +++ /dev/null @@ -1,7 +0,0 @@ -uorb start -muorb start -mavlink start -u 14556 -sleep 1 -mavlink stream -u 14556 -s HIGHRES_IMU -r 50 -mavlink stream -u 14556 -s ATTITUDE -r 50 -mavlink boot_complete diff --git a/posix-configs/eagle/200qx/mainapp-calib.config b/posix-configs/eagle/210qc/mainapp.config similarity index 86% rename from posix-configs/eagle/200qx/mainapp-calib.config rename to posix-configs/eagle/210qc/mainapp.config index 7130d85ded..c09015fa7e 100644 --- a/posix-configs/eagle/200qx/mainapp-calib.config +++ b/posix-configs/eagle/210qc/mainapp.config @@ -1,8 +1,8 @@ uorb start muorb start +param set MAV_BROADCAST 1 mavlink start -u 14556 sleep 1 mavlink stream -u 14556 -s HIGHRES_IMU -r 50 mavlink stream -u 14556 -s ATTITUDE -r 50 mavlink boot_complete -commander start diff --git a/posix-configs/eagle/210qc/px4-flight.config b/posix-configs/eagle/210qc/px4.config similarity index 59% rename from posix-configs/eagle/210qc/px4-flight.config rename to posix-configs/eagle/210qc/px4.config index d4da43dce7..945be5e8de 100644 --- a/posix-configs/eagle/210qc/px4-flight.config +++ b/posix-configs/eagle/210qc/px4.config @@ -1,8 +1,9 @@ uorb start -param set CAL_GYRO0_ID 2293760 -param set CAL_ACC0_ID 1310720 -param set CAL_ACC1_ID 1376256 -param set CAL_MAG0_ID 196608 +qshell start +param set CAL_GYRO0_ID 100 +param set CAL_ACC0_ID 100 +param set CAL_MAG0_ID 101 +param set SYS_AUTOSTART 4001 param set MAV_TYPE 2 param set MC_YAW_P 12 param set MC_YAWRATE_P 0.08 @@ -49,48 +50,43 @@ param set RC6_REV 1 param set ATT_W_MAG 0.00 param set PE_MAG_NOISE 1.0f param set SENS_BOARD_ROT 0 -param set MPU_GYRO_LPF_ENUM 4 -param set MPU_ACC_LPF_ENUM 4 -param set MPU_SAMPLE_RATE_ENUM 2 -param set GYRO0_XOFF -0.0028 -param set GYRO0_YOFF -0.0047 -param set GYRO0_ZOFF -0.0034 -param set GYRO0_XSCALE 1.0000 -param set GYRO0_YSCALE 1.0000 -param set GYRO0_ZSCALE 1.0000 -param set MAG0_XOFF 0.0000 -param set MAG0_YOFF 0.0000 -param set MAG0_ZOFF 0.0000 -param set MAG0_XSCALE 1.0000 -param set MAG0_YSCALE 1.0000 -param set MAG0_ZSCALE 1.0000 -param set ACC0_XOFF -0.0941 -param set ACC0_YOFF 0.1168 -param set ACC0_ZOFF -0.0398 -param set ACC0_XSCALE 1.0004 -param set ACC0_YSCALE 0.9974 -param set ACC0_ZSCALE 0.9951 +param set CAL_GYRO0_XOFF -0.0028 +param set CAL_GYRO0_YOFF -0.0047 +param set CAL_GYRO0_ZOFF -0.0034 +param set CAL_GYRO0_XSCALE 1.0000 +param set CAL_GYRO0_YSCALE 1.0000 +param set CAL_GYRO0_ZSCALE 1.0000 +param set CAL_MAG0_XOFF 0.0000 +param set CAL_MAG0_YOFF 0.0000 +param set CAL_MAG0_ZOFF 0.0000 +param set CAL_MAG0_XSCALE 1.0000 +param set CAL_MAG0_YSCALE 1.0000 +param set CAL_MAG0_ZSCALE 1.0000 +param set CAL_ACC0_XOFF -0.0941 +param set CAL_ACC0_YOFF 0.1168 +param set CAL_ACC0_ZOFF -0.0398 +param set CAL_ACC0_XSCALE 1.0004 +param set CAL_ACC0_YSCALE 0.9974 +param set CAL_ACC0_ZSCALE 0.9951 param set RC_RECEIVER_TYPE 1 param set UART_ESC_MODEL 2 -param set UART_ESC_BAUDRATE 250000 -param set UART_ESC_PX4MOTOR1 4 -param set UART_ESC_PX4MOTOR2 2 -param set UART_ESC_PX4MOTOR3 1 -param set UART_ESC_PX4MOTOR4 3 +param set UART_ESC_BAUDRTE 250000 +param set UART_ESC_MOTOR1 4 +param set UART_ESC_MOTOR2 2 +param set UART_ESC_MOTOR3 1 +param set UART_ESC_MOTOR4 3 sleep 1 -commander start -rc_receiver start -D /dev/tty-1 -sleep 1 -mpu9x50 start -D /dev/spi-1 +df_mpu9250_wrapper start df_bmp280_wrapper start sensors start -attitude_estimator_q start -position_estimator_inav start +commander start +ekf2 start +land_detector start multicopter mc_pos_control start mc_att_control start -land_detector start multicopter -sleep 1 uart_esc start -D /dev/tty-2 +rc_receiver start -D /dev/tty-1 +sleep 1 list_devices list_files list_tasks diff --git a/posix-configs/eagle/flight/mainapp.config b/posix-configs/eagle/flight/mainapp.config index e12d020c35..b60725fb03 100644 --- a/posix-configs/eagle/flight/mainapp.config +++ b/posix-configs/eagle/flight/mainapp.config @@ -1,8 +1,13 @@ uorb start muorb start +sdlog2 start -r 100 -e -t -a -b 200 +param set MAV_BROADCAST 1 +param set SDLOG_PRIO_BOOST 3 +dataman start +navigator start mavlink start -u 14556 -r 1000000 sleep 1 mavlink stream -u 14556 -s HIGHRES_IMU -r 50 mavlink stream -u 14556 -s ATTITUDE -r 50 +mavlink stream -u 14556 -s RC_CHANNELS -r 20 mavlink boot_complete -sdlog2 start -r 100 -e -t -a diff --git a/posix-configs/eagle/flight/px4.config b/posix-configs/eagle/flight/px4.config index 8149cb4d4c..c22796cbe0 100644 --- a/posix-configs/eagle/flight/px4.config +++ b/posix-configs/eagle/flight/px4.config @@ -1,12 +1,12 @@ uorb start qshell start param set SYS_AUTOSTART 4001 -gps start -d /dev/tty-4 sleep 1 +gps start -d /dev/tty-4 param set MAV_TYPE 2 +df_hmc5883_wrapper start df_mpu9250_wrapper start df_bmp280_wrapper start -df_hmc5883_wrapper start df_trone_wrapper start #df_isl29501_wrapper start sensors start @@ -15,9 +15,4 @@ ekf2 start land_detector start multicopter mc_pos_control start mc_att_control start -uart_esc start -d /dev/tty-2 -sleep 1 -list_devices -list_files -list_tasks -list_topics +pwm_out_rc_in start -d /dev/tty-2 diff --git a/posix-configs/eagle/hil/mainapphil.config b/posix-configs/eagle/hil/mainapphil.config index 743307e42f..5636501d6e 100644 --- a/posix-configs/eagle/hil/mainapphil.config +++ b/posix-configs/eagle/hil/mainapphil.config @@ -1,5 +1,6 @@ uorb start muorb start +param set MAV_BROADCAST 1 mavlink start -u 14556 sleep 1 mavlink stream -u 14556 -s HIGHRES_IMU -r 50 diff --git a/posix-configs/eagle/hil/px4.config b/posix-configs/eagle/hil/px4.config index 8b626bd322..e446ad144e 100644 --- a/posix-configs/eagle/hil/px4.config +++ b/posix-configs/eagle/hil/px4.config @@ -1,16 +1,12 @@ uorb start +qshell start param set CAL_GYRO0_ID 2293760 param set CAL_ACC0_ID 1310720 param set CAL_ACC1_ID 1376256 param set CAL_MAG0_ID 196608 -commander start -hil -sensors start -attitude_estimator_q start -position_estimator_inav start -mc_pos_control start -mc_att_control start -sleep 1 -pwm_out_sim mode_pwm +param set SYS_AUTOSTART 4001 +param set SYS_AUTOCONFIG 1 +param set MAV_TYPE 2 param set RC1_MAX 2015 param set RC1_MIN 996 param set RC1_TRIM 1502 @@ -42,7 +38,15 @@ param set ATT_W_ACC 0.0002 param set ATT_W_MAG 0.002 param set ATT_W_GYRO_BIAS 0.05 sleep 1 -param set MAV_TYPE 2 +commander start -hil +sensors start +attitude_estimator_q start +position_estimator_inav start +mc_pos_control start +mc_att_control start +land_detector start multicopter +sleep 1 +pwm_out_sim mode_pwm mixer load /dev/pwm_output0 /startup/quad_x.main.mix list_devices list_files diff --git a/posix-configs/rpi/px4.config b/posix-configs/rpi/px4.config new file mode 100644 index 0000000000..003fb52c44 --- /dev/null +++ b/posix-configs/rpi/px4.config @@ -0,0 +1,27 @@ +uorb start +param set SYS_AUTOSTART 4001 +param set MAV_BROADCAST 1 +sleep 1 +param set MAV_TYPE 2 +df_lsm9ds1_wrapper start -R 4 +#df_mpu9250_wrapper start -R 10 +#df_hmc5883_wrapper start +df_ms5611_wrapper start +gps start -d /dev/pts/0 +sensors start +commander start +attitude_estimator_q start +position_estimator_inav start +land_detector start multicopter +mc_pos_control start +mc_att_control start +mavlink start -u 14556 -r 1000000 +sleep 1 +mavlink stream -u 14556 -s HIGHRES_IMU -r 50 +mavlink stream -u 14556 -s ATTITUDE -r 50 +mavlink start -d /dev/ttyUSB0 +mavlink stream -d /dev/ttyUSB0 -s HIGHRES_IMU -r 50 +mavlink stream -d /dev/ttyUSB0 -s ATTITUDE -r 50 +navio_sysfs_rc_in start +navio_sysfs_pwm_out start +mavlink boot_complete \ No newline at end of file diff --git a/posix-configs/rpi2/init/rcS_navio b/posix-configs/rpi2/init/rcS_navio deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 771e209489..2a324e6d4a 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -255,14 +255,14 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; } - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); if (!_reports->resize(arg)) { - irqrestore(flags); + px4_leave_critical_section(flags); return -ENOMEM; } - irqrestore(flags); + px4_leave_critical_section(flags); return OK; } @@ -375,12 +375,11 @@ Airspeed::update_status() { if (_sensor_ok != _last_published_sensor_ok) { /* notify about state change */ - struct subsystem_info_s info = { - true, - true, - _sensor_ok, - subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE - }; + struct subsystem_info_s info = {}; + info.present = true; + info.enabled = true; + info.ok = _sensor_ok; + info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE; if (_subsys_pub != nullptr) { orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info); diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c index 8ad0ee7168..f76f0bc639 100644 --- a/src/drivers/ardrone_interface/ardrone_interface.c +++ b/src/drivers/ardrone_interface/ardrone_interface.c @@ -55,10 +55,6 @@ #include #include #include -#include -#include -#include -#include #include #include diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index b2faa43e1d..47b963df78 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -278,7 +278,7 @@ private: // internal variables bool _enabled; ///< true if we have successfully connected to battery work_s _work; ///< work queue for scheduling reads - RingBuffer *_reports; ///< buffer of recorded voltages, currents + ringbuffer::RingBuffer *_reports; ///< buffer of recorded voltages, currents struct battery_status_s _last_report; ///< last published report, used for test() orb_advert_t _batt_topic; ///< uORB battery topic orb_id_t _batt_orb_id; ///< uORB battery topic ID @@ -312,7 +312,7 @@ BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) : _enabled(false), _work{}, _reports(nullptr), - _batt_topic(-1), + _batt_topic(nullptr), _batt_orb_id(nullptr), _start_time(0), _batt_capacity(0), @@ -365,7 +365,7 @@ BATT_SMBUS::init() } else { // allocate basic report buffers - _reports = new RingBuffer(2, sizeof(struct battery_status_s)); + _reports = new ringbuffer::RingBuffer(2, sizeof(struct battery_status_s)); if (_reports == nullptr) { ret = ENOTTY; @@ -706,13 +706,13 @@ BATT_SMBUS::cycle() // publish to orb - if (_batt_topic != -1) { + if (_batt_topic != nullptr) { orb_publish(_batt_orb_id, _batt_topic, &new_report); } else { _batt_topic = orb_advertise(_batt_orb_id, &new_report); - if (_batt_topic < 0) { + if (_batt_topic == nullptr) { errx(1, "ADVERT FAIL"); } } diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp index 17757aaf70..f6a3c7a00f 100644 --- a/src/drivers/bma180/bma180.cpp +++ b/src/drivers/bma180/bma180.cpp @@ -65,6 +65,8 @@ #include #include +#define ACCEL_DEVICE_PATH "/dev/bma180" + /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -147,7 +149,7 @@ private: struct hrt_call _call; unsigned _call_interval; - RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; struct accel_calibration_s _accel_scale; float _accel_range_scale; @@ -281,7 +283,7 @@ BMA180::init() } /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(accel_report)); + _reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); if (_reports == nullptr) { goto out; @@ -463,14 +465,14 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; } - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); if (!_reports->resize(arg)) { - irqrestore(flags); + px4_leave_critical_section(flags); return -ENOMEM; } - irqrestore(flags); + px4_leave_critical_section(flags); return OK; } @@ -788,7 +790,7 @@ start() } /* create the driver */ - g_dev = new BMA180(1 /* XXX magic number */, (spi_dev_e)PX4_SPIDEV_ACCEL); + g_dev = new BMA180(1 /* XXX magic number */, (spi_dev_e)PX4_SPIDEV_BMA); if (g_dev == nullptr) { goto fail; diff --git a/src/drivers/bmi160/bmi160.cpp b/src/drivers/bmi160/bmi160.cpp index 260800755e..bf477e4e39 100644 --- a/src/drivers/bmi160/bmi160.cpp +++ b/src/drivers/bmi160/bmi160.cpp @@ -741,14 +741,14 @@ BMI160::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; } - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); if (!_accel_reports->resize(arg)) { - irqrestore(flags); + px4_leave_critical_section(flags); return -ENOMEM; } - irqrestore(flags); + px4_leave_critical_section(flags); return OK; } @@ -837,14 +837,14 @@ BMI160::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; } - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); if (!_gyro_reports->resize(arg)) { - irqrestore(flags); + px4_leave_critical_section(flags); return -ENOMEM; } - irqrestore(flags); + px4_leave_critical_section(flags); return OK; } diff --git a/src/drivers/qshell/posix/module.mk b/src/drivers/bmp280/CMakeLists.txt similarity index 87% rename from src/drivers/qshell/posix/module.mk rename to src/drivers/bmp280/CMakeLists.txt index 33a313576d..1199aa1d75 100644 --- a/src/drivers/qshell/posix/module.mk +++ b/src/drivers/bmp280/CMakeLists.txt @@ -31,16 +31,15 @@ # ############################################################################ -# -# Send shell commands to qurt -# - -MODULE_COMMAND = qshell - -SRCS = \ - qshell_start_qurt.cpp \ - qshell.cpp - -INCLUDE_DIRS += $(PX4_BASE)/src/modules/uORB \ - $(PX4_BASE)/src/platforms \ - $(PX4_BASE)/src/modules +px4_add_module( + MODULE drivers__bmp280 + MAIN bmp280 + COMPILE_FLAGS + -Os + SRCS + bmp280_spi.cpp + bmp280.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/bmp280/bmp280.cpp b/src/drivers/bmp280/bmp280.cpp new file mode 100644 index 0000000000..65fd7aca3c --- /dev/null +++ b/src/drivers/bmp280/bmp280.cpp @@ -0,0 +1,1022 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2016 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 bmp280.cpp + * Driver for the BMP280 barometric pressure sensor connected via I2C TODO or SPI. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + + +enum BMP280_BUS { + BMP280_BUS_ALL = 0, + BMP280_BUS_I2C_INTERNAL, + BMP280_BUS_I2C_EXTERNAL, + BMP280_BUS_SPI_INTERNAL, + BMP280_BUS_SPI_EXTERNAL +}; + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +/* + * BMP280 internal constants and data structures. + */ + +class BMP280 : public device::CDev +{ +public: + BMP280(bmp280::IBMP280 *interface, const char *path); + ~BMP280(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +private: + bmp280::IBMP280 *_interface; + + uint8_t _curr_ctrl; + + struct work_s _work; + unsigned _report_ticks; // 0 - no cycling, otherwise period of sending a report + unsigned _max_mesure_ticks; //ticks needed to measure + + ringbuffer::RingBuffer *_reports; + + bool _collect_phase; + + /* altitude conversion calibration */ + unsigned _msl_pressure; /* in Pa */ + + orb_advert_t _baro_topic; + int _orb_class_instance; + int _class_instance; + + perf_counter_t _sample_perf; + perf_counter_t _measure_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + + struct bmp280::calibration_s *_cal; //stored calibration constants + struct bmp280::fcalibration_s _fcal; //pre processed calibration constants + + float _P; /* in Pa */ + float _T; /* in K */ + + + /* periodic execution helpers */ + void start_cycle(); + void stop_cycle(); + void cycle(); //main execution + static void cycle_trampoline(void *arg); + + int measure(); //start measure + int collect(); //get results and publish +}; + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int bmp280_main(int argc, char *argv[]); + +BMP280::BMP280(bmp280::IBMP280 *interface, const char *path) : + CDev("BMP280", path), + _interface(interface), + _report_ticks(0), + _reports(nullptr), + _collect_phase(false), + _msl_pressure(101325), + _baro_topic(nullptr), + _orb_class_instance(-1), + _class_instance(-1), + _sample_perf(perf_alloc(PC_ELAPSED, "bmp280_read")), + _measure_perf(perf_alloc(PC_ELAPSED, "bmp280_measure")), + _comms_errors(perf_alloc(PC_COUNT, "bmp280_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "bmp280_buffer_overflows")) +{ + // work_cancel in stop_cycle called from the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +BMP280::~BMP280() +{ + /* make sure we are truly inactive */ + stop_cycle(); + + if (_class_instance != -1) { + unregister_class_devname(get_devname(), _class_instance); + } + + /* free any existing reports */ + if (_reports != nullptr) { + delete _reports; + } + + // free perf counters + perf_free(_sample_perf); + perf_free(_measure_perf); + perf_free(_comms_errors); + perf_free(_buffer_overflows); + + delete _interface; + + +} + +int +BMP280::init() +{ + int ret; + + ret = CDev::init(); + + if (ret != OK) { + DEVICE_DEBUG("CDev init failed"); + return ret; + } + + /* allocate basic report buffers */ + _reports = new ringbuffer::RingBuffer(2, sizeof(baro_report)); + + if (_reports == nullptr) { + DEVICE_DEBUG("can't get memory for reports"); + ret = -ENOMEM; + return ret; + } + + /* register alternate interfaces if we have to */ + _class_instance = register_class_devname(BARO_BASE_DEVICE_PATH); + + /* reset sensor */ + _interface->set_reg(BPM280_VALUE_RESET, BPM280_ADDR_RESET); + usleep(10000); + + /* check id*/ + if (_interface->get_reg(BPM280_ADDR_ID) != BPM280_VALUE_ID) { + warnx("id of your baro is not: 0x%02x", BPM280_VALUE_ID); + return -EIO; + } + + /* set config, recommended settings */ + _curr_ctrl = BPM280_CTRL_P16 | BPM280_CTRL_T2; + _interface->set_reg(_curr_ctrl, BPM280_ADDR_CTRL); + _max_mesure_ticks = USEC2TICK(BPM280_MT_INIT + BPM280_MT * (16 - 1 + 2 - 1)); + _interface->set_reg(BPM280_CONFIG_F16, BPM280_ADDR_CONFIG); + + /* get calibration and pre process them*/ + _cal = _interface->get_calibration(BPM280_ADDR_CAL); + + _fcal.t1 = _cal->t1 * powf(2, 4); + _fcal.t2 = _cal->t2 * powf(2, -14); + _fcal.t3 = _cal->t3 * powf(2, -34); + + _fcal.p1 = _cal->p1 * (powf(2, 4) / -100000.0f); + _fcal.p2 = _cal->p1 * _cal->p2 * (powf(2, -31) / -100000.0f); + _fcal.p3 = _cal->p1 * _cal->p3 * (powf(2, -51) / -100000.0f); + + _fcal.p4 = _cal->p4 * powf(2, 4) - powf(2, 20); + _fcal.p5 = _cal->p5 * powf(2, -14); + _fcal.p6 = _cal->p6 * powf(2, -31); + + _fcal.p7 = _cal->p7 * powf(2, -4); + _fcal.p8 = _cal->p8 * powf(2, -19) + 1.0f; + _fcal.p9 = _cal->p9 * powf(2, -35); + + /* do a first measurement cycle to populate reports with valid data */ + struct baro_report brp; + _reports->flush(); + + if (measure()) { + return -EIO; + } + + usleep(TICK2USEC(_max_mesure_ticks)); + + if (collect()) { + return -EIO; + } + + _reports->get(&brp); + + _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp, + &_orb_class_instance, _interface->is_external() ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT); + + if (_baro_topic == nullptr) { + warnx("failed to create sensor_baro publication"); + return -ENOMEM; + } + + return OK; + +} + +ssize_t +BMP280::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct baro_report); + struct baro_report *brp = reinterpret_cast(buffer); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) { + return -ENOSPC; + } + + /* if automatic measurement is enabled */ + if (_report_ticks > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the workq thread while we are doing this; + * we are careful to avoid racing with them. + */ + while (count--) { + if (_reports->get(brp)) { + ret += sizeof(*brp); + brp++; + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement - run one conversion */ + + _reports->flush(); + + if (measure()) { + return -EIO; + } + + usleep(TICK2USEC(_max_mesure_ticks)); + + if (collect()) { + return -EIO; + } + + if (_reports->get(brp)) { //get new generated report + ret = sizeof(*brp); + } + + return ret; +} + +int +BMP280::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + + unsigned ticks = 0; + + switch (arg) { + + case SENSOR_POLLRATE_MANUAL: + stop_cycle(); + _report_ticks = 0; + return OK; + + case SENSOR_POLLRATE_EXTERNAL: + case 0: + return -EINVAL; + + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: + ticks = _max_mesure_ticks; + + default: { + if (ticks == 0) { + ticks = USEC2TICK(USEC_PER_SEC / arg); + } + + /* do we need to start internal polling? */ + bool want_start = (_report_ticks == 0); + + /* check against maximum rate */ + if (ticks < _max_mesure_ticks) { + return -EINVAL; + } + + _report_ticks = ticks; + + if (want_start) { + start_cycle(); + } + + return OK; + } + } + + break; + } + + case SENSORIOCGPOLLRATE: + if (_report_ticks == 0) { + return SENSOR_POLLRATE_MANUAL; + } + + return (USEC_PER_SEC / USEC_PER_TICK / _report_ticks); + + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = px4_enter_critical_section(); + + if (!_reports->resize(arg)) { + px4_leave_critical_section(flags); + return -ENOMEM; + } + + px4_leave_critical_section(flags); + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _reports->size(); + + case SENSORIOCRESET: + /* + * Since we are initialized, we do not need to do anything, since the + * PROM is correctly read and the part does not need to be configured. + */ + return OK; + + case BAROIOCSMSLPRESSURE: + + /* range-check for sanity */ + if ((arg < 80000) || (arg > 120000)) { + return -EINVAL; + } + + _msl_pressure = arg; + return OK; + + case BAROIOCGMSLPRESSURE: + return _msl_pressure; + + default: + break; + } + + return CDev::ioctl(filp, cmd, arg); +} + +void +BMP280::start_cycle() +{ + + /* reset the report ring and state machine */ + _collect_phase = false; + _reports->flush(); + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&BMP280::cycle_trampoline, this, 1); +} + +void +BMP280::stop_cycle() +{ + work_cancel(HPWORK, &_work); +} + +void +BMP280::cycle_trampoline(void *arg) +{ + BMP280 *dev = reinterpret_cast(arg); + + dev->cycle(); +} + +void +BMP280::cycle() +{ + if (_collect_phase) { + collect(); + unsigned wait_gap = _report_ticks - _max_mesure_ticks; + + if (wait_gap != 0) { + work_queue(HPWORK, &_work, (worker_t)&BMP280::cycle_trampoline, this, + wait_gap); //need to wait some time before new measurement + return; + } + + } + + measure(); + work_queue(HPWORK, &_work, (worker_t)&BMP280::cycle_trampoline, this, _max_mesure_ticks); + +} + +int +BMP280::measure() +{ + _collect_phase = true; + + perf_begin(_measure_perf); + + /* start measure */ + int ret = _interface->set_reg(_curr_ctrl | BPM280_CTRL_MODE_FORCE, BPM280_ADDR_CTRL); + + if (ret != OK) { + perf_count(_comms_errors); + perf_cancel(_measure_perf); + return -EIO; + } + + perf_end(_measure_perf); + + return OK; +} + +int +BMP280::collect() +{ + _collect_phase = false; + + perf_begin(_sample_perf); + + struct baro_report report; + /* this should be fairly close to the end of the conversion, so the best approximation of the time */ + report.timestamp = hrt_absolute_time(); + report.error_count = perf_event_count(_comms_errors); + + bmp280::data_s *data = _interface->get_data(BPM280_ADDR_DATA); + + if (data == nullptr) { + perf_count(_comms_errors); + perf_cancel(_sample_perf); + return -EIO; + } + + //convert data to number 20 bit + uint32_t p_raw = data->p_msb << 12 | data->p_lsb << 4 | data->p_xlsb >> 4; + uint32_t t_raw = data->t_msb << 12 | data->t_lsb << 4 | data->t_xlsb >> 4; + + // Temperature + float ofs = (float) t_raw - _fcal.t1; + float t_fine = (ofs * _fcal.t3 + _fcal.t2) * ofs; + _T = t_fine * (1.0f / 5120.0f); + + // Pressure + float tf = t_fine - 128000.0f; + float x1 = (tf * _fcal.p6 + _fcal.p5) * tf + _fcal.p4; + float x2 = (tf * _fcal.p3 + _fcal.p2) * tf + _fcal.p1; + + float pf = ((float) p_raw + x1) / x2; + _P = (pf * _fcal.p9 + _fcal.p8) * pf + _fcal.p7; + + + report.temperature = _T; + report.pressure = _P / 100.0f; // to mbar + + + /* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */ + + /* tropospheric properties (0-11km) for standard atmosphere */ + const float T1 = 15.0f + 273.15f; /* temperature at base height in Kelvin */ + const float a = -6.5f / 1000.0f; /* temperature gradient in degrees per metre */ + const float g = 9.80665f; /* gravity constant in m/s/s */ + const float R = 287.05f; /* ideal gas constant in J/kg/K */ + float pK = _P / _msl_pressure; + + /* + * Solve: + * + * / -(aR / g) \ + * | (p / p1) . T1 | - T1 + * \ / + * h = ------------------------------- + h1 + * a + */ + report.altitude = (((powf(pK, (-(a * R) / g))) * T1) - T1) / a; + + + /* publish it */ + if (!(_pub_blocked)) { + /* publish it */ + orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); + } + + if (_reports->force(&report)) { + perf_count(_buffer_overflows); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + perf_end(_sample_perf); + + return OK; +} + +void +BMP280::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_buffer_overflows); + printf("poll interval: %u us \n", _report_ticks * USEC_PER_TICK); + _reports->print_info("report queue"); + printf("P Pa: %.3f\n", (double)_P); + printf("T: %.3f\n", (double)_T); + printf("MSL pressure Pa: %u\n", _msl_pressure); + +} + +/** + * Local functions in support of the shell command. + */ +namespace bmp280 +{ + +/* + list of supported bus configurations + */ +struct bmp280_bus_option { + enum BMP280_BUS busid; + const char *devpath; + BMP280_constructor interface_constructor; + uint8_t busnum; + uint8_t device; + bool external; + BMP280 *dev; +} bus_options[] = { +#if defined(PX4_SPIDEV_EXT_BARO) && defined(PX4_SPI_BUS_EXT) + { BMP280_BUS_SPI_EXTERNAL, "/dev/bmp280_spi_ext", &bmp280_spi_interface, PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_BARO , true , NULL }, +#endif +#ifdef PX4_SPIDEV_BARO + { BMP280_BUS_SPI_INTERNAL, "/dev/bmp280_spi_int", &bmp280_spi_interface, PX4_SPI_BUS_SENSORS, PX4_SPIDEV_BARO, false , NULL }, +#endif +#ifdef PX4_I2C_OBDEV_BMP280 + { BMP280_BUS_I2C_INTERNAL, "/dev/bmp280_i2c_int", nullptr, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_BMP280, false, NULL }, +#endif +#if defined(PX4_I2C_BUS_EXPANSION) && defined(PX4_I2C_EXT_OBDEV_BMP280) + { BMP280_BUS_I2C_EXTERNAL, "/dev/bmp280_i2c_ext", nullptr, PX4_I2C_BUS_EXPANSION, PX4_I2C_EXT_OBDEV_BMP280 , true , NULL }, +#endif +}; +#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) + +bool start_bus(struct bmp280_bus_option &bus); +struct bmp280_bus_option &find_bus(enum BMP280_BUS busid); +void start(enum BMP280_BUS busid); +void test(enum BMP280_BUS busid); +void reset(enum BMP280_BUS busid); +void info(); +void calibrate(unsigned altitude, enum BMP280_BUS busid); +void usage(); + + +/** + * Start the driver. + */ +bool +start_bus(struct bmp280_bus_option &bus) +{ + if (bus.dev != nullptr) { + errx(1, "bus option already started"); + } + + bmp280::IBMP280 *interface = bus.interface_constructor(bus.busnum, bus.device, bus.external); + + if (interface->init() != OK) { + delete interface; + warnx("no device on bus %u", (unsigned)bus.busid); + return false; + } + + bus.dev = new BMP280(interface, bus.devpath); + + if (bus.dev != nullptr && OK != bus.dev->init()) { + delete bus.dev; + bus.dev = NULL; + return false; + } + + int fd = open(bus.devpath, O_RDONLY); + + /* set the poll rate to default, starts automatic data collection */ + if (fd == -1) { + errx(1, "can't open baro device"); + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + close(fd); + errx(1, "failed setting default poll rate"); + } + + close(fd); + return true; +} + + +/** + * Start the driver. + * + * This function call only returns once the driver + * is either successfully up and running or failed to start. + */ +void +start(enum BMP280_BUS busid) +{ + uint8_t i; + bool started = false; + + for (i = 0; i < NUM_BUS_OPTIONS; i++) { + if (busid == BMP280_BUS_ALL && bus_options[i].dev != NULL) { + // this device is already started + continue; + } + + if (busid != BMP280_BUS_ALL && bus_options[i].busid != busid) { + // not the one that is asked for + continue; + } + + started |= start_bus(bus_options[i]); + } + + if (!started) { + errx(1, "driver start failed"); + } + + // one or more drivers started OK + exit(0); +} + + +/** + * find a bus structure for a busid + */ +struct bmp280_bus_option &find_bus(enum BMP280_BUS busid) +{ + for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) { + if ((busid == BMP280_BUS_ALL || + busid == bus_options[i].busid) && bus_options[i].dev != NULL) { + return bus_options[i]; + } + } + + errx(1, "bus %u not started", (unsigned)busid); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test(enum BMP280_BUS busid) +{ + struct bmp280_bus_option &bus = find_bus(busid); + struct baro_report report; + ssize_t sz; + int ret; + + int fd; + + fd = open(bus.devpath, O_RDONLY); + + if (fd < 0) { + err(1, "open failed (try 'bmp280 start' if the driver is not running)"); + } + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + err(1, "immediate read failed"); + } + + warnx("single read"); + warnx("pressure: %10.4f", (double)report.pressure); + warnx("altitude: %11.4f", (double)report.altitude); + warnx("temperature: %8.4f", (double)report.temperature); + warnx("time: %lld", report.timestamp); + + /* set the queue depth to 10 */ + if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) { + errx(1, "failed to set queue depth"); + } + + /* start the sensor polling at 2Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { + errx(1, "failed to set 2Hz poll rate"); + } + + /* read the sensor 5x and report each value */ + for (unsigned i = 0; i < 5; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 2000); + + if (ret != 1) { + errx(1, "timed out waiting for sensor data"); + } + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + err(1, "periodic read failed"); + } + + warnx("periodic read %u", i); + warnx("pressure: %10.4f", (double)report.pressure); + warnx("altitude: %11.4f", (double)report.altitude); + warnx("temperature K: %8.4f", (double)report.temperature); + warnx("time: %lld", report.timestamp); + } + + close(fd); + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset(enum BMP280_BUS busid) +{ + struct bmp280_bus_option &bus = find_bus(busid); + int fd; + + fd = open(bus.devpath, O_RDONLY); + + if (fd < 0) { + err(1, "failed "); + } + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { + err(1, "driver reset failed"); + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + err(1, "driver poll restart failed"); + } + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) { + struct bmp280_bus_option &bus = bus_options[i]; + + if (bus.dev != nullptr) { + warnx("%s", bus.devpath); + bus.dev->print_info(); + } + } + + exit(0); +} + +/** + * Calculate actual MSL pressure given current altitude + */ +void +calibrate(unsigned altitude, enum BMP280_BUS busid) +{ + struct bmp280_bus_option &bus = find_bus(busid); + struct baro_report report; + float pressure; + float p1; + + int fd; + + fd = open(bus.devpath, O_RDONLY); + + if (fd < 0) { + err(1, "open failed (try 'bmp280 start' if the driver is not running)"); + } + + /* start the sensor polling at max */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX)) { + errx(1, "failed to set poll rate"); + } + + /* average a few measurements */ + pressure = 0.0f; + + for (unsigned i = 0; i < 20; i++) { + struct pollfd fds; + int ret; + ssize_t sz; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 1000); + + if (ret != 1) { + errx(1, "timed out waiting for sensor data"); + } + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + err(1, "sensor read failed"); + } + + pressure += report.pressure; + } + + pressure /= 20; /* average */ + pressure /= 10; /* scale from millibar to kPa */ + + /* tropospheric properties (0-11km) for standard atmosphere */ + const float T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */ + const float a = -6.5 / 1000; /* temperature gradient in degrees per metre */ + const float g = 9.80665f; /* gravity constant in m/s/s */ + const float R = 287.05f; /* ideal gas constant in J/kg/K */ + + warnx("averaged pressure %10.4fkPa at %um", (double)pressure, altitude); + + p1 = pressure * (powf(((T1 + (a * (float)altitude)) / T1), (g / (a * R)))); + + warnx("calculated MSL pressure %10.4fkPa", (double)p1); + + /* save as integer Pa */ + p1 *= 1000.0f; + + if (ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK) { + err(1, "BAROIOCSMSLPRESSURE"); + } + + close(fd); + exit(0); +} + +void +usage() +{ + warnx("missing command: try 'start', 'info', 'test', 'test2', 'reset', 'calibrate'"); + warnx("options:"); + warnx(" -X (external I2C bus TODO)"); + warnx(" -I (internal I2C bus TODO)"); + warnx(" -S (external SPI bus)"); + warnx(" -s (internal SPI bus)"); +} + +} // namespace + +int +bmp280_main(int argc, char *argv[]) +{ + enum BMP280_BUS busid = BMP280_BUS_ALL; + int ch; + + /* jump over start/off/etc and look at options first */ + while ((ch = getopt(argc, argv, "XISs")) != EOF) { + switch (ch) { + case 'X': + busid = BMP280_BUS_I2C_EXTERNAL; + errx(1, "not supported yet"); + break; + + case 'I': + busid = BMP280_BUS_I2C_INTERNAL; + errx(1, "not supported yet"); + break; + + case 'S': + busid = BMP280_BUS_SPI_EXTERNAL; + break; + + case 's': + busid = BMP280_BUS_SPI_INTERNAL; + break; + + default: + bmp280::usage(); + exit(0); + } + } + + const char *verb = argv[optind]; + + /* + * Start/load the driver. + */ + if (!strcmp(verb, "start")) { + bmp280::start(busid); + } + + /* + * Test the driver/device. + */ + if (!strcmp(verb, "test")) { + bmp280::test(busid); + } + + /* + * Reset the driver. + */ + if (!strcmp(verb, "reset")) { + bmp280::reset(busid); + } + + /* + * Print driver information. + */ + if (!strcmp(verb, "info")) { + bmp280::info(); + } + + /* + * Perform MSL pressure calibration given an altitude in metres + */ + if (!strcmp(verb, "calibrate")) { + if (argc < 2) { + errx(1, "missing altitude"); + } + + long altitude = strtol(argv[optind + 1], nullptr, 10); + + bmp280::calibrate(altitude, busid); + } + + errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); +} diff --git a/src/drivers/bmp280/bmp280.h b/src/drivers/bmp280/bmp280.h new file mode 100644 index 0000000000..a2399bf395 --- /dev/null +++ b/src/drivers/bmp280/bmp280.h @@ -0,0 +1,152 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2016 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 bmp280.h + * + * Shared defines for the bmp280 driver. + */ +#pragma once + +#define BPM280_ADDR_CAL 0x88 /* address of 12x 2 bytes calibration data */ +#define BPM280_ADDR_DATA 0xF7 /* address of 2x 3 bytes p-t data */ + +#define BPM280_ADDR_CONFIG 0xF5 /* configuration */ +#define BPM280_ADDR_CTRL 0xF4 /* controll */ +#define BPM280_ADDR_STATUS 0xF3 /* state */ +#define BPM280_ADDR_RESET 0xE0 /* reset */ +#define BPM280_ADDR_ID 0xD0 /* id */ + +#define BPM280_VALUE_ID 0x58 /* chip id */ +#define BPM280_VALUE_RESET 0xB6 /* reset */ + +#define BPM280_STATUS_MEASURING 1<<3 /* if in process of measure */ +#define BPM280_STATUS_COPING 1<<0 /* if in process of data copy */ + +#define BPM280_CTRL_P0 0x0<<2 /* no p measure */ +#define BPM280_CTRL_P1 0x1<<2 +#define BPM280_CTRL_P2 0x2<<2 +#define BPM280_CTRL_P4 0x3<<2 +#define BPM280_CTRL_P8 0x4<<2 +#define BPM280_CTRL_P16 0x5<<2 + +#define BPM280_CTRL_T0 0x0<<5 /* no t measure */ +#define BPM280_CTRL_T1 0x1<<5 +#define BPM280_CTRL_T2 0x2<<5 +#define BPM280_CTRL_T4 0x3<<5 +#define BPM280_CTRL_T8 0x4<<5 +#define BPM280_CTRL_T16 0x5<<5 + +#define BPM280_CONFIG_F0 0x0<<2 /* no filter */ +#define BPM280_CONFIG_F2 0x1<<2 +#define BPM280_CONFIG_F4 0x2<<2 +#define BPM280_CONFIG_F8 0x3<<2 +#define BPM280_CONFIG_F16 0x4<<2 + + +#define BPM280_CTRL_MODE_SLEEP 0x0 +#define BPM280_CTRL_MODE_FORCE 0x1 /* on demand, goes to sleep after */ +#define BPM280_CTRL_MODE_NORMAL 0x3 + +#define BPM280_MT_INIT 6400 /* max measure time of initial p + t in us */ +#define BPM280_MT 2300 /* max measure time of p or t in us */ + +namespace bmp280 +{ + +#pragma pack(push,1) +struct calibration_s { + uint16_t t1; + int16_t t2; + int16_t t3; + + uint16_t p1; + int16_t p2; + int16_t p3; + int16_t p4; + int16_t p5; + int16_t p6; + int16_t p7; + int16_t p8; + int16_t p9; +}; //calibration data + +struct data_s { + uint8_t p_msb; + uint8_t p_lsb; + uint8_t p_xlsb; + + uint8_t t_msb; + uint8_t t_lsb; + uint8_t t_xlsb; +}; // data +#pragma pack(pop) + +struct fcalibration_s { + float t1; + float t2; + float t3; + + float p1; + float p2; + float p3; + float p4; + float p5; + float p6; + float p7; + float p8; + float p9; +}; + +class IBMP280 +{ +public: + virtual ~IBMP280() = 0; + + virtual bool is_external() = 0; + virtual int init() = 0; + + virtual uint8_t get_reg(uint8_t addr) = 0; //read reg value + virtual int set_reg(uint8_t value, uint8_t addr) = 0; //write reg value + virtual bmp280::data_s *get_data(uint8_t addr) = 0; //bulk read of data into buffer, return same pointer + virtual bmp280::calibration_s *get_calibration(uint8_t addr) = + 0; //bulk read of calibration data into buffer, return same pointer + +}; + +} /* namespace */ + +/* interface factories */ +extern bmp280::IBMP280 *bmp280_spi_interface(uint8_t busnum, uint8_t device, bool external); +extern bmp280::IBMP280 *bmp280_i2c_interface(uint8_t busnum, uint8_t device, bool external); +typedef bmp280::IBMP280 *(*BMP280_constructor)(uint8_t, uint8_t, bool); diff --git a/src/drivers/bmp280/bmp280_spi.cpp b/src/drivers/bmp280/bmp280_spi.cpp new file mode 100644 index 0000000000..a5b2b08d01 --- /dev/null +++ b/src/drivers/bmp280/bmp280_spi.cpp @@ -0,0 +1,157 @@ +/**************************************************************************** + * + * Copyright (c) 2016 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 bmp280_spi.cpp + * + * SPI interface for BMP280 + */ + +#include + +#include +#include + +#include "board_config.h" + +/* SPI protocol address bits */ +#define DIR_READ (1<<7) //for set +#define DIR_WRITE ~(1<<7) //for clear + +#if defined(PX4_SPIDEV_BARO) || defined(PX4_SPIDEV_EXT_BARO) + +#pragma pack(push,1) +struct spi_data_s { + uint8_t addr; + struct bmp280::data_s data; +}; + +struct spi_calibration_s { + uint8_t addr; + struct bmp280::calibration_s cal; +}; +#pragma pack(pop) + +class BMP280_SPI: public device::SPI, public bmp280::IBMP280 +{ +public: + BMP280_SPI(uint8_t bus, spi_dev_e device, bool external); + ~BMP280_SPI(); + + bool is_external(); + int init(); + + uint8_t get_reg(uint8_t addr); + int set_reg(uint8_t value, uint8_t addr); + bmp280::data_s *get_data(uint8_t addr); + bmp280::calibration_s *get_calibration(uint8_t addr); + +private: + spi_calibration_s _cal; + spi_data_s _data; + bool _external; +}; + +bmp280::IBMP280 *bmp280_spi_interface(uint8_t busnum, uint8_t device, bool external) +{ + return new BMP280_SPI(busnum, (spi_dev_e)device, external); +} + +BMP280_SPI::BMP280_SPI(uint8_t bus, spi_dev_e device, bool external) : + SPI("BMP280_SPI", nullptr, bus, device, SPIDEV_MODE3, 10 * 1000 * 1000) +{ + _external = external; +} + +bmp280::IBMP280::~IBMP280() +{ +} + +BMP280_SPI::~BMP280_SPI() +{ +} + + +bool BMP280_SPI::is_external() +{ + return _external; +}; + +int BMP280_SPI::init() +{ + return SPI::init(); +}; + +uint8_t BMP280_SPI::get_reg(uint8_t addr) +{ + uint8_t cmd[2] = { (uint8_t)(addr | DIR_READ), 0}; //set MSB bit + transfer(&cmd[0], &cmd[0], 2); + + return cmd[1]; +} + +int BMP280_SPI::set_reg(uint8_t value, uint8_t addr) +{ + uint8_t cmd[2] = { (uint8_t)(addr & DIR_WRITE), value}; //clear MSB bit + return transfer(&cmd[0], nullptr, 2); +} + +bmp280::data_s *BMP280_SPI::get_data(uint8_t addr) +{ + _data.addr = (uint8_t)(addr | DIR_READ); //set MSB bit + + if (transfer((uint8_t *)&_data, (uint8_t *)&_data, sizeof(struct spi_data_s)) == OK) { + return &(_data.data); + + } else { + return nullptr; + } + + +} + +bmp280::calibration_s *BMP280_SPI::get_calibration(uint8_t addr) +{ + _cal.addr = addr | DIR_READ; + + if (transfer((uint8_t *)&_cal, (uint8_t *)&_cal, sizeof(struct spi_calibration_s)) == OK) { + return &(_cal.cal); + + } else { + return nullptr; + } +} + + + +#endif /* PX4_SPIDEV_BARO || PX4_SPIDEV_EXT_BARO */ diff --git a/src/drivers/boards/aerocore/CMakeLists.txt b/src/drivers/boards/aerocore/CMakeLists.txt index f1fb63a255..23be262b75 100644 --- a/src/drivers/boards/aerocore/CMakeLists.txt +++ b/src/drivers/boards/aerocore/CMakeLists.txt @@ -35,6 +35,7 @@ px4_add_module( COMPILE_FLAGS -Os SRCS + ../common/board_name.c aerocore_init.c aerocore_pwm_servo.c aerocore_spi.c diff --git a/src/drivers/boards/aerocore/aerocore_init.c b/src/drivers/boards/aerocore/aerocore_init.c index 0597d56bcc..0a58f4a845 100644 --- a/src/drivers/boards/aerocore/aerocore_init.c +++ b/src/drivers/boards/aerocore/aerocore_init.c @@ -225,10 +225,10 @@ __EXPORT int nsh_archinitialize(void) { /* configure ADC pins */ - stm32_configgpio(GPIO_ADC1_IN10); /* used by VBUS valid */ - stm32_configgpio(GPIO_ADC1_IN11); /* J1 breakout */ - stm32_configgpio(GPIO_ADC1_IN12); /* J1 breakout */ - stm32_configgpio(GPIO_ADC1_IN13); /* J1 breakout */ + px4_arch_configgpio(GPIO_ADC1_IN10); /* used by VBUS valid */ + px4_arch_configgpio(GPIO_ADC1_IN11); /* J1 breakout */ + px4_arch_configgpio(GPIO_ADC1_IN12); /* J1 breakout */ + px4_arch_configgpio(GPIO_ADC1_IN13); /* J1 breakout */ /* configure the high-resolution time/callout interface */ hrt_init(); @@ -263,7 +263,7 @@ __EXPORT int nsh_archinitialize(void) led_off(LED_AMBER); /* Configure Sensors on SPI bus #3 */ - spi3 = up_spiinitialize(3); + spi3 = px4_spibus_initialize(3); if (!spi3) { message("[boot] FAILED to initialize SPI port 3\n"); @@ -282,7 +282,7 @@ __EXPORT int nsh_archinitialize(void) message("[boot] Initialized SPI port 3 (SENSORS)\n"); /* Configure FRAM on SPI bus #4 */ - spi4 = up_spiinitialize(4); + spi4 = px4_spibus_initialize(4); if (!spi4) { message("[boot] FAILED to initialize SPI port 4\n"); diff --git a/src/drivers/boards/aerocore/aerocore_led.c b/src/drivers/boards/aerocore/aerocore_led.c index 89c6367419..91956e420b 100644 --- a/src/drivers/boards/aerocore/aerocore_led.c +++ b/src/drivers/boards/aerocore/aerocore_led.c @@ -63,19 +63,19 @@ __END_DECLS __EXPORT void led_init() { - stm32_configgpio(GPIO_LED0); - stm32_configgpio(GPIO_LED1); + px4_arch_configgpio(GPIO_LED0); + px4_arch_configgpio(GPIO_LED1); } __EXPORT void led_on(int led) { switch (led) { case 0: - stm32_gpiowrite(GPIO_LED0, true); + px4_arch_gpiowrite(GPIO_LED0, true); break; case 1: - stm32_gpiowrite(GPIO_LED1, true); + px4_arch_gpiowrite(GPIO_LED1, true); break; default: @@ -87,11 +87,11 @@ __EXPORT void led_off(int led) { switch (led) { case 0: - stm32_gpiowrite(GPIO_LED0, false); + px4_arch_gpiowrite(GPIO_LED0, false); break; case 1: - stm32_gpiowrite(GPIO_LED1, false); + px4_arch_gpiowrite(GPIO_LED1, false); break; default: @@ -103,21 +103,21 @@ __EXPORT void led_toggle(int led) { switch (led) { case 0: - if (stm32_gpioread(GPIO_LED0)) { - stm32_gpiowrite(GPIO_LED0, false); + if (px4_arch_gpioread(GPIO_LED0)) { + px4_arch_gpiowrite(GPIO_LED0, false); } else { - stm32_gpiowrite(GPIO_LED0, true); + px4_arch_gpiowrite(GPIO_LED0, true); } break; case 1: - if (stm32_gpioread(GPIO_LED1)) { - stm32_gpiowrite(GPIO_LED1, false); + if (px4_arch_gpioread(GPIO_LED1)) { + px4_arch_gpiowrite(GPIO_LED1, false); } else { - stm32_gpiowrite(GPIO_LED1, true); + px4_arch_gpiowrite(GPIO_LED1, true); } break; diff --git a/src/drivers/boards/aerocore/aerocore_spi.c b/src/drivers/boards/aerocore/aerocore_spi.c index 28103c843e..c6015689c0 100644 --- a/src/drivers/boards/aerocore/aerocore_spi.c +++ b/src/drivers/boards/aerocore/aerocore_spi.c @@ -70,36 +70,36 @@ __EXPORT void stm32_spiinitialize(void) { #ifdef CONFIG_STM32_SPI1 - stm32_configgpio(GPIO_SPI1_NSS); - stm32_gpiowrite(GPIO_SPI1_NSS, 1); + px4_arch_configgpio(GPIO_SPI1_NSS); + px4_arch_gpiowrite(GPIO_SPI1_NSS, 1); #endif #ifdef CONFIG_STM32_SPI2 - stm32_configgpio(GPIO_SPI2_NSS); - stm32_gpiowrite(GPIO_SPI2_NSS, 1); + px4_arch_configgpio(GPIO_SPI2_NSS); + px4_arch_gpiowrite(GPIO_SPI2_NSS, 1); #endif #ifdef CONFIG_STM32_SPI3 - stm32_configgpio(GPIO_SPI_CS_GYRO); - stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG); - stm32_configgpio(GPIO_SPI_CS_BARO); + px4_arch_configgpio(GPIO_SPI_CS_GYRO); + px4_arch_configgpio(GPIO_SPI_CS_ACCEL_MAG); + px4_arch_configgpio(GPIO_SPI_CS_BARO); /* De-activate all peripherals, * required for some peripheral * state machines */ - stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); - stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); - stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1); - stm32_configgpio(GPIO_EXTI_GYRO_DRDY); - stm32_configgpio(GPIO_EXTI_MAG_DRDY); - stm32_configgpio(GPIO_EXTI_ACCEL_DRDY); + px4_arch_configgpio(GPIO_EXTI_GYRO_DRDY); + px4_arch_configgpio(GPIO_EXTI_MAG_DRDY); + px4_arch_configgpio(GPIO_EXTI_ACCEL_DRDY); #endif #ifdef CONFIG_STM32_SPI4 - stm32_configgpio(GPIO_SPI4_NSS); - stm32_gpiowrite(GPIO_SPI4_NSS, 1); + px4_arch_configgpio(GPIO_SPI4_NSS); + px4_arch_gpiowrite(GPIO_SPI4_NSS, 1); #endif } @@ -107,7 +107,7 @@ __EXPORT void stm32_spiinitialize(void) __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) { /* there is only one device broken-out so select it */ - stm32_gpiowrite(GPIO_SPI1_NSS, !selected); + px4_arch_gpiowrite(GPIO_SPI1_NSS, !selected); } __EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) @@ -120,7 +120,7 @@ __EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devi __EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) { /* there is only one device broken-out so select it */ - stm32_gpiowrite(GPIO_SPI2_NSS, !selected); + px4_arch_gpiowrite(GPIO_SPI2_NSS, !selected); } __EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) @@ -137,23 +137,23 @@ __EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, switch (devid) { case PX4_SPIDEV_GYRO: /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); - stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); - stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, !selected); + px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1); break; case PX4_SPIDEV_ACCEL_MAG: /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); - stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected); - stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected); + px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1); break; case PX4_SPIDEV_BARO: /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); - stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); - stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected); + px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_BARO, !selected); break; default: @@ -172,7 +172,7 @@ __EXPORT uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, enum spi_dev_e devi __EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) { /* there can only be one device on this bus, so always select it */ - stm32_gpiowrite(GPIO_SPI4_NSS, !selected); + px4_arch_gpiowrite(GPIO_SPI4_NSS, !selected); } __EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) diff --git a/src/drivers/boards/aerocore/board_config.h b/src/drivers/boards/aerocore/board_config.h index d89b383c62..9eb97d559a 100644 --- a/src/drivers/boards/aerocore/board_config.h +++ b/src/drivers/boards/aerocore/board_config.h @@ -172,6 +172,22 @@ __BEGIN_DECLS #define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0) #define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN0) +#define BOARD_NAME "AEROCORE" + +#define BOARD_HAS_PWM 8 +/* AeroCore breaks out User GPIOs on J11 */ +#define BOARD_FMU_GPIO_TAB { \ + {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, \ + {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, \ + {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0}, \ + {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0}, \ + {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0}, \ + {GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, 0}, \ + {GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, 0}, \ + {GPIO_GPIO8_INPUT, GPIO_GPIO8_OUTPUT, 0}, \ + {GPIO_GPIO9_INPUT, GPIO_GPIO9_OUTPUT, 0}, \ + {GPIO_GPIO10_INPUT, GPIO_GPIO10_OUTPUT, 0}, \ + {GPIO_GPIO11_INPUT, GPIO_GPIO11_OUTPUT, 0}, } /**************************************************************************************************** * Public Types @@ -196,6 +212,9 @@ __BEGIN_DECLS ****************************************************************************************************/ extern void stm32_spiinitialize(void); +#define board_spi_reset(ms) + +#define board_peripheral_reset(ms) /**************************************************************************** * Name: nsh_archinitialize @@ -216,6 +235,8 @@ extern void stm32_spiinitialize(void); int nsh_archinitialize(void); #endif +#include "../common/board_common.h" + #endif /* __ASSEMBLY__ */ __END_DECLS diff --git a/src/drivers/boards/common/board_common.h b/src/drivers/boards/common/board_common.h new file mode 100644 index 0000000000..d98e218924 --- /dev/null +++ b/src/drivers/boards/common/board_common.h @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * Copyright (C) 2016 PX4 Development Team. All rights reserved. + * Author: David Sidrane + * + * 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 board_common.h + * + * Provide the the common board interfaces + */ + +#pragma once + +/************************************************************************************ + * Included Files + ************************************************************************************/ +#include +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: board_name + * + * Description: + * All boards must provide this API to return the board name. + * + ************************************************************************************/ + +__EXPORT const char *board_name(void); + +/************************************************************************************ + * Name: board_dma_alloc_init + * + * Description: + * All boards may optionally provide this API to instantiate a pool of + * memory for uses with FAST FS DMA operations. + * + * Provision is controlled by declaring BOARD_DMA_ALLOC_POOL_SIZE in board_config.h + * + * + ************************************************************************************/ +#if defined(BOARD_DMA_ALLOC_POOL_SIZE) +__EXPORT int board_dma_alloc_init(void); +#else +#define board_dma_alloc_init() (-EPERM) +#endif + +/************************************************************************************ + * Name: board_get_dma_usage + * + * Description: + * All boards may optionally provide this API to supply instrumentation for a pool of + * memory used for DMA operations. + * + * Provision is controlled by declaring BOARD_DMA_ALLOC_POOL_SIZE in board_config.h + * + * + ************************************************************************************/ +#if defined(BOARD_DMA_ALLOC_POOL_SIZE) +__EXPORT int board_get_dma_usage(uint16_t *dma_total, uint16_t *dma_used, uint16_t *dma_peak_used); +#else +#define board_get_dma_usage(dma_total,dma_used, dma_peak_used) (-ENOMEM) +#endif diff --git a/src/drivers/boards/common/board_dma_alloc.c b/src/drivers/boards/common/board_dma_alloc.c new file mode 100644 index 0000000000..9ba4f23483 --- /dev/null +++ b/src/drivers/boards/common/board_dma_alloc.c @@ -0,0 +1,156 @@ +/**************************************************************************** + * + * Copyright (C) 2016 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 board_dma_alloc.c + * + * Provide the board dma allocator interface. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include +#include "board_config.h" + +#include +#include +#include + +#include + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: board_dma_alloc_init + * + * Description: + * All boards may optionally provide this API to instantiate a pool of + * memory for uses with FAST FS DMA operations. + * + ************************************************************************************/ +#if defined(BOARD_DMA_ALLOC_POOL_SIZE) + +# if !defined(CONFIG_GRAN) || !defined(CONFIG_FAT_DMAMEMORY) +# error microSD DMA support requires CONFIG_GRAN +# endif + +static GRAN_HANDLE dma_allocator; + +/* + * The DMA heap size constrains the total number of things that can be + * ready to do DMA at a time. + * + * For example, FAT DMA depends on one sector-sized buffer per filesystem plus + * one sector-sized buffer per file. + * + * We use a fundamental alignment / granule size of 64B; this is sufficient + * to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits). + */ +static uint8_t g_dma_heap[BOARD_DMA_ALLOC_POOL_SIZE] __attribute__((aligned(64))); +static perf_counter_t g_dma_perf; +static uint16_t dma_heap_inuse; +static uint16_t dma_heap_peak_use; +/**************************************************************************** + * Public Functions + ****************************************************************************/ +__EXPORT int board_dma_alloc_init(void) +{ + dma_allocator = gran_initialize(g_dma_heap, + sizeof(g_dma_heap), + 7, /* 128B granule - must be > alignment (XXX bug?) */ + 6); /* 64B alignment */ + + if (dma_allocator == NULL) { + return -ENOMEM; + + } else { + dma_heap_inuse = 0; + dma_heap_peak_use = 0; + g_dma_perf = perf_alloc(PC_COUNT, "dma_alloc"); + } + + return OK; +} + +__EXPORT int board_get_dma_usage(uint16_t *dma_total, uint16_t *dma_used, uint16_t *dma_peak_used) +{ + *dma_total = sizeof(g_dma_heap); + *dma_used = dma_heap_inuse; + *dma_peak_used = dma_heap_peak_use; + + return OK; +} +/* + * DMA-aware allocator stubs for the FAT filesystem. + */ + +__EXPORT void *fat_dma_alloc(size_t size); +__EXPORT void fat_dma_free(FAR void *memory, size_t size); + +void * +fat_dma_alloc(size_t size) +{ + void *rv = NULL; + perf_count(g_dma_perf); + rv = gran_alloc(dma_allocator, size); + + if (rv != NULL) { + dma_heap_inuse += size; + + if (dma_heap_inuse > dma_heap_peak_use) { + dma_heap_peak_use = dma_heap_inuse; + } + } + + return rv; +} + +void +fat_dma_free(FAR void *memory, size_t size) +{ + gran_free(dma_allocator, memory, size); + dma_heap_inuse -= size; +} +#endif /* defined(BOARD_DMA_ALLOC_POOL_SIZE) */ diff --git a/src/drivers/boards/common/board_name.c b/src/drivers/boards/common/board_name.c new file mode 100644 index 0000000000..8108cec4aa --- /dev/null +++ b/src/drivers/boards/common/board_name.c @@ -0,0 +1,71 @@ +/**************************************************************************** + * + * Copyright (C) 2016 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 board_name.c + * + * Provide the board_name interface. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include +#include "board_config.h" +#include + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: board_name + * + * Description: + * All boards must provide this API to return the board name. + * + ************************************************************************************/ + +__EXPORT const char *board_name() +{ + return BOARD_NAME; +} diff --git a/src/drivers/boards/mindpx-v2/CMakeLists.txt b/src/drivers/boards/mindpx-v2/CMakeLists.txt index 306c3ab0b0..06598cda39 100644 --- a/src/drivers/boards/mindpx-v2/CMakeLists.txt +++ b/src/drivers/boards/mindpx-v2/CMakeLists.txt @@ -35,6 +35,8 @@ px4_add_module( COMPILE_FLAGS -Os SRCS + ../common/board_dma_alloc.c + ../common/board_name.c mindpx_can.c mindpx2_init.c mindpx_timer_config.c diff --git a/src/drivers/boards/mindpx-v2/board_config.h b/src/drivers/boards/mindpx-v2/board_config.h index b07949e1a6..9176246e66 100644 --- a/src/drivers/boards/mindpx-v2/board_config.h +++ b/src/drivers/boards/mindpx-v2/board_config.h @@ -43,7 +43,7 @@ * Included Files ****************************************************************************************************/ -#include +#include #include #include @@ -226,6 +226,11 @@ __BEGIN_DECLS #define GPIO_GPIO12_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN15) #define GPIO_GPIO13_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN6) +#define BOARD_HAS_LED_PWM +#define BOARD_LED_PWM_DRIVE_ACTIVE_LOW +#define LED_TIM3_CH3OUT (GPIO_ALT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OPENDRAIN|GPIO_PORTB|GPIO_PIN0) +#define LED_TIM3_CH2OUT (GPIO_ALT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OPENDRAIN|GPIO_PORTC|GPIO_PIN7) +#define LED_TIM3_CH4OUT (GPIO_ALT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OPENDRAIN|GPIO_PORTB|GPIO_PIN1) /* Power supply control and monitoring GPIOs */ //#define GPIO_VDD_5V_PERIPH_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8) @@ -290,6 +295,39 @@ __BEGIN_DECLS #define PWMIN_TIMER_CHANNEL 1 #define GPIO_PWM_IN GPIO_TIM3_CH1IN_3 +#define BOARD_NAME "MINDPX_V2" + +/* By Providing BOARD_ADC_USB_CONNECTED this board support the ADC + * system_power interface, and therefore provides the true logic + * GPIO BOARD_ADC_xxxx macros. + */ +#define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS)) +#define BOARD_ADC_BRICK_VALID (1) +#define BOARD_ADC_SERVO_VALID (1) +#define BOARD_ADC_PERIPH_5V_OC (0) +#define BOARD_ADC_HIPOWER_5V_OC (0) + +#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS + +#define BOARD_FMU_GPIO_TAB { \ + {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, \ + {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, \ + {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0}, \ + {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0}, \ + {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0}, \ + {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0}, \ + {GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, 0}, \ + {GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, 0}, \ + {GPIO_GPIO8_INPUT, GPIO_GPIO8_OUTPUT, 0}, \ + {GPIO_GPIO9_INPUT, GPIO_GPIO9_OUTPUT, 0}, \ + {GPIO_GPIO10_INPUT, GPIO_GPIO10_OUTPUT, 0}, \ + {GPIO_GPIO11_INPUT, GPIO_GPIO11_OUTPUT, 0}, \ + {GPIO_GPIO12_INPUT, GPIO_GPIO12_OUTPUT, 0}, } + +/* This board provides a DMA pool and APIs */ + +#define BOARD_DMA_ALLOC_POOL_SIZE 5120 + /**************************************************************************************************** * Public Types ****************************************************************************************************/ @@ -313,9 +351,13 @@ __BEGIN_DECLS ****************************************************************************************************/ extern void stm32_spiinitialize(void); +void board_spi_reset(int ms); extern void stm32_usbinitialize(void); + +#define board_peripheral_reset(ms) + /**************************************************************************** * Name: nsh_archinitialize * @@ -335,6 +377,8 @@ extern void stm32_usbinitialize(void); int nsh_archinitialize(void); #endif +#include "../common/board_common.h" + #endif /* __ASSEMBLY__ */ __END_DECLS diff --git a/src/drivers/boards/mindpx-v2/mindpx2_init.c b/src/drivers/boards/mindpx-v2/mindpx2_init.c index 94064c7669..5639a3a0f8 100644 --- a/src/drivers/boards/mindpx-v2/mindpx2_init.c +++ b/src/drivers/boards/mindpx-v2/mindpx2_init.c @@ -45,7 +45,7 @@ * Included Files ****************************************************************************/ -#include +#include #include #include @@ -110,73 +110,9 @@ __END_DECLS /**************************************************************************** * Protected Functions ****************************************************************************/ - -#if defined(CONFIG_FAT_DMAMEMORY) -# if !defined(CONFIG_GRAN) || !defined(CONFIG_FAT_DMAMEMORY) -# error microSD DMA support requires CONFIG_GRAN -# endif - -static GRAN_HANDLE dma_allocator; - -/* - * The DMA heap size constrains the total number of things that can be - * ready to do DMA at a time. - * - * For example, FAT DMA depends on one sector-sized buffer per filesystem plus - * one sector-sized buffer per file. - * - * We use a fundamental alignment / granule size of 64B; this is sufficient - * to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits). - */ -static uint8_t g_dma_heap[8192] __attribute__((aligned(64))); -static perf_counter_t g_dma_perf; - -static void -dma_alloc_init(void) -{ - dma_allocator = gran_initialize(g_dma_heap, - sizeof(g_dma_heap), - 7, /* 128B granule - must be > alignment (XXX bug?) */ - 6); /* 64B alignment */ - - if (dma_allocator == NULL) { - message("DMA alloc FAILED"); - - } else { - g_dma_perf = perf_alloc(PC_COUNT, "dma_alloc"); - } -} - /**************************************************************************** * Public Functions ****************************************************************************/ - -/* - * DMA-aware allocator stubs for the FAT filesystem. - */ - -__EXPORT void *fat_dma_alloc(size_t size); -__EXPORT void fat_dma_free(FAR void *memory, size_t size); - -void * -fat_dma_alloc(size_t size) -{ - perf_count(g_dma_perf); - return gran_alloc(dma_allocator, size); -} - -void -fat_dma_free(FAR void *memory, size_t size) -{ - gran_free(dma_allocator, memory, size); -} - -#else - -# define dma_alloc_init() - -#endif - /************************************************************************************ * Name: stm32_boardinitialize * @@ -230,29 +166,32 @@ __EXPORT int nsh_archinitialize(void) { /* configure ADC pins */ - stm32_configgpio(GPIO_ADC1_IN3); /* BATT_VOLTAGE_SENS */ - stm32_configgpio(GPIO_ADC1_IN2); /* BATT_CURRENT_SENS */ - stm32_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */ - stm32_configgpio(GPIO_ADC1_IN10); /* used by VBUS valid */ - // stm32_configgpio(GPIO_ADC1_IN11); /* unused */ - // stm32_configgpio(GPIO_ADC1_IN12); /* used by MPU6000 CS */ - stm32_configgpio(GPIO_ADC1_IN13); /* FMU_AUX_ADC_1 */ - stm32_configgpio(GPIO_ADC1_IN14); /* FMU_AUX_ADC_2 */ - stm32_configgpio(GPIO_ADC1_IN15); /* PRESSURE_SENS */ + px4_arch_configgpio(GPIO_ADC1_IN3); /* BATT_VOLTAGE_SENS */ + px4_arch_configgpio(GPIO_ADC1_IN2); /* BATT_CURRENT_SENS */ + px4_arch_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */ + px4_arch_configgpio(GPIO_ADC1_IN10); /* used by VBUS valid */ + // px4_arch_configgpio(GPIO_ADC1_IN11); /* unused */ + // px4_arch_configgpio(GPIO_ADC1_IN12); /* used by MPU6000 CS */ + px4_arch_configgpio(GPIO_ADC1_IN13); /* FMU_AUX_ADC_1 */ + px4_arch_configgpio(GPIO_ADC1_IN14); /* FMU_AUX_ADC_2 */ + px4_arch_configgpio(GPIO_ADC1_IN15); /* PRESSURE_SENS */ /* configure power supply control/sense pins */ -// stm32_configgpio(GPIO_VDD_5V_PERIPH_EN); -// stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); -// stm32_configgpio(GPIO_VDD_BRICK_VALID); -// stm32_configgpio(GPIO_VDD_SERVO_VALID); -// stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC); -// stm32_configgpio(GPIO_VDD_5V_PERIPH_OC); +// px4_arch_configgpio(GPIO_VDD_5V_PERIPH_EN); +// px4_arch_configgpio(GPIO_VDD_3V3_SENSORS_EN); +// px4_arch_configgpio(GPIO_VDD_BRICK_VALID); +// px4_arch_configgpio(GPIO_VDD_SERVO_VALID); +// px4_arch_configgpio(GPIO_VDD_5V_HIPOWER_OC); +// px4_arch_configgpio(GPIO_VDD_5V_PERIPH_OC); /* configure the high-resolution time/callout interface */ hrt_init(); /* configure the DMA allocator */ - dma_alloc_init(); + + if (board_dma_alloc_init() < 0) { + message("DMA alloc FAILED"); + } /* configure CPU load estimation */ #ifdef CONFIG_SCHED_INSTRUMENTATION @@ -282,7 +221,7 @@ __EXPORT int nsh_archinitialize(void) /* Configure SPI-based devices */ message("[boot] Initialized SPI port 4 (SENSORS)\n"); - spi4 = up_spiinitialize(4); + spi4 = px4_spibus_initialize(4); if (!spi4) { message("[boot] FAILED to initialize SPI port 4\n"); @@ -303,7 +242,7 @@ __EXPORT int nsh_archinitialize(void) /* Get the SPI port for the FRAM */ message("[boot] Initialized SPI port 1 (RAMTRON FRAM)\n"); - spi1 = up_spiinitialize(1); + spi1 = px4_spibus_initialize(1); if (!spi1) { message("[boot] FAILED to initialize SPI port 1\n"); @@ -324,7 +263,7 @@ __EXPORT int nsh_archinitialize(void) message("[boot] Initialized SPI port 2 (nRF24 and ext)\n"); - spi2 = up_spiinitialize(2); + spi2 = px4_spibus_initialize(2); /* Default SPI2 to 10MHz and de-assert the known chip selects. */ SPI_SETFREQUENCY(spi2, 10000000); @@ -360,12 +299,12 @@ __EXPORT int nsh_archinitialize(void) #endif - stm32_configgpio(GPIO_I2C2_SCL); - stm32_configgpio(GPIO_I2C2_SDA); + px4_arch_configgpio(GPIO_I2C2_SCL); + px4_arch_configgpio(GPIO_I2C2_SDA); message("[boot] Initialized ext I2C Port\n"); - stm32_configgpio(GPIO_I2C1_SCL); - stm32_configgpio(GPIO_I2C1_SDA); + px4_arch_configgpio(GPIO_I2C1_SCL); + px4_arch_configgpio(GPIO_I2C1_SDA); message("[boot] Initialized onboard I2C Port\n"); diff --git a/src/drivers/boards/mindpx-v2/mindpx2_led.c b/src/drivers/boards/mindpx-v2/mindpx2_led.c index 5c3985d7d3..b4656c1a33 100644 --- a/src/drivers/boards/mindpx-v2/mindpx2_led.c +++ b/src/drivers/boards/mindpx-v2/mindpx2_led.c @@ -37,7 +37,7 @@ * PX4FMU LED backend. */ -#include +#include #include @@ -64,14 +64,14 @@ __EXPORT void led_init() { /* Configure LED1 GPIO for output */ - stm32_configgpio(GPIO_LED1); + px4_arch_configgpio(GPIO_LED1); } __EXPORT void led_on(int led) { if (led == 1) { /* Pull down to switch on */ - stm32_gpiowrite(GPIO_LED1, false); + px4_arch_gpiowrite(GPIO_LED1, false); } } @@ -79,18 +79,18 @@ __EXPORT void led_off(int led) { if (led == 1) { /* Pull up to switch off */ - stm32_gpiowrite(GPIO_LED1, true); + px4_arch_gpiowrite(GPIO_LED1, true); } } __EXPORT void led_toggle(int led) { if (led == 1) { - if (stm32_gpioread(GPIO_LED1)) { - stm32_gpiowrite(GPIO_LED1, false); + if (px4_arch_gpioread(GPIO_LED1)) { + px4_arch_gpiowrite(GPIO_LED1, false); } else { - stm32_gpiowrite(GPIO_LED1, true); + px4_arch_gpiowrite(GPIO_LED1, true); } } } diff --git a/src/drivers/boards/mindpx-v2/mindpx_can.c b/src/drivers/boards/mindpx-v2/mindpx_can.c index 58f8023597..84c7a28bf2 100644 --- a/src/drivers/boards/mindpx-v2/mindpx_can.c +++ b/src/drivers/boards/mindpx-v2/mindpx_can.c @@ -41,7 +41,7 @@ * Included Files ************************************************************************************/ -#include +#include #include #include diff --git a/src/drivers/boards/mindpx-v2/mindpx_spi.c b/src/drivers/boards/mindpx-v2/mindpx_spi.c index 012455b604..b25797d095 100644 --- a/src/drivers/boards/mindpx-v2/mindpx_spi.c +++ b/src/drivers/boards/mindpx-v2/mindpx_spi.c @@ -41,7 +41,7 @@ * Included Files ************************************************************************************/ -#include +#include #include #include @@ -54,6 +54,7 @@ #include #include #include "board_config.h" +#include /************************************************************************************ * Public Functions @@ -70,42 +71,42 @@ __EXPORT void weak_function stm32_spiinitialize(void) { #ifdef CONFIG_STM32_SPI4 - stm32_configgpio(GPIO_SPI_CS_GYRO); - stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG); - stm32_configgpio(GPIO_SPI_CS_BARO); -// stm32_configgpio(GPIO_SPI_CS_FRAM); - stm32_configgpio(GPIO_SPI_CS_MPU); + px4_arch_configgpio(GPIO_SPI_CS_GYRO); + px4_arch_configgpio(GPIO_SPI_CS_ACCEL_MAG); + px4_arch_configgpio(GPIO_SPI_CS_BARO); +// px4_arch_configgpio(GPIO_SPI_CS_FRAM); + px4_arch_configgpio(GPIO_SPI_CS_MPU); /* De-activate all peripherals, * required for some peripheral * state machines */ - stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); - stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); - stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); -// stm32_gpiowrite(GPIO_SPI_CS_FRAM,1); - stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1); +// px4_arch_gpiowrite(GPIO_SPI_CS_FRAM,1); + px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1); - stm32_configgpio(GPIO_EXTI_GYRO_DRDY); - stm32_configgpio(GPIO_EXTI_MAG_DRDY); - stm32_configgpio(GPIO_EXTI_ACCEL_DRDY); - stm32_configgpio(GPIO_EXTI_MPU_DRDY); + px4_arch_configgpio(GPIO_EXTI_GYRO_DRDY); + px4_arch_configgpio(GPIO_EXTI_MAG_DRDY); + px4_arch_configgpio(GPIO_EXTI_ACCEL_DRDY); + px4_arch_configgpio(GPIO_EXTI_MPU_DRDY); #endif #ifdef CONFIG_STM32_SPI1 - stm32_configgpio(GPIO_SPI_CS_FRAM); - stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1); + px4_arch_configgpio(GPIO_SPI_CS_FRAM); + px4_arch_gpiowrite(GPIO_SPI_CS_FRAM, 1); #endif #ifdef CONFIG_STM32_SPI2 - stm32_configgpio(GPIO_SPI_CS_EXT0); - stm32_configgpio(GPIO_SPI_CS_EXT1); - stm32_configgpio(GPIO_SPI_CS_EXT2); - stm32_configgpio(GPIO_SPI_CS_EXT3); - stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1); - stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1); - stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1); - stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1); + px4_arch_configgpio(GPIO_SPI_CS_EXT0); + px4_arch_configgpio(GPIO_SPI_CS_EXT1); + px4_arch_configgpio(GPIO_SPI_CS_EXT2); + px4_arch_configgpio(GPIO_SPI_CS_EXT3); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT0, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT1, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT2, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT3, 1); #endif } @@ -116,40 +117,40 @@ __EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, switch (devid) { case PX4_SPIDEV_GYRO: /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); - stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); - stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); - stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, !selected); + px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1); break; case PX4_SPIDEV_ACCEL_MAG: /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); - stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected); - stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); - stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected); + px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1); break; case PX4_SPIDEV_BARO: /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); - stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); - stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected); -// stm32_gpiowrite(GPIO_SPI_CS_FRAM,1); - stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_BARO, !selected); +// px4_arch_gpiowrite(GPIO_SPI_CS_FRAM,1); + px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1); break; // case PX4_SPIDEV_FLASH: -// stm32_gpiowrite(GPIO_SPI_CS_BARO,1); -// stm32_gpiowrite(GPIO_SPI_CS_FRAM,!selected); +// px4_arch_gpiowrite(GPIO_SPI_CS_BARO,1); +// px4_arch_gpiowrite(GPIO_SPI_CS_FRAM,!selected); // break; case PX4_SPIDEV_MPU: /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); - stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); - stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); - stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected); + px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_MPU, !selected); break; default: @@ -167,7 +168,7 @@ __EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, enum spi_dev_e devi __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) { /* there can only be one device on this bus, so always select it */ - stm32_gpiowrite(GPIO_SPI_CS_FRAM, !selected); + px4_arch_gpiowrite(GPIO_SPI_CS_FRAM, !selected); } __EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) @@ -184,34 +185,34 @@ __EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, switch (devid) { case PX4_SPIDEV_EXT0: /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_EXT0, !selected); - stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1); - stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1); - stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT0, !selected); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT1, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT2, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT3, 1); break; case PX4_SPIDEV_EXT1: /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1); - stm32_gpiowrite(GPIO_SPI_CS_EXT1, !selected); - stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1); - stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT0, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT1, !selected); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT2, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT3, 1); break; case PX4_SPIDEV_EXT2: /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1); - stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1); - stm32_gpiowrite(GPIO_SPI_CS_EXT2, !selected); - stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT0, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT1, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT2, !selected); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT3, 1); break; case PX4_SPIDEV_EXT3: /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1); - stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1); - stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1); - stm32_gpiowrite(GPIO_SPI_CS_EXT3, !selected); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT0, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT1, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT2, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT3, !selected); break; default: @@ -224,3 +225,83 @@ __EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devi { return SPI_STATUS_PRESENT; } + +__EXPORT void board_spi_reset(int ms) +{ + /* disable SPI bus */ + px4_arch_configgpio(GPIO_SPI_CS_GYRO_OFF); + px4_arch_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF); + px4_arch_configgpio(GPIO_SPI_CS_BARO_OFF); + // px4_arch_configgpio(GPIO_SPI_CS_FRAM_OFF); + px4_arch_configgpio(GPIO_SPI_CS_MPU_OFF); + + px4_arch_gpiowrite(GPIO_SPI_CS_GYRO_OFF, 0); + px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG_OFF, 0); + px4_arch_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0); + // px4_arch_gpiowrite(GPIO_SPI_CS_FRAM_OFF,0); + px4_arch_gpiowrite(GPIO_SPI_CS_MPU_OFF, 0); + + px4_arch_configgpio(GPIO_SPI4_SCK_OFF); + px4_arch_configgpio(GPIO_SPI4_MISO_OFF); + px4_arch_configgpio(GPIO_SPI4_MOSI_OFF); + + px4_arch_gpiowrite(GPIO_SPI4_SCK_OFF, 0); + px4_arch_gpiowrite(GPIO_SPI4_MISO_OFF, 0); + px4_arch_gpiowrite(GPIO_SPI4_MOSI_OFF, 0); + + px4_arch_configgpio(GPIO_GYRO_DRDY_OFF); + px4_arch_configgpio(GPIO_MAG_DRDY_OFF); + px4_arch_configgpio(GPIO_ACCEL_DRDY_OFF); + px4_arch_configgpio(GPIO_EXTI_MPU_DRDY_OFF); + + px4_arch_gpiowrite(GPIO_GYRO_DRDY_OFF, 0); + px4_arch_gpiowrite(GPIO_MAG_DRDY_OFF, 0); + px4_arch_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0); + px4_arch_gpiowrite(GPIO_EXTI_MPU_DRDY_OFF, 0); + + // /* set the sensor rail off */ + // px4_arch_configgpio(GPIO_VDD_3V3_SENSORS_EN); + // px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0); + // + /* wait for the sensor rail to reach GND */ + usleep(ms * 1000); + warnx("reset done, %d ms", ms); + // + // /* re-enable power */ + // + // /* switch the sensor rail back on */ + // px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1); + // + /* wait a bit before starting SPI, different times didn't influence results */ + usleep(100); + + /* reconfigure the SPI pins */ +#ifdef CONFIG_STM32_SPI4 + px4_arch_configgpio(GPIO_SPI_CS_GYRO); + px4_arch_configgpio(GPIO_SPI_CS_ACCEL_MAG); + px4_arch_configgpio(GPIO_SPI_CS_BARO); + // px4_arch_configgpio(GPIO_SPI_CS_FRAM); + px4_arch_configgpio(GPIO_SPI_CS_MPU); + + /* De-activate all peripherals, + * required for some peripheral + * state machines + */ + px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_FRAM, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1); + + px4_arch_configgpio(GPIO_SPI4_SCK); + px4_arch_configgpio(GPIO_SPI4_MISO); + px4_arch_configgpio(GPIO_SPI4_MOSI); + + // // XXX bring up the EXTI pins again + // px4_arch_configgpio(GPIO_GYRO_DRDY); + // px4_arch_configgpio(GPIO_MAG_DRDY); + // px4_arch_configgpio(GPIO_ACCEL_DRDY); + +#endif + +} diff --git a/src/drivers/boards/mindpx-v2/mindpx_timer_config.c b/src/drivers/boards/mindpx-v2/mindpx_timer_config.c index 9cd2205f1e..20d332c50d 100644 --- a/src/drivers/boards/mindpx-v2/mindpx_timer_config.c +++ b/src/drivers/boards/mindpx-v2/mindpx_timer_config.c @@ -129,3 +129,33 @@ __EXPORT const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { // .default_value = 1000, // } }; + +__EXPORT const struct io_timers_t led_pwm_timers[1] = { + { + .base = STM32_TIM3_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM3EN, + .clock_freq = STM32_APB1_TIM3_CLKIN, + } +}; + +__EXPORT const struct timer_io_channels_t led_pwm_channels[3] = { + { + .gpio_out = LED_TIM3_CH3OUT, + .gpio_in = 0, + .timer_index = 0, + .timer_channel = 3, + }, + { + .gpio_out = LED_TIM3_CH2OUT, + .gpio_in = 0, + .timer_index = 0, + .timer_channel = 2, + }, + { + .gpio_out = LED_TIM3_CH4OUT, + .gpio_in = 0, + .timer_index = 0, + .timer_channel = 4, + } +}; diff --git a/src/drivers/boards/mindpx-v2/mindpx_usb.c b/src/drivers/boards/mindpx-v2/mindpx_usb.c index 1cbc5649c4..70d4371d79 100644 --- a/src/drivers/boards/mindpx-v2/mindpx_usb.c +++ b/src/drivers/boards/mindpx-v2/mindpx_usb.c @@ -41,7 +41,7 @@ * Included Files ************************************************************************************/ -#include +#include #include #include @@ -82,10 +82,10 @@ __EXPORT void stm32_usbinitialize(void) /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ #ifdef CONFIG_STM32_OTGFS - stm32_configgpio(GPIO_OTGFS_VBUS); + px4_arch_configgpio(GPIO_OTGFS_VBUS); /* XXX We only support device mode - stm32_configgpio(GPIO_OTGFS_PWRON); - stm32_configgpio(GPIO_OTGFS_OVER); + px4_arch_configgpio(GPIO_OTGFS_PWRON); + px4_arch_configgpio(GPIO_OTGFS_OVER); */ #endif } diff --git a/src/drivers/boards/px4-stm32f4discovery/CMakeLists.txt b/src/drivers/boards/px4-stm32f4discovery/CMakeLists.txt index e4af694e5d..ee75158403 100644 --- a/src/drivers/boards/px4-stm32f4discovery/CMakeLists.txt +++ b/src/drivers/boards/px4-stm32f4discovery/CMakeLists.txt @@ -33,6 +33,7 @@ px4_add_module( MODULE drivers__boards__px4-stm32f4discovery SRCS + ../common/board_name.c px4discovery_init.c px4discovery_usb.c px4discovery_led.c diff --git a/src/drivers/boards/px4-stm32f4discovery/board_config.h b/src/drivers/boards/px4-stm32f4discovery/board_config.h index d06a3a317b..2b476bf644 100644 --- a/src/drivers/boards/px4-stm32f4discovery/board_config.h +++ b/src/drivers/boards/px4-stm32f4discovery/board_config.h @@ -43,7 +43,7 @@ * Included Files ****************************************************************************************************/ -#include +#include #include #include @@ -87,6 +87,8 @@ __BEGIN_DECLS #define HRT_TIMER 8 /* use timer8 for the HRT */ #define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */ +#define BOARD_NAME "PX4_STM32F4DISCOVERY" + /**************************************************************************************************** * Public Types ****************************************************************************************************/ @@ -132,6 +134,8 @@ extern void stm32_usbinitialize(void); int nsh_archinitialize(void); #endif +#include "../common/board_common.h" + #endif /* __ASSEMBLY__ */ __END_DECLS diff --git a/src/drivers/boards/px4-stm32f4discovery/px4discovery_init.c b/src/drivers/boards/px4-stm32f4discovery/px4discovery_init.c index 1c74be9058..adbfcc55b1 100644 --- a/src/drivers/boards/px4-stm32f4discovery/px4discovery_init.c +++ b/src/drivers/boards/px4-stm32f4discovery/px4discovery_init.c @@ -45,7 +45,7 @@ * Included Files ****************************************************************************/ -#include +#include #include #include diff --git a/src/drivers/boards/px4-stm32f4discovery/px4discovery_led.c b/src/drivers/boards/px4-stm32f4discovery/px4discovery_led.c index 6a6c6eaa0d..58ba3372c9 100644 --- a/src/drivers/boards/px4-stm32f4discovery/px4discovery_led.c +++ b/src/drivers/boards/px4-stm32f4discovery/px4discovery_led.c @@ -37,7 +37,7 @@ * PX4-stm32f4discovery LED backend. */ -#include +#include #include @@ -64,14 +64,14 @@ __EXPORT void led_init() { /* Configure LED1 GPIO for output */ - stm32_configgpio(GPIO_LED1); + px4_arch_configgpio(GPIO_LED1); } __EXPORT void led_on(int led) { if (led == 1) { /* Pull down to switch on */ - stm32_gpiowrite(GPIO_LED1, false); + px4_arch_gpiowrite(GPIO_LED1, false); } } @@ -79,18 +79,18 @@ __EXPORT void led_off(int led) { if (led == 1) { /* Pull up to switch off */ - stm32_gpiowrite(GPIO_LED1, true); + px4_arch_gpiowrite(GPIO_LED1, true); } } __EXPORT void led_toggle(int led) { if (led == 1) { - if (stm32_gpioread(GPIO_LED1)) { - stm32_gpiowrite(GPIO_LED1, false); + if (px4_arch_gpioread(GPIO_LED1)) { + px4_arch_gpiowrite(GPIO_LED1, false); } else { - stm32_gpiowrite(GPIO_LED1, true); + px4_arch_gpiowrite(GPIO_LED1, true); } } } diff --git a/src/drivers/boards/px4-stm32f4discovery/px4discovery_usb.c b/src/drivers/boards/px4-stm32f4discovery/px4discovery_usb.c index 5cd01909b4..379c033456 100644 --- a/src/drivers/boards/px4-stm32f4discovery/px4discovery_usb.c +++ b/src/drivers/boards/px4-stm32f4discovery/px4discovery_usb.c @@ -41,7 +41,7 @@ * Included Files ************************************************************************************/ -#include +#include #include #include @@ -82,10 +82,10 @@ __EXPORT void stm32_usbinitialize(void) /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ #ifdef CONFIG_STM32_OTGFS - stm32_configgpio(GPIO_OTGFS_VBUS); + px4_arch_configgpio(GPIO_OTGFS_VBUS); /* XXX We only support device mode - stm32_configgpio(GPIO_OTGFS_PWRON); - stm32_configgpio(GPIO_OTGFS_OVER); + px4_arch_configgpio(GPIO_OTGFS_PWRON); + px4_arch_configgpio(GPIO_OTGFS_OVER); */ #endif } diff --git a/src/drivers/boards/px4fmu-v1/CMakeLists.txt b/src/drivers/boards/px4fmu-v1/CMakeLists.txt index 3402556f4b..c10b3e2b29 100644 --- a/src/drivers/boards/px4fmu-v1/CMakeLists.txt +++ b/src/drivers/boards/px4fmu-v1/CMakeLists.txt @@ -35,6 +35,7 @@ px4_add_module( COMPILE_FLAGS -Os SRCS + ../common/board_name.c px4fmu_can.c px4fmu_init.c px4fmu_timer_config.c diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h index d1e42a8c6f..5f6edb6d70 100644 --- a/src/drivers/boards/px4fmu-v1/board_config.h +++ b/src/drivers/boards/px4fmu-v1/board_config.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2016 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 @@ -155,7 +155,7 @@ __BEGIN_DECLS #define GPIO_GPIO6_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN13) #define GPIO_GPIO7_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN12) #define GPIO_GPIO_DIR (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) - +#define BOARD_GPIO_SHARED_BUFFERED_BITS 3 /* * Tone alarm output */ @@ -203,6 +203,27 @@ __BEGIN_DECLS #define HRT_PPM_CHANNEL 3 /* use capture/compare channel 3 */ #define GPIO_PPM_IN (GPIO_ALT|GPIO_AF1|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN10) + +#define BOARD_NAME "PX4FMU_V1" + +#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS + +#define BOARD_FMU_GPIO_TAB { \ + {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, \ + {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, \ + {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1}, \ + {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, GPIO_USART2_RTS_1}, \ + {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, GPIO_USART2_TX_1}, \ + {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, GPIO_USART2_RX_1}, \ + {GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2}, \ + {GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2}, } + +/* BOARD_HAS_MULTI_PURPOSE_GPIO defined because the board + * has alternate uses for GPIO as noted in that the third + * column above has entries. + */ +#define BOARD_HAS_MULTI_PURPOSE_GPIO 1 + /**************************************************************************************************** * Public Types ****************************************************************************************************/ @@ -226,9 +247,12 @@ __BEGIN_DECLS ****************************************************************************************************/ extern void stm32_spiinitialize(void); +#define board_spi_reset(ms) extern void stm32_usbinitialize(void); +#define board_peripheral_reset(ms) + /**************************************************************************** * Name: nsh_archinitialize * @@ -248,6 +272,8 @@ extern void stm32_usbinitialize(void); int nsh_archinitialize(void); #endif +#include "../common/board_common.h" + #endif /* __ASSEMBLY__ */ __END_DECLS diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_init.c b/src/drivers/boards/px4fmu-v1/px4fmu_init.c index 4e969692bc..d807f6726f 100644 --- a/src/drivers/boards/px4fmu-v1/px4fmu_init.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_init.c @@ -150,8 +150,8 @@ __EXPORT int nsh_archinitialize(void) int result; /* configure always-on ADC pins */ - stm32_configgpio(GPIO_ADC1_IN10); - stm32_configgpio(GPIO_ADC1_IN11); + px4_arch_configgpio(GPIO_ADC1_IN10); + px4_arch_configgpio(GPIO_ADC1_IN11); /* IN12 and IN13 further below */ /* configure the high-resolution time/callout interface */ @@ -187,7 +187,7 @@ __EXPORT int nsh_archinitialize(void) /* Configure SPI-based devices */ - spi1 = up_spiinitialize(1); + spi1 = px4_spibus_initialize(1); if (!spi1) { message("[boot] FAILED to initialize SPI port 1\r\n"); @@ -210,7 +210,7 @@ __EXPORT int nsh_archinitialize(void) */ #ifdef CONFIG_STM32_SPI2 - spi2 = up_spiinitialize(2); + spi2 = px4_spibus_initialize(2); /* Default SPI2 to 1MHz and de-assert the known chip selects. */ SPI_SETFREQUENCY(spi2, 10000000); SPI_SETBITS(spi2, 8); @@ -223,13 +223,13 @@ __EXPORT int nsh_archinitialize(void) spi2 = NULL; message("[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 + px4_arch_configgpio(GPIO_ADC1_IN12); + px4_arch_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards #endif /* Get the SPI port for the microSD slot */ - spi3 = up_spiinitialize(3); + spi3 = px4_spibus_initialize(3); if (!spi3) { message("[boot] FAILED to initialize SPI port 3\n"); diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_led.c b/src/drivers/boards/px4fmu-v1/px4fmu_led.c index 8a92cc6e3f..5678056d51 100644 --- a/src/drivers/boards/px4fmu-v1/px4fmu_led.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_led.c @@ -64,20 +64,20 @@ __EXPORT void led_init(void) { /* Configure LED1-2 GPIOs for output */ - stm32_configgpio(GPIO_LED1); - stm32_configgpio(GPIO_LED2); + px4_arch_configgpio(GPIO_LED1); + px4_arch_configgpio(GPIO_LED2); } __EXPORT void led_on(int led) { if (led == 0) { /* Pull down to switch on */ - stm32_gpiowrite(GPIO_LED1, false); + px4_arch_gpiowrite(GPIO_LED1, false); } if (led == 1) { /* Pull down to switch on */ - stm32_gpiowrite(GPIO_LED2, false); + px4_arch_gpiowrite(GPIO_LED2, false); } } @@ -85,32 +85,32 @@ __EXPORT void led_off(int led) { if (led == 0) { /* Pull up to switch off */ - stm32_gpiowrite(GPIO_LED1, true); + px4_arch_gpiowrite(GPIO_LED1, true); } if (led == 1) { /* Pull up to switch off */ - stm32_gpiowrite(GPIO_LED2, true); + px4_arch_gpiowrite(GPIO_LED2, true); } } __EXPORT void led_toggle(int led) { if (led == 0) { - if (stm32_gpioread(GPIO_LED1)) { - stm32_gpiowrite(GPIO_LED1, false); + if (px4_arch_gpioread(GPIO_LED1)) { + px4_arch_gpiowrite(GPIO_LED1, false); } else { - stm32_gpiowrite(GPIO_LED1, true); + px4_arch_gpiowrite(GPIO_LED1, true); } } if (led == 1) { - if (stm32_gpioread(GPIO_LED2)) { - stm32_gpiowrite(GPIO_LED2, false); + if (px4_arch_gpioread(GPIO_LED2)) { + px4_arch_gpiowrite(GPIO_LED2, false); } else { - stm32_gpiowrite(GPIO_LED2, true); + px4_arch_gpiowrite(GPIO_LED2, true); } } } diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_spi.c b/src/drivers/boards/px4fmu-v1/px4fmu_spi.c index e877a6dff4..3a0348fd1f 100644 --- a/src/drivers/boards/px4fmu-v1/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_spi.c @@ -69,19 +69,19 @@ __EXPORT void stm32_spiinitialize(void) { - stm32_configgpio(GPIO_SPI_CS_GYRO); - stm32_configgpio(GPIO_SPI_CS_ACCEL); - stm32_configgpio(GPIO_SPI_CS_MPU); - stm32_configgpio(GPIO_SPI_CS_SDCARD); + px4_arch_configgpio(GPIO_SPI_CS_GYRO); + px4_arch_configgpio(GPIO_SPI_CS_ACCEL); + px4_arch_configgpio(GPIO_SPI_CS_MPU); + px4_arch_configgpio(GPIO_SPI_CS_SDCARD); /* De-activate all peripherals, * required for some peripheral * state machines */ - stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); - stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1); - stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); - stm32_gpiowrite(GPIO_SPI_CS_SDCARD, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_SDCARD, 1); } __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) @@ -91,23 +91,23 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, switch (devid) { case PX4_SPIDEV_GYRO: /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); - stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); - stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, !selected); + px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL, 1); break; case PX4_SPIDEV_ACCEL: /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_ACCEL, !selected); - stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); - stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL, !selected); + px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1); break; case PX4_SPIDEV_MPU: /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1); - stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); - stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected); + px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_MPU, !selected); break; default: @@ -143,7 +143,7 @@ __EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devi __EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) { /* there can only be one device on this bus, so always select it */ - stm32_gpiowrite(GPIO_SPI_CS_SDCARD, !selected); + px4_arch_gpiowrite(GPIO_SPI_CS_SDCARD, !selected); } __EXPORT uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_usb.c b/src/drivers/boards/px4fmu-v1/px4fmu_usb.c index 073b3d34f9..3b73d43ed3 100644 --- a/src/drivers/boards/px4fmu-v1/px4fmu_usb.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_usb.c @@ -82,10 +82,10 @@ __EXPORT void stm32_usbinitialize(void) /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ #ifdef CONFIG_STM32_OTGFS - stm32_configgpio(GPIO_OTGFS_VBUS); + px4_arch_configgpio(GPIO_OTGFS_VBUS); /* XXX We only support device mode - stm32_configgpio(GPIO_OTGFS_PWRON); - stm32_configgpio(GPIO_OTGFS_OVER); + px4_arch_configgpio(GPIO_OTGFS_PWRON); + px4_arch_configgpio(GPIO_OTGFS_OVER); */ #endif } diff --git a/src/drivers/boards/px4fmu-v2/CMakeLists.txt b/src/drivers/boards/px4fmu-v2/CMakeLists.txt index 7e20979100..e53cc92ac0 100644 --- a/src/drivers/boards/px4fmu-v2/CMakeLists.txt +++ b/src/drivers/boards/px4fmu-v2/CMakeLists.txt @@ -35,6 +35,8 @@ px4_add_module( COMPILE_FLAGS -Os SRCS + ../common/board_name.c + ../common/board_dma_alloc.c px4fmu_can.c px4fmu2_init.c px4fmu_timer_config.c diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index d0962f13a4..c2c408b7af 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2016 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 @@ -242,6 +242,37 @@ __BEGIN_DECLS #define PWMIN_TIMER_CHANNEL 2 #define GPIO_PWM_IN GPIO_TIM4_CH2IN_2 +#define BOARD_NAME "PX4FMU_V2" + +/* By Providing BOARD_ADC_USB_CONNECTED this board support the ADC + * system_power interface, and therefore provides the true logic + * GPIO BOARD_ADC_xxxx macros. + */ +#define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS)) +#define BOARD_ADC_BRICK_VALID (!px4_arch_gpioread(GPIO_VDD_BRICK_VALID)) +#define BOARD_ADC_SERVO_VALID (!px4_arch_gpioread(GPIO_VDD_SERVO_VALID)) +#define BOARD_ADC_PERIPH_5V_OC (!px4_arch_gpioread(GPIO_VDD_5V_PERIPH_OC)) +#define BOARD_ADC_HIPOWER_5V_OC (!px4_arch_gpioread(GPIO_VDD_5V_HIPOWER_OC)) + +#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS + +#define BOARD_FMU_GPIO_TAB { \ + {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, \ + {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, \ + {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0}, \ + {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0}, \ + {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0}, \ + {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0}, \ + {0, GPIO_VDD_5V_PERIPH_EN, 0}, \ + {0, GPIO_VDD_3V3_SENSORS_EN, 0}, \ + {GPIO_VDD_BRICK_VALID, 0, 0}, \ + {GPIO_VDD_SERVO_VALID, 0, 0}, \ + {GPIO_VDD_5V_HIPOWER_OC, 0, 0}, \ + {GPIO_VDD_5V_PERIPH_OC, 0, 0}, } + +/* This board provides a DMA pool and APIs */ + +#define BOARD_DMA_ALLOC_POOL_SIZE 5120 /**************************************************************************************************** * Public Types ****************************************************************************************************/ @@ -265,9 +296,12 @@ __BEGIN_DECLS ****************************************************************************************************/ extern void stm32_spiinitialize(void); +extern void board_spi_reset(int ms); extern void stm32_usbinitialize(void); +extern void board_peripheral_reset(int ms); + /**************************************************************************** * Name: nsh_archinitialize * @@ -287,6 +321,8 @@ extern void stm32_usbinitialize(void); int nsh_archinitialize(void); #endif +#include "../common/board_common.h" + #endif /* __ASSEMBLY__ */ __END_DECLS diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c index 8055a06e18..494341f46f 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -58,7 +58,6 @@ #include #include #include -#include #include #include "board_config.h" @@ -71,6 +70,7 @@ #include #include +#include /**************************************************************************** * Pre-Processor Definitions @@ -110,73 +110,31 @@ __END_DECLS /**************************************************************************** * Protected Functions ****************************************************************************/ - -#if defined(CONFIG_FAT_DMAMEMORY) -# if !defined(CONFIG_GRAN) || !defined(CONFIG_FAT_DMAMEMORY) -# error microSD DMA support requires CONFIG_GRAN -# endif - -static GRAN_HANDLE dma_allocator; - -/* - * The DMA heap size constrains the total number of things that can be - * ready to do DMA at a time. - * - * For example, FAT DMA depends on one sector-sized buffer per filesystem plus - * one sector-sized buffer per file. - * - * We use a fundamental alignment / granule size of 64B; this is sufficient - * to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits). - */ -static uint8_t g_dma_heap[8192] __attribute__((aligned(64))); -static perf_counter_t g_dma_perf; - -static void -dma_alloc_init(void) -{ - dma_allocator = gran_initialize(g_dma_heap, - sizeof(g_dma_heap), - 7, /* 128B granule - must be > alignment (XXX bug?) */ - 6); /* 64B alignment */ - - if (dma_allocator == NULL) { - message("DMA alloc FAILED"); - - } else { - g_dma_perf = perf_alloc(PC_COUNT, "dma_alloc"); - } -} - /**************************************************************************** * Public Functions ****************************************************************************/ - -/* - * DMA-aware allocator stubs for the FAT filesystem. - */ - -__EXPORT void *fat_dma_alloc(size_t size); -__EXPORT void fat_dma_free(FAR void *memory, size_t size); - -void * -fat_dma_alloc(size_t size) +/************************************************************************************ + * Name: board_peripheral_reset + * + * Description: + * + ************************************************************************************/ +__EXPORT void board_peripheral_reset(int ms) { - perf_count(g_dma_perf); - return gran_alloc(dma_allocator, size); + /* set the peripheral rails off */ + px4_arch_configgpio(GPIO_VDD_5V_PERIPH_EN); + px4_arch_gpiowrite(GPIO_VDD_5V_PERIPH_EN, 1); + + /* wait for the peripheral rail to reach GND */ + usleep(ms * 1000); + warnx("reset done, %d ms", ms); + + /* re-enable power */ + + /* switch the peripheral rail back on */ + px4_arch_gpiowrite(GPIO_VDD_5V_PERIPH_EN, 0); } -void -fat_dma_free(FAR void *memory, size_t size) -{ - gran_free(dma_allocator, memory, size); -} - -#else - -# define dma_alloc_init() - -#endif - /************************************************************************************ * Name: stm32_boardinitialize * @@ -216,37 +174,40 @@ __EXPORT int nsh_archinitialize(void) { /* configure ADC pins */ - stm32_configgpio(GPIO_ADC1_IN2); /* BATT_VOLTAGE_SENS */ - stm32_configgpio(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */ - stm32_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */ - // stm32_configgpio(GPIO_ADC1_IN10); /* used by VBUS valid */ - // stm32_configgpio(GPIO_ADC1_IN11); /* unused */ - // stm32_configgpio(GPIO_ADC1_IN12); /* used by MPU6000 CS */ - stm32_configgpio(GPIO_ADC1_IN13); /* FMU_AUX_ADC_1 */ - stm32_configgpio(GPIO_ADC1_IN14); /* FMU_AUX_ADC_2 */ - stm32_configgpio(GPIO_ADC1_IN15); /* PRESSURE_SENS */ + px4_arch_configgpio(GPIO_ADC1_IN2); /* BATT_VOLTAGE_SENS */ + px4_arch_configgpio(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */ + px4_arch_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */ + // px4_arch_configgpio(GPIO_ADC1_IN10); /* used by VBUS valid */ + // px4_arch_configgpio(GPIO_ADC1_IN11); /* unused */ + // px4_arch_configgpio(GPIO_ADC1_IN12); /* used by MPU6000 CS */ + px4_arch_configgpio(GPIO_ADC1_IN13); /* FMU_AUX_ADC_1 */ + px4_arch_configgpio(GPIO_ADC1_IN14); /* FMU_AUX_ADC_2 */ + px4_arch_configgpio(GPIO_ADC1_IN15); /* PRESSURE_SENS */ /* configure power supply control/sense pins */ - stm32_configgpio(GPIO_VDD_5V_PERIPH_EN); - stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); - stm32_configgpio(GPIO_VDD_BRICK_VALID); - stm32_configgpio(GPIO_VDD_SERVO_VALID); - stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC); - stm32_configgpio(GPIO_VDD_5V_PERIPH_OC); + px4_arch_configgpio(GPIO_VDD_5V_PERIPH_EN); + px4_arch_configgpio(GPIO_VDD_3V3_SENSORS_EN); + px4_arch_configgpio(GPIO_VDD_BRICK_VALID); + px4_arch_configgpio(GPIO_VDD_SERVO_VALID); + px4_arch_configgpio(GPIO_VDD_5V_HIPOWER_OC); + px4_arch_configgpio(GPIO_VDD_5V_PERIPH_OC); /* configure the GPIO pins to outputs and keep them low */ - stm32_configgpio(GPIO_GPIO0_OUTPUT); - stm32_configgpio(GPIO_GPIO1_OUTPUT); - stm32_configgpio(GPIO_GPIO2_OUTPUT); - stm32_configgpio(GPIO_GPIO3_OUTPUT); - stm32_configgpio(GPIO_GPIO4_OUTPUT); - stm32_configgpio(GPIO_GPIO5_OUTPUT); + px4_arch_configgpio(GPIO_GPIO0_OUTPUT); + px4_arch_configgpio(GPIO_GPIO1_OUTPUT); + px4_arch_configgpio(GPIO_GPIO2_OUTPUT); + px4_arch_configgpio(GPIO_GPIO3_OUTPUT); + px4_arch_configgpio(GPIO_GPIO4_OUTPUT); + px4_arch_configgpio(GPIO_GPIO5_OUTPUT); /* configure the high-resolution time/callout interface */ hrt_init(); /* configure the DMA allocator */ - dma_alloc_init(); + + if (board_dma_alloc_init() < 0) { + message("DMA alloc FAILED"); + } /* configure CPU load estimation */ #ifdef CONFIG_SCHED_INSTRUMENTATION @@ -276,7 +237,7 @@ __EXPORT int nsh_archinitialize(void) /* Configure SPI-based devices */ - spi1 = up_spiinitialize(1); + spi1 = px4_spibus_initialize(1); if (!spi1) { message("[boot] FAILED to initialize SPI port 1\n"); @@ -296,7 +257,7 @@ __EXPORT int nsh_archinitialize(void) /* Get the SPI port for the FRAM */ - spi2 = up_spiinitialize(2); + spi2 = px4_spibus_initialize(2); if (!spi2) { message("[boot] FAILED to initialize SPI port 2\n"); @@ -313,7 +274,7 @@ __EXPORT int nsh_archinitialize(void) SPI_SETMODE(spi2, SPIDEV_MODE3); SPI_SELECT(spi2, SPIDEV_FLASH, false); - spi4 = up_spiinitialize(4); + spi4 = px4_spibus_initialize(4); /* Default SPI4 to 1MHz and de-assert the known chip selects. */ SPI_SETFREQUENCY(spi4, 10000000); diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_led.c b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c index 09edb2ba66..6eb8aa58f3 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_led.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c @@ -64,14 +64,14 @@ __EXPORT void led_init() { /* Configure LED1 GPIO for output */ - stm32_configgpio(GPIO_LED1); + px4_arch_configgpio(GPIO_LED1); } __EXPORT void led_on(int led) { if (led == 1) { /* Pull down to switch on */ - stm32_gpiowrite(GPIO_LED1, false); + px4_arch_gpiowrite(GPIO_LED1, false); } } @@ -79,18 +79,18 @@ __EXPORT void led_off(int led) { if (led == 1) { /* Pull up to switch off */ - stm32_gpiowrite(GPIO_LED1, true); + px4_arch_gpiowrite(GPIO_LED1, true); } } __EXPORT void led_toggle(int led) { if (led == 1) { - if (stm32_gpioread(GPIO_LED1)) { - stm32_gpiowrite(GPIO_LED1, false); + if (px4_arch_gpioread(GPIO_LED1)) { + px4_arch_gpiowrite(GPIO_LED1, false); } else { - stm32_gpiowrite(GPIO_LED1, true); + px4_arch_gpiowrite(GPIO_LED1, true); } } } diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c index 26808a6825..6fd5280015 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c @@ -46,7 +46,7 @@ #include #include #include - +#include #include #include @@ -54,6 +54,7 @@ #include #include #include "board_config.h" +#include /************************************************************************************ * Public Functions @@ -70,42 +71,42 @@ __EXPORT void stm32_spiinitialize(void) { #ifdef CONFIG_STM32_SPI1 - stm32_configgpio(GPIO_SPI_CS_GYRO); - stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG); - stm32_configgpio(GPIO_SPI_CS_BARO); - stm32_configgpio(GPIO_SPI_CS_HMC); - stm32_configgpio(GPIO_SPI_CS_MPU); + px4_arch_configgpio(GPIO_SPI_CS_GYRO); + px4_arch_configgpio(GPIO_SPI_CS_ACCEL_MAG); + px4_arch_configgpio(GPIO_SPI_CS_BARO); + px4_arch_configgpio(GPIO_SPI_CS_HMC); + px4_arch_configgpio(GPIO_SPI_CS_MPU); /* De-activate all peripherals, * required for some peripheral * state machines */ - stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); - stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); - stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); - stm32_gpiowrite(GPIO_SPI_CS_HMC, 1); - stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_HMC, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1); - stm32_configgpio(GPIO_EXTI_GYRO_DRDY); - stm32_configgpio(GPIO_EXTI_MAG_DRDY); - stm32_configgpio(GPIO_EXTI_ACCEL_DRDY); - stm32_configgpio(GPIO_EXTI_MPU_DRDY); + px4_arch_configgpio(GPIO_EXTI_GYRO_DRDY); + px4_arch_configgpio(GPIO_EXTI_MAG_DRDY); + px4_arch_configgpio(GPIO_EXTI_ACCEL_DRDY); + px4_arch_configgpio(GPIO_EXTI_MPU_DRDY); #endif #ifdef CONFIG_STM32_SPI2 - stm32_configgpio(GPIO_SPI_CS_FRAM); - stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1); + px4_arch_configgpio(GPIO_SPI_CS_FRAM); + px4_arch_gpiowrite(GPIO_SPI_CS_FRAM, 1); #endif #ifdef CONFIG_STM32_SPI4 - stm32_configgpio(GPIO_SPI_CS_EXT0); - stm32_configgpio(GPIO_SPI_CS_EXT1); - stm32_configgpio(GPIO_SPI_CS_EXT2); - stm32_configgpio(GPIO_SPI_CS_EXT3); - stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1); - stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1); - stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1); - stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1); + px4_arch_configgpio(GPIO_SPI_CS_EXT0); + px4_arch_configgpio(GPIO_SPI_CS_EXT1); + px4_arch_configgpio(GPIO_SPI_CS_EXT2); + px4_arch_configgpio(GPIO_SPI_CS_EXT3); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT0, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT1, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT2, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT3, 1); #endif } @@ -116,56 +117,56 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, switch (devid) { case PX4_SPIDEV_GYRO: /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); - stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); - stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); - stm32_gpiowrite(GPIO_SPI_CS_HMC, 1); - stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, !selected); + px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_HMC, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1); break; case PX4_SPIDEV_ACCEL_MAG: /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); - stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected); - stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); - stm32_gpiowrite(GPIO_SPI_CS_HMC, 1); - stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected); + px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_HMC, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1); break; case PX4_SPIDEV_BARO: /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); - stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); - stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected); - stm32_gpiowrite(GPIO_SPI_CS_HMC, 1); - stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_BARO, !selected); + px4_arch_gpiowrite(GPIO_SPI_CS_HMC, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1); break; case PX4_SPIDEV_HMC: /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); - stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); - stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); - stm32_gpiowrite(GPIO_SPI_CS_HMC, !selected); - stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_HMC, !selected); + px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1); break; case PX4_SPIDEV_LIS: /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); - stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); - stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); - stm32_gpiowrite(GPIO_SPI_CS_LIS, !selected); - stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_LIS, !selected); + px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1); break; case PX4_SPIDEV_MPU: /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); - stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); - stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); - stm32_gpiowrite(GPIO_SPI_CS_HMC, 1); - stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected); + px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_HMC, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_MPU, !selected); break; default: @@ -183,7 +184,7 @@ __EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devi __EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) { /* there can only be one device on this bus, so always select it */ - stm32_gpiowrite(GPIO_SPI_CS_FRAM, !selected); + px4_arch_gpiowrite(GPIO_SPI_CS_FRAM, !selected); } __EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) @@ -200,34 +201,34 @@ __EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, switch (devid) { case PX4_SPIDEV_EXT0: /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_EXT0, !selected); - stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1); - stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1); - stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT0, !selected); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT1, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT2, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT3, 1); break; case PX4_SPIDEV_EXT1: /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1); - stm32_gpiowrite(GPIO_SPI_CS_EXT1, !selected); - stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1); - stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT0, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT1, !selected); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT2, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT3, 1); break; case PX4_SPIDEV_EXT2: /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1); - stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1); - stm32_gpiowrite(GPIO_SPI_CS_EXT2, !selected); - stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT0, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT1, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT2, !selected); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT3, 1); break; case PX4_SPIDEV_EXT3: /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1); - stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1); - stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1); - stm32_gpiowrite(GPIO_SPI_CS_EXT3, !selected); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT0, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT1, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT2, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_EXT3, !selected); break; default: @@ -240,3 +241,79 @@ __EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, enum spi_dev_e devi { return SPI_STATUS_PRESENT; } + +__EXPORT void board_spi_reset(int ms) +{ + /* disable SPI bus */ + px4_arch_configgpio(GPIO_SPI_CS_GYRO_OFF); + px4_arch_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF); + px4_arch_configgpio(GPIO_SPI_CS_BARO_OFF); + px4_arch_configgpio(GPIO_SPI_CS_MPU_OFF); + + px4_arch_gpiowrite(GPIO_SPI_CS_GYRO_OFF, 0); + px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG_OFF, 0); + px4_arch_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0); + px4_arch_gpiowrite(GPIO_SPI_CS_MPU_OFF, 0); + + px4_arch_configgpio(GPIO_SPI1_SCK_OFF); + px4_arch_configgpio(GPIO_SPI1_MISO_OFF); + px4_arch_configgpio(GPIO_SPI1_MOSI_OFF); + + px4_arch_gpiowrite(GPIO_SPI1_SCK_OFF, 0); + px4_arch_gpiowrite(GPIO_SPI1_MISO_OFF, 0); + px4_arch_gpiowrite(GPIO_SPI1_MOSI_OFF, 0); + + px4_arch_configgpio(GPIO_GYRO_DRDY_OFF); + px4_arch_configgpio(GPIO_MAG_DRDY_OFF); + px4_arch_configgpio(GPIO_ACCEL_DRDY_OFF); + px4_arch_configgpio(GPIO_EXTI_MPU_DRDY_OFF); + + px4_arch_gpiowrite(GPIO_GYRO_DRDY_OFF, 0); + px4_arch_gpiowrite(GPIO_MAG_DRDY_OFF, 0); + px4_arch_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0); + px4_arch_gpiowrite(GPIO_EXTI_MPU_DRDY_OFF, 0); + + /* set the sensor rail off */ + px4_arch_configgpio(GPIO_VDD_3V3_SENSORS_EN); + px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0); + + /* wait for the sensor rail to reach GND */ + usleep(ms * 1000); + warnx("reset done, %d ms", ms); + + /* re-enable power */ + + /* switch the sensor rail back on */ + px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1); + + /* wait a bit before starting SPI, different times didn't influence results */ + usleep(100); + + /* reconfigure the SPI pins */ +#ifdef CONFIG_STM32_SPI1 + px4_arch_configgpio(GPIO_SPI_CS_GYRO); + px4_arch_configgpio(GPIO_SPI_CS_ACCEL_MAG); + px4_arch_configgpio(GPIO_SPI_CS_BARO); + px4_arch_configgpio(GPIO_SPI_CS_MPU); + + /* De-activate all peripherals, + * required for some peripheral + * state machines + */ + px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1); + + px4_arch_configgpio(GPIO_SPI1_SCK); + px4_arch_configgpio(GPIO_SPI1_MISO); + px4_arch_configgpio(GPIO_SPI1_MOSI); + + // // XXX bring up the EXTI pins again + // px4_arch_configgpio(GPIO_GYRO_DRDY); + // px4_arch_configgpio(GPIO_MAG_DRDY); + // px4_arch_configgpio(GPIO_ACCEL_DRDY); + // px4_arch_configgpio(GPIO_EXTI_MPU_DRDY); + +#endif +} diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_usb.c b/src/drivers/boards/px4fmu-v2/px4fmu_usb.c index d42c35301f..47359ca688 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu_usb.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu_usb.c @@ -82,10 +82,10 @@ __EXPORT void stm32_usbinitialize(void) /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ #ifdef CONFIG_STM32_OTGFS - stm32_configgpio(GPIO_OTGFS_VBUS); + px4_arch_configgpio(GPIO_OTGFS_VBUS); /* XXX We only support device mode - stm32_configgpio(GPIO_OTGFS_PWRON); - stm32_configgpio(GPIO_OTGFS_OVER); + px4_arch_configgpio(GPIO_OTGFS_PWRON); + px4_arch_configgpio(GPIO_OTGFS_OVER); */ #endif } diff --git a/src/drivers/boards/px4fmu-v4/CMakeLists.txt b/src/drivers/boards/px4fmu-v4/CMakeLists.txt index 00b2b41c41..8370a7c857 100644 --- a/src/drivers/boards/px4fmu-v4/CMakeLists.txt +++ b/src/drivers/boards/px4fmu-v4/CMakeLists.txt @@ -35,6 +35,8 @@ px4_add_module( COMPILE_FLAGS -Os SRCS + ../common/board_name.c + ../common/board_dma_alloc.c px4fmu_can.c px4fmu_init.c px4fmu_timer_config.c diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index c1e214f7d3..97499d0136 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2016 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 @@ -126,11 +126,13 @@ __BEGIN_DECLS /* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */ #define PX4_SPIDEV_GYRO 1 -#define PX4_SPIDEV_ACCEL_MAG 2 +#define PX4_SPIDEV_ACCEL_MAG 2 #define PX4_SPIDEV_MPU 4 #define PX4_SPIDEV_HMC 5 #define PX4_SPIDEV_ICM 6 #define PX4_SPIDEV_LIS 7 +#define PX4_SPIDEV_BMI 8 +#define PX4_SPIDEV_BMA 9 /* onboard MS5611 and FRAM are both on bus SPI2 * spi_dev_e:SPIDEV_FLASH has the value 2 and is used in the NuttX ramtron driver @@ -249,10 +251,10 @@ __BEGIN_DECLS #define GPIO_PERIPH_3V3_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN5) /* for R07, this signal is active low */ //#define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) -//#define INVERT_RC_INPUT(_s) stm32_gpiowrite(GPIO_SBUS_INV, 1-_s); +//#define INVERT_RC_INPUT(_s) px4_arch_gpiowrite(GPIO_SBUS_INV, 1-_s); /* for R12, this signal is active high */ #define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) -#define INVERT_RC_INPUT(_s) stm32_gpiowrite(GPIO_SBUS_INV, _s); +#define INVERT_RC_INPUT(_s) px4_arch_gpiowrite(GPIO_SBUS_INV, _s); #define GPIO_8266_GPIO0 (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN2) #define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) @@ -261,14 +263,43 @@ __BEGIN_DECLS /* Power switch controls ******************************************************/ -#define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (1-_s)) +#define POWER_SPEKTRUM(_s) px4_arch_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (1-_s)) //#define GPIO_USART1_RX_SPEKTRUM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN7) -#define SPEKTRUM_RX_AS_UART() stm32_configgpio(GPIO_USART1_RX) +#define SPEKTRUM_RX_AS_UART() px4_arch_configgpio(GPIO_USART1_RX) // FMUv4 has a separate GPIO for serial RC output -#define GPIO_RC_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN6) -#define SPEKTRUM_RX_AS_GPIO() stm32_configgpio(GPIO_RC_OUT) -#define SPEKTRUM_RX_HIGH(_s) stm32_gpiowrite(GPIO_RC_OUT, (_s)) +#define GPIO_RC_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0) +#define SPEKTRUM_RX_AS_GPIO() px4_arch_configgpio(GPIO_RC_OUT) +#define SPEKTRUM_RX_HIGH(_s) px4_arch_gpiowrite(GPIO_RC_OUT, (_s)) + + +#define BOARD_NAME "PX4FMU_V4" + +/* By Providing BOARD_ADC_USB_CONNECTED this board support the ADC + * system_power interface, and herefore provides the true logic + * GPIO BOARD_ADC_xxxx macros. + */ +#define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS)) +#define BOARD_ADC_BRICK_VALID (px4_arch_gpioread(GPIO_VDD_BRICK_VALID)) +#define BOARD_ADC_SERVO_VALID (1) +#define BOARD_ADC_PERIPH_5V_OC (0) +#define BOARD_ADC_HIPOWER_5V_OC (0) + +#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS + +#define BOARD_FMU_GPIO_TAB { \ + {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, \ + {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, \ + {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0}, \ + {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0}, \ + {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0}, \ + {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0}, \ + {0, GPIO_VDD_3V3_SENSORS_EN, 0}, \ + {GPIO_VDD_BRICK_VALID, 0, 0}, } + +/* This board provides a DMA pool and APIs */ + +#define BOARD_DMA_ALLOC_POOL_SIZE 5120 /**************************************************************************************************** * Public Types @@ -293,9 +324,13 @@ __BEGIN_DECLS ****************************************************************************************************/ extern void stm32_spiinitialize(void); +void board_spi_reset(int ms); extern void stm32_usbinitialize(void); +extern void board_peripheral_reset(int ms); + + /**************************************************************************** * Name: nsh_archinitialize * @@ -315,6 +350,8 @@ extern void stm32_usbinitialize(void); int nsh_archinitialize(void); #endif +#include "../common/board_common.h" + #endif /* __ASSEMBLY__ */ __END_DECLS diff --git a/src/drivers/boards/px4fmu-v4/px4fmu_init.c b/src/drivers/boards/px4fmu-v4/px4fmu_init.c index 3b72690ba8..2e6adaf000 100644 --- a/src/drivers/boards/px4fmu-v4/px4fmu_init.c +++ b/src/drivers/boards/px4fmu-v4/px4fmu_init.c @@ -71,6 +71,7 @@ #include #include +#include /**************************************************************************** * Pre-Processor Definitions @@ -110,73 +111,38 @@ __END_DECLS /**************************************************************************** * Protected Functions ****************************************************************************/ - -#if defined(CONFIG_FAT_DMAMEMORY) -# if !defined(CONFIG_GRAN) || !defined(CONFIG_FAT_DMAMEMORY) -# error microSD DMA support requires CONFIG_GRAN -# endif - -static GRAN_HANDLE dma_allocator; - -/* - * The DMA heap size constrains the total number of things that can be - * ready to do DMA at a time. - * - * For example, FAT DMA depends on one sector-sized buffer per filesystem plus - * one sector-sized buffer per file. - * - * We use a fundamental alignment / granule size of 64B; this is sufficient - * to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits). - */ -static uint8_t g_dma_heap[8192] __attribute__((aligned(64))); -static perf_counter_t g_dma_perf; - -static void -dma_alloc_init(void) -{ - dma_allocator = gran_initialize(g_dma_heap, - sizeof(g_dma_heap), - 7, /* 128B granule - must be > alignment (XXX bug?) */ - 6); /* 64B alignment */ - - if (dma_allocator == NULL) { - message("DMA alloc FAILED"); - - } else { - g_dma_perf = perf_alloc(PC_COUNT, "dma_alloc"); - } -} - /**************************************************************************** * Public Functions ****************************************************************************/ - -/* - * DMA-aware allocator stubs for the FAT filesystem. - */ - -__EXPORT void *fat_dma_alloc(size_t size); -__EXPORT void fat_dma_free(FAR void *memory, size_t size); - -void * -fat_dma_alloc(size_t size) +/************************************************************************************ + * Name: board_peripheral_reset + * + * Description: + * + ************************************************************************************/ +__EXPORT void board_peripheral_reset(int ms) { - perf_count(g_dma_perf); - return gran_alloc(dma_allocator, size); + /* set the peripheral rails off */ + px4_arch_configgpio(GPIO_PERIPH_3V3_EN); + + px4_arch_gpiowrite(GPIO_PERIPH_3V3_EN, 0); + + bool last = px4_arch_gpioread(GPIO_SPEKTRUM_PWR_EN); + /* Keep Spektum on to discharge rail*/ + px4_arch_gpiowrite(GPIO_SPEKTRUM_PWR_EN, 1); + + /* wait for the peripheral rail to reach GND */ + usleep(ms * 1000); + warnx("reset done, %d ms", ms); + + /* re-enable power */ + + /* switch the peripheral rail back on */ + px4_arch_gpiowrite(GPIO_SPEKTRUM_PWR_EN, last); + px4_arch_gpiowrite(GPIO_PERIPH_3V3_EN, 1); + } -void -fat_dma_free(FAR void *memory, size_t size) -{ - gran_free(dma_allocator, memory, size); -} - -#else - -# define dma_alloc_init() - -#endif - /************************************************************************************ * Name: stm32_boardinitialize * @@ -215,40 +181,43 @@ __EXPORT int nsh_archinitialize(void) { /* configure ADC pins */ - stm32_configgpio(GPIO_ADC1_IN2); /* BATT_VOLTAGE_SENS */ - stm32_configgpio(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */ - stm32_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */ - stm32_configgpio(GPIO_ADC1_IN11); /* RSSI analog in */ + px4_arch_configgpio(GPIO_ADC1_IN2); /* BATT_VOLTAGE_SENS */ + px4_arch_configgpio(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */ + px4_arch_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */ + px4_arch_configgpio(GPIO_ADC1_IN11); /* RSSI analog in */ /* configure power supply control/sense pins */ - stm32_configgpio(GPIO_PERIPH_3V3_EN); - stm32_configgpio(GPIO_VDD_BRICK_VALID); + px4_arch_configgpio(GPIO_PERIPH_3V3_EN); + px4_arch_configgpio(GPIO_VDD_BRICK_VALID); - stm32_configgpio(GPIO_SBUS_INV); - stm32_configgpio(GPIO_8266_GPIO0); - stm32_configgpio(GPIO_SPEKTRUM_PWR_EN); - stm32_configgpio(GPIO_8266_PD); - stm32_configgpio(GPIO_8266_RST); - stm32_configgpio(GPIO_BTN_SAFETY); + px4_arch_configgpio(GPIO_SBUS_INV); + px4_arch_configgpio(GPIO_8266_GPIO0); + px4_arch_configgpio(GPIO_SPEKTRUM_PWR_EN); + px4_arch_configgpio(GPIO_8266_PD); + px4_arch_configgpio(GPIO_8266_RST); + px4_arch_configgpio(GPIO_BTN_SAFETY); #ifdef GPIO_RC_OUT - stm32_configgpio(GPIO_RC_OUT); /* Serial RC output pin */ - stm32_gpiowrite(GPIO_RC_OUT, 1); /* set it high to pull RC input up */ + px4_arch_configgpio(GPIO_RC_OUT); /* Serial RC output pin */ + px4_arch_gpiowrite(GPIO_RC_OUT, 1); /* set it high to pull RC input up */ #endif /* configure the GPIO pins to outputs and keep them low */ - stm32_configgpio(GPIO_GPIO0_OUTPUT); - stm32_configgpio(GPIO_GPIO1_OUTPUT); - stm32_configgpio(GPIO_GPIO2_OUTPUT); - stm32_configgpio(GPIO_GPIO3_OUTPUT); - stm32_configgpio(GPIO_GPIO4_OUTPUT); - stm32_configgpio(GPIO_GPIO5_OUTPUT); + px4_arch_configgpio(GPIO_GPIO0_OUTPUT); + px4_arch_configgpio(GPIO_GPIO1_OUTPUT); + px4_arch_configgpio(GPIO_GPIO2_OUTPUT); + px4_arch_configgpio(GPIO_GPIO3_OUTPUT); + px4_arch_configgpio(GPIO_GPIO4_OUTPUT); + px4_arch_configgpio(GPIO_GPIO5_OUTPUT); /* configure the high-resolution time/callout interface */ hrt_init(); /* configure the DMA allocator */ - dma_alloc_init(); + + if (board_dma_alloc_init() < 0) { + message("DMA alloc FAILED"); + } /* configure CPU load estimation */ #ifdef CONFIG_SCHED_INSTRUMENTATION @@ -280,7 +249,7 @@ __EXPORT int nsh_archinitialize(void) /* Configure SPI-based devices */ - spi1 = up_spiinitialize(1); + spi1 = px4_spibus_initialize(1); if (!spi1) { message("[boot] FAILED to initialize SPI port 1\n"); @@ -299,7 +268,7 @@ __EXPORT int nsh_archinitialize(void) /* Get the SPI port for the FRAM */ - spi2 = up_spiinitialize(2); + spi2 = px4_spibus_initialize(2); if (!spi2) { message("[boot] FAILED to initialize SPI port 2\n"); diff --git a/src/drivers/boards/px4fmu-v4/px4fmu_led.c b/src/drivers/boards/px4fmu-v4/px4fmu_led.c index ddb34e8a56..4ea7a6227b 100644 --- a/src/drivers/boards/px4fmu-v4/px4fmu_led.c +++ b/src/drivers/boards/px4fmu-v4/px4fmu_led.c @@ -73,20 +73,20 @@ __EXPORT void led_init(void) { /* Configure LED GPIOs for output */ for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) { - stm32_configgpio(g_ledmap[l]); + px4_arch_configgpio(g_ledmap[l]); } } static void phy_set_led(int led, bool state) { /* Pull Down to switch on */ - stm32_gpiowrite(g_ledmap[led], !state); + px4_arch_gpiowrite(g_ledmap[led], !state); } static bool phy_get_led(int led) { - return !stm32_gpioread(g_ledmap[led]); + return !px4_arch_gpioread(g_ledmap[led]); } __EXPORT void led_on(int led) diff --git a/src/drivers/boards/px4fmu-v4/px4fmu_spi.c b/src/drivers/boards/px4fmu-v4/px4fmu_spi.c index 30560b717f..b1776bd953 100644 --- a/src/drivers/boards/px4fmu-v4/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v4/px4fmu_spi.c @@ -46,6 +46,7 @@ #include #include #include +#include #include #include @@ -54,6 +55,7 @@ #include #include #include "board_config.h" +#include /************************************************************************************ * Public Functions @@ -70,28 +72,28 @@ __EXPORT void stm32_spiinitialize(void) { #ifdef CONFIG_STM32_SPI1 - stm32_configgpio(GPIO_SPI_CS_MPU9250); - stm32_configgpio(GPIO_SPI_CS_HMC5983); - stm32_configgpio(GPIO_SPI_CS_MS5611); - stm32_configgpio(GPIO_SPI_CS_ICM_20608_G); + px4_arch_configgpio(GPIO_SPI_CS_MPU9250); + px4_arch_configgpio(GPIO_SPI_CS_HMC5983); + px4_arch_configgpio(GPIO_SPI_CS_MS5611); + px4_arch_configgpio(GPIO_SPI_CS_ICM_20608_G); /* De-activate all peripherals, * required for some peripheral * state machines */ - stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1); - stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1); - stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1); - stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_MPU9250, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_HMC5983, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_MS5611, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1); - stm32_configgpio(GPIO_DRDY_MPU9250); - stm32_configgpio(GPIO_DRDY_HMC5983); - stm32_configgpio(GPIO_DRDY_ICM_20608_G); + px4_arch_configgpio(GPIO_DRDY_MPU9250); + px4_arch_configgpio(GPIO_DRDY_HMC5983); + px4_arch_configgpio(GPIO_DRDY_ICM_20608_G); #endif #ifdef CONFIG_STM32_SPI2 - stm32_configgpio(GPIO_SPI_CS_FRAM); - stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1); + px4_arch_configgpio(GPIO_SPI_CS_FRAM); + px4_arch_gpiowrite(GPIO_SPI_CS_FRAM, 1); #endif } @@ -103,10 +105,10 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, switch (devid) { case PX4_SPIDEV_ICM: /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1); - stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1); - stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1); - stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, !selected); + px4_arch_gpiowrite(GPIO_SPI_CS_MPU9250, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_HMC5983, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_MS5611, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_ICM_20608_G, !selected); break; case PX4_SPIDEV_ACCEL_MAG: @@ -115,26 +117,26 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, case PX4_SPIDEV_BARO: /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1); - stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1); - stm32_gpiowrite(GPIO_SPI_CS_MS5611, !selected); - stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_MPU9250, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_HMC5983, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_MS5611, !selected); + px4_arch_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1); break; case PX4_SPIDEV_HMC: /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1); - stm32_gpiowrite(GPIO_SPI_CS_HMC5983, !selected); - stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1); - stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_MPU9250, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_HMC5983, !selected); + px4_arch_gpiowrite(GPIO_SPI_CS_MS5611, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1); break; case PX4_SPIDEV_MPU: /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_MPU9250, !selected); - stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1); - stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1); - stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_MPU9250, !selected); + px4_arch_gpiowrite(GPIO_SPI_CS_HMC5983, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_MS5611, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1); break; default: @@ -156,14 +158,14 @@ __EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, switch (devid) { case SPIDEV_FLASH: /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1); - stm32_gpiowrite(GPIO_SPI_CS_FRAM, !selected); + px4_arch_gpiowrite(GPIO_SPI_CS_MS5611, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_FRAM, !selected); break; case PX4_SPIDEV_BARO: /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1); - stm32_gpiowrite(GPIO_SPI_CS_MS5611, !selected); + px4_arch_gpiowrite(GPIO_SPI_CS_FRAM, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_MS5611, !selected); break; default: @@ -177,3 +179,78 @@ __EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devi return SPI_STATUS_PRESENT; } #endif + +__EXPORT void board_spi_reset(int ms) +{ + /* disable SPI bus */ + px4_arch_configgpio(GPIO_SPI_CS_OFF_MPU9250); + px4_arch_configgpio(GPIO_SPI_CS_OFF_HMC5983); + px4_arch_configgpio(GPIO_SPI_CS_OFF_MS5611); + px4_arch_configgpio(GPIO_SPI_CS_OFF_ICM_20608_G); + + px4_arch_gpiowrite(GPIO_SPI_CS_OFF_MPU9250, 0); + px4_arch_gpiowrite(GPIO_SPI_CS_OFF_HMC5983, 0); + px4_arch_gpiowrite(GPIO_SPI_CS_OFF_MS5611, 0); + px4_arch_gpiowrite(GPIO_SPI_CS_OFF_ICM_20608_G, 0); + + px4_arch_configgpio(GPIO_SPI1_SCK_OFF); + px4_arch_configgpio(GPIO_SPI1_MISO_OFF); + px4_arch_configgpio(GPIO_SPI1_MOSI_OFF); + + px4_arch_gpiowrite(GPIO_SPI1_SCK_OFF, 0); + px4_arch_gpiowrite(GPIO_SPI1_MISO_OFF, 0); + px4_arch_gpiowrite(GPIO_SPI1_MOSI_OFF, 0); + + px4_arch_configgpio(GPIO_DRDY_OFF_MPU9250); + px4_arch_configgpio(GPIO_DRDY_OFF_HMC5983); + px4_arch_configgpio(GPIO_DRDY_OFF_ICM_20608_G); + + px4_arch_gpiowrite(GPIO_DRDY_OFF_MPU9250, 0); + px4_arch_gpiowrite(GPIO_DRDY_OFF_HMC5983, 0); + px4_arch_gpiowrite(GPIO_DRDY_OFF_ICM_20608_G, 0); + + /* set the sensor rail off */ + px4_arch_configgpio(GPIO_VDD_3V3_SENSORS_EN); + px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0); + + /* wait for the sensor rail to reach GND */ + usleep(ms * 1000); + warnx("reset done, %d ms", ms); + + /* re-enable power */ + + /* switch the sensor rail back on */ + px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1); + + /* wait a bit before starting SPI, different times didn't influence results */ + usleep(100); + + /* reconfigure the SPI pins */ +#ifdef CONFIG_STM32_SPI1 + px4_arch_configgpio(GPIO_SPI_CS_MPU9250); + px4_arch_configgpio(GPIO_SPI_CS_HMC5983); + px4_arch_configgpio(GPIO_SPI_CS_MS5611); + px4_arch_configgpio(GPIO_SPI_CS_ICM_20608_G); + + /* De-activate all peripherals, + * required for some peripheral + * state machines + */ + px4_arch_gpiowrite(GPIO_SPI_CS_MPU9250, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_HMC5983, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_MS5611, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1); + + px4_arch_configgpio(GPIO_SPI1_SCK); + px4_arch_configgpio(GPIO_SPI1_MISO); + px4_arch_configgpio(GPIO_SPI1_MOSI); + + // // XXX bring up the EXTI pins again + // px4_arch_configgpio(GPIO_GYRO_DRDY); + // px4_arch_configgpio(GPIO_MAG_DRDY); + // px4_arch_configgpio(GPIO_ACCEL_DRDY); + // px4_arch_configgpio(GPIO_EXTI_MPU_DRDY); + +#endif + +} diff --git a/src/drivers/boards/px4fmu-v4/px4fmu_usb.c b/src/drivers/boards/px4fmu-v4/px4fmu_usb.c index d42c35301f..47359ca688 100644 --- a/src/drivers/boards/px4fmu-v4/px4fmu_usb.c +++ b/src/drivers/boards/px4fmu-v4/px4fmu_usb.c @@ -82,10 +82,10 @@ __EXPORT void stm32_usbinitialize(void) /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ #ifdef CONFIG_STM32_OTGFS - stm32_configgpio(GPIO_OTGFS_VBUS); + px4_arch_configgpio(GPIO_OTGFS_VBUS); /* XXX We only support device mode - stm32_configgpio(GPIO_OTGFS_PWRON); - stm32_configgpio(GPIO_OTGFS_OVER); + px4_arch_configgpio(GPIO_OTGFS_PWRON); + px4_arch_configgpio(GPIO_OTGFS_OVER); */ #endif } diff --git a/src/drivers/boards/px4io-v1/board_config.h b/src/drivers/boards/px4io-v1/board_config.h index 6475d717d8..862112ccac 100644 --- a/src/drivers/boards/px4io-v1/board_config.h +++ b/src/drivers/boards/px4io-v1/board_config.h @@ -84,11 +84,11 @@ #define GPIO_RELAY2_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN11) #define GPIO_SPEKTRUM_PWR_EN GPIO_RELAY1_EN -#define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s)) +#define POWER_SPEKTRUM(_s) px4_arch_gpiowrite(GPIO_RELAY1_EN, (_s)) -#define SPEKTRUM_RX_HIGH(_s) stm32_gpiowrite(GPIO_USART1_RX_SPEKTRUM, (_s)) -#define SPEKTRUM_RX_AS_UART() stm32_configgpio(GPIO_USART1_RX) -#define SPEKTRUM_RX_AS_GPIO() stm32_configgpio(GPIO_USART1_RX_SPEKTRUM) +#define SPEKTRUM_RX_HIGH(_s) px4_arch_gpiowrite(GPIO_USART1_RX_SPEKTRUM, (_s)) +#define SPEKTRUM_RX_AS_UART() px4_arch_configgpio(GPIO_USART1_RX) +#define SPEKTRUM_RX_AS_GPIO() px4_arch_configgpio(GPIO_USART1_RX_SPEKTRUM) /* Analog inputs ********************************************************************/ diff --git a/src/drivers/boards/px4io-v1/px4io_init.c b/src/drivers/boards/px4io-v1/px4io_init.c index 019f011fa7..1de9875df5 100644 --- a/src/drivers/boards/px4io-v1/px4io_init.c +++ b/src/drivers/boards/px4io-v1/px4io_init.c @@ -81,26 +81,26 @@ __EXPORT void stm32_boardinitialize(void) { /* configure GPIOs */ - stm32_configgpio(GPIO_ACC1_PWR_EN); - stm32_configgpio(GPIO_ACC2_PWR_EN); - stm32_configgpio(GPIO_SERVO_PWR_EN); - stm32_configgpio(GPIO_RELAY1_EN); - stm32_configgpio(GPIO_RELAY2_EN); + px4_arch_configgpio(GPIO_ACC1_PWR_EN); + px4_arch_configgpio(GPIO_ACC2_PWR_EN); + px4_arch_configgpio(GPIO_SERVO_PWR_EN); + px4_arch_configgpio(GPIO_RELAY1_EN); + px4_arch_configgpio(GPIO_RELAY2_EN); /* turn off - all leds are active low */ - stm32_gpiowrite(GPIO_LED1, true); - stm32_gpiowrite(GPIO_LED2, true); - stm32_gpiowrite(GPIO_LED3, true); + px4_arch_gpiowrite(GPIO_LED1, true); + px4_arch_gpiowrite(GPIO_LED2, true); + px4_arch_gpiowrite(GPIO_LED3, true); /* LED config */ - stm32_configgpio(GPIO_LED1); - stm32_configgpio(GPIO_LED2); - stm32_configgpio(GPIO_LED3); + px4_arch_configgpio(GPIO_LED1); + px4_arch_configgpio(GPIO_LED2); + px4_arch_configgpio(GPIO_LED3); - stm32_configgpio(GPIO_ACC_OC_DETECT); - stm32_configgpio(GPIO_SERVO_OC_DETECT); - stm32_configgpio(GPIO_BTN_SAFETY); + px4_arch_configgpio(GPIO_ACC_OC_DETECT); + px4_arch_configgpio(GPIO_SERVO_OC_DETECT); + px4_arch_configgpio(GPIO_BTN_SAFETY); - stm32_configgpio(GPIO_ADC_VBATT); - stm32_configgpio(GPIO_ADC_IN5); + px4_arch_configgpio(GPIO_ADC_VBATT); + px4_arch_configgpio(GPIO_ADC_IN5); } diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h index c54cf7b6f6..5269c825d2 100644 --- a/src/drivers/boards/px4io-v2/board_config.h +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -88,11 +88,11 @@ /* Power switch controls ******************************************************/ #define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) -#define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s)) +#define POWER_SPEKTRUM(_s) px4_arch_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s)) -#define SPEKTRUM_RX_HIGH(_s) stm32_gpiowrite(GPIO_USART1_RX_SPEKTRUM, (_s)) -#define SPEKTRUM_RX_AS_UART() stm32_configgpio(GPIO_USART1_RX) -#define SPEKTRUM_RX_AS_GPIO() stm32_configgpio(GPIO_USART1_RX_SPEKTRUM) +#define SPEKTRUM_RX_HIGH(_s) px4_arch_gpiowrite(GPIO_USART1_RX_SPEKTRUM, (_s)) +#define SPEKTRUM_RX_AS_UART() px4_arch_configgpio(GPIO_USART1_RX) +#define SPEKTRUM_RX_AS_GPIO() px4_arch_configgpio(GPIO_USART1_RX_SPEKTRUM) #define GPIO_SERVO_FAULT_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN15) diff --git a/src/drivers/boards/px4io-v2/px4io_init.c b/src/drivers/boards/px4io-v2/px4io_init.c index 0856e0b7f1..103105beb4 100644 --- a/src/drivers/boards/px4io-v2/px4io_init.c +++ b/src/drivers/boards/px4io-v2/px4io_init.c @@ -105,55 +105,55 @@ __EXPORT void stm32_boardinitialize(void) /* configure GPIOs */ /* LEDS - default to off */ - stm32_configgpio(GPIO_LED1); - stm32_configgpio(GPIO_LED2); - stm32_configgpio(GPIO_LED3); - stm32_configgpio(GPIO_LED4); + px4_arch_configgpio(GPIO_LED1); + px4_arch_configgpio(GPIO_LED2); + px4_arch_configgpio(GPIO_LED3); + px4_arch_configgpio(GPIO_LED4); - stm32_configgpio(GPIO_BTN_SAFETY); + px4_arch_configgpio(GPIO_BTN_SAFETY); /* spektrum power enable is active high - enable it by default */ - stm32_configgpio(GPIO_SPEKTRUM_PWR_EN); + px4_arch_configgpio(GPIO_SPEKTRUM_PWR_EN); - stm32_configgpio(GPIO_SERVO_FAULT_DETECT); + px4_arch_configgpio(GPIO_SERVO_FAULT_DETECT); /* RSSI inputs */ - stm32_configgpio(GPIO_TIM_RSSI); /* xxx alternate function */ - stm32_configgpio(GPIO_ADC_RSSI); + px4_arch_configgpio(GPIO_TIM_RSSI); /* xxx alternate function */ + px4_arch_configgpio(GPIO_ADC_RSSI); /* servo rail voltage */ - stm32_configgpio(GPIO_ADC_VSERVO); + px4_arch_configgpio(GPIO_ADC_VSERVO); - stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */ - stm32_configgpio(GPIO_SBUS_OUTPUT); + px4_arch_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */ + px4_arch_configgpio(GPIO_SBUS_OUTPUT); /* sbus output enable is active low - disable it by default */ - stm32_gpiowrite(GPIO_SBUS_OENABLE, true); - stm32_configgpio(GPIO_SBUS_OENABLE); + px4_arch_gpiowrite(GPIO_SBUS_OENABLE, true); + px4_arch_configgpio(GPIO_SBUS_OENABLE); - stm32_configgpio(GPIO_PPM); /* xxx alternate function */ + px4_arch_configgpio(GPIO_PPM); /* xxx alternate function */ - stm32_gpiowrite(GPIO_PWM1, true); - stm32_configgpio(GPIO_PWM1); + px4_arch_gpiowrite(GPIO_PWM1, true); + px4_arch_configgpio(GPIO_PWM1); - stm32_gpiowrite(GPIO_PWM2, true); - stm32_configgpio(GPIO_PWM2); + px4_arch_gpiowrite(GPIO_PWM2, true); + px4_arch_configgpio(GPIO_PWM2); - stm32_gpiowrite(GPIO_PWM3, true); - stm32_configgpio(GPIO_PWM3); + px4_arch_gpiowrite(GPIO_PWM3, true); + px4_arch_configgpio(GPIO_PWM3); - stm32_gpiowrite(GPIO_PWM4, true); - stm32_configgpio(GPIO_PWM4); + px4_arch_gpiowrite(GPIO_PWM4, true); + px4_arch_configgpio(GPIO_PWM4); - stm32_gpiowrite(GPIO_PWM5, true); - stm32_configgpio(GPIO_PWM5); + px4_arch_gpiowrite(GPIO_PWM5, true); + px4_arch_configgpio(GPIO_PWM5); - stm32_gpiowrite(GPIO_PWM6, true); - stm32_configgpio(GPIO_PWM6); + px4_arch_gpiowrite(GPIO_PWM6, true); + px4_arch_configgpio(GPIO_PWM6); - stm32_gpiowrite(GPIO_PWM7, true); - stm32_configgpio(GPIO_PWM7); + px4_arch_gpiowrite(GPIO_PWM7, true); + px4_arch_configgpio(GPIO_PWM7); - stm32_gpiowrite(GPIO_PWM8, true); - stm32_configgpio(GPIO_PWM8); + px4_arch_gpiowrite(GPIO_PWM8, true); + px4_arch_configgpio(GPIO_PWM8); } diff --git a/src/drivers/boards/tap-v1/CMakeLists.txt b/src/drivers/boards/tap-v1/CMakeLists.txt new file mode 100644 index 0000000000..b44014b0d7 --- /dev/null +++ b/src/drivers/boards/tap-v1/CMakeLists.txt @@ -0,0 +1,50 @@ +############################################################################ +# +# Copyright (c) 2016 PX4 Development Team. All rights reserved. +# Author: David Sidrane +# +# 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. +# +############################################################################ +px4_add_module( + MODULE drivers__boards__tap-v1 + COMPILE_FLAGS + -Os + SRCS + ../common/board_name.c + tap_init.c + tap_pwr.c + tap_sdio.c + tap_timer_config.c + tap_spi.c + tap_usb.c + tap_led.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/boards/tap-v1/board_config.h b/src/drivers/boards/tap-v1/board_config.h new file mode 100644 index 0000000000..f1f5ac31d8 --- /dev/null +++ b/src/drivers/boards/tap-v1/board_config.h @@ -0,0 +1,345 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. + * Author: David Sidrane + * + * 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 board_config.h + * + * TAP_V1 internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +__BEGIN_DECLS + +/* these headers are not C++ safe */ +#include +#include + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ +/* Configuration ************************************************************************************/ + +#define UDID_START 0x1FFF7A10 + +/* PX4FMU GPIOs ***********************************************************************************/ +/* LEDs + * + * PC4 BLUE_LED D4 Blue LED cathode + * PC5 RED_LED D5 Red LED cathode +*/ + +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN4) +#define GPIO_LED2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN5) +#define GPIO_BLUE_LED GPIO_LED1 +#define GPIO_RED_LED GPIO_LED2 +/* + * SPI + * + * Peripheral Port Signal Name CONN + * SPI2_NSS PB12 SD_SPI2_NSS SD-2 CS + * SPI2_SCK PB13 SD_SPI2_SCK SD-5 CLK + * SPI2_MISO PB14 SD_SPI2_MISO SD-7 D0 + * SPI2_MOSI PB15 SD_SPI2_MOSI SD-3 DI + * + * PC2 SD_SW SD-9 SW + * + */ +#define GPIO_SPI_CS_SDCARD (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN12) +#define GPIO_SPI_SD_SW (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN2) + + +/* + * I2C busses + * + * Peripheral Port Signal Name CONN + * I2C1_SDA PB9 I2C1_SDA J2-4,9,16,21 mpu6050, U4 MS6507 + * I2C1_SDL PB8 I2C1_SCL J2-3,10,15,22 mpu6050, U4 MS6507 + * + * I2C2_SDA PB11 Sonar Echo/I2C_SDA JP2-31,32 + * I2C2_SDL PB10 Sonar Trig/I2C_SCL JP2-29,30 + * + * I2C3_SDA PC9 COMPASS_I2C3_SDA JP1-27,28 + * I2C3_SDL PA8 COMPASS_I2C3_SCL JP1-25,26 + * + */ +#define PX4_I2C_BUS_ONBOARD 1 +#define PX4_I2C_BUS_SONAR 2 +#define PX4_I2C_BUS_EXPANSION 3 + +#define PX4_I2C_OBDEV_HMC5883 0x1e + +/* + * Devices on the onboard bus. + * + * Note that these are unshifted addresses (not includinf R/W). + */ + +/* todo: + * Cannot tell from the schematic if there is one or 2 MPU6050 + * The slave address of the MPU-60X0 is b110100X which is 7 bits long. + * The LSB bit of the 7 bit address is determined by the logic level + * on pin AD0. This allows two MPU-60X0s to be connected to the same I2C bus. + * When used in this configuration, the address of the one of the devices + * should be b1101000 (pin AD0 is logic low) and the address of the other + * should be b1101001 (pin AD0 is logic high). + */ +#define PX4_I2C_ON_BOARD_MPU6050_ADDRS {0x68,0x69} + + +/* + * ADC channels + * + * These are the channel numbers of the ADCs of the microcontroller that can be used by the Px4 Firmware in the adc driver + * PC0 VOLTAGE JP2-13,14 - 1.84 @16.66 1.67 @15.12 Scale 0.1105 + * + */ +#define ADC_CHANNELS (1 << 10) +/* todo:Revisit - cannnot tell from schematic - some could be ADC */ + +// ADC defines to be used in sensors.cpp to read from a particular channel +#define ADC_BATTERY_VOLTAGE_CHANNEL 10 +#define ADC_BATTERY_CURRENT_CHANNEL ((uint8_t)(-1)) + + +/* User GPIOs + * + * TIM3_CH1 PA6 LED_R JP2-23,24 + * TIM3_CH2 PA7 LED_G JP2-25,26 + * TIM3_CH3 PB0 LED_B JP2-27,28 + * TIM3_CH4 PB1 nPWM_1 AUX1(Landing Gear) JP1-21,22 + * + * I2C2_SDA PB11 Sonar Echo/I2C_SDA JP2-31,32 + * I2C2_SDL PB10 Sonar Trig/I2C_SCL JP2-29,30 + * + */ +#define GPIO_GPIO0_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN6) +#define GPIO_GPIO1_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN7) +#define GPIO_GPIO2_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN0) +#define GPIO_GPIO3_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN1) +#define GPIO_GPIO4_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN10) +#define GPIO_GPIO5_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN11) + +#define GPIO_GPIO0_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN6) +#define GPIO_GPIO1_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7) +#define GPIO_GPIO2_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0) +#define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1) +#define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) +#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN11) + +/* + * Tone alarm output + */ +/* todo:Revisit - cannnot tell from schematic - one could be tone alarm*/ +#define TONE_ALARM_TIMER 8 /* timer 8 */ +#define TONE_ALARM_CHANNEL 3 /* channel 3 */ +#define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN8) +#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF2|GPIO_SPEED_2MHz|GPIO_FLOAT|GPIO_PUSHPULL|GPIO_PORTC|GPIO_PIN8) + +/* + * PWM + * + * Four PWM outputs can be configured on pins + * + * + * Peripheral Port Signal Name CONN + * TIM3_CH1 PA6 LED_R JP2-23,24 + * TIM3_CH2 PA7 LED_G JP2-25,26 + * TIM3_CH3 PB0 LED_B JP2-27,28 + * TIM3_CH4 PB1 nPWM_1 AUX1(Landing Gear) JP1-21,22 + * + */ +#define GPIO_TIM3_CH1OUT GPIO_TIM3_CH1OUT_1 +#define GPIO_TIM3_CH2OUT GPIO_TIM3_CH2OUT_1 +#define GPIO_TIM3_CH3OUT GPIO_TIM3_CH3OUT_1 +#define GPIO_TIM3_CH4OUT GPIO_TIM3_CH4OUT_1 +#define DIRECT_PWM_OUTPUT_CHANNELS 4 + +#define BOARD_HAS_LED_PWM +#define LED_TIM3_CH1OUT GPIO_TIM3_CH1OUT +#define LED_TIM3_CH2OUT GPIO_TIM3_CH2OUT +#define LED_TIM3_CH3OUT GPIO_TIM3_CH3OUT + + +/* USB OTG FS + * + * PA9 OTG_FS_VBUS VBUS sensing (also connected to the green LED) + */ +#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9) + +/* High-resolution timer + */ +#define HRT_TIMER 1 /* use timer1 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */ + +#define BOARD_NAME "TAP_V1" + +/* By Providing BOARD_ADC_USB_CONNECTED this board support the ADC + * system_power interface, and herefore provides the true logic + * GPIO BOARD_ADC_xxxx macros. + */ +#define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS)) +#define BOARD_ADC_BRICK_VALID (1) +#define BOARD_ADC_SERVO_VALID (1) +#define BOARD_ADC_PERIPH_5V_OC (0) +#define BOARD_ADC_HIPOWER_5V_OC (0) + +#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS + +#define BOARD_FMU_GPIO_TAB { \ + {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, \ + {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, \ + {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0}, \ + {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0}, \ + {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0}, \ + {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0}, } + + +#define MS_PWR_BUTTON_DOWN 750 +#define KEY_AD_GPIO (GPIO_INPUT|GPIO_PULLDOWN|GPIO_EXTI|GPIO_PORTC|GPIO_PIN1) +#define POWER_ON_GPIO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) +#define POWER_OFF_GPIO (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN4) + +#define GPIO_S0 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN15) +#define GPIO_S1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN14) +#define GPIO_S2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) + +#define GPIO_PCON_RADIO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN3) +#define RF_RADIO_CONTOL(_on_true) px4_arch_gpiowrite(GPIO_PCON_RADIO, !(_on_true)) + +#define GPIO_TEMP_CONT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN4) +#define TEMP_CONTROL(_on_true) px4_arch_gpiowrite(GPIO_TEMP_CONT, (_on_true)) + +#define FLASH_BASED_PARAMS +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +/**************************************************************************************************** + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ****************************************************************************************************/ + +extern void stm32_spiinitialize(void); + +#define board_spi_reset(ms) +#define board_peripheral_reset(ms) + +extern void stm32_usbinitialize(void); + +/************************************************************************************ + * Name: board_sdio_initialize + * + * Description: + * Called to configure SDIO. + * + ************************************************************************************/ + +extern int board_sdio_initialize(void); + +/**************************************************************************** + * Name: nsh_archinitialize + * + * Description: + * Perform architecture specific initialization for NSH. + * + * CONFIG_NSH_ARCHINIT=y : + * Called from the NSH library + * + * CONFIG_BOARD_INITIALIZE=y, CONFIG_NSH_LIBRARY=y, && + * CONFIG_NSH_ARCHINIT=n : + * Called from board_initialize(). + * + ****************************************************************************/ + +#ifdef CONFIG_NSH_LIBRARY +int nsh_archinitialize(void); +#endif + + +/************************************************************************************ + * Name: board_pwr_init() + * + * Description: + * Called to configure power control for the tap-v1 board. + * + * Input Parameters: + * stage- 0 for boot, 1 for board init + * + ************************************************************************************/ + +void board_pwr_init(int stage); + +/**************************************************************************** + * Name: board_pwr_button_down + * + * Description: + * Called to Read the logical state of the power button + ****************************************************************************/ + +bool board_pwr_button_down(void); + +/**************************************************************************** + * Name: board_pwr + * + * Description: + * Called to turn on or off the TAP + * + ****************************************************************************/ + +void board_pwr(bool on_not_off); + +#endif /* __ASSEMBLY__ */ +__END_DECLS diff --git a/src/drivers/boards/tap-v1/tap_init.c b/src/drivers/boards/tap-v1/tap_init.c new file mode 100644 index 0000000000..d221456f00 --- /dev/null +++ b/src/drivers/boards/tap-v1/tap_init.c @@ -0,0 +1,239 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. + * Author: David Sidrane + * + * 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 tap-v1_init.c + * + * tap-v1-specific early startup code. This file implements the + * nsh_archinitialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include +#include + + + +#include "stm32.h" +#include "board_config.h" +#include "stm32_uart.h" + +#include + +#include +#include + +#include + +# if defined(FLASH_BASED_PARAMS) +# include +#endif + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* 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 + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void stm32_boardinitialize(void) +{ + /* Hold power state */ + + board_pwr_init(0); + + /* TEMP ctrl Off (active high, init is clear) */ + + stm32_configgpio(GPIO_TEMP_CONT); + + + /* Select 0 */ + + stm32_configgpio(GPIO_S0); + stm32_configgpio(GPIO_S1); + stm32_configgpio(GPIO_S2); + + /* Radio Off (active low, init is set) */ + + stm32_configgpio(GPIO_PCON_RADIO); + + + /* configure always-on ADC pins */ + + stm32_configgpio(GPIO_ADC1_IN10); + + + /* configure SPI interfaces */ + + stm32_spiinitialize(); + + /* configure LEDs (empty call to NuttX' ledinit) */ + + up_ledinit(); +} + +/**************************************************************************** + * Name: nsh_archinitialize + * + * Description: + * Perform architecture specific initialization + * + ****************************************************************************/ + +__EXPORT int nsh_archinitialize(void) +{ + int result; + + /* configure the high-resolution time/callout interface */ + hrt_init(); + + /* configure CPU load estimation */ +#ifdef CONFIG_SCHED_INSTRUMENTATION + cpuload_initialize_once(); +#endif + + /* set up the serial DMA polling */ + static struct hrt_call serial_dma_call; + struct timespec ts; + + /* + * Poll at 1ms intervals for received bytes that have not triggered + * a DMA event. + */ + ts.tv_sec = 0; + ts.tv_nsec = 1000000; + + hrt_call_every(&serial_dma_call, + ts_to_abstime(&ts), + ts_to_abstime(&ts), + (hrt_callout)stm32_serial_dma_poll, + NULL); + + board_pwr_init(1); + + /* initial LED state */ + drv_led_start(); + led_off(LED_AMBER); + led_off(LED_BLUE); + +#if defined(FLASH_BASED_PARAMS) + static sector_descriptor_t sector_map[] = { + {1, 16 * 1024, 0x08004000}, + {2, 16 * 1024, 0x08008000}, + {0, 0, 0}, + }; + + /* Initalizee the flashfs layer to use heap allocated memory */ + + result = parameter_flashfs_init(sector_map, NULL, 0); + + if (result != OK) { + message("[boot] FAILED to init params in FLASH %d\n", result); + up_ledon(LED_AMBER); + return -ENODEV; + } + +#endif + + /* Init the microSD slot */ + + result = board_sdio_initialize(); + + if (result != OK) { + up_ledon(LED_AMBER); + return -ENODEV; + } + + return OK; +} diff --git a/src/drivers/boards/tap-v1/tap_led.c b/src/drivers/boards/tap-v1/tap_led.c new file mode 100644 index 0000000000..b09d04216c --- /dev/null +++ b/src/drivers/boards/tap-v1/tap_led.c @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. + * Author: David Sidrane + * + * 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 tap-v1_led.c + * + * TAP_V1 LED backend. + */ + +#include + +#include + +#include "stm32.h" +#include "board_config.h" + +#include + +/* + * 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); +extern void led_toggle(int led); +__END_DECLS + +__EXPORT void led_init(void) +{ + /* Configure LED1-2 GPIOs for output */ + + stm32_configgpio(GPIO_BLUE_LED); + stm32_configgpio(GPIO_RED_LED); +} + +__EXPORT void led_on(int led) +{ + if (led == 0) { + /* Pull down to switch on */ + stm32_gpiowrite(GPIO_BLUE_LED, false); + } + + if (led == 1) { + /* Pull down to switch on */ + stm32_gpiowrite(GPIO_RED_LED, false); + } +} + +__EXPORT void led_off(int led) +{ + if (led == 0) { + /* Pull up to switch off */ + stm32_gpiowrite(GPIO_BLUE_LED, true); + } + + if (led == 1) { + /* Pull up to switch off */ + stm32_gpiowrite(GPIO_RED_LED, true); + } +} + +__EXPORT void led_toggle(int led) +{ + if (led == 0) { + stm32_gpiowrite(GPIO_BLUE_LED, !stm32_gpioread(GPIO_BLUE_LED)); + } + + if (led == 1) { + stm32_gpiowrite(GPIO_RED_LED, !stm32_gpioread(GPIO_RED_LED)); + } +} diff --git a/src/systemcmds/tests/test_int.c b/src/drivers/boards/tap-v1/tap_pwr.c similarity index 55% rename from src/systemcmds/tests/test_int.c rename to src/drivers/boards/tap-v1/tap_pwr.c index 01092aa2d4..04b232a223 100644 --- a/src/systemcmds/tests/test_int.c +++ b/src/drivers/boards/tap-v1/tap_pwr.c @@ -1,7 +1,7 @@ /**************************************************************************** - * px4/sensors/test_gpio.c * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * Author: David Sidrane * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -13,7 +13,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. * @@ -32,122 +32,136 @@ * ****************************************************************************/ -/**************************************************************************** - * Included Files - ****************************************************************************/ +/** + * @file tap-v1_pwr.c + * + * Board-specific SPI functions. + */ -#define __STDC_FORMAT_MACROS -#include +/************************************************************************************ + * Included Files + ************************************************************************************/ #include -#include -#include - -#include -#include #include -#include -#include -#include +#include +#include +#include #include +#include -#include "tests.h" +#include "up_arch.h" +#include "board_config.h" +#include -#include -#include +extern void led_on(int led); +extern void led_off(int led); - -/**************************************************************************** - * Pre-processor Definitions - ****************************************************************************/ - -/**************************************************************************** - * Private Types - ****************************************************************************/ - -/**************************************************************************** - * Private Function Prototypes - ****************************************************************************/ - -/**************************************************************************** +/************************************************************************************ * Private Data - ****************************************************************************/ + ************************************************************************************/ -/**************************************************************************** - * Public Data - ****************************************************************************/ - -/**************************************************************************** +/************************************************************************************ * Private Functions - ****************************************************************************/ + ************************************************************************************/ - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/**************************************************************************** - * Name: test_led - ****************************************************************************/ - -typedef union { - int32_t i; - int64_t l; - uint8_t b[8]; -} test_32_64_t; - -int test_int(int argc, char *argv[]) +static int board_button_irq(int irq, FAR void *context) { - int ret = 0; + static struct timespec time_down; - printf("\n--- 64 BIT MATH TESTS ---\n"); + if (board_pwr_button_down()) { - int64_t large = 354156329598; + led_on(BOARD_LED_RED); - int64_t calc = large * 5; - - if (calc == 1770781647990) { - printf("\t success: 354156329598 * 5 == %" PRId64 "\n", calc); + clock_gettime(CLOCK_REALTIME, &time_down); } else { - printf("\t FAIL: 354156329598 * 5 != %" PRId64 "\n", calc); - ret = -1; + + led_off(BOARD_LED_RED); + + struct timespec now; + + clock_gettime(CLOCK_REALTIME, &now); + + uint64_t tdown_ms = time_down.tv_sec * 1000 + time_down.tv_nsec / 1000000; + + uint64_t tnow_ms = now.tv_sec * 1000 + now.tv_nsec / 1000000; + + if (tdown_ms != 0 && (tnow_ms - tdown_ms) >= MS_PWR_BUTTON_DOWN) { + + led_on(BOARD_LED_BLUE); + + up_mdelay(750); + stm32_pwr_enablebkp(); + /* XXX wow, this is evil - write a magic number into backup register zero */ + *(uint32_t *)0x40002850 = 0xdeaddead; + up_mdelay(750); + up_systemreset(); + + while (1); + } } - fflush(stdout); - - - - - - printf("\n--- 32 BIT / 64 BIT MIXED MATH TESTS ---\n"); - - - int32_t small = 50; - int32_t large_int = 2147483647; // MAX INT value - - uint64_t small_times_large = large_int * (uint64_t)small; - - if (small_times_large == 107374182350) { - printf("\t success: 64bit calculation: 50 * 2147483647 (max int val) == %" PRId64 "\n", small_times_large); - - } else { - printf("\t FAIL: 50 * 2147483647 != %" PRId64 ", 64bit cast might fail\n", small_times_large); - ret = -1; - } - - fflush(stdout); - - if (ret == 0) { - printf("\n SUCCESS: All float and double tests passed.\n"); - - } else { - printf("\n FAIL: One or more tests failed.\n"); - } - - printf("\n"); - - return ret; + return OK; +} + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: board_pwr_init() + * + * Description: + * Called to configure power control for the tap-v1 board. + * + * Input Parameters: + * stage- 0 for boot, 1 for board init + * + ************************************************************************************/ + +void board_pwr_init(int stage) +{ + if (stage == 0) { + stm32_configgpio(POWER_ON_GPIO); + stm32_configgpio(KEY_AD_GPIO); + } + + if (stage == 1) { + stm32_gpiosetevent(KEY_AD_GPIO, true, true, true, board_button_irq); + } +} + +/**************************************************************************** + * Name: board_pwr_button_down + * + * Description: + * Called to Read the logical state of the active low power button. + * + ****************************************************************************/ + +bool board_pwr_button_down(void) +{ + return 0 == stm32_gpioread(KEY_AD_GPIO); +} + +/**************************************************************************** + * Name: board_pwr + * + * Description: + * Called to turn on or off the TAP + * + ****************************************************************************/ + +void board_pwr(bool on_not_off) +{ + if (on_not_off) { + stm32_configgpio(POWER_ON_GPIO); + + } else { + + stm32_configgpio(POWER_OFF_GPIO); + } } diff --git a/src/drivers/boards/tap-v1/tap_sdio.c b/src/drivers/boards/tap-v1/tap_sdio.c new file mode 100644 index 0000000000..4367134c63 --- /dev/null +++ b/src/drivers/boards/tap-v1/tap_sdio.c @@ -0,0 +1,122 @@ +/**************************************************************************** + * + * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * Author: David Sidrane + * + * 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 tap-v1_sdio.c + * + * Board-specific SDIOfunctions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "up_arch.h" +#include "chip.h" +#include "stm32.h" +#include "board_config.h" + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* 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 + +/************************************************************************************ + * Private Data + ************************************************************************************/ +static struct spi_dev_s *spi; + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: board_sdio_initialize + * + * Description: + * Called to configure SDIO. + * + ************************************************************************************/ + +__EXPORT int board_sdio_initialize(void) +{ + /* Get the SPI port for the microSD slot */ + + spi = up_spiinitialize(CONFIG_NSH_MMCSDSPIPORTNO); + + if (!spi) { + message("[boot] FAILED to initialize SPI port %d\n", CONFIG_NSH_MMCSDSPIPORTNO); + return -ENODEV; + } + + /* Now bind the SPI interface to the MMCSD driver */ + int result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi); + + if (result != OK) { + message("[boot] FAILED to bind SPI port 2 to the MMCSD driver\n"); + return -ENODEV; + } + + return OK; +} + diff --git a/src/drivers/boards/tap-v1/tap_spi.c b/src/drivers/boards/tap-v1/tap_spi.c new file mode 100644 index 0000000000..b7a6c2c59e --- /dev/null +++ b/src/drivers/boards/tap-v1/tap_spi.c @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. + * Author: David Sidrane + * + * 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 tap-v1_spi.c + * + * Board-specific SPI functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include + +#include +#include + +#include "up_arch.h" +#include "chip.h" +#include "stm32.h" +#include "board_config.h" + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the tap-v1 board. + * + ************************************************************************************/ + +__EXPORT void stm32_spiinitialize(void) +{ + stm32_configgpio(GPIO_SPI_CS_SDCARD); + stm32_configgpio(GPIO_SPI_SD_SW); +} + + +__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* there can only be one device on this bus, so always select it */ + stm32_gpiowrite(GPIO_SPI_CS_SDCARD, !selected); +} + +__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return !stm32_gpioread(GPIO_SPI_SD_SW); +} + diff --git a/src/drivers/boards/tap-v1/tap_timer_config.c b/src/drivers/boards/tap-v1/tap_timer_config.c new file mode 100644 index 0000000000..0d43e7931c --- /dev/null +++ b/src/drivers/boards/tap-v1/tap_timer_config.c @@ -0,0 +1,133 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. + * Author: David Sidrane + * + * 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 tap-v1_timer_config.c + * + * Configuration data for the stm32 pwm_servo, input capture and pwm input driver. + * + * Note that these arrays must always be fully-sized. + */ + +#include + +#include +#include +#include + +#include +#include + +#include "board_config.h" + +__EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = { + { + .base = STM32_TIM3_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM3EN, + .clock_freq = STM32_APB1_TIM3_CLKIN, + .first_channel_index = 0, + .last_channel_index = 3, + .handler = io_timer_handler0, + .vectorno = STM32_IRQ_TIM3, + } +}; + +__EXPORT const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + { + .gpio_out = GPIO_TIM3_CH1OUT, + .gpio_in = 0, + .timer_index = 0, + .timer_channel = 1, + .ccr_offset = STM32_GTIM_CCR1_OFFSET, + .masks = GTIM_SR_CC1IF | GTIM_SR_CC1OF + }, + { + .gpio_out = GPIO_TIM3_CH2OUT, + .gpio_in = 0, + .timer_index = 0, + .timer_channel = 2, + .ccr_offset = STM32_GTIM_CCR2_OFFSET, + .masks = GTIM_SR_CC2IF | GTIM_SR_CC2OF + }, + { + .gpio_out = GPIO_TIM3_CH3OUT, + .gpio_in = 0, + .timer_index = 0, + .timer_channel = 3, + .ccr_offset = STM32_GTIM_CCR3_OFFSET, + .masks = GTIM_SR_CC3IF | GTIM_SR_CC3OF + }, + { + .gpio_out = GPIO_TIM3_CH4OUT, + .gpio_in = 0, + .timer_index = 0, + .timer_channel = 4, + .ccr_offset = STM32_GTIM_CCR4_OFFSET, + .masks = GTIM_SR_CC4IF | GTIM_SR_CC4OF + } +}; + +__EXPORT const struct io_timers_t led_pwm_timers[1] = { + { + .base = STM32_TIM3_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM3EN, + .clock_freq = STM32_APB1_TIM3_CLKIN, + .vectorno = STM32_IRQ_TIM3, + .first_channel_index = 0, + .last_channel_index = 2, + } +}; + +__EXPORT const struct timer_io_channels_t led_pwm_channels[3] = { + { + .gpio_out = LED_TIM3_CH1OUT, + .gpio_in = 0, + .timer_index = 0, + .timer_channel = 1, + }, + { + .gpio_out = LED_TIM3_CH2OUT, + .gpio_in = 0, + .timer_index = 0, + .timer_channel = 2, + }, + { + .gpio_out = LED_TIM3_CH3OUT, + .gpio_in = 0, + .timer_index = 0, + .timer_channel = 3, + } +}; diff --git a/src/drivers/boards/tap-v1/tap_usb.c b/src/drivers/boards/tap-v1/tap_usb.c new file mode 100644 index 0000000000..2afd1e44e4 --- /dev/null +++ b/src/drivers/boards/tap-v1/tap_usb.c @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. + * Author: David Sidrane + * + * 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 tap-v1_usb.c + * + * Board-specific USB functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include "up_arch.h" +#include "stm32.h" +#include "board_config.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_usbinitialize + * + * Description: + * Called to setup USB-related GPIO pins for the tap-v1 board. + * + ************************************************************************************/ + +__EXPORT void stm32_usbinitialize(void) +{ + /* The OTG FS has an internal soft pull-up */ + + /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ + +#ifdef CONFIG_STM32_OTGFS + stm32_configgpio(GPIO_OTGFS_VBUS); +#endif +} + +/************************************************************************************ + * Name: stm32_usbsuspend + * + * Description: + * Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is + * used. This function is called whenever the USB enters or leaves suspend mode. + * This is an opportunity for the board logic to shutdown clocks, power, etc. + * while the USB is suspended. + * + ************************************************************************************/ + +__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + //ulldbg("resume: %d\n", resume); +} + diff --git a/src/drivers/camera_trigger/CMakeLists.txt b/src/drivers/camera_trigger/CMakeLists.txt index 8674c222d7..1a1491dbe3 100644 --- a/src/drivers/camera_trigger/CMakeLists.txt +++ b/src/drivers/camera_trigger/CMakeLists.txt @@ -39,6 +39,9 @@ px4_add_module( SRCS camera_trigger.cpp camera_trigger_params.c + interfaces/src/camera_interface.cpp + interfaces/src/pwm.cpp + interfaces/src/relay.cpp DEPENDS platforms__common ) diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp index 5c144b45c8..37299cb312 100644 --- a/src/drivers/camera_trigger/camera_trigger.cpp +++ b/src/drivers/camera_trigger/camera_trigger.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2015-2016 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,7 +36,11 @@ * * External camera-IMU synchronisation and triggering via FMU auxiliary pins. * + * Support for camera manipulation via PWM signal over servo pins. + * * @author Mohammed Kabir + * @author Kelly Steich + * @author Andreas Bircher */ #include @@ -63,10 +67,19 @@ #include #include +#include "interfaces/src/pwm.h" +#include "interfaces/src/relay.h" + #define TRIGGER_PIN_DEFAULT 1 extern "C" __EXPORT int camera_trigger_main(int argc, char *argv[]); +typedef enum { + CAMERA_INTERFACE_MODE_NONE = 0, + CAMERA_INTERFACE_MODE_RELAY, + CAMERA_INTERFACE_MODE_SEAGULL_MAP2_PWM +} camera_interface_mode_t; + class CameraTrigger { public: @@ -90,6 +103,11 @@ public: */ void shootOnce(); + /** + * Toggle keep camera alive functionality + */ + void keepAlive(bool on); + /** * Start the task. */ @@ -105,24 +123,29 @@ public: */ void info(); - int _pins[6]; + /** + * Trigger one image + */ + void test(); private: struct hrt_call _engagecall; struct hrt_call _disengagecall; + struct hrt_call _keepalivecall_up; + struct hrt_call _keepalivecall_down; + static struct work_s _work; int _gpio_fd; - int _polarity; - int _mode; - float _activation_time; - float _interval; - float _distance; + int _mode; + float _activation_time; + float _interval; + float _distance; uint32_t _trigger_seq; - bool _trigger_enabled; - math::Vector<2> _last_shoot_position; + bool _trigger_enabled; + math::Vector<2> _last_shoot_position; bool _valid_position; int _vcommand_sub; @@ -130,21 +153,15 @@ private: orb_advert_t _trigger_pub; - param_t _p_polarity; - param_t _p_mode; - param_t _p_activation_time; - param_t _p_interval; - param_t _p_distance; - param_t _p_pin; + param_t _p_mode; + param_t _p_activation_time; + param_t _p_interval; + param_t _p_distance; + param_t _p_pin; + param_t _p_interface; - static constexpr uint32_t _gpios[6] = { - GPIO_GPIO0_OUTPUT, - GPIO_GPIO1_OUTPUT, - GPIO_GPIO2_OUTPUT, - GPIO_GPIO3_OUTPUT, - GPIO_GPIO4_OUTPUT, - GPIO_GPIO5_OUTPUT - }; + camera_interface_mode_t _camera_interface_mode; + CameraInterface *_camera_interface; ///< instance of camera interface /** * Vehicle command handler @@ -158,13 +175,18 @@ private: * Resets trigger */ static void disengage(void *arg); - - static void trigger(CameraTrigger *trig, bool trigger); + /** + * Fires trigger + */ + static void keep_alive_up(void *arg); + /** + * Resets trigger + */ + static void keep_alive_down(void *arg); }; struct work_s CameraTrigger::_work; -constexpr uint32_t CameraTrigger::_gpios[6]; namespace camera_trigger { @@ -173,11 +195,9 @@ CameraTrigger *g_camera_trigger; } CameraTrigger::CameraTrigger() : - _pins{}, _engagecall {}, _disengagecall {}, _gpio_fd(-1), - _polarity(0), _mode(0), _activation_time(0.5f /* ms */), _interval(100.0f /* ms */), @@ -188,54 +208,54 @@ CameraTrigger::CameraTrigger() : _valid_position(false), _vcommand_sub(-1), _vlposition_sub(-1), - _trigger_pub(nullptr) + _trigger_pub(nullptr), + _camera_interface_mode(CAMERA_INTERFACE_MODE_RELAY), + _camera_interface(nullptr) { + //Initiate Camera interface basedon camera_interface_mode + if (_camera_interface != nullptr) { + delete(_camera_interface); + /* set to zero to ensure parser is not used while not instantiated */ + _camera_interface = nullptr; + } + memset(&_work, 0, sizeof(_work)); // Parameters - _p_polarity = param_find("TRIG_POLARITY"); _p_interval = param_find("TRIG_INTERVAL"); _p_distance = param_find("TRIG_DISTANCE"); _p_activation_time = param_find("TRIG_ACT_TIME"); _p_mode = param_find("TRIG_MODE"); - _p_pin = param_find("TRIG_PINS"); + _p_interface = param_find("TRIG_INTERFACE"); - param_get(_p_polarity, &_polarity); param_get(_p_activation_time, &_activation_time); param_get(_p_interval, &_interval); param_get(_p_distance, &_distance); param_get(_p_mode, &_mode); - int pin_list; - param_get(_p_pin, &pin_list); + param_get(_p_interface, &_camera_interface_mode); - // Set all pins as invalid - for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) { - _pins[i] = -1; + switch (_camera_interface_mode) { + case CAMERA_INTERFACE_MODE_RELAY: + _camera_interface = new CameraInterfaceRelay; + break; + + case CAMERA_INTERFACE_MODE_SEAGULL_MAP2_PWM: + _camera_interface = new CameraInterfacePWM; + break; + + default: + break; } - // Convert number to individual channels - unsigned i = 0; - int single_pin; + struct camera_trigger_s report = {}; - while ((single_pin = pin_list % 10)) { - - _pins[i] = single_pin - 1; - - if (_pins[i] < 0 || _pins[i] >= static_cast(sizeof(_gpios) / sizeof(_gpios[0]))) { - _pins[i] = -1; - } - - pin_list /= 10; - i++; - } - - struct camera_trigger_s camera_trigger = {}; - - _trigger_pub = orb_advertise(ORB_ID(camera_trigger), &camera_trigger); + _trigger_pub = orb_advertise(ORB_ID(camera_trigger), &report); } CameraTrigger::~CameraTrigger() { + delete(_camera_interface); + camera_trigger::g_camera_trigger = nullptr; } @@ -266,6 +286,26 @@ CameraTrigger::control(bool on) _trigger_enabled = on; } +void +CameraTrigger::keepAlive(bool on) +{ + if (on) { + // schedule keep-alive up and down calls + hrt_call_every(&_keepalivecall_up, 0, (60000 * 1000), + (hrt_callout)&CameraTrigger::keep_alive_up, this); + + // schedule keep-alive up and down calls + hrt_call_every(&_keepalivecall_down, 0 + (30000 * 1000), (60000 * 1000), + (hrt_callout)&CameraTrigger::keep_alive_down, this); + + } else { + // cancel all calls + hrt_cancel(&_keepalivecall_up); + hrt_cancel(&_keepalivecall_down); + } + +} + void CameraTrigger::shootOnce() { @@ -281,17 +321,19 @@ CameraTrigger::shootOnce() void CameraTrigger::start() { - - for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) { - stm32_configgpio(_gpios[_pins[i]]); - stm32_gpiowrite(_gpios[_pins[i]], !_polarity); - } - // enable immediate if configured that way if (_mode == 2) { control(true); } + // Prevent camera from sleeping, if triggering is enabled + if (_mode > 0) { + keepAlive(true); + + } else { + keepAlive(false); + } + // start to monitor at high rate for trigger enable command work_queue(LPWORK, &_work, (worker_t)&CameraTrigger::cycle_trampoline, this, USEC2TICK(1)); @@ -303,12 +345,26 @@ CameraTrigger::stop() work_cancel(LPWORK, &_work); hrt_cancel(&_engagecall); hrt_cancel(&_disengagecall); + hrt_cancel(&_keepalivecall_up); + hrt_cancel(&_keepalivecall_down); if (camera_trigger::g_camera_trigger != nullptr) { delete(camera_trigger::g_camera_trigger); } } +void +CameraTrigger::test() +{ + struct vehicle_command_s cmd = {}; + cmd.command = vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL; + cmd.param5 = 1.0f; + + orb_advert_t pub; + pub = orb_advertise(ORB_ID(vehicle_command), &cmd); + (void)orb_unadvertise(pub); +} + void CameraTrigger::cycle_trampoline(void *arg) { @@ -381,7 +437,14 @@ CameraTrigger::cycle_trampoline(void *arg) if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST) { - // Set trigger to false if the set distance is not positive + // Set trigger to disabled if the set distance is not positive + if (cmd.param1 > 0.0f && !trig->_trigger_enabled) { + trig->_camera_interface->powerOn(); + + } else if (cmd.param1 <= 0.0f && trig->_trigger_enabled) { + trig->_camera_interface->powerOff(); + } + trig->_trigger_enabled = cmd.param1 > 0.0f; trig->_distance = cmd.param1; } @@ -426,7 +489,7 @@ CameraTrigger::engage(void *arg) /* set timestamp the instant before the trigger goes off */ report.timestamp = hrt_absolute_time(); - CameraTrigger::trigger(trig, trig->_polarity); + trig->_camera_interface->trigger(true); report.seq = trig->_trigger_seq++; @@ -436,64 +499,71 @@ CameraTrigger::engage(void *arg) void CameraTrigger::disengage(void *arg) { - CameraTrigger *trig = reinterpret_cast(arg); - CameraTrigger::trigger(trig, !(trig->_polarity)); + trig->_camera_interface->trigger(false); } void -CameraTrigger::trigger(CameraTrigger *trig, bool trigger) +CameraTrigger::keep_alive_up(void *arg) { - for (unsigned i = 0; i < sizeof(trig->_pins) / sizeof(trig->_pins[0]); i++) { - if (trig->_pins[i] >= 0) { - // ACTIVE_LOW == 1 - stm32_gpiowrite(trig->_gpios[trig->_pins[i]], trigger); - } - } + CameraTrigger *trig = reinterpret_cast(arg); + + trig->_camera_interface->keep_alive(true); +} + +void +CameraTrigger::keep_alive_down(void *arg) +{ + CameraTrigger *trig = reinterpret_cast(arg); + + trig->_camera_interface->keep_alive(false); } void CameraTrigger::info() { - warnx("state : %s", _trigger_enabled ? "enabled" : "disabled"); - warnx("pins 1-3 : %d,%d,%d polarity : %s", _pins[0], _pins[1], _pins[2], - _polarity ? "ACTIVE_HIGH" : "ACTIVE_LOW"); - warnx("mode : %i", _mode); - warnx("interval : %.2f", (double)_interval); - warnx("distance : %.2f", (double)_distance); + PX4_INFO("state : %s", _trigger_enabled ? "enabled" : "disabled"); + PX4_INFO("mode : %i", _mode); + PX4_INFO("interval : %.2f [ms]", (double)_interval); + PX4_INFO("distance : %.2f [m]", (double)_distance); + PX4_INFO("activation time : %.2f [ms]", (double)_activation_time); + _camera_interface->info(); } -static void usage() +static int usage() { - errx(1, "usage: camera_trigger {start|stop|info} [-p ]\n"); + PX4_ERR("usage: camera_trigger {start|stop|info|test}\n"); + return 1; } int camera_trigger_main(int argc, char *argv[]) { if (argc < 2) { - usage(); + return usage(); } if (!strcmp(argv[1], "start")) { if (camera_trigger::g_camera_trigger != nullptr) { - errx(0, "already running"); + PX4_WARN("already running"); + return 0; } - camera_trigger::g_camera_trigger = new CameraTrigger; + camera_trigger::g_camera_trigger = new CameraTrigger(); if (camera_trigger::g_camera_trigger == nullptr) { - errx(1, "alloc failed"); + PX4_WARN("alloc failed"); + return 1; } camera_trigger::g_camera_trigger->start(); - return 0; } if (camera_trigger::g_camera_trigger == nullptr) { - errx(1, "not running"); + PX4_WARN("not running"); + return 1; } else if (!strcmp(argv[1], "stop")) { camera_trigger::g_camera_trigger->stop(); @@ -507,8 +577,11 @@ int camera_trigger_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "disable")) { camera_trigger::g_camera_trigger->control(false); + } else if (!strcmp(argv[1], "test")) { + camera_trigger::g_camera_trigger->test(); + } else { - usage(); + return usage(); } return 0; diff --git a/src/drivers/camera_trigger/camera_trigger_params.c b/src/drivers/camera_trigger/camera_trigger_params.c index eea1986c81..09b3cbabc5 100644 --- a/src/drivers/camera_trigger/camera_trigger_params.c +++ b/src/drivers/camera_trigger/camera_trigger_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2015-2016 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,11 +36,26 @@ * Camera trigger parameters * * @author Mohammed Kabir + * @author Andreas Bircher */ #include #include +/** +* Camera trigger Interface +* +* Selects the trigger interface +* +* @value 1 GPIO +* @value 2 Seagull MAP2 (PWM) +* +* @reboot_required true +* +* @group Camera trigger +*/ +PARAM_DEFINE_INT32(TRIG_INTERFACE, 2); + /** * Camera trigger interval * @@ -74,7 +89,7 @@ PARAM_DEFINE_INT32(TRIG_POLARITY, 0); * * @unit ms * @min 0.1 - * @max 3 + * @max 3000 * @decimal 1 * @group Camera trigger */ @@ -83,11 +98,8 @@ PARAM_DEFINE_FLOAT(TRIG_ACT_TIME, 0.5f); /** * Camera trigger mode * - * 0 disables the trigger, 1 sets it to enabled on command, 2 time based and always on, - * 3 distance based and always on, 4 distance based and started / stopped via mission or command. - * * @value 0 Disable - * @value 1 On invididual commands + * @value 1 On individual commands * @value 2 Time based, always on * @value 3 Distance based, always on * @value 4 Distance, mission controlled @@ -101,7 +113,9 @@ PARAM_DEFINE_INT32(TRIG_MODE, 0); /** * Camera trigger pin * - * Selects which pin is used, ranges from 1 to 6 (AUX1-AUX6) + * Selects which pin is used, ranges from 1 to 6 (AUX1-AUX6 on px4fmu-v2 and the rail + * pins on px4fmu-v4). The PWM interface takes two pins per camera, while relay + * triggers on every pin individually. Example: Value 34 would trigger on pins 3 and 4. * * @min 1 * @max 123456 @@ -109,7 +123,7 @@ PARAM_DEFINE_INT32(TRIG_MODE, 0); * @reboot_required true * @group Camera trigger */ -PARAM_DEFINE_INT32(TRIG_PINS, 12); +PARAM_DEFINE_INT32(TRIG_PINS, 6); /** * Camera trigger distance diff --git a/src/drivers/camera_trigger/interfaces/src/camera_interface.cpp b/src/drivers/camera_trigger/interfaces/src/camera_interface.cpp new file mode 100644 index 0000000000..d464296dda --- /dev/null +++ b/src/drivers/camera_trigger/interfaces/src/camera_interface.cpp @@ -0,0 +1,14 @@ +#include "camera_interface.h" + +/** + * @file camera_interface.cpp + * + */ + +CameraInterface::CameraInterface() +{ +} + +CameraInterface::~CameraInterface() +{ +} diff --git a/src/drivers/camera_trigger/interfaces/src/camera_interface.h b/src/drivers/camera_trigger/interfaces/src/camera_interface.h new file mode 100644 index 0000000000..6ce9e5011d --- /dev/null +++ b/src/drivers/camera_trigger/interfaces/src/camera_interface.h @@ -0,0 +1,58 @@ +/** + * @file camera_interface.h + */ + +#pragma once + +class CameraInterface +{ +public: + + /** + * Constructor + */ + CameraInterface(); + + /** + * Destructor. + */ + virtual ~CameraInterface(); + + /** + * trigger the camera + * @param trigger: + */ + virtual void trigger(bool enable) {}; + + /** + * prevent the camera from sleeping + * @param keep alive signal: + */ + virtual void keep_alive(bool signal_on) {}; + + /** + * Display info. + */ + virtual void info() {}; + + /** + * Power on the camera + * @return 0 on success, <0 on error + */ + virtual int powerOn() { return -1; } + + /** + * Power off the camera + * @return 0 on success, <0 on error + */ + virtual int powerOff() { return -1; } + + +protected: + + /** + * setup the interface + */ + virtual void setup() {}; + +}; diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.cpp b/src/drivers/camera_trigger/interfaces/src/pwm.cpp new file mode 100644 index 0000000000..1ce5d91e94 --- /dev/null +++ b/src/drivers/camera_trigger/interfaces/src/pwm.cpp @@ -0,0 +1,171 @@ +#include +#include +#include + +#include "drivers/drv_pwm_output.h" +#include "pwm.h" + +// PWM levels of the interface to seagull MAP converter to +// Multiport (http://www.seagulluav.com/manuals/Seagull_MAP2-Manual.pdf) +#define PWM_CAMERA_DISARMED 900 +#define PWM_CAMERA_ON 1100 +#define PWM_CAMERA_AUTOFOCUS_SHOOT 1300 +#define PWM_CAMERA_NEUTRAL 1500 +#define PWM_CAMERA_INSTANT_SHOOT 1700 +#define PWM_CAMERA_OFF 1900 +#define PWM_2_CAMERA_KEEP_ALIVE 1700 +#define PWM_2_CAMERA_ON_OFF 1900 + +CameraInterfacePWM::CameraInterfacePWM(): + CameraInterface(), + _camera_is_on(false) +{ + _p_pin = param_find("TRIG_PINS"); + int pin_list; + param_get(_p_pin, &pin_list); + + // Set all pins as invalid + for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) { + _pins[i] = -1; + } + + // Convert number to individual channels + unsigned i = 0; + int single_pin; + + while ((single_pin = pin_list % 10)) { + + _pins[i] = single_pin - 1; + + if (_pins[i] < 0) { + _pins[i] = -1; + } + + pin_list /= 10; + i++; + } + + setup(); +} + +CameraInterfacePWM::~CameraInterfacePWM() +{ + // Deinitialise pwm channels + // Can currently not be used, because up_pwm_servo_deinit() will deinitialise all pwm channels + // up_pwm_servo_deinit(); +} + +void CameraInterfacePWM::setup() +{ + for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) { + if (_pins[i] >= 0 && _pins[i + 1] >= 0) { + uint8_t pin_bitmask = (1 << _pins[i + 1]) | (1 << _pins[i]); + up_pwm_servo_init(pin_bitmask); + up_pwm_servo_set(_pins[i + 1], math::constrain(PWM_CAMERA_DISARMED, PWM_CAMERA_DISARMED, 2000)); + up_pwm_servo_set(_pins[i], math::constrain(PWM_CAMERA_DISARMED, PWM_CAMERA_DISARMED, 2000)); + } + } +} + +void CameraInterfacePWM::trigger(bool enable) +{ + // This only starts working upon prearming + + if (!_camera_is_on) { + // (TODO: powerOn does not work yet) + // Turn camera on and give time to start up + // powerOn(); + return; + } + + if (enable) { + // Set all valid pins to shoot level + for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) { + if (_pins[i] >= 0 && _pins[i + 1] >= 0) { + up_pwm_servo_set(_pins[i + 1], math::constrain(PWM_CAMERA_INSTANT_SHOOT, 1000, 2000)); + } + } + + } else { + // Set all valid pins back to neutral level + for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) { + if (_pins[i] >= 0 && _pins[i + 1] >= 0) { + up_pwm_servo_set(_pins[i + 1], math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000)); + } + } + } +} + +void CameraInterfacePWM::keep_alive(bool signal_on) +{ + // This should alternate between signal_on and !signal_on to keep the camera alive + + if (!_camera_is_on) { + // (TODO: powerOn does not work yet) + // Turn camera on and give time to start up + powerOn(); + } + + if (signal_on) { + // Set channel 2 pin to keep_alive signal + for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) { + if (_pins[i] >= 0 && _pins[i + 1] >= 0) { + up_pwm_servo_set(_pins[i], math::constrain(PWM_2_CAMERA_KEEP_ALIVE, 1000, 2000)); + } + } + + } else { + // Set channel 2 pin to neutral signal + for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) { + if (_pins[i] >= 0 && _pins[i + 1] >= 0) { + up_pwm_servo_set(_pins[i], math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000)); + } + } + } +} + +int CameraInterfacePWM::powerOn() +{ + // This only starts working upon prearming + + // Set all valid pins to turn on level + // for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) { + // if (_pins[i] >= 0 && _pins[i + 1] >= 0) { + // up_pwm_servo_set(_pins[i + 1], math::constrain(PWM_CAMERA_ON, 1000, 2000)); + // up_pwm_servo_set(_pins[i], math::constrain(PWM_2_CAMERA_ON_OFF, 1000, 2000)); + // } + // } + + // For now, set channel one on neutral upon startup. + for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) { + if (_pins[i] >= 0 && _pins[i + 1] >= 0) { + up_pwm_servo_set(_pins[i + 1], math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000)); + } + } + + _camera_is_on = true; + + return 0; +} + +int CameraInterfacePWM::powerOff() +{ + // This only starts working upon prearming + + // Set all valid pins to turn off level + for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) { + if (_pins[i] >= 0 && _pins[i + 1] >= 0) { + up_pwm_servo_set(_pins[i + 1], math::constrain(PWM_CAMERA_OFF, 1000, 2000)); + up_pwm_servo_set(_pins[i], math::constrain(PWM_2_CAMERA_ON_OFF, 1000, 2000)); + } + } + + _camera_is_on = false; + + return 0; +} + +void CameraInterfacePWM::info() +{ + warnx("PWM - interface, pin config: %d,%d,%d", _pins[0] + 1, _pins[1] + 1, _pins[2] + 1); +} diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.h b/src/drivers/camera_trigger/interfaces/src/pwm.h new file mode 100644 index 0000000000..33392d6047 --- /dev/null +++ b/src/drivers/camera_trigger/interfaces/src/pwm.h @@ -0,0 +1,35 @@ +/** + * @file pwm.h + * + * Interface with cameras via pwm. + * + */ +#pragma once + +#include + +#include +#include "camera_interface.h" + +class CameraInterfacePWM : public CameraInterface +{ +public: + CameraInterfacePWM(); + virtual ~CameraInterfacePWM(); + + void trigger(bool enable); + void keep_alive(bool signal_on); + + int powerOn(); + int powerOff(); + + void info(); + + int _pins[6]; +private: + void setup(); + + param_t _p_pin; + bool _camera_is_on; + +}; diff --git a/src/drivers/camera_trigger/interfaces/src/relay.cpp b/src/drivers/camera_trigger/interfaces/src/relay.cpp new file mode 100644 index 0000000000..d67903bd29 --- /dev/null +++ b/src/drivers/camera_trigger/interfaces/src/relay.cpp @@ -0,0 +1,77 @@ +#include "relay.h" + +constexpr uint32_t CameraInterfaceRelay::_gpios[6]; + +CameraInterfaceRelay::CameraInterfaceRelay(): + CameraInterface(), + _pins{}, + _polarity(0) +{ + _p_pin = param_find("TRIG_PINS"); + _p_polarity = param_find("TRIG_POLARITY"); + + int pin_list; + param_get(_p_pin, &pin_list); + param_get(_p_polarity, &_polarity); + + // Set all pins as invalid + for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) { + _pins[i] = -1; + } + + // Convert number to individual channels + unsigned i = 0; + int single_pin; + + while ((single_pin = pin_list % 10)) { + + _pins[i] = single_pin - 1; + + if (_pins[i] < 0 || _pins[i] >= static_cast(sizeof(_gpios) / sizeof(_gpios[0]))) { + _pins[i] = -1; + } + + pin_list /= 10; + i++; + } + + setup(); +} + +CameraInterfaceRelay::~CameraInterfaceRelay() +{ +} + +void CameraInterfaceRelay::setup() +{ + for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) { + px4_arch_configgpio(_gpios[_pins[i]]); + px4_arch_gpiowrite(_gpios[_pins[i]], !_polarity); + } +} + +void CameraInterfaceRelay::trigger(bool enable) +{ + if (enable) { + for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) { + if (_pins[i] >= 0) { + // ACTIVE_LOW == 1 + px4_arch_gpiowrite(_gpios[_pins[i]], _polarity); + } + } + + } else { + for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) { + if (_pins[i] >= 0) { + // ACTIVE_LOW == 1 + px4_arch_gpiowrite(_gpios[_pins[i]], !_polarity); + } + } + } +} + +void CameraInterfaceRelay::info() +{ + warnx("Relay - camera triggering, pins 1-3 : %d,%d,%d polarity : %s", _pins[0], _pins[1], _pins[2], + _polarity ? "ACTIVE_HIGH" : "ACTIVE_LOW"); +} diff --git a/src/drivers/camera_trigger/interfaces/src/relay.h b/src/drivers/camera_trigger/interfaces/src/relay.h new file mode 100644 index 0000000000..5a63251f48 --- /dev/null +++ b/src/drivers/camera_trigger/interfaces/src/relay.h @@ -0,0 +1,45 @@ +/** + * @file relay.h + * + * Interface with cameras via FMU auxiliary pins. + * + */ +#pragma once + +#include +#include +#include + +#include "camera_interface.h" + + +class CameraInterfaceRelay : public CameraInterface +{ +public: + CameraInterfaceRelay(); + virtual ~CameraInterfaceRelay(); + + void trigger(bool enable); + + void info(); + + int _pins[6]; + int _polarity; + +private: + + void setup(); + + param_t _p_pin; + param_t _p_polarity; + + static constexpr uint32_t _gpios[6] = { + GPIO_GPIO0_OUTPUT, + GPIO_GPIO1_OUTPUT, + GPIO_GPIO2_OUTPUT, + GPIO_GPIO3_OUTPUT, + GPIO_GPIO4_OUTPUT, + GPIO_GPIO5_OUTPUT + }; + +}; diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp index c43ab1353e..3b9d56e253 100644 --- a/src/drivers/device/cdev.cpp +++ b/src/drivers/device/cdev.cpp @@ -339,14 +339,14 @@ void CDev::poll_notify(pollevent_t events) { /* lock against poll() as well as other wakeups */ - irqstate_t state = irqsave(); + irqstate_t state = px4_enter_critical_section(); for (unsigned i = 0; i < _max_pollwaiters; i++) if (nullptr != _pollset[i]) { poll_notify_one(_pollset[i], events); } - irqrestore(state); + px4_leave_critical_section(state); } void diff --git a/src/drivers/device/i2c_nuttx.cpp b/src/drivers/device/i2c_nuttx.cpp index 5efd2eac23..d7bbee0280 100644 --- a/src/drivers/device/i2c_nuttx.cpp +++ b/src/drivers/device/i2c_nuttx.cpp @@ -75,7 +75,7 @@ I2C::I2C(const char *name, I2C::~I2C() { if (_dev) { - up_i2cuninitialize(_dev); + px4_i2cbus_uninitialize(_dev); _dev = nullptr; } } @@ -105,7 +105,7 @@ I2C::init() unsigned bus_index; // attach to the i2c bus - _dev = up_i2cinitialize(_bus); + _dev = px4_i2cbus_initialize(_bus); if (_dev == nullptr) { DEVICE_DEBUG("failed to init I2C"); @@ -120,7 +120,7 @@ I2C::init() // abort if the max frequency we allow (the frequency we ask) // is smaller than the bus frequency if (_bus_clocks[bus_index] > _frequency) { - (void)up_i2cuninitialize(_dev); + (void)px4_i2cbus_uninitialize(_dev); _dev = nullptr; DEVICE_LOG("FAIL: too slow for bus #%u: %u KHz, device max: %u KHz)", _bus, _bus_clocks[bus_index] / 1000, _frequency / 1000); @@ -168,7 +168,7 @@ I2C::init() out: if ((ret != OK) && (_dev != nullptr)) { - up_i2cuninitialize(_dev); + px4_i2cbus_uninitialize(_dev); _dev = nullptr; } diff --git a/src/drivers/device/integrator.cpp b/src/drivers/device/integrator.cpp index 60f3af5bb4..000b9513cc 100644 --- a/src/drivers/device/integrator.cpp +++ b/src/drivers/device/integrator.cpp @@ -46,9 +46,11 @@ Integrator::Integrator(uint64_t auto_reset_interval, bool coning_compensation) : _auto_reset_interval(auto_reset_interval), _last_integration_time(0), _last_reset_time(0), - _integral(0.0f, 0.0f, 0.0f), + _alpha(0.0f, 0.0f, 0.0f), + _last_alpha(0.0f, 0.0f, 0.0f), + _beta(0.0f, 0.0f, 0.0f), _last_val(0.0f, 0.0f, 0.0f), - _last_delta(0.0f, 0.0f, 0.0f), + _last_delta_alpha(0.0f, 0.0f, 0.0f), _coning_comp_on(coning_compensation) { @@ -80,28 +82,39 @@ Integrator::put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integ dt = (double)(timestamp - _last_integration_time) / 1000000.0; } - math::Vector<3> delta = (val + _last_val) * dt * 0.5f; + // Use trapezoidal integration to calculate the delta integral + math::Vector<3> delta_alpha = (val + _last_val) * dt * 0.5f; + _last_val = val; - // Apply coning compensation if required + // Calculate coning corrections if required if (_coning_comp_on) { // Coning compensation derived by Paul Riseborough and Jonathan Challinger, // following: // Tian et al (2010) Three-loop Integration of GPS and Strapdown INS with Coning and Sculling Compensation - // Available: http://www.sage.unsw.edu.au/snap/publications/tian_etal2010b.pdf - - delta += ((_integral + _last_delta * (1.0f / 6.0f)) % delta) * 0.5f; + // Sourced: http://www.sage.unsw.edu.au/snap/publications/tian_etal2010b.pdf + // Simulated: https://github.com/priseborough/InertialNav/blob/master/models/imu_error_modelling.m + _beta += ((_last_alpha + _last_delta_alpha * (1.0f / 6.0f)) % delta_alpha) * 0.5f; + _last_delta_alpha = delta_alpha; + _last_alpha = _alpha; } - _integral += delta; + // accumulate delta integrals + _alpha += delta_alpha; _last_integration_time = timestamp; - _last_val = val; - _last_delta = delta; // Only do auto reset if auto reset interval is not 0. if (_auto_reset_interval > 0 && (timestamp - _last_reset_time) > _auto_reset_interval) { - integral = _integral; + // apply coning corrections if required + if (_coning_comp_on) { + integral = _alpha + _beta; + + } else { + integral = _alpha; + } + + // reset the integrals and coning corrections _reset(integral_dt); return true; @@ -134,7 +147,7 @@ Integrator::put_with_interval(unsigned interval_us, math::Vector<3> &val, math:: math::Vector<3> Integrator::get(bool reset, uint64_t &integral_dt) { - math::Vector<3> val = _integral; + math::Vector<3> val = _alpha; if (reset) { _reset(integral_dt); @@ -143,12 +156,32 @@ Integrator::get(bool reset, uint64_t &integral_dt) return val; } +math::Vector<3> +Integrator::get_and_filtered(bool reset, uint64_t &integral_dt, math::Vector<3> &filtered_val) +{ + // Do the usual get with reset first but don't return yet. + math::Vector<3> ret_integral = get(reset, integral_dt); + + // Because we need both the integral and the integral_dt. + filtered_val(0) = ret_integral(0) * 1000000 / integral_dt; + filtered_val(1) = ret_integral(1) * 1000000 / integral_dt; + filtered_val(2) = ret_integral(2) * 1000000 / integral_dt; + + return ret_integral; +} + void Integrator::_reset(uint64_t &integral_dt) { - _integral(0) = 0.0f; - _integral(1) = 0.0f; - _integral(2) = 0.0f; + _alpha(0) = 0.0f; + _alpha(1) = 0.0f; + _alpha(2) = 0.0f; + _last_alpha(0) = 0.0f; + _last_alpha(1) = 0.0f; + _last_alpha(2) = 0.0f; + _beta(0) = 0.0f; + _beta(1) = 0.0f; + _beta(2) = 0.0f; integral_dt = (_last_integration_time - _last_reset_time); _last_reset_time = _last_integration_time; diff --git a/src/drivers/device/integrator.h b/src/drivers/device/integrator.h index 11aca58e80..2917fbcab6 100644 --- a/src/drivers/device/integrator.h +++ b/src/drivers/device/integrator.h @@ -86,15 +86,28 @@ public: */ math::Vector<3> get(bool reset, uint64_t &integral_dt); + /** + * Get the current integral and reset the integrator if needed. Additionally give the + * integral over the samples differentiated by the integration time (mean filtered values). + * + * @param reset Reset the integral to zero. + * @param integral_dt Get the dt in us of the current integration (only if reset). + * @param filtered_val The integral differentiated by the integration time. + * @return the integral since the last read-reset + */ + math::Vector<3> get_and_filtered(bool reset, uint64_t &integral_dt, math::Vector<3> &filtered_val); + private: uint64_t _auto_reset_interval; /**< the interval after which the content will be published and the integrator reset, 0 if no auto-reset */ uint64_t _last_integration_time; /**< timestamp of the last integration step */ uint64_t _last_reset_time; /**< last auto-announcement of integral value */ - math::Vector<3> _integral; /**< the integrated value */ - math::Vector<3> _last_val; /**< previously integrated last value */ - math::Vector<3> _last_delta; /**< last local delta */ - bool _coning_comp_on; /**< coning compensation */ + math::Vector<3> _alpha; /**< integrated value before coning corrections are applied */ + math::Vector<3> _last_alpha; /**< previous value of _alpha */ + math::Vector<3> _beta; /**< accumulated coning corrections */ + math::Vector<3> _last_val; /**< previous input */ + math::Vector<3> _last_delta_alpha; /**< integral from previous previous sampling interval */ + bool _coning_comp_on; /**< true to turn on coning corrections */ /* we don't want this class to be copied */ Integrator(const Integrator &); diff --git a/src/drivers/device/spi.cpp b/src/drivers/device/spi.cpp index ce81878102..a25fca10f8 100644 --- a/src/drivers/device/spi.cpp +++ b/src/drivers/device/spi.cpp @@ -95,7 +95,7 @@ SPI::init() /* attach to the spi bus */ if (_dev == nullptr) { - _dev = up_spiinitialize(_bus); + _dev = px4_spibus_initialize(_bus); } if (_dev == nullptr) { @@ -152,9 +152,9 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len) switch (mode) { default: case LOCK_PREEMPTION: { - irqstate_t state = irqsave(); + irqstate_t state = px4_enter_critical_section(); result = _transfer(send, recv, len); - irqrestore(state); + px4_leave_critical_section(state); } break; diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h index 5d4ac5ea31..a985191a4e 100644 --- a/src/drivers/drv_accel.h +++ b/src/drivers/drv_accel.h @@ -103,4 +103,10 @@ struct accel_calibration_s { /** get the result of a sensor self-test */ #define ACCELIOCSELFTEST _ACCELIOC(9) +/** set the hardware low-pass filter cut-off no lower than (arg) Hz */ +#define ACCELIOCSHWLOWPASS _ACCELIOC(10) + +/** get the hardware low-pass filter cut-off in Hz*/ +#define ACCELIOCGHWLOWPASS _ACCELIOC(11) + #endif /* _DRV_ACCEL_H */ diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h index 5d320fe7e6..6b600fbe2d 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -117,6 +117,24 @@ #endif +#ifdef CONFIG_ARCH_BOARD_TAP_V1 +/* + * PX4FMUv3 GPIO numbers. + * + * There are no alternate functions on this board. + */ +# define GPIO_SERVO_1 (1<<0) /**< servo 1 output */ +# define GPIO_SERVO_2 (1<<1) /**< servo 2 output */ +# define GPIO_SERVO_3 (1<<2) /**< servo 3 output */ +# define GPIO_SERVO_4 (1<<3) /**< servo 4 output */ + +/** + * Device paths for things that support the GPIO ioctl protocol. + */ +# define PX4FMU_DEVICE_PATH "/dev/px4fmu" + +#endif + #ifdef CONFIG_ARCH_BOARD_AEROCORE /* * AeroCore GPIO numbers and configuration. @@ -144,8 +162,9 @@ #if !defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2) && \ !defined(CONFIG_ARCH_BOARD_PX4FMU_V1) && !defined(CONFIG_ARCH_BOARD_PX4FMU_V2) && \ !defined(CONFIG_ARCH_BOARD_AEROCORE) && !defined(CONFIG_ARCH_BOARD_PX4_STM32F4DISCOVERY) && \ - !defined(CONFIG_ARCH_BOARD_MINDPX_V2) &&\ - !defined(CONFIG_ARCH_BOARD_PX4FMU_V4) && !defined(CONFIG_ARCH_BOARD_SITL) + !defined(CONFIG_ARCH_BOARD_MINDPX_V2) && \ + !defined(CONFIG_ARCH_BOARD_PX4FMU_V4) && !defined(CONFIG_ARCH_BOARD_SITL) && \ + !defined(CONFIG_ARCH_BOARD_TAP_V1) # error No CONFIG_ARCH_BOARD_xxxx set #endif /* @@ -194,4 +213,10 @@ #define GPIO_PERIPHERAL_RAIL_RESET GPIOC(14) +/** configure the board GPIOs in (arg) as outputs, initially low */ +#define GPIO_SET_OUTPUT_LOW GPIOC(15) + +/** configure the board GPIOs in (arg) as outputs, initially high */ +#define GPIO_SET_OUTPUT_HIGH GPIOC(16) + #endif /* _DRV_GPIO_H */ diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h index 124c6d2b34..431c2fac67 100644 --- a/src/drivers/drv_gyro.h +++ b/src/drivers/drv_gyro.h @@ -100,4 +100,10 @@ struct gyro_calibration_s { /** check the status of the sensor */ #define GYROIOCSELFTEST _GYROIOC(8) +/** set the hardware low-pass filter cut-off no lower than (arg) Hz */ +#define GYROIOCSHWLOWPASS _GYROIOC(9) + +/** get the hardware low-pass filter cut-off in Hz*/ +#define GYROIOCGHWLOWPASS _GYROIOC(10) + #endif /* _DRV_GYRO_H */ diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h index beaf9b1ad9..befbfe799d 100644 --- a/src/drivers/drv_hrt.h +++ b/src/drivers/drv_hrt.h @@ -41,7 +41,6 @@ #include #include -#define __STDC_FORMAT_MACROS #include #include diff --git a/src/drivers/drv_irlock.h b/src/drivers/drv_irlock.h index fa6b3016cd..ea0eac8d75 100644 --- a/src/drivers/drv_irlock.h +++ b/src/drivers/drv_irlock.h @@ -49,12 +49,17 @@ #define IRLOCK_OBJECTS_MAX 5 /** up to 5 objects can be detected/reported **/ +struct irlock_target_s { + uint16_t signature; /** target signature **/ + float pos_x; /** x-axis distance from center of image to center of target in units of tan(theta) **/ + float pos_y; /** y-axis distance from center of image to center of target in units of tan(theta) **/ + float size_x; /** size of target along x-axis in units of tan(theta) **/ + float size_y; /** size of target along y-axis in units of tan(theta) **/ +}; + /** irlock_s structure returned from read calls **/ struct irlock_s { uint64_t timestamp; /** microseconds since system start **/ - uint16_t target_num; /** target number prioritised by size (largest is 0) **/ - float angle_x; /** x-axis angle in radians from center of image to center of target **/ - float angle_y; /** y-axis angle in radians from center of image to center of target **/ - float size_x; /** size in radians of target along x-axis **/ - float size_y; /** size in radians of target along y-axis **/ + uint8_t num_targets; + struct irlock_target_s targets[IRLOCK_OBJECTS_MAX]; }; diff --git a/src/drivers/drv_orb_dev.h b/src/drivers/drv_orb_dev.h index 37e9bdb7cf..2a0d5ca7d9 100644 --- a/src/drivers/drv_orb_dev.h +++ b/src/drivers/drv_orb_dev.h @@ -45,9 +45,6 @@ #include #include -/* XXX for ORB_DECLARE used in many drivers */ -#include "../modules/uORB/uORB.h" - /* * ioctl() definitions */ @@ -90,4 +87,10 @@ /** Get the priority for the topic */ #define ORBIOCGPRIORITY _ORBIOC(14) +/** Set the queue size of the topic */ +#define ORBIOCSETQUEUESIZE _ORBIOC(15) + +/** Get the minimum interval at which the topic can be seen to be updated for this subscription */ +#define ORBIOCGETINTERVAL _ORBIOC(16) + #endif /* _DRV_UORB_H */ diff --git a/src/drivers/drv_oreoled.h b/src/drivers/drv_oreoled.h index 00e3683184..8be6eb8d1a 100644 --- a/src/drivers/drv_oreoled.h +++ b/src/drivers/drv_oreoled.h @@ -61,6 +61,33 @@ /** send bytes */ #define OREOLED_SEND_BYTES _OREOLEDIOC(3) +/** send reset */ +#define OREOLED_SEND_RESET _OREOLEDIOC(4) + +/** boot ping */ +#define OREOLED_BL_PING _OREOLEDIOC(5) + +/** boot version */ +#define OREOLED_BL_VER _OREOLEDIOC(6) + +/** boot write flash */ +#define OREOLED_BL_FLASH _OREOLEDIOC(7) + +/** boot application version */ +#define OREOLED_BL_APP_VER _OREOLEDIOC(8) + +/** boot application crc */ +#define OREOLED_BL_APP_CRC _OREOLEDIOC(9) + +/** boot startup colour */ +#define OREOLED_BL_SET_COLOUR _OREOLEDIOC(10) + +/** boot application */ +#define OREOLED_BL_BOOT_APP _OREOLEDIOC(11) + +/** force an i2c gencall */ +#define OREOLED_FORCE_SYNC _OREOLEDIOC(12) + /* Oreo LED driver supports up to 4 leds */ #define OREOLED_NUM_LEDS 4 @@ -68,7 +95,46 @@ #define OREOLED_ALL_INSTANCES 0xff /* maximum command length that can be sent to LEDs */ -#define OREOLED_CMD_LENGTH_MAX 24 +#define OREOLED_CMD_LENGTH_MAX 70 + +/* maximum command length that can be read from LEDs */ +#define OREOLED_CMD_READ_LENGTH_MAX 10 + +/* maximum number of commands retries */ +#define OEROLED_COMMAND_RETRIES 10 + +/* magic number used to verify the software reset is valid */ +#define OEROLED_RESET_NONCE 0x2A + +/* microseconds to hold-off between write and reads */ +#define OREOLED_WRITE_READ_HOLDOFF_US 500 + +/* microseconds to hold-off between retries */ +#define OREOLED_RETRY_HOLDOFF_US 200 + +#define OEROLED_BOOT_COMMAND_RETRIES 25 +#define OREOLED_BOOT_FLASH_WAITMS 10 + +#define OREOLED_BOOT_SUPPORTED_VER 0x01 + +#define OREOLED_BOOT_CMD_PING 0x40 +#define OREOLED_BOOT_CMD_BL_VER 0x41 +#define OREOLED_BOOT_CMD_APP_VER 0x42 +#define OREOLED_BOOT_CMD_APP_CRC 0x43 +#define OREOLED_BOOT_CMD_SET_COLOUR 0x44 + +#define OREOLED_BOOT_CMD_WRITE_FLASH_A 0x50 +#define OREOLED_BOOT_CMD_WRITE_FLASH_B 0x51 +#define OREOLED_BOOT_CMD_FINALISE_FLASH 0x55 + +#define OREOLED_BOOT_CMD_BOOT_APP 0x60 + +#define OREOLED_BOOT_CMD_PING_NONCE 0x2A +#define OREOLED_BOOT_CMD_BOOT_NONCE 0xA2 + +#define OREOLED_FW_FILE_HEADER_LENGTH 2 +#define OREOLED_FW_FILE_SIZE_LIMIT 6144 +#define OREOLED_FW_FILE "/etc/firmware/oreoled.bin" /* enum passed to OREOLED_SET_MODE ioctl() * defined by hardware */ @@ -97,6 +163,8 @@ enum oreoled_param { OREOLED_PARAM_REPEAT = 7, OREOLED_PARAM_PHASEOFFSET = 8, OREOLED_PARAM_MACRO = 9, + OREOLED_PARAM_RESET = 10, + OREOLED_PARAM_APP_CHECKSUM = 11, OREOLED_PARAM_ENUM_COUNT }; @@ -114,6 +182,8 @@ enum oreoled_macro { OREOLED_PARAM_MACRO_BLUE = 8, OREOLED_PARAM_MACRO_YELLOW = 9, OREOLED_PARAM_MACRO_WHITE = 10, + OREOLED_PARAM_MACRO_AUTOMOBILE = 11, + OREOLED_PARAM_MACRO_AVIATION = 12, OREOLED_PARAM_MACRO_ENUM_COUNT }; diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index a1100287c2..fac3dc0eff 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -72,7 +72,7 @@ #define DRV_RNG_DEVTYPE_MB12XX 0x31 #define DRV_RNG_DEVTYPE_LL40LS 0x32 -#ifdef CONFIG_ARCH_BOARD_MINDPX_V2 +#if defined (CONFIG_ARCH_BOARD_MINDPX_V2) || defined (CONFIG_ARCH_BOARD_TAP_V1) #define DRV_ACC_DEVTYPE_MPU6050 0x14 #define DRV_ACC_DEVTYPE_MPU6500 0x13 diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index a5304603fd..cfff8cfac0 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -204,20 +204,17 @@ void frsky_update_topics() void frsky_send_frame1(int uart) { /* send formatted frame */ - frsky_send_data(uart, FRSKY_ID_ACCEL_X, - roundf(sensor_combined->accelerometer_m_s2[0] * 1000.0f)); - frsky_send_data(uart, FRSKY_ID_ACCEL_Y, - roundf(sensor_combined->accelerometer_m_s2[1] * 1000.0f)); - frsky_send_data(uart, FRSKY_ID_ACCEL_Z, - roundf(sensor_combined->accelerometer_m_s2[2] * 1000.0f)); + frsky_send_data(uart, FRSKY_ID_ACCEL_X, roundf(sensor_combined->accelerometer_m_s2[0] * 1000.0f)); + frsky_send_data(uart, FRSKY_ID_ACCEL_Y, roundf(sensor_combined->accelerometer_m_s2[1] * 1000.0f)); + frsky_send_data(uart, FRSKY_ID_ACCEL_Z, roundf(sensor_combined->accelerometer_m_s2[2] * 1000.0f)); frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP, - sensor_combined->baro_alt_meter[0]); + sensor_combined->baro_alt_meter); frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP, - roundf(frac(sensor_combined->baro_alt_meter[0]) * 100.0f)); + roundf(frac(sensor_combined->baro_alt_meter) * 100.0f)); frsky_send_data(uart, FRSKY_ID_TEMP1, - roundf(sensor_combined->baro_temp_celcius[0])); + roundf(sensor_combined->baro_temp_celcius)); frsky_send_data(uart, FRSKY_ID_VFAS, roundf(battery_status->voltage_v * 10.0f)); diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c index 77507d2e6b..ad0e6a5a8c 100644 --- a/src/drivers/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -67,8 +67,8 @@ static volatile bool thread_should_exit = false; static volatile bool thread_running = false; static int frsky_task; -typedef enum { IDLE, SPORT, DTYPE } frsky_state_t; -static frsky_state_t frsky_state = IDLE; +typedef enum { SCANNING, SPORT, DTYPE } frsky_state_t; +static frsky_state_t frsky_state = SCANNING; /* functions */ static int sPort_open_uart(const char *uart_name, struct termios *uart_config, struct termios *uart_config_original); @@ -77,6 +77,7 @@ static void usage(void); static int frsky_telemetry_thread_main(int argc, char *argv[]); __EXPORT int frsky_telemetry_main(int argc, char *argv[]); +#define DEBUG /** * Opens the UART device and sets all required serial parameters. @@ -106,7 +107,7 @@ static int sPort_open_uart(const char *uart_name, struct termios *uart_config, s uart_config->c_oflag &= ~OPOST; /* Set baud rate */ - static const speed_t speed = B57600; + static const speed_t speed = B9600; if (cfsetispeed(uart_config, speed) < 0 || cfsetospeed(uart_config, speed) < 0) { warnx("ERR: %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); @@ -175,7 +176,7 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) } } - /* Open UART assuming SmartPort telemetry */ + /* Open UART assuming D type telemetry */ struct termios uart_config_original; struct termios uart_config; const int uart = sPort_open_uart(device_name, &uart_config, &uart_config_original); @@ -194,9 +195,10 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) /* Main thread loop */ char sbuf[20]; - frsky_state = IDLE; + frsky_state = SCANNING; + frsky_state_t baudRate = DTYPE; - while (!thread_should_exit && frsky_state == IDLE) { + while (!thread_should_exit && frsky_state == SCANNING) { /* 2 byte polling frames indicate SmartPort telemetry * 11 byte packets indicate D type telemetry */ @@ -204,18 +206,64 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) if (status > 0) { /* traffic on the port, D type is 11 bytes per frame, SmartPort is only 2 - * allow a little time to receive the entire packet + * Wait long enough for 11 bytes at 9600 baud */ - usleep(5000); - status = read(uart, &sbuf[0], sizeof(sbuf)); - } + usleep(12000); + int nbytes = read(uart, &sbuf[0], sizeof(sbuf)); + PX4_DEBUG("frsky input: %d bytes: %x %x, speed: %d", nbytes, sbuf[0], sbuf[1], baudRate); - /* received some data; check size of packet */ - if (status > 0 && status < 3) { - frsky_state = SPORT; + // look for valid header byte + if (nbytes > 10) { + if (baudRate == DTYPE) { + // see if we got a valid D-type hostframe + struct adc_linkquality host_frame; + + if (frsky_parse_host((uint8_t *)&sbuf[0], nbytes, &host_frame)) { + frsky_state = baudRate; + break; + } + + } else { + // check for alternating S.port start bytes + int index = 0; + + while (index < 2 && sbuf[index] != 0x7E) { index++; } + + if (index < 2) { + + int success = 1; + + for (int i = index + 2; i < nbytes; i += 2) { + if (sbuf[i] != 0x7E) { success = 0; break; } + } + + if (success) { + frsky_state = baudRate; + break; + } + } + } + + } + + // alternate between S.port and D-type baud rates + if (baudRate == SPORT) { + PX4_DEBUG("setting baud rate to %d", 9600); + set_uart_speed(uart, &uart_config, B9600); + baudRate = DTYPE; + + } else { + PX4_DEBUG("setting baud rate to %d", 57600); + set_uart_speed(uart, &uart_config, B57600); + baudRate = SPORT; + + } + + // wait a second + usleep(1000000); + // flush buffer + read(uart, &sbuf[0], sizeof(sbuf)); - } else if (status > 0) { - frsky_state = DTYPE; } } @@ -225,7 +273,7 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) err(1, "could not allocate memory"); } - warnx("sending FrSky SmartPort telemetry"); + PX4_INFO("sending FrSky SmartPort telemetry"); struct sensor_baro_s *sensor_baro = malloc(sizeof(struct sensor_baro_s)); @@ -434,7 +482,7 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) } } - warnx("freeing sPort memory"); + PX4_DEBUG("freeing sPort memory"); sPort_deinit(); free(sensor_baro); @@ -442,11 +490,11 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) } else if (frsky_state == DTYPE) { /* detected D type telemetry: reconfigure UART */ - warnx("sending FrSky D type telemetry"); + PX4_INFO("sending FrSky D type telemetry"); int status = set_uart_speed(uart, &uart_config, B9600); if (status < 0) { - warnx("error setting speed for %s, quitting", device_name); + PX4_DEBUG("error setting speed for %s, quitting", device_name); /* Reset the UART flags to original state */ tcsetattr(uart, TCSANOW, &uart_config_original); close(uart); @@ -464,8 +512,6 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) struct adc_linkquality host_frame; -// uint8_t dbuf[45]; - /* send D8 mode telemetry */ while (!thread_should_exit) { @@ -506,7 +552,7 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) // /* TODO: flush the input buffer if in full duplex mode */ // read(uart, &sbuf[0], sizeof(sbuf)); - warnx("freeing frsky memory"); + PX4_DEBUG("freeing frsky memory"); frsky_deinit(); } @@ -573,8 +619,8 @@ int frsky_telemetry_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { switch (frsky_state) { - case IDLE: - errx(0, "running: IDLE"); + case SCANNING: + errx(0, "running: SCANNING"); break; case SPORT: diff --git a/src/drivers/frsky_telemetry/sPort_data.c b/src/drivers/frsky_telemetry/sPort_data.c index c266b84924..c728ea6f8a 100644 --- a/src/drivers/frsky_telemetry/sPort_data.c +++ b/src/drivers/frsky_telemetry/sPort_data.c @@ -236,7 +236,7 @@ void sPort_send_CUR(int uart) void sPort_send_ALT(int uart) { /* send data */ - uint32_t alt = (int)(100 * sensor_combined->baro_alt_meter[0]); + uint32_t alt = (int)(100 * sensor_combined->baro_alt_meter); sPort_send_data(uart, SMARTPORT_ID_ALT, alt); } diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices index 8f2d911a52..3cac6a5bc5 160000 --- a/src/drivers/gps/devices +++ b/src/drivers/gps/devices @@ -1 +1 @@ -Subproject commit 8f2d911a5203e7f6d29c1e3cf3405972a87215d8 +Subproject commit 3cac6a5bc5826d7ac495827a46b63f476d82ba86 diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 1b12a9dd09..4640be99e3 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -52,6 +52,7 @@ #include +#include #include #include #include @@ -71,11 +72,13 @@ #include #include #include +#include #include #include #include #include #include +#include #include @@ -100,7 +103,7 @@ public: class GPS { public: - GPS(const char *uart_path, bool fake_gps, bool enable_sat_info); + GPS(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num); virtual ~GPS(); virtual int init(); @@ -125,17 +128,24 @@ private: GPS_Sat_Info *_sat_info; ///< instance of GPS sat info data object struct vehicle_gps_position_s _report_gps_pos; ///< uORB topic for gps position orb_advert_t _report_gps_pos_pub; ///< uORB pub for gps position + int _gps_orb_instance; ///< uORB multi-topic instance struct satellite_info_s *_p_report_sat_info; ///< pointer to uORB topic for satellite info + int _gps_sat_orb_instance; ///< uORB multi-topic instance for satellite info orb_advert_t _report_sat_info_pub; ///< uORB pub for satellite info float _rate; ///< position update rate float _rate_rtcm_injection; ///< RTCM message injection rate unsigned _last_rate_rtcm_injection_count; ///< counter for number of RTCM messages bool _fake_gps; ///< fake gps output + int _gps_num; ///< number of GPS connected static const int _orb_inject_data_fd_count = 4; int _orb_inject_data_fd[_orb_inject_data_fd_count]; int _orb_inject_data_next = 0; + orb_advert_t _dump_communication_pub; ///< if non-null, dump communication + gps_dump_s *_dump_to_device; + gps_dump_s *_dump_from_device; + /** * Try to configure the GPS, handle outgoing communication to the GPS */ @@ -144,7 +154,7 @@ private: /** * Trampoline to the worker task */ - static void task_main_trampoline(void *arg); + static void task_main_trampoline(int argc, char *argv[]); /** * Worker task: main GPS thread that configures the GPS and parses incoming data, always running @@ -161,6 +171,16 @@ private: */ void cmd_reset(); + /** + * Publish the gps struct + */ + void publish(); + + /** + * Publish the satellite info + */ + void publishSatelliteInfo(); + /** * This is an abstraction for the poll on serial used. * @@ -196,6 +216,16 @@ private: * callback from the driver for the platform specific stuff */ static int callback(GPSCallbackType type, void *data1, int data2, void *user); + + /** + * Dump gps communication. + * @param data message + * @param len length of the message + * @param msg_to_gps_device if true, this is a message sent to the gps device, otherwise it's from the device + */ + void dumpGpsData(uint8_t *data, size_t len, bool msg_to_gps_device); + + void initializeCommunicationDump(); }; @@ -207,32 +237,37 @@ extern "C" __EXPORT int gps_main(int argc, char *argv[]); namespace { -GPS *g_dev = nullptr; - +GPS *g_dev[2] = {nullptr, nullptr}; +volatile bool is_gps1_advertised = false; ///< for the second gps we want to make sure that it gets instance 1 +/// and thus we wait until the first one publishes at least one message. } -GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info) : + +GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num) : _task_should_exit(false), _healthy(false), _mode_changed(false), _mode(GPS_DRIVER_MODE_UBX), _helper(nullptr), _sat_info(nullptr), - _report_gps_pos_pub(nullptr), + _report_gps_pos_pub{nullptr}, + _gps_orb_instance(-1), _p_report_sat_info(nullptr), _report_sat_info_pub(nullptr), _rate(0.0f), _rate_rtcm_injection(0.0f), _last_rate_rtcm_injection_count(0), - _fake_gps(fake_gps) + _fake_gps(fake_gps), + _gps_num(gps_num), + _dump_communication_pub(nullptr), + _dump_to_device(nullptr), + _dump_from_device(nullptr) { /* store port name */ strncpy(_port, uart_path, sizeof(_port)); /* enforce null termination */ _port[sizeof(_port) - 1] = '\0'; - /* we need this potentially before it could be set in task_main */ - g_dev = this; memset(&_report_gps_pos, 0, sizeof(_report_gps_pos)); /* create satellite info data object if requested */ @@ -267,28 +302,38 @@ GPS::~GPS() delete(_sat_info); } - g_dev = nullptr; + if (_dump_to_device) { + delete(_dump_to_device); + } + + if (_dump_from_device) { + delete(_dump_from_device); + } } int GPS::init() { + char gps_num[2] = {(char)('0' + _gps_num), 0}; + char *const args[2] = { gps_num, NULL }; + /* start the GPS driver worker task */ _task = px4_task_spawn_cmd("gps", SCHED_DEFAULT, - SCHED_PRIORITY_SLOW_DRIVER, 1200, (px4_main_t)&GPS::task_main_trampoline, nullptr); + SCHED_PRIORITY_SLOW_DRIVER, 1200, (px4_main_t)&GPS::task_main_trampoline, args); if (_task < 0) { PX4_WARN("task start failed: %d", errno); + _task = -1; return -errno; } return OK; } -void GPS::task_main_trampoline(void *arg) +void GPS::task_main_trampoline(int argc, char *argv[]) { - g_dev->task_main(); + g_dev[argv[argc - 1][0] - '1']->task_main(); } int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user) @@ -296,10 +341,19 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user) GPS *gps = (GPS *)user; switch (type) { - case GPSCallbackType::readDeviceData: - return gps->pollOrRead((uint8_t *)data1, data2, *((int *)data1)); + case GPSCallbackType::readDeviceData: { + int num_read = gps->pollOrRead((uint8_t *)data1, data2, *((int *)data1)); + + if (num_read > 0) { + gps->dumpGpsData((uint8_t *)data1, (size_t)num_read, false); + } + + return num_read; + } case GPSCallbackType::writeDeviceData: + gps->dumpGpsData((uint8_t *)data1, (size_t)data2, true); + return write(gps->_serial_fd, data1, (size_t)data2); case GPSCallbackType::setBaudrate: @@ -385,6 +439,11 @@ void GPS::handleInjectDataTopic() if (updated) { struct gps_inject_data_s msg; orb_copy(ORB_ID(gps_inject_data), orb_inject_data_cur_fd, &msg); + + /* Write the message to the gps device. Note that the message could be fragmented. + * But as we don't write anywhere else to the device during operation, we don't + * need to assemble the message first. + */ injectData(msg.data, msg.len); _orb_inject_data_next = (_orb_inject_data_next + 1) % _orb_inject_data_fd_count; @@ -396,7 +455,11 @@ void GPS::handleInjectDataTopic() bool GPS::injectData(uint8_t *data, size_t len) { - return ::write(_serial_fd, data, len) == len; + dumpGpsData(data, len, true); + + size_t written = ::write(_serial_fd, data, len); + ::fsync(_serial_fd); + return written == len; } int GPS::setBaudrate(unsigned baud) @@ -511,6 +574,68 @@ int GPS::setBaudrate(unsigned baud) return 0; } +void GPS::initializeCommunicationDump() +{ + param_t gps_dump_comm_ph = param_find("GPS_DUMP_COMM"); + int32_t param_dump_comm; + + if (gps_dump_comm_ph == PARAM_INVALID || param_get(gps_dump_comm_ph, ¶m_dump_comm) != 0) { + return; + } + + if (param_dump_comm != 1) { + return; //dumping disabled + } + + _dump_from_device = new gps_dump_s(); + _dump_to_device = new gps_dump_s(); + + if (!_dump_from_device || !_dump_to_device) { + PX4_ERR("failed to allocated dump data"); + return; + } + + memset(_dump_to_device, 0, sizeof(gps_dump_s)); + memset(_dump_from_device, 0, sizeof(gps_dump_s)); + + int instance; + //make sure to use a large enough queue size, so that we don't lose messages. You may also want + //to increase the logger rate for that. + _dump_communication_pub = orb_advertise_multi_queue(ORB_ID(gps_dump), _dump_from_device, &instance, + ORB_PRIO_DEFAULT, 8); +} + +void GPS::dumpGpsData(uint8_t *data, size_t len, bool msg_to_gps_device) +{ + if (!_dump_communication_pub) { + return; + } + + gps_dump_s *dump_data = msg_to_gps_device ? _dump_to_device : _dump_from_device; + + while (len > 0) { + size_t write_len = len; + + if (write_len > sizeof(dump_data->data) - dump_data->len) { + write_len = sizeof(dump_data->data) - dump_data->len; + } + + memcpy(dump_data->data + dump_data->len, data, write_len); + data += write_len; + dump_data->len += write_len; + len -= write_len; + + if (dump_data->len >= sizeof(dump_data->data)) { + if (msg_to_gps_device) { + dump_data->len |= 1 << 7; + } + + dump_data->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(gps_dump), _dump_communication_pub, dump_data); + dump_data->len = 0; + } + } +} void GPS::task_main() @@ -537,6 +662,8 @@ GPS::task_main() _orb_inject_data_fd[i] = orb_subscribe_multi(ORB_ID(gps_inject_data), i); } + initializeCommunicationDump(); + uint64_t last_rate_measurement = hrt_absolute_time(); unsigned last_rate_count = 0; @@ -544,17 +671,15 @@ GPS::task_main() while (!_task_should_exit) { if (_fake_gps) { - _report_gps_pos.timestamp_position = hrt_absolute_time(); + _report_gps_pos.timestamp = hrt_absolute_time(); _report_gps_pos.lat = (int32_t)47.378301e7f; _report_gps_pos.lon = (int32_t)8.538777e7f; _report_gps_pos.alt = (int32_t)1200e3f; - _report_gps_pos.timestamp_variance = hrt_absolute_time(); _report_gps_pos.s_variance_m_s = 10.0f; _report_gps_pos.c_variance_rad = 0.1f; _report_gps_pos.fix_type = 3; _report_gps_pos.eph = 0.9f; _report_gps_pos.epv = 1.8f; - _report_gps_pos.timestamp_velocity = hrt_absolute_time(); _report_gps_pos.vel_n_m_s = 0.0f; _report_gps_pos.vel_e_m_s = 0.0f; _report_gps_pos.vel_d_m_s = 0.0f; @@ -566,12 +691,7 @@ GPS::task_main() /* no time and satellite information simulated */ - if (_report_gps_pos_pub != nullptr) { - orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); - - } else { - _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); - } + publish(); usleep(2e5); @@ -616,24 +736,16 @@ GPS::task_main() * no valid position lock */ - _report_gps_pos.timestamp_time = hrt_absolute_time(); - - /* reset the timestamps for data, because we have no data yet */ - _report_gps_pos.timestamp_position = 0; - _report_gps_pos.timestamp_variance = 0; - _report_gps_pos.timestamp_velocity = 0; + /* reset the timestamp for data, because we have no data yet */ + _report_gps_pos.timestamp = 0; + _report_gps_pos.timestamp_time_relative = 0; /* set a massive variance */ _report_gps_pos.eph = 10000.0f; _report_gps_pos.epv = 10000.0f; _report_gps_pos.fix_type = 0; - if (_report_gps_pos_pub != nullptr) { - orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); - - } else { - _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); - } + publish(); /* GPS is obviously detected successfully, reset statistics */ _helper->resetUpdateRates(); @@ -644,23 +756,13 @@ GPS::task_main() while ((helper_ret = _helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) { if (helper_ret & 1) { - if (_report_gps_pos_pub != nullptr) { - orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); - - } else { - _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); - } + publish(); last_rate_count++; } if (_p_report_sat_info && (helper_ret & 2)) { - if (_report_sat_info_pub != nullptr) { - orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info); - - } else { - _report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), _p_report_sat_info); - } + publishSatelliteInfo(); } /* measure update rate every 5 seconds */ @@ -730,15 +832,21 @@ GPS::task_main() } - PX4_WARN("exiting"); + PX4_INFO("exiting"); for (size_t i = 0; i < _orb_inject_data_fd_count; ++i) { orb_unsubscribe(_orb_inject_data_fd[i]); _orb_inject_data_fd[i] = -1; } + if (_dump_communication_pub) { + orb_unadvertise(_dump_communication_pub); + } + ::close(_serial_fd); + orb_unadvertise(_report_gps_pos_pub); + /* tell the dtor that we are exiting */ _task = -1; px4_task_exit(0); @@ -751,10 +859,10 @@ GPS::cmd_reset() { #ifdef GPIO_GPS_NRESET PX4_WARN("Toggling GPS reset pin"); - stm32_configgpio(GPIO_GPS_NRESET); - stm32_gpiowrite(GPIO_GPS_NRESET, 0); + px4_arch_configgpio(GPIO_GPS_NRESET); + px4_arch_gpiowrite(GPIO_GPS_NRESET, 0); usleep(100); - stm32_gpiowrite(GPIO_GPS_NRESET, 1); + px4_arch_gpiowrite(GPIO_GPS_NRESET, 1); PX4_WARN("Toggled GPS reset pin"); #endif } @@ -762,6 +870,8 @@ GPS::cmd_reset() void GPS::print_info() { + PX4_WARN("GPS %i:", _gps_num); + //GPS Mode if (_fake_gps) { PX4_WARN("protocol: SIMULATED"); @@ -792,9 +902,9 @@ GPS::print_info() _report_gps_pos.noise_per_ms, _report_gps_pos.jamming_indicator == 255 ? "YES" : "NO"); - if (_report_gps_pos.timestamp_position != 0) { + if (_report_gps_pos.timestamp != 0) { PX4_WARN("position lock: %d, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type, - _report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0); + _report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp) / 1000.0); PX4_WARN("lat: %d, lon: %d, alt: %d", _report_gps_pos.lat, _report_gps_pos.lon, _report_gps_pos.alt); PX4_WARN("vel: %.2fm/s, %.2fm/s, %.2fm/s", (double)_report_gps_pos.vel_n_m_s, (double)_report_gps_pos.vel_e_m_s, (double)_report_gps_pos.vel_d_m_s); @@ -810,15 +920,41 @@ GPS::print_info() usleep(100000); } +void +GPS::publish() +{ + if (_gps_num == 1) { + orb_publish_auto(ORB_ID(vehicle_gps_position), &_report_gps_pos_pub, &_report_gps_pos, &_gps_orb_instance, + ORB_PRIO_DEFAULT); + is_gps1_advertised = true; + + } else if (is_gps1_advertised) { + orb_publish_auto(ORB_ID(vehicle_gps_position), &_report_gps_pos_pub, &_report_gps_pos, &_gps_orb_instance, + ORB_PRIO_DEFAULT); + } + +} +void +GPS::publishSatelliteInfo() +{ + if (_gps_num == 1) { + orb_publish_auto(ORB_ID(satellite_info), &_report_sat_info_pub, _p_report_sat_info, &_gps_sat_orb_instance, + ORB_PRIO_DEFAULT); + + } else { + //we don't publish satellite info for the secondary gps + } + +} + /** * Local functions in support of the shell command. */ namespace gps { -GPS *g_dev = nullptr; -void start(const char *uart_path, bool fake_gps, bool enable_sat_info); +void start(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num); void stop(); void test(); void reset(); @@ -828,35 +964,24 @@ void info(); * Start the driver. */ void -start(const char *uart_path, bool fake_gps, bool enable_sat_info) +start(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num) { - if (g_dev != nullptr) { - PX4_WARN("gps already started"); + if (g_dev[gps_num - 1] != nullptr) { + PX4_WARN("GPS %i already started", gps_num); return; } /* create the driver */ - g_dev = new GPS(uart_path, fake_gps, enable_sat_info); + g_dev[gps_num - 1] = new GPS(uart_path, fake_gps, enable_sat_info, gps_num); - if (g_dev == nullptr) { - goto fail; + if (!g_dev[gps_num - 1] || OK != g_dev[gps_num - 1]->init()) { + if (g_dev[gps_num - 1] != nullptr) { + delete g_dev[gps_num - 1]; + g_dev[gps_num - 1] = nullptr; + } + + PX4_ERR("start of GPS %i failed", gps_num); } - - if (OK != g_dev->init()) { - goto fail; - } - - return; - -fail: - - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - } - - PX4_ERR("start failed"); - return; } /** @@ -865,8 +990,14 @@ fail: void stop() { - delete g_dev; - g_dev = nullptr; + delete g_dev[0]; + g_dev[0] = nullptr; + + if (g_dev[1] != nullptr) { + delete g_dev[1]; + } + + g_dev[1] = nullptr; } /** @@ -897,12 +1028,18 @@ reset() void info() { - if (g_dev == nullptr) { + if (g_dev[0] == nullptr) { PX4_ERR("GPS Not running"); return; } - g_dev->print_info(); + g_dev[0]->print_info(); + + if (g_dev[1] != nullptr) { + g_dev[1]->print_info(); + } + + return; } } // namespace @@ -913,6 +1050,7 @@ gps_main(int argc, char *argv[]) { /* set to default */ const char *device_name = GPS_DEFAULT_UART_PORT; + const char *device_name2 = nullptr; bool fake_gps = false; bool enable_sat_info = false; @@ -949,7 +1087,24 @@ gps_main(int argc, char *argv[]) } } - gps::start(device_name, fake_gps, enable_sat_info); + /* Allow to use a second gps device */ + for (int i = 2; i < argc; i++) { + if (!strcmp(argv[i], "-dualgps")) { + if (argc > i + 1) { + device_name2 = argv[i + 1]; + + } else { + PX4_ERR("Did not get second device address"); + } + } + } + + gps::start(device_name, fake_gps, enable_sat_info, 1); + + if (device_name2) { + gps::start(device_name2, fake_gps, enable_sat_info, 2); + } + } if (!strcmp(argv[1], "stop")) { @@ -980,6 +1135,7 @@ gps_main(int argc, char *argv[]) return 0; out: - PX4_ERR("unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status'\n [-d /dev/ttyS0-n][-f (for enabling fake)][-s (to enable sat info)]"); + PX4_ERR("unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status'"); + PX4_ERR("[-d " GPS_DEFAULT_UART_PORT "][-f (for enabling fake)][-s (to enable sat info)]"); return 1; } diff --git a/src/drivers/gps/params.c b/src/drivers/gps/params.c new file mode 100644 index 0000000000..75f4c3dc22 --- /dev/null +++ b/src/drivers/gps/params.c @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * Copyright (c) 2016 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. + * + ****************************************************************************/ + +/** + * Dump GPS communication to a file. + * + * If this is set to 1, all GPS communication data will be published via uORB, + * and written to the log file as gps_dump message. + * @min 0 + * @max 1 + * @value 0 Disable + * @value 1 Enable + * @group GPS + */ +PARAM_DEFINE_INT32(GPS_DUMP_COMM, 0); + diff --git a/src/drivers/hc_sr04/hc_sr04.cpp b/src/drivers/hc_sr04/hc_sr04.cpp index efe239e53f..e5571c5fa2 100644 --- a/src/drivers/hc_sr04/hc_sr04.cpp +++ b/src/drivers/hc_sr04/hc_sr04.cpp @@ -297,9 +297,9 @@ HC_SR04::init() /* init echo port : */ for (unsigned i = 0; i <= _sonars; i++) { - stm32_configgpio(_gpio_tab[i].trig_port); - stm32_gpiowrite(_gpio_tab[i].trig_port, false); - stm32_configgpio(_gpio_tab[i].echo_port); + px4_arch_configgpio(_gpio_tab[i].trig_port); + px4_arch_gpiowrite(_gpio_tab[i].trig_port, false); + px4_arch_configgpio(_gpio_tab[i].echo_port); _latest_sonar_measurements.push_back(0); } @@ -441,14 +441,14 @@ HC_SR04::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; } - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); if (!_reports->resize(arg)) { - irqrestore(flags); + px4_leave_critical_section(flags); return -ENOMEM; } - irqrestore(flags); + px4_leave_critical_section(flags); return OK; } @@ -546,9 +546,9 @@ HC_SR04::measure() /* * Send a plus begin a measurement. */ - stm32_gpiowrite(_gpio_tab[_cycle_counter].trig_port, true); + px4_arch_gpiowrite(_gpio_tab[_cycle_counter].trig_port, true); usleep(10); // 10us - stm32_gpiowrite(_gpio_tab[_cycle_counter].trig_port, false); + px4_arch_gpiowrite(_gpio_tab[_cycle_counter].trig_port, false); stm32_gpiosetevent(_gpio_tab[_cycle_counter].echo_port, true, true, false, sonar_isr); _status = 0; diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 73d81020f7..c16cd69dfd 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -698,14 +698,14 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; } - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); if (!_reports->resize(arg)) { - irqrestore(flags); + px4_leave_critical_section(flags); return -ENOMEM; } - irqrestore(flags); + px4_leave_critical_section(flags); return OK; } diff --git a/src/drivers/hott/hott_sensors/module.mk b/src/drivers/hott/hott_sensors/module.mk deleted file mode 100644 index 25a6f0ac04..0000000000 --- a/src/drivers/hott/hott_sensors/module.mk +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Graupner HoTT Sensors application. -# - -MODULE_COMMAND = hott_sensors - -SRCS = hott_sensors.cpp - -MAXOPTIMIZATION = -Os diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp index 1150fd0722..92b1664ccf 100644 --- a/src/drivers/hott/messages.cpp +++ b/src/drivers/hott/messages.cpp @@ -149,12 +149,12 @@ build_eam_response(uint8_t *buffer, size_t *size) msg.eam_sensor_id = EAM_SENSOR_ID; msg.sensor_text_id = EAM_SENSOR_TEXT_ID; - msg.temperature1 = (uint8_t)(raw.baro_temp_celcius[0] + 20); + msg.temperature1 = (uint8_t)(raw.baro_temp_celcius + 20); msg.temperature2 = msg.temperature1 - BOARD_TEMP_OFFSET_DEG; msg.main_voltage_L = (uint8_t)(battery.voltage_v * 10); - uint16_t alt = (uint16_t)(raw.baro_alt_meter[0] + 500); + uint16_t alt = (uint16_t)(raw.baro_alt_meter + 500); msg.altitude_L = (uint8_t)alt & 0xff; msg.altitude_H = (uint8_t)(alt >> 8) & 0xff; diff --git a/src/drivers/irlock/irlock.cpp b/src/drivers/irlock/irlock.cpp index 620cbf0824..e412bb57cc 100644 --- a/src/drivers/irlock/irlock.cpp +++ b/src/drivers/irlock/irlock.cpp @@ -67,10 +67,20 @@ #define IRLOCK_RESYNC 0x5500 #define IRLOCK_ADJUST 0xAA -#define IRLOCK_CENTER_X 159 // the x-axis center pixel position -#define IRLOCK_CENTER_Y 99 // the y-axis center pixel position -#define IRLOCK_PIXELS_PER_RADIAN_X 307.9075f // x-axis pixel to radian scaler assuming 60deg FOV on x-axis -#define IRLOCK_PIXELS_PER_RADIAN_Y 326.4713f // y-axis pixel to radian scaler assuming 35deg FOV on y-axis +#define IRLOCK_RES_X 320 +#define IRLOCK_RES_Y 200 + +#define IRLOCK_CENTER_X (IRLOCK_RES_X/2) // the x-axis center pixel position +#define IRLOCK_CENTER_Y (IRLOCK_RES_Y/2) // the y-axis center pixel position + +#define IRLOCK_FOV_X (60.0f*M_PI_F/180.0f) +#define IRLOCK_FOV_Y (35.0f*M_PI_F/180.0f) + +#define IRLOCK_TAN_HALF_FOV_X 0.57735026919f // tan(0.5 * 60 * pi/180) +#define IRLOCK_TAN_HALF_FOV_Y 0.31529878887f // tan(0.5 * 35 * pi/180) + +#define IRLOCK_TAN_ANG_PER_PIXEL_X (2*IRLOCK_TAN_HALF_FOV_X/IRLOCK_RES_X) +#define IRLOCK_TAN_ANG_PER_PIXEL_Y (2*IRLOCK_TAN_HALF_FOV_Y/IRLOCK_RES_Y) #ifndef CONFIG_SCHED_WORKQUEUE # error This requires CONFIG_SCHED_WORKQUEUE. @@ -107,7 +117,7 @@ private: int read_device(); bool sync_device(); int read_device_word(uint16_t *word); - int read_device_block(struct irlock_s *block); + int read_device_block(struct irlock_target_s *block); /** internal variables **/ ringbuffer::RingBuffer *_reports; @@ -158,7 +168,7 @@ int IRLOCK::init() } /** allocate buffer storing values read from sensor **/ - _reports = new ringbuffer::RingBuffer(IRLOCK_OBJECTS_MAX, sizeof(struct irlock_s)); + _reports = new ringbuffer::RingBuffer(2, sizeof(struct irlock_s)); if (_reports == nullptr) { return ENOTTY; @@ -224,20 +234,20 @@ int IRLOCK::test() warnx("searching for object for 10 seconds"); /** read from sensor for 10 seconds **/ - struct irlock_s obj_report; + struct irlock_s report; uint64_t start_time = hrt_absolute_time(); while ((hrt_absolute_time() - start_time) < 10000000) { - - /** output all objects found **/ - while (_reports->count() > 0) { - _reports->get(&obj_report); - warnx("sig:%d x:%4.3f y:%4.3f width:%4.3f height:%4.3f", - (int)obj_report.target_num, - (double)obj_report.angle_x, - (double)obj_report.angle_y, - (double)obj_report.size_x, - (double)obj_report.size_y); + if (_reports->get(&report)) { + /** output all objects found **/ + for (uint8_t i = 0; i < report.num_targets; i++) { + warnx("sig:%d x:%4.3f y:%4.3f width:%4.3f height:%4.3f", + (int)report.targets[i].signature, + (double)report.targets[i].pos_x, + (double)report.targets[i].pos_y, + (double)report.targets[i].size_x, + (double)report.targets[i].size_y); + } } /** sleep for 0.05 seconds **/ @@ -337,20 +347,22 @@ int IRLOCK::read_device() return -ENOTTY; } - /** now read blocks until sync stops, first flush stale queue data **/ - _reports->flush(); - int num_objects = 0; + struct irlock_s report; - while (sync_device() && (num_objects < IRLOCK_OBJECTS_MAX)) { - struct irlock_s block; + report.timestamp = hrt_absolute_time(); - if (read_device_block(&block) != OK) { + report.num_targets = 0; + + while (report.num_targets < IRLOCK_OBJECTS_MAX) { + if (!sync_device() || read_device_block(&report.targets[report.num_targets]) != OK) { break; } - _reports->force(&block); + report.num_targets++; } + _reports->force(&report); + return OK; } @@ -367,33 +379,31 @@ int IRLOCK::read_device_word(uint16_t *word) } /** read a single block (a full frame) from sensor **/ -int IRLOCK::read_device_block(struct irlock_s *block) +int IRLOCK::read_device_block(struct irlock_target_s *block) { uint8_t bytes[12]; memset(bytes, 0, sizeof bytes); int status = transfer(nullptr, 0, &bytes[0], 12); uint16_t checksum = bytes[1] << 8 | bytes[0]; - uint16_t target_num = bytes[3] << 8 | bytes[2]; + uint16_t signature = bytes[3] << 8 | bytes[2]; uint16_t pixel_x = bytes[5] << 8 | bytes[4]; uint16_t pixel_y = bytes[7] << 8 | bytes[6]; uint16_t pixel_size_x = bytes[9] << 8 | bytes[8]; uint16_t pixel_size_y = bytes[11] << 8 | bytes[10]; /** crc check **/ - if (target_num + pixel_x + pixel_y + pixel_size_x + pixel_size_y != checksum) { + if (signature + pixel_x + pixel_y + pixel_size_x + pixel_size_y != checksum) { _read_failures++; return -EIO; } /** convert to angles **/ - block->target_num = target_num; - block->angle_x = (((float)(pixel_x - IRLOCK_CENTER_X)) / IRLOCK_PIXELS_PER_RADIAN_X); - block->angle_y = (((float)(pixel_y - IRLOCK_CENTER_Y)) / IRLOCK_PIXELS_PER_RADIAN_Y); - block->size_x = pixel_size_x / IRLOCK_PIXELS_PER_RADIAN_X; - block->size_y = pixel_size_y / IRLOCK_PIXELS_PER_RADIAN_Y; - - block->timestamp = hrt_absolute_time(); + block->signature = signature; + block->pos_x = (pixel_x - IRLOCK_CENTER_X) * IRLOCK_TAN_ANG_PER_PIXEL_X; + block->pos_y = (pixel_y - IRLOCK_CENTER_Y) * IRLOCK_TAN_ANG_PER_PIXEL_Y; + block->size_x = pixel_size_x * IRLOCK_TAN_ANG_PER_PIXEL_X; + block->size_y = pixel_size_y * IRLOCK_TAN_ANG_PER_PIXEL_Y; return status; } diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 61345628b1..fccbbfbb21 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -666,14 +666,14 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; } - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); if (!_reports->resize(arg)) { - irqrestore(flags); + px4_leave_critical_section(flags); return -ENOMEM; } - irqrestore(flags); + px4_leave_critical_section(flags); return OK; } diff --git a/src/drivers/lis3mdl/CMakeLists.txt b/src/drivers/lis3mdl/CMakeLists.txt index 4eeea263e4..d97e1dc221 100644 --- a/src/drivers/lis3mdl/CMakeLists.txt +++ b/src/drivers/lis3mdl/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__lis3mdl MAIN lis3mdl - STACK_MAIN 1200 COMPILE_FLAGS -Weffc++ diff --git a/src/drivers/lis3mdl/lis3mdl.cpp b/src/drivers/lis3mdl/lis3mdl.cpp index 5a994df648..a3dfe3d869 100644 --- a/src/drivers/lis3mdl/lis3mdl.cpp +++ b/src/drivers/lis3mdl/lis3mdl.cpp @@ -706,14 +706,14 @@ LIS3MDL::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; } - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); if (!_reports->resize(arg)) { - irqrestore(flags); + px4_leave_critical_section(flags); return -ENOMEM; } - irqrestore(flags); + px4_leave_critical_section(flags); return OK; } diff --git a/src/drivers/ll40ls/LidarLiteI2C.cpp b/src/drivers/ll40ls/LidarLiteI2C.cpp index f85b672c5d..64bdf9a11c 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.cpp +++ b/src/drivers/ll40ls/LidarLiteI2C.cpp @@ -216,14 +216,14 @@ int LidarLiteI2C::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; } - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); if (!_reports->resize(arg)) { - irqrestore(flags); + px4_leave_critical_section(flags); return -ENOMEM; } - irqrestore(flags); + px4_leave_critical_section(flags); return OK; } diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 71796cfe8c..e2f02e397c 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -928,14 +928,14 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; } - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); if (!_accel_reports->resize(arg)) { - irqrestore(flags); + px4_leave_critical_section(flags); return -ENOMEM; } - irqrestore(flags); + px4_leave_critical_section(flags); return OK; } @@ -1063,14 +1063,14 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; } - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); if (!_mag_reports->resize(arg)) { - irqrestore(flags); + px4_leave_critical_section(flags); return -ENOMEM; } - irqrestore(flags); + px4_leave_critical_section(flags); return OK; } diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index 9a6fa36d79..6c6222c1f9 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -431,14 +431,14 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; } - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); if (!_reports->resize(arg)) { - irqrestore(flags); + px4_leave_critical_section(flags); return -ENOMEM; } - irqrestore(flags); + px4_leave_critical_section(flags); return OK; } @@ -616,12 +616,12 @@ MB12XX::start() work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 5); /* notify about state change */ - struct subsystem_info_s info = { - true, - true, - true, - subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER - }; + struct subsystem_info_s info = {}; + info.present = true; + info.enabled = true; + info.ok = true; + info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER; + static orb_advert_t pub = nullptr; if (pub != nullptr) { diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index fd0ac59551..3e53e32d47 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -75,7 +75,6 @@ #include #include -#include #include #include #include diff --git a/src/drivers/mkblctrl/mkblctrl_params.c b/src/drivers/mkblctrl/mkblctrl_params.c index 0264efeb90..7fe3bd9361 100644 --- a/src/drivers/mkblctrl/mkblctrl_params.c +++ b/src/drivers/mkblctrl/mkblctrl_params.c @@ -44,12 +44,9 @@ #include /** - * Enables testmode (Identify) of MKBLCTRL Driver + * Test mode (Identify) of MKBLCTRL Driver * - * @value 0 Disabled - * @value 1 Enabled - * @min 0 - * @max 1 + * @boolean * @group MKBLCTRL Testmode */ PARAM_DEFINE_INT32(MKBLCTRL_TEST, 0); diff --git a/src/drivers/mpu6000/CMakeLists.txt b/src/drivers/mpu6000/CMakeLists.txt index c4cabd418d..13009f31b0 100644 --- a/src/drivers/mpu6000/CMakeLists.txt +++ b/src/drivers/mpu6000/CMakeLists.txt @@ -33,7 +33,7 @@ px4_add_module( MODULE drivers__mpu6000 MAIN mpu6000 - STACK_MAIN 1200 + STACK_MAIN 1300 COMPILE_FLAGS -Weffc++ -Os diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 4bf6384b3f..fce303315e 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -737,7 +737,7 @@ int MPU6000::reset() while (--tries != 0) { irqstate_t state; - state = irqsave(); + state = px4_enter_critical_section(); write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); up_udelay(10000); @@ -750,7 +750,7 @@ int MPU6000::reset() // Disable I2C bus (recommended on datasheet) write_checked_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS); - irqrestore(state); + px4_leave_critical_section(state); if (read_reg(MPUREG_PWR_MGMT_1) == MPU_CLK_SEL_PLLGYROZ) { break; @@ -1364,14 +1364,14 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; } - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); if (!_accel_reports->resize(arg)) { - irqrestore(flags); + px4_leave_critical_section(flags); return -ENOMEM; } - irqrestore(flags); + px4_leave_critical_section(flags); return OK; } @@ -1449,14 +1449,14 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; } - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); if (!_gyro_reports->resize(arg)) { - irqrestore(flags); + px4_leave_critical_section(flags); return -ENOMEM; } - irqrestore(flags); + px4_leave_critical_section(flags); return OK; } @@ -2394,7 +2394,7 @@ usage() warnx("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump', 'factorytest', 'testerror'"); warnx("options:"); warnx(" -X (external bus)"); - warnx(" -M 6000|20608 (default 6000)"); + warnx(" -T 6000|20608 (default 6000)"); warnx(" -R rotation"); warnx(" -a accel range (in g)"); } diff --git a/src/drivers/mpu6500/mpu6500.cpp b/src/drivers/mpu6500/mpu6500.cpp index c3457c5b8b..96256c2909 100644 --- a/src/drivers/mpu6500/mpu6500.cpp +++ b/src/drivers/mpu6500/mpu6500.cpp @@ -703,7 +703,7 @@ int MPU6500::reset() while (--tries != 0) { irqstate_t state; - state = irqsave(); + state = px4_enter_critical_section(); write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); up_udelay(10000); @@ -716,7 +716,7 @@ int MPU6500::reset() // Disable I2C bus (recommended on datasheet) write_checked_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS); - irqrestore(state); + px4_leave_critical_section(state); if (read_reg(MPUREG_PWR_MGMT_1) == MPU_CLK_SEL_PLLGYROZ) { break; @@ -1292,14 +1292,14 @@ MPU6500::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; } - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); if (!_accel_reports->resize(arg)) { - irqrestore(flags); + px4_leave_critical_section(flags); return -ENOMEM; } - irqrestore(flags); + px4_leave_critical_section(flags); return OK; } @@ -1377,14 +1377,14 @@ MPU6500::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; } - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); if (!_gyro_reports->resize(arg)) { - irqrestore(flags); + px4_leave_critical_section(flags); return -ENOMEM; } - irqrestore(flags); + px4_leave_critical_section(flags); return OK; } diff --git a/src/drivers/mpu9250/mag.cpp b/src/drivers/mpu9250/mag.cpp index 6180cb13c0..2609965042 100644 --- a/src/drivers/mpu9250/mag.cpp +++ b/src/drivers/mpu9250/mag.cpp @@ -412,14 +412,14 @@ MPU9250_mag::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; } - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); if (!_mag_reports->resize(arg)) { - irqrestore(flags); + px4_leave_critical_section(flags); return -ENOMEM; } - irqrestore(flags); + px4_leave_critical_section(flags); return OK; } diff --git a/src/drivers/mpu9250/mpu9250.cpp b/src/drivers/mpu9250/mpu9250.cpp index 761c8f1008..02a21f52fd 100644 --- a/src/drivers/mpu9250/mpu9250.cpp +++ b/src/drivers/mpu9250/mpu9250.cpp @@ -169,6 +169,8 @@ #define BITS_DLPF_CFG_3600HZ 0x07 #define BITS_DLPF_CFG_MASK 0x07 +#define BITS_ACCEL_CONFIG2_41HZ 0x03 + #define BIT_RAW_RDY_EN 0x01 #define BIT_INT_ANYRD_2CLEAR 0x10 @@ -471,6 +473,9 @@ int MPU9250::reset() write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read usleep(1000); + write_checked_reg(MPUREG_ACCEL_CONFIG2, BITS_ACCEL_CONFIG2_41HZ); + usleep(1000); + uint8_t retries = 10; while (retries--) { @@ -876,14 +881,14 @@ MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; } - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); if (!_accel_reports->resize(arg)) { - irqrestore(flags); + px4_leave_critical_section(flags); return -ENOMEM; } - irqrestore(flags); + px4_leave_critical_section(flags); return OK; } @@ -972,14 +977,14 @@ MPU9250::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; } - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); if (!_gyro_reports->resize(arg)) { - irqrestore(flags); + px4_leave_critical_section(flags); return -ENOMEM; } - irqrestore(flags); + px4_leave_critical_section(flags); return OK; } diff --git a/src/drivers/ms5611/CMakeLists.txt b/src/drivers/ms5611/CMakeLists.txt index 7359bd9507..1c5ebbb673 100644 --- a/src/drivers/ms5611/CMakeLists.txt +++ b/src/drivers/ms5611/CMakeLists.txt @@ -33,23 +33,13 @@ set(srcs ms5611_spi.cpp ms5611_i2c.cpp - ) - -if(${OS} STREQUAL "nuttx") - list(APPEND srcs - ms5611_nuttx.cpp - ) -else() - list(APPEND srcs - ms5611_posix.cpp - ms5611_sim.cpp - ) - -endif() + ms5611.cpp +) px4_add_module( MODULE drivers__ms5611 MAIN ms5611 + STACK_MAIN 1200 COMPILE_FLAGS -Os SRCS ${srcs} diff --git a/src/drivers/ms5611/ms5611_nuttx.cpp b/src/drivers/ms5611/ms5611.cpp similarity index 90% rename from src/drivers/ms5611/ms5611_nuttx.cpp rename to src/drivers/ms5611/ms5611.cpp index 7de852eebc..300c0aac8f 100644 --- a/src/drivers/ms5611/ms5611_nuttx.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -33,7 +33,7 @@ /** * @file ms5611.cpp - * Driver for the MS5611 barometric pressure sensor connected via I2C or SPI. + * Driver for the MS5611 and MS6507 barometric pressure sensor connected via I2C or SPI. */ #include @@ -66,9 +66,15 @@ #include #include +#include #include "ms5611.h" +enum MS56XX_DEVICE_TYPES { + MS5611_DEVICE = 0, + MS5607_DEVICE = 1 +}; + enum MS5611_BUS { MS5611_BUS_ALL = 0, MS5611_BUS_I2C_INTERNAL, @@ -94,11 +100,11 @@ static const int ERROR = -1; #define POW2(_x) ((_x) * (_x)) /* - * MS5611 internal constants and data structures. + * MS5611/MS5607 internal constants and data structures. */ /* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */ -#define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */ +#define MS5611_CONVERSION_INTERVAL 25000 /* microseconds */ #define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ #define MS5611_BARO_DEVICE_PATH_EXT "/dev/ms5611_ext" #define MS5611_BARO_DEVICE_PATH_INT "/dev/ms5611_int" @@ -106,7 +112,7 @@ static const int ERROR = -1; class MS5611 : public device::CDev { public: - MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path); + MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path, enum MS56XX_DEVICE_TYPES device_type); ~MS5611(); virtual int init(); @@ -128,11 +134,11 @@ protected: unsigned _measure_ticks; ringbuffer::RingBuffer *_reports; - + enum MS56XX_DEVICE_TYPES _device_type; bool _collect_phase; unsigned _measure_phase; - /* intermediate temperature values per MS5611 datasheet */ + /* intermediate temperature values per MS5611/MS5607 datasheet */ int32_t _TEMP; int64_t _OFF; int64_t _SENS; @@ -214,12 +220,14 @@ protected: */ extern "C" __EXPORT int ms5611_main(int argc, char *argv[]); -MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path) : +MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path, + enum MS56XX_DEVICE_TYPES device_type) : CDev("MS5611", path), _interface(interface), _prom(prom_buf.s), _measure_ticks(0), _reports(nullptr), + _device_type(device_type), _collect_phase(false), _measure_phase(0), _TEMP(0), @@ -486,14 +494,14 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; } - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); if (!_reports->resize(arg)) { - irqrestore(flags); + px4_leave_critical_section(flags); return -ENOMEM; } - irqrestore(flags); + px4_leave_critical_section(flags); return OK; } @@ -691,27 +699,62 @@ MS5611::collect() _TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.c6_temp_coeff_temp) >> 23); /* base sensor scale/offset values */ - _SENS = ((int64_t)_prom.c1_pressure_sens << 15) + (((int64_t)_prom.c3_temp_coeff_pres_sens * dT) >> 8); - _OFF = ((int64_t)_prom.c2_pressure_offset << 16) + (((int64_t)_prom.c4_temp_coeff_pres_offset * dT) >> 7); + if (_device_type == MS5611_DEVICE) { - /* temperature compensation */ - if (_TEMP < 2000) { + /* Perform MS5611 Caculation */ - int32_t T2 = POW2(dT) >> 31; + _OFF = ((int64_t)_prom.c2_pressure_offset << 16) + (((int64_t)_prom.c4_temp_coeff_pres_offset * dT) >> 7); + _SENS = ((int64_t)_prom.c1_pressure_sens << 15) + (((int64_t)_prom.c3_temp_coeff_pres_sens * dT) >> 8); - int64_t f = POW2((int64_t)_TEMP - 2000); - int64_t OFF2 = 5 * f >> 1; - int64_t SENS2 = 5 * f >> 2; + /* MS5611 temperature compensation */ - if (_TEMP < -1500) { - int64_t f2 = POW2(_TEMP + 1500); - OFF2 += 7 * f2; - SENS2 += 11 * f2 >> 1; + if (_TEMP < 2000) { + + int32_t T2 = POW2(dT) >> 31; + + int64_t f = POW2((int64_t)_TEMP - 2000); + int64_t OFF2 = 5 * f >> 1; + int64_t SENS2 = 5 * f >> 2; + + if (_TEMP < -1500) { + + int64_t f2 = POW2(_TEMP + 1500); + OFF2 += 7 * f2; + SENS2 += 11 * f2 >> 1; + } + + _TEMP -= T2; + _OFF -= OFF2; + _SENS -= SENS2; } - _TEMP -= T2; - _OFF -= OFF2; - _SENS -= SENS2; + } else if (_device_type == MS5607_DEVICE) { + + /* Perform MS5607 Caculation */ + + _OFF = ((int64_t)_prom.c2_pressure_offset << 17) + (((int64_t)_prom.c4_temp_coeff_pres_offset * dT) >> 6); + _SENS = ((int64_t)_prom.c1_pressure_sens << 16) + (((int64_t)_prom.c3_temp_coeff_pres_sens * dT) >> 7); + + /* MS5607 temperature compensation */ + + if (_TEMP < 2000) { + + int32_t T2 = POW2(dT) >> 31; + + int64_t f = POW2((int64_t)_TEMP - 2000); + int64_t OFF2 = 61 * f >> 4; + int64_t SENS2 = 2 * f; + + if (_TEMP < -1500) { + int64_t f2 = POW2(_TEMP + 1500); + OFF2 += 15 * f2; + SENS2 += 8 * f2; + } + + _TEMP -= T2; + _OFF -= OFF2; + _SENS -= SENS2; + } } } else { @@ -840,9 +883,9 @@ struct ms5611_bus_option { }; #define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) -bool start_bus(struct ms5611_bus_option &bus); +bool start_bus(struct ms5611_bus_option &bus, enum MS56XX_DEVICE_TYPES device_type); struct ms5611_bus_option &find_bus(enum MS5611_BUS busid); -void start(enum MS5611_BUS busid); +void start(enum MS5611_BUS busid, enum MS56XX_DEVICE_TYPES device_type); void test(enum MS5611_BUS busid); void reset(enum MS5611_BUS busid); void info(); @@ -900,7 +943,7 @@ crc4(uint16_t *n_prom) * Start the driver. */ bool -start_bus(struct ms5611_bus_option &bus) +start_bus(struct ms5611_bus_option &bus, enum MS56XX_DEVICE_TYPES device_type) { if (bus.dev != nullptr) { errx(1, "bus option already started"); @@ -915,7 +958,7 @@ start_bus(struct ms5611_bus_option &bus) return false; } - bus.dev = new MS5611(interface, prom_buf, bus.devpath); + bus.dev = new MS5611(interface, prom_buf, bus.devpath, device_type); if (bus.dev != nullptr && OK != bus.dev->init()) { delete bus.dev; @@ -947,7 +990,7 @@ start_bus(struct ms5611_bus_option &bus) * is either successfully up and running or failed to start. */ void -start(enum MS5611_BUS busid) +start(enum MS5611_BUS busid, enum MS56XX_DEVICE_TYPES device_type) { uint8_t i; bool started = false; @@ -963,7 +1006,7 @@ start(enum MS5611_BUS busid) continue; } - started = started | start_bus(bus_options[i]); + started = started | start_bus(bus_options[i], device_type); } if (!started) { @@ -1195,6 +1238,8 @@ usage() warnx(" -I (intternal I2C bus)"); warnx(" -S (external SPI bus)"); warnx(" -s (internal SPI bus)"); + warnx(" -T 5611|5607 (default 5611)"); + } } // namespace @@ -1203,10 +1248,13 @@ int ms5611_main(int argc, char *argv[]) { enum MS5611_BUS busid = MS5611_BUS_ALL; + int device_type = 5611; // Default to MS5611 int ch; + int myoptind = 1; + const char *myoptarg = NULL; /* jump over start/off/etc and look at options first */ - while ((ch = getopt(argc, argv, "XISs")) != EOF) { + while ((ch = px4_getopt(argc, argv, "T:XISs", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'X': busid = MS5611_BUS_I2C_EXTERNAL; @@ -1224,19 +1272,28 @@ ms5611_main(int argc, char *argv[]) busid = MS5611_BUS_SPI_INTERNAL; break; + case 'T': + device_type = atoi(myoptarg); + + if (device_type == 5611 || device_type == 5607) { + break; + } + + //no break default: ms5611::usage(); exit(0); } } - const char *verb = argv[optind]; + + const char *verb = argv[myoptind]; /* * Start/load the driver. */ if (!strcmp(verb, "start")) { - ms5611::start(busid); + ms5611::start(busid, device_type == 5607 ? MS5607_DEVICE : MS5611_DEVICE); } /* diff --git a/src/drivers/ms5611/ms5611_posix.cpp b/src/drivers/ms5611/ms5611_posix.cpp deleted file mode 100644 index b4b22f6c35..0000000000 --- a/src/drivers/ms5611/ms5611_posix.cpp +++ /dev/null @@ -1,1335 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * 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 ms5611.cpp - * Driver for the MS5611 barometric pressure sensor connected via I2C or SPI. - */ - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include - -#include -#include - -#include "ms5611.h" - -enum MS5611_BUS { - MS5611_BUS_ALL = 0, - MS5611_BUS_I2C_INTERNAL, - MS5611_BUS_I2C_EXTERNAL, - MS5611_BUS_SPI_INTERNAL, - MS5611_BUS_SPI_EXTERNAL, - MS5611_BUS_SIM_EXTERNAL -}; - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - -//#ifndef CONFIG_SCHED_WORKQUEUE -//# error This requires CONFIG_SCHED_WORKQUEUE. -//#endif - -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) - -/* helper macro for arithmetic - returns the square of the argument */ -#define POW2(_x) ((_x) * (_x)) - -/* - * MS5611 internal constants and data structures. - */ - -/* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */ -#define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */ -#define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ -#define MS5611_BARO_DEVICE_PATH_EXT "/dev/ms5611_ext" -#define MS5611_BARO_DEVICE_PATH_INT "/dev/ms5611_int" - -class MS5611 : public device::VDev -{ -public: - MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path); - ~MS5611(); - - virtual int init(); - - virtual ssize_t read(device::file_t *handlep, char *buffer, size_t buflen); - virtual int ioctl(device::file_t *handlep, int cmd, unsigned long arg); - - /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); - -protected: - Device *_interface; - - ms5611::prom_s _prom; - - struct work_s _work; - unsigned _measure_ticks; - - ringbuffer::RingBuffer *_reports; - - bool _collect_phase; - unsigned _measure_phase; - - /* intermediate temperature values per MS5611 datasheet */ - int32_t _TEMP; - int64_t _OFF; - int64_t _SENS; - float _P; - float _T; - - /* altitude conversion calibration */ - unsigned _msl_pressure; /* in Pa */ - - orb_advert_t _baro_topic; - int _orb_class_instance; - int _class_instance; - - perf_counter_t _sample_perf; - perf_counter_t _measure_perf; - perf_counter_t _comms_errors; - perf_counter_t _buffer_overflows; - - /** - * Initialize the automatic measurement state machine and start it. - * - * @note This function is called at open and error time. It might make sense - * to make it more aggressive about resetting the bus in case of errors. - */ - void start_cycle(); - - /** - * Stop the automatic measurement state machine. - */ - void stop_cycle(); - - /** - * Perform a poll cycle; collect from the previous measurement - * and start a new one. - * - * This is the heart of the measurement state machine. This function - * alternately starts a measurement, or collects the data from the - * previous measurement. - * - * When the interval between measurements is greater than the minimum - * measurement interval, a gap is inserted between collection - * and measurement to provide the most recent measurement possible - * at the next interval. - */ - void cycle(); - - /** - * Get the internal / external state - * - * @return true if the sensor is not on the main MCU board - */ - bool is_external() { return (_orb_class_instance == 0); /* XXX put this into the interface class */ } - - /** - * Static trampoline from the workq context; because we don't have a - * generic workq wrapper yet. - * - * @param arg Instance pointer for the driver that is polling. - */ - static void cycle_trampoline(void *arg); - - /** - * Issue a measurement command for the current state. - * - * @return OK if the measurement command was successful. - */ - virtual int measure(); - - /** - * Collect the result of the most recent measurement. - */ - virtual int collect(); -}; - -/* - * Driver 'main' command. - */ -extern "C" __EXPORT int ms5611_main(int argc, char *argv[]); - -MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path) : - VDev("MS5611", path), - _interface(interface), - _prom(prom_buf.s), - _measure_ticks(0), - _reports(nullptr), - _collect_phase(false), - _measure_phase(0), - _TEMP(0), - _OFF(0), - _SENS(0), - _msl_pressure(101325), - _baro_topic(nullptr), - _orb_class_instance(-1), - _class_instance(-1), - _sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")), - _measure_perf(perf_alloc(PC_ELAPSED, "ms5611_measure")), - _comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")), - _buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows")) -{ - // work_cancel in stop_cycle called from the dtor will explode if we don't do this... - memset(&_work, 0, sizeof(_work)); -} - -MS5611::~MS5611() -{ - /* make sure we are truly inactive */ - stop_cycle(); - - if (_class_instance != -1) { - unregister_class_devname(get_devname(), _class_instance); - } - - /* free any existing reports */ - if (_reports != nullptr) { - delete _reports; - } - - // free perf counters - perf_free(_sample_perf); - perf_free(_measure_perf); - perf_free(_comms_errors); - perf_free(_buffer_overflows); - - delete _interface; -} - -int -MS5611::init() -{ - int ret; - warnx("MS5611::init"); - - ret = VDev::init(); - - if (ret != OK) { - DEVICE_DEBUG("VDev init failed"); - goto out; - } - - /* allocate basic report buffers */ - _reports = new ringbuffer::RingBuffer(2, sizeof(baro_report)); - - if (_reports == nullptr) { - DEVICE_DEBUG("can't get memory for reports"); - ret = -ENOMEM; - goto out; - } - - /* register alternate interfaces if we have to */ - _class_instance = register_class_devname(BARO_BASE_DEVICE_PATH); - - struct baro_report brp; - /* do a first measurement cycle to populate reports with valid data */ - _measure_phase = 0; - _reports->flush(); - - /* this do..while is goto without goto */ - do { - /* do temperature first */ - if (OK != measure()) { - ret = -EIO; - warnx("temp measure failed"); - break; - } - - usleep(MS5611_CONVERSION_INTERVAL); - - if (OK != collect()) { - ret = -EIO; - warnx("temp collect failed"); - break; - } - - /* now do a pressure measurement */ - if (OK != measure()) { - ret = -EIO; - warnx("pressure collect failed"); - break; - } - - usleep(MS5611_CONVERSION_INTERVAL); - - if (OK != collect()) { - ret = -EIO; - warnx("pressure collect failed"); - break; - } - - /* state machine will have generated a report, copy it out */ - _reports->get(&brp); - - ret = OK; - - _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp, - &_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT); - - if (_baro_topic == nullptr) { - warnx("failed to create sensor_baro publication"); - } - - //warnx("sensor_baro publication %ld", _baro_topic); - - } while (0); - -out: - return ret; -} - -ssize_t -MS5611::read(device::file_t *handlep, char *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(struct baro_report); - struct baro_report *brp = reinterpret_cast(buffer); - int ret = 0; - - /* buffer must be large enough */ - if (count < 1) { - return -ENOSPC; - } - - /* if automatic measurement is enabled */ - if (_measure_ticks > 0) { - - /* - * While there is space in the caller's buffer, and reports, copy them. - * Note that we may be pre-empted by the workq thread while we are doing this; - * we are careful to avoid racing with them. - */ - while (count--) { - if (_reports->get(brp)) { - ret += sizeof(*brp); - brp++; - } - } - - /* if there was no data, warn the caller */ - return ret ? ret : -EAGAIN; - } - - /* manual measurement - run one conversion */ - do { - _measure_phase = 0; - _reports->flush(); - - /* do temperature first */ - if (OK != measure()) { - ret = -EIO; - break; - } - - usleep(MS5611_CONVERSION_INTERVAL); - - if (OK != collect()) { - ret = -EIO; - break; - } - - /* now do a pressure measurement */ - if (OK != measure()) { - ret = -EIO; - break; - } - - usleep(MS5611_CONVERSION_INTERVAL); - - if (OK != collect()) { - ret = -EIO; - break; - } - - /* state machine will have generated a report, copy it out */ - if (_reports->get(brp)) { - ret = sizeof(*brp); - } - - } while (0); - - return ret; -} - -int -MS5611::ioctl(device::file_t *handlep, int cmd, unsigned long arg) -{ - switch (cmd) { - - case SENSORIOCSPOLLRATE: { - switch (arg) { - - /* switching to manual polling */ - case SENSOR_POLLRATE_MANUAL: - stop_cycle(); - _measure_ticks = 0; - return OK; - - /* external signalling not supported */ - case SENSOR_POLLRATE_EXTERNAL: - - /* zero would be bad */ - case 0: - return -EINVAL; - - /* set default/max polling rate */ - case SENSOR_POLLRATE_MAX: - case SENSOR_POLLRATE_DEFAULT: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); - - /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL); - - /* if we need to start the poll state machine, do it */ - if (want_start) { - start_cycle(); - } - - return OK; - } - - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); - - /* convert hz to tick interval via microseconds */ - unsigned ticks = USEC2TICK(1000000 / arg); - - /* check against maximum rate */ - if ((unsigned long)ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL)) { - return -EINVAL; - } - - /* update interval for next measurement */ - _measure_ticks = ticks; - - /* if we need to start the poll state machine, do it */ - if (want_start) { - start_cycle(); - } - - return OK; - } - } - } - - case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) { - return SENSOR_POLLRATE_MANUAL; - } - - return (1000 / _measure_ticks); - - case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) { - return -EINVAL; - } - - if (!_reports->resize(arg)) { - return -ENOMEM; - } - - return OK; - } - - case SENSORIOCGQUEUEDEPTH: - return _reports->size(); - - case SENSORIOCRESET: - /* - * Since we are initialized, we do not need to do anything, since the - * PROM is correctly read and the part does not need to be configured. - */ - return OK; - - case BAROIOCSMSLPRESSURE: - - /* range-check for sanity */ - if ((arg < 80000) || (arg > 120000)) { - return -EINVAL; - } - - _msl_pressure = arg; - return OK; - - case BAROIOCGMSLPRESSURE: - return _msl_pressure; - - default: - break; - } - - /* give it to the bus-specific superclass */ - // return bus_ioctl(filp, cmd, arg); - return VDev::ioctl(handlep, cmd, arg); -} - -void -MS5611::start_cycle() -{ - - /* reset the report ring and state machine */ - _collect_phase = false; - _measure_phase = 0; - _reports->flush(); - - /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1); -} - -void -MS5611::stop_cycle() -{ - work_cancel(HPWORK, &_work); -} - -void -MS5611::cycle_trampoline(void *arg) -{ - MS5611 *dev = reinterpret_cast(arg); - - dev->cycle(); -} - -void -MS5611::cycle() -{ - int ret; - unsigned dummy; - - /* collection phase? */ - if (_collect_phase) { - - /* perform collection */ - ret = collect(); - - if (ret != OK) { - if (ret == -6) { - /* - * The ms5611 seems to regularly fail to respond to - * its address; this happens often enough that we'd rather not - * spam the console with a message for this. - */ - } else { - //DEVICE_LOG("collection error %d", ret); - } - - /* issue a reset command to the sensor */ - _interface->dev_ioctl(IOCTL_RESET, dummy); - /* reset the collection state machine and try again */ - start_cycle(); - return; - } - - /* next phase is measurement */ - _collect_phase = false; - - /* - * Is there a collect->measure gap? - * Don't inject one after temperature measurements, so we can keep - * doing pressure measurements at something close to the desired rate. - */ - if ((_measure_phase != 0) && - ((unsigned long)_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) { - - /* schedule a fresh cycle call when we are ready to measure again */ - work_queue(HPWORK, - &_work, - (worker_t)&MS5611::cycle_trampoline, - this, - _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL)); - - return; - } - } - - /* measurement phase */ - ret = measure(); - - if (ret != OK) { - //DEVICE_LOG("measure error %d", ret); - /* issue a reset command to the sensor */ - _interface->dev_ioctl(IOCTL_RESET, dummy); - /* reset the collection state machine and try again */ - start_cycle(); - return; - } - - /* next phase is collection */ - _collect_phase = true; - - /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&MS5611::cycle_trampoline, - this, - USEC2TICK(MS5611_CONVERSION_INTERVAL)); -} - -int -MS5611::measure() -{ - int ret; - - perf_begin(_measure_perf); - - /* - * In phase zero, request temperature; in other phases, request pressure. - */ - unsigned addr = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1; - - /* - * Send the command to begin measuring. - */ - ret = _interface->dev_ioctl(IOCTL_MEASURE, addr); - - if (OK != ret) { - perf_count(_comms_errors); - } - - perf_end(_measure_perf); - - return ret; -} - -int -MS5611::collect() -{ - int ret; - uint32_t raw; - - perf_begin(_sample_perf); - - struct baro_report report; - /* this should be fairly close to the end of the conversion, so the best approximation of the time */ - report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); - - /* read the most recent measurement - read offset/size are hardcoded in the interface */ - ret = _interface->dev_read(0, (void *)&raw, 0); - - if (ret < 0) { - perf_count(_comms_errors); - perf_end(_sample_perf); - return ret; - } - - /* handle a measurement */ - if (_measure_phase == 0) { - - /* temperature offset (in ADC units) */ - int32_t dT = (int32_t)raw - ((int32_t)_prom.c5_reference_temp << 8); - - /* absolute temperature in centidegrees - note intermediate value is outside 32-bit range */ - _TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.c6_temp_coeff_temp) >> 23); - - /* base sensor scale/offset values */ - _SENS = ((int64_t)_prom.c1_pressure_sens << 15) + (((int64_t)_prom.c3_temp_coeff_pres_sens * dT) >> 8); - _OFF = ((int64_t)_prom.c2_pressure_offset << 16) + (((int64_t)_prom.c4_temp_coeff_pres_offset * dT) >> 7); - - /* temperature compensation */ - if (_TEMP < 2000) { - - int32_t T2 = POW2(dT) >> 31; - - int64_t f = POW2((int64_t)_TEMP - 2000); - int64_t OFF2 = 5 * f >> 1; - int64_t SENS2 = 5 * f >> 2; - - if (_TEMP < -1500) { - int64_t f2 = POW2(_TEMP + 1500); - OFF2 += 7 * f2; - SENS2 += 11 * f2 >> 1; - } - - _TEMP -= T2; - _OFF -= OFF2; - _SENS -= SENS2; - } - - } else { - - /* pressure calculation, result in Pa */ - int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15; - _P = P * 0.01f; - _T = _TEMP * 0.01f; - - /* generate a new report */ - report.temperature = _TEMP / 100.0f; - report.pressure = P / 100.0f; /* convert to millibar */ - - /* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */ - - /* - * PERFORMANCE HINT: - * - * The single precision calculation is 50 microseconds faster than the double - * precision variant. It is however not obvious if double precision is required. - * Pending more inspection and tests, we'll leave the double precision variant active. - * - * Measurements: - * double precision: ms5611_read: 992 events, 258641us elapsed, min 202us max 305us - * single precision: ms5611_read: 963 events, 208066us elapsed, min 202us max 241us - */ - - /* tropospheric properties (0-11km) for standard atmosphere */ - const double T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */ - const double a = -6.5 / 1000; /* temperature gradient in degrees per metre */ - const double g = 9.80665; /* gravity constant in m/s/s */ - const double R = 287.05; /* ideal gas constant in J/kg/K */ - - /* current pressure at MSL in kPa */ - double p1 = _msl_pressure / 1000.0; - - /* measured pressure in kPa */ - double p = P / 1000.0; - - /* - * Solve: - * - * / -(aR / g) \ - * | (p / p1) . T1 | - T1 - * \ / - * h = ------------------------------- + h1 - * a - */ - report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; - - /* publish it */ - if (!(_pub_blocked)) { - if (_baro_topic != (orb_advert_t)(-1)) { - /* publish it */ - orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); - - } else { - printf("MS5611::collect _baro_topic not initialized\n"); - } - } - - if (_reports->force(&report)) { - perf_count(_buffer_overflows); - } - - /* notify anyone waiting for data */ - poll_notify(POLLIN); - } - - /* update the measurement state machine */ - INCREMENT(_measure_phase, MS5611_MEASUREMENT_RATIO + 1); - - perf_end(_sample_perf); - - return OK; -} - -void -MS5611::print_info() -{ - perf_print_counter(_sample_perf); - perf_print_counter(_comms_errors); - perf_print_counter(_buffer_overflows); - printf("poll interval: %u ticks\n", _measure_ticks); - _reports->print_info("report queue"); - printf("TEMP: %d\n", _TEMP); - printf("SENS: %lld\n", (long long)_SENS); - printf("OFF: %lld\n", (long long)_OFF); - printf("P: %.3f\n", (double)_P); - printf("T: %.3f\n", (double)_T); - printf("MSL pressure: %10.4f\n", (double)(_msl_pressure / 100.f)); - - printf("factory_setup %u\n", _prom.factory_setup); - printf("c1_pressure_sens %u\n", _prom.c1_pressure_sens); - printf("c2_pressure_offset %u\n", _prom.c2_pressure_offset); - printf("c3_temp_coeff_pres_sens %u\n", _prom.c3_temp_coeff_pres_sens); - printf("c4_temp_coeff_pres_offset %u\n", _prom.c4_temp_coeff_pres_offset); - printf("c5_reference_temp %u\n", _prom.c5_reference_temp); - printf("c6_temp_coeff_temp %u\n", _prom.c6_temp_coeff_temp); - printf("serial_and_crc %u\n", _prom.serial_and_crc); -} - -/** - * Local functions in support of the shell command. - */ -namespace ms5611 -{ - -/* - list of supported bus configurations - */ -struct ms5611_bus_option { - enum MS5611_BUS busid; - const char *devpath; - MS5611_constructor interface_constructor; - uint8_t busnum; - MS5611 *dev; -} bus_options[] = { -#if 0 -#if defined(PX4_SPIDEV_EXT_BARO) && defined(PX4_SPI_BUS_EXT) - { MS5611_BUS_SPI_EXTERNAL, "/dev/ms5611_spi_ext", &MS5611_spi_interface, PX4_SPI_BUS_EXT, NULL }, -#endif -#ifdef PX4_SPIDEV_BARO - { MS5611_BUS_SPI_INTERNAL, "/dev/ms5611_spi_int", &MS5611_spi_interface, PX4_SPI_BUS_SENSORS, NULL }, -#endif -#ifdef PX4_I2C_BUS_ONBOARD - { MS5611_BUS_I2C_INTERNAL, "/dev/ms5611_int", &MS5611_i2c_interface, PX4_I2C_BUS_ONBOARD, NULL }, -#endif -#ifdef PX4_I2C_BUS_EXPANSION - { MS5611_BUS_I2C_EXTERNAL, "/dev/ms5611_ext", &MS5611_i2c_interface, PX4_I2C_BUS_EXPANSION, NULL }, -#endif -#endif -#ifdef PX4_SIM_BUS_TEST - { MS5611_BUS_SIM_EXTERNAL, "/dev/ms5611_sim", &MS5611_sim_interface, PX4_SIM_BUS_TEST, NULL }, -#endif -}; -#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) - -bool start_bus(struct ms5611_bus_option &bus); -bool find_bus(enum MS5611_BUS busid, ms5611_bus_option &bus); -int start(enum MS5611_BUS busid); -int test(enum MS5611_BUS busid); -int reset(enum MS5611_BUS busid); -int info(); -int calibrate(unsigned altitude, enum MS5611_BUS busid); -void usage(); - -/** - * MS5611 crc4 cribbed from the datasheet - */ -bool -crc4(uint16_t *n_prom) -{ - int16_t cnt; - uint16_t n_rem; - uint16_t crc_read; - uint8_t n_bit; - - n_rem = 0x00; - - /* save the read crc */ - crc_read = n_prom[7]; - - /* remove CRC byte */ - n_prom[7] = (0xFF00 & (n_prom[7])); - - for (cnt = 0; cnt < 16; cnt++) { - /* uneven bytes */ - if (cnt & 1) { - n_rem ^= (uint8_t)((n_prom[cnt >> 1]) & 0x00FF); - - } else { - n_rem ^= (uint8_t)(n_prom[cnt >> 1] >> 8); - } - - for (n_bit = 8; n_bit > 0; n_bit--) { - if (n_rem & 0x8000) { - n_rem = (n_rem << 1) ^ 0x3000; - - } else { - n_rem = (n_rem << 1); - } - } - } - - /* final 4 bit remainder is CRC value */ - n_rem = (0x000F & (n_rem >> 12)); - n_prom[7] = crc_read; - - /* return true if CRCs match */ - return (0x000F & crc_read) == (n_rem ^ 0x00); -} - - -/** - * Start the driver. - */ -bool -start_bus(struct ms5611_bus_option &bus) -{ - if (bus.dev != nullptr) { - warnx("bus option already started"); - return false; - } - - prom_u prom_buf; - device::Device *interface = bus.interface_constructor(prom_buf, bus.busnum); - - if (interface->init() != OK) { - delete interface; - warnx("no device on bus %u", (unsigned)bus.busid); - return false; - } - - bus.dev = new MS5611(interface, prom_buf, bus.devpath); - - if (bus.dev != nullptr && OK != bus.dev->init()) { - delete bus.dev; - bus.dev = NULL; - warnx("bus init failed %p", bus.dev); - return false; - } - - int fd = px4_open(bus.devpath, O_RDONLY); - - /* set the poll rate to default, starts automatic data collection */ - if (fd == -1) { - warnx("can't open baro device"); - return false; - } - - if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - px4_close(fd); - warnx("failed setting default poll rate"); - return false; - } - - px4_close(fd); - return true; -} - - -/** - * Start the driver. - * - * This function call only returns once the driver - * is either successfully up and running or failed to start. - */ -int -start(enum MS5611_BUS busid) -{ - uint8_t i; - bool started = false; - - for (i = 0; i < NUM_BUS_OPTIONS; i++) { - if (busid == MS5611_BUS_ALL && bus_options[i].dev != NULL) { - // this device is already started - continue; - } - - if (busid != MS5611_BUS_ALL && bus_options[i].busid != busid) { - // not the one that is asked for - continue; - } - - started |= start_bus(bus_options[i]); - } - - if (!started) { - warnx("driver start failed"); - return 1; - } - - // one or more drivers started OK - return 0; -} - - -/** - * find a bus structure for a busid - */ -bool find_bus(enum MS5611_BUS busid, ms5611_bus_option &bus) -{ - for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) { - if ((busid == MS5611_BUS_ALL || - busid == bus_options[i].busid) && bus_options[i].dev != NULL) { - bus = bus_options[i]; - return true; - } - } - - PX4_WARN("bus %u not started", (unsigned)busid); - return false; -} - -/** - * Perform some basic functional tests on the driver; - * make sure we can collect data from the sensor in polled - * and automatic modes. - */ -int -test(enum MS5611_BUS busid) -{ - struct ms5611_bus_option bus; - - if (!find_bus(busid, bus)) { - return 1; - } - - struct baro_report report; - - ssize_t sz; - - int ret; - - int fd; - - fd = px4_open(bus.devpath, O_RDONLY); - - if (fd < 0) { - warn("open failed (try 'ms5611 start' if the driver is not running)"); - return 1; - } - - /* do a simple demand read */ - sz = px4_read(fd, &report, sizeof(report)); - - if (sz != sizeof(report)) { - warn("immediate read failed"); - return 1; - } - - warnx("single read"); - warnx("pressure: %10.4f", (double)report.pressure); - warnx("altitude: %11.4f", (double)report.altitude); - warnx("temperature: %8.4f", (double)report.temperature); - warnx("time: %lld", (long long)report.timestamp); - - /* set the queue depth to 10 */ - if (OK != px4_ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) { - warnx("failed to set queue depth"); - return 1; - } - - /* start the sensor polling at 2Hz */ - if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) { - warnx("failed to set 2Hz poll rate"); - return 1; - } - - /* read the sensor 5x and report each value */ - for (unsigned i = 0; i < 5; i++) { - struct pollfd fds; - - /* wait for data to be ready */ - fds.fd = fd; - fds.events = POLLIN; - ret = poll(&fds, 1, 2000); - - if (ret != 1) { - warnx("timed out waiting for sensor data"); - } - - /* now go get it */ - sz = px4_read(fd, &report, sizeof(report)); - - if (sz != sizeof(report)) { - warn("periodic read failed"); - return 1; - } - - warnx("periodic read %u", i); - warnx("pressure: %10.4f", (double)report.pressure); - warnx("altitude: %11.4f", (double)report.altitude); - warnx("temperature: %8.4f", (double)report.temperature); - warnx("time: %lld", (long long)report.timestamp); - } - - close(fd); - warnx("PASS"); - return 0; -} - -/** - * Reset the driver. - */ -int -reset(enum MS5611_BUS busid) -{ - struct ms5611_bus_option bus; - - if (!find_bus(busid, bus)) { - return 1; - } - - int fd; - - fd = open(bus.devpath, O_RDONLY); - - if (fd < 0) { - warn("failed "); - return 1; - } - - if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) { - warn("driver reset failed"); - return 1; - } - - if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - warn("driver poll restart failed"); - return 1; - } - - return 0; -} - -/** - * Print a little info about the driver. - */ -int -info() -{ - for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) { - struct ms5611_bus_option &bus = bus_options[i]; - - if (bus.dev != nullptr) { - warnx("%s", bus.devpath); - bus.dev->print_info(); - } - } - - return 0; -} - -/** - * Calculate actual MSL pressure given current altitude - */ -int -calibrate(unsigned altitude, enum MS5611_BUS busid) -{ - struct ms5611_bus_option bus; - - if (!find_bus(busid, bus)) { - return 1; - } - - struct baro_report report; - - float pressure; - - float p1; - - int fd; - - fd = px4_open(bus.devpath, O_RDONLY); - - if (fd < 0) { - warn("open failed (try 'ms5611 start' if the driver is not running)"); - return 1; - } - - /* start the sensor polling at max */ - if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX)) { - warnx("failed to set poll rate"); - return 1; - } - - /* average a few measurements */ - pressure = 0.0f; - - for (unsigned i = 0; i < 20; i++) { - struct pollfd fds; - int ret; - ssize_t sz; - - /* wait for data to be ready */ - fds.fd = fd; - fds.events = POLLIN; - ret = poll(&fds, 1, 1000); - - if (ret != 1) { - warnx("timed out waiting for sensor data"); - return 1; - } - - /* now go get it */ - sz = px4_read(fd, &report, sizeof(report)); - - if (sz != sizeof(report)) { - warn("sensor read failed"); - return 1; - } - - pressure += report.pressure; - } - - pressure /= 20; /* average */ - pressure /= 10; /* scale from millibar to kPa */ - - /* tropospheric properties (0-11km) for standard atmosphere */ - const float T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */ - const float a = -6.5 / 1000; /* temperature gradient in degrees per metre */ - const float g = 9.80665f; /* gravity constant in m/s/s */ - const float R = 287.05f; /* ideal gas constant in J/kg/K */ - - warnx("averaged pressure %10.4fkPa at %um", (double)pressure, altitude); - - p1 = pressure * (powf(((T1 + (a * (float)altitude)) / T1), (g / (a * R)))); - - warnx("calculated MSL pressure %10.4fkPa", (double)p1); - - /* save as integer Pa */ - p1 *= 1000.0f; - - if (px4_ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK) { - warn("BAROIOCSMSLPRESSURE"); - return 1; - } - - close(fd); - return 0; -} - -void -usage() -{ - warnx("missing command: try 'start', 'info', 'test', 'test2', 'reset', 'calibrate'"); - warnx("options:"); - warnx(" -X (external I2C bus)"); - warnx(" -I (intternal I2C bus)"); - warnx(" -S (Simulation bus)"); -} - -} // namespace - -int -ms5611_main(int argc, char *argv[]) -{ - enum MS5611_BUS busid = MS5611_BUS_ALL; - int ch; - int ret; - - if (argc < 2) { - ms5611::usage(); - return 1; - } - - /* jump over start/off/etc and look at options first */ - int myoptind = 1; - const char *myoptarg = NULL; - - while ((ch = px4_getopt(argc, argv, "XIS", &myoptind, &myoptarg)) != EOF) { - printf("ch = %d\n", ch); - - switch (ch) { - case 'X': - busid = MS5611_BUS_I2C_EXTERNAL; - break; - - case 'I': - busid = MS5611_BUS_I2C_INTERNAL; - break; - - case 'S': - busid = MS5611_BUS_SIM_EXTERNAL; - break; - - default: - ms5611::usage(); - return 1; - } - } - - const char *verb = argv[myoptind]; - - /* - * Start/load the driver. - */ - if (!strcmp(verb, "start")) { - ret = ms5611::start(busid); - } - - /* - * Test the driver/device. - */ - else if (!strcmp(verb, "test")) { - ret = ms5611::test(busid); - } - - /* - * Reset the driver. - */ - else if (!strcmp(verb, "reset")) { - ret = ms5611::reset(busid); - } - - /* - * Print driver information. - */ - else if (!strcmp(verb, "info")) { - ret = ms5611::info(); - } - - /* - * Perform MSL pressure calibration given an altitude in metres - */ - else if (!strcmp(verb, "calibrate")) { - if (argc < 2) { - PX4_WARN("missing altitude"); - } - - long altitude = strtol(argv[optind + 1], nullptr, 10); - - ret = ms5611::calibrate(altitude, busid); - - } else { - ms5611::usage(); - warnx("unrecognised command, try 'start', 'test', 'reset' or 'info'"); - return 1; - } - - return ret; -} diff --git a/src/drivers/ms5611/ms5611_sim.cpp b/src/drivers/ms5611/ms5611_sim.cpp deleted file mode 100644 index 72c02f6b38..0000000000 --- a/src/drivers/ms5611/ms5611_sim.cpp +++ /dev/null @@ -1,206 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file ms5611_i2c.cpp - * - * SIM interface for MS5611 - */ - -/* XXX trim includes */ -#include -#include - -#include -#include -#include -#include -//#include -#include -#include - -#include -#include "ms5611.h" -#include "board_config.h" - -device::Device *MS5611_sim_interface(ms5611::prom_u &prom_buf); - -class MS5611_SIM : public device::SIM -{ -public: - MS5611_SIM(uint8_t bus, ms5611::prom_u &prom_buf); - virtual ~MS5611_SIM(); - - virtual int init(); - virtual int dev_read(unsigned offset, void *data, unsigned count); - virtual int dev_ioctl(unsigned operation, unsigned &arg); - - virtual int transfer(const uint8_t *send, unsigned send_len, - uint8_t *recv, unsigned recv_len); -private: - ms5611::prom_u &_prom; - - /** - * Send a reset command to the MS5611. - * - * This is required after any bus reset. - */ - int _reset(); - - /** - * Send a measure command to the MS5611. - * - * @param addr Which address to use for the measure operation. - */ - int _measure(unsigned addr); - - /** - * Read the MS5611 PROM - * - * @return PX4_OK if the PROM reads successfully. - */ - int _read_prom(); - -}; - -device::Device * -MS5611_sim_interface(ms5611::prom_u &prom_buf, uint8_t busnum) -{ - return new MS5611_SIM(busnum, prom_buf); -} - -MS5611_SIM::MS5611_SIM(uint8_t bus, ms5611::prom_u &prom) : - SIM("MS5611_SIM", "/dev/MS5611_SIM", bus, 0), - _prom(prom) -{ -} - -MS5611_SIM::~MS5611_SIM() -{ -} - -int -MS5611_SIM::init() -{ - return SIM::init(); -} - -int -MS5611_SIM::dev_read(unsigned offset, void *data, unsigned count) -{ - union _cvt { - uint8_t b[4]; - uint32_t w; - } *cvt = (_cvt *)data; - uint8_t buf[3]; - - /* read the most recent measurement */ - uint8_t cmd = 0; - int ret = transfer(&cmd, 1, &buf[0], 3); - - if (ret == PX4_OK) { - /* fetch the raw value */ - cvt->b[0] = buf[2]; - cvt->b[1] = buf[1]; - cvt->b[2] = buf[0]; - cvt->b[3] = 0; - } - - return ret; -} - -int -MS5611_SIM::dev_ioctl(unsigned operation, unsigned &arg) -{ - int ret; - - switch (operation) { - case IOCTL_RESET: - ret = _reset(); - break; - - case IOCTL_MEASURE: - ret = _measure(arg); - break; - - default: - ret = EINVAL; - } - - return ret; -} - -int -MS5611_SIM::_reset() -{ - unsigned old_retrycount = _retries; - uint8_t cmd = ADDR_RESET_CMD; - int result; - - /* bump the retry count */ - _retries = 10; - result = transfer(&cmd, 1, nullptr, 0); - _retries = old_retrycount; - - return result; -} - -int -MS5611_SIM::_measure(unsigned addr) -{ - /* - * Disable retries on this command; we can't know whether failure - * means the device did or did not see the command. - */ - _retries = 0; - - uint8_t cmd = addr; - return transfer(&cmd, 1, nullptr, 0); -} - -int -MS5611_SIM::_read_prom() -{ - int ret = 1; - // TODO input simlation data - return ret; -} - -int -MS5611_SIM::transfer(const uint8_t *send, unsigned send_len, - uint8_t *recv, unsigned recv_len) -{ - // TODO add Simulation data connection so calls retrieve - // data from the simulator - return 0; -} diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 5fe0fc501d..c10b71be3a 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -152,11 +152,6 @@ MS5611_SPI::init() goto out; } - /* sharing a bus with NuttX drivers */ -#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4) - //set_lockmode(SPI::LOCK_THREADS); -#endif - /* send reset command */ ret = _reset(); diff --git a/src/modules/muorb/adsp/module.mk b/src/drivers/navio_sysfs_pwm_out/CMakeLists.txt similarity index 85% rename from src/modules/muorb/adsp/module.mk rename to src/drivers/navio_sysfs_pwm_out/CMakeLists.txt index d6ff5a74df..36841dbe6d 100644 --- a/src/modules/muorb/adsp/module.mk +++ b/src/drivers/navio_sysfs_pwm_out/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. +# Copyright (c) 2015-2016 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,23 +30,14 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ - -# -# Makefile to build muorb -# - - -ifeq ($(PX4_TARGET_OS),qurt) - -SRCS = \ - px4muorb.cpp \ - uORBFastRpcChannel.cpp -endif - - -INCLUDE_DIRS += \ - ${PX4_BASE}/src/modules/uORB - -endif - -MAXOPTIMIZATION = -Os +px4_add_module( + MODULE drivers__navio_sysfs_pwm_out + MAIN navio_sysfs_pwm_out + COMPILE_FLAGS + -Os + SRCS + navio_sysfs_pwm_out.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/navio_sysfs_pwm_out/navio_sysfs_pwm_out.cpp b/src/drivers/navio_sysfs_pwm_out/navio_sysfs_pwm_out.cpp new file mode 100644 index 0000000000..2f794ceffe --- /dev/null +++ b/src/drivers/navio_sysfs_pwm_out/navio_sysfs_pwm_out.cpp @@ -0,0 +1,500 @@ +/**************************************************************************** + * + * Copyright (c) 2015-2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace navio_sysfs_pwm_out +{ +static px4_task_t _task_handle = -1; +volatile bool _task_should_exit = false; +static bool _is_running = false; + +static const int NUM_PWM = 4; +static char _device[32] = "/sys/class/pwm/pwmchip0"; +static int _pwm_fd[NUM_PWM]; + +static const int FREQUENCY_PWM = 400; +static const char *MIXER_FILENAME = ""; + +// subscriptions +int _controls_sub; +int _armed_sub; + +// publications +orb_advert_t _outputs_pub = nullptr; +orb_advert_t _rc_pub = nullptr; + +// topic structures +actuator_controls_s _controls; +actuator_outputs_s _outputs; +actuator_armed_s _armed; + +pwm_limit_t _pwm_limit; + +// esc parameters +int32_t _pwm_disarmed; +int32_t _pwm_min; +int32_t _pwm_max; + +MultirotorMixer *_mixer = nullptr; + +void usage(); + +void start(); + +void stop(); + +int pwm_write_sysfs(char *path, int value); + +int pwm_initialize(const char *device); + +void pwm_deinitialize(); + +void send_outputs_pwm(const uint16_t *pwm); + +void task_main_trampoline(int argc, char *argv[]); + +void task_main(int argc, char *argv[]); + +/* mixer initialization */ +int initialize_mixer(const char *mixer_filename); +int mixer_control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input); + + +int mixer_control_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &input) +{ + const actuator_controls_s *controls = (actuator_controls_s *)handle; + + input = controls[control_group].control[control_index]; + + return 0; +} + + +int initialize_mixer(const char *mixer_filename) +{ + char buf[2048]; + size_t buflen = sizeof(buf); + + PX4_INFO("Trying to initialize mixer from config file %s", mixer_filename); + + int fd_load = ::open(mixer_filename, O_RDONLY); + + if (fd_load != -1) { + int nRead = ::read(fd_load, buf, buflen); + close(fd_load); + + if (nRead > 0) { + _mixer = MultirotorMixer::from_text(mixer_control_callback, (uintptr_t)&_controls, buf, buflen); + + if (_mixer != nullptr) { + PX4_INFO("Successfully initialized mixer from config file"); + return 0; + + } else { + PX4_ERR("Unable to parse from mixer config file"); + return -1; + } + + } else { + PX4_WARN("Unable to read from mixer config file"); + return -2; + } + + } else { + PX4_WARN("No mixer config file found, using default mixer."); + + /* Mixer file loading failed, fall back to default mixer configuration for + * QUAD_X airframe. */ + float roll_scale = 1; + float pitch_scale = 1; + float yaw_scale = 1; + float deadband = 0; + + _mixer = new MultirotorMixer(mixer_control_callback, (uintptr_t)&_controls, + MultirotorGeometry::QUAD_X, + roll_scale, pitch_scale, yaw_scale, deadband); + + // TODO: temporary hack to make this compile + (void)_config_index[0]; + + if (_mixer == nullptr) { + PX4_ERR("Mixer initialization failed"); + return -1; + } + + return 0; + } +} + +int pwm_write_sysfs(char *path, int value) +{ + int fd = ::open(path, O_WRONLY | O_CLOEXEC); + int n; + char *data; + + ::free(path); + + if (fd == -1) { + return -errno; + } + + n = ::asprintf(&data, "%u", value); + + if (n > 0) { + ::write(fd, data, n); + ::free(data); + } + + ::close(fd); + + return 0; +} + +int pwm_initialize(const char *device) +{ + int i; + char *path; + + for (i = 0; i < NUM_PWM; ++i) { + ::asprintf(&path, "%s/export", device); + + if (pwm_write_sysfs(path, i) < 0) { + PX4_ERR("PWM export failed"); + } + } + + for (i = 0; i < NUM_PWM; ++i) { + ::asprintf(&path, "%s/pwm%u/enable", device, i); + + if (pwm_write_sysfs(path, 1) < 0) { + PX4_ERR("PWM enable failed"); + } + } + + for (i = 0; i < NUM_PWM; ++i) { + ::asprintf(&path, "%s/pwm%u/period", device, i); + + if (pwm_write_sysfs(path, (int)1e9 / FREQUENCY_PWM)) { + PX4_ERR("PWM period failed"); + } + } + + for (i = 0; i < NUM_PWM; ++i) { + ::asprintf(&path, "%s/pwm%u/duty_cycle", device, i); + _pwm_fd[i] = ::open(path, O_WRONLY | O_CLOEXEC); + ::free(path); + + if (_pwm_fd[i] == -1) { + PX4_ERR("PWM: Failed to open duty_cycle."); + return -errno; + } + } + + return 0; +} + +void pwm_deinitialize() +{ + for (int i = 0; i < NUM_PWM; ++i) { + if (_pwm_fd[i] != -1) { + ::close(_pwm_fd[i]); + } + } +} + +void send_outputs_pwm(const uint16_t *pwm) +{ + int n; + char *data; + + //convert this to duty_cycle in ns + for (unsigned i = 0; i < NUM_PWM; ++i) { + n = ::asprintf(&data, "%u", pwm[i] * 1000); + ::write(_pwm_fd[i], data, n); + } +} + + + +void task_main(int argc, char *argv[]) +{ + _is_running = true; + + if (pwm_initialize(_device) < 0) { + PX4_ERR("Failed to initialize PWM."); + return; + } + + // Subscribe for orb topics + _controls_sub = orb_subscribe(ORB_ID(actuator_controls_0)); + _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + + // Start disarmed + _armed.armed = false; + _armed.prearmed = false; + + // Set up poll topic + px4_pollfd_struct_t fds[1]; + fds[0].fd = _controls_sub; + fds[0].events = POLLIN; + /* Don't limit poll intervall for now, 250 Hz should be fine. */ + //orb_set_interval(_controls_sub, 10); + + // Set up mixer + if (initialize_mixer(MIXER_FILENAME) < 0) { + PX4_ERR("Mixer initialization failed."); + return; + } + + pwm_limit_init(&_pwm_limit); + + // Main loop + while (!_task_should_exit) { + + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 10); + + /* Timed out, do a periodic check for _task_should_exit. */ + if (pret == 0) { + continue; + } + + /* This is undesirable but not much we can do. */ + if (pret < 0) { + PX4_WARN("poll error %d, %d", pret, errno); + /* sleep a bit before next try */ + usleep(100000); + continue; + } + + if (fds[0].revents & POLLIN) { + orb_copy(ORB_ID(actuator_controls_0), _controls_sub, &_controls); + + _outputs.timestamp = _controls.timestamp; + + /* do mixing */ + _outputs.noutputs = _mixer->mix(_outputs.output, + 0 /* not used */, + NULL); + + + /* disable unused ports by setting their output to NaN */ + for (size_t i = _outputs.noutputs; + i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); + i++) { + _outputs.output[i] = NAN; + } + + const uint16_t reverse_mask = 0; + uint16_t disarmed_pwm[4]; + uint16_t min_pwm[4]; + uint16_t max_pwm[4]; + + for (unsigned int i = 0; i < 4; i++) { + disarmed_pwm[i] = _pwm_disarmed; + min_pwm[i] = _pwm_min; + max_pwm[i] = _pwm_max; + } + + uint16_t pwm[4]; + + // TODO FIXME: pre-armed seems broken + pwm_limit_calc(_armed.armed, + false/*_armed.prearmed*/, + _outputs.noutputs, + reverse_mask, + disarmed_pwm, + min_pwm, + max_pwm, + _outputs.output, + pwm, + &_pwm_limit); + + send_outputs_pwm(pwm); + + if (_outputs_pub != nullptr) { + orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs); + + } else { + _outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs); + } + } + + bool updated; + orb_check(_armed_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); + } + } + + pwm_deinitialize(); + orb_unsubscribe(_controls_sub); + orb_unsubscribe(_armed_sub); + + _is_running = false; + +} + +void task_main_trampoline(int argc, char *argv[]) +{ + task_main(argc, argv); +} + +void start() +{ + ASSERT(_task_handle == -1); + + _task_should_exit = false; + + /* start the task */ + _task_handle = px4_task_spawn_cmd("pwm_out_main", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX, + 1500, + (px4_main_t)&task_main_trampoline, + nullptr); + + if (_task_handle < 0) { + warn("task start failed"); + return; + } + +} + +void stop() +{ + _task_should_exit = true; + + while (_is_running) { + usleep(200000); + PX4_INFO("."); + } + + _task_handle = -1; +} + +void usage() +{ + PX4_INFO("usage: pwm_out start -d /sys/class/pwm/pwmchip0"); + PX4_INFO(" pwm_out stop"); + PX4_INFO(" pwm_out status"); +} + +} // namespace navio_sysfs_pwm_out + +/* driver 'main' command */ +extern "C" __EXPORT int navio_sysfs_pwm_out_main(int argc, char *argv[]); + +int navio_sysfs_pwm_out_main(int argc, char *argv[]) +{ + const char *device = nullptr; + int ch; + int myoptind = 1; + const char *myoptarg = nullptr; + + char *verb = nullptr; + + if (argc >= 2) { + verb = argv[1]; + + } else { + return 1; + } + + while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'd': + device = myoptarg; + strncpy(navio_sysfs_pwm_out::_device, device, strlen(device)); + break; + } + } + + // gets the parameters for the esc's pwm + param_get(param_find("PWM_DISARMED"), &navio_sysfs_pwm_out::_pwm_disarmed); + param_get(param_find("PWM_MIN"), &navio_sysfs_pwm_out::_pwm_min); + param_get(param_find("PWM_MAX"), &navio_sysfs_pwm_out::_pwm_max); + + /* + * Start/load the driver. + */ + if (!strcmp(verb, "start")) { + if (navio_sysfs_pwm_out::_is_running) { + PX4_WARN("pwm_out already running"); + return 1; + } + + navio_sysfs_pwm_out::start(); + } + + else if (!strcmp(verb, "stop")) { + if (!navio_sysfs_pwm_out::_is_running) { + PX4_WARN("pwm_out is not running"); + return 1; + } + + navio_sysfs_pwm_out::stop(); + } + + else if (!strcmp(verb, "status")) { + PX4_WARN("pwm_out is %s", navio_sysfs_pwm_out::_is_running ? "running" : "not running"); + return 0; + + } else { + navio_sysfs_pwm_out::usage(); + return 1; + } + + return 0; +} diff --git a/src/drivers/navio_sysfs_rc_in/CMakeLists.txt b/src/drivers/navio_sysfs_rc_in/CMakeLists.txt new file mode 100644 index 0000000000..03910a27ff --- /dev/null +++ b/src/drivers/navio_sysfs_rc_in/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2016 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. +# +############################################################################ +px4_add_module( + MODULE drivers__navio_sysfs_rc_in + MAIN navio_sysfs_rc_in + STACK_MAIN 1200 + COMPILE_FLAGS -Os + SRCS + navio_sysfs_rc_in.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/navio_sysfs_rc_in/navio_sysfs_rc_in.cpp b/src/drivers/navio_sysfs_rc_in/navio_sysfs_rc_in.cpp new file mode 100644 index 0000000000..598f354ba4 --- /dev/null +++ b/src/drivers/navio_sysfs_rc_in/navio_sysfs_rc_in.cpp @@ -0,0 +1,304 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * Copyright (C) 2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include + +namespace navio_sysfs_rc_in +{ + +extern "C" __EXPORT int navio_sysfs_rc_in_main(int argc, char *argv[]); + +#define RCINPUT_DEVICE_PATH_BASE "/sys/kernel/rcio/rcin" + +#define RCINPUT_MEASURE_INTERVAL_US 20000 // microseconds + + +class RcInput +{ +public: + RcInput() : + _shouldExit(false), + _isRunning(false), + _work{}, + _rcinput_pub(nullptr), + _channels(8), //D8R-II plus + _data{} + { + memset(_ch_fd, 0, sizeof(_ch_fd)); + } + ~RcInput() + { + work_cancel(HPWORK, &_work); + _isRunning = false; + } + + /* @return 0 on success, -errno on failure */ + int start(); + + /* @return 0 on success, -errno on failure */ + void stop(); + + /* Trampoline for the work queue. */ + static void cycle_trampoline(void *arg); + + bool isRunning() { return _isRunning; } + +private: + void _cycle(); + void _measure(); + + bool _shouldExit; + bool _isRunning; + struct work_s _work; + + orb_advert_t _rcinput_pub; + + int _channels; + int _ch_fd[input_rc_s::RC_INPUT_MAX_CHANNELS]; + struct input_rc_s _data; + + int navio_rc_init(); +}; + +int RcInput::navio_rc_init() +{ + int i; + char *buf; + + for (i = 0; i < _channels; ++i) { + ::asprintf(&buf, "%s/ch%d", RCINPUT_DEVICE_PATH_BASE, i); + int fd = ::open(buf, O_RDONLY); + ::free(buf); + + if (fd < 0) { + PX4_WARN("error: open %d failed", i); + break; + } + + _ch_fd[i] = fd; + } + + for (; i < input_rc_s::RC_INPUT_MAX_CHANNELS; ++i) { + _data.values[i] = UINT16_MAX; + } + + _rcinput_pub = orb_advertise(ORB_ID(input_rc), &_data); + + if (_rcinput_pub == nullptr) { + PX4_WARN("error: advertise failed"); + return -1; + } + + return 0; +} + +int RcInput::start() +{ + int result = 0; + + result = navio_rc_init(); + + if (result != 0) { + PX4_WARN("error: RC initialization failed"); + return -1; + } + + _isRunning = true; + result = work_queue(HPWORK, &_work, (worker_t)&RcInput::cycle_trampoline, this, 0); + + if (result == -1) { + _isRunning = false; + } + + return result; +} + +void RcInput::stop() +{ + _shouldExit = true; +} + +void RcInput::cycle_trampoline(void *arg) +{ + RcInput *dev = reinterpret_cast(arg); + dev->_cycle(); +} + +void RcInput::_cycle() +{ + _measure(); + + if (!_shouldExit) { + work_queue(HPWORK, &_work, (worker_t)&RcInput::cycle_trampoline, this, + USEC2TICK(RCINPUT_MEASURE_INTERVAL_US)); + } +} + +void RcInput::_measure(void) +{ + uint64_t ts; + char buf[12]; + + for (int i = 0; i < _channels; ++i) { + int res; + + if ((res = ::pread(_ch_fd[i], buf, sizeof(buf) - 1, 0)) < 0) { + _data.values[i] = UINT16_MAX; + continue; + } + + buf[sizeof(buf) - 1] = '\0'; + + _data.values[i] = atoi(buf); + } + + ts = hrt_absolute_time(); + _data.timestamp_publication = ts; + _data.timestamp_last_signal = ts; + _data.channel_count = _channels; + _data.rssi = 100; + _data.rc_lost_frame_count = 0; + _data.rc_total_frame_count = 1; + _data.rc_ppm_frame_length = 100; + _data.rc_failsafe = false; + _data.rc_lost = false; + _data.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_PPM; + + orb_publish(ORB_ID(input_rc), _rcinput_pub, &_data); +} + +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +static void +usage(const char *reason) +{ + if (reason) { + PX4_ERR("%s", reason); + } + + PX4_INFO("usage: navio_sysfs_rc_in {start|stop|status}"); +} + +static RcInput *rc_input = nullptr; + +int navio_sysfs_rc_in_main(int argc, char *argv[]) +{ + if (argc < 2) { + usage("missing command"); + return 1; + } + + if (!strcmp(argv[1], "start")) { + + if (rc_input != nullptr && rc_input->isRunning()) { + PX4_WARN("already running"); + /* this is not an error */ + return 0; + } + + rc_input = new RcInput(); + + // Check if alloc worked. + if (rc_input == nullptr) { + PX4_ERR("alloc failed"); + return -1; + } + + int ret = rc_input->start(); + + if (ret != 0) { + PX4_ERR("start failed"); + } + + return 0; + } + + if (!strcmp(argv[1], "stop")) { + + if (rc_input == nullptr || rc_input->isRunning()) { + PX4_WARN("not running"); + /* this is not an error */ + return 0; + } + + rc_input->stop(); + + // Wait for task to die + int i = 0; + + do { + /* wait up to 3s */ + usleep(100000); + + } while (rc_input->isRunning() && ++i < 30); + + delete rc_input; + rc_input = nullptr; + + return 0; + } + + if (!strcmp(argv[1], "status")) { + if (rc_input != nullptr && rc_input->isRunning()) { + PX4_INFO("running"); + + } else { + PX4_INFO("not running\n"); + } + + return 0; + } + + usage("unrecognized command"); + return 1; + +} + +}; // namespace navio_sysfs_rc_in diff --git a/src/drivers/oreoled/oreoled.cpp b/src/drivers/oreoled/oreoled.cpp index 5ee8dc030b..bbf96b98f4 100644 --- a/src/drivers/oreoled/oreoled.cpp +++ b/src/drivers/oreoled/oreoled.cpp @@ -35,7 +35,7 @@ /** * @file oreoled.cpp * - * Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C. + * Driver for oreoled ESCs found in solo, connected via I2C. * */ @@ -53,6 +53,7 @@ #include #include #include +#include #include #include @@ -69,7 +70,8 @@ #define OREOLED_NUM_LEDS 4 ///< maximum number of LEDs the oreo led driver can support #define OREOLED_BASE_I2C_ADDR 0x68 ///< base i2c address (7-bit) -#define OREOLED_TIMEOUT_MS 10000000U ///< timeout looking for battery 10seconds after startup +#define OPEOLED_I2C_RETRYCOUNT 2 ///< i2c retry count +#define OREOLED_TIMEOUT_USEC 2000000U ///< timeout looking for oreoleds 2 seconds after startup #define OREOLED_GENERALCALL_US 4000000U ///< general call sent every 4 seconds #define OREOLED_GENERALCALL_CMD 0x00 ///< general call command sent at regular intervals @@ -81,7 +83,7 @@ class OREOLED : public device::I2C { public: - OREOLED(int bus, int i2c_addr); + OREOLED(int bus, int i2c_addr, bool autoupdate, bool alwaysupdate); virtual ~OREOLED(); virtual int init(); @@ -95,6 +97,9 @@ public: /* send cmd to an LEDs (used for testing only) */ int send_cmd(oreoled_cmd_t sb); + /* returns true once the driver finished bootloading and ready for commands */ + bool is_ready(); + private: /** @@ -117,13 +122,40 @@ private: */ void cycle(); + int bootloader_app_reset(int led_num); + int bootloader_app_ping(int led_num); + uint16_t bootloader_inapp_checksum(int led_num); + int bootloader_ping(int led_num); + uint8_t bootloader_version(int led_num); + uint16_t bootloader_app_version(int led_num); + uint16_t bootloader_app_checksum(int led_num); + int bootloader_set_colour(int led_num, uint8_t red, uint8_t green); + int bootloader_flash(int led_num); + int bootloader_boot(int led_num); + uint16_t bootloader_fw_checksum(void); + int bootloader_coerce_healthy(void); + /* internal variables */ work_s _work; ///< work queue for scheduling reads bool _healthy[OREOLED_NUM_LEDS]; ///< health of each LED + bool _in_boot[OREOLED_NUM_LEDS]; ///< true for each LED that is in bootloader mode uint8_t _num_healthy; ///< number of healthy LEDs + uint8_t _num_inboot; ///< number of LEDs in bootloader ringbuffer::RingBuffer *_cmd_queue; ///< buffer of commands to send to LEDs uint64_t _last_gencall; uint64_t _start_time; ///< system time we first attempt to communicate with battery + bool _autoupdate; ///< true if the driver should update all LEDs + bool _alwaysupdate; ///< true if the driver should update all LEDs + bool _is_bootloading; ///< true if a bootloading operation is in progress + bool _is_ready; ///< set to true once the driver has completly initialised + uint16_t _fw_checksum; ///< the current 16bit XOR checksum of the built in oreoled firmware binary + + /* performance checking */ + perf_counter_t _call_perf; + perf_counter_t _gcall_perf; + perf_counter_t _probe_perf; + perf_counter_t _comms_errors; + perf_counter_t _reply_errors; }; /* for now, we only support one OREOLED */ @@ -137,16 +169,30 @@ void oreoled_usage(); extern "C" __EXPORT int oreoled_main(int argc, char *argv[]); /* constructor */ -OREOLED::OREOLED(int bus, int i2c_addr) : +OREOLED::OREOLED(int bus, int i2c_addr, bool autoupdate, bool alwaysupdate) : I2C("oreoled", OREOLED0_DEVICE_PATH, bus, i2c_addr, 100000), _work{}, _num_healthy(0), + _num_inboot(0), _cmd_queue(nullptr), - _last_gencall(0) + _last_gencall(0), + _autoupdate(autoupdate), + _alwaysupdate(alwaysupdate), + _is_bootloading(false), + _is_ready(false), + _fw_checksum(0x0000), + _call_perf(perf_alloc(PC_ELAPSED, "oreoled_call")), + _gcall_perf(perf_alloc(PC_ELAPSED, "oreoled_gcall")), + _probe_perf(perf_alloc(PC_ELAPSED, "oreoled_probe")), + _comms_errors(perf_alloc(PC_COUNT, "oreoled_comms_errors")), + _reply_errors(perf_alloc(PC_COUNT, "oreoled_reply_errors")) { /* initialise to unhealthy */ memset(_healthy, 0, sizeof(_healthy)); + /* initialise to in application */ + memset(_in_boot, 0, sizeof(_in_boot)); + /* capture startup time */ _start_time = hrt_absolute_time(); } @@ -161,6 +207,13 @@ OREOLED::~OREOLED() if (_cmd_queue != nullptr) { delete _cmd_queue; } + + /* free perf counters */ + perf_free(_call_perf); + perf_free(_gcall_perf); + perf_free(_probe_perf); + perf_free(_comms_errors); + perf_free(_reply_errors); } int @@ -192,6 +245,9 @@ OREOLED::init() int OREOLED::probe() { + /* set retry count */ + _retries = OPEOLED_I2C_RETRYCOUNT; + /* always return true */ return OK; } @@ -202,13 +258,20 @@ OREOLED::info() /* print health info on each LED */ for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { if (!_healthy[i]) { - DEVICE_LOG("oreo %u: BAD", (int)i); + DEVICE_LOG("oreo %u: BAD", (unsigned)i); } else { - DEVICE_LOG("oreo %u: OK", (int)i); + DEVICE_LOG("oreo %u: OK", (unsigned)i); } } + /* display perf info */ + perf_print_counter(_call_perf); + perf_print_counter(_gcall_perf); + perf_print_counter(_probe_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_reply_errors); + return OK; } @@ -241,31 +304,67 @@ OREOLED::cycle() { /* check time since startup */ uint64_t now = hrt_absolute_time(); - bool startup_timeout = (now - _start_time > OREOLED_TIMEOUT_MS); + bool startup_timeout = (now - _start_time > OREOLED_TIMEOUT_USEC); - /* if not leds found during start-up period, exit without rescheduling */ - if (startup_timeout && _num_healthy == 0) { - warnx("did not find oreoled"); - return; - } + /* prepare the response buffer */ + uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; /* during startup period keep searching for unhealthy LEDs */ if (!startup_timeout && _num_healthy < OREOLED_NUM_LEDS) { - /* prepare command to turn off LED*/ - uint8_t msg[] = {OREOLED_PATTERN_OFF}; + /* prepare command to turn off LED */ + /* add two bytes of pre-amble to for higher signal to noise ratio */ + uint8_t msg[] = {0xAA, 0x55, OREOLED_PATTERN_OFF, 0x00}; /* attempt to contact each unhealthy LED */ for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { if (!_healthy[i]) { + perf_begin(_probe_perf); + /* set I2C address */ set_address(OREOLED_BASE_I2C_ADDR + i); - /* send I2C command and record health*/ - if (transfer(msg, sizeof(msg), nullptr, 0) == OK) { - _healthy[i] = true; - _num_healthy++; - warnx("oreoled %d ok", (unsigned)i); + /* Calculate XOR CRC and append to the i2c write data */ + msg[sizeof(msg) - 1] = OREOLED_BASE_I2C_ADDR + i; + + for (uint8_t j = 0; j < sizeof(msg) - 1; j++) { + msg[sizeof(msg) - 1] ^= msg[j]; } + + /* send I2C command */ + if (transfer(msg, sizeof(msg), reply, 3) == OK) { + if (reply[1] == OREOLED_BASE_I2C_ADDR + i && + reply[2] == msg[sizeof(msg) - 1]) { + DEVICE_LOG("oreoled %u ok - in bootloader", (unsigned)i); + _healthy[i] = true; + _num_healthy++; + + /* If slaves are in application record that so we can reset if we need to bootload */ + /* This additional check is required for LED firmwares below v1.3 and can be + deprecated once all LEDs in the wild have firmware >= v1.3 */ + if (bootloader_ping(i) == OK) { + _in_boot[i] = true; + _num_inboot++; + } + + /* Check for a reply with a checksum offset of 1, + which indicates a response from firmwares >= 1.3 */ + + } else if (reply[1] == OREOLED_BASE_I2C_ADDR + i && + reply[2] == msg[sizeof(msg) - 1] + 1) { + DEVICE_LOG("oreoled %u ok - in application", (unsigned)i); + _healthy[i] = true; + _num_healthy++; + + } else { + DEVICE_LOG("oreo reply errors: %u", (unsigned)_reply_errors); + perf_count(_reply_errors); + } + + } else { + perf_count(_comms_errors); + } + + perf_end(_probe_perf); } } @@ -273,6 +372,121 @@ OREOLED::cycle() work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this, USEC2TICK(OREOLED_STARTUP_INTERVAL_US)); return; + + } else if (_alwaysupdate) { + /* reset each healthy LED */ + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i] && !_in_boot[i]) { + /* reset the LED if it's not in the bootloader */ + /* (this happens during a pixhawk OTA update, since the LEDs stay powered) */ + bootloader_app_reset(i); + } + } + + /* attempt to update each healthy LED */ + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i] && _in_boot[i]) { + /* flash the new firmware */ + bootloader_flash(i); + } + } + + /* boot each healthy LED */ + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i] && _in_boot[i]) { + /* boot the application */ + bootloader_boot(i); + } + } + + /* coerce LEDs with startup issues to be healthy again */ + bootloader_coerce_healthy(); + + /* mandatory updating has finished */ + _alwaysupdate = false; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this, + USEC2TICK(OREOLED_UPDATE_INTERVAL_US)); + return; + + } else if (_autoupdate) { + /* check booted oreoleds to see if the app can report it's checksum (release versions >= v1.2) */ + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i] && !_in_boot[i]) { + /* put any out of date oreoleds into bootloader mode */ + /* being in bootloader mode signals to be code below that the will likey need updating */ + if (bootloader_inapp_checksum(i) != bootloader_fw_checksum()) { + bootloader_app_reset(i); + } + } + } + + /* reset all healthy oreoleds if the number of outdated oreoled's is > 0 */ + /* this is done for consistency, so if only one oreoled is updating, all LEDs show the same behaviour */ + /* otherwise a single oreoled could appear broken or failed. */ + if (_num_inboot > 0) { + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i] && !_in_boot[i]) { + /* reset the LED if it's not in the bootloader */ + /* (this happens during a pixhawk OTA update, since the LEDs stay powered) */ + bootloader_app_reset(i); + } + } + + /* update each outdated and healthy LED in bootloader mode */ + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i] && _in_boot[i]) { + /* only flash LEDs with an old version of the applictioon */ + if (bootloader_app_checksum(i) != bootloader_fw_checksum()) { + /* flash the new firmware */ + bootloader_flash(i); + } + } + } + + /* boot each healthy LED */ + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i] && _in_boot[i]) { + /* boot the application */ + bootloader_boot(i); + } + } + + /* coerce LEDs with startup issues to be healthy again */ + bootloader_coerce_healthy(); + } + + /* auto updating has finished */ + _autoupdate = false; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this, + USEC2TICK(OREOLED_UPDATE_INTERVAL_US)); + return; + + } else if (_num_inboot > 0) { + /* boot any LEDs which are in still in bootloader mode */ + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_in_boot[i]) { + bootloader_boot(i); + } + } + + /* coerce LEDs with startup issues to be healthy again */ + bootloader_coerce_healthy(); + + /* ensure we don't get stuck in a loop */ + _num_inboot = 0; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this, + USEC2TICK(OREOLED_UPDATE_INTERVAL_US)); + return; + + } else if (!_is_ready) { + /* indicate a ready state since startup has finished */ + _is_ready = true; } /* get next command from queue */ @@ -282,23 +496,856 @@ OREOLED::cycle() /* send valid messages to healthy LEDs */ if ((next_cmd.led_num < OREOLED_NUM_LEDS) && _healthy[next_cmd.led_num] && (next_cmd.num_bytes <= OREOLED_CMD_LENGTH_MAX)) { + /* start performance timer */ + perf_begin(_call_perf); + /* set I2C address */ set_address(OREOLED_BASE_I2C_ADDR + next_cmd.led_num); - /* send I2C command */ - transfer(next_cmd.buff, next_cmd.num_bytes, nullptr, 0); + + /* Calculate XOR CRC and append to the i2c write data */ + uint8_t next_cmd_xor = OREOLED_BASE_I2C_ADDR + next_cmd.led_num; + + for (uint8_t i = 0; i < next_cmd.num_bytes; i++) { + next_cmd_xor ^= next_cmd.buff[i]; + } + + next_cmd.buff[next_cmd.num_bytes++] = next_cmd_xor; + + /* send I2C command with a retry limit */ + for (uint8_t retry = OEROLED_COMMAND_RETRIES; retry > 0; retry--) { + if (transfer(next_cmd.buff, next_cmd.num_bytes, reply, 3) == OK) { + if (reply[1] == (OREOLED_BASE_I2C_ADDR + next_cmd.led_num) && reply[2] == next_cmd_xor) { + /* slave returned a valid response */ + break; + + } else { + perf_count(_reply_errors); + } + + } else { + perf_count(_comms_errors); + } + } + + perf_end(_call_perf); } } - /* send general call every 4 seconds*/ - if ((now - _last_gencall) > OREOLED_GENERALCALL_US) { + /* send general call every 4 seconds, if we aren't bootloading*/ + if (!_is_bootloading && ((now - _last_gencall) > OREOLED_GENERALCALL_US)) { + perf_begin(_gcall_perf); send_general_call(); + perf_end(_gcall_perf); } - /* schedule a fresh cycle call when the measurement is done */ + /* schedule a fresh cycle call when the command is sent */ work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this, USEC2TICK(OREOLED_UPDATE_INTERVAL_US)); } +int +OREOLED::bootloader_app_reset(int led_num) +{ + _is_bootloading = true; + oreoled_cmd_t boot_cmd; + boot_cmd.led_num = led_num; + + int ret = -1; + + /* Set the current address */ + set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + + /* send a reset */ + boot_cmd.buff[0] = OREOLED_PATTERN_PARAMUPDATE; + boot_cmd.buff[1] = OREOLED_PARAM_RESET; + boot_cmd.buff[2] = OEROLED_RESET_NONCE; + boot_cmd.buff[3] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; + boot_cmd.num_bytes = 4; + + for (uint8_t j = 0; j < boot_cmd.num_bytes - 1; j++) { + boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[j]; + } + + uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; + + /* send I2C command with a retry limit */ + for (uint8_t retry = OEROLED_COMMAND_RETRIES; retry > 0; retry--) { + if (transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 3) == OK) { + if (reply[1] == (OREOLED_BASE_I2C_ADDR + boot_cmd.led_num) && + reply[2] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { + /* slave returned a valid response */ + ret = OK; + /* set this LED as being in boot mode now */ + _in_boot[led_num] = true; + _num_inboot++; + break; + } + } + } + + /* Allow time for the LED to reboot */ + usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); + usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); + usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); + usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); + + _is_bootloading = false; + return ret; +} + +int +OREOLED::bootloader_app_ping(int led_num) +{ + oreoled_cmd_t boot_cmd; + boot_cmd.led_num = led_num; + + int ret = -1; + + /* Set the current address */ + set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + + /* send a pattern off command */ + boot_cmd.buff[0] = 0xAA; + boot_cmd.buff[1] = 0x55; + boot_cmd.buff[2] = OREOLED_PATTERN_OFF; + boot_cmd.buff[3] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; + boot_cmd.num_bytes = 4; + + for (uint8_t j = 0; j < boot_cmd.num_bytes - 1; j++) { + boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[j]; + } + + uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; + + /* send I2C command with a retry limit */ + for (uint8_t retry = OEROLED_COMMAND_RETRIES; retry > 0; retry--) { + if (transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 3) == OK) { + if (reply[1] == (OREOLED_BASE_I2C_ADDR + boot_cmd.led_num) && + reply[2] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { + /* slave returned a valid response */ + ret = OK; + break; + } + } + } + + return ret; +} + +uint16_t +OREOLED::bootloader_inapp_checksum(int led_num) +{ + _is_bootloading = true; + oreoled_cmd_t boot_cmd; + boot_cmd.led_num = led_num; + + uint16_t ret = 0x0000; + + /* Set the current address */ + set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + + boot_cmd.buff[0] = OREOLED_PATTERN_PARAMUPDATE; + boot_cmd.buff[1] = OREOLED_PARAM_APP_CHECKSUM; + boot_cmd.buff[2] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; + boot_cmd.num_bytes = 3; + + for (uint8_t j = 0; j < boot_cmd.num_bytes - 1; j++) { + boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[j]; + } + + uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; + + for (uint8_t retry = OEROLED_COMMAND_RETRIES; retry > 0; retry--) { + /* Send the I2C Write+Read */ + memset(reply, 0, sizeof(reply)); + transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 6); + + /* Check the response */ + if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_PARAM_APP_CHECKSUM && + reply[5] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { + warnx("bl app checksum OK from LED %i", boot_cmd.led_num); + warnx("bl app checksum msb: 0x%x", reply[3]); + warnx("bl app checksum lsb: 0x%x", reply[4]); + ret = ((reply[3] << 8) | reply[4]); + break; + + } else { + warnx("bl app checksum FAIL from LED %i", boot_cmd.led_num); + warnx("bl app checksum response ADDR: 0x%x", reply[1]); + warnx("bl app checksum response CMD: 0x%x", reply[2]); + warnx("bl app checksum response VER H: 0x%x", reply[3]); + warnx("bl app checksum response VER L: 0x%x", reply[4]); + warnx("bl app checksum response XOR: 0x%x", reply[5]); + + if (retry > 1) { + warnx("bl app checksum retrying LED %i", boot_cmd.led_num); + + } else { + warnx("bl app checksum failed on LED %i", boot_cmd.led_num); + break; + } + } + } + + _is_bootloading = false; + return ret; +} + +int +OREOLED::bootloader_ping(int led_num) +{ + _is_bootloading = true; + oreoled_cmd_t boot_cmd; + boot_cmd.led_num = led_num; + + int ret = -1; + + /* Set the current address */ + set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + + boot_cmd.buff[0] = OREOLED_BOOT_CMD_PING; + boot_cmd.buff[1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; + boot_cmd.num_bytes = 2; + + for (uint8_t j = 0; j < boot_cmd.num_bytes - 1; j++) { + boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[j]; + } + + uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; + + for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { + /* Send the I2C Write+Read */ + memset(reply, 0, sizeof(reply)); + transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 5); + + /* Check the response */ + if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_PING && + reply[3] == OREOLED_BOOT_CMD_PING_NONCE && + reply[4] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { + warnx("bl ping OK from LED %i", boot_cmd.led_num); + ret = OK; + break; + + } else { + warnx("bl ping FAIL from LED %i", boot_cmd.led_num); + warnx("bl ping response ADDR: 0x%x", reply[1]); + warnx("bl ping response CMD: 0x%x", reply[2]); + warnx("bl ping response NONCE: 0x%x", reply[3]); + warnx("bl ping response XOR: 0x%x", reply[4]); + + if (retry > 1) { + warnx("bl ping retrying LED %i", boot_cmd.led_num); + + } else { + warnx("bl ping failed on LED %i", boot_cmd.led_num); + break; + } + } + } + + _is_bootloading = false; + return ret; +} + +uint8_t +OREOLED::bootloader_version(int led_num) +{ + _is_bootloading = true; + oreoled_cmd_t boot_cmd; + boot_cmd.led_num = led_num; + + uint8_t ret = 0x00; + + /* Set the current address */ + set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + + boot_cmd.buff[0] = OREOLED_BOOT_CMD_BL_VER; + boot_cmd.buff[1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; + boot_cmd.num_bytes = 2; + + for (uint8_t k = 0; k < boot_cmd.num_bytes - 1; k++) { + boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[k]; + } + + uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; + + for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { + /* Send the I2C Write+Read */ + memset(reply, 0, sizeof(reply)); + transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 5); + + /* Check the response */ + if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_BL_VER && + reply[3] == OREOLED_BOOT_SUPPORTED_VER && + reply[4] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { + warnx("bl ver from LED %i = %i", boot_cmd.led_num, reply[3]); + ret = reply[3]; + break; + + } else { + warnx("bl ver response ADDR: 0x%x", reply[1]); + warnx("bl ver response CMD: 0x%x", reply[2]); + warnx("bl ver response VER: 0x%x", reply[3]); + warnx("bl ver response XOR: 0x%x", reply[4]); + + if (retry > 1) { + warnx("bl ver retrying LED %i", boot_cmd.led_num); + + } else { + warnx("bl ver failed on LED %i", boot_cmd.led_num); + break; + } + } + } + + _is_bootloading = false; + return ret; +} + +uint16_t +OREOLED::bootloader_app_version(int led_num) +{ + _is_bootloading = true; + oreoled_cmd_t boot_cmd; + boot_cmd.led_num = led_num; + + uint16_t ret = 0x0000; + + /* Set the current address */ + set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + + boot_cmd.buff[0] = OREOLED_BOOT_CMD_APP_VER; + boot_cmd.buff[1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; + boot_cmd.num_bytes = 2; + + for (uint8_t j = 0; j < boot_cmd.num_bytes - 1; j++) { + boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[j]; + } + + uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; + + for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { + /* Send the I2C Write+Read */ + memset(reply, 0, sizeof(reply)); + transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 6); + + /* Check the response */ + if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_APP_VER && + reply[5] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { + warnx("bl app version OK from LED %i", boot_cmd.led_num); + warnx("bl app version msb: 0x%x", reply[3]); + warnx("bl app version lsb: 0x%x", reply[4]); + ret = ((reply[3] << 8) | reply[4]); + break; + + } else { + warnx("bl app version FAIL from LED %i", boot_cmd.led_num); + warnx("bl app version response ADDR: 0x%x", reply[1]); + warnx("bl app version response CMD: 0x%x", reply[2]); + warnx("bl app version response VER H: 0x%x", reply[3]); + warnx("bl app version response VER L: 0x%x", reply[4]); + warnx("bl app version response XOR: 0x%x", reply[5]); + + if (retry > 1) { + warnx("bl app version retrying LED %i", boot_cmd.led_num); + + } else { + warnx("bl app version failed on LED %i", boot_cmd.led_num); + break; + } + } + } + + _is_bootloading = false; + return ret; +} + +uint16_t +OREOLED::bootloader_app_checksum(int led_num) +{ + _is_bootloading = true; + oreoled_cmd_t boot_cmd; + boot_cmd.led_num = led_num; + + uint16_t ret = 0x0000; + + /* Set the current address */ + set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + + boot_cmd.buff[0] = OREOLED_BOOT_CMD_APP_CRC; + boot_cmd.buff[1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; + boot_cmd.num_bytes = 2; + + for (uint8_t j = 0; j < boot_cmd.num_bytes - 1; j++) { + boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[j]; + } + + uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; + + for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { + /* Send the I2C Write+Read */ + memset(reply, 0, sizeof(reply)); + transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 6); + + /* Check the response */ + if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_APP_CRC && + reply[5] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { + warnx("bl app checksum OK from LED %i", boot_cmd.led_num); + warnx("bl app checksum msb: 0x%x", reply[3]); + warnx("bl app checksum lsb: 0x%x", reply[4]); + ret = ((reply[3] << 8) | reply[4]); + break; + + } else { + warnx("bl app checksum FAIL from LED %i", boot_cmd.led_num); + warnx("bl app checksum response ADDR: 0x%x", reply[1]); + warnx("bl app checksum response CMD: 0x%x", reply[2]); + warnx("bl app checksum response VER H: 0x%x", reply[3]); + warnx("bl app checksum response VER L: 0x%x", reply[4]); + warnx("bl app checksum response XOR: 0x%x", reply[5]); + + if (retry > 1) { + warnx("bl app checksum retrying LED %i", boot_cmd.led_num); + + } else { + warnx("bl app checksum failed on LED %i", boot_cmd.led_num); + break; + } + } + } + + _is_bootloading = false; + return ret; +} + +int +OREOLED::bootloader_set_colour(int led_num, uint8_t red, uint8_t green) +{ + _is_bootloading = true; + oreoled_cmd_t boot_cmd; + boot_cmd.led_num = led_num; + + int ret = -1; + + /* Set the current address */ + set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + + boot_cmd.buff[0] = OREOLED_BOOT_CMD_SET_COLOUR; + boot_cmd.buff[1] = red; + boot_cmd.buff[2] = green; + boot_cmd.buff[3] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; + boot_cmd.num_bytes = 4; + + for (uint8_t j = 0; j < boot_cmd.num_bytes - 1; j++) { + boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[j]; + } + + uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; + + for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { + /* Send the I2C Write+Read */ + memset(reply, 0, sizeof(reply)); + transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4); + + /* Check the response */ + if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_SET_COLOUR && + reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { + warnx("bl set colour OK from LED %i", boot_cmd.led_num); + ret = OK; + break; + + } else { + warnx("bl set colour FAIL from LED %i", boot_cmd.led_num); + warnx("bl set colour response ADDR: 0x%x", reply[1]); + warnx("bl set colour response CMD: 0x%x", reply[2]); + warnx("bl set colour response XOR: 0x%x", reply[3]); + + if (retry > 1) { + warnx("bl app colour retrying LED %i", boot_cmd.led_num); + + } else { + warnx("bl app colour failed on LED %i", boot_cmd.led_num); + break; + } + } + } + + _is_bootloading = false; + return ret; +} + +int +OREOLED::bootloader_flash(int led_num) +{ + _is_bootloading = true; + oreoled_cmd_t boot_cmd; + boot_cmd.led_num = led_num; + + /* Open the bootloader file */ + int fd = ::open(OREOLED_FW_FILE, O_RDONLY); + + /* check for error opening the file */ + if (fd < 0) { + return -1; + } + + struct stat s; + + /* attempt to stat the file */ + if (stat(OREOLED_FW_FILE, &s) != 0) { + ::close(fd); + return -1; + } + + uint16_t fw_length = s.st_size - OREOLED_FW_FILE_HEADER_LENGTH; + + /* sanity-check file size */ + if (fw_length > OREOLED_FW_FILE_SIZE_LIMIT) { + ::close(fd); + return -1; + } + + uint8_t *buf = new uint8_t[s.st_size]; + + /* check that the buffer has been allocated */ + if (buf == NULL) { + ::close(fd); + return -1; + } + + /* check that the firmware can be read into the buffer */ + if (::read(fd, buf, s.st_size) != s.st_size) { + ::close(fd); + delete[] buf; + return -1; + } + + ::close(fd); + + /* Grab the version bytes from the binary */ + uint8_t version_major = buf[0]; + uint8_t version_minor = buf[1]; + + /* calculate flash pages (rounded up to nearest integer) */ + uint8_t flash_pages = ((fw_length + 64 - 1) / 64); + + /* Set the current address */ + set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + + uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; + + /* Loop through flash pages */ + for (uint8_t page_idx = 0; page_idx < flash_pages; page_idx++) { + + /* Send the first half of the 64 byte flash page */ + memset(boot_cmd.buff, 0, sizeof(boot_cmd.buff)); + boot_cmd.buff[0] = OREOLED_BOOT_CMD_WRITE_FLASH_A; + boot_cmd.buff[1] = page_idx; + memcpy(boot_cmd.buff + 2, buf + (page_idx * 64) + OREOLED_FW_FILE_HEADER_LENGTH, 32); + boot_cmd.buff[32 + 2] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; + boot_cmd.num_bytes = 32 + 3; + + for (uint8_t k = 0; k < boot_cmd.num_bytes - 1; k++) { + boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[k]; + } + + for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { + /* Send the I2C Write+Read */ + memset(reply, 0, sizeof(reply)); + transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4); + + /* Check the response */ + if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_WRITE_FLASH_A && + reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { + warnx("bl flash %ia OK for LED %i", page_idx, boot_cmd.led_num); + break; + + } else { + warnx("bl flash %ia FAIL for LED %i", page_idx, boot_cmd.led_num); + warnx("bl flash %ia response ADDR: 0x%x", page_idx, reply[1]); + warnx("bl flash %ia response CMD: 0x%x", page_idx, reply[2]); + warnx("bl flash %ia response XOR: 0x%x", page_idx, reply[3]); + + if (retry > 1) { + warnx("bl flash %ia retrying LED %i", page_idx, boot_cmd.led_num); + + } else { + warnx("bl flash %ia failed on LED %i", page_idx, boot_cmd.led_num); + delete[] buf; + return -1; + } + } + } + + /* Send the second half of the 64 byte flash page */ + memset(boot_cmd.buff, 0, sizeof(boot_cmd.buff)); + boot_cmd.buff[0] = OREOLED_BOOT_CMD_WRITE_FLASH_B; + memcpy(boot_cmd.buff + 1, buf + (page_idx * 64) + 32 + OREOLED_FW_FILE_HEADER_LENGTH, 32); + boot_cmd.buff[32 + 1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; + boot_cmd.num_bytes = 32 + 2; + + for (uint8_t k = 0; k < boot_cmd.num_bytes - 1; k++) { + boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[k]; + } + + for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { + /* Send the I2C Write+Read */ + memset(reply, 0, sizeof(reply)); + transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4); + + /* Check the response */ + if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_WRITE_FLASH_B && + reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { + warnx("bl flash %ib OK for LED %i", page_idx, boot_cmd.led_num); + break; + + } else { + warnx("bl flash %ib FAIL for LED %i", page_idx, boot_cmd.led_num); + warnx("bl flash %ib response ADDR: 0x%x", page_idx, reply[1]); + warnx("bl flash %ib response CMD: 0x%x", page_idx, reply[2]); + warnx("bl flash %ib response XOR: 0x%x", page_idx, reply[3]); + + if (retry > 1) { + warnx("bl flash %ib retrying LED %i", page_idx, boot_cmd.led_num); + + } else { + errx(1, "bl flash %ib failed on LED %i", page_idx, boot_cmd.led_num); + delete[] buf; + return -1; + } + } + } + + /* Sleep to allow flash to write */ + /* Wait extra long on the first write, to allow time for EEPROM updates */ + if (page_idx == 0) { + usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); + + } else { + usleep(OREOLED_BOOT_FLASH_WAITMS * 1000); + } + } + + uint16_t app_checksum = bootloader_fw_checksum(); + + /* Flash writes must have succeeded so finalise the flash */ + boot_cmd.buff[0] = OREOLED_BOOT_CMD_FINALISE_FLASH; + boot_cmd.buff[1] = version_major; + boot_cmd.buff[2] = version_minor; + boot_cmd.buff[3] = (uint8_t)(fw_length >> 8); + boot_cmd.buff[4] = (uint8_t)(fw_length & 0xFF); + boot_cmd.buff[5] = (uint8_t)(app_checksum >> 8); + boot_cmd.buff[6] = (uint8_t)(app_checksum & 0xFF); + boot_cmd.buff[7] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; + boot_cmd.num_bytes = 8; + + for (uint8_t k = 0; k < boot_cmd.num_bytes - 1; k++) { + boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[k]; + } + + /* Try to finalise for twice the amount of normal retries */ + for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES * 2; retry > 0; retry--) { + /* Send the I2C Write */ + memset(reply, 0, sizeof(reply)); + transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4); + + /* Check the response */ + if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_FINALISE_FLASH && + reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { + warnx("bl finalise OK from LED %i", boot_cmd.led_num); + break; + + } else { + warnx("bl finalise response ADDR: 0x%x", reply[1]); + warnx("bl finalise response CMD: 0x%x", reply[2]); + warnx("bl finalise response XOR: 0x%x", reply[3]); + + if (retry > 1) { + warnx("bl finalise retrying LED %i", boot_cmd.led_num); + + } else { + warnx("bl finalise failed on LED %i", boot_cmd.led_num); + delete[] buf; + return -1; + } + } + } + + /* allow time for flash to finalise */ + usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); + usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); + + /* clean up file buffer */ + delete[] buf; + + _is_bootloading = false; + return 1; +} + +int +OREOLED::bootloader_boot(int led_num) +{ + _is_bootloading = true; + oreoled_cmd_t boot_cmd; + boot_cmd.led_num = led_num; + + int ret = -1; + + /* Set the current address */ + set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + + boot_cmd.buff[0] = OREOLED_BOOT_CMD_BOOT_APP; + boot_cmd.buff[1] = OREOLED_BOOT_CMD_BOOT_NONCE; + boot_cmd.buff[2] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; + boot_cmd.num_bytes = 3; + + for (uint8_t k = 0; k < boot_cmd.num_bytes - 1; k++) { + boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[k]; + } + + for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { + /* Send the I2C Write */ + uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; + transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4); + + /* Check the response */ + if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_BOOT_APP && + reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { + warnx("bl boot OK from LED %i", boot_cmd.led_num); + /* decrement the inboot counter so we don't get confused */ + _in_boot[led_num] = false; + _num_inboot--; + ret = OK; + break; + + } else if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_BOOT_NONCE && + reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { + warnx("bl boot error from LED %i: no app", boot_cmd.led_num); + break; + + } else { + warnx("bl boot response ADDR: 0x%x", reply[1]); + warnx("bl boot response CMD: 0x%x", reply[2]); + warnx("bl boot response XOR: 0x%x", reply[3]); + + if (retry > 1) { + warnx("bl boot retrying LED %i", boot_cmd.led_num); + + } else { + warnx("bl boot failed on LED %i", boot_cmd.led_num); + break; + } + } + } + + /* allow time for the LEDs to boot */ + usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); + usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); + usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); + usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); + + _is_bootloading = false; + return ret; +} + +uint16_t +OREOLED::bootloader_fw_checksum(void) +{ + /* Calculate the 16 bit XOR checksum of the firmware on the first call of this function */ + if (_fw_checksum == 0x0000) { + /* Open the bootloader file */ + int fd = ::open(OREOLED_FW_FILE, O_RDONLY); + + /* check for error opening the file */ + if (fd < 0) { + return -1; + } + + struct stat s; + + /* attempt to stat the file */ + if (stat(OREOLED_FW_FILE, &s) != 0) { + ::close(fd); + return -1; + } + + uint16_t fw_length = s.st_size - OREOLED_FW_FILE_HEADER_LENGTH; + + /* sanity-check file size */ + if (fw_length > OREOLED_FW_FILE_SIZE_LIMIT) { + ::close(fd); + return -1; + } + + uint8_t *buf = new uint8_t[s.st_size]; + + /* check that the buffer has been allocated */ + if (buf == NULL) { + ::close(fd); + return -1; + } + + /* check that the firmware can be read into the buffer */ + if (::read(fd, buf, s.st_size) != s.st_size) { + ::close(fd); + delete[] buf; + return -1; + } + + ::close(fd); + + /* Calculate a 16 bit XOR checksum of the flash */ + /* Skip the first two bytes which are the version information, plus + the next two bytes which are modified by the bootloader */ + uint16_t app_checksum = 0x0000; + + for (uint16_t j = 2 + OREOLED_FW_FILE_HEADER_LENGTH; j < s.st_size; j += 2) { + app_checksum ^= (buf[j] << 8) | buf[j + 1]; + } + + delete[] buf; + + warnx("fw length = %i", fw_length); + warnx("fw checksum = %i", app_checksum); + + /* Store the checksum so it's only calculated once */ + _fw_checksum = app_checksum; + } + + return _fw_checksum; +} + +int +OREOLED::bootloader_coerce_healthy(void) +{ + int ret = -1; + + /* check each unhealthy LED */ + /* this re-checks "unhealthy" LEDs as they can sometimes power up with the wrong ID, */ + /* but will have the correct ID once they jump to the application and be healthy again */ + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (!_healthy[i] && bootloader_app_ping(i) == OK) { + /* mark as healthy */ + _healthy[i] = true; + _num_healthy++; + ret = OK; + } + } + + return ret; +} + int OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg) { @@ -368,6 +1415,100 @@ OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg) return ret; + case OREOLED_SEND_RESET: + /* send a reset */ + new_cmd.led_num = OREOLED_ALL_INSTANCES; + new_cmd.buff[0] = OREOLED_PATTERN_PARAMUPDATE; + new_cmd.buff[1] = OREOLED_PARAM_RESET; + new_cmd.buff[2] = OEROLED_RESET_NONCE; + new_cmd.num_bytes = 3; + + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + /* add command to queue for all healthy leds */ + if (_healthy[i]) { + warnx("sending a reset... to %i", i); + new_cmd.led_num = i; + _cmd_queue->force(&new_cmd); + ret = OK; + } + } + + return ret; + + case OREOLED_BL_PING: + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i]) { + bootloader_ping(i); + ret = OK; + } + } + + return ret; + + case OREOLED_BL_VER: + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i]) { + bootloader_version(i); + ret = OK; + } + } + + return ret; + + case OREOLED_BL_FLASH: + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i]) { + bootloader_flash(i); + ret = OK; + } + } + + return ret; + + case OREOLED_BL_APP_VER: + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i]) { + bootloader_app_version(i); + ret = OK; + } + } + + return ret; + + case OREOLED_BL_APP_CRC: + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i]) { + bootloader_app_checksum(i); + ret = OK; + } + } + + return ret; + + case OREOLED_BL_SET_COLOUR: + new_cmd.led_num = OREOLED_ALL_INSTANCES; + + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i]) { + bootloader_set_colour(i, ((oreoled_rgbset_t *) arg)->red, ((oreoled_rgbset_t *) arg)->green); + ret = OK; + } + } + + return ret; + + case OREOLED_BL_BOOT_APP: + new_cmd.led_num = OREOLED_ALL_INSTANCES; + + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i]) { + bootloader_boot(i); + ret = OK; + } + } + + return ret; + case OREOLED_SEND_BYTES: /* send bytes */ new_cmd = *((oreoled_cmd_t *) arg); @@ -393,6 +1534,10 @@ OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg) return ret; + case OREOLED_FORCE_SYNC: + send_general_call(); + break; + default: /* see if the parent class can make any use of it */ ret = CDev::ioctl(filp, cmd, arg); @@ -444,10 +1589,18 @@ OREOLED::send_cmd(oreoled_cmd_t new_cmd) return ret; } +/* return the internal _is_ready flag indicating if initialisation is complete */ +bool +OREOLED::is_ready() +{ + return _is_ready; +} + void oreoled_usage() { - warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'rgb 30 40 50' 'macro 4' 'gencall' 'bytes 7 9 6'"); + warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'reset', 'rgb 30 40 50', 'macro 4', 'gencall', 'bytes 7 9 6'"); + warnx("bootloader commands: try 'blping', 'blver', 'blappver', 'blappcrc', 'blcolour ', 'blflash', 'blboot'"); warnx("options:"); warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); warnx(" -a addr (0x%x)", OREOLED_BASE_I2C_ADDR); @@ -498,8 +1651,21 @@ oreoled_main(int argc, char *argv[]) i2cdevice = PX4_I2C_BUS_LED; } + /* handle update flags */ + bool autoupdate = false; + bool alwaysupdate = false; + + if (argc > 2 && !strcmp(argv[2], "autoupdate")) { + warnx("autoupdate enabled"); + autoupdate = true; + + } else if (argc > 2 && !strcmp(argv[2], "alwaysupdate")) { + warnx("alwaysupdate enabled"); + alwaysupdate = true; + } + /* instantiate driver */ - g_oreoled = new OREOLED(i2cdevice, i2c_addr); + g_oreoled = new OREOLED(i2cdevice, i2c_addr, autoupdate, alwaysupdate); /* check if object was created */ if (g_oreoled == nullptr) { @@ -513,6 +1679,15 @@ oreoled_main(int argc, char *argv[]) errx(1, "failed to start driver"); } + /* wait for up to 20 seconds for the driver become ready */ + for (uint8_t i = 0; i < 20; i++) { + if (g_oreoled != nullptr && g_oreoled->is_ready()) { + break; + } + + sleep(1); + } + exit(0); } @@ -648,6 +1823,170 @@ oreoled_main(int argc, char *argv[]) exit(ret); } + /* send reset request to all LEDS */ + if (!strcmp(verb, "reset")) { + if (argc < 2) { + errx(1, "Usage: oreoled reset"); + } + + int fd = open(OREOLED0_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " OREOLED0_DEVICE_PATH); + } + + if ((ret = ioctl(fd, OREOLED_SEND_RESET, 0)) != OK) { + errx(1, "failed to run macro"); + } + + close(fd); + exit(ret); + } + + /* attempt to flash all LEDS in bootloader mode*/ + if (!strcmp(verb, "blflash")) { + if (argc < 2) { + errx(1, "Usage: oreoled blflash"); + } + + int fd = open(OREOLED0_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " OREOLED0_DEVICE_PATH); + } + + if ((ret = ioctl(fd, OREOLED_BL_FLASH, 0)) != OK) { + errx(1, "failed to run flash"); + } + + close(fd); + exit(ret); + } + + /* send bootloader boot request to all LEDS */ + if (!strcmp(verb, "blboot")) { + if (argc < 2) { + errx(1, "Usage: oreoled blboot"); + } + + int fd = open(OREOLED0_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " OREOLED0_DEVICE_PATH); + } + + if ((ret = ioctl(fd, OREOLED_BL_BOOT_APP, 0)) != OK) { + errx(1, "failed to run boot"); + } + + close(fd); + exit(ret); + } + + /* send bootloader ping all LEDs */ + if (!strcmp(verb, "blping")) { + if (argc < 2) { + errx(1, "Usage: oreoled blping"); + } + + int fd = open(OREOLED0_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " OREOLED0_DEVICE_PATH); + } + + if ((ret = ioctl(fd, OREOLED_BL_PING, 0)) != OK) { + errx(1, "failed to run blping"); + } + + close(fd); + exit(ret); + } + + /* ask all LEDs for their bootloader version */ + if (!strcmp(verb, "blver")) { + if (argc < 2) { + errx(1, "Usage: oreoled blver"); + } + + int fd = open(OREOLED0_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " OREOLED0_DEVICE_PATH); + } + + if ((ret = ioctl(fd, OREOLED_BL_VER, 0)) != OK) { + errx(1, "failed to get bootloader version"); + } + + close(fd); + exit(ret); + } + + /* ask all LEDs for their application version */ + if (!strcmp(verb, "blappver")) { + if (argc < 2) { + errx(1, "Usage: oreoled blappver"); + } + + int fd = open(OREOLED0_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " OREOLED0_DEVICE_PATH); + } + + if ((ret = ioctl(fd, OREOLED_BL_APP_VER, 0)) != OK) { + errx(1, "failed to get boot app version"); + } + + close(fd); + exit(ret); + } + + /* ask all LEDs for their application crc */ + if (!strcmp(verb, "blappcrc")) { + if (argc < 2) { + errx(1, "Usage: oreoled blappcrc"); + } + + int fd = open(OREOLED0_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " OREOLED0_DEVICE_PATH); + } + + if ((ret = ioctl(fd, OREOLED_BL_APP_CRC, 0)) != OK) { + errx(1, "failed to get boot app crc"); + } + + close(fd); + exit(ret); + } + + /* set the default bootloader LED colour on all LEDs */ + if (!strcmp(verb, "blcolour")) { + if (argc < 4) { + errx(1, "Usage: oreoled blcolour "); + } + + int fd = open(OREOLED0_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " OREOLED0_DEVICE_PATH); + } + + uint8_t red = (uint8_t)strtol(argv[2], NULL, 0); + uint8_t green = (uint8_t)strtol(argv[3], NULL, 0); + oreoled_rgbset_t rgb_set = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_SOLID, red, green, 0}; + + if ((ret = ioctl(fd, OREOLED_BL_SET_COLOUR, (unsigned long)&rgb_set)) != OK) { + errx(1, "failed to set boot startup colours"); + } + + close(fd); + exit(ret); + } + /* send general hardware call to all LEDS */ if (!strcmp(verb, "gencall")) { ret = g_oreoled->send_general_call(); @@ -670,18 +2009,18 @@ oreoled_main(int argc, char *argv[]) } /* check led num */ - sendb.led_num = (uint8_t)strtol(argv[2], NULL, 0); + sendb.led_num = (uint8_t)strtol(argv[optind + 1], NULL, 0); if (sendb.led_num > 3) { errx(1, "led number must be between 0 ~ 3"); } /* get bytes */ - sendb.num_bytes = argc - 3; + sendb.num_bytes = argc - (optind + 2); uint8_t byte_count; for (byte_count = 0; byte_count < sendb.num_bytes; byte_count++) { - sendb.buff[byte_count] = (uint8_t)strtol(argv[byte_count + 3], NULL, 0); + sendb.buff[byte_count] = (uint8_t)strtol(argv[byte_count + optind + 2], NULL, 0); } /* send bytes */ diff --git a/src/drivers/pwm_input/pwm_input.cpp b/src/drivers/pwm_input/pwm_input.cpp index 0ec2af74f7..af2ed43a36 100644 --- a/src/drivers/pwm_input/pwm_input.cpp +++ b/src/drivers/pwm_input/pwm_input.cpp @@ -321,14 +321,14 @@ void PWMIN::_timer_init(void) { /* run with interrupts disabled in case the timer is already * setup. We don't want it firing while we are doing the setup */ - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); /* configure input pin */ - stm32_configgpio(GPIO_PWM_IN); + px4_arch_configgpio(GPIO_PWM_IN); // XXX refactor this out of this driver /* configure reset pin */ - stm32_configgpio(GPIO_VDD_RANGEFINDER_EN); + px4_arch_configgpio(GPIO_VDD_RANGEFINDER_EN); /* claim our interrupt vector */ irq_attach(PWMIN_TIMER_VECTOR, pwmin_tim_isr); @@ -373,7 +373,7 @@ void PWMIN::_timer_init(void) /* enable interrupts */ up_enable_irq(PWMIN_TIMER_VECTOR); - irqrestore(flags); + px4_leave_critical_section(flags); _timer_started = true; } @@ -392,14 +392,14 @@ PWMIN::_freeze_test() void PWMIN::_turn_on() { - stm32_gpiowrite(GPIO_VDD_RANGEFINDER_EN, 1); + px4_arch_gpiowrite(GPIO_VDD_RANGEFINDER_EN, 1); } // XXX refactor this out of this driver void PWMIN::_turn_off() { - stm32_gpiowrite(GPIO_VDD_RANGEFINDER_EN, 0); + px4_arch_gpiowrite(GPIO_VDD_RANGEFINDER_EN, 0); } // XXX refactor this out of this driver @@ -444,14 +444,14 @@ PWMIN::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; } - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); if (!_reports->resize(arg)) { - irqrestore(flags); + px4_leave_critical_section(flags); return -ENOMEM; } - irqrestore(flags); + px4_leave_critical_section(flags); return OK; } diff --git a/src/drivers/hott/hott_telemetry/module.mk b/src/drivers/pwm_out_rc_in/CMakeLists.txt similarity index 84% rename from src/drivers/hott/hott_telemetry/module.mk rename to src/drivers/pwm_out_rc_in/CMakeLists.txt index 9de47b356c..24faeff27f 100644 --- a/src/drivers/hott/hott_telemetry/module.mk +++ b/src/drivers/pwm_out_rc_in/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2015-2016 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,13 +30,15 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ - -# -# Graupner HoTT Telemetry applications. -# - -MODULE_COMMAND = hott_telemetry - -SRCS = hott_telemetry.cpp - -MAXOPTIMIZATION = -Os +px4_add_module( + MODULE drivers__pwm_out_rc_in + MAIN pwm_out_rc_in + COMPILE_FLAGS + -Os + -DMAVLINK_COMM_NUM_BUFFERS=1 + SRCS + pwm_out_rc_in.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/uart_esc/uart_esc.cpp b/src/drivers/pwm_out_rc_in/pwm_out_rc_in.cpp similarity index 89% rename from src/drivers/uart_esc/uart_esc.cpp rename to src/drivers/pwm_out_rc_in/pwm_out_rc_in.cpp index 458a8463bb..e7756c3d8f 100644 --- a/src/drivers/uart_esc/uart_esc.cpp +++ b/src/drivers/pwm_out_rc_in/pwm_out_rc_in.cpp @@ -56,15 +56,20 @@ #include -namespace uart_esc +/* + * This driver is supposed to run on Snapdragon. It sends actuator_controls (PWM) + * to a Pixhawk/Pixfalcon/Pixracer over UART (mavlink) and receives RC input. + */ + +namespace pwm_out_rc_in { static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; -volatile bool _task_should_exit = false; // flag indicating if uart_esc task should exit +volatile bool _task_should_exit = false; // flag indicating if pwm_out_rc_in task should exit static char _device[32] = {}; -static bool _is_running = false; // flag indicating if uart_esc app is running +static bool _is_running = false; // flag indicating if pwm_out_rc_in app is running static px4_task_t _task_handle = -1; // handle to the task main thread // subscriptions @@ -163,7 +168,7 @@ int initialize_mixer(const char *mixer_filename) } } else { - PX4_WARN("Unable to open mixer config file, try default mixer"); + PX4_WARN("No mixer config file found, using default mixer."); /* Mixer file loading failed, fall back to default mixer configuration for * QUAD_X airframe. */ @@ -181,7 +186,6 @@ int initialize_mixer(const char *mixer_filename) return -1; } - PX4_INFO("Successfully initialized default quad x mixer."); return 0; } } @@ -298,8 +302,9 @@ void serial_callback(void *context, char *buffer, size_t num_bytes) mavlink_message_t msg; for (int i = 0; i < num_bytes; ++i) { - // TODO FIXME: we don't know if MAVLINK_COMM_1 is already taken. - if (mavlink_parse_char(MAVLINK_COMM_1, buffer[i], &msg, &serial_status)) { + // The MAVLink app doesn't use the internal buffer functions + // and hence the first port can be used here. + if (mavlink_parse_char(MAVLINK_COMM_0, buffer[i], &msg, &serial_status)) { // have a message, handle it if (msg.msgid == MAVLINK_MSG_ID_RC_CHANNELS) { // we should publish but would be great if this works @@ -473,7 +478,7 @@ void start() _task_should_exit = false; /* start the task */ - _task_handle = px4_task_spawn_cmd("uart_esc_main", + _task_handle = px4_task_spawn_cmd("pwm_out_rc_in_main", SCHED_DEFAULT, SCHED_PRIORITY_MAX, 1500, @@ -501,17 +506,17 @@ void stop() void usage() { - PX4_INFO("usage: uart_esc start -d /dev/tty-3"); - PX4_INFO(" uart_esc stop"); - PX4_INFO(" uart_esc status"); + PX4_INFO("usage: pwm_out_rc_in start -d /dev/tty-3"); + PX4_INFO(" pwm_out_rc_in stop"); + PX4_INFO(" pwm_out_rc_in status"); } -} // namespace uart_esc +} // namespace pwm_out_rc_in /* driver 'main' command */ -extern "C" __EXPORT int uart_esc_main(int argc, char *argv[]); +extern "C" __EXPORT int pwm_out_rc_in_main(int argc, char *argv[]); -int uart_esc_main(int argc, char *argv[]) +int pwm_out_rc_in_main(int argc, char *argv[]) { const char *device = nullptr; int ch; @@ -528,49 +533,49 @@ int uart_esc_main(int argc, char *argv[]) switch (ch) { case 'd': device = myoptarg; - strncpy(uart_esc::_device, device, strlen(device)); + strncpy(pwm_out_rc_in::_device, device, strlen(device)); break; } } // gets the parameters for the esc's pwm - param_get(param_find("PWM_DISARMED"), &uart_esc::_pwm_disarmed); - param_get(param_find("PWM_MIN"), &uart_esc::_pwm_min); - param_get(param_find("PWM_MAX"), &uart_esc::_pwm_max); + param_get(param_find("PWM_DISARMED"), &pwm_out_rc_in::_pwm_disarmed); + param_get(param_find("PWM_MIN"), &pwm_out_rc_in::_pwm_min); + param_get(param_find("PWM_MAX"), &pwm_out_rc_in::_pwm_max); /* * Start/load the driver. */ if (!strcmp(verb, "start")) { - if (uart_esc::_is_running) { - PX4_WARN("uart_esc already running"); + if (pwm_out_rc_in::_is_running) { + PX4_WARN("pwm_out_rc_in already running"); return 1; } // Check on required arguments if (device == nullptr || strlen(device) == 0) { - uart_esc::usage(); + pwm_out_rc_in::usage(); return 1; } - uart_esc::start(); + pwm_out_rc_in::start(); } else if (!strcmp(verb, "stop")) { - if (!uart_esc::_is_running) { - PX4_WARN("uart_esc is not running"); + if (!pwm_out_rc_in::_is_running) { + PX4_WARN("pwm_out_rc_in is not running"); return 1; } - uart_esc::stop(); + pwm_out_rc_in::stop(); } else if (!strcmp(verb, "status")) { - PX4_WARN("uart_esc is %s", uart_esc::_is_running ? "running" : "not running"); + PX4_WARN("pwm_out_rc_in is %s", pwm_out_rc_in::_is_running ? "running" : "not running"); return 0; } else { - uart_esc::usage(); + pwm_out_rc_in::usage(); return 1; } diff --git a/src/drivers/pwm_out_sim/CMakeLists.txt b/src/drivers/pwm_out_sim/CMakeLists.txt index d12a5fdfcf..573d9a7afb 100644 --- a/src/drivers/pwm_out_sim/CMakeLists.txt +++ b/src/drivers/pwm_out_sim/CMakeLists.txt @@ -34,6 +34,7 @@ px4_add_module( MODULE drivers__pwm_out_sim MAIN pwm_out_sim STACK_MAIN 1200 + STACK_MAX 1200 COMPILE_FLAGS -Os SRCS diff --git a/src/drivers/pwm_out_sim/pwm_out_sim.cpp b/src/drivers/pwm_out_sim/pwm_out_sim.cpp index 457e85bb91..af264d966c 100644 --- a/src/drivers/pwm_out_sim/pwm_out_sim.cpp +++ b/src/drivers/pwm_out_sim/pwm_out_sim.cpp @@ -77,10 +77,6 @@ #include #include -#include -#include -#include -#include #include #include @@ -186,7 +182,6 @@ PWMSim::PWMSim() : _mode(MODE_NONE), _update_rate(50), _current_update_rate(0), - _control_subs { -1}, _poll_fds{}, _poll_fds_num(0), _armed_sub(-1), @@ -205,6 +200,10 @@ PWMSim::PWMSim() : _control_topics[1] = ORB_ID(actuator_controls_1); _control_topics[2] = ORB_ID(actuator_controls_2); _control_topics[3] = ORB_ID(actuator_controls_3); + + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + _control_subs[i] = -1; + } } PWMSim::~PWMSim() @@ -256,12 +255,12 @@ PWMSim::init() _task = px4_task_spawn_cmd("pwm_out_sim", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 1000, + 1200, (px4_main_t)&PWMSim::task_main_trampoline, nullptr); if (_task < 0) { - DEVICE_DEBUG("task start failed: %d", errno); + PX4_INFO("task start failed: %d", errno); return -errno; } @@ -286,42 +285,42 @@ PWMSim::set_mode(Mode mode) */ switch (mode) { case MODE_2PWM: - DEVICE_DEBUG("MODE_2PWM"); + PX4_INFO("MODE_2PWM"); /* multi-port with flow control lines as PWM */ _update_rate = 50; /* default output rate */ _num_outputs = 2; break; case MODE_4PWM: - DEVICE_DEBUG("MODE_4PWM"); + PX4_INFO("MODE_4PWM"); /* multi-port as 4 PWM outs */ _update_rate = 50; /* default output rate */ _num_outputs = 4; break; case MODE_8PWM: - DEVICE_DEBUG("MODE_8PWM"); + PX4_INFO("MODE_8PWM"); /* multi-port as 8 PWM outs */ _update_rate = 50; /* default output rate */ _num_outputs = 8; break; case MODE_12PWM: - DEVICE_DEBUG("MODE_12PWM"); + PX4_INFO("MODE_12PWM"); /* multi-port as 12 PWM outs */ _update_rate = 50; /* default output rate */ _num_outputs = 12; break; case MODE_16PWM: - DEVICE_DEBUG("MODE_16PWM"); + PX4_INFO("MODE_16PWM"); /* multi-port as 16 PWM outs */ _update_rate = 50; /* default output rate */ _num_outputs = 16; break; case MODE_NONE: - DEVICE_DEBUG("MODE_NONE"); + PX4_INFO("MODE_NONE"); /* disable servo outputs and set a very low update rate */ _update_rate = 10; _num_outputs = 0; @@ -362,11 +361,11 @@ PWMSim::subscribe() if (unsub_groups & (1 << i)) { PX4_DEBUG("unsubscribe from actuator_controls_%d", i); - px4_close(_control_subs[i]); + orb_unsubscribe(_control_subs[i]); _control_subs[i] = -1; } - if (_control_subs[i] > 0) { + if (_control_subs[i] >= 0) { _poll_fds[_poll_fds_num].fd = _control_subs[i]; _poll_fds[_poll_fds_num].events = POLLIN; _poll_fds_num++; @@ -406,7 +405,7 @@ PWMSim::task_main() } for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { - if (_control_subs[i] > 0) { + if (_control_subs[i] >= 0) { orb_set_interval(_control_subs[i], update_rate_in_ms); } } @@ -415,21 +414,17 @@ PWMSim::task_main() _current_update_rate = _update_rate; } - /* sleep waiting for data, but no more than a second */ - int ret = 0; - + /* this can happen during boot, but after the sleep its likely resolved */ if (_poll_fds_num == 0) { usleep(1000 * 1000); - /* this can happen during boot, but after the sleep its likely resolved */ - if (_poll_fds_num == 0) { - PX4_WARN("pwm_out_sim: No valid fds"); - } - - } else { - ret = px4_poll(&_poll_fds[0], _poll_fds_num, 1000); + PX4_DEBUG("no valid fds"); + continue; } + /* sleep waiting for data, but no more than a second */ + int ret = px4_poll(&_poll_fds[0], _poll_fds_num, 1000); + /* this would be bad... */ if (ret < 0) { DEVICE_LOG("poll error %d", errno); @@ -445,7 +440,7 @@ PWMSim::task_main() unsigned poll_id = 0; for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { - if (_control_subs[i] > 0) { + if (_control_subs[i] >= 0) { if (_poll_fds[poll_id].revents & POLLIN) { orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); } @@ -531,12 +526,12 @@ PWMSim::task_main() } for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { - if (_control_subs[i] > 0) { - px4_close(_control_subs[i]); + if (_control_subs[i] >= 0) { + orb_unsubscribe(_control_subs[i]); } } - px4_close(_armed_sub); + orb_unsubscribe(_armed_sub); /* make sure servos are off */ // up_pwm_servo_deinit(); @@ -583,7 +578,7 @@ PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg) default: ret = -ENOTTY; - DEVICE_DEBUG("not in a PWM mode"); + PX4_INFO("not in a PWM mode"); break; } @@ -797,7 +792,7 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) ret = _mixers->load_from_buf(buf, buflen); if (ret != 0) { - DEVICE_DEBUG("mixer load failed with %d", ret); + PX4_ERR("mixer load failed with %d", ret); delete _mixers; _mixers = nullptr; _groups_required = 0; @@ -957,7 +952,7 @@ extern "C" __EXPORT int pwm_out_sim_main(int argc, char *argv[]); static void usage() { - PX4_WARN("pwm_out_sim: unrecognized command, try:"); + PX4_WARN("unrecognized command, try:"); PX4_WARN(" mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, mode_port2_pwm8, mode_port2_pwm12, mode_port2_pwm16"); } diff --git a/src/drivers/px4flow/CMakeLists.txt b/src/drivers/px4flow/CMakeLists.txt index 1859ab3534..d8953b1ae1 100644 --- a/src/drivers/px4flow/CMakeLists.txt +++ b/src/drivers/px4flow/CMakeLists.txt @@ -35,7 +35,6 @@ px4_add_module( MAIN px4flow STACK_MAIN 1200 COMPILE_FLAGS - -Wno-attributes -Os SRCS px4flow.cpp diff --git a/src/drivers/px4flow/i2c_frame.h b/src/drivers/px4flow/i2c_frame.h index f73d4a92cc..ec8a5e1fda 100644 --- a/src/drivers/px4flow/i2c_frame.h +++ b/src/drivers/px4flow/i2c_frame.h @@ -41,7 +41,6 @@ #ifndef I2C_FRAME_H_ #define I2C_FRAME_H_ -#define __STDC_FORMAT_MACROS #include typedef struct i2c_frame { diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 13f69a3213..03015e315b 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -364,14 +364,14 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; } - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); if (!_reports->resize(arg)) { - irqrestore(flags); + px4_leave_critical_section(flags); return -ENOMEM; } - irqrestore(flags); + px4_leave_critical_section(flags); return OK; } @@ -592,12 +592,12 @@ PX4FLOW::start() work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this, 1); /* notify about state change */ - struct subsystem_info_s info = { - true, - true, - true, - subsystem_info_s::SUBSYSTEM_TYPE_OPTICALFLOW - }; + struct subsystem_info_s info = {}; + info.present = true; + info.enabled = true; + info.ok = true; + info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_OPTICALFLOW; + static orb_advert_t pub = nullptr; if (pub != nullptr) { diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 4c39f3b7c4..abb3a36898 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -63,6 +63,7 @@ #include +#include #include #include #include @@ -79,10 +80,6 @@ #include #include -#include -#include -#include -#include #include #include #include @@ -98,7 +95,7 @@ #define SCHEDULE_INTERVAL 2000 /**< The schedule interval in usec (500 Hz) */ #define NAN_VALUE (0.0f/0.0f) /**< NaN value for throttle lock mode */ -#define BUTTON_SAFETY stm32_gpioread(GPIO_BTN_SAFETY) +#define BUTTON_SAFETY px4_arch_gpioread(GPIO_BTN_SAFETY) #define CYCLE_COUNT 10 /* safety switch must be held for 1 second to activate */ /* @@ -110,6 +107,9 @@ #define LED_PATTERN_FMU_ARMED 0x5500 /**< long off, then quad blink */ #define LED_PATTERN_IO_FMU_ARMED 0xffff /**< constantly on */ +#if !defined(BOARD_HAS_PWM) +# error "board_config.h needs to define BOARD_HAS_PWM" +#endif class PX4FMU : public device::CDev { public: @@ -221,7 +221,10 @@ private: bool _safety_disabled; orb_advert_t _to_safety; - static bool arm_nothrottle() { return (_armed.prearmed && !_armed.armed); } + static bool arm_nothrottle() + { + return ((_armed.prearmed && !_armed.armed) || _armed.in_esc_calibration_mode); + } static void cycle_trampoline(void *arg); void cycle(); @@ -273,78 +276,12 @@ private: void rc_io_invert(); void rc_io_invert(bool invert); void safety_check_button(void); + void flash_safety_button(void); }; -const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) - {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, - {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, - {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1}, - {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, GPIO_USART2_RTS_1}, - {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, GPIO_USART2_TX_1}, - {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, GPIO_USART2_RX_1}, - {GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2}, - {GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2}, -#endif -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) - {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, - {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, - {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0}, - {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0}, - {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0}, - {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0}, +const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = BOARD_FMU_GPIO_TAB; - {0, GPIO_VDD_5V_PERIPH_EN, 0}, - {0, GPIO_VDD_3V3_SENSORS_EN, 0}, - {GPIO_VDD_BRICK_VALID, 0, 0}, - {GPIO_VDD_SERVO_VALID, 0, 0}, - {GPIO_VDD_5V_HIPOWER_OC, 0, 0}, - {GPIO_VDD_5V_PERIPH_OC, 0, 0}, -#endif -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V4) - {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, - {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, - {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0}, - {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0}, - {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0}, - {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0}, - - {0, GPIO_VDD_3V3_SENSORS_EN, 0}, - {GPIO_VDD_BRICK_VALID, 0, 0}, -#endif -#if defined(CONFIG_ARCH_BOARD_AEROCORE) - /* AeroCore breaks out User GPIOs on J11 */ - {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, - {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, - {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0}, - {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0}, - {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0}, - {GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, 0}, - {GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, 0}, - {GPIO_GPIO8_INPUT, GPIO_GPIO8_OUTPUT, 0}, - {GPIO_GPIO9_INPUT, GPIO_GPIO9_OUTPUT, 0}, - {GPIO_GPIO10_INPUT, GPIO_GPIO10_OUTPUT, 0}, - {GPIO_GPIO11_INPUT, GPIO_GPIO11_OUTPUT, 0}, -#endif -#if defined(CONFIG_ARCH_BOARD_MINDPX_V2) - {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, - {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, - {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0}, - {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0}, - {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0}, - {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0}, - {GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, 0}, - {GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, 0}, - {GPIO_GPIO8_INPUT, GPIO_GPIO8_OUTPUT, 0}, - {GPIO_GPIO9_INPUT, GPIO_GPIO9_OUTPUT, 0}, - {GPIO_GPIO10_INPUT, GPIO_GPIO10_OUTPUT, 0}, - {GPIO_GPIO11_INPUT, GPIO_GPIO11_OUTPUT, 0}, - {GPIO_GPIO12_INPUT, GPIO_GPIO12_OUTPUT, 0}, - {0, 0, 0}, -#endif -}; - -const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]); +const unsigned PX4FMU::_ngpio = arraySize(PX4FMU::_gpio_tab); pwm_limit_t PX4FMU::_pwm_limit; actuator_armed_s PX4FMU::_armed = {}; @@ -406,13 +343,21 @@ PX4FMU::PX4FMU() : memset(_controls, 0, sizeof(_controls)); memset(_poll_fds, 0, sizeof(_poll_fds)); + // Safely initialize armed flags. + _armed.armed = false; + _armed.prearmed = false; + _armed.ready_to_arm = false; + _armed.lockdown = false; + _armed.force_failsafe = false; + _armed.in_esc_calibration_mode = false; + // rc input, published to ORB memset(&_rc_in, 0, sizeof(_rc_in)); _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM; #ifdef GPIO_SBUS_INV // this board has a GPIO to control SBUS inversion - stm32_configgpio(GPIO_SBUS_INV); + px4_arch_configgpio(GPIO_SBUS_INV); #endif // If there is no safety button, disable it on boot. @@ -521,7 +466,15 @@ PX4FMU:: safety_check_button(void) counter = 0; } - /* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */ +#endif +} + +void +PX4FMU::flash_safety_button() +{ +#ifdef GPIO_BTN_SAFETY + + /* Select the appropriate LED flash pattern depending on the current arm state */ uint16_t pattern = LED_PATTERN_FMU_REFUSE_TO_ARM; /* cycle the blink state machine at 10Hz */ @@ -544,7 +497,7 @@ PX4FMU:: safety_check_button(void) } /* Turn the LED on if we have a 1 at the current bit position */ - stm32_gpiowrite(GPIO_LED_SAFETY, !(pattern & (1 << blink_counter++))); + px4_arch_gpiowrite(GPIO_LED_SAFETY, !(pattern & (1 << blink_counter++))); if (blink_counter > 15) { blink_counter = 0; @@ -612,7 +565,9 @@ PX4FMU::set_mode(Mode mode) break; - case MODE_6PWM: // v2 PWMs as 6 PWM outs +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6 + + case MODE_6PWM: DEVICE_DEBUG("MODE_6PWM"); /* default output rates */ @@ -623,8 +578,9 @@ PX4FMU::set_mode(Mode mode) _pwm_initialized = false; break; +#endif -#ifdef CONFIG_ARCH_BOARD_AEROCORE +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8 case MODE_8PWM: // AeroCore PWMs as 8 PWM outs DEVICE_DEBUG("MODE_8PWM"); @@ -905,7 +861,7 @@ void PX4FMU::rc_io_invert(bool invert) if (!invert) { // set FMU_RC_OUTPUT high to pull RC_INPUT up - stm32_gpiowrite(GPIO_RC_OUT, 1); + px4_arch_gpiowrite(GPIO_RC_OUT, 1); } } #endif @@ -925,9 +881,6 @@ PX4FMU::update_pwm_out_state(bool on) up_pwm_servo_init(_pwm_mask); set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); _pwm_initialized = true; - - } else { - _pwm_initialized = false; } up_pwm_servo_arm(on); @@ -955,7 +908,7 @@ PX4FMU::cycle() // assume SBUS input sbus_config(_rcs_fd, false); // disable CPPM input by mapping it away from the timer capture input - stm32_unconfiggpio(GPIO_PPM_IN); + px4_arch_unconfiggpio(GPIO_PPM_IN); #endif _initialized = true; @@ -1044,6 +997,19 @@ PX4FMU::cycle() poll_id++; } + + /* During ESC calibration, we overwrite the throttle value. */ + if (i == 0 && _armed.in_esc_calibration_mode) { + + /* Set all controls to 0 */ + memset(&_controls[i], 0, sizeof(_controls[i])); + + /* except thrust to maximum. */ + _controls[i].control[3] = 1.0f; + + /* Switch off the PWM limit ramp for the calibration. */ + _pwm_limit.state = PWM_LIMIT_STATE_ON; + } } /* can we mix? */ @@ -1093,8 +1059,9 @@ PX4FMU::cycle() uint16_t pwm_limited[_max_actuators]; /* the PWM limit call takes care of out of band errors, NaN and constrains */ - pwm_limit_calc(_throttle_armed, arm_nothrottle(), num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm, - outputs, pwm_limited, &_pwm_limit); + pwm_limit_calc(_throttle_armed, arm_nothrottle(), num_outputs, _reverse_pwm_mask, + _disarmed_pwm, _min_pwm, _max_pwm, outputs, pwm_limited, &_pwm_limit); + /* overwrite outputs in case of lockdown with disarmed PWM values */ if (_armed.lockdown) { @@ -1125,8 +1092,6 @@ PX4FMU::cycle() struct safety_s safety = {}; if (_safety_disabled) { - /* safety switch disabled, turn LED on solid */ - stm32_gpiowrite(GPIO_LED_SAFETY, 0); _safety_off = true; } else { @@ -1134,6 +1099,9 @@ PX4FMU::cycle() safety_check_button(); } + /* Make the safety button flash anyway, no matter if it's used or not. */ + flash_safety_button(); + safety.timestamp = hrt_absolute_time(); if (_safety_off) { @@ -1162,11 +1130,14 @@ PX4FMU::cycle() if (updated) { orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); - /* update the armed status and check that we're not locked down */ - _throttle_armed = _safety_off && _armed.armed && !_armed.lockdown; + /* Update the armed status and check that we're not locked down. + * We also need to arm throttle for the ESC calibration. */ + _throttle_armed = (_safety_off && _armed.armed && !_armed.lockdown) || + (_safety_off && _armed.in_esc_calibration_mode); + /* update PWM status if armed or if disarmed PWM values are set */ - bool pwm_on = _armed.armed || _num_disarmed_set > 0; + bool pwm_on = _armed.armed || _num_disarmed_set > 0 || _armed.in_esc_calibration_mode; if (_pwm_on != pwm_on) { _pwm_on = pwm_on; @@ -1383,10 +1354,9 @@ PX4FMU::cycle() } else { // Scan the next protocol - set_rc_scan_state(RC_SCAN_SUMD); + set_rc_scan_state(RC_SCAN_PPM); } - set_rc_scan_state(RC_SCAN_PPM); break; case RC_SCAN_PPM: @@ -1395,7 +1365,7 @@ PX4FMU::cycle() if (_rc_scan_begin == 0) { _rc_scan_begin = _cycle_timestamp; // Configure timer input pin for CPPM - stm32_configgpio(GPIO_PPM_IN); + px4_arch_configgpio(GPIO_PPM_IN); rc_io_invert(false); } else if (_rc_scan_locked @@ -1416,7 +1386,7 @@ PX4FMU::cycle() } else { // disable CPPM input by mapping it away from the timer capture input - stm32_unconfiggpio(GPIO_PPM_IN); + px4_arch_unconfiggpio(GPIO_PPM_IN); // Scan the next protocol set_rc_scan_state(RC_SCAN_SBUS); } @@ -1514,7 +1484,7 @@ PX4FMU::control_callback(uintptr_t handle, } /* throttle not arming - mark throttle input as invalid */ - if (arm_nothrottle()) { + if (arm_nothrottle() && !_armed.in_esc_calibration_mode) { if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE || control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) && control_index == actuator_controls_s::INDEX_THROTTLE) { @@ -1552,8 +1522,10 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) case MODE_4PWM: case MODE_2PWM2CAP: case MODE_3PWM1CAP: +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6 case MODE_6PWM: -#ifdef CONFIG_ARCH_BOARD_AEROCORE +#endif +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8 case MODE_8PWM: #endif ret = pwm_ioctl(filp, cmd, arg); @@ -1599,7 +1571,12 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; case PWM_SERVO_DISARM: - update_pwm_out_state(false); + + /* Ignore disarm if disarmed PWM is set already. */ + if (_num_disarmed_set == 0) { + update_pwm_out_state(false); + } + break; case PWM_SERVO_GET_DEFAULT_UPDATE_RATE: @@ -1796,7 +1773,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } -#ifdef CONFIG_ARCH_BOARD_AEROCORE +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8 case PWM_SERVO_SET(7): case PWM_SERVO_SET(6): @@ -1807,6 +1784,8 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) #endif +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6 + case PWM_SERVO_SET(5): case PWM_SERVO_SET(4): if (_mode < MODE_6PWM) { @@ -1814,6 +1793,8 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } +#endif + /* FALLTHROUGH */ case PWM_SERVO_SET(3): if (_mode < MODE_4PWM) { @@ -1840,7 +1821,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; -#ifdef CONFIG_ARCH_BOARD_AEROCORE +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8 case PWM_SERVO_GET(7): case PWM_SERVO_GET(6): @@ -1851,6 +1832,8 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) #endif +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6 + case PWM_SERVO_GET(5): case PWM_SERVO_GET(4): if (_mode < MODE_6PWM) { @@ -1858,6 +1841,8 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } +#endif + /* FALLTHROUGH */ case PWM_SERVO_GET(3): if (_mode < MODE_4PWM) { @@ -1882,9 +1867,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_GET_RATEGROUP(1): case PWM_SERVO_GET_RATEGROUP(2): case PWM_SERVO_GET_RATEGROUP(3): +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6 case PWM_SERVO_GET_RATEGROUP(4): case PWM_SERVO_GET_RATEGROUP(5): -#ifdef CONFIG_ARCH_BOARD_AEROCORE +#endif +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8 case PWM_SERVO_GET_RATEGROUP(6): case PWM_SERVO_GET_RATEGROUP(7): #endif @@ -1894,16 +1881,20 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_GET_COUNT: case MIXERIOCGETOUTPUTCOUNT: switch (_mode) { -#ifdef CONFIG_ARCH_BOARD_AEROCORE + +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8 case MODE_8PWM: *(unsigned *)arg = 8; break; #endif +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6 + case MODE_6PWM: *(unsigned *)arg = 6; break; +#endif case MODE_4PWM: *(unsigned *)arg = 4; @@ -1951,14 +1942,14 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) set_mode(MODE_4PWM); break; -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) \ - || defined(CONFIG_ARCH_BOARD_MINDPX_V2) +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >=6 case 6: set_mode(MODE_6PWM); break; #endif -#if defined(CONFIG_ARCH_BOARD_AEROCORE) + +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >=8 case 8: set_mode(MODE_8PWM); @@ -2037,9 +2028,9 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) arg == DSMX8_BIND_PULSES) { dsm_bind(DSM_CMD_BIND_POWER_DOWN, 0); - usleep(500000); dsm_bind(DSM_CMD_BIND_SET_RX_OUT, 0); + usleep(500000); dsm_bind(DSM_CMD_BIND_POWER_UP, 0); usleep(72000); @@ -2142,22 +2133,11 @@ PX4FMU::write(file *filp, const char *buffer, size_t len) unsigned count = len / 2; uint16_t values[8]; -#ifdef CONFIG_ARCH_BOARD_AEROCORE - - if (count > 8) { - // we have at most 8 outputs - count = 8; + if (count > BOARD_HAS_PWM) { + // we have at most BOARD_HAS_PWM outputs + count = BOARD_HAS_PWM; } -#else - - if (count > 6) { - // we have at most 6 outputs - count = 6; - } - -#endif - // allow for misaligned values memcpy(values, buffer, count * 2); @@ -2173,301 +2153,21 @@ PX4FMU::write(file *filp, const char *buffer, size_t len) void PX4FMU::sensor_reset(int ms) { -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) - if (ms < 1) { ms = 1; } - /* disable SPI bus */ - stm32_configgpio(GPIO_SPI_CS_GYRO_OFF); - stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF); - stm32_configgpio(GPIO_SPI_CS_BARO_OFF); - stm32_configgpio(GPIO_SPI_CS_MPU_OFF); - - stm32_gpiowrite(GPIO_SPI_CS_GYRO_OFF, 0); - stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG_OFF, 0); - stm32_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0); - stm32_gpiowrite(GPIO_SPI_CS_MPU_OFF, 0); - - stm32_configgpio(GPIO_SPI1_SCK_OFF); - stm32_configgpio(GPIO_SPI1_MISO_OFF); - stm32_configgpio(GPIO_SPI1_MOSI_OFF); - - stm32_gpiowrite(GPIO_SPI1_SCK_OFF, 0); - stm32_gpiowrite(GPIO_SPI1_MISO_OFF, 0); - stm32_gpiowrite(GPIO_SPI1_MOSI_OFF, 0); - - stm32_configgpio(GPIO_GYRO_DRDY_OFF); - stm32_configgpio(GPIO_MAG_DRDY_OFF); - stm32_configgpio(GPIO_ACCEL_DRDY_OFF); - stm32_configgpio(GPIO_EXTI_MPU_DRDY_OFF); - - stm32_gpiowrite(GPIO_GYRO_DRDY_OFF, 0); - stm32_gpiowrite(GPIO_MAG_DRDY_OFF, 0); - stm32_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0); - stm32_gpiowrite(GPIO_EXTI_MPU_DRDY_OFF, 0); - - /* set the sensor rail off */ - stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); - stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0); - - /* wait for the sensor rail to reach GND */ - usleep(ms * 1000); - warnx("reset done, %d ms", ms); - - /* re-enable power */ - - /* switch the sensor rail back on */ - stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1); - - /* wait a bit before starting SPI, different times didn't influence results */ - usleep(100); - - /* reconfigure the SPI pins */ -#ifdef CONFIG_STM32_SPI1 - stm32_configgpio(GPIO_SPI_CS_GYRO); - stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG); - stm32_configgpio(GPIO_SPI_CS_BARO); - stm32_configgpio(GPIO_SPI_CS_MPU); - - /* De-activate all peripherals, - * required for some peripheral - * state machines - */ - stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); - stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); - stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); - stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); - - stm32_configgpio(GPIO_SPI1_SCK); - stm32_configgpio(GPIO_SPI1_MISO); - stm32_configgpio(GPIO_SPI1_MOSI); - - // // XXX bring up the EXTI pins again - // stm32_configgpio(GPIO_GYRO_DRDY); - // stm32_configgpio(GPIO_MAG_DRDY); - // stm32_configgpio(GPIO_ACCEL_DRDY); - // stm32_configgpio(GPIO_EXTI_MPU_DRDY); - -#endif -#endif -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V4) - - if (ms < 1) { - ms = 1; - } - - /* disable SPI bus */ - stm32_configgpio(GPIO_SPI_CS_OFF_MPU9250); - stm32_configgpio(GPIO_SPI_CS_OFF_HMC5983); - stm32_configgpio(GPIO_SPI_CS_OFF_MS5611); - stm32_configgpio(GPIO_SPI_CS_OFF_ICM_20608_G); - - stm32_gpiowrite(GPIO_SPI_CS_OFF_MPU9250, 0); - stm32_gpiowrite(GPIO_SPI_CS_OFF_HMC5983, 0); - stm32_gpiowrite(GPIO_SPI_CS_OFF_MS5611, 0); - stm32_gpiowrite(GPIO_SPI_CS_OFF_ICM_20608_G, 0); - - stm32_configgpio(GPIO_SPI1_SCK_OFF); - stm32_configgpio(GPIO_SPI1_MISO_OFF); - stm32_configgpio(GPIO_SPI1_MOSI_OFF); - - stm32_gpiowrite(GPIO_SPI1_SCK_OFF, 0); - stm32_gpiowrite(GPIO_SPI1_MISO_OFF, 0); - stm32_gpiowrite(GPIO_SPI1_MOSI_OFF, 0); - - stm32_configgpio(GPIO_DRDY_OFF_MPU9250); - stm32_configgpio(GPIO_DRDY_OFF_HMC5983); - stm32_configgpio(GPIO_DRDY_OFF_ICM_20608_G); - - stm32_gpiowrite(GPIO_DRDY_OFF_MPU9250, 0); - stm32_gpiowrite(GPIO_DRDY_OFF_HMC5983, 0); - stm32_gpiowrite(GPIO_DRDY_OFF_ICM_20608_G, 0); - - /* set the sensor rail off */ - stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); - stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0); - - /* wait for the sensor rail to reach GND */ - usleep(ms * 1000); - warnx("reset done, %d ms", ms); - - /* re-enable power */ - - /* switch the sensor rail back on */ - stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1); - - /* wait a bit before starting SPI, different times didn't influence results */ - usleep(100); - - /* reconfigure the SPI pins */ -#ifdef CONFIG_STM32_SPI1 - stm32_configgpio(GPIO_SPI_CS_MPU9250); - stm32_configgpio(GPIO_SPI_CS_HMC5983); - stm32_configgpio(GPIO_SPI_CS_MS5611); - stm32_configgpio(GPIO_SPI_CS_ICM_20608_G); - - /* De-activate all peripherals, - * required for some peripheral - * state machines - */ - stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1); - stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1); - stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1); - stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1); - - stm32_configgpio(GPIO_SPI1_SCK); - stm32_configgpio(GPIO_SPI1_MISO); - stm32_configgpio(GPIO_SPI1_MOSI); - - // // XXX bring up the EXTI pins again - // stm32_configgpio(GPIO_GYRO_DRDY); - // stm32_configgpio(GPIO_MAG_DRDY); - // stm32_configgpio(GPIO_ACCEL_DRDY); - // stm32_configgpio(GPIO_EXTI_MPU_DRDY); - -#endif -#endif - -#if defined(CONFIG_ARCH_BOARD_MINDPX_V2) - - if (ms < 1) { - ms = 1; - } - - /* disable SPI bus */ - stm32_configgpio(GPIO_SPI_CS_GYRO_OFF); - stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF); - stm32_configgpio(GPIO_SPI_CS_BARO_OFF); - // stm32_configgpio(GPIO_SPI_CS_FRAM_OFF); - stm32_configgpio(GPIO_SPI_CS_MPU_OFF); - - stm32_gpiowrite(GPIO_SPI_CS_GYRO_OFF, 0); - stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG_OFF, 0); - stm32_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0); - // stm32_gpiowrite(GPIO_SPI_CS_FRAM_OFF,0); - stm32_gpiowrite(GPIO_SPI_CS_MPU_OFF, 0); - - stm32_configgpio(GPIO_SPI4_SCK_OFF); - stm32_configgpio(GPIO_SPI4_MISO_OFF); - stm32_configgpio(GPIO_SPI4_MOSI_OFF); - - stm32_gpiowrite(GPIO_SPI4_SCK_OFF, 0); - stm32_gpiowrite(GPIO_SPI4_MISO_OFF, 0); - stm32_gpiowrite(GPIO_SPI4_MOSI_OFF, 0); - - stm32_configgpio(GPIO_GYRO_DRDY_OFF); - stm32_configgpio(GPIO_MAG_DRDY_OFF); - stm32_configgpio(GPIO_ACCEL_DRDY_OFF); - stm32_configgpio(GPIO_EXTI_MPU_DRDY_OFF); - - stm32_gpiowrite(GPIO_GYRO_DRDY_OFF, 0); - stm32_gpiowrite(GPIO_MAG_DRDY_OFF, 0); - stm32_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0); - stm32_gpiowrite(GPIO_EXTI_MPU_DRDY_OFF, 0); - - // /* set the sensor rail off */ - // stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); - // stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0); - // - /* wait for the sensor rail to reach GND */ - usleep(ms * 1000); - warnx("reset done, %d ms", ms); - // - // /* re-enable power */ - // - // /* switch the sensor rail back on */ - // stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1); - // - /* wait a bit before starting SPI, different times didn't influence results */ - usleep(100); - - /* reconfigure the SPI pins */ -#ifdef CONFIG_STM32_SPI4 - stm32_configgpio(GPIO_SPI_CS_GYRO); - stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG); - stm32_configgpio(GPIO_SPI_CS_BARO); - // stm32_configgpio(GPIO_SPI_CS_FRAM); - stm32_configgpio(GPIO_SPI_CS_MPU); - - /* De-activate all peripherals, - * required for some peripheral - * state machines - */ - stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); - stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); - stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); - stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1); - stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); - - stm32_configgpio(GPIO_SPI4_SCK); - stm32_configgpio(GPIO_SPI4_MISO); - stm32_configgpio(GPIO_SPI4_MOSI); - - // // XXX bring up the EXTI pins again - // stm32_configgpio(GPIO_GYRO_DRDY); - // stm32_configgpio(GPIO_MAG_DRDY); - // stm32_configgpio(GPIO_ACCEL_DRDY); - -#endif -#endif + board_spi_reset(ms); } void PX4FMU::peripheral_reset(int ms) { -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) - if (ms < 1) { ms = 10; } - /* set the peripheral rails off */ - stm32_configgpio(GPIO_VDD_5V_PERIPH_EN); - stm32_gpiowrite(GPIO_VDD_5V_PERIPH_EN, 1); - - /* wait for the peripheral rail to reach GND */ - usleep(ms * 1000); - warnx("reset done, %d ms", ms); - - /* re-enable power */ - - /* switch the peripheral rail back on */ - stm32_gpiowrite(GPIO_VDD_5V_PERIPH_EN, 0); -#endif -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V4) - - if (ms < 1) { - ms = 10; - } - - /* set the peripheral rails off */ - stm32_configgpio(GPIO_PERIPH_3V3_EN); - - stm32_gpiowrite(GPIO_PERIPH_3V3_EN, 0); - - bool last = stm32_gpioread(GPIO_SPEKTRUM_PWR_EN); - /* Keep Spektum on to discharge rail*/ - stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, 1); - - /* wait for the peripheral rail to reach GND */ - usleep(ms * 1000); - warnx("reset done, %d ms", ms); - - /* re-enable power */ - - /* switch the peripheral rail back on */ - stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, last); - stm32_gpiowrite(GPIO_PERIPH_3V3_EN, 1); -#endif -#if defined(CONFIG_ARCH_BOARD_MINDPX_V2) - - if (ms < 1) { - ms = 10; - } - -#endif + board_peripheral_reset(ms); } void @@ -2479,35 +2179,37 @@ PX4FMU::gpio_reset(void) */ for (unsigned i = 0; i < _ngpio; i++) { if (_gpio_tab[i].input != 0) { - stm32_configgpio(_gpio_tab[i].input); + px4_arch_configgpio(_gpio_tab[i].input); } else if (_gpio_tab[i].output != 0) { - stm32_configgpio(_gpio_tab[i].output); + px4_arch_configgpio(_gpio_tab[i].output); } } -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) +#if defined(GPIO_GPIO_DIR) /* if we have a GPIO direction control, set it to zero (input) */ - stm32_gpiowrite(GPIO_GPIO_DIR, 0); - stm32_configgpio(GPIO_GPIO_DIR); + px4_arch_gpiowrite(GPIO_GPIO_DIR, 0); + px4_arch_configgpio(GPIO_GPIO_DIR); #endif } void PX4FMU::gpio_set_function(uint32_t gpios, int function) { -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) +#if defined(BOARD_GPIO_SHARED_BUFFERED_BITS) && defined(GPIO_GPIO_DIR) /* * GPIOs 0 and 1 must have the same direction as they are buffered * by a shared 2-port driver. Any attempt to set either sets both. */ - if (gpios & 3) { - gpios |= 3; + if (gpios & BOARD_GPIO_SHARED_BUFFERED_BITS) { + gpios |= BOARD_GPIO_SHARED_BUFFERED_BITS; /* flip the buffer to output mode if required */ - if (GPIO_SET_OUTPUT == function) { - stm32_gpiowrite(GPIO_GPIO_DIR, 1); + if (GPIO_SET_OUTPUT == function || + GPIO_SET_OUTPUT_LOW == function || + GPIO_SET_OUTPUT_HIGH == function) { + px4_arch_gpiowrite(GPIO_GPIO_DIR, 1); } } @@ -2518,16 +2220,24 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function) if (gpios & (1 << i)) { switch (function) { case GPIO_SET_INPUT: - stm32_configgpio(_gpio_tab[i].input); + px4_arch_configgpio(_gpio_tab[i].input); break; case GPIO_SET_OUTPUT: - stm32_configgpio(_gpio_tab[i].output); + px4_arch_configgpio(_gpio_tab[i].output); + break; + + case GPIO_SET_OUTPUT_LOW: + px4_arch_configgpio((_gpio_tab[i].output & ~(GPIO_OUTPUT_SET)) | GPIO_OUTPUT_CLEAR); + break; + + case GPIO_SET_OUTPUT_HIGH: + px4_arch_configgpio((_gpio_tab[i].output & ~(GPIO_OUTPUT_CLEAR)) | GPIO_OUTPUT_SET); break; case GPIO_SET_ALT_1: if (_gpio_tab[i].alt != 0) { - stm32_configgpio(_gpio_tab[i].alt); + px4_arch_configgpio(_gpio_tab[i].alt); } break; @@ -2535,11 +2245,11 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function) } } -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) +#if defined(BOARD_GPIO_SHARED_BUFFERED_BITS) && defined(GPIO_GPIO_DIR) /* flip buffer to input mode if required */ - if ((GPIO_SET_INPUT == function) && (gpios & 3)) { - stm32_gpiowrite(GPIO_GPIO_DIR, 0); + if ((GPIO_SET_INPUT == function) && (gpios & BOARD_GPIO_SHARED_BUFFERED_BITS)) { + px4_arch_gpiowrite(GPIO_GPIO_DIR, 0); } #endif @@ -2552,7 +2262,7 @@ PX4FMU::gpio_write(uint32_t gpios, int function) for (unsigned i = 0; i < _ngpio; i++) if (gpios & (1 << i)) { - stm32_gpiowrite(_gpio_tab[i].output, value); + px4_arch_gpiowrite(_gpio_tab[i].output, value); } } @@ -2562,7 +2272,7 @@ PX4FMU::gpio_read(void) uint32_t bits = 0; for (unsigned i = 0; i < _ngpio; i++) - if (stm32_gpioread(_gpio_tab[i].input)) { + if (px4_arch_gpioread(_gpio_tab[i].input)) { bits |= (1 << i); } @@ -2720,6 +2430,8 @@ PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg) break; case GPIO_SET_OUTPUT: + case GPIO_SET_OUTPUT_LOW: + case GPIO_SET_OUTPUT_HIGH: case GPIO_SET_INPUT: case GPIO_SET_ALT_1: gpio_set_function(arg, cmd); @@ -2806,21 +2518,19 @@ fmu_new_mode(PortMode new_mode) break; case PORT_FULL_PWM: -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 4 /* select 4-pin PWM mode */ servo_mode = PX4FMU::MODE_4PWM; #endif -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) \ - || defined(CONFIG_ARCH_BOARD_MINDPX_V2) +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 6 servo_mode = PX4FMU::MODE_6PWM; #endif -#if defined(CONFIG_ARCH_BOARD_AEROCORE) +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 8 servo_mode = PX4FMU::MODE_8PWM; #endif break; -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) \ - || defined(CONFIG_ARCH_BOARD_MINDPX_V2) +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6 case PORT_PWM4: /* select 4-pin PWM mode */ @@ -2828,7 +2538,7 @@ fmu_new_mode(PortMode new_mode) break; case PORT_PWM3: - /* select 4-pin PWM mode */ + /* select 3-pin PWM mode */ servo_mode = PX4FMU::MODE_3PWM; break; @@ -2851,7 +2561,7 @@ fmu_new_mode(PortMode new_mode) #endif /* mixed modes supported on v1 board only */ -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) +#if defined(BOARD_HAS_MULTI_PURPOSE_GPIO) case PORT_FULL_SERIAL: /* set all multi-GPIOs to serial mode */ @@ -3253,8 +2963,7 @@ fmu_main(int argc, char *argv[]) } else if (!strcmp(verb, "mode_pwm")) { new_mode = PORT_FULL_PWM; -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) \ - || defined(CONFIG_ARCH_BOARD_MINDPX_V2) +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6 } else if (!strcmp(verb, "mode_pwm4")) { new_mode = PORT_PWM4; @@ -3271,7 +2980,7 @@ fmu_main(int argc, char *argv[]) } else if (!strcmp(verb, "mode_pwm2cap2")) { new_mode = PORT_PWM2CAP2; #endif -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) +#if defined(BOARD_HAS_MULTI_PURPOSE_GPIO) } else if (!strcmp(verb, "mode_serial")) { new_mode = PORT_FULL_SERIAL; @@ -3342,11 +3051,10 @@ fmu_main(int argc, char *argv[]) } fprintf(stderr, "FMU: unrecognised command %s, try:\n", verb); -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) +#if defined(BOARD_HAS_MULTI_PURPOSE_GPIO) fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test, fake, sensor_reset, id\n"); -#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) || defined(CONFIG_ARCH_BOARD_AEROCORE) \ - || defined(CONFIG_ARCH_BOARD_MINDPX_V2) +#elif defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6 fprintf(stderr, " mode_gpio, mode_pwm, mode_pwm4, test, sensor_reset [milliseconds], i2c \n"); #endif exit(1); diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 58850cdedf..652dc93073 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -77,10 +77,6 @@ #include #include -#include -#include -#include -#include #include #include #include @@ -222,6 +218,16 @@ public: */ int print_debug(); + /* + * To test what happens if IO stops receiving updates from FMU. + * + * @param is_fail true for failure condition, false for normal operation. + */ + void test_fmu_fail(bool is_fail) + { + _test_fmu_fail = is_fail; + }; + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 /** * Set the DSM VCC is controlled by relay one flag @@ -315,8 +321,11 @@ private: int32_t _rssi_pwm_chan; ///< RSSI PWM input channel int32_t _rssi_pwm_max; ///< max RSSI input on PWM channel int32_t _rssi_pwm_min; ///< min RSSI input on PWM channel + bool _analog_rc_rssi_stable; ///< true when analog RSSI input is stable + float _analog_rc_rssi_volt; ///< analog RSSI voltage float _last_throttle; ///< last throttle value for battery calculation + bool _test_fmu_fail; ///< To test what happens if IO looses FMU #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 bool _dsm_vcc_ctl; ///< true if relay 1 controls DSM satellite RX power @@ -549,7 +558,10 @@ PX4IO::PX4IO(device::Device *interface) : _rssi_pwm_chan(0), _rssi_pwm_max(0), _rssi_pwm_min(0), - _last_throttle(0.0f) + _analog_rc_rssi_stable(false), + _analog_rc_rssi_volt(-1.0f), + _last_throttle(0.0f), + _test_fmu_fail(false) #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 , _dsm_vcc_ctl(false) #endif @@ -900,7 +912,7 @@ PX4IO::init() ret = register_driver(PWM_OUTPUT0_DEVICE_PATH, &fops, 0666, (void *)this); if (ret == OK) { - DEVICE_LOG("default PWM output device"); + PX4_INFO("default PWM output device"); _primary_pwm_device = true; } @@ -1126,7 +1138,7 @@ PX4IO::task_main() } int32_t safety_param_val; - param_t safety_param = param_find("RC_FAILS_THR"); + param_t safety_param = param_find("CBRK_IO_SAFETY"); if (safety_param != PARAM_INVALID) { @@ -1166,28 +1178,49 @@ PX4IO::task_main() (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_REVERSE, pwm_invert_mask); - float trim_val; + float param_val; param_t parm_handle; parm_handle = param_find("TRIM_ROLL"); if (parm_handle != PARAM_INVALID) { - param_get(parm_handle, &trim_val); - (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_ROLL, FLOAT_TO_REG(trim_val)); + param_get(parm_handle, ¶m_val); + (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_ROLL, FLOAT_TO_REG(param_val)); } parm_handle = param_find("TRIM_PITCH"); if (parm_handle != PARAM_INVALID) { - param_get(parm_handle, &trim_val); - (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_PITCH, FLOAT_TO_REG(trim_val)); + param_get(parm_handle, ¶m_val); + (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_PITCH, FLOAT_TO_REG(param_val)); } parm_handle = param_find("TRIM_YAW"); if (parm_handle != PARAM_INVALID) { - param_get(parm_handle, &trim_val); - (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_YAW, FLOAT_TO_REG(trim_val)); + param_get(parm_handle, ¶m_val); + (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_YAW, FLOAT_TO_REG(param_val)); + } + + parm_handle = param_find("FW_MAN_R_SC"); + + if (parm_handle != PARAM_INVALID) { + param_get(parm_handle, ¶m_val); + (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SCALE_ROLL, FLOAT_TO_REG(param_val)); + } + + parm_handle = param_find("FW_MAN_P_SC"); + + if (parm_handle != PARAM_INVALID) { + param_get(parm_handle, ¶m_val); + (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SCALE_PITCH, FLOAT_TO_REG(param_val)); + } + + parm_handle = param_find("FW_MAN_Y_SC"); + + if (parm_handle != PARAM_INVALID) { + param_get(parm_handle, ¶m_val); + (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SCALE_YAW, FLOAT_TO_REG(param_val)); } /* S.BUS output */ @@ -1328,8 +1361,13 @@ PX4IO::io_set_control_state(unsigned group) _last_throttle = controls.control[3]; } - /* copy values to registers in IO */ - return io_reg_set(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT, regs, _max_controls); + if (!_test_fmu_fail) { + /* copy values to registers in IO */ + return io_reg_set(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT, regs, _max_controls); + + } else { + return OK; + } } @@ -1716,6 +1754,16 @@ PX4IO::io_handle_vservo(uint16_t vservo, uint16_t vrssi) _servorail_status.voltage_v = vservo * 0.001f; _servorail_status.rssi_v = vrssi * 0.001f; + if (_analog_rc_rssi_volt < 0.0f) { + _analog_rc_rssi_volt = _servorail_status.rssi_v; + } + + _analog_rc_rssi_volt = _analog_rc_rssi_volt * 0.99f + _servorail_status.rssi_v * 0.01f; + + if (_analog_rc_rssi_volt > 2.5f) { + _analog_rc_rssi_stable = true; + } + /* lazily publish the servorail voltages */ if (_to_servorail != nullptr) { orb_publish(ORB_ID(servorail_status), _to_servorail, &_servorail_status); @@ -1794,7 +1842,24 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) input_rc.timestamp_publication = hrt_absolute_time(); input_rc.rc_ppm_frame_length = regs[PX4IO_P_RAW_RC_DATA]; - input_rc.rssi = regs[PX4IO_P_RAW_RC_NRSSI]; + + if (!_analog_rc_rssi_stable) { + input_rc.rssi = 255;// we do not actually get digital RSSI regs[PX4IO_P_RAW_RC_NRSSI]; + + } else { + float rssi_analog = ((_analog_rc_rssi_volt - 0.2f) / 3.0f) * 100.0f; + + if (rssi_analog > 100.0f) { + rssi_analog = 100.0f; + } + + if (rssi_analog < 0.0f) { + rssi_analog = 0.0f; + } + + input_rc.rssi = rssi_analog; + } + input_rc.rc_failsafe = (regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_FAILSAFE); input_rc.rc_lost = !(regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_RC_OK); input_rc.rc_lost_frame_count = regs[PX4IO_P_RAW_LOST_FRAME_COUNT]; @@ -1923,7 +1988,9 @@ PX4IO::io_publish_pwm_outputs() /* get mixer status flags from IO */ uint16_t mixer_status; ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_MIXER, &mixer_status, sizeof(mixer_status) / sizeof(uint16_t)); - memcpy(&motor_limits, &mixer_status, sizeof(motor_limits)); + motor_limits.lower_limit = mixer_status & PX4IO_P_STATUS_MIXER_LOWER_LIMIT; + motor_limits.upper_limit = mixer_status & PX4IO_P_STATUS_MIXER_UPPER_LIMIT; + motor_limits.yaw = mixer_status & PX4IO_P_STATUS_MIXER_YAW_LIMIT; if (ret != OK) { return ret; @@ -2685,8 +2752,15 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) ret = -EINVAL; } else { - /* send a direct PWM value */ - ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg); + if (!_test_fmu_fail) { + /* send a direct PWM value */ + ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg); + + } else { + /* Just silently accept the ioctl without doing anything + * in test mode. */ + ret = OK; + } } break; @@ -2987,7 +3061,14 @@ PX4IO::write(file * /*filp*/, const char *buffer, size_t len) if (count > 0) { perf_begin(_perf_write); - int ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count); + + int ret = OK; + + /* The write() is silently ignored in test mode. */ + if (!_test_fmu_fail) { + ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count); + } + perf_end(_perf_write); if (ret != OK) { @@ -3819,8 +3900,31 @@ px4io_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "test_fmu_fail")) { + if (g_dev != nullptr) { + g_dev->test_fmu_fail(true); + exit(0); + + } else { + + errx(1, "px4io must be started first"); + } + } + + if (!strcmp(argv[1], "test_fmu_ok")) { + if (g_dev != nullptr) { + g_dev->test_fmu_fail(false); + exit(0); + + } else { + + errx(1, "px4io must be started first"); + } + } + out: errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug ',\n" "'recovery', 'limit ', 'current', 'bind', 'checkcrc', 'safety_on', 'safety_off',\n" - "'forceupdate', 'update', 'sbus1_out', 'sbus2_out', 'rssi_analog' or 'rssi_pwm'"); + "'forceupdate', 'update', 'sbus1_out', 'sbus2_out', 'rssi_analog' or 'rssi_pwm',\n" + "'test_fmu_fail', 'test_fmu_ok'"); } diff --git a/src/drivers/px4io/px4io_params.c b/src/drivers/px4io/px4io_params.c index bc1e349b85..2be6c5efc3 100644 --- a/src/drivers/px4io/px4io_params.c +++ b/src/drivers/px4io/px4io_params.c @@ -131,7 +131,7 @@ PARAM_DEFINE_INT32(PWM_MAIN_REV7, 0); PARAM_DEFINE_INT32(PWM_MAIN_REV8, 0); /** - * Enable S.BUS out + * S.BUS out * * Set to 1 to enable S.BUS version 1 output instead of RSSI. * diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp index 258fa0b5db..f5a08d8545 100644 --- a/src/drivers/px4io/px4io_serial.cpp +++ b/src/drivers/px4io/px4io_serial.cpp @@ -228,8 +228,8 @@ PX4IO_serial::~PX4IO_serial() irq_detach(PX4IO_SERIAL_VECTOR); /* restore the GPIOs */ - stm32_unconfiggpio(PX4IO_SERIAL_TX_GPIO); - stm32_unconfiggpio(PX4IO_SERIAL_RX_GPIO); + px4_arch_unconfiggpio(PX4IO_SERIAL_TX_GPIO); + px4_arch_unconfiggpio(PX4IO_SERIAL_RX_GPIO); /* and kill our semaphores */ px4_sem_destroy(&_completion_semaphore); @@ -264,8 +264,8 @@ PX4IO_serial::init() } /* configure pins for serial use */ - stm32_configgpio(PX4IO_SERIAL_TX_GPIO); - stm32_configgpio(PX4IO_SERIAL_RX_GPIO); + px4_arch_configgpio(PX4IO_SERIAL_TX_GPIO); + px4_arch_configgpio(PX4IO_SERIAL_RX_GPIO); /* reset & configure the UART */ rCR1 = 0; diff --git a/src/drivers/qshell/qurt/qshell.cpp b/src/drivers/qshell/qurt/qshell.cpp index 5be2495450..56ace44e03 100644 --- a/src/drivers/qshell/qurt/qshell.cpp +++ b/src/drivers/qshell/qurt/qshell.cpp @@ -62,6 +62,7 @@ #include "DriverFramework.hpp" extern void init_app_map(std::map &apps); +extern void list_builtins(std::map &apps); using std::map; using std::string; @@ -146,6 +147,11 @@ int QShell::run_cmd(const std::vector &appargs) // command is appargs[0] std::string command = appargs[0]; + if (command.compare("help") == 0) { + list_builtins(apps); + return 0; + } + //replaces app.find with iterator code to avoid null pointer exception for (map::iterator it = apps.begin(); it != apps.end(); ++it) { if (it->first == command) { diff --git a/src/drivers/qshell/qurt/qshell_start_qurt.cpp b/src/drivers/qshell/qurt/qshell_start_qurt.cpp index b769147817..ca6119fe54 100644 --- a/src/drivers/qshell/qurt/qshell_start_qurt.cpp +++ b/src/drivers/qshell/qurt/qshell_start_qurt.cpp @@ -56,17 +56,17 @@ int qshell_entry(int argc, char **argv) { //px4::init(argc, argv, "qshell"); - PX4_INFO("qshell entry....."); + PX4_DEBUG("qshell entry....."); QShell qshell; qshell.main(); - PX4_INFO("goodbye"); + PX4_DEBUG("goodbye"); return 0; } static void usage() { - PX4_DEBUG("usage: qshell {start|stop|status}"); + PX4_INFO("usage: qshell {start|stop|status}"); } int qshell_main(int argc, char *argv[]) { @@ -78,12 +78,12 @@ int qshell_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (QShell::appState.isRunning()) { - PX4_DEBUG("already running"); + PX4_INFO("already running"); /* this is not an error */ return 0; } - PX4_INFO("before starting the qshell_entry task"); + PX4_DEBUG("before starting the qshell_entry task"); daemon_task = px4_task_spawn_cmd("qshell", SCHED_DEFAULT, @@ -102,10 +102,10 @@ int qshell_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (QShell::appState.isRunning()) { - PX4_DEBUG("is running"); + PX4_INFO("is running"); } else { - PX4_DEBUG("not started"); + PX4_INFO("not started"); } return 0; diff --git a/src/drivers/rgbled_pwm/CMakeLists.txt b/src/drivers/rgbled_pwm/CMakeLists.txt index 7bc768d91b..0805c8aea9 100644 --- a/src/drivers/rgbled_pwm/CMakeLists.txt +++ b/src/drivers/rgbled_pwm/CMakeLists.txt @@ -36,7 +36,6 @@ px4_add_module( COMPILE_FLAGS -Os SRCS - drv_led_pwm.cpp rgbled_pwm.cpp DEPENDS platforms__common diff --git a/src/drivers/rgbled_pwm/rgbled_pwm.cpp b/src/drivers/rgbled_pwm/rgbled_pwm.cpp index cf2a91b91c..1857d9447c 100644 --- a/src/drivers/rgbled_pwm/rgbled_pwm.cpp +++ b/src/drivers/rgbled_pwm/rgbled_pwm.cpp @@ -170,11 +170,9 @@ RGBLED_PWM::init() { /* switch off LED on start */ CDev::init(); -#if defined(CONFIG_ARCH_BOARD_MINDPX_V2) led_pwm_servo_init(); send_led_enable(false); send_led_rgb(); -#endif return OK; } void @@ -536,7 +534,6 @@ RGBLED_PWM::send_led_enable(bool enable) int RGBLED_PWM::send_led_rgb() { -#if defined(CONFIG_ARCH_BOARD_MINDPX_V2) if (_enable) { led_pwm_servo_set(0, _r); @@ -549,7 +546,6 @@ RGBLED_PWM::send_led_rgb() led_pwm_servo_set(2, 0); } -#endif return (OK); } diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index c1f4a01960..d3eda8609e 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -54,7 +54,6 @@ #include #include -#include #include #include diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 0106261dd6..13492ca2f7 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -423,14 +423,14 @@ SF0X::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; } - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); if (!_reports->resize(arg)) { - irqrestore(flags); + px4_leave_critical_section(flags); return -ENOMEM; } - irqrestore(flags); + px4_leave_critical_section(flags); return OK; } diff --git a/src/drivers/sf0x/sf0x_tests/CMakeLists.txt b/src/drivers/sf0x/sf0x_tests/CMakeLists.txt new file mode 100644 index 0000000000..3ded382609 --- /dev/null +++ b/src/drivers/sf0x/sf0x_tests/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2016 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. +# +############################################################################ +px4_add_module( + MODULE drivers__sf0x__sf0x_tests + MAIN sf0x_tests + COMPILE_FLAGS + SRCS + SF0XTest.cpp + ../sf0x_parser.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/unittests/sf0x_test.cpp b/src/drivers/sf0x/sf0x_tests/SF0XTest.cpp similarity index 57% rename from unittests/sf0x_test.cpp rename to src/drivers/sf0x/sf0x_tests/SF0XTest.cpp index 56e673a06d..af95214c64 100644 --- a/unittests/sf0x_test.cpp +++ b/src/drivers/sf0x/sf0x_tests/SF0XTest.cpp @@ -1,19 +1,35 @@ -#include -#include -#include -#include +#include -#include #include + #include -#include "gtest/gtest.h" +#include +#include -TEST(SF0XTest, SF0X) +extern "C" __EXPORT int sf0x_tests_main(int argc, char *argv[]); + +class SF0XTest : public UnitTest +{ +public: + virtual bool run_tests(void); + +private: + bool sf0xTest(); +}; + +bool SF0XTest::run_tests(void) +{ + ut_run_test(sf0xTest); + + return (_tests_failed == 0); +} + +bool SF0XTest::sf0xTest(void) { const char _LINE_MAX = 20; - char _linebuf[_LINE_MAX]; - _linebuf[0] = '\0'; + //char _linebuf[_LINE_MAX]; + //_linebuf[0] = '\0'; const char *lines[] = {"0.01\r\n", "0.02\r\n", @@ -39,7 +55,6 @@ TEST(SF0XTest, SF0X) unsigned _parsebuf_index = 0; for (unsigned l = 0; l < sizeof(lines) / sizeof(lines[0]); l++) { - //printf("\n%s", _linebuf); int parse_ret; @@ -48,6 +63,19 @@ TEST(SF0XTest, SF0X) parse_ret = sf0x_parser(lines[l][i], _parserbuf, &_parsebuf_index, &state, &dist_m); if (parse_ret == 0) { + if (l == 0) { + ut_test(dist_m - 0.010000f < 0.001f); + + } else if (l == 1) { + ut_test(dist_m - 0.020000f < 0.001f); + + } else if (l == 2) { + ut_test(dist_m - 0.030000f < 0.001f); + + } else if (l == 3) { + ut_test(dist_m - 0.040000f < 0.001f); + } + //printf("\nparsed: %f %s\n", dist_m, (parse_ret == 0) ? "OK" : ""); } } @@ -56,5 +84,8 @@ TEST(SF0XTest, SF0X) } - warnx("test finished"); + return true; } + +ut_declare_test_c(sf0x_tests_main, SF0XTest) + diff --git a/src/drivers/sf10a/CMakeLists.txt b/src/drivers/sf1xx/CMakeLists.txt similarity index 97% rename from src/drivers/sf10a/CMakeLists.txt rename to src/drivers/sf1xx/CMakeLists.txt index 5db2934a74..098bd54171 100644 --- a/src/drivers/sf10a/CMakeLists.txt +++ b/src/drivers/sf1xx/CMakeLists.txt @@ -32,12 +32,12 @@ ############################################################################ px4_add_module( - MODULE drivers__sf10a - MAIN sf10a + MODULE drivers__sf1xx + MAIN sf1xx COMPILE_FLAGS -Os SRCS - sf10a.cpp + sf1xx.cpp DEPENDS platforms__common ) diff --git a/src/drivers/sf10a/sf10a.cpp b/src/drivers/sf1xx/sf1xx.cpp similarity index 80% rename from src/drivers/sf10a/sf10a.cpp rename to src/drivers/sf1xx/sf1xx.cpp index 967aae5176..a2b9e13f5f 100644 --- a/src/drivers/sf10a/sf10a.cpp +++ b/src/drivers/sf1xx/sf1xx.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2016 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,12 +32,13 @@ ****************************************************************************/ /** - * @file sf10a.cpp + * @file sf1xx.cpp * * @author ecmnet + * @author Vasily Evseenko * - * Driver for the Lightware SF10x lidar range finder series. - * Default I2C address 0x55 is used. + * Driver for the Lightware SF1xx lidar range finder series. + * Default I2C address 0x66 is used. */ #include @@ -70,25 +71,14 @@ #include #include -#include #include #include /* Configuration Constants */ -#define SF10A_BUS PX4_I2C_BUS_EXPANSION -#define SF10A_BASEADDR 0x55 -#define SF10A_DEVICE_PATH "/dev/sf10a" - -/* Device limits */ -#define SF10A_MIN_DISTANCE (0.0f) -#define SF10A_MAX_DISTANCE (25.0f) - - -/* conversion rates */ - -//#define SF10A_CONVERSION_INTERVAL 25000 // Overclocking SF10a to 40 Hz -#define SF10A_CONVERSION_INTERVAL 31250 // Maximum rate according to datasheet is 32Hz +#define SF1XX_BUS PX4_I2C_BUS_EXPANSION +#define SF1XX_BASEADDR 0x66 +#define SF1XX_DEVICE_PATH "/dev/sf1xx" /* oddly, ERROR is not defined for c++ */ @@ -101,11 +91,11 @@ static const int ERROR = -1; # error This requires CONFIG_SCHED_WORKQUEUE. #endif -class SF10A : public device::I2C +class SF1XX : public device::I2C { public: - SF10A(int bus = SF10A_BUS, int address = SF10A_BASEADDR); - virtual ~SF10A(); + SF1XX(int bus = SF1XX_BUS, int address = SF1XX_BASEADDR); + virtual ~SF1XX(); virtual int init(); @@ -123,12 +113,13 @@ protected: private: float _min_distance; float _max_distance; + int _conversion_interval; work_s _work; - ringbuffer::RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; bool _sensor_ok; - int _measure_ticks; - int _class_instance; - int _orb_class_instance; + int _measure_ticks; + int _class_instance; + int _orb_class_instance; orb_advert_t _distance_sensor_topic; @@ -162,8 +153,8 @@ private: /** * Set the min and max distance thresholds if you want the end points of the sensors - * range to be brought in at all, otherwise it will use the defaults SF10A_MIN_DISTANCE - * and SF10A_MAX_DISTANCE + * range to be brought in at all, otherwise it will use the defaults SF1XX_MIN_DISTANCE + * and SF1XX_MAX_DISTANCE */ void set_minimum_distance(float min); void set_maximum_distance(float max); @@ -191,21 +182,22 @@ private: /* * Driver 'main' command. */ -extern "C" __EXPORT int sf10a_main(int argc, char *argv[]); +extern "C" __EXPORT int sf1xx_main(int argc, char *argv[]); -SF10A::SF10A(int bus, int address) : - I2C("SF10A", SF10A_DEVICE_PATH, bus, address, 100000), - _min_distance(SF10A_MIN_DISTANCE), - _max_distance(SF10A_MAX_DISTANCE), +SF1XX::SF1XX(int bus, int address) : + I2C("SF1XX", SF1XX_DEVICE_PATH, bus, address, 400000), + _min_distance(-1.0f), + _max_distance(-1.0f), + _conversion_interval(-1), _reports(nullptr), _sensor_ok(false), _measure_ticks(0), _class_instance(-1), _orb_class_instance(-1), _distance_sensor_topic(nullptr), - _sample_perf(perf_alloc(PC_ELAPSED, "sf10a_read")), - _comms_errors(perf_alloc(PC_COUNT, "sf10a_com_err")), - _buffer_overflows(perf_alloc(PC_COUNT, "sf10a_buf_of")) + _sample_perf(perf_alloc(PC_ELAPSED, "sf1xx_read")), + _comms_errors(perf_alloc(PC_COUNT, "sf1xx_com_err")), + _buffer_overflows(perf_alloc(PC_COUNT, "sf1xx_buf_of")) { /* enable debug() calls */ @@ -215,7 +207,7 @@ SF10A::SF10A(int bus, int address) : memset(&_work, 0, sizeof(_work)); } -SF10A::~SF10A() +SF1XX::~SF1XX() { /* make sure we are truly inactive */ stop(); @@ -225,6 +217,10 @@ SF10A::~SF10A() delete _reports; } + if (_distance_sensor_topic != nullptr) { + orb_unadvertise(_distance_sensor_topic); + } + if (_class_instance != -1) { unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance); } @@ -236,9 +232,46 @@ SF10A::~SF10A() } int -SF10A::init() +SF1XX::init() { int ret = ERROR; + int hw_model; + param_get(param_find("SENS_EN_SF1XX"), &hw_model); + + switch (hw_model) { + case 0: + DEVICE_LOG("disabled."); + return ret; + + case 1: /* SF10/a (25m 32Hz) */ + _min_distance = 0.01f; + _max_distance = 25.0f; + _conversion_interval = 31250; + break; + + case 2: /* SF10/b (50m 32Hz) */ + _min_distance = 0.01f; + _max_distance = 50.0f; + _conversion_interval = 31250; + break; + + case 3: /* SF10/c (100m 16Hz) */ + _min_distance = 0.01f; + _max_distance = 100.0f; + _conversion_interval = 62500; + break; + + case 4: + /* SF11/c (120m 20Hz) */ + _min_distance = 0.01f; + _max_distance = 120.0f; + _conversion_interval = 50000; + break; + + default: + DEVICE_LOG("invalid HW model %d.", hw_model); + return ret; + } /* do I2C init (and probe) first */ if (I2C::init() != OK) { @@ -248,7 +281,7 @@ SF10A::init() /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s)); - set_address(SF10A_BASEADDR); + set_address(SF1XX_BASEADDR); if (_reports == nullptr) { return ret; @@ -266,50 +299,51 @@ SF10A::init() DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); } - + // Select altitude register int ret2 = measure(); if (ret2 == 0) { ret = OK; _sensor_ok = true; - DEVICE_LOG("SF10x with address %d found", SF10A_BASEADDR); + DEVICE_LOG("(%dm %dHz) with address %d found", (int)_max_distance, + (int)(1e6f / _conversion_interval), SF1XX_BASEADDR); } return ret; } int -SF10A::probe() +SF1XX::probe() { return measure(); } void -SF10A::set_minimum_distance(float min) +SF1XX::set_minimum_distance(float min) { _min_distance = min; } void -SF10A::set_maximum_distance(float max) +SF1XX::set_maximum_distance(float max) { _max_distance = max; } float -SF10A::get_minimum_distance() +SF1XX::get_minimum_distance() { return _min_distance; } float -SF10A::get_maximum_distance() +SF1XX::get_maximum_distance() { return _max_distance; } int -SF10A::ioctl(struct file *filp, int cmd, unsigned long arg) +SF1XX::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { @@ -336,7 +370,7 @@ SF10A::ioctl(struct file *filp, int cmd, unsigned long arg) bool want_start = (_measure_ticks == 0); /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(SF10A_CONVERSION_INTERVAL); + _measure_ticks = USEC2TICK(_conversion_interval); /* if we need to start the poll state machine, do it */ if (want_start) { @@ -356,7 +390,7 @@ SF10A::ioctl(struct file *filp, int cmd, unsigned long arg) int ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ - if (ticks < USEC2TICK(SF10A_CONVERSION_INTERVAL)) { + if (ticks < USEC2TICK(_conversion_interval)) { return -EINVAL; } @@ -386,14 +420,14 @@ SF10A::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; } - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); if (!_reports->resize(arg)) { - irqrestore(flags); + px4_leave_critical_section(flags); return -ENOMEM; } - irqrestore(flags); + px4_leave_critical_section(flags); return OK; } @@ -424,9 +458,8 @@ SF10A::ioctl(struct file *filp, int cmd, unsigned long arg) } ssize_t -SF10A::read(struct file *filp, char *buffer, size_t buflen) +SF1XX::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct distance_sensor_s); struct distance_sensor_s *rbuf = reinterpret_cast(buffer); int ret = 0; @@ -466,7 +499,7 @@ SF10A::read(struct file *filp, char *buffer, size_t buflen) } /* wait for it to complete */ - usleep(SF10A_CONVERSION_INTERVAL); + usleep(_conversion_interval); /* run the collection phase */ if (OK != collect()) { @@ -485,18 +518,16 @@ SF10A::read(struct file *filp, char *buffer, size_t buflen) } int -SF10A::measure() +SF1XX::measure() { - int ret; /* - * Send the command to begin a measurement. + * Send the command '0' -- read altitude */ - uint8_t cmd[1]; - cmd[0] = SF10A_BASEADDR; - ret = transfer(cmd, 1, nullptr, 0); + uint8_t cmd = 0; + ret = transfer(&cmd, 1, nullptr, 0); if (OK != ret) { perf_count(_comms_errors); @@ -510,16 +541,14 @@ SF10A::measure() } int -SF10A::collect() +SF1XX::collect() { int ret = -EIO; /* read from the sensor */ uint8_t val[2] = {0, 0}; - uint8_t cmd = SF10A_BASEADDR; perf_begin(_sample_perf); - ret = transfer(&cmd, 1, nullptr, 0); ret = transfer(nullptr, 0, &val[0], 2); if (ret < 0) { @@ -562,56 +591,35 @@ SF10A::collect() } void -SF10A::start() +SF1XX::start() { - /* reset the report ring and state machine */ _reports->flush(); + /* set register to '0' */ + measure(); + /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&SF10A::cycle_trampoline, this, 5); - - /* notify about state change */ - struct subsystem_info_s info = { - true, - true, - true, - subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER - }; - static orb_advert_t pub = nullptr; - - if (pub != nullptr) { - orb_publish(ORB_ID(subsystem_info), pub, &info); - - - } else { - pub = orb_advertise(ORB_ID(subsystem_info), &info); - - } + work_queue(HPWORK, &_work, (worker_t)&SF1XX::cycle_trampoline, this, USEC2TICK(_conversion_interval)); } void -SF10A::stop() +SF1XX::stop() { work_cancel(HPWORK, &_work); } void -SF10A::cycle_trampoline(void *arg) +SF1XX::cycle_trampoline(void *arg) { - - SF10A *dev = (SF10A *)arg; + SF1XX *dev = (SF1XX *)arg; dev->cycle(); - } void -SF10A::cycle() +SF1XX::cycle() { - - set_address(SF10A_BASEADDR); - /* Collect results */ if (OK != collect()) { DEVICE_DEBUG("collection error"); @@ -620,25 +628,17 @@ SF10A::cycle() return; } - - /* Trigger measurement */ - if (OK != measure()) { - DEVICE_DEBUG("measure error lidar"); - } - - - /* schedule a fresh cycle call when the measurement is done */ work_queue(HPWORK, &_work, - (worker_t)&SF10A::cycle_trampoline, + (worker_t)&SF1XX::cycle_trampoline, this, - USEC2TICK(SF10A_CONVERSION_INTERVAL)); + USEC2TICK(_conversion_interval)); } void -SF10A::print_info() +SF1XX::print_info() { perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); @@ -650,7 +650,7 @@ SF10A::print_info() /** * Local functions in support of the shell command. */ -namespace sf10a +namespace sf1xx { /* oddly, ERROR is not defined for c++ */ @@ -659,7 +659,7 @@ namespace sf10a #endif const int ERROR = -1; -SF10A *g_dev; +SF1XX *g_dev; void start(); void stop(); @@ -673,14 +673,14 @@ void info(); void start() { - int fd; + int fd = -1; if (g_dev != nullptr) { errx(1, "already started"); } /* create the driver */ - g_dev = new SF10A(SF10A_BUS); + g_dev = new SF1XX(SF1XX_BUS); if (g_dev == nullptr) { goto fail; @@ -691,16 +691,18 @@ start() } /* set the poll rate to default, starts automatic data collection */ - fd = open(SF10A_DEVICE_PATH, O_RDONLY); + fd = open(SF1XX_DEVICE_PATH, O_RDONLY); if (fd < 0) { goto fail; } if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + ::close(fd); goto fail; } + ::close(fd); exit(0); fail: @@ -741,10 +743,10 @@ test() ssize_t sz; int ret; - int fd = open(SF10A_DEVICE_PATH, O_RDONLY); + int fd = open(SF1XX_DEVICE_PATH, O_RDONLY); if (fd < 0) { - err(1, "%s open failed (try 'sf10a start' if the driver is not running", SF10A_DEVICE_PATH); + err(1, "%s open failed (try 'sf1xx start' if the driver is not running", SF1XX_DEVICE_PATH); } /* do a simple demand read */ @@ -795,6 +797,7 @@ test() errx(1, "failed to set default poll rate"); } + ::close(fd); errx(0, "PASS"); } @@ -804,7 +807,7 @@ test() void reset() { - int fd = open(SF10A_DEVICE_PATH, O_RDONLY); + int fd = open(SF1XX_DEVICE_PATH, O_RDONLY); if (fd < 0) { err(1, "failed "); @@ -818,6 +821,7 @@ reset() err(1, "driver poll restart failed"); } + ::close(fd); exit(0); } @@ -840,41 +844,41 @@ info() } /* namespace */ int -sf10a_main(int argc, char *argv[]) +sf1xx_main(int argc, char *argv[]) { /* * Start/load the driver. */ if (!strcmp(argv[1], "start")) { - sf10a::start(); + sf1xx::start(); } /* * Stop the driver */ if (!strcmp(argv[1], "stop")) { - sf10a::stop(); + sf1xx::stop(); } /* * Test the driver/device. */ if (!strcmp(argv[1], "test")) { - sf10a::test(); + sf1xx::test(); } /* * Reset the driver. */ if (!strcmp(argv[1], "reset")) { - sf10a::reset(); + sf1xx::reset(); } /* * Print driver information. */ if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { - sf10a::info(); + sf1xx::info(); } errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); diff --git a/src/drivers/snapdragon_rc_pwm/CMakeLists.txt b/src/drivers/snapdragon_rc_pwm/CMakeLists.txt index 405b903306..254e85d931 100644 --- a/src/drivers/snapdragon_rc_pwm/CMakeLists.txt +++ b/src/drivers/snapdragon_rc_pwm/CMakeLists.txt @@ -35,8 +35,7 @@ px4_add_module( MAIN snapdragon_rc_pwm COMPILE_FLAGS -Os - -Wno-attributes - -Wno-packed + -DMAVLINK_COMM_NUM_BUFFERS=1 SRCS snapdragon_rc_pwm.cpp DEPENDS diff --git a/src/drivers/snapdragon_rc_pwm/snapdragon_rc_pwm.cpp b/src/drivers/snapdragon_rc_pwm/snapdragon_rc_pwm.cpp index 438151ae4e..bda765783e 100644 --- a/src/drivers/snapdragon_rc_pwm/snapdragon_rc_pwm.cpp +++ b/src/drivers/snapdragon_rc_pwm/snapdragon_rc_pwm.cpp @@ -35,9 +35,10 @@ /** * @file snapdragon_rc_pwm.cpp * @author Roman Bapst + * @author Julian Oes * - * This driver sends rc commands to the Snapdragon via UART. On the same UART it receives pwm - * motor commands from the Snapdragon. + * This driver sends RC commands to the Snapdragon via UART. On the same UART + * it receives pwm motor commands from the Snapdragon. */ @@ -51,7 +52,7 @@ #include #include #include -#include + #include #include @@ -67,9 +68,9 @@ namespace snapdragon_rc_pwm static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; -volatile bool _task_should_exit = false; // flag indicating if uart_esc task should exit +volatile bool _task_should_exit = false; // flag indicating if snapdragon_rc_pwm task should exit static char _device[MAX_LEN_DEV_PATH]; -static bool _is_running = false; // flag indicating if uart_esc app is running +static bool _is_running = false; // flag indicating if snapdragon_rc_pwm app is running static px4_task_t _task_handle = -1; // handle to the task main thread static int _uart_fd = -1; int _pwm_fd = -1; @@ -86,7 +87,7 @@ void usage(); void start(); -/** uart_esc stop */ +/** snapdragon_rc_pwm stop */ void stop(); int initialise_uart(); @@ -104,7 +105,7 @@ void set_pwm_output(mavlink_actuator_control_target_t *actuator_controls); /** task main trampoline function */ void task_main_trampoline(int argc, char *argv[]); -/** uart_esc thread primary entry point */ +/** snapdragon_rc_pwm thread primary entry point */ void task_main(int argc, char *argv[]); void task_main(int argc, char *argv[]) @@ -155,7 +156,7 @@ void task_main(int argc, char *argv[]) mavlink_message_t msg; for (int i = 0; i < len; ++i) { - if (mavlink_parse_char(MAVLINK_COMM_1, serial_buf[i], &msg, &serial_status)) { + if (mavlink_parse_char(MAVLINK_COMM_0, serial_buf[i], &msg, &serial_status)) { // have a message, handle it handle_message(&msg); } @@ -366,7 +367,7 @@ int deinitialize_uart() return close(_uart_fd); } -// uart_esc main entrance +// snapdragon_rc_pwm main entrance void task_main_trampoline(int argc, char *argv[]) { task_main(argc, argv); @@ -451,7 +452,7 @@ int snapdragon_rc_pwm_main(int argc, char *argv[]) if (snapdragon_rc_pwm::_is_running) { - PX4_WARN("uart_esc already running"); + PX4_WARN("snapdragon_rc_pwm already running"); return 1; } diff --git a/src/drivers/srf02/srf02.cpp b/src/drivers/srf02/srf02.cpp index f585acb6a6..7988196049 100644 --- a/src/drivers/srf02/srf02.cpp +++ b/src/drivers/srf02/srf02.cpp @@ -430,14 +430,14 @@ SRF02::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; } - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); if (!_reports->resize(arg)) { - irqrestore(flags); + px4_leave_critical_section(flags); return -ENOMEM; } - irqrestore(flags); + px4_leave_critical_section(flags); return OK; } @@ -618,12 +618,12 @@ SRF02::start() work_queue(HPWORK, &_work, (worker_t)&SRF02::cycle_trampoline, this, 5); /* notify about state change */ - struct subsystem_info_s info = { - true, - true, - true, - subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER - }; + struct subsystem_info_s info = {}; + info.present = true; + info.enabled = true; + info.ok = true; + info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER; + static orb_advert_t pub = nullptr; if (pub != nullptr) { diff --git a/src/drivers/srf02_i2c/srf02_i2c.cpp b/src/drivers/srf02_i2c/srf02_i2c.cpp index 3ea72f7e15..de8ed55cff 100644 --- a/src/drivers/srf02_i2c/srf02_i2c.cpp +++ b/src/drivers/srf02_i2c/srf02_i2c.cpp @@ -432,14 +432,14 @@ SRF02_I2C::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; } - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); if (!_reports->resize(arg)) { - irqrestore(flags); + px4_leave_critical_section(flags); return -ENOMEM; } - irqrestore(flags); + px4_leave_critical_section(flags); return OK; } diff --git a/src/drivers/stm32/CMakeLists.txt b/src/drivers/stm32/CMakeLists.txt index b7b3825aa8..40bbef8ad7 100644 --- a/src/drivers/stm32/CMakeLists.txt +++ b/src/drivers/stm32/CMakeLists.txt @@ -39,6 +39,7 @@ px4_add_module( drv_io_timer.c drv_pwm_servo.c drv_input_capture.c + drv_led_pwm.cpp DEPENDS platforms__common ) diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index bb2c992730..3151f5ac3a 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -251,8 +251,6 @@ ADC::init() } - DEVICE_DEBUG("init done"); - /* create the device node */ return CDev::init(); } @@ -273,9 +271,9 @@ ADC::read(file *filp, char *buffer, size_t len) } /* block interrupts while copying samples to avoid racing with an update */ - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); memcpy(buffer, _samples, len); - irqrestore(flags); + px4_leave_critical_section(flags); return len; } @@ -343,14 +341,14 @@ ADC::update_adc_report(hrt_abstime now) void ADC::update_system_power(hrt_abstime now) { -#if defined (CONFIG_ARCH_BOARD_PX4FMU_V2) || \ - defined (CONFIG_ARCH_BOARD_MINDPX_V2) || \ - defined (CONFIG_ARCH_BOARD_PX4FMU_V4) +#if defined (BOARD_ADC_USB_CONNECTED) system_power_s system_power = {}; system_power.timestamp = now; system_power.voltage5V_v = 0; +#if defined(ADC_5V_RAIL_SENSE) + for (unsigned i = 0; i < _channel_count; i++) { if (_samples[i].am_channel == ADC_5V_RAIL_SENSE) { // it is 2:1 scaled @@ -358,35 +356,21 @@ ADC::update_system_power(hrt_abstime now) } } +#endif + + /* Note once the board_config.h provides BOARD_ADC_USB_CONNECTED, + * It must provide the true logic GPIO BOARD_ADC_xxxx macros. + */ // these are not ADC related, but it is convenient to // publish these to the same topic - system_power.usb_connected = stm32_gpioread(GPIO_OTGFS_VBUS); + system_power.usb_connected = BOARD_ADC_USB_CONNECTED; -#if defined (CONFIG_ARCH_BOARD_MINDPX_V2) - // note that the valid pins are active low - system_power.brick_valid = 1; - system_power.servo_valid = 1; + system_power.brick_valid = BOARD_ADC_BRICK_VALID; + system_power.servo_valid = BOARD_ADC_SERVO_VALID; // OC pins are active low - system_power.periph_5V_OC = 1; - system_power.hipower_5V_OC = 1; -#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V4) - // note that the valid pins are active high - system_power.brick_valid = stm32_gpioread(GPIO_VDD_BRICK_VALID); - system_power.servo_valid = 1; - - // OC pins are not supported - system_power.periph_5V_OC = 0; - system_power.hipower_5V_OC = 0; -#else - // note that the valid pins are active low - system_power.brick_valid = !stm32_gpioread(GPIO_VDD_BRICK_VALID); - system_power.servo_valid = !stm32_gpioread(GPIO_VDD_SERVO_VALID); - - // OC pins are active low - system_power.periph_5V_OC = !stm32_gpioread(GPIO_VDD_5V_PERIPH_OC); - system_power.hipower_5V_OC = !stm32_gpioread(GPIO_VDD_5V_HIPOWER_OC); -#endif + system_power.periph_5V_OC = BOARD_ADC_PERIPH_5V_OC; + system_power.hipower_5V_OC = BOARD_ADC_HIPOWER_5V_OC; /* lazily publish */ if (_to_system_power != nullptr) { @@ -396,7 +380,7 @@ ADC::update_system_power(hrt_abstime now) _to_system_power = orb_advertise(ORB_ID(system_power), &system_power); } -#endif // CONFIG_ARCH_BOARD_PX4FMU_V2 +#endif // BOARD_ADC_USB_CONNECTED } uint16_t diff --git a/src/drivers/stm32/adc/module.mk b/src/drivers/stm32/adc/module.mk deleted file mode 100644 index 38ea490a0e..0000000000 --- a/src/drivers/stm32/adc/module.mk +++ /dev/null @@ -1,44 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# STM32 ADC driver -# - -MODULE_COMMAND = adc - -SRCS = adc.cpp - -INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common - -MAXOPTIMIZATION = -Os diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index 3bea3fda05..f3ec622a3d 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -667,7 +667,7 @@ hrt_absolute_time(void) static volatile uint32_t last_count; /* prevent re-entry */ - flags = irqsave(); + flags = px4_enter_critical_section(); /* get the current counter value */ count = rCNT; @@ -689,7 +689,7 @@ hrt_absolute_time(void) /* compute the current time */ abstime = HRT_COUNTER_SCALE(base_time + count); - irqrestore(flags); + px4_leave_critical_section(flags); return abstime; } @@ -725,11 +725,11 @@ abstime_to_ts(struct timespec *ts, hrt_abstime abstime) hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then) { - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); hrt_abstime delta = hrt_absolute_time() - *then; - irqrestore(flags); + px4_leave_critical_section(flags); return delta; } @@ -740,11 +740,11 @@ hrt_elapsed_time(const volatile hrt_abstime *then) hrt_abstime hrt_store_absolute_time(volatile hrt_abstime *now) { - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); hrt_abstime ts = hrt_absolute_time(); - irqrestore(flags); + px4_leave_critical_section(flags); return ts; } @@ -760,7 +760,7 @@ hrt_init(void) #ifdef HRT_PPM_CHANNEL /* configure the PPM input pin */ - stm32_configgpio(GPIO_PPM_IN); + px4_arch_configgpio(GPIO_PPM_IN); #endif } @@ -802,7 +802,7 @@ hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, static void hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg) { - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); /* if the entry is currently queued, remove it */ /* note that we are using a potentially uninitialised @@ -823,7 +823,7 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte hrt_call_enter(entry); - irqrestore(flags); + px4_leave_critical_section(flags); } /** @@ -843,7 +843,7 @@ hrt_called(struct hrt_call *entry) void hrt_cancel(struct hrt_call *entry) { - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); sq_rem(&entry->link, &callout_queue); entry->deadline = 0; @@ -853,7 +853,7 @@ hrt_cancel(struct hrt_call *entry) */ entry->period = 0; - irqrestore(flags); + px4_leave_critical_section(flags); } static void diff --git a/src/drivers/stm32/drv_input_capture.c b/src/drivers/stm32/drv_input_capture.c index 330404582b..850397c3df 100644 --- a/src/drivers/stm32/drv_input_capture.c +++ b/src/drivers/stm32/drv_input_capture.c @@ -107,7 +107,7 @@ static void input_capture_chan_handler(void *context, const io_timers_t *timer, hrt_abstime isrs_time , uint16_t isrs_rcnt) { uint16_t capture = _REG32(timer, chan->ccr_offset); - channel_stats[chan_index].last_edge = stm32_gpioread(chan->gpio_in); + channel_stats[chan_index].last_edge = px4_arch_gpioread(chan->gpio_in); if ((isrs_rcnt - capture) > channel_stats[chan_index].latnecy) { channel_stats[chan_index].latnecy = (isrs_rcnt - capture); @@ -133,10 +133,10 @@ static void input_capture_chan_handler(void *context, const io_timers_t *timer, static void input_capture_bind(unsigned channel, capture_callback_t callback, void *context) { - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); channel_handlers[channel].callback = callback; channel_handlers[channel].context = context; - irqrestore(flags); + px4_leave_critical_section(flags); } static void input_capture_unbind(unsigned channel) @@ -258,7 +258,7 @@ int up_input_capture_set_filter(unsigned channel, capture_filter_t filter) uint32_t timer = timer_io_channels[channel].timer_index; uint16_t rvalue; - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); switch (timer_io_channels[channel].timer_channel) { @@ -290,7 +290,7 @@ int up_input_capture_set_filter(unsigned channel, capture_filter_t filter) rv = -EIO; } - irqrestore(flags); + px4_leave_critical_section(flags); } } @@ -401,7 +401,7 @@ int up_input_capture_set_trigger(unsigned channel, input_capture_edge edge) uint16_t rvalue; rv = OK; - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); switch (timer_io_channels[channel].timer_channel) { @@ -437,7 +437,7 @@ int up_input_capture_set_trigger(unsigned channel, input_capture_edge edge) rv = -EIO; } - irqrestore(flags); + px4_leave_critical_section(flags); } } @@ -456,10 +456,10 @@ int up_input_capture_get_callback(unsigned channel, capture_callback_t *callback if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) { - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); *callback = channel_handlers[channel].callback; *context = channel_handlers[channel].context; - irqrestore(flags); + px4_leave_critical_section(flags); rv = OK; } } @@ -492,14 +492,14 @@ int up_input_capture_get_stats(unsigned channel, input_capture_stats_t *stats, b int rv = io_timer_validate_channel_index(channel); if (rv == 0) { - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); *stats = channel_stats[channel]; if (clear) { memset(&channel_stats[channel], 0, sizeof(*stats)); } - irqrestore(flags); + px4_leave_critical_section(flags); } return rv; diff --git a/src/drivers/stm32/drv_io_timer.c b/src/drivers/stm32/drv_io_timer.c index 9fb9393cbb..385d250ef0 100644 --- a/src/drivers/stm32/drv_io_timer.c +++ b/src/drivers/stm32/drv_io_timer.c @@ -397,7 +397,7 @@ static int io_timer_init_timer(unsigned timer) if (rv == 0) { - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); set_timer_initalized(timer); @@ -448,7 +448,7 @@ static int io_timer_init_timer(unsigned timer) up_enable_irq(io_timers[timer].vectorno); - irqrestore(flags); + px4_leave_critical_section(flags); } return rv; @@ -520,11 +520,11 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, io_timer_init_timer(channels_timer(channel)); - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); /* Set up IO */ if (gpio) { - stm32_configgpio(gpio); + px4_arch_configgpio(gpio); } @@ -570,7 +570,7 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, channel_handlers[channel].callback = channel_handler; channel_handlers[channel].context = context; rDIER(timer) |= dier_setbits << shifts; - irqrestore(flags); + px4_leave_critical_section(flags); } return rv; @@ -643,7 +643,7 @@ int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_chann } } - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); for (unsigned actions = 0; actions < arraySize(action_cache) && action_cache[actions].base != 0 ; actions++) { uint32_t rvalue = _REG32(action_cache[actions].base, STM32_GTIM_CCER_OFFSET); @@ -667,7 +667,7 @@ int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_chann for (unsigned chan = 0; chan < arraySize(action_cache[actions].gpio); chan++) { if (action_cache[actions].gpio[chan]) { - stm32_configgpio(action_cache[actions].gpio[chan]); + px4_arch_configgpio(action_cache[actions].gpio[chan]); action_cache[actions].gpio[chan] = 0; } } @@ -681,7 +681,7 @@ int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_chann } } - irqrestore(flags); + px4_leave_critical_section(flags); return 0; } diff --git a/src/drivers/stm32/drv_io_timer.h b/src/drivers/stm32/drv_io_timer.h index 0cd59a4e1e..250f7ce3eb 100644 --- a/src/drivers/stm32/drv_io_timer.h +++ b/src/drivers/stm32/drv_io_timer.h @@ -47,6 +47,10 @@ __BEGIN_DECLS /* configuration limits */ #define MAX_IO_TIMERS 4 #define MAX_TIMER_IO_CHANNELS 8 + +#define MAX_LED_TIMERS 1 +#define MAX_TIMER_LED_CHANNELS 3 + #define IO_TIMER_ALL_MODES_CHANNELS 0 typedef enum io_timer_channel_mode_t { @@ -90,6 +94,10 @@ typedef void (*channel_handler_t)(void *context, const io_timers_t *timer, uint3 /* supplied by board-specific code */ __EXPORT extern const io_timers_t io_timers[MAX_IO_TIMERS]; __EXPORT extern const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS]; + +__EXPORT extern const io_timers_t led_pwm_timers[MAX_LED_TIMERS]; +__EXPORT extern const timer_io_channels_t led_pwm_channels[MAX_TIMER_LED_CHANNELS]; + __EXPORT extern io_timer_channel_allocation_t allocations[IOTimerChanModeSize]; __EXPORT int io_timer_handler0(int irq, void *context); __EXPORT int io_timer_handler1(int irq, void *context); diff --git a/src/drivers/rgbled_pwm/drv_led_pwm.cpp b/src/drivers/stm32/drv_led_pwm.cpp similarity index 66% rename from src/drivers/rgbled_pwm/drv_led_pwm.cpp rename to src/drivers/stm32/drv_led_pwm.cpp index e495e975ca..263ca6645c 100644 --- a/src/drivers/rgbled_pwm/drv_led_pwm.cpp +++ b/src/drivers/stm32/drv_led_pwm.cpp @@ -54,6 +54,7 @@ #include #include #include +#include #include #include @@ -61,7 +62,7 @@ #include - +#if defined(BOARD_HAS_LED_PWM) #define REG(_tmr, _reg) (*(volatile uint32_t *)(led_pwm_timers[_tmr].base + _reg)) #define rCR1(_tmr) REG(_tmr, STM32_GTIM_CR1_OFFSET) @@ -96,103 +97,41 @@ void led_pwm_servo_arm(bool armed); unsigned led_pwm_timer_get_period(unsigned timer); -const struct io_timers_t led_pwm_timers[3] = { - { - .base = STM32_TIM3_BASE, - .clock_register = STM32_RCC_APB1ENR, - .clock_bit = RCC_APB1ENR_TIM3EN, - .clock_freq = STM32_APB1_TIM3_CLKIN, - .vectorno = STM32_IRQ_TIM3, - .first_channel_index = 4, - .last_channel_index = 7, - .handler = io_timer_handler0, - }, - { - .base = STM32_TIM3_BASE, - .clock_register = STM32_RCC_APB1ENR, - .clock_bit = RCC_APB1ENR_TIM3EN, - .clock_freq = STM32_APB1_TIM3_CLKIN, - .vectorno = STM32_IRQ_TIM3, - .first_channel_index = 4, - .last_channel_index = 7, - .handler = io_timer_handler1, - }, - { - .base = STM32_TIM3_BASE, - .clock_register = STM32_RCC_APB1ENR, - .clock_bit = RCC_APB1ENR_TIM3EN, - .clock_freq = STM32_APB1_TIM3_CLKIN, - .vectorno = STM32_IRQ_TIM3, - .first_channel_index = 4, - .last_channel_index = 7, - .handler = io_timer_handler2, - } -}; - -#define LED_TIM3_CH3OUT (GPIO_ALT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OPENDRAIN|GPIO_PORTB|GPIO_PIN0) -#define LED_TIM3_CH2OUT (GPIO_ALT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OPENDRAIN|GPIO_PORTC|GPIO_PIN7) -#define LED_TIM3_CH4OUT (GPIO_ALT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OPENDRAIN|GPIO_PORTB|GPIO_PIN1) -#define LED_TIM3_CH3IN (GPIO_INPUT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OPENDRAIN|GPIO_PORTB|GPIO_PIN0) -#define LED_TIM3_CH2IN (GPIO_INPUT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OPENDRAIN|GPIO_PORTC|GPIO_PIN7) -#define LED_TIM3_CH4IN (GPIO_INPUT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OPENDRAIN|GPIO_PORTB|GPIO_PIN1) - -const struct timer_io_channels_t led_pwm_channels[3] = { - { - .gpio_out = LED_TIM3_CH3OUT, - .gpio_in = LED_TIM3_CH3IN, - .timer_index = 0, - .timer_channel = 3, - .masks = GTIM_SR_CC3IF | GTIM_SR_CC3OF, - .ccr_offset = STM32_GTIM_CCR3_OFFSET, - }, - { - .gpio_out = LED_TIM3_CH2OUT, - .gpio_in = LED_TIM3_CH2IN, - .timer_index = 1, - .timer_channel = 2, - .masks = GTIM_SR_CC2IF | GTIM_SR_CC2OF, - .ccr_offset = STM32_GTIM_CCR2_OFFSET, - }, - { - .gpio_out = LED_TIM3_CH4OUT, - .gpio_in = LED_TIM3_CH4IN, - .timer_index = 2, - .timer_channel = 4, - .masks = GTIM_SR_CC4IF | GTIM_SR_CC4OF, - .ccr_offset = STM32_GTIM_CCR4_OFFSET, - } -}; - - static void led_pwm_timer_init(unsigned timer) { - /* enable the timer clock before we try to talk to it */ - modifyreg32(led_pwm_timers[timer].clock_register, 0, led_pwm_timers[timer].clock_bit); + /* valid Timer */ - /* disable and configure the timer */ - rCR1(timer) = 0; - rCR2(timer) = 0; - rSMCR(timer) = 0; - rDIER(timer) = 0; - rCCER(timer) = 0; - rCCMR1(timer) = 0; - rCCMR2(timer) = 0; - rCCER(timer) = 0; - rDCR(timer) = 0; + if (led_pwm_timers[timer].base != 0) { - if ((led_pwm_timers[timer].base == STM32_TIM1_BASE) || (led_pwm_timers[timer].base == STM32_TIM8_BASE)) { - /* master output enable = on */ - rBDTR(timer) = ATIM_BDTR_MOE; + /* enable the timer clock before we try to talk to it */ + + modifyreg32(led_pwm_timers[timer].clock_register, 0, led_pwm_timers[timer].clock_bit); + + /* disable and configure the timer */ + rCR1(timer) = 0; + rCR2(timer) = 0; + rSMCR(timer) = 0; + rDIER(timer) = 0; + rCCER(timer) = 0; + rCCMR1(timer) = 0; + rCCMR2(timer) = 0; + rCCER(timer) = 0; + rDCR(timer) = 0; + + if ((led_pwm_timers[timer].base == STM32_TIM1_BASE) || (led_pwm_timers[timer].base == STM32_TIM8_BASE)) { + /* master output enable = on */ + rBDTR(timer) = ATIM_BDTR_MOE; + } + + /* configure the timer to free-run at 1MHz */ + rPSC(timer) = (led_pwm_timers[timer].clock_freq / 1000000) - 1; + + /* default to updating at 50Hz */ + led_pwm_timer_set_rate(timer, 50); + + /* note that the timer is left disabled - arming is performed separately */ } - - /* configure the timer to free-run at 1MHz */ - rPSC(timer) = (led_pwm_timers[timer].clock_freq / 1000000) - 1; - - /* default to updating at 50Hz */ - led_pwm_timer_set_rate(timer, 50); - - /* note that the timer is left disabled - arming is performed separately */ } unsigned led_pwm_timer_get_period(unsigned timer) @@ -216,31 +155,28 @@ led_pwm_channel_init(unsigned channel) unsigned timer = led_pwm_channels[channel].timer_index; /* configure the GPIO first */ - stm32_configgpio(led_pwm_channels[channel].gpio_out); + + px4_arch_configgpio(led_pwm_channels[channel].gpio_out); /* configure the channel */ switch (led_pwm_channels[channel].timer_channel) { case 1: rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE; - //rCCR1(timer) = led_pwm_channels[channel].default_value; rCCER(timer) |= GTIM_CCER_CC1E; break; case 2: rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC2M_SHIFT) | GTIM_CCMR1_OC2PE; - //rCCR2(timer) = led_pwm_channels[channel].default_value; rCCER(timer) |= GTIM_CCER_CC2E; break; case 3: rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC3M_SHIFT) | GTIM_CCMR2_OC3PE; - //rCCR3(timer) = led_pwm_channels[channel].default_value; rCCER(timer) |= GTIM_CCER_CC3E; break; case 4: rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC4M_SHIFT) | GTIM_CCMR2_OC4PE; - //rCCR4(timer) = led_pwm_channels[channel].default_value; rCCER(timer) |= GTIM_CCER_CC4E; break; } @@ -249,7 +185,7 @@ led_pwm_channel_init(unsigned channel) int led_pwm_servo_set(unsigned channel, uint8_t cvalue) { - if (channel >= 3) { + if (channel >= arraySize(led_pwm_channels)) { return -1; } @@ -263,7 +199,11 @@ led_pwm_servo_set(unsigned channel, uint8_t cvalue) unsigned period = led_pwm_timer_get_period(timer); +#if defined(BOARD_LED_PWM_DRIVE_ACTIVE_LOW) unsigned value = period - (unsigned)cvalue * period / 255; +#else + unsigned value = (unsigned)cvalue * period / 255; +#endif /* configure the channel */ if (value > 0) { @@ -336,12 +276,12 @@ int led_pwm_servo_init(void) { /* do basic timer initialisation first */ - for (unsigned i = 0; i < 3; i++) { + for (unsigned i = 0; i < arraySize(led_pwm_timers); i++) { led_pwm_timer_init(i); } /* now init channels */ - for (unsigned i = 0; i < 3; i++) { + for (unsigned i = 0; i < arraySize(led_pwm_channels); i++) { led_pwm_channel_init(i); } @@ -359,7 +299,7 @@ void led_pwm_servo_arm(bool armed) { /* iterate timers and arm/disarm appropriately */ - for (unsigned i = 0; i < 3; i++) { + for (unsigned i = 0; i < arraySize(led_pwm_timers); i++) { if (led_pwm_timers[i].base != 0) { if (armed) { /* force an update to preload all registers */ @@ -369,14 +309,10 @@ led_pwm_servo_arm(bool armed) rCR1(i) |= GTIM_CR1_CEN | GTIM_CR1_ARPE; } else { - // XXX This leads to FMU PWM being still active - // but uncontrollable. Just disable the timer - // and risk a runt. - ///* on disarm, just stop auto-reload so we don't generate runts */ - //rCR1(i) &= ~GTIM_CR1_ARPE; rCR1(i) = 0; } } } } +#endif // BOARD_HAS_LED_PWM diff --git a/src/drivers/stm32/tone_alarm/module.mk b/src/drivers/stm32/tone_alarm/module.mk deleted file mode 100644 index 25a194ef61..0000000000 --- a/src/drivers/stm32/tone_alarm/module.mk +++ /dev/null @@ -1,44 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Tone alarm driver -# - -MODULE_COMMAND = tone_alarm - -SRCS = tone_alarm.cpp - -INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common - -MAXOPTIMIZATION = -Os diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index 1a54e11809..34f7b3b296 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -439,7 +439,7 @@ ToneAlarm::init() } /* configure the GPIO to the idle state */ - stm32_configgpio(GPIO_TONE_ALARM_IDLE); + px4_arch_configgpio(GPIO_TONE_ALARM_IDLE); /* clock/power on our timer */ modifyreg32(TONE_ALARM_CLOCK_POWER_REG, 0, TONE_ALARM_CLOCK_ENABLE); @@ -571,7 +571,7 @@ ToneAlarm::start_note(unsigned note) rCCER |= TONE_CCER; // enable the output // configure the GPIO to enable timer output - stm32_configgpio(GPIO_TONE_ALARM); + px4_arch_configgpio(GPIO_TONE_ALARM); } void @@ -583,7 +583,7 @@ ToneAlarm::stop_note() /* * Make sure the GPIO is not driving the speaker. */ - stm32_configgpio(GPIO_TONE_ALARM_IDLE); + px4_arch_configgpio(GPIO_TONE_ALARM_IDLE); } void @@ -876,7 +876,7 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) DEVICE_DEBUG("ioctl %i %u", cmd, arg); -// irqstate_t flags = irqsave(); +// irqstate_t flags = px4_enter_critical_section(); /* decide whether to increase the alarm level to cmd or leave it alone */ switch (cmd) { @@ -911,7 +911,7 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) break; } -// irqrestore(flags); +// px4_leave_critical_section(flags); /* give it to the superclass if we didn't like it */ if (result == -ENOTTY) { diff --git a/src/platforms/nuttx/module.mk b/src/drivers/tap_esc/CMakeLists.txt similarity index 87% rename from src/platforms/nuttx/module.mk rename to src/drivers/tap_esc/CMakeLists.txt index 4a2aff8249..1869d3c43f 100644 --- a/src/platforms/nuttx/module.mk +++ b/src/drivers/tap_esc/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2014 PX4 Development Team. All rights reserved. +# Copyright (c) 2016 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,11 +30,14 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ - -# -# NuttX / uORB adapter library -# - -SRCS = px4_nuttx_impl.cpp - -MAXOPTIMIZATION = -Os +px4_add_module( + MODULE drivers__tap_esc + MAIN tap_esc + COMPILE_FLAGS + -Os + SRCS + tap_esc.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/tap_esc/drv_tap_esc.h b/src/drivers/tap_esc/drv_tap_esc.h new file mode 100644 index 0000000000..abf210751b --- /dev/null +++ b/src/drivers/tap_esc/drv_tap_esc.h @@ -0,0 +1,259 @@ +/**************************************************************************** + * + * Copyright (c) 2016 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. + * + ****************************************************************************/ + +#define TAP_ESC_CRC {\ + 0x00, 0xE7, 0x29, 0xCE, 0x52, 0xB5, 0x7B, 0x9C, 0xA4, 0x43, 0x8D, 0x6A,\ + 0xF6, 0x11, 0xDF, 0x38, 0xAF, 0x48, 0x86, 0x61, 0xFD, 0x1A, 0xD4, 0x33,\ + 0x0B, 0xEC, 0x22, 0xC5, 0x59, 0xBE, 0x70, 0x97, 0xB9, 0x5E, 0x90, 0x77,\ + 0xEB, 0x0C, 0xC2, 0x25, 0x1D, 0xFA, 0x34, 0xD3, 0x4F, 0xA8, 0x66, 0x81,\ + 0x16, 0xF1, 0x3F, 0xD8, 0x44, 0xA3, 0x6D, 0x8A, 0xB2, 0x55, 0x9B, 0x7C,\ + 0xE0, 0x07, 0xC9, 0x2E, 0x95, 0x72, 0xBC, 0x5B, 0xC7, 0x20, 0xEE, 0x09,\ + 0x31, 0xD6, 0x18, 0xFF, 0x63, 0x84, 0x4A, 0xAD, 0x3A, 0xDD, 0x13, 0xF4,\ + 0x68, 0x8F, 0x41, 0xA6, 0x9E, 0x79, 0xB7, 0x50, 0xCC, 0x2B, 0xE5, 0x02,\ + 0x2C, 0xCB, 0x05, 0xE2, 0x7E, 0x99, 0x57, 0xB0, 0x88, 0x6F, 0xA1, 0x46,\ + 0xDA, 0x3D, 0xF3, 0x14, 0x83, 0x64, 0xAA, 0x4D, 0xD1, 0x36, 0xF8, 0x1F,\ + 0x27, 0xC0, 0x0E, 0xE9, 0x75, 0x92, 0x5C, 0xBB, 0xCD, 0x2A, 0xE4, 0x03,\ + 0x9F, 0x78, 0xB6, 0x51, 0x69, 0x8E, 0x40, 0xA7, 0x3B, 0xDC, 0x12, 0xF5,\ + 0x62, 0x85, 0x4B, 0xAC, 0x30, 0xD7, 0x19, 0xFE, 0xC6, 0x21, 0xEF, 0x08,\ + 0x94, 0x73, 0xBD, 0x5A, 0x74, 0x93, 0x5D, 0xBA, 0x26, 0xC1, 0x0F, 0xE8,\ + 0xD0, 0x37, 0xF9, 0x1E, 0x82, 0x65, 0xAB, 0x4C, 0xDB, 0x3C, 0xF2, 0x15,\ + 0x89, 0x6E, 0xA0, 0x47, 0x7F, 0x98, 0x56, 0xB1, 0x2D, 0xCA, 0x04, 0xE3,\ + 0x58, 0xBF, 0x71, 0x96, 0x0A, 0xED, 0x23, 0xC4, 0xFC, 0x1B, 0xD5, 0x32,\ + 0xAE, 0x49, 0x87, 0x60, 0xF7, 0x10, 0xDE, 0x39, 0xA5, 0x42, 0x8C, 0x6B,\ + 0x53, 0xB4, 0x7A, 0x9D, 0x01, 0xE6, 0x28, 0xCF, 0xE1, 0x06, 0xC8, 0x2F,\ + 0xB3, 0x54, 0x9A, 0x7D, 0x45, 0xA2, 0x6C, 0x8B, 0x17, 0xF0, 0x3E, 0xD9,\ + 0x4E, 0xA9, 0x67, 0x80, 0x1C, 0xFB, 0x35, 0xD2, 0xEA, 0x0D, 0xC3, 0x24,\ + 0xB8, 0x5F, 0x91, 0x76\ + } + +#define TAP_ESC_MAX_PACKET_LEN 20 +#define TAP_ESC_MAX_MOTOR_NUM 8 + +/* ESC_POS maps the values stored in the channelMapTable to reorder the ESC's + * id so that that match the mux setting, so that the ressonder's data + * will be read. + * The index on channelMapTable[p] p is the physical ESC + * The value it is set to is the logical value from ESC_POS[p] + * Phy Log + * 0 0 + * 1 1 + * 2 4 + * 3 3 + * 4 2 + * 5 5 + * .... + * + */ +#define ESC_POS {0, 1, 4, 3, 2, 5, 7, 8} + +#define RPMMAX 1900 +#define RPMMIN 1200 +#define RPMSTOPPED (RPMMIN - 10) + + +#define MAX_BOOT_TIME_MS (500) // Minimum time to wait after Power on before sending commands + +#pragma pack(push,1) + +/****** Run ***********/ + +#define RUN_CHANNEL_VALUE_MASK (uint16_t)0x07ff +#define RUN_RED_LED_ON_MASK (uint16_t)0x0800 +#define RUN_GREEN_LED_ON_MASK (uint16_t)0x1000 +#define RUN_BLUE_LED_ON_MASK (uint16_t)0x2000 +#define RUN_LED_ON_MASK (uint16_t)0x3800 +#define RUN_FEEDBACK_ENABLE_MASK (uint16_t)0x4000 +#define RUN_REVERSE_MASK (uint16_t)0x8000 + +typedef struct { + + uint16_t rpm_flags[TAP_ESC_MAX_MOTOR_NUM]; +} RunReq; + +typedef struct { + uint8_t channelID; + uint8_t ESCStatus; + int16_t speed; // -32767 - 32768 +#if defined(ESC_HAVE_VOLTAGE_SENSOR) + uint16_t voltage; // 0.00 - 100.00 V +#endif +#if defined(ESC_HAVE_CURRENT_SENSOR) + uint16_t current; // 0.0 - 200.0 A +#endif +#if defined(ESC_HAVE_TEMPERATURE_SENSOR) + uint8_t temperature; // 0 - 256 degree celsius +#endif +} RunInfoRepsonse; +/****** Run ***********/ + +/****** ConFigInfoBasic ***********/ +typedef struct { + uint8_t maxChannelInUse; + uint8_t channelMapTable[TAP_ESC_MAX_MOTOR_NUM]; + uint8_t monitorMsgType; + uint8_t controlMode; + uint16_t minChannelValue; + uint16_t maxChannelValue; +} ConfigInfoBasicRequest; + +typedef struct { + uint8_t channelID; + ConfigInfoBasicRequest resp; +} ConfigInfoBasicResponse; + +#define ESC_CHANNEL_MAP_CHANNEL 0x0f +#define ESC_CHANNEL_MAP_RUNNING_DIRECTION 0xf0 +/****** ConFigInfoBasicResponse ***********/ + +/****** InfoRequest ***********/ +typedef enum { + REQEST_INFO_BASIC = 0, + REQEST_INFO_FUll, + REQEST_INFO_RUN, + REQEST_INFO_STUDY, + REQEST_INFO_COMM, + REQEST_INFO_DEVICE, +} InfoTypes; + +typedef struct { + uint8_t channelID; + uint8_t requestInfoType; +} InfoRequest; + +/****** InfoRequest ***********/ + +typedef struct { + uint8_t head; + uint8_t len; + uint8_t msg_id; + union { + InfoRequest reqInfo; + ConfigInfoBasicRequest reqConfigInfoBasic; + RunReq reqRun; + + ConfigInfoBasicResponse rspConfigInfoBasic; + RunInfoRepsonse rspRunInfo; + uint8_t bytes[100]; + } d; + uint8_t crc_data; + +} EscPacket; + +#define UART_BUFFER_SIZE 128 +typedef struct { + uint8_t head; + uint8_t tail; + uint8_t dat_cnt; + uint8_t esc_feedback_buf[UART_BUFFER_SIZE]; +} ESC_UART_BUF; + +#pragma pack(pop) +/****************************************************************************************** + * ESCBUS_MSG_ID_RUN_INFO packet + * + * Monitor message of ESCs while motor is running + * + * channelID: assigned channel number + * + * ESCStatus: status of ESC + * Num Health status + * 0 HEALTHY + * 1 WARNING_LOW_VOLTAGE + * 2 WARNING_OVER_CURRENT + * 3 WARNING_OVER_HEAT + * 4 ERROR_MOTOR_LOW_SPEED_LOSE_STEP + * 5 ERROR_MOTOR_STALL + * 6 ERROR_HARDWARE + * 7 ERROR_LOSE_PROPELLER + * 8 ERROR_OVER_CURRENT + * + * speed: -32767 - 32767 rpm + * + * temperature: 0 - 256 celsius degree (if available) + * voltage: 0.00 - 100.00 V (if available) + * current: 0.0 - 200.0 A (if available) + */ + +typedef enum { + ESC_STATUS_HEALTHY, + ESC_STATUS_WARNING_LOW_VOLTAGE, + ESC_STATUS_WARNING_OVER_HEAT, + ESC_STATUS_ERROR_MOTOR_LOW_SPEED_LOSE_STEP, + ESC_STATUS_ERROR_MOTOR_STALL, + ESC_STATUS_ERROR_HARDWARE, + ESC_STATUS_ERROR_LOSE_PROPELLER, + ESC_STATUS_ERROR_OVER_CURRENT, + ESC_STATUS_ERROR_MOTOR_HIGH_SPEED_LOSE_STEP, + ESC_STATUS_ERROR_LOSE_CMD, +} ESCBUS_ENUM_ESC_STATUS; + + +typedef enum { +// messages or command to ESC + ESCBUS_MSG_ID_CONFIG_BASIC = 0, + ESCBUS_MSG_ID_CONFIG_FULL, + ESCBUS_MSG_ID_RUN, + ESCBUS_MSG_ID_TUNE, + ESCBUS_MSG_ID_DO_CMD, +// messages from ESC + ESCBUS_MSG_ID_REQUEST_INFO, + ESCBUS_MSG_ID_CONFIG_INFO_BASIC, // simple configuration info for request from flight controller + ESCBUS_MSG_ID_CONFIG_INFO_FULL,// full configuration info for request from host such as computer + ESCBUS_MSG_ID_RUN_INFO,// feedback message in RUN mode + ESCBUS_MSG_ID_STUDY_INFO, // studied parameters in STUDY mode + ESCBUS_MSG_ID_COMM_INFO, // communication method info + ESCBUS_MSG_ID_DEVICE_INFO,// ESC device info + ESCBUS_MSG_ID_ASSIGNED_ID, // never touch ESCBUS_MSG_ID_MAX_NUM + //boot loader used + PROTO_OK = 0x10, // INSYNC/OK - 'ok' response + PROTO_FAILED = 0x11, // INSYNC/FAILED - 'fail' response + + ESCBUS_MSG_ID_BOOT_SYNC = 0x21, // boot loader used + PROTO_GET_DEVICE = 0x22, // get device ID bytes + PROTO_CHIP_ERASE = 0x23, // erase program area and reset program address + PROTO_PROG_MULTI = 0x27, // write bytes at program address and increment + PROTO_GET_CRC = 0x29, // compute & return a CRC + PROTO_BOOT = 0x30, // boot the application + PROTO_GET_SOFTWARE_VERSION = 0x40, + ESCBUS_MSG_ID_MAX_NUM, +} +ESCBUS_ENUM_MESSAGE_ID; + +typedef enum { + HEAD, + LEN, + ID, + DATA, + CRC, + +} PARSR_ESC_STATE; diff --git a/src/drivers/tap_esc/tap_esc.cpp b/src/drivers/tap_esc/tap_esc.cpp new file mode 100644 index 0000000000..d2afacd36c --- /dev/null +++ b/src/drivers/tap_esc/tap_esc.cpp @@ -0,0 +1,1183 @@ +/**************************************************************************** + * + * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#define NAN_VALUE (0.0f/0.0f) + +#ifndef B250000 +#define B250000 250000 +#endif + +#define ESC_HAVE_CURRENT_SENSOR + +#include "drv_tap_esc.h" + +/* + * This driver connects to TAP ESCs via serial. + */ + +static int _uart_fd = -1; //todo:refactor in to class +class TAP_ESC : public device::CDev +{ +public: + enum Mode { + MODE_NONE, + MODE_2PWM, + MODE_2PWM2CAP, + MODE_3PWM, + MODE_3PWM1CAP, + MODE_4PWM, + MODE_6PWM, + MODE_8PWM, + MODE_4CAP, + MODE_5CAP, + MODE_6CAP, + }; + TAP_ESC(int channels_count); + virtual ~TAP_ESC(); + virtual int init(); + virtual int ioctl(file *filp, int cmd, unsigned long arg); + void cycle(); +protected: + void select_responder(uint8_t sel); +private: + + static const uint8_t crcTable[256]; + static const uint8_t device_mux_map[TAP_ESC_MAX_MOTOR_NUM]; + + bool _is_armed; + + unsigned _poll_fds_num; + Mode _mode; + // subscriptions + int _armed_sub; + int _test_motor_sub; + orb_advert_t _outputs_pub = nullptr; + actuator_outputs_s _outputs; + static actuator_armed_s _armed; + + //todo:refactor dynamic based on _channels_count + // It needs to support the numbe of ESC + int _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; + + pollfd _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; + + actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; + + orb_id_t _control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; + + orb_advert_t _esc_feedback_pub = nullptr; + esc_status_s _esc_feedback; + uint8_t _channels_count; // The number of ESC channels + + MixerGroup *_mixers; + uint32_t _groups_required; + uint32_t _groups_subscribed; + volatile bool _initialized; + unsigned _pwm_default_rate; + unsigned _current_update_rate; + ESC_UART_BUF uartbuf; + EscPacket _packet; + void subscribe(); + + void work_start(); + void work_stop(); + void send_esc_outputs(const float *pwm, const unsigned num_pwm); + uint8_t crc8_esc(uint8_t *p, uint8_t len); + uint8_t crc_packet(EscPacket &p); + int send_packet(EscPacket &p, int responder); + void read_data_from_uart(); + bool parse_tap_esc_feedback(ESC_UART_BUF *serial_buf, EscPacket *packetdata); + static int control_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &input); +}; + +const uint8_t TAP_ESC::crcTable[256] = TAP_ESC_CRC; +const uint8_t TAP_ESC::device_mux_map[TAP_ESC_MAX_MOTOR_NUM] = ESC_POS; + +actuator_armed_s TAP_ESC::_armed = {}; + +namespace +{ +TAP_ESC *tap_esc = nullptr; +} + +# define TAP_ESC_DEVICE_PATH "/dev/tap_esc" + +TAP_ESC::TAP_ESC(int channels_count): + CDev("tap_esc", TAP_ESC_DEVICE_PATH), + _is_armed(false), + _poll_fds_num(0), + _mode(MODE_4PWM), //FIXME: what is this mode used for??? + _armed_sub(-1), + _test_motor_sub(-1), + _outputs_pub(nullptr), + _control_subs{ -1}, + _esc_feedback_pub(nullptr), + _esc_feedback{}, + _channels_count(channels_count), + _mixers(nullptr), + _groups_required(0), + _groups_subscribed(0), + _initialized(false), + _pwm_default_rate(400), + _current_update_rate(0) + +{ + _control_topics[0] = ORB_ID(actuator_controls_0); + _control_topics[1] = ORB_ID(actuator_controls_1); + _control_topics[2] = ORB_ID(actuator_controls_2); + _control_topics[3] = ORB_ID(actuator_controls_3); + memset(_controls, 0, sizeof(_controls)); + memset(_poll_fds, 0, sizeof(_poll_fds)); + uartbuf.head = 0; + uartbuf.tail = 0; + uartbuf.dat_cnt = 0; + memset(uartbuf.esc_feedback_buf, 0, sizeof(uartbuf.esc_feedback_buf)); + + for (size_t i = 0; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) { + _outputs.output[i] = NAN; + } + + _outputs.noutputs = 0; +} + +TAP_ESC::~TAP_ESC() +{ + if (_initialized) { + /* tell the task we want it to go away */ + work_stop(); + + int i = 10; + + do { + /* wait 50ms - it should wake every 100ms or so worst-case */ + usleep(50000); + i--; + + } while (_initialized && i > 0); + } + + // clean up the alternate device node + //unregister_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH, _class_instance); + + tap_esc = nullptr; +} + +int +TAP_ESC::init() +{ + int ret; + + ASSERT(!_initialized); + + /* Respect boot time requierd by the ESC FW */ + + hrt_abstime uptime_us = hrt_absolute_time(); + + if (uptime_us < MAX_BOOT_TIME_MS * 1000) { + usleep((MAX_BOOT_TIME_MS * 1000) - uptime_us); + } + + /* Issue Basic Config */ + + EscPacket packet = {0xfe, sizeof(ConfigInfoBasicRequest), ESCBUS_MSG_ID_CONFIG_BASIC}; + ConfigInfoBasicRequest &config = packet.d.reqConfigInfoBasic; + memset(&config, 0, sizeof(ConfigInfoBasicRequest)); + config.maxChannelInUse = _channels_count; + + /* Asign the id's to the ESCs to match the mux */ + + for (uint8_t phy_chan_index = 0; phy_chan_index < _channels_count; phy_chan_index++) { + config.channelMapTable[phy_chan_index] = device_mux_map[phy_chan_index] & + ESC_CHANNEL_MAP_CHANNEL; // Use ESC_CHANNEL_MAP_RUNNING_DIRECTION; + } + + config.maxChannelValue = RPMMAX; + config.minChannelValue = RPMMIN; + + ret = send_packet(packet, 0); + + if (ret < 0) { + return ret; + } + + /* Verify All ESC got the config */ + + for (uint8_t cid = 0; cid < _channels_count; cid++) { + + /* Send the InfoRequest querying CONFIG_BASIC */ + EscPacket packet_info = {0xfe, sizeof(InfoRequest), ESCBUS_MSG_ID_REQUEST_INFO}; + InfoRequest &info_req = packet_info.d.reqInfo; + info_req.channelID = cid; + info_req.requestInfoType = REQEST_INFO_BASIC; + + ret = send_packet(packet_info, cid); + + if (ret < 0) { + return ret; + } + + /* Get a response */ + + int retries = 10; + bool valid = false; + + while (retries--) { + + read_data_from_uart(); + + if (parse_tap_esc_feedback(&uartbuf, &_packet)) { + valid = (_packet.msg_id == ESCBUS_MSG_ID_CONFIG_INFO_BASIC + && _packet.d.rspConfigInfoBasic.channelID == cid + && 0 == memcmp(&_packet.d.rspConfigInfoBasic.resp, &config, sizeof(ConfigInfoBasicRequest))); + break; + + } else { + + /* Give it time to come in */ + + usleep(1000); + } + } + + if (!valid) { + return -EIO; + } + } + + /* To Unlock the ESC from the Power up state we need to issue 10 + * ESCBUS_MSG_ID_RUN request with all the values 0; + */ + + EscPacket unlock_packet = {0xfe, _channels_count, ESCBUS_MSG_ID_RUN}; + unlock_packet.len *= sizeof(unlock_packet.d.reqRun.rpm_flags[0]); + memset(unlock_packet.d.bytes, 0, sizeof(packet.d.bytes)); + + int unlock_times = 10; + + while (unlock_times--) { + + send_packet(unlock_packet, -1); + + /* Min Packet to Packet time is 1 Ms so use 2 */ + + usleep(1000 * 2); + } + + /* do regular cdev init */ + + ret = CDev::init(); + + return ret; +} + +int TAP_ESC::send_packet(EscPacket &packet, int responder) +{ + if (responder >= 0) { + + if (responder > _channels_count) { + return -EINVAL; + } + + select_responder(responder); + } + + int packet_len = crc_packet(packet); + int ret = ::write(_uart_fd, &packet.head, packet_len); + + if (ret != packet_len) { + PX4_WARN("TX ERROR: ret: %d, errno: %d", ret, errno); + } + + return ret; +} + +void +TAP_ESC::subscribe() +{ + /* subscribe/unsubscribe to required actuator control groups */ + uint32_t sub_groups = _groups_required & ~_groups_subscribed; + uint32_t unsub_groups = _groups_subscribed & ~_groups_required; + _poll_fds_num = 0; + + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (sub_groups & (1 << i)) { + DEVICE_DEBUG("subscribe to actuator_controls_%d", i); + _control_subs[i] = orb_subscribe(_control_topics[i]); + } + + if (unsub_groups & (1 << i)) { + DEVICE_DEBUG("unsubscribe from actuator_controls_%d", i); + ::close(_control_subs[i]); + _control_subs[i] = -1; + } + + if (_control_subs[i] > 0) { + _poll_fds[_poll_fds_num].fd = _control_subs[i]; + _poll_fds[_poll_fds_num].events = POLLIN; + _poll_fds_num++; + } + } +} + +uint8_t TAP_ESC::crc8_esc(uint8_t *p, uint8_t len) +{ + uint8_t crc = 0; + + for (uint8_t i = 0; i < len; i++) { + crc = crcTable[crc^*p++]; + } + + return crc; +} + +uint8_t TAP_ESC::crc_packet(EscPacket &p) +{ + /* Calculate the crc over Len,ID,data */ + p.d.bytes[p.len] = crc8_esc(&p.len, p.len + 2); + return p.len + offsetof(EscPacket, d) + 1; +} +void TAP_ESC::select_responder(uint8_t sel) +{ +#if defined(GPIO_S0) + px4_arch_gpiowrite(GPIO_S0, sel & 1); + px4_arch_gpiowrite(GPIO_S1, sel & 2); + px4_arch_gpiowrite(GPIO_S2, sel & 4); +#endif +} + + +void TAP_ESC:: send_esc_outputs(const float *pwm, const unsigned num_pwm) +{ + + uint16_t rpm[TAP_ESC_MAX_MOTOR_NUM]; + memset(rpm, 0, sizeof(rpm)); + uint8_t motor_cnt = num_pwm; + static uint8_t which_to_respone = 0; + + for (uint8_t i = 0; i < motor_cnt; i++) { + rpm[i] = pwm[i]; + + if (rpm[i] > RPMMAX) { + rpm[i] = RPMMAX; + + } else if (rpm[i] < RPMSTOPPED) { + rpm[i] = RPMSTOPPED; + } + } + + rpm[which_to_respone] |= (RUN_FEEDBACK_ENABLE_MASK | RUN_BLUE_LED_ON_MASK); + + + EscPacket packet = {0xfe, _channels_count, ESCBUS_MSG_ID_RUN}; + packet.len *= sizeof(packet.d.reqRun.rpm_flags[0]); + + for (uint8_t i = 0; i < _channels_count; i++) { + packet.d.reqRun.rpm_flags[i] = rpm[i]; + } + + int ret = send_packet(packet, which_to_respone); + + if (++which_to_respone == _channels_count) { + which_to_respone = 0; + } + + if (ret < 1) { + PX4_WARN("TX ERROR: ret: %d, errno: %d", ret, errno); + } +} + +void TAP_ESC::read_data_from_uart() +{ + uint8_t tmp_serial_buf[UART_BUFFER_SIZE]; + + int len =::read(_uart_fd, tmp_serial_buf, arraySize(tmp_serial_buf)); + + if (len > 0 && (uartbuf.dat_cnt + len < UART_BUFFER_SIZE)) { + for (int i = 0; i < len; i++) { + uartbuf.esc_feedback_buf[uartbuf.tail++] = tmp_serial_buf[i]; + uartbuf.dat_cnt++; + + if (uartbuf.tail >= UART_BUFFER_SIZE) { + uartbuf.tail = 0; + } + } + } +} + +bool TAP_ESC:: parse_tap_esc_feedback(ESC_UART_BUF *serial_buf, EscPacket *packetdata) +{ + static PARSR_ESC_STATE state = HEAD; + static uint8_t data_index = 0; + static uint8_t crc_data_cal; + + if (serial_buf->dat_cnt > 0) { + int count = serial_buf->dat_cnt; + + for (int i = 0; i < count; i++) { + switch (state) { + case HEAD: + if (serial_buf->esc_feedback_buf[serial_buf->head] == 0xFE) { + packetdata->head = 0xFE; //just_keep the format + state = LEN; + } + + break; + + case LEN: + if (serial_buf->esc_feedback_buf[serial_buf->head] < sizeof(packetdata->d)) { + packetdata->len = serial_buf->esc_feedback_buf[serial_buf->head]; + state = ID; + + } else { + state = HEAD; + } + + break; + + case ID: + if (serial_buf->esc_feedback_buf[serial_buf->head] < ESCBUS_MSG_ID_MAX_NUM) { + packetdata->msg_id = serial_buf->esc_feedback_buf[serial_buf->head]; + data_index = 0; + state = DATA; + + } else { + state = HEAD; + } + + break; + + case DATA: + packetdata->d.bytes[data_index++] = serial_buf->esc_feedback_buf[serial_buf->head]; + + if (data_index >= packetdata->len) { + + crc_data_cal = crc8_esc((uint8_t *)(&packetdata->len), packetdata->len + 2); + state = CRC; + } + + break; + + case CRC: + if (crc_data_cal == serial_buf->esc_feedback_buf[serial_buf->head]) { + packetdata->crc_data = serial_buf->esc_feedback_buf[serial_buf->head]; + + if (++serial_buf->head >= UART_BUFFER_SIZE) { + serial_buf->head = 0; + } + + serial_buf->dat_cnt--; + state = HEAD; + return true; + } + + state = HEAD; + break; + + default: + state = HEAD; + break; + + } + + if (++serial_buf->head >= UART_BUFFER_SIZE) { + serial_buf->head = 0; + } + + serial_buf->dat_cnt--; + + } + + + } + + return false; +} + +void +TAP_ESC::cycle() +{ + + if (!_initialized) { + _current_update_rate = 0; + /* advertise the mixed control outputs, insist on the first group output */ + _outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs); + _esc_feedback_pub = orb_advertise(ORB_ID(esc_report), &_esc_feedback); + _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + _test_motor_sub = orb_subscribe(ORB_ID(test_motor)); + _initialized = true; + } + + if (_groups_subscribed != _groups_required) { + subscribe(); + _groups_subscribed = _groups_required; + _current_update_rate = 0; + } + + unsigned max_rate = _pwm_default_rate ; + + if (_current_update_rate != max_rate) { + _current_update_rate = max_rate; + int update_rate_in_ms = int(1000 / _current_update_rate); + + /* reject faster than 500 Hz updates */ + if (update_rate_in_ms < 2) { + update_rate_in_ms = 2; + } + + /* reject slower than 10 Hz updates */ + if (update_rate_in_ms > 100) { + update_rate_in_ms = 100; + } + + DEVICE_DEBUG("adjusted actuator update interval to %ums", update_rate_in_ms); + + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs[i] > 0) { + orb_set_interval(_control_subs[i], update_rate_in_ms); + } + } + + // set to current max rate, even if we are actually checking slower/faster + _current_update_rate = max_rate; + } + + + + /* check if anything updated */ + int ret = ::poll(_poll_fds, _poll_fds_num, 5); + + + /* this would be bad... */ + if (ret < 0) { + DEVICE_LOG("poll error %d", errno); + + } else { /* update even in the case of a timeout, to check for test_motor commands */ + + /* get controls for required topics */ + unsigned poll_id = 0; + + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs[i] > 0) { + if (_poll_fds[poll_id].revents & POLLIN) { + orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); + + } + + poll_id++; + } + } + + size_t num_outputs = _channels_count; + + /* + // FIXME: don't know what this mode should be used for. It's hardcoded in initialization and never changed. + switch (_mode) { + case MODE_2PWM: + num_outputs = 2; + break; + + case MODE_4PWM: + num_outputs = 4; + break; + + case MODE_6PWM: + num_outputs = 6; + break; + + case MODE_8PWM: + num_outputs = 8; + break; + + default: + num_outputs = 0; + break; + } + */ + + /* can we mix? */ + if (_is_armed && _mixers != nullptr) { + + + /* do mixing */ + num_outputs = _mixers->mix(&_outputs.output[0], num_outputs, NULL); + _outputs.noutputs = num_outputs; + _outputs.timestamp = hrt_absolute_time(); + + /* disable unused ports by setting their output to NaN */ + for (size_t i = num_outputs; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) { + _outputs.output[i] = NAN; + } + + /* iterate actuators */ + for (unsigned i = 0; i < num_outputs; i++) { + /* last resort: catch NaN, INF and out-of-band errors */ + if (i < _outputs.noutputs && + PX4_ISFINITE(_outputs.output[i]) && + _outputs.output[i] >= -1.0f && + _outputs.output[i] <= 1.0f) { + /* scale for PWM output 1000 - 2000us */ + _outputs.output[i] = 1600 + (350 * _outputs.output[i]); + + } else { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + _outputs.output[i] = RPMSTOPPED; + } + } + + } else { + + _outputs.noutputs = num_outputs; + _outputs.timestamp = hrt_absolute_time(); + + /* check for motor test commands */ + bool test_motor_updated; + orb_check(_test_motor_sub, &test_motor_updated); + + if (test_motor_updated) { + struct test_motor_s test_motor; + orb_copy(ORB_ID(test_motor), _test_motor_sub, &test_motor); + _outputs.output[test_motor.motor_number] = RPMSTOPPED + ((RPMMAX - RPMSTOPPED) * test_motor.value); + PX4_INFO("setting motor %i to %.1lf", test_motor.motor_number, + (double)_outputs.output[test_motor.motor_number]); + } + + /* set the invalid values to the minimum */ + for (unsigned i = 0; i < num_outputs; i++) { + if (!PX4_ISFINITE(_outputs.output[i])) { + _outputs.output[i] = RPMSTOPPED; + } + } + + /* disable unused ports by setting their output to NaN */ + for (size_t i = num_outputs; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) { + _outputs.output[i] = NAN; + } + + } + + const unsigned esc_count = num_outputs; + float motor_out[TAP_ESC_MAX_MOTOR_NUM]; + + for (int i = 0; i < esc_count; ++i) { + motor_out[i] = _outputs.output[i]; + } + + send_esc_outputs(motor_out, esc_count); + read_data_from_uart(); + + if (parse_tap_esc_feedback(&uartbuf, &_packet) == true) { + if (_packet.msg_id == ESCBUS_MSG_ID_RUN_INFO) { + RunInfoRepsonse &feed_back_data = _packet.d.rspRunInfo; + + if (feed_back_data.channelID < esc_status_s::CONNECTED_ESC_MAX) { + _esc_feedback.esc[feed_back_data.channelID].esc_rpm = feed_back_data.speed; +// _esc_feedback.esc[feed_back_data.channelID].esc_voltage = feed_back_data.voltage; + _esc_feedback.esc[feed_back_data.channelID].esc_state = feed_back_data.ESCStatus; + _esc_feedback.esc[feed_back_data.channelID].esc_vendor = esc_status_s::ESC_VENDOR_TAP; + // printf("vol is %d\n",feed_back_data.voltage ); + // printf("speed is %d\n",feed_back_data.speed ); + + _esc_feedback.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_SERIAL; + _esc_feedback.counter++; + _esc_feedback.esc_count = esc_count; + + _esc_feedback.timestamp = hrt_absolute_time(); + + orb_publish(ORB_ID(esc_status), _esc_feedback_pub, &_esc_feedback); + } + } + } + + /* and publish for anyone that cares to see */ + orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs); + + } + + bool updated; + + orb_check(_armed_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); + + if (_is_armed != _armed.armed) { + /* reset all outputs */ + for (size_t i = 0; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) { + _outputs.output[i] = NAN; + } + } + + /* do not obey the lockdown value, as lockdown is for PWMSim */ + _is_armed = _armed.armed; + + } + + +} + +void TAP_ESC::work_stop() +{ + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs[i] > 0) { + orb_unsubscribe(_control_subs[i]); + _control_subs[i] = -1; + } + } + + orb_unsubscribe(_armed_sub); + _armed_sub = -1; + orb_unsubscribe(_test_motor_sub); + _test_motor_sub = -1; + + DEVICE_LOG("stopping"); + _initialized = false; +} + +int +TAP_ESC::control_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &input) +{ + const actuator_controls_s *controls = (actuator_controls_s *)handle; + + input = controls[control_group].control[control_index]; + + /* limit control input */ + if (input > 1.0f) { + input = 1.0f; + + } else if (input < -1.0f) { + input = -1.0f; + } + + /* motor spinup phase - lock throttle to zero */ + // if (_pwm_limit.state == PWM_LIMIT_STATE_RAMP) { + // if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE || + // control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) && + // control_index == actuator_controls_s::INDEX_THROTTLE) { + // /* limit the throttle output to zero during motor spinup, + // * as the motors cannot follow any demand yet + // */ + // input = 0.0f; + // } + // } + + /* throttle not arming - mark throttle input as invalid */ + if (_armed.prearmed && !_armed.armed) { + if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE || + control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) && + control_index == actuator_controls_s::INDEX_THROTTLE) { + /* set the throttle to an invalid value */ + input = NAN_VALUE; + } + } + + return 0; +} + +int +TAP_ESC::ioctl(file *filp, int cmd, unsigned long arg) +{ + int ret = OK; + + switch (cmd) { + + case MIXERIOCRESET: + if (_mixers != nullptr) { + delete _mixers; + _mixers = nullptr; + _groups_required = 0; + } + + break; + + case MIXERIOCLOADBUF: { + const char *buf = (const char *)arg; + unsigned buflen = strnlen(buf, 1024); + + if (_mixers == nullptr) { + _mixers = new MixerGroup(control_callback, (uintptr_t)_controls); + } + + if (_mixers == nullptr) { + _groups_required = 0; + ret = -ENOMEM; + + } else { + + ret = _mixers->load_from_buf(buf, buflen); + + if (ret != 0) { + DEVICE_DEBUG("mixer load failed with %d", ret); + delete _mixers; + _mixers = nullptr; + _groups_required = 0; + ret = -EINVAL; + + } else { + + _mixers->groups_required(_groups_required); + } + } + + break; + } + + default: + ret = -ENOTTY; + break; + } + + + + return ret; +} + +namespace tap_esc_drv +{ + + +volatile bool _task_should_exit = false; // flag indicating if tap_esc task should exit +static char _device[32] = {}; +static bool _is_running = false; // flag indicating if tap_esc app is running +static px4_task_t _task_handle = -1; // handle to the task main thread +static int _supported_channel_count = 0; + +static bool _flow_control_enabled = false; + +void usage(); + +void start(); +void stop(); +int tap_esc_start(void); +int tap_esc_stop(void); + +void task_main_trampoline(int argc, char *argv[]); + +void task_main(int argc, char *argv[]); + +int initialise_uart(); + +int deinitialize_uart(); + +int enable_flow_control(bool enabled); + +int tap_esc_start(void) +{ + int ret = OK; + + if (tap_esc == nullptr) { + + tap_esc = new TAP_ESC(_supported_channel_count); + + if (tap_esc == nullptr) { + ret = -ENOMEM; + + } else { + ret = tap_esc->init(); + + if (ret != OK) { + PX4_ERR("failed to initialize tap_esc (%i)", ret); + delete tap_esc; + tap_esc = nullptr; + } + } + } + + return ret; +} + +int tap_esc_stop(void) +{ + int ret = OK; + + if (tap_esc != nullptr) { + + delete tap_esc; + tap_esc = nullptr; + } + + return ret; +} + +int initialise_uart() +{ + // open uart + _uart_fd = open(_device, O_RDWR | O_NOCTTY | O_NONBLOCK); + int termios_state = -1; + + if (_uart_fd < 0) { + PX4_ERR("failed to open uart device!"); + return -1; + } + + // set baud rate + int speed = B250000; + struct termios uart_config; + tcgetattr(_uart_fd, &uart_config); + + // clear ONLCR flag (which appends a CR for every LF) + uart_config.c_oflag &= ~ONLCR; + + // set baud rate + if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { + PX4_ERR("failed to set baudrate for %s: %d\n", _device, termios_state); + close(_uart_fd); + return -1; + } + + if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) { + PX4_ERR("tcsetattr failed for %s\n", _device); + close(_uart_fd); + return -1; + } + + // setup output flow control + if (enable_flow_control(false)) { + PX4_WARN("hardware flow disable failed"); + } + + return _uart_fd; +} + +int enable_flow_control(bool enabled) +{ + struct termios uart_config; + + int ret = tcgetattr(_uart_fd, &uart_config); + + if (enabled) { + uart_config.c_cflag |= CRTSCTS; + + } else { + uart_config.c_cflag &= ~CRTSCTS; + } + + ret = tcsetattr(_uart_fd, TCSANOW, &uart_config); + + if (!ret) { + _flow_control_enabled = enabled; + } + + return ret; +} + +int deinitialize_uart() +{ + return close(_uart_fd); +} + +void task_main(int argc, char *argv[]) +{ + + _is_running = true; + + if (initialise_uart() < 0) { + PX4_ERR("Failed to initialize UART."); + + while (_task_should_exit == false) { + usleep(100000); + } + + _is_running = false; + return; + } + + if (tap_esc_start() != OK) { + PX4_ERR("failed to start tap_esc."); + _is_running = false; + return; + } + + + // Main loop + while (!_task_should_exit) { + + tap_esc->cycle(); + + } + + + _is_running = false; +} + +void task_main_trampoline(int argc, char *argv[]) +{ + task_main(argc, argv); +} + +void start() +{ + ASSERT(_task_handle == -1); + + _task_should_exit = false; + + /* start the task */ + _task_handle = px4_task_spawn_cmd("tap_esc_main", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX, + 2500, + (px4_main_t)&task_main_trampoline, + nullptr); + + if (_task_handle < 0) { + PX4_ERR("task start failed"); + _task_handle = -1; + return; + } +} + +void stop() +{ + _task_should_exit = true; + + while (_is_running) { + usleep(200000); + PX4_INFO("tap_esc_stop"); + } + + tap_esc_stop(); + deinitialize_uart(); + _task_handle = -1; +} + +void usage() +{ + PX4_INFO("usage: tap_esc start -d /dev/ttyS2 -n <1-8>"); + PX4_INFO(" tap_esc stop"); + PX4_INFO(" tap_esc status"); +} + +} // namespace tap_esc + +// driver 'main' command +extern "C" __EXPORT int tap_esc_main(int argc, char *argv[]); + +int tap_esc_main(int argc, char *argv[]) +{ + const char *device = nullptr; + int ch; + int myoptind = 1; + const char *myoptarg = nullptr; + + char *verb = nullptr; + + if (argc >= 2) { + verb = argv[1]; + } + + while ((ch = px4_getopt(argc, argv, "d:n:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'd': + device = myoptarg; + strncpy(tap_esc_drv::_device, device, strlen(device)); + break; + + case 'n': + tap_esc_drv::_supported_channel_count = atoi(myoptarg); + break; + } + } + + if (!tap_esc && tap_esc_drv::_task_handle != -1) { + tap_esc_drv::_task_handle = -1; + } + + // Start/load the driver. + if (!strcmp(verb, "start")) { + if (tap_esc_drv::_is_running) { + PX4_WARN("tap_esc already running"); + return 1; + } + + // Check on required arguments + if (tap_esc_drv::_supported_channel_count == 0 || device == nullptr || strlen(device) == 0) { + tap_esc_drv::usage(); + return 1; + } + + tap_esc_drv::start(); + } + + else if (!strcmp(verb, "stop")) { + if (!tap_esc_drv::_is_running) { + PX4_WARN("tap_esc is not running"); + return 1; + } + + tap_esc_drv::stop(); + } + + else if (!strcmp(verb, "status")) { + PX4_WARN("tap_esc is %s", tap_esc_drv::_is_running ? "running" : "not running"); + return 0; + + } else { + tap_esc_drv::usage(); + return 1; + } + + return 0; +} diff --git a/src/drivers/test_ppm/test_ppm.cpp b/src/drivers/test_ppm/test_ppm.cpp index 5dce26cfe5..db36b7a16b 100644 --- a/src/drivers/test_ppm/test_ppm.cpp +++ b/src/drivers/test_ppm/test_ppm.cpp @@ -118,7 +118,7 @@ TEST_PPM::~TEST_PPM() int TEST_PPM::init() { - stm32_configgpio(TEST_PPM_PIN); + px4_arch_configgpio(TEST_PPM_PIN); start(); return OK; } @@ -148,11 +148,11 @@ void TEST_PPM::do_out(void) { if ((_call_times % 2) == 0) { - stm32_gpiowrite(TEST_PPM_PIN, false); + px4_arch_gpiowrite(TEST_PPM_PIN, false); hrt_call_after(&_call, _values[_call_times / 2] - _plus_width, (hrt_callout)&TEST_PPM::loops, this); } else { - stm32_gpiowrite(TEST_PPM_PIN, true); + px4_arch_gpiowrite(TEST_PPM_PIN, true); hrt_call_after(&_call, _plus_width, (hrt_callout)&TEST_PPM::loops, this); } diff --git a/src/drivers/trone/trone.cpp b/src/drivers/trone/trone.cpp index 3ec40c624d..ff6362b2ff 100644 --- a/src/drivers/trone/trone.cpp +++ b/src/drivers/trone/trone.cpp @@ -442,14 +442,14 @@ TRONE::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; } - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); if (!_reports->resize(arg)) { - irqrestore(flags); + px4_leave_critical_section(flags); return -ENOMEM; } - irqrestore(flags); + px4_leave_critical_section(flags); return OK; } @@ -628,12 +628,12 @@ TRONE::start() work_queue(HPWORK, &_work, (worker_t)&TRONE::cycle_trampoline, this, 1); /* notify about state change */ - struct subsystem_info_s info = { - true, - true, - true, - subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER - }; + struct subsystem_info_s info = {}; + info.present = true; + info.enabled = true; + info.ok = true; + info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER; + static orb_advert_t pub = nullptr; if (pub != nullptr) { diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index 3d6aa88269..f8a9de39ce 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -61,10 +61,6 @@ #include #include #include -#include -#include -#include -#include #include #include #include diff --git a/src/examples/hwtest/hwtest.c b/src/examples/hwtest/hwtest.c index 97a525a8e9..b1b1b465f9 100644 --- a/src/examples/hwtest/hwtest.c +++ b/src/examples/hwtest/hwtest.c @@ -47,10 +47,6 @@ #include #include #include -#include -#include -#include -#include #include __EXPORT int ex_hwtest_main(int argc, char *argv[]); diff --git a/src/examples/px4_simple_app/module.mk b/src/examples/px4_simple_app/module.mk deleted file mode 100644 index 32b06c3a58..0000000000 --- a/src/examples/px4_simple_app/module.mk +++ /dev/null @@ -1,40 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Basic example application -# - -MODULE_COMMAND = px4_simple_app - -SRCS = px4_simple_app.c diff --git a/src/examples/rover_steering_control/main.cpp b/src/examples/rover_steering_control/main.cpp index 7a227c2366..563d6b0cca 100644 --- a/src/examples/rover_steering_control/main.cpp +++ b/src/examples/rover_steering_control/main.cpp @@ -58,10 +58,6 @@ #include #include #include -#include -#include -#include -#include #include #include #include diff --git a/src/firmware/nuttx/CMakeLists.txt b/src/firmware/nuttx/CMakeLists.txt index 926d331ec1..53988b43cf 100644 --- a/src/firmware/nuttx/CMakeLists.txt +++ b/src/firmware/nuttx/CMakeLists.txt @@ -45,16 +45,25 @@ if(NOT ${BOARD} STREQUAL "sim") if (config_io_board) set(extras "${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}.bin") endif() + + set(romfs_dir "ROMFS/px4fmu_common") + if (${BOARD} STREQUAL "tap-v1") + set(romfs_dir "ROMFS/tap_common") + endif() + if (${BOARD} STREQUAL "px4fmu-v2" AND ${LABEL} STREQUAL "test") + set(romfs_dir "ROMFS/px4fmu_test") + endif() + px4_nuttx_add_romfs(OUT romfs - ROOT ROMFS/px4fmu_common + ROOT ${romfs_dir} EXTRAS ${extras} ) if (config_io_board) add_dependencies(romfs fw_io) endif() - set(fw_file - ${CMAKE_CURRENT_BINARY_DIR}/${OS}-${BOARD}-${LABEL}.px4) + + set(fw_file ${CMAKE_CURRENT_BINARY_DIR}/${OS}-${BOARD}-${LABEL}.px4) px4_nuttx_add_firmware(OUT ${fw_file} BOARD ${BOARD} diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt index 030ad0793c..3e8e8b5227 100644 --- a/src/firmware/posix/CMakeLists.txt +++ b/src/firmware/posix/CMakeLists.txt @@ -13,7 +13,7 @@ if ("${BOARD}" STREQUAL "eagle" OR ("${BOARD}" STREQUAL "excelsior")) set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wno-missing-prototypes -Wno-missing-declarations") LINUX_APP( - APP_NAME mainapp + APP_NAME px4 IDL_NAME px4muorb APPS_DEST "/home/linaro" SOURCES @@ -32,18 +32,47 @@ if ("${BOARD}" STREQUAL "eagle" OR ("${BOARD}" STREQUAL "excelsior")) px4_add_adb_push(OUT upload OS ${OS} BOARD ${BOARD} - FILES ${CMAKE_CURRENT_BINARY_DIR}/mainapp + FILES ${CMAKE_CURRENT_BINARY_DIR}/px4 ${CMAKE_SOURCE_DIR}/posix-configs/eagle/flight/mainapp.config - DEPENDS mainapp + DEPENDS px4 DEST /home/linaro) -else() - add_executable(mainapp +elseif ("${BOARD}" STREQUAL "rpi") + + add_executable(px4 ${CMAKE_SOURCE_DIR}/src/platforms/posix/main.cpp apps.h ) + + target_link_libraries(px4 + -Wl,--start-group + ${module_libraries} + df_driver_framework + ${df_driver_libs} + pthread m rt + -Wl,--end-group + ) + + px4_add_scp_push(OUT upload + OS ${OS} + BOARD ${BOARD} + FILES ${CMAKE_CURRENT_BINARY_DIR}/px4 + ${CMAKE_SOURCE_DIR}/posix-configs/rpi/px4.config + DEPENDS px4 + DEST /home/pi) + +elseif ("${BOARD}" STREQUAL "bebop") + + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -static") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -static") + + add_executable(px4 + ${CMAKE_SOURCE_DIR}/src/platforms/posix/main.cpp + apps.h + ) + if (NOT APPLE) - target_link_libraries(mainapp + target_link_libraries(px4 -Wl,--start-group ${module_libraries} ${df_driver_libs} @@ -51,7 +80,37 @@ else() -Wl,--end-group ) else() - target_link_libraries(mainapp + target_link_libraries(px4 + ${module_libraries} + ${df_driver_libs} + pthread m + ) + endif() + + px4_add_adb_push_to_bebop(OUT upload + OS ${OS} + BOARD ${BOARD} + FILES ${CMAKE_CURRENT_BINARY_DIR}/px4 + ${CMAKE_SOURCE_DIR}/posix-configs/bebop/px4.config + DEPENDS px4 + DEST /usr/bin) + +else() + + add_executable(px4 + ${CMAKE_SOURCE_DIR}/src/platforms/posix/main.cpp + apps.h + ) + if (NOT APPLE) + target_link_libraries(px4 + -Wl,--start-group + ${module_libraries} + ${df_driver_libs} + pthread m rt + -Wl,--end-group + ) + else() + target_link_libraries(px4 ${module_libraries} ${df_driver_libs} pthread m @@ -65,11 +124,11 @@ add_custom_target(run_config WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} USES_TERMINAL ) -add_dependencies(run_config mainapp) +add_dependencies(run_config px4) foreach(viewer none jmavsim gazebo replay) foreach(debugger none gdb lldb ddd valgrind) - foreach(model none iris iris_opt_flow tailsitter standard_vtol plane) + foreach(model none iris iris_opt_flow tailsitter standard_vtol plane solo typhoon_h480) if (debugger STREQUAL "none") if (model STREQUAL "none") set(_targ_name "${viewer}") @@ -90,9 +149,17 @@ foreach(viewer none jmavsim gazebo replay) WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} USES_TERMINAL ) - add_dependencies(${_targ_name} mainapp) + add_dependencies(${_targ_name} px4) endforeach() endforeach() endforeach() +#============================================================================= +# install +# + +install(TARGETS px4 DESTINATION bin) +install(DIRECTORY ${PROJECT_SOURCE_DIR}/ROMFS DESTINATION ${CMAKE_INSTALL_DATADIR}/${PROJECT_NAME}) +install(DIRECTORY ${PROJECT_SOURCE_DIR}/posix-configs DESTINATION ${CMAKE_INSTALL_DATADIR}/${PROJECT_NAME}) + # vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/firmware/qurt/CMakeLists.txt b/src/firmware/qurt/CMakeLists.txt index 29046e54bb..81be14afc5 100644 --- a/src/firmware/qurt/CMakeLists.txt +++ b/src/firmware/qurt/CMakeLists.txt @@ -19,11 +19,11 @@ if ("${QURT_ENABLE_STUBS}" STREQUAL "1") ${CMAKE_CURRENT_BINARY_DIR} ${FASTRPC_DSP_INCLUDES} ) - add_executable(mainapp + add_executable(px4 ${CMAKE_BINARY_DIR}/src/firmware/qurt/px4muorb_skel.c ${CMAKE_BINARY_DIR}/apps.h) - target_link_libraries(mainapp + target_link_libraries(px4 -Wl,--start-group ${module_libraries} ${target_libraries} @@ -34,9 +34,10 @@ else() message("module_libraries = ${module_libraries}") message("target_libraries = ${target_libraries}") message("df_driver_libs = ${df_driver_libs}") + message("module_external_libraries = ${module_external_libraries}") # Generate the DSP lib and stubs but not the apps side executable # The Apps side executable is generated via the posix_eagle_xxxx target - QURT_LIB(LIB_NAME mainapp + QURT_LIB(LIB_NAME px4 IDL_NAME px4muorb SOURCES ${CMAKE_BINARY_DIR}/apps.h @@ -45,15 +46,16 @@ else() ${target_libraries} ${df_driver_libs} m + ${module_external_libraries} ) px4_add_adb_push(OUT upload OS ${OS} BOARD ${BOARD} - FILES ${CMAKE_CURRENT_BINARY_DIR}/libmainapp.so + FILES ${CMAKE_CURRENT_BINARY_DIR}/libpx4.so ${CMAKE_CURRENT_BINARY_DIR}/libpx4muorb_skel.so ${CMAKE_SOURCE_DIR}/posix-configs/eagle/flight/px4.config - DEPENDS mainapp px4muorb_skel + DEPENDS px4 px4muorb_skel DEST /usr/share/data/adsp) endif() diff --git a/src/firmware/qurt/px4muorb.idl b/src/firmware/qurt/px4muorb.idl index 49e1305ffd..e87492fb3e 100644 --- a/src/firmware/qurt/px4muorb.idl +++ b/src/firmware/qurt/px4muorb.idl @@ -49,6 +49,28 @@ interface px4muorb{ * @param time_us: pointer to time in us */ AEEResult get_absolute_time(rout unsigned long long time_us); + + /** + * Interface to update param for krait. + * + * @param param: param index. + * @param value: param value. + */ + AEEResult param_update_to_shmem( in unsigned long param, in sequence value); + + /** + * Interface to update index for krait. + * @param data: param index array. + */ + AEEResult param_update_index_from_shmem( rout sequence data); + + /** + * Interface to get param value for krait. + * + * @param param: param index. + * @param value: param value. + */ + AEEResult param_update_value_from_shmem( in unsigned long param, rout sequence value); /** * Interface to add a subscriber to the identified topic diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 62449aef6d..8658b2e40e 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 62449aef6decd2faa75bc2abd509333df5a675c4 +Subproject commit 8658b2e40ee41c75d0a2a445fccbb07fd637f836 diff --git a/src/lib/controllib/block/Block.hpp b/src/lib/controllib/block/Block.hpp index e74258b523..7c2fd4a524 100644 --- a/src/lib/controllib/block/Block.hpp +++ b/src/lib/controllib/block/Block.hpp @@ -39,7 +39,6 @@ #pragma once -#define __STDC_FORMAT_MACROS #include #include diff --git a/src/lib/controllib/block/BlockParam.hpp b/src/lib/controllib/block/BlockParam.hpp index 693a8ec3b2..f88a80e6ba 100644 --- a/src/lib/controllib/block/BlockParam.hpp +++ b/src/lib/controllib/block/BlockParam.hpp @@ -78,6 +78,9 @@ class BlockParam : public BlockParamBase public: BlockParam(Block *block, const char *name, bool parent_prefix = true, T *extern_address = NULL); + BlockParam(const BlockParam &) = delete; + BlockParam &operator=(const BlockParam &) = delete; + T get(); void commit(); void set(T val); diff --git a/src/lib/controllib/blocks.cpp b/src/lib/controllib/blocks.cpp index aeef0f0977..0e0c3a6ccc 100644 --- a/src/lib/controllib/blocks.cpp +++ b/src/lib/controllib/blocks.cpp @@ -84,6 +84,7 @@ float BlockLowPass::update(float input) return getState(); } + float BlockHighPass::update(float input) { float b = 2 * float(M_PI) * getFCut() * getDt(); diff --git a/src/lib/controllib/blocks.hpp b/src/lib/controllib/blocks.hpp index 28b73aa62d..a56a32b216 100644 --- a/src/lib/controllib/blocks.hpp +++ b/src/lib/controllib/blocks.hpp @@ -127,6 +127,45 @@ protected: control::BlockParamFloat _fCut; }; +template +class __EXPORT BlockLowPassVector: public Block +{ +public: +// methods + BlockLowPassVector(SuperBlock *parent, + const char *name) : + Block(parent, name), + _state(), + _fCut(this, "") // only one parameter, no need to name + { + for (int i = 0; i < M; i++) { + _state(i) = 0.0f / 0.0f; + } + }; + virtual ~BlockLowPassVector() {}; + matrix::Vector update(const matrix::Matrix &input) + { + for (int i = 0; i < M; i++) { + if (!PX4_ISFINITE(getState()(i))) { + setState(input); + } + } + + float b = 2 * float(M_PI) * getFCut() * getDt(); + float a = b / (1 + b); + setState(input * a + getState() * (1 - a)); + return getState(); + } +// accessors + matrix::Vector getState() { return _state; } + float getFCut() { return _fCut.get(); } + void setState(const matrix::Vector &state) { _state = state; } +private: +// attributes + matrix::Vector _state; + control::BlockParamFloat _fCut; +}; + /** * A high pass filter as described here: * http://en.wikipedia.org/wiki/High-pass_filter. diff --git a/src/lib/controllib/uorb/blocks.hpp b/src/lib/controllib/uorb/blocks.hpp index 7766b67f6b..42aae92284 100644 --- a/src/lib/controllib/uorb/blocks.hpp +++ b/src/lib/controllib/uorb/blocks.hpp @@ -47,10 +47,6 @@ #include #include #include -#include -#include -#include -#include #include #include diff --git a/src/lib/ecl b/src/lib/ecl index 199c423f1f..df0f8fed45 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit 199c423f1f38d8b4a12f4003aa1d0a585ce72d12 +Subproject commit df0f8fed45b94b44f18595abafbd5f6fd1de4275 diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 8e8e03efc0..94181dc22b 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -40,7 +40,7 @@ void TECS::update_state(float baro_altitude, float airspeed, const math::Matrix< // Calculate time in seconds since last update uint64_t now = ecl_absolute_time(); - float DT = max((now - _update_50hz_last_usec), 0ULL) * 1.0e-6f; + float DT = max((now - _update_50hz_last_usec), UINT64_C(0)) * 1.0e-6f; // printf("dt: %10.6f baro alt: %6.2f eas: %6.2f R(0,0): %6.2f, R(1,1): %6.2f\naccel body: %6.2f %6.2f %6.2f\naccel earth: %6.2f %6.2f %6.2f\n", // DT, baro_altitude, airspeed, rotMat(0, 0), rotMat(1, 1), accel_body(0), accel_body(1), accel_body(2), @@ -122,7 +122,7 @@ void TECS::_update_speed(float airspeed_demand, float indicated_airspeed, { // Calculate time in seconds since last update uint64_t now = ecl_absolute_time(); - float DT = max((now - _update_speed_last_usec), 0ULL) * 1.0e-6f; + float DT = max((now - _update_speed_last_usec), UINT64_C(0)) * 1.0e-6f; // Convert equivalent airspeeds to true airspeeds @@ -550,7 +550,7 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f // Calculate time in seconds since last update uint64_t now = ecl_absolute_time(); - _DT = max((now - _update_pitch_throttle_last_usec), 0ULL) * 1.0e-6f; + _DT = max((now - _update_pitch_throttle_last_usec), UINT64_C(0)) * 1.0e-6f; // printf("tecs in: dt:%10.6f pitch: %6.2f baro_alt: %6.2f alt sp: %6.2f\neas sp: %6.2f eas: %6.2f, eas2tas: %6.2f\n %s pitch min C0: %6.2f thr min: %6.2f, thr max: %6.2f thr cruis: %6.2f pt min: %6.2f, pt max: %6.2f\n", // _DT, pitch, baro_altitude, hgt_dem, EAS_dem, indicated_airspeed, EAS2TAS, (climbOutDem) ? "climb" : "level", ptchMinCO, throttle_min, throttle_max, throttle_cruise, pitch_limit_min, pitch_limit_max); diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index e892a72f7a..cbcc6ed9ae 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -66,6 +66,8 @@ public: _TASmin(3.0f), _TAS_dem(0.0f), _TAS_dem_last(0.0f), + _EAS_dem(0.0f), + _hgt_dem(0.0f), _hgt_dem_in_old(0.0f), _hgt_dem_adj(0.0f), _hgt_dem_adj_last(0.0f), diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h index 3b276c5563..cc64f8b431 100644 --- a/src/lib/launchdetection/LaunchDetector.h +++ b/src/lib/launchdetection/LaunchDetector.h @@ -55,7 +55,10 @@ class __EXPORT LaunchDetector : public control::SuperBlock { public: LaunchDetector(); - ~LaunchDetector(); + LaunchDetector(const LaunchDetector &) = delete; + LaunchDetector operator=(const LaunchDetector &) = delete; + virtual ~LaunchDetector(); + void reset(); void update(float accel_x); @@ -80,7 +83,6 @@ private: control::BlockParamInt launchdetection_on; control::BlockParamFloat throttlePreTakeoff; - }; } diff --git a/src/lib/launchdetection/LaunchMethod.h b/src/lib/launchdetection/LaunchMethod.h index 66ee3f2c6e..065811aa9c 100644 --- a/src/lib/launchdetection/LaunchMethod.h +++ b/src/lib/launchdetection/LaunchMethod.h @@ -58,6 +58,8 @@ enum LaunchDetectionResult { class LaunchMethod { public: + virtual ~LaunchMethod() {}; + virtual void update(float accel_x) = 0; virtual LaunchDetectionResult getLaunchDetected() const = 0; virtual void reset() = 0; diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c index 9af90279d2..6af608b48b 100644 --- a/src/lib/launchdetection/launchdetection_params.c +++ b/src/lib/launchdetection/launchdetection_params.c @@ -45,11 +45,9 @@ */ /** - * Enable launch detection. + * Launch detection * * @boolean - * @min 0 - * @max 1 * @group Launch detection */ PARAM_DEFINE_INT32(LAUN_ALL_ON, 0); @@ -61,6 +59,8 @@ PARAM_DEFINE_INT32(LAUN_ALL_ON, 0); * * @unit m/s/s * @min 0 + * @decimal 1 + * @increment 0.5 * @group Launch detection */ PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f); @@ -71,7 +71,10 @@ PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f); * LAUN_CAT_A for LAUN_CAT_T serves as threshold to trigger launch detection. * * @unit s - * @min 0 + * @min 0.0 + * @max 5.0 + * @decimal 2 + * @increment 0.05 * @group Launch detection */ PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f); @@ -83,7 +86,10 @@ PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f); * Before this timespan is up the throttle will be set to FW_THR_IDLE, set to 0 to deactivate * * @unit s - * @min 0 + * @min 0.0 + * @max 10.0 + * @decimal 1 + * @increment 0.5 * @group Launch detection */ PARAM_DEFINE_FLOAT(LAUN_CAT_MDEL, 0.0f); @@ -95,8 +101,10 @@ PARAM_DEFINE_FLOAT(LAUN_CAT_MDEL, 0.0f); * This allows to limit the maximum pitch angle during a bungee launch (make the launch less steep). * * @unit deg - * @min 0 - * @max 45 + * @min 0.0 + * @max 45.0 + * @decimal 1 + * @increment 0.5 * @group Launch detection */ PARAM_DEFINE_FLOAT(LAUN_CAT_PMAX, 30.0f); diff --git a/src/lib/mathlib/CMakeLists.txt b/src/lib/mathlib/CMakeLists.txt index 6bfe964d79..e19201da74 100644 --- a/src/lib/mathlib/CMakeLists.txt +++ b/src/lib/mathlib/CMakeLists.txt @@ -32,6 +32,8 @@ ############################################################################ px4_add_module( MODULE lib__mathlib + COMPILE_FLAGS + -Os SRCS math/test/test.cpp math/Limits.cpp diff --git a/src/lib/mathlib/math/Limits.cpp b/src/lib/mathlib/math/Limits.cpp index 4b1273c7a1..114929ba17 100644 --- a/src/lib/mathlib/math/Limits.cpp +++ b/src/lib/mathlib/math/Limits.cpp @@ -51,82 +51,6 @@ namespace math #define M_PI_F 3.14159265358979323846f #endif -float __EXPORT min(float val1, float val2) -{ - return (val1 < val2) ? val1 : val2; -} - -int __EXPORT min(int val1, int val2) -{ - return (val1 < val2) ? val1 : val2; -} - -unsigned __EXPORT min(unsigned val1, unsigned val2) -{ - return (val1 < val2) ? val1 : val2; -} - -uint64_t __EXPORT min(uint64_t val1, uint64_t val2) -{ - return (val1 < val2) ? val1 : val2; -} - -double __EXPORT min(double val1, double val2) -{ - return (val1 < val2) ? val1 : val2; -} - -float __EXPORT max(float val1, float val2) -{ - return (val1 > val2) ? val1 : val2; -} - -int __EXPORT max(int val1, int val2) -{ - return (val1 > val2) ? val1 : val2; -} - -unsigned __EXPORT max(unsigned val1, unsigned val2) -{ - return (val1 > val2) ? val1 : val2; -} - -uint64_t __EXPORT max(uint64_t val1, uint64_t val2) -{ - return (val1 > val2) ? val1 : val2; -} - -double __EXPORT max(double val1, double val2) -{ - return (val1 > val2) ? val1 : val2; -} - - -float __EXPORT constrain(float val, float min, float max) -{ - return (val < min) ? min : ((val > max) ? max : val); -} - -int __EXPORT constrain(int val, int min, int max) -{ - return (val < min) ? min : ((val > max) ? max : val); -} - -unsigned __EXPORT constrain(unsigned val, unsigned min, unsigned max) -{ - return (val < min) ? min : ((val > max) ? max : val); -} - -uint64_t __EXPORT constrain(uint64_t val, uint64_t min, uint64_t max) -{ - return (val < min) ? min : ((val > max) ? max : val); -} - -double __EXPORT constrain(double val, double min, double max) -{ - return (val < min) ? min : ((val > max) ? max : val); -} - float __EXPORT radians(float degrees) { return (degrees / 180.0f) * M_PI_F; diff --git a/src/lib/mathlib/math/Limits.hpp b/src/lib/mathlib/math/Limits.hpp index f04d725078..6215b8f955 100644 --- a/src/lib/mathlib/math/Limits.hpp +++ b/src/lib/mathlib/math/Limits.hpp @@ -42,40 +42,36 @@ #include #include +//this should be defined in stdint.h, but seems to be missing in the ARM toolchain (5.2.0) +#ifndef UINT64_C +# if __WORDSIZE == 64 +# define UINT64_C(c) c ## UL +# else +# define UINT64_C(c) c ## ULL +# endif +#endif + + namespace math { +template +inline const _Tp &min(const _Tp &a, const _Tp &b) +{ + return (a < b) ? a : b; +} -float __EXPORT min(float val1, float val2); +template +inline const _Tp &max(const _Tp &a, const _Tp &b) +{ + return (a > b) ? a : b; +} -int __EXPORT min(int val1, int val2); - -unsigned __EXPORT min(unsigned val1, unsigned val2); - -uint64_t __EXPORT min(uint64_t val1, uint64_t val2); - -double __EXPORT min(double val1, double val2); - -float __EXPORT max(float val1, float val2); - -int __EXPORT max(int val1, int val2); - -unsigned __EXPORT max(unsigned val1, unsigned val2); - -uint64_t __EXPORT max(uint64_t val1, uint64_t val2); - -double __EXPORT max(double val1, double val2); - - -float __EXPORT constrain(float val, float min, float max); - -int __EXPORT constrain(int val, int min, int max); - -unsigned __EXPORT constrain(unsigned val, unsigned min, unsigned max); - -uint64_t __EXPORT constrain(uint64_t val, uint64_t min, uint64_t max); - -double __EXPORT constrain(double val, double min, double max); +template +inline const _Tp &constrain(const _Tp &val, const _Tp &min_val, const _Tp &max_val) +{ + return (val < min_val) ? min_val : ((val > max_val) ? max_val : val); +} float __EXPORT radians(float degrees); diff --git a/src/lib/mathlib/math/filter/module.mk b/src/lib/mathlib/math/filter/module.mk deleted file mode 100644 index f5c0647c88..0000000000 --- a/src/lib/mathlib/math/filter/module.mk +++ /dev/null @@ -1,43 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# filter library -# -SRCS = LowPassFilter2p.cpp - -# -# In order to include .config we first have to save off the -# current makefile name, since app.mk needs it. -# -APP_MAKEFILE := $(lastword $(MAKEFILE_LIST)) diff --git a/src/lib/matrix b/src/lib/matrix index 07fba8322a..cbb7e06a1f 160000 --- a/src/lib/matrix +++ b/src/lib/matrix @@ -1 +1 @@ -Subproject commit 07fba8322a16cb2dac47e6a8ac7d21ec346313ff +Subproject commit cbb7e06a1fa7e20304c3ca3099b18c704da6c3a4 diff --git a/src/lib/rc/CMakeLists.txt b/src/lib/rc/CMakeLists.txt index 15bbd71bb9..8f9471124e 100644 --- a/src/lib/rc/CMakeLists.txt +++ b/src/lib/rc/CMakeLists.txt @@ -34,6 +34,7 @@ px4_add_module( MODULE lib__rc COMPILE_FLAGS -Os + -Wno-unused-result SRCS st24.c sumd.c diff --git a/src/lib/rc/rc_tests/CMakeLists.txt b/src/lib/rc/rc_tests/CMakeLists.txt new file mode 100644 index 0000000000..42a5a1b1ba --- /dev/null +++ b/src/lib/rc/rc_tests/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2016 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. +# +############################################################################ +px4_add_module( + MODULE lib__rc__rc_tests + MAIN rc_tests + COMPILE_FLAGS + -Wno-unused-result + SRCS + RCTest.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/lib/rc/rc_tests/RCTest.cpp b/src/lib/rc/rc_tests/RCTest.cpp new file mode 100644 index 0000000000..d7a834d5c8 --- /dev/null +++ b/src/lib/rc/rc_tests/RCTest.cpp @@ -0,0 +1,306 @@ +#include + +#include + +#include +#include + +#include + +#define DSM_DEBUG +#include +#include +#include +#include + +#if !defined(CONFIG_ARCH_BOARD_SITL) +#define TEST_DATA_PATH "/fs/microsd" +#else +#define TEST_DATA_PATH "../../../../src/lib/rc/rc_tests/test_data/" +#endif + +extern "C" __EXPORT int rc_tests_main(int argc, char *argv[]); + +class RCTest : public UnitTest +{ +public: + virtual bool run_tests(void); + +private: + bool dsmTest(); + bool sbus2Test(); + bool st24Test(); + bool sumdTest(); +}; + +bool RCTest::run_tests(void) +{ + ut_run_test(dsmTest); + ut_run_test(sbus2Test); + ut_run_test(st24Test); + ut_run_test(sumdTest); + + return (_tests_failed == 0); +} + +bool RCTest::dsmTest(void) +{ + + const char *filepath = TEST_DATA_PATH "dsm_x_data.txt"; + + FILE *fp; + fp = fopen(filepath, "rt"); + + ut_test(fp != nullptr); + //warnx("loading data from: %s", filepath); + + float f; + unsigned x; + int ret; + + // Trash the first 20 lines + for (unsigned i = 0; i < 20; i++) { + char buf[200]; + (void)fgets(buf, sizeof(buf), fp); + } + + // Init the parser + uint8_t frame[20]; + uint16_t rc_values[18]; + uint16_t num_values; + bool dsm_11_bit; + unsigned dsm_frame_drops = 0; + uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]); + + int rate_limiter = 0; + unsigned last_drop = 0; + + while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) { + + ut_test(ret > 0); + + frame[0] = x; + unsigned len = 1; + + // Pipe the data into the parser + bool result = dsm_parse(f * 1e6f, &frame[0], len, rc_values, &num_values, + &dsm_11_bit, &dsm_frame_drops, max_channels); + + if (result) { + //warnx("decoded packet with %d channels and %s encoding:", num_values, (dsm_11_bit) ? "11 bit" : "10 bit"); + + for (unsigned i = 0; i < num_values; i++) { + //printf("chan #%u:\t%d\n", i, (int)rc_values[i]); + } + } + + if (last_drop != (dsm_frame_drops)) { + //warnx("frame dropped, now #%d", (dsm_frame_drops)); + last_drop = dsm_frame_drops; + } + + rate_limiter++; + } + + ut_test(ret == EOF); + + return true; +} + +bool RCTest::sbus2Test(void) +{ + const char *filepath = TEST_DATA_PATH "sbus2_r7008SB.txt"; + + FILE *fp; + fp = fopen(filepath, "rt"); + + ut_test(fp != nullptr); + //warnx("loading data from: %s", filepath); + + // if (argc < 2) + // errx(1, "Need a filename for the input file"); + + //int byte_offset = 7; + + // if (argc > 2) { + // char* end; + // byte_offset = strtol(argv[2],&end,10); + // } + + //warnx("RUNNING TEST WITH BYTE OFFSET OF: %d", byte_offset); + + float f; + unsigned x; + int ret; + + // Trash the first 20 lines + for (unsigned i = 0; i < 20; i++) { + char buf[200]; + (void)fgets(buf, sizeof(buf), fp); + } + + // Init the parser + uint8_t frame[SBUS_BUFFER_SIZE]; + uint16_t rc_values[18]; + uint16_t num_values; + unsigned sbus_frame_drops = 0; + unsigned sbus_frame_resets = 0; + bool sbus_failsafe; + bool sbus_frame_drop; + uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]); + + int rate_limiter = 0; + unsigned last_drop = 0; + + while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) { + + ut_test(ret > 0); + + frame[0] = x; + unsigned len = 1; + + // Pipe the data into the parser + hrt_abstime now = hrt_absolute_time(); + + // if (rate_limiter % byte_offset == 0) { + bool result = sbus_parse(now, &frame[0], len, rc_values, &num_values, + &sbus_failsafe, &sbus_frame_drop, &sbus_frame_drops, max_channels); + + if (result) { + //warnx("decoded packet"); + } + + // } + + if (last_drop != (sbus_frame_drops + sbus_frame_resets)) { + warnx("frame dropped, now #%d", (sbus_frame_drops + sbus_frame_resets)); + last_drop = sbus_frame_drops + sbus_frame_resets; + } + + rate_limiter++; + } + + ut_test(ret == EOF); + + return true; +} + +bool RCTest::st24Test(void) +{ + const char *filepath = TEST_DATA_PATH "st24_data.txt"; + + //warnx("loading data from: %s", filepath); + + FILE *fp; + + fp = fopen(filepath, "rt"); + ut_test(fp != nullptr); + + float f; + unsigned x; + int ret; + + // Trash the first 20 lines + for (unsigned i = 0; i < 20; i++) { + char buf[200]; + (void)fgets(buf, sizeof(buf), fp); + } + + float last_time = 0; + + while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) { + ut_test(ret > 0); + + if (((f - last_time) * 1000 * 1000) > 3000) { + // warnx("FRAME RESET\n\n"); + } + + uint8_t b = static_cast(x); + + last_time = f; + + // Pipe the data into the parser + //hrt_abstime now = hrt_absolute_time(); + + uint8_t rssi; + uint8_t rx_count; + uint16_t channel_count; + uint16_t channels[20]; + + if (!st24_decode(b, &rssi, &rx_count, &channel_count, channels, sizeof(channels) / sizeof(channels[0]))) { + //warnx("decoded: %u channels (converted to PPM range)", (unsigned)channel_count); + + for (unsigned i = 0; i < channel_count; i++) { + //int16_t val = channels[i]; + //warnx("channel %u: %d 0x%03X", i, static_cast(val), static_cast(val)); + } + } + } + + ut_test(ret == EOF); + + return true; +} + +bool RCTest::sumdTest(void) +{ + const char *filepath = TEST_DATA_PATH "sumd_data.txt"; + + //warnx("loading data from: %s", filepath); + + FILE *fp; + + fp = fopen(filepath, "rt"); + ut_test(fp != nullptr); + + float f; + unsigned x; + int ret; + + // Trash the first 20 lines + for (unsigned i = 0; i < 20; i++) { + char buf[200]; + (void)fgets(buf, sizeof(buf), fp); + } + + float last_time = 0; + + while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) { + ut_test(ret > 0); + + if (((f - last_time) * 1000 * 1000) > 3000) { + // warnx("FRAME RESET\n\n"); + } + + uint8_t b = static_cast(x); + + last_time = f; + + // Pipe the data into the parser + //hrt_abstime now = hrt_absolute_time(); + + uint8_t rssi; + uint8_t rx_count; + uint16_t channel_count; + uint16_t channels[32]; + + + if (!sumd_decode(b, &rssi, &rx_count, &channel_count, channels, 32)) { + //warnx("decoded: %u channels (converted to PPM range)", (unsigned)channel_count); + + for (unsigned i = 0; i < channel_count; i++) { + //int16_t val = channels[i]; + //warnx("channel %u: %d 0x%03X", i, static_cast(val), static_cast(val)); + } + } + } + + ut_test(ret == EOF); + + return true; +} + + + +ut_declare_test_c(rc_tests_main, RCTest) + diff --git a/unittests/testdata/dsm_x_data.txt b/src/lib/rc/rc_tests/test_data/dsm_x_data.txt similarity index 100% rename from unittests/testdata/dsm_x_data.txt rename to src/lib/rc/rc_tests/test_data/dsm_x_data.txt diff --git a/unittests/testdata/sbus2_r7008SB.txt b/src/lib/rc/rc_tests/test_data/sbus2_r7008SB.txt similarity index 100% rename from unittests/testdata/sbus2_r7008SB.txt rename to src/lib/rc/rc_tests/test_data/sbus2_r7008SB.txt diff --git a/unittests/testdata/st24_data.txt b/src/lib/rc/rc_tests/test_data/st24_data.txt similarity index 100% rename from unittests/testdata/st24_data.txt rename to src/lib/rc/rc_tests/test_data/st24_data.txt diff --git a/unittests/testdata/sumd_data.txt b/src/lib/rc/rc_tests/test_data/sumd_data.txt similarity index 100% rename from unittests/testdata/sumd_data.txt rename to src/lib/rc/rc_tests/test_data/sumd_data.txt diff --git a/src/lib/runway_takeoff/runway_takeoff_params.c b/src/lib/runway_takeoff/runway_takeoff_params.c index aed95e99eb..7b91c8865b 100644 --- a/src/lib/runway_takeoff/runway_takeoff_params.c +++ b/src/lib/runway_takeoff/runway_takeoff_params.c @@ -40,9 +40,7 @@ */ /** - * Enable or disable runway takeoff with landing gear - * - * 0: disabled, 1: enabled + * Runway takeoff with landing gear * * @boolean * @group Runway Takeoff @@ -71,6 +69,8 @@ PARAM_DEFINE_INT32(RWTO_HDG, 0); * @unit m * @min 0.0 * @max 100.0 + * @decimal 1 + * @increment 1 * @group Runway Takeoff */ PARAM_DEFINE_FLOAT(RWTO_NAV_ALT, 5.0); @@ -79,8 +79,11 @@ PARAM_DEFINE_FLOAT(RWTO_NAV_ALT, 5.0); * Max throttle during runway takeoff. * (Can be used to test taxi on runway) * + * @unit norm * @min 0.0 * @max 1.0 + * @decimal 2 + * @increment 0.01 * @group Runway Takeoff */ PARAM_DEFINE_FLOAT(RWTO_MAX_THR, 1.0); @@ -94,6 +97,8 @@ PARAM_DEFINE_FLOAT(RWTO_MAX_THR, 1.0); * @unit deg * @min 0.0 * @max 20.0 + * @decimal 1 + * @increment 0.5 * @group Runway Takeoff */ PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0); @@ -106,6 +111,8 @@ PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0); * @unit deg * @min 0.0 * @max 60.0 + * @decimal 1 + * @increment 0.5 * @group Runway Takeoff */ PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0); @@ -118,6 +125,8 @@ PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0); * @unit deg * @min 0.0 * @max 60.0 + * @decimal 1 + * @increment 0.5 * @group Runway Takeoff */ PARAM_DEFINE_FLOAT(RWTO_MAX_ROLL, 25.0); @@ -127,8 +136,11 @@ PARAM_DEFINE_FLOAT(RWTO_MAX_ROLL, 25.0); * Pitch up will be commanded when the following airspeed is reached: * FW_AIRSPD_MIN * RWTO_AIRSPD_SCL * + * @unit norm * @min 0.0 * @max 2.0 + * @decimal 2 + * @increment 0.01 * @group Runway Takeoff */ PARAM_DEFINE_FLOAT(RWTO_AIRSPD_SCL, 1.3); diff --git a/src/lib/terrain_estimation/terrain_estimator.cpp b/src/lib/terrain_estimation/terrain_estimator.cpp index 64a747f532..1b2f075a5a 100644 --- a/src/lib/terrain_estimation/terrain_estimator.cpp +++ b/src/lib/terrain_estimation/terrain_estimator.cpp @@ -67,7 +67,7 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu { if (attitude->R_valid) { matrix::Matrix R_att(attitude->R); - matrix::Vector a(&sensor->accelerometer_m_s2[0]); + matrix::Vector a(sensor->accelerometer_m_s2); matrix::Vector u; u = R_att * a; _u_z = u(2) + 9.81f; // compensate for gravity @@ -153,7 +153,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl _distance_last = distance->current_distance; } - if (gps->timestamp_position > _time_last_gps && gps->fix_type >= 3) { + if (gps->timestamp > _time_last_gps && gps->fix_type >= 3) { matrix::Matrix C; C(0, 1) = 1; @@ -172,7 +172,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl _x += K * r; _P -= K * C * _P; - _time_last_gps = gps->timestamp_position; + _time_last_gps = gps->timestamp; } // reinitialise filter if we find bad data diff --git a/src/lib/version/version.h b/src/lib/version/version.h index e20bbc5c3a..b524cb2878 100644 --- a/src/lib/version/version.h +++ b/src/lib/version/version.h @@ -43,44 +43,28 @@ #ifndef VERSION_H_ #define VERSION_H_ -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 -#define HW_ARCH "PX4FMU_V1" +/* The preferred method for publishing a board name up is to + * provide board_name() + * + */ +__BEGIN_DECLS + +__EXPORT const char *board_name(void); + +__END_DECLS + +#if defined(CONFIG_ARCH_BOARD_SITL) +# define HW_ARCH "SITL" +#elif defined(CONFIG_ARCH_BOARD_EAGLE) +# define HW_ARCH "EAGLE" +#elif defined(CONFIG_ARCH_BOARD_EXCELSIOR) +# define HW_ARCH "EXCELSIOR" +#elif defined(CONFIG_ARCH_BOARD_RPI) || defined(CONFIG_ARCH_BOARD_NAVIO2) +# define HW_ARCH "RPI" +#elif defined(CONFIG_ARCH_BOARD_BEBOP) +# define HW_ARCH "BEBOP" +#else +#define HW_ARCH (board_name()) #endif -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 -#define HW_ARCH "PX4FMU_V2" -#endif - -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V4 -#define HW_ARCH "PX4FMU_V4" -#endif - -#ifdef CONFIG_ARCH_BOARD_AEROCORE -#define HW_ARCH "AEROCORE" -#endif - -#ifdef CONFIG_ARCH_BOARD_MINDPX_V2 -#define HW_ARCH "MINDPX_V2" -#endif - -#ifdef CONFIG_ARCH_BOARD_PX4_STM32F4DISCOVERY -#define HW_ARCH "PX4_STM32F4DISCOVERY" -#endif - -#ifdef CONFIG_ARCH_BOARD_SITL -#define HW_ARCH "LINUXTEST" -#endif - -#ifdef CONFIG_ARCH_BOARD_EAGLE -#define HW_ARCH "LINUXTEST" -#endif - -#ifdef CONFIG_ARCH_BOARD_EXCELSIOR -#define HW_ARCH "LINUXTEST" -#endif - - -#ifdef CONFIG_ARCH_BOARD_RPI2 -#define HW_ARCH "LINUXTEST" -#endif #endif /* VERSION_H_ */ diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 273d159f2e..480c4cdd21 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -175,7 +175,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) attitude_estimator_ekf_task = px4_task_spawn_cmd("attitude_estimator_ekf", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 7700, + 7000, attitude_estimator_ekf_thread_main, (argv) ? (char * const *)&argv[2] : (char * const *)NULL); return 0; @@ -381,7 +381,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) if (gps_updated) { orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps); - if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) { + if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp) < 1000000) { mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f)); /* update mag declination rotation matrix */ @@ -399,9 +399,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) // XXX disabling init for now initialized = true; - // gyro_offsets[0] += raw.gyro_rad_s[0]; - // gyro_offsets[1] += raw.gyro_rad_s[1]; - // gyro_offsets[2] += raw.gyro_rad_s[2]; + // gyro_offsets[0] += raw.gyro_rad[0]; + // gyro_offsets[1] += raw.gyro_rad[1]; + // gyro_offsets[2] += raw.gyro_rad[2]; // offset_count++; // if (hrt_absolute_time() - start_time > 3000000LL) { @@ -421,21 +421,21 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) uint8_t update_vect[3] = {0, 0, 0}; /* Fill in gyro measurements */ - if (sensor_last_timestamp[0] != raw.gyro_timestamp[0]) { + if (sensor_last_timestamp[0] != raw.timestamp) { update_vect[0] = 1; // sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); - sensor_last_timestamp[0] = raw.gyro_timestamp[0]; + sensor_last_timestamp[0] = raw.timestamp; } - z_k[0] = raw.gyro_rad_s[0] - gyro_offsets[0]; - z_k[1] = raw.gyro_rad_s[1] - gyro_offsets[1]; - z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; + z_k[0] = raw.gyro_rad[0] - gyro_offsets[0]; + z_k[1] = raw.gyro_rad[1] - gyro_offsets[1]; + z_k[2] = raw.gyro_rad[2] - gyro_offsets[2]; /* update accelerometer measurements */ - if (sensor_last_timestamp[1] != raw.accelerometer_timestamp[0]) { + if (sensor_last_timestamp[1] != raw.timestamp + raw.accelerometer_timestamp_relative) { update_vect[1] = 1; // sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); - sensor_last_timestamp[1] = raw.accelerometer_timestamp[0]; + sensor_last_timestamp[1] = raw.timestamp + raw.accelerometer_timestamp_relative; } hrt_abstime vel_t = 0; @@ -475,14 +475,14 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) z_k[5] = raw.accelerometer_m_s2[2] - acc(2); /* update magnetometer measurements */ - if (sensor_last_timestamp[2] != raw.magnetometer_timestamp[0] && + if (sensor_last_timestamp[2] != raw.timestamp + raw.magnetometer_timestamp_relative && /* check that the mag vector is > 0 */ fabsf(sqrtf(raw.magnetometer_ga[0] * raw.magnetometer_ga[0] + raw.magnetometer_ga[1] * raw.magnetometer_ga[1] + raw.magnetometer_ga[2] * raw.magnetometer_ga[2])) > 0.1f) { update_vect[2] = 1; // sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); - sensor_last_timestamp[2] = raw.magnetometer_timestamp[0]; + sensor_last_timestamp[2] = raw.timestamp + raw.magnetometer_timestamp_relative; } bool vision_updated = false; @@ -499,7 +499,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(att_pos_mocap), mocap_sub, &mocap); } - if (mocap.timestamp_boot > 0 && (hrt_elapsed_time(&mocap.timestamp_boot) < 500000)) { + if (mocap.timestamp > 0 && (hrt_elapsed_time(&mocap.timestamp) < 500000)) { math::Quaternion q(mocap.q); math::Matrix<3, 3> Rmoc = q.to_dcm(); @@ -511,7 +511,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) z_k[6] = vn(0); z_k[7] = vn(1); z_k[8] = vn(2); - }else if (vision.timestamp_boot > 0 && (hrt_elapsed_time(&vision.timestamp_boot) < 500000)) { + } else if (vision.timestamp > 0 && (hrt_elapsed_time(&vision.timestamp) < 500000)) { math::Quaternion q(vision.q); math::Matrix<3, 3> Rvis = q.to_dcm(); @@ -537,7 +537,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) parameters_update(&ekf_param_handles, &ekf_params); /* update mag declination rotation matrix */ - if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) { + if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp) < 1000000) { mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f)); } diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 92156c93e8..c10f871022 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -125,7 +125,6 @@ PARAM_DEFINE_FLOAT(ATT_J33, 0.0037); * If set to != 0 the moment of inertia will be used in the estimator * * @group Attitude EKF estimator - * @min 0 - * @max 1 + * @boolean */ PARAM_DEFINE_INT32(ATT_J_EN, 0); diff --git a/src/modules/attitude_estimator_q/CMakeLists.txt b/src/modules/attitude_estimator_q/CMakeLists.txt index 83f17d3e81..115d0820ba 100644 --- a/src/modules/attitude_estimator_q/CMakeLists.txt +++ b/src/modules/attitude_estimator_q/CMakeLists.txt @@ -34,7 +34,8 @@ set(MODULE_CFLAGS) px4_add_module( MODULE modules__attitude_estimator_q MAIN attitude_estimator_q - COMPILE_FLAGS ${MODULE_CFLAGS} + COMPILE_FLAGS + -Os STACK_MAIN 1200 STACK_MAX 1600 SRCS diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index af2ad11dfe..281551832a 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -67,7 +67,6 @@ #include #include #include -#include #include #include @@ -139,8 +138,8 @@ private: param_t mag_decl_auto; param_t acc_comp; param_t bias_max; - param_t vibe_thresh; param_t ext_hdg_mode; + param_t airspeed_mode; } _params_handles; /**< handles for interesting parameters */ float _w_accel = 0.0f; @@ -151,9 +150,8 @@ private: bool _mag_decl_auto = false; bool _acc_comp = false; float _bias_max = 0.0f; - float _vibration_warning_threshold = 2.0f; - hrt_abstime _vibration_warning_timestamp = 0; int _ext_hdg_mode = 0; + int _airspeed_mode = 0; Vector<3> _gyro; Vector<3> _accel; @@ -175,21 +173,18 @@ private: Vector<3> _vel_prev; Vector<3> _pos_acc; - DataValidatorGroup _voter_gyro; - DataValidatorGroup _voter_accel; - DataValidatorGroup _voter_mag; - - /* Low pass filter for attitude rates */ - math::LowPassFilter2p _lp_roll_rate; - math::LowPassFilter2p _lp_pitch_rate; - math::LowPassFilter2p _lp_yaw_rate; + /* Low pass filter for accel/gyro */ + math::LowPassFilter2p _lp_accel_x; + math::LowPassFilter2p _lp_accel_y; + math::LowPassFilter2p _lp_accel_z; + math::LowPassFilter2p _lp_gyro_x; + math::LowPassFilter2p _lp_gyro_y; + math::LowPassFilter2p _lp_gyro_z; hrt_abstime _vel_prev_t = 0; bool _inited = false; bool _data_good = false; - bool _failsafe = false; - bool _vibration_warning = false; bool _ext_hdg_good = false; orb_advert_t _mavlink_log_pub = nullptr; @@ -213,15 +208,13 @@ private: AttitudeEstimatorQ::AttitudeEstimatorQ() : _vel_prev(0, 0, 0), _pos_acc(0, 0, 0), - _voter_gyro(3), - _voter_accel(3), - _voter_mag(3), - _lp_roll_rate(250.0f, 30.0f), - _lp_pitch_rate(250.0f, 30.0f), - _lp_yaw_rate(250.0f, 20.0f) + _lp_accel_x(250.0f, 30.0f), + _lp_accel_y(250.0f, 30.0f), + _lp_accel_z(250.0f, 30.0f), + _lp_gyro_x(250.0f, 30.0f), + _lp_gyro_y(250.0f, 30.0f), + _lp_gyro_z(250.0f, 30.0f) { - _voter_mag.set_timeout(200000); - _params_handles.w_acc = param_find("ATT_W_ACC"); _params_handles.w_mag = param_find("ATT_W_MAG"); _params_handles.w_ext_hdg = param_find("ATT_W_EXT_HDG"); @@ -230,8 +223,8 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() : _params_handles.mag_decl_auto = param_find("ATT_MAG_DECL_A"); _params_handles.acc_comp = param_find("ATT_ACC_COMP"); _params_handles.bias_max = param_find("ATT_BIAS_MAX"); - _params_handles.vibe_thresh = param_find("ATT_VIBE_THRESH"); _params_handles.ext_hdg_mode = param_find("ATT_EXT_HDG_M"); + _params_handles.airspeed_mode = param_find("FW_ARSP_MODE"); } /** @@ -269,7 +262,7 @@ int AttitudeEstimatorQ::start() _control_task = px4_task_spawn_cmd("attitude_estimator_q", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 2500, + 2000, (px4_main_t)&AttitudeEstimatorQ::task_main_trampoline, nullptr); @@ -283,12 +276,6 @@ int AttitudeEstimatorQ::start() void AttitudeEstimatorQ::print() { - warnx("gyro status:"); - _voter_gyro.print(); - warnx("accel status:"); - _voter_accel.print(); - warnx("mag status:"); - _voter_mag.print(); } void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[]) @@ -343,132 +330,40 @@ void AttitudeEstimatorQ::task_main() // Update sensors sensor_combined_s sensors; - int best_gyro = 0; - int best_accel = 0; - int best_mag = 0; - if (!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)) { // Feed validator with recent sensor data - for (unsigned i = 0; i < (sizeof(sensors.gyro_timestamp) / sizeof(sensors.gyro_timestamp[0])); i++) { + if (sensors.timestamp > 0) { + // Filter gyro signal since it is not fildered in the drivers. + _gyro(0) = _lp_gyro_x.apply(sensors.gyro_rad[0]); + _gyro(1) = _lp_gyro_y.apply(sensors.gyro_rad[1]); + _gyro(2) = _lp_gyro_z.apply(sensors.gyro_rad[2]); + } - /* ignore empty fields */ - if (sensors.gyro_timestamp[i] > 0) { + if (sensors.accelerometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) { + // Filter accel signal since it is not fildered in the drivers. + _accel(0) = _lp_accel_x.apply(sensors.accelerometer_m_s2[0]); + _accel(1) = _lp_accel_y.apply(sensors.accelerometer_m_s2[1]); + _accel(2) = _lp_accel_z.apply(sensors.accelerometer_m_s2[2]); - float gyro[3]; - - for (unsigned j = 0; j < 3; j++) { - if (sensors.gyro_integral_dt[i] > 0) { - gyro[j] = (double)sensors.gyro_integral_rad[i * 3 + j] / (sensors.gyro_integral_dt[i] / 1e6); - - } else { - /* fall back to angular rate */ - gyro[j] = sensors.gyro_rad_s[i * 3 + j]; - } - } - - _voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]); - } - - /* ignore empty fields */ - if (sensors.accelerometer_timestamp[i] > 0) { - _voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3], - sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]); - } - - /* ignore empty fields */ - if (sensors.magnetometer_timestamp[i] > 0) { - _voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3], - sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]); + if (_accel.length() < 0.01f) { + PX4_DEBUG("WARNING: degenerate accel!"); + continue; } } - // Get best measurement values - hrt_abstime curr_time = hrt_absolute_time(); - _gyro.set(_voter_gyro.get_best(curr_time, &best_gyro)); - _accel.set(_voter_accel.get_best(curr_time, &best_accel)); - _mag.set(_voter_mag.get_best(curr_time, &best_mag)); + if (sensors.magnetometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) { + _mag(0) = sensors.magnetometer_ga[0]; + _mag(1) = sensors.magnetometer_ga[1]; + _mag(2) = sensors.magnetometer_ga[2]; - if (_accel.length() < 0.01f) { - warnx("WARNING: degenerate accel!"); - continue; - } - - if (_mag.length() < 0.01f) { - warnx("WARNING: degenerate mag!"); - continue; + if (_mag.length() < 0.01f) { + PX4_DEBUG("WARNING: degenerate mag!"); + continue; + } } _data_good = true; - - if (!_failsafe) { - uint32_t flags = DataValidator::ERROR_FLAG_NO_ERROR; - -#ifdef __PX4_POSIX - perf_end(_perf_accel); - perf_end(_perf_mpu); - perf_end(_perf_mag); -#endif - - if (_voter_gyro.failover_count() > 0) { - _failsafe = true; - flags = _voter_gyro.failover_state(); - mavlink_and_console_log_emergency(&_mavlink_log_pub, "Gyro #%i failure :%s%s%s%s%s!", - _voter_gyro.failover_index(), - ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""), - ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""), - ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""), - ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""), - ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : "")); - } - - if (_voter_accel.failover_count() > 0) { - _failsafe = true; - flags = _voter_accel.failover_state(); - mavlink_and_console_log_emergency(&_mavlink_log_pub, "Accel #%i failure :%s%s%s%s%s!", - _voter_accel.failover_index(), - ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""), - ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""), - ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""), - ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""), - ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : "")); - } - - if (_voter_mag.failover_count() > 0) { - _failsafe = true; - flags = _voter_mag.failover_state(); - mavlink_and_console_log_emergency(&_mavlink_log_pub, "Mag #%i failure :%s%s%s%s%s!", - _voter_mag.failover_index(), - ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""), - ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""), - ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""), - ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""), - ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : "")); - } - - if (_failsafe) { - mavlink_and_console_log_emergency(&_mavlink_log_pub, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY"); - } - } - - if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold || - _voter_accel.get_vibration_factor(curr_time) > _vibration_warning_threshold || - _voter_mag.get_vibration_factor(curr_time) > _vibration_warning_threshold)) { - - if (_vibration_warning_timestamp == 0) { - _vibration_warning_timestamp = curr_time; - - } else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000000) { - _vibration_warning = true; - // mavlink_and_console_log_critical(&_mavlink_log_pub, "HIGH VIBRATION! g: %d a: %d m: %d", - // (int)(100 * _voter_gyro.get_vibration_factor(curr_time)), - // (int)(100 * _voter_accel.get_vibration_factor(curr_time)), - // (int)(100 * _voter_mag.get_vibration_factor(curr_time))); - } - - } else { - _vibration_warning_timestamp = 0; - } } // Update vision and motion capture heading @@ -514,10 +409,10 @@ void AttitudeEstimatorQ::task_main() // Check for timeouts on data if (_ext_hdg_mode == 1) { - _ext_hdg_good = _vision.timestamp_boot > 0 && (hrt_elapsed_time(&_vision.timestamp_boot) < 500000); + _ext_hdg_good = _vision.timestamp > 0 && (hrt_elapsed_time(&_vision.timestamp) < 500000); } else if (_ext_hdg_mode == 2) { - _ext_hdg_good = _mocap.timestamp_boot > 0 && (hrt_elapsed_time(&_mocap.timestamp_boot) < 500000); + _ext_hdg_good = _mocap.timestamp > 0 && (hrt_elapsed_time(&_mocap.timestamp) < 500000); } bool gpos_updated; @@ -596,10 +491,6 @@ void AttitudeEstimatorQ::task_main() memcpy(&att.q[0], _q.data, sizeof(att.q)); att.q_valid = true; - att.rate_vibration = _voter_gyro.get_vibration_factor(hrt_absolute_time()); - att.accel_vibration = _voter_accel.get_vibration_factor(hrt_absolute_time()); - att.mag_vibration = _voter_mag.get_vibration_factor(hrt_absolute_time()); - /* the instance count is not used here */ int att_inst; orb_publish_auto(ORB_ID(vehicle_attitude), &_att_pub, &att, &att_inst, ORB_PRIO_HIGH); @@ -620,20 +511,33 @@ void AttitudeEstimatorQ::task_main() ctrl_state.z_acc = _accel(2); /* attitude rates for control state */ - ctrl_state.roll_rate = _lp_roll_rate.apply(_rates(0)); + ctrl_state.roll_rate = _rates(0); - ctrl_state.pitch_rate = _lp_pitch_rate.apply(_rates(1)); + ctrl_state.pitch_rate = _rates(1); - ctrl_state.yaw_rate = _lp_yaw_rate.apply(_rates(2)); + ctrl_state.yaw_rate = _rates(2); - /* Airspeed - take airspeed measurement directly here as no wind is estimated */ - if (PX4_ISFINITE(_airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - _airspeed.timestamp < 1e6 - && _airspeed.timestamp > 0) { - ctrl_state.airspeed = _airspeed.indicated_airspeed_m_s; - ctrl_state.airspeed_valid = true; + ctrl_state.airspeed_valid = false; - } else { - ctrl_state.airspeed_valid = false; + if (_airspeed_mode == control_state_s::AIRSPD_MODE_MEAS) { + // use measured airspeed + if (PX4_ISFINITE(_airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - _airspeed.timestamp < 1e6 + && _airspeed.timestamp > 0) { + ctrl_state.airspeed = _airspeed.indicated_airspeed_m_s; + ctrl_state.airspeed_valid = true; + } + } + + else if (_airspeed_mode == control_state_s::AIRSPD_MODE_EST) { + // use estimated body velocity as airspeed estimate + if (hrt_absolute_time() - _gpos.timestamp < 1e6) { + ctrl_state.airspeed = sqrtf(_gpos.vel_n * _gpos.vel_n + _gpos.vel_e * _gpos.vel_e + _gpos.vel_d * _gpos.vel_d); + ctrl_state.airspeed_valid = true; + } + + } else if (_airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED) { + // do nothing, airspeed has been declared as non-valid above, controllers + // will handle this assuming always trim airspeed } /* the instance count is not used here */ @@ -643,12 +547,9 @@ void AttitudeEstimatorQ::task_main() } { - struct estimator_status_s est = {}; + //struct estimator_status_s est = {}; - est.timestamp = sensors.timestamp; - est.vibe[0] = _voter_accel.get_vibration_offset(est.timestamp, 0); - est.vibe[1] = _voter_accel.get_vibration_offset(est.timestamp, 1); - est.vibe[2] = _voter_accel.get_vibration_offset(est.timestamp, 2); + //est.timestamp = sensors.timestamp; /* the instance count is not used here */ //int est_inst; @@ -658,6 +559,19 @@ void AttitudeEstimatorQ::task_main() // orb_publish_auto(ORB_ID(estimator_status), &_est_state_pub, &est, &est_inst, ORB_PRIO_HIGH); } } + +#ifdef __PX4_POSIX + perf_end(_perf_accel); + perf_end(_perf_mpu); + perf_end(_perf_mag); +#endif + + orb_unsubscribe(_sensors_sub); + orb_unsubscribe(_vision_sub); + orb_unsubscribe(_mocap_sub); + orb_unsubscribe(_airspeed_sub); + orb_unsubscribe(_params_sub); + orb_unsubscribe(_global_pos_sub); } void AttitudeEstimatorQ::update_parameters(bool force) @@ -686,8 +600,8 @@ void AttitudeEstimatorQ::update_parameters(bool force) param_get(_params_handles.acc_comp, &acc_comp_int); _acc_comp = acc_comp_int != 0; param_get(_params_handles.bias_max, &_bias_max); - param_get(_params_handles.vibe_thresh, &_vibration_warning_threshold); param_get(_params_handles.ext_hdg_mode, &_ext_hdg_mode); + param_get(_params_handles.airspeed_mode, &_airspeed_mode); } } @@ -780,6 +694,7 @@ bool AttitudeEstimatorQ::update(float dt) _q.normalize(); + // Accelerometer correction // Project 'k' unit vector of earth frame to body frame // Vector<3> k = _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, 1.0f)); @@ -793,7 +708,9 @@ bool AttitudeEstimatorQ::update(float dt) corr += (k % (_accel - _pos_acc).normalized()) * _w_accel; // Gyro bias estimation - _gyro_bias += corr * (_w_gyro_bias * dt); + if (_gyro.length() < 1.0f) { + _gyro_bias += corr * (_w_gyro_bias * dt); + } for (int i = 0; i < 3; i++) { _gyro_bias(i) = math::constrain(_gyro_bias(i), -_bias_max, _bias_max); diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c index e2babfee43..bb7f57ced0 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c @@ -94,7 +94,7 @@ PARAM_DEFINE_FLOAT(ATT_W_GYRO_BIAS, 0.1f); PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f); /** - * Enable automatic GPS based declination compensation + * Automatic GPS based declination compensation * * @group Attitude Q estimator * @boolean @@ -116,7 +116,7 @@ PARAM_DEFINE_INT32(ATT_MAG_DECL_A, 1); PARAM_DEFINE_INT32(ATT_EXT_HDG_M, 0); /** - * Enable acceleration compensation based on GPS + * Acceleration compensation based on GPS * velocity. * * @group Attitude Q estimator diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 39284f42f8..0c17fb52be 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -57,10 +57,6 @@ #include #include #include -#include -#include -#include -#include #include #include #include diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index be0d32dec7..8972791a41 100644 --- a/src/modules/commander/CMakeLists.txt +++ b/src/modules/commander/CMakeLists.txt @@ -36,8 +36,6 @@ px4_add_module( STACK_MAIN 4096 STACK_MAX 2450 COMPILE_FLAGS - -Wno-attributes - -Wno-packed -Os SRCS commander.cpp diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 5fdd5b7f4c..a11432c994 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -366,7 +366,7 @@ static bool gnssCheck(orb_advert_t *mavlink_log_pub, bool report_fail) else { struct vehicle_gps_position_s gps; if ( (OK != orb_copy(ORB_ID(vehicle_gps_position), gpsSub, &gps)) || - (hrt_elapsed_time(&gps.timestamp_position) > 1000000)) { + (hrt_elapsed_time(&gps.timestamp) > 1000000)) { success = false; } } @@ -389,7 +389,10 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkMag, bool checkAcc, #ifdef __PX4_QURT // WARNING: Preflight checks are important and should be added back when // all the sensors are supported - PX4_WARN("WARNING: Preflight checks PASS always."); + PX4_WARN("Preflight checks always pass on Snapdragon."); + return true; +#elif defined(__LINUX) + PX4_WARN("Preflight checks always pass on Linux (RPI)."); return true; #endif diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 4ce21a6dbd..72e82cc1e2 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -175,7 +175,7 @@ typedef struct { int do_accel_calibration(orb_advert_t *mavlink_log_pub) { -#ifndef __PX4_QURT +#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP) int fd; #endif @@ -195,7 +195,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) /* reset all sensors */ for (unsigned s = 0; s < max_accel_sens; s++) { -#ifndef __PX4_QURT +#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP) sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s); /* reset all offsets to zero and all scales to one */ fd = px4_open(str, 0); @@ -329,7 +329,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) return ERROR; } -#ifndef __PX4_QURT +#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP) sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, i); fd = px4_open(str, 0); @@ -419,7 +419,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub break; } -#ifdef __PX4_QURT +#if defined(__PX4_QURT) || defined(__PX4_POSIX_EAGLE) || defined(__PX4_POSIX_RPI) || defined(__PX4_POSIX_BEBOP) // For QURT respectively the driver framework, we need to get the device ID by copying one report. struct accel_report accel_report; orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &accel_report); diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 0556e04b0a..94ca6e33dd 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -50,7 +50,6 @@ #include #include #include -#include #include #include #include diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index b351ac1bd3..b1ee6f5aea 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -273,18 +273,7 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub, for (unsigned i = 0; i < ndim; i++) { - float di = 0.0f; - switch (i) { - case 0: - di = sensor.accelerometer_m_s2[0]; - break; - case 1: - di = sensor.accelerometer_m_s2[1]; - break; - case 2: - di = sensor.accelerometer_m_s2[2]; - break; - } + float di = sensor.accelerometer_m_s2[i]; float d = di - accel_ema[i]; accel_ema[i] += d * w; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f073142890..6e2eafa0a3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -44,88 +44,67 @@ * @author Sander Smeets */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -//#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "px4_custom_mode.h" -#include "commander_helper.h" -#include "state_machine_helper.h" -#include "calibration_routines.h" +/* commander module headers */ #include "accelerometer_calibration.h" +#include "airspeed_calibration.h" +#include "baro_calibration.h" +#include "calibration_routines.h" +#include "commander_helper.h" +#include "esc_calibration.h" #include "gyro_calibration.h" #include "mag_calibration.h" -#include "baro_calibration.h" -#include "rc_calibration.h" -#include "airspeed_calibration.h" -#include "esc_calibration.h" #include "PreflightCheck.h" +#include "px4_custom_mode.h" +#include "rc_calibration.h" +#include "state_machine_helper.h" + +/* PX4 headers */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -133,19 +112,28 @@ #endif static const int ERROR = -1; -extern struct system_load_s system_load; +typedef enum VEHICLE_MODE_FLAG +{ + VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED=1, /* 0b00000001 Reserved for future use. | */ + VEHICLE_MODE_FLAG_TEST_ENABLED=2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */ + VEHICLE_MODE_FLAG_AUTO_ENABLED=4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */ + VEHICLE_MODE_FLAG_GUIDED_ENABLED=8, /* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */ + VEHICLE_MODE_FLAG_STABILIZE_ENABLED=16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */ + VEHICLE_MODE_FLAG_HIL_ENABLED=32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */ + VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED=64, /* 0b01000000 remote control input is enabled. | */ + VEHICLE_MODE_FLAG_SAFETY_ARMED=128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. | */ + VEHICLE_MODE_FLAG_ENUM_END=129, /* | */ +} VEHICLE_MODE_FLAG; static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage signal to noise ratio allowed for GPS reception */ -/* Decouple update interval and hysteris counters, all depends on intervals */ +/* Decouple update interval and hysteresis counters, all depends on intervals */ #define COMMANDER_MONITORING_INTERVAL 10000 #define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f)) #define MAVLINK_OPEN_INTERVAL 50000 #define STICK_ON_OFF_LIMIT 0.9f -#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 -#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) #define POSITION_TIMEOUT (1 * 1000 * 1000) /**< consider the local or global position estimate invalid after 1000ms */ #define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */ @@ -162,19 +150,6 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage #define HIL_ID_MIN 1000 #define HIL_ID_MAX 1999 -enum MAV_MODE_FLAG { - MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */ - MAV_MODE_FLAG_TEST_ENABLED = 2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */ - MAV_MODE_FLAG_AUTO_ENABLED = 4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */ - MAV_MODE_FLAG_GUIDED_ENABLED = 8, /* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */ - MAV_MODE_FLAG_STABILIZE_ENABLED = 16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */ - MAV_MODE_FLAG_HIL_ENABLED = 32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */ - MAV_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, /* 0b01000000 remote control input is enabled. | */ - MAV_MODE_FLAG_SAFETY_ARMED = 128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. | */ - MAV_MODE_FLAG_ENUM_END = 129, /* | */ -}; - - /* Mavlink log uORB handle */ static orb_advert_t mavlink_log_pub = 0; @@ -193,7 +168,8 @@ static hrt_abstime commander_boot_timestamp = 0; static unsigned int leds_counter; /* To remember when last notification was sent */ static uint64_t last_print_mode_reject_time = 0; -static uint64_t _inair_last_time = 0; + +static systemlib::Hysteresis auto_disarm_hysteresis(false); static float eph_threshold = 5.0f; static float epv_threshold = 10.0f; @@ -214,6 +190,7 @@ struct manual_control_setpoint_s sp_man = {}; ///< the current manual control s static manual_control_setpoint_s _last_sp_man = {}; ///< the manual control setpoint valid at the last mode switch static struct vtol_vehicle_status_s vtol_status = {}; +static struct cpuload_s cpuload = {}; static uint8_t main_state_prev = 0; @@ -225,6 +202,8 @@ static uint64_t rc_signal_lost_timestamp; // Time at which the RC reception was static float avionics_power_rail_voltage; // voltage of the avionics power rail +static bool can_arm_without_gps = false; + /** * The daemon app only briefly exists to start @@ -257,7 +236,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe int commander_thread_main(int argc, char *argv[]); void control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed, - battery_status_s *battery); + battery_status_s *battery_local, const cpuload_s *cpuload_local); void get_circuit_breaker_params(); @@ -275,9 +254,6 @@ void print_reject_arm(const char *msg); void print_status(); -transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, - struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos); - transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub, const char *armedBy); /** @@ -299,9 +275,7 @@ void answer_command(struct vehicle_command_s &cmd, unsigned result, /** * check whether autostart ID is in the reserved range for HIL setups */ -bool is_hil_setup(int id); - -bool is_hil_setup(int id) { +static bool is_hil_setup(int id) { return (id >= HIL_ID_MIN) && (id <= HIL_ID_MAX); } @@ -325,7 +299,7 @@ int commander_main(int argc, char *argv[]) daemon_task = px4_task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT + 40, - 3600, + 3000, commander_thread_main, (char * const *)&argv[0]); @@ -405,9 +379,9 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "check")) { int checkres = 0; - checkres = preflight_check(&status, &mavlink_log_pub, false, true, &status_flags, &battery); + checkres = preflight_check(&status, &mavlink_log_pub, false, true, &status_flags, &battery, false); warnx("Preflight check: %s", (checkres == 0) ? "OK" : "FAILED"); - checkres = preflight_check(&status, &mavlink_log_pub, true, true, &status_flags, &battery); + checkres = preflight_check(&status, &mavlink_log_pub, true, true, &status_flags, &battery, true); warnx("Prearm check: %s", (checkres == 0) ? "OK" : "FAILED"); return 0; } @@ -438,15 +412,17 @@ int commander_main(int argc, char *argv[]) cmd.target_component = status.component_id; cmd.command = vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF; - // cmd.param1 = 0.25f; /* minimum pitch */ - // /* param 2-3 unused */ - // cmd.param4 = home_position.yaw; - // cmd.param5 = home_position.lat; - // cmd.param6 = home_position.lon; - // cmd.param7 = home_position.alt; + cmd.param1 = NAN; /* minimum pitch */ + /* param 2-3 unused */ + cmd.param2 = NAN; + cmd.param3 = NAN; + cmd.param4 = NAN; + cmd.param5 = NAN; + cmd.param6 = NAN; + cmd.param7 = NAN; - // XXX inspect use of publication handle - (void)orb_advertise(ORB_ID(vehicle_command), &cmd); + orb_advert_t h = orb_advertise(ORB_ID(vehicle_command), &cmd); + (void)orb_unadvertise(h); } else { warnx("arming failed"); @@ -466,15 +442,39 @@ int commander_main(int argc, char *argv[]) cmd.target_component = status.component_id; cmd.command = vehicle_command_s::VEHICLE_CMD_NAV_LAND; - // cmd.param1 = 0.25f; /* minimum pitch */ - // /* param 2-3 unused */ - // cmd.param4 = home_position.yaw; - // cmd.param5 = home_position.lat; - // cmd.param6 = home_position.lon; - // cmd.param7 = home_position.alt; + /* param 2-3 unused */ + cmd.param2 = NAN; + cmd.param3 = NAN; + cmd.param4 = NAN; + cmd.param5 = NAN; + cmd.param6 = NAN; + cmd.param7 = NAN; - // XXX inspect use of publication handle - (void)orb_advertise(ORB_ID(vehicle_command), &cmd); + orb_advert_t h = orb_advertise(ORB_ID(vehicle_command), &cmd); + (void)orb_unadvertise(h); + + return 0; + } + + if (!strcmp(argv[1], "transition")) { + + vehicle_command_s cmd = {}; + cmd.target_system = status.system_id; + cmd.target_component = status.component_id; + + cmd.command = vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION; + /* transition to the other mode */ + cmd.param1 = (status.is_rotary_wing) ? vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW : vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; + /* param 2-3 unused */ + cmd.param2 = NAN; + cmd.param3 = NAN; + cmd.param4 = NAN; + cmd.param5 = NAN; + cmd.param6 = NAN; + cmd.param7 = NAN; + + orb_advert_t h = orb_advertise(ORB_ID(vehicle_command), &cmd); + (void)orb_unadvertise(h); return 0; } @@ -510,9 +510,7 @@ int commander_main(int argc, char *argv[]) warnx("argument %s unsupported.", argv[2]); } - if (TRANSITION_DENIED == main_state_transition(&status, new_main_state, main_state_prev, - &status_flags, &internal_state)) { - ; + if (TRANSITION_DENIED == main_state_transition(&status, new_main_state, main_state_prev, &status_flags, &internal_state)) { warnx("mode change failed"); } return 0; @@ -553,7 +551,7 @@ void usage(const char *reason) PX4_INFO("%s", reason); } - PX4_INFO("usage: commander {start|stop|status|calibrate|check|arm|disarm|takeoff|land|mode}\n"); + PX4_INFO("usage: commander {start|stop|status|calibrate|check|arm|disarm|takeoff|land|transition|mode}\n"); } void print_status() @@ -641,7 +639,8 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, co true /* fRunPreArmChecks */, mavlink_log_pub_local, &status_flags, - avionics_power_rail_voltage); + avionics_power_rail_voltage, + can_arm_without_gps); if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_log_pub_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); @@ -703,11 +702,11 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s transition_result_t main_ret = TRANSITION_NOT_CHANGED; /* set HIL state */ - hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? vehicle_status_s::HIL_STATE_ON : vehicle_status_s::HIL_STATE_OFF; + hil_state_t new_hil_state = (base_mode & VEHICLE_MODE_FLAG_HIL_ENABLED) ? vehicle_status_s::HIL_STATE_ON : vehicle_status_s::HIL_STATE_OFF; transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status_local, &mavlink_log_pub); // Transition the arming state - bool cmd_arm = base_mode & MAV_MODE_FLAG_SAFETY_ARMED; + bool cmd_arm = base_mode & VEHICLE_MODE_FLAG_SAFETY_ARMED; arming_ret = arm_disarm(cmd_arm, &mavlink_log_pub, "set mode command"); @@ -718,7 +717,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s commander_set_home_position(*home_pub, *home, *local_pos, *global_pos, *attitude); } - if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { + if (base_mode & VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED) { /* use autopilot-specific mode */ if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { /* MANUAL */ @@ -751,7 +750,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s case PX4_CUSTOM_SUB_MODE_AUTO_LAND: main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LAND, main_state_prev, &status_flags, &internal_state); break; - case PX4_CUSTOM_SUB_MODE_FOLLOW_TARGET: + case PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET: main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET, main_state_prev, &status_flags, &internal_state); break; @@ -784,16 +783,16 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } else { /* use base mode */ - if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { + if (base_mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { /* AUTO */ main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, main_state_prev, &status_flags, &internal_state); - } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { - if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { + } else if (base_mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { + if (base_mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { /* POSCTL */ main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags, &internal_state); - } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { + } else if (base_mode & VEHICLE_MODE_FLAG_STABILIZE_ENABLED) { /* STABILIZED */ main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, main_state_prev, &status_flags, &internal_state); } else { @@ -812,10 +811,6 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s if (arming_ret == TRANSITION_DENIED) { mavlink_log_critical(&mavlink_log_pub, "Rejecting arming cmd"); } - - if (main_ret == TRANSITION_DENIED) { - mavlink_log_critical(&mavlink_log_pub, "Rejecting mode switch cmd"); - } } } break; @@ -834,9 +829,8 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s // Flick to inair restore first if this comes from an onboard system if (cmd->source_system == status_local->system_id && cmd->source_component == status_local->component_id) { status.arming_state = vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE; - } - else { + } else { // Refuse to arm if preflight checks have failed if ((!status_local->hil_state) != vehicle_status_s::HIL_STATE_ON && !status_flags.condition_system_sensors_initialized) { mavlink_log_critical(&mavlink_log_pub, "Arming DENIED. Preflight checks have failed."); @@ -1035,6 +1029,13 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s res = main_state_transition(status_local, main_state_pre_offboard, main_state_prev, &status_flags, &internal_state); status_flags.offboard_control_set_by_command = false; } + + if (res == TRANSITION_DENIED) { + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + + } else { + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; + } } break; @@ -1042,42 +1043,48 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s /* ok, home set, use it to take off */ if (TRANSITION_CHANGED == main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_TAKEOFF, main_state_prev, &status_flags, &internal_state)) { mavlink_and_console_log_info(&mavlink_log_pub, "Taking off"); + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; + } else { mavlink_and_console_log_critical(&mavlink_log_pub, "Takeoff denied, disarm and re-try"); + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } } break; case vehicle_command_s::VEHICLE_CMD_NAV_LAND: { - /* ok, home set, use it to take off */ if (TRANSITION_CHANGED == main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_LAND, main_state_prev, &status_flags, &internal_state)) { mavlink_and_console_log_info(&mavlink_log_pub, "Landing at current position"); + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; + } else { mavlink_and_console_log_critical(&mavlink_log_pub, "Landing denied, land manually."); + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } } break; + case vehicle_command_s::VEHICLE_CMD_CUSTOM_0: + case vehicle_command_s::VEHICLE_CMD_CUSTOM_1: + case vehicle_command_s::VEHICLE_CMD_CUSTOM_2: case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION: case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE: case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_UAVCAN: - case vehicle_command_s::VEHICLE_CMD_CUSTOM_0: - case vehicle_command_s::VEHICLE_CMD_CUSTOM_1: - case vehicle_command_s::VEHICLE_CMD_CUSTOM_2: case vehicle_command_s::VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY: case vehicle_command_s::VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY: - case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL: - case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT: - case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE: case vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL: case vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION: case vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL: + case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE: + case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL: case vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST: + case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT: case vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED: + case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR: /* ignore commands that handled in low prio loop */ break; @@ -1164,14 +1171,14 @@ int commander_thread_main(int argc, char *argv[]) #ifdef __PX4_NUTTX /* NuttX indicates 3 arguments when only 2 are present */ argc -= 1; + argv += 1; #endif if (argc > 2) { - if (!strcmp(argv[3],"-hil")) { + if (!strcmp(argv[2],"-hil")) { startup_in_hil = true; } else { - PX4_ERR("Argument %s not supported.", argv[3]); - PX4_ERR("COMMANDER NOT STARTED"); + PX4_ERR("Argument %s not supported, abort.", argv[2]); thread_should_exit = true; } } @@ -1181,6 +1188,8 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_system_id = param_find("MAV_SYS_ID"); param_t _param_component_id = param_find("MAV_COMP_ID"); param_t _param_enable_datalink_loss = param_find("NAV_DLL_ACT"); + param_t _param_offboard_loss_act = param_find("COM_OBL_ACT"); + param_t _param_offboard_loss_rc_act = param_find("COM_OBL_RC_ACT"); param_t _param_enable_rc_loss = param_find("NAV_RCL_ACT"); param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T"); param_t _param_rc_loss_timeout = param_find("COM_RC_LOSS_T"); @@ -1191,11 +1200,14 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_autostart_id = param_find("SYS_AUTOSTART"); param_t _param_autosave_params = param_find("COM_AUTOS_PAR"); param_t _param_rc_in_off = param_find("COM_RC_IN_MODE"); + param_t _param_rc_arm_hyst = param_find("COM_RC_ARM_HYST"); param_t _param_eph = param_find("COM_HOME_H_T"); param_t _param_epv = param_find("COM_HOME_V_T"); param_t _param_geofence_action = param_find("GF_ACTION"); param_t _param_disarm_land = param_find("COM_DISARM_LAND"); param_t _param_low_bat_act = param_find("COM_LOW_BAT_ACT"); + param_t _param_offboard_loss_timeout = param_find("COM_OF_LOSS_T"); + param_t _param_arm_without_gps = param_find("COM_ARM_WO_GPS"); param_t _param_fmode_1 = param_find("COM_FLTMODE1"); param_t _param_fmode_2 = param_find("COM_FLTMODE2"); @@ -1218,15 +1230,6 @@ int commander_thread_main(int argc, char *argv[]) // main_states_str[commander_state_s::MAIN_STATE_STAB] = "STAB"; // main_states_str[commander_state_s::MAIN_STATE_OFFBOARD] = "OFFBOARD"; - // const char *arming_states_str[vehicle_status_s::ARMING_STATE_MAX]; - // arming_states_str[vehicle_status_s::ARMING_STATE_INIT] = "INIT"; - // arming_states_str[vehicle_status_s::ARMING_STATE_STANDBY] = "STANDBY"; - // arming_states_str[vehicle_status_s::ARMING_STATE_ARMED] = "ARMED"; - // arming_states_str[vehicle_status_s::ARMING_STATE_ARMED_ERROR] = "ARMED_ERROR"; - // arming_states_str[vehicle_status_s::ARMING_STATE_STANDBY_ERROR] = "STANDBY_ERROR"; - // arming_states_str[vehicle_status_s::ARMING_STATE_REBOOT] = "REBOOT"; - // arming_states_str[vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE"; - // const char *nav_states_str[vehicle_status_s::NAVIGATION_STATE_MAX]; // nav_states_str[vehicle_status_s::NAVIGATION_STATE_MANUAL] = "MANUAL"; // nav_states_str[vehicle_status_s::NAVIGATION_STATE_STAB] = "STAB"; @@ -1265,12 +1268,13 @@ int commander_thread_main(int argc, char *argv[]) // We want to accept RC inputs as default status_flags.rc_input_blocked = false; status.rc_input_mode = vehicle_status_s::RC_IN_MODE_DEFAULT; - internal_state.main_state =commander_state_s::MAIN_STATE_MANUAL; + internal_state.main_state = commander_state_s::MAIN_STATE_MANUAL; + internal_state.timestamp = hrt_absolute_time(); main_state_prev = commander_state_s::MAIN_STATE_MAX; status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; status.arming_state = vehicle_status_s::ARMING_STATE_INIT; - if(startup_in_hil) { + if (startup_in_hil) { status.hil_state = vehicle_status_s::HIL_STATE_ON; } else { status.hil_state = vehicle_status_s::HIL_STATE_OFF; @@ -1285,6 +1289,7 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_lost = true; status_flags.offboard_control_signal_lost = true; status.data_link_lost = true; + status_flags.offboard_control_loss_timeout = false; status_flags.condition_system_prearm_error_reported = false; status_flags.condition_system_hotplug_timeout = false; @@ -1368,8 +1373,6 @@ int commander_thread_main(int argc, char *argv[]) bool low_battery_voltage_actions_done = false; bool critical_battery_voltage_actions_done = false; - hrt_abstime last_idle_time = 0; - bool status_changed = true; bool param_init_forced = true; @@ -1494,8 +1497,10 @@ int commander_thread_main(int argc, char *argv[]) memset(&vtol_status, 0, sizeof(vtol_status)); vtol_status.vtol_in_rw_mode = true; //default for vtol is rotary wing + int cpuload_sub = orb_subscribe(ORB_ID(cpuload)); + memset(&cpuload, 0, sizeof(cpuload)); - control_status_leds(&status, &armed, true, &battery); + control_status_leds(&status, &armed, true, &battery, &cpuload); /* now initialized */ commander_initialized = true; @@ -1515,21 +1520,29 @@ int commander_thread_main(int argc, char *argv[]) // Run preflight check int32_t rc_in_off = 0; bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT; + int32_t arm_without_gps = 0; param_get(_param_autostart_id, &autostart_id); param_get(_param_rc_in_off, &rc_in_off); + param_get(_param_arm_without_gps, &arm_without_gps); + can_arm_without_gps = (arm_without_gps == 1); status.rc_input_mode = rc_in_off; if (is_hil_setup(autostart_id)) { // HIL configuration selected: real sensors will be disabled status_flags.condition_system_sensors_initialized = false; set_tune_override(TONE_STARTUP_TUNE); //normal boot tune } else { - // sensor diagnostics done continiously, not just at boot so don't warn about any issues just yet + // sensor diagnostics done continuously, not just at boot so don't warn about any issues just yet status_flags.condition_system_sensors_initialized = Commander::preflightCheck(&mavlink_log_pub, true, true, true, true, checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), - !status_flags.circuit_breaker_engaged_gpsfailure_check, false); + !can_arm_without_gps, /*checkDynamic */ false, /* reportFailures */ false); set_tune_override(TONE_STARTUP_TUNE); //normal boot tune } + // user adjustable duration required to assert arm/disarm via throttle/rudder stick + int32_t rc_arm_hyst = 100; + param_get(_param_rc_arm_hyst, &rc_arm_hyst); + rc_arm_hyst *= COMMANDER_MONITORING_LOOPSPERMSEC; + commander_boot_timestamp = hrt_absolute_time(); transition_result_t arming_ret; @@ -1539,6 +1552,9 @@ int commander_thread_main(int argc, char *argv[]) int32_t datalink_loss_timeout = 10; float rc_loss_timeout = 0.5; int32_t datalink_regain_timeout = 0; + float offboard_loss_timeout = 0.0f; + int32_t offboard_loss_act = 0; + int32_t offboard_loss_rc_act = 0; int32_t geofence_action = 0; @@ -1624,13 +1640,23 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_rc_loss_timeout, &rc_loss_timeout); param_get(_param_rc_in_off, &rc_in_off); status.rc_input_mode = rc_in_off; + param_get(_param_rc_arm_hyst, &rc_arm_hyst); + rc_arm_hyst *= COMMANDER_MONITORING_LOOPSPERMSEC; param_get(_param_datalink_regain_timeout, &datalink_regain_timeout); param_get(_param_ef_throttle_thres, &ef_throttle_thres); param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres); param_get(_param_ef_time_thres, &ef_time_thres); param_get(_param_geofence_action, &geofence_action); param_get(_param_disarm_land, &disarm_when_landed); + auto_disarm_hysteresis.set_hysteresis_time_from(false, + (hrt_abstime)disarm_when_landed * 1000000); + param_get(_param_low_bat_act, &low_bat_action); + param_get(_param_offboard_loss_timeout, &offboard_loss_timeout); + param_get(_param_offboard_loss_act, &offboard_loss_act); + param_get(_param_offboard_loss_rc_act, &offboard_loss_rc_act); + param_get(_param_arm_without_gps, &arm_without_gps); + can_arm_without_gps = (arm_without_gps == 1); /* Autostart id */ param_get(_param_autostart_id, &autostart_id); @@ -1673,6 +1699,7 @@ int commander_thread_main(int argc, char *argv[]) offboard_control_mode.timestamp + OFFBOARD_TIMEOUT > hrt_absolute_time()) { if (status_flags.offboard_control_signal_lost) { status_flags.offboard_control_signal_lost = false; + status_flags.offboard_control_loss_timeout = false; status_changed = true; } @@ -1681,6 +1708,23 @@ int commander_thread_main(int argc, char *argv[]) status_flags.offboard_control_signal_lost = true; status_changed = true; } + + /* check timer if offboard was there but now lost */ + if (!status_flags.offboard_control_loss_timeout && offboard_control_mode.timestamp != 0) { + if (offboard_loss_timeout < FLT_EPSILON) { + /* execute loss action immediately */ + status_flags.offboard_control_loss_timeout = true; + + } else { + /* wait for timeout if set */ + status_flags.offboard_control_loss_timeout = offboard_control_mode.timestamp + + OFFBOARD_TIMEOUT + offboard_loss_timeout * 1e6f < hrt_absolute_time(); + } + + if (status_flags.offboard_control_loss_timeout) { + status_changed = true; + } + } } for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { @@ -1721,11 +1765,11 @@ int commander_thread_main(int argc, char *argv[]) if (is_hil_setup(autostart_id)) { /* HIL configuration: check only RC input */ (void)Commander::preflightCheck(&mavlink_log_pub, false, false, false, false, false, - (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), false, true); + (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), /* checkGNSS */ false, /* checkDynamic */ true, /* reportFailures */ false); } else { /* check sensors also */ (void)Commander::preflightCheck(&mavlink_log_pub, true, true, true, true, chAirspeed, - (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status_flags.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout); + (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !can_arm_without_gps, /* checkDynamic */ true, hotplug_timeout); } } @@ -1750,7 +1794,8 @@ int commander_thread_main(int argc, char *argv[]) * vertical separation from other airtraffic the operator has to know when the * barometer is inoperational. * */ - if (hrt_elapsed_time(&sensors.baro_timestamp[0]) < FAILSAFE_DEFAULT_TIMEOUT) { + hrt_abstime baro_timestamp = sensors.timestamp + sensors.baro_timestamp_relative; + if (hrt_elapsed_time(&baro_timestamp) < FAILSAFE_DEFAULT_TIMEOUT) { /* handle the case where baro was regained */ if (status_flags.barometer_failure) { status_flags.barometer_failure = false; @@ -1830,7 +1875,8 @@ int commander_thread_main(int argc, char *argv[]) true /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags, - avionics_power_rail_voltage)) { + avionics_power_rail_voltage, + can_arm_without_gps)) { mavlink_log_info(&mavlink_log_pub, "DISARMED by safety switch"); arming_state_changed = true; } @@ -1952,43 +1998,38 @@ int commander_thread_main(int argc, char *argv[]) &(status_flags.condition_local_altitude_valid), &status_changed); /* Update land detector */ - static bool check_for_disarming = false; orb_check(land_detector_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_land_detected), land_detector_sub, &land_detector); - } - if ((updated && status_flags.condition_local_altitude_valid) || check_for_disarming) { if (was_landed != land_detector.landed) { if (land_detector.landed) { - mavlink_and_console_log_info(&mavlink_log_pub, "LANDING DETECTED"); + mavlink_and_console_log_info(&mavlink_log_pub, "Landing detected"); } else { - mavlink_and_console_log_info(&mavlink_log_pub, "TAKEOFF DETECTED"); + mavlink_and_console_log_info(&mavlink_log_pub, "Takeoff detected"); } } if (was_falling != land_detector.freefall) { if (land_detector.freefall) { - mavlink_and_console_log_info(&mavlink_log_pub, "FREEFALL DETECTED"); + mavlink_and_console_log_info(&mavlink_log_pub, "Freefall detected"); } } - if (disarm_when_landed > 0) { - if (land_detector.landed) { - if (!check_for_disarming && _inair_last_time > 0) { - _inair_last_time = land_detector.timestamp; - check_for_disarming = true; - } - if (_inair_last_time > 0 && ((hrt_absolute_time() - _inair_last_time) > (hrt_abstime)disarm_when_landed * 1000 * 1000)) { - arm_disarm(false, &mavlink_log_pub, "auto disarm on land"); - _inair_last_time = 0; - check_for_disarming = false; - } - } else { - _inair_last_time = land_detector.timestamp; - } - } + was_landed = land_detector.landed; + was_falling = land_detector.freefall; + } + + // Check for auto-disarm + if (armed.armed && land_detector.landed && disarm_when_landed > 0) { + auto_disarm_hysteresis.set_state_and_update(true); + } else { + auto_disarm_hysteresis.set_state_and_update(false); + } + + if (auto_disarm_hysteresis.get_state()) { + arm_disarm(false, &mavlink_log_pub, "auto disarm on land"); } if (!rtl_on) { @@ -1997,6 +2038,12 @@ int commander_thread_main(int argc, char *argv[]) main_state_before_rtl = internal_state.main_state; } + orb_check(cpuload_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(cpuload), cpuload_sub, &cpuload); + } + /* update battery status */ orb_check(battery_sub, &updated); @@ -2095,17 +2142,6 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet); } - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - /* compute system load */ - uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; - - if (last_idle_time > 0) { - status.load = 1.0f - ((float)interval_runtime / 1e6f); //system load is time spent in non-idle - } - - last_idle_time = system_load.tasks[0].total_runtime; - } - /* If in INIT state, try to proceed to STANDBY state */ if (!status_flags.condition_calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) { arming_ret = arming_state_transition(&status, @@ -2116,7 +2152,8 @@ int commander_thread_main(int argc, char *argv[]) true /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags, - avionics_power_rail_voltage); + avionics_power_rail_voltage, + can_arm_without_gps); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; @@ -2147,7 +2184,7 @@ int commander_thread_main(int argc, char *argv[]) if (!map_projection_global_initialized() && (gps_position.eph < eph_threshold) && (gps_position.epv < epv_threshold) - && hrt_elapsed_time((hrt_abstime *)&gps_position.timestamp_position) < 1e6) { + && hrt_elapsed_time((hrt_abstime *)&gps_position.timestamp) < 1e6) { /* set reference for global coordinates <--> local coordiantes conversion and map_projection */ globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, (float)gps_position.alt * 1.0e-3f, hrt_absolute_time()); @@ -2169,12 +2206,14 @@ int commander_thread_main(int argc, char *argv[]) } } - if (gps_position.fix_type >= 3 && hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT) { + if (gps_position.fix_type >= 3 && hrt_elapsed_time(&gps_position.timestamp) < FAILSAFE_DEFAULT_TIMEOUT) { /* handle the case where gps was regained */ if (status_flags.gps_failure && !gpsIsNoisy) { status_flags.gps_failure = false; status_changed = true; - mavlink_log_critical(&mavlink_log_pub, "GPS fix regained"); + if (status_flags.condition_home_position_valid) { + mavlink_log_critical(&mavlink_log_pub, "GPS fix regained"); + } } } else if (!status_flags.gps_failure) { @@ -2288,7 +2327,8 @@ int commander_thread_main(int argc, char *argv[]) /* Check for mission flight termination */ - if (armed.armed && mission_result.flight_termination) { + if (armed.armed && mission_result.flight_termination && + !status_flags.circuit_breaker_flight_termination_disabled) { armed.force_failsafe = true; status_changed = true; static bool flight_termination_printed = false; @@ -2357,7 +2397,7 @@ int commander_thread_main(int argc, char *argv[]) land_detector.landed) && sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { - if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { + if (stick_off_counter > rc_arm_hyst) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ arming_state_t new_arming_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? vehicle_status_s::ARMING_STATE_STANDBY : vehicle_status_s::ARMING_STATE_STANDBY_ERROR); @@ -2369,7 +2409,8 @@ int commander_thread_main(int argc, char *argv[]) true /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags, - avionics_power_rail_voltage); + avionics_power_rail_voltage, + can_arm_without_gps); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; @@ -2387,7 +2428,7 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ if (sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f && status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF ) { - if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { + if (stick_on_counter > rc_arm_hyst) { /* we check outside of the transition function here because the requirement * for being in manual mode only applies to manual arming actions. @@ -2397,7 +2438,10 @@ int commander_thread_main(int argc, char *argv[]) if ((internal_state.main_state != commander_state_s::MAIN_STATE_MANUAL) && (internal_state.main_state != commander_state_s::MAIN_STATE_ACRO) && (internal_state.main_state != commander_state_s::MAIN_STATE_STAB) - && (internal_state.main_state != commander_state_s::MAIN_STATE_ALTCTL)) { + && (internal_state.main_state != commander_state_s::MAIN_STATE_ALTCTL) + && (internal_state.main_state != commander_state_s::MAIN_STATE_POSCTL) + && (internal_state.main_state != commander_state_s::MAIN_STATE_RATTITUDE) + ) { print_reject_arm("NOT ARMING: Switch to a manual mode first."); } else if (!status_flags.condition_home_position_valid && @@ -2413,13 +2457,14 @@ int commander_thread_main(int argc, char *argv[]) true /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags, - avionics_power_rail_voltage); + avionics_power_rail_voltage, + can_arm_without_gps); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; } else { usleep(100000); - print_reject_arm("NOT ARMING: Configuration error"); + print_reject_arm("NOT ARMING: Preflight checks failed"); } } stick_on_counter = 0; @@ -2616,7 +2661,8 @@ int commander_thread_main(int argc, char *argv[]) } /* Check for failure combinations which lead to flight termination */ - if (armed.armed) { + if (armed.armed && + !status_flags.circuit_breaker_flight_termination_disabled) { /* At this point the data link and the gps system have been checked * If we are not in a manual (RC stick controlled mode) * and both failed we want to terminate the flight */ @@ -2682,8 +2728,6 @@ int commander_thread_main(int argc, char *argv[]) commander_set_home_position(home_pub, _home, local_position, global_position, attitude); } - was_landed = land_detector.landed; - was_falling = land_detector.freefall; was_armed = armed.armed; /* print new state */ @@ -2700,7 +2744,9 @@ int commander_thread_main(int argc, char *argv[]) mission_result.stay_in_failsafe, &status_flags, land_detector.landed, - (rc_loss_enabled > 0)); + (rc_loss_enabled > 0), + offboard_loss_act, + offboard_loss_rc_act); if (status.failsafe != failsafe_old) { status_changed = true; @@ -2803,12 +2849,12 @@ int commander_thread_main(int argc, char *argv[]) /* blinking LED message, don't touch LEDs */ if (blink_state == 2) { /* blinking LED message completed, restore normal state */ - control_status_leds(&status, &armed, true, &battery); + control_status_leds(&status, &armed, true, &battery, &cpuload); } } else { /* normal state */ - control_status_leds(&status, &armed, status_changed, &battery); + control_status_leds(&status, &armed, status_changed, &battery, &cpuload); } status_changed = false; @@ -2860,10 +2906,11 @@ void get_circuit_breaker_params() { status_flags.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY); - status_flags.cb_usb = circuit_breaker_enabled("CBRK_USB_CHK", CBRK_USB_CHK_KEY); + status_flags.circuit_breaker_engaged_usb_check = circuit_breaker_enabled("CBRK_USB_CHK", CBRK_USB_CHK_KEY); status_flags.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY); status_flags.circuit_breaker_engaged_enginefailure_check = circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY); status_flags.circuit_breaker_engaged_gpsfailure_check = circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY); + status_flags.circuit_breaker_flight_termination_disabled = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY); } void @@ -2879,7 +2926,7 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val } void -control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed, battery_status_s *battery_status) +control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed, battery_status_s *battery_local, const cpuload_s *cpuload_local) { /* driving rgbled */ if (changed) { @@ -2912,9 +2959,9 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu /* set color */ if (status.failsafe) { rgbled_set_color(RGBLED_COLOR_PURPLE); - } else if (battery_status->warning == battery_status_s::BATTERY_WARNING_LOW) { + } else if (battery_local->warning == battery_status_s::BATTERY_WARNING_LOW) { rgbled_set_color(RGBLED_COLOR_AMBER); - } else if (battery_status->warning == battery_status_s::BATTERY_WARNING_CRITICAL) { + } else if (battery_local->warning == battery_status_s::BATTERY_WARNING_CRITICAL) { rgbled_set_color(RGBLED_COLOR_RED); } else { if (status_flags.condition_home_position_valid && status_flags.condition_global_position_valid) { @@ -2950,7 +2997,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu #endif /* give system warnings on error LED, XXX maybe add memory usage warning too */ - if (status_local->load > 0.95f) { + if (cpuload_local->load > 0.95f) { if (leds_counter % 2 == 0) { led_toggle(LED_AMBER); } @@ -3479,17 +3526,17 @@ set_control_mode() control_mode.flag_control_rattitude_enabled = false; - control_mode.flag_control_acceleration_enabled = !offboard_control_mode.ignore_acceleration_force && - !status.in_transition_mode; + control_mode.flag_control_acceleration_enabled = !offboard_control_mode.ignore_acceleration_force && + !status.in_transition_mode; control_mode.flag_control_velocity_enabled = (!offboard_control_mode.ignore_velocity || - !offboard_control_mode.ignore_position) && !status.in_transition_mode && + !offboard_control_mode.ignore_position) && !status.in_transition_mode && !control_mode.flag_control_acceleration_enabled; control_mode.flag_control_climb_rate_enabled = (!offboard_control_mode.ignore_velocity || !offboard_control_mode.ignore_position) && !control_mode.flag_control_acceleration_enabled; - control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position && !status.in_transition_mode && + control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position && !status.in_transition_mode && !control_mode.flag_control_acceleration_enabled; control_mode.flag_control_altitude_enabled = (!offboard_control_mode.ignore_velocity || @@ -3617,7 +3664,7 @@ void *commander_low_prio_loop(void *arg) if (ret != OK) { mavlink_and_console_log_critical(&mavlink_log_pub, "settings auto save error"); } else { - warnx("settings saved."); + PX4_DEBUG("commander: settings saved."); } need_param_autosave = false; @@ -3685,7 +3732,8 @@ void *commander_low_prio_loop(void *arg) false /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags, - avionics_power_rail_voltage)) { + avionics_power_rail_voltage, + can_arm_without_gps)) { answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub, command_ack); break; } else { @@ -3769,7 +3817,7 @@ void *commander_low_prio_loop(void *arg) } status_flags.condition_system_sensors_initialized = Commander::preflightCheck(&mavlink_log_pub, true, true, true, true, checkAirspeed, - !(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), !status_flags.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout); + !(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), !can_arm_without_gps, /* checkDynamic */ true, hotplug_timeout); arming_state_transition(&status, &battery, @@ -3779,7 +3827,8 @@ void *commander_low_prio_loop(void *arg) false /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags, - avionics_power_rail_voltage); + avionics_power_rail_voltage, + can_arm_without_gps); } else { tune_negative(true); diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 812e2ed09d..8bd5cc3324 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -62,8 +62,6 @@ #include #include -#include // For MAV_TYPE - #include "commander_helper.h" #include "DevMgr.hpp" @@ -75,30 +73,44 @@ using namespace DriverFramework; #endif static const int ERROR = -1; +#define VEHICLE_TYPE_QUADROTOR 2 +#define VEHICLE_TYPE_COAXIAL 3 +#define VEHICLE_TYPE_HELICOPTER 4 +#define VEHICLE_TYPE_HEXAROTOR 13 +#define VEHICLE_TYPE_OCTOROTOR 14 +#define VEHICLE_TYPE_TRICOPTER 15 +#define VEHICLE_TYPE_VTOL_DUOROTOR 19 +#define VEHICLE_TYPE_VTOL_QUADROTOR 20 +#define VEHICLE_TYPE_VTOL_TILTROTOR 21 +#define VEHICLE_TYPE_VTOL_RESERVED2 22 +#define VEHICLE_TYPE_VTOL_RESERVED3 23 +#define VEHICLE_TYPE_VTOL_RESERVED4 24 +#define VEHICLE_TYPE_VTOL_RESERVED5 25 + #define BLINK_MSG_TIME 700000 // 3 fast blinks bool is_multirotor(const struct vehicle_status_s *current_status) { - return ((current_status->system_type == MAV_TYPE_QUADROTOR) || - (current_status->system_type == MAV_TYPE_HEXAROTOR) || - (current_status->system_type == MAV_TYPE_OCTOROTOR) || - (current_status->system_type == MAV_TYPE_TRICOPTER)); + return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) || + (current_status->system_type == VEHICLE_TYPE_TRICOPTER)); } bool is_rotary_wing(const struct vehicle_status_s *current_status) { - return is_multirotor(current_status) || (current_status->system_type == MAV_TYPE_HELICOPTER) - || (current_status->system_type == MAV_TYPE_COAXIAL); + return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER) + || (current_status->system_type == VEHICLE_TYPE_COAXIAL); } bool is_vtol(const struct vehicle_status_s * current_status) { - return (current_status->system_type == MAV_TYPE_VTOL_DUOROTOR || - current_status->system_type == MAV_TYPE_VTOL_QUADROTOR || - current_status->system_type == MAV_TYPE_VTOL_TILTROTOR || - current_status->system_type == MAV_TYPE_VTOL_RESERVED2 || - current_status->system_type == MAV_TYPE_VTOL_RESERVED3 || - current_status->system_type == MAV_TYPE_VTOL_RESERVED4 || - current_status->system_type == MAV_TYPE_VTOL_RESERVED5); + return (current_status->system_type == VEHICLE_TYPE_VTOL_DUOROTOR || + current_status->system_type == VEHICLE_TYPE_VTOL_QUADROTOR || + current_status->system_type == VEHICLE_TYPE_VTOL_TILTROTOR || + current_status->system_type == VEHICLE_TYPE_VTOL_RESERVED2 || + current_status->system_type == VEHICLE_TYPE_VTOL_RESERVED3 || + current_status->system_type == VEHICLE_TYPE_VTOL_RESERVED4 || + current_status->system_type == VEHICLE_TYPE_VTOL_RESERVED5); } static hrt_abstime blink_msg_end = 0; // end time for currently blinking LED message, 0 if no blink message diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index f6532adea3..83d67a1264 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -127,10 +127,11 @@ PARAM_DEFINE_INT32(COM_DL_REG_T, 0); * Engine failure triggers only above this throttle value * * @group Commander + * @unit norm * @min 0.0 * @max 1.0 - * @decimal 1 - * @increment 0.05 + * @decimal 2 + * @increment 0.01 */ PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f); @@ -142,7 +143,7 @@ PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f); * @group Commander * @min 0.0 * @max 50.0 - * @unit A + * @unit A/% * @decimal 2 * @increment 1 */ @@ -234,6 +235,17 @@ PARAM_DEFINE_INT32(COM_AUTOS_PAR, 1); */ PARAM_DEFINE_INT32(COM_RC_IN_MODE, 0); +/** + * RC input arm/disarm command duration + * + * The default value of 1000 requires the stick to be held in the arm or disarm position for 1 second. + * + * @group Commander + * @min 100 + * @max 1500 + */ +PARAM_DEFINE_INT32(COM_RC_ARM_HYST, 1000); + /** * Time-out for auto disarm after landing * @@ -250,6 +262,19 @@ PARAM_DEFINE_INT32(COM_RC_IN_MODE, 0); */ PARAM_DEFINE_INT32(COM_DISARM_LAND, 0); +/** + * Allow arming without GPS + * + * The default allows to arm the vehicle without GPS signal. + * + * @group Commander + * @min 0 + * @max 1 + * @value 0 Don't allow arming without GPS + * @value 1 Allow arming without GPS + */ +PARAM_DEFINE_INT32(COM_ARM_WO_GPS, 1); + /** * Battery failsafe mode * @@ -264,6 +289,48 @@ PARAM_DEFINE_INT32(COM_DISARM_LAND, 0); */ PARAM_DEFINE_INT32(COM_LOW_BAT_ACT, 0); +/** + * Time-out to wait when offboard connection is lost before triggering offboard lost action. + * See COM_OBL_ACT and COM_OBL_RC_ACT to configure action. + * + * @group Commander + * @unit second + * @min 0 + * @max 60 + * @increment 1 + */ +PARAM_DEFINE_FLOAT(COM_OF_LOSS_T, 0.0f); + +/** + * Set offboard loss failsafe mode + * + * The offboard loss failsafe will only be entered after a timeout, + * set by COM_OF_LOSS_T in seconds. + * + * @value 0 Land at current position + * @value 1 Loiter + * @value 2 Return to Land + * + * @group Mission + */ +PARAM_DEFINE_INT32(COM_OBL_ACT, 0); + +/** + * Set offboard loss failsafe mode when RC is available + * + * The offboard loss failsafe will only be entered after a timeout, + * set by COM_OF_LOSS_T in seconds. + * + * @value 0 Position control + * @value 1 Altitude control + * @value 2 Manual + * @value 3 Return to Land + * @value 4 Land at current position + * + * @group Mission + */ +PARAM_DEFINE_INT32(COM_OBL_RC_ACT, 0); + /** * First flightmode slot (1000-1160) * @@ -357,7 +424,7 @@ PARAM_DEFINE_INT32(COM_FLTMODE3, -1); PARAM_DEFINE_INT32(COM_FLTMODE4, -1); /** - * Fift flightmode slot (1640-1800) + * Fifth flightmode slot (1640-1800) * * If the main switch channel is in this range the * selected flight mode will be applied. diff --git a/src/modules/commander/commander_tests/module.mk b/src/modules/commander/commander_tests/module.mk deleted file mode 100644 index 64afa86c44..0000000000 --- a/src/modules/commander/commander_tests/module.mk +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (c) 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# System state machine tests. -# - -MODULE_COMMAND = commander_tests -SRCS = commander_tests.cpp \ - state_machine_helper_test.cpp \ - ../state_machine_helper.cpp \ - ../PreflightCheck.cpp diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index d50d0a4d92..27756ffea1 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -255,10 +255,10 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) // Sensor tests - { "transition to standby error: init to standby - sensors not initialized", - { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_STANDBY, - { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + //{ "transition to standby error: init to standby - sensors not initialized", + // { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + // vehicle_status_s::ARMING_STATE_STANDBY, + // { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, // Safety switch arming tests @@ -268,33 +268,41 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, }; - struct vehicle_status_s status; - struct safety_s safety; - struct actuator_armed_s armed; + struct vehicle_status_s status = {}; + struct status_flags_s status_flags = {}; + struct safety_s safety = {}; + struct actuator_armed_s armed = {}; + struct battery_status_s battery = {}; size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]); for (size_t i=0; icurrent_state.arming_state; - status.condition_system_sensors_initialized = test->condition_system_sensors_initialized; + status_flags.condition_system_sensors_initialized = test->condition_system_sensors_initialized; status.hil_state = test->hil_state; // The power status of the test unit is not relevant for the unit test - status.circuit_breaker_engaged_power_check = true; + status_flags.circuit_breaker_engaged_power_check = true; safety.safety_switch_available = test->safety_switch_available; safety.safety_off = test->safety_off; armed.armed = test->current_state.armed; armed.ready_to_arm = test->current_state.ready_to_arm; // Attempt transition - transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, false /* no pre-arm checks */, nullptr /* no mavlink_log_pub */); + transition_result_t result = arming_state_transition(&status, &battery, &safety, test->requested_state, &armed, + false /* no pre-arm checks */, + nullptr /* no mavlink_log_pub */, + &status_flags, + 5.0f, check_gps); // Validate result of transition - ut_assert(test->assertMsg, test->expected_transition_result == result); - ut_assert(test->assertMsg, status.arming_state == test->expected_state.arming_state); - ut_assert(test->assertMsg, armed.armed == test->expected_state.armed); - ut_assert(test->assertMsg, armed.ready_to_arm == test->expected_state.ready_to_arm); + ut_compare(test->assertMsg, test->expected_transition_result, result); + ut_compare(test->assertMsg, status.arming_state, test->expected_state.arming_state); + ut_compare(test->assertMsg, armed.armed, test->expected_state.armed); + ut_compare(test->assertMsg, armed.ready_to_arm, test->expected_state.ready_to_arm); } return true; @@ -304,10 +312,10 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) { // This structure represent a single test case for testing Main State transitions. typedef struct { - const char* assertMsg; // Text to show when test case fails - uint8_t condition_bits; // Bits for various condition_* values - main_state_t from_state; // State prior to transition request - main_state_t to_state; // State to transition to + const char* assertMsg; // Text to show when test case fails + uint8_t condition_bits; // Bits for various condition_* values + uint8_t from_state; // State prior to transition request + main_state_t to_state; // State to transition to transition_result_t expected_transition_result; // Expected result from main_state_transition call } MainTransitionTest_t; @@ -325,99 +333,107 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) { "no transition: identical states", MTT_ALL_NOT_VALID, - vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED }, + commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED }, // TRANSITION_CHANGED tests - { "transition: MANUAL to ACRO", - MTT_ALL_NOT_VALID, - vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ACRO, TRANSITION_CHANGED }, + { "transition: MANUAL to ACRO - rotary", + MTT_ROTARY_WING, + commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ACRO, TRANSITION_CHANGED }, { "transition: ACRO to MANUAL", MTT_ALL_NOT_VALID, - vehicle_status_s::MAIN_STATE_ACRO, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + commander_state_s::MAIN_STATE_ACRO, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, { "transition: MANUAL to AUTO_MISSION - global position valid, home position valid", MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, - vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED }, + commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED }, { "transition: AUTO_MISSION to MANUAL - global position valid, home position valid", MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, - vehicle_status_s::MAIN_STATE_AUTO_MISSION, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + commander_state_s::MAIN_STATE_AUTO_MISSION, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, { "transition: MANUAL to AUTO_LOITER - global position valid", MTT_GLOBAL_POS_VALID, - vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED }, + commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED }, { "transition: AUTO_LOITER to MANUAL - global position valid", MTT_GLOBAL_POS_VALID, - vehicle_status_s::MAIN_STATE_AUTO_LOITER, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + commander_state_s::MAIN_STATE_AUTO_LOITER, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, { "transition: MANUAL to AUTO_RTL - global position valid, home position valid", MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, - vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED }, + commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED }, { "transition: AUTO_RTL to MANUAL - global position valid, home position valid", MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, - vehicle_status_s::MAIN_STATE_AUTO_RTL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + commander_state_s::MAIN_STATE_AUTO_RTL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, { "transition: MANUAL to ALTCTL - not rotary", - MTT_ALL_NOT_VALID, - vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, + MTT_LOC_ALT_VALID, + commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, { "transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude valid", MTT_ROTARY_WING | MTT_LOC_ALT_VALID, - vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, + commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, { "transition: MANUAL to ALTCTL - rotary, global position valid, local altitude not valid", MTT_ROTARY_WING | MTT_GLOBAL_POS_VALID, - vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, + commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, { "transition: ALTCTL to MANUAL - local altitude valid", MTT_LOC_ALT_VALID, - vehicle_status_s::MAIN_STATE_ALTCTL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + commander_state_s::MAIN_STATE_ALTCTL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, { "transition: MANUAL to POSCTL - local position not valid, global position valid", MTT_GLOBAL_POS_VALID, - vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED }, + commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED }, { "transition: MANUAL to POSCTL - local position valid, global position not valid", MTT_LOC_POS_VALID, - vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED }, + commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED }, { "transition: POSCTL to MANUAL - local position valid, global position valid", MTT_LOC_POS_VALID, - vehicle_status_s::MAIN_STATE_POSCTL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + commander_state_s::MAIN_STATE_POSCTL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, // TRANSITION_DENIED tests + { "transition: MANUAL to ACRO - not rotary", + MTT_ALL_NOT_VALID, + commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ACRO, TRANSITION_DENIED }, + { "no transition: MANUAL to AUTO_MISSION - global position not valid", MTT_ALL_NOT_VALID, - vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED }, + commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED }, { "no transition: MANUAL to AUTO_LOITER - global position not valid", MTT_ALL_NOT_VALID, - vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED }, + commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED }, { "no transition: MANUAL to AUTO_RTL - global position not valid, home position not valid", MTT_ALL_NOT_VALID, - vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, + commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, { "no transition: MANUAL to AUTO_RTL - global position not valid, home position valid", MTT_HOME_POS_VALID, - vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, + commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, { "no transition: MANUAL to AUTO_RTL - global position valid, home position not valid", MTT_GLOBAL_POS_VALID, - vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, + commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, + + { "transition: MANUAL to ALTCTL - not rotary", + MTT_ALL_NOT_VALID, + commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_DENIED }, { "no transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude not valid", MTT_ROTARY_WING, - vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_DENIED }, + commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_DENIED }, { "no transition: MANUAL to POSCTL - local position not valid, global position not valid", MTT_ALL_NOT_VALID, - vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_POSCTL, TRANSITION_DENIED }, + commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_POSCTL, TRANSITION_DENIED }, }; size_t cMainTransitionTests = sizeof(rgMainTransitionTests) / sizeof(rgMainTransitionTests[0]); @@ -425,29 +441,34 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) const MainTransitionTest_t* test = &rgMainTransitionTests[i]; // Setup initial machine state - struct vehicle_status_s current_state; - current_state.main_state = test->from_state; - current_state.is_rotary_wing = test->condition_bits & MTT_ROTARY_WING; - current_state.condition_local_altitude_valid = test->condition_bits & MTT_LOC_ALT_VALID; - current_state.condition_local_position_valid = test->condition_bits & MTT_LOC_POS_VALID; - current_state.condition_home_position_valid = test->condition_bits & MTT_HOME_POS_VALID; - current_state.condition_global_position_valid = test->condition_bits & MTT_GLOBAL_POS_VALID; + struct vehicle_status_s current_vehicle_status = {}; + struct commander_state_s current_commander_state = {}; + struct status_flags_s current_status_flags = {}; + + uint8_t main_state_prev = 0; + + current_commander_state.main_state = test->from_state; + current_vehicle_status.is_rotary_wing = test->condition_bits & MTT_ROTARY_WING; + current_status_flags.condition_local_altitude_valid = test->condition_bits & MTT_LOC_ALT_VALID; + current_status_flags.condition_local_position_valid = test->condition_bits & MTT_LOC_POS_VALID; + current_status_flags.condition_home_position_valid = test->condition_bits & MTT_HOME_POS_VALID; + current_status_flags.condition_global_position_valid = test->condition_bits & MTT_GLOBAL_POS_VALID; // Attempt transition - transition_result_t result = main_state_transition(¤t_state, test->to_state); + transition_result_t result = main_state_transition(¤t_vehicle_status, test->to_state, main_state_prev, + ¤t_status_flags, ¤t_commander_state); // Validate result of transition - ut_assert(test->assertMsg, test->expected_transition_result == result); + ut_compare(test->assertMsg, test->expected_transition_result, result); if (test->expected_transition_result == result) { if (test->expected_transition_result == TRANSITION_CHANGED) { - ut_assert(test->assertMsg, test->to_state == current_state.main_state); + ut_compare(test->assertMsg, test->to_state, current_commander_state.main_state); } else { - ut_assert(test->assertMsg, test->from_state == current_state.main_state); + ut_compare(test->assertMsg, test->from_state, current_commander_state.main_state); } } } - return true; } @@ -461,31 +482,31 @@ bool StateMachineHelperTest::isSafeTest(void) armed.lockdown = false; safety.safety_switch_available = true; safety.safety_off = false; - ut_assert("is safe: not armed", is_safe(¤t_state, &safety, &armed)); + ut_compare("is safe: not armed", is_safe(¤t_state, &safety, &armed), true); armed.armed = false; armed.lockdown = true; safety.safety_switch_available = true; safety.safety_off = true; - ut_assert("is safe: software lockdown", is_safe(¤t_state, &safety, &armed)); + ut_compare("is safe: software lockdown", is_safe(¤t_state, &safety, &armed), true); armed.armed = true; armed.lockdown = false; safety.safety_switch_available = true; safety.safety_off = true; - ut_assert("not safe: safety off", !is_safe(¤t_state, &safety, &armed)); + ut_compare("not safe: safety off", is_safe(¤t_state, &safety, &armed), false); armed.armed = true; armed.lockdown = false; safety.safety_switch_available = true; safety.safety_off = false; - ut_assert("is safe: safety off", is_safe(¤t_state, &safety, &armed)); + ut_compare("is safe: safety off", is_safe(¤t_state, &safety, &armed), true); armed.armed = true; armed.lockdown = false; safety.safety_switch_available = false; safety.safety_off = false; - ut_assert("not safe: no safety switch", !is_safe(¤t_state, &safety, &armed)); + ut_compare("not safe: no safety switch", is_safe(¤t_state, &safety, &armed), false); return true; } diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.h b/src/modules/commander/commander_tests/state_machine_helper_test.h index cf67190955..ad502618ca 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.h +++ b/src/modules/commander/commander_tests/state_machine_helper_test.h @@ -36,9 +36,7 @@ * @file state_machine_helper_test.h */ -#ifndef STATE_MACHINE_HELPER_TEST_H_ -#define STATE_MACHINE_HELPER_TEST_ +#pragma once bool stateMachineHelperTest(void); -#endif /* STATE_MACHINE_HELPER_TEST_H_ */ diff --git a/src/modules/commander/esc_calibration.cpp b/src/modules/commander/esc_calibration.cpp index 8bf042cde7..4e202d9f09 100644 --- a/src/modules/commander/esc_calibration.cpp +++ b/src/modules/commander/esc_calibration.cpp @@ -91,7 +91,7 @@ int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s* a bool batt_connected = false; hrt_abstime battery_connect_wait_timeout = 30000000; - hrt_abstime pwm_high_timeout = 4000000; + hrt_abstime pwm_high_timeout = 3000000; hrt_abstime timeout_start; calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "esc"); diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 2f93432bc9..26fa5886a9 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -92,7 +92,6 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data) } memset(&worker_data->gyro_report_0, 0, sizeof(worker_data->gyro_report_0)); - memset(&worker_data->gyro_scale, 0, sizeof(worker_data->gyro_scale)); /* use first gyro to pace, but count correctly per-gyro for statistics */ while (calibration_counter[0] < calibration_count) { @@ -186,7 +185,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) // Reset all offsets to 0 and scales to 1 (void)memcpy(&worker_data.gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale_zero)); -#ifndef __PX4_QURT +#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP) sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); int fd = px4_open(str, 0); if (fd >= 0) { @@ -239,9 +238,9 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) for (unsigned s = 0; s < gyro_count; s++) { worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s); -#ifdef __PX4_QURT +#if defined(__PX4_QURT) || defined(__PX4_POSIX_EAGLE) || defined(__PX4_POSIX_RPI) || defined(__PX4_POSIX_BEBOP) // For QURT respectively the driver framework, we need to get the device ID by copying one report. - struct gyro_report gyro_report; + struct gyro_report gyro_report; orb_copy(ORB_ID(sensor_gyro), worker_data.gyro_sensor_sub[s], &gyro_report); worker_data.device_id[s] = gyro_report.device_id; #endif @@ -337,7 +336,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) (void)sprintf(str, "CAL_GYRO%u_ID", s); failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.device_id[s]))); -#ifndef __PX4_QURT +#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP) /* apply new scaling and offsets */ (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); int fd = px4_open(str, 0); diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index dfaf1d9766..7cfcc6a700 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -124,7 +124,7 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub) _last_mag_progress = 0; for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) { -#ifndef __PX4_QURT +#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP) // Reset mag id to mag not available (void)sprintf(str, "CAL_MAG%u_ID", cur_mag); result = param_set_no_notification(param_find(str), &(device_ids[cur_mag])); @@ -166,7 +166,7 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub) #endif /* for calibration, commander will run on apps, so orb messages are used to get info from dsp */ -#ifndef __PX4_QURT +#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP) // Attempt to open mag (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag); int fd = px4_open(str, O_RDONLY); @@ -508,7 +508,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub) // Mag in this slot is available worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), cur_mag); -#ifdef __PX4_QURT +#if defined(__PX4_QURT) || defined(__PX4_POSIX_RPI) || defined(__PX4_POSIX_BEBOP) // For QURT respectively the driver framework, we need to get the device ID by copying one report. struct mag_report mag_report; orb_copy(ORB_ID(sensor_mag), worker_data.sub_mag[cur_mag], &mag_report); @@ -661,12 +661,14 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub) if (result == calibrate_return_ok) { - (void)param_set_no_notification(param_find("CAL_MAG_PRIME"), &(device_id_primary)); - for (unsigned cur_mag=0; cur_mag= 0) { px4_close(fd_mag); @@ -718,10 +720,10 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub) failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_offset))); (void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag); failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_offset))); - (void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag); // FIXME: scaling is not used right now on QURT #ifndef __PX4_QURT + (void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag); failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_scale))); (void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag); failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_scale))); @@ -746,6 +748,10 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub) } } } + + // Trigger a param set on the last step so the whole + // system updates + (void)param_set(param_find("CAL_MAG_PRIME"), &(device_id_primary)); } return result; diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index 85f2ef590e..b3e9de3aba 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2016 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 @@ -35,7 +35,6 @@ * @file px4_custom_mode.h * PX4 custom flight modes * - * @author Anton Babushkin */ #ifndef PX4_CUSTOM_MODE_H_ @@ -62,7 +61,7 @@ enum PX4_CUSTOM_SUB_MODE_AUTO { PX4_CUSTOM_SUB_MODE_AUTO_RTL, PX4_CUSTOM_SUB_MODE_AUTO_LAND, PX4_CUSTOM_SUB_MODE_AUTO_RTGS, - PX4_CUSTOM_SUB_MODE_FOLLOW_TARGET + PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET }; union px4_custom_mode { diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp index 5423c03a82..fa99b4da27 100644 --- a/src/modules/commander/rc_calibration.cpp +++ b/src/modules/commander/rc_calibration.cpp @@ -79,18 +79,26 @@ int do_trim_calibration(orb_advert_t *mavlink_log_pub) float yaw_trim_active; param_get(param_find("TRIM_YAW"), &yaw_trim_active); + /* get manual control scale values */ + float roll_scale; + param_get(param_find("FW_MAN_R_SC"), &roll_scale); + float pitch_scale; + param_get(param_find("FW_MAN_P_SC"), &pitch_scale); + float yaw_scale; + param_get(param_find("FW_MAN_Y_SC"), &yaw_scale); + /* set parameters: the new trim values are the combination of active trim values and the values coming from the remote control of the user */ - float p = sp.y + roll_trim_active; + float p = sp.y * roll_scale + roll_trim_active; int p1r = param_set(param_find("TRIM_ROLL"), &p); /* we explicitly swap sign here because the trim is added to the actuator controls which are moving in an inverse sense to manual pitch inputs */ - p = -sp.x + pitch_trim_active; + p = -sp.x * pitch_scale + pitch_trim_active; int p2r = param_set(param_find("TRIM_PITCH"), &p); - p = sp.r + yaw_trim_active; + p = sp.r * yaw_scale + yaw_trim_active; int p3r = param_set(param_find("TRIM_YAW"), &p); /* store to permanent storage */ diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 8616f0f27f..e279ee159a 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -39,6 +39,7 @@ * @author Julian Oes * @author Sander Smeets */ +#include #include #include @@ -80,7 +81,7 @@ using namespace DriverFramework; #endif // This array defines the arming state transitions. The rows are the new state, and the columns -// are the current state. Using new state and current state you can index into the array which +// are the current state. Using new state and current state you can index into the array which // will be true for a valid transition or false for a invalid transition. In some cases even // though the transition is marked as true additional checks must be made. See arming_state_transition // code for those checks. @@ -96,7 +97,7 @@ static const bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle }; // You can index into the array with an arming_state_t in order to get its textual representation -static const char * const state_names[vehicle_status_s::ARMING_STATE_MAX] = { +static const char *const state_names[vehicle_status_s::ARMING_STATE_MAX] = { "ARMING_STATE_INIT", "ARMING_STATE_STANDBY", "ARMING_STATE_ARMED", @@ -110,14 +111,15 @@ static hrt_abstime last_preflight_check = 0; ///< initialize so it gets checked static int last_prearm_ret = 1; ///< initialize to fail transition_result_t arming_state_transition(struct vehicle_status_s *status, - struct battery_status_s *battery, - const struct safety_s *safety, - arming_state_t new_arming_state, - struct actuator_armed_s *armed, - bool fRunPreArmChecks, - orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log - status_flags_s *status_flags, - float avionics_power_rail_voltage) + struct battery_status_s *battery, + const struct safety_s *safety, + arming_state_t new_arming_state, + struct actuator_armed_s *armed, + bool fRunPreArmChecks, + orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log + status_flags_s *status_flags, + float avionics_power_rail_voltage, + bool can_arm_without_gps) { // Double check that our static arrays are still valid ASSERT(vehicle_status_s::ARMING_STATE_INIT == 0); @@ -140,23 +142,25 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status, /* only perform the pre-arm check if we have to */ if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED - && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { + && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { prearm_ret = preflight_check(status, mavlink_log_pub, true /* pre-arm */, false /* force_report */, - status_flags, battery); + status_flags, battery, can_arm_without_gps); } + /* re-run the pre-flight check as long as sensors are failing */ if (!status_flags->condition_system_sensors_initialized - && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED - || new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) - && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { + && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED + || new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) + && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { if (last_preflight_check == 0 || hrt_absolute_time() - last_preflight_check > 1000 * 1000) { prearm_ret = preflight_check(status, mavlink_log_pub, false /* pre-flight */, false /* force_report */, - status_flags, battery); + status_flags, battery, can_arm_without_gps); status_flags->condition_system_sensors_initialized = !prearm_ret; last_preflight_check = hrt_absolute_time(); last_prearm_ret = prearm_ret; + } else { prearm_ret = last_prearm_ret; } @@ -165,9 +169,9 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status, /* * Perform an atomic state update */ - #ifdef __PX4_NUTTX - irqstate_t flags = irqsave(); - #endif +#ifdef __PX4_NUTTX + irqstate_t flags = px4_enter_critical_section(); +#endif /* enforce lockdown in HIL */ if (status->hil_state == vehicle_status_s::HIL_STATE_ON) { @@ -194,7 +198,7 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status, // Do not perform pre-arm checks if coming from in air restore // Allow if vehicle_status_s::HIL_STATE_ON if (status->arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE && - status->hil_state == vehicle_status_s::HIL_STATE_OFF) { + status->hil_state == vehicle_status_s::HIL_STATE_OFF) { // Fail transition if pre-arm check fails if (prearm_ret) { @@ -202,7 +206,8 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status, feedback_provided = true; valid_transition = false; - // Fail transition if we need safety switch press + // Fail transition if we need safety switch press + } else if (safety->safety_switch_available && !safety->safety_off) { mavlink_and_console_log_critical(mavlink_log_pub, "NOT ARMING: Press safety switch first!"); @@ -226,14 +231,19 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status, if (status_flags->condition_power_input_valid && (avionics_power_rail_voltage > 0.0f)) { // Check avionics rail voltages if (avionics_power_rail_voltage < 4.5f) { - mavlink_and_console_log_critical(mavlink_log_pub, "NOT ARMING: Avionics power low: %6.2f Volt", (double)avionics_power_rail_voltage); + mavlink_and_console_log_critical(mavlink_log_pub, "NOT ARMING: Avionics power low: %6.2f Volt", + (double)avionics_power_rail_voltage); feedback_provided = true; valid_transition = false; + } else if (avionics_power_rail_voltage < 4.9f) { - mavlink_and_console_log_critical(mavlink_log_pub, "CAUTION: Avionics power low: %6.2f Volt", (double)avionics_power_rail_voltage); + mavlink_and_console_log_critical(mavlink_log_pub, "CAUTION: Avionics power low: %6.2f Volt", + (double)avionics_power_rail_voltage); feedback_provided = true; + } else if (avionics_power_rail_voltage > 5.4f) { - mavlink_and_console_log_critical(mavlink_log_pub, "CAUTION: Avionics power high: %6.2f Volt", (double)avionics_power_rail_voltage); + mavlink_and_console_log_critical(mavlink_log_pub, "CAUTION: Avionics power high: %6.2f Volt", + (double)avionics_power_rail_voltage); feedback_provided = true; } } @@ -241,7 +251,8 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status, } - } else if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) { + } else if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY + && status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) { new_arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; } } @@ -258,60 +269,72 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status, if (status_flags->condition_system_sensors_initialized) { mavlink_and_console_log_critical(mavlink_log_pub, "Preflight check resolved, reboot before arming"); + } else { mavlink_and_console_log_critical(mavlink_log_pub, "Preflight check failed, refusing to arm"); } + feedback_provided = true; } else if ((new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && - status_flags->condition_system_sensors_initialized) { + status_flags->condition_system_sensors_initialized) { mavlink_and_console_log_critical(mavlink_log_pub, "Preflight check resolved, reboot to complete"); feedback_provided = true; + } else { // Silent ignore feedback_provided = true; } - // Sensors need to be initialized for STANDBY state, except for HIL + // Sensors need to be initialized for STANDBY state, except for HIL + } else if ((status->hil_state != vehicle_status_s::HIL_STATE_ON) && - (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && - (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR) && - (!status_flags->condition_system_sensors_initialized)) { - if ((!status_flags->condition_system_prearm_error_reported && - status_flags->condition_system_hotplug_timeout) || - (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)) { - mavlink_and_console_log_critical(mavlink_log_pub, "Not ready to fly: Sensors not set up correctly"); - status_flags->condition_system_prearm_error_reported = true; + (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && + (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR)) { + + if (!status_flags->condition_system_sensors_initialized) { + + if (status_flags->condition_system_hotplug_timeout) { + if (!status_flags->condition_system_prearm_error_reported) { + mavlink_and_console_log_critical(mavlink_log_pub, + "Not ready to fly: Sensors not set up correctly"); + status_flags->condition_system_prearm_error_reported = true; + } + } + + feedback_provided = true; + valid_transition = false; } - feedback_provided = true; - valid_transition = false; } // Finish up the state transition if (valid_transition) { - armed->armed = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR; - armed->ready_to_arm = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY; + armed->armed = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED + || new_arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR; + armed->ready_to_arm = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED + || new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY; ret = TRANSITION_CHANGED; status->arming_state = new_arming_state; } /* reset feedback state */ if (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR && - status->arming_state != vehicle_status_s::ARMING_STATE_INIT && - valid_transition) { + status->arming_state != vehicle_status_s::ARMING_STATE_INIT && + valid_transition) { status_flags->condition_system_prearm_error_reported = false; } /* end of atomic state update */ - #ifdef __PX4_NUTTX - irqrestore(flags); - #endif +#ifdef __PX4_NUTTX + px4_leave_critical_section(flags); +#endif } if (ret == TRANSITION_DENIED) { /* print to MAVLink and console if we didn't provide any feedback yet */ if (!feedback_provided) { - mavlink_and_console_log_critical(mavlink_log_pub, "TRANSITION_DENIED: %s - %s", state_names[status->arming_state], state_names[new_arming_state]); + mavlink_and_console_log_critical(mavlink_log_pub, "TRANSITION_DENIED: %s - %s", state_names[status->arming_state], + state_names[new_arming_state]); } } @@ -341,35 +364,45 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta /* transition may be denied even if the same state is requested because conditions may have changed */ switch (new_main_state) { case commander_state_s::MAIN_STATE_MANUAL: - case commander_state_s::MAIN_STATE_ACRO: - case commander_state_s::MAIN_STATE_RATTITUDE: case commander_state_s::MAIN_STATE_STAB: ret = TRANSITION_CHANGED; break; - case commander_state_s::MAIN_STATE_ALTCTL: - /* need at minimum altitude estimate */ - /* TODO: add this for fixedwing as well */ - if (!status->is_rotary_wing || - (status_flags->condition_local_altitude_valid || - status_flags->condition_global_position_valid)) { + case commander_state_s::MAIN_STATE_ACRO: + case commander_state_s::MAIN_STATE_RATTITUDE: + if (status->is_rotary_wing) { ret = TRANSITION_CHANGED; } + + break; + + case commander_state_s::MAIN_STATE_ALTCTL: + + /* need at minimum altitude estimate */ + if (status_flags->condition_local_altitude_valid || + status_flags->condition_global_position_valid) { + ret = TRANSITION_CHANGED; + } + break; case commander_state_s::MAIN_STATE_POSCTL: + /* need at minimum local position estimate */ if (status_flags->condition_local_position_valid || status_flags->condition_global_position_valid) { ret = TRANSITION_CHANGED; } + break; case commander_state_s::MAIN_STATE_AUTO_LOITER: + /* need global position estimate */ if (status_flags->condition_global_position_valid) { ret = TRANSITION_CHANGED; } + break; case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET: @@ -377,10 +410,12 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta case commander_state_s::MAIN_STATE_AUTO_RTL: case commander_state_s::MAIN_STATE_AUTO_TAKEOFF: case commander_state_s::MAIN_STATE_AUTO_LAND: + /* need global position and home position */ if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) { ret = TRANSITION_CHANGED; } + break; case commander_state_s::MAIN_STATE_OFFBOARD: @@ -396,10 +431,13 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta default: break; } + if (ret == TRANSITION_CHANGED) { if (internal_state->main_state != new_main_state) { main_state_prev = internal_state->main_state; internal_state->main_state = new_main_state; + internal_state->timestamp = hrt_absolute_time(); + } else { ret = TRANSITION_NOT_CHANGED; } @@ -411,7 +449,8 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta /** * Transition from one hil state to another */ -transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t status_pub, struct vehicle_status_s *current_status, orb_advert_t *mavlink_log_pub) +transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t status_pub, + struct vehicle_status_s *current_status, orb_advert_t *mavlink_log_pub) { transition_result_t ret = TRANSITION_DENIED; @@ -492,6 +531,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR"); } + closedir(d); ret = TRANSITION_CHANGED; @@ -508,14 +548,17 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta // Handle VDev devices const char *devname; unsigned int handle = 0; - for(;;) { - devname = px4_get_device_names(&handle); - if (devname == NULL) - break; - /* skip mavlink */ + for (;;) { + devname = px4_get_device_names(&handle); + + if (devname == NULL) { + break; + } + + /* skip mavlink */ if (!strcmp("/dev/mavlink", devname)) { - continue; + continue; } @@ -523,7 +566,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta if (sensfd < 0) { warn("failed opening device %s", devname); - continue; + continue; } int block_ret = px4_ioctl(sensfd, DEVIOCSPUBBLOCK, 1); @@ -536,7 +579,8 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta // Handle DF devices const char *df_dev_path; unsigned int index = 0; - for(;;) { + + for (;;) { if (DevMgr::getNextDeviceName(index, &df_dev_path) < 0) { break; } @@ -563,6 +607,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta mavlink_and_console_log_critical(mavlink_log_pub, "Not switching to HIL when armed"); ret = TRANSITION_DENIED; } + break; default: @@ -577,6 +622,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta // XXX also set lockdown here orb_publish(ORB_ID(vehicle_status), status_pub, current_status); } + return ret; } @@ -585,11 +631,13 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta */ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *internal_state, const bool data_link_loss_enabled, const bool mission_finished, - const bool stay_in_failsafe, status_flags_s *status_flags, bool landed, const bool rc_loss_enabled) + const bool stay_in_failsafe, status_flags_s *status_flags, bool landed, + const bool rc_loss_enabled, const int offb_loss_act, const int offb_loss_rc_act) { navigation_state_t nav_state_old = status->nav_state; - bool armed = (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED || status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR); + bool armed = (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED + || status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR); status->failsafe = false; /* evaluate main state to decide in normal (non-failsafe) mode */ @@ -599,17 +647,20 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in case commander_state_s::MAIN_STATE_RATTITUDE: case commander_state_s::MAIN_STATE_STAB: case commander_state_s::MAIN_STATE_ALTCTL: - case commander_state_s::MAIN_STATE_POSCTL: + /* require RC for all manual modes */ - if (rc_loss_enabled && (status->rc_signal_lost || status_flags->rc_signal_lost_cmd) && armed && !landed) { + if (rc_loss_enabled && (status->rc_signal_lost || status_flags->rc_signal_lost_cmd) && armed) { status->failsafe = true; if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; + } else if (status_flags->condition_local_position_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; + } else if (status_flags->condition_local_altitude_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + } else { status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; } @@ -636,15 +687,57 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL; break; - case commander_state_s::MAIN_STATE_POSCTL: - status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; - break; - default: status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; break; } } + + break; + + case commander_state_s::MAIN_STATE_POSCTL: { + const bool rc_lost = rc_loss_enabled && (status->rc_signal_lost || status_flags->rc_signal_lost_cmd); + + if (rc_lost && armed) { + status->failsafe = true; + + if (status_flags->condition_global_position_valid && + status_flags->condition_home_position_valid && + !status_flags->gps_failure) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; + + } else if (status_flags->condition_local_position_valid && + !status_flags->gps_failure) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; + + } else if (status_flags->condition_local_altitude_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + } + + /* As long as there is RC, we can fallback to ALTCTL, or STAB. */ + /* A local position estimate is enough for POSCTL for multirotors, + * this enables POSCTL using e.g. flow. + * For fixedwing, a global position is needed. */ + + } else if (((status->is_rotary_wing && !status_flags->condition_local_position_valid) || + (!status->is_rotary_wing && !status_flags->condition_global_position_valid)) + && armed) { + status->failsafe = true; + + if (status_flags->condition_local_altitude_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL; + + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_STAB; + } + + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; + } + } break; case commander_state_s::MAIN_STATE_AUTO_MISSION: @@ -658,206 +751,329 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in /* first look at the commands */ if (status->engine_failure_cmd) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; + } else if (status_flags->data_link_lost_cmd) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; + } else if (status_flags->gps_failure_cmd) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + status->failsafe = true; + } else if (status_flags->rc_signal_lost_cmd) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; + } else if (status_flags->vtol_transition_failure_cmd) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; - /* finished handling commands which have priority, now handle failures */ + /* finished handling commands which have priority, now handle failures */ + } else if (status_flags->gps_failure) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + status->failsafe = true; + } else if (status->engine_failure) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; + } else if (status_flags->vtol_transition_failure) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; + } else if (status->mission_failure) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; - /* datalink loss enabled: - * check for datalink lost: this should always trigger RTGS */ + /* datalink loss enabled: + * check for datalink lost: this should always trigger RTGS */ + } else if (data_link_loss_enabled && status->data_link_lost) { status->failsafe = true; if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; + } else if (status_flags->condition_local_position_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; + } else if (status_flags->condition_local_altitude_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + } else { status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; } - /* datalink loss disabled: - * check if both, RC and datalink are lost during the mission - * or RC is lost after the mission is finished: this should always trigger RCRECOVER */ - } else if (!data_link_loss_enabled && ((status->rc_signal_lost && status->data_link_lost) || - (status->rc_signal_lost && mission_finished))) { + /* datalink loss disabled: + * check if both, RC and datalink are lost during the mission + * or all links are lost after the mission finishes in air: this should always trigger RCRECOVER */ + + } else if (!data_link_loss_enabled && status->rc_signal_lost && status->data_link_lost && !landed && mission_finished) { status->failsafe = true; if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; + } else if (status_flags->condition_local_position_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; + } else if (status_flags->condition_local_altitude_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + } else { status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; } - /* stay where you are if you should stay in failsafe, otherwise everything is perfect */ - } else if (!stay_in_failsafe){ + /* stay where you are if you should stay in failsafe, otherwise everything is perfect */ + + } else if (!stay_in_failsafe) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; } + break; case commander_state_s::MAIN_STATE_AUTO_LOITER: + /* go into failsafe on a engine failure */ if (status->engine_failure) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; - /* also go into failsafe if just datalink is lost */ + + } else if (status_flags->gps_failure) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + status->failsafe = true; + + /* also go into failsafe if just datalink is lost */ + } else if (status->data_link_lost && data_link_loss_enabled) { status->failsafe = true; if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; + } else if (status_flags->condition_local_position_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; + } else if (status_flags->condition_local_altitude_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + } else { status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; } - /* go into failsafe if RC is lost and datalink loss is not set up */ + /* go into failsafe if RC is lost and datalink loss is not set up */ + } else if (status->rc_signal_lost && !data_link_loss_enabled) { status->failsafe = true; if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; + } else if (status_flags->condition_local_position_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; + } else if (status_flags->condition_local_altitude_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + } else { status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; } - /* don't bother if RC is lost if datalink is connected */ + /* don't bother if RC is lost if datalink is connected */ + } else if (status->rc_signal_lost) { /* this mode is ok, we don't need RC for loitering */ status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; + } else { /* everything is perfect */ status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; } + break; case commander_state_s::MAIN_STATE_AUTO_RTL: + /* require global position and home, also go into failsafe on an engine failure */ if (status->engine_failure) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; + + } else if (status_flags->gps_failure) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + status->failsafe = true; + } else if ((!status_flags->condition_global_position_valid || - !status_flags->condition_home_position_valid)) { + !status_flags->condition_home_position_valid)) { status->failsafe = true; if (status_flags->condition_local_position_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; + } else if (status_flags->condition_local_altitude_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + } else { status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; } + } else { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; } + break; case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET: + /* require global position and home */ if (status->engine_failure) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; + } else if (!status_flags->condition_global_position_valid) { status->failsafe = true; if (status_flags->condition_local_position_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; + } else if (status_flags->condition_local_altitude_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + } else { status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; } + } else { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET; } + break; case commander_state_s::MAIN_STATE_AUTO_TAKEOFF: + /* require global position and home */ if (status->engine_failure) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; - } else if ((!status_flags->condition_global_position_valid || - !status_flags->condition_home_position_valid)) { + + } else if (status_flags->gps_failure || (!status_flags->condition_global_position_valid || + !status_flags->condition_home_position_valid)) { status->failsafe = true; if (status_flags->condition_local_position_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; + } else if (status_flags->condition_local_altitude_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + } else { status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; } + } else { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF; } + break; case commander_state_s::MAIN_STATE_AUTO_LAND: + /* require global position and home */ if (status->engine_failure) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; - } else if ((!status_flags->condition_global_position_valid || - !status_flags->condition_home_position_valid)) { + + } else if (status_flags->gps_failure || (!status_flags->condition_global_position_valid || + !status_flags->condition_home_position_valid)) { status->failsafe = true; if (status_flags->condition_local_altitude_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + } else { status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; } + } else { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; } + break; case commander_state_s::MAIN_STATE_OFFBOARD: + /* require offboard control, otherwise stay where you are */ if (status_flags->offboard_control_signal_lost && !status->rc_signal_lost) { status->failsafe = true; - status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; + if (status_flags->offboard_control_loss_timeout && offb_loss_rc_act < 5 && offb_loss_rc_act >= 0) { + if (offb_loss_rc_act == 3 && status_flags->condition_global_position_valid + && status_flags->condition_home_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; + + } else if (offb_loss_rc_act == 0 && status_flags->condition_global_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; + + } else if (offb_loss_rc_act == 1 && status_flags->condition_local_altitude_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL; + + } else if (offb_loss_rc_act == 2) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; + + } else if (offb_loss_rc_act == 4 && status_flags->condition_global_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; + + } else if (status_flags->condition_local_altitude_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + } + + } else { + if (status_flags->condition_global_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; + + } else if (status_flags->condition_local_altitude_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL; + + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + } + } + } else if (status_flags->offboard_control_signal_lost && status->rc_signal_lost) { status->failsafe = true; - if (status_flags->condition_local_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; - } else if (status_flags->condition_local_altitude_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + if (status_flags->offboard_control_loss_timeout && offb_loss_act < 3 && offb_loss_act >= 0) { + if (offb_loss_act == 2 && status_flags->condition_global_position_valid + && status_flags->condition_home_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; + + } else if (offb_loss_act == 1 && status_flags->condition_global_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; + + } else if (offb_loss_act == 0 && status_flags->condition_global_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; + + } else if (status_flags->condition_local_altitude_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + } + } else { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + if (status_flags->condition_global_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; + + } else if (status_flags->condition_local_altitude_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + } } + } else { status->nav_state = vehicle_status_s::NAVIGATION_STATE_OFFBOARD; } + default: break; } @@ -865,14 +1081,16 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in return status->nav_state != nav_state_old; } -int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report, status_flags_s *status_flags, battery_status_s *battery) +int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report, + status_flags_s *status_flags, battery_status_s *battery, bool can_arm_without_gps) { /* */ bool reportFailures = force_report || (!status_flags->condition_system_prearm_error_reported && - status_flags->condition_system_hotplug_timeout); + status_flags->condition_system_hotplug_timeout); bool checkAirspeed = false; + /* Perform airspeed check only if circuit breaker is not * engaged and it's not a rotary wing */ if (!status_flags->circuit_breaker_engaged_airspd_check && (!status->is_rotary_wing || status->is_vtol)) { @@ -880,11 +1098,12 @@ int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_p } bool preflight_ok = Commander::preflightCheck(mavlink_log_pub, true, true, true, true, - checkAirspeed, (status->rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), - !status_flags->circuit_breaker_engaged_gpsfailure_check, true, reportFailures); + checkAirspeed, (status->rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), + !can_arm_without_gps, true, reportFailures); - if (!status_flags->cb_usb && status_flags->usb_connected && prearm) { + if (!status_flags->circuit_breaker_engaged_usb_check && status_flags->usb_connected && prearm) { preflight_ok = false; + if (reportFailures) { mavlink_and_console_log_critical(mavlink_log_pub, "ARMING DENIED: Flying with USB is not safe"); } @@ -892,6 +1111,7 @@ int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_p if (battery->warning == battery_status_s::BATTERY_WARNING_CRITICAL) { preflight_ok = false; + if (reportFailures) { mavlink_and_console_log_critical(mavlink_log_pub, "ARMING DENIED: VERY LOW BATTERY"); } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 8a951a0375..1506ff8acb 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -76,11 +76,13 @@ struct status_flags_s { bool circuit_breaker_engaged_airspd_check; bool circuit_breaker_engaged_enginefailure_check; bool circuit_breaker_engaged_gpsfailure_check; - bool cb_usb; + bool circuit_breaker_flight_termination_disabled; + bool circuit_breaker_engaged_usb_check; bool offboard_control_signal_found_once; bool offboard_control_signal_lost; bool offboard_control_signal_weak; bool offboard_control_set_by_command; // true if the offboard mode was set by a mavlink command and should not be overridden by RC + bool offboard_control_loss_timeout; // true if offboard is lost for a certain amount of time bool rc_signal_found_once; bool rc_signal_lost_cmd; // true if RC lost mode is commanded bool rc_input_blocked; // set if RC input should be ignored temporarily @@ -102,7 +104,8 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status, bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log status_flags_s *status_flags, - float avionics_power_rail_voltage); + float avionics_power_rail_voltage, + bool can_arm_without_gps); transition_result_t main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state, uint8_t &main_state_prev, @@ -113,8 +116,8 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *internal_state, const bool data_link_loss_enabled, const bool mission_finished, const bool stay_in_failsafe, status_flags_s *status_flags, bool landed, - const bool rc_loss_enabled); + const bool rc_loss_enabled, const int offb_loss_act, const int offb_loss_rc_act); -int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report, status_flags_s *status_flags, battery_status_s *battery); +int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report, status_flags_s *status_flags, battery_status_s *battery, bool can_arm_without_gps); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/modules/controllib_test/CMakeLists.txt b/src/modules/controllib_test/CMakeLists.txt index 5d3a221fce..7cb86e5f8d 100644 --- a/src/modules/controllib_test/CMakeLists.txt +++ b/src/modules/controllib_test/CMakeLists.txt @@ -37,7 +37,6 @@ px4_add_module( -Os SRCS controllib_test_main.cpp - test_params.c DEPENDS platforms__common ) diff --git a/src/modules/controllib_test/test_params.c b/src/modules/controllib_test/test_params.c index 7c609cad39..5ef469b3e2 100644 --- a/src/modules/controllib_test/test_params.c +++ b/src/modules/controllib_test/test_params.c @@ -4,19 +4,63 @@ // you want to recompute the // answers for all of the unit tests + +/** + * @group Testing + */ PARAM_DEFINE_FLOAT(TEST_MIN, -1.0f); + +/** + * @group Testing + */ PARAM_DEFINE_FLOAT(TEST_MAX, 1.0f); + +/** + * @group Testing + */ PARAM_DEFINE_FLOAT(TEST_TRIM, 0.5f); + +/** + * @group Testing + */ PARAM_DEFINE_FLOAT(TEST_HP, 10.0f); + +/** + * @group Testing + */ PARAM_DEFINE_FLOAT(TEST_LP, 10.0f); +/** + * @group Testing + */ PARAM_DEFINE_FLOAT(TEST_P, 0.2f); +/** + * @group Testing + */ PARAM_DEFINE_FLOAT(TEST_I, 0.1f); + +/** + * @group Testing + */ PARAM_DEFINE_FLOAT(TEST_I_MAX, 1.0f); +/** + * @group Testing + */ PARAM_DEFINE_FLOAT(TEST_D, 0.01f); + +/** + * @group Testing + */ PARAM_DEFINE_FLOAT(TEST_D_LP, 10.0f); +/** + * @group Testing + */ PARAM_DEFINE_FLOAT(TEST_MEAN, 1.0f); + +/** + * @group Testing + */ PARAM_DEFINE_FLOAT(TEST_DEV, 2.0f); diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 30d7e359da..8a22c62d23 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -134,7 +134,11 @@ static px4_sem_t g_sys_state_mutex; /* The data manager store file handle and file name */ static int g_fd = -1, g_task_fd = -1; +#ifdef __PX4_POSIX_EAGLE +static const char *default_device_path = PX4_ROOTFSDIR"/dataman"; +#else static const char *default_device_path = PX4_ROOTFSDIR"/fs/microsd/dataman"; +#endif static char *k_data_manager_device_path = NULL; /* The data manager work queues */ @@ -711,6 +715,11 @@ task_main(int argc, char *argv[]) g_task_fd = open(k_data_manager_device_path, O_RDONLY | O_BINARY); if (g_task_fd >= 0) { + +#ifndef __PX4_POSIX + // XXX on Mac OS and Linux the file is not a multiple of the sector sizes + // this might need further inspection. + /* File exists, check its size */ int file_size = lseek(g_task_fd, 0, SEEK_END); @@ -718,54 +727,47 @@ task_main(int argc, char *argv[]) PX4_WARN("Incompatible data manager file %s, resetting it", k_data_manager_device_path); PX4_WARN("Size: %u, sector size: %d", file_size, k_sector_size); close(g_task_fd); -#ifndef __PX4_POSIX - // XXX on Mac OS and Linux the file is not a multiple of the sector sizes - // this might need further inspection unlink(k_data_manager_device_path); + } + +#else + close(g_task_fd); #endif - } else { - close(g_task_fd); - } } /* Open or create the data manager file */ g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY, PX4_O_MODE_666); if (g_task_fd < 0) { - warnx("Could not open data manager file %s", k_data_manager_device_path); + PX4_WARN("Could not open data manager file %s", k_data_manager_device_path); px4_sem_post(&g_init_sema); /* Don't want to hang startup */ return -1; } if ((unsigned)lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) { close(g_task_fd); - warnx("Could not seek data manager file %s", k_data_manager_device_path); + PX4_WARN("Could not seek data manager file %s", k_data_manager_device_path); px4_sem_post(&g_init_sema); /* Don't want to hang startup */ return -1; } fsync(g_task_fd); - printf("dataman: "); /* see if we need to erase any items based on restart type */ int sys_restart_val; + const char *restart_type_str = "Unkown restart"; + if (param_get(param_find("SYS_RESTART_TYPE"), &sys_restart_val) == OK) { if (sys_restart_val == DM_INIT_REASON_POWER_ON) { - printf("Power on restart"); + restart_type_str = "Power on restart"; _restart(DM_INIT_REASON_POWER_ON); } else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) { - printf("In flight restart"); + restart_type_str = "In flight restart"; _restart(DM_INIT_REASON_IN_FLIGHT); - - } else { - printf("Unknown restart"); } - - } else { - printf("Unknown restart"); } /* We use two file descriptors, one for the caller context and one for the worker thread */ @@ -773,7 +775,8 @@ task_main(int argc, char *argv[]) /* worker thread is shutting down but still processing requests */ g_fd = g_task_fd; - printf(", data manager file '%s' size is %d bytes\n", k_data_manager_device_path, max_offset); + PX4_INFO("%s, data manager file '%s' size is %d bytes", + restart_type_str, k_data_manager_device_path, max_offset); /* Tell startup that the worker thread has completed its initialization */ px4_sem_post(&g_init_sema); @@ -881,11 +884,11 @@ static void status(void) { /* display usage statistics */ - warnx("Writes %d", g_func_counts[dm_write_func]); - warnx("Reads %d", g_func_counts[dm_read_func]); - warnx("Clears %d", g_func_counts[dm_clear_func]); - warnx("Restarts %d", g_func_counts[dm_restart_func]); - warnx("Max Q lengths work %d, free %d", g_work_q.max_size, g_free_q.max_size); + PX4_INFO("Writes %d", g_func_counts[dm_write_func]); + PX4_INFO("Reads %d", g_func_counts[dm_read_func]); + PX4_INFO("Clears %d", g_func_counts[dm_clear_func]); + PX4_INFO("Restarts %d", g_func_counts[dm_restart_func]); + PX4_INFO("Max Q lengths work %d, free %d", g_work_q.max_size, g_free_q.max_size); } static void @@ -899,7 +902,7 @@ stop(void) static void usage(void) { - warnx("usage: dataman {start [-f datafile]|stop|status|poweronrestart|inflightrestart}"); + PX4_INFO("usage: dataman {start [-f datafile]|stop|status|poweronrestart|inflightrestart}"); } int @@ -913,13 +916,13 @@ dataman_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (g_fd >= 0) { - warnx("dataman already running"); + PX4_WARN("dataman already running"); return -1; } if (argc == 4 && strcmp(argv[2], "-f") == 0) { k_data_manager_device_path = strdup(argv[3]); - warnx("dataman file set to: %s\n", k_data_manager_device_path); + PX4_INFO("dataman file set to: %s", k_data_manager_device_path); } else { k_data_manager_device_path = strdup(default_device_path); @@ -928,7 +931,7 @@ dataman_main(int argc, char *argv[]) start(); if (g_fd < 0) { - warnx("dataman start failed"); + PX4_ERR("dataman start failed"); free(k_data_manager_device_path); k_data_manager_device_path = NULL; return -1; @@ -939,7 +942,7 @@ dataman_main(int argc, char *argv[]) /* Worker thread should be running for all other commands */ if (g_fd < 0) { - warnx("dataman worker thread not running"); + PX4_WARN("dataman worker thread not running"); usage(); return -1; } diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 5fae22e36e..148402e533 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -33,7 +33,7 @@ px4_add_module( MODULE modules__ekf2 MAIN ekf2 - COMPILE_FLAGS + COMPILE_FLAGS -Os STACK_MAIN 2500 STACK_MAX 4000 SRCS diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index a77cf62072..67d12ef0a6 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -69,6 +69,7 @@ #include #include #include +#include #include #include #include @@ -79,6 +80,7 @@ #include #include #include +#include #include @@ -114,7 +116,7 @@ public: */ int start(); - void set_replay_mode(bool replay) {_replay_mode = true;}; + void set_replay_mode(bool replay) {_replay_mode = replay;}; static void task_main_trampoline(int argc, char *argv[]); @@ -126,21 +128,25 @@ public: private: static constexpr float _dt_max = 0.02; - bool _task_should_exit = false; - int _control_task = -1; // task handle for task - bool _replay_mode; // should we use replay data from a log - int _publish_replay_mode; // defines if we should publish replay messages + bool _task_should_exit = false; + int _control_task = -1; // task handle for task + bool _replay_mode; // should we use replay data from a log + int _publish_replay_mode; // defines if we should publish replay messages + float _default_ev_pos_noise = 0.05f; // external vision position noise used when an invalid value is supplied + float _default_ev_ang_noise = 0.05f; // external vision angle noise used when an invalid value is supplied - int _sensors_sub = -1; - int _gps_sub = -1; - int _airspeed_sub = -1; - int _params_sub = -1; + int _sensors_sub = -1; + int _gps_sub = -1; + int _airspeed_sub = -1; + int _params_sub = -1; int _optical_flow_sub = -1; int _range_finder_sub = -1; - int _actuator_armed_sub = -1; - int _vehicle_land_detected_sub = -1; + int _ev_pos_sub = -1; + int _actuator_armed_sub = -1; + int _vehicle_land_detected_sub = -1; + int _status_sub = -1; - bool _prev_motors_armed = false; // motors armed status from the previous frame + bool _prev_landed = true; // landed status from the previous frame float _acc_hor_filt = 0.0f; // low-pass filtered horizontal acceleration @@ -168,6 +174,7 @@ private: control::BlockParamFloat _flow_delay_ms; control::BlockParamFloat _rng_delay_ms; control::BlockParamFloat _airspeed_delay_ms; + control::BlockParamFloat _ev_delay_ms; control::BlockParamFloat _gyro_noise; control::BlockParamFloat _accel_noise; @@ -175,7 +182,6 @@ private: // process noise control::BlockParamFloat _gyro_bias_p_noise; control::BlockParamFloat _accel_bias_p_noise; - control::BlockParamFloat _gyro_scale_p_noise; control::BlockParamFloat _mage_p_noise; control::BlockParamFloat _magb_p_noise; control::BlockParamFloat _wind_vel_p_noise; @@ -189,6 +195,7 @@ private: control::BlockParamFloat _baro_innov_gate; // innovation gate for barometric height innovation test (std dev) control::BlockParamFloat _posNE_innov_gate; // innovation gate for GPS horizontal position innovation test (std dev) control::BlockParamFloat _vel_innov_gate; // innovation gate for GPS velocity innovation test (std dev) + control::BlockParamFloat _tas_innov_gate; // innovation gate for tas innovation test (std dev) control::BlockParamFloat _mag_heading_noise; // measurement noise used for simple heading fusion control::BlockParamFloat _mag_noise; // measurement noise used for 3-axis magnetoemter fusion (Gauss) @@ -220,6 +227,11 @@ private: control::BlockParamFloat _range_innov_gate; // range finder fusion innovation consistency gate size (STD) control::BlockParamFloat _rng_gnd_clearance; // minimum valid value for range when on ground (m) + // vision estimate fusion + control::BlockParamFloat _ev_pos_noise; // default position observation noise for exernal vision measurements (m) + control::BlockParamFloat _ev_ang_noise; // default angular observation noise for exernal vision measurements (rad) + control::BlockParamFloat _ev_innov_gate; // external vision position innovation consistency gate size (STD) + // optical flow fusion control::BlockParamFloat _flow_noise; // best quality observation noise for optical flow LOS rate measurements (rad/sec) @@ -242,6 +254,25 @@ private: control::BlockParamFloat _flow_pos_x; // X position of optical flow sensor focal point in body frame (m) control::BlockParamFloat _flow_pos_y; // Y position of optical flow sensor focal point in body frame (m) control::BlockParamFloat _flow_pos_z; // Z position of optical flow sensor focal point in body frame (m) + control::BlockParamFloat _ev_pos_x; // X position of VI sensor focal point in body frame (m) + control::BlockParamFloat _ev_pos_y; // Y position of VI sensor focal point in body frame (m) + control::BlockParamFloat _ev_pos_z; // Z position of VI sensor focal point in body frame (m) + // control of airspeed and sideslip fusion + control::BlockParamFloat + _arspFusionThreshold; // a value of zero will disabled airspeed fusion. Any another positive value will determine + // the minimum airspeed which will still be fused + + // output predictor filter time constants + control::BlockParamFloat _tau_vel; // time constant used by the output velocity complementary filter (s) + control::BlockParamFloat _tau_pos; // time constant used by the output position complementary filter (s) + + // IMU switch on bias paameters + control::BlockParamFloat _gyr_bias_init; // 1-sigma gyro bias uncertainty at switch-on (rad/sec) + control::BlockParamFloat _acc_bias_init; // 1-sigma accelerometer bias uncertainty at switch-on (m/s**2) + control::BlockParamFloat _ang_err_init; // 1-sigma uncertainty in tilt angle after gravity vector alignment (rad) + + // airspeed mode parameter + control::BlockParamInt _airspeed_mode; int update_subscriptions(); @@ -270,11 +301,11 @@ Ekf2::Ekf2(): _flow_delay_ms(this, "EKF2_OF_DELAY", false, &_params->flow_delay_ms), _rng_delay_ms(this, "EKF2_RNG_DELAY", false, &_params->range_delay_ms), _airspeed_delay_ms(this, "EKF2_ASP_DELAY", false, &_params->airspeed_delay_ms), + _ev_delay_ms(this, "EKF2_EV_DELAY", false, &_params->ev_delay_ms), _gyro_noise(this, "EKF2_GYR_NOISE", false, &_params->gyro_noise), _accel_noise(this, "EKF2_ACC_NOISE", false, &_params->accel_noise), _gyro_bias_p_noise(this, "EKF2_GYR_B_NOISE", false, &_params->gyro_bias_p_noise), _accel_bias_p_noise(this, "EKF2_ACC_B_NOISE", false, &_params->accel_bias_p_noise), - _gyro_scale_p_noise(this, "EKF2_GYR_S_NOISE", false, &_params->gyro_scale_p_noise), _mage_p_noise(this, "EKF2_MAG_E_NOISE", false, &_params->mage_p_noise), _magb_p_noise(this, "EKF2_MAG_B_NOISE", false, &_params->magb_p_noise), _wind_vel_p_noise(this, "EKF2_WIND_NOISE", false, &_params->wind_vel_p_noise), @@ -287,6 +318,7 @@ Ekf2::Ekf2(): _baro_innov_gate(this, "EKF2_BARO_GATE", false, &_params->baro_innov_gate), _posNE_innov_gate(this, "EKF2_GPS_P_GATE", false, &_params->posNE_innov_gate), _vel_innov_gate(this, "EKF2_GPS_V_GATE", false, &_params->vel_innov_gate), + _tas_innov_gate(this, "EKF2_TAS_GATE", false, &_params->tas_innov_gate), _mag_heading_noise(this, "EKF2_HEAD_NOISE", false, &_params->mag_heading_noise), _mag_noise(this, "EKF2_MAG_NOISE", false, &_params->mag_noise), _eas_noise(this, "EKF2_EAS_NOISE", false, &_params->eas_noise), @@ -309,6 +341,9 @@ Ekf2::Ekf2(): _range_noise(this, "EKF2_RNG_NOISE", false, &_params->range_noise), _range_innov_gate(this, "EKF2_RNG_GATE", false, &_params->range_innov_gate), _rng_gnd_clearance(this, "EKF2_MIN_RNG", false, &_params->rng_gnd_clearance), + _ev_pos_noise(this, "EKF2_EVP_NOISE", false, &_default_ev_pos_noise), + _ev_ang_noise(this, "EKF2_EVA_NOISE", false, &_default_ev_ang_noise), + _ev_innov_gate(this, "EKF2_EV_GATE", false, &_params->ev_innov_gate), _flow_noise(this, "EKF2_OF_N_MIN", false, &_params->flow_noise), _flow_noise_qual_min(this, "EKF2_OF_N_MAX", false, &_params->flow_noise_qual_min), _flow_qual_min(this, "EKF2_OF_QMIN", false, &_params->flow_qual_min), @@ -325,7 +360,17 @@ Ekf2::Ekf2(): _rng_pos_z(this, "EKF2_RNG_POS_Z", false, &_params->rng_pos_body(2)), _flow_pos_x(this, "EKF2_OF_POS_X", false, &_params->flow_pos_body(0)), _flow_pos_y(this, "EKF2_OF_POS_Y", false, &_params->flow_pos_body(1)), - _flow_pos_z(this, "EKF2_OF_POS_Z", false, &_params->flow_pos_body(2)) + _flow_pos_z(this, "EKF2_OF_POS_Z", false, &_params->flow_pos_body(2)), + _ev_pos_x(this, "EKF2_EV_POS_X", false, &_params->ev_pos_body(0)), + _ev_pos_y(this, "EKF2_EV_POS_Y", false, &_params->ev_pos_body(1)), + _ev_pos_z(this, "EKF2_EV_POS_Z", false, &_params->ev_pos_body(2)), + _arspFusionThreshold(this, "EKF2_ARSP_THR", false), + _tau_vel(this, "EKF2_TAU_VEL", false, &_params->vel_Tau), + _tau_pos(this, "EKF2_TAU_POS", false, &_params->pos_Tau), + _gyr_bias_init(this, "EKF2_GBIAS_INIT", false, &_params->switch_on_gyro_bias), + _acc_bias_init(this, "EKF2_ABIAS_INIT", false, &_params->switch_on_accel_bias), + _ang_err_init(this, "EKF2_ANGERR_INIT", false, &_params->initial_tilt_err), + _airspeed_mode(this, "FW_ARSP_MODE", false) { } @@ -350,8 +395,9 @@ void Ekf2::task_main() _params_sub = orb_subscribe(ORB_ID(parameter_update)); _optical_flow_sub = orb_subscribe(ORB_ID(optical_flow)); _range_finder_sub = orb_subscribe(ORB_ID(distance_sensor)); - _actuator_armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + _ev_pos_sub = orb_subscribe(ORB_ID(vision_position_estimate)); _vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); + _status_sub = orb_subscribe(ORB_ID(vehicle_status)); px4_pollfd_struct_t fds[2] = {}; fds[0].fd = _sensors_sub; @@ -370,7 +416,9 @@ void Ekf2::task_main() airspeed_s airspeed = {}; optical_flow_s optical_flow = {}; distance_sensor_s range_finder = {}; - actuator_armed_s actuator_armed = {}; + vehicle_land_detected_s vehicle_land_detected = {}; + vision_position_estimate_s ev = {}; + vehicle_status_s _vehicle_status = {}; while (!_task_should_exit) { int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000); @@ -403,11 +451,19 @@ void Ekf2::task_main() bool airspeed_updated = false; bool optical_flow_updated = false; bool range_finder_updated = false; - bool actuator_armed_updated = false; bool vehicle_land_detected_updated = false; + bool vision_position_updated = false; + bool vehicle_status_updated = false; orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors); // update all other topics if they have new data + + orb_check(_status_sub, &vehicle_status_updated); + + if (vehicle_status_updated) { + orb_copy(ORB_ID(vehicle_status), _status_sub, &_vehicle_status); + } + orb_check(_gps_sub, &gps_updated); if (gps_updated) { @@ -430,6 +486,15 @@ void Ekf2::task_main() if (range_finder_updated) { orb_copy(ORB_ID(distance_sensor), _range_finder_sub, &range_finder); + if (range_finder.min_distance >= range_finder.current_distance || range_finder.max_distance <= range_finder.current_distance) { + range_finder_updated = false; + } + } + + orb_check(_ev_pos_sub, &vision_position_updated); + + if (vision_position_updated) { + orb_copy(ORB_ID(vision_position_estimate), _ev_pos_sub, &ev); } // in replay mode we are getting the actual timestamp from the sensor topic @@ -443,19 +508,35 @@ void Ekf2::task_main() } // push imu data into estimator - _ekf.setIMUData(now, sensors.gyro_integral_dt[0], sensors.accelerometer_integral_dt[0], - &sensors.gyro_integral_rad[0], &sensors.accelerometer_integral_m_s[0]); + float gyro_integral[3]; + gyro_integral[0] = sensors.gyro_rad[0] * sensors.gyro_integral_dt; + gyro_integral[1] = sensors.gyro_rad[1] * sensors.gyro_integral_dt; + gyro_integral[2] = sensors.gyro_rad[2] * sensors.gyro_integral_dt; + float accel_integral[3]; + accel_integral[0] = sensors.accelerometer_m_s2[0] * sensors.accelerometer_integral_dt; + accel_integral[1] = sensors.accelerometer_m_s2[1] * sensors.accelerometer_integral_dt; + accel_integral[2] = sensors.accelerometer_m_s2[2] * sensors.accelerometer_integral_dt; + _ekf.setIMUData(now, sensors.gyro_integral_dt * 1.e6f, sensors.accelerometer_integral_dt * 1.e6f, + gyro_integral, accel_integral); // read mag data - _ekf.setMagData(sensors.magnetometer_timestamp[0], &sensors.magnetometer_ga[0]); + if (sensors.magnetometer_timestamp_relative == sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) { + _ekf.setMagData(0, sensors.magnetometer_ga); + } else { + _ekf.setMagData(sensors.timestamp + sensors.magnetometer_timestamp_relative, sensors.magnetometer_ga); + } // read baro data - _ekf.setBaroData(sensors.baro_timestamp[0], &sensors.baro_alt_meter[0]); + if (sensors.baro_timestamp_relative == sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) { + _ekf.setBaroData(0, &sensors.baro_alt_meter); + } else { + _ekf.setBaroData(sensors.timestamp + sensors.baro_timestamp_relative, &sensors.baro_alt_meter); + } // read gps data if available if (gps_updated) { struct gps_message gps_msg = {}; - gps_msg.time_usec = gps.timestamp_position; + gps_msg.time_usec = gps.timestamp; gps_msg.lat = gps.lat; gps_msg.lon = gps.lon; gps_msg.alt = gps.alt; @@ -463,7 +544,6 @@ void Ekf2::task_main() gps_msg.eph = gps.eph; gps_msg.epv = gps.epv; gps_msg.sacc = gps.s_variance_m_s; - gps_msg.time_usec_vel = gps.timestamp_velocity; gps_msg.vel_m_s = gps.vel_m_s; gps_msg.vel_ned[0] = gps.vel_n_m_s; gps_msg.vel_ned[1] = gps.vel_e_m_s; @@ -473,13 +553,15 @@ void Ekf2::task_main() //TODO add gdop to gps topic gps_msg.gdop = 0.0f; - _ekf.setGpsData(gps.timestamp_position, &gps_msg); + _ekf.setGpsData(gps.timestamp, &gps_msg); } - // read airspeed data if available - float eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s; + // only set airspeed data if condition for airspeed fusion are met + bool fuse_airspeed = airspeed_updated && !_vehicle_status.is_rotary_wing + && _arspFusionThreshold.get() <= airspeed.true_airspeed_m_s && _arspFusionThreshold.get() >= 0.1f; - if (airspeed_updated && airspeed.true_airspeed_m_s > 7.0f) { + if (fuse_airspeed) { + float eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s; _ekf.setAirspeedData(airspeed.timestamp, &airspeed.true_airspeed_m_s, &eas2tas); } @@ -503,17 +585,39 @@ void Ekf2::task_main() _ekf.setRangeData(range_finder.timestamp, &range_finder.current_distance); } - orb_check(_actuator_armed_sub, &actuator_armed_updated); + // get external vision data + // if error estimates are unavailable, use parameter defined defaults + if (vision_position_updated) { + ext_vision_message ev_data; + ev_data.posNED(0) = ev.x; + ev_data.posNED(1) = ev.y; + ev_data.posNED(2) = ev.z; + Quaternion q(ev.q); + ev_data.quat = q; - if (actuator_armed_updated) { - orb_copy(ORB_ID(actuator_armed), _actuator_armed_sub, &actuator_armed); - _ekf.set_arm_status(actuator_armed.armed); + // position measurement error + if (ev.pos_err >= 0.001f) { + ev_data.posErr = ev.pos_err; + + } else { + ev_data.posErr = _default_ev_pos_noise; + } + + // angle measurement error + if (ev.ang_err >= 0.001f) { + ev_data.angErr = ev.ang_err; + + } else { + ev_data.angErr = _default_ev_ang_noise; + } + + // use timestamp from external computer, clocks are synchronized when using MAVROS + _ekf.setExtVisionData(ev.timestamp, &ev_data); } orb_check(_vehicle_land_detected_sub, &vehicle_land_detected_updated); if (vehicle_land_detected_updated) { - struct vehicle_land_detected_s vehicle_land_detected = {}; orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &vehicle_land_detected); _ekf.set_in_air_status(!vehicle_land_detected.landed); } @@ -527,10 +631,16 @@ void Ekf2::task_main() // generate control state data control_state_s ctrl_state = {}; + float gyro_bias[3] = {}; + _ekf.get_gyro_bias(gyro_bias); ctrl_state.timestamp = hrt_absolute_time(); - ctrl_state.roll_rate = _lp_roll_rate.apply(sensors.gyro_rad_s[0]); - ctrl_state.pitch_rate = _lp_pitch_rate.apply(sensors.gyro_rad_s[1]); - ctrl_state.yaw_rate = _lp_yaw_rate.apply(sensors.gyro_rad_s[2]); + float gyro_rad[3]; + gyro_rad[0] = sensors.gyro_rad[0] - gyro_bias[0]; + gyro_rad[1] = sensors.gyro_rad[1] - gyro_bias[1]; + gyro_rad[2] = sensors.gyro_rad[2] - gyro_bias[2]; + ctrl_state.roll_rate = _lp_roll_rate.apply(gyro_rad[0]); + ctrl_state.pitch_rate = _lp_pitch_rate.apply(gyro_rad[1]); + ctrl_state.yaw_rate = _lp_yaw_rate.apply(gyro_rad[2]); // Velocity in body frame float velocity[3]; @@ -557,13 +667,13 @@ void Ekf2::task_main() ctrl_state.q[3] = q(3); // Acceleration data - matrix::Vector acceleration = {&sensors.accelerometer_m_s2[0]}; + matrix::Vector acceleration(sensors.accelerometer_m_s2); - float accel_bias = 0.0f; - _ekf.get_accel_bias(&accel_bias); - ctrl_state.x_acc = acceleration(0); - ctrl_state.y_acc = acceleration(1); - ctrl_state.z_acc = acceleration(2) - accel_bias; + float accel_bias[3]; + _ekf.get_accel_bias(accel_bias); + ctrl_state.x_acc = acceleration(0) - accel_bias[0]; + ctrl_state.y_acc = acceleration(1) - accel_bias[1]; + ctrl_state.z_acc = acceleration(2) - accel_bias[2]; // compute lowpass filtered horizontal acceleration acceleration = R_to_body.transpose() * acceleration; @@ -571,14 +681,29 @@ void Ekf2::task_main() 1) * acceleration(1)); ctrl_state.horz_acc_mag = _acc_hor_filt; - // Airspeed - take airspeed measurement directly here as no wind is estimated - if (PX4_ISFINITE(airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - airspeed.timestamp < 1e6 - && airspeed.timestamp > 0) { - ctrl_state.airspeed = airspeed.indicated_airspeed_m_s; - ctrl_state.airspeed_valid = true; + float vel[3] = {}; + _ekf.get_velocity(vel); - } else { - ctrl_state.airspeed_valid = false; + ctrl_state.airspeed_valid = false; + + // use estimated velocity for airspeed estimate + if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_MEAS) { + // use measured airspeed + if (PX4_ISFINITE(airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - airspeed.timestamp < 1e6 + && airspeed.timestamp > 0) { + ctrl_state.airspeed = airspeed.indicated_airspeed_m_s; + ctrl_state.airspeed_valid = true; + } + + } else if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_EST) { + if (_ekf.local_position_is_valid()) { + ctrl_state.airspeed = sqrtf(vel[0] * vel[0] + vel[1] * vel[1] + vel[2] * vel[2]); + ctrl_state.airspeed_valid = true; + } + + } else if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_DISABLED) { + // do nothing, airspeed has been declared as non-valid above, controllers + // will handle this assuming always trim airspeed } // publish control state data @@ -603,9 +728,9 @@ void Ekf2::task_main() att.q[3] = q(3); att.q_valid = true; - att.rollspeed = sensors.gyro_rad_s[0]; - att.pitchspeed = sensors.gyro_rad_s[1]; - att.yawspeed = sensors.gyro_rad_s[2]; + att.rollspeed = gyro_rad[0]; + att.pitchspeed = gyro_rad[1]; + att.yawspeed = gyro_rad[2]; // publish vehicle attitude data if (_att_pub == nullptr) { @@ -618,18 +743,16 @@ void Ekf2::task_main() // generate vehicle local position data struct vehicle_local_position_s lpos = {}; float pos[3] = {}; - float vel[3] = {}; lpos.timestamp = hrt_absolute_time(); // Position of body origin in local NED frame _ekf.get_position(pos); - lpos.x = pos[0]; - lpos.y = pos[1]; + lpos.x = (_ekf.local_position_is_valid()) ? pos[0] : 0.0f; + lpos.y = (_ekf.local_position_is_valid()) ? pos[1] : 0.0f; lpos.z = pos[2]; // Velocity of body origin in local NED frame (m/s) - _ekf.get_velocity(vel); lpos.vx = vel[0]; lpos.vy = vel[1]; lpos.vz = vel[2]; @@ -696,13 +819,18 @@ void Ekf2::task_main() global_pos.eph = sqrt(pos_var(0) + pos_var(1));; // Standard deviation of position estimate horizontally global_pos.epv = sqrt(pos_var(2)); // Standard deviation of position vertically - // TODO: implement terrain estimator - global_pos.terrain_alt = 0.0f; // Terrain altitude in m, WGS84 - global_pos.terrain_alt_valid = false; // Terrain altitude estimate is valid + if (lpos.dist_bottom_valid) { + global_pos.terrain_alt = lpos.ref_alt - terrain_vpos; // Terrain altitude in m, WGS84 + global_pos.terrain_alt_valid = true; // Terrain altitude estimate is valid + } else { + global_pos.terrain_alt = 0.0f; // Terrain altitude in m, WGS84 + global_pos.terrain_alt_valid = false; // Terrain altitude estimate is valid + } + // TODO use innovatun consistency check timouts to set this global_pos.dead_reckoning = false; // True if this position is estimated through dead-reckoning - global_pos.pressure_alt = sensors.baro_alt_meter[0]; // Pressure altitude AMSL (m) + global_pos.pressure_alt = sensors.baro_alt_meter; // Pressure altitude AMSL (m) if (_vehicle_global_position_pub == nullptr) { _vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); @@ -733,6 +861,7 @@ void Ekf2::task_main() _ekf.get_covariances(status.covariances); _ekf.get_gps_check_status(&status.gps_check_fail_flags); _ekf.get_control_mode(&status.control_mode_flags); + _ekf.get_filter_fault_status(&status.filter_fault_flags); if (_estimator_status_pub == nullptr) { _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &status); @@ -780,33 +909,34 @@ void Ekf2::task_main() orb_publish(ORB_ID(ekf2_innovations), _estimator_innovations_pub, &innovations); } - // save the declination to the EKF2_MAG_DECL parameter when a dis-arm event is detected - if ((_params->mag_declination_source & (1 << 1)) && _prev_motors_armed && !actuator_armed.armed) { + // save the declination to the EKF2_MAG_DECL parameter when a land event is detected + if ((_params->mag_declination_source & (1 << 1)) && !_prev_landed && vehicle_land_detected.landed) { float decl_deg; _ekf.copy_mag_decl_deg(&decl_deg); _mag_declination_deg.set(decl_deg); } + _prev_landed = vehicle_land_detected.landed; + // publish replay message if in replay mode bool publish_replay_message = (bool)_param_record_replay_msg.get(); if (publish_replay_message) { struct ekf2_replay_s replay = {}; replay.time_ref = now; - replay.gyro_integral_dt = sensors.gyro_integral_dt[0]; - replay.accelerometer_integral_dt = sensors.accelerometer_integral_dt[0]; - replay.magnetometer_timestamp = sensors.magnetometer_timestamp[0]; - replay.baro_timestamp = sensors.baro_timestamp[0]; - memcpy(&replay.gyro_integral_rad[0], &sensors.gyro_integral_rad[0], sizeof(replay.gyro_integral_rad)); - memcpy(&replay.accelerometer_integral_m_s[0], &sensors.accelerometer_integral_m_s[0], - sizeof(replay.accelerometer_integral_m_s)); - memcpy(&replay.magnetometer_ga[0], &sensors.magnetometer_ga[0], sizeof(replay.magnetometer_ga)); - replay.baro_alt_meter = sensors.baro_alt_meter[0]; + replay.gyro_integral_dt = sensors.gyro_integral_dt; + replay.accelerometer_integral_dt = sensors.accelerometer_integral_dt; + replay.magnetometer_timestamp = sensors.timestamp + sensors.magnetometer_timestamp_relative; + replay.baro_timestamp = sensors.timestamp + sensors.baro_timestamp_relative; + memcpy(replay.gyro_rad, sensors.gyro_rad, sizeof(replay.gyro_rad)); + memcpy(replay.accelerometer_m_s2, sensors.accelerometer_m_s2, sizeof(replay.accelerometer_m_s2)); + memcpy(replay.magnetometer_ga, sensors.magnetometer_ga, sizeof(replay.magnetometer_ga)); + replay.baro_alt_meter = sensors.baro_alt_meter; // only write gps data if we had a gps update. if (gps_updated) { - replay.time_usec = gps.timestamp_position; - replay.time_usec_vel = gps.timestamp_velocity; + replay.time_usec = gps.timestamp; + replay.time_usec_vel = gps.timestamp; replay.lat = gps.lat; replay.lon = gps.lon; replay.alt = gps.alt; @@ -851,14 +981,26 @@ void Ekf2::task_main() replay.asp_timestamp = airspeed.timestamp; replay.indicated_airspeed_m_s = airspeed.indicated_airspeed_m_s; replay.true_airspeed_m_s = airspeed.true_airspeed_m_s; - replay.true_airspeed_unfiltered_m_s = airspeed.true_airspeed_unfiltered_m_s; - replay.air_temperature_celsius = airspeed.air_temperature_celsius; - replay.confidence = airspeed.confidence; } else { replay.asp_timestamp = 0; } + if (vision_position_updated) { + replay.ev_timestamp = ev.timestamp; + replay.pos_ev[0] = ev.x; + replay.pos_ev[1] = ev.y; + replay.pos_ev[2] = ev.z; + replay.quat_ev[0] = ev.q[0]; + replay.quat_ev[1] = ev.q[1]; + replay.quat_ev[2] = ev.q[2]; + replay.quat_ev[3] = ev.q[3]; + replay.pos_err = ev.pos_err; + replay.ang_err = ev.ang_err; + + } else { + replay.ev_timestamp = 0; + } if (_replay_pub == nullptr) { _replay_pub = orb_advertise(ORB_ID(ekf2_replay), &replay); @@ -882,11 +1024,19 @@ int Ekf2::start() { ASSERT(_control_task == -1); +#ifdef __PX4_QURT + // On the DSP we seem to get random crashes with a stack size below 13000. + const unsigned stack_size = 15000; +#else + const unsigned stack_size = 8500; +#endif + + /* start the task */ _control_task = px4_task_spawn_cmd("ekf2", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 9000, + stack_size, (px4_main_t)&Ekf2::task_main_trampoline, nullptr); diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index 98c96ba20d..25c48086ea 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -108,23 +108,43 @@ PARAM_DEFINE_FLOAT(EKF2_RNG_DELAY, 5); */ PARAM_DEFINE_FLOAT(EKF2_ASP_DELAY, 200); +/** + * Vision Position Estimator delay relative to IMU measurements + * + * @group EKF2 + * @min 0 + * @max 300 + * @unit ms + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_EV_DELAY, 175); + /** * Integer bitmask controlling GPS checks. - * + * * Set bits to 1 to enable checks. Checks enabled by the following bit positions * 0 : Minimum required sat count set by EKF2_REQ_NSATS * 1 : Minimum required GDoP set by EKF2_REQ_GDOP * 2 : Maximum allowed horizontal position error set by EKF2_REQ_EPH * 3 : Maximum allowed vertical position error set by EKF2_REQ_EPV * 4 : Maximum allowed speed error set by EKF2_REQ_SACC - * 5 : Maximum allowed horizontal position rate set by EKF2_REQ_HDRIFT. This check can only be used if the vehciel is stationary during alignment. - * 6 : Maximum allowed vertical position rate set by EKF2_REQ_VDRIFT. This check can only be used if the vehciel is stationary during alignment. - * 7 : Maximum allowed horizontal speed set by EKF2_REQ_HDRIFT. This check can only be used if the vehciel is stationary during alignment. + * 5 : Maximum allowed horizontal position rate set by EKF2_REQ_HDRIFT. This check can only be used if the vehicle is stationary during alignment. + * 6 : Maximum allowed vertical position rate set by EKF2_REQ_VDRIFT. This check can only be used if the vehicle is stationary during alignment. + * 7 : Maximum allowed horizontal speed set by EKF2_REQ_HDRIFT. This check can only be used if the vehicle is stationary during alignment. * 8 : Maximum allowed vertical velocity discrepancy set by EKF2_REQ_VDRIFT * * @group EKF2 * @min 0 * @max 511 + * @bit 0 Min sat count (EKF2_REQ_NSATS) + * @bit 1 Min GDoP (EKF2_REQ_GDOP) + * @bit 2 Max horizontal position error (EKF2_REQ_EPH) + * @bit 3 Max vertical position error (EKF2_REQ_EPV) + * @bit 4 Max speed error (EKF2_REQ_SACC) + * @bit 5 Max horizontal position rate (EKF2_REQ_HDRIFT) + * @bit 6 Max vertical position rate (EKF2_REQ_VDRIFT) + * @bit 7 Max horizontal speed (EKF2_REQ_HDRIFT) + * @bit 8 Max vertical velocity discrepancy (EKF2_REQ_VDRIFT) */ PARAM_DEFINE_INT32(EKF2_GPS_CHECK, 21); @@ -211,7 +231,7 @@ PARAM_DEFINE_FLOAT(EKF2_REQ_VDRIFT, 0.5f); * @unit rad/s * @decimal 4 */ -PARAM_DEFINE_FLOAT(EKF2_GYR_NOISE, 6.0e-2f); +PARAM_DEFINE_FLOAT(EKF2_GYR_NOISE, 1.5e-2f); /** * Accelerometer noise for covariance prediction. @@ -222,39 +242,29 @@ PARAM_DEFINE_FLOAT(EKF2_GYR_NOISE, 6.0e-2f); * @unit m/s/s * @decimal 2 */ -PARAM_DEFINE_FLOAT(EKF2_ACC_NOISE, 0.25f); +PARAM_DEFINE_FLOAT(EKF2_ACC_NOISE, 3.5e-1f); /** - * Process noise for delta angle bias prediction. - * - * @group EKF2 - * @min 0.0 - * @max 0.0001 - * @unit rad/s - * @decimal 8 - */ -PARAM_DEFINE_FLOAT(EKF2_GYR_B_NOISE, 2.5e-6f); - -/** - * Process noise for delta velocity z bias prediction. - * - * @group EKF2 - * @min 0.0 - * @max 0.01 - * @unit m/s/s - * @decimal 7 - */ -PARAM_DEFINE_FLOAT(EKF2_ACC_B_NOISE, 3.0e-5f); - -/** - * Process noise for delta angle scale factor prediction. + * Process noise for IMU rate gyro bias prediction. * * @group EKF2 * @min 0.0 * @max 0.01 + * @unit rad/s**2 * @decimal 6 */ -PARAM_DEFINE_FLOAT(EKF2_GYR_S_NOISE, 3.0e-4f); +PARAM_DEFINE_FLOAT(EKF2_GYR_B_NOISE, 1.0e-3f); + +/** + * Process noise for IMU accelerometer bias prediction. + * + * @group EKF2 + * @min 0.0 + * @max 0.01 + * @unit m/s**3 + * @decimal 6 + */ +PARAM_DEFINE_FLOAT(EKF2_ACC_B_NOISE, 3.0e-3f); /** * Process noise for body magnetic field prediction. @@ -265,7 +275,7 @@ PARAM_DEFINE_FLOAT(EKF2_GYR_S_NOISE, 3.0e-4f); * @unit Gauss/s * @decimal 6 */ -PARAM_DEFINE_FLOAT(EKF2_MAG_B_NOISE, 5.0e-4f); +PARAM_DEFINE_FLOAT(EKF2_MAG_B_NOISE, 1.0e-4f); /** * Process noise for earth magnetic field prediction. @@ -276,7 +286,7 @@ PARAM_DEFINE_FLOAT(EKF2_MAG_B_NOISE, 5.0e-4f); * @unit Gauss/s * @decimal 6 */ -PARAM_DEFINE_FLOAT(EKF2_MAG_E_NOISE, 2.5e-3f); +PARAM_DEFINE_FLOAT(EKF2_MAG_E_NOISE, 1.0e-3f); /** * Process noise for wind velocity prediction. @@ -331,7 +341,7 @@ PARAM_DEFINE_FLOAT(EKF2_NOAID_NOISE, 10.0f); * @unit m * @decimal 2 */ -PARAM_DEFINE_FLOAT(EKF2_BARO_NOISE, 3.0f); +PARAM_DEFINE_FLOAT(EKF2_BARO_NOISE, 2.0f); /** * Measurement noise for magnetic heading fusion. @@ -364,7 +374,7 @@ PARAM_DEFINE_FLOAT(EKF2_MAG_NOISE, 5.0e-2f); * @unit m/s * @decimal 1 */ - PARAM_DEFINE_FLOAT(EKF2_EAS_NOISE, 1.4f); +PARAM_DEFINE_FLOAT(EKF2_EAS_NOISE, 1.4f); /** * Magnetic declination @@ -401,11 +411,14 @@ PARAM_DEFINE_FLOAT(EKF2_MAG_GATE, 3.0f); * Set bits in the following positions to enable functions. * 0 : Set to true to use the declination from the geo_lookup library when the GPS position becomes available, set to false to always use the EKF2_MAG_DECL value. * 1 : Set to true to save the EKF2_MAG_DECL parameter to the value returned by the EKF when the vehicle disarms. - * 2 : Set to true to always use the declination as an observaton when 3-axis magnetometer fusion is being used. + * 2 : Set to true to always use the declination as an observation when 3-axis magnetometer fusion is being used. * * @group EKF2 * @min 0 * @max 7 + * @bit 0 use geo_lookup declination + * @bit 1 save EKF2_MAG_DECL on disarm + * @bit 2 use declination as an observation */ PARAM_DEFINE_INT32(EKF2_DECL_TYPE, 7); @@ -414,13 +427,12 @@ PARAM_DEFINE_INT32(EKF2_DECL_TYPE, 7); * * Integer controlling the type of magnetometer fusion used - magnetic heading or 3-axis magnetometer. * If set to automatic: heading fusion on-ground and 3-axis fusion in-flight - * + * * @group EKF2 * @value 0 Automatic * @value 1 Magnetic heading * @value 2 3-axis fusion - * @value 3 2-D projection - * @value 4 None + * @value 3 None */ PARAM_DEFINE_INT32(EKF2_MAG_TYPE, 0); @@ -454,6 +466,16 @@ PARAM_DEFINE_FLOAT(EKF2_GPS_P_GATE, 5.0f); */ PARAM_DEFINE_FLOAT(EKF2_GPS_V_GATE, 5.0f); +/** + * Gate size for TAS fusion + * + * @group EKF2 + * @min 1.0 + * @unit SD + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_TAS_GATE, 3.0f); + /** * Replay mode * @@ -466,15 +488,23 @@ PARAM_DEFINE_FLOAT(EKF2_GPS_V_GATE, 5.0f); PARAM_DEFINE_INT32(EKF2_REC_RPL, 0); /** - * Integer bitmask controlling which external aiding sources will be used. + * Integer bitmask controlling data fusion and aiding methods. * * Set bits in the following positions to enable: * 0 : Set to true to use GPS data if available * 1 : Set to true to use optical flow data if available + * 2 : Set to true to inhibit IMU bias estimation + * 3 : Set to true to enable vision position fusion + * 4 : Set to true to enable vision yaw fusion * * @group EKF2 * @min 0 - * @max 3 + * @max 28 + * @bit 0 use GPS + * @bit 1 use optical flow + * @bit 2 inhibit IMU bias estimation + * @bit 3 vision position fusion + * @bit 4 vision yaw fusion */ PARAM_DEFINE_INT32(EKF2_AID_MASK, 1); @@ -485,8 +515,10 @@ PARAM_DEFINE_INT32(EKF2_AID_MASK, 1); * * @group EKF2 * @value 0 Barometric pressure - * @value 1 Reserved (GPS) + * @value 1 GPS * @value 2 Range sensor + * @value 3 Vision + * */ PARAM_DEFINE_INT32(EKF2_HGT_MODE, 0); @@ -520,6 +552,46 @@ PARAM_DEFINE_FLOAT(EKF2_RNG_GATE, 5.0f); */ PARAM_DEFINE_FLOAT(EKF2_MIN_RNG, 0.1f); + +/** + * Measurement noise for vision position observations used when the vision system does not supply error estimates + * + * @group EKF2 + * @min 0.01 + * @unit m + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_EVP_NOISE, 0.05f); + +/** + * Measurement noise for vision angle observations used when the vision system does not supply error estimates + * + * @group EKF2 + * @min 0.01 + * @unit rad + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_EVA_NOISE, 0.05f); + +/** + * Gate size for vision estimate fusion + * + * @group EKF2 + * @min 1.0 + * @unit SD + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_EV_GATE, 5.0f); + +/** + * Minimum valid range for the vision estimate + * + * @group EKF2 + * @min 0.01 + * @unit m + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_MIN_EV, 0.01f); /** * Measurement noise for the optical flow sensor when it's reported quality metric is at the maximum * @@ -699,3 +771,96 @@ PARAM_DEFINE_FLOAT(EKF2_OF_POS_Y, 0.0f); * @decimal 3 */ PARAM_DEFINE_FLOAT(EKF2_OF_POS_Z, 0.0f); + +/** +* X position of VI sensor focal point in body frame + * + * @group EKF2 + * @unit m + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(EKF2_EV_POS_X, 0.0f); + +/** + * Y position of VI sensor focal point in body frame + * + * @group EKF2 + * @unit m + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(EKF2_EV_POS_Y, 0.0f); + +/** + * Z position of VI sensor focal point in body frame + * + * @group EKF2 + * @unit m + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(EKF2_EV_POS_Z, 0.0f); + +/** +* Airspeed fusion threshold. A value of zero will deactivate airspeed fusion. Any other positive +* value will determine the minimum airspeed which will still be fused. +* +* @group EKF2 +* @min 0.0 +* @unit m/s +* @decimal 1 +*/ +PARAM_DEFINE_FLOAT(EKF2_ARSP_THR, 0.0f); + +/** + + * Time constant of the velocity output prediction and smoothing filter + * + * @group EKF2 + * @max 1.0 + * @unit s + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_TAU_VEL, 0.5f); + +/** + * Time constant of the position output prediction and smoothing filter. Controls how tightly the output track the EKF states. + * + * @group EKF2 + * @min 0.1 + * @max 1.0 + * @unit s + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_TAU_POS, 0.25f); + +/** + * 1-sigma IMU gyro switch-on bias + * + * @group EKF2 + * @min 0.0 + * @max 0.2 + * @unit rad/sec + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_GBIAS_INIT, 0.1f); + +/** + * 1-sigma IMU accelerometer switch-on bias + * + * @group EKF2 + * @min 0.0 + * @max 0.5 + * @unit m/s/s + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_ABIAS_INIT, 0.2f); + +/** + * 1-sigma tilt angle uncertainty after gravity vector alignment + * + * @group EKF2 + * @min 0.0 + * @max 0.5 + * @unit rad + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(EKF2_ANGERR_INIT, 0.1f); diff --git a/src/modules/ekf2_replay/ekf2_replay_main.cpp b/src/modules/ekf2_replay/ekf2_replay_main.cpp index abe3a548ab..e175662b04 100644 --- a/src/modules/ekf2_replay/ekf2_replay_main.cpp +++ b/src/modules/ekf2_replay/ekf2_replay_main.cpp @@ -69,6 +69,8 @@ #include #include #include +#include +#include #include @@ -133,6 +135,9 @@ private: orb_advert_t _landed_pub; orb_advert_t _flow_pub; orb_advert_t _range_pub; + orb_advert_t _airspeed_pub; + orb_advert_t _ev_pub; + orb_advert_t _vehicle_status_pub; int _att_sub; int _estimator_status_sub; @@ -148,12 +153,17 @@ private: struct vehicle_land_detected_s _land_detected; struct optical_flow_s _flow; struct distance_sensor_s _range; + struct airspeed_s _airspeed; + struct vision_position_estimate_s _ev; + struct vehicle_status_s _vehicle_status; unsigned _message_counter; // counter which will increase with every message read from the log unsigned _part1_counter_ref; // this is the value of _message_counter when the part1 of the replay message is read (imu data) bool _read_part2; // indicates if part 2 of replay message has been read bool _read_part3; bool _read_part4; + bool _read_part6; + bool _read_part5; int _write_fd = -1; px4_pollfd_struct_t _fds[1]; @@ -201,6 +211,9 @@ Ekf2Replay::Ekf2Replay(char *logfile) : _landed_pub(nullptr), _flow_pub(nullptr), _range_pub(nullptr), + _airspeed_pub(nullptr), + _ev_pub(nullptr), + _vehicle_status_pub(nullptr), _att_sub(-1), _estimator_status_sub(-1), _innov_sub(-1), @@ -212,11 +225,15 @@ Ekf2Replay::Ekf2Replay(char *logfile) : _land_detected{}, _flow{}, _range{}, + _airspeed{}, + _vehicle_status{}, _message_counter(0), _part1_counter_ref(0), _read_part2(false), _read_part3(false), _read_part4(false), + _read_part6(false), + _read_part5(false), _write_fd(-1) { // build the path to the log @@ -264,12 +281,30 @@ void Ekf2Replay::publishEstimatorInput() _read_part4 = false; + if (_ev_pub == nullptr && _read_part5) { + _ev_pub = orb_advertise(ORB_ID(vision_position_estimate), &_ev); + + } else if (_ev_pub != nullptr && _read_part5) { + orb_publish(ORB_ID(vision_position_estimate), _ev_pub, &_ev); + } + + _read_part5 = false; + if (_sensors_pub == nullptr) { _sensors_pub = orb_advertise(ORB_ID(sensor_combined), &_sensors); } else if (_sensors_pub != nullptr) { orb_publish(ORB_ID(sensor_combined), _sensors_pub, &_sensors); } + + if (_airspeed_pub == nullptr && _read_part6) { + _airspeed_pub = orb_advertise(ORB_ID(airspeed), &_airspeed); + + } else if (_airspeed_pub != nullptr) { + orb_publish(ORB_ID(airspeed), _airspeed_pub, &_airspeed); + } + + _read_part6 = false; } void Ekf2Replay::parseMessage(uint8_t *source, uint8_t *destination, uint8_t type) @@ -332,34 +367,36 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type) struct log_RPL2_s replay_part2 = {}; struct log_RPL3_s replay_part3 = {}; struct log_RPL4_s replay_part4 = {}; + struct log_RPL6_s replay_part6 = {}; + struct log_RPL5_s replay_part5 = {}; struct log_LAND_s vehicle_landed = {}; + struct log_STAT_s vehicle_status = {}; if (type == LOG_RPL1_MSG) { uint8_t *dest_ptr = (uint8_t *)&replay_part1.time_ref; parseMessage(data, dest_ptr, type); _sensors.timestamp = replay_part1.time_ref; - _sensors.gyro_integral_dt[0] = replay_part1.gyro_integral_dt; - _sensors.accelerometer_integral_dt[0] = replay_part1.accelerometer_integral_dt; - _sensors.magnetometer_timestamp[0] = replay_part1.magnetometer_timestamp; - _sensors.baro_timestamp[0] = replay_part1.baro_timestamp; - _sensors.gyro_integral_rad[0] = replay_part1.gyro_integral_x_rad; - _sensors.gyro_integral_rad[1] = replay_part1.gyro_integral_y_rad; - _sensors.gyro_integral_rad[2] = replay_part1.gyro_integral_z_rad; - _sensors.accelerometer_integral_m_s[0] = replay_part1.accelerometer_integral_x_m_s; - _sensors.accelerometer_integral_m_s[1] = replay_part1.accelerometer_integral_y_m_s; - _sensors.accelerometer_integral_m_s[2] = replay_part1.accelerometer_integral_z_m_s; + _sensors.gyro_integral_dt = replay_part1.gyro_integral_dt; + _sensors.accelerometer_integral_dt = replay_part1.accelerometer_integral_dt; + _sensors.magnetometer_timestamp_relative = (int32_t)(replay_part1.magnetometer_timestamp - _sensors.timestamp); + _sensors.baro_timestamp_relative = (int32_t)(replay_part1.baro_timestamp - _sensors.timestamp); + _sensors.gyro_rad[0] = replay_part1.gyro_x_rad; + _sensors.gyro_rad[1] = replay_part1.gyro_y_rad; + _sensors.gyro_rad[2] = replay_part1.gyro_z_rad; + _sensors.accelerometer_m_s2[0] = replay_part1.accelerometer_x_m_s2; + _sensors.accelerometer_m_s2[1] = replay_part1.accelerometer_y_m_s2; + _sensors.accelerometer_m_s2[2] = replay_part1.accelerometer_z_m_s2; _sensors.magnetometer_ga[0] = replay_part1.magnetometer_x_ga; _sensors.magnetometer_ga[1] = replay_part1.magnetometer_y_ga; _sensors.magnetometer_ga[2] = replay_part1.magnetometer_z_ga; - _sensors.baro_alt_meter[0] = replay_part1.baro_alt_meter; + _sensors.baro_alt_meter = replay_part1.baro_alt_meter; _part1_counter_ref = _message_counter; } else if (type == LOG_RPL2_MSG) { uint8_t *dest_ptr = (uint8_t *)&replay_part2.time_pos_usec; parseMessage(data, dest_ptr, type); - _gps.timestamp_position = replay_part2.time_pos_usec; - _gps.timestamp_velocity = replay_part2.time_vel_usec; + _gps.timestamp = replay_part2.time_pos_usec; _gps.lat = replay_part2.lat; _gps.lon = replay_part2.lon; _gps.fix_type = replay_part2.fix_type; @@ -394,6 +431,30 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type) _range.current_distance = replay_part4.range_to_ground; _read_part4 = true; + } else if (type == LOG_RPL6_MSG) { + uint8_t *dest_ptr = (uint8_t *)&replay_part6.time_airs_usec; + parseMessage(data, dest_ptr, type); + _airspeed.timestamp = replay_part6.time_airs_usec; + _airspeed.indicated_airspeed_m_s = replay_part6.indicated_airspeed_m_s; + _airspeed.true_airspeed_m_s = replay_part6.true_airspeed_m_s; + _read_part6 = true; + + } else if (type == LOG_RPL5_MSG) { + uint8_t *dest_ptr = (uint8_t *)&replay_part5.time_ev_usec; + parseMessage(data, dest_ptr, type); + _ev.timestamp = replay_part5.time_ev_usec; + _ev.timestamp_received = replay_part5.time_ev_usec; // fake this timestamp + _ev.x = replay_part5.x; + _ev.y = replay_part5.y; + _ev.z = replay_part5.z; + _ev.q[0] = replay_part5.q0; + _ev.q[1] = replay_part5.q1; + _ev.q[2] = replay_part5.q2; + _ev.q[3] = replay_part5.q3; + _ev.pos_err = replay_part5.pos_err; + _ev.ang_err = replay_part5.pos_err; + _read_part5 = true; + } else if (type == LOG_LAND_MSG) { uint8_t *dest_ptr = (uint8_t *)&vehicle_landed.landed; parseMessage(data, dest_ptr, type); @@ -406,6 +467,19 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type) orb_publish(ORB_ID(vehicle_land_detected), _landed_pub, &_land_detected); } } + + else if (type == LOG_STAT_MSG) { + uint8_t *dest_ptr = (uint8_t *)&vehicle_status.main_state; + parseMessage(data, dest_ptr, type); + _vehicle_status.is_rotary_wing = vehicle_status.is_rot_wing; + + if (_vehicle_status_pub == nullptr) { + _vehicle_status_pub = orb_advertise(ORB_ID(vehicle_status), &_vehicle_status); + + } else if (_vehicle_status_pub != nullptr) { + orb_publish(ORB_ID(vehicle_status), _vehicle_status_pub, &_vehicle_status); + } + } } void Ekf2Replay::writeMessage(int &fd, void *data, size_t size) @@ -518,7 +592,7 @@ void Ekf2Replay::logIfUpdated() memcpy(&(log_message.body.est0.s), est_status.states, maxcopy0); log_message.body.est0.n_states = est_status.n_states; log_message.body.est0.nan_flags = est_status.nan_flags; - log_message.body.est0.health_flags = est_status.health_flags; + log_message.body.est0.fault_flags = est_status.filter_fault_flags; log_message.body.est0.timeout_flags = est_status.timeout_flags; writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_EST0_MSG].length); @@ -582,6 +656,9 @@ void Ekf2Replay::logIfUpdated() log_message.body.innov2.s[6] = innov.heading_innov; log_message.body.innov2.s[7] = innov.heading_innov_var; + log_message.body.innov2.s[8] = innov.airspeed_innov; + log_message.body.innov2.s[9] = innov.airspeed_innov_var; + writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_EST5_MSG].length); // optical flow innovations and innovation variances diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index 04a8b7cb07..92a729a413 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -71,7 +71,6 @@ #include #include #include -#include #include "estimator_22states.h" #include @@ -191,8 +190,6 @@ private: float _filter_ref_offset; /**< offset between initial baro reference and GPS init baro altitude */ float _baro_gps_offset; /**< offset between baro altitude (at GPS init time) and GPS altitude */ hrt_abstime _last_debug_print = 0; - float _vibration_warning_threshold = 2.0f; - hrt_abstime _vibration_warning_timestamp = 0; perf_counter_t _loop_perf; ///< loop performance counter perf_counter_t _loop_intvl; ///< loop rate counter @@ -213,17 +210,10 @@ private: hrt_abstime _filter_start_time; hrt_abstime _last_sensor_timestamp; hrt_abstime _distance_last_valid; - DataValidatorGroup _voter_gyro; - DataValidatorGroup _voter_accel; - DataValidatorGroup _voter_mag; - int _gyro_main; ///< index of the main gyroscope - int _accel_main; ///< index of the main accelerometer - int _mag_main; ///< index of the main magnetometer bool _data_good; ///< all required filter data is ok - bool _failsafe; ///< failsafe on one of the sensors - bool _vibration_warning; ///< high vibration levels detected bool _ekf_logging; ///< log EKF state unsigned _debug; ///< debug level - default 0 + bool _was_landed; ///< indicates if was landed in previous iteration bool _newHgtData; bool _newAdsData; @@ -255,6 +245,7 @@ private: float magb_pnoise; float eas_noise; float pos_stddev_threshold; + int32_t airspeed_mode; } _parameters; /**< local copies of interesting parameters */ struct { @@ -276,6 +267,7 @@ private: param_t magb_pnoise; param_t eas_noise; param_t pos_stddev_threshold; + param_t airspeed_mode; } _parameter_handles; /**< handles for interesting parameters */ AttPosEKF *_ekf; diff --git a/src/modules/ekf_att_pos_estimator/CMakeLists.txt b/src/modules/ekf_att_pos_estimator/CMakeLists.txt index 1a24c4805d..4c0f4ae1a4 100644 --- a/src/modules/ekf_att_pos_estimator/CMakeLists.txt +++ b/src/modules/ekf_att_pos_estimator/CMakeLists.txt @@ -34,6 +34,7 @@ px4_add_module( MODULE modules__ekf_att_pos_estimator MAIN ekf_att_pos_estimator + COMPILE_FLAGS -Os STACK_MAIN 3000 STACK_MAX 3400 SRCS 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 ae452c01d3..92e49b3eab 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 @@ -205,17 +205,10 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _filter_start_time(0), _last_sensor_timestamp(hrt_absolute_time()), _distance_last_valid(0), - _voter_gyro(3), - _voter_accel(3), - _voter_mag(3), - _gyro_main(-1), - _accel_main(-1), - _mag_main(-1), _data_good(false), - _failsafe(false), - _vibration_warning(false), _ekf_logging(true), _debug(0), + _was_landed(true), _newHgtData(false), _newAdsData(false), @@ -235,8 +228,6 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _LP_att_Q(250.0f, 20.0f), _LP_att_R(250.0f, 20.0f) { - _voter_mag.set_timeout(200000); - _terrain_estimator = new TerrainEstimator(); _parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS"); @@ -257,6 +248,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _parameter_handles.magb_pnoise = param_find("PE_MAGB_PNOISE"); _parameter_handles.eas_noise = param_find("PE_EAS_NOISE"); _parameter_handles.pos_stddev_threshold = param_find("PE_POSDEV_INIT"); + _parameter_handles.airspeed_mode = param_find("FW_AIRSPD_MODE"); /* indicate consumers that the current position data is not valid */ _gps.eph = 10000.0f; @@ -324,6 +316,7 @@ int AttitudePositionEstimatorEKF::parameters_update() param_get(_parameter_handles.magb_pnoise, &(_parameters.magb_pnoise)); param_get(_parameter_handles.eas_noise, &(_parameters.eas_noise)); param_get(_parameter_handles.pos_stddev_threshold, &(_parameters.pos_stddev_threshold)); + param_get(_parameter_handles.airspeed_mode, &_parameters.airspeed_mode); if (_ekf) { // _ekf->yawVarScale = 1.0f; @@ -379,8 +372,8 @@ void AttitudePositionEstimatorEKF::vehicle_land_detected_poll() orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &_vehicle_land_detected); - // Save params on landed - if (!_vehicle_land_detected.landed) { + // Save params on landed and previously not landed + if (_vehicle_land_detected.landed && !_was_landed) { _mag_offset_x.set(_ekf->magBias.x); _mag_offset_x.commit(); _mag_offset_y.set(_ekf->magBias.y); @@ -388,6 +381,8 @@ void AttitudePositionEstimatorEKF::vehicle_land_detected_poll() _mag_offset_z.set(_ekf->magBias.z); _mag_offset_z.commit(); } + + _was_landed = _vehicle_land_detected.landed; } } @@ -443,7 +438,7 @@ int AttitudePositionEstimatorEKF::check_filter_state() // Set up height correctly orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); - initReferencePosition(_gps.timestamp_position, _gpsIsGood, lat, lon, gps_alt, _baro.altitude); + initReferencePosition(_gps.timestamp, _gpsIsGood, lat, lon, gps_alt, _baro.altitude); } else if (_ekf_logging) { _ekf->GetFilterState(&ekf_report); @@ -615,19 +610,9 @@ void AttitudePositionEstimatorEKF::task_main() /* system is in HIL now, wait for measurements to come in one last round */ usleep(60000); - /* HIL is slow, set permissive timeouts */ - _voter_gyro.set_timeout(500000); - _voter_accel.set_timeout(500000); - _voter_mag.set_timeout(500000); - /* now read all sensor publications to ensure all real sensor data is purged */ orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); - /* set sensors to de-initialized state */ - _gyro_main = -1; - _accel_main = -1; - _mag_main = -1; - _baro_init = false; _gps_initialized = false; @@ -659,7 +644,9 @@ void AttitudePositionEstimatorEKF::task_main() * We run the filter only once all data has been fetched **/ - if (_baro_init && (_gyro_main >= 0) && (_accel_main >= 0) && (_mag_main >= 0)) { + if (_baro_init && _sensor_combined.timestamp && + _sensor_combined.accelerometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID && + _sensor_combined.magnetometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) { // maintain filtered baro and gps altitudes to calculate weather offset // baro sample rate is ~70Hz and measurement bandwidth is high @@ -816,7 +803,7 @@ void AttitudePositionEstimatorEKF::initializeGPS() _ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); - initReferencePosition(_gps.timestamp_position, _gpsIsGood, lat, lon, gps_alt, _baro.altitude); + initReferencePosition(_gps.timestamp, _gpsIsGood, lat, lon, gps_alt, _baro.altitude); #if 0 PX4_INFO("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, @@ -917,17 +904,25 @@ void AttitudePositionEstimatorEKF::publishControlState() _ctrl_state.q[2] = _ekf->states[2]; _ctrl_state.q[3] = _ekf->states[3]; - /* Airspeed (Groundspeed - Windspeed) */ - //_ctrl_state.airspeed = sqrt(pow(_ekf->states[4] - _ekf->states[14], 2) + pow(_ekf->states[5] - _ekf->states[15], 2) + pow(_ekf->states[6], 2)); - // the line above was introduced by the control state PR. The airspeed it gives is totally wrong and leads to horrible flight performance in SITL - // and in outdoor tests - if (PX4_ISFINITE(_airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - _airspeed.timestamp < 1e6 - && _airspeed.timestamp > 0) { - _ctrl_state.airspeed = _airspeed.indicated_airspeed_m_s; - _ctrl_state.airspeed_valid = true; + // use estimated velocity for airspeed estimate + if (_parameters.airspeed_mode == control_state_s::AIRSPD_MODE_MEAS) { + // use measured airspeed + if (PX4_ISFINITE(_airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - _airspeed.timestamp < 1e6 + && _airspeed.timestamp > 0) { + _ctrl_state.airspeed = _airspeed.indicated_airspeed_m_s; + _ctrl_state.airspeed_valid = true; + } - } else { - _ctrl_state.airspeed_valid = false; + } else if (_parameters.airspeed_mode == control_state_s::AIRSPD_MODE_EST) { + if (_local_pos.v_xy_valid && _local_pos.v_z_valid) { + _ctrl_state.airspeed = sqrtf(_ekf->states[4] * _ekf->states[4] + + _ekf->states[5] * _ekf->states[5] + _ekf->states[6] * _ekf->states[6]); + _ctrl_state.airspeed_valid = true; + } + + } else if (_parameters.airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED) { + // do nothing, airspeed has been declared as non-valid above, controllers + // will handle this assuming always trim airspeed } /* Attitude Rates */ @@ -1055,7 +1050,7 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() if (!_local_pos.xy_global || !_local_pos.v_xy_valid || - _gps.timestamp_position == 0 || + _gps.timestamp == 0 || (dtLastGoodGPS >= POS_RESET_THRESHOLD)) { _global_pos.eph = EPH_LARGE_VALUE; @@ -1330,13 +1325,6 @@ void AttitudePositionEstimatorEKF::print_status() (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS", (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); - - PX4_INFO("gyro status:"); - _voter_gyro.print(); - PX4_INFO("accel status:"); - _voter_accel.print(); - PX4_INFO("mag status:"); - _voter_mag.print(); } void AttitudePositionEstimatorEKF::pollData() @@ -1369,112 +1357,37 @@ void AttitudePositionEstimatorEKF::pollData() /* fill in last data set */ _ekf->dtIMU = deltaT; - // Feed validator with recent sensor data + _ekf->angRate.x = _sensor_combined.gyro_rad[0]; + _ekf->angRate.y = _sensor_combined.gyro_rad[1]; + _ekf->angRate.z = _sensor_combined.gyro_rad[2]; - for (unsigned i = 0; i < (sizeof(_sensor_combined.gyro_timestamp) / sizeof(_sensor_combined.gyro_timestamp[0])); i++) { - _voter_gyro.put(i, _sensor_combined.gyro_timestamp[i], &_sensor_combined.gyro_rad_s[i * 3], - _sensor_combined.gyro_errcount[i], _sensor_combined.gyro_priority[i]); - _voter_accel.put(i, _sensor_combined.accelerometer_timestamp[i], &_sensor_combined.accelerometer_m_s2[i * 3], - _sensor_combined.accelerometer_errcount[i], _sensor_combined.accelerometer_priority[i]); - _voter_mag.put(i, _sensor_combined.magnetometer_timestamp[i], &_sensor_combined.magnetometer_ga[i * 3], - _sensor_combined.magnetometer_errcount[i], _sensor_combined.magnetometer_priority[i]); + float gyro_dt = _sensor_combined.gyro_integral_dt; + _ekf->dAngIMU = _ekf->angRate * gyro_dt; + + perf_count(_perf_gyro); + + if (_last_accel != _sensor_combined.timestamp + _sensor_combined.accelerometer_timestamp_relative) { + + _ekf->accel.x = _sensor_combined.accelerometer_m_s2[0]; + _ekf->accel.y = _sensor_combined.accelerometer_m_s2[1]; + _ekf->accel.z = _sensor_combined.accelerometer_m_s2[2]; + + float accel_dt = _sensor_combined.accelerometer_integral_dt; + _ekf->dVelIMU = _ekf->accel * accel_dt; + + _last_accel = _sensor_combined.timestamp + _sensor_combined.accelerometer_timestamp_relative; } - // Get best measurement values - hrt_abstime curr_time = hrt_absolute_time(); - (void)_voter_gyro.get_best(curr_time, &_gyro_main); + Vector3f mag(_sensor_combined.magnetometer_ga[0], _sensor_combined.magnetometer_ga[1], + _sensor_combined.magnetometer_ga[2]); - if (_gyro_main >= 0) { - - // Use pre-integrated values if possible - if (_sensor_combined.gyro_integral_dt[_gyro_main] > 0) { - _ekf->dAngIMU.x = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 0]; - _ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 1]; - _ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 2]; - - } else { - float dt_gyro = _sensor_combined.gyro_integral_dt[_gyro_main] / 1e6f; - - if (PX4_ISFINITE(dt_gyro) && (dt_gyro < 0.5f) && (dt_gyro > 0.00001f)) { - deltaT = dt_gyro; - _ekf->dtIMU = deltaT; - } - - _ekf->dAngIMU.x = 0.5f * (_ekf->angRate.x + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0]) * _ekf->dtIMU; - _ekf->dAngIMU.y = 0.5f * (_ekf->angRate.y + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 1]) * _ekf->dtIMU; - _ekf->dAngIMU.z = 0.5f * (_ekf->angRate.z + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2]) * _ekf->dtIMU; - } - - _ekf->angRate.x = _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0]; - _ekf->angRate.y = _sensor_combined.gyro_rad_s[_gyro_main * 3 + 1]; - _ekf->angRate.z = _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2]; - perf_count(_perf_gyro); - } - - (void)_voter_accel.get_best(curr_time, &_accel_main); - - if (_accel_main >= 0 && (_last_accel != _sensor_combined.accelerometer_timestamp[_accel_main])) { - - // Use pre-integrated values if possible - if (_sensor_combined.accelerometer_integral_dt[_accel_main] > 0) { - _ekf->dVelIMU.x = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 0]; - _ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 1]; - _ekf->dVelIMU.z = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 2]; - - } else { - _ekf->dVelIMU.x = 0.5f * (_ekf->accel.x + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]) * _ekf->dtIMU; - _ekf->dVelIMU.y = 0.5f * (_ekf->accel.y + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1]) * _ekf->dtIMU; - _ekf->dVelIMU.z = 0.5f * (_ekf->accel.z + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]) * _ekf->dtIMU; - } - - _ekf->accel.x = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]; - _ekf->accel.y = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1]; - _ekf->accel.z = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]; - _last_accel = _sensor_combined.accelerometer_timestamp[_accel_main]; - } - - (void)_voter_mag.get_best(curr_time, &_mag_main); - - if (_mag_main >= 0) { - Vector3f mag(_sensor_combined.magnetometer_ga[_mag_main * 3 + 0], _sensor_combined.magnetometer_ga[_mag_main * 3 + 1], - _sensor_combined.magnetometer_ga[_mag_main * 3 + 2]); - - /* fail over to the 2nd mag if we know the first is down */ - if (mag.length() > 0.1f && (_last_mag != _sensor_combined.magnetometer_timestamp[_mag_main])) { - _ekf->magData.x = mag.x; - _ekf->magData.y = mag.y; - _ekf->magData.z = mag.z; - _newDataMag = true; - _last_mag = _sensor_combined.magnetometer_timestamp[_mag_main]; - perf_count(_perf_mag); - } - } - - if (!_failsafe && (_voter_gyro.failover_count() > 0 || - _voter_accel.failover_count() > 0 || - _voter_mag.failover_count() > 0)) { - - _failsafe = true; - mavlink_and_console_log_emergency(&_mavlink_log_pub, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY"); - } - - if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold || - _voter_accel.get_vibration_factor(curr_time) > _vibration_warning_threshold || - _voter_mag.get_vibration_factor(curr_time) > _vibration_warning_threshold)) { - - if (_vibration_warning_timestamp == 0) { - _vibration_warning_timestamp = curr_time; - - } else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000000) { - _vibration_warning = true; - // mavlink_and_console_log_critical(&_mavlink_log_pub, "HIGH VIBRATION! g: %d a: %d m: %d", - // (int)(100 * _voter_gyro.get_vibration_factor(curr_time)), - // (int)(100 * _voter_accel.get_vibration_factor(curr_time)), - // (int)(100 * _voter_mag.get_vibration_factor(curr_time))); - } - - } else { - _vibration_warning_timestamp = 0; + if (mag.length() > 0.1f && _last_mag != _sensor_combined.timestamp + _sensor_combined.magnetometer_timestamp_relative) { + _ekf->magData.x = mag.x; + _ekf->magData.y = mag.y; + _ekf->magData.z = mag.z; + _newDataMag = true; + _last_mag = _sensor_combined.timestamp + _sensor_combined.magnetometer_timestamp_relative; + perf_count(_perf_mag); } _last_sensor_timestamp = _sensor_combined.timestamp; @@ -1483,8 +1396,8 @@ void AttitudePositionEstimatorEKF::pollData() // leave this in as long as larger improvements are still being made. #if 0 - float deltaTIntegral = (_sensor_combined.gyro_integral_dt[0]) / 1e6f; - float deltaTIntAcc = (_sensor_combined.accelerometer_integral_dt[0]) / 1e6f; + float deltaTIntegral = _sensor_combined.gyro_integral_dt; + float deltaTIntAcc = _sensor_combined.accelerometer_integral_dt; static unsigned dtoverflow5 = 0; static unsigned dtoverflow10 = 0; @@ -1500,25 +1413,14 @@ void AttitudePositionEstimatorEKF::pollData() (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z); PX4_WARN("INT: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f", - (double)(_sensor_combined.gyro_integral_rad[0]), (double)(_sensor_combined.gyro_integral_rad[2]), - (double)(_sensor_combined.accelerometer_integral_m_s[0]), - (double)(_sensor_combined.accelerometer_integral_m_s[1]), - (double)(_sensor_combined.accelerometer_integral_m_s[2])); - - PX4_WARN("DRV: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f", - (double)(_sensor_combined.gyro_rad_s[0] * deltaT), (double)(_sensor_combined.gyro_rad_s[2] * deltaT), - (double)(_sensor_combined.accelerometer_m_s2[0] * deltaT), - (double)(_sensor_combined.accelerometer_m_s2[1] * deltaT), - (double)(_sensor_combined.accelerometer_m_s2[2] * deltaT)); + (double)(_sensor_combined.gyro_rad[0]), (double)(_sensor_combined.gyro_rad[2]), + (double)(_sensor_combined.accelerometer_m_s2[0]), + (double)(_sensor_combined.accelerometer_m_s2[1]), + (double)(_sensor_combined.accelerometer_m_s2[2])); PX4_WARN("EKF rate: %8.4f, %8.4f, %8.4f", (double)_att.rollspeed, (double)_att.pitchspeed, (double)_att.yawspeed); - PX4_WARN("DRV rate: %8.4f, %8.4f, %8.4f", - (double)_sensor_combined.gyro_rad_s[0], - (double)_sensor_combined.gyro_rad_s[1], - (double)_sensor_combined.gyro_rad_s[2]); - lastprint = hrt_absolute_time(); } @@ -1571,7 +1473,7 @@ void AttitudePositionEstimatorEKF::pollData() if (_gpsIsGood) { //Calculate time since last good GPS fix - const float dtLastGoodGPS = static_cast(_gps.timestamp_position - _previousGPSTimestamp) / 1e6f; + const float dtLastGoodGPS = static_cast(_gps.timestamp - _previousGPSTimestamp) / 1e6f; //Stop dead-reckoning mode if (_global_pos.dead_reckoning) { @@ -1630,7 +1532,7 @@ void AttitudePositionEstimatorEKF::pollData() // PX4_INFO("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m); - _previousGPSTimestamp = _gps.timestamp_position; + _previousGPSTimestamp = _gps.timestamp; } } 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 653d471e75..83f50050fb 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -62,8 +62,6 @@ #include #include #include -#include -#include #include #include #include @@ -211,6 +209,9 @@ private: float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */ float man_roll_max; /**< Max Roll in rad */ float man_pitch_max; /**< Max Pitch in rad */ + float man_roll_scale; /**< scale factor applied to roll actuator control in pure manual mode */ + float man_pitch_scale; /**< scale factor applied to pitch actuator control in pure manual mode */ + float man_yaw_scale; /**< scale factor applied to yaw actuator control in pure manual mode */ float flaps_scale; /**< Scale factor for flaps */ float flaperon_scale; /**< Scale factor for flaperons */ @@ -259,6 +260,9 @@ private: param_t pitchsp_offset_deg; param_t man_roll_max; param_t man_pitch_max; + param_t man_roll_scale; + param_t man_pitch_scale; + param_t man_yaw_scale; param_t flaps_scale; param_t flaperon_scale; @@ -446,6 +450,9 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.man_roll_max = param_find("FW_MAN_R_MAX"); _parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX"); + _parameter_handles.man_roll_scale = param_find("FW_MAN_R_SC"); + _parameter_handles.man_pitch_scale = param_find("FW_MAN_P_SC"); + _parameter_handles.man_yaw_scale = param_find("FW_MAN_Y_SC"); _parameter_handles.flaps_scale = param_find("FW_FLAPS_SCL"); _parameter_handles.flaperon_scale = param_find("FW_FLAPERON_SCL"); @@ -534,6 +541,9 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.man_pitch_max, &(_parameters.man_pitch_max)); _parameters.man_roll_max = math::radians(_parameters.man_roll_max); _parameters.man_pitch_max = math::radians(_parameters.man_pitch_max); + param_get(_parameter_handles.man_roll_scale, &(_parameters.man_roll_scale)); + param_get(_parameter_handles.man_pitch_scale, &(_parameters.man_pitch_scale)); + param_get(_parameter_handles.man_yaw_scale, &(_parameters.man_yaw_scale)); param_get(_parameter_handles.flaps_scale, &_parameters.flaps_scale); param_get(_parameter_handles.flaperon_scale, &_parameters.flaperon_scale); @@ -965,129 +975,41 @@ FixedwingAttitudeControl::task_main() float yaw_manual = 0.0f; float throttle_sp = 0.0f; - /* Read attitude setpoint from uorb if - * - velocity control or position control is enabled (pos controller is running) - * - manual control is disabled (another app may send the setpoint, but it should - * for sure not be set from the remote control values) - */ - if (_vcontrol_mode.flag_control_auto_enabled || - !_vcontrol_mode.flag_control_manual_enabled) { - /* read in attitude setpoint from attitude setpoint uorb topic */ - roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; - pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; - yaw_sp = _att_sp.yaw_body; - throttle_sp = _att_sp.thrust; + // in STABILIZED mode we need to generate the attitude setpoint + // from manual user inputs + if (!_vcontrol_mode.flag_control_climb_rate_enabled) { + _att_sp.roll_body = _manual.y * _parameters.man_roll_max + _parameters.rollsp_offset_rad; + _att_sp.roll_body = math::constrain(_att_sp.roll_body, -_parameters.man_roll_max, _parameters.man_roll_max); + _att_sp.pitch_body = -_manual.x * _parameters.man_pitch_max + _parameters.pitchsp_offset_rad; + _att_sp.pitch_body = math::constrain(_att_sp.pitch_body, -_parameters.man_pitch_max, _parameters.man_pitch_max); + _att_sp.yaw_body = 0.0f; + _att_sp.thrust = _manual.z; + int instance; + orb_publish_auto(ORB_ID(vehicle_attitude_setpoint), &_attitude_sp_pub, &_att_sp, &instance, ORB_PRIO_DEFAULT); + } - /* reset integrals where needed */ - if (_att_sp.roll_reset_integral) { - _roll_ctrl.reset_integrator(); - } + roll_sp = _att_sp.roll_body; + pitch_sp = _att_sp.pitch_body; + yaw_sp = _att_sp.yaw_body; + throttle_sp = _att_sp.thrust; - if (_att_sp.pitch_reset_integral) { - _pitch_ctrl.reset_integrator(); - } - - if (_att_sp.yaw_reset_integral) { - _yaw_ctrl.reset_integrator(); - _wheel_ctrl.reset_integrator(); - } - - } else if (_vcontrol_mode.flag_control_velocity_enabled) { - - /* the pilot does not want to change direction, - * take straight attitude setpoint from position controller - */ - if (fabsf(_manual.y) < 0.01f && fabsf(_roll) < 0.2f) { - roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; - - } else { - roll_sp = (_manual.y * _parameters.man_roll_max) - + _parameters.rollsp_offset_rad; - } - - pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; - throttle_sp = _att_sp.thrust; - - /* reset integrals where needed */ - if (_att_sp.roll_reset_integral) { - _roll_ctrl.reset_integrator(); - } - - if (_att_sp.pitch_reset_integral) { - _pitch_ctrl.reset_integrator(); - } - - if (_att_sp.yaw_reset_integral) { - _yaw_ctrl.reset_integrator(); - _wheel_ctrl.reset_integrator(); - } - - } else if (_vcontrol_mode.flag_control_altitude_enabled) { - /* - * Velocity should be controlled and manual is enabled. - */ - roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad; - pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; - throttle_sp = _att_sp.thrust; - - /* reset integrals where needed */ - if (_att_sp.roll_reset_integral) { - _roll_ctrl.reset_integrator(); - } - - if (_att_sp.pitch_reset_integral) { - _pitch_ctrl.reset_integrator(); - } - - if (_att_sp.yaw_reset_integral) { - _yaw_ctrl.reset_integrator(); - _wheel_ctrl.reset_integrator(); - } - - } else { - /* - * Scale down roll and pitch as the setpoints are radians - * and a typical remote can only do around 45 degrees, the mapping is - * -1..+1 to -man_roll_max rad..+man_roll_max rad (equivalent for pitch) - * - * With this mapping the stick angle is a 1:1 representation of - * the commanded attitude. - * - * The trim gets subtracted here from the manual setpoint to get - * the intended attitude setpoint. Later, after the rate control step the - * trim is added again to the control signal. - */ - roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad; - pitch_sp = -(_manual.x * _parameters.man_pitch_max) + _parameters.pitchsp_offset_rad; - /* allow manual control of rudder deflection */ + /* allow manual yaw in manual modes */ + if (_vcontrol_mode.flag_control_manual_enabled) { yaw_manual = _manual.r; - throttle_sp = _manual.z; + } - /* - * in manual mode no external source should / does emit attitude setpoints. - * emit the manual setpoint here to allow attitude controller tuning - * in attitude control mode. - */ - struct vehicle_attitude_setpoint_s att_sp = {}; - att_sp.timestamp = hrt_absolute_time(); - att_sp.roll_body = roll_sp; - att_sp.pitch_body = pitch_sp; - att_sp.yaw_body = 0.0f - _parameters.trim_yaw; - att_sp.thrust = throttle_sp; + /* reset integrals where needed */ + if (_att_sp.roll_reset_integral) { + _roll_ctrl.reset_integrator(); + } - att_sp.roll_reset_integral = false; - att_sp.pitch_reset_integral = false; - att_sp.yaw_reset_integral = false; + if (_att_sp.pitch_reset_integral) { + _pitch_ctrl.reset_integrator(); + } - /* lazily publish the setpoint only once available */ - if (_attitude_sp_pub != nullptr) { - /* publish the attitude setpoint */ - orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &att_sp); - - } else if (_attitude_setpoint_id) { - /* advertise and publish */ - _attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &att_sp); - } + if (_att_sp.yaw_reset_integral) { + _yaw_ctrl.reset_integrator(); + _wheel_ctrl.reset_integrator(); } /* If the aircraft is on ground reset the integrators */ @@ -1245,9 +1167,10 @@ FixedwingAttitudeControl::task_main() } else { /* manual/direct control */ - _actuators.control[actuator_controls_s::INDEX_ROLL] = _manual.y + _parameters.trim_roll; - _actuators.control[actuator_controls_s::INDEX_PITCH] = -_manual.x + _parameters.trim_pitch; - _actuators.control[actuator_controls_s::INDEX_YAW] = _manual.r + _parameters.trim_yaw; + _actuators.control[actuator_controls_s::INDEX_ROLL] = _manual.y * _parameters.man_roll_scale + _parameters.trim_roll; + _actuators.control[actuator_controls_s::INDEX_PITCH] = -_manual.x * _parameters.man_pitch_scale + + _parameters.trim_pitch; + _actuators.control[actuator_controls_s::INDEX_YAW] = _manual.r * _parameters.man_yaw_scale + _parameters.trim_yaw; _actuators.control[actuator_controls_s::INDEX_THROTTLE] = _manual.z; } @@ -1304,7 +1227,7 @@ FixedwingAttitudeControl::start() _control_task = px4_task_spawn_cmd("fw_att_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 1300, + 1400, (px4_main_t)&FixedwingAttitudeControl::task_main_trampoline, nullptr); diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 3d5ed72058..ebc6547c96 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -57,6 +57,8 @@ * @unit s * @min 0.4 * @max 1.0 + * @decimal 2 + * @increment 0.05 * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_R_TC, 0.4f); @@ -73,6 +75,8 @@ PARAM_DEFINE_FLOAT(FW_R_TC, 0.4f); * @unit s * @min 0.2 * @max 1.0 + * @decimal 2 + * @increment 0.05 * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_P_TC, 0.4f); @@ -86,6 +90,8 @@ PARAM_DEFINE_FLOAT(FW_P_TC, 0.4f); * @unit %/rad/s * @min 0.005 * @max 1.0 + * @decimal 3 + * @increment 0.005 * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_PR_P, 0.08f); @@ -99,6 +105,8 @@ PARAM_DEFINE_FLOAT(FW_PR_P, 0.08f); * @unit %/rad * @min 0.005 * @max 0.5 + * @decimal 3 + * @increment 0.005 * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_PR_I, 0.02f); @@ -112,6 +120,8 @@ PARAM_DEFINE_FLOAT(FW_PR_I, 0.02f); * @unit deg/s * @min 0.0 * @max 90.0 + * @decimal 1 + * @increment 0.5 * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 60.0f); @@ -125,6 +135,8 @@ PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 60.0f); * @unit deg/s * @min 0.0 * @max 90.0 + * @decimal 1 + * @increment 0.5 * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 60.0f); @@ -135,9 +147,10 @@ PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 60.0f); * The portion of the integrator part in the control surface deflection is * limited to this value * - * @unit % * @min 0.0 * @max 1.0 + * @decimal 2 + * @increment 0.05 * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.4f); @@ -151,6 +164,8 @@ PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.4f); * @unit %/rad/s * @min 0.005 * @max 1.0 + * @decimal 3 + * @increment 0.005 * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f); @@ -164,6 +179,8 @@ PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f); * @unit %/rad * @min 0.005 * @max 0.2 + * @decimal 3 + * @increment 0.005 * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_RR_I, 0.01f); @@ -173,9 +190,10 @@ PARAM_DEFINE_FLOAT(FW_RR_I, 0.01f); * * The portion of the integrator part in the control surface deflection is limited to this value. * - * @unit % * @min 0.0 * @max 1.0 + * @decimal 2 + * @increment 0.05 * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.2f); @@ -189,6 +207,8 @@ PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.2f); * @unit deg/s * @min 0.0 * @max 90.0 + * @decimal 1 + * @increment 0.5 * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_R_RMAX, 70.0f); @@ -202,6 +222,8 @@ PARAM_DEFINE_FLOAT(FW_R_RMAX, 70.0f); * @unit %/rad/s * @min 0.005 * @max 1.0 + * @decimal 3 + * @increment 0.005 * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_YR_P, 0.05f); @@ -215,6 +237,8 @@ PARAM_DEFINE_FLOAT(FW_YR_P, 0.05f); * @unit %/rad * @min 0.0 * @max 50.0 + * @decimal 1 + * @increment 0.5 * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_YR_I, 0.0f); @@ -225,9 +249,10 @@ PARAM_DEFINE_FLOAT(FW_YR_I, 0.0f); * The portion of the integrator part in the control surface deflection is * limited to this value * - * @unit % * @min 0.0 * @max 1.0 + * @decimal 2 + * @increment 0.05 * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f); @@ -241,6 +266,8 @@ PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f); * @unit deg/s * @min 0.0 * @max 90.0 + * @decimal 1 + * @increment 0.5 * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f); @@ -254,6 +281,8 @@ PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f); * @unit %/rad/s * @min 0.005 * @max 1.0 + * @decimal 3 + * @increment 0.005 * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_WR_P, 0.5f); @@ -265,8 +294,10 @@ PARAM_DEFINE_FLOAT(FW_WR_P, 0.5f); * state error. It trims any constant error. * * @unit %/rad - * @min 0.0 - * @max 50.0 + * @min 0.005 + * @max 0.5 + * @decimal 3 + * @increment 0.005 * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_WR_I, 0.1f); @@ -277,9 +308,10 @@ PARAM_DEFINE_FLOAT(FW_WR_I, 0.1f); * The portion of the integrator part in the control surface deflection is * limited to this value * - * @unit % * @min 0.0 * @max 1.0 + * @decimal 2 + * @increment 0.05 * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_WR_IMAX, 1.0f); @@ -293,6 +325,8 @@ PARAM_DEFINE_FLOAT(FW_WR_IMAX, 1.0f); * @unit deg/s * @min 0.0 * @max 90.0 + * @decimal 1 + * @increment 0.5 * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_W_RMAX, 0.0f); @@ -307,6 +341,8 @@ PARAM_DEFINE_FLOAT(FW_W_RMAX, 0.0f); * @unit %/rad/s * @min 0.0 * @max 10.0 + * @decimal 2 + * @increment 0.05 * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_RR_FF, 0.5f); @@ -319,6 +355,8 @@ PARAM_DEFINE_FLOAT(FW_RR_FF, 0.5f); * @unit %/rad/s * @min 0.0 * @max 10.0 + * @decimal 2 + * @increment 0.05 * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_PR_FF, 0.5f); @@ -331,6 +369,8 @@ PARAM_DEFINE_FLOAT(FW_PR_FF, 0.5f); * @unit %/rad/s * @min 0.0 * @max 10.0 + * @decimal 2 + * @increment 0.05 * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f); @@ -343,6 +383,8 @@ PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f); * @unit %/rad/s * @min 0.0 * @max 10.0 + * @decimal 2 + * @increment 0.05 * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_WR_FF, 0.2f); @@ -354,6 +396,10 @@ PARAM_DEFINE_FLOAT(FW_WR_FF, 0.2f); * turn. Set to a very high value to disable. * * @unit m/s + * @min 0.0 + * @max 1000.0 + * @decimal 1 + * @increment 0.5 * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_YCO_VMIN, 1000.0f); @@ -373,49 +419,6 @@ PARAM_DEFINE_FLOAT(FW_YCO_VMIN, 1000.0f); */ PARAM_DEFINE_INT32(FW_YCO_METHOD, 0); -/* Airspeed parameters: - * The following parameters about airspeed are used by the attitude and the - * position controller. - * */ - -/** - * Minimum Airspeed - * - * If the airspeed falls below this value, the TECS controller will try to - * increase airspeed more aggressively. - * - * @unit m/s - * @min 0.0 - * @max 40 - * @group FW Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 10.0f); - -/** - * Trim Airspeed - * - * The TECS controller tries to fly at this airspeed. - * - * @unit m/s - * @min 0.0 - * @max 40 - * @group FW Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 15.0f); - -/** - * Maximum Airspeed - * - * If the airspeed is above this value, the TECS controller will try to decrease - * airspeed more aggressively. - * - * @unit m/s - * @min 0.0 - * @max 40 - * @group FW Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 20.0f); - /** * Roll Setpoint Offset * @@ -426,6 +429,8 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 20.0f); * @unit deg * @min -90.0 * @max 90.0 + * @decimal 1 + * @increment 0.5 * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_RSP_OFF, 0.0f); @@ -440,6 +445,8 @@ PARAM_DEFINE_FLOAT(FW_RSP_OFF, 0.0f); * @unit deg * @min -90.0 * @max 90.0 + * @decimal 1 + * @increment 0.5 * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f); @@ -452,6 +459,8 @@ PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f); * @unit deg * @min 0.0 * @max 90.0 + * @decimal 1 + * @increment 0.5 * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f); @@ -464,6 +473,8 @@ PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f); * @unit deg * @min 0.0 * @max 90.0 + * @decimal 1 + * @increment 0.5 * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 45.0f); @@ -471,8 +482,11 @@ PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 45.0f); /** * Scale factor for flaps * + * @unit norm * @min 0.0 * @max 1.0 + * @decimal 2 + * @increment 0.01 * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_FLAPS_SCL, 1.0f); @@ -480,8 +494,70 @@ PARAM_DEFINE_FLOAT(FW_FLAPS_SCL, 1.0f); /** * Scale factor for flaperons * + * @unit norm * @min 0.0 * @max 1.0 + * @decimal 2 + * @increment 0.01 * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_FLAPERON_SCL, 0.0f); + +/** + * Airspeed mode + * + * The param value sets the method used to publish the control state airspeed. + * For small wings or VTOL without airspeed sensor this parameter can be used to + * enable flying without an airspeed reading + * + * @min 0 + * @max 2 + * @value 0 use measured airspeed + * @value 1 use vehicle ground velocity as airspeed + * @value 2 declare airspeed invalid + * @group FW Attitude Control + */ +PARAM_DEFINE_INT32(FW_ARSP_MODE, 0); + +/** + * Manual roll scale + * + * Scale factor applied to the desired roll actuator command in full manual mode. This parameter allows + * to adjust the throws of the control surfaces. + * + * @unit norm + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_MAN_R_SC, 1.0f); + +/** + * Manual pitch scale + * + * Scale factor applied to the desired pitch actuator command in full manual mode. This parameter allows + * to adjust the throws of the control surfaces. + * + * @unit norm + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_MAN_P_SC, 1.0f); + +/** + * Manual yaw scale + * + * Scale factor applied to the desired yaw actuator command in full manual mode. This parameter allows + * to adjust the throws of the control surfaces. + * + * @unit norm + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_MAN_Y_SC, 1.0f); diff --git a/src/modules/fw_pos_control_l1/CMakeLists.txt b/src/modules/fw_pos_control_l1/CMakeLists.txt index 53d12b0401..5a4e7fa6f1 100644 --- a/src/modules/fw_pos_control_l1/CMakeLists.txt +++ b/src/modules/fw_pos_control_l1/CMakeLists.txt @@ -35,7 +35,7 @@ px4_add_module( MAIN fw_pos_control_l1 STACK_MAIN 1200 COMPILE_FLAGS - -Os + -Os -Weffc++ SRCS fw_pos_control_l1_main.cpp landingslope.cpp 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 3e93fba958..d956bfcdfd 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 @@ -86,9 +86,9 @@ #include #include #include +#include #include #include -#include #include #include #include @@ -103,18 +103,16 @@ #include static int _control_task = -1; /**< task handle for sensor task */ + #define HDG_HOLD_DIST_NEXT 3000.0f // initial distance of waypoint in front of plane in heading hold mode #define HDG_HOLD_REACHED_DIST 1000.0f // distance (plane to waypoint in front) at which waypoints are reset in heading hold mode #define HDG_HOLD_SET_BACK_DIST 100.0f // distance by which previous waypoint is set behind the plane #define HDG_HOLD_YAWRATE_THRESH 0.15f // max yawrate at which plane locks yaw for heading hold mode -#define HDG_HOLD_MAN_INPUT_THRESH 0.01f // max manual roll input from user which does not change the locked heading +#define HDG_HOLD_MAN_INPUT_THRESH 0.01f // max manual roll/yaw input from user which does not change the locked heading #define T_ALT_TIMEOUT 1 // time after which we abort landing if terrain estimate is not valid - -static constexpr float THROTTLE_THRESH = - 0.05f; ///< max throttle from user which will not lead to motors spinning up in altitude controlled modes -static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH = - 0.85f; ///< a throttle / pitch input above this value leads to the system switching to climbout mode -static constexpr float ALTHOLD_EPV_RESET_THRESH = 5.0f; +#define THROTTLE_THRESH 0.05f ///< max throttle from user which will not lead to motors spinning up in altitude controlled modes +#define MANUAL_THROTTLE_CLIMBOUT_THRESH 0.85f ///< a throttle / pitch input above this value leads to the system switching to climbout mode +#define ALTHOLD_EPV_RESET_THRESH 5.0f /** * L1 control app start / stop handling function @@ -138,6 +136,10 @@ public: */ ~FixedwingPositionControl(); + // prevent copying + FixedwingPositionControl(const FixedwingPositionControl &) = delete; + FixedwingPositionControl operator=(const FixedwingPositionControl &other) = delete; + /** * Start the sensors task. * @@ -164,19 +166,19 @@ private: int _control_mode_sub; /**< control mode subscription */ int _vehicle_status_sub; /**< vehicle status subscription */ int _vehicle_land_detected_sub; /**< vehicle land detected subscription */ - int _params_sub; /**< notification of parameter updates */ - int _manual_control_sub; /**< notification of manual control updates */ + int _params_sub; /**< notification of parameter updates */ + int _manual_control_sub; /**< notification of manual control updates */ int _sensor_combined_sub; /**< for body frame accelerations */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint */ orb_advert_t _tecs_status_pub; /**< TECS status publication */ - orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */ + orb_advert_t _fw_pos_ctrl_status_pub; /**< navigation capabilities publication */ orb_id_t _attitude_setpoint_id; struct control_state_s _ctrl_state; /**< control state */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ - struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */ + struct fw_pos_ctrl_status_s _fw_pos_ctrl_status; /**< navigation capabilities */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ struct vehicle_control_mode_s _control_mode; /**< control mode */ struct vehicle_status_s _vehicle_status; /**< vehicle status */ @@ -198,39 +200,35 @@ private: struct position_setpoint_s _hdg_hold_curr_wp; /**< position to which heading hold flies */ hrt_abstime _control_position_last_called; /** &c } } -void FixedwingPositionControl::navigation_capabilities_publish() +void FixedwingPositionControl::fw_pos_ctrl_status_publish() { - _nav_capabilities.timestamp = hrt_absolute_time(); + _fw_pos_ctrl_status.timestamp = hrt_absolute_time(); - if (_nav_capabilities_pub != nullptr) { - orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities); + if (_fw_pos_ctrl_status_pub != nullptr) { + orb_publish(ORB_ID(fw_pos_ctrl_status), _fw_pos_ctrl_status_pub, &_fw_pos_ctrl_status); } else { - _nav_capabilities_pub = orb_advertise(ORB_ID(navigation_capabilities), &_nav_capabilities); + _fw_pos_ctrl_status_pub = orb_advertise(ORB_ID(fw_pos_ctrl_status), &_fw_pos_ctrl_status); } } @@ -1081,9 +1117,9 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint /* Decide if the terrain estimation can be used, once we switched to using the terrain we stick with it * for the whole landing */ if (_parameters.land_use_terrain_estimate && global_pos.terrain_alt_valid) { - if (!land_useterrain) { + if (!_land_useterrain) { mavlink_log_info(&_mavlink_log_pub, "Landing, using terrain estimate"); - land_useterrain = true; + _land_useterrain = true; } return global_pos.terrain_alt; @@ -1171,11 +1207,12 @@ bool FixedwingPositionControl::update_desired_altitude(float dt) bool FixedwingPositionControl::in_takeoff_situation() { + // in air for < 10s const hrt_abstime delta_takeoff = 10000000; - const float throttle_threshold = 0.1f; - if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold + if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _global_pos.alt <= _takeoff_ground_alt + _parameters.climbout_diff) { + return true; } @@ -1235,6 +1272,22 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)}; calculate_gndspeed_undershoot(current_position, ground_speed_2d, pos_sp_triplet); + // l1 navigation logic breaks down when wind speed exceeds max airspeed + // compute 2D groundspeed from airspeed-heading projection + math::Vector<2> air_speed_2d = {_ctrl_state.airspeed * cosf(_yaw), _ctrl_state.airspeed * sinf(_yaw)}; + math::Vector<2> nav_speed_2d = {0, 0}; + + // angle between air_speed_2d and ground_speed_2d + float air_gnd_angle = acosf((air_speed_2d * ground_speed_2d) / (air_speed_2d.length() * ground_speed_2d.length())); + + // if angle > 90 degrees or groundspeed is less than threshold, replace groundspeed with airspeed projection + if ((fabsf(air_gnd_angle) > (float)M_PI) || (ground_speed_2d.length() < 3.0f)) { + nav_speed_2d = air_speed_2d; + + } else { + nav_speed_2d = ground_speed_2d; + } + /* define altitude error */ float altitude_error = pos_sp_triplet.current.alt - _global_pos.alt; @@ -1311,6 +1364,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi mission_airspeed = _pos_sp_triplet.current.cruising_speed; } + float mission_throttle = _parameters.throttle_cruise; + + if (PX4_ISFINITE(_pos_sp_triplet.current.cruising_throttle) && + _pos_sp_triplet.current.cruising_throttle > 0.01f) { + + mission_throttle = _pos_sp_triplet.current.cruising_throttle; + } + if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { _att_sp.thrust = 0.0f; _att_sp.roll_body = 0.0f; @@ -1318,41 +1379,45 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { /* waypoint is a plain navigation waypoint */ - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, nav_speed_2d); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); tecs_update_pitch_throttle(pos_sp_triplet.current.alt, calculate_target_airspeed(mission_airspeed), eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + _parameters.throttle_min, _parameters.throttle_max, mission_throttle, false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { /* waypoint is a loiter waypoint */ _l1_control.navigate_loiter(curr_wp, current_position, pos_sp_triplet.current.loiter_radius, - pos_sp_triplet.current.loiter_direction, ground_speed_2d); + pos_sp_triplet.current.loiter_direction, nav_speed_2d); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); float alt_sp; - if (_nav_capabilities.abort_landing == true) { + if (_fw_pos_ctrl_status.abort_landing == true) { // if we entered loiter due to an aborted landing, demand // altitude setpoint well above landing waypoint alt_sp = pos_sp_triplet.current.alt + 2.0f * _parameters.climbout_diff; } else { - // use altitude given by wapoint + // use altitude given by waypoint alt_sp = pos_sp_triplet.current.alt; } + if (in_takeoff_situation()) { + alt_sp = math::max(alt_sp, _takeoff_ground_alt + _parameters.climbout_diff); + } + if (in_takeoff_situation() || ((_global_pos.alt < pos_sp_triplet.current.alt + _parameters.climbout_diff) - && _nav_capabilities.abort_landing == true)) { + && _fw_pos_ctrl_status.abort_landing == true)) { /* limit roll motion to ensure enough lift */ - _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), - math::radians(15.0f)); + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), + math::radians(10.0f)); } tecs_update_pitch_throttle(alt_sp, calculate_target_airspeed(mission_airspeed), eas2tas, @@ -1378,6 +1443,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Horizontal landing control */ /* switch to heading hold for the last meters, continue heading hold after */ float wp_distance = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); + /* calculate a waypoint distance value which is 0 when the aircraft is behind the waypoint */ float wp_distance_save = wp_distance; @@ -1392,24 +1458,25 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::Vector<2> curr_wp_shifted; double lat; double lon; - create_waypoint_from_line_and_dist(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon, pos_sp_triplet.previous.lat, - pos_sp_triplet.previous.lon, -1000.0f, &lat, &lon); + create_waypoint_from_line_and_dist(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon, + pos_sp_triplet.previous.lat, pos_sp_triplet.previous.lon, -1000.0f, &lat, &lon); curr_wp_shifted(0) = (float)lat; curr_wp_shifted(1) = (float)lon; // we want the plane to keep tracking the desired flight path until we start flaring // if we go into heading hold mode earlier then we risk to be pushed away from the runway by cross winds //if (land_noreturn_vertical) { - if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) { + if (wp_distance < _parameters.land_heading_hold_horizontal_distance || _land_noreturn_horizontal) { /* heading hold, along the line connecting this and the last waypoint */ - if (!land_noreturn_horizontal) {//set target_bearing in first occurrence + if (!_land_noreturn_horizontal) { + // set target_bearing in first occurrence if (pos_sp_triplet.previous.valid) { - target_bearing = bearing_lastwp_currwp; + _target_bearing = bearing_lastwp_currwp; } else { - target_bearing = _yaw; + _target_bearing = _yaw; } mavlink_log_info(&_mavlink_log_pub, "#Landing, heading hold"); @@ -1417,20 +1484,20 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_yaw)); - _l1_control.navigate_heading(target_bearing, _yaw, ground_speed_2d); + _l1_control.navigate_heading(_target_bearing, _yaw, nav_speed_2d); - land_noreturn_horizontal = true; + _land_noreturn_horizontal = true; } else { /* normal navigation */ - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, nav_speed_2d); } _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - if (land_noreturn_horizontal) { + if (_land_noreturn_horizontal) { /* limit roll motion to prevent wings from touching the ground first */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); } @@ -1465,11 +1532,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else { // still no valid terrain, abort landing terrain_alt = pos_sp_triplet.current.alt; - _nav_capabilities.abort_landing = true; + _fw_pos_ctrl_status.abort_landing = true; } } else if ((!_global_pos.terrain_alt_valid && hrt_elapsed_time(&_time_last_t_alt) < T_ALT_TIMEOUT * 1000 * 1000) - || land_noreturn_vertical) { + || _land_noreturn_vertical) { // use previous terrain estimate for some time and hope to recover // if we are already flaring (land_noreturn_vertical) then just // go with the old estimate @@ -1478,7 +1545,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else { // terrain alt was not valid for long time, abort landing terrain_alt = _t_alt_prev_valid; - _nav_capabilities.abort_landing = true; + _fw_pos_ctrl_status.abort_landing = true; } } else { @@ -1491,75 +1558,81 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float L_altitude_rel = pos_sp_triplet.previous.valid ? pos_sp_triplet.previous.alt - terrain_alt : 0.0f; - float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, + float landing_slope_alt_rel_desired = _landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp); /* Check if we should start flaring with a vertical and a * horizontal limit (with some tolerance) * The horizontal limit is only applied when we are in front of the wp */ - if (((_global_pos.alt < terrain_alt + landingslope.flare_relative_alt()) && - (wp_distance_save < landingslope.flare_length() + 5.0f)) || - land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out + if (((_global_pos.alt < terrain_alt + _landingslope.flare_relative_alt()) && + (wp_distance_save < _landingslope.flare_length() + 5.0f)) || + _land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out + /* land with minimal speed */ -// /* force TECS to only control speed with pitch, altitude is only implicitely controlled now */ -// _tecs.set_speed_weight(2.0f); + /* force TECS to only control speed with pitch, altitude is only implicitly controlled now */ + // _tecs.set_speed_weight(2.0f); /* kill the throttle if param requests it */ throttle_max = _parameters.throttle_max; /* enable direct yaw control using rudder/wheel */ - _att_sp.yaw_body = target_bearing; + _att_sp.yaw_body = _target_bearing; _att_sp.fw_control_yaw = true; - if (_global_pos.alt < terrain_alt + landingslope.motor_lim_relative_alt() || land_motor_lim) { + if (_global_pos.alt < terrain_alt + _landingslope.motor_lim_relative_alt() || _land_motor_lim) { throttle_max = math::min(throttle_max, _parameters.throttle_land_max); - if (!land_motor_lim) { - land_motor_lim = true; + if (!_land_motor_lim) { + _land_motor_lim = true; mavlink_log_info(&_mavlink_log_pub, "#Landing, limiting throttle"); } } - float flare_curve_alt_rel = landingslope.getFlareCurveRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, + float flare_curve_alt_rel = _landingslope.getFlareCurveRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp); /* avoid climbout */ - if ((flare_curve_alt_rel_last < flare_curve_alt_rel && land_noreturn_vertical) || land_stayonground) { + if ((_flare_curve_alt_rel_last < flare_curve_alt_rel && _land_noreturn_vertical) || _land_stayonground) { flare_curve_alt_rel = 0.0f; // stay on ground - land_stayonground = true; + _land_stayonground = true; } tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel, - calculate_target_airspeed(airspeed_land), eas2tas, + calculate_target_airspeed(airspeed_land), + eas2tas, math::radians(_parameters.land_flare_pitch_min_deg), math::radians(_parameters.land_flare_pitch_max_deg), - 0.0f, throttle_max, throttle_land, - false, land_motor_lim ? math::radians(_parameters.land_flare_pitch_min_deg) : math::radians( - _parameters.pitch_limit_min), - _global_pos.alt, ground_speed, - land_motor_lim ? tecs_status_s::TECS_MODE_LAND_THROTTLELIM : tecs_status_s::TECS_MODE_LAND); + 0.0f, + throttle_max, + throttle_land, + false, + _land_motor_lim ? math::radians(_parameters.land_flare_pitch_min_deg) + : math::radians(_parameters.pitch_limit_min), + _global_pos.alt, + ground_speed, + _land_motor_lim ? tecs_status_s::TECS_MODE_LAND_THROTTLELIM : tecs_status_s::TECS_MODE_LAND); - if (!land_noreturn_vertical) { + if (!_land_noreturn_vertical) { // just started with the flaring phase _att_sp.pitch_body = 0.0f; - height_flare = _global_pos.alt - terrain_alt; + _flare_height = _global_pos.alt - terrain_alt; mavlink_log_info(&_mavlink_log_pub, "#Landing, flaring"); - land_noreturn_vertical = true; + _land_noreturn_vertical = true; } else { if (_global_pos.vel_d > 0.1f) { _att_sp.pitch_body = math::radians(_parameters.land_flare_pitch_min_deg) * - math::constrain((height_flare - (_global_pos.alt - terrain_alt)) / height_flare, 0.0f, 1.0f); + math::constrain((_flare_height - (_global_pos.alt - terrain_alt)) / _flare_height, 0.0f, 1.0f); } else { _att_sp.pitch_body = _att_sp.pitch_body; } } - flare_curve_alt_rel_last = flare_curve_alt_rel; + _flare_curve_alt_rel_last = flare_curve_alt_rel; } else { @@ -1574,13 +1647,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi * */ float altitude_desired_rel; - if (_global_pos.alt > terrain_alt + landing_slope_alt_rel_desired || land_onslope) { + if (_global_pos.alt > terrain_alt + landing_slope_alt_rel_desired || _land_onslope) { /* stay on slope */ altitude_desired_rel = landing_slope_alt_rel_desired; - if (!land_onslope) { + if (!_land_onslope) { mavlink_log_info(&_mavlink_log_pub, "#Landing, on slope"); - land_onslope = true; + _land_onslope = true; } } else { @@ -1631,7 +1704,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi * Update navigation: _runway_takeoff returns the start WP according to mode and phase. * If we use the navigator heading or not is decided later. */ - _l1_control.navigate_waypoints(_runway_takeoff.getStartWP(), curr_wp, current_position, ground_speed_2d); + _l1_control.navigate_waypoints(_runway_takeoff.getStartWP(), curr_wp, current_position, nav_speed_2d); // update tecs float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_parameters.pitch_limit_max); @@ -1670,44 +1743,44 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else { /* Perform launch detection */ - if (launchDetector.launchDetectionEnabled() && - launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { + if (_launchDetector.launchDetectionEnabled() && + _launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { /* Inform user that launchdetection is running */ static hrt_abstime last_sent = 0; if (hrt_absolute_time() - last_sent > 4e6) { - mavlink_log_critical(&_mavlink_log_pub, "#Launchdetection running"); + mavlink_log_critical(&_mavlink_log_pub, "#Launch detection running"); last_sent = hrt_absolute_time(); } /* Detect launch */ - launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); + _launchDetector.update(accel_body(0)); /* update our copy of the launch detection state */ - launch_detection_state = launchDetector.getLaunchDetected(); + _launch_detection_state = _launchDetector.getLaunchDetected(); } else { /* no takeoff detection --> fly */ - launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; + _launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; } /* Set control values depending on the detection state */ - if (launch_detection_state != LAUNCHDETECTION_RES_NONE) { + if (_launch_detection_state != LAUNCHDETECTION_RES_NONE) { /* Launch has been detected, hence we have to control the plane. */ - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, nav_speed_2d); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); /* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use * full throttle, otherwise we use the preTakeOff Throttle */ - float takeoff_throttle = launch_detection_state != + float takeoff_throttle = _launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ? - launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max; + _launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max; /* select maximum pitch: the launchdetector may impose another limit for the pitch * depending on the state of the launch */ - float takeoff_pitch_max_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max); + float takeoff_pitch_max_deg = _launchDetector.getPitchMax(_parameters.pitch_limit_max); float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); /* apply minimum pitch and limit roll if target altitude is not within climbout_diff @@ -1789,6 +1862,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _hdg_hold_yaw = _yaw; _hdg_hold_enabled = false; // this makes sure the waypoints are reset below _yaw_lock_engaged = false; + + /* reset setpoints from other modes (auto) otherwise we won't + * level out without new manual input */ + _att_sp.roll_body = _manual.y * _parameters.man_roll_max_rad; + _att_sp.yaw_body = 0; } /* Reset integrators if switching to this mode from a other mode in which posctl was not active */ @@ -1833,8 +1911,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi tecs_status_s::TECS_MODE_NORMAL); /* heading control */ + if (fabsf(_manual.y) < HDG_HOLD_MAN_INPUT_THRESH && + fabsf(_manual.r) < HDG_HOLD_MAN_INPUT_THRESH) { - if (fabsf(_manual.y) < HDG_HOLD_MAN_INPUT_THRESH) { /* heading / roll is zero, lock onto current heading */ if (fabsf(_ctrl_state.yaw_rate) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) { // little yaw movement, lock to current heading @@ -1887,9 +1966,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } } - } else { + } + + if (!_yaw_lock_engaged || fabsf(_manual.y) >= HDG_HOLD_MAN_INPUT_THRESH || + fabsf(_manual.r) >= HDG_HOLD_MAN_INPUT_THRESH) { _hdg_hold_enabled = false; _yaw_lock_engaged = false; + _att_sp.roll_body = _manual.y * _parameters.man_roll_max_rad; + _att_sp.yaw_body = 0; } } else if (_control_mode.flag_control_altitude_enabled) { @@ -1941,10 +2025,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi ground_speed, tecs_status_s::TECS_MODE_NORMAL); + _att_sp.roll_body = _manual.y * _parameters.man_roll_max_rad; + _att_sp.yaw_body = 0; + } else { _control_mode_current = FW_POSCTRL_MODE_OTHER; - /** MANUAL FLIGHT **/ + /* do not publish the setpoint */ + setpoint = false; // reset hold altitude _hold_alt = _global_pos.alt; @@ -1954,13 +2042,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _time_last_t_alt = 0; // reset lading abort state - _nav_capabilities.abort_landing = false; - - /* no flight mode applies, do not publish an attitude setpoint */ - setpoint = false; + _fw_pos_ctrl_status.abort_landing = false; /* reset landing and takeoff state */ - if (!last_manual) { + if (!_last_manual) { reset_landing_state(); reset_takeoff_state(); } @@ -1973,12 +2058,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && - launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS && + _launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS && !_runway_takeoff.runwayTakeoffEnabled()) { /* making sure again that the correct thrust is used, * without depending on library calls for safety reasons. the pre-takeoff throttle and the idle throttle normally map to the same parameter. */ - _att_sp.thrust = (launchDetector.launchDetectionEnabled()) ? launchDetector.getThrottlePreTakeoff() : + _att_sp.thrust = (_launchDetector.launchDetectionEnabled()) ? _launchDetector.getThrottlePreTakeoff() : _parameters.throttle_idle; } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && @@ -1990,6 +2075,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { _att_sp.thrust = 0.0f; + } else if (_control_mode_current == FW_POSCTRL_MODE_OTHER) { + _att_sp.thrust = math::min(_att_sp.thrust, _parameters.throttle_max); + } else { /* Copy thrust and pitch values from tecs */ if (_vehicle_land_detected.landed) { @@ -2003,23 +2091,32 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } - /* During a takeoff waypoint while waiting for launch the pitch sp is set - * already (not by tecs) */ - if (!(_control_mode_current == FW_POSCTRL_MODE_AUTO && ( - (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && - (launch_detection_state == LAUNCHDETECTION_RES_NONE || - _runway_takeoff.runwayTakeoffEnabled())) || - (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND && - land_noreturn_vertical)) - )) { + // decide when to use pitch setpoint from TECS because in some cases pitch + // setpoint is generated by other means + bool use_tecs_pitch = true; + + // auto runway takeoff + use_tecs_pitch &= !(_control_mode_current == FW_POSCTRL_MODE_AUTO && + pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && + (_launch_detection_state == LAUNCHDETECTION_RES_NONE || _runway_takeoff.runwayTakeoffEnabled())); + + + // flaring during landing + use_tecs_pitch &= !(pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND && + _land_noreturn_vertical); + + // manual attitude control + use_tecs_pitch &= !(_control_mode_current == FW_POSCTRL_MODE_OTHER); + + if (use_tecs_pitch) { _att_sp.pitch_body = get_tecs_pitch(); } if (_control_mode.flag_control_position_enabled) { - last_manual = false; + _last_manual = false; } else { - last_manual = true; + _last_manual = true; } @@ -2155,6 +2252,15 @@ FixedwingPositionControl::task_main() if (control_position(current_position, ground_speed, _pos_sp_triplet)) { _att_sp.timestamp = hrt_absolute_time(); + // add attitude setpoint offsets + _att_sp.roll_body += _parameters.rollsp_offset_rad; + _att_sp.pitch_body += _parameters.pitchsp_offset_rad; + + if (_control_mode.flag_control_manual_enabled) { + _att_sp.roll_body = math::constrain(_att_sp.roll_body, -_parameters.man_roll_max_rad, _parameters.man_roll_max_rad); + _att_sp.pitch_body = math::constrain(_att_sp.pitch_body, -_parameters.man_pitch_max_rad, _parameters.man_pitch_max_rad); + } + /* lazily publish the setpoint only once available */ if (_attitude_sp_pub != nullptr) { /* publish the attitude setpoint */ @@ -2169,15 +2275,25 @@ FixedwingPositionControl::task_main() float turn_distance = _l1_control.switch_distance(100.0f); /* lazily publish navigation capabilities */ - if ((hrt_elapsed_time(&_nav_capabilities.timestamp) > 1000000) - || (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON + if ((hrt_elapsed_time(&_fw_pos_ctrl_status.timestamp) > 1000000) + || (fabsf(turn_distance - _fw_pos_ctrl_status.turn_distance) > FLT_EPSILON && turn_distance > 0)) { /* set new turn distance */ - _nav_capabilities.turn_distance = turn_distance; + _fw_pos_ctrl_status.turn_distance = turn_distance; - navigation_capabilities_publish(); + _fw_pos_ctrl_status.nav_roll = _l1_control.nav_roll(); + _fw_pos_ctrl_status.nav_pitch = get_tecs_pitch(); + _fw_pos_ctrl_status.nav_bearing = _l1_control.nav_bearing(); + _fw_pos_ctrl_status.target_bearing = _l1_control.target_bearing(); + _fw_pos_ctrl_status.xtrack_error = _l1_control.crosstrack_error(); + + math::Vector<2> curr_wp((float)_pos_sp_triplet.current.lat, (float)_pos_sp_triplet.current.lon); + _fw_pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), + curr_wp(1)); + + fw_pos_ctrl_status_publish(); } } @@ -2196,19 +2312,21 @@ FixedwingPositionControl::task_main() void FixedwingPositionControl::reset_takeoff_state() { - launch_detection_state = LAUNCHDETECTION_RES_NONE; - launchDetector.reset(); + _launch_detection_state = LAUNCHDETECTION_RES_NONE; + _launchDetector.reset(); _runway_takeoff.reset(); } void FixedwingPositionControl::reset_landing_state() { - land_noreturn_horizontal = false; - land_noreturn_vertical = false; - land_stayonground = false; - land_motor_lim = false; - land_onslope = false; - land_useterrain = false; + _time_started_landing = 0; + + _land_noreturn_horizontal = false; + _land_noreturn_vertical = false; + _land_stayonground = false; + _land_motor_lim = false; + _land_onslope = false; + _land_useterrain = false; } void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas, @@ -2240,11 +2358,20 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ // we're in transition if (_vehicle_status.is_vtol && _vehicle_status.in_transition_mode) { _was_in_transition = true; - _asp_after_transition = _ctrl_state.airspeed; - // after transition we ramp up desired airspeed from the speed we had coming out of the transition + // set this to transition airspeed to init tecs correctly + if (_parameters.airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED) { + // some vtols fly without airspeed sensor + _asp_after_transition = _parameters.airspeed_trans; + + } else { + _asp_after_transition = _ctrl_state.airspeed; + } + + _asp_after_transition = math::constrain(_asp_after_transition, _parameters.airspeed_min, _parameters.airspeed_max); } else if (_was_in_transition) { + // after transition we ramp up desired airspeed from the speed we had coming out of the transition _asp_after_transition += dt * 2; // increase 2m/s if (_asp_after_transition < v_sp && _ctrl_state.airspeed < v_sp) { @@ -2358,7 +2485,7 @@ FixedwingPositionControl::start() ASSERT(_control_task == -1); /* start the task */ - _control_task = px4_task_spawn_cmd("fw_pos_control_l1", + _control_task = px4_task_spawn_cmd("fw_pos_ctrl_l1", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1400, diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 322205b138..561d4e4349 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -54,6 +54,8 @@ * @unit m * @min 12.0 * @max 50.0 + * @decimal 1 + * @increment 0.5 * @group FW L1 Control */ PARAM_DEFINE_FLOAT(FW_L1_PERIOD, 20.0f); @@ -65,6 +67,8 @@ PARAM_DEFINE_FLOAT(FW_L1_PERIOD, 20.0f); * * @min 0.6 * @max 0.9 + * @decimal 2 + * @increment 0.05 * @group FW L1 Control */ PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f); @@ -77,6 +81,8 @@ PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f); * @unit norm * @min 0.0 * @max 1.0 + * @decimal 2 + * @increment 0.01 * @group FW L1 Control */ PARAM_DEFINE_FLOAT(FW_THR_CRUISE, 0.6f); @@ -100,6 +106,8 @@ PARAM_DEFINE_FLOAT(FW_THR_SLEW_MAX, 0.0f); * @unit deg * @min -60.0 * @max 0.0 + * @decimal 1 + * @increment 0.5 * @group FW L1 Control */ PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -45.0f); @@ -112,6 +120,8 @@ PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -45.0f); * @unit deg * @min 0.0 * @max 60.0 + * @decimal 1 + * @increment 0.5 * @group FW L1 Control */ PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f); @@ -124,6 +134,8 @@ PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f); * @unit deg * @min 35.0 * @max 65.0 + * @decimal 1 + * @increment 0.5 * @group FW L1 Control */ PARAM_DEFINE_FLOAT(FW_R_LIM, 50.0f); @@ -138,6 +150,8 @@ PARAM_DEFINE_FLOAT(FW_R_LIM, 50.0f); * @unit norm * @min 0.0 * @max 1.0 + * @decimal 2 + * @increment 0.01 * @group FW L1 Control */ PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f); @@ -157,6 +171,8 @@ PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f); * @unit norm * @min 0.0 * @max 1.0 + * @decimal 2 + * @increment 0.01 * @group FW L1 Control */ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); @@ -172,6 +188,8 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); * @unit norm * @min 0.0 * @max 0.4 + * @decimal 2 + * @increment 0.01 * @group FW L1 Control */ PARAM_DEFINE_FLOAT(FW_THR_IDLE, 0.15f); @@ -180,11 +198,13 @@ PARAM_DEFINE_FLOAT(FW_THR_IDLE, 0.15f); * Throttle limit value before flare * * This throttle value will be set as throttle limit at FW_LND_TLALT, - * before arcraft will flare. + * before aircraft will flare. * * @unit norm * @min 0.0 * @max 1.0 + * @decimal 2 + * @increment 0.01 * @group FW L1 Control */ PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); @@ -200,10 +220,167 @@ PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); * @unit m * @min 0.0 * @max 150.0 + * @decimal 1 + * @increment 0.5 * @group FW L1 Control */ PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 10.0f); +/** + * Landing slope angle + * + * @unit deg + * @min 1.0 + * @max 15.0 + * @decimal 1 + * @increment 0.5 + * @group FW L1 Control + */ +PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f); + +/** + * + * + * @unit m + * @min 1.0 + * @max 15.0 + * @decimal 1 + * @increment 0.5 + * @group FW L1 Control + */ +PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f); + +/** + * Landing flare altitude (relative to landing altitude) + * + * @unit m + * @min 0.0 + * @max 25.0 + * @decimal 1 + * @increment 0.5 + * @group FW L1 Control + */ +PARAM_DEFINE_FLOAT(FW_LND_FLALT, 8.0f); + +/** + * Landing throttle limit altitude (relative landing altitude) + * + * Default of -1.0 lets the system default to applying throttle + * limiting at 2/3 of the flare altitude. + * + * @unit m + * @min -1.0 + * @max 30.0 + * @decimal 1 + * @increment 0.5 + * @group FW L1 Control + */ +PARAM_DEFINE_FLOAT(FW_LND_TLALT, -1.0f); + +/** + * Landing heading hold horizontal distance + * + * @unit m + * @min 0 + * @max 30.0 + * @decimal 1 + * @increment 0.5 + * @group FW L1 Control + */ +PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); + +/** + * Use terrain estimate during landing + * + * @boolean + * @group FW L1 Control + */ +PARAM_DEFINE_INT32(FW_LND_USETER, 0); + +/** + * Flare, minimum pitch + * + * Minimum pitch during flare, a positive sign means nose up + * Applied once FW_LND_TLALT is reached + * + * @unit deg + * @min 0 + * @max 15.0 + * @decimal 1 + * @increment 0.5 + * @group FW L1 Control + */ +PARAM_DEFINE_FLOAT(FW_LND_FL_PMIN, 2.5f); + +/** + * Flare, maximum pitch + * + * Maximum pitch during flare, a positive sign means nose up + * Applied once FW_LND_TLALT is reached + * + * @unit deg + * @min 0 + * @max 45.0 + * @decimal 1 + * @increment 0.5 + * @group FW L1 Control + */ +PARAM_DEFINE_FLOAT(FW_LND_FL_PMAX, 15.0f); + +/** + * Min. airspeed scaling factor for landing + * + * Multiplying this factor with the minimum airspeed of the plane + * gives the target airspeed the landing approach. + * FW_AIRSPD_MIN * FW_LND_AIRSPD_SC + * + * @unit norm + * @min 1.0 + * @max 1.5 + * @decimal 2 + * @increment 0.01 + * @group FW L1 Control + */ +PARAM_DEFINE_FLOAT(FW_LND_AIRSPD_SC, 1.3f); + + + +/* + * TECS parameters + * + */ + + +/** + * Minimum Airspeed + * + * If the airspeed falls below this value, the TECS controller will try to + * increase airspeed more aggressively. + * + * @unit m/s + * @min 0.0 + * @max 40 + * @decimal 1 + * @increment 0.5 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 10.0f); + +/** + * Maximum Airspeed + * + * If the airspeed is above this value, the TECS controller will try to decrease + * airspeed more aggressively. + * + * @unit m/s + * @min 0.0 + * @max 40 + * @decimal 1 + * @increment 0.5 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 20.0f); + /** * Maximum climb rate * @@ -221,8 +398,10 @@ PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 10.0f); * FW_THR_MAX reduced. * * @unit m/s - * @min 2.0 - * @max 10.0 + * @min 1.0 + * @max 15.0 + * @decimal 1 + * @increment 0.5 * @group FW TECS */ PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); @@ -235,6 +414,10 @@ PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); * to measure FW_T_CLMB_MAX. * * @unit m/s + * @min 1.0 + * @max 5.0 + * @decimal 1 + * @increment 0.5 * @group FW TECS */ PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f); @@ -249,6 +432,10 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f); * the aircraft. * * @unit m/s + * @min 2.0 + * @max 15.0 + * @decimal 1 + * @increment 0.5 * @group FW TECS */ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); @@ -261,6 +448,10 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); * to respond. * * @unit s + * @min 1.0 + * @max 10.0 + * @decimal 1 + * @increment 0.5 * @group FW TECS */ PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f); @@ -273,6 +464,10 @@ PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f); * to respond. * * @unit s + * @min 1.0 + * @max 10.0 + * @decimal 1 + * @increment 0.5 * @group FW TECS */ PARAM_DEFINE_FLOAT(FW_T_THRO_CONST, 8.0f); @@ -283,6 +478,10 @@ PARAM_DEFINE_FLOAT(FW_T_THRO_CONST, 8.0f); * This is the damping gain for the throttle demand loop. * Increase to add damping to correct for oscillations in speed and height. * + * @min 0.0 + * @max 2.0 + * @decimal 1 + * @increment 0.1 * @group FW TECS */ PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f); @@ -295,6 +494,10 @@ PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f); * and height offsets are trimmed out, but reduces damping and * increases overshoot. * + * @min 0.0 + * @max 2.0 + * @decimal 2 + * @increment 0.05 * @group FW TECS */ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); @@ -309,6 +512,10 @@ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); * from under-speed conditions. * * @unit m/s/s + * @min 1.0 + * @max 10.0 + * @decimal 1 + * @increment 0.5 * @group FW TECS */ PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); @@ -323,6 +530,10 @@ PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); * the solution more towards use of the accelerometer data. * * @unit rad/s + * @min 1.0 + * @max 10.0 + * @decimal 1 + * @increment 0.5 * @group FW TECS */ PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f); @@ -333,10 +544,14 @@ PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f); * This is the cross-over frequency (in radians/second) of the complementary * filter used to fuse longitudinal acceleration and airspeed to obtain an * improved airspeed estimate. Increasing this frequency weights the solution - * more towards use of the arispeed sensor, whilst reducing it weights the + * more towards use of the airspeed sensor, whilst reducing it weights the * solution more towards use of the accelerometer data. * * @unit rad/s + * @min 1.0 + * @max 10.0 + * @decimal 1 + * @increment 0.5 * @group FW TECS */ PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f); @@ -353,6 +568,10 @@ PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f); * aircraft (eg powered sailplanes) can use a lower value, whereas * inefficient low aspect-ratio models (eg delta wings) can use a higher value. * + * @min 0.0 + * @max 20.0 + * @decimal 1 + * @increment 0.5 * @group FW TECS */ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 15.0f); @@ -373,6 +592,8 @@ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 15.0f); * * @min 0.0 * @max 2.0 + * @decimal 1 + * @increment 1.0 * @group FW TECS */ PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f); @@ -385,6 +606,10 @@ PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f); * will work well provided the pitch to servo controller has been tuned * properly. * + * @min 0.0 + * @max 2.0 + * @decimal 1 + * @increment 0.1 * @group FW TECS */ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f); @@ -392,6 +617,10 @@ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f); /** * Height rate P factor * + * @min 0.0 + * @max 2.0 + * @decimal 2 + * @increment 0.05 * @group FW TECS */ PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f); @@ -399,6 +628,10 @@ PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f); /** * Height rate FF factor * + * @min 0.0 + * @max 2.0 + * @decimal 2 + * @increment 0.05 * @group FW TECS */ PARAM_DEFINE_FLOAT(FW_T_HRATE_FF, 0.0f); @@ -406,107 +639,10 @@ PARAM_DEFINE_FLOAT(FW_T_HRATE_FF, 0.0f); /** * Speed rate P factor * + * @min 0.0 + * @max 2.0 + * @decimal 2 + * @increment 0.01 * @group FW TECS */ PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.02f); - -/** - * Landing slope angle - * - * @unit deg - * @min 1.0 - * @max 15.0 - * @group FW L1 Control - */ -PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f); - -/** - * - * - * @unit m - * @group FW L1 Control - */ -PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f); - -/** - * Landing flare altitude (relative to landing altitude) - * - * @unit m - * @min 0.0 - * @max 25.0 - * @group FW L1 Control - */ -PARAM_DEFINE_FLOAT(FW_LND_FLALT, 8.0f); - -/** - * Landing throttle limit altitude (relative landing altitude) - * - * Default of -1.0 lets the system default to applying throttle - * limiting at 2/3 of the flare altitude. - * - * @unit m - * @min -1.0 - * @max 30.0 - * @group FW L1 Control - */ -PARAM_DEFINE_FLOAT(FW_LND_TLALT, -1.0f); - -/** - * Landing heading hold horizontal distance - * - * @unit m - * @min 0 - * @max 30.0 - * @group FW L1 Control - */ -PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); - -/** - * Enable or disable usage of terrain estimate during landing - * - * 0: disabled, 1: enabled - * - * @boolean - * @group FW L1 Control - */ -PARAM_DEFINE_INT32(FW_LND_USETER, 0); - -/** - * Flare, minimum pitch - * - * Minimum pitch during flare, a positive sign means nose up - * Applied once FW_LND_TLALT is reached - * - * @unit deg - * @min 0 - * @max 15.0 - * @group FW L1 Control - * - */ -PARAM_DEFINE_FLOAT(FW_LND_FL_PMIN, 2.5f); - -/** - * Flare, maximum pitch - * - * Maximum pitch during flare, a positive sign means nose up - * Applied once FW_LND_TLALT is reached - * - * @unit deg - * @min 0 - * @max 45.0 - * @group FW L1 Control - * - */ -PARAM_DEFINE_FLOAT(FW_LND_FL_PMAX, 15.0f); - -/** - * Landing airspeed scale factor - * - * Multiplying this factor with the minimum airspeed of the plane - * gives the target airspeed the landing approach. - * - * @min 1.0 - * @max 1.5 - * @group FW L1 Control - */ -PARAM_DEFINE_FLOAT(FW_LND_AIRSPD_SC, 1.3f); diff --git a/src/modules/fw_pos_control_l1/landingslope.cpp b/src/modules/fw_pos_control_l1/landingslope.cpp index 02f6ab47f7..56d7cfb434 100644 --- a/src/modules/fw_pos_control_l1/landingslope.cpp +++ b/src/modules/fw_pos_control_l1/landingslope.cpp @@ -46,6 +46,19 @@ #include #include +Landingslope::Landingslope() : + _landing_slope_angle_rad(0.0f), + _flare_relative_alt(0.0f), + _motor_lim_relative_alt(0.0f), + _H1_virt(0.0f), + _H0(0.0f), + _d1(0.0f), + _flare_constant(0.0f), + _flare_length(0.0f), + _horizontal_slope_displacement(0.0f) +{ +} + void Landingslope::update(float landing_slope_angle_rad_new, float flare_relative_alt_new, float motor_lim_relative_alt_new, @@ -69,7 +82,7 @@ void Landingslope::calculateSlopeValues() _horizontal_slope_displacement = (_flare_length - _d1); } -float Landingslope::getLandingSlopeRelativeAltitude(float wp_landing_distance) +float Landingslope::getLandingSlopeRelativeAltitude(float wp_landing_distance) { return Landingslope::getLandingSlopeRelativeAltitude(wp_landing_distance, _horizontal_slope_displacement, _landing_slope_angle_rad); diff --git a/src/modules/fw_pos_control_l1/landingslope.h b/src/modules/fw_pos_control_l1/landingslope.h index f7b7d7d960..b801b40f8b 100644 --- a/src/modules/fw_pos_control_l1/landingslope.h +++ b/src/modules/fw_pos_control_l1/landingslope.h @@ -60,7 +60,7 @@ private: void calculateSlopeValues(); public: - Landingslope() {} + Landingslope(); ~Landingslope() {} diff --git a/src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp b/src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp index 58795edb65..66dbf81f70 100644 --- a/src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp @@ -41,10 +41,11 @@ #include "limitoverride.h" -namespace fwPosctrl { +namespace fwPosctrl +{ bool LimitOverride::applyOverride(BlockOutputLimiter &outputLimiterThrottle, - BlockOutputLimiter &outputLimiterPitch) + BlockOutputLimiter &outputLimiterPitch) { bool ret = false; @@ -52,14 +53,17 @@ bool LimitOverride::applyOverride(BlockOutputLimiter &outputLimiterThrottle, outputLimiterThrottle.setMin(overrideThrottleMin); ret = true; } + if (overrideThrottleMaxEnabled) { outputLimiterThrottle.setMax(overrideThrottleMax); ret = true; } + if (overridePitchMinEnabled) { outputLimiterPitch.setMin(overridePitchMin); ret = true; } + if (overridePitchMaxEnabled) { outputLimiterPitch.setMax(overridePitchMax); ret = true; diff --git a/src/modules/fw_pos_control_l1/mtecs/limitoverride.h b/src/modules/fw_pos_control_l1/mtecs/limitoverride.h index 64c2e7bbde..5b2ea9c53a 100644 --- a/src/modules/fw_pos_control_l1/mtecs/limitoverride.h +++ b/src/modules/fw_pos_control_l1/mtecs/limitoverride.h @@ -69,20 +69,32 @@ public: * @return true if the limit was applied */ bool applyOverride(BlockOutputLimiter &outputLimiterThrottle, - BlockOutputLimiter &outputLimiterPitch); + BlockOutputLimiter &outputLimiterPitch); /* Functions to enable or disable the override */ - void enableThrottleMinOverride(float value) { enable(&overrideThrottleMinEnabled, - &overrideThrottleMin, value); } + void enableThrottleMinOverride(float value) + { + enable(&overrideThrottleMinEnabled, + &overrideThrottleMin, value); + } void disableThrottleMinOverride() { disable(&overrideThrottleMinEnabled); } - void enableThrottleMaxOverride(float value) { enable(&overrideThrottleMaxEnabled, - &overrideThrottleMax, value); } + void enableThrottleMaxOverride(float value) + { + enable(&overrideThrottleMaxEnabled, + &overrideThrottleMax, value); + } void disableThrottleMaxOverride() { disable(&overrideThrottleMaxEnabled); } - void enablePitchMinOverride(float value) { enable(&overridePitchMinEnabled, - &overridePitchMin, value); } + void enablePitchMinOverride(float value) + { + enable(&overridePitchMinEnabled, + &overridePitchMin, value); + } void disablePitchMinOverride() { disable(&overridePitchMinEnabled); } - void enablePitchMaxOverride(float value) { enable(&overridePitchMaxEnabled, - &overridePitchMax, value); } + void enablePitchMaxOverride(float value) + { + enable(&overridePitchMaxEnabled, + &overridePitchMax, value); + } void disablePitchMaxOverride() { disable(&overridePitchMaxEnabled); } protected: diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 7a045fb1bb..333d8a5b8f 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -44,7 +44,8 @@ #include #include -namespace fwPosctrl { +namespace fwPosctrl +{ mTecs::mTecs() : SuperBlock(NULL, "MT"), @@ -85,11 +86,11 @@ mTecs::~mTecs() } int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, - float airspeedSp, unsigned mode, LimitOverride limitOverride) + float airspeedSp, unsigned mode, LimitOverride limitOverride) { /* check if all input arguments are numbers and abort if not so */ if (!PX4_ISFINITE(flightPathAngle) || !PX4_ISFINITE(altitude) || - !PX4_ISFINITE(altitudeSp) || !PX4_ISFINITE(airspeed) || !PX4_ISFINITE(airspeedSp) || !PX4_ISFINITE(mode)) { + !PX4_ISFINITE(altitudeSp) || !PX4_ISFINITE(airspeed) || !PX4_ISFINITE(airspeedSp) || !PX4_ISFINITE(mode)) { return -1; } @@ -106,7 +107,8 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti /* Debug output */ if (_counter % 10 == 0) { debug("***"); - debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, altitude filtered %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)altitudeFiltered, (double)flightPathAngleSp); + debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, altitude filtered %.4f, flightPathAngleSp %.4f", + (double)altitudeSp, (double)altitude, (double)altitudeFiltered, (double)flightPathAngleSp); } #if defined(__PX4_NUTTX) @@ -118,15 +120,15 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti /* use flightpath angle setpoint for total energy control */ return updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed, - airspeedSp, mode, limitOverride); + airspeedSp, mode, limitOverride); } int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, - float airspeedSp, unsigned mode, LimitOverride limitOverride) + float airspeedSp, unsigned mode, LimitOverride limitOverride) { /* check if all input arguments are numbers and abort if not so */ if (!PX4_ISFINITE(flightPathAngle) || !PX4_ISFINITE(flightPathAngleSp) || - !PX4_ISFINITE(airspeed) || !PX4_ISFINITE(airspeedSp) || !PX4_ISFINITE(mode)) { + !PX4_ISFINITE(airspeed) || !PX4_ISFINITE(airspeedSp) || !PX4_ISFINITE(mode)) { return -1; } @@ -142,9 +144,9 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng /* Debug output */ if (_counter % 10 == 0) { debug("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f airspeedFiltered %.4f," - "accelerationLongitudinalSp%.4f", - (double)airspeedSp, (double)airspeed, - (double)airspeedFiltered, (double)accelerationLongitudinalSp); + "accelerationLongitudinalSp%.4f", + (double)airspeedSp, (double)airspeed, + (double)airspeedFiltered, (double)accelerationLongitudinalSp); } #if defined(__PX4_NUTTX) @@ -164,9 +166,10 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight { /* check if all input arguments are numbers and abort if not so */ if (!PX4_ISFINITE(flightPathAngle) || !PX4_ISFINITE(flightPathAngleSp) || - !PX4_ISFINITE(airspeedFiltered) || !PX4_ISFINITE(accelerationLongitudinalSp) || !PX4_ISFINITE(mode)) { + !PX4_ISFINITE(airspeedFiltered) || !PX4_ISFINITE(accelerationLongitudinalSp) || !PX4_ISFINITE(mode)) { return -1; } + /* time measurement */ updateTimeMeasurement(); @@ -179,9 +182,11 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight /* calculate values (energies) */ float flightPathAngleError = flightPathAngleSp - flightPathAngleFiltered; float airspeedDerivative = 0.0f; - if(_airspeedDerivative.getDt() > 0.0f) { + + if (_airspeedDerivative.getDt() > 0.0f) { airspeedDerivative = _airspeedDerivative.update(airspeedFiltered); } + float airspeedDerivativeNorm = airspeedDerivative / CONSTANTS_ONE_G; float airspeedDerivativeSp = accelerationLongitudinalSp; float airspeedDerivativeNormSp = airspeedDerivativeSp / CONSTANTS_ONE_G; @@ -200,9 +205,11 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight /* Debug output */ if (_counter % 10 == 0) { debug("totalEnergyRateSp %.2f, totalEnergyRate %.2f, totalEnergyRateError %.2f totalEnergyRateError2 %.2f airspeedDerivativeNorm %.4f", - (double)totalEnergyRateSp, (double)totalEnergyRate, (double)totalEnergyRateError, (double)totalEnergyRateError2, (double)airspeedDerivativeNorm); + (double)totalEnergyRateSp, (double)totalEnergyRate, (double)totalEnergyRateError, (double)totalEnergyRateError2, + (double)airspeedDerivativeNorm); debug("energyDistributionRateSp %.2f, energyDistributionRate %.2f, energyDistributionRateError %.2f energyDistributionRateError2 %.2f", - (double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError, (double)energyDistributionRateError2); + (double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError, + (double)energyDistributionRateError2); } /* Check airspeed: if below safe value switch to underspeed mode (if not in land or takeoff mode) */ @@ -213,15 +220,19 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight /* Set special output limiters if we are not in MTECS_MODE_NORMAL */ BlockOutputLimiter *outputLimiterThrottle = &_controlTotalEnergy.getOutputLimiter(); BlockOutputLimiter *outputLimiterPitch = &_controlEnergyDistribution.getOutputLimiter(); + if (mode == MTECS_MODE_TAKEOFF) { outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle; outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch; + } else if (mode == MTECS_MODE_LAND) { // only limit pitch but do not limit throttle outputLimiterPitch = &_BlockOutputLimiterLandPitch; + } else if (mode == MTECS_MODE_LAND_THROTTLELIM) { outputLimiterThrottle = &_BlockOutputLimiterLandThrottle; outputLimiterPitch = &_BlockOutputLimiterLandPitch; + } else if (mode == MTECS_MODE_UNDERSPEED) { outputLimiterThrottle = &_BlockOutputLimiterUnderspeedThrottle; outputLimiterPitch = &_BlockOutputLimiterUnderspeedPitch; @@ -256,9 +267,9 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight if (_counter % 10 == 0) { debug("_throttleSp %.1f, _pitchSp %.1f, flightPathAngleSp %.1f, flightPathAngle %.1f accelerationLongitudinalSp %.1f, airspeedDerivative %.1f", - (double)_throttleSp, (double)_pitchSp, - (double)flightPathAngleSp, (double)flightPathAngle, - (double)accelerationLongitudinalSp, (double)airspeedDerivative); + (double)_throttleSp, (double)_pitchSp, + (double)flightPathAngleSp, (double)flightPathAngle, + (double)accelerationLongitudinalSp, (double)airspeedDerivative); } #if defined(__PX4_NUTTX) @@ -293,11 +304,13 @@ void mTecs::updateTimeMeasurement() { if (!_dtCalculated) { float deltaTSeconds = 0.0f; + if (!_firstIterationAfterReset) { hrt_abstime timestampNow = hrt_absolute_time(); deltaTSeconds = (float)(timestampNow - timestampLastIteration) * 1e-6f; timestampLastIteration = timestampNow; } + setDt(deltaTSeconds); _dtCalculated = true; @@ -312,7 +325,8 @@ void mTecs::debug_print(const char *fmt, va_list args) fprintf(stderr, "\n"); } -void mTecs::debug(const char *fmt, ...) { +void mTecs::debug(const char *fmt, ...) +{ if (!_debug) { return; diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h index bb20654728..1363d121f7 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h @@ -68,21 +68,25 @@ public: * * @param value is changed to be on the interval _min to _max * @param difference if the value is changed this corresponds to the change of value * (-1) - * otherwise unchanged + * otherwise unchanged * @return: true if the limit is applied, false otherwise */ - bool limit(float& value, float& difference) { + bool limit(float &value, float &difference) + { float minimum = getIsAngularLimit() ? getMin() * M_DEG_TO_RAD_F : getMin(); float maximum = getIsAngularLimit() ? getMax() * M_DEG_TO_RAD_F : getMax(); + if (value < minimum) { difference = value - minimum; value = minimum; return true; + } else if (value > maximum) { difference = value - maximum; value = maximum; return true; } + return false; } //accessor: @@ -126,15 +130,18 @@ protected: BlockOutputLimiter _outputLimiter; float calcUnlimitedOutput(float inputValue, float inputError) {return getOffset() + getKFF() * inputValue + getKP() * inputError + getKI() * getIntegral().update(inputError);} - float calcLimitedOutput(float inputValue, float inputError, BlockOutputLimiter &outputLimiter) { + float calcLimitedOutput(float inputValue, float inputError, BlockOutputLimiter &outputLimiter) + { float difference = 0.0f; float integralYPrevious = _integral.getY(); float output = calcUnlimitedOutput(inputValue, inputError); - if(outputLimiter.limit(output, difference) && - (((difference < 0) && (getKI() * getIntegral().getY() < 0)) || - ((difference > 0) && (getKI() * getIntegral().getY() > 0)))) { - getIntegral().setY(integralYPrevious); + + if (outputLimiter.limit(output, difference) && + (((difference < 0) && (getKI() * getIntegral().getY() < 0)) || + ((difference > 0) && (getKI() * getIntegral().getY() > 0)))) { + getIntegral().setY(integralYPrevious); } + return output; } private: @@ -152,9 +159,10 @@ public: // methods BlockFFPILimitedCustom(SuperBlock *parent, const char *name, bool isAngularLimit = false) : BlockFFPILimited(parent, name, isAngularLimit) - {}; + {}; virtual ~BlockFFPILimitedCustom() {}; - float update(float inputValue, float inputError, BlockOutputLimiter *outputLimiter = NULL) { + float update(float inputValue, float inputError, BlockOutputLimiter *outputLimiter = NULL) + { return calcLimitedOutput(inputValue, inputError, outputLimiter == NULL ? _outputLimiter : *outputLimiter); } }; @@ -170,7 +178,8 @@ public: _outputLimiter(this, "", isAngularLimit) {}; virtual ~BlockPLimited() {}; - float update(float input) { + float update(float input) + { float difference = 0.0f; float output = getKP() * input; getOutputLimiter().limit(output, difference); @@ -197,7 +206,8 @@ public: _outputLimiter(this, "", isAngularLimit) {}; virtual ~BlockPDLimited() {}; - float update(float input) { + float update(float input) + { float difference = 0.0f; float output = getKP() * input + (getDerivative().getDt() > 0.0f ? getKD() * getDerivative().update(input) : 0.0f); getOutputLimiter().limit(output, difference); diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index dd457b664a..a6a7b210c0 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -45,8 +45,6 @@ /** * mTECS enabled * - * Set to 1 to enable mTECS - * * @boolean * @group mTECS */ diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index af3cfaa61f..0721e68e8e 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -31,22 +31,23 @@ * ****************************************************************************/ -/** +/* * @file FixedwingLandDetector.cpp - * Land detection algorithm for fixedwings * * @author Johan Jansen * @author Lorenz Meier + * @author Julian Oes */ -#include "FixedwingLandDetector.h" - #include #include #include #include -namespace landdetection +#include "FixedwingLandDetector.h" + + +namespace land_detector { FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), @@ -58,12 +59,10 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), _controlState{}, _arming{}, _airspeed{}, - _parameterSub(-1), _velocity_xy_filtered(0.0f), _velocity_z_filtered(0.0f), _airspeed_filtered(0.0f), - _accel_horz_lp(0.0f), - _landDetectTrigger(0) + _accel_horz_lp(0.0f) { _paramHandle.maxVelocity = param_find("LNDFW_VEL_XY_MAX"); _paramHandle.maxClimbRate = param_find("LNDFW_VEL_Z_MAX"); @@ -71,54 +70,41 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), _paramHandle.maxIntVelocity = param_find("LNDFW_VELI_MAX"); } -void FixedwingLandDetector::initialize() +void FixedwingLandDetector::_initialize_topics() { _controlStateSub = orb_subscribe(ORB_ID(control_state)); _armingSub = orb_subscribe(ORB_ID(actuator_armed)); _airspeedSub = orb_subscribe(ORB_ID(airspeed)); - - updateParameterCache(true); } -void FixedwingLandDetector::updateSubscriptions() +void FixedwingLandDetector::_update_topics() { - orb_update(ORB_ID(control_state), _controlStateSub, &_controlState); - orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); - orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); + _orb_update(ORB_ID(control_state), _controlStateSub, &_controlState); + _orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); + _orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); } -LandDetectionResult FixedwingLandDetector::update() +void FixedwingLandDetector::_update_params() { - // First poll for new data from our subscriptions - updateSubscriptions(); - - if (get_freefall_state()) { - _state = LANDDETECTION_RES_FREEFALL; - - } else if (get_landed_state()) { - _state = LANDDETECTION_RES_LANDED; - - } else { - _state = LANDDETECTION_RES_FLYING; - } - - return _state; + param_get(_paramHandle.maxVelocity, &_params.maxVelocity); + param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate); + param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed); + param_get(_paramHandle.maxIntVelocity, &_params.maxIntVelocity); } -bool FixedwingLandDetector::get_freefall_state() +bool FixedwingLandDetector::_get_freefall_state() { - //TODO + // TODO return false; } -bool FixedwingLandDetector::get_landed_state() +bool FixedwingLandDetector::_get_landed_state() { // only trigger flight conditions if we are armed if (!_arming.armed) { return true; } - const uint64_t now = hrt_absolute_time(); bool landDetected = false; if (hrt_elapsed_time(&_controlState.timestamp) < 500 * 1000) { @@ -147,14 +133,10 @@ bool FixedwingLandDetector::get_landed_state() && _airspeed_filtered < _params.maxAirSpeed && _accel_horz_lp < _params.maxIntVelocity) { - // these conditions need to be stable for a period of time before we trust them - if (now > _landDetectTrigger) { - landDetected = true; - } + landDetected = true; } else { - // reset land detect trigger - _landDetectTrigger = now + LAND_DETECTOR_TRIGGER_TIME; + landDetected = false; } } else { @@ -165,23 +147,4 @@ bool FixedwingLandDetector::get_landed_state() return landDetected; } -void FixedwingLandDetector::updateParameterCache(const bool force) -{ - bool updated; - parameter_update_s paramUpdate; - - orb_check(_parameterSub, &updated); - - if (updated) { - orb_copy(ORB_ID(parameter_update), _parameterSub, ¶mUpdate); - } - - if (updated || force) { - param_get(_paramHandle.maxVelocity, &_params.maxVelocity); - param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate); - param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed); - param_get(_paramHandle.maxIntVelocity, &_params.maxIntVelocity); - } -} - -} +} // namespace land_detector diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index f450f94b82..e52ce1d95e 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2016 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,23 +33,22 @@ /** * @file FixedwingLandDetector.h - * Land detection algorithm for fixedwing + * Land detector implementation for fixedwing. * * @author Johan Jansen + * @author Morten Lysgaard + * @author Julian Oes */ -#ifndef __FIXED_WING_LAND_DETECTOR_H__ -#define __FIXED_WING_LAND_DETECTOR_H__ +#pragma once -#include "LandDetector.h" #include #include -#include -#include #include -#include -namespace landdetection +#include "LandDetector.h" + +namespace land_detector { class FixedwingLandDetector : public LandDetector @@ -58,46 +57,22 @@ public: FixedwingLandDetector(); protected: - /** - * @brief blocking loop, should be run in a separate thread or task. Runs at 50Hz - **/ - LandDetectionResult update() override; + virtual void _initialize_topics() override; - /** - * @brief Initializes the land detection algorithm - **/ - void initialize() override; + virtual void _update_params() override; - /** - * @brief polls all subscriptions and pulls any data that has changed - **/ - void updateSubscriptions(); + virtual void _update_topics() override; - /** - * @brief get UAV landed state - **/ - bool get_landed_state(); - - /** - * @brief returns true if UAV is in free-fall state - **/ - bool get_freefall_state(); + virtual bool _get_landed_state() override; + virtual bool _get_freefall_state() override; private: - /** - * @brief download and update local parameter cache - **/ - void updateParameterCache(const bool force); - - /** - * @brief Handles for interesting parameters - **/ struct { param_t maxVelocity; param_t maxClimbRate; param_t maxAirSpeed; param_t maxIntVelocity; - } _paramHandle; + } _paramHandle; struct { float maxVelocity; @@ -106,22 +81,18 @@ private: float maxIntVelocity; } _params; -private: - int _controlStateSub; /**< notification of local position */ - int _armingSub; - int _airspeedSub; - struct control_state_s _controlState; /**< the result from local position subscription */ - struct actuator_armed_s _arming; - struct airspeed_s _airspeed; - int _parameterSub; + int _controlStateSub; + int _armingSub; + int _airspeedSub; + + struct control_state_s _controlState; + struct actuator_armed_s _arming; + struct airspeed_s _airspeed; float _velocity_xy_filtered; float _velocity_z_filtered; float _airspeed_filtered; float _accel_horz_lp; - uint64_t _landDetectTrigger; }; -} - -#endif //__FIXED_WING_LAND_DETECTOR_H__ +} // namespace land_detector diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index f55d5b83d7..5ed6961b59 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -31,32 +31,35 @@ * ****************************************************************************/ -/** +/* * @file LandDetector.cpp - * Land detection algorithm * * @author Johan Jansen - * @author Morten Lysgaard + * @author Julian Oes */ -#include "LandDetector.h" -#include //usleep -#include #include #include +#include -namespace landdetection +#include "LandDetector.h" + + +namespace land_detector { + LandDetector::LandDetector() : - _landDetectedPub(0), + _landDetectedPub(nullptr), _landDetected{0, false, false}, - _arming_time(0), + _freefall_hysteresis(false), + _landed_hysteresis(true), _taskShouldExit(false), _taskIsRunning(false), _work{} { - // ctor + // Use Trigger time when transitioning from in-air (false) to landed (true). + _landed_hysteresis.set_hysteresis_time_from(false, LAND_DETECTOR_TRIGGER_TIME_US); } LandDetector::~LandDetector() @@ -67,64 +70,114 @@ LandDetector::~LandDetector() int LandDetector::start() { + _taskShouldExit = false; + /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, 0); + work_queue(HPWORK, &_work, (worker_t)&LandDetector::_cycle_trampoline, this, 0); return 0; } -void LandDetector::shutdown() +void LandDetector::stop() { _taskShouldExit = true; } void -LandDetector::cycle_trampoline(void *arg) +LandDetector::_cycle_trampoline(void *arg) { LandDetector *dev = reinterpret_cast(arg); - dev->cycle(); + dev->_cycle(); } -void LandDetector::cycle() +void LandDetector::_cycle() { if (!_taskIsRunning) { - // advertise the first land detected uORB + // Advertise the first land detected uORB. _landDetected.timestamp = hrt_absolute_time(); _landDetected.landed = false; _landDetected.freefall = false; - _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); - // initialize land detection algorithm - initialize(); + // Initialize uORB topics. + _initialize_topics(); - // task is now running, keep doing so until shutdown() has been called + _check_params(true); + + // Task is now running, keep doing so until we need to stop. _taskIsRunning = true; - _taskShouldExit = false; } - LandDetectionResult current_state = update(); - bool landDetected = (current_state == LANDDETECTION_RES_LANDED); - bool freefallDetected = (current_state == LANDDETECTION_RES_FREEFALL); + _check_params(false); + + _update_topics(); + + _update_state(); + + bool landDetected = (_state == LandDetectionState::LANDED); + bool freefallDetected = (_state == LandDetectionState::FREEFALL); + + // Only publish very first time or when the result has changed. + if ((_landDetectedPub == nullptr) || + (_landDetected.landed != landDetected) || + (_landDetected.freefall != freefallDetected)) { - // publish if land detection state has changed - if ((_landDetected.landed != landDetected) || (_landDetected.freefall != freefallDetected)) { _landDetected.timestamp = hrt_absolute_time(); - _landDetected.landed = landDetected; - _landDetected.freefall = freefallDetected; + _landDetected.landed = (_state == LandDetectionState::LANDED); + _landDetected.freefall = (_state == LandDetectionState::FREEFALL); - // publish the land detected broadcast - orb_publish(ORB_ID(vehicle_land_detected), (orb_advert_t)_landDetectedPub, &_landDetected); - //warnx("in air status changed: %s", (_landDetected.landed) ? "LANDED" : "TAKEOFF"); + int instance; + orb_publish_auto(ORB_ID(vehicle_land_detected), &_landDetectedPub, &_landDetected, + &instance, ORB_PRIO_DEFAULT); } if (!_taskShouldExit) { - work_queue(HPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, - USEC2TICK(1000000 / LAND_DETECTOR_UPDATE_RATE)); + + // Schedule next cycle. + work_queue(HPWORK, &_work, (worker_t)&LandDetector::_cycle_trampoline, this, + USEC2TICK(1000000 / LAND_DETECTOR_UPDATE_RATE_HZ)); + + } else { + _taskIsRunning = false; } } -bool LandDetector::orb_update(const struct orb_metadata *meta, int handle, void *buffer) +void LandDetector::_check_params(const bool force) +{ + bool updated; + parameter_update_s paramUpdate; + + orb_check(_parameterSub, &updated); + + if (updated) { + orb_copy(ORB_ID(parameter_update), _parameterSub, ¶mUpdate); + } + + if (updated || force) { + _update_params(); + } +} + +void LandDetector::_update_state() +{ + bool landed = _get_landed_state(); + _landed_hysteresis.set_state_and_update(landed); + _freefall_hysteresis.set_state_and_update(_get_freefall_state()); + + if (_freefall_hysteresis.get_state()) { + _state = LandDetectionState::FREEFALL; + + } else if (_landed_hysteresis.get_state()) { + _state = LandDetectionState::LANDED; + + } else { + _state = LandDetectionState::FLYING; + } + + return; +} + +bool LandDetector::_orb_update(const struct orb_metadata *meta, int handle, void *buffer) { bool newData = false; @@ -144,4 +197,5 @@ bool LandDetector::orb_update(const struct orb_metadata *meta, int handle, void return true; } -} + +} // namespace land_detector diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index 05b229b7a5..1b71e8d475 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2016 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 @@ -31,101 +31,131 @@ * ****************************************************************************/ -/** +/* * @file LandDetector.h - * Abstract interface for land detector algorithms +Land detector interface for multicopter, fixedwing and VTOL implementations. * * @author Johan Jansen + * @author Julian Oes */ -#ifndef __LAND_DETECTOR_H__ -#define __LAND_DETECTOR_H__ +#pragma once #include +#include #include #include -namespace landdetection +namespace land_detector { -enum LandDetectionResult { - LANDDETECTION_RES_FLYING = 0, /**< UAV is flying */ - LANDDETECTION_RES_LANDED = 1, /**< Land has been detected */ - LANDDETECTION_RES_FREEFALL = 2 /**< Free-fall has been detected */ -}; class LandDetector { public: + enum class LandDetectionState { + FLYING = 0, + LANDED = 1, + FREEFALL = 2 + }; LandDetector(); virtual ~LandDetector(); - /** - * @return true if this task is currently running - **/ - inline bool isRunning() const {return _taskIsRunning;} - - /** - * @return the current landed state + /* + * @return true if this task is currently running. */ - bool isLanded() { return _landDetected.landed; } + inline bool is_running() const + { + return _taskIsRunning; + } - /** - * @brief Tells the Land Detector task that it should exit - **/ - void shutdown(); - /** - * @brief Blocking function that should be called from it's own task thread. This method will - * run the underlying algorithm at the desired update rate and publish if the landing state changes. - **/ + /* + * @return current state. + */ + LandDetectionState get_state() const + { + return _state; + } + + /* + * Tells the task that it should exit. + */ + void stop(); + + /* + * Get the work queue going. + */ int start(); - static void cycle_trampoline(void *arg); - protected: + /* + * Called once to initialize uORB topics. + */ + virtual void _initialize_topics() = 0; - /** - * @brief Pure abstract method that must be overriden by sub-classes. This actually runs the underlying algorithm - * @return true if a landing was detected and this should be broadcast to the rest of the system - **/ - virtual LandDetectionResult update() = 0; + /* + * Update uORB topics. + */ + virtual void _update_topics() = 0; - /** - * @brief Pure abstract method that is called by this class once for initializing the uderlying algorithm (memory allocation, - * uORB topic subscription, creating file descriptors, etc.) - **/ - virtual void initialize() = 0; - /** - * @brief Convenience function for polling uORB subscriptions - * @return true if there was new data and it was successfully copied - **/ - bool orb_update(const struct orb_metadata *meta, int handle, void *buffer); + /* + * Update parameters. + */ + virtual void _update_params() = 0; - static constexpr uint32_t LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */ + /* + * @return true if UAV is in a landed state. + */ + virtual bool _get_landed_state() = 0; - static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold - before triggering a land */ - static constexpr uint64_t LAND_DETECTOR_ARM_PHASE_TIME = - 2000000; /**< time interval in which wider acceptance thresholds are used after arming */ + /* + * @return true if UAV is in free-fall state. + */ + virtual bool _get_freefall_state() = 0; -protected: - orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ - struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ - uint64_t _arming_time; /**< timestamp of arming time */ - LandDetectionResult - _state; /**< Result of land detection. Can be LANDDETECTION_RES_FLYING, LANDDETECTION_RES_LANDED or LANDDETECTION_RES_FREEFALL */ + /* + * Convenience function for polling uORB subscriptions. + * + * @return true if there was new data and it was successfully copied + */ + static bool _orb_update(const struct orb_metadata *meta, int handle, void *buffer); + + /* Run main land detector loop at this rate in Hz. */ + static constexpr uint32_t LAND_DETECTOR_UPDATE_RATE_HZ = 50; + + /* Time in us that landing conditions have to hold before triggering a land. */ + static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME_US = 2000000; + + /* Time interval in us in which wider acceptance thresholds are used after arming. */ + static constexpr uint64_t LAND_DETECTOR_ARM_PHASE_TIME_US = 2000000; + + orb_advert_t _landDetectedPub; + struct vehicle_land_detected_s _landDetected; + + int _parameterSub; + + LandDetectionState _state; + + systemlib::Hysteresis _freefall_hysteresis; + systemlib::Hysteresis _landed_hysteresis; private: - bool _taskShouldExit; /**< true if it is requested that this task should exit */ - bool _taskIsRunning; /**< task has reached main loop and is currently running */ - struct work_s _work; + static void _cycle_trampoline(void *arg); - void cycle(); + void _cycle(); + + void _check_params(const bool force); + + void _update_state(); + + bool _taskShouldExit; + bool _taskIsRunning; + + struct work_s _work; }; -} -#endif //__LAND_DETECTOR_H__ +} // namespace land_detector diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index b6eeb92dab..7bd347b422 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -33,19 +33,20 @@ /** * @file MulticopterLandDetector.cpp - * Land detection algorithm for multicopters * * @author Johan Jansen * @author Morten Lysgaard + * @author Julian Oes */ -#include "MulticopterLandDetector.h" - #include #include #include -namespace landdetection +#include "MulticopterLandDetector.h" + + +namespace land_detector { MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), @@ -54,7 +55,6 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _vehicleLocalPositionSub(-1), _actuatorsSub(-1), _armingSub(-1), - _parameterSub(-1), _attitudeSub(-1), _manualSub(-1), _ctrl_state_sub(-1), @@ -63,10 +63,11 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _actuators{}, _arming{}, _vehicleAttitude{}, + _manual{}, _ctrl_state{}, _ctrl_mode{}, - _landTimer(0), - _freefallTimer(0) + _min_trust_start(0), + _arming_time(0) { _paramHandle.maxRotation = param_find("LNDMC_ROT_MAX"); _paramHandle.maxVelocity = param_find("LNDMC_XY_VEL_MAX"); @@ -77,7 +78,7 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _paramHandle.ff_trigger_time = param_find("LNDMC_FFALL_TTRI"); } -void MulticopterLandDetector::initialize() +void MulticopterLandDetector::_initialize_topics() { // subscribe to position, attitude, arming and velocity changes _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position)); @@ -88,67 +89,54 @@ void MulticopterLandDetector::initialize() _manualSub = orb_subscribe(ORB_ID(manual_control_setpoint)); _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - - // download parameters - updateParameterCache(true); } -void MulticopterLandDetector::updateSubscriptions() +void MulticopterLandDetector::_update_topics() { - orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition); - orb_update(ORB_ID(vehicle_attitude), _attitudeSub, &_vehicleAttitude); - orb_update(ORB_ID(actuator_controls_0), _actuatorsSub, &_actuators); - orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); - orb_update(ORB_ID(manual_control_setpoint), _manualSub, &_manual); - orb_update(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); - orb_update(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_ctrl_mode); + _orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition); + _orb_update(ORB_ID(vehicle_attitude), _attitudeSub, &_vehicleAttitude); + _orb_update(ORB_ID(actuator_controls_0), _actuatorsSub, &_actuators); + _orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); + _orb_update(ORB_ID(manual_control_setpoint), _manualSub, &_manual); + _orb_update(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); + _orb_update(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_ctrl_mode); } -LandDetectionResult MulticopterLandDetector::update() +void MulticopterLandDetector::_update_params() { - // first poll for new data from our subscriptions - updateSubscriptions(); - - updateParameterCache(false); - - if (get_freefall_state()) { - _state = LANDDETECTION_RES_FREEFALL; - - } else if (get_landed_state()) { - _state = LANDDETECTION_RES_LANDED; - - } else { - _state = LANDDETECTION_RES_FLYING; - } - - return _state; + param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate); + param_get(_paramHandle.maxVelocity, &_params.maxVelocity); + param_get(_paramHandle.maxRotation, &_params.maxRotation_rad_s); + _params.maxRotation_rad_s = math::radians(_params.maxRotation_rad_s); + param_get(_paramHandle.maxThrottle, &_params.maxThrottle); + param_get(_paramHandle.minManThrottle, &_params.minManThrottle); + param_get(_paramHandle.acc_threshold_m_s2, &_params.acc_threshold_m_s2); + param_get(_paramHandle.ff_trigger_time, &_params.ff_trigger_time); + _freefall_hysteresis.set_hysteresis_time_from(false, 1e6f * _params.ff_trigger_time); } -bool MulticopterLandDetector::get_freefall_state() + +bool MulticopterLandDetector::_get_freefall_state() { if (_params.acc_threshold_m_s2 < 0.1f || _params.acc_threshold_m_s2 > 10.0f) { //if parameter is set to zero or invalid, disable free-fall detection. return false; } - const uint64_t now = hrt_absolute_time(); + if (_ctrl_state.timestamp == 0) { + // _ctrl_state is not valid yet, we have to assume we're not falling. + return false; + } float acc_norm = _ctrl_state.x_acc * _ctrl_state.x_acc + _ctrl_state.y_acc * _ctrl_state.y_acc + _ctrl_state.z_acc * _ctrl_state.z_acc; acc_norm = sqrtf(acc_norm); //norm of specific force. Should be close to 9.8 m/s^2 when landed. - bool freefall = (acc_norm < _params.acc_threshold_m_s2); //true if we are currently falling - - if (!freefall || _freefallTimer == 0) { //reset timer if uav not falling - _freefallTimer = now; - return false; - } - - return (now - _freefallTimer) / 1000000.0f > _params.ff_trigger_time; + return (acc_norm < _params.acc_threshold_m_s2); //true if we are currently falling } -bool MulticopterLandDetector::get_landed_state() +bool MulticopterLandDetector::_get_landed_state() { // Time base for this function const uint64_t now = hrt_absolute_time(); @@ -173,6 +161,7 @@ bool MulticopterLandDetector::get_landed_state() // only trigger flight conditions if we are armed if (!_arming.armed) { _arming_time = 0; + return true; } else if (_arming_time == 0) { @@ -199,7 +188,8 @@ bool MulticopterLandDetector::get_landed_state() // quite acrobatic flight. if ((_min_trust_start > 0) && (hrt_elapsed_time(&_min_trust_start) > 8 * 1000 * 1000)) { - return !get_freefall_state(); + + return true; } else { return false; @@ -210,7 +200,7 @@ bool MulticopterLandDetector::get_landed_state() // Widen acceptance thresholds for landed state right after arming // so that motor spool-up and other effects do not trigger false negatives. - if (hrt_elapsed_time(&_arming_time) < LAND_DETECTOR_ARM_PHASE_TIME) { + if (hrt_elapsed_time(&_arming_time) < LAND_DETECTOR_ARM_PHASE_TIME_US) { armThresholdFactor = 2.5f; } @@ -233,34 +223,11 @@ bool MulticopterLandDetector::get_landed_state() if (verticalMovement || rotating || !minimalThrust || horizontalMovement) { // Sensed movement or thottle high, so reset the land detector. - _landTimer = now; return false; } - return (now - _landTimer > LAND_DETECTOR_TRIGGER_TIME); + return true; } -void MulticopterLandDetector::updateParameterCache(const bool force) -{ - bool updated; - parameter_update_s paramUpdate; - - orb_check(_parameterSub, &updated); - - if (updated) { - orb_copy(ORB_ID(parameter_update), _parameterSub, ¶mUpdate); - } - - if (updated || force) { - param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate); - param_get(_paramHandle.maxVelocity, &_params.maxVelocity); - param_get(_paramHandle.maxRotation, &_params.maxRotation_rad_s); - _params.maxRotation_rad_s = math::radians(_params.maxRotation_rad_s); - param_get(_paramHandle.maxThrottle, &_params.maxThrottle); - param_get(_paramHandle.minManThrottle, &_params.minManThrottle); - param_get(_paramHandle.acc_threshold_m_s2, &_params.acc_threshold_m_s2); - param_get(_paramHandle.ff_trigger_time, &_params.ff_trigger_time); - } -} } diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 20bb611cb7..dbfaf9aa75 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -33,28 +33,28 @@ /** * @file MulticopterLandDetector.h - * Land detection algorithm for multicopters + * Land detection implementation for multicopters. * * @author Johan Jansen * @author Morten Lysgaard + * @author Julian Oes */ -#ifndef __MULTICOPTER_LAND_DETECTOR_H__ -#define __MULTICOPTER_LAND_DETECTOR_H__ +#pragma once -#include "LandDetector.h" +#include #include #include -#include #include #include #include #include #include #include -#include -namespace landdetection +#include "LandDetector.h" + +namespace land_detector { class MulticopterLandDetector : public LandDetector @@ -63,36 +63,15 @@ public: MulticopterLandDetector(); protected: - /** - * @brief polls all subscriptions and pulls any data that has changed - **/ - virtual void updateSubscriptions(); + virtual void _initialize_topics() override; - /** - * @brief Runs one iteration of the land detection algorithm - **/ - virtual LandDetectionResult update() override; + virtual void _update_params() override; - /** - * @brief Initializes the land detection algorithm - **/ - virtual void initialize() override; + virtual void _update_topics() override; - /** - * @brief download and update local parameter cache - **/ - virtual void updateParameterCache(const bool force); - - /** - * @brief get multicopter landed state - **/ - bool get_landed_state(); - - /** - * @brief returns true if multicopter is in free-fall state - **/ - bool get_freefall_state(); + virtual bool _get_landed_state() override; + virtual bool _get_freefall_state() override; private: /** @@ -106,7 +85,7 @@ private: param_t minManThrottle; param_t acc_threshold_m_s2; param_t ff_trigger_time; - } _paramHandle; + } _paramHandle; struct { float maxClimbRate; @@ -118,11 +97,9 @@ private: float ff_trigger_time; } _params; -private: int _vehicleLocalPositionSub; int _actuatorsSub; int _armingSub; - int _parameterSub; int _attitudeSub; int _manualSub; int _ctrl_state_sub; @@ -136,11 +113,9 @@ private: struct control_state_s _ctrl_state; struct vehicle_control_mode_s _ctrl_mode; - uint64_t _landTimer; ///< timestamp in microseconds since a possible land was detected - uint64_t _freefallTimer; ///< timestamp in microseconds since a possible freefall was detected uint64_t _min_trust_start; ///< timestamp when minimum trust was applied first + uint64_t _arming_time; }; -} -#endif //__MULTICOPTER_LAND_DETECTOR_H__ +} // namespace land_detector diff --git a/src/modules/land_detector/VtolLandDetector.cpp b/src/modules/land_detector/VtolLandDetector.cpp index 50a39c5fea..de058d812f 100644 --- a/src/modules/land_detector/VtolLandDetector.cpp +++ b/src/modules/land_detector/VtolLandDetector.cpp @@ -36,19 +36,20 @@ * Land detection algorithm for VTOL * * @author Roman Bapst + * @author Julian Oes */ -#include "VtolLandDetector.h" #include -namespace landdetection +#include "VtolLandDetector.h" + +namespace land_detector { VtolLandDetector::VtolLandDetector() : MulticopterLandDetector(), _paramHandle(), _params(), _airspeedSub(-1), - _parameterSub(-1), _airspeed{}, _was_in_air(false), _airspeed_filtered(0) @@ -56,29 +57,24 @@ VtolLandDetector::VtolLandDetector() : MulticopterLandDetector(), _paramHandle.maxAirSpeed = param_find("LNDFW_AIRSPD_MAX"); } -void VtolLandDetector::initialize() +void VtolLandDetector::_initialize_topics() { - MulticopterLandDetector::initialize(); + MulticopterLandDetector::_initialize_topics(); + _airspeedSub = orb_subscribe(ORB_ID(airspeed)); - - // download parameters - updateParameterCache(true); } -void VtolLandDetector::updateSubscriptions() +void VtolLandDetector::_update_topics() { - MulticopterLandDetector::updateSubscriptions(); + MulticopterLandDetector::_update_topics(); - orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); + _orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); } -LandDetectionResult VtolLandDetector::update() +bool VtolLandDetector::_get_landed_state() { - updateSubscriptions(); - updateParameterCache(false); - // this is returned from the mutlicopter land detector - bool landed = get_landed_state(); + bool landed = MulticopterLandDetector::_get_landed_state(); // for vtol we additionally consider airspeed if (hrt_elapsed_time(&_airspeed.timestamp) < 500 * 1000) { @@ -97,27 +93,19 @@ LandDetectionResult VtolLandDetector::update() _was_in_air = !landed; - _state = (landed) ? LANDDETECTION_RES_LANDED : LANDDETECTION_RES_FLYING; - - return _state; + return landed; } -void VtolLandDetector::updateParameterCache(const bool force) +bool VtolLandDetector::_get_freefall_state() { - MulticopterLandDetector::updateParameterCache(force); + return MulticopterLandDetector::_get_freefall_state(); +} - bool updated; - parameter_update_s paramUpdate; +void VtolLandDetector::_update_params() +{ + MulticopterLandDetector::_update_params(); - orb_check(_parameterSub, &updated); - - if (updated) { - orb_copy(ORB_ID(parameter_update), _parameterSub, ¶mUpdate); - } - - if (updated || force) { - param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed); - } + param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed); } } diff --git a/src/modules/land_detector/VtolLandDetector.h b/src/modules/land_detector/VtolLandDetector.h index a9b409f610..31a1e448fb 100644 --- a/src/modules/land_detector/VtolLandDetector.h +++ b/src/modules/land_detector/VtolLandDetector.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2016 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,18 +33,19 @@ /** * @file VtolLandDetector.h - * Land detection algorithm for vtol + * Land detection implementation for VTOL also called hybrids. * * @author Roman Bapst + * @author Julian Oes */ -#ifndef __VTOL_LAND_DETECTOR_H__ -#define __VTOL_LAND_DETECTOR_H__ +#pragma once -#include "MulticopterLandDetector.h" #include -namespace landdetection +#include "MulticopterLandDetector.h" + +namespace land_detector { class VtolLandDetector : public MulticopterLandDetector @@ -52,41 +53,27 @@ class VtolLandDetector : public MulticopterLandDetector public: VtolLandDetector(); +protected: + virtual void _initialize_topics() override; + + virtual void _update_params() override; + + virtual void _update_topics() override; + + virtual bool _get_landed_state() override; + + virtual bool _get_freefall_state() override; private: - /** - * @brief polls all subscriptions and pulls any data that has changed - **/ - void updateSubscriptions() override; - - /** - * @brief Runs one iteration of the land detection algorithm - **/ - LandDetectionResult update() override; - - /** - * @brief Initializes the land detection algorithm - **/ - void initialize() override; - - /** - * @brief download and update local parameter cache - **/ - void updateParameterCache(const bool force) override; - - /** - * @brief Handles for interesting parameters - **/ struct { param_t maxAirSpeed; - } _paramHandle; + } _paramHandle; struct { float maxAirSpeed; } _params; int _airspeedSub; - int _parameterSub; struct airspeed_s _airspeed; @@ -94,6 +81,5 @@ private: float _airspeed_filtered; /**< low pass filtered airspeed */ }; -#endif -} +} // namespace land_detector diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index bc9b9f8d76..3ccb81ea1e 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -55,7 +55,8 @@ #include "MulticopterLandDetector.h" #include "VtolLandDetector.h" -namespace landdetection + +namespace land_detector { //Function prototypes @@ -83,7 +84,7 @@ static void land_detector_stop() return; } - land_detector_task->shutdown(); + land_detector_task->stop(); // Wait for task to die int i = 0; @@ -92,7 +93,7 @@ static void land_detector_stop() /* wait 20ms */ usleep(20000); - } while (land_detector_task->isRunning() && ++i < 50); + } while (land_detector_task->is_running() && ++i < 50); delete land_detector_task; @@ -146,8 +147,8 @@ static int land_detector_start(const char *mode) usleep(10000); /* check if the waiting involving dots and a newline are still needed */ - if (!land_detector_task->isRunning()) { - while (!land_detector_task->isRunning()) { + if (!land_detector_task->is_running()) { + while (!land_detector_task->is_running()) { usleep(50000); if (hrt_absolute_time() > timeout) { @@ -191,8 +192,27 @@ int land_detector_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (land_detector_task) { - if (land_detector_task->isRunning()) { - PX4_WARN("running (%s): %s", _currentMode, (land_detector_task->isLanded()) ? "LANDED" : "IN AIR"); + if (land_detector_task->is_running()) { + PX4_INFO("running (%s): %s", _currentMode); + LandDetector::LandDetectionState state = land_detector_task->get_state(); + + switch (state) { + case LandDetector::LandDetectionState::FLYING: + PX4_INFO("State: Flying"); + break; + + case LandDetector::LandDetectionState::LANDED: + PX4_INFO("State: Landed"); + break; + + case LandDetector::LandDetectionState::FREEFALL: + PX4_INFO("State: Freefall"); + break; + + default: + PX4_ERR("State: unknown"); + break; + } } else { PX4_WARN("exists, but not running (%s)", _currentMode); diff --git a/src/drivers/uart_esc/CMakeLists.txt b/src/modules/load_mon/CMakeLists.txt similarity index 91% rename from src/drivers/uart_esc/CMakeLists.txt rename to src/modules/load_mon/CMakeLists.txt index 43c201b26d..bd435caf57 100644 --- a/src/drivers/uart_esc/CMakeLists.txt +++ b/src/modules/load_mon/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# Copyright (c) 2016 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 @@ -31,13 +31,12 @@ # ############################################################################ px4_add_module( - MODULE drivers__uart_esc - MAIN uart_esc - COMPILE_FLAGS - -Os - -Wno-packed + MODULE modules__load_mon + MAIN load_mon + STACK_MAIN 1200 + COMPILE_FLAGS -Os SRCS - uart_esc.cpp + load_mon.cpp DEPENDS platforms__common ) diff --git a/src/modules/load_mon/load_mon.cpp b/src/modules/load_mon/load_mon.cpp new file mode 100644 index 0000000000..690e955a38 --- /dev/null +++ b/src/modules/load_mon/load_mon.cpp @@ -0,0 +1,275 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2016 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 load_mon.cpp + * + * @author Jonathan Challinger + * @author Julian Oes +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include + +extern struct system_load_s system_load; + + +namespace load_mon +{ + +extern "C" __EXPORT int load_mon_main(int argc, char *argv[]); + +// Run it at 1 Hz. +const unsigned LOAD_MON_INTERVAL_US = 1000000; + +class LoadMon +{ +public: + LoadMon(); + ~LoadMon(); + + /* Start the load monitoring + * + * @return 0 if successfull, -1 on error. */ + int start(); + + /* Stop the load monitoring */ + void stop(); + + /* Trampoline for the work queue. */ + static void cycle_trampoline(void *arg); + + bool isRunning() { return _taskIsRunning; } + +private: + /* Do a compute and schedule the next cycle. */ + void _cycle(); + + /* Do a calculation of the CPU load and publish it. */ + void _compute(); + + bool _taskShouldExit; + bool _taskIsRunning; + struct work_s _work; + + struct cpuload_s _cpuload; + orb_advert_t _cpuload_pub; + hrt_abstime _last_idle_time; +}; + + +LoadMon::LoadMon() : + _taskShouldExit(false), + _taskIsRunning(false), + _work{}, + _cpuload{}, + _cpuload_pub(nullptr), + _last_idle_time(0) +{} + +LoadMon::~LoadMon() +{ + work_cancel(HPWORK, &_work); + _taskIsRunning = false; +} + +int LoadMon::start() +{ + /* Schedule a cycle to start things. */ + return work_queue(HPWORK, &_work, (worker_t)&LoadMon::cycle_trampoline, this, 0); +} + +void LoadMon::stop() +{ + _taskShouldExit = true; +} + +void +LoadMon::cycle_trampoline(void *arg) +{ + LoadMon *dev = reinterpret_cast(arg); + + dev->_cycle(); +} + +void LoadMon::_cycle() +{ + _taskIsRunning = true; + + _compute(); + + if (!_taskShouldExit) { + work_queue(HPWORK, &_work, (worker_t)&LoadMon::cycle_trampoline, this, + USEC2TICK(LOAD_MON_INTERVAL_US)); + } +} + +void LoadMon::_compute() +{ + if (_last_idle_time == 0) { + /* Just get the time in the first iteration */ + _last_idle_time = system_load.tasks[0].total_runtime; + return; + } + + /* compute system load */ + const hrt_abstime interval_idletime = system_load.tasks[0].total_runtime - _last_idle_time; + _last_idle_time = system_load.tasks[0].total_runtime; + + _cpuload.timestamp = hrt_absolute_time(); + _cpuload.load = 1.0f - (float)interval_idletime / (float)LOAD_MON_INTERVAL_US; + + if (_cpuload_pub == nullptr) { + _cpuload_pub = orb_advertise(ORB_ID(cpuload), &_cpuload); + + } else { + orb_publish(ORB_ID(cpuload), _cpuload_pub, &_cpuload); + } +} + + + +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +static void +usage(const char *reason) +{ + if (reason) { + PX4_ERR("%s", reason); + } + + PX4_INFO("usage: load_mon {start|stop|status}"); +} + + +static LoadMon *load_mon = nullptr; + +/** + * The daemon 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(). + */ +int load_mon_main(int argc, char *argv[]) +{ + if (argc < 2) { + usage("missing command"); + return 1; + } + + if (!strcmp(argv[1], "start")) { + + if (load_mon != nullptr && load_mon->isRunning()) { + PX4_WARN("already running"); + /* this is not an error */ + return 0; + } + + load_mon = new LoadMon(); + + // Check if alloc worked. + if (load_mon == nullptr) { + PX4_ERR("alloc failed"); + return -1; + } + + int ret = load_mon->start(); + + if (ret != 0) { + PX4_ERR("start failed"); + } + + return 0; + } + + if (!strcmp(argv[1], "stop")) { + + if (load_mon == nullptr || load_mon->isRunning()) { + PX4_WARN("not running"); + /* this is not an error */ + return 0; + } + + load_mon->stop(); + + // Wait for task to die + int i = 0; + + do { + /* wait up to 3s */ + usleep(100000); + + } while (load_mon->isRunning() && ++i < 30); + + delete load_mon; + load_mon = nullptr; + + return 0; + } + + if (!strcmp(argv[1], "status")) { + if (load_mon != nullptr && load_mon->isRunning()) { + PX4_INFO("running"); + + } else { + PX4_INFO("not running\n"); + } + + return 0; + } + + usage("unrecognized command"); + return 1; +} + +} // namespace load_mon diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index cf7f639f9d..ede4309e58 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -3,40 +3,45 @@ #include #include #include +#include orb_advert_t mavlink_log_pub = nullptr; // timeouts for sensors in microseconds static const uint32_t EST_SRC_TIMEOUT = 10000; // 0.01 s +// required standard deviation of estimate for estimator to publish data +static const uint32_t EST_STDDEV_XY_VALID = 2.0; // 2.0 m +static const uint32_t EST_STDDEV_Z_VALID = 2.0; // 2.0 m +static const uint32_t EST_STDDEV_TZ_VALID = 2.0; // 2.0 m +static const bool integrate = true; // use accel for integrating + // minimum flow altitude static const float flow_min_agl = 0.3; BlockLocalPositionEstimator::BlockLocalPositionEstimator() : // this block has no parent, and has name LPE SuperBlock(NULL, "LPE"), - // subscriptions, set rate, add to list - // TODO topic speed limiting? - _sub_status(ORB_ID(vehicle_status), 0, 0, &getSubscriptions()), - _sub_armed(ORB_ID(actuator_armed), 0, 0, &getSubscriptions()), - _sub_control_mode(ORB_ID(vehicle_control_mode), - 0, 0, &getSubscriptions()), - _sub_att(ORB_ID(vehicle_attitude), 0, 0, &getSubscriptions()), - _sub_att_sp(ORB_ID(vehicle_attitude_setpoint), - 0, 0, &getSubscriptions()), - _sub_flow(ORB_ID(optical_flow), 0, 0, &getSubscriptions()), - _sub_sensor(ORB_ID(sensor_combined), 0, 0, &getSubscriptions()), - _sub_param_update(ORB_ID(parameter_update), 0, 0, &getSubscriptions()), - _sub_manual(ORB_ID(manual_control_setpoint), 0, 0, &getSubscriptions()), - _sub_home(ORB_ID(home_position), 0, 0, &getSubscriptions()), - _sub_gps(ORB_ID(vehicle_gps_position), 0, 0, &getSubscriptions()), - _sub_vision_pos(ORB_ID(vision_position_estimate), 0, 0, &getSubscriptions()), - _sub_mocap(ORB_ID(att_pos_mocap), 0, 0, &getSubscriptions()), - _sub_dist0(ORB_ID(distance_sensor), 0, 0, &getSubscriptions()), - _sub_dist1(ORB_ID(distance_sensor), 0, 1, &getSubscriptions()), - _sub_dist2(ORB_ID(distance_sensor), 0, 2, &getSubscriptions()), - _sub_dist3(ORB_ID(distance_sensor), 0, 3, &getSubscriptions()), + _sub_armed(ORB_ID(actuator_armed), 1000 / 2, 0, &getSubscriptions()), + _sub_att(ORB_ID(vehicle_attitude), 1000 / 100, 0, &getSubscriptions()), + // set flow max update rate higher than expected to we don't lose packets + _sub_flow(ORB_ID(optical_flow), 1000 / 100, 0, &getSubscriptions()), + // main prediction loop, 100 hz + _sub_sensor(ORB_ID(sensor_combined), 1000 / 100, 0, &getSubscriptions()), + // status updates 2 hz + _sub_param_update(ORB_ID(parameter_update), 1000 / 2, 0, &getSubscriptions()), + _sub_manual(ORB_ID(manual_control_setpoint), 1000 / 2, 0, &getSubscriptions()), + // gps 10 hz + _sub_gps(ORB_ID(vehicle_gps_position), 1000 / 10, 0, &getSubscriptions()), + // vision 5 hz + _sub_vision_pos(ORB_ID(vision_position_estimate), 1000 / 5, 0, &getSubscriptions()), + // all distance sensors, 10 hz + _sub_mocap(ORB_ID(att_pos_mocap), 1000 / 10, 0, &getSubscriptions()), + _sub_dist0(ORB_ID(distance_sensor), 1000 / 10, 0, &getSubscriptions()), + _sub_dist1(ORB_ID(distance_sensor), 1000 / 10, 1, &getSubscriptions()), + _sub_dist2(ORB_ID(distance_sensor), 1000 / 10, 2, &getSubscriptions()), + _sub_dist3(ORB_ID(distance_sensor), 1000 / 10, 3, &getSubscriptions()), _dist_subs(), _sub_lidar(NULL), _sub_sonar(NULL), @@ -45,12 +50,14 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _pub_lpos(ORB_ID(vehicle_local_position), -1, &getPublications()), _pub_gpos(ORB_ID(vehicle_global_position), -1, &getPublications()), _pub_est_status(ORB_ID(estimator_status), -1, &getPublications()), + _pub_innov(ORB_ID(ekf2_innovations), -1, &getPublications()), // map projection _map_ref(), // block parameters - _integrate(this, "INTEGRATE"), + _xy_pub_thresh(this, "XY_PUB"), + _z_pub_thresh(this, "Z_PUB"), _sonar_z_stddev(this, "SNR_Z"), _sonar_z_offset(this, "SNR_OFF_Z"), _lidar_z_stddev(this, "LDR_Z"), @@ -65,6 +72,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _gps_vxy_stddev(this, "GPS_VXY"), _gps_vz_stddev(this, "GPS_VZ"), _gps_eph_max(this, "EPH_MAX"), + _gps_epv_max(this, "EPV_MAX"), _vision_xy_stddev(this, "VIS_XY"), _vision_z_stddev(this, "VIS_Z"), _vision_on(this, "VIS_ON"), @@ -77,11 +85,11 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _pn_p_noise_density(this, "PN_P"), _pn_v_noise_density(this, "PN_V"), _pn_b_noise_density(this, "PN_B"), - _pn_t_noise_density(this, "PN_T"), + _t_max_grade(this, "T_MAX_GRADE"), - // init home - _init_home_lat(this, "LAT"), - _init_home_lon(this, "LON"), + // init origin + _init_origin_lat(this, "LAT"), + _init_origin_lon(this, "LON"), // flow gyro _flow_gyro_x_high_pass(this, "FGYRO_HP"), @@ -96,7 +104,12 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _mocapStats(this, ""), _gpsStats(this, ""), - // stats + // low pass + _xLowPass(this, "X_LP"), + // use same lp constant for agl + _aglLowPass(this, "X_LP"), + + // delay _xDelay(this, ""), _tDelay(this, ""), @@ -112,6 +125,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _time_last_gps(0), _time_last_lidar(0), _time_last_sonar(0), + _time_init_sonar(0), _time_last_vision_p(0), _time_last_mocap(0), @@ -126,12 +140,12 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _mocapInitialized(false), // reference altitudes - _altHome(0), - _altHomeInitialized(false), - _baroAltHome(0), - _gpsAltHome(0), - _visionHome(), - _mocapHome(), + _altOrigin(0), + _altOriginInitialized(false), + _baroAltOrigin(0), + _gpsAltOrigin(0), + _visionOrigin(), + _mocapOrigin(), // flow integration _flowX(0), @@ -139,9 +153,9 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _flowMeanQual(0), // status - _canEstimateXY(false), - _canEstimateZ(false), - _canEstimateT(false), + _validXY(false), + _validZ(false), + _validTZ(false), _xyTimeout(true), _zTimeout(true), _tzTimeout(true), @@ -180,12 +194,12 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _polls[POLL_SENSORS].fd = _sub_sensor.getHandle(); _polls[POLL_SENSORS].events = POLLIN; - // initialize P, x, u - initP(); + // initialize A, B, P, x, u _x.setZero(); _u.setZero(); _flowX = 0; _flowY = 0; + initSS(); // perf counters _loop_perf = perf_alloc(PC_ELAPSED, @@ -205,6 +219,14 @@ BlockLocalPositionEstimator::~BlockLocalPositionEstimator() { } +Vector BlockLocalPositionEstimator::dynamics( + float t, + const Vector &x, + const Vector &u) +{ + return _A * x + _B * u; +} + void BlockLocalPositionEstimator::update() { @@ -232,27 +254,34 @@ void BlockLocalPositionEstimator::update() } // reset pos, vel, and terrain on arming - if (!_lastArmedState && armedState) { - // we just armed, we are at home position on the ground - _x(X_x) = 0; - _x(X_y) = 0; + // XXX this will be re-enabled for indoor use cases using a + // selection param, but is really not helping outdoors + // right now. - // the pressure altitude of home may have drifted, so we don't - // reset z to zero + // if (!_lastArmedState && armedState) { - // reset flow integral - _flowX = 0; - _flowY = 0; + // // we just armed, we are at origin on the ground + // _x(X_x) = 0; + // _x(X_y) = 0; + // // reset Z or not? _x(X_z) = 0; - // we aren't moving, all velocities are zero - _x(X_vx) = 0; - _x(X_vy) = 0; - _x(X_vz) = 0; + // // reset flow integral + // _flowX = 0; + // _flowY = 0; - // assume we are on the ground, so terrain alt is local alt - _x(X_tz) = _x(X_z); - } + // // we aren't moving, all velocities are zero + // _x(X_vx) = 0; + // _x(X_vy) = 0; + // _x(X_vz) = 0; + + // // assume we are on the ground, so terrain alt is local alt + // _x(X_tz) = _x(X_z); + + // // reset lowpass filter as well + // _xLowPass.setState(_x); + // _aglLowPass.setState(0); + // } _lastArmedState = armedState; @@ -261,7 +290,6 @@ void BlockLocalPositionEstimator::update() bool paramsUpdated = _sub_param_update.updated(); bool baroUpdated = _sub_sensor.updated(); bool gpsUpdated = _gps_on.get() && _sub_gps.updated(); - bool homeUpdated = _sub_home.updated(); bool visionUpdated = _vision_on.get() && _sub_vision_pos.updated(); bool mocapUpdated = _sub_mocap.updated(); bool lidarUpdated = (_sub_lidar != NULL) && _sub_lidar->updated(); @@ -273,34 +301,63 @@ void BlockLocalPositionEstimator::update() // update parameters if (paramsUpdated) { updateParams(); + updateSSParams(); } - // update home position projection - if (homeUpdated) { - updateHome(); + // is xy valid? + bool xy_stddev_ok = sqrtf(math::max(_P(X_x, X_x), _P(X_y, X_y))) < _xy_pub_thresh.get(); + + if (_validXY) { + // if valid and gps has timed out, set to not valid + if (!xy_stddev_ok && !_gpsInitialized) { + _validXY = false; + } + + } else { + if (xy_stddev_ok) { + _validXY = true; + } } - // determine if we should start estimating - _canEstimateZ = - (_baroInitialized && _baroFault < fault_lvl_disable); - _canEstimateXY = - (_gpsInitialized && _gpsFault < fault_lvl_disable) || - (_flowInitialized && _flowFault < fault_lvl_disable) || - (_visionInitialized && _visionFault < fault_lvl_disable) || - (_mocapInitialized && _mocapFault < fault_lvl_disable); - _canEstimateT = - (_lidarInitialized && _lidarFault < fault_lvl_disable) || - (_sonarInitialized && _sonarFault < fault_lvl_disable); + // is z valid? + bool z_stddev_ok = sqrtf(_P(X_z, X_z)) < _z_pub_thresh.get(); - if (_canEstimateXY) { + if (_validZ) { + // if valid and baro has timed out, set to not valid + if (!z_stddev_ok && !_baroInitialized) { + _validZ = false; + } + + } else { + if (z_stddev_ok) { + _validZ = true; + } + } + + // is terrain valid? + bool tz_stddev_ok = sqrtf(_P(X_tz, X_tz)) < _z_pub_thresh.get(); + + if (_validTZ) { + if (!tz_stddev_ok) { + _validTZ = false; + } + + } else { + if (tz_stddev_ok) { + _validTZ = true; + } + } + + // timeouts + if (_validXY) { _time_last_xy = _timeStamp; } - if (_canEstimateZ) { + if (_validZ) { _time_last_z = _timeStamp; } - if (_canEstimateT) { + if (_validTZ) { _time_last_tz = _timeStamp; } @@ -308,10 +365,10 @@ void BlockLocalPositionEstimator::update() checkTimeouts(); // if we have no lat, lon initialize projection at 0,0 - if (_canEstimateXY && !_map_ref.init_done) { + if (_validXY && !_map_ref.init_done) { map_projection_init(&_map_ref, - _init_home_lat.get(), - _init_home_lon.get()); + _init_origin_lat.get(), + _init_origin_lon.get()); } // reinitialize x if necessary @@ -424,12 +481,13 @@ void BlockLocalPositionEstimator::update() } } - if (_altHomeInitialized) { + if (_altOriginInitialized) { // update all publications if possible publishLocalPos(); publishEstimatorStatus(); + _pub_innov.update(); - if (_canEstimateXY) { + if (_validXY) { publishGlobalPos(); } } @@ -496,6 +554,54 @@ float BlockLocalPositionEstimator::agl() return _x(X_tz) - _x(X_z); } +void BlockLocalPositionEstimator::correctionLogic(Vector &dx) +{ + // don't correct bias when rotating rapidly + float ang_speed = sqrtf( + _sub_att.get().rollspeed * _sub_att.get().rollspeed + + _sub_att.get().pitchspeed * _sub_att.get().pitchspeed + + _sub_att.get().yawspeed * _sub_att.get().yawspeed); + + if (ang_speed > 1) { + dx(X_bx) = 0; + dx(X_by) = 0; + dx(X_bz) = 0; + } + + // if xy not valid, stop estimating + if (!_validXY) { + dx(X_x) = 0; + dx(X_y) = 0; + dx(X_vx) = 0; + dx(X_vy) = 0; + dx(X_bx) = 0; + dx(X_by) = 0; + } + + // if z not valid, stop estimating + if (!_validZ) { + dx(X_z) = 0; + dx(X_vz) = 0; + dx(X_bz) = 0; + } + + // if terrain not valid, stop estimating + if (!_validTZ) { + dx(X_tz) = 0; + } + + // saturate bias + float bx = dx(X_bx) + _x(X_bx); + float by = dx(X_by) + _x(X_by); + float bz = dx(X_bz) + _x(X_bz); + + if (std::abs(bx) > BIAS_MAX) { bx = BIAS_MAX * bx / std::abs(bx); } + + if (std::abs(by) > BIAS_MAX) { by = BIAS_MAX * by / std::abs(by); } + + if (std::abs(bz) > BIAS_MAX) { bz = BIAS_MAX * bz / std::abs(bz); } +} + void BlockLocalPositionEstimator::detectDistanceSensors() { for (int i = 0; i < N_DIST_SUBS; i++) { @@ -524,57 +630,37 @@ void BlockLocalPositionEstimator::detectDistanceSensors() } } -void BlockLocalPositionEstimator::updateHome() -{ - double lat = _sub_home.get().lat; - double lon = _sub_home.get().lon; - float alt = _sub_home.get().alt; - - // updating home causes absolute measurements - // like gps and baro to be off, need to allow it - // to reset by resetting covariance - initP(); - mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] home " - "lat %6.2f lon %6.2f alt %5.1f m", - lat, lon, double(alt)); - map_projection_init(&_map_ref, lat, lon); - float delta_alt = alt - _altHome; - _altHomeInitialized = true; - _altHome = alt; - _gpsAltHome += delta_alt; - _baroAltHome += delta_alt; - _visionHome(2) += delta_alt; - _mocapHome(2) += delta_alt; -} void BlockLocalPositionEstimator::publishLocalPos() { + const Vector &xLP = _xLowPass.getState(); + // publish local position if (PX4_ISFINITE(_x(X_x)) && PX4_ISFINITE(_x(X_y)) && PX4_ISFINITE(_x(X_z)) && PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy)) && PX4_ISFINITE(_x(X_vz))) { _pub_lpos.get().timestamp = _timeStamp; - _pub_lpos.get().xy_valid = _canEstimateXY; - _pub_lpos.get().z_valid = _canEstimateZ; - _pub_lpos.get().v_xy_valid = _canEstimateXY; - _pub_lpos.get().v_z_valid = _canEstimateZ; - _pub_lpos.get().x = _x(X_x); // north - _pub_lpos.get().y = _x(X_y); // east - _pub_lpos.get().z = _x(X_z); // down - _pub_lpos.get().vx = _x(X_vx); // north - _pub_lpos.get().vy = _x(X_vy); // east - _pub_lpos.get().vz = _x(X_vz); // down + _pub_lpos.get().xy_valid = _validXY; + _pub_lpos.get().z_valid = _validZ; + _pub_lpos.get().v_xy_valid = _validXY; + _pub_lpos.get().v_z_valid = _validZ; + _pub_lpos.get().x = xLP(X_x); // north + _pub_lpos.get().y = xLP(X_y); // east + _pub_lpos.get().z = xLP(X_z); // down + _pub_lpos.get().vx = xLP(X_vx); // north + _pub_lpos.get().vy = xLP(X_vy); // east + _pub_lpos.get().vz = xLP(X_vz); // down _pub_lpos.get().yaw = _sub_att.get().yaw; - _pub_lpos.get().xy_global = _sub_home.get().timestamp != 0; // need home for reference + _pub_lpos.get().xy_global = _validXY; _pub_lpos.get().z_global = _baroInitialized; - _pub_lpos.get().ref_timestamp = _sub_home.get().timestamp; + _pub_lpos.get().ref_timestamp = _timeStamp; _pub_lpos.get().ref_lat = _map_ref.lat_rad * 180 / M_PI; _pub_lpos.get().ref_lon = _map_ref.lon_rad * 180 / M_PI; - _pub_lpos.get().ref_alt = _sub_home.get().alt; - _pub_lpos.get().dist_bottom = agl(); - _pub_lpos.get().dist_bottom_rate = -_x(X_vz); + _pub_lpos.get().ref_alt = _altOrigin; + _pub_lpos.get().dist_bottom = _aglLowPass.getState(); + _pub_lpos.get().dist_bottom_rate = - xLP(X_vz); _pub_lpos.get().surface_bottom_timestamp = _timeStamp; - _pub_lpos.get().dist_bottom_valid = _canEstimateZ; + _pub_lpos.get().dist_bottom_valid = _validTZ && _validZ; _pub_lpos.get().eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y)); _pub_lpos.get().epv = sqrtf(_P(X_z, X_z)); _pub_lpos.update(); @@ -583,37 +669,32 @@ void BlockLocalPositionEstimator::publishLocalPos() void BlockLocalPositionEstimator::publishEstimatorStatus() { - if (PX4_ISFINITE(_x(X_x)) && - PX4_ISFINITE(_x(X_y)) && PX4_ISFINITE(_x(X_z)) && - PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy)) - && PX4_ISFINITE(_x(X_vz))) { - _pub_est_status.get().timestamp = _timeStamp; + _pub_est_status.get().timestamp = _timeStamp; - for (int i = 0; i < n_x; i++) { - _pub_est_status.get().states[i] = _x(i); - _pub_est_status.get().covariances[i] = _P(i, i); - } - - _pub_est_status.get().n_states = n_x; - _pub_est_status.get().nan_flags = 0; - _pub_est_status.get().health_flags = - ((_baroFault > fault_lvl_disable) << SENSOR_BARO) - + ((_gpsFault > fault_lvl_disable) << SENSOR_GPS) - + ((_lidarFault > fault_lvl_disable) << SENSOR_LIDAR) - + ((_flowFault > fault_lvl_disable) << SENSOR_FLOW) - + ((_sonarFault > fault_lvl_disable) << SENSOR_SONAR) - + ((_visionFault > fault_lvl_disable) << SENSOR_VISION) - + ((_mocapFault > fault_lvl_disable) << SENSOR_MOCAP); - _pub_est_status.get().timeout_flags = - (_baroInitialized << SENSOR_BARO) - + (_gpsInitialized << SENSOR_GPS) - + (_flowInitialized << SENSOR_FLOW) - + (_lidarInitialized << SENSOR_LIDAR) - + (_sonarInitialized << SENSOR_SONAR) - + (_visionInitialized << SENSOR_VISION) - + (_mocapInitialized << SENSOR_MOCAP); - _pub_est_status.update(); + for (int i = 0; i < n_x; i++) { + _pub_est_status.get().states[i] = _x(i); + _pub_est_status.get().covariances[i] = _P(i, i); } + + _pub_est_status.get().n_states = n_x; + _pub_est_status.get().nan_flags = 0; + _pub_est_status.get().health_flags = + ((_baroFault > FAULT_NONE) << SENSOR_BARO) + + ((_gpsFault > FAULT_NONE) << SENSOR_GPS) + + ((_lidarFault > FAULT_NONE) << SENSOR_LIDAR) + + ((_flowFault > FAULT_NONE) << SENSOR_FLOW) + + ((_sonarFault > FAULT_NONE) << SENSOR_SONAR) + + ((_visionFault > FAULT_NONE) << SENSOR_VISION) + + ((_mocapFault > FAULT_NONE) << SENSOR_MOCAP); + _pub_est_status.get().timeout_flags = + (_baroInitialized << SENSOR_BARO) + + (_gpsInitialized << SENSOR_GPS) + + (_flowInitialized << SENSOR_FLOW) + + (_lidarInitialized << SENSOR_LIDAR) + + (_sonarInitialized << SENSOR_SONAR) + + (_visionInitialized << SENSOR_VISION) + + (_mocapInitialized << SENSOR_MOCAP); + _pub_est_status.update(); } void BlockLocalPositionEstimator::publishGlobalPos() @@ -621,27 +702,28 @@ void BlockLocalPositionEstimator::publishGlobalPos() // publish global position double lat = 0; double lon = 0; - map_projection_reproject(&_map_ref, _x(X_x), _x(X_y), &lat, &lon); - float alt = -_x(X_z) + _altHome; + const Vector &xLP = _xLowPass.getState(); + map_projection_reproject(&_map_ref, xLP(X_x), xLP(X_y), &lat, &lon); + float alt = -xLP(X_z) + _altOrigin; if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt) && - PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy)) && - PX4_ISFINITE(_x(X_vz))) { + PX4_ISFINITE(xLP(X_vx)) && PX4_ISFINITE(xLP(X_vy)) && + PX4_ISFINITE(xLP(X_vz))) { _pub_gpos.get().timestamp = _timeStamp; _pub_gpos.get().time_utc_usec = _sub_gps.get().time_utc_usec; _pub_gpos.get().lat = lat; _pub_gpos.get().lon = lon; _pub_gpos.get().alt = alt; - _pub_gpos.get().vel_n = _x(X_vx); - _pub_gpos.get().vel_e = _x(X_vy); - _pub_gpos.get().vel_d = _x(X_vz); + _pub_gpos.get().vel_n = xLP(X_vx); + _pub_gpos.get().vel_e = xLP(X_vy); + _pub_gpos.get().vel_d = xLP(X_vz); _pub_gpos.get().yaw = _sub_att.get().yaw; _pub_gpos.get().eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y)); _pub_gpos.get().epv = sqrtf(_P(X_z, X_z)); - _pub_gpos.get().terrain_alt = _altHome - _x(X_tz); - _pub_gpos.get().terrain_alt_valid = _canEstimateT; - _pub_gpos.get().dead_reckoning = !_canEstimateXY && !_xyTimeout; - _pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter[0]; + _pub_gpos.get().terrain_alt = _altOrigin - xLP(X_tz); + _pub_gpos.get().terrain_alt_valid = _validTZ; + _pub_gpos.get().dead_reckoning = !_validXY && !_xyTimeout; + _pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter; _pub_gpos.update(); } } @@ -649,25 +731,97 @@ void BlockLocalPositionEstimator::publishGlobalPos() void BlockLocalPositionEstimator::initP() { _P.setZero(); - _P(X_x, X_x) = 1; - _P(X_y, X_y) = 1; - _P(X_z, X_z) = 1; + _P(X_x, X_x) = 2 * EST_STDDEV_XY_VALID; // initialize to twice valid condition + _P(X_y, X_y) = 2 * EST_STDDEV_XY_VALID; + _P(X_z, X_z) = 2 * EST_STDDEV_Z_VALID; _P(X_vx, X_vx) = 1; _P(X_vy, X_vy) = 1; _P(X_vz, X_vz) = 1; _P(X_bx, X_bx) = 1e-6; _P(X_by, X_by) = 1e-6; _P(X_bz, X_bz) = 1e-6; - _P(X_tz, X_tz) = 1; + _P(X_tz, X_tz) = 2 * EST_STDDEV_TZ_VALID; +} + +void BlockLocalPositionEstimator::initSS() +{ + initP(); + + // dynamics matrix + _A.setZero(); + // derivative of position is velocity + _A(X_x, X_vx) = 1; + _A(X_y, X_vy) = 1; + _A(X_z, X_vz) = 1; + + // input matrix + _B.setZero(); + _B(X_vx, U_ax) = 1; + _B(X_vy, U_ay) = 1; + _B(X_vz, U_az) = 1; + + // update components that depend on current state + updateSSStates(); + updateSSParams(); +} + +void BlockLocalPositionEstimator::updateSSStates() +{ + // derivative of velocity is accelerometer acceleration + // (in input matrix) - bias (in body frame) + Matrix3f R_att(_sub_att.get().R); + _A(X_vx, X_bx) = -R_att(0, 0); + _A(X_vx, X_by) = -R_att(0, 1); + _A(X_vx, X_bz) = -R_att(0, 2); + + _A(X_vy, X_bx) = -R_att(1, 0); + _A(X_vy, X_by) = -R_att(1, 1); + _A(X_vy, X_bz) = -R_att(1, 2); + + _A(X_vz, X_bx) = -R_att(2, 0); + _A(X_vz, X_by) = -R_att(2, 1); + _A(X_vz, X_bz) = -R_att(2, 2); +} + +void BlockLocalPositionEstimator::updateSSParams() +{ + // input noise covariance matrix + _R.setZero(); + _R(U_ax, U_ax) = _accel_xy_stddev.get() * _accel_xy_stddev.get(); + _R(U_ay, U_ay) = _accel_xy_stddev.get() * _accel_xy_stddev.get(); + _R(U_az, U_az) = _accel_z_stddev.get() * _accel_z_stddev.get(); + + // process noise power matrix + _Q.setZero(); + float pn_p_sq = _pn_p_noise_density.get() * _pn_p_noise_density.get(); + float pn_v_sq = _pn_v_noise_density.get() * _pn_v_noise_density.get(); + _Q(X_x, X_x) = pn_p_sq; + _Q(X_y, X_y) = pn_p_sq; + _Q(X_z, X_z) = pn_p_sq; + _Q(X_vx, X_vx) = pn_v_sq; + _Q(X_vy, X_vy) = pn_v_sq; + _Q(X_vz, X_vz) = pn_v_sq; + + // technically, the noise is in the body frame, + // but the components are all the same, so + // ignoring for now + float pn_b_sq = _pn_b_noise_density.get() * _pn_b_noise_density.get(); + _Q(X_bx, X_bx) = pn_b_sq; + _Q(X_by, X_by) = pn_b_sq; + _Q(X_bz, X_bz) = pn_b_sq; + + // terrain random walk noise ((m/s)/sqrt(hz)), scales with velocity + float pn_t_stddev = (_t_max_grade.get() / 100.0f) * sqrtf(_x(X_vx) * _x(X_vx) + _x(X_vy) * _x(X_vy)); + _Q(X_tz, X_tz) = pn_t_stddev * pn_t_stddev; } void BlockLocalPositionEstimator::predict() { // if can't update anything, don't propagate // state or covariance - if (!_canEstimateXY && !_canEstimateZ) { return; } + if (!_validXY && !_validZ) { return; } - if (_integrate.get() && _sub_att.get().R_valid) { + if (integrate && _sub_att.get().R_valid) { Matrix3f R_att(_sub_att.get().R); Vector3f a(_sub_sensor.get().accelerometer_m_s2); _u = R_att * a; @@ -677,86 +831,27 @@ void BlockLocalPositionEstimator::predict() _u = Vector3f(0, 0, 0); } - // dynamics matrix - Matrix A; // state dynamics matrix - A.setZero(); - // derivative of position is velocity - A(X_x, X_vx) = 1; - A(X_y, X_vy) = 1; - A(X_z, X_vz) = 1; - - // derivative of velocity is accelerometer acceleration - // (in input matrix) - bias (in body frame) - Matrix3f R_att(_sub_att.get().R); - A(X_vx, X_bx) = -R_att(0, 0); - A(X_vx, X_by) = -R_att(0, 1); - A(X_vx, X_bz) = -R_att(0, 2); - - A(X_vy, X_bx) = -R_att(1, 0); - A(X_vy, X_by) = -R_att(1, 1); - A(X_vy, X_bz) = -R_att(1, 2); - - A(X_vz, X_bx) = -R_att(2, 0); - A(X_vz, X_by) = -R_att(2, 1); - A(X_vz, X_bz) = -R_att(2, 2); - - // input matrix - Matrix B; // input matrix - B.setZero(); - B(X_vx, U_ax) = 1; - B(X_vy, U_ay) = 1; - B(X_vz, U_az) = 1; - - // input noise covariance matrix - Matrix R; - R.setZero(); - R(U_ax, U_ax) = _accel_xy_stddev.get() * _accel_xy_stddev.get(); - R(U_ay, U_ay) = _accel_xy_stddev.get() * _accel_xy_stddev.get(); - R(U_az, U_az) = _accel_z_stddev.get() * _accel_z_stddev.get(); - - // process noise power matrix - Matrix Q; - Q.setZero(); - float pn_p_sq = _pn_p_noise_density.get() * _pn_p_noise_density.get(); - float pn_v_sq = _pn_v_noise_density.get() * _pn_v_noise_density.get(); - Q(X_x, X_x) = pn_p_sq; - Q(X_y, X_y) = pn_p_sq; - Q(X_z, X_z) = pn_p_sq; - Q(X_vx, X_vx) = pn_v_sq; - Q(X_vy, X_vy) = pn_v_sq; - Q(X_vz, X_vz) = pn_v_sq; - - // technically, the noise is in the body frame, - // but the components are all the same, so - // ignoring for now - float pn_b_sq = _pn_b_noise_density.get() * _pn_b_noise_density.get(); - Q(X_bx, X_bx) = pn_b_sq; - Q(X_by, X_by) = pn_b_sq; - Q(X_bz, X_bz) = pn_b_sq; - - // terrain random walk noise - float pn_t_sq = _pn_t_noise_density.get() * _pn_t_noise_density.get(); - Q(X_tz, X_tz) = pn_t_sq; + // update state space based on new states + updateSSStates(); // continuous time kalman filter prediction - Vector dx = (A * _x + B * _u) * getDt(); - - // only predict for components we have - // valid measurements for - if (!_canEstimateXY) { - dx(X_x) = 0; - dx(X_y) = 0; - dx(X_vx) = 0; - dx(X_vy) = 0; - } - - if (!_canEstimateZ) { - dx(X_z) = 0; - dx(X_vz) = 0; - } + // integrate runge kutta 4th order + // TODO move rk4 algorithm to matrixlib + // https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods + float h = getDt(); + Vector k1, k2, k3, k4; + k1 = dynamics(0, _x, _u); + k2 = dynamics(h / 2, _x + k1 * h / 2, _u); + k3 = dynamics(h / 2, _x + k2 * h / 2, _u); + k4 = dynamics(h, _x + k3 * h, _u); + Vector dx = (k1 + k2 * 2 + k3 * 2 + k4) * (h / 6); // propagate + correctionLogic(dx); _x += dx; - _P += (A * _P + _P * A.transpose() + - B * R * B.transpose() + Q) * getDt(); + _P += (_A * _P + _P * _A.transpose() + + _B * _R * _B.transpose() + + _Q) * getDt(); + _xLowPass.update(_x); + _aglLowPass.update(agl()); } diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index 4ccdb329f4..761251b0de 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -19,7 +19,6 @@ #include #include #include -#include #include #include #include @@ -29,12 +28,14 @@ #include #include #include +#include using namespace matrix; using namespace control; -static const float GPS_DELAY_MAX = 0.5; // seconds -static const float HIST_STEP = 0.05; // 20 hz +static const float GPS_DELAY_MAX = 0.5f; // seconds +static const float HIST_STEP = 0.05f; // 20 hz +static const float BIAS_MAX = 1e-1f; static const size_t HIST_LEN = 10; // GPS_DELAY_MAX / HIST_STEP; static const size_t N_DIST_SUBS = 4; @@ -134,6 +135,10 @@ public: BlockLocalPositionEstimator(); void update(); + Vector dynamics( + float t, + const Vector &x, + const Vector &u); virtual ~BlockLocalPositionEstimator(); private: @@ -144,6 +149,9 @@ private: // methods // ---------------------------- void initP(); + void initSS(); + void updateSSStates(); + void updateSSParams(); // predict the next state void predict(); @@ -195,8 +203,8 @@ private: // misc float agl(); + void correctionLogic(Vector &dx); void detectDistanceSensors(); - void updateHome(); // publications void publishLocalPos(); @@ -207,16 +215,12 @@ private: // ---------------------------- // subscriptions - uORB::Subscription _sub_status; uORB::Subscription _sub_armed; - uORB::Subscription _sub_control_mode; uORB::Subscription _sub_att; - uORB::Subscription _sub_att_sp; uORB::Subscription _sub_flow; uORB::Subscription _sub_sensor; uORB::Subscription _sub_param_update; uORB::Subscription _sub_manual; - uORB::Subscription _sub_home; uORB::Subscription _sub_gps; uORB::Subscription _sub_vision_pos; uORB::Subscription _sub_mocap; @@ -232,12 +236,14 @@ private: uORB::Publication _pub_lpos; uORB::Publication _pub_gpos; uORB::Publication _pub_est_status; + uORB::Publication _pub_innov; // map projection struct map_projection_reference_s _map_ref; // general parameters - BlockParamInt _integrate; + BlockParamFloat _xy_pub_thresh; + BlockParamFloat _z_pub_thresh; // sonar parameters BlockParamFloat _sonar_z_stddev; @@ -262,6 +268,7 @@ private: BlockParamFloat _gps_vxy_stddev; BlockParamFloat _gps_vz_stddev; BlockParamFloat _gps_eph_max; + BlockParamFloat _gps_epv_max; // vision parameters BlockParamFloat _vision_xy_stddev; @@ -282,11 +289,11 @@ private: BlockParamFloat _pn_p_noise_density; BlockParamFloat _pn_v_noise_density; BlockParamFloat _pn_b_noise_density; - BlockParamFloat _pn_t_noise_density; + BlockParamFloat _t_max_grade; - // init home - BlockParamFloat _init_home_lat; - BlockParamFloat _init_home_lon; + // init origin + BlockParamFloat _init_origin_lat; + BlockParamFloat _init_origin_lon; // flow gyro filter BlockHighPass _flow_gyro_x_high_pass; @@ -301,6 +308,10 @@ private: BlockStats _mocapStats; BlockStats _gpsStats; + // low pass + BlockLowPassVector _xLowPass; + BlockLowPass _aglLowPass; + // delay blocks BlockDelay _xDelay; BlockDelay _tDelay; @@ -317,6 +328,7 @@ private: uint64_t _time_last_gps; uint64_t _time_last_lidar; uint64_t _time_last_sonar; + uint64_t _time_init_sonar; uint64_t _time_last_vision_p; uint64_t _time_last_mocap; @@ -331,12 +343,12 @@ private: bool _mocapInitialized; // reference altitudes - float _altHome; - bool _altHomeInitialized; - float _baroAltHome; - float _gpsAltHome; - Vector3f _visionHome; - Vector3f _mocapHome; + float _altOrigin; + bool _altOriginInitialized; + float _baroAltOrigin; + float _gpsAltOrigin; + Vector3f _visionOrigin; + Vector3f _mocapOrigin; // flow integration float _flowX; @@ -344,9 +356,9 @@ private: float _flowMeanQual; // status - bool _canEstimateXY; - bool _canEstimateZ; - bool _canEstimateT; + bool _validXY; + bool _validZ; + bool _validTZ; bool _xyTimeout; bool _zTimeout; bool _tzTimeout; @@ -370,4 +382,8 @@ private: Vector _x; // state vector Vector _u; // input vector Matrix _P; // state covariance matrix + Matrix _A; // dynamics matrix + Matrix _B; // input matrix + Matrix _R; // input covariance + Matrix _Q; // process noise covariance }; diff --git a/src/modules/local_position_estimator/CMakeLists.txt b/src/modules/local_position_estimator/CMakeLists.txt index 56ed858d57..1421be5969 100644 --- a/src/modules/local_position_estimator/CMakeLists.txt +++ b/src/modules/local_position_estimator/CMakeLists.txt @@ -35,7 +35,7 @@ px4_add_module( MAIN local_position_estimator COMPILE_FLAGS -Os STACK_MAIN 5700 - STACK_MAX 10000 + STACK_MAX 13000 SRCS local_position_estimator_main.cpp BlockLocalPositionEstimator.cpp @@ -46,7 +46,6 @@ px4_add_module( sensors/baro.cpp sensors/vision.cpp sensors/mocap.cpp - params.c DEPENDS platforms__common ) diff --git a/src/modules/local_position_estimator/local_position_estimator_main.cpp b/src/modules/local_position_estimator/local_position_estimator_main.cpp index 27256fc8e7..4ba3946076 100644 --- a/src/modules/local_position_estimator/local_position_estimator_main.cpp +++ b/src/modules/local_position_estimator/local_position_estimator_main.cpp @@ -112,7 +112,7 @@ int local_position_estimator_main(int argc, char *argv[]) deamon_task = px4_task_spawn_cmd("lp_estimator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 12000, + 13000, local_position_estimator_thread_main, (argv && argc > 2) ? (char *const *) &argv[2] : (char *const *) NULL); return 0; diff --git a/src/modules/local_position_estimator/params.c b/src/modules/local_position_estimator/params.c index 18edfa7492..3543377a10 100644 --- a/src/modules/local_position_estimator/params.c +++ b/src/modules/local_position_estimator/params.c @@ -3,14 +3,6 @@ // 16 is max name length -/** - * Enable accelerometer integration for prediction. - * - * @boolean - * @group Local Position Estimator - */ -PARAM_DEFINE_INT32(LPE_INTEGRATE, 1); - /** * Optical flow z offset from center * @@ -88,34 +80,32 @@ PARAM_DEFINE_FLOAT(LPE_LDR_Z, 0.03f); PARAM_DEFINE_FLOAT(LPE_LDR_OFF_Z, 0.00f); /** - * Accelerometer xy standard deviation + * Accelerometer xy noise density * - * Data sheet sqrt(Noise power) = 150ug/sqrt(Hz) - * std dev = (150*9.8*1e-6)*sqrt(1000 Hz) m/s^2 - * Since accels sampled at 1000 Hz. + * Data sheet noise density = 150ug/sqrt(Hz) = 0.0015 m/s^2/sqrt(Hz) * - * should be 0.0464 + * Larger than data sheet to account for tilt error. * * @group Local Position Estimator - * @unit m/s^2 + * @unit m/s^2/srqt(Hz) * @min 0.00001 * @max 2 * @decimal 4 */ -PARAM_DEFINE_FLOAT(LPE_ACC_XY, 0.0454f); +PARAM_DEFINE_FLOAT(LPE_ACC_XY, 0.0015f); /** - * Accelerometer z standard deviation + * Accelerometer z noise density * - * (see Accel x comments) + * Data sheet noise density = 150ug/sqrt(Hz) = 0.0015 m/s^2/sqrt(Hz) * * @group Local Position Estimator - * @unit m/s^2 + * @unit m/s^2/srqt(Hz) * @min 0.00001 * @max 2 * @decimal 4 */ -PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.0454f); +PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.0015f); /** * Barometric presssure altitude z standard deviation. @@ -126,10 +116,10 @@ PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.0454f); * @max 3 * @decimal 2 */ -PARAM_DEFINE_FLOAT(LPE_BAR_Z, 1.0f); +PARAM_DEFINE_FLOAT(LPE_BAR_Z, 3.0f); /** - * Enable GPS + * Enables GPS data, also forces alt init with GPS * * @group Local Position Estimator * @boolean @@ -145,11 +135,11 @@ PARAM_DEFINE_INT32(LPE_GPS_ON, 1); * @max 0.4 * @decimal 2 */ -PARAM_DEFINE_FLOAT(LPE_GPS_DELAY, 0.25f); +PARAM_DEFINE_FLOAT(LPE_GPS_DELAY, 0.29f); /** - * GPS xy standard deviation. + * Minimum GPS xy standard deviation, uses reported EPH if greater. * * @group Local Position Estimator * @unit m @@ -157,10 +147,10 @@ PARAM_DEFINE_FLOAT(LPE_GPS_DELAY, 0.25f); * @max 5 * @decimal 2 */ -PARAM_DEFINE_FLOAT(LPE_GPS_XY, 2.0f); +PARAM_DEFINE_FLOAT(LPE_GPS_XY, 1.0f); /** - * GPS z standard deviation. + * Minimum GPS z standard deviation, uses reported EPV if greater. * * @group Local Position Estimator * @unit m @@ -168,10 +158,11 @@ PARAM_DEFINE_FLOAT(LPE_GPS_XY, 2.0f); * @max 200 * @decimal 2 */ -PARAM_DEFINE_FLOAT(LPE_GPS_Z, 100.0f); +PARAM_DEFINE_FLOAT(LPE_GPS_Z, 3.0f); /** * GPS xy velocity standard deviation. + * EPV used if greater than this value. * * @group Local Position Estimator * @unit m/s @@ -193,7 +184,7 @@ PARAM_DEFINE_FLOAT(LPE_GPS_VXY, 0.25f); PARAM_DEFINE_FLOAT(LPE_GPS_VZ, 0.25f); /** - * GPS max eph + * Max EPH allowed for GPS initialization * * @group Local Position Estimator * @unit m @@ -203,6 +194,17 @@ PARAM_DEFINE_FLOAT(LPE_GPS_VZ, 0.25f); */ PARAM_DEFINE_FLOAT(LPE_EPH_MAX, 3.0f); +/** + * Max EPV allowed for GPS initialization + * + * @group Local Position Estimator + * @unit m + * @min 1.0 + * @max 5.0 + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(LPE_EPV_MAX, 5.0f); + /** * Vision xy standard deviation. * @@ -226,7 +228,7 @@ PARAM_DEFINE_FLOAT(LPE_VIS_XY, 0.5f); PARAM_DEFINE_FLOAT(LPE_VIS_Z, 0.5f); /** - * Enabled vision correction + * Vision correction * * @group Local Position Estimator * @boolean @@ -247,6 +249,9 @@ PARAM_DEFINE_FLOAT(LPE_VIC_P, 0.05f); /** * Position propagation noise density * + * Increase to trust measurements more. + * Decrease to trust model more. + * * @group Local Position Estimator * @unit m/s/sqrt(Hz) * @min 0 @@ -258,6 +263,9 @@ PARAM_DEFINE_FLOAT(LPE_PN_P, 0.1f); /** * Velocity propagation noise density * + * Increase to trust measurements more. + * Decrease to trust model more. + * * @group Local Position Estimator * @unit (m/s)/s/sqrt(Hz) * @min 0 @@ -278,15 +286,15 @@ PARAM_DEFINE_FLOAT(LPE_PN_V, 0.1f); PARAM_DEFINE_FLOAT(LPE_PN_B, 1e-3f); /** - * Terrain random walk noise density + * Terrain maximum percent grade, hilly/outdoor (100 = 45 deg), flat/Indoor (0 = 0 deg) * * @group Local Position Estimator - * @unit m/s/sqrt(Hz) + * @unit % * @min 0 - * @max 1 + * @max 100 * @decimal 3 */ -PARAM_DEFINE_FLOAT(LPE_PN_T, 1e-3f); +PARAM_DEFINE_FLOAT(LPE_T_MAX_GRADE, 1.0f); /** * Flow gyro high pass filter cut off frequency @@ -300,7 +308,7 @@ PARAM_DEFINE_FLOAT(LPE_PN_T, 1e-3f); PARAM_DEFINE_FLOAT(LPE_FGYRO_HP, 0.1f); /** - * Home latitude for nav w/o GPS + * Local origin latitude for nav w/o GPS * * @group Local Position Estimator * @unit deg @@ -311,7 +319,7 @@ PARAM_DEFINE_FLOAT(LPE_FGYRO_HP, 0.1f); PARAM_DEFINE_FLOAT(LPE_LAT, 40.430f); /** - * Home longitude for nav w/o GPS + * Local origin longitude for nav w/o GPS * * @group Local Position Estimator * @unit deg @@ -321,3 +329,35 @@ PARAM_DEFINE_FLOAT(LPE_LAT, 40.430f); */ PARAM_DEFINE_FLOAT(LPE_LON, -86.929); +/** + * Cut frequency for state publication + * + * @group Local Position Estimator + * @unit Hz + * @min 5 + * @max 1000 + * @decimal 0 + */ +PARAM_DEFINE_FLOAT(LPE_X_LP, 5.0f); + +/** + * Required xy standard deviation to publish position + * + * @group Local Position Estimator + * @unit m + * @min 0.3 + * @max 5.0 + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(LPE_XY_PUB, 1.0f); + +/** + * Required z standard deviation to publish altitude/ terrain + * + * @group Local Position Estimator + * @unit m + * @min 0.3 + * @max 5.0 + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(LPE_Z_PUB, 1.0f); diff --git a/src/modules/local_position_estimator/sensors/baro.cpp b/src/modules/local_position_estimator/sensors/baro.cpp index f3de04856e..c41bfed60c 100644 --- a/src/modules/local_position_estimator/sensors/baro.cpp +++ b/src/modules/local_position_estimator/sensors/baro.cpp @@ -21,7 +21,7 @@ void BlockLocalPositionEstimator::baroInit() // if finished if (_baroStats.getCount() > REQ_BARO_INIT_COUNT) { - _baroAltHome = _baroStats.getMean()(0); + _baroAltOrigin = _baroStats.getMean()(0); mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] baro init %d m std %d cm", (int)_baroStats.getMean()(0), @@ -29,9 +29,10 @@ void BlockLocalPositionEstimator::baroInit() _baroInitialized = true; _baroFault = FAULT_NONE; - if (!_altHomeInitialized) { - _altHomeInitialized = true; - _altHome = _baroAltHome; + // only initialize alt origin with baro and no gps + if (!_altOriginInitialized && !_gps_on.get()) { + _altOriginInitialized = true; + _altOrigin = _baroAltOrigin; } } } @@ -40,7 +41,7 @@ int BlockLocalPositionEstimator::baroMeasure(Vector &y) { //measure y.setZero(); - y(0) = _sub_sensor.get().baro_alt_meter[0]; + y(0) = _sub_sensor.get().baro_alt_meter; _baroStats.update(y); _time_last_baro = _timeStamp; return OK; @@ -53,8 +54,8 @@ void BlockLocalPositionEstimator::baroCorrect() if (baroMeasure(y) != OK) { return; } - // subtract baro home alt - y -= _baroAltHome; + // subtract baro origin alt + y -= _baroAltOrigin; // baro measurement matrix Matrix C; @@ -75,8 +76,11 @@ void BlockLocalPositionEstimator::baroCorrect() if (beta > BETA_TABLE[n_y_baro]) { if (_baroFault < FAULT_MINOR) { - //mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] baro fault, r %5.2f m, beta %5.2f", - //double(r(0)), double(beta)); + if (beta > 2.0f * BETA_TABLE[n_y_baro]) { + mavlink_and_console_log_critical(&mavlink_log_pub, "[lpe] baro fault, r %5.2f m, beta %5.2f", + double(r(0)), double(beta)); + } + _baroFault = FAULT_MINOR; } @@ -89,14 +93,7 @@ void BlockLocalPositionEstimator::baroCorrect() if (_baroFault < fault_lvl_disable) { Matrix K = _P * C.transpose() * S_I; Vector dx = K * r; - - if (!_canEstimateXY) { - dx(X_x) = 0; - dx(X_y) = 0; - dx(X_vx) = 0; - dx(X_vy) = 0; - } - + correctionLogic(dx); _x += dx; _P -= K * C * _P; } diff --git a/src/modules/local_position_estimator/sensors/flow.cpp b/src/modules/local_position_estimator/sensors/flow.cpp index 331123ab2f..bdfcb3fb7e 100644 --- a/src/modules/local_position_estimator/sensors/flow.cpp +++ b/src/modules/local_position_estimator/sensors/flow.cpp @@ -49,21 +49,12 @@ int BlockLocalPositionEstimator::flowMeasure(Vector &y) } // calculate range to center of image for flow - float d = 0; - - if (_lidarInitialized && (_lidarFault < fault_lvl_disable)) { - d = _sub_lidar->get().current_distance - + (_lidar_z_offset.get() - _flow_z_offset.get()); - - } else if (_sonarInitialized && (_sonarFault < fault_lvl_disable)) { - d = _sub_sonar->get().current_distance - + (_sonar_z_offset.get() - _flow_z_offset.get()); - - } else { - // no valid distance data + if (!_validTZ) { return -1; } + float d = agl() * cosf(_sub_att.get().roll) * cosf(_sub_att.get().pitch); + // check for global accuracy if (_gpsInitialized) { double lat = _sub_gps.get().lat * 1.0e-7; @@ -91,6 +82,9 @@ int BlockLocalPositionEstimator::flowMeasure(Vector &y) float gyro_y_rad = _flow_gyro_y_high_pass.update( _sub_flow.get().gyro_y_rate_integral); + //warnx("flow x: %10.4f y: %10.4f gyro_x: %10.4f gyro_y: %10.4f d: %10.4f", + //double(flow_x_rad), double(flow_y_rad), double(gyro_x_rad), double(gyro_y_rad), double(d)); + // compute velocities in camera frame using ground distance // assume camera frame is body frame Vector3f delta_b( @@ -131,7 +125,7 @@ void BlockLocalPositionEstimator::flowCorrect() C(Y_flow_x, X_x) = 1; C(Y_flow_y, X_y) = 1; - Matrix R; + SquareMatrix R; R.setZero(); R(Y_flow_x, Y_flow_x) = _flow_xy_stddev.get() * _flow_xy_stddev.get(); @@ -140,6 +134,10 @@ void BlockLocalPositionEstimator::flowCorrect() // residual Vector r = y - C * _x; + _pub_innov.get().flow_innov[0] = r(0); + _pub_innov.get().flow_innov[1] = r(1); + _pub_innov.get().flow_innov_var[0] = R(0, 0); + _pub_innov.get().flow_innov_var[1] = R(1, 1); // residual covariance, (inverse) Matrix S_I = @@ -162,7 +160,9 @@ void BlockLocalPositionEstimator::flowCorrect() if (_flowFault < fault_lvl_disable) { Matrix K = _P * C.transpose() * S_I; - _x += K * r; + Vector dx = K * r; + correctionLogic(dx); + _x += dx; _P -= K * C * _P; } else { @@ -180,7 +180,7 @@ void BlockLocalPositionEstimator::flowCheckTimeout() if (_flowInitialized) { _flowInitialized = false; _flowQStats.reset(); - mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow timeout "); + mavlink_and_console_log_critical(&mavlink_log_pub, "[lpe] flow timeout "); } } } diff --git a/src/modules/local_position_estimator/sensors/gps.cpp b/src/modules/local_position_estimator/sensors/gps.cpp index 5bff337a55..ff331072be 100644 --- a/src/modules/local_position_estimator/sensors/gps.cpp +++ b/src/modules/local_position_estimator/sensors/gps.cpp @@ -11,6 +11,22 @@ static const uint32_t GPS_TIMEOUT = 1000000; // 1.0 s void BlockLocalPositionEstimator::gpsInit() { + // check for good gps signal + uint8_t nSat = _sub_gps.get().satellites_used; + float eph = _sub_gps.get().eph; + float epv = _sub_gps.get().epv; + uint8_t fix_type = _sub_gps.get().fix_type; + + if ( + nSat < 6 || + eph > _gps_eph_max.get() || + epv > _gps_epv_max.get() || + fix_type < 3 + ) { + _gpsStats.reset(); + return; + } + // measure Vector y; @@ -21,41 +37,33 @@ void BlockLocalPositionEstimator::gpsInit() // if finished if (_gpsStats.getCount() > REQ_GPS_INIT_COUNT) { - double gpsLatHome = _gpsStats.getMean()(0); - double gpsLonHome = _gpsStats.getMean()(1); + double gpsLatOrigin = _gpsStats.getMean()(0); + double gpsLonOrigin = _gpsStats.getMean()(1); if (!_receivedGps) { _receivedGps = true; - map_projection_init(&_map_ref, gpsLatHome, gpsLonHome); + map_projection_init(&_map_ref, gpsLatOrigin, gpsLonOrigin); } - _gpsAltHome = _gpsStats.getMean()(2); - mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] gps init " - "lat %6.2f lon %6.2f alt %5.1f m", - gpsLatHome, - gpsLonHome, - double(_gpsAltHome)); + _gpsAltOrigin = _gpsStats.getMean()(2); + PX4_INFO("[lpe] gps init " + "lat %6.2f lon %6.2f alt %5.1f m", + gpsLatOrigin, + gpsLonOrigin, + double(_gpsAltOrigin)); _gpsInitialized = true; _gpsFault = FAULT_NONE; _gpsStats.reset(); - if (!_altHomeInitialized) { - _altHomeInitialized = true; - _altHome = _gpsAltHome; + if (!_altOriginInitialized) { + _altOriginInitialized = true; + _altOrigin = _gpsAltOrigin; } } } int BlockLocalPositionEstimator::gpsMeasure(Vector &y) { - // check for good gps signal - uint8_t nSat = _sub_gps.get().satellites_used; - float eph = _sub_gps.get().eph; - - if (nSat < 6 || eph > _gps_eph_max.get()) { - return -1; - } - // gps measurement y.setZero(); y(0) = _sub_gps.get().lat * 1e-7; @@ -84,7 +92,7 @@ void BlockLocalPositionEstimator::gpsCorrect() float alt = y_global(2); float px = 0; float py = 0; - float pz = -(alt - _gpsAltHome); + float pz = -(alt - _gpsAltOrigin); map_projection_project(&_map_ref, lat, lon, &px, &py); Vector y; y.setZero(); @@ -106,7 +114,7 @@ void BlockLocalPositionEstimator::gpsCorrect() C(Y_gps_vz, X_vz) = 1; // gps covariance matrix - Matrix R; + SquareMatrix R; R.setZero(); // default to parameter, use gps cov if provided @@ -115,12 +123,12 @@ void BlockLocalPositionEstimator::gpsCorrect() float var_vxy = _gps_vxy_stddev.get() * _gps_vxy_stddev.get(); float var_vz = _gps_vz_stddev.get() * _gps_vz_stddev.get(); - // if field is not zero, set it to the value provided - if (_sub_gps.get().eph > 1e-3f) { + // if field is not below minimum, set it to the value provided + if (_sub_gps.get().eph > _gps_xy_stddev.get()) { var_xy = _sub_gps.get().eph * _sub_gps.get().eph; } - if (_sub_gps.get().epv > 1e-3f) { + if (_sub_gps.get().epv > _gps_z_stddev.get()) { var_z = _sub_gps.get().epv * _sub_gps.get().epv; } @@ -133,10 +141,10 @@ void BlockLocalPositionEstimator::gpsCorrect() // get delayed x and P float t_delay = 0; - int i = 0; + int i_hist = 0; - for (i = 1; i < HIST_LEN; i++) { - t_delay = 1.0e-6f * (_timeStamp - _tDelay.get(i)(0, 0)); + for (i_hist = 1; i_hist < HIST_LEN; i_hist++) { + t_delay = 1.0e-6f * (_timeStamp - _tDelay.get(i_hist)(0, 0)); if (t_delay > _gps_delay.get()) { break; @@ -150,10 +158,16 @@ void BlockLocalPositionEstimator::gpsCorrect() return; } - Vector x0 = _xDelay.get(i); + Vector x0 = _xDelay.get(i_hist); // residual Vector r = y - C * x0; + + for (int i = 0; i < 6; i ++) { + _pub_innov.get().vel_pos_innov[i] = r(i); + _pub_innov.get().vel_pos_innov_var[i] = R(i, i); + } + Matrix S_I = inv(C * _P * C.transpose() + R); // fault detection @@ -161,13 +175,12 @@ void BlockLocalPositionEstimator::gpsCorrect() if (beta > BETA_TABLE[n_y_gps]) { if (_gpsFault < FAULT_MINOR) { - //mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] gps fault, beta: %5.2f", double(beta)); - //mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] r: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f", - //double(r(0)), double(r(1)), double(r(2)), - //double(r(3)), double(r(4)), double(r(5))); - //mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] S_I: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f", - //double(S_I(0, 0)), double(S_I(1, 1)), double(S_I(2, 2)), - //double(S_I(3, 3)), double(S_I(4, 4)), double(S_I(5, 5))); + if (beta > 2.0f * BETA_TABLE[n_y_gps]) { + mavlink_and_console_log_critical(&mavlink_log_pub, "[lpe] gps fault %3g %3g %3g %3g %3g %3g", + double(r(0)*r(0) / S_I(0, 0)), double(r(1)*r(1) / S_I(1, 1)), double(r(2)*r(2) / S_I(2, 2)), + double(r(3)*r(3) / S_I(3, 3)), double(r(4)*r(4) / S_I(4, 4)), double(r(5)*r(5) / S_I(5, 5))); + } + _gpsFault = FAULT_MINOR; } @@ -179,7 +192,9 @@ void BlockLocalPositionEstimator::gpsCorrect() // kalman filter correction if no hard fault if (_gpsFault < fault_lvl_disable) { Matrix K = _P * C.transpose() * S_I; - _x += K * r; + Vector dx = K * r; + correctionLogic(dx); + _x += dx; _P -= K * C * _P; } } @@ -190,7 +205,7 @@ void BlockLocalPositionEstimator::gpsCheckTimeout() if (_gpsInitialized) { _gpsInitialized = false; _gpsStats.reset(); - mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] GPS timeout "); + mavlink_and_console_log_critical(&mavlink_log_pub, "[lpe] GPS timeout "); } } } diff --git a/src/modules/local_position_estimator/sensors/lidar.cpp b/src/modules/local_position_estimator/sensors/lidar.cpp index ecbdb4d733..407a49ace6 100644 --- a/src/modules/local_position_estimator/sensors/lidar.cpp +++ b/src/modules/local_position_estimator/sensors/lidar.cpp @@ -33,19 +33,23 @@ void BlockLocalPositionEstimator::lidarInit() int BlockLocalPositionEstimator::lidarMeasure(Vector &y) { // measure - float d = _sub_lidar->get().current_distance + _lidar_z_offset.get(); - warnx("d %10.2g, lidar z offset %10.2g\n", double(d), double(_lidar_z_offset.get())); - float eps = 0.01f; + float d = _sub_lidar->get().current_distance; + float eps = 0.01f; // 1 cm float min_dist = _sub_lidar->get().min_distance + eps; float max_dist = _sub_lidar->get().max_distance - eps; + // prevent driver from setting min dist below eps + if (min_dist < eps) { + min_dist = eps; + } + // check for bad data if (d > max_dist || d < min_dist) { return -1; } // update stats - _lidarStats.update(Scalarf(d)); + _lidarStats.update(Scalarf(d + _lidar_z_offset.get())); _time_last_lidar = _timeStamp; y.setZero(); y(0) = d; @@ -73,7 +77,7 @@ void BlockLocalPositionEstimator::lidarCorrect() C(Y_lidar_z, X_tz) = 1; // measured altitude, negative down dir. // use parameter covariance unless sensor provides reasonable value - Matrix R; + SquareMatrix R; R.setZero(); float cov = _sub_lidar->get().covariance; @@ -87,6 +91,8 @@ void BlockLocalPositionEstimator::lidarCorrect() // residual Matrix S_I = inv((C * _P * C.transpose()) + R); Vector r = y - C * _x; + _pub_innov.get().hagl_innov = r(0); + _pub_innov.get().hagl_innov_var = R(0, 0); // fault detection float beta = (r.transpose() * (S_I * r))(0, 0); @@ -109,14 +115,7 @@ void BlockLocalPositionEstimator::lidarCorrect() if (_lidarFault < fault_lvl_disable) { Matrix K = _P * C.transpose() * S_I; Vector dx = K * r; - - if (!_canEstimateXY) { - dx(X_x) = 0; - dx(X_y) = 0; - dx(X_vx) = 0; - dx(X_vy) = 0; - } - + correctionLogic(dx); _x += dx; _P -= K * C * _P; } diff --git a/src/modules/local_position_estimator/sensors/mocap.cpp b/src/modules/local_position_estimator/sensors/mocap.cpp index f3c26e4fde..d87f94593d 100644 --- a/src/modules/local_position_estimator/sensors/mocap.cpp +++ b/src/modules/local_position_estimator/sensors/mocap.cpp @@ -21,7 +21,7 @@ void BlockLocalPositionEstimator::mocapInit() // if finished if (_mocapStats.getCount() > REQ_MOCAP_INIT_COUNT) { - _mocapHome = _mocapStats.getMean(); + _mocapOrigin = _mocapStats.getMean(); mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap position init: " "%5.2f, %5.2f, %5.2f m std %5.2f, %5.2f, %5.2f m", double(_mocapStats.getMean()(0)), @@ -33,9 +33,9 @@ void BlockLocalPositionEstimator::mocapInit() _mocapInitialized = true; _mocapFault = FAULT_NONE; - if (!_altHomeInitialized) { - _altHomeInitialized = true; - _altHome = _mocapHome(2); + if (!_altOriginInitialized) { + _altOriginInitialized = true; + _altOrigin = _mocapOrigin(2); } } } @@ -47,7 +47,7 @@ int BlockLocalPositionEstimator::mocapMeasure(Vector &y) y(Y_mocap_y) = _sub_mocap.get().y; y(Y_mocap_z) = _sub_mocap.get().z; _mocapStats.update(y); - _time_last_mocap = _sub_mocap.get().timestamp_boot; + _time_last_mocap = _sub_mocap.get().timestamp; return OK; } @@ -58,8 +58,8 @@ void BlockLocalPositionEstimator::mocapCorrect() if (mocapMeasure(y) != OK) { return; } - // make measurement relative to home - y -= _mocapHome; + // make measurement relative to origin + y -= _mocapOrigin; // mocap measurement matrix, measures position Matrix C; @@ -98,7 +98,9 @@ void BlockLocalPositionEstimator::mocapCorrect() // kalman filter correction if no fault if (_mocapFault < fault_lvl_disable) { Matrix K = _P * C.transpose() * S_I; - _x += K * r; + Vector dx = K * r; + correctionLogic(dx); + _x += dx; _P -= K * C * _P; } } diff --git a/src/modules/local_position_estimator/sensors/sonar.cpp b/src/modules/local_position_estimator/sensors/sonar.cpp index 5e6349b671..c03015a53a 100644 --- a/src/modules/local_position_estimator/sensors/sonar.cpp +++ b/src/modules/local_position_estimator/sensors/sonar.cpp @@ -7,37 +7,56 @@ extern orb_advert_t mavlink_log_pub; // required number of samples for sensor // to initialize static const int REQ_SONAR_INIT_COUNT = 10; -static const uint32_t SONAR_TIMEOUT = 1000000; // 1.0 s +static const uint32_t SONAR_TIMEOUT = 5000000; // 2.0 s +static const float SONAR_MAX_INIT_STD = 0.3f; // meters void BlockLocalPositionEstimator::sonarInit() { // measure Vector y; + if (_sonarStats.getCount() == 0) { + _time_init_sonar = _timeStamp; + } + if (sonarMeasure(y) != OK) { - _sonarStats.reset(); return; } // if finished if (_sonarStats.getCount() > REQ_SONAR_INIT_COUNT) { - mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar init " - "mean %d cm std %d cm", - int(100 * _sonarStats.getMean()(0)), - int(100 * _sonarStats.getStdDev()(0))); - _sonarInitialized = true; - _sonarFault = FAULT_NONE; + if (_sonarStats.getStdDev()(0) > SONAR_MAX_INIT_STD) { + mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar init std > min"); + _sonarStats.reset(); + + } else if ((_timeStamp - _time_init_sonar) > SONAR_TIMEOUT) { + mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar init timeout "); + _sonarStats.reset(); + + } else { + PX4_INFO("[lpe] sonar init " + "mean %d cm std %d cm", + int(100 * _sonarStats.getMean()(0)), + int(100 * _sonarStats.getStdDev()(0))); + _sonarInitialized = true; + _sonarFault = FAULT_NONE; + } } } int BlockLocalPositionEstimator::sonarMeasure(Vector &y) { // measure - float d = _sub_sonar->get().current_distance + _sonar_z_offset.get(); - float eps = 0.01f; + float d = _sub_sonar->get().current_distance; + float eps = 0.01f; // 1 cm float min_dist = _sub_sonar->get().min_distance + eps; float max_dist = _sub_sonar->get().max_distance - eps; + // prevent driver from setting min dist below eps + if (min_dist < eps) { + min_dist = eps; + } + // check for bad data if (d > max_dist || d < min_dist) { return -1; @@ -47,7 +66,7 @@ int BlockLocalPositionEstimator::sonarMeasure(Vector &y) _sonarStats.update(Scalarf(d)); _time_last_sonar = _timeStamp; y.setZero(); - y(0) = d * + y(0) = (d + _sonar_z_offset.get()) * cosf(_sub_att.get().roll) * cosf(_sub_att.get().pitch); return OK; @@ -80,12 +99,14 @@ void BlockLocalPositionEstimator::sonarCorrect() C(Y_sonar_z, X_tz) = 1; // measured altitude, negative down dir. // covariance matrix - Matrix R; + SquareMatrix R; R.setZero(); R(0, 0) = cov; // residual Vector r = y - C * _x; + _pub_innov.get().hagl_innov = r(0); + _pub_innov.get().hagl_innov_var = R(0, 0); // residual covariance, (inverse) Matrix S_I = @@ -113,14 +134,7 @@ void BlockLocalPositionEstimator::sonarCorrect() Matrix K = _P * C.transpose() * S_I; Vector dx = K * r; - - if (!_canEstimateXY) { - dx(X_x) = 0; - dx(X_y) = 0; - dx(X_vx) = 0; - dx(X_vy) = 0; - } - + correctionLogic(dx); _x += dx; _P -= K * C * _P; } diff --git a/src/modules/local_position_estimator/sensors/vision.cpp b/src/modules/local_position_estimator/sensors/vision.cpp index 1b3c7ddbca..0832b22473 100644 --- a/src/modules/local_position_estimator/sensors/vision.cpp +++ b/src/modules/local_position_estimator/sensors/vision.cpp @@ -21,7 +21,7 @@ void BlockLocalPositionEstimator::visionInit() // increament sums for mean if (_visionStats.getCount() > REQ_VISION_INIT_COUNT) { - _visionHome = _visionStats.getMean(); + _visionOrigin = _visionStats.getMean(); mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position init: " "%5.2f %5.2f %5.2f m std %5.2f %5.2f %5.2f m", double(_visionStats.getMean()(0)), @@ -33,9 +33,9 @@ void BlockLocalPositionEstimator::visionInit() _visionInitialized = true; _visionFault = FAULT_NONE; - if (!_altHomeInitialized) { - _altHomeInitialized = true; - _altHome = _visionHome(2); + if (!_altOriginInitialized) { + _altOriginInitialized = true; + _altOrigin = _visionOrigin(2); } } } @@ -47,7 +47,7 @@ int BlockLocalPositionEstimator::visionMeasure(Vector &y) y(Y_vision_y) = _sub_vision_pos.get().y; y(Y_vision_z) = _sub_vision_pos.get().z; _visionStats.update(y); - _time_last_vision_p = _sub_vision_pos.get().timestamp_boot; + _time_last_vision_p = _sub_vision_pos.get().timestamp; return OK; } @@ -58,8 +58,8 @@ void BlockLocalPositionEstimator::visionCorrect() if (visionMeasure(y) != OK) { return; } - // make measurement relative to home - y -= _visionHome; + // make measurement relative to origin + y -= _visionOrigin; // vision measurement matrix, measures position Matrix C; @@ -96,7 +96,9 @@ void BlockLocalPositionEstimator::visionCorrect() // kalman filter correction if no fault if (_visionFault < fault_lvl_disable) { Matrix K = _P * C.transpose() * S_I; - _x += K * r; + Vector dx = K * r; + correctionLogic(dx); + _x += dx; _P -= K * C * _P; } } @@ -107,7 +109,7 @@ void BlockLocalPositionEstimator::visionCheckTimeout() if (_visionInitialized) { _visionInitialized = false; _visionStats.reset(); - mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position timeout "); + mavlink_and_console_log_critical(&mavlink_log_pub, "[lpe] vision position timeout "); } } } diff --git a/src/drivers/qshell/qurt/module.mk b/src/modules/logger/CMakeLists.txt similarity index 86% rename from src/drivers/qshell/qurt/module.mk rename to src/modules/logger/CMakeLists.txt index 97473d9554..c918621779 100644 --- a/src/drivers/qshell/qurt/module.mk +++ b/src/modules/logger/CMakeLists.txt @@ -31,16 +31,17 @@ # ############################################################################ -# -# Listener for shell commands from posix -# - -MODULE_COMMAND = qshell - -SRCS = \ - qshell_start_qurt.cpp \ - qshell.cpp - -INCLUDE_DIRS += $(PX4_BASE)/src/modules/uORB \ - $(PX4_BASE)/src/platforms \ - $(PX4_BASE)/src/modules +px4_add_module( + MODULE modules__logger + MAIN logger + PRIORITY "SCHED_PRIORITY_MAX-30" + STACK_MAIN 2200 + COMPILE_FLAGS -Os + SRCS + logger.cpp + log_writer.cpp + DEPENDS + platforms__common + modules__uORB + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/logger/array.h b/src/modules/logger/array.h new file mode 100644 index 0000000000..9ac1d00062 --- /dev/null +++ b/src/modules/logger/array.h @@ -0,0 +1,160 @@ +/**************************************************************************** + * + * Copyright (c) 2016 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. + * + ****************************************************************************/ + +#pragma once + +#include + +namespace px4 +{ + +template +class Array +{ + typedef TYPE &reference; + typedef const TYPE &const_reference; + typedef TYPE *iterator; + typedef const TYPE *const_iterator; + +public: + Array() + : _size(0), _overflow(false) + {} + + bool push_back(const TYPE &x) + { + if (_size == N) { + _overflow = true; + return false; + + } else { + _items[_size] = x; + ++_size; + return true; + } + } + + void remove(unsigned idx) + { + if (idx < _size) { + --_size; + + for (unsigned i = idx; i < _size; ++i) { + _items[i] = _items[i + 1]; + } + } + } + + reference operator[](size_t n) + { + return _items[n]; + } + + const_reference operator[](size_t n) const + { + return _items[n]; + } + + reference at(size_t n) + { + return _items[n]; + } + + const_reference at(size_t n) const + { + return _items[n]; + } + + size_t size() const + { + return _size; + } + + size_t max_size() const + { + return N; + } + + size_t capacity() const + { + return N; + } + + bool empty() const + { + return _size == 0; + } + + bool is_overflowed() + { + return _overflow; + } + + iterator begin() + { + return &_items[0]; + } + + iterator end() + { + return &_items[_size]; + } + + const_iterator begin() const + { + return &_items[0]; + } + + const_iterator end() const + { + return &_items[_size]; + } + + void erase(iterator item) + { + if (item - _items < static_cast(_size)) { + --_size; + + for (iterator it = item; it != &_items[_size]; ++it) { + *it = *(it + 1); + } + } + } + +private: + TYPE _items[N]; + size_t _size; + bool _overflow; +}; + +} diff --git a/src/modules/logger/log_writer.cpp b/src/modules/logger/log_writer.cpp new file mode 100644 index 0000000000..c645b6dbeb --- /dev/null +++ b/src/modules/logger/log_writer.cpp @@ -0,0 +1,313 @@ +/**************************************************************************** + * + * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "log_writer.h" +#include "messages.h" +#include +#include + +#include + +namespace px4 +{ +namespace logger +{ +constexpr size_t LogWriter::_min_write_chunk; + + +LogWriter::LogWriter(size_t buffer_size) : + //We always write larger chunks (orb messages) to the buffer, so the buffer + //needs to be larger than the minimum write chunk (300 is somewhat arbitrary) + _buffer_size(math::max(buffer_size, _min_write_chunk + 300)) +{ + pthread_mutex_init(&_mtx, nullptr); + pthread_cond_init(&_cv, nullptr); + /* allocate write performance counters */ + _perf_write = perf_alloc(PC_ELAPSED, "sd write"); + _perf_fsync = perf_alloc(PC_ELAPSED, "sd fsync"); +} + +bool LogWriter::init() +{ + if (_buffer) { + return true; + } + + _buffer = new uint8_t[_buffer_size]; + + return _buffer; +} + +LogWriter::~LogWriter() +{ + pthread_mutex_destroy(&_mtx); + pthread_cond_destroy(&_cv); + perf_free(_perf_write); + perf_free(_perf_fsync); + + if (_buffer) { + delete[] _buffer; + } +} + +void LogWriter::start_log(const char *filename) +{ + ::strncpy(_filename, filename, sizeof(_filename)); + _fd = ::open(_filename, O_CREAT | O_WRONLY, PX4_O_MODE_666); + + if (_fd < 0) { + PX4_ERR("Can't open log file %s", _filename); + _should_run = false; + return; + + } else { + PX4_INFO("Opened log file: %s", _filename); + _should_run = true; + _running = true; + } + + // Clear buffer and counters + _head = 0; + _count = 0; + _total_written = 0; + notify(); +} + +void LogWriter::stop_log() +{ + _should_run = false; + notify(); +} + +int LogWriter::thread_start(pthread_t &thread) +{ + pthread_attr_t thr_attr; + pthread_attr_init(&thr_attr); + + sched_param param; + /* low priority, as this is expensive disk I/O */ + param.sched_priority = SCHED_PRIORITY_DEFAULT - 40; + (void)pthread_attr_setschedparam(&thr_attr, ¶m); + + pthread_attr_setstacksize(&thr_attr, 1024); + + int ret = pthread_create(&thread, &thr_attr, &LogWriter::run_helper, this); + pthread_attr_destroy(&thr_attr); + + return ret; +} + +void LogWriter::thread_stop() +{ + // this will terminate the main loop of the writer thread + _exit_thread = true; + _should_run = false; +} + +void *LogWriter::run_helper(void *context) +{ + px4_prctl(PR_SET_NAME, "log_writer", px4_getpid()); + + reinterpret_cast(context)->run(); + return nullptr; +} + +void LogWriter::run() +{ + while (!_exit_thread) { + // Outer endless loop + // Wait for _should_run flag + while (!_exit_thread) { + bool start = false; + pthread_mutex_lock(&_mtx); + pthread_cond_wait(&_cv, &_mtx); + start = _should_run; + pthread_mutex_unlock(&_mtx); + + if (start) { + break; + } + } + + int poll_count = 0; + int written = 0; + + while (true) { + size_t available = 0; + void *read_ptr = nullptr; + bool is_part = false; + + /* lock _buffer + * wait for sufficient data, cycle on notify() + */ + pthread_mutex_lock(&_mtx); + + while (true) { + available = get_read_ptr(&read_ptr, &is_part); + + /* if sufficient data available or partial read or terminating, exit this wait loop */ + if ((available >= _min_write_chunk) || is_part || !_should_run) { + /* GOTO end of block */ + break; + } + + /* wait for a call to notify() + * this call unlocks the mutex while waiting, and returns with the mutex locked + */ + pthread_cond_wait(&_cv, &_mtx); + } + + pthread_mutex_unlock(&_mtx); + written = 0; + + if (available > 0) { + perf_begin(_perf_write); + written = ::write(_fd, read_ptr, available); + perf_end(_perf_write); + + /* call fsync periodically to minimize potential loss of data */ + if (++poll_count >= 100) { + perf_begin(_perf_fsync); + ::fsync(_fd); + perf_end(_perf_fsync); + poll_count = 0; + } + + if (written < 0) { + PX4_WARN("error writing log file"); + _should_run = false; + /* GOTO end of block */ + break; + } + + pthread_mutex_lock(&_mtx); + /* subtract bytes written from number in _buffer (_count -= written) */ + mark_read(written); + pthread_mutex_unlock(&_mtx); + + _total_written += written; + } + + if (!_should_run && written == static_cast(available) && !is_part) { + // Stop only when all data written + _running = false; + _head = 0; + _count = 0; + + if (_fd >= 0) { + int res = ::close(_fd); + _fd = -1; + + if (res) { + PX4_WARN("error closing log file"); + + } else { + PX4_INFO("closed logfile: %s, bytes written: %zu", _filename, _total_written); + } + } + + break; + } + } + } +} + +bool LogWriter::write(void *ptr, size_t size, uint64_t dropout_start) +{ + + // Bytes available to write + size_t available = _buffer_size - _count; + size_t dropout_size = 0; + + if (dropout_start) { + dropout_size = sizeof(ulog_message_dropout_s); + } + + if (size + dropout_size > available) { + // buffer overflow + return false; + } + + if (dropout_start) { + //write dropout msg + ulog_message_dropout_s dropout_msg; + dropout_msg.duration = (uint16_t)(hrt_elapsed_time(&dropout_start) / 1000); + write_no_check(&dropout_msg, sizeof(dropout_msg)); + } + + write_no_check(ptr, size); + return true; +} + +void LogWriter::write_no_check(void *ptr, size_t size) +{ + size_t n = _buffer_size - _head; // bytes to end of the buffer + + uint8_t *buffer_c = reinterpret_cast(ptr); + + if (size > n) { + // Message goes over the end of the buffer + memcpy(&(_buffer[_head]), buffer_c, n); + _head = 0; + + } else { + n = 0; + } + + // now: n = bytes already written + size_t p = size - n; // number of bytes to write + memcpy(&(_buffer[_head]), &(buffer_c[n]), p); + _head = (_head + p) % _buffer_size; + _count += size; +} + +size_t LogWriter::get_read_ptr(void **ptr, bool *is_part) +{ + // bytes available to read + int read_ptr = _head - _count; + + if (read_ptr < 0) { + read_ptr += _buffer_size; + *ptr = &_buffer[read_ptr]; + *is_part = true; + return _buffer_size - read_ptr; + + } else { + *ptr = &_buffer[read_ptr]; + *is_part = false; + return _count; + } +} + +} +} diff --git a/src/modules/logger/log_writer.h b/src/modules/logger/log_writer.h new file mode 100644 index 0000000000..1d230f5602 --- /dev/null +++ b/src/modules/logger/log_writer.h @@ -0,0 +1,142 @@ +/**************************************************************************** + * + * Copyright (c) 2016 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include + +namespace px4 +{ +namespace logger +{ + +class LogWriter +{ +public: + LogWriter(size_t buffer_size); + ~LogWriter(); + + bool init(); + + /** + * start the thread + * @param thread will be set to the created thread on success + * @return 0 on success, error number otherwise (@see pthread_create) + */ + int thread_start(pthread_t &thread); + + void thread_stop(); + + void start_log(const char *filename); + + void stop_log(); + + /** + * Write data to be logged. The caller must call lock() before calling this. + * @param dropout_start timestamp when lastest dropout occured. 0 if no dropout at the moment. + * @return true on success, false if not enough space in the buffer left + */ + bool write(void *ptr, size_t size, uint64_t dropout_start = 0); + + void lock() + { + pthread_mutex_lock(&_mtx); + } + + void unlock() + { + pthread_mutex_unlock(&_mtx); + } + + void notify() + { + pthread_cond_broadcast(&_cv); + } + + size_t get_total_written() const + { + return _total_written; + } + + size_t get_buffer_size() const + { + return _buffer_size; + } + + size_t get_buffer_fill_count() const + { + return _count; + } + +private: + static void *run_helper(void *); + + void run(); + + size_t get_read_ptr(void **ptr, bool *is_part); + + void mark_read(size_t n) + { + _count -= n; + } + + /** + * Write to the buffer but assuming there is enough space + */ + inline void write_no_check(void *ptr, size_t size); + + /* 512 didn't seem to work properly, 4096 should match the FAT cluster size */ + static constexpr size_t _min_write_chunk = 4096; + + char _filename[64]; + int _fd = -1; + uint8_t *_buffer = nullptr; + const size_t _buffer_size; + size_t _head = 0; ///< next position to write to + size_t _count = 0; ///< number of bytes in _buffer to be written + size_t _total_written = 0; + bool _should_run = false; + bool _running = false; + bool _exit_thread = false; + pthread_mutex_t _mtx; + pthread_cond_t _cv; + perf_counter_t _perf_write; + perf_counter_t _perf_fsync; +}; + +} +} diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp new file mode 100644 index 0000000000..0331ebf2ba --- /dev/null +++ b/src/modules/logger/logger.cpp @@ -0,0 +1,1361 @@ +/**************************************************************************** + * + * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "logger.h" +#include "messages.h" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#ifdef __PX4_DARWIN +#include +#include +#else +#include +#endif + +#define GPS_EPOCH_SECS ((time_t)1234567890ULL) + +//#define DBGPRINT //write status output every few seconds + +#if defined(DBGPRINT) +// needed for mallinfo +#if defined(__PX4_POSIX) && !defined(__PX4_DARWIN) +#include +#endif /* __PX4_POSIX */ + +// struct mallinfo not available on OSX? +#if defined(__PX4_DARWIN) +#undef DBGPRINT +#endif /* defined(__PX4_DARWIN) */ +#endif /* defined(DBGPRINT) */ + +using namespace px4::logger; + +static Logger *logger_ptr = nullptr; +static int logger_task = -1; +static pthread_t writer_thread; + +/* This is used to schedule work for the logger (periodic scan for updated topics) */ +static void timer_callback(void *arg) +{ + px4_sem_t *semaphore = (px4_sem_t *)arg; + px4_sem_post(semaphore); +} + + +int logger_main(int argc, char *argv[]) +{ + // logger currently assumes little endian + int num = 1; + + if (*(char *)&num != 1) { + PX4_ERR("Logger only works on little endian!\n"); + return 1; + } + + if (argc < 2) { + PX4_INFO("usage: logger {start|stop|status}"); + return 1; + } + + //Check if thread exited, but the object has not been destroyed yet (happens in case of an error) + if (logger_task == -1 && logger_ptr) { + delete logger_ptr; + logger_ptr = nullptr; + } + + if (!strcmp(argv[1], "start")) { + + if (logger_ptr != nullptr) { + PX4_INFO("already running"); + return 1; + } + + if (OK != Logger::start((char *const *)argv)) { + PX4_WARN("start failed"); + return 1; + } + + return 0; + } + + if (!strcmp(argv[1], "on")) { + if (logger_ptr != nullptr) { + logger_ptr->set_arm_override(true); + return 0; + } + + return 1; + } + + if (!strcmp(argv[1], "off")) { + if (logger_ptr != nullptr) { + logger_ptr->set_arm_override(false); + return 0; + } + + return 1; + } + + if (!strcmp(argv[1], "stop")) { + if (logger_ptr == nullptr) { + PX4_INFO("not running"); + return 1; + } + + logger_ptr->print_statistics(); + delete logger_ptr; + logger_ptr = nullptr; + return 0; + } + + if (!strcmp(argv[1], "status")) { + if (logger_ptr) { + logger_ptr->status(); + return 0; + + } else { + PX4_INFO("not running"); + return 1; + } + } + + Logger::usage("unrecognized command"); + return 1; +} + +namespace px4 +{ +namespace logger +{ + +void Logger::usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PX4_INFO("usage: logger {start|stop|on|off|status} [-r ] [-b ] -e -a -t -x\n" + "\t-r\tLog rate in Hz, 0 means unlimited rate\n" + "\t-b\tLog buffer size in KiB, default is 12\n" + "\t-e\tEnable logging right after start until disarm (otherwise only when armed)\n" + "\t-f\tLog until shutdown (implies -e)\n" + "\t-t\tUse date/time for naming log directories and files"); +} + +int Logger::start(char *const *argv) +{ + ASSERT(logger_task == -1); + + /* start the task */ + logger_task = px4_task_spawn_cmd("logger", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 3200, + (px4_main_t)&Logger::run_trampoline, + (char *const *)argv); + + if (logger_task < 0) { + logger_task = -1; + PX4_WARN("task start failed"); + return -errno; + } + + return OK; +} + +void Logger::status() +{ + if (!_enabled) { + PX4_INFO("Running, but not logging"); + + } else { + PX4_INFO("Running"); + print_statistics(); + } +} +void Logger::print_statistics() +{ + if (!_enabled) { + return; + } + + float kibibytes = _writer.get_total_written() / 1024.0f; + float mebibytes = kibibytes / 1024.0f; + float seconds = ((float)(hrt_absolute_time() - _start_time)) / 1000000.0f; + + PX4_INFO("Wrote %4.2f MiB (avg %5.2f KiB/s)", (double)mebibytes, (double)(kibibytes / seconds)); + PX4_INFO("Since last status: dropouts: %zu (max len: %.3f s), max used buffer: %zu / %zu B", + _write_dropouts, (double)_max_dropout_duration, _high_water, _writer.get_buffer_size()); + _high_water = 0; + _write_dropouts = 0; + _max_dropout_duration = 0.f; +} + +void Logger::run_trampoline(int argc, char *argv[]) +{ + uint32_t log_interval = 3500; + int log_buffer_size = 12 * 1024; + bool log_on_start = false; + bool log_until_shutdown = false; + bool error_flag = false; + bool log_name_timestamp = false; + + int myoptind = 1; + int ch; + const char *myoptarg = NULL; + + while ((ch = px4_getopt(argc, argv, "r:b:etf", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'r': { + unsigned long r = strtoul(myoptarg, NULL, 10); + + if (r <= 0) { + r = 1e6; + } + + log_interval = 1e6 / r; + } + break; + + case 'e': + log_on_start = true; + break; + + case 'b': { + unsigned long s = strtoul(myoptarg, NULL, 10); + + if (s < 1) { + s = 1; + } + + log_buffer_size = 1024 * s; + } + break; + + case 't': + log_name_timestamp = true; + break; + + case 'f': + log_on_start = true; + log_until_shutdown = true; + break; + + case '?': + error_flag = true; + break; + + default: + PX4_WARN("unrecognized flag"); + error_flag = true; + break; + } + } + + if (error_flag) { + logger_task = -1; + return; + } + + logger_ptr = new Logger(log_buffer_size, log_interval, log_on_start, + log_until_shutdown, log_name_timestamp); + +#if defined(DBGPRINT) && defined(__PX4_NUTTX) + struct mallinfo alloc_info = mallinfo(); + PX4_INFO("largest free chunk: %d bytes", alloc_info.mxordblk); + PX4_INFO("remaining free heap: %d bytes", alloc_info.fordblks); +#endif /* DBGPRINT */ + + if (logger_ptr == nullptr) { + PX4_ERR("alloc failed"); + + } else { + //check for replay mode + const char *logfile = getenv(px4::replay::ENV_FILENAME); + + if (logfile) { + logger_ptr->setReplayFile(logfile); + } + + logger_ptr->run(); + } + + logger_task = -1; +} + + +Logger::Logger(size_t buffer_size, uint32_t log_interval, bool log_on_start, + bool log_until_shutdown, bool log_name_timestamp) : + _arm_override(false), + _log_on_start(log_on_start), + _log_until_shutdown(log_until_shutdown), + _log_name_timestamp(log_name_timestamp), + _writer(buffer_size), + _log_interval(log_interval) +{ + _log_utc_offset = param_find("SDLOG_UTC_OFFSET"); +} + +Logger::~Logger() +{ + if (logger_task != -1) { + /* task wakes up every 100ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned int i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 200) { + px4_task_delete(logger_task); + logger_task = -1; + break; + } + } while (logger_task != -1); + } + + if (_replay_file_name) { + free(_replay_file_name); + } + + if (_msg_buffer) { + delete[](_msg_buffer); + } +} + +int Logger::add_topic(const orb_metadata *topic) +{ + int fd = -1; + size_t fields_len = strlen(topic->o_fields) + strlen(topic->o_name) + 1; //1 for ':' + + if (fields_len > sizeof(ulog_message_format_s::format)) { + PX4_WARN("skip topic %s, format string is too large: %zu (max is %zu)", topic->o_name, fields_len, + sizeof(ulog_message_format_s::format)); + + return -1; + } + + fd = orb_subscribe(topic); + + if (fd < 0) { + PX4_WARN("logger: orb_subscribe failed"); + return -1; + } + + if (!_subscriptions.push_back(LoggerSubscription(fd, topic))) { + PX4_WARN("logger: failed to add topic. Too many subscriptions"); + orb_unsubscribe(fd); + fd = -1; + } + + return fd; +} + +int Logger::add_topic(const char *name, unsigned interval = 0) +{ + const orb_metadata **topics = orb_get_topics(); + int fd = -1; + + for (size_t i = 0; i < orb_topics_count(); i++) { + if (strcmp(name, topics[i]->o_name) == 0) { + fd = add_topic(topics[i]); + PX4_DEBUG("logging topic: %s, interval: %i", topics[i]->o_name, interval); + break; + } + } + + if (fd >= 0 && interval != 0) { + orb_set_interval(fd, interval); + } + + return fd; +} + +bool Logger::copy_if_updated_multi(LoggerSubscription &sub, int multi_instance, void *buffer) +{ + bool updated = false; + int &handle = sub.fd[multi_instance]; + + // only try to subscribe to topic if this is the first time + // after that just check after a certain interval to avoid high cpu usage + if (handle < 0 && (sub.time_tried_subscribe == 0 || + hrt_elapsed_time(&sub.time_tried_subscribe) > TRY_SUBSCRIBE_INTERVAL)) { + sub.time_tried_subscribe = hrt_absolute_time(); + + if (OK == orb_exists(sub.metadata, multi_instance)) { + handle = orb_subscribe_multi(sub.metadata, multi_instance); + + //PX4_INFO("subscribed to instance %d of topic %s", multi_instance, topic->o_name); + + /* copy first data */ + if (handle >= 0) { + write_add_logged_msg(sub, multi_instance); + + /* set to the same interval as the first instance */ + unsigned int interval; + + if (orb_get_interval(sub.fd[0], &interval) == 0 && interval > 0) { + orb_set_interval(handle, interval); + } + + orb_copy(sub.metadata, handle, buffer); + updated = true; + } + } + + } else if (handle >= 0) { + orb_check(handle, &updated); + + if (updated) { + orb_copy(sub.metadata, handle, buffer); + } + } + + return updated; +} + +void Logger::add_default_topics() +{ + add_topic("vehicle_attitude", 10); + add_topic("actuator_outputs", 50); + add_topic("telemetry_status", 50); + add_topic("vehicle_command"); + add_topic("vehicle_status", 200); + add_topic("vtol_vehicle_status", 100); + add_topic("commander_state", 100); + add_topic("satellite_info", 1000); + add_topic("vehicle_attitude_setpoint", 20); + add_topic("vehicle_rates_setpoint", 10); + add_topic("actuator_controls", 20); + add_topic("actuator_controls_0", 20); + add_topic("actuator_controls_1", 20); + add_topic("vehicle_local_position", 100); + add_topic("vehicle_local_position_setpoint", 50); + add_topic("vehicle_global_position", 100); + add_topic("vehicle_global_velocity_setpoint", 100); + add_topic("battery_status", 300); + add_topic("system_power", 300); + add_topic("position_setpoint_triplet", 10); + add_topic("att_pos_mocap", 50); + add_topic("vision_position_estimate", 50); + add_topic("optical_flow", 50); + add_topic("rc_channels"); + add_topic("airspeed", 50); + add_topic("distance_sensor", 20); + add_topic("esc_status", 20); + add_topic("estimator_status", 50); //this one is large + add_topic("ekf2_innovations", 20); + add_topic("tecs_status", 20); + add_topic("wind_estimate", 100); + add_topic("control_state", 20); + add_topic("camera_trigger"); + add_topic("cpuload"); + add_topic("gps_dump"); //this will only be published if GPS_DUMP_COMM is set + + /* for estimator replay (need to be at full rate) */ + add_topic("sensor_combined"); + add_topic("vehicle_gps_position"); + add_topic("vehicle_land_detected"); +} + +int Logger::add_topics_from_file(const char *fname) +{ + FILE *fp; + char line[80]; + char topic_name[80]; + unsigned interval; + int ntopics = 0; + + /* open the topic list file */ + fp = fopen(fname, "r"); + + if (fp == NULL) { + return -1; + } + + /* call add_topic for each topic line in the file */ + // format is TOPIC_NAME, [interval] + for (;;) { + + /* get a line, bail on error/EOF */ + line[0] = '\0'; + + if (fgets(line, sizeof(line), fp) == NULL) { + break; + } + + /* skip comment lines */ + if ((strlen(line) < 2) || (line[0] == '#')) { + continue; + } + + // default interval to zero + interval = 0; + int nfields = sscanf(line, "%s, %u", topic_name, &interval); + + if (nfields > 0) { + /* add topic with specified interval */ + add_topic(topic_name, interval); + ntopics++; + } + } + + fclose(fp); + return ntopics; +} + +void Logger::run() +{ +#ifdef DBGPRINT + struct mallinfo alloc_info = {}; +#endif /* DBGPRINT */ + + PX4_INFO("logger started"); + + int mkdir_ret = mkdir(LOG_ROOT, S_IRWXU | S_IRWXG | S_IRWXO); + + if (mkdir_ret == 0) { + PX4_INFO("log root dir created: %s", LOG_ROOT); + + } else if (errno != EEXIST) { + PX4_ERR("failed creating log root dir: %s", LOG_ROOT); + return; + } + + if (check_free_space() == 1) { + return; + } + + uORB::Subscription vehicle_status_sub(ORB_ID(vehicle_status)); + uORB::Subscription parameter_update_sub(ORB_ID(parameter_update)); + uORB::Subscription mavlink_log_sub(ORB_ID(mavlink_log)); + + int ntopics = add_topics_from_file(PX4_ROOTFSDIR "/fs/microsd/etc/logging/logger_topics.txt"); + + if (ntopics > 0) { + PX4_INFO("logging %d topics from logger_topics.txt", ntopics); + + } else { + add_default_topics(); + } + + //all topics added. Get required message buffer size + int max_msg_size = 0; + + for (const auto &subscription : _subscriptions) { + //use o_size, because that's what orb_copy will use + if (subscription.metadata->o_size > max_msg_size) { + max_msg_size = subscription.metadata->o_size; + } + } + + max_msg_size += sizeof(ulog_message_data_header_s); + + if (sizeof(ulog_message_logging_s) > max_msg_size) { + max_msg_size = sizeof(ulog_message_logging_s); + } + + if (max_msg_size > _msg_buffer_len) { + if (_msg_buffer) { + delete[](_msg_buffer); + } + + _msg_buffer_len = max_msg_size; + _msg_buffer = new uint8_t[_msg_buffer_len]; + + if (!_msg_buffer) { + PX4_ERR("failed to alloc message buffer"); + return; + } + } + + + if (!_writer.init()) { + PX4_ERR("init of writer failed (alloc failed)"); + return; + } + + int ret = _writer.thread_start(writer_thread); + + if (ret) { + PX4_ERR("failed to create writer thread (%i)", ret); + return; + } + + _task_should_exit = false; + +#ifdef DBGPRINT + hrt_abstime timer_start = 0; + uint32_t total_bytes = 0; +#endif /* DBGPRINT */ + + // we start logging immediately + // the case where we wait with logging until vehicle is armed is handled below + if (_log_on_start) { + start_log(); + } + + /* init the update timer */ + struct hrt_call timer_call; + px4_sem_t timer_semaphore; + px4_sem_init(&timer_semaphore, 0, 0); + hrt_call_every(&timer_call, _log_interval, _log_interval, timer_callback, &timer_semaphore); + + + while (!_task_should_exit) { + + // Start/stop logging when system arm/disarm + if (vehicle_status_sub.check_updated()) { + vehicle_status_sub.update(); + bool armed = (vehicle_status_sub.get().arming_state == vehicle_status_s::ARMING_STATE_ARMED) || + (vehicle_status_sub.get().arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) || + _arm_override; + + if (_was_armed != armed && !_log_until_shutdown) { + _was_armed = armed; + + if (armed) { + start_log(); + +#ifdef DBGPRINT + timer_start = hrt_absolute_time(); + total_bytes = 0; +#endif /* DBGPRINT */ + + } else { + stop_log(); + } + } + } + + if (_enabled) { + + bool data_written = false; + + /* Check if parameters have changed */ + // this needs to change to a timestamped record to record a history of parameter changes + if (parameter_update_sub.check_updated()) { + parameter_update_sub.update(); + write_changed_parameters(); + } + + /* wait for lock on log buffer */ + _writer.lock(); + + for (LoggerSubscription &sub : _subscriptions) { + /* each message consists of a header followed by an orb data object + */ + size_t msg_size = sizeof(ulog_message_data_header_s) + sub.metadata->o_size_no_padding; + + /* if this topic has been updated, copy the new data into the message buffer + * and write a message to the log + */ + for (uint8_t instance = 0; instance < ORB_MULTI_MAX_INSTANCES; instance++) { + if (copy_if_updated_multi(sub, instance, _msg_buffer + sizeof(ulog_message_data_header_s))) { + + uint16_t write_msg_size = static_cast(msg_size - ULOG_MSG_HEADER_LEN); + //write one byte after another (necessary because of alignment) + _msg_buffer[0] = (uint8_t)write_msg_size; + _msg_buffer[1] = (uint8_t)(write_msg_size >> 8); + _msg_buffer[2] = static_cast(ULogMessageType::DATA); + uint16_t write_msg_id = sub.msg_ids[instance]; + _msg_buffer[3] = (uint8_t)write_msg_id; + _msg_buffer[4] = (uint8_t)(write_msg_id >> 8); + + //PX4_INFO("topic: %s, size = %zu, out_size = %zu", sub.metadata->o_name, sub.metadata->o_size, msg_size); + + if (write(_msg_buffer, msg_size)) { + +#ifdef DBGPRINT + total_bytes += msg_size; +#endif /* DBGPRINT */ + + data_written = true; + + } else { + break; // Write buffer overflow, skip this record + } + } + } + } + + //check for new mavlink log message + if (mavlink_log_sub.check_updated()) { + mavlink_log_sub.update(); + const char *message = (const char *)mavlink_log_sub.get().text; + int message_len = strlen(message); + + if (message_len > 0) { + uint16_t write_msg_size = sizeof(ulog_message_logging_s) - sizeof(ulog_message_logging_s::message) + - ULOG_MSG_HEADER_LEN + message_len; + _msg_buffer[0] = (uint8_t)write_msg_size; + _msg_buffer[1] = (uint8_t)(write_msg_size >> 8); + _msg_buffer[2] = static_cast(ULogMessageType::LOGGING); + _msg_buffer[3] = mavlink_log_sub.get().severity + '0'; + memcpy(_msg_buffer + 4, &mavlink_log_sub.get().timestamp, sizeof(ulog_message_logging_s::timestamp)); + strncpy((char *)(_msg_buffer + 12), message, sizeof(ulog_message_logging_s::message)); + + write(_msg_buffer, write_msg_size + ULOG_MSG_HEADER_LEN); + } + } + + if (!_dropout_start && _writer.get_buffer_fill_count() > _high_water) { + _high_water = _writer.get_buffer_fill_count(); + } + + /* release the log buffer */ + _writer.unlock(); + + /* notify the writer thread if data is available */ + if (data_written) { + _writer.notify(); + } + +#ifdef DBGPRINT + double deltat = (double)(hrt_absolute_time() - timer_start) * 1e-6; + + if (deltat > 4.0) { + alloc_info = mallinfo(); + double throughput = total_bytes / deltat; + PX4_INFO("%8.1f kB/s, %zu highWater, %d dropouts, %5.3f sec max, free heap: %d", + throughput / 1.e3, _high_water, _write_dropouts, (double)_max_dropout_duration, + alloc_info.fordblks); + + _high_water = 0; + _max_dropout_duration = 0.f; + total_bytes = 0; + timer_start = hrt_absolute_time(); + } + +#endif /* DBGPRINT */ + + } + + /* + * We wait on the semaphore, which periodically gets updated by a high-resolution timer. + * The simpler alternative would be: + * usleep(max(300, _log_interval - elapsed_time_since_loop_start)); + * And on linux this is quite accurate as well, but under NuttX it is not accurate, + * because usleep() has only a granularity of CONFIG_MSEC_PER_TICK (=1ms). + */ + while (px4_sem_wait(&timer_semaphore) != 0); + } + + hrt_cancel(&timer_call); + px4_sem_destroy(&timer_semaphore); + + // stop the writer thread + _writer.thread_stop(); + + _writer.notify(); + + // wait for thread to complete + ret = pthread_join(writer_thread, NULL); + + if (ret) { + PX4_WARN("join failed: %d", ret); + } + + //unsubscribe + for (LoggerSubscription &sub : _subscriptions) { + for (uint8_t instance = 0; instance < ORB_MULTI_MAX_INSTANCES; instance++) { + if (sub.fd[instance] != -1) { + orb_unsubscribe(sub.fd[instance]); + sub.fd[instance] = -1; + } + } + } + + if (_mavlink_log_pub) { + orb_unadvertise(_mavlink_log_pub); + _mavlink_log_pub = nullptr; + } +} + +bool Logger::write(void *ptr, size_t size) +{ + if (_writer.write(ptr, size, _dropout_start)) { + + if (_dropout_start) { + float dropout_duration = (float)(hrt_elapsed_time(&_dropout_start) / 1000) / 1.e3f; + + if (dropout_duration > _max_dropout_duration) { + _max_dropout_duration = dropout_duration; + } + + _dropout_start = 0; + } + + return true; + } + + if (!_dropout_start) { + _dropout_start = hrt_absolute_time(); + ++_write_dropouts; + _high_water = 0; + } + + return false; +} + +int Logger::create_log_dir(tm *tt) +{ + /* create dir on sdcard if needed */ + int mkdir_ret; + + if (tt) { + int n = snprintf(_log_dir, sizeof(_log_dir), "%s/", LOG_ROOT); + + if (n >= sizeof(_log_dir)) { + PX4_ERR("log path too long"); + return -1; + } + + strftime(_log_dir + n, sizeof(_log_dir) - n, "%Y-%m-%d", tt); + mkdir_ret = mkdir(_log_dir, S_IRWXU | S_IRWXG | S_IRWXO); + + if (mkdir_ret != OK && errno != EEXIST) { + PX4_ERR("failed creating new dir: %s", _log_dir); + return -1; + } + + } else { + uint16_t dir_number = 1; // start with dir sess001 + + /* look for the next dir that does not exist */ + while (!_has_log_dir && dir_number <= MAX_NO_LOGFOLDER) { + /* format log dir: e.g. /fs/microsd/sess001 */ + int n = snprintf(_log_dir, sizeof(_log_dir), "%s/sess%03u", LOG_ROOT, dir_number); + + if (n >= sizeof(_log_dir)) { + PX4_ERR("log path too long"); + return -1; + } + + mkdir_ret = mkdir(_log_dir, S_IRWXU | S_IRWXG | S_IRWXO); + + if (mkdir_ret == 0) { + PX4_DEBUG("log dir created: %s", _log_dir); + _has_log_dir = true; + + } else if (errno != EEXIST) { + PX4_ERR("failed creating new dir: %s (%i)", _log_dir, errno); + return -1; + } + + /* dir exists already */ + dir_number++; + } + + if (dir_number >= MAX_NO_LOGFOLDER) { + /* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */ + PX4_ERR("All %d possible dirs exist already", MAX_NO_LOGFOLDER); + return -1; + } + + _has_log_dir = true; + } + + return 0; +} + +bool Logger::file_exist(const char *filename) +{ + struct stat buffer; + return stat(filename, &buffer) == 0; +} + +int Logger::get_log_file_name(char *file_name, size_t file_name_size) +{ + tm tt; + bool time_ok = false; + + if (_log_name_timestamp) { + /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.ulg */ + time_ok = get_log_time(&tt, false); + } + + const char *replay_suffix = ""; + + if (_replay_file_name) { + replay_suffix = "_replayed"; + } + + + if (time_ok) { + if (create_log_dir(&tt)) { + return -1; + } + + char log_file_name[64] = ""; + strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S", &tt); + snprintf(file_name, file_name_size, "%s/%s%s.ulg", _log_dir, log_file_name, replay_suffix); + + } else { + if (create_log_dir(nullptr)) { + return -1; + } + + uint16_t file_number = 1; // start with file log001 + + /* look for the next file that does not exist */ + while (file_number <= MAX_NO_LOGFILE) { + /* format log file path: e.g. /fs/microsd/sess001/log001.ulg */ + snprintf(file_name, file_name_size, "%s/log%03u%s.ulg", _log_dir, file_number, replay_suffix); + + if (!file_exist(file_name)) { + break; + } + + file_number++; + } + + if (file_number > MAX_NO_LOGFILE) { + /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ + return -1; + } + } + + + return 0; +} + +void Logger::setReplayFile(const char *file_name) +{ + if (_replay_file_name) { + free(_replay_file_name); + } + + _replay_file_name = strdup(file_name); +} + +bool Logger::get_log_time(struct tm *tt, bool boot_time) +{ + int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + + if (vehicle_gps_position_sub < 0) { + return false; + } + + /* Get the latest GPS publication */ + vehicle_gps_position_s gps_pos; + time_t utc_time_sec; + bool use_clock_time = true; + + if (orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps_pos) == 0) { + utc_time_sec = gps_pos.time_utc_usec / 1e6; + orb_unsubscribe(vehicle_gps_position_sub); + + if (gps_pos.fix_type >= 2 && utc_time_sec >= GPS_EPOCH_SECS) { + use_clock_time = false; + } + } + + if (use_clock_time) { + /* take clock time if there's no fix (yet) */ + struct timespec ts; + px4_clock_gettime(CLOCK_REALTIME, &ts); + utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); + + if (utc_time_sec < GPS_EPOCH_SECS) { + return false; + } + } + + /* strip the time elapsed since boot */ + if (boot_time) { + utc_time_sec -= hrt_absolute_time() / 1e6; + } + + int32_t utc_offset = 0; + + if (_log_utc_offset != PARAM_INVALID) { + param_get(_log_utc_offset, &utc_offset); + } + + /* apply utc offset */ + utc_time_sec += utc_offset * 60; + + return gmtime_r(&utc_time_sec, tt) != nullptr; +} + +void Logger::start_log() +{ + if (_enabled) { + return; + } + + PX4_INFO("start log"); + + char file_name[LOG_DIR_LEN] = ""; + + if (get_log_file_name(file_name, sizeof(file_name))) { + PX4_ERR("logger: failed to get log file name"); + return; + } + + /* print logging path, important to find log file later */ + mavlink_log_info(&_mavlink_log_pub, "[logger] file: %s", file_name); + _next_topic_id = 0; + + _writer.start_log(file_name); + write_header(); + write_version(); + write_formats(); + write_parameters(); + write_all_add_logged_msg(); + _writer.notify(); + _enabled = true; + _start_time = hrt_absolute_time(); +} + +void Logger::stop_log() +{ + if (!_enabled) { + return; + } + + _enabled = false; + _writer.stop_log(); +} + +bool Logger::write_wait(void *ptr, size_t size) +{ + while (!_writer.write(ptr, size)) { + _writer.unlock(); + _writer.notify(); + usleep(_log_interval); + _writer.lock(); + } + + return true; +} + +void Logger::write_formats() +{ + _writer.lock(); + ulog_message_format_s msg; + const orb_metadata **topics = orb_get_topics(); + + //write all known formats + for (size_t i = 0; i < orb_topics_count(); i++) { + int format_len = snprintf(msg.format, sizeof(msg.format), "%s:%s", topics[i]->o_name, topics[i]->o_fields); + size_t msg_size = sizeof(msg) - sizeof(msg.format) + format_len; + msg.msg_size = msg_size - ULOG_MSG_HEADER_LEN; + + write_wait(&msg, msg_size); + } + + _writer.unlock(); +} + +void Logger::write_all_add_logged_msg() +{ + _writer.lock(); + + for (LoggerSubscription &sub : _subscriptions) { + for (int instance = 0; instance < ORB_MULTI_MAX_INSTANCES; ++instance) { + if (sub.fd[instance] >= 0) { + write_add_logged_msg(sub, instance); + } + } + } + + _writer.unlock(); +} + +void Logger::write_add_logged_msg(LoggerSubscription &subscription, int instance) +{ + ulog_message_add_logged_s msg; + + msg.multi_id = instance; + subscription.msg_ids[instance] = _next_topic_id; + msg.msg_id = _next_topic_id; + + int message_name_len = strlen(subscription.metadata->o_name); + + memcpy(msg.message_name, subscription.metadata->o_name, message_name_len); + + size_t msg_size = sizeof(msg) - sizeof(msg.message_name) + message_name_len; + msg.msg_size = msg_size - ULOG_MSG_HEADER_LEN; + + write_wait(&msg, msg_size); + + ++_next_topic_id; +} + +/* write info message */ +void Logger::write_info(const char *name, const char *value) +{ + _writer.lock(); + uint8_t buffer[sizeof(ulog_message_info_header_s)]; + ulog_message_info_header_s *msg = reinterpret_cast(buffer); + msg->msg_type = static_cast(ULogMessageType::INFO); + + /* construct format key (type and name) */ + size_t vlen = strlen(value); + msg->key_len = snprintf(msg->key, sizeof(msg->key), "char[%zu] %s", vlen, name); + size_t msg_size = sizeof(*msg) - sizeof(msg->key) + msg->key_len; + + /* copy string value directly to buffer */ + if (vlen < (sizeof(*msg) - msg_size)) { + memcpy(&buffer[msg_size], value, vlen); + msg_size += vlen; + + msg->msg_size = msg_size - ULOG_MSG_HEADER_LEN; + + write_wait(buffer, msg_size); + } + + _writer.unlock(); +} +void Logger::write_info(const char *name, int32_t value) +{ + _writer.lock(); + uint8_t buffer[sizeof(ulog_message_info_header_s)]; + ulog_message_info_header_s *msg = reinterpret_cast(buffer); + msg->msg_type = static_cast(ULogMessageType::INFO); + + /* construct format key (type and name) */ + msg->key_len = snprintf(msg->key, sizeof(msg->key), "int32_t %s", name); + size_t msg_size = sizeof(*msg) - sizeof(msg->key) + msg->key_len; + + /* copy string value directly to buffer */ + memcpy(&buffer[msg_size], &value, sizeof(int32_t)); + msg_size += sizeof(int32_t); + + msg->msg_size = msg_size - ULOG_MSG_HEADER_LEN; + + write_wait(buffer, msg_size); + + _writer.unlock(); +} + +void Logger::write_header() +{ + ulog_file_header_s header; + header.magic[0] = 'U'; + header.magic[1] = 'L'; + header.magic[2] = 'o'; + header.magic[3] = 'g'; + header.magic[4] = 0x01; + header.magic[5] = 0x12; + header.magic[6] = 0x35; + header.magic[7] = 0x00; //file version 0 + header.timestamp = hrt_absolute_time(); + _writer.lock(); + write_wait(&header, sizeof(header)); + _writer.unlock(); +} + +/* write version info messages */ +void Logger::write_version() +{ + write_info("ver_sw", PX4_GIT_VERSION_STR); + write_info("ver_hw", HW_ARCH); + write_info("sys_name", "PX4"); + int32_t utc_offset = 0; + + if (_log_utc_offset != PARAM_INVALID) { + param_get(_log_utc_offset, &utc_offset); + write_info("time_ref_utc", utc_offset * 60); + } + + if (_replay_file_name) { + write_info("replay", _replay_file_name); + } +} + +void Logger::write_parameters() +{ + _writer.lock(); + uint8_t buffer[sizeof(ulog_message_parameter_header_s) + sizeof(param_value_u)]; + ulog_message_parameter_header_s *msg = reinterpret_cast(buffer); + + msg->msg_type = static_cast(ULogMessageType::PARAMETER); + int param_idx = 0; + param_t param = 0; + + do { + // get next parameter which is invalid OR used + do { + param = param_for_index(param_idx); + ++param_idx; + } while (param != PARAM_INVALID && !param_used(param)); + + // save parameters which are valid AND used + if (param != PARAM_INVALID) { + /* get parameter type and size */ + const char *type_str; + param_type_t type = param_type(param); + size_t value_size = 0; + + switch (type) { + case PARAM_TYPE_INT32: + type_str = "int32_t"; + value_size = sizeof(int32_t); + break; + + case PARAM_TYPE_FLOAT: + type_str = "float"; + value_size = sizeof(float); + break; + + default: + continue; + } + + /* format parameter key (type and name) */ + msg->key_len = snprintf(msg->key, sizeof(msg->key), "%s %s", type_str, param_name(param)); + size_t msg_size = sizeof(*msg) - sizeof(msg->key) + msg->key_len; + + /* copy parameter value directly to buffer */ + param_get(param, &buffer[msg_size]); + msg_size += value_size; + + msg->msg_size = msg_size - ULOG_MSG_HEADER_LEN; + + write_wait(buffer, msg_size); + } + } while ((param != PARAM_INVALID) && (param_idx < (int) param_count())); + + _writer.unlock(); + _writer.notify(); +} + +void Logger::write_changed_parameters() +{ + _writer.lock(); + uint8_t buffer[sizeof(ulog_message_parameter_header_s) + sizeof(param_value_u)]; + ulog_message_parameter_header_s *msg = reinterpret_cast(buffer); + + msg->msg_type = static_cast(ULogMessageType::PARAMETER); + int param_idx = 0; + param_t param = 0; + + do { + // get next parameter which is invalid OR used + do { + param = param_for_index(param_idx); + ++param_idx; + } while (param != PARAM_INVALID && !param_used(param)); + + // log parameters which are valid AND used AND unsaved + if ((param != PARAM_INVALID) && param_value_unsaved(param)) { + + /* get parameter type and size */ + const char *type_str; + param_type_t type = param_type(param); + size_t value_size = 0; + + switch (type) { + case PARAM_TYPE_INT32: + type_str = "int32_t"; + value_size = sizeof(int32_t); + break; + + case PARAM_TYPE_FLOAT: + type_str = "float"; + value_size = sizeof(float); + break; + + default: + continue; + } + + /* format parameter key (type and name) */ + msg->key_len = snprintf(msg->key, sizeof(msg->key), "%s %s", type_str, param_name(param)); + size_t msg_size = sizeof(*msg) - sizeof(msg->key) + msg->key_len; + + /* copy parameter value directly to buffer */ + param_get(param, &buffer[msg_size]); + msg_size += value_size; + + /* msg_size is now 1 (msg_type) + 2 (msg_size) + 1 (key_len) + key_len + value_size */ + msg->msg_size = msg_size - ULOG_MSG_HEADER_LEN; + + write_wait(buffer, msg_size); + } + } while ((param != PARAM_INVALID) && (param_idx < (int) param_count())); + + _writer.unlock(); + _writer.notify(); +} + +int Logger::check_free_space() +{ + /* use statfs to determine the number of blocks left */ + struct statfs statfs_buf; + + if (statfs(LOG_ROOT, &statfs_buf) != 0) { + return PX4_ERROR; + } + + /* use a threshold of 50 MiB */ + if (statfs_buf.f_bavail < (px4_statfs_buf_f_bavail_t)(50 * 1024 * 1024 / statfs_buf.f_bsize)) { + mavlink_and_console_log_critical(&_mavlink_log_pub, + "[logger] Not logging; SD almost full: %u MiB", + (unsigned int)(statfs_buf.f_bavail / 1024U * statfs_buf.f_bsize / 1024U)); + return 1; + } + + return PX4_OK; +} + +} +} diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h new file mode 100644 index 0000000000..44ad42ebbf --- /dev/null +++ b/src/modules/logger/logger.h @@ -0,0 +1,246 @@ +/**************************************************************************** + * + * Copyright (c) 2016 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. + * + ****************************************************************************/ + +#pragma once + +#include "log_writer.h" +#include "array.h" +#include +#include +#include +#include +#include +#include + +extern "C" __EXPORT int logger_main(int argc, char *argv[]); + +#define TRY_SUBSCRIBE_INTERVAL 1000*1000 // interval in microseconds at which we try to subscribe to a topic +// if we haven't succeeded before + +#ifdef __PX4_NUTTX +#define LOG_DIR_LEN 64 +#else +#define LOG_DIR_LEN 256 +#endif + +namespace px4 +{ +namespace logger +{ + +struct LoggerSubscription { + int fd[ORB_MULTI_MAX_INSTANCES]; + uint16_t msg_ids[ORB_MULTI_MAX_INSTANCES]; + uint64_t time_tried_subscribe; // captures the time at which we checked last time if this instance existed + const orb_metadata *metadata = nullptr; + + LoggerSubscription() {} + + LoggerSubscription(int fd_, const orb_metadata *metadata_) : + metadata(metadata_) + { + fd[0] = fd_; + time_tried_subscribe = 0; + + for (int i = 1; i < ORB_MULTI_MAX_INSTANCES; i++) { + fd[i] = -1; + } + } +}; + +class Logger +{ +public: + Logger(size_t buffer_size, uint32_t log_interval, bool log_on_start, + bool log_until_shutdown, bool log_name_timestamp); + + ~Logger(); + + /** + * Tell the logger that we're in replay mode. This must be called + * before starting the logger. + * @param file_name file name of the used log replay file. Will be copied. + */ + void setReplayFile(const char *file_name); + + /** + * Add a topic to be logged. This must be called before start_log() + * (because it does not write an ADD_LOGGED_MSG message). + * @param name topic name + * @param interval limit rate if >0, otherwise log as fast as the topic is updated. + * @return 0 on success + */ + int add_topic(const char *name, unsigned interval); + + /** + * add a logged topic (called by add_topic() above) + */ + int add_topic(const orb_metadata *topic); + + static int start(char *const *argv); + + static void usage(const char *reason); + + void status(); + void print_statistics(); + + void set_arm_override(bool override) { _arm_override = override; } + +private: + static void run_trampoline(int argc, char *argv[]); + + void run(); + + /** + * Write an ADD_LOGGED_MSG to the log for a all current subscriptions and instances + */ + void write_all_add_logged_msg(); + + /** + * Write an ADD_LOGGED_MSG to the log for a given subscription and instance. + * _writer.lock() must be held when calling this. + */ + void write_add_logged_msg(LoggerSubscription &subscription, int instance); + + /** + * Create logging directory + * @param tt if not null, use it for the directory name + * @return 0 on success + */ + int create_log_dir(tm *tt); + + static bool file_exist(const char *filename); + + /** + * Get log file name with directory (create it if necessary) + */ + int get_log_file_name(char *file_name, size_t file_name_size); + + /** + * Check if there is enough free space left on the SD Card + * @return 0 on success, 1 if not enough space, <0 on error + */ + int check_free_space(); + + void start_log(); + + void stop_log(); + + /** + * write the file header with file magic and timestamp. + */ + void write_header(); + + void write_formats(); + + void write_version(); + + void write_info(const char *name, const char *value); + void write_info(const char *name, int32_t value); + + void write_parameters(); + + void write_changed_parameters(); + + bool copy_if_updated_multi(LoggerSubscription &sub, int multi_instance, void *buffer); + + /** + * Write data to the logger. Waits if buffer is full until all data is written. + * Must be called with _writer.lock() held. + */ + bool write_wait(void *ptr, size_t size); + + /** + * Write data to the logger and handle dropouts. + * Must be called with _writer.lock() held. + * @return true if data written, false otherwise (on overflow) + */ + bool write(void *ptr, size_t size); + + /** + * Get the time for log file name + * @param tt returned time + * @param boot_time use time when booted instead of current time + * @return true on success, false otherwise (eg. if no gps) + */ + bool get_log_time(struct tm *tt, bool boot_time = false); + + /** + * Parse a file containing a list of uORB topics to log, calling add_topic for each + * @param fname name of file + * @return number of topics added + */ + int add_topics_from_file(const char *fname); + + void add_default_topics(); + + static constexpr size_t MAX_TOPICS_NUM = 64; /**< Maximum number of logged topics */ + static constexpr unsigned MAX_NO_LOGFOLDER = 999; /**< Maximum number of log dirs */ + static constexpr unsigned MAX_NO_LOGFILE = 999; /**< Maximum number of log files */ +#ifdef __PX4_POSIX_EAGLE + static constexpr const char *LOG_ROOT = PX4_ROOTFSDIR"/log"; +#else + static constexpr const char *LOG_ROOT = PX4_ROOTFSDIR"/fs/microsd/log"; +#endif + + uint8_t *_msg_buffer = nullptr; + int _msg_buffer_len = 0; + bool _task_should_exit = true; + char _log_dir[LOG_DIR_LEN]; + bool _has_log_dir = false; + bool _enabled = false; + bool _was_armed = false; + bool _arm_override; + + + // statistics + hrt_abstime _start_time; ///< Time when logging started (not the logger thread) + hrt_abstime _dropout_start = 0; ///< start of current dropout (0 = no dropout) + float _max_dropout_duration = 0.f; ///< max duration of dropout [s] + size_t _write_dropouts = 0; ///< failed buffer writes due to buffer overflow + size_t _high_water = 0; ///< maximum used write buffer + + const bool _log_on_start; + const bool _log_until_shutdown; + const bool _log_name_timestamp; + Array _subscriptions; + LogWriter _writer; + uint32_t _log_interval; + param_t _log_utc_offset; + orb_advert_t _mavlink_log_pub = nullptr; + uint16_t _next_topic_id; ///< id of next subscribed topic + char *_replay_file_name = nullptr; +}; + +} //namespace logger +} //namespace px4 diff --git a/src/modules/logger/messages.h b/src/modules/logger/messages.h new file mode 100644 index 0000000000..bcbe015730 --- /dev/null +++ b/src/modules/logger/messages.h @@ -0,0 +1,132 @@ +/**************************************************************************** + * + * Copyright (c) 2016 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. + * + ****************************************************************************/ + +#pragma once + +enum class ULogMessageType : uint8_t { + FORMAT = 'F', + DATA = 'D', + INFO = 'I', + PARAMETER = 'P', + ADD_LOGGED_MSG = 'A', + REMOVE_LOGGED_MSG = 'R', + SYNC = 'S', + DROPOUT = 'O', + LOGGING = 'L', +}; + + +/* declare message data structs with byte alignment (no padding) */ +#pragma pack(push, 1) + +/** first bytes of the file */ +struct ulog_file_header_s { + uint8_t magic[8]; + uint64_t timestamp; +}; + +#define ULOG_MSG_HEADER_LEN 3 //accounts for msg_size and msg_type +struct ulog_message_header_s { + uint16_t msg_size; + uint8_t msg_type; +}; + +struct ulog_message_format_s { + uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN + uint8_t msg_type = static_cast(ULogMessageType::FORMAT); + + char format[2096]; +}; + +struct ulog_message_add_logged_s { + uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN + uint8_t msg_type = static_cast(ULogMessageType::ADD_LOGGED_MSG); + + uint8_t multi_id; + uint16_t msg_id; + char message_name[255]; +}; + +struct ulog_message_remove_logged_s { + uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN + uint8_t msg_type = static_cast(ULogMessageType::REMOVE_LOGGED_MSG); + + uint16_t msg_id; +}; + +struct ulog_message_sync_s { + uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN + uint8_t msg_type = static_cast(ULogMessageType::SYNC); + + uint8_t sync_magic[8]; +}; + +struct ulog_message_dropout_s { + uint16_t msg_size = sizeof(uint16_t); //size of message - ULOG_MSG_HEADER_LEN + uint8_t msg_type = static_cast(ULogMessageType::DROPOUT); + + uint16_t duration; //in ms +}; + +struct ulog_message_data_header_s { + uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN + uint8_t msg_type = static_cast(ULogMessageType::DATA); + + uint16_t msg_id; +}; + +struct ulog_message_info_header_s { + uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN + uint8_t msg_type = static_cast(ULogMessageType::INFO); + + uint8_t key_len; + char key[255]; +}; + +struct ulog_message_logging_s { + uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN + uint8_t msg_type = static_cast(ULogMessageType::LOGGING); + + uint8_t log_level; //same levels as in the linux kernel + uint64_t timestamp; + char message[128]; //defines the maximum length of a logged message string +}; + +struct ulog_message_parameter_header_s { + uint16_t msg_size; + uint8_t msg_type = static_cast(ULogMessageType::PARAMETER); + + uint8_t key_len; + char key[255]; +}; +#pragma pack(pop) diff --git a/src/modules/logger/params.c b/src/modules/logger/params.c new file mode 100644 index 0000000000..3ffc1eb6ca --- /dev/null +++ b/src/modules/logger/params.c @@ -0,0 +1,50 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2016 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. + * + ****************************************************************************/ + +/** + * UTC offset (unit: min) + * + * the difference in hours and minutes from Coordinated + * Universal Time (UTC) for a your place and date. + * + * for example, In case of South Korea(UTC+09:00), + * UTC offset is 540 min (9*60) + * + * refer to https://en.wikipedia.org/wiki/List_of_UTC_time_offsets + * + * @unit min + * @min -1000 + * @max 1000 + * @group SD Logging + */ +PARAM_DEFINE_INT32(SDLOG_UTC_OFFSET, 0); diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt index 01a8b5fa5c..023fddf476 100644 --- a/src/modules/mavlink/CMakeLists.txt +++ b/src/modules/mavlink/CMakeLists.txt @@ -36,11 +36,6 @@ px4_add_module( STACK_MAIN 1200 STACK_MAX 1500 COMPILE_FLAGS - -Wno-attributes - -Wno-packed - -DMAVLINK_COMM_NUM_BUFFERS=4 - -Wno-packed - -Wno-tautological-constant-out-of-range-compare -Os SRCS mavlink.c @@ -54,6 +49,7 @@ px4_add_module( mavlink_receiver.cpp mavlink_ftp.cpp mavlink_log_handler.cpp + mavlink_shell.cpp DEPENDS platforms__common ) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index bb6dc44789..ec4b71bf4d 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -50,21 +50,3 @@ mavlink_system_t mavlink_system = { 1, 1 }; // System ID, 1-255, Component/Subsystem ID, 1-255 - -/* - * Internal function to give access to the channel status for each channel - */ -extern mavlink_status_t *mavlink_get_channel_status(uint8_t channel) -{ - static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; - return &m_mavlink_status[channel]; -} - -/* - * Internal function to give access to the channel buffer for each channel - */ -extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel) -{ - static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS]; - return &m_mavlink_buffer[channel]; -} diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h index 0cd08769e1..799df3da36 100644 --- a/src/modules/mavlink/mavlink_bridge_header.h +++ b/src/modules/mavlink/mavlink_bridge_header.h @@ -44,20 +44,17 @@ __BEGIN_DECLS -/* - * We are NOT using convenience functions, - * but instead send messages with a custom function. - * So we do NOT do this: - * #define MAVLINK_USE_CONVENIENCE_FUNCTIONS - */ +#define MAVLINK_USE_CONVENIENCE_FUNCTIONS /* use efficient approach, see mavlink_helpers.h */ #define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes +#define MAVLINK_END_UART_SEND mavlink_end_uart_send + #define MAVLINK_GET_CHANNEL_BUFFER mavlink_get_channel_buffer #define MAVLINK_GET_CHANNEL_STATUS mavlink_get_channel_status -#include +#include #include @@ -81,10 +78,12 @@ extern mavlink_system_t mavlink_system; */ void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length); +void mavlink_end_uart_send(mavlink_channel_t chan, int length); + extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan); extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan); -#include +#include __END_DECLS diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index e8ea9c89b9..674fe66c83 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-2016 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 @@ -292,7 +292,7 @@ MavlinkFTP::_reply(mavlink_file_transfer_protocol_t* ftp_req) // Unit test hook is set, call that instead _utRcvMsgFunc(ftp_req, _worker_data); #else - _mavlink->send_message(MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL, ftp_req); + mavlink_msg_file_transfer_protocol_send_struct(_mavlink->get_channel(), ftp_req); #endif } diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 998c6b3cc6..017ac07d67 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -50,7 +50,7 @@ class MavlinkFtpTest; class MavlinkFTP : public MavlinkStream { public: - /// @brief Contructor is only public so unit test code can new objects. + /// @brief Constructor is only public so unit test code can new objects. MavlinkFTP(Mavlink *mavlink); ~MavlinkFTP(); diff --git a/src/modules/mavlink/mavlink_log_handler.cpp b/src/modules/mavlink/mavlink_log_handler.cpp index 08197f51fe..443c31183f 100644 --- a/src/modules/mavlink/mavlink_log_handler.cpp +++ b/src/modules/mavlink/mavlink_log_handler.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-2016 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 @@ -40,17 +40,18 @@ #include #define MOUNTPOINT PX4_ROOTFSDIR "/fs/microsd" - + +static const char *kSDRoot = MOUNTPOINT "/"; static const char *kLogRoot = MOUNTPOINT "/log"; static const char *kLogData = MOUNTPOINT "/logdata.txt"; static const char *kTmpData = MOUNTPOINT "/$log$.txt"; #ifdef __PX4_NUTTX - #define PX4LOG_REGULAR_FILE DTYPE_FILE - #define PX4LOG_DIRECTORY DTYPE_DIRECTORY +#define PX4LOG_REGULAR_FILE DTYPE_FILE +#define PX4LOG_DIRECTORY DTYPE_DIRECTORY #else - #define PX4LOG_REGULAR_FILE DT_REG - #define PX4LOG_DIRECTORY DT_DIR +#define PX4LOG_REGULAR_FILE DT_REG +#define PX4LOG_DIRECTORY DT_DIR #endif //#define MAVLINK_LOG_HANDLER_VERBOSE @@ -77,13 +78,13 @@ stat_file(const char* file, time_t* date = 0, uint32_t* size = 0) { MavlinkLogHandler * MavlinkLogHandler::new_instance(Mavlink *mavlink) { - return new MavlinkLogHandler(mavlink); + return new MavlinkLogHandler(mavlink); } //------------------------------------------------------------------- MavlinkLogHandler::MavlinkLogHandler(Mavlink *mavlink) - : MavlinkStream(mavlink) - , _pLogHandlerHelper(0) + : MavlinkStream(mavlink) + , _pLogHandlerHelper(0) { } @@ -112,14 +113,14 @@ MavlinkLogHandler::handle_message(const mavlink_message_t *msg) const char* MavlinkLogHandler::get_name(void) const { - return "MAVLINK_LOG_HANDLER"; + return "MAVLINK_LOG_HANDLER"; } //------------------------------------------------------------------- uint8_t MavlinkLogHandler::get_id(void) { - return MAVLINK_MSG_ID_LOG_ENTRY; + return MAVLINK_MSG_ID_LOG_ENTRY; } //------------------------------------------------------------------- @@ -142,15 +143,17 @@ MavlinkLogHandler::get_size(void) void MavlinkLogHandler::send(const hrt_abstime /*t*/) { - //-- Send log entry stream packets until buffer is full - while (_pLogHandlerHelper && _pLogHandlerHelper->current_status == LogListHelper::LOG_HANDLER_LISTING && _mavlink->get_free_tx_buf() > get_size()) { - _log_send_listing(); - }; - //-- An arbitrary count of max log data packets in one go - int count = 100; - while (_pLogHandlerHelper && _pLogHandlerHelper->current_status == LogListHelper::LOG_HANDLER_SENDING_DATA && _mavlink->get_free_tx_buf() > get_size() && --count) { - _log_send_data(); - }; + //-- An arbitrary count of max bytes in one go (one of the two below but never both) + #define MAX_BYTES_SEND 64 * 1024 + size_t count = 0; + //-- Log Entries + while (_pLogHandlerHelper && _pLogHandlerHelper->current_status == LogListHelper::LOG_HANDLER_LISTING && _mavlink->get_free_tx_buf() > get_size() && count < MAX_BYTES_SEND) { + count += _log_send_listing(); + } + //-- Log Data + while (_pLogHandlerHelper && _pLogHandlerHelper->current_status == LogListHelper::LOG_HANDLER_SENDING_DATA && _mavlink->get_free_tx_buf() > get_size() && count < MAX_BYTES_SEND) { + count += _log_send_data(); + } } //------------------------------------------------------------------- @@ -172,16 +175,16 @@ MavlinkLogHandler::_log_request_list(const mavlink_message_t *msg) //-- Prepare new request _pLogHandlerHelper = new LogListHelper; } - if (_pLogHandlerHelper->log_count) - { - //-- Define (and clamp) range + if (_pLogHandlerHelper->log_count) + { + //-- Define (and clamp) range _pLogHandlerHelper->next_entry = request.start < _pLogHandlerHelper->log_count ? request.start : _pLogHandlerHelper->log_count - 1; _pLogHandlerHelper->last_entry = request.end < _pLogHandlerHelper->log_count ? request.end : _pLogHandlerHelper->log_count - 1; - } + } PX4LOG_WARN("\nMavlinkLogHandler::_log_request_list: start: %u last: %u count: %u\n", - _pLogHandlerHelper->next_entry, - _pLogHandlerHelper->last_entry, - _pLogHandlerHelper->log_count); + _pLogHandlerHelper->next_entry, + _pLogHandlerHelper->last_entry, + _pLogHandlerHelper->log_count); //-- Enable streaming _pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_LISTING; } @@ -203,23 +206,27 @@ MavlinkLogHandler::_log_request_data(const mavlink_message_t *msg) return; } //-- If we were sending log entries, stop it - _pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE; - //-- Init send log dataset - _pLogHandlerHelper->current_log_filename[0] = 0; - _pLogHandlerHelper->current_log_index = request.id; - uint32_t time_utc = 0; - _pLogHandlerHelper->get_entry(_pLogHandlerHelper->current_log_index, _pLogHandlerHelper->current_log_size, time_utc, _pLogHandlerHelper->current_log_filename); - _pLogHandlerHelper->current_log_data_offset = request.ofs; - if (_pLogHandlerHelper->current_log_data_offset >= _pLogHandlerHelper->current_log_size) { + _pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE; + if (_pLogHandlerHelper->current_log_index != request.id) { + //-- Init send log dataset + _pLogHandlerHelper->current_log_filename[0] = 0; + _pLogHandlerHelper->current_log_index = request.id; + uint32_t time_utc = 0; + _pLogHandlerHelper->get_entry(_pLogHandlerHelper->current_log_index, _pLogHandlerHelper->current_log_size, time_utc, _pLogHandlerHelper->current_log_filename); + _pLogHandlerHelper->open_for_transmit(); + } + + _pLogHandlerHelper->current_log_data_offset = request.ofs; + if (_pLogHandlerHelper->current_log_data_offset >= _pLogHandlerHelper->current_log_size) { _pLogHandlerHelper->current_log_data_remaining = 0; - } else { + } else { _pLogHandlerHelper->current_log_data_remaining = _pLogHandlerHelper->current_log_size - request.ofs; - } - if (_pLogHandlerHelper->current_log_data_remaining > request.count) { + } + if (_pLogHandlerHelper->current_log_data_remaining > request.count) { _pLogHandlerHelper->current_log_data_remaining = request.count; - } + } //-- Enable streaming - _pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_SENDING_DATA; + _pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_SENDING_DATA; } //------------------------------------------------------------------- @@ -234,7 +241,29 @@ MavlinkLogHandler::_log_request_erase(const mavlink_message_t* /*msg*/) delete _pLogHandlerHelper; _pLogHandlerHelper = 0; } + //-- Delete all logs LogListHelper::delete_all(kLogRoot); + //-- Now delete all "msgs_*" from root + DIR* dp = opendir(kSDRoot); + if (dp) { + struct dirent entry, *result = nullptr; + while (readdir_r(dp, &entry, &result) == 0) { + // no more entries? + if (!result) { + break; + } + if (entry.d_type == PX4LOG_REGULAR_FILE) { + if(!memcmp(entry.d_name, "msgs_", 5)) { + char msg_path[128]; + snprintf(msg_path, sizeof(msg_path), "%s%s", kSDRoot, entry.d_name); + if(unlink(msg_path)) { + PX4LOG_WARN("MavlinkLogHandler::_log_request_erase Error deleting %s\n", msg_path); + } + } + } + } + closedir(dp); + } } //------------------------------------------------------------------- @@ -249,52 +278,55 @@ MavlinkLogHandler::_log_request_end(const mavlink_message_t* /*msg*/) } //------------------------------------------------------------------- -void +size_t MavlinkLogHandler::_log_send_listing() { mavlink_log_entry_t response; - _pLogHandlerHelper->get_entry(_pLogHandlerHelper->next_entry, response.size, response.time_utc); - response.id = _pLogHandlerHelper->next_entry; - response.num_logs = _pLogHandlerHelper->log_count; - response.last_log_num = _pLogHandlerHelper->last_entry; - _mavlink->send_message(MAVLINK_MSG_ID_LOG_ENTRY, &response); - //-- If we're done listing, flag it. - if (_pLogHandlerHelper->next_entry == _pLogHandlerHelper->last_entry) { + uint32_t size, date; + _pLogHandlerHelper->get_entry(_pLogHandlerHelper->next_entry, size, date); + response.size = size; + response.time_utc = date; + response.id = _pLogHandlerHelper->next_entry; + response.num_logs = _pLogHandlerHelper->log_count; + response.last_log_num = _pLogHandlerHelper->last_entry; + mavlink_msg_log_entry_send_struct(_mavlink->get_channel(), &response); + //-- If we're done listing, flag it. + if (_pLogHandlerHelper->next_entry == _pLogHandlerHelper->last_entry) { _pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE; - } else { + } else { _pLogHandlerHelper->next_entry++; - } + } PX4LOG_WARN("MavlinkLogHandler::_log_send_listing id: %u count: %u last: %u size: %u date: %u status: %d\n", - response.id, - response.num_logs, - response.last_log_num, - response.size, - response.time_utc, - _pLogHandlerHelper->current_status); + response.id, + response.num_logs, + response.last_log_num, + response.size, + response.time_utc, + _pLogHandlerHelper->current_status); + return sizeof(response); } //------------------------------------------------------------------- -void +size_t MavlinkLogHandler::_log_send_data() { mavlink_log_data_t response; + memset(&response, 0, sizeof(response)); uint32_t len = _pLogHandlerHelper->current_log_data_remaining; if (len > sizeof(response.data)) { len = sizeof(response.data); } size_t read_size = _pLogHandlerHelper->get_log_data(len, response.data); - if (read_size < sizeof(response.data)) { - memset(&response.data[read_size], 0, sizeof(response.data) - read_size); - } - response.ofs = _pLogHandlerHelper->current_log_data_offset; - response.id = _pLogHandlerHelper->current_log_index; - response.count = read_size; - _mavlink->send_message(MAVLINK_MSG_ID_LOG_DATA, &response); + response.ofs = _pLogHandlerHelper->current_log_data_offset; + response.id = _pLogHandlerHelper->current_log_index; + response.count = read_size; + mavlink_msg_log_data_send_struct(_mavlink->get_channel(), &response); _pLogHandlerHelper->current_log_data_offset += read_size; _pLogHandlerHelper->current_log_data_remaining -= read_size; if (read_size < sizeof(response.data) || _pLogHandlerHelper->current_log_data_remaining == 0) { _pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE; } + return sizeof(response); } //------------------------------------------------------------------- @@ -303,10 +335,11 @@ LogListHelper::LogListHelper() , last_entry(0) , log_count(0) , current_status(LOG_HANDLER_IDLE) - , current_log_index(0) + , current_log_index(UINT16_MAX) , current_log_size(0) , current_log_data_offset(0) , current_log_data_remaining(0) + , current_log_filep(0) { _init(); } @@ -351,24 +384,39 @@ LogListHelper::get_entry(int idx, uint32_t& size, uint32_t& date, char* filename return result; } +//------------------------------------------------------------------- +bool +LogListHelper::open_for_transmit() +{ + if (current_log_filep) { + ::fclose(current_log_filep); + current_log_filep = 0; + } + current_log_filep = ::fopen(current_log_filename, "rb"); + if (!current_log_filep) { + PX4LOG_WARN("MavlinkLogHandler::open_for_transmit Could not open %s\n", current_log_filename); + return false; + } + return true; +} + //------------------------------------------------------------------- size_t LogListHelper::get_log_data(uint8_t len, uint8_t* buffer) { - if(!current_log_filename[0]) + if(!current_log_filename[0]) return 0; - FILE* f = fopen(current_log_filename, "r"); - if (!f) { - PX4LOG_WARN("MavlinkLogHandler::get_log_data Could not open %s\n", current_log_filename); + if (!current_log_filep) { + PX4LOG_WARN("MavlinkLogHandler::get_log_data file not open %s\n", current_log_filename); return 0; } - if(fseek(f, current_log_data_offset, SEEK_SET)) { - fclose(f); + long int offset = current_log_data_offset - ftell(current_log_filep); + if(offset && fseek(current_log_filep, offset, SEEK_CUR)) { + fclose(current_log_filep); PX4LOG_WARN("MavlinkLogHandler::get_log_data Seek error in %s\n", current_log_filename); return 0; } - size_t result = fread(buffer, 1, len, f); - fclose(f); + size_t result = fread(buffer, 1, len, current_log_filep); return result; } @@ -377,7 +425,7 @@ void LogListHelper::_init() { /* - + When this helper is created, it scans the log directory and collects all log files found into one file for easy, subsequent access. @@ -386,14 +434,14 @@ LogListHelper::_init() current_log_filename[0] = 0; // Remove old log data file (if any) unlink(kLogData); - // Open log directory + // Open log directory DIR *dp = opendir(kLogRoot); if (dp == nullptr) { // No log directory. Nothing to do. return; } - // Create work file - FILE* f = ::fopen(kTmpData, "w"); + // Create work file + FILE* f = ::fopen(kTmpData, "w"); if (!f) { PX4LOG_WARN("MavlinkLogHandler::init Error creating %s\n", kTmpData); closedir(dp); @@ -411,7 +459,7 @@ LogListHelper::_init() time_t tt; char log_path[128]; snprintf(log_path, sizeof(log_path), "%s/%s", kLogRoot, entry.d_name); - if (_get_session_date(log_path, entry.d_name, tt)) { + if (_get_session_date(log_path, entry.d_name, tt)) { _scan_logs(f, log_path, tt); } } @@ -516,8 +564,8 @@ LogListHelper::_get_log_time_size(const char* path, const char* file, time_t& da void LogListHelper::delete_all(const char* dir) { - //-- Open log directory - DIR *dp = opendir(dir); + //-- Open log directory + DIR* dp = opendir(dir); if (dp == nullptr) { return; } diff --git a/src/modules/mavlink/mavlink_log_handler.h b/src/modules/mavlink/mavlink_log_handler.h index ebbbe116a8..231fc1d39f 100644 --- a/src/modules/mavlink/mavlink_log_handler.h +++ b/src/modules/mavlink/mavlink_log_handler.h @@ -40,7 +40,7 @@ #include #include #include -#include +#include #include "mavlink_stream.h" class Mavlink; @@ -54,11 +54,12 @@ public: public: static void delete_all(const char* dir); - + public: - bool get_entry (int idx, uint32_t& size, uint32_t& date, char* filename = 0); - size_t get_log_data (uint8_t len, uint8_t* buffer); + bool get_entry (int idx, uint32_t& size, uint32_t& date, char* filename = 0); + bool open_for_transmit(); + size_t get_log_data (uint8_t len, uint8_t* buffer); enum { LOG_HANDLER_IDLE, @@ -66,22 +67,23 @@ public: LOG_HANDLER_SENDING_DATA }; - int next_entry; - int last_entry; - int log_count; + int next_entry; + int last_entry; + int log_count; - int current_status; - uint16_t current_log_index; - uint32_t current_log_size; - uint32_t current_log_data_offset; - uint32_t current_log_data_remaining; - char current_log_filename[128]; + int current_status; + uint16_t current_log_index; + uint32_t current_log_size; + uint32_t current_log_data_offset; + uint32_t current_log_data_remaining; + FILE* current_log_filep; + char current_log_filename[128]; private: - void _init (); - bool _get_session_date (const char* path, const char* dir, time_t& date); - void _scan_logs (FILE* f, const char* dir, time_t& date); - bool _get_log_time_size (const char* path, const char* file, time_t& date, uint32_t& size); + void _init (); + bool _get_session_date (const char* path, const char* dir, time_t& date); + void _scan_logs (FILE* f, const char* dir, time_t& date); + bool _get_log_time_size (const char* path, const char* file, time_t& date, uint32_t& size); }; // MAVLink LOG_* Message Handler @@ -93,24 +95,25 @@ public: static MavlinkLogHandler *new_instance(Mavlink *mavlink); // Handle possible LOG message - void handle_message (const mavlink_message_t *msg); + void handle_message (const mavlink_message_t *msg); // Overrides from MavlinkStream - const char* get_name (void) const; - uint8_t get_id (void); - unsigned get_size (void); - void send (const hrt_abstime t); + const char* get_name (void) const; + uint8_t get_id (void); + unsigned get_size (void); + void send (const hrt_abstime t); private: - void _log_message (const mavlink_message_t *msg); - void _log_request_list (const mavlink_message_t *msg); - void _log_request_data (const mavlink_message_t *msg); - void _log_request_erase (const mavlink_message_t *msg); - void _log_request_end (const mavlink_message_t *msg); - void _log_send_listing (); - void _log_send_data (); + void _log_message (const mavlink_message_t *msg); + void _log_request_list (const mavlink_message_t *msg); + void _log_request_data (const mavlink_message_t *msg); + void _log_request_erase (const mavlink_message_t *msg); + void _log_request_end (const mavlink_message_t *msg); + + size_t _log_send_listing(); + size_t _log_send_data (); private: - LogListHelper *_pLogHandlerHelper; + LogListHelper *_pLogHandlerHelper; }; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c6e9772f42..3818ddf818 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -106,9 +106,6 @@ static const int ERROR = -1; static Mavlink *_mavlink_instances = nullptr; -static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; -static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; - /** * mavlink app start / stop handling function * @@ -118,6 +115,48 @@ extern "C" __EXPORT int mavlink_main(int argc, char *argv[]); extern mavlink_system_t mavlink_system; +void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length) +{ + Mavlink* m = Mavlink::get_instance((unsigned)chan); + if (m != nullptr) { + m->send_bytes(ch, length); + } +} + +void mavlink_end_uart_send(mavlink_channel_t chan, int length) +{ + Mavlink* m = Mavlink::get_instance((unsigned)chan); + if (m != nullptr) { + (void)m->send_packet(); + } +} + +/* + * Internal function to give access to the channel status for each channel + */ +mavlink_status_t *mavlink_get_channel_status(uint8_t channel) +{ + Mavlink* m = Mavlink::get_instance((unsigned)channel); + if (m != nullptr) { + return m->get_status(); + } else { + return nullptr; + } +} + +/* + * Internal function to give access to the channel buffer for each channel + */ +mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel) +{ + Mavlink* m = Mavlink::get_instance((unsigned)channel); + if (m != nullptr) { + return m->get_buffer(); + } else { + return nullptr; + } +} + static void usage(void); bool Mavlink::_boot_complete = false; @@ -130,6 +169,8 @@ Mavlink::Mavlink() : _instance_id(0), _mavlink_log_pub(nullptr), _task_running(false), + _mavlink_buffer{}, + _mavlink_status{}, _hil_enabled(false), _generate_rc(false), _use_hil_gps(false), @@ -144,6 +185,7 @@ Mavlink::Mavlink() : _parameters_manager(nullptr), _mavlink_ftp(nullptr), _mavlink_log_handler(nullptr), + _mavlink_shell(nullptr), _mode(MAVLINK_MODE_NORMAL), _channel(MAVLINK_COMM_0), _radio_id(0), @@ -164,10 +206,11 @@ Mavlink::Mavlink() : _subscribe_to_stream(nullptr), _subscribe_to_stream_rate(0.0f), _udp_initialised(false), - _flow_control_enabled(true), + _flow_control_enabled(false), _last_write_success_time(0), _last_write_try_time(0), _mavlink_start_time(0), + _protocol_version(0), _bytes_tx(0), _bytes_txerr(0), _bytes_rx(0), @@ -181,6 +224,10 @@ Mavlink::Mavlink() : _bcast_addr{}, _src_addr_initialized(false), _broadcast_address_found(false), + _broadcast_address_not_found_warned(false), + _broadcast_failed_warned(false), + _network_buf{}, + _network_buf_len(0), #endif _socket_fd(-1), _protocol(SERIAL), @@ -191,12 +238,15 @@ Mavlink::Mavlink() : _message_buffer_mutex {}, _send_mutex {}, _param_initialized(false), - _param_system_id(0), - _param_component_id(0), - _param_radio_id(0), - _param_system_type(MAV_TYPE_FIXED_WING), - _param_use_hil_gps(0), - _param_forward_externalsp(0), + _logging_enabled(false), + _broadcast_mode(Mavlink::BROADCAST_MODE_OFF), + _param_system_id(PARAM_INVALID), + _param_component_id(PARAM_INVALID), + _param_radio_id(PARAM_INVALID), + _param_system_type(PARAM_INVALID), + _param_use_hil_gps(PARAM_INVALID), + _param_forward_externalsp(PARAM_INVALID), + _param_broadcast(PARAM_INVALID), _system_type(0), /* performance counters */ @@ -274,9 +324,17 @@ Mavlink::~Mavlink() } } while (_task_running); } +} - if (_mavlink_instances) { - LL_DELETE(_mavlink_instances, this); +void +Mavlink::set_proto_version(unsigned version) +{ + _protocol_version = version; + + if (version == 1 || ((version == 0) && !_received_messages)) { + get_status()->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; + } else if (version == 2) { + get_status()->flags &= ~(MAVLINK_STATUS_FLAG_OUT_MAVLINK1); } } @@ -309,13 +367,10 @@ Mavlink * Mavlink::get_instance(unsigned instance) { Mavlink *inst; - unsigned inst_index = 0; LL_FOREACH(::_mavlink_instances, inst) { - if (instance == inst_index) { + if (instance == inst->get_instance_id()) { return inst; } - - inst_index++; } return nullptr; @@ -378,6 +433,14 @@ Mavlink::destroy_all_instances() return ERROR; } } + + } + + //we know all threads have exited, so it's safe to manipulate the linked list and delete objects. + while (_mavlink_instances) { + inst_to_del = _mavlink_instances; + LL_DELETE(_mavlink_instances, inst_to_del); + delete (inst_to_del); } printf("\n"); @@ -476,10 +539,12 @@ void Mavlink::mavlink_update_system(void) if (!_param_initialized) { _param_system_id = param_find("MAV_SYS_ID"); _param_component_id = param_find("MAV_COMP_ID"); + _param_proto_ver = param_find("MAV_PROTO_VER"); _param_radio_id = param_find("MAV_RADIO_ID"); _param_system_type = param_find("MAV_TYPE"); _param_use_hil_gps = param_find("MAV_USEHILGPS"); _param_forward_externalsp = param_find("MAV_FWDEXTSP"); + _param_broadcast = param_find("MAV_BROADCAST"); /* test param - needs to be referenced, but is unused */ (void)param_find("MAV_TEST_PAR"); @@ -492,6 +557,10 @@ void Mavlink::mavlink_update_system(void) int32_t component_id; param_get(_param_component_id, &component_id); + int32_t proto; + param_get(_param_proto_ver, &proto); + set_proto_version(proto); + param_get(_param_radio_id, &_radio_id); /* only allow system ID and component ID updates @@ -534,6 +603,8 @@ void Mavlink::mavlink_update_system(void) int32_t forward_externalsp; param_get(_param_forward_externalsp, &forward_externalsp); + param_get(_param_broadcast, &_broadcast_mode); + _forward_externalsp = (bool)forward_externalsp; } @@ -794,6 +865,9 @@ Mavlink::get_free_tx_buf() // No FIONWRITE on Linux #if !defined(__PX4_LINUX) && !defined(__PX4_DARWIN) (void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free); +#else + //Linux cp210x does not support TIOCOUTQ + buf_free = 256; #endif if (get_flow_control_enabled() && buf_free < FLOW_CONTROL_DISABLE_THRESHOLD) { @@ -813,8 +887,64 @@ Mavlink::get_free_tx_buf() return buf_free; } +int +Mavlink::send_packet() +{ + int ret = -1; + +#ifdef __PX4_POSIX + + /* Only send packets if there is something in the buffer. */ + if (_network_buf_len == 0) { + return 0; + } + + if (get_protocol() == UDP) { + + + ret = sendto(_socket_fd, _network_buf, _network_buf_len, 0, + (struct sockaddr *)&_src_addr, sizeof(_src_addr)); + + struct telemetry_status_s &tstatus = get_rx_status(); + + /* resend message via broadcast if no valid connection exists */ + if ((_mode != MAVLINK_MODE_ONBOARD) && broadcast_enabled() && + (!get_client_source_initialized() + || (hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000))) { + + if (!_broadcast_address_found) { + find_broadcast_address(); + } + + if (_broadcast_address_found && _network_buf_len > 0) { + + int bret = sendto(_socket_fd, _network_buf, _network_buf_len, 0, + (struct sockaddr *)&_bcast_addr, sizeof(_bcast_addr)); + + if (bret <= 0) { + if (!_broadcast_failed_warned) { + PX4_ERR("sending broadcast failed, errno: %d: %s", errno, strerror(errno)); + _broadcast_failed_warned = true; + } + } else { + _broadcast_failed_warned = false; + } + } + } + + } else if (get_protocol() == TCP) { + /* not implemented, but possible to do so */ + PX4_ERR("TCP transport pending implementation"); + } + + _network_buf_len = 0; +#endif + + return ret; +} + void -Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID) +Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len) { /* If the wait until transmit flag is on, only transmit after we've received messages. Otherwise, transmit all the time. */ @@ -824,9 +954,6 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID pthread_mutex_lock(&_send_mutex); - uint8_t payload_len = mavlink_message_lengths[msgid]; - unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES; - _last_write_try_time = hrt_absolute_time(); if (_mavlink_start_time == 0) { @@ -845,29 +972,6 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID } } - uint8_t buf[MAVLINK_MAX_PACKET_LEN]; - - /* header */ - buf[0] = MAVLINK_STX; - buf[1] = payload_len; - /* use mavlink's internal counter for the TX seq */ - buf[2] = mavlink_get_channel_status(_channel)->current_tx_seq++; - buf[3] = mavlink_system.sysid; - buf[4] = (component_ID == 0) ? mavlink_system.compid : component_ID; - buf[5] = msgid; - - /* payload */ - memcpy(&buf[MAVLINK_NUM_HEADER_BYTES], msg, payload_len); - - /* checksum */ - uint16_t checksum; - crc_init(&checksum); - crc_accumulate_buffer(&checksum, (const char *) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len); - crc_accumulate(mavlink_message_crcs[msgid], &checksum); - - buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF); - buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8); - size_t ret = -1; /* send message to UART */ @@ -876,34 +980,14 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID } #ifdef __PX4_POSIX - if (get_protocol() == UDP) { - ret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr)); - struct telemetry_status_s &tstatus = get_rx_status(); + else { + if (_network_buf_len + packet_len < sizeof(_network_buf) / sizeof(_network_buf[0])) { + memcpy(&_network_buf[_network_buf_len], buf, packet_len); + _network_buf_len += packet_len; - /* resend heartbeat via broadcast */ - if ((_mode != MAVLINK_MODE_ONBOARD) && - (!get_client_source_initialized() - || (hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000)) - && (msgid == MAVLINK_MSG_ID_HEARTBEAT)) { - - if (!_broadcast_address_found) { - find_broadcast_address(); - } - - if (_broadcast_address_found) { - - int bret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_bcast_addr, sizeof(_bcast_addr)); - - if (bret <= 0) { - PX4_WARN("sending broadcast failed, errno: %d: %s", errno, strerror(errno)); - } - } + ret = packet_len; } - - } else if (get_protocol() == TCP) { - /* not implemented, but possible to do so */ - warnx("TCP transport pending implementation"); } #endif @@ -919,58 +1003,6 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID pthread_mutex_unlock(&_send_mutex); } -void -Mavlink::resend_message(mavlink_message_t *msg) -{ - /* If the wait until transmit flag is on, only transmit after we've received messages. - Otherwise, transmit all the time. */ - if (!should_transmit()) { - return; - } - - pthread_mutex_lock(&_send_mutex); - - unsigned buf_free = get_free_tx_buf(); - - _last_write_try_time = hrt_absolute_time(); - - unsigned packet_len = msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; - - /* check if there is space in the buffer, let it overflow else */ - if (buf_free < packet_len) { - /* no enough space in buffer to send */ - count_txerr(); - count_txerrbytes(packet_len); - pthread_mutex_unlock(&_send_mutex); - return; - } - - uint8_t buf[MAVLINK_MAX_PACKET_LEN]; - - /* header and payload */ - memcpy(&buf[0], &msg->magic, MAVLINK_NUM_HEADER_BYTES + msg->len); - - /* checksum */ - buf[MAVLINK_NUM_HEADER_BYTES + msg->len] = (uint8_t)(msg->checksum & 0xFF); - buf[MAVLINK_NUM_HEADER_BYTES + msg->len + 1] = (uint8_t)(msg->checksum >> 8); - - if (_uart_fd >= 0) { - /* send message to UART */ - ssize_t ret = ::write(_uart_fd, buf, packet_len); - - if (ret != (int) packet_len) { - count_txerr(); - count_txerrbytes(packet_len); - - } else { - _last_write_success_time = _last_write_try_time; - count_txbytes(packet_len); - } - } - - pthread_mutex_unlock(&_send_mutex); -} - void Mavlink::find_broadcast_address() { @@ -1093,9 +1125,13 @@ Mavlink::find_broadcast_address() if (setsockopt(_socket_fd, SOL_SOCKET, SO_BROADCAST, &broadcast_opt, sizeof(broadcast_opt)) < 0) { PX4_WARN("setting broadcast permission failed"); } + _broadcast_address_not_found_warned = false; } else { - PX4_WARN("no broadcasting address found"); + if (!_broadcast_address_not_found_warned) { + PX4_WARN("no broadcasting address found"); + _broadcast_address_not_found_warned = true; + } } delete[] ifconf.ifc_req; @@ -1108,7 +1144,7 @@ Mavlink::init_udp() { #if defined (__PX4_LINUX) || defined (__PX4_DARWIN) - PX4_INFO("Setting up UDP w/port %d", _network_port); + PX4_DEBUG("Setting up UDP with port %d", _network_port); _myaddr.sin_family = AF_INET; _myaddr.sin_addr.s_addr = htonl(INADDR_ANY); @@ -1193,8 +1229,8 @@ void Mavlink::send_autopilot_capabilites() msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET; msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED; msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_ACTUATOR_TARGET; - msg.flight_sw_version = version_tag_to_number(px4_git_version); - msg.middleware_sw_version = version_tag_to_number(px4_git_version); + msg.flight_sw_version = version_tag_to_number(px4_git_tag); + msg.middleware_sw_version = version_tag_to_number(px4_git_tag); msg.os_sw_version = version_tag_to_number(os_git_tag); msg.board_version = px4_board_version; memcpy(&msg.flight_custom_version, &px4_git_version_binary, sizeof(msg.flight_custom_version)); @@ -1214,7 +1250,7 @@ void Mavlink::send_autopilot_capabilites() mcu_unique_id(uid); msg.uid = (((uint64_t)uid[1]) << 32) | uid[2]; - this->send_message(MAVLINK_MSG_ID_AUTOPILOT_VERSION, &msg); + mavlink_msg_autopilot_version_send_struct(get_channel(), &msg); } } @@ -1268,7 +1304,7 @@ Mavlink::configure_stream(const char *stream_name, const float rate) } } - if (interval == 0) { + if (interval <= 0) { /* stream was not active and is requested to be disabled, do nothing */ return OK; } @@ -1482,6 +1518,35 @@ Mavlink::get_rate_mult() return _rate_mult; } +MavlinkShell* +Mavlink::get_shell() +{ + if (!_mavlink_shell) { + _mavlink_shell = new MavlinkShell(); + if (!_mavlink_shell) { + PX4_ERR("Failed to allocate a shell"); + } else { + int ret = _mavlink_shell->start(); + if (ret != 0) { + PX4_ERR("Failed to start shell (%i)", ret); + delete _mavlink_shell; + _mavlink_shell = nullptr; + } + } + } + + return _mavlink_shell; +} + +void +Mavlink::close_shell() +{ + if (_mavlink_shell) { + delete _mavlink_shell; + _mavlink_shell = nullptr; + } +} + void Mavlink::update_rate_mult() { @@ -1492,15 +1557,20 @@ Mavlink::update_rate_mult() MavlinkStream *stream; LL_FOREACH(_streams, stream) { if (stream->const_rate()) { - const_rate += stream->get_size() * 1000000.0f / stream->get_interval(); + const_rate += (stream->get_interval() > 0) ? stream->get_size_avg() * 1000000.0f / stream->get_interval() : 0; } else { - rate += stream->get_size() * 1000000.0f / stream->get_interval(); + rate += (stream->get_interval() > 0) ? stream->get_size_avg() * 1000000.0f / stream->get_interval() : 0; } } - /* don't scale up rates, only scale down if needed */ - float bandwidth_mult = fminf(1.0f, ((float)_datarate - const_rate) / rate); + /* scale up and down as the link permits */ + float bandwidth_mult = (float)(_datarate - const_rate) / rate; + + /* if we do not have flow control, limit to the set data rate */ + if (!get_flow_control_enabled()) { + bandwidth_mult = fminf(1.0f, bandwidth_mult); + } /* check if we have radio feedback */ struct telemetry_status_s &tstatus = get_rx_status(); @@ -1623,7 +1693,6 @@ Mavlink::task_main(int argc, char *argv[]) case 'o': temp_int_arg = strtoul(myoptarg, &eptr, 10); if ( *eptr == '\0' ) { - warnx("set remote port %d", temp_int_arg); _remote_port = temp_int_arg; set_protocol(UDP); } else { @@ -1721,7 +1790,8 @@ Mavlink::task_main(int argc, char *argv[]) return ERROR; } - warnx("mode: %u, data rate: %d B/s on %s @ %dB", _mode, _datarate, _device_name, _baudrate); + PX4_INFO("mode: %s, data rate: %d B/s on %s @ %dB", + mavlink_mode_str(_mode), _datarate, _device_name, _baudrate); /* flush stdout in case MAVLink is about to take it over */ fflush(stdout); @@ -1743,7 +1813,8 @@ Mavlink::task_main(int argc, char *argv[]) return ERROR; } - warnx("mode: %u, data rate: %d B/s on udp port %hu", _mode, _datarate, _network_port); + PX4_INFO("mode: %s, data rate: %d B/s on udp port %hu remote port %hu", + mavlink_mode_str(_mode), _datarate, _network_port, _remote_port); } /* initialize send mutex */ @@ -1777,7 +1848,6 @@ Mavlink::task_main(int argc, char *argv[]) MavlinkOrbSubscription *ack_sub = add_orb_subscription(ORB_ID(vehicle_command_ack)); uint64_t ack_time = 0; MavlinkOrbSubscription *mavlink_log_sub = add_orb_subscription(ORB_ID(mavlink_log)); - uint64_t mavlink_log_time = 0; struct vehicle_status_s status; status_sub->update(&status_time, &status); @@ -1821,72 +1891,76 @@ Mavlink::task_main(int argc, char *argv[]) switch (_mode) { case MAVLINK_MODE_NORMAL: configure_stream("SYS_STATUS", 1.0f); - configure_stream("HOME_POSITION", 0.5f); - configure_stream("HIGHRES_IMU", 2.0f); - configure_stream("ATTITUDE", 20.0f); - configure_stream("VFR_HUD", 8.0f); - configure_stream("GPS_RAW_INT", 1.0f); - configure_stream("GLOBAL_POSITION_INT", 3.0f); - configure_stream("LOCAL_POSITION_NED", 3.0f); - configure_stream("RC_CHANNELS", 1.0f); - configure_stream("SERVO_OUTPUT_RAW_0", 1.0f); - configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f); - configure_stream("ATTITUDE_TARGET", 8.0f); - configure_stream("DISTANCE_SENSOR", 0.5f); - configure_stream("OPTICAL_FLOW_RAD", 5.0f); configure_stream("EXTENDED_SYS_STATE", 1.0f); + configure_stream("HIGHRES_IMU", 1.5f); + configure_stream("ATTITUDE", 20.0f); + configure_stream("RC_CHANNELS", 5.0f); + configure_stream("SERVO_OUTPUT_RAW_0", 1.0f); configure_stream("ALTITUDE", 1.0f); - configure_stream("VISION_POSITION_NED", 10.0f); - configure_stream("NAMED_VALUE_FLOAT", 1.0f); - configure_stream("ESTIMATOR_STATUS", 0.5f); + configure_stream("GPS_RAW_INT", 1.0f); configure_stream("ADSB_VEHICLE", 2.0f); + configure_stream("DISTANCE_SENSOR", 0.5f); + configure_stream("OPTICAL_FLOW_RAD", 1.0f); + configure_stream("VISION_POSITION_NED", 1.0f); + configure_stream("ESTIMATOR_STATUS", 0.5f); + configure_stream("NAV_CONTROLLER_OUTPUT", 1.5f); + configure_stream("GLOBAL_POSITION_INT", 5.0f); + configure_stream("LOCAL_POSITION_NED", 1.0f); + configure_stream("POSITION_TARGET_GLOBAL_INT", 1.5f); + configure_stream("ATTITUDE_TARGET", 2.0f); + configure_stream("HOME_POSITION", 0.5f); + configure_stream("NAMED_VALUE_FLOAT", 1.0f); + configure_stream("VFR_HUD", 4.0f); + configure_stream("WIND_COV", 1.0f); break; case MAVLINK_MODE_ONBOARD: configure_stream("SYS_STATUS", 1.0f); - configure_stream("ATTITUDE", 250.0f); + configure_stream("EXTENDED_SYS_STATE", 2.0f); configure_stream("HIGHRES_IMU", 50.0f); - configure_stream("GPS_RAW_INT", 5.0f); - configure_stream("GLOBAL_POSITION_INT", 50.0f); - configure_stream("LOCAL_POSITION_NED", 30.0f); - configure_stream("NAMED_VALUE_FLOAT", 10.0f); - configure_stream("CAMERA_CAPTURE", 2.0f); - configure_stream("HOME_POSITION", 0.5f); - configure_stream("ATTITUDE_TARGET", 10.0f); - configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f); - configure_stream("POSITION_TARGET_LOCAL_NED", 10.0f); - configure_stream("DISTANCE_SENSOR", 10.0f); - configure_stream("OPTICAL_FLOW_RAD", 10.0f); + configure_stream("ATTITUDE", 250.0f); configure_stream("RC_CHANNELS", 20.0f); configure_stream("SERVO_OUTPUT_RAW_0", 10.0f); + configure_stream("ALTITUDE", 10.0f); + configure_stream("GPS_RAW_INT", 5.0f); + configure_stream("ADSB_VEHICLE", 10.0f); + configure_stream("DISTANCE_SENSOR", 10.0f); + configure_stream("OPTICAL_FLOW_RAD", 10.0f); + configure_stream("VISION_POSITION_NED", 10.0f); + configure_stream("ESTIMATOR_STATUS", 1.0f); + configure_stream("NAV_CONTROLLER_OUTPUT", 10.0f); + configure_stream("GLOBAL_POSITION_INT", 50.0f); + configure_stream("LOCAL_POSITION_NED", 30.0f); + configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f); + configure_stream("ATTITUDE_TARGET", 10.0f); + configure_stream("HOME_POSITION", 0.5f); + configure_stream("NAMED_VALUE_FLOAT", 10.0f); configure_stream("VFR_HUD", 10.0f); + configure_stream("WIND_COV", 10.0f); + configure_stream("POSITION_TARGET_LOCAL_NED", 10.0f); configure_stream("SYSTEM_TIME", 1.0f); configure_stream("TIMESYNC", 10.0f); - configure_stream("ACTUATOR_CONTROL_TARGET0", 10.0f); + configure_stream("CAMERA_CAPTURE", 2.0f); //camera trigger is rate limited at the source, do not limit here configure_stream("CAMERA_TRIGGER", 500.0f); - configure_stream("EXTENDED_SYS_STATE", 2.0f); - configure_stream("ALTITUDE", 10.0f); - configure_stream("VISION_POSITION_NED", 10.0f); - configure_stream("NAMED_VALUE_FLOAT", 10.0f); - configure_stream("ESTIMATOR_STATUS", 1.0f); - configure_stream("ADSB_VEHICLE", 10.0f); + configure_stream("ACTUATOR_CONTROL_TARGET0", 10.0f); break; case MAVLINK_MODE_OSD: configure_stream("SYS_STATUS", 5.0f); + configure_stream("EXTENDED_SYS_STATE", 1.0f); configure_stream("ATTITUDE", 25.0f); - configure_stream("VFR_HUD", 25.0f); - configure_stream("GPS_RAW_INT", 1.0f); - configure_stream("GLOBAL_POSITION_INT", 10.0f); - configure_stream("HOME_POSITION", 0.5f); - configure_stream("ATTITUDE_TARGET", 10.0f); - configure_stream("SYSTEM_TIME", 1.0f); configure_stream("RC_CHANNELS", 5.0f); configure_stream("SERVO_OUTPUT_RAW_0", 1.0f); - configure_stream("EXTENDED_SYS_STATE", 1.0f); configure_stream("ALTITUDE", 1.0f); + configure_stream("GPS_RAW_INT", 1.0f); configure_stream("ESTIMATOR_STATUS", 1.0f); + configure_stream("GLOBAL_POSITION_INT", 10.0f); + configure_stream("ATTITUDE_TARGET", 10.0f); + configure_stream("HOME_POSITION", 0.5f); + configure_stream("VFR_HUD", 25.0f); + configure_stream("WIND_COV", 2.0f); + configure_stream("SYSTEM_TIME", 1.0f); break; case MAVLINK_MODE_MAGIC: @@ -1896,32 +1970,32 @@ Mavlink::task_main(int argc, char *argv[]) case MAVLINK_MODE_CONFIG: // Enable a number of interesting streams we want via USB configure_stream("SYS_STATUS", 1.0f); - configure_stream("HOME_POSITION", 0.5f); - configure_stream("GLOBAL_POSITION_INT", 10.0f); - configure_stream("ATTITUDE_TARGET", 8.0f); - //configure_stream("PARAM_VALUE", 300.0f); - configure_stream("MISSION_ITEM", 50.0f); - configure_stream("NAMED_VALUE_FLOAT", 50.0f); - configure_stream("OPTICAL_FLOW_RAD", 10.0f); - configure_stream("DISTANCE_SENSOR", 10.0f); - configure_stream("VFR_HUD", 20.0f); + configure_stream("EXTENDED_SYS_STATE", 2.0f); + configure_stream("HIGHRES_IMU", 50.0f); configure_stream("ATTITUDE", 100.0f); - configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f); configure_stream("RC_CHANNELS", 10.0f); configure_stream("SERVO_OUTPUT_RAW_0", 20.0f); configure_stream("SERVO_OUTPUT_RAW_1", 20.0f); - configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f); - configure_stream("LOCAL_POSITION_NED", 30.0f); - configure_stream("MANUAL_CONTROL", 5.0f); - configure_stream("HIGHRES_IMU", 50.0f); - configure_stream("GPS_RAW_INT", 20.0f); - configure_stream("CAMERA_TRIGGER", 500.0f); - configure_stream("EXTENDED_SYS_STATE", 2.0f); configure_stream("ALTITUDE", 10.0f); - configure_stream("VISION_POSITION_NED", 10.0f); - configure_stream("NAMED_VALUE_FLOAT", 50.0f); - configure_stream("ESTIMATOR_STATUS", 5.0f); + configure_stream("GPS_RAW_INT", 10.0f); configure_stream("ADSB_VEHICLE", 20.0f); + configure_stream("DISTANCE_SENSOR", 10.0f); + configure_stream("OPTICAL_FLOW_RAD", 10.0f); + configure_stream("VISION_POSITION_NED", 10.0f); + configure_stream("ESTIMATOR_STATUS", 5.0f); + configure_stream("NAV_CONTROLLER_OUTPUT", 10.0f); + configure_stream("GLOBAL_POSITION_INT", 10.0f); + configure_stream("LOCAL_POSITION_NED", 30.0f); + configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f); + configure_stream("ATTITUDE_TARGET", 8.0f); + configure_stream("HOME_POSITION", 0.5f); + configure_stream("NAMED_VALUE_FLOAT", 50.0f); + configure_stream("VFR_HUD", 20.0f); + configure_stream("WIND_COV", 10.0f); + configure_stream("CAMERA_TRIGGER", 500.0f); + configure_stream("MISSION_ITEM", 50.0f); + configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f); + configure_stream("MANUAL_CONTROL", 5.0f); default: break; } @@ -1944,7 +2018,6 @@ Mavlink::task_main(int argc, char *argv[]) /* init socket if necessary */ if (get_protocol() == UDP) { - find_broadcast_address(); init_udp(); } @@ -2006,7 +2079,7 @@ Mavlink::task_main(int argc, char *argv[]) // allocated. //fclose(fs); } else { - warnx("open fd %d failed", _uart_fd); + PX4_WARN("open fd %d failed", _uart_fd); } /* reset param and save */ @@ -2027,39 +2100,50 @@ Mavlink::task_main(int argc, char *argv[]) msg.result = command_ack.result; msg.command = command_ack.command; - send_message(MAVLINK_MSG_ID_COMMAND_ACK, &msg); + mavlink_msg_command_ack_send_struct(get_channel(), &msg); } struct mavlink_log_s mavlink_log; - if (mavlink_log_sub->update(&mavlink_log_time, &mavlink_log)) { + if (mavlink_log_sub->update_if_changed(&mavlink_log)) { _logbuffer.put(&mavlink_log); } + /* check for shell output */ + if (_mavlink_shell && _mavlink_shell->available() > 0) { + mavlink_serial_control_t msg; + msg.baudrate = 0; + msg.flags = SERIAL_CONTROL_FLAG_REPLY; + msg.timeout = 0; + msg.device = SERIAL_CONTROL_DEV_SHELL; + msg.count = _mavlink_shell->read(msg.data, sizeof(msg.data)); + mavlink_msg_serial_control_send_struct(get_channel(), &msg); + } + /* check for requested subscriptions */ if (_subscribe_to_stream != nullptr) { if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) { if (_subscribe_to_stream_rate > 0.0f) { if ( get_protocol() == SERIAL ) { - warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, - (double)_subscribe_to_stream_rate); + PX4_DEBUG("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, + (double)_subscribe_to_stream_rate); } else if ( get_protocol() == UDP ) { - warnx("stream %s on UDP port %d enabled with rate %.1f Hz", _subscribe_to_stream, _network_port, - (double)_subscribe_to_stream_rate); + PX4_DEBUG("stream %s on UDP port %d enabled with rate %.1f Hz", _subscribe_to_stream, _network_port, + (double)_subscribe_to_stream_rate); } } else { if ( get_protocol() == SERIAL ) { - warnx("stream %s on device %s disabled", _subscribe_to_stream, _device_name); + PX4_INFO("stream %s on device %s disabled", _subscribe_to_stream, _device_name); } else if ( get_protocol() == UDP ) { - warnx("stream %s on UDP port %d disabled", _subscribe_to_stream, _network_port); + PX4_INFO("stream %s on UDP port %d disabled", _subscribe_to_stream, _network_port); } } } else { if ( get_protocol() == SERIAL ) { - warnx("stream %s on device %s not found", _subscribe_to_stream, _device_name); + PX4_WARN("stream %s on device %s not found", _subscribe_to_stream, _device_name); } else if ( get_protocol() == UDP ) { - warnx("stream %s on UDP port %d not found", _subscribe_to_stream, _network_port); + PX4_WARN("stream %s on UDP port %d not found", _subscribe_to_stream, _network_port); } } @@ -2179,13 +2263,17 @@ Mavlink::task_main(int argc, char *argv[]) ::close(_uart_fd); } + if (_socket_fd >= 0) { + close(_socket_fd); + _socket_fd = -1; + } + if (_forwarding_on || _ftp_on) { message_buffer_destroy(); pthread_mutex_destroy(&_message_buffer_mutex); } - warnx("exiting"); - _task_running = false; + warnx("exiting channel %i", (int)_channel); return OK; } @@ -2206,9 +2294,8 @@ int Mavlink::start_helper(int argc, char *argv[]) } else { /* this will actually only return once MAVLink exits */ res = instance->task_main(argc, argv); + instance->_task_running = false; - /* delete instance on main thread end */ - delete instance; } return res; @@ -2221,9 +2308,9 @@ Mavlink::start(int argc, char *argv[]) // before returning to the shell int ic = Mavlink::instance_count(); - if (ic == MAVLINK_COMM_NUM_BUFFERS) { + if (ic == Mavlink::MAVLINK_MAX_INSTANCES) { warnx("Maximum MAVLink instance count of %d reached.", - (int)MAVLINK_COMM_NUM_BUFFERS); + (int)Mavlink::MAVLINK_MAX_INSTANCES); return 1; } @@ -2404,16 +2491,49 @@ Mavlink::stream_command(int argc, char *argv[]) } } else { - warnx("usage: mavlink stream [-d device] [-u network_port] -s stream -r rate"); + PX4_INFO("usage: mavlink stream [-d device] [-u network_port] -s stream -r rate"); return 1; } return OK; } +void +Mavlink::set_boot_complete() +{ + _boot_complete = true; + +#ifdef __PX4_POSIX + Mavlink *inst; + LL_FOREACH(::_mavlink_instances, inst) { + if ((inst->get_mode() != MAVLINK_MODE_ONBOARD) && + (!inst->broadcast_enabled()) && + ((inst->get_protocol() == UDP) || (inst->get_protocol() == TCP))) { + PX4_INFO("MAVLink only on localhost (set param MAV_BROADCAST = 1 to enable network)"); + } + } +#endif + +} + static void usage() { - warnx("usage: mavlink {start|stop|stream} [-d device] [-u network_port] [-o remote_port] [-t partner_ip] [-b baudrate]\n\t[-r rate][-m mode] [-s stream] [-f] [-p] [-v] [-w] [-x]"); + PX4_INFO("usage: mavlink {start|status|stream|stop-all|boot_complete}"); + PX4_INFO(" [-d device]"); +#ifdef __PX4_POSIX + PX4_INFO(" [-u network_port]"); + PX4_INFO(" [-o remote_port]"); + PX4_INFO(" [-t partner_ip]"); +#endif + PX4_INFO(" [-b baudrate]"); + PX4_INFO(" [-r rate]"); + PX4_INFO(" [-m mode]"); + PX4_INFO(" [-s stream]"); + PX4_INFO(" [-f]"); + PX4_INFO(" [-p]"); + PX4_INFO(" [-v]"); + PX4_INFO(" [-w]"); + PX4_INFO(" [-x]"); } int mavlink_main(int argc, char *argv[]) @@ -2427,11 +2547,11 @@ int mavlink_main(int argc, char *argv[]) return Mavlink::start(argc, argv); } else if (!strcmp(argv[1], "stop")) { - warnx("mavlink stop is deprecated, use stop-all instead"); + PX4_WARN("mavlink stop is deprecated, use stop-all instead"); usage(); return 1; - } else if (!strcmp(argv[1], "stop") || !strcmp(argv[1], "stop-all") ) { + } else if (!strcmp(argv[1], "stop-all") ) { return Mavlink::destroy_all_instances(); } else if (!strcmp(argv[1], "status")) { diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 9372c53f0f..a4c695cfa3 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -69,6 +69,7 @@ #include "mavlink_parameters.h" #include "mavlink_ftp.h" #include "mavlink_log_handler.h" +#include "mavlink_shell.h" enum Protocol { SERIAL = 0, @@ -114,6 +115,19 @@ public: static Mavlink *get_instance_for_network_port(unsigned long port); + mavlink_message_t *get_buffer() { return &_mavlink_buffer; } + + mavlink_status_t *get_status() { return &_mavlink_status; } + + /** + * Set the MAVLink version + * + * Currently supporting v1 and v2 + * + * @param version MAVLink version + */ + void set_proto_version(unsigned version); + static int destroy_all_instances(); static int get_status_all_instances(); @@ -151,6 +165,30 @@ public: MAVLINK_MODE_CONFIG }; + enum BROADCAST_MODE { + BROADCAST_MODE_OFF = 0, + BROADCAST_MODE_ON + }; + + static const char *mavlink_mode_str(enum MAVLINK_MODE mode) { + switch (mode) { + case MAVLINK_MODE_NORMAL: + return "Normal"; + case MAVLINK_MODE_CUSTOM: + return "Custom"; + case MAVLINK_MODE_ONBOARD: + return "Onboard"; + case MAVLINK_MODE_OSD: + return "OSD"; + case MAVLINK_MODE_MAGIC: + return "Magic"; + case MAVLINK_MODE_CONFIG: + return "Config"; + default: + return "Unknown"; + } + } + void set_mode(enum MAVLINK_MODE); enum MAVLINK_MODE get_mode() { return _mode; } @@ -168,13 +206,15 @@ public: void set_config_link_on(bool on) { _config_link_on = on; } + bool broadcast_enabled() { return _broadcast_mode > BROADCAST_MODE_OFF; } + /** * Set the boot complete flag on all instances * * Setting the flag unblocks parameter transmissions, which are gated * beforehand to ensure that the system is fully initialized. */ - static void set_boot_complete() { _boot_complete = true; } + static void set_boot_complete(); /** * Get the free space in the transmit buffer @@ -224,12 +264,24 @@ public: */ bool get_manual_input_mode_generation() { return _generate_rc; } - void send_message(const uint8_t msgid, const void *msg, uint8_t component_ID = 0); + /** + * Send bytes out on the link. + * + * On a network port these might actually get buffered to form a packet. + */ + void send_bytes(const uint8_t *buf, unsigned packet_len); + + /** + * Flush the transmit buffer and send one MAVLink packet + * + * @return the number of bytes sent or -1 in case of error + */ + int send_packet(); /** * Resend message as is, don't change sequence number and CRC. */ - void resend_message(mavlink_message_t *msg); + void resend_message(mavlink_message_t *msg) { _mavlink_resend_uart(_channel, msg); } void handle_message(const mavlink_message_t *msg); @@ -298,7 +350,7 @@ public: bool get_has_received_messages() { return _received_messages; } void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; } bool get_wait_to_transmit() { return _wait_to_transmit; } - bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } + bool should_transmit() { return (_boot_complete && (!_wait_to_transmit || (_wait_to_transmit && _received_messages))); } bool message_buffer_write(const void *ptr, int size); @@ -360,12 +412,21 @@ public: bool accepting_commands() { return true; /* non-trivial side effects ((!_config_link_on) || (_mode == MAVLINK_MODE_CONFIG));*/ } /** - * Wether or not the system should be logging + * Whether or not the system should be logging */ bool get_logging_enabled() { return _logging_enabled; } void set_logging_enabled(bool logging) { _logging_enabled = logging; } + int get_data_rate() { return _datarate; } + void set_data_rate(int rate) { if (rate > 0) _datarate = rate; } + + /** get the Mavlink shell. Create a new one if there isn't one. It is *always* created via MavlinkReceiver thread. + * Returns nullptr if shell cannot be created */ + MavlinkShell *get_shell(); + /** close the Mavlink shell if it is open */ + void close_shell(); + protected: Mavlink *next; @@ -375,6 +436,9 @@ private: orb_advert_t _mavlink_log_pub; bool _task_running; static bool _boot_complete; + static const unsigned MAVLINK_MAX_INSTANCES = 4; + mavlink_message_t _mavlink_buffer; + mavlink_status_t _mavlink_status; /* states */ bool _hil_enabled; /**< Hardware In the Loop mode */ @@ -394,6 +458,7 @@ private: MavlinkParametersManager *_parameters_manager; MavlinkFTP *_mavlink_ftp; MavlinkLogHandler *_mavlink_log_handler; + MavlinkShell *_mavlink_shell; MAVLINK_MODE _mode; @@ -434,6 +499,7 @@ private: uint64_t _last_write_success_time; uint64_t _last_write_try_time; uint64_t _mavlink_start_time; + int32_t _protocol_version; unsigned _bytes_tx; unsigned _bytes_txerr; @@ -449,7 +515,10 @@ private: struct sockaddr_in _bcast_addr; bool _src_addr_initialized; bool _broadcast_address_found; - + bool _broadcast_address_not_found_warned; + bool _broadcast_failed_warned; + uint8_t _network_buf[MAVLINK_MAX_PACKET_LEN]; + unsigned _network_buf_len; #endif int _socket_fd; Protocol _protocol; @@ -472,13 +541,16 @@ private: bool _param_initialized; bool _logging_enabled; + uint32_t _broadcast_mode; param_t _param_system_id; param_t _param_component_id; + param_t _param_proto_ver; param_t _param_radio_id; param_t _param_system_type; param_t _param_use_hil_gps; param_t _param_forward_externalsp; + param_t _param_broadcast; unsigned _system_type; static bool _config_link_on; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index b31a9815e6..a2c27610f3 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2016 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 @@ -39,57 +39,62 @@ * @author Anton Babushkin */ -#include #include #include +#include "mavlink_main.h" +#include "mavlink_messages.h" + #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include +#include +#include +#include +#include #include #include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include -#include "mavlink_messages.h" -#include "mavlink_main.h" static uint16_t cm_uint16_from_m_float(float m); -static void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, - uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); +static void get_mavlink_mode_state(struct vehicle_status_s *status, uint8_t *mavlink_state, + uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); uint16_t cm_uint16_from_m_float(float m) @@ -104,8 +109,8 @@ cm_uint16_from_m_float(float m) return (uint16_t)(m * 100.0f); } -void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, - uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) +void get_mavlink_mode_state(struct vehicle_status_s *status, uint8_t *mavlink_state, + uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) { *mavlink_state = 0; *mavlink_base_mode = 0; @@ -128,11 +133,14 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set union px4_custom_mode custom_mode; custom_mode.data = 0; - switch (status->nav_state) { + const uint8_t auto_mode_flags = MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + switch (status->nav_state) { case vehicle_status_s::NAVIGATION_STATE_MANUAL: - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED - | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; break; @@ -147,60 +155,52 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set break; case vehicle_status_s::NAVIGATION_STATE_STAB: - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED; + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_STABILIZED; break; case vehicle_status_s::NAVIGATION_STATE_ALTCTL: - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED; + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL; break; case vehicle_status_s::NAVIGATION_STATE_POSCTL: - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED - | MAV_MODE_FLAG_GUIDED_ENABLED; + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; // TODO: is POSCTL GUIDED? custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED - | MAV_MODE_FLAG_GUIDED_ENABLED; + *mavlink_base_mode |= auto_mode_flags; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED - | MAV_MODE_FLAG_GUIDED_ENABLED; + *mavlink_base_mode |= auto_mode_flags; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED - | MAV_MODE_FLAG_GUIDED_ENABLED; + *mavlink_base_mode |= auto_mode_flags; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED - | MAV_MODE_FLAG_GUIDED_ENABLED; + *mavlink_base_mode |= auto_mode_flags; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_FOLLOW_TARGET; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET; break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: /* fallthrough */ case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER: - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED - | MAV_MODE_FLAG_GUIDED_ENABLED; + *mavlink_base_mode |= auto_mode_flags; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; break; @@ -210,17 +210,13 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: /* fallthrough */ case vehicle_status_s::NAVIGATION_STATE_DESCEND: - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED - | MAV_MODE_FLAG_GUIDED_ENABLED; + *mavlink_base_mode |= auto_mode_flags; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED - | MAV_MODE_FLAG_GUIDED_ENABLED; + *mavlink_base_mode |= auto_mode_flags; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTGS; break; @@ -231,9 +227,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set break; case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED - | MAV_MODE_FLAG_GUIDED_ENABLED; + *mavlink_base_mode |= auto_mode_flags; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD; break; @@ -282,11 +276,16 @@ public: return "HEARTBEAT"; } - uint8_t get_id() + static uint8_t get_id_static() { return MAVLINK_MSG_ID_HEARTBEAT; } + uint8_t get_id() + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamHeartbeat(mavlink); @@ -303,7 +302,6 @@ public: private: MavlinkOrbSubscription *_status_sub; - MavlinkOrbSubscription *_pos_sp_triplet_sub; /* do not allow top copying this class */ MavlinkStreamHeartbeat(MavlinkStreamHeartbeat &); @@ -311,14 +309,12 @@ private: protected: explicit MavlinkStreamHeartbeat(Mavlink *mavlink) : MavlinkStream(mavlink), - _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))), - _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))) + _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))) {} void send(const hrt_abstime t) { struct vehicle_status_s status; - struct position_setpoint_triplet_s pos_sp_triplet; /* always send the heartbeat, independent of the update status of the topics */ if (!_status_sub->update(&status)) { @@ -326,21 +322,13 @@ protected: memset(&status, 0, sizeof(status)); } - if (!_pos_sp_triplet_sub->update(&pos_sp_triplet)) { - /* if topic update failed fill it with defaults */ - memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet)); - } + uint8_t base_mode = 0; + uint32_t custom_mode = 0; + uint8_t system_status = 0; + get_mavlink_mode_state(&status, &system_status, &base_mode, &custom_mode); - mavlink_heartbeat_t msg; - - msg.base_mode = 0; - msg.custom_mode = 0; - get_mavlink_mode_state(&status, &pos_sp_triplet, &msg.system_status, &msg.base_mode, &msg.custom_mode); - msg.type = _mavlink->get_system_type(); - msg.autopilot = MAV_AUTOPILOT_PX4; - msg.mavlink_version = 3; - - _mavlink->send_message(MAVLINK_MSG_ID_HEARTBEAT, &msg); + mavlink_msg_heartbeat_send(_mavlink->get_channel(), _mavlink->get_system_type(), MAV_AUTOPILOT_PX4, + base_mode, custom_mode, system_status); } }; @@ -357,11 +345,16 @@ public: return "STATUSTEXT"; } - uint8_t get_id() + static uint8_t get_id_static() { return MAVLINK_MSG_ID_STATUSTEXT; } + uint8_t get_id() + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamStatustext(mavlink); @@ -376,23 +369,20 @@ private: MavlinkStreamStatustext(MavlinkStreamStatustext &); MavlinkStreamStatustext& operator = (const MavlinkStreamStatustext &); - unsigned write_err_count = 0; + unsigned _write_err_count = 0; static const unsigned write_err_threshold = 5; #ifndef __PX4_POSIX_EAGLE - FILE *fp; + FILE *_fp = nullptr; #endif protected: explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink) -#ifndef __PX4_POSIX_EAGLE - , fp(nullptr) -#endif {} ~MavlinkStreamStatustext() { #ifndef __PX4_POSIX_EAGLE - if (fp != nullptr) { - fclose(fp); + if (_fp != nullptr) { + fclose(_fp); } #endif } @@ -410,7 +400,7 @@ protected: strncpy(msg.text, (const char *)mavlink_log.text, sizeof(msg.text)); msg.text[sizeof(msg.text) - 1] = '\0'; - _mavlink->send_message(MAVLINK_MSG_ID_STATUSTEXT, &msg); + mavlink_msg_statustext_send_struct(_mavlink->get_channel(), &msg); // TODO: the logging doesn't work on Snapdragon yet because of file paths. #ifndef __PX4_POSIX_EAGLE @@ -426,27 +416,27 @@ protected: strftime(tstamp, sizeof(tstamp) - 1, "%Y_%m_%d_%H_%M_%S", &tt); if (_mavlink->get_instance_id() == 0/* && _mavlink->get_logging_enabled()*/) { - if (fp != nullptr) { - fputs(tstamp, fp); - fputs(": ", fp); - if (EOF == fputs(msg.text, fp)) { - write_err_count++; + if (_fp != nullptr) { + fputs(tstamp, _fp); + fputs(": ", _fp); + if (EOF == fputs(msg.text, _fp)) { + _write_err_count++; } else { - write_err_count = 0; + _write_err_count = 0; } - if (write_err_count >= write_err_threshold) { - (void)fclose(fp); - fp = nullptr; + if (_write_err_count >= write_err_threshold) { + (void)fclose(_fp); + _fp = nullptr; PX4_WARN("mavlink logging disabled"); } else { - (void)fputs("\n", fp); + (void)fputs("\n", _fp); #ifdef __PX4_NUTTX - fsync(fp->fs_filedes); + fsync(_fp->fs_filedes); #endif } - } else if (write_err_count < write_err_threshold) { + } else if (_write_err_count < write_err_threshold) { /* string to hold the path to the log */ char log_file_path[128]; @@ -454,19 +444,20 @@ protected: /* store the log file in the root directory */ snprintf(log_file_path, sizeof(log_file_path) - 1, PX4_ROOTFSDIR"/fs/microsd/msgs_%s.txt", tstamp); - fp = fopen(log_file_path, "ab"); + _fp = fopen(log_file_path, "ab"); - if (fp != nullptr) { + if (_fp != nullptr) { /* write first message */ - fputs(tstamp, fp); - fputs(": ", fp); - fputs(msg.text, fp); - fputs("\n", fp); + fputs(tstamp, _fp); + fputs(": ", _fp); + fputs(msg.text, _fp); + fputs("\n", _fp); #ifdef __PX4_NUTTX - fsync(fp->fs_filedes); + fsync(_fp->fs_filedes); #endif } else { PX4_WARN("Failed to open MAVLink log: %s", log_file_path); + _write_err_count = write_err_threshold; //only try to open the file once } } } @@ -489,11 +480,16 @@ public: return "COMMAND_LONG"; } - uint8_t get_id() + static uint8_t get_id_static() { return MAVLINK_MSG_ID_COMMAND_LONG; } + uint8_t get_id() + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamCommandLong(mavlink); @@ -538,7 +534,7 @@ protected: msg.param6 = cmd.param6; msg.param7 = cmd.param7; - _mavlink->send_message(MAVLINK_MSG_ID_COMMAND_LONG, &msg); + mavlink_msg_command_long_send_struct(_mavlink->get_channel(), &msg); } } } @@ -557,11 +553,16 @@ public: return "SYS_STATUS"; } - uint8_t get_id() + static uint8_t get_id_static() { return MAVLINK_MSG_ID_SYS_STATUS; } + uint8_t get_id() + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamSysStatus(mavlink); @@ -574,6 +575,7 @@ public: private: MavlinkOrbSubscription *_status_sub; + MavlinkOrbSubscription *_cpuload_sub; MavlinkOrbSubscription *_battery_status_sub; /* do not allow top copying this class */ @@ -583,15 +585,18 @@ private: protected: explicit MavlinkStreamSysStatus(Mavlink *mavlink) : MavlinkStream(mavlink), _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))), + _cpuload_sub(_mavlink->add_orb_subscription(ORB_ID(cpuload))), _battery_status_sub(_mavlink->add_orb_subscription(ORB_ID(battery_status))) {} void send(const hrt_abstime t) { struct vehicle_status_s status = {}; + struct cpuload_s cpuload = {}; struct battery_status_s battery_status = {}; const bool updated_status = _status_sub->update(&status); + const bool updated_cpuload = _cpuload_sub->update(&cpuload); const bool updated_battery = _battery_status_sub->update(&battery_status); if (updated_status) { @@ -602,13 +607,13 @@ protected: } } - if (updated_status || updated_battery) { + if (updated_status || updated_battery || updated_cpuload) { mavlink_sys_status_t msg; msg.onboard_control_sensors_present = status.onboard_control_sensors_present; msg.onboard_control_sensors_enabled = status.onboard_control_sensors_enabled; msg.onboard_control_sensors_health = status.onboard_control_sensors_health; - msg.load = status.load * 1000.0f; + msg.load = cpuload.load * 1000.0f; msg.voltage_battery = (battery_status.connected) ? battery_status.voltage_filtered_v * 1000.0f : UINT16_MAX; msg.current_battery = (battery_status.connected) ? battery_status.current_filtered_a * 100.0f : -1; msg.battery_remaining = (battery_status.connected) ? battery_status.remaining * 100.0f : -1; @@ -620,7 +625,7 @@ protected: msg.errors_count3 = 0; msg.errors_count4 = 0; - _mavlink->send_message(MAVLINK_MSG_ID_SYS_STATUS, &msg); + mavlink_msg_sys_status_send_struct(_mavlink->get_channel(), &msg); /* battery status message with higher resolution */ mavlink_battery_status_t bat_msg = {}; @@ -640,7 +645,7 @@ protected: } } - _mavlink->send_message(MAVLINK_MSG_ID_BATTERY_STATUS, &bat_msg); + mavlink_msg_battery_status_send_struct(_mavlink->get_channel(), &bat_msg); } } }; @@ -659,11 +664,16 @@ public: return "HIGHRES_IMU"; } - uint8_t get_id() + static uint8_t get_id_static() { return MAVLINK_MSG_ID_HIGHRES_IMU; } + uint8_t get_id() + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamHighresIMU(mavlink); @@ -678,6 +688,9 @@ private: MavlinkOrbSubscription *_sensor_sub; uint64_t _sensor_time; + MavlinkOrbSubscription *_differential_pressure_sub; + uint64_t _differential_pressure_time; + uint64_t _accel_timestamp; uint64_t _gyro_timestamp; uint64_t _mag_timestamp; @@ -691,6 +704,8 @@ protected: explicit MavlinkStreamHighresIMU(Mavlink *mavlink) : MavlinkStream(mavlink), _sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))), _sensor_time(0), + _differential_pressure_sub(_mavlink->add_orb_subscription(ORB_ID(differential_pressure))), + _differential_pressure_time(0), _accel_timestamp(0), _gyro_timestamp(0), _mag_timestamp(0), @@ -700,33 +715,35 @@ protected: void send(const hrt_abstime t) { struct sensor_combined_s sensor; + struct differential_pressure_s differential_pressure; if (_sensor_sub->update(&_sensor_time, &sensor)) { uint16_t fields_updated = 0; - if (_accel_timestamp != sensor.accelerometer_timestamp[0]) { + if (_accel_timestamp != sensor.timestamp + sensor.accelerometer_timestamp_relative) { /* mark first three dimensions as changed */ fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); - _accel_timestamp = sensor.accelerometer_timestamp[0]; + _accel_timestamp = sensor.timestamp + sensor.accelerometer_timestamp_relative; } - if (_gyro_timestamp != sensor.gyro_timestamp[0]) { + if (_gyro_timestamp != sensor.timestamp) { /* mark second group dimensions as changed */ fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); - _gyro_timestamp = sensor.gyro_timestamp[0]; + _gyro_timestamp = sensor.timestamp; } - if (_mag_timestamp != sensor.magnetometer_timestamp[0]) { + if (_mag_timestamp != sensor.timestamp + sensor.magnetometer_timestamp_relative) { /* mark third group dimensions as changed */ fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); - _mag_timestamp = sensor.magnetometer_timestamp[0]; + _mag_timestamp = sensor.timestamp + sensor.magnetometer_timestamp_relative; } - if (_baro_timestamp != sensor.baro_timestamp[0]) { + if (_baro_timestamp != sensor.timestamp + sensor.baro_timestamp_relative) { /* mark last group dimensions as changed */ fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); - _baro_timestamp = sensor.baro_timestamp[0]; + _baro_timestamp = sensor.timestamp + sensor.baro_timestamp_relative; } + _differential_pressure_sub->update(&_differential_pressure_time, &differential_pressure); mavlink_highres_imu_t msg; @@ -734,19 +751,19 @@ protected: msg.xacc = sensor.accelerometer_m_s2[0]; msg.yacc = sensor.accelerometer_m_s2[1]; msg.zacc = sensor.accelerometer_m_s2[2]; - msg.xgyro = sensor.gyro_rad_s[0]; - msg.ygyro = sensor.gyro_rad_s[1]; - msg.zgyro = sensor.gyro_rad_s[2]; + msg.xgyro = sensor.gyro_rad[0]; + msg.ygyro = sensor.gyro_rad[1]; + msg.zgyro = sensor.gyro_rad[2]; msg.xmag = sensor.magnetometer_ga[0]; msg.ymag = sensor.magnetometer_ga[1]; msg.zmag = sensor.magnetometer_ga[2]; - msg.abs_pressure = sensor.baro_pres_mbar[0]; - msg.diff_pressure = sensor.differential_pressure_pa[0]; - msg.pressure_alt = sensor.baro_alt_meter[0]; - msg.temperature = sensor.baro_temp_celcius[0]; + msg.abs_pressure = 0; + msg.diff_pressure = differential_pressure.differential_pressure_raw_pa; + msg.pressure_alt = sensor.baro_alt_meter; + msg.temperature = sensor.baro_temp_celcius; msg.fields_updated = fields_updated; - _mavlink->send_message(MAVLINK_MSG_ID_HIGHRES_IMU, &msg); + mavlink_msg_highres_imu_send_struct(_mavlink->get_channel(), &msg); } } }; @@ -765,11 +782,16 @@ public: return "ATTITUDE"; } - uint8_t get_id() + static uint8_t get_id_static() { return MAVLINK_MSG_ID_ATTITUDE; } + uint8_t get_id() + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamAttitude(mavlink); @@ -810,7 +832,7 @@ protected: msg.pitchspeed = att.pitchspeed; msg.yawspeed = att.yawspeed; - _mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE, &msg); + mavlink_msg_attitude_send_struct(_mavlink->get_channel(), &msg); } } }; @@ -829,11 +851,16 @@ public: return "ATTITUDE_QUATERNION"; } - uint8_t get_id() + static uint8_t get_id_static() { return MAVLINK_MSG_ID_ATTITUDE_QUATERNION; } + uint8_t get_id() + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamAttitudeQuaternion(mavlink); @@ -874,7 +901,7 @@ protected: msg.pitchspeed = att.pitchspeed; msg.yawspeed = att.yawspeed; - _mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE_QUATERNION, &msg); + mavlink_msg_attitude_quaternion_send_struct(_mavlink->get_channel(), &msg); } } }; @@ -894,11 +921,16 @@ public: return "VFR_HUD"; } - uint8_t get_id() + static uint8_t get_id_static() { return MAVLINK_MSG_ID_VFR_HUD; } + uint8_t get_id() + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamVFRHUD(mavlink); @@ -976,11 +1008,11 @@ protected: /* fall back to baro altitude */ sensor_combined_s sensor; (void)_sensor_sub->update(&_sensor_time, &sensor); - msg.alt = sensor.baro_alt_meter[0]; + msg.alt = sensor.baro_alt_meter; } msg.climb = -pos.vel_d; - _mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg); + mavlink_msg_vfr_hud_send_struct(_mavlink->get_channel(), &msg); } } }; @@ -999,11 +1031,16 @@ public: return "GPS_RAW_INT"; } - uint8_t get_id() + static uint8_t get_id_static() { return MAVLINK_MSG_ID_GPS_RAW_INT; } + uint8_t get_id() + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamGPSRawInt(mavlink); @@ -1035,7 +1072,7 @@ protected: if (_gps_sub->update(&_gps_time, &gps)) { mavlink_gps_raw_int_t msg = {}; - msg.time_usec = gps.timestamp_position; + msg.time_usec = gps.timestamp; msg.fix_type = gps.fix_type; msg.lat = gps.lat; msg.lon = gps.lon; @@ -1046,7 +1083,7 @@ protected: msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, msg.satellites_visible = gps.satellites_used; - _mavlink->send_message(MAVLINK_MSG_ID_GPS_RAW_INT, &msg); + mavlink_msg_gps_raw_int_send_struct(_mavlink->get_channel(), &msg); } } }; @@ -1062,10 +1099,15 @@ public: return "SYSTEM_TIME"; } - uint8_t get_id() { + static uint8_t get_id_static() { return MAVLINK_MSG_ID_SYSTEM_TIME; } + uint8_t get_id() + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamSystemTime(mavlink); } @@ -1092,7 +1134,7 @@ protected: msg.time_boot_ms = hrt_absolute_time() / 1000; msg.time_unix_usec = (uint64_t)tv.tv_sec * 1000000 + tv.tv_nsec / 1000; - _mavlink->send_message(MAVLINK_MSG_ID_SYSTEM_TIME, &msg); + mavlink_msg_system_time_send_struct(_mavlink->get_channel(), &msg); } }; @@ -1107,10 +1149,15 @@ public: return "TIMESYNC"; } - uint8_t get_id() { + static uint8_t get_id_static() { return MAVLINK_MSG_ID_TIMESYNC; } + uint8_t get_id() + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamTimesync(mavlink); } @@ -1134,7 +1181,7 @@ protected: msg.tc1 = 0; msg.ts1 = hrt_absolute_time() * 1000; // boot time in nanoseconds - _mavlink->send_message(MAVLINK_MSG_ID_TIMESYNC, &msg); + mavlink_msg_timesync_send_struct(_mavlink->get_channel(), &msg); } }; @@ -1151,11 +1198,16 @@ public: return "ADSB_VEHICLE"; } - uint8_t get_id() + static uint8_t get_id_static() { return MAVLINK_MSG_ID_ADSB_VEHICLE; } + uint8_t get_id() + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamADSBVehicle(mavlink); @@ -1163,7 +1215,7 @@ public: unsigned get_size() { - return MAVLINK_MSG_ID_ADSB_VEHICLE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return (_pos_time > 0) ? MAVLINK_MSG_ID_ADSB_VEHICLE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: @@ -1201,7 +1253,7 @@ protected: msg.flags = pos.flags; msg.squawk = pos.squawk; - _mavlink->send_message(MAVLINK_MSG_ID_ADSB_VEHICLE, &msg); + mavlink_msg_adsb_vehicle_send_struct(_mavlink->get_channel(), &msg); } } }; @@ -1219,11 +1271,16 @@ public: return "CAMERA_TRIGGER"; } - uint8_t get_id() + static uint8_t get_id_static() { return MAVLINK_MSG_ID_CAMERA_TRIGGER; } + uint8_t get_id() + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamCameraTrigger(mavlink); @@ -1231,7 +1288,7 @@ public: unsigned get_size() { - return MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return (_trigger_time > 0) ? MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: @@ -1260,7 +1317,7 @@ protected: /* ensure that only active trigger events are sent */ if (trigger.timestamp > 0) { - _mavlink->send_message(MAVLINK_MSG_ID_CAMERA_TRIGGER, &msg); + mavlink_msg_camera_trigger_send_struct(_mavlink->get_channel(), &msg); } } } @@ -1279,11 +1336,16 @@ public: return "GLOBAL_POSITION_INT"; } - uint8_t get_id() + static uint8_t get_id_static() { return MAVLINK_MSG_ID_GLOBAL_POSITION_INT; } + uint8_t get_id() + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamGlobalPositionInt(mavlink); @@ -1334,7 +1396,7 @@ protected: msg.vz = pos.vel_d * 100.0f; msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f; - _mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, &msg); + mavlink_msg_global_position_int_send_struct(_mavlink->get_channel(), &msg); } } }; @@ -1352,11 +1414,16 @@ public: return "VISION_POSITION_NED"; } - uint8_t get_id() + static uint8_t get_id_static() { return MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; } + uint8_t get_id() + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamVisionPositionNED(mavlink); @@ -1364,7 +1431,7 @@ public: unsigned get_size() { - return MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return (_pos_time > 0) ? MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: @@ -1388,7 +1455,7 @@ protected: if (_pos_sub->update(&_pos_time, &vpos)) { mavlink_vision_position_estimate_t vmsg; - vmsg.usec = vpos.timestamp_boot / 1000; + vmsg.usec = vpos.timestamp; vmsg.x = vpos.x; vmsg.y = vpos.y; vmsg.z = vpos.z; @@ -1398,7 +1465,7 @@ protected: vmsg.pitch = rpy(1); vmsg.yaw = rpy(2); - _mavlink->send_message(MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, &vmsg); + mavlink_msg_vision_position_estimate_send_struct(_mavlink->get_channel(), &vmsg); } } }; @@ -1416,11 +1483,16 @@ public: return "LOCAL_POSITION_NED"; } - uint8_t get_id() + static uint8_t get_id_static() { return MAVLINK_MSG_ID_LOCAL_POSITION_NED; } + uint8_t get_id() + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamLocalPositionNED(mavlink); @@ -1460,7 +1532,7 @@ protected: msg.vy = pos.vy; msg.vz = pos.vz; - _mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_NED, &msg); + mavlink_msg_local_position_ned_send_struct(_mavlink->get_channel(), &msg); } } }; @@ -1479,11 +1551,16 @@ public: return "LOCAL_POSITION_NED_COV"; } - uint8_t get_id() + static uint8_t get_id_static() { return MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV; } + uint8_t get_id() + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamLocalPositionNEDCOV(mavlink); @@ -1533,7 +1610,7 @@ protected: msg.covariance[11] = est.timeout_flags; memcpy(msg.covariance, est.covariances, sizeof(est.covariances)); - _mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV, &msg); + mavlink_msg_local_position_ned_cov_send_struct(_mavlink->get_channel(), &msg); } } }; @@ -1551,11 +1628,16 @@ public: return "ESTIMATOR_STATUS"; } - uint8_t get_id() + static uint8_t get_id_static() { return MAVLINK_MSG_ID_VIBRATION; } + uint8_t get_id() + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamEstimatorStatus(mavlink); @@ -1588,7 +1670,7 @@ protected: // To be added and filled // mavlink_estimator_status_t msg = {}; - //_mavlink->send_message(MAVLINK_MSG_ID_ESTIMATOR_STATUS, &msg); + //mavlink_msg_statustext_send_struct(_mavlink->get_channel(), &msg); mavlink_vibration_t msg = {}; @@ -1596,7 +1678,7 @@ protected: msg.vibration_y = est.vibe[1]; msg.vibration_z = est.vibe[2]; - _mavlink->send_message(MAVLINK_MSG_ID_VIBRATION, &msg); + mavlink_msg_vibration_send_struct(_mavlink->get_channel(), &msg); } } }; @@ -1614,11 +1696,16 @@ public: return "ATT_POS_MOCAP"; } - uint8_t get_id() + static uint8_t get_id_static() { return MAVLINK_MSG_ID_ATT_POS_MOCAP; } + uint8_t get_id() + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamAttPosMocap(mavlink); @@ -1650,7 +1737,7 @@ protected: if (_mocap_sub->update(&_mocap_time, &mocap)) { mavlink_att_pos_mocap_t msg; - msg.time_usec = mocap.timestamp_boot; + msg.time_usec = mocap.timestamp; msg.q[0] = mocap.q[0]; msg.q[1] = mocap.q[1]; msg.q[2] = mocap.q[2]; @@ -1659,7 +1746,7 @@ protected: msg.y = mocap.y; msg.z = mocap.z; - _mavlink->send_message(MAVLINK_MSG_ID_ATT_POS_MOCAP, &msg); + mavlink_msg_att_pos_mocap_send_struct(_mavlink->get_channel(), &msg); } } }; @@ -1678,11 +1765,16 @@ public: return "HOME_POSITION"; } - uint8_t get_id() + static uint8_t get_id_static() { return MAVLINK_MSG_ID_HOME_POSITION; } + uint8_t get_id() + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamHomePosition(mavlink); @@ -1732,7 +1824,7 @@ protected: msg.approach_y = 0.0f; msg.approach_z = 0.0f; - _mavlink->send_message(MAVLINK_MSG_ID_HOME_POSITION, &msg); + mavlink_msg_home_position_send_struct(_mavlink->get_channel(), &msg); } } } @@ -1748,11 +1840,16 @@ public: return MavlinkStreamServoOutputRaw::get_name_static(); } - uint8_t get_id() + static uint8_t get_id_static() { return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; } + uint8_t get_id() + { + return get_id_static(); + } + static const char *get_name_static() { switch (N) { @@ -1814,7 +1911,7 @@ protected: msg.servo7_raw = act.output[6]; msg.servo8_raw = act.output[7]; - _mavlink->send_message(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, &msg); + mavlink_msg_servo_output_raw_send_struct(_mavlink->get_channel(), &msg); } } }; @@ -1845,11 +1942,16 @@ public: } } - uint8_t get_id() + static uint8_t get_id_static() { return MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET; } + uint8_t get_id() + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamActuatorControlTarget(mavlink); @@ -1907,7 +2009,7 @@ protected: msg.controls[i] = att_ctrl.control[i]; } - _mavlink->send_message(MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET, &msg); + mavlink_msg_actuator_control_target_send_struct(_mavlink->get_channel(), &msg); } } }; @@ -1926,11 +2028,16 @@ public: return "HIL_CONTROLS"; } - uint8_t get_id() + static uint8_t get_id_static() { return MAVLINK_MSG_ID_HIL_CONTROLS; } + uint8_t get_id() + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamHILControls(mavlink); @@ -1945,9 +2052,6 @@ private: MavlinkOrbSubscription *_status_sub; uint64_t _status_time; - MavlinkOrbSubscription *_pos_sp_triplet_sub; - uint64_t _pos_sp_triplet_time; - MavlinkOrbSubscription *_act_sub; uint64_t _act_time; @@ -1959,8 +2063,6 @@ protected: explicit MavlinkStreamHILControls(Mavlink *mavlink) : MavlinkStream(mavlink), _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))), _status_time(0), - _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))), - _pos_sp_triplet_time(0), _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_outputs))), _act_time(0) {} @@ -1968,11 +2070,9 @@ protected: void send(const hrt_abstime t) { struct vehicle_status_s status; - struct position_setpoint_triplet_s pos_sp_triplet; struct actuator_outputs_s act; bool updated = _act_sub->update(&_act_time, &act); - updated |= _pos_sp_triplet_sub->update(&_pos_sp_triplet_time, &pos_sp_triplet); updated |= _status_sub->update(&_status_time, &status); if (updated && (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)) { @@ -1980,7 +2080,7 @@ protected: uint8_t mavlink_state; uint8_t mavlink_base_mode; uint32_t mavlink_custom_mode; - get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + get_mavlink_mode_state(&status, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); float out[8]; @@ -2073,7 +2173,7 @@ protected: msg.mode = mavlink_base_mode; msg.nav_mode = 0; - _mavlink->send_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg); + mavlink_msg_hil_controls_send_struct(_mavlink->get_channel(), &msg); } } }; @@ -2092,11 +2192,16 @@ public: return "POSITION_TARGET_GLOBAL_INT"; } - uint8_t get_id() + static uint8_t get_id_static() { return MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT; } + uint8_t get_id() + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamPositionTargetGlobalInt(mavlink); @@ -2132,7 +2237,7 @@ protected: msg.lon_int = pos_sp_triplet.current.lon * 1e7; msg.alt = pos_sp_triplet.current.alt; - _mavlink->send_message(MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT, &msg); + mavlink_msg_position_target_global_int_send_struct(_mavlink->get_channel(), &msg); } } }; @@ -2151,11 +2256,16 @@ public: return "POSITION_TARGET_LOCAL_NED"; } - uint8_t get_id() + static uint8_t get_id_static() { return MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED; } + uint8_t get_id() + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamLocalPositionSetpoint(mavlink); @@ -2200,7 +2310,7 @@ protected: msg.afy = pos_sp.acc_y; msg.afz = pos_sp.acc_z; - _mavlink->send_message(MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, &msg); + mavlink_msg_position_target_local_ned_send_struct(_mavlink->get_channel(), &msg); } } }; @@ -2219,11 +2329,16 @@ public: return "ATTITUDE_TARGET"; } - uint8_t get_id() + static uint8_t get_id_static() { return MAVLINK_MSG_ID_ATTITUDE_TARGET; } + uint8_t get_id() + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamAttitudeTarget(mavlink); @@ -2272,7 +2387,7 @@ protected: msg.thrust = att_sp.thrust; - _mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE_TARGET, &msg); + mavlink_msg_attitude_target_send_struct(_mavlink->get_channel(), &msg); } } }; @@ -2291,11 +2406,16 @@ public: return "RC_CHANNELS"; } - uint8_t get_id() + static uint8_t get_id_static() { return MAVLINK_MSG_ID_RC_CHANNELS; } + uint8_t get_id() + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamRCChannels(mavlink); @@ -2352,7 +2472,7 @@ protected: msg.rssi = (rc.channel_count > 0) ? rc.rssi : 0; - _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS, &msg); + mavlink_msg_rc_channels_send_struct(_mavlink->get_channel(), &msg); /* send override message - harmless if connected to GCS, allows to connect a board to a Linux system */ /* http://mavlink.org/messages/common#RC_CHANNELS_OVERRIDE */ @@ -2368,7 +2488,7 @@ protected: over.chan7_raw = msg.chan7_raw; over.chan8_raw = msg.chan8_raw; - _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, &over); + mavlink_msg_rc_channels_override_send_struct(_mavlink->get_channel(), &over); } } }; @@ -2387,11 +2507,16 @@ public: return "MANUAL_CONTROL"; } - uint8_t get_id() + static uint8_t get_id_static() { return MAVLINK_MSG_ID_MANUAL_CONTROL; } + uint8_t get_id() + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamManualControl(mavlink); @@ -2437,7 +2562,7 @@ protected: msg.buttons |= (manual.acro_switch << (shift * 4)); msg.buttons |= (manual.offboard_switch << (shift * 5)); - _mavlink->send_message(MAVLINK_MSG_ID_MANUAL_CONTROL, &msg); + mavlink_msg_manual_control_send_struct(_mavlink->get_channel(), &msg); } } }; @@ -2455,11 +2580,16 @@ public: return "OPTICAL_FLOW_RAD"; } - uint8_t get_id() + static uint8_t get_id_static() { return MAVLINK_MSG_ID_OPTICAL_FLOW_RAD; } + uint8_t get_id() + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamOpticalFlowRad(mavlink); @@ -2505,7 +2635,7 @@ protected: msg.time_delta_distance_us = flow.time_since_last_sonar_update; msg.temperature = flow.gyro_temperature; - _mavlink->send_message(MAVLINK_MSG_ID_OPTICAL_FLOW_RAD, &msg); + mavlink_msg_optical_flow_rad_send_struct(_mavlink->get_channel(), &msg); } } }; @@ -2523,11 +2653,16 @@ public: return "NAMED_VALUE_FLOAT"; } - uint8_t get_id() + static uint8_t get_id_static() { return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; } + uint8_t get_id() + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamNamedValueFloat(mavlink); @@ -2535,7 +2670,7 @@ public: unsigned get_size() { - return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return (_debug_time > 0) ? MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: @@ -2565,11 +2700,83 @@ protected: msg.name[sizeof(msg.name) - 1] = '\0'; msg.value = debug.value; - _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg); + mavlink_msg_named_value_float_send_struct(_mavlink->get_channel(), &msg); } } }; +class MavlinkStreamNavControllerOutput : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamNavControllerOutput::get_name_static(); + } + + static const char *get_name_static() + { + return "NAV_CONTROLLER_OUTPUT"; + } + + static uint8_t get_id_static() + { + return MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; + } + + uint8_t get_id() + { + return get_id_static(); + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamNavControllerOutput(mavlink); + } + + unsigned get_size() + { + return (_fw_pos_ctrl_status_sub->is_published()) ? + MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + MavlinkOrbSubscription *_fw_pos_ctrl_status_sub; + MavlinkOrbSubscription *_tecs_status_sub; + + /* do not allow top copying this class */ + MavlinkStreamNavControllerOutput(MavlinkStreamNavControllerOutput &); + MavlinkStreamNavControllerOutput& operator = (const MavlinkStreamNavControllerOutput &); + +protected: + explicit MavlinkStreamNavControllerOutput(Mavlink *mavlink) : MavlinkStream(mavlink), + _fw_pos_ctrl_status_sub(_mavlink->add_orb_subscription(ORB_ID(fw_pos_ctrl_status))), + _tecs_status_sub(_mavlink->add_orb_subscription(ORB_ID(tecs_status))) + {} + + void send(const hrt_abstime t) + { + struct fw_pos_ctrl_status_s _fw_pos_ctrl_status = {}; + struct tecs_status_s _tecs_status = {}; + + const bool updated_fw_pos_ctrl_status = _fw_pos_ctrl_status_sub->update(&_fw_pos_ctrl_status); + const bool updated_tecs = _tecs_status_sub->update(&_tecs_status); + + if (updated_fw_pos_ctrl_status || updated_tecs) { + mavlink_nav_controller_output_t msg = {}; + + msg.nav_roll = math::degrees(_fw_pos_ctrl_status.nav_roll); + msg.nav_pitch = math::degrees(_fw_pos_ctrl_status.nav_pitch); + msg.nav_bearing = (int16_t)math::degrees(_fw_pos_ctrl_status.nav_bearing); + msg.target_bearing = (int16_t)math::degrees(_fw_pos_ctrl_status.target_bearing); + msg.wp_dist = (uint16_t)_fw_pos_ctrl_status.wp_dist; + msg.xtrack_error = _fw_pos_ctrl_status.xtrack_error; + msg.alt_error = _tecs_status.altitude_filtered - _tecs_status.altitudeSp; + msg.aspd_error = _tecs_status.airspeed_filtered - _tecs_status.airspeedSp; + + mavlink_msg_nav_controller_output_send_struct(_mavlink->get_channel(), &msg); + } + } +}; class MavlinkStreamCameraCapture : public MavlinkStream { @@ -2584,11 +2791,16 @@ public: return "CAMERA_CAPTURE"; } - uint8_t get_id() + static uint8_t get_id_static() { return 0; } + uint8_t get_id() + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamCameraCapture(mavlink); @@ -2631,7 +2843,7 @@ protected: msg.param6 = 0; msg.param7 = 0; - _mavlink->send_message(MAVLINK_MSG_ID_COMMAND_LONG, &msg); + mavlink_msg_command_long_send_struct(_mavlink->get_channel(), &msg); } }; @@ -2648,11 +2860,16 @@ public: return "DISTANCE_SENSOR"; } - uint8_t get_id() + static uint8_t get_id_static() { return MAVLINK_MSG_ID_DISTANCE_SENSOR; } + uint8_t get_id() + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamDistanceSensor(mavlink); @@ -2714,7 +2931,7 @@ protected: msg.current_distance = dist_sensor.current_distance * 100.0f; /* m to cm */ msg.covariance = dist_sensor.covariance; - _mavlink->send_message(MAVLINK_MSG_ID_DISTANCE_SENSOR, &msg); + mavlink_msg_distance_sensor_send_struct(_mavlink->get_channel(), &msg); } } }; @@ -2732,11 +2949,16 @@ public: return "EXTENDED_SYS_STATE"; } - uint8_t get_id() + static uint8_t get_id_static() { return MAVLINK_MSG_ID_EXTENDED_SYS_STATE; } + uint8_t get_id() + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamExtendedSysState(mavlink); @@ -2808,7 +3030,7 @@ protected: } if (updated) { - _mavlink->send_message(MAVLINK_MSG_ID_EXTENDED_SYS_STATE, &_msg); + mavlink_msg_extended_sys_state_send_struct(_mavlink->get_channel(), &_msg); } } }; @@ -2826,11 +3048,16 @@ public: return "ALTITUDE"; } - uint8_t get_id() + static uint8_t get_id_static() { return MAVLINK_MSG_ID_ALTITUDE; } + uint8_t get_id() + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamAltitude(mavlink); @@ -2838,7 +3065,7 @@ public: unsigned get_size() { - return MAVLINK_MSG_ID_ALTITUDE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return (_local_pos_time > 0) ? MAVLINK_MSG_ID_ALTITUDE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: @@ -2889,7 +3116,7 @@ protected: msg.time_usec = hrt_absolute_time(); - msg.altitude_monotonic = (_sensor_time > 0) ? sensor.baro_alt_meter[0] : NAN; + msg.altitude_monotonic = (_sensor_time > 0) ? sensor.baro_alt_meter : NAN; msg.altitude_amsl = (_global_pos_time > 0) ? global_pos.alt : NAN; msg.altitude_local = (_local_pos_time > 0) ? -local_pos.z : NAN; msg.altitude_relative = (_home_time > 0) ? (global_pos.alt - home.alt) : NAN; @@ -2902,51 +3129,137 @@ protected: msg.bottom_clearance = NAN; } - _mavlink->send_message(MAVLINK_MSG_ID_ALTITUDE, &msg); + mavlink_msg_altitude_send_struct(_mavlink->get_channel(), &msg); + } + } +}; + +class MavlinkStreamWind : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamWind::get_name_static(); + } + + static const char *get_name_static() + { + return "WIND_COV"; + } + + static uint8_t get_id_static() + { + return MAVLINK_MSG_ID_WIND_COV; + } + + uint8_t get_id() + { + return get_id_static(); + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamWind(mavlink); + } + + unsigned get_size() + { + return (_wind_estimate_time > 0) ? MAVLINK_MSG_ID_WIND_COV_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + MavlinkOrbSubscription *_wind_estimate_sub; + uint64_t _wind_estimate_time; + + MavlinkOrbSubscription *_global_pos_sub; + uint64_t _global_pos_time; + + /* do not allow top copying this class */ + MavlinkStreamWind(MavlinkStreamWind &); + MavlinkStreamWind& operator = (const MavlinkStreamWind &); + +protected: + explicit MavlinkStreamWind(Mavlink *mavlink) : MavlinkStream(mavlink), + _wind_estimate_sub(_mavlink->add_orb_subscription(ORB_ID(wind_estimate))), + _wind_estimate_time(0), + _global_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))), + _global_pos_time(0) + {} + + void send(const hrt_abstime t) + { + struct wind_estimate_s wind_estimate; + + bool updated = _wind_estimate_sub->update(&_wind_estimate_time, &wind_estimate); + + if (updated) { + + mavlink_wind_cov_t msg; + + msg.time_usec = wind_estimate.timestamp; + + msg.wind_x = wind_estimate.windspeed_north; + msg.wind_y = wind_estimate.windspeed_east; + msg.wind_z = 0.0f; + + msg.var_horiz = wind_estimate.covariance_north + wind_estimate.covariance_east; + msg.var_vert = 0.0f; + + struct vehicle_global_position_s global_pos; + _global_pos_sub->update(&_global_pos_time, &global_pos); + msg.wind_alt = (_global_pos_time > 0) ? global_pos.alt : NAN; + + + msg.horiz_accuracy = 0.0f; + msg.vert_accuracy = 0.0f; + + mavlink_msg_wind_cov_send_struct(_mavlink->get_channel(), &msg); } } }; const StreamListItem *streams_list[] = { - new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static), - new StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static), - new StreamListItem(&MavlinkStreamCommandLong::new_instance, &MavlinkStreamCommandLong::get_name_static), - new StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static), - new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static), - new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static), - new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static), - new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static), - new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static), - new StreamListItem(&MavlinkStreamSystemTime::new_instance, &MavlinkStreamSystemTime::get_name_static), - new StreamListItem(&MavlinkStreamTimesync::new_instance, &MavlinkStreamTimesync::get_name_static), - new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), - new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static), - new StreamListItem(&MavlinkStreamVisionPositionNED::new_instance, &MavlinkStreamVisionPositionNED::get_name_static), - new StreamListItem(&MavlinkStreamLocalPositionNEDCOV::new_instance, &MavlinkStreamLocalPositionNEDCOV::get_name_static), - new StreamListItem(&MavlinkStreamEstimatorStatus::new_instance, &MavlinkStreamEstimatorStatus::get_name_static), - new StreamListItem(&MavlinkStreamAttPosMocap::new_instance, &MavlinkStreamAttPosMocap::get_name_static), - new StreamListItem(&MavlinkStreamHomePosition::new_instance, &MavlinkStreamHomePosition::get_name_static), - new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static), - new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static), - new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static), - new StreamListItem(&MavlinkStreamServoOutputRaw<3>::new_instance, &MavlinkStreamServoOutputRaw<3>::get_name_static), - new StreamListItem(&MavlinkStreamHILControls::new_instance, &MavlinkStreamHILControls::get_name_static), - new StreamListItem(&MavlinkStreamPositionTargetGlobalInt::new_instance, &MavlinkStreamPositionTargetGlobalInt::get_name_static), - new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static), - new StreamListItem(&MavlinkStreamAttitudeTarget::new_instance, &MavlinkStreamAttitudeTarget::get_name_static), - new StreamListItem(&MavlinkStreamRCChannels::new_instance, &MavlinkStreamRCChannels::get_name_static), - new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static), - new StreamListItem(&MavlinkStreamOpticalFlowRad::new_instance, &MavlinkStreamOpticalFlowRad::get_name_static), - new StreamListItem(&MavlinkStreamActuatorControlTarget<0>::new_instance, &MavlinkStreamActuatorControlTarget<0>::get_name_static), - new StreamListItem(&MavlinkStreamActuatorControlTarget<1>::new_instance, &MavlinkStreamActuatorControlTarget<1>::get_name_static), - new StreamListItem(&MavlinkStreamActuatorControlTarget<2>::new_instance, &MavlinkStreamActuatorControlTarget<2>::get_name_static), - new StreamListItem(&MavlinkStreamActuatorControlTarget<3>::new_instance, &MavlinkStreamActuatorControlTarget<3>::get_name_static), - new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static), - new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static), - new StreamListItem(&MavlinkStreamCameraTrigger::new_instance, &MavlinkStreamCameraTrigger::get_name_static), - new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static), - new StreamListItem(&MavlinkStreamExtendedSysState::new_instance, &MavlinkStreamExtendedSysState::get_name_static), - new StreamListItem(&MavlinkStreamAltitude::new_instance, &MavlinkStreamAltitude::get_name_static), - new StreamListItem(&MavlinkStreamADSBVehicle::new_instance, &MavlinkStreamADSBVehicle::get_name_static), + new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static, &MavlinkStreamHeartbeat::get_id_static), + new StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static, &MavlinkStreamStatustext::get_id_static), + new StreamListItem(&MavlinkStreamCommandLong::new_instance, &MavlinkStreamCommandLong::get_name_static, &MavlinkStreamCommandLong::get_id_static), + new StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static, &MavlinkStreamSysStatus::get_id_static), + new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static, &MavlinkStreamHighresIMU::get_id_static), + new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static, &MavlinkStreamAttitude::get_id_static), + new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static, &MavlinkStreamAttitudeQuaternion::get_id_static), + new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static, &MavlinkStreamVFRHUD::get_id_static), + new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static, &MavlinkStreamGPSRawInt::get_id_static), + new StreamListItem(&MavlinkStreamSystemTime::new_instance, &MavlinkStreamSystemTime::get_name_static, &MavlinkStreamSystemTime::get_id_static), + new StreamListItem(&MavlinkStreamTimesync::new_instance, &MavlinkStreamTimesync::get_name_static, &MavlinkStreamTimesync::get_id_static), + new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static, &MavlinkStreamGlobalPositionInt::get_id_static), + new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static, &MavlinkStreamLocalPositionNED::get_id_static), + new StreamListItem(&MavlinkStreamVisionPositionNED::new_instance, &MavlinkStreamVisionPositionNED::get_name_static, &MavlinkStreamVisionPositionNED::get_id_static), + new StreamListItem(&MavlinkStreamLocalPositionNEDCOV::new_instance, &MavlinkStreamLocalPositionNEDCOV::get_name_static, &MavlinkStreamLocalPositionNEDCOV::get_id_static), + new StreamListItem(&MavlinkStreamEstimatorStatus::new_instance, &MavlinkStreamEstimatorStatus::get_name_static, &MavlinkStreamEstimatorStatus::get_id_static), + new StreamListItem(&MavlinkStreamAttPosMocap::new_instance, &MavlinkStreamAttPosMocap::get_name_static, &MavlinkStreamAttPosMocap::get_id_static), + new StreamListItem(&MavlinkStreamHomePosition::new_instance, &MavlinkStreamHomePosition::get_name_static, &MavlinkStreamHomePosition::get_id_static), + new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static, &MavlinkStreamServoOutputRaw<0>::get_id_static), + new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static, &MavlinkStreamServoOutputRaw<1>::get_id_static), + new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static, &MavlinkStreamServoOutputRaw<2>::get_id_static), + new StreamListItem(&MavlinkStreamServoOutputRaw<3>::new_instance, &MavlinkStreamServoOutputRaw<3>::get_name_static, &MavlinkStreamServoOutputRaw<3>::get_id_static), + new StreamListItem(&MavlinkStreamHILControls::new_instance, &MavlinkStreamHILControls::get_name_static, &MavlinkStreamHILControls::get_id_static), + new StreamListItem(&MavlinkStreamPositionTargetGlobalInt::new_instance, &MavlinkStreamPositionTargetGlobalInt::get_name_static, &MavlinkStreamPositionTargetGlobalInt::get_id_static), + new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static, &MavlinkStreamLocalPositionSetpoint::get_id_static), + new StreamListItem(&MavlinkStreamAttitudeTarget::new_instance, &MavlinkStreamAttitudeTarget::get_name_static, &MavlinkStreamAttitudeTarget::get_id_static), + new StreamListItem(&MavlinkStreamRCChannels::new_instance, &MavlinkStreamRCChannels::get_name_static, &MavlinkStreamRCChannels::get_id_static), + new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static, &MavlinkStreamManualControl::get_id_static), + new StreamListItem(&MavlinkStreamOpticalFlowRad::new_instance, &MavlinkStreamOpticalFlowRad::get_name_static, &MavlinkStreamOpticalFlowRad::get_id_static), + new StreamListItem(&MavlinkStreamActuatorControlTarget<0>::new_instance, &MavlinkStreamActuatorControlTarget<0>::get_name_static, &MavlinkStreamActuatorControlTarget<0>::get_id_static), + new StreamListItem(&MavlinkStreamActuatorControlTarget<1>::new_instance, &MavlinkStreamActuatorControlTarget<1>::get_name_static, &MavlinkStreamActuatorControlTarget<1>::get_id_static), + new StreamListItem(&MavlinkStreamActuatorControlTarget<2>::new_instance, &MavlinkStreamActuatorControlTarget<2>::get_name_static, &MavlinkStreamActuatorControlTarget<2>::get_id_static), + new StreamListItem(&MavlinkStreamActuatorControlTarget<3>::new_instance, &MavlinkStreamActuatorControlTarget<3>::get_name_static, &MavlinkStreamActuatorControlTarget<3>::get_id_static), + new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static, &MavlinkStreamNamedValueFloat::get_id_static), + new StreamListItem(&MavlinkStreamNavControllerOutput::new_instance, &MavlinkStreamNavControllerOutput::get_name_static, &MavlinkStreamNavControllerOutput::get_id_static), + new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static, &MavlinkStreamCameraCapture::get_id_static), + new StreamListItem(&MavlinkStreamCameraTrigger::new_instance, &MavlinkStreamCameraTrigger::get_name_static, &MavlinkStreamCameraTrigger::get_id_static), + new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static, &MavlinkStreamDistanceSensor::get_id_static), + new StreamListItem(&MavlinkStreamExtendedSysState::new_instance, &MavlinkStreamExtendedSysState::get_name_static, &MavlinkStreamExtendedSysState::get_id_static), + new StreamListItem(&MavlinkStreamAltitude::new_instance, &MavlinkStreamAltitude::get_name_static, &MavlinkStreamAltitude::get_id_static), + new StreamListItem(&MavlinkStreamADSBVehicle::new_instance, &MavlinkStreamADSBVehicle::get_name_static, &MavlinkStreamADSBVehicle::get_id_static), + new StreamListItem(&MavlinkStreamWind::new_instance, &MavlinkStreamWind::get_name_static, &MavlinkStreamWind::get_id_static), nullptr }; diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h index 5b6b9d6335..3c302891a2 100644 --- a/src/modules/mavlink/mavlink_messages.h +++ b/src/modules/mavlink/mavlink_messages.h @@ -43,15 +43,18 @@ #include "mavlink_stream.h" -class StreamListItem { +class StreamListItem +{ public: - MavlinkStream* (*new_instance)(Mavlink *mavlink); - const char* (*get_name)(); + MavlinkStream *(*new_instance)(Mavlink *mavlink); + const char *(*get_name)(); + uint8_t (*get_id)(); - StreamListItem(MavlinkStream* (*inst)(Mavlink *mavlink), const char* (*name)()) : + StreamListItem(MavlinkStream * (*inst)(Mavlink *mavlink), const char *(*name)(), uint8_t (*id)()) : new_instance(inst), - get_name(name) {}; + get_name(name), + get_id(id) {}; ~StreamListItem() {}; }; diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 929e67a0c1..976076e73e 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2016 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 @@ -122,16 +122,21 @@ MavlinkMissionManager::init_offboard_mission() mission_s mission_state; if (!_dataman_init) { _dataman_init = true; - if (dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) == sizeof(mission_s)) { + int ret = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) == sizeof(mission_s); + if (ret > 0) { _dataman_id = mission_state.dataman_id; _count = mission_state.count; _current_seq = mission_state.current_seq; - } else { + } else if (ret == 0) { + _dataman_id = 0; + _count = 0; + _current_seq = 0; + } else { + PX4_WARN("offboard mission init failed"); _dataman_id = 0; _count = 0; _current_seq = 0; - warnx("offboard mission init: ERROR"); } } _my_dataman_id = _dataman_id; @@ -188,7 +193,7 @@ MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t t wpa.target_component = compid; wpa.type = type; - _mavlink->send_message(MAVLINK_MSG_ID_MISSION_ACK, &wpa); + mavlink_msg_mission_ack_send_struct(_mavlink->get_channel(), &wpa); if (_verbose) { warnx("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); } } @@ -202,7 +207,7 @@ MavlinkMissionManager::send_mission_current(uint16_t seq) wpc.seq = seq; - _mavlink->send_message(MAVLINK_MSG_ID_MISSION_CURRENT, &wpc); + mavlink_msg_mission_current_send_struct(_mavlink->get_channel(), &wpc); } else if (seq == 0 && _count == 0) { /* don't broadcast if no WPs */ @@ -226,7 +231,7 @@ MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_ wpc.target_component = compid; wpc.count = _count; - _mavlink->send_message(MAVLINK_MSG_ID_MISSION_COUNT, &wpc); + mavlink_msg_mission_count_send_struct(_mavlink->get_channel(), &wpc); if (_verbose) { warnx("WPM: Send MISSION_COUNT %u to ID %u", wpc.count, wpc.target_system); } } @@ -250,7 +255,7 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t wp.seq = seq; wp.current = (_current_seq == seq) ? 1 : 0; - _mavlink->send_message(MAVLINK_MSG_ID_MISSION_ITEM, &wp); + mavlink_msg_mission_item_send_struct(_mavlink->get_channel(), &wp); if (_verbose) { warnx("WPM: Send MISSION_ITEM seq %u to ID %u", wp.seq, wp.target_system); } @@ -276,7 +281,7 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1 wpr.target_component = compid; wpr.seq = seq; - _mavlink->send_message(MAVLINK_MSG_ID_MISSION_REQUEST, &wpr); + mavlink_msg_mission_request_send_struct(_mavlink->get_channel(), &wpr); if (_verbose) { warnx("WPM: Send MISSION_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); } @@ -295,7 +300,7 @@ MavlinkMissionManager::send_mission_item_reached(uint16_t seq) wp_reached.seq = seq; - _mavlink->send_message(MAVLINK_MSG_ID_MISSION_ITEM_REACHED, &wp_reached); + mavlink_msg_mission_item_reached_send_struct(_mavlink->get_channel(), &wp_reached); if (_verbose) { warnx("WPM: Send MISSION_ITEM_REACHED reached_seq %u", wp_reached.seq); } } @@ -341,6 +346,9 @@ MavlinkMissionManager::send(const hrt_abstime now) _state = MAVLINK_WPM_STATE_IDLE; + // since we are giving up, reset this state also, so another request can be started. + _transfer_in_progress = false; + } else if (_state == MAVLINK_WPM_STATE_GETLIST && hrt_elapsed_time(&_time_last_sent) > _retry_timeout) { /* try to request item again after timeout */ send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq); @@ -579,7 +587,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) if (CHECK_SYSID_COMPID_MISSION(wpc)) { if (_state == MAVLINK_WPM_STATE_IDLE) { _time_last_recv = hrt_absolute_time(); - + if(_transfer_in_progress) { send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); @@ -762,24 +770,83 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) int MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item) { - /* only support global waypoints for now */ - switch (mavlink_mission_item->frame) { - case MAV_FRAME_GLOBAL: + if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL || + mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT) { + + // only support global waypoints for now + mission_item->lat = (double)mavlink_mission_item->x; mission_item->lon = (double)mavlink_mission_item->y; mission_item->altitude = mavlink_mission_item->z; - mission_item->altitude_is_relative = false; - break; - case MAV_FRAME_GLOBAL_RELATIVE_ALT: - mission_item->lat = (double)mavlink_mission_item->x; - mission_item->lon = (double)mavlink_mission_item->y; - mission_item->altitude = mavlink_mission_item->z; - mission_item->altitude_is_relative = true; - break; + if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL) { + mission_item->altitude_is_relative = false; + } else if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT) { + mission_item->altitude_is_relative = true; + } + + mission_item->time_inside = 0.0f; + + switch (mavlink_mission_item->command) { + case MAV_CMD_NAV_WAYPOINT: + mission_item->nav_cmd = NAV_CMD_WAYPOINT; + mission_item->time_inside = mavlink_mission_item->param1; + mission_item->acceptance_radius = mavlink_mission_item->param2; + mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F); + break; + + case MAV_CMD_NAV_LOITER_UNLIM: + mission_item->nav_cmd = NAV_CMD_LOITER_UNLIMITED; + mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); + mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ + mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F); + break; + + case MAV_CMD_NAV_LOITER_TIME: + mission_item->nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + mission_item->time_inside = mavlink_mission_item->param1; + mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); + mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ + mission_item->loiter_exit_xtrack = (mavlink_mission_item->param4 > 0) ? true : false; + break; + + case MAV_CMD_NAV_LAND: + mission_item->nav_cmd = NAV_CMD_LAND; + // TODO: abort alt param1 + mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F); + break; + + case MAV_CMD_NAV_TAKEOFF: + mission_item->nav_cmd = NAV_CMD_TAKEOFF; + mission_item->pitch_min = mavlink_mission_item->param1; + mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F); + break; + + case MAV_CMD_NAV_LOITER_TO_ALT: + mission_item->nav_cmd = NAV_CMD_LOITER_TO_ALT; + mission_item->force_heading = (mavlink_mission_item->param1 > 0) ? true : false; + mission_item->loiter_radius = fabsf(mavlink_mission_item->param2); + mission_item->loiter_direction = (mavlink_mission_item->param2 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ + mission_item->loiter_exit_xtrack = (mavlink_mission_item->param4 > 0) ? true : false; + break; + + case MAV_CMD_NAV_VTOL_TAKEOFF: + case MAV_CMD_NAV_VTOL_LAND: + mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; + mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F); + break; + + default: + mission_item->nav_cmd = NAV_CMD_INVALID; + return MAV_MISSION_UNSUPPORTED; + } + + mission_item->frame = mavlink_mission_item->frame; + + } else if (mavlink_mission_item->frame == MAV_FRAME_MISSION) { + + // this is a mission item with no coordinates - case MAV_FRAME_MISSION: - // This is a mission item with no coordinate mission_item->params[0] = mavlink_mission_item->param1; mission_item->params[1] = mavlink_mission_item->param2; mission_item->params[2] = mavlink_mission_item->param3; @@ -787,50 +854,44 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * mission_item->params[4] = mavlink_mission_item->x; mission_item->params[5] = mavlink_mission_item->y; mission_item->params[6] = mavlink_mission_item->z; - break; - - default: - return MAV_MISSION_UNSUPPORTED_FRAME; - } - mission_item->frame = mavlink_mission_item->frame; + switch (mavlink_mission_item->command) { + case MAV_CMD_DO_JUMP: + mission_item->nav_cmd = NAV_CMD_DO_JUMP; + mission_item->do_jump_mission_index = mavlink_mission_item->param1; + mission_item->do_jump_current_count = 0; + mission_item->do_jump_repeat_count = mavlink_mission_item->param2; + break; - switch (mavlink_mission_item->command) { - case MAV_CMD_NAV_TAKEOFF: - mission_item->pitch_min = mavlink_mission_item->param1; - break; + case MAV_CMD_DO_CHANGE_SPEED: + case MAV_CMD_DO_SET_SERVO: + case MAV_CMD_DO_DIGICAM_CONTROL: + case MAV_CMD_DO_MOUNT_CONFIGURE: + case MAV_CMD_DO_MOUNT_CONTROL: + case MAV_CMD_DO_SET_CAM_TRIGG_DIST: + case MAV_CMD_DO_VTOL_TRANSITION: + mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; + break; - case MAV_CMD_DO_JUMP: - mission_item->do_jump_mission_index = mavlink_mission_item->param1; - mission_item->do_jump_current_count = 0; - mission_item->do_jump_repeat_count = mavlink_mission_item->param2; - break; + default: + mission_item->nav_cmd = NAV_CMD_INVALID; + return MAV_MISSION_UNSUPPORTED; + } - default: - mission_item->acceptance_radius = mavlink_mission_item->param2; - mission_item->time_inside = mavlink_mission_item->param1; - break; - } - - mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F); - mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); - mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ - - /* unsigned, so can't be negative, only check for overflow */ - if (mavlink_mission_item->command >= NAV_CMD_INVALID) { - mission_item->nav_cmd = NAV_CMD_INVALID; + mission_item->frame = MAV_FRAME_MISSION; } else { - mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; + return MAV_MISSION_UNSUPPORTED_FRAME; } mission_item->autocontinue = mavlink_mission_item->autocontinue; // mission_item->index = mavlink_mission_item->seq; - mission_item->origin = ORIGIN_MAVLINK; /* reset DO_JUMP count */ mission_item->do_jump_current_count = 0; + mission_item->origin = ORIGIN_MAVLINK; + return MAV_MISSION_ACCEPTED; } @@ -852,32 +913,82 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * mavlink_mission_item->y = mission_item->params[5]; mavlink_mission_item->z = mission_item->params[6]; - // default mappings for regular waypoints + switch (mavlink_mission_item->command) { + case NAV_CMD_DO_JUMP: + mavlink_mission_item->param1 = mission_item->do_jump_mission_index; + mavlink_mission_item->param2 = mission_item->do_jump_repeat_count; + break; + + case NAV_CMD_DO_CHANGE_SPEED: + case NAV_CMD_DO_SET_SERVO: + case NAV_CMD_DO_DIGICAM_CONTROL: + case NAV_CMD_DO_MOUNT_CONFIGURE: + case NAV_CMD_DO_MOUNT_CONTROL: + case NAV_CMD_DO_SET_CAM_TRIGG_DIST: + case NAV_CMD_DO_VTOL_TRANSITION: + break; + + default: + return ERROR; + } + } else { + mavlink_mission_item->param1 = 0.0f; + mavlink_mission_item->param2 = 0.0f; + mavlink_mission_item->param3 = 0.0f; + mavlink_mission_item->param4 = 0.0f; mavlink_mission_item->x = (float)mission_item->lat; mavlink_mission_item->y = (float)mission_item->lon; mavlink_mission_item->z = mission_item->altitude; - mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F; - mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction; - } + if (mission_item->altitude_is_relative) { + mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; + } else { + mavlink_mission_item->frame = MAV_FRAME_GLOBAL; + } - // specific item handling - switch (mission_item->nav_cmd) { - case NAV_CMD_TAKEOFF: - case NAV_CMD_VTOL_TAKEOFF: - mavlink_mission_item->param1 = mission_item->pitch_min; - break; + switch (mission_item->nav_cmd) { + case NAV_CMD_WAYPOINT: + mavlink_mission_item->param1 = mission_item->time_inside; + mavlink_mission_item->param2 = mission_item->acceptance_radius; + mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F; + break; - case NAV_CMD_DO_JUMP: - mavlink_mission_item->param1 = mission_item->do_jump_mission_index; - mavlink_mission_item->param2 = mission_item->do_jump_repeat_count - mission_item->do_jump_current_count; - break; + case NAV_CMD_LOITER_UNLIMITED: + mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction; + mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F; + break; - default: - mavlink_mission_item->param2 = mission_item->acceptance_radius; - mavlink_mission_item->param1 = mission_item->time_inside; - break; + case NAV_CMD_LOITER_TIME_LIMIT: + mavlink_mission_item->param1 = mission_item->time_inside; + mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction; + mavlink_mission_item->param4 = mission_item->loiter_exit_xtrack; + break; + + case NAV_CMD_LAND: + // TODO: param1 abort alt + mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F; + break; + + case NAV_CMD_TAKEOFF: + mavlink_mission_item->param1 = mission_item->pitch_min; + mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F; + break; + + case NAV_CMD_LOITER_TO_ALT: + mavlink_mission_item->param1 = mission_item->force_heading; + mavlink_mission_item->param2 = mission_item->loiter_radius * (float)mission_item->loiter_direction; + mavlink_mission_item->param4 = mission_item->loiter_exit_xtrack; + break; + + case MAV_CMD_NAV_VTOL_TAKEOFF: + case MAV_CMD_NAV_VTOL_LAND: + mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F; + break; + + default: + return ERROR; + } } return OK; diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 631461a454..87cf8624d3 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -58,7 +58,7 @@ MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic, int instanc MavlinkOrbSubscription::~MavlinkOrbSubscription() { - close(_fd); + orb_unsubscribe(_fd); } orb_id_t @@ -74,7 +74,7 @@ MavlinkOrbSubscription::get_instance() const } bool -MavlinkOrbSubscription::update(uint64_t *time, void* data) +MavlinkOrbSubscription::update(uint64_t *time, void *data) { // TODO this is NOT atomic operation, we can get data newer than time // if topic was published between orb_stat and orb_copy calls. @@ -106,11 +106,34 @@ MavlinkOrbSubscription::update(uint64_t *time, void* data) } bool -MavlinkOrbSubscription::update(void* data) +MavlinkOrbSubscription::update(void *data) { return !orb_copy(_topic, _fd, data); } +bool +MavlinkOrbSubscription::update_if_changed(void *data) +{ + bool updated; + if (orb_check(_fd, &updated)) { + return false; + } + + if (!updated) { + return false; + } + + if (orb_copy(_topic, _fd, data)) { + if (data != nullptr) { + /* error copying topic data */ + memset(data, 0, _topic->o_size); + } + return false; + } + + return true; +} + bool MavlinkOrbSubscription::is_published() { diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index 5394e5097b..913f19ec97 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -54,26 +54,37 @@ public: ~MavlinkOrbSubscription(); /** - * Check if subscription updated and get data. + * Check if subscription updated based on timestamp. * - * @return true only if topic was updated and data copied to buffer successfully. - * If topic was not updated since last check it will return false but still copy the data. - * If no data available data buffer will be filled with zeroes. + * @return true only if topic was updated based on a timestamp and + * copied to buffer successfully. + * If topic was not updated since last check it will return false but + * still copy the data. + * If no data available data buffer will be filled with zeros. */ - bool update(uint64_t *time, void* data); + bool update(uint64_t *time, void *data); /** * Copy topic data to given buffer. * * @return true only if topic data copied successfully. */ - bool update(void* data); + bool update(void *data); + + /** + * Check if the subscription has been updated. + * + * @return true if there has been an update which has been + * copied successfully. + */ + bool update_if_changed(void *data); /** * Check if the topic has been published. * * This call will return true if the topic was ever published. * @return true if the topic has been published at least once. + * If no data is available the buffer will be filled with zeros. */ bool is_published(); orb_id_t get_topic() const; diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index b24d0464fb..b846daf1e0 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -47,8 +47,6 @@ #include "mavlink_parameters.h" #include "mavlink_main.h" -ORB_DEFINE(uavcan_parameter_request, struct uavcan_parameter_request_s); -ORB_DEFINE(uavcan_parameter_value, struct uavcan_parameter_value_s); #define HASH_PARAM "_HASH_CHECK" MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -59,6 +57,15 @@ MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkSt _uavcan_parameter_value_sub(-1) { } +MavlinkParametersManager::~MavlinkParametersManager() +{ + if (_uavcan_parameter_value_sub >= 0) { + orb_unsubscribe(_uavcan_parameter_value_sub); + } + if (_uavcan_parameter_request_pub) { + orb_unadvertise(_uavcan_parameter_request_pub); + } +} unsigned MavlinkParametersManager::get_size() @@ -66,6 +73,12 @@ MavlinkParametersManager::get_size() return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } +unsigned +MavlinkParametersManager::get_size_avg() +{ + return 0; +} + void MavlinkParametersManager::handle_message(const mavlink_message_t *msg) { @@ -188,7 +201,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) strncpy(param_value.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); param_value.param_type = MAV_PARAM_TYPE_UINT32; memcpy(¶m_value.param_value, &hash, sizeof(hash)); - _mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, ¶m_value); + mavlink_msg_param_value_send_struct(_mavlink->get_channel(), ¶m_value); } else { /* local name buffer to enforce null-terminated string */ char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; @@ -304,7 +317,7 @@ MavlinkParametersManager::send(const hrt_abstime t) memcpy(&msg.param_value, &val, sizeof(int32_t)); msg.param_type = MAVLINK_TYPE_INT32_T; } - _mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg, value.node_id); + mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &msg); } else if (_send_all_index >= 0 && _mavlink->boot_complete()) { /* send all parameters if requested, but only after the system has booted */ @@ -327,7 +340,7 @@ MavlinkParametersManager::send(const hrt_abstime t) strncpy(msg.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); msg.param_type = MAV_PARAM_TYPE_UINT32; memcpy(&msg.param_value, &hash, sizeof(hash)); - _mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg); + mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &msg); /* after this we should start sending all params */ _send_all_index = 0; @@ -398,7 +411,7 @@ MavlinkParametersManager::send_param(param_t param) msg.param_type = MAVLINK_TYPE_FLOAT; } - _mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg); + mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &msg); return 0; } diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h index d258f3b240..1ef29411c7 100644 --- a/src/modules/mavlink/mavlink_parameters.h +++ b/src/modules/mavlink/mavlink_parameters.h @@ -73,35 +73,10 @@ public: unsigned get_size(); + unsigned get_size_avg(); + void handle_message(const mavlink_message_t *msg); - /** - * Send one parameter identified by index. - * - * @param index The index of the parameter to send. - * @return zero on success, nonzero else. - */ - void start_send_one(int index); - - - /** - * Send one parameter identified by name. - * - * @param name The index of the parameter to send. - * @return zero on success, nonzero else. - */ - int start_send_for_name(const char *name); - - /** - * Start sending the parameter queue. - * - * This function will not directly send parameters, but instead - * activate the sending of one parameter on each call of - * mavlink_pm_queued_send(). - * @see mavlink_pm_queued_send() - */ - void start_send_all(); - private: int _send_all_index; @@ -111,6 +86,7 @@ private: protected: explicit MavlinkParametersManager(Mavlink *mavlink); + ~MavlinkParametersManager(); void send(const hrt_abstime t); diff --git a/src/modules/mavlink/mavlink_params.c b/src/modules/mavlink/mavlink_params.c index ad7e9712cf..c988b9aa0f 100644 --- a/src/modules/mavlink/mavlink_params.c +++ b/src/modules/mavlink/mavlink_params.c @@ -47,6 +47,15 @@ PARAM_DEFINE_INT32(MAV_SYS_ID, 1); */ PARAM_DEFINE_INT32(MAV_COMP_ID, 1); +/** + * MAVLink protocol version + * @group MAVLink + * @value 0 Default to 1, switch to 2 if GCS sends version 2 + * @value 1 Always use version 1 + * @value 2 Always use version 2 + */ +PARAM_DEFINE_INT32(MAV_PROTO_VER, 1); + /** * MAVLink Radio ID * @@ -90,6 +99,18 @@ PARAM_DEFINE_INT32(MAV_USEHILGPS, 0); */ PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1); +/** + * Broadcast heartbeats on local network + * + * This allows a ground control station to automatically find the drone + * on the local network. + * + * @value 0 Never broadcast + * @value 1 Always broadcast + * @group MAVLink + */ +PARAM_DEFINE_INT32(MAV_BROADCAST, 0); + /** * Test parameter * diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 48a08c312f..d0a97ab1bd 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -150,6 +150,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : MavlinkReceiver::~MavlinkReceiver() { + orb_unsubscribe(_control_mode_sub); } void @@ -166,12 +167,14 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) if (_mavlink->accepting_commands()) { handle_message_command_long(msg); } + break; case MAVLINK_MSG_ID_COMMAND_INT: if (_mavlink->accepting_commands()) { handle_message_command_int(msg); } + break; case MAVLINK_MSG_ID_OPTICAL_FLOW_RAD: @@ -186,6 +189,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) if (_mavlink->accepting_commands()) { handle_message_set_mode(msg); } + break; case MAVLINK_MSG_ID_ATT_POS_MOCAP: @@ -228,6 +232,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) if (_mavlink->accepting_commands()) { handle_message_request_data_stream(msg); } + break; case MAVLINK_MSG_ID_SYSTEM_TIME: @@ -250,8 +255,16 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_adsb_vehicle(msg); break; - case MAVLINK_MSG_ID_GPS_INJECT_DATA: - handle_message_gps_inject_data(msg); + case MAVLINK_MSG_ID_GPS_RTCM_DATA: + handle_message_gps_rtcm_data(msg); + break; + + case MAVLINK_MSG_ID_BATTERY_STATUS: + handle_message_battery_status(msg); + break; + + case MAVLINK_MSG_ID_SERIAL_CONTROL: + handle_message_serial_control(msg); break; default: @@ -354,6 +367,12 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) } else if (cmd_mavlink.command == MAV_CMD_GET_HOME_POSITION) { _mavlink->configure_stream_threadsafe("HOME_POSITION", 0.5f); + } else if (cmd_mavlink.command == MAV_CMD_SET_MESSAGE_INTERVAL) { + set_message_interval((int)(cmd_mavlink.param1 + 0.5f), cmd_mavlink.param2, cmd_mavlink.param3); + + } else if (cmd_mavlink.command == MAV_CMD_GET_MESSAGE_INTERVAL) { + get_message_interval((int)cmd_mavlink.param1); + } else { if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) { @@ -490,8 +509,7 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) enum Rotation flow_rot; param_get(param_find("SENS_FLOW_ROT"), &flow_rot); - struct optical_flow_s f; - memset(&f, 0, sizeof(f)); + struct optical_flow_s f = {}; f.timestamp = flow.time_usec; f.integration_timespan = flow.integration_time_us; @@ -533,7 +551,7 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) if (_flow_distance_sensor_pub == nullptr) { _flow_distance_sensor_pub = orb_advertise_multi(ORB_ID(distance_sensor), &d, - &_orb_class_instance, ORB_PRIO_HIGH); + &_orb_class_instance, ORB_PRIO_HIGH); } else { orb_publish(ORB_ID(distance_sensor), _flow_distance_sensor_pub, &d); @@ -663,14 +681,13 @@ MavlinkReceiver::handle_message_att_pos_mocap(mavlink_message_t *msg) mavlink_att_pos_mocap_t mocap; mavlink_msg_att_pos_mocap_decode(msg, &mocap); - struct att_pos_mocap_s att_pos_mocap; - memset(&att_pos_mocap, 0, sizeof(att_pos_mocap)); + struct att_pos_mocap_s att_pos_mocap = {}; // Use the component ID to identify the mocap system att_pos_mocap.id = msg->compid; - att_pos_mocap.timestamp_boot = hrt_absolute_time(); // Monotonic time - att_pos_mocap.timestamp_computer = sync_stamp(mocap.time_usec); // Synced time + att_pos_mocap.timestamp = sync_stamp(mocap.time_usec); + att_pos_mocap.timestamp_received = hrt_absolute_time(); att_pos_mocap.q[0] = mocap.q[0]; att_pos_mocap.q[1] = mocap.q[1]; @@ -943,14 +960,13 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) mavlink_vision_position_estimate_t pos; mavlink_msg_vision_position_estimate_decode(msg, &pos); - struct vision_position_estimate_s vision_position; - memset(&vision_position, 0, sizeof(vision_position)); + struct vision_position_estimate_s vision_position = {}; // Use the component ID to identify the vision sensor vision_position.id = msg->compid; - vision_position.timestamp_boot = hrt_absolute_time(); // Monotonic time - vision_position.timestamp_computer = sync_stamp(pos.usec); // Synced time + vision_position.timestamp = sync_stamp(pos.usec); + vision_position.timestamp_received = hrt_absolute_time(); vision_position.x = pos.x; vision_position.y = pos.y; vision_position.z = pos.z; @@ -968,6 +984,10 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) vision_position.q[2] = q(2); vision_position.q[3] = q(3); + // TODO fix this + vision_position.pos_err = 0.0f; + vision_position.ang_err = 0.0f; + if (_vision_position_pub == nullptr) { _vision_position_pub = orb_advertise(ORB_ID(vision_position_estimate), &vision_position); @@ -1110,7 +1130,7 @@ void MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) { /* telemetry status supported only on first ORB_MULTI_MAX_INSTANCES mavlink channels */ - if (_mavlink->get_channel() < ORB_MULTI_MAX_INSTANCES) { + if (_mavlink->get_channel() < (mavlink_channel_t)ORB_MULTI_MAX_INSTANCES) { mavlink_radio_status_t rstatus; mavlink_msg_radio_status_decode(msg, &rstatus); @@ -1140,6 +1160,67 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) } } +void +MavlinkReceiver::handle_message_battery_status(mavlink_message_t *msg) +{ + // external battery measurements + mavlink_battery_status_t battery_mavlink; + mavlink_msg_battery_status_decode(msg, &battery_mavlink); + + battery_status_s battery_status = {}; + battery_status.timestamp = hrt_absolute_time(); + + float voltage_sum = 0.0f; + uint8_t cell_count = 0; + + while (battery_mavlink.voltages[cell_count] < UINT16_MAX && cell_count < 10) { + voltage_sum += (float)(battery_mavlink.voltages[cell_count]) / 1000.0f; + cell_count++; + } + + battery_status.voltage_v = voltage_sum; + battery_status.voltage_filtered_v = voltage_sum; + battery_status.current_a = battery_status.current_filtered_a = (float)(battery_mavlink.current_battery) / 100.0f; + battery_status.current_filtered_a = battery_status.current_a; + battery_status.remaining = (float)battery_mavlink.battery_remaining / 100.0f; + battery_status.discharged_mah = (float)battery_mavlink.current_consumed; + battery_status.cell_count = cell_count; + battery_status.connected = true; + + if (_battery_pub == nullptr) { + _battery_pub = orb_advertise(ORB_ID(battery_status), &battery_status); + + } else { + orb_publish(ORB_ID(battery_status), _battery_pub, &battery_status); + } +} + +void +MavlinkReceiver::handle_message_serial_control(mavlink_message_t *msg) +{ + mavlink_serial_control_t serial_control_mavlink; + mavlink_msg_serial_control_decode(msg, &serial_control_mavlink); + + // we only support shell commands + if (serial_control_mavlink.device != SERIAL_CONTROL_DEV_SHELL + || (serial_control_mavlink.flags & SERIAL_CONTROL_FLAG_REPLY)) { + return; + } + + MavlinkShell* shell = _mavlink->get_shell(); + if (shell) { + // we ignore the timeout, EXCLUSIVE & BLOCKING flags of the SERIAL_CONTROL message + if (serial_control_mavlink.count > 0) { + shell->write(serial_control_mavlink.data, serial_control_mavlink.count); + } + + // if no response requested, assume the shell is no longer used + if ((serial_control_mavlink.flags & SERIAL_CONTROL_FLAG_RESPOND) == 0) { + _mavlink->close_shell(); + } + } +} + switch_pos_t MavlinkReceiver::decode_switch_pos(uint16_t buttons, unsigned sw) { @@ -1320,7 +1401,7 @@ void MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) { /* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */ - if (_mavlink->get_channel() < ORB_MULTI_MAX_INSTANCES) { + if (_mavlink->get_channel() < (mavlink_channel_t)ORB_MULTI_MAX_INSTANCES) { mavlink_heartbeat_t hb; mavlink_msg_heartbeat_decode(msg, &hb); @@ -1354,31 +1435,69 @@ MavlinkReceiver::handle_message_ping(mavlink_message_t *msg) if ((mavlink_system.sysid == ping.target_system) && (mavlink_system.compid == ping.target_component)) { - mavlink_message_t msg_out; - mavlink_msg_ping_encode(_mavlink->get_system_id(), _mavlink->get_component_id(), &msg_out, &ping); - _mavlink->send_message(MAVLINK_MSG_ID_PING, &msg_out); + mavlink_msg_ping_send_struct(_mavlink->get_channel(), &ping); } } void MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg) { - mavlink_request_data_stream_t req; - mavlink_msg_request_data_stream_decode(msg, &req); + // REQUEST_DATA_STREAM is deprecated, please use SET_MESSAGE_INTERVAL instead +} - if (req.target_system == mavlink_system.sysid && req.target_component == mavlink_system.compid - && req.req_message_rate != 0) { - float rate = req.start_stop ? (1000.0f / req.req_message_rate) : 0.0f; +void +MavlinkReceiver::set_message_interval(int msgId, float interval, int data_rate) +{ + if (data_rate > 0) { + _mavlink->set_data_rate(data_rate); + } - MavlinkStream *stream; - LL_FOREACH(_mavlink->get_streams(), stream) { - if (req.req_stream_id == stream->get_id()) { - _mavlink->configure_stream_threadsafe(stream->get_name(), rate); + // configure_stream wants a rate (msgs/second), so convert here. + float rate = 0; + + if (interval < 0) { + // stop the stream. + rate = 0; + + } else if (interval > 0) { + rate = 1000000.0f / interval; + + } else { + // note: mavlink spec says rate == 0 is requesting a default rate but our streams + // don't publish a default rate so for now let's pick a default rate of zero. + } + + // The interval between two messages is in microseconds. + // Set to -1 to disable and 0 to request default rate + if (msgId != 0) { + for (unsigned int i = 0; streams_list[i] != nullptr; i++) { + const StreamListItem *item = streams_list[i]; + + if (msgId == item->get_id()) { + _mavlink->configure_stream_threadsafe(item->get_name(), rate); + break; } } } } +void +MavlinkReceiver::get_message_interval(int msgId) +{ + unsigned interval = 0; + + MavlinkStream *stream; + LL_FOREACH(_mavlink->get_streams(), stream) { + if (stream->get_id() == msgId) { + interval = stream->get_interval(); + break; + } + } + + // send back this value... + mavlink_msg_message_interval_send(_mavlink->get_channel(), msgId, interval); +} + void MavlinkReceiver::handle_message_system_time(mavlink_message_t *msg) { @@ -1424,7 +1543,7 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) rsync.tc1 = now_ns; rsync.ts1 = tsync.ts1; - _mavlink->send_message(MAVLINK_MSG_ID_TIMESYNC, &rsync); + mavlink_msg_timesync_send_struct(_mavlink->get_channel(), &rsync); return; @@ -1582,62 +1701,32 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) { struct sensor_combined_s hil_sensors = {}; + hil_sensors.gyro_rad[0] = 0.5f * (imu.xgyro + _hil_prev_gyro[0]); + hil_sensors.gyro_rad[1] = 0.5f * (imu.ygyro + _hil_prev_gyro[1]); + hil_sensors.gyro_rad[2] = 0.5f * (imu.zgyro + _hil_prev_gyro[2]); + _hil_prev_gyro[0] = imu.xgyro; + _hil_prev_gyro[1] = imu.ygyro; + _hil_prev_gyro[2] = imu.zgyro; + hil_sensors.gyro_integral_dt = dt; hil_sensors.timestamp = timestamp; - hil_sensors.gyro_raw[0] = imu.xgyro * 1000.0f; - hil_sensors.gyro_raw[1] = imu.ygyro * 1000.0f; - hil_sensors.gyro_raw[2] = imu.zgyro * 1000.0f; - hil_sensors.gyro_rad_s[0] = imu.xgyro; - hil_sensors.gyro_rad_s[1] = imu.ygyro; - hil_sensors.gyro_rad_s[2] = imu.zgyro; - hil_sensors.gyro_integral_rad[0] = 0.5f * (imu.xgyro + _hil_prev_gyro[0]) * dt; - hil_sensors.gyro_integral_rad[1] = 0.5f * (imu.ygyro + _hil_prev_gyro[1]) * dt; - hil_sensors.gyro_integral_rad[2] = 0.5f * (imu.zgyro + _hil_prev_gyro[2]) * dt; - memcpy(&_hil_prev_gyro[0], &hil_sensors.gyro_rad_s[0], sizeof(_hil_prev_gyro)); - hil_sensors.gyro_integral_dt[0] = dt * 1e6f; - hil_sensors.gyro_timestamp[0] = timestamp; - hil_sensors.gyro_priority[0] = ORB_PRIO_HIGH; + hil_sensors.accelerometer_m_s2[0] = 0.5f * (imu.xacc + _hil_prev_accel[0]); + hil_sensors.accelerometer_m_s2[1] = 0.5f * (imu.yacc + _hil_prev_accel[1]); + hil_sensors.accelerometer_m_s2[2] = 0.5f * (imu.zacc + _hil_prev_accel[2]); + _hil_prev_accel[0] = imu.xacc; + _hil_prev_accel[1] = imu.yacc; + _hil_prev_accel[2] = imu.zacc; + hil_sensors.accelerometer_integral_dt = dt; + hil_sensors.accelerometer_timestamp_relative = 0; - hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2; - hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2; - hil_sensors.accelerometer_raw[2] = imu.zacc / mg2ms2; - hil_sensors.accelerometer_m_s2[0] = imu.xacc; - hil_sensors.accelerometer_m_s2[1] = imu.yacc; - hil_sensors.accelerometer_m_s2[2] = imu.zacc; - hil_sensors.accelerometer_integral_m_s[0] = 0.5f * (imu.xacc + _hil_prev_accel[0]) * dt; - hil_sensors.accelerometer_integral_m_s[1] = 0.5f * (imu.yacc + _hil_prev_accel[1]) * dt; - hil_sensors.accelerometer_integral_m_s[2] = 0.5f * (imu.zacc + _hil_prev_accel[2]) * dt; - memcpy(&_hil_prev_accel[0], &hil_sensors.accelerometer_m_s2[0], sizeof(_hil_prev_accel)); - hil_sensors.accelerometer_integral_dt[0] = dt * 1e6f; - hil_sensors.accelerometer_mode[0] = 0; // TODO what is this? - hil_sensors.accelerometer_range_m_s2[0] = 32.7f; // int16 - hil_sensors.accelerometer_timestamp[0] = timestamp; - hil_sensors.accelerometer_priority[0] = ORB_PRIO_HIGH; - - hil_sensors.adc_voltage_v[0] = 0.0f; - hil_sensors.adc_voltage_v[1] = 0.0f; - hil_sensors.adc_voltage_v[2] = 0.0f; - - hil_sensors.magnetometer_raw[0] = imu.xmag * 1000.0f; - hil_sensors.magnetometer_raw[1] = imu.ymag * 1000.0f; - hil_sensors.magnetometer_raw[2] = imu.zmag * 1000.0f; hil_sensors.magnetometer_ga[0] = imu.xmag; hil_sensors.magnetometer_ga[1] = imu.ymag; hil_sensors.magnetometer_ga[2] = imu.zmag; - hil_sensors.magnetometer_range_ga[0] = 32.7f; // int16 - hil_sensors.magnetometer_mode[0] = 0; // TODO what is this - hil_sensors.magnetometer_cuttoff_freq_hz[0] = 50.0f; - hil_sensors.magnetometer_timestamp[0] = timestamp; - hil_sensors.magnetometer_priority[0] = ORB_PRIO_HIGH; + hil_sensors.magnetometer_timestamp_relative = 0; - hil_sensors.baro_pres_mbar[0] = imu.abs_pressure; - hil_sensors.baro_alt_meter[0] = imu.pressure_alt; - hil_sensors.baro_temp_celcius[0] = imu.temperature; - hil_sensors.baro_timestamp[0] = timestamp; - - hil_sensors.differential_pressure_pa[0] = imu.diff_pressure * 1e2f; //from hPa to Pa - hil_sensors.differential_pressure_filtered_pa[0] = hil_sensors.differential_pressure_pa[0]; - hil_sensors.differential_pressure_timestamp[0] = timestamp; + hil_sensors.baro_alt_meter = imu.pressure_alt; + hil_sensors.baro_temp_celcius = imu.temperature; + hil_sensors.baro_timestamp_relative = 0; /* publish combined sensor topic */ if (_sensors_pub == nullptr) { @@ -1688,20 +1777,18 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) struct vehicle_gps_position_s hil_gps; memset(&hil_gps, 0, sizeof(hil_gps)); - hil_gps.timestamp_time = timestamp; + hil_gps.timestamp_time_relative = 0; hil_gps.time_utc_usec = gps.time_usec; - hil_gps.timestamp_position = timestamp; + hil_gps.timestamp = timestamp; hil_gps.lat = gps.lat; hil_gps.lon = gps.lon; hil_gps.alt = gps.alt; hil_gps.eph = (float)gps.eph * 1e-2f; // from cm to m hil_gps.epv = (float)gps.epv * 1e-2f; // from cm to m - hil_gps.timestamp_variance = timestamp; - hil_gps.s_variance_m_s = 5.0f; + hil_gps.s_variance_m_s = 1.0f; - hil_gps.timestamp_velocity = timestamp; hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m hil_gps.vel_e_m_s = gps.ve * 1e-2f; // from cm to m @@ -1774,16 +1861,17 @@ void MavlinkReceiver::handle_message_adsb_vehicle(mavlink_message_t *msg) } } -void MavlinkReceiver::handle_message_gps_inject_data(mavlink_message_t *msg) +void MavlinkReceiver::handle_message_gps_rtcm_data(mavlink_message_t *msg) { - mavlink_gps_inject_data_t gps_inject_data_msg; + mavlink_gps_rtcm_data_t gps_rtcm_data_msg; gps_inject_data_s gps_inject_data_topic; - mavlink_msg_gps_inject_data_decode(msg, &gps_inject_data_msg); + mavlink_msg_gps_rtcm_data_decode(msg, &gps_rtcm_data_msg); - gps_inject_data_topic.len = gps_inject_data_msg.len; - memcpy(gps_inject_data_topic.data, gps_inject_data_msg.data, - math::min((int)sizeof(gps_inject_data_topic.data), (int)sizeof(uint8_t) * gps_inject_data_msg.len)); + gps_inject_data_topic.len = gps_rtcm_data_msg.len; + gps_inject_data_topic.flags = gps_rtcm_data_msg.flags; + memcpy(gps_inject_data_topic.data, gps_rtcm_data_msg.data, + math::min((int)sizeof(gps_inject_data_topic.data), (int)sizeof(uint8_t) * gps_rtcm_data_msg.len)); orb_advert_t &pub = _gps_inject_data_pub[_gps_inject_data_next_idx]; @@ -2076,7 +2164,7 @@ MavlinkReceiver::receive_thread(void *arg) srcaddr_last->sin_addr.s_addr = srcaddr.sin_addr.s_addr; srcaddr_last->sin_port = srcaddr.sin_port; _mavlink->set_client_source_initialized(); - warnx("changing partner IP to: %s", inet_ntoa(srcaddr.sin_addr)); + PX4_INFO("partner IP: %s", inet_ntoa(srcaddr.sin_addr)); } } @@ -2087,6 +2175,10 @@ MavlinkReceiver::receive_thread(void *arg) /* if read failed, this loop won't execute */ for (ssize_t i = 0; i < nread; i++) { if (mavlink_parse_char(_mavlink->get_channel(), buf[i], &msg, &status)) { + + /* check if we received version 2 */ + // XXX todo _mavlink->set_proto_version(2); + /* handle generic messages and commands */ handle_message(&msg); @@ -2113,8 +2205,9 @@ void MavlinkReceiver::print_status() uint64_t MavlinkReceiver::sync_stamp(uint64_t usec) { - if (_time_offset > 0) { - return usec - (_time_offset / 1000) ; + + if (_time_offset != 0) { + return usec + (_time_offset / 1000) ; } else { return hrt_absolute_time(); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 96a7da80cb..b2521a6813 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -32,8 +32,8 @@ ****************************************************************************/ /** - * @file mavlink_orb_listener.h - * MAVLink 1.0 uORB listener definition + * @file mavlink_receiver.h + * MAVLink receiver thread * * @author Lorenz Meier * @author Anton Babushkin @@ -141,10 +141,23 @@ private: void handle_message_distance_sensor(mavlink_message_t *msg); void handle_message_follow_target(mavlink_message_t *msg); void handle_message_adsb_vehicle(mavlink_message_t *msg); - void handle_message_gps_inject_data(mavlink_message_t *msg); + void handle_message_gps_rtcm_data(mavlink_message_t *msg); + void handle_message_battery_status(mavlink_message_t *msg); + void handle_message_serial_control(mavlink_message_t *msg); void *receive_thread(void *arg); + /** + * Set the interval at which the given message stream is published. + * The rate is the number of messages per second. + * + * @param msgId the message ID of to change the interval of + * @param interval the interval in us to send the message at + * @param data_rate the total link data rate in bytes per second + */ + void set_message_interval(int msgId, float interval, int data_rate=-1); + void get_message_interval(int msgId); + /** * Convert remote timestamp to local hrt time (usec) * Use timesync if available, monotonic boot time otherwise diff --git a/src/modules/mavlink/mavlink_shell.cpp b/src/modules/mavlink/mavlink_shell.cpp new file mode 100644 index 0000000000..ea509151ef --- /dev/null +++ b/src/modules/mavlink/mavlink_shell.cpp @@ -0,0 +1,172 @@ +/**************************************************************************** + * + * Copyright (c) 2016 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 mavlink_shell.cpp + * A shell to be used via MAVLink + * + * @author Beat Küng + */ + +#include "mavlink_shell.h" +#include + +#include +#include +#include + + +#ifdef __PX4_NUTTX +#include +#endif /* __PX4_NUTTX */ + + +MavlinkShell::MavlinkShell() +{ +} + +MavlinkShell::~MavlinkShell() +{ + //closing the pipes will stop the thread as well + if (_to_shell_fd >= 0) { + PX4_INFO("Stopping mavlink shell"); + close(_to_shell_fd); + } + if (_from_shell_fd >= 0) { + close(_from_shell_fd); + } +} + +int MavlinkShell::start() +{ + //this currently only works for NuttX +#ifndef __PX4_NUTTX + return -1; +#endif /* __PX4_NUTTX */ + + + PX4_INFO("Starting mavlink shell"); + + int p1[2], p2[2]; + + /* Create the shell task and redirect its stdin & stdout. If we used pthread, we would redirect + * stdin/out of the calling process as well, so we need px4_task_spawn_cmd. However NuttX only + * keeps (duplicates) the first 3 fd's when creating a new task, all others are not inherited. + * This means we need to temporarily change the first 3 fd's of the current task (or at least + * the first 2 if stdout=stderr). + * And we hope :-) that during the temporary phase, no other thread from the same task writes to + * stdout (as it would end up in the pipe). + */ + + if (pipe(p1) != 0) { + return -errno; + } + if (pipe(p2) != 0) { + close(p1[0]); + close(p1[1]); + return -errno; + } + + int ret = 0; + + _from_shell_fd = p1[0]; + _to_shell_fd = p2[1]; + _shell_fds[0] = p2[0]; + _shell_fds[1] = p1[1]; + + int fd_backups[2]; //we don't touch stderr, we will redirect it to stdout in the startup of the shell task + for (int i = 0; i < 2; ++i) { + fd_backups[i] = dup(i); + if (fd_backups[i] == -1) { + ret = -errno; + } + } + dup2(_shell_fds[0], 0); + dup2(_shell_fds[1], 1); + + if (ret == 0) { + _task = px4_task_spawn_cmd("mavlink_shell", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + &MavlinkShell::shell_start_thread, + nullptr); + if (_task < 0) { + ret = -1; + } + } + + //restore fd's + for (int i = 0; i < 2; ++i) { + if (dup2(fd_backups[i], i) == -1) { + ret = -errno; + } + close(fd_backups[i]); + } + + //close unused pipe fd's + close(_shell_fds[0]); + close(_shell_fds[1]); + + return ret; +} + +int MavlinkShell::shell_start_thread(int argc, char *argv[]) +{ + dup2(1, 2); //redirect stderror to stdout + +#ifdef __PX4_NUTTX + nsh_consolemain(0, NULL); +#endif /* __PX4_NUTTX */ + + return 0; +} + +size_t MavlinkShell::write(uint8_t* buffer, size_t len) +{ + return ::write(_to_shell_fd, buffer, len); +} + +size_t MavlinkShell::read(uint8_t* buffer, size_t len) +{ + return ::read(_from_shell_fd, buffer, len); +} + +size_t MavlinkShell::available() +{ + int ret = 0; + if (ioctl(_from_shell_fd, FIONREAD, (unsigned long)&ret) == OK) { + return ret; + } + return 0; +} diff --git a/src/modules/mavlink/mavlink_shell.h b/src/modules/mavlink/mavlink_shell.h new file mode 100644 index 0000000000..f4a788a023 --- /dev/null +++ b/src/modules/mavlink/mavlink_shell.h @@ -0,0 +1,93 @@ +/**************************************************************************** + * + * Copyright (c) 2016 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 mavlink_shell.h + * A shell to be used via MAVLink + * + * @author Beat Küng + */ + + +#include +#include +#include + +#pragma once + +class MavlinkShell +{ +public: + MavlinkShell(); + + ~MavlinkShell(); + + /** + * Start the mavlink shell. + * + * @return 0 on success. + */ + int start(); + + + /** + * Write to the shell + * @return number of written bytes + */ + size_t write(uint8_t* buffer, size_t len); + + /** + * Read from the shell. This is blocking, if 0 bytes are available, this will block. + * @param len buffer length + * @return number of bytes read. + */ + size_t read(uint8_t* buffer, size_t len); + + /** + * Get the number of bytes that can be read. + */ + size_t available(); + +private: + + int _to_shell_fd = -1; /** fd to write to the shell */ + int _from_shell_fd = -1; /** fd to read from the shell */ + int _shell_fds[2] = {-1, -1}; /** stdin & out used by the shell */ + px4_task_t _task; + + static int shell_start_thread(int argc, char *argv[]); + + /* do not allow copying this class */ + MavlinkShell(const MavlinkShell &) = delete; + MavlinkShell operator=(const MavlinkShell &) = delete; +}; diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index c37b791b04..f77a8d94a1 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -86,6 +86,16 @@ public: */ virtual unsigned get_size() = 0; + /** + * Get the average message size + * + * For a normal stream this equals the message size, + * for something like a parameter or mission message + * this equals usually zero, as no bandwidth + * needs to be reserved + */ + virtual unsigned get_size_avg() { return get_size(); } + protected: Mavlink *_mavlink; unsigned int _interval; diff --git a/src/modules/mavlink/mavlink_tests/CMakeLists.txt b/src/modules/mavlink/mavlink_tests/CMakeLists.txt index 710f15cd8f..26b7d0d4ba 100644 --- a/src/modules/mavlink/mavlink_tests/CMakeLists.txt +++ b/src/modules/mavlink/mavlink_tests/CMakeLists.txt @@ -35,11 +35,7 @@ px4_add_module( MAIN mavlink_tests STACK_MAIN 5000 COMPILE_FLAGS - -Weffc++ -DMAVLINK_FTP_UNIT_TEST - -Wno-attributes - -Wno-packed - -Wno-packed -Os SRCS mavlink_tests.cpp diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp index 6fe0ed9c8f..39af11f955 100644 --- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp @@ -61,7 +61,7 @@ MavlinkFtpTest::MavlinkFtpTest() : MavlinkFtpTest::~MavlinkFtpTest() { - + } /// @brief Called before every test to initialize the FTP Server. @@ -78,7 +78,7 @@ void MavlinkFtpTest::_init(void) void MavlinkFtpTest::_cleanup(void) { delete _ftp_server; - + _cleanup_microsd(); } @@ -87,20 +87,21 @@ bool MavlinkFtpTest::_ack_test(void) { MavlinkFTP::PayloadHeader payload; const MavlinkFTP::PayloadHeader *reply; - + payload.opcode = MavlinkFTP::kCmdNone; bool success = _send_receive_msg(&payload, // FTP payload header 0, // size in bytes of data nullptr, // Data to start into FTP message payload &reply); // Payload inside FTP message response + if (!success) { return false; } - + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); ut_compare("Incorrect payload size", reply->size, 0); - + return true; } @@ -109,21 +110,22 @@ bool MavlinkFtpTest::_bad_opcode_test(void) { MavlinkFTP::PayloadHeader payload; const MavlinkFTP::PayloadHeader *reply; - + payload.opcode = 0xFF; // bogus opcode bool success = _send_receive_msg(&payload, // FTP payload header 0, // size in bytes of data nullptr, // Data to start into FTP message payload &reply); // Payload inside FTP message response + if (!success) { return false; } - + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); ut_compare("Incorrect payload size", reply->size, 1); ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrUnknownCommand); - + return true; } @@ -133,24 +135,25 @@ bool MavlinkFtpTest::_bad_datasize_test(void) mavlink_message_t msg; MavlinkFTP::PayloadHeader payload; const MavlinkFTP::PayloadHeader *reply; - + payload.opcode = MavlinkFTP::kCmdListDirectory; - + _setup_ftp_msg(&payload, 0, nullptr, &msg); - + // Set the data size to be one larger than is legal - ((MavlinkFTP::PayloadHeader*)((mavlink_file_transfer_protocol_t*)msg.payload64)->payload)->size = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN + 1; - + ((MavlinkFTP::PayloadHeader *)((mavlink_file_transfer_protocol_t *)msg.payload64)->payload)->size = + MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN + 1; + _ftp_server->handle_message(&msg); - + if (!_decode_message(&_reply_msg, &reply)) { return false; } - + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); ut_compare("Incorrect payload size", reply->size, 1); ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidDataSize); - + return true; } @@ -158,10 +161,10 @@ bool MavlinkFtpTest::_list_test(void) { MavlinkFTP::PayloadHeader payload; const MavlinkFTP::PayloadHeader *reply; - + char response1[] = "Dempty_dir|Ftest_238.data\t238|Ftest_239.data\t239|Ftest_240.data\t240"; char response2[] = "Ddev|Detc|Dfs|Dobj"; - + struct _testCase { const char *dir; ///< Directory to run List command on char *response; ///< Expected response entries from List command @@ -174,74 +177,81 @@ bool MavlinkFtpTest::_list_test(void) { "/", response2, 4, true }, }; - for (size_t i=0; idir)+1, // size in bytes of data - (uint8_t*)test->dir, // Data to start into FTP message payload - &reply); // Payload inside FTP message response + strlen(test->dir) + 1, // size in bytes of data + (uint8_t *)test->dir, // Data to start into FTP message payload + &reply); // Payload inside FTP message response + if (!success) { return false; } - + if (test->success) { ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); ut_compare("Incorrect payload size", reply->size, strlen(test->response) + 1); - + // The return order of directories from the List command is not repeatable. So we can't do a direct comparison // to a hardcoded return result string. - + // Convert null terminators to seperator char so we can use strok to parse returned data char list_entry[256]; - for (uint8_t j=0; jsize-1; j++) { + + for (uint8_t j = 0; j < reply->size - 1; j++) { if (reply->data[j] == 0) { list_entry[j] = '|'; + } else { list_entry[j] = reply->data[j]; } } - list_entry[reply->size-1] = 0; - + + list_entry[reply->size - 1] = 0; + // Loop over returned directory entries trying to find then in the response list char *dir; int response_count = 0; dir = strtok(list_entry, "|"); + while (dir != nullptr) { ut_assert("Returned directory not found in expected response", strstr(test->response, dir)); response_count++; dir = strtok(nullptr, "|"); } - + ut_compare("Incorrect number of directory entires returned", test->response_count, response_count); + } else { ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); ut_compare("Incorrect payload size", reply->size, 2); ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno); } } - + return true; } -/// @brief Tests for correct reponse to a List command on a valid directory, but with an offset that +/// @brief Tests for correct response to a List command on a valid directory, but with an offset that /// is beyond the last directory entry. bool MavlinkFtpTest::_list_eof_test(void) { MavlinkFTP::PayloadHeader payload; const MavlinkFTP::PayloadHeader *reply; const char *dir = "/"; - + payload.opcode = MavlinkFTP::kCmdListDirectory; payload.offset = 4; // offset past top level dirs - + bool success = _send_receive_msg(&payload, // FTP payload header - strlen(dir)+1, // size in bytes of data - (uint8_t*)dir, // Data to start into FTP message payload - &reply); // Payload inside FTP message response + strlen(dir) + 1, // size in bytes of data + (uint8_t *)dir, // Data to start into FTP message payload + &reply); // Payload inside FTP message response + if (!success) { return false; } @@ -249,32 +259,33 @@ bool MavlinkFtpTest::_list_eof_test(void) ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); ut_compare("Incorrect payload size", reply->size, 1); ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrEOF); - + return true; } -/// @brief Tests for correct reponse to an Open command on a file which does not exist. +/// @brief Tests for correct response to an Open command on a file which does not exist. bool MavlinkFtpTest::_open_badfile_test(void) { MavlinkFTP::PayloadHeader payload; const MavlinkFTP::PayloadHeader *reply; const char *dir = "/foo"; // non-existent file - + payload.opcode = MavlinkFTP::kCmdOpenFileRO; payload.offset = 0; - + bool success = _send_receive_msg(&payload, // FTP payload header - strlen(dir)+1, // size in bytes of data - (uint8_t*)dir, // Data to start into FTP message payload - &reply); // Payload inside FTP message response + strlen(dir) + 1, // size in bytes of data + (uint8_t *)dir, // Data to start into FTP message payload + &reply); // Payload inside FTP message response + if (!success) { return false; } - + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); ut_compare("Incorrect payload size", reply->size, 2); ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno); - + return true; } @@ -283,41 +294,43 @@ bool MavlinkFtpTest::_open_terminate_test(void) { MavlinkFTP::PayloadHeader payload; const MavlinkFTP::PayloadHeader *reply; - - for (size_t i=0; ifile)+1, // size in bytes of data - (uint8_t*)test->file, // Data to start into FTP message payload + strlen(test->file) + 1, // size in bytes of data + (uint8_t *)test->file, // Data to start into FTP message payload &reply); // Payload inside FTP message response + if (!success) { return false; } - + ut_compare("stat failed", stat(test->file, &st), 0); - - + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); ut_compare("Incorrect payload size", reply->size, sizeof(uint32_t)); - ut_compare("File size incorrect", *((uint32_t*)&reply->data[0]), (uint32_t)st.st_size); + ut_compare("File size incorrect", *((uint32_t *)&reply->data[0]), (uint32_t)st.st_size); payload.opcode = MavlinkFTP::kCmdTerminateSession; payload.session = reply->session; payload.size = 0; - + success = _send_receive_msg(&payload, // FTP payload header 0, // size in bytes of data nullptr, // Data to start into FTP message payload &reply); // Payload inside FTP message response + if (!success) { return false; } - + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); ut_compare("Incorrect payload size", reply->size, 0); } @@ -331,36 +344,38 @@ bool MavlinkFtpTest::_terminate_badsession_test(void) MavlinkFTP::PayloadHeader payload; const MavlinkFTP::PayloadHeader *reply; const char *file = _rgDownloadTestCases[0].file; - + payload.opcode = MavlinkFTP::kCmdOpenFileRO; payload.offset = 0; - + bool success = _send_receive_msg(&payload, // FTP payload header - strlen(file)+1, // size in bytes of data - (uint8_t*)file, // Data to start into FTP message payload - &reply); // Payload inside FTP message response + strlen(file) + 1, // size in bytes of data + (uint8_t *)file, // Data to start into FTP message payload + &reply); // Payload inside FTP message response + if (!success) { return false; } - + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); - + payload.opcode = MavlinkFTP::kCmdTerminateSession; payload.session = reply->session + 1; payload.size = 0; - + success = _send_receive_msg(&payload, // FTP payload header - 0, // size in bytes of data - nullptr, // Data to start into FTP message payload - &reply); // Payload inside FTP message response + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &reply); // Payload inside FTP message response + if (!success) { return false; } - + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); ut_compare("Incorrect payload size", reply->size, 1); ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidSession); - + return true; } @@ -369,11 +384,11 @@ bool MavlinkFtpTest::_read_test(void) { MavlinkFTP::PayloadHeader payload; const MavlinkFTP::PayloadHeader *reply; - - for (size_t i=0; ifile, &st), 0); uint8_t *bytes = new uint8_t[st.st_size]; @@ -383,35 +398,37 @@ bool MavlinkFtpTest::_read_test(void) int bytes_read = ::read(fd, bytes, st.st_size); ut_compare("read failed", bytes_read, st.st_size); ::close(fd); - + // Test case data files are created for specific boundary conditions ut_compare("Test case data files are out of date", test->length, st.st_size); - + payload.opcode = MavlinkFTP::kCmdOpenFileRO; payload.offset = 0; - + bool success = _send_receive_msg(&payload, // FTP payload header - strlen(test->file)+1, // size in bytes of data - (uint8_t*)test->file, // Data to start into FTP message payload + strlen(test->file) + 1, // size in bytes of data + (uint8_t *)test->file, // Data to start into FTP message payload &reply); // Payload inside FTP message response + if (!success) { return false; } - + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); - + payload.opcode = MavlinkFTP::kCmdReadFile; payload.session = reply->session; payload.offset = 0; - + success = _send_receive_msg(&payload, // FTP payload header 0, // size in bytes of data nullptr, // Data to start into FTP message payload &reply); // Payload inside FTP message response + if (!success) { return false; } - + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); ut_compare("Offset incorrect", reply->offset, 0); @@ -419,29 +436,32 @@ bool MavlinkFtpTest::_read_test(void) uint32_t expected_bytes = test->singlePacketRead ? (uint32_t)st.st_size : full_packet_bytes; ut_compare("Payload size incorrect", reply->size, expected_bytes); ut_compare("File contents differ", memcmp(reply->data, bytes, expected_bytes), 0); - + payload.offset += expected_bytes; - + if (test->singlePacketRead) { // Try going past EOF success = _send_receive_msg(&payload, // FTP payload header 0, // size in bytes of data nullptr, // Data to start into FTP message payload &reply); // Payload inside FTP message response + if (!success) { return false; } - + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + } else { success = _send_receive_msg(&payload, // FTP payload header 0, // size in bytes of data nullptr, // Data to start into FTP message payload &reply); // Payload inside FTP message response + if (!success) { return false; } - + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); ut_compare("Offset incorrect", reply->offset, payload.offset); @@ -449,23 +469,24 @@ bool MavlinkFtpTest::_read_test(void) ut_compare("Payload size incorrect", reply->size, expected_bytes); ut_compare("File contents differ", memcmp(reply->data, &bytes[payload.offset], expected_bytes), 0); } - + payload.opcode = MavlinkFTP::kCmdTerminateSession; payload.session = reply->session; payload.size = 0; - + success = _send_receive_msg(&payload, // FTP payload header 0, // size in bytes of data nullptr, // Data to start into FTP message payload &reply); // Payload inside FTP message response + if (!success) { return false; } - + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); ut_compare("Incorrect payload size", reply->size, 0); } - + return true; } @@ -477,11 +498,11 @@ bool MavlinkFtpTest::_burst_test(void) BurstInfo burst_info; - - for (size_t i=0; ifile, &st), 0); uint8_t *bytes = new uint8_t[st.st_size]; @@ -491,23 +512,24 @@ bool MavlinkFtpTest::_burst_test(void) int bytes_read = ::read(fd, bytes, st.st_size); ut_compare("read failed", bytes_read, st.st_size); ::close(fd); - + // Test case data files are created for specific boundary conditions ut_compare("Test case data files are out of date", test->length, st.st_size); - + payload.opcode = MavlinkFTP::kCmdOpenFileRO; payload.offset = 0; - + bool success = _send_receive_msg(&payload, // FTP payload header - strlen(test->file)+1, // size in bytes of data - (uint8_t*)test->file, // Data to start into FTP message payload + strlen(test->file) + 1, // size in bytes of data + (uint8_t *)test->file, // Data to start into FTP message payload &reply); // Payload inside FTP message response + if (!success) { return false; } ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); - + // Setup for burst response handler burst_info.burst_state = burst_state_first_ack; burst_info.single_packet_file = test->singlePacketRead; @@ -520,7 +542,7 @@ bool MavlinkFtpTest::_burst_test(void) payload.opcode = MavlinkFTP::kCmdBurstReadFile; payload.session = reply->session; payload.offset = 0; - + mavlink_message_t msg; _setup_ftp_msg(&payload, 0, nullptr, &msg); _ftp_server->handle_message(&msg); @@ -528,29 +550,30 @@ bool MavlinkFtpTest::_burst_test(void) // First packet is sent using stream mechanism, so we need to force it out ourselves hrt_abstime t = 0; _ftp_server->send(t); - + ut_compare("Incorrect sequence of messages", burst_info.burst_state, burst_state_complete); - + // Put back generic message handler _ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message_handler_generic, this); - + // Terminate session payload.opcode = MavlinkFTP::kCmdTerminateSession; payload.session = reply->session; payload.size = 0; - + success = _send_receive_msg(&payload, // FTP payload header 0, // size in bytes of data nullptr, // Data to start into FTP message payload &reply); // Payload inside FTP message response + if (!success) { return false; } - + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); ut_compare("Incorrect payload size", reply->size, 0); } - + return true; } @@ -560,36 +583,38 @@ bool MavlinkFtpTest::_read_badsession_test(void) MavlinkFTP::PayloadHeader payload; const MavlinkFTP::PayloadHeader *reply; const char *file = _rgDownloadTestCases[0].file; - + payload.opcode = MavlinkFTP::kCmdOpenFileRO; payload.offset = 0; - + bool success = _send_receive_msg(&payload, // FTP payload header - strlen(file)+1, // size in bytes of data - (uint8_t*)file, // Data to start into FTP message payload + strlen(file) + 1, // size in bytes of data + (uint8_t *)file, // Data to start into FTP message payload &reply); // Payload inside FTP message response + if (!success) { return false; } - + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); - + payload.opcode = MavlinkFTP::kCmdReadFile; payload.session = reply->session + 1; // Invalid session payload.offset = 0; - + success = _send_receive_msg(&payload, // FTP payload header 0, // size in bytes of data nullptr, // Data to start into FTP message payload &reply); // Payload inside FTP message response + if (!success) { return false; } - + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); ut_compare("Incorrect payload size", reply->size, 1); ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidSession); - + return true; } @@ -598,7 +623,7 @@ bool MavlinkFtpTest::_removedirectory_test(void) MavlinkFTP::PayloadHeader payload; const MavlinkFTP::PayloadHeader *reply; int fd; - + struct _testCase { const char *dir; bool success; @@ -611,39 +636,41 @@ bool MavlinkFtpTest::_removedirectory_test(void) { _unittest_microsd_file, false, false }, { _unittest_microsd_dir, true, true }, }; - + ut_compare("mkdir failed", ::mkdir(_unittest_microsd_dir, S_IRWXU | S_IRWXG | S_IRWXO), 0); - ut_assert("open failed", (fd = ::open(_unittest_microsd_file, O_CREAT | O_EXCL)) != -1); + ut_assert("open failed", (fd = ::open(_unittest_microsd_file, O_CREAT | O_EXCL, S_IRWXU | S_IRWXG | S_IRWXO)) != -1); ::close(fd); - - for (size_t i=0; ideleteFile) { ut_compare("unlink failed", ::unlink(_unittest_microsd_file), 0); } - + payload.opcode = MavlinkFTP::kCmdRemoveDirectory; payload.offset = 0; - + bool success = _send_receive_msg(&payload, // FTP payload header - strlen(test->dir)+1, // size in bytes of data - (uint8_t*)test->dir, // Data to start into FTP message payload + strlen(test->dir) + 1, // size in bytes of data + (uint8_t *)test->dir, // Data to start into FTP message payload &reply); // Payload inside FTP message response + if (!success) { return false; } - + if (test->success) { ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); ut_compare("Incorrect payload size", reply->size, 0); + } else { ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); ut_compare("Incorrect payload size", reply->size, 2); ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno); } } - + return true; } @@ -651,7 +678,7 @@ bool MavlinkFtpTest::_createdirectory_test(void) { MavlinkFTP::PayloadHeader payload; const MavlinkFTP::PayloadHeader *reply; - + struct _testCase { const char *dir; bool success; @@ -662,31 +689,33 @@ bool MavlinkFtpTest::_createdirectory_test(void) { _unittest_microsd_dir, false }, { "/fs/microsd/bogus/bogus", false }, }; - - for (size_t i=0; idir)+1, // size in bytes of data - (uint8_t*)test->dir, // Data to start into FTP message payload + strlen(test->dir) + 1, // size in bytes of data + (uint8_t *)test->dir, // Data to start into FTP message payload &reply); // Payload inside FTP message response + if (!success) { return false; } - + if (test->success) { ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); ut_compare("Incorrect payload size", reply->size, 0); + } else { ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); ut_compare("Incorrect payload size", reply->size, 2); ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno); } } - + return true; } @@ -695,7 +724,7 @@ bool MavlinkFtpTest::_removefile_test(void) MavlinkFTP::PayloadHeader payload; const MavlinkFTP::PayloadHeader *reply; int fd; - + struct _testCase { const char *file; bool success; @@ -707,46 +736,48 @@ bool MavlinkFtpTest::_removefile_test(void) { _unittest_microsd_file, true }, { _unittest_microsd_file, false }, }; - + ut_compare("mkdir failed", ::mkdir(_unittest_microsd_dir, S_IRWXU | S_IRWXG | S_IRWXO), 0); - ut_assert("open failed", (fd = ::open(_unittest_microsd_file, O_CREAT | O_EXCL)) != -1); + ut_assert("open failed", (fd = ::open(_unittest_microsd_file, O_CREAT | O_EXCL, S_IRWXU | S_IRWXG | S_IRWXO)) != -1); ::close(fd); - - for (size_t i=0; ifile)+1, // size in bytes of data - (uint8_t*)test->file, // Data to start into FTP message payload + strlen(test->file) + 1, // size in bytes of data + (uint8_t *)test->file, // Data to start into FTP message payload &reply); // Payload inside FTP message response + if (!success) { return false; } - + if (test->success) { ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); ut_compare("Incorrect payload size", reply->size, 0); + } else { ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); ut_compare("Incorrect payload size", reply->size, 2); ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno); } } - + return true; } /// Static method used as callback from MavlinkFTP for generic use. This method will be called by MavlinkFTP when /// it needs to send a message out on Mavlink. -void MavlinkFtpTest::receive_message_handler_generic(const mavlink_file_transfer_protocol_t* ftp_req, void *worker_data) +void MavlinkFtpTest::receive_message_handler_generic(const mavlink_file_transfer_protocol_t *ftp_req, void *worker_data) { - ((MavlinkFtpTest*)worker_data)->_receive_message_handler_generic(ftp_req); + ((MavlinkFtpTest *)worker_data)->_receive_message_handler_generic(ftp_req); } -void MavlinkFtpTest::_receive_message_handler_generic(const mavlink_file_transfer_protocol_t* ftp_req) +void MavlinkFtpTest::_receive_message_handler_generic(const mavlink_file_transfer_protocol_t *ftp_req) { // Move the message into our own member variable memcpy(&_reply_msg, ftp_req, sizeof(mavlink_file_transfer_protocol_t)); @@ -754,62 +785,63 @@ void MavlinkFtpTest::_receive_message_handler_generic(const mavlink_file_transfe /// Static method used as callback from MavlinkFTP for stream download testing. This method will be called by MavlinkFTP when /// it needs to send a message out on Mavlink. -void MavlinkFtpTest::receive_message_handler_burst(const mavlink_file_transfer_protocol_t* ftp_req, void *worker_data) +void MavlinkFtpTest::receive_message_handler_burst(const mavlink_file_transfer_protocol_t *ftp_req, void *worker_data) { - BurstInfo* burst_info = (BurstInfo*)worker_data; + BurstInfo *burst_info = (BurstInfo *)worker_data; burst_info->ftp_test_class->_receive_message_handler_burst(ftp_req, burst_info); } -bool MavlinkFtpTest::_receive_message_handler_burst(const mavlink_file_transfer_protocol_t* ftp_msg, BurstInfo* burst_info) +bool MavlinkFtpTest::_receive_message_handler_burst(const mavlink_file_transfer_protocol_t *ftp_msg, + BurstInfo *burst_info) { hrt_abstime t = 0; - const MavlinkFTP::PayloadHeader* reply; + const MavlinkFTP::PayloadHeader *reply; uint32_t full_packet_bytes = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader); uint32_t expected_bytes; - + _decode_message(ftp_msg, &reply); switch (burst_info->burst_state) { - case burst_state_first_ack: - ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); - ut_compare("Offset incorrect", reply->offset, 0); - - expected_bytes = burst_info->single_packet_file ? burst_info->file_size : full_packet_bytes; - ut_compare("Payload size incorrect", reply->size, expected_bytes); - ut_compare("burst_complete incorrect", reply->burst_complete, 0); - ut_compare("File contents differ", memcmp(reply->data, burst_info->file_bytes, expected_bytes), 0); - - // Setup for next expected message - burst_info->burst_state = burst_info->single_packet_file ? burst_state_nak_eof : burst_state_last_ack; - - ut_assert("Remaining stream packets missing", _ftp_server->get_size()); - _ftp_server->send(t); - break; - - case burst_state_last_ack: - ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); - ut_compare("Offset incorrect", reply->offset, full_packet_bytes); - - expected_bytes = burst_info->file_size - full_packet_bytes; - ut_compare("Payload size incorrect", reply->size, expected_bytes); - ut_compare("burst_complete incorrect", reply->burst_complete, 0); - ut_compare("File contents differ", memcmp(reply->data, &burst_info->file_bytes[full_packet_bytes], expected_bytes), 0); - - // Setup for next expected message - burst_info->burst_state = burst_state_nak_eof; - - ut_assert("Remaining stream packets missing", _ftp_server->get_size()); - _ftp_server->send(t); - break; - - case burst_state_nak_eof: - // Signal complete - burst_info->burst_state = burst_state_complete; - ut_compare("All packets should have been seent", _ftp_server->get_size(), 0); - break; - + case burst_state_first_ack: + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Offset incorrect", reply->offset, 0); + + expected_bytes = burst_info->single_packet_file ? burst_info->file_size : full_packet_bytes; + ut_compare("Payload size incorrect", reply->size, expected_bytes); + ut_compare("burst_complete incorrect", reply->burst_complete, 0); + ut_compare("File contents differ", memcmp(reply->data, burst_info->file_bytes, expected_bytes), 0); + + // Setup for next expected message + burst_info->burst_state = burst_info->single_packet_file ? burst_state_nak_eof : burst_state_last_ack; + + ut_assert("Remaining stream packets missing", _ftp_server->get_size()); + _ftp_server->send(t); + break; + + case burst_state_last_ack: + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Offset incorrect", reply->offset, full_packet_bytes); + + expected_bytes = burst_info->file_size - full_packet_bytes; + ut_compare("Payload size incorrect", reply->size, expected_bytes); + ut_compare("burst_complete incorrect", reply->burst_complete, 0); + ut_compare("File contents differ", memcmp(reply->data, &burst_info->file_bytes[full_packet_bytes], expected_bytes), 0); + + // Setup for next expected message + burst_info->burst_state = burst_state_nak_eof; + + ut_assert("Remaining stream packets missing", _ftp_server->get_size()); + _ftp_server->send(t); + break; + + case burst_state_nak_eof: + // Signal complete + burst_info->burst_state = burst_state_complete; + ut_compare("All packets should have been seent", _ftp_server->get_size(), 0); + break; + } - + return true; } @@ -818,18 +850,18 @@ bool MavlinkFtpTest::_decode_message(const mavlink_file_transfer_protocol_t *ftp const MavlinkFTP::PayloadHeader **payload) ///< Payload inside FTP message response { //warnx("_decode_message"); - + // Make sure the targets are correct ut_compare("Target network non-zero", ftp_msg->target_network, 0); ut_compare("Target system id mismatch", ftp_msg->target_system, clientSystemId); ut_compare("Target component id mismatch", ftp_msg->target_component, clientComponentId); - + *payload = reinterpret_cast(ftp_msg->payload); - + // Make sure we have a good sequence number ut_compare("Sequence number mismatch", (*payload)->seq_number, _expected_seq_number); _expected_seq_number++; - + return true; } @@ -841,18 +873,19 @@ void MavlinkFtpTest::_setup_ftp_msg(const MavlinkFTP::PayloadHeader *payload_hea { uint8_t payload_bytes[MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN]; MavlinkFTP::PayloadHeader *payload = reinterpret_cast(payload_bytes); - + memcpy(payload, payload_header, sizeof(MavlinkFTP::PayloadHeader)); - + payload->seq_number = _expected_seq_number++; payload->size = size; + if (size != 0) { memcpy(payload->data, data, size); } - + payload->burst_complete = 0; payload->padding = 0; - + msg->checksum = 0; mavlink_msg_file_transfer_protocol_pack(clientSystemId, // Sender system id clientComponentId, // Sender component id @@ -865,9 +898,9 @@ void MavlinkFtpTest::_setup_ftp_msg(const MavlinkFTP::PayloadHeader *payload_hea /// @brief Sends the specified FTP message to the server and returns response bool MavlinkFtpTest::_send_receive_msg(MavlinkFTP::PayloadHeader *payload_header, ///< FTP payload header - uint8_t size, ///< size in bytes of data - const uint8_t *data, ///< Data to start into FTP message payload - const MavlinkFTP::PayloadHeader **payload_reply) ///< Payload inside FTP message response + uint8_t size, ///< size in bytes of data + const uint8_t *data, ///< Data to start into FTP message payload + const MavlinkFTP::PayloadHeader **payload_reply) ///< Payload inside FTP message response { mavlink_message_t msg; @@ -875,7 +908,7 @@ bool MavlinkFtpTest::_send_receive_msg(MavlinkFTP::PayloadHeader *payload_header _ftp_server->handle_message(&msg); return _decode_message(&_reply_msg, payload_reply); } - + /// @brief Cleans up an files created on microsd during testing void MavlinkFtpTest::_cleanup_microsd(void) { @@ -889,7 +922,7 @@ bool MavlinkFtpTest::run_tests(void) ut_run_test(_ack_test); ut_run_test(_bad_opcode_test); ut_run_test(_bad_datasize_test); - ut_run_test(_list_test); + //ut_run_test(_list_test); // TODO: cmake build system needs to run mavlink_ftp_test_data.py ut_run_test(_list_eof_test); ut_run_test(_open_badfile_test); ut_run_test(_open_terminate_test); @@ -900,7 +933,7 @@ bool MavlinkFtpTest::run_tests(void) ut_run_test(_removedirectory_test); ut_run_test(_createdirectory_test); ut_run_test(_removefile_test); - + return (_tests_failed == 0); } diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h index 14c9369b05..7708d2a630 100644 --- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h @@ -45,37 +45,37 @@ class MavlinkFtpTest : public UnitTest public: MavlinkFtpTest(); virtual ~MavlinkFtpTest(); - + virtual bool run_tests(void); - - static void receive_message_handler_generic(const mavlink_file_transfer_protocol_t* ftp_req, void *worker_data); - + + static void receive_message_handler_generic(const mavlink_file_transfer_protocol_t *ftp_req, void *worker_data); + /// Worker data for stream handler struct BurstInfo { - MavlinkFtpTest* ftp_test_class; + MavlinkFtpTest *ftp_test_class; int burst_state; bool single_packet_file; uint32_t file_size; - uint8_t* file_bytes; + uint8_t *file_bytes; }; - - static void receive_message_handler_burst(const mavlink_file_transfer_protocol_t* ftp_req, void *worker_data); - + + static void receive_message_handler_burst(const mavlink_file_transfer_protocol_t *ftp_req, void *worker_data); + static const uint8_t serverSystemId = 50; ///< System ID for server static const uint8_t serverComponentId = 1; ///< Component ID for server static const uint8_t serverChannel = 0; ///< Channel to send to - + static const uint8_t clientSystemId = 1; ///< System ID for client static const uint8_t clientComponentId = 0; ///< Component ID for client - + // We don't want any of these - MavlinkFtpTest(const MavlinkFtpTest&); - MavlinkFtpTest& operator=(const MavlinkFtpTest&); - + MavlinkFtpTest(const MavlinkFtpTest &); + MavlinkFtpTest &operator=(const MavlinkFtpTest &); + private: virtual void _init(void); virtual void _cleanup(void); - + bool _ack_test(void); bool _bad_opcode_test(void); bool _bad_datasize_test(void); @@ -90,16 +90,17 @@ private: bool _removedirectory_test(void); bool _createdirectory_test(void); bool _removefile_test(void); - - void _receive_message_handler_generic(const mavlink_file_transfer_protocol_t* ftp_req); - void _setup_ftp_msg(const MavlinkFTP::PayloadHeader *payload_header, uint8_t size, const uint8_t *data, mavlink_message_t *msg); + + void _receive_message_handler_generic(const mavlink_file_transfer_protocol_t *ftp_req); + void _setup_ftp_msg(const MavlinkFTP::PayloadHeader *payload_header, uint8_t size, const uint8_t *data, + mavlink_message_t *msg); bool _decode_message(const mavlink_file_transfer_protocol_t *ftp_msg, const MavlinkFTP::PayloadHeader **payload); bool _send_receive_msg(MavlinkFTP::PayloadHeader *payload_header, - uint8_t size, - const uint8_t *data, - const MavlinkFTP::PayloadHeader **payload_reply); + uint8_t size, + const uint8_t *data, + const MavlinkFTP::PayloadHeader **payload_reply); void _cleanup_microsd(void); - + /// A single download test case struct DownloadTestCase { const char *file; @@ -107,10 +108,10 @@ private: bool singlePacketRead; bool exactlyFillPacket; }; - + /// The set of test cases for download testing static const DownloadTestCase _rgDownloadTestCases[]; - + /// States for stream download handler enum { burst_state_first_ack, @@ -118,14 +119,14 @@ private: burst_state_nak_eof, burst_state_complete }; - - bool _receive_message_handler_burst(const mavlink_file_transfer_protocol_t* ftp_req, BurstInfo* burst_info); - - MavlinkFTP* _ftp_server; + + bool _receive_message_handler_burst(const mavlink_file_transfer_protocol_t *ftp_req, BurstInfo *burst_info); + + MavlinkFTP *_ftp_server; uint16_t _expected_seq_number; - + mavlink_file_transfer_protocol_t _reply_msg; - + static const char _unittest_microsd_dir[]; static const char _unittest_microsd_file[]; }; diff --git a/src/modules/mc_att_control/CMakeLists.txt b/src/modules/mc_att_control/CMakeLists.txt index 20aec10ac2..6071940370 100644 --- a/src/modules/mc_att_control/CMakeLists.txt +++ b/src/modules/mc_att_control/CMakeLists.txt @@ -35,7 +35,7 @@ px4_add_module( MAIN mc_att_control STACK_MAIN 1200 STACK_MAX 3500 - COMPILE_FLAGS + COMPILE_FLAGS -Os SRCS mc_att_control_main.cpp DEPENDS 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 3ba1f4769e..695db0a668 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -70,8 +70,6 @@ #include #include #include -#include -#include #include #include #include @@ -104,6 +102,11 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); #define MANUAL_THROTTLE_MAX_MULTICOPTER 0.9f #define ATTITUDE_TC_DEFAULT 0.2f +#define AXIS_INDEX_ROLL 0 +#define AXIS_INDEX_PITCH 1 +#define AXIS_INDEX_YAW 2 +#define AXIS_COUNT 3 + class MulticopterAttitudeControl { public: @@ -726,32 +729,26 @@ MulticopterAttitudeControl::control_attitude(float dt) /* limit rates */ for (int i = 0; i < 3; i++) { - if (_v_control_mode.flag_control_velocity_enabled && !_v_control_mode.flag_control_manual_enabled) { + if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) && + !_v_control_mode.flag_control_manual_enabled) { _rates_sp(i) = math::constrain(_rates_sp(i), -_params.auto_rate_max(i), _params.auto_rate_max(i)); + } else { _rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i)); } } - /* weather-vane mode, dampen yaw rate */ - if (_v_att_sp.disable_mc_yaw_control == true && _v_control_mode.flag_control_velocity_enabled && !_v_control_mode.flag_control_manual_enabled) { - float wv_yaw_rate_max = _params.auto_rate_max(2) * _params.vtol_wv_yaw_rate_scale; - _rates_sp(2) = math::constrain(_rates_sp(2), -wv_yaw_rate_max, wv_yaw_rate_max); - // prevent integrator winding up in weathervane mode - _rates_int(2) = 0.0f; - } - /* feed forward yaw setpoint rate */ _rates_sp(2) += _v_att_sp.yaw_sp_move_rate * yaw_w * _params.yaw_ff; - /* weather-vane mode, scale down yaw rate */ - if (_v_att_sp.disable_mc_yaw_control == true && _v_control_mode.flag_control_velocity_enabled && !_v_control_mode.flag_control_manual_enabled) { + /* weather-vane mode, dampen yaw rate */ + if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) && + _v_att_sp.disable_mc_yaw_control == true && !_v_control_mode.flag_control_manual_enabled) { float wv_yaw_rate_max = _params.auto_rate_max(2) * _params.vtol_wv_yaw_rate_scale; _rates_sp(2) = math::constrain(_rates_sp(2), -wv_yaw_rate_max, wv_yaw_rate_max); // prevent integrator winding up in weathervane mode _rates_int(2) = 0.0f; } - } /* @@ -782,12 +779,14 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) /* update integral only if not saturated on low limit and if motor commands are not saturated */ if (_thrust_sp > MIN_TAKEOFF_THRUST && !_motor_limits.lower_limit && !_motor_limits.upper_limit) { - for (int i = 0; i < 3; i++) { + for (int i = AXIS_INDEX_ROLL; i < AXIS_COUNT; i++) { if (fabsf(_att_control(i)) < _thrust_sp) { float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; if (PX4_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) { + _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT && + /* if the axis is the yaw axis, do not update the integral if the limit is hit */ + !((i == AXIS_INDEX_YAW) && _motor_limits.yaw)) { _rates_int(i) = rate_i; } } diff --git a/src/modules/mc_pos_control/CMakeLists.txt b/src/modules/mc_pos_control/CMakeLists.txt index 5d58414732..0de1f49235 100644 --- a/src/modules/mc_pos_control/CMakeLists.txt +++ b/src/modules/mc_pos_control/CMakeLists.txt @@ -33,6 +33,7 @@ px4_add_module( MODULE modules__mc_pos_control MAIN mc_pos_control + COMPILE_FLAGS -Os STACK_MAIN 1200 SRCS mc_pos_control_main.cpp 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 bd5bac0f47..0f3a453a2f 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -374,6 +374,17 @@ MulticopterPositionControl::MulticopterPositionControl() : _local_pos_sp_pub(nullptr), _global_vel_sp_pub(nullptr), _attitude_setpoint_id(0), + _vehicle_status{}, + _vehicle_land_detected{}, + _ctrl_state{}, + _att_sp{}, + _manual{}, + _control_mode{}, + _arming{}, + _local_pos{}, + _pos_sp_triplet{}, + _local_pos_sp{}, + _global_vel_sp{}, _manual_thr_min(this, "MANTHR_MIN"), _manual_thr_max(this, "MANTHR_MAX"), _vel_x_deriv(this, "VELD"), @@ -398,17 +409,8 @@ MulticopterPositionControl::MulticopterPositionControl() : _takeoff_thrust_sp(0.0f), control_vel_enabled_prev(false) { - memset(&_vehicle_status, 0, sizeof(_vehicle_status)); - memset(&_ctrl_state, 0, sizeof(_ctrl_state)); + // Make the quaternion valid for control state _ctrl_state.q[0] = 1.0f; - memset(&_att_sp, 0, sizeof(_att_sp)); - memset(&_manual, 0, sizeof(_manual)); - memset(&_control_mode, 0, sizeof(_control_mode)); - memset(&_arming, 0, sizeof(_arming)); - memset(&_local_pos, 0, sizeof(_local_pos)); - memset(&_pos_sp_triplet, 0, sizeof(_pos_sp_triplet)); - memset(&_local_pos_sp, 0, sizeof(_local_pos_sp)); - memset(&_global_vel_sp, 0, sizeof(_global_vel_sp)); memset(&_ref_pos, 0, sizeof(_ref_pos)); @@ -583,6 +585,11 @@ MulticopterPositionControl::parameters_update(bool force) _params.hold_max_z = (v < 0.0f ? 0.0f : v); param_get(_params_handles.acc_hor_max, &v); _params.acc_hor_max = v; + /* + * increase the maximum horizontal acceleration such that stopping + * within 1 s from full speed is feasible + */ + _params.acc_hor_max = math::max(_params.vel_cruise(0), _params.acc_hor_max); param_get(_params_handles.alt_mode, &v_i); _params.alt_mode = v_i; @@ -1147,7 +1154,10 @@ void MulticopterPositionControl::control_auto(float dt) _pos_sp = pos_sp_s.edivide(scale); /* update yaw setpoint if needed */ - if (PX4_ISFINITE(_pos_sp_triplet.current.yaw)) { + + if (_pos_sp_triplet.current.yawspeed_valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET) { + _att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt; + } else if (PX4_ISFINITE(_pos_sp_triplet.current.yaw)) { _att_sp.yaw_body = _pos_sp_triplet.current.yaw; } @@ -1226,12 +1236,12 @@ MulticopterPositionControl::task_main() fds[0].events = POLLIN; while (!_task_should_exit) { - /* wait for up to 500ms for data */ - int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500); + /* wait for up to 20ms for data */ + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 20); /* timed out - periodic check for _task_should_exit */ if (pret == 0) { - continue; + // Go through the loop anyway to copy manual input at 50 Hz. } /* this is undesirable but not much we can do */ @@ -1415,10 +1425,16 @@ MulticopterPositionControl::task_main() _vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1); } + // guard against any bad velocity values + + bool velocity_valid = PX4_ISFINITE(_pos_sp_triplet.current.vx) && + PX4_ISFINITE(_pos_sp_triplet.current.vy) && + _pos_sp_triplet.current.velocity_valid; + // do not go slower than the follow target velocity when position tracking is active (set to valid) if(_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET && - _pos_sp_triplet.current.velocity_valid && + velocity_valid && _pos_sp_triplet.current.position_valid) { math::Vector<3> ft_vel(_pos_sp_triplet.current.vx, _pos_sp_triplet.current.vy, 0); @@ -1442,7 +1458,7 @@ MulticopterPositionControl::task_main() // track target using velocity only } else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET && - _pos_sp_triplet.current.velocity_valid) { + velocity_valid) { _vel_sp(0) = _pos_sp_triplet.current.vx; _vel_sp(1) = _pos_sp_triplet.current.vy; @@ -1964,10 +1980,10 @@ MulticopterPositionControl::task_main() float yaw_offs = _wrap_pi(yaw_target - _yaw); // If the yaw offset became too big for the system to track stop - // shifting it - - // XXX this needs inspection - probably requires a clamp, not an if - if (fabsf(yaw_offs) < yaw_offset_max) { + // shifting it, only allow if it would make the offset smaller again. + if (fabsf(yaw_offs) < yaw_offset_max || + (_att_sp.yaw_sp_move_rate > 0 && yaw_offs < 0) || + (_att_sp.yaw_sp_move_rate < 0 && yaw_offs > 0)) { _att_sp.yaw_body = yaw_target; } } @@ -1991,48 +2007,50 @@ MulticopterPositionControl::task_main() math::Matrix<3, 3> R_sp; - // construct attitude setpoint rotation matrix. modify the setpoints for roll - // and pitch such that they reflect the user's intention even if a yaw error - // (yaw_sp - yaw) is present. In the presence of a yaw error constructing a rotation matrix - // from the pure euler angle setpoints will lead to unexpected attitude behaviour from - // the user's view as the euler angle sequence uses the yaw setpoint and not the current - // heading of the vehicle. + if (!_control_mode.flag_control_velocity_enabled) { + // construct attitude setpoint rotation matrix. modify the setpoints for roll + // and pitch such that they reflect the user's intention even if a yaw error + // (yaw_sp - yaw) is present. In the presence of a yaw error constructing a rotation matrix + // from the pure euler angle setpoints will lead to unexpected attitude behaviour from + // the user's view as the euler angle sequence uses the yaw setpoint and not the current + // heading of the vehicle. - // calculate our current yaw error - float yaw_error = _wrap_pi(_att_sp.yaw_body - _yaw); + // calculate our current yaw error + float yaw_error = _wrap_pi(_att_sp.yaw_body - _yaw); - // compute the vector obtained by rotating a z unit vector by the rotation - // given by the roll and pitch commands of the user - math::Vector<3> zB = {0, 0, 1}; - math::Matrix<3,3> R_sp_roll_pitch; - R_sp_roll_pitch.from_euler(_att_sp.roll_body, _att_sp.pitch_body, 0); - math::Vector<3> z_roll_pitch_sp = R_sp_roll_pitch * zB; + // compute the vector obtained by rotating a z unit vector by the rotation + // given by the roll and pitch commands of the user + math::Vector<3> zB = {0, 0, 1}; + math::Matrix<3,3> R_sp_roll_pitch; + R_sp_roll_pitch.from_euler(_att_sp.roll_body, _att_sp.pitch_body, 0); + math::Vector<3> z_roll_pitch_sp = R_sp_roll_pitch * zB; - // transform the vector into a new frame which is rotated around the z axis - // by the current yaw error. this vector defines the desired tilt when we look - // into the direction of the desired heading - math::Matrix<3,3> R_yaw_correction; - R_yaw_correction.from_euler(0.0f, 0.0f, -yaw_error); - z_roll_pitch_sp = R_yaw_correction * z_roll_pitch_sp; + // transform the vector into a new frame which is rotated around the z axis + // by the current yaw error. this vector defines the desired tilt when we look + // into the direction of the desired heading + math::Matrix<3,3> R_yaw_correction; + R_yaw_correction.from_euler(0.0f, 0.0f, -yaw_error); + z_roll_pitch_sp = R_yaw_correction * z_roll_pitch_sp; - // use the formula z_roll_pitch_sp = R_tilt * [0;0;1] - // to calculate the new desired roll and pitch angles - // R_tilt can be written as a function of the new desired roll and pitch - // angles. we get three equations and have to solve for 2 unknowns - float pitch_new = asinf(z_roll_pitch_sp(0)); - float roll_new = -atan2f(z_roll_pitch_sp(1), z_roll_pitch_sp(2)); + // use the formula z_roll_pitch_sp = R_tilt * [0;0;1] + // to calculate the new desired roll and pitch angles + // R_tilt can be written as a function of the new desired roll and pitch + // angles. we get three equations and have to solve for 2 unknowns + float pitch_new = asinf(z_roll_pitch_sp(0)); + float roll_new = -atan2f(z_roll_pitch_sp(1), z_roll_pitch_sp(2)); - R_sp.from_euler(roll_new, pitch_new, _att_sp.yaw_body); + R_sp.from_euler(roll_new, pitch_new, _att_sp.yaw_body); - memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body)); + memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body)); - /* reset the acceleration set point for all non-attitude flight modes */ - if (!(_control_mode.flag_control_offboard_enabled && - !(_control_mode.flag_control_position_enabled || - _control_mode.flag_control_velocity_enabled))) { + /* reset the acceleration set point for all non-attitude flight modes */ + if (!(_control_mode.flag_control_offboard_enabled && + !(_control_mode.flag_control_position_enabled || + _control_mode.flag_control_velocity_enabled))) { - _thrust_sp_prev = R_sp * math::Vector<3>(0, 0, -_att_sp.thrust); + _thrust_sp_prev = R_sp * math::Vector<3>(0, 0, -_att_sp.thrust); + } } /* copy quaternion setpoint to attitude setpoint topic */ @@ -2043,6 +2061,7 @@ MulticopterPositionControl::task_main() } else { reset_yaw_sp = true; + _att_sp.yaw_sp_move_rate = 0.0f; } /* update previous velocity for velocity controller D part */ diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 88ad616431..5e817addad 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -47,6 +47,7 @@ * @min 0.05 * @max 1.0 * @decimal 2 + * @increment 0.01 * @group Multicopter Position Control */ PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f); @@ -65,6 +66,7 @@ PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f); * @min 0.2 * @max 0.8 * @decimal 2 + * @increment 0.01 * @group Multicopter Position Control */ PARAM_DEFINE_FLOAT(MPC_THR_HOVER, 0.5f); @@ -80,6 +82,7 @@ PARAM_DEFINE_FLOAT(MPC_THR_HOVER, 0.5f); * @min 0.0 * @max 0.2 * @decimal 2 + * @increment 0.05 * @group Multicopter Position Control */ PARAM_DEFINE_FLOAT(MPC_ALTCTL_DZ, 0.1f); @@ -243,7 +246,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_FF, 0.5f); * @decimal 2 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_XY_P, 1.25f); +PARAM_DEFINE_FLOAT(MPC_XY_P, 0.95f); /** * Proportional gain for horizontal velocity error @@ -453,7 +456,7 @@ PARAM_DEFINE_FLOAT(MPC_VELD_LP, 5.0f); * @decimal 2 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_ACC_HOR_MAX, 10.0f); +PARAM_DEFINE_FLOAT(MPC_ACC_HOR_MAX, 5.0f); /** * Altitude control mode, note mode 1 only tested with LPE diff --git a/src/modules/muorb/adsp/px4muorb.cpp b/src/modules/muorb/adsp/px4muorb.cpp index efe8fde2d0..4e25989b17 100644 --- a/src/modules/muorb/adsp/px4muorb.cpp +++ b/src/modules/muorb/adsp/px4muorb.cpp @@ -42,11 +42,17 @@ #include "px4_log.h" #include "uORB/topics/sensor_combined.h" #include "uORB.h" +#include "systemlib/param/param.h" +#include __BEGIN_DECLS extern int dspal_main(int argc, char *argv[]); +extern struct shmem_info *shmem_info_p; +extern int get_shmem_lock(const char *caller_file_name, int caller_line_number); +extern void release_shmem_lock(const char *caller_file_name, + int caller_line_number); +extern void init_shared_memory(void); __END_DECLS - int px4muorb_orb_initialize() { HAP_power_request(100, 100, 1000); @@ -54,13 +60,14 @@ int px4muorb_orb_initialize() // The uORB Manager needs to be initialized first up, otherwise the instance is nullptr. uORB::Manager::initialize(); // Register the fastrpc muorb with uORBManager. - uORB::Manager::get_instance()->set_uorb_communicator(uORB::FastRpcChannel::GetInstance()); + uORB::Manager::get_instance()->set_uorb_communicator( + uORB::FastRpcChannel::GetInstance()); // Now continue with the usual dspal startup. - const char *argv[] = {"dspal", "start"}; + const char *argv[] = { "dspal", "start" }; int argc = 2; int rc; - rc = dspal_main(argc, (char **)argv); + rc = dspal_main(argc, (char **) argv); return rc; } @@ -76,6 +83,83 @@ int px4muorb_get_absolute_time(uint64_t *time_us) return 0; } +/*update value and param's change bit in shared memory*/ +int px4muorb_param_update_to_shmem(uint32_t param, const uint8_t *value, + int data_len_in_bytes) +{ + unsigned int byte_changed, bit_changed; + union param_value_u *param_value = (union param_value_u *) value; + + if (!shmem_info_p) { + init_shared_memory(); + } + + if (get_shmem_lock(__FILE__, __LINE__) != 0) { + PX4_INFO("Could not get shmem lock\n"); + return -1; + } + + shmem_info_p->params_val[param] = *param_value; + + byte_changed = param / 8; + bit_changed = 1 << param % 8; + shmem_info_p->krait_changed_index[byte_changed] |= bit_changed; + + release_shmem_lock(__FILE__, __LINE__); + + return 0; +} + +int px4muorb_param_update_index_from_shmem(unsigned char *data, + int data_len_in_bytes) +{ + unsigned int i; + + if (!shmem_info_p) { + return -1; + } + + if (get_shmem_lock(__FILE__, __LINE__) != 0) { + PX4_INFO("Could not get shmem lock\n"); + return -1; + } + + for (i = 0; i < data_len_in_bytes; i++) { + data[i] = shmem_info_p->adsp_changed_index[i]; + } + + release_shmem_lock(__FILE__, __LINE__); + + return 0; +} + +int px4muorb_param_update_value_from_shmem(uint32_t param, const uint8_t *value, + int data_len_in_bytes) +{ + unsigned int byte_changed, bit_changed; + union param_value_u *param_value = (union param_value_u *) value; + + if (!shmem_info_p) { + return -1; + } + + if (get_shmem_lock(__FILE__, __LINE__) != 0) { + PX4_INFO("Could not get shmem lock\n"); + return -1; + } + + *param_value = shmem_info_p->params_val[param]; + + /*also clear the index since we are holding the lock*/ + byte_changed = param / 8; + bit_changed = 1 << param % 8; + shmem_info_p->adsp_changed_index[byte_changed] &= ~bit_changed; + + release_shmem_lock(__FILE__, __LINE__); + + return 0; +} + int px4muorb_add_subscriber(const char *name) { int rc = 0; @@ -114,14 +198,16 @@ int px4muorb_remove_subscriber(const char *name) return rc; } -int px4muorb_send_topic_data(const char *name, const uint8_t *data, int data_len_in_bytes) +int px4muorb_send_topic_data(const char *name, const uint8_t *data, + int data_len_in_bytes) { int rc = 0; uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler(); if (rxHandler != nullptr) { - rc = rxHandler->process_received_message(name, data_len_in_bytes, (uint8_t *)data); + rc = rxHandler->process_received_message(name, data_len_in_bytes, + (uint8_t *) data); } else { rc = -1; @@ -138,37 +224,39 @@ int px4muorb_is_subscriber_present(const char *topic_name, int *status) rc = channel->is_subscriber_present(topic_name, &local_status); if (rc == 0) { - *status = (int)local_status; + *status = (int) local_status; } return rc; } -int px4muorb_receive_msg(int *msg_type, char *topic_name, int topic_name_len, uint8_t *data, int data_len_in_bytes, - int *bytes_returned) +int px4muorb_receive_msg(int *msg_type, char *topic_name, int topic_name_len, + uint8_t *data, int data_len_in_bytes, int *bytes_returned) { int rc = 0; int32_t local_msg_type = 0; int32_t local_bytes_returned = 0; uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); //PX4_DEBUG( "topic_namePtr: [0x%p] dataPtr: [0x%p]", topic_name, data ); - rc = channel->get_data(&local_msg_type, topic_name, topic_name_len, data, data_len_in_bytes, &local_bytes_returned); - *msg_type = (int)local_msg_type; - *bytes_returned = (int)local_bytes_returned; + rc = channel->get_data(&local_msg_type, topic_name, topic_name_len, data, + data_len_in_bytes, &local_bytes_returned); + *msg_type = (int) local_msg_type; + *bytes_returned = (int) local_bytes_returned; return rc; } -int px4muorb_receive_bulk_data(uint8_t *bulk_transfer_buffer, int max_size_in_bytes, - int *returned_length_in_bytes, int *topic_count) +int px4muorb_receive_bulk_data(uint8_t *bulk_transfer_buffer, + int max_size_in_bytes, int *returned_length_in_bytes, int *topic_count) { int rc = 0; int32_t local_bytes_returned = 0; int32_t local_topic_count = 0; uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); //PX4_DEBUG( "topic_namePtr: [0x%p] dataPtr: [0x%p]", topic_name, data ); - rc = channel->get_bulk_data(bulk_transfer_buffer, max_size_in_bytes, &local_bytes_returned, &local_topic_count); - *returned_length_in_bytes = (int)local_bytes_returned; - *topic_count = (int)local_topic_count; + rc = channel->get_bulk_data(bulk_transfer_buffer, max_size_in_bytes, + &local_bytes_returned, &local_topic_count); + *returned_length_in_bytes = (int) local_bytes_returned; + *topic_count = (int) local_topic_count; return rc; } diff --git a/src/modules/muorb/adsp/px4muorb.hpp b/src/modules/muorb/adsp/px4muorb.hpp index 56154becfa..36d5f98ca5 100644 --- a/src/modules/muorb/adsp/px4muorb.hpp +++ b/src/modules/muorb/adsp/px4muorb.hpp @@ -43,6 +43,12 @@ extern "C" { int px4muorb_get_absolute_time(uint64_t *time_us) __EXPORT; + int px4muorb_param_update_to_shmem(uint32_t param, const uint8_t *value, int data_len_in_bytes) __EXPORT; + + int px4muorb_param_update_index_from_shmem(unsigned char *data, int data_len_in_bytes) __EXPORT; + + int px4muorb_param_update_value_from_shmem(uint32_t param, const uint8_t *value, int data_len_in_bytes) __EXPORT; + int px4muorb_add_subscriber(const char *name) __EXPORT; int px4muorb_remove_subscriber(const char *name) __EXPORT; diff --git a/src/modules/muorb/krait/CMakeLists.txt b/src/modules/muorb/krait/CMakeLists.txt index fe4c509303..b9e054e3e7 100644 --- a/src/modules/muorb/krait/CMakeLists.txt +++ b/src/modules/muorb/krait/CMakeLists.txt @@ -30,11 +30,11 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ +set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon") +include(hexagon_sdk) + include_directories(${CMAKE_BINARY_DIR}/src/firmware/posix) -include_directories( - ${HEXAGON_SDK_ROOT}/inc/stddef - ${HEXAGON_SDK_ROOT}/lib/common/rpcmem - ) +include_directories(${HEXAGON_SDK_INCLUDES}) px4_add_module( MODULE modules__muorb__krait diff --git a/src/modules/muorb/krait/module.mk b/src/modules/muorb/krait/module.mk deleted file mode 100644 index 4f30ee46e1..0000000000 --- a/src/modules/muorb/krait/module.mk +++ /dev/null @@ -1,48 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# 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. -# -############################################################################ - -# -# Makefile to build uORB -# - -MODULE_COMMAND = muorb - -ifeq ($(PX4_TARGET_OS),posix-arm) -SRCS = uORBKraitFastRpcChannel.cpp \ - muorb_main.cpp -INCLUDE_DIRS += $(EXT_MUORB_LIB_ROOT)/krait/include \ - $(PX4_BASE)/src/modules/uORB \ - $(PX4_BASE)/src/modules - -EXTRA_LIBS += $(EXT_MUORB_LIB_ROOT)/krait/libs/libpx4muorb.so -endif diff --git a/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp b/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp index 00fd0e3286..b7bea9cad9 100644 --- a/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp +++ b/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp @@ -38,6 +38,7 @@ #include #include "px4muorb.h" #include "px4_log.h" +#include using namespace px4muorb; @@ -51,7 +52,7 @@ using namespace px4muorb; static char *_TopicNameBuffer = 0; static const int32_t _MAX_TOPIC_NAME_BUFFER = 256; -static uint8_t *_DataBuffer = 0; +static uint8_t *_DataBuffer = 0; static const uint32_t _MAX_DATA_BUFFER_SIZE = 2048; static bool _Initialized = false; @@ -60,9 +61,12 @@ static bool _Initialized = false; // hence we are trying to allocation 64K of byte buffers. static const uint32_t _MAX_TOPIC_DATA_BUFFER_SIZE = 1024; static const uint32_t _MAX_TOPICS = 64; -static const uint32_t _MAX_BULK_TRANSFER_BUFFER_SIZE = _MAX_TOPIC_DATA_BUFFER_SIZE * _MAX_TOPICS; +static const uint32_t _MAX_BULK_TRANSFER_BUFFER_SIZE = + _MAX_TOPIC_DATA_BUFFER_SIZE * _MAX_TOPICS; static uint8_t *_BulkTransferBuffer = 0; +unsigned char *adsp_changed_index = 0; + // The DSP timer can be read from this file. #define DSP_TIMER_FILE "/sys/kernel/boot_adsp/qdsp_qtimer" @@ -99,12 +103,13 @@ int calc_timer_diff_to_dsp_us(int32_t *time_diff_us) // Do this call right after reading to avoid latency here. timespec ts; clock_gettime(CLOCK_MONOTONIC, &ts); - uint64_t time_appsproc = ((uint64_t)ts.tv_sec) * 1000000llu + (ts.tv_nsec / 1000); + uint64_t time_appsproc = ((uint64_t) ts.tv_sec) * 1000000llu + + (ts.tv_nsec / 1000); close(fd); uint64_t time_dsp; - int ret = sscanf(buffer, "%llx", &time_dsp); + int ret = sscanf(buffer, "%llx", &time_dsp); if (ret < 0) { PX4_ERR("Could not parse DSP timer."); @@ -116,8 +121,9 @@ int calc_timer_diff_to_dsp_us(int32_t *time_diff_us) time_dsp /= 19.2; // Before casting to in32_t, check if it fits. - uint64_t abs_diff = (time_appsproc > time_dsp) - ? (time_appsproc - time_dsp) : (time_dsp - time_appsproc); + uint64_t abs_diff = + (time_appsproc > time_dsp) ? + (time_appsproc - time_dsp) : (time_dsp - time_appsproc); if (abs_diff > INT32_MAX) { PX4_ERR("Timer difference too big"); @@ -126,13 +132,14 @@ int calc_timer_diff_to_dsp_us(int32_t *time_diff_us) *time_diff_us = time_appsproc - time_dsp; - PX4_DEBUG("found time_dsp: %llu us, time_appsproc: %llu us", time_dsp, time_appsproc); - PX4_DEBUG("found time_diff: %li us, %.6f s", *time_diff_us, ((double)*time_diff_us) / 1e6); + PX4_DEBUG("found time_dsp: %llu us, time_appsproc: %llu us", + time_dsp, time_appsproc); + PX4_DEBUG("found time_diff: %li us, %.6f s", + *time_diff_us, ((double)*time_diff_us) / 1e6); return 0; } - px4muorb::KraitRpcWrapper::KraitRpcWrapper() { } @@ -150,21 +157,25 @@ bool px4muorb::KraitRpcWrapper::Initialize() PX4_DEBUG("%s Now calling rpcmem_alloc...", __FUNCTION__); - _BulkTransferBuffer = (uint8_t *) rpcmem_alloc(MUORB_KRAIT_FASTRPC_HEAP_ID, MUORB_KRAIT_FASTRPC_MEM_FLAGS, + _BulkTransferBuffer = (uint8_t *) rpcmem_alloc(MUORB_KRAIT_FASTRPC_HEAP_ID, + MUORB_KRAIT_FASTRPC_MEM_FLAGS, _MAX_BULK_TRANSFER_BUFFER_SIZE * sizeof(uint8_t)); rc = (_BulkTransferBuffer != NULL) ? true : false; if (!rc) { - PX4_ERR("%s rpcmem_alloc failed! for bulk transfer buffers", __FUNCTION__); + PX4_ERR("%s rpcmem_alloc failed! for bulk transfer buffers", + __FUNCTION__); return rc; } else { - PX4_DEBUG("%s rpcmem_alloc passed for Bulk transfer buffers buffer_size: %d addr: %p", - __FUNCTION__, (_MAX_BULK_TRANSFER_BUFFER_SIZE * sizeof(uint8_t)), _BulkTransferBuffer); + PX4_DEBUG( + "%s rpcmem_alloc passed for Bulk transfer buffers buffer_size: %d addr: %p", + __FUNCTION__, (_MAX_BULK_TRANSFER_BUFFER_SIZE * sizeof(uint8_t)), _BulkTransferBuffer); } _TopicNameBuffer = (char *) rpcmem_alloc(MUORB_KRAIT_FASTRPC_HEAP_ID, - MUORB_KRAIT_FASTRPC_MEM_FLAGS, _MAX_TOPIC_NAME_BUFFER * sizeof(char)); + MUORB_KRAIT_FASTRPC_MEM_FLAGS, + _MAX_TOPIC_NAME_BUFFER * sizeof(char)); rc = (_TopicNameBuffer != NULL) ? true : false; @@ -179,7 +190,8 @@ bool px4muorb::KraitRpcWrapper::Initialize() // now allocate the data buffer. _DataBuffer = (uint8_t *) rpcmem_alloc(MUORB_KRAIT_FASTRPC_HEAP_ID, - MUORB_KRAIT_FASTRPC_MEM_FLAGS, _MAX_DATA_BUFFER_SIZE * sizeof(uint8_t)); + MUORB_KRAIT_FASTRPC_MEM_FLAGS, + _MAX_DATA_BUFFER_SIZE * sizeof(uint8_t)); rc = (_DataBuffer != NULL) ? true : false; @@ -195,6 +207,18 @@ bool px4muorb::KraitRpcWrapper::Initialize() PX4_DEBUG("%s rpcmem_alloc passed for data_buffer", __FUNCTION__); } + adsp_changed_index = (uint8_t *) rpcmem_alloc(MUORB_KRAIT_FASTRPC_HEAP_ID, + MUORB_KRAIT_FASTRPC_MEM_FLAGS, PARAM_BUFFER_SIZE * sizeof(uint8_t)); + + rc = (adsp_changed_index != NULL) ? true : false; + + if (!rc) { + PX4_ERR("%s rpcmem_alloc failed! for adsp_changed_index", __FUNCTION__); + + } else { + memset(adsp_changed_index, 0, PARAM_BUFFER_SIZE * sizeof(uint8_t)); + } + int32_t time_diff_us; if (calc_timer_diff_to_dsp_us(&time_diff_us) != 0) { @@ -204,7 +228,8 @@ bool px4muorb::KraitRpcWrapper::Initialize() // call muorb initialize routine. if (px4muorb_orb_initialize() != 0) { - PX4_ERR("%s Error calling the uorb fastrpc initalize method..", __FUNCTION__); + PX4_ERR("%s Error calling the uorb fastrpc initalize method..", + __FUNCTION__); rc = false; return rc; } @@ -219,7 +244,8 @@ bool px4muorb::KraitRpcWrapper::Initialize() int diff = (time_dsp - time_appsproc); - PX4_INFO("time_dsp: %llu us, time appsproc: %llu us, diff: %d us", time_dsp, time_appsproc, diff); + PX4_DEBUG("time_dsp: %llu us, time appsproc: %llu us, diff: %d us", + time_dsp, time_appsproc, diff); _Initialized = true; return rc; @@ -242,6 +268,11 @@ bool px4muorb::KraitRpcWrapper::Terminate() _DataBuffer = 0; } + if (adsp_changed_index != NULL) { + rpcmem_free(adsp_changed_index); + adsp_changed_index = 0; + } + _Initialized = false; return true; } @@ -256,28 +287,32 @@ int32_t px4muorb::KraitRpcWrapper::RemoveSubscriber(const char *topic) return (_Initialized ? px4muorb_remove_subscriber(topic) : -1); } -int32_t px4muorb::KraitRpcWrapper::IsSubscriberPresent(const char *topic, int32_t *status) +int32_t px4muorb::KraitRpcWrapper::IsSubscriberPresent(const char *topic, + int32_t *status) { return (_Initialized ? px4muorb_is_subscriber_present(topic, status) : -1); } -int32_t px4muorb::KraitRpcWrapper::SendData(const char *topic, int32_t length_in_bytes, const uint8_t *data) +int32_t px4muorb::KraitRpcWrapper::SendData(const char *topic, + int32_t length_in_bytes, const uint8_t *data) { - return (_Initialized ? px4muorb_send_topic_data(topic, data, length_in_bytes) : -1); + return (_Initialized ? + px4muorb_send_topic_data(topic, data, length_in_bytes) : -1); } -int32_t px4muorb::KraitRpcWrapper::ReceiveData(int32_t *msg_type, char **topic, int32_t *length_in_bytes, - uint8_t **data) +int32_t px4muorb::KraitRpcWrapper::ReceiveData(int32_t *msg_type, char **topic, + int32_t *length_in_bytes, uint8_t **data) { int32_t rc = -1; if (_Initialized) { - rc = px4muorb_receive_msg(msg_type, _TopicNameBuffer, _MAX_TOPIC_NAME_BUFFER, _DataBuffer, _MAX_DATA_BUFFER_SIZE, + rc = px4muorb_receive_msg(msg_type, _TopicNameBuffer, + _MAX_TOPIC_NAME_BUFFER, _DataBuffer, _MAX_DATA_BUFFER_SIZE, length_in_bytes); if (rc == 0) { *topic = _TopicNameBuffer; - *data = _DataBuffer; + *data = _DataBuffer; } else { PX4_ERR("ERROR: Getting data from fastRPC link"); @@ -290,13 +325,15 @@ int32_t px4muorb::KraitRpcWrapper::ReceiveData(int32_t *msg_type, char **topic, return rc; } -int32_t px4muorb::KraitRpcWrapper::ReceiveBulkData(uint8_t **bulk_data, int32_t *length_in_bytes, int32_t *topic_count) +int32_t px4muorb::KraitRpcWrapper::ReceiveBulkData(uint8_t **bulk_data, + int32_t *length_in_bytes, int32_t *topic_count) { int32_t rc = -1; if (_Initialized) { //rc = px4muorb_receive_msg( msg_type, _TopicNameBuffer, _MAX_TOPIC_NAME_BUFFER, _DataBuffer, _MAX_DATA_BUFFER_SIZE, length_in_bytes ); - rc = px4muorb_receive_bulk_data(_BulkTransferBuffer, _MAX_BULK_TRANSFER_BUFFER_SIZE, length_in_bytes, topic_count); + rc = px4muorb_receive_bulk_data(_BulkTransferBuffer, + _MAX_BULK_TRANSFER_BUFFER_SIZE, length_in_bytes, topic_count); if (rc == 0) { *bulk_data = _BulkTransferBuffer; diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index 4fb4a5bed4..18d41d79ca 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -33,9 +33,8 @@ px4_add_module( MODULE modules__navigator MAIN navigator - STACK_MAIN 1200 + STACK_MAIN 1300 COMPILE_FLAGS - -Wno-sign-compare -Os SRCS navigator_main.cpp diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c index 8a5c59b87a..d720dcea64 100644 --- a/src/modules/navigator/datalinkloss_params.c +++ b/src/modules/navigator/datalinkloss_params.c @@ -50,6 +50,9 @@ * * @unit s * @min 0.0 + * @max 3600.0 + * @decimal 0 + * @increment 1 * @group Data Link Loss */ PARAM_DEFINE_FLOAT(NAV_DLL_CH_T, 120.0f); @@ -86,17 +89,22 @@ PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890); * @unit m * @min -50 * @max 30000 + * @decimal 1 + * @increment 0.5 * @group Data Link Loss */ PARAM_DEFINE_FLOAT(NAV_DLL_CH_ALT, 600.0f); /** - * Airfield hole wait time + * Airfield home wait time * * The amount of time in seconds the system should wait at the airfield home waypoint * * @unit s * @min 0.0 + * @max 3600.0 + * @decimal 0 + * @increment 1 * @group Data Link Loss */ PARAM_DEFINE_FLOAT(NAV_DLL_AH_T, 120.0f); @@ -106,9 +114,9 @@ PARAM_DEFINE_FLOAT(NAV_DLL_AH_T, 120.0f); * * After more than this number of data link timeouts the aircraft returns home directly * - * @group Data Link Loss * @min 0 * @max 1000 + * @group Data Link Loss */ PARAM_DEFINE_INT32(NAV_DLL_N, 2); @@ -118,7 +126,7 @@ PARAM_DEFINE_INT32(NAV_DLL_N, 2); * If set to 1 the system will skip the comms hold wp on data link loss and will directly fly to * airfield home * - * @group Data Link Loss * @boolean + * @group Data Link Loss */ PARAM_DEFINE_INT32(NAV_DLL_CHSK, 0); diff --git a/src/modules/navigator/follow_target.cpp b/src/modules/navigator/follow_target.cpp index ff23802efd..0c80f9bc99 100644 --- a/src/modules/navigator/follow_target.cpp +++ b/src/modules/navigator/follow_target.cpp @@ -52,26 +52,39 @@ #include #include #include +#include #include "navigator.h" FollowTarget::FollowTarget(Navigator *navigator, const char *name) : MissionBlock(navigator, name), _navigator(navigator), - _param_min_alt(this, "MIS_TAKEOFF_ALT", false), - _follow_target_state(WAIT_FOR_TARGET_POSITION), + _param_min_alt(this, "NAV_MIN_FT_HT", false), + _param_tracking_dist(this,"NAV_FT_DST", false), + _param_tracking_side(this,"NAV_FT_FS", false), + _param_tracking_resp(this,"NAV_FT_RS", false), + _param_yaw_auto_max(this,"MC_YAWRAUTO_MAX", false), + _follow_target_state(SET_WAIT_FOR_TARGET_POSITION), + _follow_target_position(FOLLOW_FROM_BEHIND), _follow_target_sub(-1), _step_time_in_ms(0.0f), + _follow_offset(OFFSET_M), _target_updates(0), _last_update_time(0), - _current_target_motion( {}), - _previous_target_motion({}) + _current_target_motion({}), + _previous_target_motion({}), + _yaw_rate(0.0F), + _responsiveness(0.0F), + _yaw_auto_max(0.0F), + _yaw_angle(0.0F) { updateParams(); _current_vel.zero(); _step_vel.zero(); - _target_vel.zero(); + _est_target_vel.zero(); _target_distance.zero(); + _target_position_offset.zero(); + _target_position_delta.zero(); } FollowTarget::~FollowTarget() @@ -85,6 +98,22 @@ void FollowTarget::on_inactive() void FollowTarget::on_activation() { + updateParams(); + + _follow_offset = _param_tracking_dist.get() < 1.0F ? 1.0F : _param_tracking_dist.get(); + + _responsiveness = math::constrain((float) _param_tracking_resp.get(), .1F, 1.0F); + + _yaw_auto_max = math::radians(_param_yaw_auto_max.get()); + + _follow_target_position = _param_tracking_side.get(); + + if((_follow_target_position > FOLLOW_FROM_LEFT) || (_follow_target_position < FOLLOW_FROM_RIGHT)) { + _follow_target_position = FOLLOW_FROM_BEHIND; + } + + _rot_matrix = (_follow_position_matricies[_follow_target_position]); + if (_follow_target_sub < 0) { _follow_target_sub = orb_subscribe(ORB_ID(follow_target)); } @@ -93,7 +122,8 @@ void FollowTarget::on_activation() void FollowTarget::on_active() { struct map_projection_reference_s target_ref; - math::Vector<3> target_position(0, 0, 0); + math::Vector<3> target_reported_velocity(0, 0, 0); + follow_target_s target_motion_with_offset = {}; uint64_t current_time = hrt_absolute_time(); bool _radius_entered = false; bool _radius_exited = false; @@ -103,6 +133,7 @@ void FollowTarget::on_active() orb_check(_follow_target_sub, &updated); if (updated) { + follow_target_s target_motion; _target_updates++; @@ -110,45 +141,23 @@ void FollowTarget::on_active() _previous_target_motion = _current_target_motion; - orb_copy(ORB_ID(follow_target), _follow_target_sub, &_current_target_motion); + orb_copy(ORB_ID(follow_target), _follow_target_sub, &target_motion); - } else if (((current_time - _previous_target_motion.timestamp) / 1000 / 1000) > TARGET_TIMEOUT_S - && target_velocity_valid()) { + if(_current_target_motion.timestamp == 0) { + _current_target_motion = target_motion; + } + + _current_target_motion.timestamp = target_motion.timestamp; + _current_target_motion.lat = (_current_target_motion.lat*(double)_responsiveness) + target_motion.lat*(double)(1 - _responsiveness); + _current_target_motion.lon = (_current_target_motion.lon*(double)_responsiveness) + target_motion.lon*(double)(1 - _responsiveness); + + target_reported_velocity(0) = _current_target_motion.vx; + target_reported_velocity(1) = _current_target_motion.vy; + + } else if (((current_time - _current_target_motion.timestamp) / 1000 ) > TARGET_TIMEOUT_MS && target_velocity_valid()) { reset_target_validity(); } - // update target velocity - - if (target_velocity_valid() && updated) { - - dt_ms = ((_current_target_motion.timestamp - _previous_target_motion.timestamp) / 1000); - - // get last gps known reference for target - - map_projection_init(&target_ref, _previous_target_motion.lat, _previous_target_motion.lon); - - // calculate distance the target has moved - - map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &(target_position(0)), - &(target_position(1))); - - // update the average velocity of the target based on the position - - _target_vel = target_position / (dt_ms / 1000.0f); - - // to keep the velocity increase/decrease smooth - // calculate how many velocity increments/decrements - // it will take to reach the targets velocity - // with the given amount of steps also add a feed forward input that adjusts the - // velocity as the position gap increases since - // just traveling at the exact velocity of the target will not - // get any closer to the target - - _step_vel = (_target_vel - _current_vel) + _target_distance * FF_K; - _step_vel /= (dt_ms / 1000.0f * (float) INTERPOLATION_PNTS); - _step_time_in_ms = dt_ms / (float) INTERPOLATION_PNTS; - } - // update distance to target if (target_position_valid()) { @@ -156,15 +165,113 @@ void FollowTarget::on_active() // get distance to target map_projection_init(&target_ref, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); - map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &_target_distance(0), - &_target_distance(1)); + map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &_target_distance(0), &_target_distance(1)); - // are we within the target acceptance radius? - // give a buffer to exit/enter the radius to give the velocity controller - // a chance to catch up + } - _radius_exited = (_target_distance.length() > (float) TARGET_ACCEPTANCE_RADIUS_M * 1.5f); - _radius_entered = (_target_distance.length() < (float) TARGET_ACCEPTANCE_RADIUS_M); + // update target velocity + + if (target_velocity_valid() && updated) { + + dt_ms = ((_current_target_motion.timestamp - _previous_target_motion.timestamp) / 1000); + + // ignore a small dt + + if (dt_ms > 10.0F) { + + math::Vector<3> prev_position_delta = _target_position_delta; + + // get last gps known reference for target + + map_projection_init(&target_ref, _previous_target_motion.lat, _previous_target_motion.lon); + + // calculate distance the target has moved + + map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &(_target_position_delta(0)), &(_target_position_delta(1))); + + // update the average velocity of the target based on the position + + _est_target_vel = _target_position_delta / (dt_ms / 1000.0f); + + // if the target is moving add an offset and rotation + + if(_est_target_vel.length() > .5F) { + _target_position_offset = _rot_matrix*_est_target_vel.normalized()*_follow_offset; + } + + // are we within the target acceptance radius? + // give a buffer to exit/enter the radius to give the velocity controller + // a chance to catch up + + _radius_exited = ((_target_position_offset + _target_distance).length() > (float) TARGET_ACCEPTANCE_RADIUS_M * 1.5f); + _radius_entered = ((_target_position_offset + _target_distance).length() < (float) TARGET_ACCEPTANCE_RADIUS_M); + + // to keep the velocity increase/decrease smooth + // calculate how many velocity increments/decrements + // it will take to reach the targets velocity + // with the given amount of steps also add a feed forward input that adjusts the + // velocity as the position gap increases since + // just traveling at the exact velocity of the target will not + // get any closer or farther from the target + + _step_vel = (_est_target_vel - _current_vel) + (_target_position_offset + _target_distance) * FF_K; + _step_vel /= (dt_ms / 1000.0F * (float) INTERPOLATION_PNTS); + _step_time_in_ms = (dt_ms / (float) INTERPOLATION_PNTS); + + // if we are less than 1 meter from the target don't worry about trying to yaw + // lock the yaw until we are at a distance that makes sense + + if((_target_distance).length() > 1.0F) { + + // yaw rate smoothing + + // this really needs to control the yaw rate directly in the attitude pid controller + // but seems to work ok for now since the yaw rate cannot be controlled directly in auto mode + + _yaw_angle = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, + _current_target_motion.lat, + _current_target_motion.lon); + + _yaw_rate = (_yaw_angle - _navigator->get_global_position()->yaw) / (dt_ms / 1000.0F); + + _yaw_rate = _wrap_pi(_yaw_rate); + + _yaw_rate = math::constrain(_yaw_rate, -1.0F*_yaw_auto_max, _yaw_auto_max); + + } else { + _yaw_angle = _yaw_rate = NAN; + } + } + +// warnx(" _step_vel x %3.6f y %3.6f cur vel %3.6f %3.6f tar vel %3.6f %3.6f dist = %3.6f (%3.6f) mode = %d con ratio = %3.6f yaw rate = %3.6f", +// (double) _step_vel(0), +// (double) _step_vel(1), +// (double) _current_vel(0), +// (double) _current_vel(1), +// (double) _est_target_vel(0), +// (double) _est_target_vel(1), +// (double) (_target_distance).length(), +// (double) (_target_position_offset + _target_distance).length(), +// _follow_target_state, +// (double)_avg_cos_ratio, (double) _yaw_rate); + } + + if(target_position_valid()) { + + // get the target position using the calculated offset + + map_projection_init(&target_ref, _current_target_motion.lat, _current_target_motion.lon); + map_projection_reproject(&target_ref, _target_position_offset(0), _target_position_offset(1), &target_motion_with_offset.lat, &target_motion_with_offset.lon); + } + + // clamp yaw rate smoothing if we are with in + // 3 degrees of facing target + + if (PX4_ISFINITE(_yaw_rate)) { + if (fabsf(fabsf(_yaw_angle) - fabsf(_navigator->get_global_position()->yaw)) < math::radians(3.0F)) { + _yaw_rate = NAN; + } } // update state machine @@ -175,12 +282,14 @@ void FollowTarget::on_active() if (_radius_entered == true) { _follow_target_state = TRACK_VELOCITY; - } else if (target_velocity_valid()) { - set_follow_target_item(&_mission_item, _param_min_alt.get(), _current_target_motion, NAN); + set_follow_target_item(&_mission_item, _param_min_alt.get(), target_motion_with_offset, _yaw_angle); // keep the current velocity updated with the target velocity for when it's needed - _current_vel = _target_vel; - update_position_sp(true, true); + _current_vel = _est_target_vel; + + update_position_sp(true, true, _yaw_rate); + } else { + _follow_target_state = SET_WAIT_FOR_TARGET_POSITION; } break; @@ -190,48 +299,54 @@ void FollowTarget::on_active() if (_radius_exited == true) { _follow_target_state = TRACK_POSITION; - } else if (target_velocity_valid()) { - set_follow_target_item(&_mission_item, _param_min_alt.get(), _current_target_motion, NAN); if ((current_time - _last_update_time) / 1000 >= _step_time_in_ms) { _current_vel += _step_vel; _last_update_time = current_time; } - update_position_sp(true, false); + set_follow_target_item(&_mission_item, _param_min_alt.get(), target_motion_with_offset, _yaw_angle); + + update_position_sp(true, false, _yaw_rate); + } else { + _follow_target_state = SET_WAIT_FOR_TARGET_POSITION; } break; } + case SET_WAIT_FOR_TARGET_POSITION: { + // Climb to the minimum altitude + // and wait until a position is received + + follow_target_s target = {}; + + // for now set the target at the minimum height above the uav + + target.lat = _navigator->get_global_position()->lat; + target.lon = _navigator->get_global_position()->lon; + target.alt = 0.0F; + + set_follow_target_item(&_mission_item, _param_min_alt.get(), target, _yaw_angle); + + update_position_sp(false, false, _yaw_rate); + + _follow_target_state = WAIT_FOR_TARGET_POSITION; + } case WAIT_FOR_TARGET_POSITION: { - // Climb to the minimum altitude - // and wait until a position is received - - follow_target_s target = { }; - - // for now set the target at the minimum height above the uav - - target.lat = _navigator->get_global_position()->lat; - target.lon = _navigator->get_global_position()->lon; - target.alt = 0.0F; - - set_follow_target_item(&_mission_item, _param_min_alt.get(), target, NAN); - - update_position_sp(false, false); - - if (is_mission_item_reached() && target_velocity_valid()) { - _follow_target_state = TRACK_POSITION; - } + if (is_mission_item_reached() && target_velocity_valid()) { + _target_position_offset(0) = _follow_offset; + _follow_target_state = TRACK_POSITION; + } break; } } } -void FollowTarget::update_position_sp(bool use_velocity, bool use_position) +void FollowTarget::update_position_sp(bool use_velocity, bool use_position, float yaw_rate) { // convert mission item to current setpoint @@ -248,31 +363,34 @@ void FollowTarget::update_position_sp(bool use_velocity, bool use_position) pos_sp_triplet->current.vx = _current_vel(0); pos_sp_triplet->current.vy = _current_vel(1); pos_sp_triplet->next.valid = false; - + pos_sp_triplet->current.yawspeed_valid = PX4_ISFINITE(yaw_rate); + pos_sp_triplet->current.yawspeed = yaw_rate; _navigator->set_position_setpoint_triplet_updated(); } void FollowTarget::reset_target_validity() { + _yaw_rate = NAN; _previous_target_motion = {}; _current_target_motion = {}; _target_updates = 0; _current_vel.zero(); _step_vel.zero(); - _target_vel.zero(); + _est_target_vel.zero(); _target_distance.zero(); + _target_position_offset.zero(); reset_mission_item_reached(); - _follow_target_state = WAIT_FOR_TARGET_POSITION; + _follow_target_state = SET_WAIT_FOR_TARGET_POSITION; } bool FollowTarget::target_velocity_valid() { - // need at least 2 data points for velocity estimate + // need at least 2 continuous data points for velocity estimate return (_target_updates >= 2); } bool FollowTarget::target_position_valid() { - // need at least 1 data point for position estimate + // need at least 1 continuous data points for position estimate return (_target_updates >= 1); } diff --git a/src/modules/navigator/follow_target.h b/src/modules/navigator/follow_target.h index b018bc58ee..cc6891a8ac 100644 --- a/src/modules/navigator/follow_target.h +++ b/src/modules/navigator/follow_target.h @@ -43,7 +43,7 @@ #include #include #include - +#include #include "navigator_mode.h" #include "mission_block.h" @@ -52,6 +52,10 @@ class FollowTarget : public MissionBlock public: FollowTarget(Navigator *navigator, const char *name); + + FollowTarget(const FollowTarget &) = delete; + FollowTarget &operator=(const FollowTarget &) = delete; + ~FollowTarget(); void on_inactive() override; @@ -60,41 +64,93 @@ public: private: - static constexpr int TARGET_TIMEOUT_S = 5; + static constexpr int TARGET_TIMEOUT_MS = 2500; static constexpr int TARGET_ACCEPTANCE_RADIUS_M = 5; static constexpr int INTERPOLATION_PNTS = 20; - static constexpr float FF_K = .15f; + static constexpr float FF_K = .25F; + static constexpr float OFFSET_M = 8; enum FollowTargetState { TRACK_POSITION, TRACK_VELOCITY, + SET_WAIT_FOR_TARGET_POSITION, WAIT_FOR_TARGET_POSITION }; + enum { + FOLLOW_FROM_RIGHT, + FOLLOW_FROM_BEHIND, + FOLLOW_FROM_FRONT, + FOLLOW_FROM_LEFT + }; + + float _follow_position_matricies[4][9] = { + {1.0F, -1.0F, 0.0F, + 1.0F, 1.0F, 0.0F, + 0.0F, 0.0F, 1.0F}, // follow right + + {-1.0F, 0.0F, 0.0F, + 0.0F, -1.0F, 0.0F, + 0.0F, 0.0F, 1.0F}, // follow behind + + {1.0F, 0.0F, 0.0F, + 0.0F, 1.0F, 0.0F, + 0.0F, 0.0F, 1.0F}, // follow front + + {1.0F, 1.0F, 0.0F, + -1.0F, 1.0F, 0.0F, + 0.0F, 0.0F, 1.0F}}; // follow left side + + Navigator *_navigator; - control::BlockParamFloat _param_min_alt; + control::BlockParamFloat _param_min_alt; + control::BlockParamFloat _param_tracking_dist; + control::BlockParamInt _param_tracking_side; + control::BlockParamFloat _param_tracking_resp; + control::BlockParamFloat _param_yaw_auto_max; + + FollowTargetState _follow_target_state; + int _follow_target_position; + int _follow_target_sub; float _step_time_in_ms; + float _follow_offset; uint64_t _target_updates; - uint64_t _last_update_time; math::Vector<3> _current_vel; math::Vector<3> _step_vel; - math::Vector<3> _target_vel; + math::Vector<3> _est_target_vel; math::Vector<3> _target_distance; + math::Vector<3> _target_position_offset; + math::Vector<3> _target_position_delta; + math::Vector<3> _filtered_target_position_delta; follow_target_s _current_target_motion; follow_target_s _previous_target_motion; + float _yaw_rate; + float _responsiveness; + float _yaw_auto_max; + float _yaw_angle; + // Mavlink defined motion reporting capabilities + + enum { + POS = 0, + VEL = 1, + ACCEL = 2, + ATT_RATES = 3 + }; + + math::Matrix<3,3> _rot_matrix; void track_target_position(); void track_target_velocity(); bool target_velocity_valid(); bool target_position_valid(); void reset_target_validity(); - void update_position_sp(bool velocity_valid, bool position_valid); + void update_position_sp(bool velocity_valid, bool position_valid, float yaw_rate); void update_target_motion(); void update_target_velocity(); }; diff --git a/src/modules/navigator/follow_target_params.c b/src/modules/navigator/follow_target_params.c new file mode 100644 index 0000000000..23c206b8f3 --- /dev/null +++ b/src/modules/navigator/follow_target_params.c @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * Copyright (c) 2016 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 follow_target_params.c + * + * Parameters for follow target mode + * + * @author Jimmy Johnson + */ + +/* + * Follow target parameters + */ + +/** + * Minimum follow target altitude + * + * The minimum height in meters relative to home for following a target + * + * @unit meters + * @min 8.0 + * @group Follow target + */ +PARAM_DEFINE_FLOAT(NAV_MIN_FT_HT, 8.0f); + +/** + * Distance to follow target from + * + * The distance in meters to follow the target at + * + * @unit meters + * @min 1.0 + * @group Follow target + */ +PARAM_DEFINE_FLOAT(NAV_FT_DST, 8.0f); + +/** + * Side to follow target from + * + * The side to follow the target from (front right = 0, behind = 1, front = 2, front left = 3) + * + * @unit n/a + * @min 0 + * @max 3 + * @group Follow target + */ +PARAM_DEFINE_INT32(NAV_FT_FS, 1); + +/** + * Dynamic filtering algorithm responsiveness to target movement + * lower numbers increase the responsiveness to changing long lat + * but also ignore less noise + * + * @unit n/a + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @group Follow target + */ +PARAM_DEFINE_FLOAT(NAV_FT_RS, 0.5f); + diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index f46f849b18..21f5072158 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -59,6 +59,10 @@ class Geofence : public control::SuperBlock { public: Geofence(Navigator *navigator); + + Geofence(const Geofence &) = delete; + Geofence &operator=(const Geofence &) = delete; + ~Geofence(); /* Altitude mode, corresponding to the param GF_ALTMODE */ @@ -131,7 +135,7 @@ private: control::BlockParamInt _param_max_hor_distance; control::BlockParamInt _param_max_ver_distance; - unsigned _outside_counter; + int _outside_counter; bool inside(double lat, double lon, float altitude); bool inside(const struct vehicle_global_position_s &global_position); diff --git a/src/modules/navigator/gpsfailure_params.c b/src/modules/navigator/gpsfailure_params.c index 087dba50bd..7ee5e77080 100644 --- a/src/modules/navigator/gpsfailure_params.c +++ b/src/modules/navigator/gpsfailure_params.c @@ -51,6 +51,9 @@ * * @unit s * @min 0.0 + * @max 3600.0 + * @decimal 0 + * @increment 1 * @group GPS Failure Navigation */ PARAM_DEFINE_FLOAT(NAV_GPSF_LT, 30.0f); @@ -63,6 +66,8 @@ PARAM_DEFINE_FLOAT(NAV_GPSF_LT, 30.0f); * @unit deg * @min 0.0 * @max 30.0 + * @decimal 1 + * @increment 0.5 * @group GPS Failure Navigation */ PARAM_DEFINE_FLOAT(NAV_GPSF_R, 15.0f); @@ -75,6 +80,8 @@ PARAM_DEFINE_FLOAT(NAV_GPSF_R, 15.0f); * @unit deg * @min -30.0 * @max 30.0 + * @decimal 1 + * @increment 0.5 * @group GPS Failure Navigation */ PARAM_DEFINE_FLOAT(NAV_GPSF_P, 0.0f); @@ -84,8 +91,11 @@ PARAM_DEFINE_FLOAT(NAV_GPSF_P, 0.0f); * * Thrust value which is set during the open loop loiter * + * @unit norm * @min 0.0 * @max 1.0 + * @decimal 2 + * @increment 0.05 * @group GPS Failure Navigation */ PARAM_DEFINE_FLOAT(NAV_GPSF_TR, 0.7f); diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 56fa817845..a9e0b96c05 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -149,7 +149,7 @@ Mission::on_inactive() /* Reset work item type to default if auto take-off has been paused or aborted, and we landed in manual mode. */ if (_work_item_type == WORK_ITEM_TYPE_TAKEOFF) { - _work_item_type = WORK_ITEM_TYPE_DEFAULT; + _work_item_type = WORK_ITEM_TYPE_DEFAULT; } } } @@ -348,7 +348,7 @@ Mission::set_mission_items() /* make sure param is up to date */ updateParams(); - /* reset the altitude foh logic, if altitude foh is enabled (param) a new foh element starts now */ + /* reset the altitude foh (first order hold) logic, if altitude foh is enabled (param) a new foh element starts now */ altitude_sp_foh_reset(); struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); @@ -372,7 +372,7 @@ Mission::set_mission_items() if (_param_onboard_enabled.get() && prepare_mission_items(true, &_mission_item, &mission_item_next_position, &has_next_position_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_ONBOARD) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "onboard mission now running"); + mavlink_log_info(_navigator->get_mavlink_log_pub(), "Executing internal mission"); user_feedback_done = true; } _mission_type = MISSION_TYPE_ONBOARD; @@ -381,20 +381,25 @@ Mission::set_mission_items() } else if (prepare_mission_items(false, &_mission_item, &mission_item_next_position, &has_next_position_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_OFFBOARD) { - mavlink_log_info(_navigator->get_mavlink_log_pub(), "offboard mission now running"); + mavlink_log_info(_navigator->get_mavlink_log_pub(), "Executing mission"); user_feedback_done = true; } _mission_type = MISSION_TYPE_OFFBOARD; } else { /* no mission available or mission finished, switch to loiter */ if (_mission_type != MISSION_TYPE_NONE) { - /* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */ - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "mission finished, loitering"); + + if (_navigator->get_land_detected()->landed) { + mavlink_log_info(_navigator->get_mavlink_log_pub(), "mission finished, landed"); + } else { + /* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */ + mavlink_log_info(_navigator->get_mavlink_log_pub(), "mission finished, loitering"); + + /* use last setpoint for loiter */ + _navigator->set_can_loiter_at_sp(true); + } + user_feedback_done = true; - - /* use last setpoint for loiter */ - _navigator->set_can_loiter_at_sp(true); - } _mission_type = MISSION_TYPE_NONE; @@ -442,8 +447,9 @@ Mission::set_mission_items() if (item_contains_position(&_mission_item)) { /* force vtol land */ - if(_mission_item.nav_cmd == NAV_CMD_LAND && _param_force_vtol.get() - && !_navigator->get_vstatus()->is_rotary_wing){ + if (_mission_item.nav_cmd == NAV_CMD_LAND && _navigator->get_vstatus()->is_vtol + && _param_force_vtol.get() && !_navigator->get_vstatus()->is_rotary_wing) { + _mission_item.nav_cmd = NAV_CMD_VTOL_LAND; } @@ -452,6 +458,7 @@ Mission::set_mission_items() /* do takeoff before going to setpoint if needed and not already in takeoff */ if (do_need_takeoff() && _work_item_type != WORK_ITEM_TYPE_TAKEOFF) { + new_work_item_type = WORK_ITEM_TYPE_TAKEOFF; /* use current mission item as next position item */ @@ -481,8 +488,9 @@ Mission::set_mission_items() && _navigator->get_vstatus()->is_rotary_wing && !_navigator->get_land_detected()->landed && has_next_position_item) { + /* check if the vtol_takeoff command is on top of us */ - if(do_need_move_to_takeoff()){ + if (do_need_move_to_takeoff()){ new_work_item_type = WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF; } else { new_work_item_type = WORK_ITEM_TYPE_DEFAULT; @@ -515,6 +523,7 @@ Mission::set_mission_items() if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND && _work_item_type == WORK_ITEM_TYPE_DEFAULT && !_navigator->get_land_detected()->landed) { + new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND; /* use current mission item as next position item */ memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s)); @@ -536,6 +545,7 @@ Mission::set_mission_items() && _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND && !_navigator->get_vstatus()->is_rotary_wing && !_navigator->get_land_detected()->landed) { + _mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION; _mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; _mission_item.autocontinue = true; @@ -546,6 +556,7 @@ Mission::set_mission_items() if (do_need_move_to_land() && (_work_item_type == WORK_ITEM_TYPE_DEFAULT || _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) { + new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND; /* use current mission item as next position item */ @@ -571,6 +582,7 @@ Mission::set_mission_items() /* we just moved to the landing waypoint, now descend */ if (_work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND && _navigator->get_vstatus()->is_rotary_wing) { + new_work_item_type = WORK_ITEM_TYPE_DEFAULT; } @@ -606,7 +618,7 @@ Mission::set_mission_items() /*********************************** set setpoints and check next *********************************************/ - /* set current position setpoint from mission item (is protected agains non-position items) */ + /* set current position setpoint from mission item (is protected against non-position items) */ mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); /* issue command if ready (will do nothing for position mission items) */ @@ -616,7 +628,9 @@ Mission::set_mission_items() _work_item_type = new_work_item_type; /* require takeoff after landing or idle */ - if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { + if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND + || pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { + _need_takeoff = true; } @@ -646,6 +660,7 @@ Mission::set_mission_items() /* Save the distance between the current sp and the previous one */ if (pos_sp_triplet->current.valid && pos_sp_triplet->previous.valid) { + _distance_current_previous = get_distance_to_next_waypoint( pos_sp_triplet->current.lat, pos_sp_triplet->current.lon, @@ -660,6 +675,7 @@ bool Mission::do_need_takeoff() { if (_navigator->get_vstatus()->is_rotary_wing) { + float takeoff_alt = calculate_takeoff_altitude(&_mission_item); /* force takeoff if landed (additional protection) */ @@ -673,13 +689,12 @@ Mission::do_need_takeoff() /* check if current mission item is one that requires takeoff before */ if (_need_takeoff && ( - _mission_item.nav_cmd == NAV_CMD_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_WAYPOINT || - _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF || - _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || - _mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH)) { + _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item.nav_cmd == NAV_CMD_TAKEOFF || + _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT || + _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF)) { _need_takeoff = false; return true; @@ -692,7 +707,8 @@ Mission::do_need_takeoff() bool Mission::do_need_move_to_land() { - if (_navigator->get_vstatus()->is_rotary_wing && (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND)) { + if (_navigator->get_vstatus()->is_rotary_wing + && (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND)) { float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); @@ -756,7 +772,7 @@ Mission::calculate_takeoff_altitude(struct mission_item_s *mission_item) /* calculate takeoff altitude */ float takeoff_alt = get_absolute_altitude_for_item(*mission_item); - /* takeoff to at least NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */ + /* takeoff to at least MIS_TAKEOFF_ALT above home/ground, even if first waypoint is lower */ if (_navigator->get_land_detected()->landed) { takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _param_takeoff_alt.get()); @@ -865,10 +881,14 @@ Mission::altitude_sp_foh_update() } /* Don't do FOH for non-missions, landing and takeoff waypoints, the ground may be near - * and the FW controller has a custom landing logic */ + * and the FW controller has a custom landing logic + * + * NAV_CMD_LOITER_TO_ALT doesn't change altitude until reaching desired lat/lon + * */ if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND || _mission_item.nav_cmd == NAV_CMD_TAKEOFF + || _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT || !_navigator->is_planned_mission()) { return; } @@ -1149,7 +1169,7 @@ Mission::reset_offboard_mission(struct mission_s &mission) if (mission.count > 0) { dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(mission.dataman_id); - for (int index = 0; index < mission.count; index++) { + for (unsigned index = 0; index < mission.count; index++) { struct mission_item_s item; const ssize_t len = sizeof(struct mission_item_s); diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 74dbd59f74..1f840f2930 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -75,6 +75,7 @@ MissionBlock::MissionBlock(Navigator *navigator, const char *name) : _actuators{}, _actuator_pub(nullptr), _cmd_pub(nullptr), + _param_loiter_min_alt(this, "MIS_LTRMIN_ALT", false), _param_yaw_timeout(this, "MIS_YAW_TMT", false), _param_yaw_err(this, "MIS_YAW_ERR", false), _param_vtol_wv_land(this, "VT_WV_LND_EN", false), @@ -98,13 +99,13 @@ MissionBlock::is_mission_item_reached() case NAV_CMD_VTOL_LAND: return _navigator->get_land_detected()->landed; - /* TODO: count turns */ - /*_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||*/ case NAV_CMD_IDLE: /* fall through */ case NAV_CMD_LOITER_UNLIMITED: return false; case NAV_CMD_DO_DIGICAM_CONTROL: + case NAV_CMD_DO_MOUNT_CONFIGURE: + case NAV_CMD_DO_MOUNT_CONTROL: case NAV_CMD_DO_SET_CAM_TRIGG_DIST: return true; @@ -115,19 +116,27 @@ MissionBlock::is_mission_item_reached() */ if (hrt_absolute_time() - _action_start > 500000 && !_navigator->get_vstatus()->in_transition_mode) { + _action_start = 0; return true; } else { return false; } - case vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED: + case NAV_CMD_DO_CHANGE_SPEED: // XXX not differentiating ground and airspeed yet if (_mission_item.params[1] > 0.0f) { _navigator->set_cruising_speed(_mission_item.params[1]); } else { _navigator->set_cruising_speed(); + /* if no speed target was given try to set throttle */ + if (_mission_item.params[2] > 0.0f) { + _navigator->set_cruising_throttle(_mission_item.params[2] / 100); + } else { + _navigator->set_cruising_throttle(); + } } + return true; default: @@ -145,39 +154,93 @@ MissionBlock::is_mission_item_reached() float dist_z = -1.0f; float altitude_amsl = _mission_item.altitude_is_relative - ? _mission_item.altitude + _navigator->get_home_position()->alt - : _mission_item.altitude; + ? _mission_item.altitude + _navigator->get_home_position()->alt + : _mission_item.altitude; dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl, - _navigator->get_global_position()->lat, - _navigator->get_global_position()->lon, - _navigator->get_global_position()->alt, - &dist_xy, &dist_z); + _navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, + _navigator->get_global_position()->alt, + &dist_xy, &dist_z); if ((_mission_item.nav_cmd == NAV_CMD_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) && _navigator->get_vstatus()->is_rotary_wing) { /* require only altitude for takeoff for multicopter, do not use waypoint acceptance radius */ - if (_navigator->get_global_position()->alt > - altitude_amsl - _navigator->get_acceptance_radius()) { + if (_navigator->get_global_position()->alt >= altitude_amsl) { _waypoint_position_reached = true; } } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { /* for takeoff mission items use the parameter for the takeoff acceptance radius */ - if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()) { + if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius() + && dist_z <= _navigator->get_altitude_acceptance_radius()) { _waypoint_position_reached = true; } } else if (!_navigator->get_vstatus()->is_rotary_wing && (_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || - _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT)) { + _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT)) { /* Loiter mission item on a non rotary wing: the aircraft is going to circle the * coordinates with a radius equal to the loiter_radius field. It is not flying * through the waypoint center. * Therefore the item is marked as reached once the system reaches the loiter * radius (+ some margin). Time inside and turn count is handled elsewhere. */ - if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f)) { + if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f) + && dist_z <= _navigator->get_altitude_acceptance_radius()) { + _waypoint_position_reached = true; + } else { + _time_first_inside_orbit = 0; + } + + } else if (!_navigator->get_vstatus()->is_rotary_wing && + (_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT)) { + + + // NAV_CMD_LOITER_TO_ALT only uses mission item altitude once it's in the loiter + // first check if the altitude setpoint is the mission setpoint + struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current; + + if (fabsf(curr_sp->alt - altitude_amsl) >= FLT_EPSILON) { + // check if the initial loiter has been accepted + dist = -1.0f; + dist_xy = -1.0f; + dist_z = -1.0f; + + dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, curr_sp->alt, + _navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, + _navigator->get_global_position()->alt, + &dist_xy, &dist_z); + + if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f) + && dist_z <= _navigator->get_altitude_acceptance_radius()) { + + // now set the loiter to the final altitude in the NAV_CMD_LOITER_TO_ALT mission item + curr_sp->alt = altitude_amsl; + _navigator->set_position_setpoint_triplet_updated(); + } + + } else { + if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f) + && dist_z <= _navigator->get_altitude_acceptance_radius()) { + + _waypoint_position_reached = true; + + // set required yaw from bearing to the next mission item + if (_mission_item.force_heading) { + struct position_setpoint_s next_sp = _navigator->get_position_setpoint_triplet()->next; + + if (next_sp.valid) { + _mission_item.yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, + next_sp.lat, next_sp.lon); + + _waypoint_yaw_reached = false; + } else { + _waypoint_yaw_reached = true; + } + } + } } } else { /* for normal mission items used their acceptance radius */ @@ -188,7 +251,8 @@ MissionBlock::is_mission_item_reached() mission_acceptance_radius = _navigator->get_acceptance_radius(); } - if (dist >= 0.0f && dist <= mission_acceptance_radius) { + if (dist >= 0.0f && dist <= mission_acceptance_radius + && dist_z <= _navigator->get_altitude_acceptance_radius()) { _waypoint_position_reached = true; } } @@ -203,8 +267,9 @@ MissionBlock::is_mission_item_reached() if (_waypoint_position_reached && !_waypoint_yaw_reached) { - /* TODO: removed takeoff, why? */ - if (_navigator->get_vstatus()->is_rotary_wing && PX4_ISFINITE(_mission_item.yaw)) { + if ((_navigator->get_vstatus()->is_rotary_wing + || (_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT && _mission_item.force_heading)) + && PX4_ISFINITE(_mission_item.yaw)) { /* check yaw if defined only for rotary wing except takeoff */ float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw); @@ -232,18 +297,29 @@ MissionBlock::is_mission_item_reached() if (_time_first_inside_orbit == 0) { _time_first_inside_orbit = now; - - // if (_mission_item.time_inside > 0.01f) { - // mavlink_log_critical(_mavlink_log_pub, "waypoint reached, wait for %.1fs", - // (double)_mission_item.time_inside); - // } } /* check if the MAV was long enough inside the waypoint orbit */ if (now - _time_first_inside_orbit >= (hrt_abstime)_mission_item.time_inside * 1e6f) { + + // exit xtrack location + if (_mission_item.loiter_exit_xtrack && + (_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT)) { + + // reset lat/lon of loiter waypoint so vehicle exits on a tangent + struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current; + curr_sp->lat = _navigator->get_global_position()->lat; + curr_sp->lon = _navigator->get_global_position()->lon; + } + return true; } } + + // all acceptance criteria must be met in the same iteration + _waypoint_position_reached = false; + _waypoint_yaw_reached = false; return false; } @@ -288,7 +364,7 @@ MissionBlock::issue_command(const struct mission_item_s *item) PX4_WARN("do_set_servo command"); // XXX: we should issue a vehicle command and handle this somewhere else memset(&actuators, 0, sizeof(actuators)); - // params[0] actuator number to be set 0..5 ( corresponds to AUX outputs 1..6 + // params[0] actuator number to be set 0..5 (corresponds to AUX outputs 1..6) // params[1] new value for selected actuator in ms 900...2000 actuators.control[(int)item->params[0]] = 1.0f / 2000 * -item->params[1]; actuators.timestamp = hrt_absolute_time(); @@ -322,8 +398,9 @@ MissionBlock::item_contains_position(const struct mission_item_s *item) if (item->nav_cmd == NAV_CMD_DO_JUMP || item->nav_cmd == NAV_CMD_DO_CHANGE_SPEED || item->nav_cmd == NAV_CMD_DO_SET_SERVO || - item->nav_cmd == NAV_CMD_DO_REPEAT_SERVO || item->nav_cmd == NAV_CMD_DO_DIGICAM_CONTROL || + item->nav_cmd == NAV_CMD_DO_MOUNT_CONFIGURE || + item->nav_cmd == NAV_CMD_DO_MOUNT_CONTROL || item->nav_cmd == NAV_CMD_DO_SET_CAM_TRIGG_DIST || item->nav_cmd == NAV_CMD_DO_VTOL_TRANSITION) { return false; @@ -337,8 +414,9 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite { /* set the correct setpoint for vtol transition */ - if(item->nav_cmd == NAV_CMD_DO_VTOL_TRANSITION && PX4_ISFINITE(item->yaw) + if (item->nav_cmd == NAV_CMD_DO_VTOL_TRANSITION && PX4_ISFINITE(item->yaw) && item->params[0] >= vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW - 0.5f) { + sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION; waypoint_from_heading_and_distance(_navigator->get_global_position()->lat, _navigator->get_global_position()->lon, @@ -355,7 +433,6 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite return; } - sp->valid = true; sp->lat = item->lat; sp->lon = item->lon; sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude; @@ -367,6 +444,7 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite sp->acceptance_radius = item->acceptance_radius; sp->disable_mc_yaw_control = false; sp->cruising_speed = _navigator->get_cruising_speed(); + sp->cruising_throttle = _navigator->get_cruising_throttle(); switch (item->nav_cmd) { case NAV_CMD_IDLE: @@ -381,16 +459,20 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite case NAV_CMD_LAND: case NAV_CMD_VTOL_LAND: sp->type = position_setpoint_s::SETPOINT_TYPE_LAND; - if(_navigator->get_vstatus()->is_vtol && _param_vtol_wv_land.get()){ + if (_navigator->get_vstatus()->is_vtol && _param_vtol_wv_land.get()) { sp->disable_mc_yaw_control = true; } break; + case NAV_CMD_LOITER_TO_ALT: + // initially use current altitude, and switch to mission item altitude once in loiter position + sp->alt = math::max(_navigator->get_global_position()->alt, _navigator->get_home_position()->alt + _param_loiter_min_alt.get()); + + // no break case NAV_CMD_LOITER_TIME_LIMIT: - case NAV_CMD_LOITER_TURN_COUNT: case NAV_CMD_LOITER_UNLIMITED: sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER; - if(_navigator->get_vstatus()->is_vtol && _param_vtol_wv_loiter.get()){ + if (_navigator->get_vstatus()->is_vtol && _param_vtol_wv_loiter.get()) { sp->disable_mc_yaw_control = true; } break; @@ -399,6 +481,8 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION; break; } + + sp->valid = true; } void @@ -461,7 +545,7 @@ MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clea } else { - item->nav_cmd = NAV_CMD_FOLLOW_TARGET; + item->nav_cmd = NAV_CMD_DO_FOLLOW_REPOSITION; /* use current target position */ @@ -469,7 +553,7 @@ MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clea item->lon = target.lon; item->altitude = _navigator->get_home_position()->alt; - if (min_clearance > 0.0f) { + if (min_clearance > 8.0f) { item->altitude += min_clearance; } else { item->altitude += 8.0f; // if min clearance is bad set it to 8.0 meters (well above the average height of a person) @@ -521,7 +605,7 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio { /* VTOL transition to RW before landing */ - if(_navigator->get_vstatus()->is_vtol && !_navigator->get_vstatus()->is_rotary_wing){ + if (_navigator->get_vstatus()->is_vtol && !_navigator->get_vstatus()->is_rotary_wing) { struct vehicle_command_s cmd = {}; cmd.command = NAV_CMD_DO_VTOL_TRANSITION; cmd.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; @@ -594,5 +678,3 @@ MissionBlock::set_idle_item(struct mission_item_s *item) item->autocontinue = true; item->origin = ORIGIN_ONBOARD; } - - diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 5f6e00a764..54da136d52 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -64,6 +64,9 @@ public: */ MissionBlock(Navigator *navigator, const char *name); + MissionBlock(const MissionBlock &) = delete; + MissionBlock &operator=(const MissionBlock &) = delete; + /** * Destructor */ @@ -141,6 +144,7 @@ protected: orb_advert_t _actuator_pub; orb_advert_t _cmd_pub; + control::BlockParamFloat _param_loiter_min_alt; control::BlockParamFloat _param_yaw_timeout; control::BlockParamFloat _param_yaw_err; control::BlockParamInt _param_vtol_wv_land; diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 63c441d8b8..8cbb3ef599 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -55,11 +55,11 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_log_pub(nullptr), - _capabilities_sub(-1), + _fw_pos_ctrl_status_sub(-1), _initDone(false), _dist_1wp_ok(false) { - _nav_caps = {0}; + _fw_pos_ctrl_status = {}; } @@ -238,17 +238,18 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s if (missionitem.nav_cmd != NAV_CMD_IDLE && missionitem.nav_cmd != NAV_CMD_WAYPOINT && missionitem.nav_cmd != NAV_CMD_LOITER_UNLIMITED && - /* not yet supported: missionitem.nav_cmd != NAV_CMD_LOITER_TURN_COUNT && */ missionitem.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && missionitem.nav_cmd != NAV_CMD_LAND && missionitem.nav_cmd != NAV_CMD_TAKEOFF && - missionitem.nav_cmd != NAV_CMD_VTOL_LAND && + missionitem.nav_cmd != NAV_CMD_LOITER_TO_ALT && missionitem.nav_cmd != NAV_CMD_VTOL_TAKEOFF && - missionitem.nav_cmd != NAV_CMD_PATHPLANNING && + missionitem.nav_cmd != NAV_CMD_VTOL_LAND && missionitem.nav_cmd != NAV_CMD_DO_JUMP && missionitem.nav_cmd != NAV_CMD_DO_SET_SERVO && missionitem.nav_cmd != NAV_CMD_DO_CHANGE_SPEED && missionitem.nav_cmd != NAV_CMD_DO_DIGICAM_CONTROL && + missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONFIGURE && + missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONTROL && missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_DIST && missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION) { @@ -275,7 +276,6 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size /* Go through all mission items and search for a landing waypoint * if landing waypoint is found: the previous waypoint is checked to be at a feasible distance and altitude given the landing slope */ - for (size_t i = 0; i < nMissionItems; i++) { struct mission_item_s missionitem; const ssize_t len = sizeof(missionitem); @@ -293,15 +293,15 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size } float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat , missionitem_previous.lon, missionitem.lat, missionitem.lon); - float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude, _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad); - float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude, missionitem.altitude, _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad); + float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude, _fw_pos_ctrl_status.landing_horizontal_slope_displacement, _fw_pos_ctrl_status.landing_slope_angle_rad); + float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude, missionitem.altitude, _fw_pos_ctrl_status.landing_horizontal_slope_displacement, _fw_pos_ctrl_status.landing_slope_angle_rad); float delta_altitude = missionitem.altitude - missionitem_previous.altitude; // warnx("wp_distance %.2f, delta_altitude %.2f, missionitem_previous.altitude %.2f, missionitem.altitude %.2f, slope_alt_req %.2f, wp_distance_req %.2f", // wp_distance, delta_altitude, missionitem_previous.altitude, missionitem.altitude, slope_alt_req, wp_distance_req); // warnx("_nav_caps.landing_horizontal_slope_displacement %.4f, _nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_flare_length %.4f", // _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad, _nav_caps.landing_flare_length); - if (wp_distance > _nav_caps.landing_flare_length) { + if (wp_distance > _fw_pos_ctrl_status.landing_flare_length) { /* Last wp is before flare region */ if (delta_altitude < 0) { @@ -324,11 +324,11 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size } else { /* Last wp is in flare region */ //xxx give recommendations - mavlink_log_critical(_mavlink_log_pub, "Warning: Landing: last waypoint in flare region"); + mavlink_log_critical(_mavlink_log_pub, "Last waypoint too close to landing waypoint"); return false; } } else { - mavlink_log_critical(_mavlink_log_pub, "Warning: starting with land waypoint"); + mavlink_log_critical(_mavlink_log_pub, "Invalid mission: starts with land waypoint"); return false; } } @@ -408,13 +408,15 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI bool MissionFeasibilityChecker::isPositionCommand(unsigned cmd){ - if( cmd == NAV_CMD_WAYPOINT || - cmd == NAV_CMD_LOITER_TIME_LIMIT || - cmd == NAV_CMD_LOITER_TURN_COUNT || + if (cmd == NAV_CMD_WAYPOINT || cmd == NAV_CMD_LOITER_UNLIMITED || - cmd == NAV_CMD_TAKEOFF || + cmd == NAV_CMD_LOITER_TIME_LIMIT || cmd == NAV_CMD_LAND || - cmd == NAV_CMD_PATHPLANNING) { + cmd == NAV_CMD_TAKEOFF || + cmd == NAV_CMD_LOITER_TO_ALT || + cmd == NAV_CMD_VTOL_TAKEOFF || + cmd == NAV_CMD_VTOL_LAND) { + return true; } else { return false; @@ -424,14 +426,14 @@ MissionFeasibilityChecker::isPositionCommand(unsigned cmd){ void MissionFeasibilityChecker::updateNavigationCapabilities() { - (void)orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); + (void)orb_copy(ORB_ID(fw_pos_ctrl_status), _fw_pos_ctrl_status_sub, &_fw_pos_ctrl_status); } void MissionFeasibilityChecker::init() { if (!_initDone) { - _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); + _fw_pos_ctrl_status_sub = orb_subscribe(ORB_ID(fw_pos_ctrl_status)); _initDone = true; } diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index b372cd3219..6c5dda73ab 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -44,7 +44,7 @@ #include #include -#include +#include #include #include "geofence.h" @@ -54,8 +54,8 @@ class MissionFeasibilityChecker private: orb_advert_t *_mavlink_log_pub; - int _capabilities_sub; - struct navigation_capabilities_s _nav_caps; + int _fw_pos_ctrl_status_sub; + struct fw_pos_ctrl_status_s _fw_pos_ctrl_status; bool _initDone; bool _dist_1wp_ok; @@ -78,6 +78,10 @@ private: public: MissionFeasibilityChecker(); + + MissionFeasibilityChecker(const MissionFeasibilityChecker &) = delete; + MissionFeasibilityChecker &operator=(const MissionFeasibilityChecker &) = delete; + ~MissionFeasibilityChecker() {} /* diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 217eea9eb1..3cc58dd953 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -51,6 +51,8 @@ * @unit m * @min 0 * @max 80 + * @decimal 1 + * @increment 0.5 * @group Mission */ PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f); @@ -63,12 +65,14 @@ PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f); * @unit m * @min 0 * @max 80 + * @decimal 1 + * @increment 0.5 * @group Mission */ PARAM_DEFINE_FLOAT(MIS_LTRMIN_ALT, 1.2f); /** - * Enable persistent onboard mission storage + * Persistent onboard mission storage * * When enabled, missions that have been uploaded by the GCS are stored * and reloaded after reboot persistently. @@ -88,6 +92,8 @@ PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 1); * @unit m * @min 0 * @max 1000 + * @decimal 1 + * @increment 0.5 * @group Mission */ PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 900); @@ -133,18 +139,50 @@ PARAM_DEFINE_INT32(MIS_YAWMODE, 1); * @unit s * @min -1 * @max 20 + * @decimal 1 * @increment 1 * @group Mission */ PARAM_DEFINE_FLOAT(MIS_YAW_TMT, -1.0f); /** - * Max yaw error in degree needed for waypoint heading acceptance. + * Max yaw error in degrees needed for waypoint heading acceptance. * * @unit deg * @min 0 * @max 90 + * @decimal 1 * @increment 1 * @group Mission */ PARAM_DEFINE_FLOAT(MIS_YAW_ERR, 12.0f); + +/** + * Weather-vane mode landings for missions + * + * @boolean + * @group Mission + */ +PARAM_DEFINE_INT32(VT_WV_LND_EN, 0); + +/** + * Weather-vane mode for loiter mode + * + * @boolean + * @group Mission + */ +PARAM_DEFINE_INT32(VT_WV_LTR_EN, 0); + +/** + * Cruise Airspeed + * + * The fixed wing controller tries to fly at this airspeed. + * + * @unit m/s + * @min 0.0 + * @max 40 + * @decimal 1 + * @increment 0.5 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 15.0f); diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index a7da600330..85a4dd8c22 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -54,22 +54,19 @@ enum NAV_CMD { NAV_CMD_IDLE = 0, NAV_CMD_WAYPOINT = 16, NAV_CMD_LOITER_UNLIMITED = 17, - NAV_CMD_LOITER_TURN_COUNT = 18, NAV_CMD_LOITER_TIME_LIMIT = 19, - NAV_CMD_RETURN_TO_LAUNCH = 20, NAV_CMD_LAND = 21, NAV_CMD_TAKEOFF = 22, - NAV_CMD_ROI = 80, - NAV_CMD_PATHPLANNING = 81, - NAV_CMD_FOLLOW_TARGET = 194, // temporary placeholder - NAV_CMD_GOTO_TAREGT = 195, + NAV_CMD_LOITER_TO_ALT = 31, + NAV_CMD_DO_FOLLOW_REPOSITION = 33, NAV_CMD_VTOL_TAKEOFF = 84, NAV_CMD_VTOL_LAND = 85, NAV_CMD_DO_JUMP = 177, NAV_CMD_DO_CHANGE_SPEED = 178, NAV_CMD_DO_SET_SERVO=183, - NAV_CMD_DO_REPEAT_SERVO=184, NAV_CMD_DO_DIGICAM_CONTROL=203, + NAV_CMD_DO_MOUNT_CONFIGURE=204, + NAV_CMD_DO_MOUNT_CONTROL=205, NAV_CMD_DO_SET_CAM_TRIGG_DIST=206, NAV_CMD_DO_VTOL_TRANSITION=3000, NAV_CMD_INVALID=UINT16_MAX /* ensure that casting a large number results in a specific error */ @@ -100,7 +97,8 @@ struct mission_item_s { float yaw; /**< in radians NED -PI..+PI, NAN means don't change yaw */ float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover */ int8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */ - unsigned nav_cmd; /**< navigation command */ + bool loiter_exit_xtrack; /**< exit xtrack location: 0 for center of loiter wp, 1 for exit location */ + enum NAV_CMD nav_cmd; /**< navigation command */ float acceptance_radius; /**< default radius in which the mission is accepted as reached in meters */ float time_inside; /**< time that the MAV should stay inside the radius before advancing in seconds */ float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ @@ -120,8 +118,5 @@ struct mission_item_s { * @} */ -/* register this as object request broker structure */ -ORB_DECLARE(offboard_mission); -ORB_DECLARE(onboard_mission); #endif diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 61cb005893..7e793b99d9 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -166,6 +166,13 @@ public: */ float get_acceptance_radius(); + /** + * Get the altitude acceptance radius + * + * @return the distance from the target altitude before considering the waypoint reached + */ + float get_altitude_acceptance_radius(); + /** * Get the cruising speed * @@ -178,6 +185,19 @@ public: */ void set_cruising_speed(float speed=-1.0f) { _mission_cruising_speed = speed; } + + /** + * Get the target throttle + * + * @return the desired throttle for this mission + */ + float get_cruising_throttle(); + + /** + * Set the target throttle + */ + void set_cruising_throttle(float throttle=-1.0f) { _mission_throttle = throttle; } + /** * Get the acceptance radius given the mission item preset radius * @@ -207,7 +227,7 @@ private: int _home_pos_sub; /**< home position subscription */ int _vstatus_sub; /**< vehicle status subscription */ int _land_detected_sub; /**< vehicle land detected subscription */ - int _capabilities_sub; /**< notification of vehicle capabilities updates */ + int _fw_pos_ctrl_status_sub; /**< notification of vehicle capabilities updates */ int _control_mode_sub; /**< vehicle control mode subscription */ int _onboard_mission_sub; /**< onboard mission subscription */ int _offboard_mission_sub; /**< offboard mission subscription */ @@ -229,7 +249,7 @@ private: sensor_combined_s _sensor_combined; /**< sensor values */ home_position_s _home_pos; /**< home position for RTL */ mission_item_s _mission_item; /**< current mission item */ - navigation_capabilities_s _nav_caps; /**< navigation capabilities */ + fw_pos_ctrl_status_s _fw_pos_ctrl_status; /**< fixed wing navigation capabilities */ position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ position_setpoint_triplet_s _reposition_triplet; /**< triplet for non-mission direct position command */ position_setpoint_triplet_s _takeoff_triplet; /**< triplet for non-mission direct takeoff command */ @@ -272,13 +292,17 @@ private: control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */ control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */ + control::BlockParamFloat _param_fw_alt_acceptance_radius; /**< acceptance radius for fixedwing altitude */ + control::BlockParamFloat _param_mc_alt_acceptance_radius; /**< acceptance radius for multicopter altitude */ control::BlockParamInt _param_datalinkloss_act; /**< select data link loss action */ control::BlockParamInt _param_rcloss_act; /**< select data link loss action */ control::BlockParamFloat _param_cruising_speed_hover; control::BlockParamFloat _param_cruising_speed_plane; + control::BlockParamFloat _param_cruising_throttle_plane; float _mission_cruising_speed; + float _mission_throttle; /** * Retrieve global position @@ -301,9 +325,9 @@ private: void home_position_update(bool force=false); /** - * Retreive navigation capabilities + * Retrieve fixed wing navigation capabilities */ - void navigation_capabilities_update(); + void fw_pos_ctrl_status_update(); /** * Retrieve vehicle status diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index d5d0095d64..6804fe1be0 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -70,7 +70,7 @@ #include #include #include -#include +#include #include #include @@ -105,10 +105,11 @@ Navigator::Navigator() : _mavlink_log_pub(nullptr), _global_pos_sub(-1), _gps_pos_sub(-1), + _sensor_combined_sub(-1), _home_pos_sub(-1), _vstatus_sub(-1), _land_detected_sub(-1), - _capabilities_sub(-1), + _fw_pos_ctrl_status_sub(-1), _control_mode_sub(-1), _onboard_mission_sub(-1), _offboard_mission_sub(-1), @@ -126,7 +127,7 @@ Navigator::Navigator() : _sensor_combined{}, _home_pos{}, _mission_item{}, - _nav_caps{}, + _fw_pos_ctrl_status{}, _pos_sp_triplet{}, _reposition_triplet{}, _takeoff_triplet{}, @@ -155,11 +156,15 @@ Navigator::Navigator() : _follow_target(this, "TAR"), _param_loiter_radius(this, "LOITER_RAD"), _param_acceptance_radius(this, "ACC_RAD"), + _param_fw_alt_acceptance_radius(this, "FW_ALT_RAD"), + _param_mc_alt_acceptance_radius(this, "MC_ALT_RAD"), _param_datalinkloss_act(this, "DLL_ACT"), _param_rcloss_act(this, "RCL_ACT"), _param_cruising_speed_hover(this, "MPC_XY_CRUISE", false), _param_cruising_speed_plane(this, "FW_AIRSPD_TRIM", false), - _mission_cruising_speed(-1.0f) + _param_cruising_throttle_plane(this, "FW_THR_CRUISE", false), + _mission_cruising_speed(-1.0f), + _mission_throttle(-1.0f) { /* Create a list of our possible navigation types */ _navigation_mode_array[0] = &_mission; @@ -231,9 +236,9 @@ Navigator::home_position_update(bool force) } void -Navigator::navigation_capabilities_update() +Navigator::fw_pos_ctrl_status_update() { - orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); + orb_copy(ORB_ID(fw_pos_ctrl_status), _fw_pos_ctrl_status_sub, &_fw_pos_ctrl_status); } void @@ -298,7 +303,7 @@ Navigator::task_main() _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); - _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); + _fw_pos_ctrl_status_sub = orb_subscribe(ORB_ID(fw_pos_ctrl_status)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); @@ -316,7 +321,7 @@ Navigator::task_main() gps_position_update(); sensor_combined_update(); home_position_update(true); - navigation_capabilities_update(); + fw_pos_ctrl_status_update(); params_update(); /* wakeup source(s) */ @@ -338,18 +343,20 @@ Navigator::task_main() if (pret == 0) { /* timed out - periodic check for _task_should_exit, etc. */ if (global_pos_available_once) { - PX4_WARN("navigator timed out"); + global_pos_available_once = false; + PX4_WARN("navigator: global position timeout"); } - continue; + /* Let the loop run anyway, don't do `continue` here. */ } else if (pret < 0) { /* this is undesirable but not much we can do - might want to flag unhappy status */ PX4_WARN("nav: poll error %d, %d", pret, errno); continue; + } else { + /* success, global pos was available */ + global_pos_available_once = true; } - global_pos_available_once = true; - perf_begin(_loop_perf); bool updated; @@ -395,9 +402,9 @@ Navigator::task_main() } /* navigation capabilities updated */ - orb_check(_capabilities_sub, &updated); + orb_check(_fw_pos_ctrl_status_sub, &updated); if (updated) { - navigation_capabilities_update(); + fw_pos_ctrl_status_update(); } /* home position updated */ @@ -433,8 +440,9 @@ Navigator::task_main() } if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) { - rep->current.lat = cmd.param5 / (double)1e7; - rep->current.lon = cmd.param6 / (double)1e7; + rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7; + rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7; + } else { rep->current.lat = get_global_position()->lat; rep->current.lon = get_global_position()->lon; @@ -462,8 +470,16 @@ Navigator::task_main() rep->current.loiter_direction = 1; rep->current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; rep->current.yaw = cmd.param4; - rep->current.lat = cmd.param5 / (double)1e7; - rep->current.lon = cmd.param6 / (double)1e7; + + if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) { + rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7; + rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7; + } else { + // If one of them is non-finite, reset both + rep->current.lat = NAN; + rep->current.lon = NAN; + } + rep->current.alt = cmd.param7; rep->previous.valid = true; @@ -488,7 +504,7 @@ Navigator::task_main() if (have_geofence_position_data && (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) && (hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) { - bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter[0], _home_pos, home_position_valid()); + bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos, home_position_valid()); last_geofence_check = hrt_absolute_time(); have_geofence_position_data = false; @@ -524,7 +540,7 @@ Navigator::task_main() _can_loiter_at_sp = false; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: - if (_nav_caps.abort_landing) { + if (_fw_pos_ctrl_status.abort_landing) { // pos controller aborted landing, requests loiter // above landing waypoint _navigation_mode = &_loiter; @@ -639,7 +655,7 @@ Navigator::start() _navigator_task = px4_task_spawn_cmd("navigator", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT + 5, - 1500, + 1300, (px4_main_t)&Navigator::task_main_trampoline, nullptr); @@ -710,6 +726,18 @@ Navigator::get_acceptance_radius() return get_acceptance_radius(_param_acceptance_radius.get()); } +float +Navigator::get_altitude_acceptance_radius() +{ + if (!this->get_vstatus()->is_rotary_wing) { + return _param_fw_alt_acceptance_radius.get(); + } else { + return _param_mc_alt_acceptance_radius.get(); + } +} + + + float Navigator::get_cruising_speed() { @@ -723,6 +751,17 @@ Navigator::get_cruising_speed() } } +float +Navigator::get_cruising_throttle() +{ + /* Return the mission-requested cruise speed, or default FW_THR_CRUISE value */ + if (_mission_throttle > 0.0f) { + return _mission_throttle; + } else { + return _param_cruising_throttle_plane.get(); + } +} + float Navigator::get_acceptance_radius(float mission_item_radius) { @@ -732,9 +771,9 @@ Navigator::get_acceptance_radius(float mission_item_radius) // when in fixed wing mode // this might need locking against a commanded transition // so that a stale _vstatus doesn't trigger an accepted mission item. - if (!_vstatus.is_rotary_wing && !_vstatus.in_transition_mode && hrt_elapsed_time(&_nav_caps.timestamp) < 5000000) { - if (_nav_caps.turn_distance > radius) { - radius = _nav_caps.turn_distance; + if (!_vstatus.is_rotary_wing && !_vstatus.in_transition_mode) { + if ((hrt_elapsed_time(&_fw_pos_ctrl_status.timestamp) < 5000000) && (_fw_pos_ctrl_status.turn_distance > radius)) { + radius = _fw_pos_ctrl_status.turn_distance; } } diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 5b02b73aa1..3b93795b66 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -48,6 +48,8 @@ * @unit m * @min 25 * @max 1000 + * @decimal 1 + * @increment 0.5 * @group Mission */ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); @@ -56,14 +58,45 @@ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); * Acceptance Radius * * Default acceptance radius, overridden by acceptance radius of waypoint if set. + * For fixed wing the L1 turning distance is used for horizontal acceptance. * * @unit m * @min 0.05 * @max 200.0 + * @decimal 1 + * @increment 0.5 * @group Mission */ PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 10.0f); +/** + * FW Altitude Acceptance Radius + * + * Acceptance radius for fixedwing altitude. + * + * @unit m + * @min 0.05 + * @max 200.0 + * @decimal 1 + * @increment 0.5 + * @group Mission + */ +PARAM_DEFINE_FLOAT(NAV_FW_ALT_RAD, 10.0f); + +/** + * MC Altitude Acceptance Radius + * + * Acceptance radius for multicopter altitude. + * + * @unit m + * @min 0.05 + * @max 200.0 + * @decimal 1 + * @increment 0.5 + * @group Mission + */ +PARAM_DEFINE_FLOAT(NAV_MC_ALT_RAD, 3.0f); + /** * Set data link loss failsafe mode * @@ -98,7 +131,7 @@ PARAM_DEFINE_INT32(NAV_DLL_ACT, 0); * * @group Mission */ -PARAM_DEFINE_INT32(NAV_RCL_ACT, 0); +PARAM_DEFINE_INT32(NAV_RCL_ACT, 2); /** * Airfield home Lat @@ -131,6 +164,8 @@ PARAM_DEFINE_INT32(NAV_AH_LON, 1518423250); * * @unit m * @min -50 + * @decimal 1 + * @increment 0.5 * @group Data Link Loss */ PARAM_DEFINE_FLOAT(NAV_AH_ALT, 600.0f); diff --git a/src/modules/position_estimator_inav/CMakeLists.txt b/src/modules/position_estimator_inav/CMakeLists.txt index 518acd712a..10fcf57043 100644 --- a/src/modules/position_estimator_inav/CMakeLists.txt +++ b/src/modules/position_estimator_inav/CMakeLists.txt @@ -36,7 +36,7 @@ px4_add_module( MAIN position_estimator_inav STACK_MAIN 1200 STACK_MAX 4000 - COMPILE_FLAGS + COMPILE_FLAGS -Os SRCS position_estimator_inav_main.cpp position_estimator_inav_params.cpp diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp index 114bdee530..6c2f670762 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -52,10 +52,6 @@ #include #include #include -#include -#include -#include -#include #include #include #include @@ -155,7 +151,7 @@ int position_estimator_inav_main(int argc, char *argv[]) thread_should_exit = false; position_estimator_inav_task = px4_task_spawn_cmd("position_estimator_inav", - SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5300, + SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 4600, position_estimator_inav_thread_main, (argv && argc > 2) ? (char *const *) &argv[2] : (char *const *) NULL); return 0; @@ -429,14 +425,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (fds_init[0].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - if (wait_baro && sensor.baro_timestamp[0] != baro_timestamp) { - baro_timestamp = sensor.baro_timestamp[0]; + if (wait_baro && sensor.timestamp + sensor.baro_timestamp_relative != baro_timestamp) { + baro_timestamp = sensor.timestamp + sensor.baro_timestamp_relative; baro_wait_for_sample_time = hrt_absolute_time(); /* mean calculation over several measurements */ if (baro_init_cnt < baro_init_num) { - if (PX4_ISFINITE(sensor.baro_alt_meter[0])) { - baro_offset += sensor.baro_alt_meter[0]; + if (PX4_ISFINITE(sensor.baro_alt_meter)) { + baro_offset += sensor.baro_alt_meter; baro_init_cnt++; } @@ -506,7 +502,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - if (sensor.accelerometer_timestamp[0] != accel_timestamp) { + if (sensor.timestamp + sensor.accelerometer_timestamp_relative != accel_timestamp) { if (att.R_valid) { /* correct accel bias */ sensor.accelerometer_m_s2[0] -= acc_bias[0]; @@ -528,13 +524,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memset(acc, 0, sizeof(acc)); } - accel_timestamp = sensor.accelerometer_timestamp[0]; + accel_timestamp = sensor.timestamp + sensor.accelerometer_timestamp_relative; accel_updates++; } - if (sensor.baro_timestamp[0] != baro_timestamp) { - corr_baro = baro_offset - sensor.baro_alt_meter[0] - z_est[0]; - baro_timestamp = sensor.baro_timestamp[0]; + if (sensor.timestamp + sensor.baro_timestamp_relative != baro_timestamp) { + corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0]; + baro_timestamp = sensor.timestamp + sensor.baro_timestamp_relative; baro_updates++; } } @@ -661,9 +657,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) orb_copy(ORB_ID(vehicle_rates_setpoint), vehicle_rate_sp_sub, &rates_setpoint); - double rate_threshold = 0.15f; + float rate_threshold = 0.15f; - if (fabs(rates_setpoint.pitch) < rate_threshold) { + if (fabsf(rates_setpoint.pitch) < rate_threshold) { //warnx("[inav] test ohne comp"); flow_ang[0] = (flow.pixel_flow_x_integral / (float)flow.integration_timespan * 1000000.0f) * params.flow_k;//for now the flow has to be scaled (to small) } @@ -674,7 +670,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) + gyro_offset_filtered[0]) * params.flow_k;//for now the flow has to be scaled (to small) } - if (fabs(rates_setpoint.roll) < rate_threshold) { + if (fabsf(rates_setpoint.roll) < rate_threshold) { flow_ang[1] = (flow.pixel_flow_y_integral / (float)flow.integration_timespan * 1000000.0f) * params.flow_k;//for now the flow has to be scaled (to small) } else { @@ -685,7 +681,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* flow measurements vector */ float flow_m[3]; - if (fabs(rates_setpoint.yaw) < rate_threshold) { + if (fabsf(rates_setpoint.yaw) < rate_threshold) { flow_m[0] = -flow_ang[0] * flow_dist; flow_m[1] = -flow_ang[1] * flow_dist; } else { @@ -773,8 +769,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) static hrt_abstime last_vision_time = 0; - float vision_dt = (vision.timestamp_boot - last_vision_time) / 1e6f; - last_vision_time = vision.timestamp_boot; + float vision_dt = (vision.timestamp - last_vision_time) / 1e6f; + last_vision_time = vision.timestamp; if (vision_dt > 0.000001f && vision_dt < 0.2f) { vision.vx = (vision.x - last_vision_x) / vision_dt; @@ -948,21 +944,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } /* check for timeout on GPS topic */ - if (gps_valid && (t > (gps.timestamp_position + gps_topic_timeout))) { + if (gps_valid && (t > (gps.timestamp + gps_topic_timeout))) { gps_valid = false; warnx("GPS timeout"); mavlink_log_info(&mavlink_log_pub, "[inav] GPS timeout"); } /* check for timeout on vision topic */ - if (vision_valid && (t > (vision.timestamp_boot + vision_topic_timeout))) { + if (vision_valid && (t > (vision.timestamp + vision_topic_timeout))) { vision_valid = false; warnx("VISION timeout"); mavlink_log_info(&mavlink_log_pub, "[inav] VISION timeout"); } /* check for timeout on mocap topic */ - if (mocap_valid && (t > (mocap.timestamp_boot + mocap_topic_timeout))) { + if (mocap_valid && (t > (mocap.timestamp + mocap_topic_timeout))) { mocap_valid = false; warnx("MOCAP timeout"); mavlink_log_info(&mavlink_log_pub, "[inav] MOCAP timeout"); @@ -1216,7 +1212,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p); inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_p); - if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_topic_timeout) { + if (gps.vel_ned_valid && t < gps.timestamp + gps_topic_timeout) { inertial_filter_correct(corr_gps[0][1], dt, x_est, 1, w_xy_gps_v); inertial_filter_correct(corr_gps[1][1], dt, y_est, 1, w_xy_gps_v); } @@ -1366,7 +1362,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) global_pos.terrain_alt_valid = false; } - global_pos.pressure_alt = sensor.baro_alt_meter[0]; + global_pos.pressure_alt = sensor.baro_alt_meter; if (vehicle_global_position_pub == NULL) { vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.cpp b/src/modules/position_estimator_inav/position_estimator_inav_params.cpp index 854fc61638..331e1ba004 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.cpp @@ -302,19 +302,18 @@ PARAM_DEFINE_FLOAT(INAV_FLOW_DIST_X, 0.0f); PARAM_DEFINE_FLOAT(INAV_FLOW_DIST_Y, 0.0f); /** - * Disable mocap (set 0 if using fake gps) + * Mo-cap * - * Disable mocap + * Set to 0 if using fake GPS * - * @boolean + * @value 0 Mo-cap enabled + * @value 1 Mo-cap disabled * @group Position Estimator INAV */ PARAM_DEFINE_FLOAT(INAV_DISAB_MOCAP, 0); /** - * Enable LIDAR for altitude estimation - * - * Enable LIDAR for altitude estimation + * LIDAR for altitude estimation * * @boolean * @group Position Estimator INAV diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index b857aa41fa..91476c95f6 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -51,7 +51,6 @@ #include "px4io.h" -#define RC_FAILSAFE_TIMEOUT 2000000 /**< two seconds failsafe timeout */ #define RC_CHANNEL_HIGH_THRESH 5000 /* 75% threshold */ #define RC_CHANNEL_LOW_THRESH -8000 /* 10% threshold */ @@ -482,14 +481,13 @@ controls_tick() /* * Check for manual override. * - * The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag must be set, and we - * must have R/C input (NO FAILSAFE!). - * Override is enabled if either the hardcoded channel / value combination - * is selected, or the AP has requested it. + * Firstly, manual override must be enabled, RC input available and a mixer loaded. */ - if ((!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK)) && + if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && - !(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { + !(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) && + !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { bool override = false; @@ -500,29 +498,28 @@ controls_tick() * requested override. * */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH)) { + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && + (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH)) { override = true; } /* - if the FMU is dead then enable override if we have a - mixer and OVERRIDE_IMMEDIATE is set + * If the FMU is dead then enable override if we have a mixer + * and we want to immediately override (instead of using the RC channel + * as in the case above. + * + * Also, do not enter manual override if we asked for termination + * failsafe and FMU is lost. */ if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_setup_arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { + !(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)) { override = true; } if (override) { - r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; - /* mix new RC input control values to servos */ - if (dsm_updated || sbus_updated || ppm_updated || st24_updated || sumd_updated) { - mixer_tick(); - } - } else { r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE); } @@ -542,7 +539,7 @@ ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len) } /* avoid racing with PPM updates */ - irqstate_t state = irqsave(); + irqstate_t state = px4_enter_critical_section(); /* * If we have received a new PPM frame within the last 200ms, accept it @@ -571,7 +568,7 @@ ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len) result = (*num_values > 0); } - irqrestore(state); + px4_leave_critical_section(state); return result; } diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c index 74692a2675..75aee69cac 100644 --- a/src/modules/px4iofirmware/i2c.c +++ b/src/modules/px4iofirmware/i2c.c @@ -112,8 +112,8 @@ interface_init(void) modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_I2C1RST, 0); /* configure the i2c GPIOs */ - stm32_configgpio(GPIO_I2C1_SCL); - stm32_configgpio(GPIO_I2C1_SDA); + px4_arch_configgpio(GPIO_I2C1_SCL); + px4_arch_configgpio(GPIO_I2C1_SDA); /* soft-reset the block */ rCR1 |= I2C_CR1_SWRST; @@ -301,12 +301,12 @@ i2c_rx_complete(void) } /* disable interrupts while reconfiguring DMA for the selected registers */ - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); stm32_dmastop(tx_dma); i2c_tx_setup(); - irqrestore(flags); + px4_leave_critical_section(flags); } } diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 224b971e1b..36128fc119 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -121,11 +121,10 @@ mixer_tick(void) * Decide which set of controls we're using. */ - /* do not mix if RAW_PWM mode is on and FMU is good */ + /* Do not mix if we have raw PWM and FMU is ok. */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { - /* don't actually mix anything - we already have raw PWM values */ source = MIX_NONE; } else { @@ -138,25 +137,17 @@ mixer_tick(void) source = MIX_FMU; } - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && - !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && - !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && - /* do not enter manual override if we asked for termination failsafe and FMU is lost */ - !(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)) { + else if (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) { - /* if allowed, mix from RC inputs directly */ - source = MIX_OVERRIDE; + if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { - } else if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && - !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { + /* if allowed, mix from RC inputs directly up to available rc channels */ + source = MIX_OVERRIDE_FMU_OK; - /* if allowed, mix from RC inputs directly up to available rc channels */ - source = MIX_OVERRIDE_FMU_OK; + } else { + /* if allowed, mix from RC inputs directly */ + source = MIX_OVERRIDE; + } } } @@ -374,14 +365,17 @@ mixer_callback(uintptr_t handle, if (source == MIX_OVERRIDE || source == MIX_OVERRIDE_FMU_OK) { if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && control_index == actuator_controls_s::INDEX_ROLL) { + control *= REG_TO_FLOAT(r_setup_scale_roll); control += REG_TO_FLOAT(r_setup_trim_roll); } else if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && control_index == actuator_controls_s::INDEX_PITCH) { + control *= REG_TO_FLOAT(r_setup_scale_pitch); control += REG_TO_FLOAT(r_setup_trim_pitch); } else if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && control_index == actuator_controls_s::INDEX_YAW) { + control *= REG_TO_FLOAT(r_setup_scale_yaw); control += REG_TO_FLOAT(r_setup_trim_yaw); } } diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index f61e56d204..a4c9ce235a 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -33,7 +33,6 @@ #pragma once -#define __STDC_FORMAT_MACROS #include /** @@ -237,8 +236,11 @@ enum { /* DSM bind states */ #define PX4IO_P_SETUP_TRIM_ROLL 16 /**< Roll trim, in actuator units */ #define PX4IO_P_SETUP_TRIM_PITCH 17 /**< Pitch trim, in actuator units */ #define PX4IO_P_SETUP_TRIM_YAW 18 /**< Yaw trim, in actuator units */ +#define PX4IO_P_SETUP_SCALE_ROLL 19 /**< Roll scale, in actuator units */ +#define PX4IO_P_SETUP_SCALE_PITCH 20 /**< Pitch scale, in actuator units */ +#define PX4IO_P_SETUP_SCALE_YAW 21 /**< Yaw scale, in actuator units */ -#define PX4IO_P_SETUP_SBUS_RATE 19 /* frame rate of SBUS1 output in Hz */ +#define PX4IO_P_SETUP_SBUS_RATE 22 /* frame rate of SBUS1 output in Hz */ /* autopilot control values, -10000..10000 */ #define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 0d394778a9..89dae97710 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -122,6 +122,9 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ #define r_setup_trim_roll r_page_setup[PX4IO_P_SETUP_TRIM_ROLL] #define r_setup_trim_pitch r_page_setup[PX4IO_P_SETUP_TRIM_PITCH] #define r_setup_trim_yaw r_page_setup[PX4IO_P_SETUP_TRIM_YAW] +#define r_setup_scale_roll r_page_setup[PX4IO_P_SETUP_SCALE_ROLL] +#define r_setup_scale_pitch r_page_setup[PX4IO_P_SETUP_SCALE_PITCH] +#define r_setup_scale_yaw r_page_setup[PX4IO_P_SETUP_SCALE_YAW] #define r_setup_sbus_rate r_page_setup[PX4IO_P_SETUP_SBUS_RATE] #define r_control_values (&r_page_controls[0]) @@ -151,22 +154,22 @@ extern pwm_limit_t pwm_limit; /* * GPIO handling. */ -#define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED1, !(_s)) -#define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s)) -#define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s)) -#define LED_RING(_s) stm32_gpiowrite(GPIO_LED4, (_s)) +#define LED_BLUE(_s) px4_arch_gpiowrite(GPIO_LED1, !(_s)) +#define LED_AMBER(_s) px4_arch_gpiowrite(GPIO_LED2, !(_s)) +#define LED_SAFETY(_s) px4_arch_gpiowrite(GPIO_LED3, !(_s)) +#define LED_RING(_s) px4_arch_gpiowrite(GPIO_LED4, (_s)) #ifdef CONFIG_ARCH_BOARD_PX4IO_V1 # define PX4IO_RELAY_CHANNELS 4 -# define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s)) -# define POWER_ACC1(_s) stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s)) -# define POWER_ACC2(_s) stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s)) -# define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s)) -# define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s)) +# define POWER_SERVO(_s) px4_arch_gpiowrite(GPIO_SERVO_PWR_EN, (_s)) +# define POWER_ACC1(_s) px4_arch_gpiowrite(GPIO_ACC1_PWR_EN, (_s)) +# define POWER_ACC2(_s) px4_arch_gpiowrite(GPIO_ACC2_PWR_EN, (_s)) +# define POWER_RELAY1(_s) px4_arch_gpiowrite(GPIO_RELAY1_EN, (_s)) +# define POWER_RELAY2(_s) px4_arch_gpiowrite(GPIO_RELAY2_EN, (_s)) -# define OVERCURRENT_ACC (!stm32_gpioread(GPIO_ACC_OC_DETECT)) -# define OVERCURRENT_SERVO (!stm32_gpioread(GPIO_SERVO_OC_DETECT)) +# define OVERCURRENT_ACC (!px4_arch_gpioread(GPIO_ACC_OC_DETECT)) +# define OVERCURRENT_SERVO (!px4_arch_gpioread(GPIO_SERVO_OC_DETECT)) # define PX4IO_ADC_CHANNEL_COUNT 2 # define ADC_VBATT 4 @@ -177,9 +180,9 @@ extern pwm_limit_t pwm_limit; #ifdef CONFIG_ARCH_BOARD_PX4IO_V2 # define PX4IO_RELAY_CHANNELS 0 -# define ENABLE_SBUS_OUT(_s) stm32_gpiowrite(GPIO_SBUS_OENABLE, !(_s)) +# define ENABLE_SBUS_OUT(_s) px4_arch_gpiowrite(GPIO_SBUS_OENABLE, !(_s)) -# define VDD_SERVO_FAULT (!stm32_gpioread(GPIO_SERVO_FAULT_DETECT)) +# define VDD_SERVO_FAULT (!px4_arch_gpioread(GPIO_SERVO_FAULT_DETECT)) # define PX4IO_ADC_CHANNEL_COUNT 2 # define ADC_VSERVO 4 @@ -187,7 +190,7 @@ extern pwm_limit_t pwm_limit; #endif -#define BUTTON_SAFETY stm32_gpioread(GPIO_BTN_SAFETY) +#define BUTTON_SAFETY px4_arch_gpioread(GPIO_BTN_SAFETY) #define CONTROL_PAGE_INDEX(_group, _channel) (_group * PX4IO_CONTROL_CHANNELS + _channel) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 3a7d90aecf..660a8245ce 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -177,7 +177,10 @@ volatile uint16_t r_page_setup[] = { [PX4IO_P_SETUP_PWM_REVERSE] = 0, [PX4IO_P_SETUP_TRIM_ROLL] = 0, [PX4IO_P_SETUP_TRIM_PITCH] = 0, - [PX4IO_P_SETUP_TRIM_YAW] = 0 + [PX4IO_P_SETUP_TRIM_YAW] = 0, + [PX4IO_P_SETUP_SCALE_ROLL] = 10000, + [PX4IO_P_SETUP_SCALE_PITCH] = 10000, + [PX4IO_P_SETUP_SCALE_YAW] = 10000 }; #ifdef CONFIG_ARCH_BOARD_PX4IO_V2 @@ -281,8 +284,6 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num } system_state.fmu_data_received_time = hrt_absolute_time(); - r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PWM; break; @@ -303,7 +304,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num } system_state.fmu_data_received_time = hrt_absolute_time(); - r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM; + r_status_flags |= PX4IO_P_STATUS_FLAGS_RAW_PWM; break; @@ -681,6 +682,9 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_P_SETUP_TRIM_ROLL: case PX4IO_P_SETUP_TRIM_PITCH: case PX4IO_P_SETUP_TRIM_YAW: + case PX4IO_P_SETUP_SCALE_ROLL: + case PX4IO_P_SETUP_SCALE_PITCH: + case PX4IO_P_SETUP_SCALE_YAW: r_page_setup[offset] = value; break; diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index 300ec0b784..8a00ef1242 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -104,8 +104,8 @@ interface_init(void) rx_dma = stm32_dmachannel(PX4FMU_SERIAL_RX_DMA); /* configure pins for serial use */ - stm32_configgpio(PX4FMU_SERIAL_TX_GPIO); - stm32_configgpio(PX4FMU_SERIAL_RX_GPIO); + px4_arch_configgpio(PX4FMU_SERIAL_TX_GPIO); + px4_arch_configgpio(PX4FMU_SERIAL_RX_GPIO); /* reset and configure the UART */ rCR1 = 0; diff --git a/src/modules/replay/CMakeLists.txt b/src/modules/replay/CMakeLists.txt new file mode 100644 index 0000000000..557cb070f5 --- /dev/null +++ b/src/modules/replay/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2016 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. +# +############################################################################# +px4_add_module( + MODULE modules__replay + MAIN replay + COMPILE_FLAGS + STACK_MAIN 1000 + STACK_MAX 4000 + SRCS + replay_main.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/unittests/uorb_unittests/uORBGtestTopics.hpp b/src/modules/replay/definitions.hpp similarity index 77% rename from unittests/uorb_unittests/uORBGtestTopics.hpp rename to src/modules/replay/definitions.hpp index 0616afe83e..1b728aa994 100644 --- a/unittests/uorb_unittests/uORBGtestTopics.hpp +++ b/src/modules/replay/definitions.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015 Mark Charlebois. All rights reserved. + * Copyright (c) 2016 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 @@ -31,29 +31,15 @@ * ****************************************************************************/ -#ifndef _uORBGtestTopics_hpp_ -#define _uORBGtestTopics_hpp_ +#pragma once -#include "uORB/uORB.h" - -namespace uORB_test +namespace px4 +{ +namespace replay { - struct orb_topic_A - { - int16_t val; - }; - struct orb_topic_B - { - int16_t val; - }; +static const char *ENV_FILENAME = "replay"; ///< name for getenv() - ORB_DEFINE( topicA, struct orb_topic_A ); - ORB_DEFINE( topicB, struct orb_topic_B ); - - ORB_DEFINE( topicA_clone, struct orb_topic_A ); - ORB_DEFINE( topicB_clone, struct orb_topic_B ); -} - -#endif // _UnitTest_uORBTopics_hpp_ +} //namespace replay +} //namespace px4 diff --git a/src/modules/replay/replay.hpp b/src/modules/replay/replay.hpp new file mode 100644 index 0000000000..f4fbcbff2c --- /dev/null +++ b/src/modules/replay/replay.hpp @@ -0,0 +1,161 @@ +/**************************************************************************** + * + * Copyright (c) 2016 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include + +#include "definitions.hpp" + +#include + +namespace px4 +{ + +/** + * @class Replay + * Parses an ULog file and replays it in 'real-time'. The timestamp of each replayed message is offset + * to match the starting time of replay. It keeps a stream for each subscription to find the next message + * to replay. This is necessary because data messages from different subscriptions don't need to be in + * monotonic increasing order. + */ +class Replay +{ +public: + Replay(); + + /// Destructor, also waits for task exit + ~Replay(); + + /// Start task. + /// @param quiet silently fail if no log file found + /// @param apply_params_only if true, only apply parameters from definitions section of the file + /// and user-overridden parameters, then exit w/o replaying. + /// @return OK on success. + static int start(bool quiet, bool apply_params_only); + + static void task_main_trampoline(int argc, char *argv[]); + + void task_main(); + + /** + * Tell the replay module that we want to use replay mode. + * After that, only 'replay start' must be executed (typically the last step after startup). + * @param file_name file name of the used log replay file. Will be copied. + */ + static void setupReplayFile(const char *file_name); + + static bool isSetup() { return _replay_file; } +private: + bool _task_should_exit = false; + std::set _overridden_params; + std::map _file_formats; ///< all formats we read from the file + + uint64_t _file_start_time; + uint64_t _replay_start_time; + std::streampos _data_section_start; ///< first ADD_LOGGED_MSG message + std::vector _read_buffer; + + struct Subscription { + + const orb_metadata *orb_meta = nullptr; ///< if nullptr, this subscription is invalid + orb_advert_t orb_advert = nullptr; + uint8_t multi_id; + int timestamp_offset; ///< marks the field of the timestamp + + std::streampos next_read_pos; + uint64_t next_timestamp; ///< timestamp of the file + }; + std::vector _subscriptions; + + /** keep track of file position to avoid adding a subscription multiple times. */ + std::streampos _subscription_file_pos = 0; + + bool readFileHeader(std::ifstream &file); + + /** + * Read definitions section: check formats, apply parameters and store + * the start of the data section. + * @return true on success + */ + bool readFileDefinitions(std::ifstream &file); + + ///file parsing methods. They return false, when further parsing should be aborted. + bool readFormat(std::ifstream &file, uint16_t msg_size); + bool readAndAddSubscription(std::ifstream &file, uint16_t msg_size); + + /** + * Read the file header and definitions sections. Apply the parameters from this section + * and apply user-defined overridden parameters. + * @return true on success + */ + bool readDefinitionsAndApplyParams(std::ifstream &file); + + /** + * Read and handle additional messages starting at current file position, while position < end_position. + * This handles dropout and parameter update messages. + * We need to handle these separately, because they have no timestamp. We look at the file position instead. + * @return false on file error + */ + bool readAndHandleAdditionalMessages(std::ifstream &file, std::streampos end_position); + bool readDropout(std::ifstream &file, uint16_t msg_size); + bool readAndApplyParameter(std::ifstream &file, uint16_t msg_size); + + /** + * Find next data message for this subscription, starting with the stored file offset. + * Skip the first message, and if found, read the timestamp and store the new file offset. + * This also takes care of new subscriptions and parameter updates. When reaching EOF, + * the subscription is set to invalid. + * File seek position is arbitrary after this call. + * @return false on file error + */ + bool nextDataMessage(std::ifstream &file, Subscription &subscription, int msg_id); + + static const orb_metadata *findTopic(const std::string &name); + /** get the array size from a type. eg. float[3] -> return float */ + static std::string extractArraySize(const std::string &type_name_full, int &array_size); + /** get the size of a type that is not an array */ + static size_t sizeOfType(const std::string &type_name); + /** get the size of a type that can be an array */ + static size_t sizeOfFullType(const std::string &type_name_full); + + void setUserParams(const char *filename); + + static char *_replay_file; +}; + +} //namespace px4 diff --git a/src/modules/replay/replay_main.cpp b/src/modules/replay/replay_main.cpp new file mode 100644 index 0000000000..795d358446 --- /dev/null +++ b/src/modules/replay/replay_main.cpp @@ -0,0 +1,897 @@ +/**************************************************************************** + * + * Copyright (c) 2016 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 replay_main.cpp + * This module reads messages from an ULog file and publishes them. + * It sets the parameters from the log file and handles user-defined + * parameter overrides. + * + * @author Beat Kueng +*/ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "replay.hpp" + +#define PARAMS_OVERRIDE_FILE PX4_ROOTFSDIR "/replay_params.txt" + + +extern "C" __EXPORT int replay_main(int argc, char *argv[]); + +using namespace std; + +namespace px4 +{ +class Replay; + +namespace replay +{ +Replay *instance = nullptr; +static int control_task = -1; //task handle for task + +} //namespace replay + + +char *Replay::_replay_file = nullptr; + +Replay::Replay() +{ +} + +Replay::~Replay() +{ + if (replay::control_task != -1) { + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned int i = 0; + + do { + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 200) { + px4_task_delete(replay::control_task); + replay::control_task = -1; + break; + } + } while (replay::control_task != -1); + } +} + +void Replay::setupReplayFile(const char *file_name) +{ + if (_replay_file) { + free(_replay_file); + } + + _replay_file = strdup(file_name); +} + + + +void Replay::setUserParams(const char *filename) +{ + string line, param_name, value_string; + ifstream myfile(filename); + + if (!myfile.is_open()) { + return; + } + + PX4_INFO("Applying override params from %s...", filename); + + while (!myfile.eof()) { + getline(myfile, line); + + if (line.empty() || line[0] == '#') { + continue; + } + + istringstream mystrstream(line); + mystrstream >> param_name; + mystrstream >> value_string; + + double param_value_double = stod(value_string); + + param_t handle = param_find(param_name.c_str()); + param_type_t param_format = param_type(handle); + _overridden_params.insert(param_name); + + if (param_format == PARAM_TYPE_INT32) { + int32_t value = 0; + value = (int32_t)param_value_double; + param_set(handle, (const void *)&value); + + } else if (param_format == PARAM_TYPE_FLOAT) { + float value = 0; + value = (float)param_value_double; + param_set(handle, (const void *)&value); + } + } +} + +bool Replay::readFileHeader(std::ifstream &file) +{ + file.seekg(0); + ulog_file_header_s msg_header; + file.read((char *)&msg_header, sizeof(msg_header)); + + if (!file) { + return false; + } + + _file_start_time = msg_header.timestamp; + //verify it's an ULog file + char magic[8]; + magic[0] = 'U'; + magic[1] = 'L'; + magic[2] = 'o'; + magic[3] = 'g'; + magic[4] = 0x01; + magic[5] = 0x12; + magic[6] = 0x35; + return memcmp(magic, msg_header.magic, 7) == 0; +} + +bool Replay::readFileDefinitions(std::ifstream &file) +{ + PX4_INFO("Applying params from ULog file..."); + + ulog_message_header_s message_header; + file.seekg(sizeof(ulog_file_header_s)); + + while (true) { + file.read((char *)&message_header, ULOG_MSG_HEADER_LEN); + + if (!file) { + return false; + } + + switch (message_header.msg_type) { + case (int)ULogMessageType::FORMAT: + if (!readFormat(file, message_header.msg_size)) { + return false; + } + + break; + + case (int)ULogMessageType::PARAMETER: + if (!readAndApplyParameter(file, message_header.msg_size)) { + return false; + } + + break; + + case (int)ULogMessageType::ADD_LOGGED_MSG: + _data_section_start = file.tellg() - (streamoff)ULOG_MSG_HEADER_LEN; + return true; + + case (int)ULogMessageType::INFO: //skip + file.seekg(message_header.msg_size, ios::cur); + break; + + default: + PX4_ERR("unknown log definition type %i, size %i (offset %i)", + (int)message_header.msg_type, (int)message_header.msg_size, (int)file.tellg()); + file.seekg(message_header.msg_size, ios::cur); + break; + } + } + + return true; +} + +bool Replay::readFormat(std::ifstream &file, uint16_t msg_size) +{ + _read_buffer.reserve(msg_size + 1); + char *format = (char *)_read_buffer.data(); + file.read(format, msg_size); + format[msg_size] = 0; + + if (!file) { + return false; + } + + string str_format(format); + size_t pos = str_format.find(':'); + + if (pos == string::npos) { + return false; + } + + string name = str_format.substr(0, pos); + string fields = str_format.substr(pos + 1); + _file_formats[name] = fields; + + return true; +} + +bool Replay::readAndAddSubscription(std::ifstream &file, uint16_t msg_size) +{ + _read_buffer.reserve(msg_size + 1); + char *message = (char *)_read_buffer.data(); + streampos this_message_pos = file.tellg() - (streamoff)ULOG_MSG_HEADER_LEN; + file.read(message, msg_size); + message[msg_size] = 0; + + if (!file) { + return false; + } + + if (file.tellg() <= _subscription_file_pos) { //already read this subscription + return true; + } + + _subscription_file_pos = file.tellg(); + + uint8_t multi_id = *(uint8_t *)message; + uint16_t msg_id = ((uint16_t) message[1]) | (((uint16_t) message[2]) << 8); + string topic_name(message + 3); + const orb_metadata *orb_meta = findTopic(topic_name); + + if (!orb_meta) { + PX4_WARN("Topic %s not found internally. Will ignore it", topic_name.c_str()); + return true; + } + + //check the format: the field definitions must match + //FIXME: this should check recursively, all used nested types + string file_format = _file_formats[topic_name]; + + if (file_format != orb_meta->o_fields) { + PX4_WARN("Formats for %s don't match. Will ignore it.", topic_name.c_str()); + PX4_WARN(" Internal format: %s", orb_meta->o_fields); + PX4_WARN(" File format : %s", file_format.c_str()); + } + + Subscription subscription; + subscription.orb_meta = orb_meta; + subscription.multi_id = multi_id; + + + //find the timestamp offset (not necessarily the first field) + string fields = orb_meta->o_fields; + size_t prev_field_end = 0; + size_t field_end = fields.find(';'); + bool timestamp_found = false; + subscription.timestamp_offset = 0; + + while (field_end != string::npos && !timestamp_found) { + size_t space_pos = fields.find(' ', prev_field_end); + + if (space_pos != string::npos) { + string type_name_full = fields.substr(prev_field_end, space_pos - prev_field_end); + string field_name = fields.substr(space_pos + 1, field_end - space_pos - 1); + + if (field_name == "timestamp") { + timestamp_found = true; + + if (type_name_full != "uint64_t") { + PX4_ERR("Unsupported timestamp type %s, ignoring the topic %s", type_name_full.c_str(), + orb_meta->o_name); + return true; + } + + } else { + subscription.timestamp_offset += sizeOfFullType(type_name_full); + } + } + + prev_field_end = field_end + 1; + field_end = fields.find(';', prev_field_end); + } + + if (!timestamp_found) { + return true; + } + + + //find first data message (and the timestamp) + streampos cur_pos = file.tellg(); + subscription.next_read_pos = this_message_pos; //this will be skipped + + if (!nextDataMessage(file, subscription, msg_id)) { + return false; + } + + file.seekg(cur_pos); + + if (!subscription.orb_meta) { + //no message found. This is not a fatal error + return true; + } + + PX4_DEBUG("adding subscription for %s (msg_id %i)", subscription.orb_meta->o_name, msg_id); + + //add subscription + if (_subscriptions.size() <= msg_id) { + _subscriptions.resize(msg_id + 1); + } + + _subscriptions[msg_id] = subscription; + + return true; +} + +bool Replay::readAndHandleAdditionalMessages(std::ifstream &file, std::streampos end_position) +{ + ulog_message_header_s message_header; + + while (file.tellg() < end_position) { + file.read((char *)&message_header, ULOG_MSG_HEADER_LEN); + + if (!file) { + return false; + } + + switch (message_header.msg_type) { + case (int)ULogMessageType::PARAMETER: + if (!readAndApplyParameter(file, message_header.msg_size)) { + return false; + } + + break; + + case (int)ULogMessageType::DROPOUT: + readDropout(file, message_header.msg_size); + break; + + default: //skip all others + file.seekg(message_header.msg_size, ios::cur); + break; + } + } + + return true; +} + +bool Replay::readAndApplyParameter(std::ifstream &file, uint16_t msg_size) +{ + _read_buffer.reserve(msg_size); + uint8_t *message = (uint8_t *)_read_buffer.data(); + file.read((char *)message, msg_size); + + if (!file) { + return false; + } + + uint8_t key_len = message[0]; + string key((char *)message + 1, key_len); + + size_t pos = key.find(' '); + + if (pos == string::npos) { + return false; + } + + string type = key.substr(0, pos); + string param_name = key.substr(pos + 1); + + if (_overridden_params.find(param_name) != _overridden_params.end()) { + //this parameter is overridden, so don't apply it + return true; + } + + if (type != "int32_t" && type != "float") { + PX4_WARN("unknown parameter type %s, name %s (ignoring it)", type.c_str(), param_name.c_str()); + return true; + } + + param_t handle = param_find(param_name.c_str()); + + if (handle != PARAM_INVALID) { + param_set(handle, (const void *)(message + 1 + key_len)); + } + + return true; +} + +bool Replay::readDropout(std::ifstream &file, uint16_t msg_size) +{ + uint16_t duration; + file.read((char *)&duration, sizeof(duration)); + + PX4_INFO("Dropout in replayed log, %i ms", (int)duration); + return file.good(); +} + +bool Replay::nextDataMessage(std::ifstream &file, Subscription &subscription, int msg_id) +{ + ulog_message_header_s message_header; + file.seekg(subscription.next_read_pos); + //ignore the first message (it's data we already read) + file.read((char *)&message_header, ULOG_MSG_HEADER_LEN); + + if (file) { + file.seekg(message_header.msg_size, ios::cur); + } + + uint16_t file_msg_id; + bool done = false; + + while (file && !done) { + streampos cur_pos = file.tellg(); + file.read((char *)&message_header, ULOG_MSG_HEADER_LEN); + + if (!file) { + break; + } + + switch (message_header.msg_type) { + case (int)ULogMessageType::ADD_LOGGED_MSG: + readAndAddSubscription(file, message_header.msg_size); + break; + + case (int)ULogMessageType::DATA: + file.read((char *)&file_msg_id, sizeof(file_msg_id)); + + if (file) { + if (msg_id == file_msg_id) { + if (message_header.msg_size == subscription.orb_meta->o_size_no_padding + 2) { + subscription.next_read_pos = cur_pos; + file.seekg(subscription.timestamp_offset, ios::cur); + file.read((char *)&subscription.next_timestamp, sizeof(subscription.next_timestamp)); + done = true; + + } else { //sanity check failed! + PX4_ERR("data message %s has wrong size %i (expected %i). Skipping", + subscription.orb_meta->o_name, message_header.msg_size, + subscription.orb_meta->o_size_no_padding + 2); + file.seekg(message_header.msg_size - sizeof(file_msg_id), ios::cur); + } + + } else { //not the one we are looking for + file.seekg(message_header.msg_size - sizeof(file_msg_id), ios::cur); + } + } + + break; + + case (int)ULogMessageType::REMOVE_LOGGED_MSG: //skip these + case (int)ULogMessageType::PARAMETER: + case (int)ULogMessageType::DROPOUT: + case (int)ULogMessageType::SYNC: + case (int)ULogMessageType::LOGGING: + file.seekg(message_header.msg_size, ios::cur); + break; + + default: + //this really should not happen + PX4_ERR("unknown log message type %i, size %i (offset %i)", + (int)message_header.msg_type, (int)message_header.msg_size, (int)file.tellg()); + file.seekg(message_header.msg_size, ios::cur); + break; + } + } + + if (file.eof()) { //no more data messages for this subscription + subscription.orb_meta = nullptr; + file.clear(); + } + + return file.good(); +} + +const orb_metadata *Replay::findTopic(const std::string &name) +{ + const orb_metadata **topics = orb_get_topics(); + + for (size_t i = 0; i < orb_topics_count(); i++) { + if (name == topics[i]->o_name) { + return topics[i]; + } + } + + return nullptr; +} + +std::string Replay::extractArraySize(const std::string &type_name_full, int &array_size) +{ + size_t start_pos = type_name_full.find('['); + size_t end_pos = type_name_full.find(']'); + + if (start_pos == string::npos || end_pos == string::npos) { + array_size = 1; + return type_name_full; + } + + array_size = atoi(type_name_full.substr(start_pos + 1, end_pos - start_pos - 1).c_str()); + return type_name_full.substr(0, start_pos); +} + +size_t Replay::sizeOfType(const std::string &type_name) +{ + if (type_name == "int8_t" || type_name == "uint8_t") { + return 1; + + } else if (type_name == "int16_t" || type_name == "uint16_t") { + return 2; + + } else if (type_name == "int32_t" || type_name == "uint32_t") { + return 4; + + } else if (type_name == "int64_t" || type_name == "uint64_t") { + return 8; + + } else if (type_name == "float") { + return 4; + + } else if (type_name == "double") { + return 8; + + } else if (type_name == "char" || type_name == "bool") { + return 1; + } + + const orb_metadata *orb_meta = findTopic(type_name); + + if (orb_meta) { + return orb_meta->o_size; + } + + PX4_ERR("unknown type: %s", type_name.c_str()); + return 0; +} +size_t Replay::sizeOfFullType(const std::string &type_name_full) +{ + int array_size; + string type_name = extractArraySize(type_name_full, array_size); + return sizeOfType(type_name) * array_size; +} + +bool Replay::readDefinitionsAndApplyParams(std::ifstream &file) +{ + // log reader currently assumes little endian + int num = 1; + + if (*(char *)&num != 1) { + PX4_ERR("Replay only works on little endian!"); + return false; + } + + if (!file.is_open()) { + PX4_ERR("Failed to open replay file"); + return false; + } + + if (!readFileHeader(file)) { + PX4_ERR("Failed to read file header. Not a valid ULog file"); + return false; + } + + //initialize the formats and apply the parameters from the log file + if (!readFileDefinitions(file)) { + PX4_ERR("Failed to read ULog definitions section. Broken file?"); + return false; + } + + setUserParams(PARAMS_OVERRIDE_FILE); + return true; +} + +void Replay::task_main() +{ + ifstream replay_file(_replay_file, ios::in | ios::binary); + + if (!readDefinitionsAndApplyParams(replay_file)) { + return; + } + + _replay_start_time = hrt_absolute_time(); + + PX4_INFO("Replay in progress..."); + + ulog_message_header_s message_header; + replay_file.seekg(_data_section_start); + + //we know the next message must be an ADD_LOGGED_MSG + replay_file.read((char *)&message_header, ULOG_MSG_HEADER_LEN); + + if (!readAndAddSubscription(replay_file, message_header.msg_size)) { + PX4_ERR("Failed to read subscription"); + return; + } + + + //we update the timestamps from the file by a constant offset to match + //the current replay time + const uint64_t timestamp_offset = _replay_start_time - _file_start_time; + uint32_t nr_published_messages = 0; + streampos last_additional_message_pos = _data_section_start; + + while (!_task_should_exit && replay_file) { + + //Find the next message to publish. Messages from different subscriptions don't need + //to be in chronological order, so we need to check all subscriptions + uint64_t next_file_time = 0; + int next_msg_id = -1; + + for (size_t i = 0; i < _subscriptions.size(); ++i) { + const Subscription &subscription = _subscriptions[i]; + + if (subscription.orb_meta) { + if (next_file_time == 0 || subscription.next_timestamp < next_file_time) { + next_msg_id = (int)i; + next_file_time = subscription.next_timestamp; + } + } + } + + if (next_msg_id == -1) { + break; //no active subscription anymore. We're done. + } + + Subscription &sub = _subscriptions[next_msg_id]; + + if (next_file_time == 0) { + //someone didn't set the timestamp properly. Consider the message invalid + nextDataMessage(replay_file, sub, next_msg_id); + continue; + } + + + //handle additional messages between last and next published data + replay_file.seekg(last_additional_message_pos); + streampos next_additional_message_pos = sub.next_read_pos; + readAndHandleAdditionalMessages(replay_file, next_additional_message_pos); + last_additional_message_pos = next_additional_message_pos; + + + //wait if necessary + const uint64_t publish_timestamp = next_file_time + timestamp_offset; + uint64_t cur_time = hrt_absolute_time(); + + if (cur_time < publish_timestamp) { + usleep(publish_timestamp - cur_time); + } + + //It's time to publish + const size_t msg_read_size = sub.orb_meta->o_size_no_padding; + const size_t msg_write_size = sub.orb_meta->o_size; + _read_buffer.reserve(msg_write_size); + replay_file.seekg(sub.next_read_pos + (streamoff)(ULOG_MSG_HEADER_LEN + 2)); //skip header & msg id + replay_file.read((char *)_read_buffer.data(), msg_read_size); + *(uint64_t *)(_read_buffer.data() + sub.timestamp_offset) = publish_timestamp; + + if (sub.orb_advert) { + orb_publish(sub.orb_meta, sub.orb_advert, _read_buffer.data()); + ++nr_published_messages; + + } else { + if (sub.multi_id == 0) { + sub.orb_advert = orb_advertise(sub.orb_meta, _read_buffer.data()); + ++nr_published_messages; + + } else { + // make sure the other instances are advertised already so that we get the correct instance + bool advertised = false; + + for (const auto &subscription : _subscriptions) { + if (subscription.orb_meta) { + if (strcmp(sub.orb_meta->o_name, subscription.orb_meta->o_name) == 0 && + subscription.orb_advert && subscription.multi_id == sub.multi_id - 1) { + advertised = true; + } + } + } + + if (advertised) { + int instance; + sub.orb_advert = orb_advertise_multi(sub.orb_meta, _read_buffer.data(), + &instance, ORB_PRIO_DEFAULT); + ++nr_published_messages; + } + } + } + + + nextDataMessage(replay_file, _subscriptions[next_msg_id], next_msg_id); + + //TODO: output status (eg. every sec), including total duration... + } + + for (auto &subscription : _subscriptions) { + if (subscription.orb_advert) { + orb_unadvertise(subscription.orb_advert); + subscription.orb_advert = nullptr; + } + } + + if (!_task_should_exit) { + PX4_INFO("Replay done (published %u msgs, %.3lf s)", nr_published_messages, + (double)hrt_elapsed_time(&_replay_start_time) / 1.e6); + + //TODO: should we close the log file & exit (optionally, by adding a parameter -q) ? + } +} + +void Replay::task_main_trampoline(int argc, char *argv[]) +{ + replay::instance = new Replay(); + + if (replay::instance == nullptr) { + PX4_ERR("alloc failed"); + return; + } + + replay::instance->task_main(); + replay::control_task = -1; +} + +int Replay::start(bool quiet, bool apply_params_only) +{ + ASSERT(replay::control_task == -1); + int ret = PX4_OK; + + //check for logfile env variable + const char *logfile = getenv(replay::ENV_FILENAME); + + if (logfile) { + if (!isSetup()) { + PX4_INFO("using replay log file: %s", logfile); + setupReplayFile(logfile); + } + + } else { + if (quiet) { + return PX4_OK; + + } else { + PX4_ERR("no log file given (via env variable %s)", replay::ENV_FILENAME); + return -1; + } + } + + if (apply_params_only) { + Replay *r = new Replay(); + + if (r == nullptr) { + PX4_ERR("alloc failed"); + return -ENOMEM; + } + + ifstream replay_file(_replay_file, ios::in | ios::binary); + + if (!r->readDefinitionsAndApplyParams(replay_file)) { + ret = -1; + } + + delete(r); + + } else { + + /* start the task */ + replay::control_task = px4_task_spawn_cmd("replay", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 4000, + (px4_main_t)&Replay::task_main_trampoline, + nullptr); + + if (replay::control_task < 0) { + replay::control_task = -1; + PX4_ERR("task start failed"); + return -errno; + } + } + + return ret; +} + +} //namespace px4 + +using namespace px4; + +int replay_main(int argc, char *argv[]) +{ + if (argc < 1) { + PX4_WARN("usage: replay {tryapplyparams|trystart|start|stop|status}"); + return 1; + } + + bool do_start = false; + bool quiet = false; + bool apply_params_only = false; + + if (!strcmp(argv[1], "start")) { + do_start = true; + + } else if (!strcmp(argv[1], "trystart")) { + do_start = true; + quiet = true; + + } else if (!strcmp(argv[1], "tryapplyparams")) { + do_start = true; + quiet = true; + apply_params_only = true; + } + + if (do_start) { + if (replay::instance != nullptr) { + PX4_WARN("already running"); + return 1; + } + + if (PX4_OK != replay::instance->start(quiet, apply_params_only)) { + PX4_ERR("start failed"); + return 1; + } + + return 0; + } + + if (!strcmp(argv[1], "stop")) { + if (replay::instance == nullptr) { + PX4_WARN("not running"); + return 1; + } + + delete replay::instance; + replay::instance = nullptr; + + return 0; + } + + if (!strcmp(argv[1], "status")) { + if (replay::instance) { + PX4_WARN("running"); + return 0; + + } else { + PX4_WARN("not running"); + return 1; + } + } + + PX4_ERR("unrecognized command"); + return 1; +} diff --git a/src/modules/sdlog2/params.c b/src/modules/sdlog2/params.c index db96d564ac..0e9f49f950 100644 --- a/src/modules/sdlog2/params.c +++ b/src/modules/sdlog2/params.c @@ -48,7 +48,7 @@ PARAM_DEFINE_INT32(SDLOG_RATE, -1); /** - * Enable extended logging mode. + * Extended logging mode * * A value of -1 indicates the command line argument * should be obeyed. A value of 0 disables extended @@ -68,7 +68,7 @@ PARAM_DEFINE_INT32(SDLOG_EXT, -1); /** * Use timestamps only if GPS 3D fix is available * - * A value of 1 constrains the log folder creation + * Constrain the log folder creation * to only use the time stamp if a 3D GPS lock is * present. * @@ -77,24 +77,6 @@ PARAM_DEFINE_INT32(SDLOG_EXT, -1); */ PARAM_DEFINE_INT32(SDLOG_GPSTIME, 1); -/** - * UTC offset (unit: min) - * - * the difference in hours and minutes from Coordinated - * Universal Time (UTC) for a your place and date. - * - * for example, In case of South Korea(UTC+09:00), - * UTC offset is 540 min (9*60) - * - * refer to https://en.wikipedia.org/wiki/List_of_UTC_time_offsets - * - * @unit min - * @min -1000 - * @max 1000 - * @group SD Logging - */ -PARAM_DEFINE_INT32(SDLOG_UTC_OFFSET, 0); - /** * Give logging app higher thread priority to avoid data loss. * This is used for gathering replay logs for the ekf2 module. diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 3af4872203..e0815b7a3a 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -78,10 +78,6 @@ #include #include #include -#include -#include -#include -#include #include #include #include @@ -106,7 +102,6 @@ #include #include #include -#include #include #include #include @@ -115,6 +110,7 @@ #include #include #include +#include #include #include @@ -147,8 +143,14 @@ static bool logwriter_should_exit = false; /**< Logwriter thread exit flag */ static const unsigned MAX_NO_LOGFOLDER = 999; /**< Maximum number of log dirs */ static const unsigned MAX_NO_LOGFILE = 999; /**< Maximum number of log files */ static const int LOG_BUFFER_SIZE_DEFAULT = 8192; + +#if defined __PX4_POSIX +static const int MAX_WRITE_CHUNK = 2048; +static const int MIN_BYTES_TO_WRITE = 512; +#else static const int MAX_WRITE_CHUNK = 512; static const int MIN_BYTES_TO_WRITE = 512; +#endif static bool _extended_logging = false; static bool _gpstime_only = false; @@ -168,7 +170,11 @@ struct logbuffer_s lb; static pthread_mutex_t logbuffer_mutex; static pthread_cond_t logbuffer_cond; +#ifdef __PX4_NUTTX #define LOG_BASE_PATH_LEN 64 +#else +#define LOG_BASE_PATH_LEN 256 +#endif static char log_dir[LOG_BASE_PATH_LEN]; @@ -199,6 +205,10 @@ static pthread_attr_t logwriter_attr; static perf_counter_t perf_write; +/* Keep track if we've already created a folder named sessXXX because + * we don't want to create yet another one. */ +static bool sess_folder_created = false; + /** * Log buffer writing thread. Open and close file here. */ @@ -411,7 +421,7 @@ bool get_log_time_tt(struct tm *tt, bool boot_time) { struct timespec ts; px4_clock_gettime(CLOCK_REALTIME, &ts); /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.px4log */ - time_t utc_time_sec; + time_t utc_time_sec = 0; if (_gpstime_only && has_gps_3d_fix) { utc_time_sec = gps_time_sec; @@ -446,6 +456,10 @@ int create_log_dir() if (log_name_timestamp && time_ok) { int n = snprintf(log_dir, sizeof(log_dir), "%s/", log_root); + if (n >= sizeof(log_dir)) { + PX4_ERR("log path too long"); + return -1; + } strftime(log_dir + n, sizeof(log_dir) - n, "%Y-%m-%d", &tt); mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); @@ -455,13 +469,21 @@ int create_log_dir() } } else { - /* look for the next dir that does not exist */ - while (dir_number <= MAX_NO_LOGFOLDER) { + /* Look for the next dir that does not exist. + * However, if we've already crated a sessXXX folder in this session + * let's re-use it. */ + while (dir_number <= MAX_NO_LOGFOLDER && !sess_folder_created) { /* format log dir: e.g. /fs/microsd/sess001 */ - sprintf(log_dir, "%s/sess%03u", log_root, dir_number); + int n = snprintf(log_dir, sizeof(log_dir), "%s/sess%03u", log_root, dir_number); + if (n >= sizeof(log_dir)) { + PX4_ERR("log path too long"); + return -1; + } + mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); if (mkdir_ret == 0) { + sess_folder_created = true; break; } else if (errno != EEXIST) { @@ -920,8 +942,13 @@ bool copy_if_updated_multi(orb_id_t topic, int multi_instance, int *handle, void *handle = orb_subscribe_multi(topic, multi_instance); /* copy first data */ if (*handle >= 0) { - orb_copy(topic, *handle, buffer); - updated = true; + + /* but only if it has really been updated */ + orb_check(*handle, &updated); + + if (updated) { + orb_copy(topic, *handle, buffer); + } } } } else { @@ -1108,7 +1135,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* initialize log buffer with specified size */ - PX4_WARN("log buffer size: %i bytes", log_buffer_size); + PX4_DEBUG("log buffer size: %i bytes", log_buffer_size); if (OK != logbuffer_init(&lb, log_buffer_size)) { PX4_WARN("can't allocate log buffer, exiting"); @@ -1127,13 +1154,29 @@ int sdlog2_thread_main(int argc, char *argv[]) struct commander_state_s buf_commander_state; memset(&buf_commander_state, 0, sizeof(buf_commander_state)); - // check if we are gathering data for a replay log for ekf2 - // is yes then disable logging of some topics to avoid dropouts + /* There are different log types possible on different platforms. */ + enum { + LOG_TYPE_NORMAL, + LOG_TYPE_REPLAY_ONLY, + LOG_TYPE_ALL + } log_type; + + /* Check if we are gathering data for a replay log for ekf2. */ param_t replay_handle = param_find("EKF2_REC_RPL"); int32_t tmp = 0; param_get(replay_handle, &tmp); bool record_replay_log = (bool)tmp; + if (record_replay_log) { +#if defined(__PX4_QURT) || defined(__PX4_POSIX) + log_type = LOG_TYPE_ALL; +#else + log_type = LOG_TYPE_REPLAY_ONLY; +#endif + } else { + log_type = LOG_TYPE_NORMAL; + } + /* warning! using union here to save memory, elements should be used separately! */ union { struct vehicle_command_s cmd; @@ -1165,7 +1208,6 @@ int sdlog2_thread_main(int argc, char *argv[]) struct servorail_status_s servorail_status; struct satellite_info_s sat_info; struct wind_estimate_s wind_estimate; - struct encoders_s encoders; struct vtol_vehicle_status_s vtol_status; struct time_offset_s time_offset; struct mc_att_ctrl_status_s mc_att_ctrl_status; @@ -1174,6 +1216,8 @@ int sdlog2_thread_main(int argc, char *argv[]) struct camera_trigger_s camera_trigger; struct ekf2_replay_s replay; struct vehicle_land_detected_s land_detected; + struct cpuload_s cpuload; + struct vehicle_gps_position_s dual_gps_pos; } buf; memset(&buf, 0, sizeof(buf)); @@ -1231,8 +1275,10 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_EST6_s log_INO3; struct log_RPL3_s log_RPL3; struct log_RPL4_s log_RPL4; - struct log_RPL6_s log_RPL6; + struct log_RPL5_s log_RPL5; struct log_LAND_s log_LAND; + struct log_RPL6_s log_RPL6; + struct log_LOAD_s log_LOAD; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1256,7 +1302,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int local_pos_sp_sub; int global_pos_sub; int triplet_sub; - int gps_pos_sub; + int gps_pos_sub[2]; int sat_info_sub; int att_pos_mocap_sub; int vision_pos_sub; @@ -1273,7 +1319,6 @@ int sdlog2_thread_main(int argc, char *argv[]) int system_power_sub; int servorail_status_sub; int wind_sub; - int encoders_sub; int tsync_sub; int mc_att_ctrl_status_sub; int ctrl_state_sub; @@ -1282,12 +1327,14 @@ int sdlog2_thread_main(int argc, char *argv[]) int replay_sub; int land_detected_sub; int commander_state_sub; + int cpuload_sub; } subs; subs.cmd_sub = -1; subs.status_sub = -1; subs.vtol_status_sub = -1; - subs.gps_pos_sub = -1; + subs.gps_pos_sub[0] = -1; + subs.gps_pos_sub[1] = -1; subs.sensor_sub = -1; subs.att_sub = -1; subs.att_sp_sub = -1; @@ -1317,12 +1364,12 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.tsync_sub = -1; subs.mc_att_ctrl_status_sub = -1; subs.ctrl_state_sub = -1; - subs.encoders_sub = -1; subs.innov_sub = -1; subs.cam_trig_sub = -1; subs.replay_sub = -1; subs.land_detected_sub = -1; subs.commander_state_sub = -1; + subs.cpuload_sub = -1; /* add new topics HERE */ @@ -1338,11 +1385,10 @@ int sdlog2_thread_main(int argc, char *argv[]) pthread_cond_init(&logbuffer_cond, NULL); /* track changes in sensor_combined topic */ - hrt_abstime gyro_timestamp[3] = {0, 0, 0}; - hrt_abstime accelerometer_timestamp[3] = {0, 0, 0}; - hrt_abstime magnetometer_timestamp[3] = {0, 0, 0}; - hrt_abstime barometer_timestamp[3] = {0, 0, 0}; - hrt_abstime differential_pressure_timestamp[3] = {0, 0, 0}; + hrt_abstime gyro_timestamp = 0; + hrt_abstime accelerometer_timestamp = 0; + hrt_abstime magnetometer_timestamp = 0; + hrt_abstime barometer_timestamp = 0; /* initialize calculated mean SNR */ float snr_mean = 0.0f; @@ -1351,7 +1397,7 @@ int sdlog2_thread_main(int argc, char *argv[]) if (log_on_start) { /* check GPS topic to get GPS time */ if (log_name_timestamp) { - if (!orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) { + if (!copy_if_updated_multi(ORB_ID(vehicle_gps_position), 0, &subs.gps_pos_sub[0], &buf_gps_pos)) { gps_time_sec = buf_gps_pos.time_utc_usec / 1e6; } } @@ -1363,23 +1409,53 @@ int sdlog2_thread_main(int argc, char *argv[]) thread_running = true; // wakeup source - px4_pollfd_struct_t fds[1]; + px4_pollfd_struct_t fds[2]; + unsigned px4_pollfd_len = 0; int poll_counter = 0; int poll_to_logging_factor = 1; - if (record_replay_log) { - subs.replay_sub = orb_subscribe(ORB_ID(ekf2_replay)); - fds[0].fd = subs.replay_sub; - fds[0].events = POLLIN; - poll_to_logging_factor = 1; - } else { - subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - fds[0].fd = subs.sensor_sub; - fds[0].events = POLLIN; - // TODO Remove hardcoded rate! - poll_to_logging_factor = 250 / (log_rate < 1 ? 1 : log_rate); + switch (log_type) { + case LOG_TYPE_ALL: + subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + fds[0].fd = subs.sensor_sub; + fds[0].events = POLLIN; + + subs.replay_sub = orb_subscribe(ORB_ID(ekf2_replay)); + fds[1].fd = subs.replay_sub; + fds[1].events = POLLIN; + + px4_pollfd_len = 2; + + poll_to_logging_factor = 1; + + break; + + case LOG_TYPE_NORMAL: + + subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + fds[0].fd = subs.sensor_sub; + fds[0].events = POLLIN; + + px4_pollfd_len = 1; + + // TODO Remove hardcoded rate! + poll_to_logging_factor = 250 / (log_rate < 1 ? 1 : log_rate); + + break; + + case LOG_TYPE_REPLAY_ONLY: + + subs.replay_sub = orb_subscribe(ORB_ID(ekf2_replay)); + fds[0].fd = subs.replay_sub; + fds[0].events = POLLIN; + + px4_pollfd_len = 1; + + poll_to_logging_factor = 1; + + break; } if (poll_to_logging_factor < 1) { @@ -1389,41 +1465,9 @@ int sdlog2_thread_main(int argc, char *argv[]) while (!main_thread_should_exit) { - // wait for up to 100ms for data - int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); - - // timed out - periodic check for _task_should_exit - if (pret == 0) { - continue; - } - - // this is undesirable but not much we can do - might want to flag unhappy status - if (pret < 0) { - PX4_WARN("poll error %d, %d", pret, errno); - // sleep a bit before next try - usleep(100000); - continue; - } - - if (!fds[0].revents & POLLIN) { - continue; - } - - // copy topic always to mark them as read - // so poll doesn't return immediately - if (record_replay_log) { - orb_copy(ORB_ID(ekf2_replay), subs.replay_sub, &buf.replay); - } else { - orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor); - } - - if ((poll_counter + 1) % poll_to_logging_factor == 0) { - poll_counter = 0; - } else { - // copy topic - poll_counter++; - continue; - } + /* Check below's topics first even if logging is not enabled. + * We need to do this because should only poll further below if we're + * actually going to orb_copy the data after the poll. */ /* --- VEHICLE COMMAND - LOG MANAGEMENT --- */ if (copy_if_updated(ORB_ID(vehicle_command), &subs.cmd_sub, &buf_cmd)) { @@ -1440,7 +1484,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- GPS POSITION - LOG MANAGEMENT --- */ - bool gps_pos_updated = copy_if_updated(ORB_ID(vehicle_gps_position), &subs.gps_pos_sub, &buf_gps_pos); + bool gps_pos_updated = copy_if_updated_multi(ORB_ID(vehicle_gps_position), 0, &subs.gps_pos_sub[0], &buf_gps_pos); if (gps_pos_updated && log_name_timestamp) { gps_time_sec = buf_gps_pos.time_utc_usec / 1e6; @@ -1448,6 +1492,55 @@ int sdlog2_thread_main(int argc, char *argv[]) } if (!logging_enabled) { + usleep(50000); + continue; + } + + // wait for up to 100ms for data + int pret = px4_poll(&fds[0], px4_pollfd_len, 100); + + // timed out - periodic check for _task_should_exit + if (pret == 0) { + continue; + } + + // this is undesirable but not much we can do - might want to flag unhappy status + if (pret < 0) { + PX4_WARN("poll error %d, %d", pret, errno); + // sleep a bit before next try + usleep(100000); + continue; + } + + if ((poll_counter+1) >= poll_to_logging_factor) { + poll_counter = 0; + } else { + + /* In this case, we still need to do orb_copy, otherwise we'll stall. */ + switch (log_type) { + case LOG_TYPE_ALL: + if (fds[0].revents & POLLIN) { + orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor); + } + + if (fds[1].revents & POLLIN) { + orb_copy(ORB_ID(ekf2_replay), subs.replay_sub, &buf.replay); + } + break; + + case LOG_TYPE_NORMAL: + if (fds[0].revents & POLLIN) { + orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor); + } + break; + + case LOG_TYPE_REPLAY_ONLY: + if (fds[0].revents & POLLIN) { + orb_copy(ORB_ID(ekf2_replay), subs.replay_sub, &buf.replay); + } + break; + } + poll_counter++; continue; } @@ -1467,167 +1560,167 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_STAT.nav_state = buf_status.nav_state; log_msg.body.log_STAT.arming_state = buf_status.arming_state; log_msg.body.log_STAT.failsafe = (uint8_t) buf_status.failsafe; - log_msg.body.log_STAT.load = buf_status.load; + log_msg.body.log_STAT.is_rot_wing = (uint8_t)buf_status.is_rotary_wing; LOGBUFFER_WRITE_AND_COUNT(STAT); } /* --- EKF2 REPLAY --- */ - if(record_replay_log) { - // we poll on the replay topic so we know that it was updated - // but we need to copy it again since we are re-using the buffer. - orb_copy(ORB_ID(ekf2_replay), subs.replay_sub, &buf.replay); - log_msg.msg_type = LOG_RPL1_MSG; - log_msg.body.log_RPL1.time_ref = buf.replay.time_ref; - log_msg.body.log_RPL1.gyro_integral_dt = buf.replay.gyro_integral_dt; - log_msg.body.log_RPL1.accelerometer_integral_dt = buf.replay.accelerometer_integral_dt; - log_msg.body.log_RPL1.magnetometer_timestamp = buf.replay.magnetometer_timestamp; - log_msg.body.log_RPL1.baro_timestamp = buf.replay.baro_timestamp; - log_msg.body.log_RPL1.gyro_integral_x_rad = buf.replay.gyro_integral_rad[0]; - log_msg.body.log_RPL1.gyro_integral_y_rad = buf.replay.gyro_integral_rad[1]; - log_msg.body.log_RPL1.gyro_integral_z_rad = buf.replay.gyro_integral_rad[2]; - log_msg.body.log_RPL1.accelerometer_integral_x_m_s = buf.replay.accelerometer_integral_m_s[0]; - log_msg.body.log_RPL1.accelerometer_integral_y_m_s = buf.replay.accelerometer_integral_m_s[1]; - log_msg.body.log_RPL1.accelerometer_integral_z_m_s = buf.replay.accelerometer_integral_m_s[2]; - log_msg.body.log_RPL1.magnetometer_x_ga = buf.replay.magnetometer_ga[0]; - log_msg.body.log_RPL1.magnetometer_y_ga = buf.replay.magnetometer_ga[1]; - log_msg.body.log_RPL1.magnetometer_z_ga = buf.replay.magnetometer_ga[2]; - log_msg.body.log_RPL1.baro_alt_meter = buf.replay.baro_alt_meter; - LOGBUFFER_WRITE_AND_COUNT(RPL1); + if (log_type == LOG_TYPE_ALL || log_type == LOG_TYPE_REPLAY_ONLY) { - // only log the gps replay data if it actually updated - if (buf.replay.time_usec > 0) { - log_msg.msg_type = LOG_RPL2_MSG; - log_msg.body.log_RPL2.time_pos_usec = buf.replay.time_usec; - log_msg.body.log_RPL2.time_vel_usec = buf.replay.time_usec_vel; - log_msg.body.log_RPL2.lat = buf.replay.lat; - log_msg.body.log_RPL2.lon = buf.replay.lon; - log_msg.body.log_RPL2.alt = buf.replay.alt; - log_msg.body.log_RPL2.fix_type = buf.replay.fix_type; - log_msg.body.log_RPL2.nsats = buf.replay.nsats; - log_msg.body.log_RPL2.eph = buf.replay.eph; - log_msg.body.log_RPL2.epv = buf.replay.epv; - log_msg.body.log_RPL2.sacc = buf.replay.sacc; - log_msg.body.log_RPL2.vel_m_s = buf.replay.vel_m_s; - log_msg.body.log_RPL2.vel_n_m_s = buf.replay.vel_n_m_s; - log_msg.body.log_RPL2.vel_e_m_s = buf.replay.vel_e_m_s; - log_msg.body.log_RPL2.vel_d_m_s = buf.replay.vel_d_m_s; - log_msg.body.log_RPL2.vel_ned_valid = buf.replay.vel_ned_valid; - LOGBUFFER_WRITE_AND_COUNT(RPL2); + bool replay_updated = false; + + if (log_type == LOG_TYPE_ALL) { + + if (fds[1].revents & POLLIN) { + orb_copy(ORB_ID(ekf2_replay), subs.replay_sub, &buf.replay); + replay_updated = true; + } + + } else if (log_type == LOG_TYPE_REPLAY_ONLY) { + if (fds[0].revents & POLLIN) { + orb_copy(ORB_ID(ekf2_replay), subs.replay_sub, &buf.replay); + replay_updated = true; + } } - if (buf.replay.flow_timestamp > 0) { - log_msg.msg_type = LOG_RPL3_MSG; - log_msg.body.log_RPL3.time_flow_usec = buf.replay.flow_timestamp; - log_msg.body.log_RPL3.flow_integral_x = buf.replay.flow_pixel_integral[0]; - log_msg.body.log_RPL3.flow_integral_y = buf.replay.flow_pixel_integral[1]; - log_msg.body.log_RPL3.gyro_integral_x = buf.replay.flow_gyro_integral[0]; - log_msg.body.log_RPL3.gyro_integral_y = buf.replay.flow_gyro_integral[1]; - log_msg.body.log_RPL3.flow_time_integral = buf.replay.flow_time_integral; - log_msg.body.log_RPL3.flow_quality = buf.replay.flow_quality; - LOGBUFFER_WRITE_AND_COUNT(RPL3); + if (replay_updated) { + log_msg.msg_type = LOG_RPL1_MSG; + log_msg.body.log_RPL1.time_ref = buf.replay.time_ref; + log_msg.body.log_RPL1.gyro_integral_dt = buf.replay.gyro_integral_dt; + log_msg.body.log_RPL1.accelerometer_integral_dt = buf.replay.accelerometer_integral_dt; + log_msg.body.log_RPL1.magnetometer_timestamp = buf.replay.magnetometer_timestamp; + log_msg.body.log_RPL1.baro_timestamp = buf.replay.baro_timestamp; + log_msg.body.log_RPL1.gyro_x_rad = buf.replay.gyro_rad[0]; + log_msg.body.log_RPL1.gyro_y_rad = buf.replay.gyro_rad[1]; + log_msg.body.log_RPL1.gyro_z_rad = buf.replay.gyro_rad[2]; + log_msg.body.log_RPL1.accelerometer_x_m_s2 = buf.replay.accelerometer_m_s2[0]; + log_msg.body.log_RPL1.accelerometer_y_m_s2 = buf.replay.accelerometer_m_s2[1]; + log_msg.body.log_RPL1.accelerometer_z_m_s2 = buf.replay.accelerometer_m_s2[2]; + log_msg.body.log_RPL1.magnetometer_x_ga = buf.replay.magnetometer_ga[0]; + log_msg.body.log_RPL1.magnetometer_y_ga = buf.replay.magnetometer_ga[1]; + log_msg.body.log_RPL1.magnetometer_z_ga = buf.replay.magnetometer_ga[2]; + log_msg.body.log_RPL1.baro_alt_meter = buf.replay.baro_alt_meter; + LOGBUFFER_WRITE_AND_COUNT(RPL1); + + // only log the gps replay data if it actually updated + if (buf.replay.time_usec > 0) { + log_msg.msg_type = LOG_RPL2_MSG; + log_msg.body.log_RPL2.time_pos_usec = buf.replay.time_usec; + log_msg.body.log_RPL2.time_vel_usec = buf.replay.time_usec_vel; + log_msg.body.log_RPL2.lat = buf.replay.lat; + log_msg.body.log_RPL2.lon = buf.replay.lon; + log_msg.body.log_RPL2.alt = buf.replay.alt; + log_msg.body.log_RPL2.fix_type = buf.replay.fix_type; + log_msg.body.log_RPL2.nsats = buf.replay.nsats; + log_msg.body.log_RPL2.eph = buf.replay.eph; + log_msg.body.log_RPL2.epv = buf.replay.epv; + log_msg.body.log_RPL2.sacc = buf.replay.sacc; + log_msg.body.log_RPL2.vel_m_s = buf.replay.vel_m_s; + log_msg.body.log_RPL2.vel_n_m_s = buf.replay.vel_n_m_s; + log_msg.body.log_RPL2.vel_e_m_s = buf.replay.vel_e_m_s; + log_msg.body.log_RPL2.vel_d_m_s = buf.replay.vel_d_m_s; + log_msg.body.log_RPL2.vel_ned_valid = buf.replay.vel_ned_valid; + LOGBUFFER_WRITE_AND_COUNT(RPL2); + } + + if (buf.replay.flow_timestamp > 0) { + log_msg.msg_type = LOG_RPL3_MSG; + log_msg.body.log_RPL3.time_flow_usec = buf.replay.flow_timestamp; + log_msg.body.log_RPL3.flow_integral_x = buf.replay.flow_pixel_integral[0]; + log_msg.body.log_RPL3.flow_integral_y = buf.replay.flow_pixel_integral[1]; + log_msg.body.log_RPL3.gyro_integral_x = buf.replay.flow_gyro_integral[0]; + log_msg.body.log_RPL3.gyro_integral_y = buf.replay.flow_gyro_integral[1]; + log_msg.body.log_RPL3.flow_time_integral = buf.replay.flow_time_integral; + log_msg.body.log_RPL3.flow_quality = buf.replay.flow_quality; + LOGBUFFER_WRITE_AND_COUNT(RPL3); + } + + if (buf.replay.rng_timestamp > 0) { + log_msg.msg_type = LOG_RPL4_MSG; + log_msg.body.log_RPL4.time_rng_usec = buf.replay.rng_timestamp; + log_msg.body.log_RPL4.range_to_ground = buf.replay.range_to_ground; + LOGBUFFER_WRITE_AND_COUNT(RPL4); + } + + if (buf.replay.asp_timestamp > 0) { + log_msg.msg_type = LOG_RPL6_MSG; + log_msg.body.log_RPL6.time_airs_usec = buf.replay.asp_timestamp; + log_msg.body.log_RPL6.indicated_airspeed_m_s = buf.replay.indicated_airspeed_m_s; + log_msg.body.log_RPL6.true_airspeed_m_s = buf.replay.true_airspeed_m_s;; + LOGBUFFER_WRITE_AND_COUNT(RPL6); + } + + if (buf.replay.ev_timestamp > 0) { + log_msg.msg_type = LOG_RPL5_MSG; + log_msg.body.log_RPL5.time_ev_usec = buf.replay.ev_timestamp; + log_msg.body.log_RPL5.x = buf.replay.pos_ev[0]; + log_msg.body.log_RPL5.y = buf.replay.pos_ev[1]; + log_msg.body.log_RPL5.z = buf.replay.pos_ev[2]; + log_msg.body.log_RPL5.q0 = buf.replay.quat_ev[0]; + log_msg.body.log_RPL5.q1 = buf.replay.quat_ev[1]; + log_msg.body.log_RPL5.q2 = buf.replay.quat_ev[2]; + log_msg.body.log_RPL5.q3 = buf.replay.quat_ev[3]; + log_msg.body.log_RPL5.pos_err = buf.replay.pos_err; + log_msg.body.log_RPL5.ang_err = buf.replay.ang_err; + LOGBUFFER_WRITE_AND_COUNT(RPL5); + } } + } - if (buf.replay.rng_timestamp > 0) { - log_msg.msg_type = LOG_RPL4_MSG; - log_msg.body.log_RPL4.time_rng_usec = buf.replay.rng_timestamp; - log_msg.body.log_RPL4.range_to_ground = buf.replay.range_to_ground; - LOGBUFFER_WRITE_AND_COUNT(RPL4); - } + if (log_type == LOG_TYPE_ALL || log_type == LOG_TYPE_NORMAL) { + if (fds[0].revents & POLLIN) { + orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor); - if (buf.replay.asp_timestamp > 0) { - log_msg.msg_type = LOG_RPL6_MSG; - log_msg.body.log_RPL6.timestamp = buf.replay.asp_timestamp; - log_msg.body.log_RPL6.indicated_airspeed_m_s = buf.replay.indicated_airspeed_m_s; - log_msg.body.log_RPL6.true_airspeed_m_s = buf.replay.true_airspeed_m_s; - log_msg.body.log_RPL6.true_airspeed_unfiltered_m_s = buf.replay.true_airspeed_unfiltered_m_s; - log_msg.body.log_RPL6.air_temperature_celsius = buf.replay.air_temperature_celsius; - log_msg.body.log_RPL6.confidence = buf.replay.confidence; - LOGBUFFER_WRITE_AND_COUNT(RPL6); - } - - - } else { /* !record_replay_log */ - - // we poll on sensor combined, so we know it has updated just now - // but we need to copy it again because we are re-using the buffer - orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor); - - for (unsigned i = 0; i < 3; i++) { bool write_IMU = false; bool write_SENS = false; - if (buf.sensor.gyro_timestamp[i] != gyro_timestamp[i]) { - gyro_timestamp[i] = buf.sensor.gyro_timestamp[i]; + if (buf.sensor.timestamp != gyro_timestamp) { + gyro_timestamp = buf.sensor.timestamp; write_IMU = true; } - if (buf.sensor.accelerometer_timestamp[i] != accelerometer_timestamp[i]) { - accelerometer_timestamp[i] = buf.sensor.accelerometer_timestamp[i]; + if (buf.sensor.timestamp + buf.sensor.accelerometer_timestamp_relative != accelerometer_timestamp) { + accelerometer_timestamp = buf.sensor.timestamp + buf.sensor.accelerometer_timestamp_relative; write_IMU = true; } - if (buf.sensor.magnetometer_timestamp[i] != magnetometer_timestamp[i]) { - magnetometer_timestamp[i] = buf.sensor.magnetometer_timestamp[i]; + if (buf.sensor.timestamp + buf.sensor.magnetometer_timestamp_relative != magnetometer_timestamp) { + magnetometer_timestamp = buf.sensor.timestamp + buf.sensor.magnetometer_timestamp_relative; write_IMU = true; } - if (buf.sensor.baro_timestamp[i] != barometer_timestamp[i]) { - barometer_timestamp[i] = buf.sensor.baro_timestamp[i]; - write_SENS = true; - } - - if (buf.sensor.differential_pressure_timestamp[i] != differential_pressure_timestamp[i]) { - differential_pressure_timestamp[i] = buf.sensor.differential_pressure_timestamp[i]; + if (buf.sensor.timestamp + buf.sensor.baro_timestamp_relative != barometer_timestamp) { + barometer_timestamp = buf.sensor.timestamp + buf.sensor.baro_timestamp_relative; write_SENS = true; } if (write_IMU) { - switch (i) { - case 0: - log_msg.msg_type = LOG_IMU_MSG; - break; - case 1: - log_msg.msg_type = LOG_IMU1_MSG; - break; - case 2: - log_msg.msg_type = LOG_IMU2_MSG; - break; - } + log_msg.msg_type = LOG_IMU_MSG; - log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad_s[i * 3 + 0]; - log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad_s[i * 3 + 1]; - log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad_s[i * 3 + 2]; - log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[i * 3 + 0]; - log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[i * 3 + 1]; - log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[i * 3 + 2]; - log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[i * 3 + 0]; - log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[i * 3 + 1]; - log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[i * 3 + 2]; - log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro_temp[i]; - log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer_temp[i]; - log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer_temp[i]; + log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad[0]; + log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad[1]; + log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad[2]; + log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[0]; + log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[1]; + log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[2]; + log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[0]; + log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[1]; + log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[2]; + log_msg.body.log_IMU.temp_gyro = 0; + log_msg.body.log_IMU.temp_acc = 0; + log_msg.body.log_IMU.temp_mag = 0; LOGBUFFER_WRITE_AND_COUNT(IMU); } if (write_SENS) { - switch (i) { - case 0: - log_msg.msg_type = LOG_SENS_MSG; - break; - case 1: - log_msg.msg_type = LOG_AIR1_MSG; - break; - case 2: - continue; - break; - } + log_msg.msg_type = LOG_SENS_MSG; - log_msg.body.log_SENS.baro_pres = buf.sensor.baro_pres_mbar[i]; - log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter[i]; - log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius[i]; - log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa[i]; - log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure_filtered_pa[i]; + log_msg.body.log_SENS.baro_pres = 0; + log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter; + log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius; + log_msg.body.log_SENS.diff_pres = 0; + log_msg.body.log_SENS.diff_pres_filtered = 0; LOGBUFFER_WRITE_AND_COUNT(SENS); } } @@ -1664,6 +1757,27 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(GPS); } + /* --- GPS POSITION - UNIT #2 --- */ + if (copy_if_updated_multi(ORB_ID(vehicle_gps_position), 1, &subs.gps_pos_sub[1], &buf.dual_gps_pos)) { + log_msg.msg_type = LOG_DGPS_MSG; + log_msg.body.log_GPS.gps_time = buf.dual_gps_pos.time_utc_usec; + log_msg.body.log_GPS.fix_type = buf.dual_gps_pos.fix_type; + log_msg.body.log_GPS.eph = buf.dual_gps_pos.eph; + log_msg.body.log_GPS.epv = buf.dual_gps_pos.epv; + log_msg.body.log_GPS.lat = buf.dual_gps_pos.lat; + log_msg.body.log_GPS.lon = buf.dual_gps_pos.lon; + log_msg.body.log_GPS.alt = buf.dual_gps_pos.alt * 0.001f; + log_msg.body.log_GPS.vel_n = buf.dual_gps_pos.vel_n_m_s; + log_msg.body.log_GPS.vel_e = buf.dual_gps_pos.vel_e_m_s; + log_msg.body.log_GPS.vel_d = buf.dual_gps_pos.vel_d_m_s; + log_msg.body.log_GPS.cog = buf.dual_gps_pos.cog_rad; + log_msg.body.log_GPS.sats = buf.dual_gps_pos.satellites_used; + log_msg.body.log_GPS.snr_mean = snr_mean; + log_msg.body.log_GPS.noise_per_ms = buf.dual_gps_pos.noise_per_ms; + log_msg.body.log_GPS.jamming_indicator = buf.dual_gps_pos.jamming_indicator; + LOGBUFFER_WRITE_AND_COUNT(GPS); + } + /* --- SATELLITE INFO - UNIT #1 --- */ if (_extended_logging) { @@ -1834,7 +1948,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_BATT_MSG; log_msg.body.log_BATT.voltage = buf.battery.voltage_v; log_msg.body.log_BATT.voltage_filtered = buf.battery.voltage_filtered_v; - log_msg.body.log_BATT.current = buf.battery.current_a; + log_msg.body.log_BATT.current = buf.battery.current_a; log_msg.body.log_BATT.current_filtered = buf.battery.current_filtered_a; log_msg.body.log_BATT.discharged = buf.battery.discharged_mah; log_msg.body.log_BATT.remaining = buf.battery.remaining; @@ -2019,7 +2133,7 @@ int sdlog2_thread_main(int argc, char *argv[]) memcpy(&(log_msg.body.log_EST0.s), buf.estimator_status.states, maxcopy0); log_msg.body.log_EST0.n_states = buf.estimator_status.n_states; log_msg.body.log_EST0.nan_flags = buf.estimator_status.nan_flags; - log_msg.body.log_EST0.health_flags = buf.estimator_status.health_flags; + log_msg.body.log_EST0.fault_flags = buf.estimator_status.filter_fault_flags; log_msg.body.log_EST0.timeout_flags = buf.estimator_status.timeout_flags; LOGBUFFER_WRITE_AND_COUNT(EST0); @@ -2035,6 +2149,7 @@ int sdlog2_thread_main(int argc, char *argv[]) memcpy(&(log_msg.body.log_EST2.cov), buf.estimator_status.covariances, maxcopy2); log_msg.body.log_EST2.gps_check_fail_flags = buf.estimator_status.gps_check_fail_flags; log_msg.body.log_EST2.control_mode_flags = buf.estimator_status.control_mode_flags; + log_msg.body.log_EST2.health_flags = buf.estimator_status.health_flags; LOGBUFFER_WRITE_AND_COUNT(EST2); log_msg.msg_type = LOG_EST3_MSG; @@ -2109,16 +2224,6 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(WIND); } - /* --- ENCODERS --- */ - if (copy_if_updated(ORB_ID(encoders), &subs.encoders_sub, &buf.encoders)) { - log_msg.msg_type = LOG_ENCD_MSG; - log_msg.body.log_ENCD.cnt0 = buf.encoders.counts[0]; - log_msg.body.log_ENCD.vel0 = buf.encoders.velocity[0]; - log_msg.body.log_ENCD.cnt1 = buf.encoders.counts[1]; - log_msg.body.log_ENCD.vel1 = buf.encoders.velocity[1]; - LOGBUFFER_WRITE_AND_COUNT(ENCD); - } - /* --- TIMESYNC OFFSET --- */ if (copy_if_updated(ORB_ID(time_offset), &subs.tsync_sub, &buf.time_offset)) { log_msg.msg_type = LOG_TSYN_MSG; @@ -2183,6 +2288,14 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(LAND); } + /* --- LOAD --- */ + if (copy_if_updated(ORB_ID(cpuload), &subs.cpuload_sub, &buf.cpuload)) { + log_msg.msg_type = LOG_LOAD_MSG; + log_msg.body.log_LOAD.cpu_load = buf.cpuload.load; + LOGBUFFER_WRITE_AND_COUNT(LOAD); + + } + pthread_mutex_lock(&logbuffer_mutex); /* signal the other thread new data, but not yet unlock */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 9a8d958d6c..4d1f3f1f87 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -146,6 +146,7 @@ struct log_LPSP_s { /* --- GPS - GPS POSITION --- */ #define LOG_GPS_MSG 8 +#define LOG_DGPS_MSG 58 struct log_GPS_s { uint64_t gps_time; uint8_t fix_type; @@ -181,7 +182,7 @@ struct log_STAT_s { uint8_t nav_state; uint8_t arming_state; uint8_t failsafe; - float load; + uint8_t is_rot_wing; }; /* --- RC - RC INPUT CHANNELS --- */ @@ -396,7 +397,7 @@ struct log_EST0_s { float s[12]; uint8_t n_states; uint8_t nan_flags; - uint8_t health_flags; + uint16_t fault_flags; uint8_t timeout_flags; }; @@ -409,9 +410,10 @@ struct log_EST1_s { /* --- EST2 - ESTIMATOR STATUS --- */ #define LOG_EST2_MSG 34 struct log_EST2_s { - float cov[12]; - uint16_t gps_check_fail_flags; - uint16_t control_mode_flags; + float cov[12]; + uint16_t gps_check_fail_flags; + uint16_t control_mode_flags; + uint8_t health_flags; }; /* --- EST3 - ESTIMATOR STATUS --- */ @@ -420,24 +422,6 @@ struct log_EST3_s { float cov[16]; }; -/* --- EST4 - ESTIMATOR INNOVATIONS --- */ -#define LOG_EST4_MSG 48 -struct log_EST4_s { - float s[12]; -}; - -/* --- EST5 - ESTIMATOR INNOVATIONS --- */ -#define LOG_EST5_MSG 49 -struct log_EST5_s { - float s[10]; -}; - -/* --- EST6 - ESTIMATOR INNOVATIONS --- */ -#define LOG_EST6_MSG 53 -struct log_EST6_s { - float s[6]; -}; - /* --- TEL0..3 - TELEMETRY STATUS --- */ #define LOG_TEL0_MSG 36 #define LOG_TEL1_MSG 37 @@ -518,22 +502,34 @@ struct log_CTS_s { float yaw_rate; }; +/* --- EST4 - ESTIMATOR INNOVATIONS --- */ +#define LOG_EST4_MSG 48 +struct log_EST4_s { + float s[12]; +}; + +/* --- EST5 - ESTIMATOR INNOVATIONS --- */ +#define LOG_EST5_MSG 49 +struct log_EST5_s { + float s[10]; +}; + #define LOG_OUT1_MSG 50 /* --- EKF2 REPLAY Part 1 --- */ #define LOG_RPL1_MSG 51 struct log_RPL1_s { uint64_t time_ref; - uint64_t gyro_integral_dt; - uint64_t accelerometer_integral_dt; + float gyro_integral_dt; + float accelerometer_integral_dt; uint64_t magnetometer_timestamp; uint64_t baro_timestamp; - float gyro_integral_x_rad; - float gyro_integral_y_rad; - float gyro_integral_z_rad; - float accelerometer_integral_x_m_s; - float accelerometer_integral_y_m_s; - float accelerometer_integral_z_m_s; + float gyro_x_rad; + float gyro_y_rad; + float gyro_z_rad; + float accelerometer_x_m_s2; + float accelerometer_y_m_s2; + float accelerometer_z_m_s2; float magnetometer_x_ga; float magnetometer_y_ga; float magnetometer_z_ga; @@ -558,6 +554,13 @@ struct log_RPL2_s { float vel_d_m_s; bool vel_ned_valid; }; + +/* --- EST6 - ESTIMATOR INNOVATIONS --- */ +#define LOG_EST6_MSG 53 +struct log_EST6_s { + float s[6]; +}; + /* --- EKF2 REPLAY Part 3 --- */ #define LOG_RPL3_MSG 54 struct log_RPL3_s { @@ -570,26 +573,6 @@ struct log_RPL3_s { uint8_t flow_quality; }; -/* --- EKF2 REPLAY Part 4 --- */ -#define LOG_RPL4_MSG 56 -struct log_RPL4_s { - uint64_t time_rng_usec; - float range_to_ground; -}; - -/* --- EKF2 REPLAY Part 4 --- */ -#define LOG_RPL6_MSG 59 -struct log_RPL6_s { - uint64_t timestamp; - float indicated_airspeed_m_s; - float true_airspeed_m_s; - float true_airspeed_unfiltered_m_s; - float air_temperature_celsius; - float confidence; -}; - - - /* --- CAMERA TRIGGER --- */ #define LOG_CAMT_MSG 55 struct log_CAMT_s { @@ -597,11 +580,51 @@ struct log_CAMT_s { uint32_t seq; }; +/* --- EKF2 REPLAY Part 4 --- */ +#define LOG_RPL4_MSG 56 +struct log_RPL4_s { + uint64_t time_rng_usec; + float range_to_ground; +}; + +/* --- LAND DETECTOR --- */ #define LOG_LAND_MSG 57 struct log_LAND_s { uint8_t landed; }; +/* 58 used for DGPS message + shares struct with GPS MSG 8*/ + +/* --- EKF2 REPLAY Part 6 --- */ +#define LOG_RPL6_MSG 59 +struct log_RPL6_s { + uint64_t time_airs_usec; + float indicated_airspeed_m_s; + float true_airspeed_m_s; +}; + +/* --- EKF2 REPLAY Part 5 --- */ +#define LOG_RPL5_MSG 60 +struct log_RPL5_s { + uint64_t time_ev_usec; + float x; + float y; + float z; + float q0; + float q1; + float q2; + float q3; + float pos_err; + float ang_err; +}; + +/* --- SYSTEM LOAD --- */ +#define LOG_LOAD_MSG 61 +struct log_LOAD_s { + float cpu_load; +}; + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -623,11 +646,11 @@ struct log_PARM_s { char name[16]; float value; }; +#pragma pack(pop) // the lower type of initialisation is not supported in C++ #ifndef __cplusplus -#pragma pack(pop) /* construct list of all message formats */ static const struct log_format_s log_formats[] = { /* business-level messages, ID < 0x80 */ @@ -641,9 +664,10 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(LPOS, "ffffffffLLfBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,GFlg,EPH,EPV"), LOG_FORMAT(LPSP, "ffffffffff", "X,Y,Z,Yaw,VX,VY,VZ,AX,AY,AZ"), LOG_FORMAT(GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"), + LOG_FORMAT_S(DGPS, GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"), LOG_FORMAT_S(ATTC, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT_S(ATC1, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), - LOG_FORMAT(STAT, "BBBBf", "MainState,NavState,ArmS,Failsafe,Load"), + LOG_FORMAT(STAT, "BBBBB", "MainState,NavState,ArmS,Failsafe,IsRotWing"), LOG_FORMAT(VTOL, "fBBB", "Arsp,RwMode,TransMode,Failsafe"), LOG_FORMAT(CTS, "fffffff", "Vx_b,Vy_b,Vz_b,Vinf,P,Q,R"), LOG_FORMAT(RC, "ffffffffffffBBBL", "C0,C1,C2,C3,C4,C5,C6,C7,C8,C9,C10,C11,RSSI,CNT,Lost,Drop"), @@ -662,9 +686,9 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT_S(TEL1, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), LOG_FORMAT_S(TEL2, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), LOG_FORMAT_S(TEL3, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), - LOG_FORMAT(EST0, "ffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fHealth,fTOut"), + LOG_FORMAT(EST0, "ffffffffffffBBHB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fFault,fTOut"), LOG_FORMAT(EST1, "ffffffffffffffff", "s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"), - LOG_FORMAT(EST2, "ffffffffffffHH", "P0,P1,P2,P3,P4,P5,P6,P7,P8,P9,P10,P11,GCHK,CTRL"), + LOG_FORMAT(EST2, "ffffffffffffHHB", "P0,P1,P2,P3,P4,P5,P6,P7,P8,P9,P10,P11,GCHK,CTRL,fHealth"), LOG_FORMAT(EST3, "ffffffffffffffff", "P12,P13,P14,P15,P16,P17,P18,P19,P20,P21,P22,P23,P24,P25,P26,P27"), LOG_FORMAT(EST4, "ffffffffffff", "VxI,VyI,VzI,PxI,PyI,PzI,VxIV,VyIV,VzIV,PxIV,PyIV,PzIV"), LOG_FORMAT(EST5, "ffffffffff", "MAGxI,MAGyI,MAGzI,MAGxIV,MAGyIV,MAGzIV,HeadI,HeadIV,AirI,AirIV"), @@ -682,12 +706,14 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(TSYN, "Q", "TimeOffset"), LOG_FORMAT(MACS, "fff", "RRint,PRint,YRint"), LOG_FORMAT(CAMT, "QI", "timestamp,seq"), - LOG_FORMAT(RPL1, "QQQQQffffffffff", "t,gIdt,aIdt,Tm,Tb,gIx,gIy,gIz,aIx,aIy,aIz,magX,magY,magZ,b_alt"), + LOG_FORMAT(RPL1, "QffQQffffffffff", "t,gIdt,aIdt,Tm,Tb,gx,gy,gz,ax,ay,az,magX,magY,magZ,b_alt"), LOG_FORMAT(RPL2, "QQLLiMMfffffffM", "Tpos,Tvel,lat,lon,alt,fix,nsats,eph,epv,sacc,v,vN,vE,vD,v_val"), LOG_FORMAT(RPL3, "QffffIB", "Tflow,fx,fy,gx,gy,delT,qual"), LOG_FORMAT(RPL4, "Qf", "Trng,rng"), - LOG_FORMAT(RPL6, "Qfffff", "Tasp,inAsp,trAsp,ufAsp,tpAsp,confAsp"), + LOG_FORMAT(RPL5, "Qfffffffff", "Tev,x,y,z,q0,q1,q2,q3,posErr,angErr"), + LOG_FORMAT(RPL6, "Qff", "Tasp,inAsp,trAsp"), LOG_FORMAT(LAND, "B", "Landed"), + LOG_FORMAT(LOAD, "f", "CPU"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ LOG_FORMAT(TIME, "Q", "StartTime"), diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index 93f2f59d2f..a23f3c5341 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -35,10 +35,9 @@ px4_add_module( MODULE modules__sensors MAIN sensors PRIORITY "SCHED_PRIORITY_MAX-5" - STACK_MAIN 1200 + STACK_MAIN 1300 COMPILE_FLAGS - -Wno-type-limits - -O3 + -Os SRCS sensors.cpp sensors_init.cpp diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 5454f34586..d66fdb3121 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -1932,7 +1932,7 @@ PARAM_DEFINE_FLOAT(RC18_REV, 1.0f); PARAM_DEFINE_FLOAT(RC18_DZ, 0.0f); /** - * Enable relay control of relay 1 mapped to the Spektrum receiver power supply + * Relay control of relay 1 mapped to the Spektrum receiver power supply * * @min 0 * @max 1 @@ -1964,28 +1964,80 @@ PARAM_DEFINE_INT32(RC_DSM_BIND, -1); PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000); /** - * Scaling factor for battery voltage sensor on FMU v2. + * Scaling from ADC counts to volt on the ADC input (battery voltage) + * + * This is not the battery voltage, but the intermediate ADC voltage. + * A value of -1 signifies that the board defaults are used, which is + * highly recommended. * * @group Battery Calibration * @decimal 8 */ -PARAM_DEFINE_FLOAT(BAT_V_SCALING, -1.0f); +PARAM_DEFINE_FLOAT(BAT_CNT_V_VOLT, -1.0f); /** - * Scaling factor for battery current sensor. + * Scaling from ADC counts to volt on the ADC input (battery current) + * + * This is not the battery current, but the intermediate ADC voltage. + * A value of -1 signifies that the board defaults are used, which is + * highly recommended. * * @group Battery Calibration * @decimal 8 */ -PARAM_DEFINE_FLOAT(BAT_C_SCALING, -1.0); +PARAM_DEFINE_FLOAT(BAT_CNT_V_CURR, -1.0); /** - * Offset for battery current sensor. + * Offset in volt as seen by the ADC input of the current sensor. + * + * This offset will be subtracted before calculating the battery + * current based on the voltage. * * @group Battery Calibration * @decimal 8 */ -PARAM_DEFINE_FLOAT(BAT_C_OFFSET, -1.0); +PARAM_DEFINE_FLOAT(BAT_V_OFFS_CURR, 0.0); + +/** + * Battery voltage divider (V divider) + * + * This is the divider from battery voltage to 3.3V ADC voltage. + * If using e.g. Mauch power modules the value from the datasheet + * can be applied straight here. A value of -1 means to use + * the board default. + * + * @group Battery Calibration + * @decimal 8 + */ +PARAM_DEFINE_FLOAT(BAT_V_DIV, -1.0); + +/** + * Battery current per volt (A/V) + * + * The voltage seen by the 3.3V ADC multiplied by this factor + * will determine the battery current. A value of -1 means to use + * the board default. + * + * @group Battery Calibration + * @decimal 8 + */ +PARAM_DEFINE_FLOAT(BAT_A_PER_V, -1.0); + +/** + * Battery monitoring source. + * + * This parameter controls the source of battery data. The value 'Power Module' + * means that measurements are expected to come from a power module. If the value is set to + * 'External' then the system expects to receive mavlink battery status messages. + * + * @min 0 + * @max 1 + * @value 0 Power Module + * @value 1 External + * @group Battery Calibration + */ + +PARAM_DEFINE_INT32(BAT_SOURCE, 0); /** @@ -2464,6 +2516,34 @@ PARAM_DEFINE_INT32(RC_MAP_KILL_SW, 0); */ PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0); +/** + * VTOL transition switch channel mapping + * + * @min 0 + * @max 18 + * @group Radio Switches + * @value 0 Unassigned + * @value 1 Channel 1 + * @value 2 Channel 2 + * @value 3 Channel 3 + * @value 4 Channel 4 + * @value 5 Channel 5 + * @value 6 Channel 6 + * @value 7 Channel 7 + * @value 8 Channel 8 + * @value 9 Channel 9 + * @value 10 Channel 10 + * @value 11 Channel 11 + * @value 12 Channel 12 + * @value 13 Channel 13 + * @value 14 Channel 14 + * @value 15 Channel 15 + * @value 16 Channel 16 + * @value 17 Channel 17 + * @value 18 Channel 18 + */ +PARAM_DEFINE_INT32(RC_MAP_TRANS_SW, 0); + /** * AUX1 Passthrough RC Channel * @@ -2878,6 +2958,24 @@ PARAM_DEFINE_FLOAT(RC_OFFB_TH, 0.5f); */ PARAM_DEFINE_FLOAT(RC_KILLSWITCH_TH, 0.25f); +/** + * Threshold for the VTOL transition switch + * + * 0-1 indicate where in the full channel range the threshold sits + * 0 : min + * 1 : max + * sign indicates polarity of comparison + * positive : true when channel>th + * negative : true when channel */ -// TODO-JYW: TESTING-TESTING -#define DEBUG_BUILD 1 - #include +#include #include -#include #include +#include #include #include @@ -67,8 +65,6 @@ #include #include -#include - #include #include #include @@ -79,16 +75,18 @@ #include #include +#include +#include #include #include #include #include #include + #include -#include - #include +#include #include #include @@ -139,12 +137,6 @@ using namespace DriverFramework; #define SENSOR_COUNT_MAX 3 -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - #define CAL_ERROR_APPLY_CAL_MSG "FAILED APPLYING %s CAL #%u" /** @@ -174,6 +166,9 @@ public: */ int start(); + + void print_status(); + private: static const unsigned _rc_max_chan_count = input_rc_s::RC_INPUT_MAX_CHANNELS; /**< maximum number of r/c channels we handle */ @@ -210,19 +205,35 @@ private: int _sensors_task; /**< task handle for sensor task */ bool _hil_enabled; /**< if true, HIL is active */ - bool _publishing; /**< if true, we are publishing sensor data */ + bool _publishing; /**< if true, we are publishing sensor data (in HIL mode, we don't) */ bool _armed; /**< arming status of the vehicle */ - int _gyro_sub[SENSOR_COUNT_MAX]; /**< raw gyro data subscription */ - int _accel_sub[SENSOR_COUNT_MAX]; /**< raw accel data subscription */ - int _mag_sub[SENSOR_COUNT_MAX]; /**< raw mag data subscription */ - int _baro_sub[SENSOR_COUNT_MAX]; /**< raw baro data subscription */ - int _actuator_ctrl_0_sub; /**< attitude controls sub */ - unsigned _gyro_count; /**< raw gyro data count */ - unsigned _accel_count; /**< raw accel data count */ - unsigned _mag_count; /**< raw mag data count */ - unsigned _baro_count; /**< raw baro data count */ + struct SensorData { + SensorData() + : last_best_vote(0), + subscription_count(0), + voter(SENSOR_COUNT_MAX), + last_failover_count(0) + { + for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) { + subscription[i] = -1; + } + } + int subscription[SENSOR_COUNT_MAX]; /**< raw sensor data subscription */ + uint8_t priority[SENSOR_COUNT_MAX]; /**< sensor priority */ + uint8_t last_best_vote; /**< index of the latest best vote */ + int subscription_count; + DataValidatorGroup voter; + unsigned int last_failover_count; + }; + + SensorData _gyro; + SensorData _accel; + SensorData _mag; + SensorData _baro; + + int _actuator_ctrl_0_sub; /**< attitude controls sub */ int _rc_sub; /**< raw rc channels data subscription */ int _diff_pres_sub; /**< raw differential pressure subscription */ int _vcontrol_mode_sub; /**< vehicle control mode subscription */ @@ -237,6 +248,7 @@ private: orb_advert_t _battery_pub; /**< battery status */ orb_advert_t _airspeed_pub; /**< airspeed */ orb_advert_t _diff_pres_pub; /**< differential_pressure */ + orb_advert_t _mavlink_log_pub; perf_counter_t _loop_perf; /**< loop performance counter */ @@ -244,7 +256,6 @@ private: struct rc_channels_s _rc; /**< r/c channel data */ struct battery_status_s _battery_status; /**< battery status */ - struct baro_report _barometer; /**< barometer data */ struct differential_pressure_s _diff_pres; struct airspeed_s _airspeed; struct rc_parameter_map_s _rc_parameter_map; @@ -255,6 +266,16 @@ private: Battery _battery; /**< Helper lib to publish battery_status topic. */ + float _last_baro_pressure[SENSOR_COUNT_MAX]; /**< pressure from last baro sensors */ + float _last_best_baro_pressure; /**< pressure from last best baro */ + sensor_combined_s _last_sensor_data[SENSOR_COUNT_MAX]; /**< latest sensor data from all sensors instances */ + uint64_t _last_accel_timestamp[SENSOR_COUNT_MAX]; /**< latest full timestamp */ + uint64_t _last_mag_timestamp[SENSOR_COUNT_MAX]; /**< latest full timestamp */ + uint64_t _last_baro_timestamp[SENSOR_COUNT_MAX]; /**< latest full timestamp */ + + hrt_abstime _vibration_warning_timestamp; + bool _vibration_warning; + struct { float min[_rc_max_chan_count]; float trim[_rc_max_chan_count]; @@ -284,6 +305,7 @@ private: int rc_map_acro_sw; int rc_map_offboard_sw; int rc_map_kill_sw; + int rc_map_trans_sw; int rc_map_flaps; @@ -307,6 +329,7 @@ private: float rc_acro_th; float rc_offboard_th; float rc_killswitch_th; + float rc_trans_th; bool rc_assist_inv; bool rc_auto_inv; bool rc_rattitude_inv; @@ -316,13 +339,19 @@ private: bool rc_acro_inv; bool rc_offboard_inv; bool rc_killswitch_inv; + bool rc_trans_inv; float battery_voltage_scaling; float battery_current_scaling; float battery_current_offset; + float battery_v_div; + float battery_a_per_v; + int32_t battery_source; float baro_qnh; + float vibration_warning_threshold; + } _parameters; /**< local copies of interesting parameters */ struct { @@ -349,6 +378,7 @@ private: param_t rc_map_acro_sw; param_t rc_map_offboard_sw; param_t rc_map_kill_sw; + param_t rc_map_trans_sw; param_t rc_map_flaps; @@ -376,10 +406,14 @@ private: param_t rc_acro_th; param_t rc_offboard_th; param_t rc_killswitch_th; + param_t rc_trans_th; param_t battery_voltage_scaling; param_t battery_current_scaling; param_t battery_current_offset; + param_t battery_v_div; + param_t battery_a_per_v; + param_t battery_source; param_t board_rotation; @@ -387,11 +421,12 @@ private: param_t baro_qnh; + param_t vibe_thresh; /**< vibration threshold */ + } _parameter_handles; /**< handles for interesting parameters */ - int init_sensor_class(const struct orb_metadata *meta, int *subs, - uint32_t *priorities, uint32_t *errcount); + void init_sensor_class(const struct orb_metadata *meta, SensorData &sensor_data); /** * Update our local parameter cache. @@ -496,6 +531,18 @@ private: */ void adc_poll(struct sensor_combined_s &raw); + /** + * Check & handle failover of a sensor + * @return true if a switch occured (could be for a non-critical reason) + */ + bool check_failover(SensorData &sensor, const char *sensor_name); + + /** + * check vibration levels and output a warning if they're high + * @return true on high vibration + */ + bool check_vibration(); + /** * Shim for calling task_main from task_create. */ @@ -522,16 +569,6 @@ Sensors::Sensors() : _hil_enabled(false), _publishing(true), _armed(false), - - /* subscriptions */ - _gyro_sub{ -1, -1, -1}, - _accel_sub{ -1, -1, -1}, - _mag_sub{ -1, -1, -1}, - _baro_sub{ -1, -1, -1}, - _gyro_count(0), - _accel_count(0), - _mag_count(0), - _baro_count(0), _rc_sub(-1), _vcontrol_mode_sub(-1), _params_sub(-1), @@ -546,6 +583,7 @@ Sensors::Sensors() : _battery_pub(nullptr), _airspeed_pub(nullptr), _diff_pres_pub(nullptr), + _mavlink_log_pub(nullptr), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "sensors")), @@ -553,20 +591,23 @@ Sensors::Sensors() : _param_rc_values{}, _board_rotation{}, - _mag_rotation{} + _mag_rotation{}, + + _last_best_baro_pressure(0.f), + + _vibration_warning_timestamp(0), + _vibration_warning(false) { - /* initialize subscriptions */ - for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) { - _gyro_sub[i] = -1; - _accel_sub[i] = -1; - _mag_sub[i] = -1; - _baro_sub[i] = -1; - } + _mag.voter.set_timeout(300000); memset(&_rc, 0, sizeof(_rc)); memset(&_diff_pres, 0, sizeof(_diff_pres)); memset(&_parameters, 0, sizeof(_parameters)); memset(&_rc_parameter_map, 0, sizeof(_rc_parameter_map)); + memset(&_last_sensor_data, 0, sizeof(_last_sensor_data)); + memset(&_last_accel_timestamp, 0, sizeof(_last_accel_timestamp)); + memset(&_last_mag_timestamp, 0, sizeof(_last_mag_timestamp)); + memset(&_last_baro_timestamp, 0, sizeof(_last_baro_timestamp)); /* basic r/c parameters */ for (unsigned i = 0; i < _rc_max_chan_count; i++) { @@ -614,6 +655,7 @@ Sensors::Sensors() : _parameter_handles.rc_map_acro_sw = param_find("RC_MAP_ACRO_SW"); _parameter_handles.rc_map_offboard_sw = param_find("RC_MAP_OFFB_SW"); _parameter_handles.rc_map_kill_sw = param_find("RC_MAP_KILL_SW"); + _parameter_handles.rc_map_trans_sw = param_find("RC_MAP_TRANS_SW"); _parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1"); _parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2"); @@ -642,14 +684,19 @@ Sensors::Sensors() : _parameter_handles.rc_acro_th = param_find("RC_ACRO_TH"); _parameter_handles.rc_offboard_th = param_find("RC_OFFB_TH"); _parameter_handles.rc_killswitch_th = param_find("RC_KILLSWITCH_TH"); + _parameter_handles.rc_trans_th = param_find("RC_TRANS_TH"); + /* Differential pressure offset */ _parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF"); _parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC"); - _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); - _parameter_handles.battery_current_scaling = param_find("BAT_C_SCALING"); - _parameter_handles.battery_current_offset = param_find("BAT_C_OFFSET"); + _parameter_handles.battery_voltage_scaling = param_find("BAT_CNT_V_VOLT"); + _parameter_handles.battery_current_scaling = param_find("BAT_CNT_V_CURR"); + _parameter_handles.battery_current_offset = param_find("BAT_V_OFFS_CURR"); + _parameter_handles.battery_v_div = param_find("BAT_V_DIV"); + _parameter_handles.battery_a_per_v = param_find("BAT_A_PER_V"); + _parameter_handles.battery_source = param_find("BAT_SOURCE"); /* rotations */ _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); @@ -662,6 +709,8 @@ Sensors::Sensors() : /* Barometer QNH */ _parameter_handles.baro_qnh = param_find("SENS_BARO_QNH"); + _parameter_handles.vibe_thresh = param_find("ATT_VIBE_THRESH"); + // These are parameters for which QGroundControl always expects to be returned in a list request. // We do a param_find here to force them into the list. (void)param_find("RC_CHAN_CNT"); @@ -721,6 +770,7 @@ Sensors::parameters_update() bool rc_valid = true; float tmpScaleFactor = 0.0f; float tmpRevFactor = 0.0f; + int ret = PX4_OK; /* rc values */ for (unsigned int i = 0; i < _rc_max_chan_count; i++) { @@ -738,7 +788,7 @@ Sensors::parameters_update() if (!PX4_ISFINITE(tmpScaleFactor) || (tmpRevFactor < 0.000001f) || (tmpRevFactor > 0.2f)) { - warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, (double)tmpScaleFactor, (int)(_parameters.rev[i])); + PX4_WARN("RC chan %u not sane, scaling: %8.6f, rev: %d", i, (double)tmpScaleFactor, (int)(_parameters.rev[i])); /* scaling factors do not make sense, lock them down */ _parameters.scaling_factor[i] = 0.0f; rc_valid = false; @@ -750,66 +800,70 @@ Sensors::parameters_update() /* handle wrong values */ if (!rc_valid) { - warnx("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n"); + PX4_ERR("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n"); } const char *paramerr = "FAIL PARM LOAD"; /* channel mapping */ if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) { - warnx("%s", paramerr); + PX4_WARN("%s", paramerr); } if (param_get(_parameter_handles.rc_map_pitch, &(_parameters.rc_map_pitch)) != OK) { - warnx("%s", paramerr); + PX4_WARN("%s", paramerr); } if (param_get(_parameter_handles.rc_map_yaw, &(_parameters.rc_map_yaw)) != OK) { - warnx("%s", paramerr); + PX4_WARN("%s", paramerr); } if (param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle)) != OK) { - warnx("%s", paramerr); + PX4_WARN("%s", paramerr); } if (param_get(_parameter_handles.rc_map_failsafe, &(_parameters.rc_map_failsafe)) != OK) { - warnx("%s", paramerr); + PX4_WARN("%s", paramerr); } if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) { - warnx("%s", paramerr); + PX4_WARN("%s", paramerr); } if (param_get(_parameter_handles.rc_map_return_sw, &(_parameters.rc_map_return_sw)) != OK) { - warnx("%s", paramerr); + PX4_WARN("%s", paramerr); } if (param_get(_parameter_handles.rc_map_rattitude_sw, &(_parameters.rc_map_rattitude_sw)) != OK) { - warnx("%s", paramerr); + PX4_WARN("%s", paramerr); } if (param_get(_parameter_handles.rc_map_posctl_sw, &(_parameters.rc_map_posctl_sw)) != OK) { - warnx("%s", paramerr); + PX4_WARN("%s", paramerr); } if (param_get(_parameter_handles.rc_map_loiter_sw, &(_parameters.rc_map_loiter_sw)) != OK) { - warnx("%s", paramerr); + PX4_WARN("%s", paramerr); } if (param_get(_parameter_handles.rc_map_acro_sw, &(_parameters.rc_map_acro_sw)) != OK) { - warnx("%s", paramerr); + PX4_WARN("%s", paramerr); } if (param_get(_parameter_handles.rc_map_offboard_sw, &(_parameters.rc_map_offboard_sw)) != OK) { - warnx("%s", paramerr); + PX4_WARN("%s", paramerr); } if (param_get(_parameter_handles.rc_map_kill_sw, &(_parameters.rc_map_kill_sw)) != OK) { + PX4_WARN("%s", paramerr); + } + + if (param_get(_parameter_handles.rc_map_trans_sw, &(_parameters.rc_map_trans_sw)) != OK) { warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { - warnx("%s", paramerr); + PX4_WARN("%s", paramerr); } param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)); @@ -852,6 +906,9 @@ Sensors::parameters_update() param_get(_parameter_handles.rc_killswitch_th, &(_parameters.rc_killswitch_th)); _parameters.rc_killswitch_inv = (_parameters.rc_killswitch_th < 0); _parameters.rc_killswitch_th = fabs(_parameters.rc_killswitch_th); + param_get(_parameter_handles.rc_trans_th, &(_parameters.rc_trans_th)); + _parameters.rc_trans_inv = (_parameters.rc_trans_th < 0); + _parameters.rc_trans_th = fabs(_parameters.rc_trans_th); /* update RC function mappings */ _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE] = _parameters.rc_map_throttle - 1; @@ -867,6 +924,7 @@ Sensors::parameters_update() _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ACRO] = _parameters.rc_map_acro_sw - 1; _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD] = _parameters.rc_map_offboard_sw - 1; _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_KILLSWITCH] = _parameters.rc_map_kill_sw - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_TRANSITION] = _parameters.rc_map_trans_sw - 1; _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS] = _parameters.rc_map_flaps - 1; @@ -886,53 +944,75 @@ Sensors::parameters_update() /* scaling of ADC ticks to battery voltage */ if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) { - warnx("%s", paramerr); + PX4_WARN("%s", paramerr); } else if (_parameters.battery_voltage_scaling < 0.0f) { /* apply scaling according to defaults if set to default */ -#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4) - _parameters.battery_voltage_scaling = 0.011f; -#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V2) || defined ( CONFIG_ARCH_BOARD_MINDPX_V2 ) - _parameters.battery_voltage_scaling = 0.0082f; -#elif defined (CONFIG_ARCH_BOARD_AEROCORE) - _parameters.battery_voltage_scaling = 0.0063f; -#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V1) - _parameters.battery_voltage_scaling = 0.00459340659f; -#else - /* ensure a missing default trips a low voltage lockdown */ - _parameters.battery_voltage_scaling = 0.00001f; -#endif + _parameters.battery_voltage_scaling = (3.3f / 4096); + param_set(_parameter_handles.battery_voltage_scaling, &_parameters.battery_voltage_scaling); } /* scaling of ADC ticks to battery current */ if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) { - warnx("%s", paramerr); + PX4_WARN("%s", paramerr); } else if (_parameters.battery_current_scaling < 0.0f) { /* apply scaling according to defaults if set to default */ -#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4) - /* current scaling for ACSP4 */ - _parameters.battery_current_scaling = 0.0293f; -#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V2) || defined ( CONFIG_ARCH_BOARD_MINDPX_V2 ) - /* current scaling for 3DR power brick */ - _parameters.battery_current_scaling = 0.0124f; -#elif defined (CONFIG_ARCH_BOARD_AEROCORE) - _parameters.battery_current_scaling = 0.0124f; -#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V1) - _parameters.battery_current_scaling = 0.0124f; -#else - /* ensure a missing default leads to an unrealistic current value */ - _parameters.battery_current_scaling = 0.00001f; -#endif + _parameters.battery_current_scaling = (3.3f / 4096); + param_set(_parameter_handles.battery_current_scaling, &_parameters.battery_current_scaling); } if (param_get(_parameter_handles.battery_current_offset, &(_parameters.battery_current_offset)) != OK) { - warnx("%s", paramerr); + PX4_WARN("%s", paramerr); - } else if (_parameters.battery_current_offset < 0.0f) { - _parameters.battery_current_offset = 0.0f; } + if (param_get(_parameter_handles.battery_v_div, &(_parameters.battery_v_div)) != OK) { + PX4_WARN("%s", paramerr); + _parameters.battery_v_div = 0.0f; + + } else if (_parameters.battery_v_div <= 0.0f) { + /* apply scaling according to defaults if set to default */ +#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4) + _parameters.battery_v_div = 13.653333333f; +#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V2) || defined ( CONFIG_ARCH_BOARD_MINDPX_V2 ) + _parameters.battery_v_div = 10.177939394f; +#elif defined (CONFIG_ARCH_BOARD_AEROCORE) + _parameters.battery_v_div = 7.8196363636f; +#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V1) + _parameters.battery_v_div = 5.7013919372f; +#elif defined (CONFIG_ARCH_BOARD_SITL) + _parameters.battery_v_div = 10.177939394f; +#else + /* ensure a missing default trips a low voltage lockdown */ + _parameters.battery_v_div = 0.0f; +#endif + param_set(_parameter_handles.battery_v_div, &_parameters.battery_v_div); + } + + if (param_get(_parameter_handles.battery_a_per_v, &(_parameters.battery_a_per_v)) != OK) { + PX4_WARN("%s", paramerr); + _parameters.battery_a_per_v = 0.0f; + + } else if (_parameters.battery_a_per_v <= 0.0f) { + /* apply scaling according to defaults if set to default */ +#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4) + /* current scaling for ACSP4 */ + _parameters.battery_a_per_v = 36.367515152f; +#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V2) || defined (CONFIG_ARCH_BOARD_MINDPX_V2) || defined (CONFIG_ARCH_BOARD_AEROCORE) || defined (CONFIG_ARCH_BOARD_PX4FMU_V1) + /* current scaling for 3DR power brick */ + _parameters.battery_a_per_v = 15.391030303f; +#elif defined (CONFIG_ARCH_BOARD_SITL) + _parameters.battery_a_per_v = 15.391030303f; +#else + /* ensure a missing default leads to an unrealistic current value */ + _parameters.battery_a_per_v = 0.0f; +#endif + param_set(_parameter_handles.battery_a_per_v, &_parameters.battery_a_per_v); + } + + param_get(_parameter_handles.battery_source, &(_parameters.battery_source)); + param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation)); get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation); @@ -953,25 +1033,27 @@ Sensors::parameters_update() DevHandle h_baro; DevMgr::getHandle(BARO0_DEVICE_PATH, h_baro); -#if !defined(__PX4_QURT) && !defined(__RPI2) +#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP) // TODO: this needs fixing for QURT and Raspberry Pi if (!h_baro.isValid()) { - warnx("ERROR: no barometer found on %s (%d)", BARO0_DEVICE_PATH, h_baro.getError()); - return ERROR; + PX4_ERR("no barometer found on %s (%d)", BARO0_DEVICE_PATH, h_baro.getError()); + ret = PX4_ERROR; } else { int baroret = h_baro.ioctl(BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100)); if (baroret) { - warnx("qnh could not be set"); - return ERROR; + PX4_ERR("qnh for baro could not be set"); + ret = PX4_ERROR; } } #endif - return OK; + param_get(_parameter_handles.vibe_thresh, &_parameters.vibration_warning_threshold); + + return ret; } @@ -982,8 +1064,8 @@ Sensors::adc_init() DevMgr::getHandle(ADC0_DEVICE_PATH, _h_adc); if (!_h_adc.isValid()) { - warnx("FATAL: no ADC found: %s (%d)", ADC0_DEVICE_PATH, _h_adc.getError()); - return ERROR; + PX4_ERR("no ADC found: %s (%d)", ADC0_DEVICE_PATH, _h_adc.getError()); + return PX4_ERROR; } return OK; @@ -992,38 +1074,66 @@ Sensors::adc_init() void Sensors::accel_poll(struct sensor_combined_s &raw) { - for (unsigned i = 0; i < _accel_count; i++) { + bool got_update = false; + + for (unsigned i = 0; i < _accel.subscription_count; i++) { bool accel_updated; - orb_check(_accel_sub[i], &accel_updated); + orb_check(_accel.subscription[i], &accel_updated); if (accel_updated) { - struct accel_report accel_report; + struct accel_report accel_report; - orb_copy(ORB_ID(sensor_accel), _accel_sub[i], &accel_report); + orb_copy(ORB_ID(sensor_accel), _accel.subscription[i], &accel_report); - math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); - vect = _board_rotation * vect; + if (accel_report.timestamp == 0) { + continue; //ignore invalid data + } - raw.accelerometer_m_s2[i * 3 + 0] = vect(0); - raw.accelerometer_m_s2[i * 3 + 1] = vect(1); - raw.accelerometer_m_s2[i * 3 + 2] = vect(2); + got_update = true; - math::Vector<3> vect_int(accel_report.x_integral, accel_report.y_integral, accel_report.z_integral); - vect_int = _board_rotation * vect_int; + if (accel_report.integral_dt != 0) { + math::Vector<3> vect_int(accel_report.x_integral, accel_report.y_integral, accel_report.z_integral); + vect_int = _board_rotation * vect_int; - raw.accelerometer_integral_m_s[i * 3 + 0] = vect_int(0); - raw.accelerometer_integral_m_s[i * 3 + 1] = vect_int(1); - raw.accelerometer_integral_m_s[i * 3 + 2] = vect_int(2); + float dt = accel_report.integral_dt / 1.e6f; + _last_sensor_data[i].accelerometer_integral_dt = dt; - raw.accelerometer_integral_dt[i] = accel_report.integral_dt; + _last_sensor_data[i].accelerometer_m_s2[0] = vect_int(0) / dt; + _last_sensor_data[i].accelerometer_m_s2[1] = vect_int(1) / dt; + _last_sensor_data[i].accelerometer_m_s2[2] = vect_int(2) / dt; - raw.accelerometer_raw[i * 3 + 0] = accel_report.x_raw; - raw.accelerometer_raw[i * 3 + 1] = accel_report.y_raw; - raw.accelerometer_raw[i * 3 + 2] = accel_report.z_raw; + } else { + //using the value instead of the integral (the integral is the prefered choice) + math::Vector<3> vect_val(accel_report.x, accel_report.y, accel_report.z); + vect_val = _board_rotation * vect_val; - raw.accelerometer_timestamp[i] = accel_report.timestamp; - raw.accelerometer_errcount[i] = accel_report.error_count; - raw.accelerometer_temp[i] = accel_report.temperature; + if (_last_accel_timestamp[i] == 0) { + _last_accel_timestamp[i] = accel_report.timestamp - 1000; + } + + _last_sensor_data[i].accelerometer_integral_dt = + (accel_report.timestamp - _last_accel_timestamp[i]) / 1.e6f; + _last_sensor_data[i].accelerometer_m_s2[0] = vect_val(0); + _last_sensor_data[i].accelerometer_m_s2[1] = vect_val(1); + _last_sensor_data[i].accelerometer_m_s2[2] = vect_val(2); + } + + _last_accel_timestamp[i] = accel_report.timestamp; + _accel.voter.put(i, accel_report.timestamp, _last_sensor_data[i].accelerometer_m_s2, + accel_report.error_count, _accel.priority[i]); + } + } + + if (got_update) { + int best_index; + _accel.voter.get_best(hrt_absolute_time(), &best_index); + + if (best_index >= 0) { + raw.accelerometer_m_s2[0] = _last_sensor_data[best_index].accelerometer_m_s2[0]; + raw.accelerometer_m_s2[1] = _last_sensor_data[best_index].accelerometer_m_s2[1]; + raw.accelerometer_m_s2[2] = _last_sensor_data[best_index].accelerometer_m_s2[2]; + raw.accelerometer_integral_dt = _last_sensor_data[best_index].accelerometer_integral_dt; + _accel.last_best_vote = (uint8_t)best_index; } } } @@ -1031,43 +1141,67 @@ Sensors::accel_poll(struct sensor_combined_s &raw) void Sensors::gyro_poll(struct sensor_combined_s &raw) { - for (unsigned i = 0; i < _gyro_count; i++) { + bool got_update = false; + + for (unsigned i = 0; i < _gyro.subscription_count; i++) { bool gyro_updated; - orb_check(_gyro_sub[i], &gyro_updated); + orb_check(_gyro.subscription[i], &gyro_updated); if (gyro_updated) { - struct gyro_report gyro_report; + struct gyro_report gyro_report; - orb_copy(ORB_ID(sensor_gyro), _gyro_sub[i], &gyro_report); + orb_copy(ORB_ID(sensor_gyro), _gyro.subscription[i], &gyro_report); - math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); - vect = _board_rotation * vect; - - raw.gyro_rad_s[i * 3 + 0] = vect(0); - raw.gyro_rad_s[i * 3 + 1] = vect(1); - raw.gyro_rad_s[i * 3 + 2] = vect(2); - - math::Vector<3> vect_int(gyro_report.x_integral, gyro_report.y_integral, gyro_report.z_integral); - vect_int = _board_rotation * vect_int; - - raw.gyro_integral_rad[i * 3 + 0] = vect_int(0); - raw.gyro_integral_rad[i * 3 + 1] = vect_int(1); - raw.gyro_integral_rad[i * 3 + 2] = vect_int(2); - - raw.gyro_integral_dt[i] = gyro_report.integral_dt; - - raw.gyro_raw[i * 3 + 0] = gyro_report.x_raw; - raw.gyro_raw[i * 3 + 1] = gyro_report.y_raw; - raw.gyro_raw[i * 3 + 2] = gyro_report.z_raw; - - raw.gyro_timestamp[i] = gyro_report.timestamp; - - if (i == 0) { - raw.timestamp = gyro_report.timestamp; + if (gyro_report.timestamp == 0) { + continue; //ignore invalid data } - raw.gyro_errcount[i] = gyro_report.error_count; - raw.gyro_temp[i] = gyro_report.temperature; + got_update = true; + + if (gyro_report.integral_dt != 0) { + math::Vector<3> vect_int(gyro_report.x_integral, gyro_report.y_integral, gyro_report.z_integral); + vect_int = _board_rotation * vect_int; + + float dt = gyro_report.integral_dt / 1.e6f; + _last_sensor_data[i].gyro_integral_dt = dt; + + _last_sensor_data[i].gyro_rad[0] = vect_int(0) / dt; + _last_sensor_data[i].gyro_rad[1] = vect_int(1) / dt; + _last_sensor_data[i].gyro_rad[2] = vect_int(2) / dt; + + } else { + //using the value instead of the integral (the integral is the prefered choice) + math::Vector<3> vect_val(gyro_report.x, gyro_report.y, gyro_report.z); + vect_val = _board_rotation * vect_val; + + if (_last_sensor_data[i].timestamp == 0) { + _last_sensor_data[i].timestamp = gyro_report.timestamp - 1000; + } + + _last_sensor_data[i].gyro_integral_dt = + (gyro_report.timestamp - _last_sensor_data[i].timestamp) / 1.e6f; + _last_sensor_data[i].gyro_rad[0] = vect_val(0); + _last_sensor_data[i].gyro_rad[1] = vect_val(1); + _last_sensor_data[i].gyro_rad[2] = vect_val(2); + } + + _last_sensor_data[i].timestamp = gyro_report.timestamp; + _gyro.voter.put(i, gyro_report.timestamp, _last_sensor_data[i].gyro_rad, + gyro_report.error_count, _gyro.priority[i]); + } + } + + if (got_update) { + int best_index; + _gyro.voter.get_best(hrt_absolute_time(), &best_index); + + if (best_index >= 0) { + raw.gyro_rad[0] = _last_sensor_data[best_index].gyro_rad[0]; + raw.gyro_rad[1] = _last_sensor_data[best_index].gyro_rad[1]; + raw.gyro_rad[2] = _last_sensor_data[best_index].gyro_rad[2]; + raw.gyro_integral_dt = _last_sensor_data[best_index].gyro_integral_dt; + raw.timestamp = _last_sensor_data[best_index].timestamp; + _gyro.last_best_vote = (uint8_t)best_index; } } } @@ -1075,30 +1209,44 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) void Sensors::mag_poll(struct sensor_combined_s &raw) { - for (unsigned i = 0; i < _mag_count; i++) { + bool got_update = false; + + for (unsigned i = 0; i < _mag.subscription_count; i++) { bool mag_updated; - orb_check(_mag_sub[i], &mag_updated); + orb_check(_mag.subscription[i], &mag_updated); if (mag_updated) { - struct mag_report mag_report; + struct mag_report mag_report; - orb_copy(ORB_ID(sensor_mag), _mag_sub[i], &mag_report); + orb_copy(ORB_ID(sensor_mag), _mag.subscription[i], &mag_report); + if (mag_report.timestamp == 0) { + continue; //ignore invalid data + } + + got_update = true; math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); - vect = _mag_rotation[i] * vect; - raw.magnetometer_ga[i * 3 + 0] = vect(0); - raw.magnetometer_ga[i * 3 + 1] = vect(1); - raw.magnetometer_ga[i * 3 + 2] = vect(2); + _last_sensor_data[i].magnetometer_ga[0] = vect(0); + _last_sensor_data[i].magnetometer_ga[1] = vect(1); + _last_sensor_data[i].magnetometer_ga[2] = vect(2); - raw.magnetometer_raw[i * 3 + 0] = mag_report.x_raw; - raw.magnetometer_raw[i * 3 + 1] = mag_report.y_raw; - raw.magnetometer_raw[i * 3 + 2] = mag_report.z_raw; + _last_mag_timestamp[i] = mag_report.timestamp; + _mag.voter.put(i, mag_report.timestamp, vect.data, + mag_report.error_count, _mag.priority[i]); + } + } - raw.magnetometer_timestamp[i] = mag_report.timestamp; - raw.magnetometer_errcount[i] = mag_report.error_count; - raw.magnetometer_temp[i] = mag_report.temperature; + if (got_update) { + int best_index; + _mag.voter.get_best(hrt_absolute_time(), &best_index); + + if (best_index >= 0) { + raw.magnetometer_ga[0] = _last_sensor_data[best_index].magnetometer_ga[0]; + raw.magnetometer_ga[1] = _last_sensor_data[best_index].magnetometer_ga[1]; + raw.magnetometer_ga[2] = _last_sensor_data[best_index].magnetometer_ga[2]; + _mag.last_best_vote = (uint8_t)best_index; } } } @@ -1106,19 +1254,43 @@ Sensors::mag_poll(struct sensor_combined_s &raw) void Sensors::baro_poll(struct sensor_combined_s &raw) { - for (unsigned i = 0; i < _baro_count; i++) { + bool got_update = false; + + for (unsigned i = 0; i < _baro.subscription_count; i++) { bool baro_updated; - orb_check(_baro_sub[i], &baro_updated); + orb_check(_baro.subscription[i], &baro_updated); if (baro_updated) { + struct baro_report baro_report; - orb_copy(ORB_ID(sensor_baro), _baro_sub[i], &_barometer); + orb_copy(ORB_ID(sensor_baro), _baro.subscription[i], &baro_report); - raw.baro_pres_mbar[i] = _barometer.pressure; // Pressure in mbar - raw.baro_alt_meter[i] = _barometer.altitude; // Altitude in meters - raw.baro_temp_celcius[i] = _barometer.temperature; // Temperature in degrees celcius + if (baro_report.timestamp == 0) { + continue; //ignore invalid data + } - raw.baro_timestamp[i] = _barometer.timestamp; + got_update = true; + math::Vector<3> vect(baro_report.altitude, 0.f, 0.f); + + _last_sensor_data[i].baro_alt_meter = baro_report.altitude; + _last_sensor_data[i].baro_temp_celcius = baro_report.temperature; + _last_baro_pressure[i] = baro_report.pressure; + + _last_baro_timestamp[i] = baro_report.timestamp; + _baro.voter.put(i, baro_report.timestamp, vect.data, + baro_report.error_count, _baro.priority[i]); + } + } + + if (got_update) { + int best_index; + _baro.voter.get_best(hrt_absolute_time(), &best_index); + + if (best_index >= 0) { + raw.baro_alt_meter = _last_sensor_data[best_index].baro_alt_meter; + raw.baro_temp_celcius = _last_sensor_data[best_index].baro_temp_celcius; + _last_best_baro_pressure = _last_baro_pressure[best_index]; + _baro.last_best_vote = (uint8_t)best_index; } } } @@ -1132,12 +1304,8 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) if (updated) { orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres); - raw.differential_pressure_pa[0] = _diff_pres.differential_pressure_raw_pa; - raw.differential_pressure_timestamp[0] = _diff_pres.timestamp; - raw.differential_pressure_filtered_pa[0] = _diff_pres.differential_pressure_filtered_pa; - float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : - (raw.baro_temp_celcius[0] - PCB_TEMP_ESTIMATE_DEG); + (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); _airspeed.timestamp = _diff_pres.timestamp; @@ -1155,11 +1323,11 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa)); _airspeed.true_airspeed_m_s = math::max(0.0f, - calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar[0] * 1e2f, - raw.baro_pres_mbar[0] * 1e2f, air_temperature_celsius)); + calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + _last_best_baro_pressure * 1e2f, + _last_best_baro_pressure * 1e2f, air_temperature_celsius)); _airspeed.true_airspeed_unfiltered_m_s = math::max(0.0f, - calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + raw.baro_pres_mbar[0] * 1e2f, - raw.baro_pres_mbar[0] * 1e2f, air_temperature_celsius)); + calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + _last_best_baro_pressure * 1e2f, + _last_best_baro_pressure * 1e2f, air_temperature_celsius)); _airspeed.air_temperature_celsius = air_temperature_celsius; @@ -1273,14 +1441,14 @@ Sensors::parameter_update_poll(bool forced) failed = failed || (OK != param_get(param_find(str), &gscale.z_scale)); if (failed) { - warnx(CAL_ERROR_APPLY_CAL_MSG, "gyro", i); + PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "gyro", i); } else { /* apply new scaling and offsets */ config_ok = apply_gyro_calibration(h, &gscale, device_id); if (!config_ok) { - warnx(CAL_ERROR_APPLY_CAL_MSG, "gyro ", i); + PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "gyro ", i); } } @@ -1341,14 +1509,14 @@ Sensors::parameter_update_poll(bool forced) failed = failed || (OK != param_get(param_find(str), &ascale.z_scale)); if (failed) { - warnx(CAL_ERROR_APPLY_CAL_MSG, "accel", i); + PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "accel", i); } else { /* apply new scaling and offsets */ config_ok = apply_accel_calibration(h, &ascale, device_id); if (!config_ok) { - warnx(CAL_ERROR_APPLY_CAL_MSG, "accel ", i); + PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "accel ", i); } } @@ -1474,7 +1642,7 @@ Sensors::parameter_update_poll(bool forced) } if (failed) { - warnx(CAL_ERROR_APPLY_CAL_MSG, "mag", i); + PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "mag", i); } else { @@ -1482,7 +1650,7 @@ Sensors::parameter_update_poll(bool forced) config_ok = apply_mag_calibration(h, &mscale, device_id); if (!config_ok) { - warnx(CAL_ERROR_APPLY_CAL_MSG, "mag ", i); + PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "mag ", i); } } @@ -1512,8 +1680,6 @@ Sensors::parameter_update_poll(bool forced) px4_close(fd); } - /* do not output this for now, as its covered in preflight checks */ - // warnx("valid configs: %u gyros, %u mags, %u accels", gyro_count, mag_count, accel_count); _battery.updateParams(); } } @@ -1521,17 +1687,10 @@ Sensors::parameter_update_poll(bool forced) bool Sensors::apply_gyro_calibration(DevHandle &h, const struct gyro_calibration_s *gcal, const int device_id) { -#if !defined(__PX4_QURT) && !defined(__RPI2) +#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP) /* On most systems, we can just use the IOCTL call to set the calibration params. */ - const int res = h.ioctl(GYROIOCSSCALE, (long unsigned int)gcal); - - if (res) { - return false; - - } else { - return true; - } + return !h.ioctl(GYROIOCSSCALE, (long unsigned int)gcal); #else /* On QURT, the params are read directly in the respective wrappers. */ @@ -1542,17 +1701,10 @@ Sensors::apply_gyro_calibration(DevHandle &h, const struct gyro_calibration_s *g bool Sensors::apply_accel_calibration(DevHandle &h, const struct accel_calibration_s *acal, const int device_id) { -#if !defined(__PX4_QURT) && !defined(__RPI2) +#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP) /* On most systems, we can just use the IOCTL call to set the calibration params. */ - const int res = h.ioctl(ACCELIOCSSCALE, (long unsigned int)acal); - - if (res) { - return false; - - } else { - return true; - } + return !h.ioctl(ACCELIOCSSCALE, (long unsigned int)acal); #else /* On QURT, the params are read directly in the respective wrappers. */ @@ -1563,17 +1715,10 @@ Sensors::apply_accel_calibration(DevHandle &h, const struct accel_calibration_s bool Sensors::apply_mag_calibration(DevHandle &h, const struct mag_calibration_s *mcal, const int device_id) { -#if !defined(__PX4_QURT) && !defined(__RPI2) +#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP) /* On most systems, we can just use the IOCTL call to set the calibration params. */ - const int res = h.ioctl(MAGIOCSSCALE, (long unsigned int)mcal); - - if (res) { - return false; - - } else { - return true; - } + return !h.ioctl(MAGIOCSSCALE, (long unsigned int)mcal); #else /* On QURT, the params are read directly in the respective wrappers. */ @@ -1609,17 +1754,17 @@ Sensors::rc_parameter_map_poll(bool forced) } - warnx("rc to parameter map updated"); + PX4_DEBUG("rc to parameter map updated"); for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { - warnx("\ti %d param_id %s scale %.3f value0 %.3f, min %.3f, max %.3f", - i, - &_rc_parameter_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)], - (double)_rc_parameter_map.scale[i], - (double)_rc_parameter_map.value0[i], - (double)_rc_parameter_map.value_min[i], - (double)_rc_parameter_map.value_max[i] - ); + PX4_DEBUG("\ti %d param_id %s scale %.3f value0 %.3f, min %.3f, max %.3f", + i, + &_rc_parameter_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)], + (double)_rc_parameter_map.scale[i], + (double)_rc_parameter_map.value0[i], + (double)_rc_parameter_map.value_min[i], + (double)_rc_parameter_map.value_max[i] + ); } } } @@ -1649,23 +1794,19 @@ Sensors::adc_poll(struct sensor_combined_s &raw) /* Read add channels we got */ for (unsigned i = 0; i < ret / sizeof(buf_adc[0]); i++) { - /* Save raw voltage values */ - if (i < (sizeof(raw.adc_voltage_v) / sizeof(raw.adc_voltage_v[0]))) { - raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f); - raw.adc_mapping[i] = buf_adc[i].am_channel; - } /* look for specific channels and process the raw voltage to measurement data */ if (ADC_BATTERY_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { /* Voltage in volts */ - bat_voltage_v = (buf_adc[i].am_data * _parameters.battery_voltage_scaling); + bat_voltage_v = (buf_adc[i].am_data * _parameters.battery_voltage_scaling) * _parameters.battery_v_div; if (bat_voltage_v > 0.5f) { updated_battery = true; } } else if (ADC_BATTERY_CURRENT_CHANNEL == buf_adc[i].am_channel) { - bat_current_a = (buf_adc[i].am_data * _parameters.battery_current_scaling); + bat_current_a = ((buf_adc[i].am_data * _parameters.battery_current_scaling) + - _parameters.battery_current_offset) * _parameters.battery_a_per_v; #ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL @@ -1702,7 +1843,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } } - if (updated_battery) { + if (_parameters.battery_source == 0 && updated_battery) { actuator_controls_s ctrl; orb_copy(ORB_ID(actuator_controls_0), _actuator_ctrl_0_sub, &ctrl); _battery.updateBatteryStatus(t, bat_voltage_v, bat_current_a, ctrl.control[actuator_controls_s::INDEX_THROTTLE], @@ -1945,7 +2086,7 @@ Sensors::rc_poll() if (_parameters.rc_map_flightmode > 0) { /* the number of valid slots equals the index of the max marker minus one */ - const unsigned num_slots = manual_control_setpoint_s::MODE_SLOT_MAX; + const int num_slots = manual_control_setpoint_s::MODE_SLOT_MAX; /* the half width of the range of a slot is the total range * divided by the number of slots, again divided by two @@ -1987,6 +2128,8 @@ Sensors::rc_poll() _parameters.rc_offboard_th, _parameters.rc_offboard_inv); manual.kill_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_KILLSWITCH, _parameters.rc_killswitch_th, _parameters.rc_killswitch_inv); + manual.transition_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_TRANSITION, + _parameters.rc_trans_th, _parameters.rc_trans_inv); /* publish manual_control_setpoint topic */ if (_manual_control_pub != nullptr) { @@ -2029,15 +2172,72 @@ Sensors::rc_poll() } } +bool +Sensors::check_failover(SensorData &sensor, const char *sensor_name) +{ + if (sensor.last_failover_count != sensor.voter.failover_count()) { + + uint32_t flags = sensor.voter.failover_state(); + + if (flags == DataValidator::ERROR_FLAG_NO_ERROR) { + //we switched due to a non-critical reason. No need to panic. + PX4_INFO("%s sensor switch from #%i", sensor_name, sensor.voter.failover_index()); + + } else { + mavlink_and_console_log_emergency(&_mavlink_log_pub, "%s #%i failover :%s%s%s%s%s!", + sensor_name, + sensor.voter.failover_index(), + ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""), + ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""), + ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : "")); + } + + sensor.last_failover_count = sensor.voter.failover_count(); + return true; + } + + return false; +} + +bool +Sensors::check_vibration() +{ + bool ret = false; + hrt_abstime cur_time = hrt_absolute_time(); + + if (!_vibration_warning && (_gyro.voter.get_vibration_factor(cur_time) > _parameters.vibration_warning_threshold || + _accel.voter.get_vibration_factor(cur_time) > _parameters.vibration_warning_threshold || + _mag.voter.get_vibration_factor(cur_time) > _parameters.vibration_warning_threshold)) { + + if (_vibration_warning_timestamp == 0) { + _vibration_warning_timestamp = cur_time; + + } else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000 * 1000) { + _vibration_warning = true; + mavlink_and_console_log_critical(&_mavlink_log_pub, "HIGH VIBRATION! g: %d a: %d m: %d", + (int)(100 * _gyro.voter.get_vibration_factor(cur_time)), + (int)(100 * _accel.voter.get_vibration_factor(cur_time)), + (int)(100 * _mag.voter.get_vibration_factor(cur_time))); + ret = true; + } + + } else { + _vibration_warning_timestamp = 0; + } + + return ret; +} + void Sensors::task_main_trampoline(int argc, char *argv[]) { sensors::g_sensors->task_main(); } -int -Sensors::init_sensor_class(const struct orb_metadata *meta, int *subs, - uint32_t *priorities, uint32_t *errcount) +void +Sensors::init_sensor_class(const struct orb_metadata *meta, SensorData &sensor_data) { unsigned group_count = orb_group_count(meta); @@ -2046,13 +2246,16 @@ Sensors::init_sensor_class(const struct orb_metadata *meta, int *subs, } for (unsigned i = 0; i < group_count; i++) { - if (subs[i] < 0) { - subs[i] = orb_subscribe_multi(meta, i); - orb_priority(subs[i], (int32_t *)&priorities[i]); + if (sensor_data.subscription[i] < 0) { + sensor_data.subscription[i] = orb_subscribe_multi(meta, i); } + + int32_t priority; + orb_priority(sensor_data.subscription[i], &priority); + sensor_data.priority[i] = (uint8_t)priority; } - return group_count; + sensor_data.subscription_count = group_count; } void @@ -2065,108 +2268,90 @@ Sensors::task_main() /* This calls a sensors_init which can have different implementations on NuttX, POSIX, QURT. */ ret = sensors_init(); -#if !defined(__PX4_QURT) && !defined(__RPI2) +#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP) // TODO: move adc_init into the sensors_init call. ret = ret || adc_init(); #endif if (ret) { - warnx("sensor initialization failed"); - _sensors_task = -1; - - DevMgr::releaseHandle(_h_adc); - - return; + PX4_ERR("sensor initialization failed"); } struct sensor_combined_s raw = {}; - /* ensure no overflows can occur */ - static_assert((sizeof(raw.gyro_timestamp) / sizeof(raw.gyro_timestamp[0])) >= SENSOR_COUNT_MAX, - "SENSOR_COUNT_MAX larger than sensor_combined datastructure fields. Overflow would occur"); + raw.accelerometer_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID; + + raw.magnetometer_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID; + + raw.baro_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID; /* * do subscriptions */ + init_sensor_class(ORB_ID(sensor_gyro), _gyro); - unsigned gcount_prev = _gyro_count; + init_sensor_class(ORB_ID(sensor_mag), _mag); - unsigned mcount_prev = _mag_count; + init_sensor_class(ORB_ID(sensor_accel), _accel); - unsigned acount_prev = _accel_count; + init_sensor_class(ORB_ID(sensor_baro), _baro); - unsigned bcount_prev = _baro_count; - - _gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0], - &raw.gyro_priority[0], &raw.gyro_errcount[0]); - - _mag_count = init_sensor_class(ORB_ID(sensor_mag), &_mag_sub[0], - &raw.magnetometer_priority[0], &raw.magnetometer_errcount[0]); - - _accel_count = init_sensor_class(ORB_ID(sensor_accel), &_accel_sub[0], - &raw.accelerometer_priority[0], &raw.accelerometer_errcount[0]); - - _baro_count = init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0], - &raw.baro_priority[0], &raw.baro_errcount[0]); - - if (gcount_prev != _gyro_count || - mcount_prev != _mag_count || - acount_prev != _accel_count || - bcount_prev != _baro_count) { - - /* reload calibration params */ - parameter_update_poll(true); - } + /* reload calibration params */ + parameter_update_poll(true); _rc_sub = orb_subscribe(ORB_ID(input_rc)); + _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); + _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _rc_parameter_map_sub = orb_subscribe(ORB_ID(rc_parameter_map)); + _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + _actuator_ctrl_0_sub = orb_subscribe(ORB_ID(actuator_controls_0)); - /* - * do advertisements - */ - raw.timestamp = hrt_absolute_time(); - raw.adc_voltage_v[0] = 0.0f; - raw.adc_voltage_v[1] = 0.0f; - raw.adc_voltage_v[2] = 0.0f; - raw.adc_voltage_v[3] = 0.0f; + raw.timestamp = 0; _battery.reset(&_battery_status); /* get a set of initial values */ accel_poll(raw); + gyro_poll(raw); + mag_poll(raw); + baro_poll(raw); + diff_pres_poll(raw); parameter_update_poll(true /* forced */); + rc_parameter_map_poll(true /* forced */); /* advertise the sensor_combined topic and make the initial publication */ _sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw); - /* wakeup source(s) */ - px4_pollfd_struct_t fds[1] = {}; + /* wakeup source */ + px4_pollfd_struct_t poll_fds = {}; - /* use the gyro to pace output */ - fds[0].fd = _gyro_sub[0]; - fds[0].events = POLLIN; + poll_fds.events = POLLIN; _task_should_exit = false; - raw.timestamp = 0; - uint64_t _last_config_update = hrt_absolute_time(); while (!_task_should_exit) { - /* wait for up to 50ms for data */ - int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50); + /* use the best-voted gyro to pace output */ + poll_fds.fd = _gyro.subscription[_gyro.last_best_vote]; + + /* wait for up to 50ms for data (Note that this implies, we can have a fail-over time of 50ms, + * if a gyro fails) */ + int pret = px4_poll(&poll_fds, 1, 50); /* if pret == 0 it timed out - periodic check for _task_should_exit, etc. */ @@ -2175,12 +2360,12 @@ Sensors::task_main() /* if the polling operation failed because no gyro sensor is available yet, * then attempt to subscribe once again */ - if (_gyro_count == 0) { - _gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0], - &raw.gyro_priority[0], &raw.gyro_errcount[0]); - fds[0].fd = _gyro_sub[0]; + if (_gyro.subscription_count == 0) { + init_sensor_class(ORB_ID(sensor_gyro), _gyro); } + usleep(1000); + continue; } @@ -2189,62 +2374,52 @@ Sensors::task_main() /* check vehicle status for changes to publication state */ vehicle_control_mode_poll(); - /* the timestamp of the raw struct is updated by the gyro_poll() method */ - /* copy most recent sensor data */ + /* the timestamp of the raw struct is updated by the gyro_poll() method (this makes the gyro + * a mandatory sensor) */ gyro_poll(raw); accel_poll(raw); mag_poll(raw); baro_poll(raw); - // FIXME TODO: this needs more thinking, otherwise we spam the console and keep switching. - /* Work out if main gyro timed out and fail over to alternate gyro. - * However, don't do this if the secondary is not available. */ - if (hrt_elapsed_time(&raw.gyro_timestamp[0]) > 20 * 1000 && _gyro_sub[1] >= 0) { - warnx("gyro has timed out"); - - /* If the secondary failed as well, go to the tertiary, also only if available. */ - if (hrt_elapsed_time(&raw.gyro_timestamp[1]) > 20 * 1000 && _gyro_sub[2] >= 0) { - fds[0].fd = _gyro_sub[2]; - - if (!_hil_enabled) { - warnx("failing over to third gyro"); - } - - } else if (_gyro_sub[1] >= 0) { - fds[0].fd = _gyro_sub[1]; - - if (!_hil_enabled) { - warnx("failing over to second gyro"); - } - } - } /* check battery voltage */ adc_poll(raw); diff_pres_poll(raw); - /* Inform other processes that new data is available to copy */ if (_publishing && raw.timestamp > 0) { + + /* construct relative timestamps */ + if (_last_accel_timestamp[_accel.last_best_vote]) { + raw.accelerometer_timestamp_relative = (int32_t)(_last_accel_timestamp[_accel.last_best_vote] - raw.timestamp); + } + + if (_last_mag_timestamp[_mag.last_best_vote]) { + raw.magnetometer_timestamp_relative = (int32_t)(_last_mag_timestamp[_mag.last_best_vote] - raw.timestamp); + } + + if (_last_baro_timestamp[_baro.last_best_vote]) { + raw.baro_timestamp_relative = (int32_t)(_last_baro_timestamp[_baro.last_best_vote] - raw.timestamp); + } + orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw); + + check_failover(_accel, "Accel"); + check_failover(_gyro, "Gyro"); + check_failover(_mag, "Mag"); + check_failover(_baro, "Baro"); + + //check_vibration(); //disabled for now, as it does not seem to be reliable } /* keep adding sensors as long as we are not armed, * when not adding sensors poll for param updates */ if (!_armed && hrt_elapsed_time(&_last_config_update) > 500 * 1000) { - _gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0], - &raw.gyro_priority[0], &raw.gyro_errcount[0]); - - _mag_count = init_sensor_class(ORB_ID(sensor_mag), &_mag_sub[0], - &raw.magnetometer_priority[0], &raw.magnetometer_errcount[0]); - - _accel_count = init_sensor_class(ORB_ID(sensor_accel), &_accel_sub[0], - &raw.accelerometer_priority[0], &raw.accelerometer_errcount[0]); - - _baro_count = init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0], - &raw.baro_priority[0], &raw.baro_errcount[0]); - + init_sensor_class(ORB_ID(sensor_gyro), _gyro); + init_sensor_class(ORB_ID(sensor_mag), _mag); + init_sensor_class(ORB_ID(sensor_accel), _accel); + init_sensor_class(ORB_ID(sensor_baro), _baro); _last_config_update = hrt_absolute_time(); } else { @@ -2262,7 +2437,31 @@ Sensors::task_main() perf_end(_loop_perf); } - warnx("exiting."); + for (unsigned i = 0; i < _gyro.subscription_count; i++) { + orb_unsubscribe(_gyro.subscription[i]); + } + + for (unsigned i = 0; i < _accel.subscription_count; i++) { + orb_unsubscribe(_accel.subscription[i]); + } + + for (unsigned i = 0; i < _mag.subscription_count; i++) { + orb_unsubscribe(_mag.subscription[i]); + } + + for (unsigned i = 0; i < _baro.subscription_count; i++) { + orb_unsubscribe(_baro.subscription[i]); + } + + orb_unsubscribe(_rc_sub); + orb_unsubscribe(_diff_pres_sub); + orb_unsubscribe(_vcontrol_mode_sub); + orb_unsubscribe(_params_sub); + orb_unsubscribe(_rc_parameter_map_sub); + orb_unsubscribe(_manual_control_sub); + orb_unsubscribe(_actuator_ctrl_0_sub); + orb_unadvertise(_sensor_pub); + _sensors_task = -1; px4_task_exit(ret); } @@ -2276,7 +2475,7 @@ Sensors::start() _sensors_task = px4_task_spawn_cmd("sensors", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 2000, + 1500, (px4_main_t)&Sensors::task_main_trampoline, nullptr); @@ -2286,37 +2485,50 @@ Sensors::start() } if (_sensors_task < 0) { - return -ERROR; + return -PX4_ERROR; } return OK; } +void Sensors::print_status() +{ + PX4_INFO("gyro status:"); + _gyro.voter.print(); + PX4_INFO("accel status:"); + _accel.voter.print(); + PX4_INFO("mag status:"); + _mag.voter.print(); + PX4_INFO("baro status:"); + _baro.voter.print(); +} + + int sensors_main(int argc, char *argv[]) { if (argc < 2) { - warnx("usage: sensors {start|stop|status}"); + PX4_INFO("usage: sensors {start|stop|status}"); return 0; } if (!strcmp(argv[1], "start")) { if (sensors::g_sensors != nullptr) { - warnx("already running"); + PX4_INFO("already running"); return 0; } sensors::g_sensors = new Sensors; if (sensors::g_sensors == nullptr) { - warnx("alloc failed"); + PX4_ERR("alloc failed"); return 1; } if (OK != sensors::g_sensors->start()) { delete sensors::g_sensors; sensors::g_sensors = nullptr; - warnx("start failed"); + PX4_ERR("start failed"); return 1; } @@ -2325,7 +2537,7 @@ int sensors_main(int argc, char *argv[]) if (!strcmp(argv[1], "stop")) { if (sensors::g_sensors == nullptr) { - warnx("not running"); + PX4_INFO("not running"); return 1; } @@ -2336,15 +2548,15 @@ int sensors_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (sensors::g_sensors) { - warnx("is running"); + sensors::g_sensors->print_status(); return 0; } else { - warnx("not running"); + PX4_INFO("not running"); return 1; } } - warnx("unrecognized command"); + PX4_ERR("unrecognized command"); return 1; } diff --git a/src/modules/sensors/sensors_init.cpp b/src/modules/sensors/sensors_init.cpp index e13142c754..ae9cf4db86 100644 --- a/src/modules/sensors/sensors_init.cpp +++ b/src/modules/sensors/sensors_init.cpp @@ -52,7 +52,7 @@ using namespace DriverFramework; -#if defined(__PX4_QURT) || defined(__RPI2) +#if defined(__PX4_QURT) || defined(__PX4_POSIX_RPI) || defined(__PX4_POSIX_BEBOP) // Sensor initialization is performed automatically when the QuRT sensor drivers // are loaded. @@ -66,12 +66,6 @@ sensors_init(void) #else -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - /** * Do accel-related initialisation. */ @@ -126,7 +120,7 @@ accel_init() if (!h_accel.isValid()) { warnx("FATAL: no accelerometer found: %s (%d)", ACCEL0_DEVICE_PATH, h_accel.getError()); - return ERROR; + return PX4_ERROR; } else { @@ -148,7 +142,7 @@ gyro_init() if (!h_gyro.isValid()) { warnx("FATAL: no gyro found: %s (%d)", GYRO0_DEVICE_PATH, h_gyro.getError()); - return ERROR; + return PX4_ERROR; } @@ -171,7 +165,7 @@ mag_init() if (!h_mag.isValid()) { warnx("FATAL: no magnetometer found: %s (%d)", MAG0_DEVICE_PATH, h_mag.getError()); - return ERROR; + return PX4_ERROR; } /* try different mag sampling rates */ @@ -193,7 +187,7 @@ mag_init() } else { warnx("FATAL: mag sampling rate could not be set"); - return ERROR; + return PX4_ERROR; } } @@ -208,7 +202,7 @@ baro_init() if (!h_baro.isValid()) { warnx("FATAL: No barometer found: %s (%d)", BARO0_DEVICE_PATH, h_baro.getError()); - return ERROR; + return PX4_ERROR; } /* set the driver to poll at 150Hz */ diff --git a/src/modules/simulator/CMakeLists.txt b/src/modules/simulator/CMakeLists.txt index 0a7e221086..98db9d8731 100644 --- a/src/modules/simulator/CMakeLists.txt +++ b/src/modules/simulator/CMakeLists.txt @@ -40,9 +40,6 @@ px4_add_module( MODULE modules__simulator MAIN simulator COMPILE_FLAGS - -Wno-attributes - -Wno-packed - -Wno-packed SRCS ${SIMULATOR_SRCS} DEPENDS diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index 9eaa725cdd..7cf3e199a1 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include #include @@ -124,21 +125,26 @@ void Simulator::write_airspeed_data(void *buf) int Simulator::start(int argc, char *argv[]) { int ret = 0; + int udp_port = 0; _instance = new Simulator(); if (_instance) { drv_led_start(); + if (argc == 5 && strcmp(argv[3], "-u") == 0) { + udp_port = atoi(argv[4]); + } + if (argv[2][1] == 's') { _instance->initializeSensorData(); #ifndef __PX4_QURT // Update sensor data - _instance->pollForMAVLinkMessages(false); + _instance->pollForMAVLinkMessages(false, udp_port); #endif } else if (argv[2][1] == 'p') { // Update sensor data - _instance->pollForMAVLinkMessages(true); + _instance->pollForMAVLinkMessages(true, udp_port); } else { _instance->initializeSensorData(); @@ -155,7 +161,7 @@ int Simulator::start(int argc, char *argv[]) static void usage() { - PX4_WARN("Usage: simulator {start -[spt] |stop}"); + PX4_WARN("Usage: simulator {start -[spt] [-u udp_port] |stop}"); PX4_WARN("Simulate raw sensors: simulator start -s"); PX4_WARN("Publish sensors combined: simulator start -p"); PX4_WARN("Dummy unit test data: simulator start -t"); @@ -171,7 +177,7 @@ extern "C" { { int ret = 0; - if (argc == 3 && strcmp(argv[1], "start") == 0) { + if (argc > 2 && strcmp(argv[1], "start") == 0) { if (strcmp(argv[2], "-s") == 0 || strcmp(argv[2], "-p") == 0 || strcmp(argv[2], "-t") == 0) { diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index a5afa72c7a..fd6fc267c6 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -55,6 +55,7 @@ #include #include #include +#include #include #include namespace simulator @@ -238,12 +239,13 @@ private: _gyro_pub(nullptr), _mag_pub(nullptr), _flow_pub(nullptr), + _dist_pub(nullptr), _battery_pub(nullptr), _initialized(false) #ifndef __PX4_QURT , _rc_channels_pub(nullptr), - _actuator_outputs_sub(-1), + _actuator_outputs_sub{}, _vehicle_attitude_sub(-1), _manual_sub(-1), _vehicle_status_sub(-1), @@ -253,7 +255,12 @@ private: _manual{}, _vehicle_status{} #endif - {} + { + for (unsigned i = 0; i < (sizeof(_actuator_outputs_sub) / sizeof(_actuator_outputs_sub[0])); i++) + { + _actuator_outputs_sub[i] = -1; + } + } ~Simulator() { _instance = NULL; } void initializeSensorData(); @@ -283,6 +290,7 @@ private: orb_advert_t _gyro_pub; orb_advert_t _mag_pub; orb_advert_t _flow_pub; + orb_advert_t _dist_pub; orb_advert_t _battery_pub; bool _initialized; @@ -293,20 +301,21 @@ private: // class methods int publish_sensor_topics(mavlink_hil_sensor_t *imu); int publish_flow_topic(mavlink_hil_optical_flow_t *flow); + int publish_distance_topic(mavlink_distance_sensor_t *dist); #ifndef __PX4_QURT // uORB publisher handlers orb_advert_t _rc_channels_pub; // uORB subscription handlers - int _actuator_outputs_sub; + int _actuator_outputs_sub[ORB_MULTI_MAX_INSTANCES]; int _vehicle_attitude_sub; int _manual_sub; int _vehicle_status_sub; // uORB data containers struct rc_input_values _rc_input; - struct actuator_outputs_s _actuators; + struct actuator_outputs_s _actuators[ORB_MULTI_MAX_INSTANCES]; struct vehicle_attitude_s _attitude; struct manual_control_setpoint_s _manual; struct vehicle_status_s _vehicle_status; @@ -314,9 +323,9 @@ private: void poll_topics(); void handle_message(mavlink_message_t *msg, bool publish); void send_controls(); - void pollForMAVLinkMessages(bool publish); + void pollForMAVLinkMessages(bool publish, int udp_port); - void pack_actuator_message(mavlink_hil_controls_t &actuator_msg); + void pack_actuator_message(mavlink_hil_controls_t &actuator_msg, unsigned index); void send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID); void update_sensors(mavlink_hil_sensor_t *imu); void update_gps(mavlink_hil_gps_t *gps_sim); diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 9be4da1329..feccfeca45 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -69,71 +69,58 @@ static int _fd; static unsigned char _buf[1024]; sockaddr_in _srcaddr; static socklen_t _addrlen = sizeof(_srcaddr); -static bool actuators_on = false; static hrt_abstime batt_sim_start = 0; +const unsigned mode_flag_armed = 128; // following MAVLink spec +const unsigned mode_flag_custom = 1; + using namespace simulator; -void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) +void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg, unsigned index) { + // reset state + memset(&actuator_msg, 0, sizeof(actuator_msg)); + actuator_msg.time_usec = hrt_absolute_time(); + float out[8] = {}; + bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); const float pwm_center = (PWM_DEFAULT_MAX + PWM_DEFAULT_MIN) / 2; - // for now we only support quadrotors - unsigned n = 4; + for (unsigned i = 0; i < (sizeof(out) / sizeof(out[0])); i++) { + // scale PWM out 900..2100 us to -1..1 */ + out[i] = (_actuators[index].output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2); - if (_vehicle_status.is_rotary_wing || _vehicle_status.is_vtol) { - for (unsigned i = 0; i < 8; i++) { - if (_actuators.output[i] > PWM_DEFAULT_MIN / 2) { - if (i < n) { - // scale PWM out 900..2100 us to 0..1 for rotors */ - out[i] = (_actuators.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN); - - } else { - // scale PWM out 900..2100 us to -1..1 for other channels */ - out[i] = (_actuators.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2); - } - - } else { - // send 0 when disarmed and for disabled channels */ - out[i] = 0.0f; - } - } - - } else { - // convert back to range [-1, 1] - for (unsigned i = 0; i < 8; i++) { - out[i] = (_actuators.output[i] - 1500) / 600.0f; + if (!PX4_ISFINITE(out[i])) { + out[i] = -1.0f; } } - // if vehicle status has not yet been updated, set actuator commands to zero - // this is to prevent the simulation getting into a bad state - if (_vehicle_status.timestamp == 0) { - memset(out, 0, sizeof(out)); - } - - actuators_on = (out[3] > 0.1f); - - actuator_msg.time_usec = hrt_absolute_time(); actuator_msg.roll_ailerons = out[0]; - actuator_msg.pitch_elevator = (_vehicle_status.is_rotary_wing || _vehicle_status.is_vtol) ? out[1] : -out[1]; + actuator_msg.pitch_elevator = out[1]; actuator_msg.yaw_rudder = out[2]; actuator_msg.throttle = out[3]; actuator_msg.aux1 = out[4]; actuator_msg.aux2 = out[5]; - actuator_msg.aux3 = _actuators.output[6] > PWM_DEFAULT_MIN / 2 ? out[6] : -1.0f;; + actuator_msg.aux3 = out[6]; actuator_msg.aux4 = out[7]; - actuator_msg.mode = 0; // need to put something here - actuator_msg.nav_mode = 0; + actuator_msg.mode = mode_flag_custom; + actuator_msg.mode |= (armed) ? mode_flag_armed : 0; + actuator_msg.nav_mode = index; // XXX this indicates the output group in our use of the message } void Simulator::send_controls() { - mavlink_hil_controls_t msg; - pack_actuator_message(msg); - send_mavlink_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg, 200); + for (unsigned i = 0; i < (sizeof(_actuator_outputs_sub) / sizeof(_actuator_outputs_sub[0])); i++) { + + if (_actuator_outputs_sub[i] < 0 || _actuators[i].timestamp == 0) { + continue; + } + + mavlink_hil_controls_t msg; + pack_actuator_message(msg, i); + send_mavlink_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg, 200); + } } static void fill_rc_input_msg(struct rc_input_values *rc, mavlink_rc_channels_t *rc_channels) @@ -143,17 +130,6 @@ static void fill_rc_input_msg(struct rc_input_values *rc, mavlink_rc_channels_t rc->channel_count = rc_channels->chancount; rc->rssi = rc_channels->rssi; - /* PX4_WARN("RC: %d, %d, %d, %d, %d, %d, %d, %d", - rc_channels->chan1_raw, - rc_channels->chan2_raw, - rc_channels->chan3_raw, - rc_channels->chan4_raw, - rc_channels->chan5_raw, - rc_channels->chan6_raw, - rc_channels->chan7_raw, - rc_channels->chan8_raw); - */ - rc->values[0] = rc_channels->chan1_raw; rc->values[1] = rc_channels->chan2_raw; rc->values[2] = rc_channels->chan3_raw; @@ -268,7 +244,9 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish) const float discharge_interval_us = 60 * 1000 * 1000; - if (!actuators_on || batt_sim_start == 0 || batt_sim_start > now) { + bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + + if (!armed || batt_sim_start == 0 || batt_sim_start > now) { batt_sim_start = now; } @@ -291,7 +269,7 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish) // TODO: don't hard-code throttle. const float throttle = 0.5f; - _battery.updateBatteryStatus(now, vbatt, ibatt, throttle, actuators_on, &battery_status); + _battery.updateBatteryStatus(now, vbatt, ibatt, throttle, armed, &battery_status); // publish the battery voltage int batt_multi; @@ -305,6 +283,12 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish) publish_flow_topic(&flow); break; + case MAVLINK_MSG_ID_DISTANCE_SENSOR: + mavlink_distance_sensor_t dist; + mavlink_msg_distance_sensor_decode(msg, &dist); + publish_distance_topic(&dist); + break; + case MAVLINK_MSG_ID_HIL_GPS: mavlink_hil_gps_t gps_sim; mavlink_msg_hil_gps_decode(msg, &gps_sim); @@ -371,10 +355,14 @@ void Simulator::poll_topics() { // copy new actuator data if available bool updated; - orb_check(_actuator_outputs_sub, &updated); - if (updated) { - orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuators); + for (unsigned i = 0; i < (sizeof(_actuator_outputs_sub) / sizeof(_actuator_outputs_sub[0])); i++) { + + orb_check(_actuator_outputs_sub[i], &updated); + + if (updated) { + orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub[i], &_actuators[i]); + } } orb_check(_vehicle_status_sub, &updated); @@ -393,7 +381,7 @@ void *Simulator::sending_trampoline(void *) void Simulator::send() { px4_pollfd_struct_t fds[1] = {}; - fds[0].fd = _actuator_outputs_sub; + fds[0].fd = _actuator_outputs_sub[0]; fds[0].events = POLLIN; @@ -462,7 +450,7 @@ void Simulator::initializeSensorData() write_airspeed_data(&airspeed); } -void Simulator::pollForMAVLinkMessages(bool publish) +void Simulator::pollForMAVLinkMessages(bool publish, int udp_port) { // set the threads name #ifdef __PX4_DARWIN @@ -473,13 +461,16 @@ void Simulator::pollForMAVLinkMessages(bool publish) // udp socket data struct sockaddr_in _myaddr; - const int _port = UDP_PORT; + + if (udp_port < 1) { + udp_port = UDP_PORT; + } // try to setup udp socket for communcation with simulator memset((char *)&_myaddr, 0, sizeof(_myaddr)); _myaddr.sin_family = AF_INET; _myaddr.sin_addr.s_addr = htonl(INADDR_ANY); - _myaddr.sin_port = htons(_port); + _myaddr.sin_port = htons(udp_port); if ((_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { PX4_WARN("create socket failed\n"); @@ -532,7 +523,7 @@ void Simulator::pollForMAVLinkMessages(bool publish) // wait for first data from simulator and respond with first controls // this is important for the UDP communication to work int pret = -1; - PX4_INFO("Waiting for initial data on UDP. Please start the flight simulator to proceed.."); + PX4_INFO("Waiting for initial data on UDP port %i. Please start the flight simulator to proceed..", udp_port); uint64_t pstart_time = 0; @@ -549,6 +540,8 @@ void Simulator::pollForMAVLinkMessages(bool publish) len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen); // send hearbeat mavlink_heartbeat_t hb = {}; + hb.autopilot = 12; + hb.base_mode |= (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) ? 128 : 0; send_mavlink_message(MAVLINK_MSG_ID_HEARTBEAT, &hb, 200); if (len > 0) { @@ -579,7 +572,10 @@ void Simulator::pollForMAVLinkMessages(bool publish) (void)hrt_reset(); // subscribe to topics - _actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs), 0); + for (unsigned i = 0; i < (sizeof(_actuator_outputs_sub) / sizeof(_actuator_outputs_sub[0])); i++) { + _actuator_outputs_sub[i] = orb_subscribe_multi(ORB_ID(actuator_outputs), i); + } + _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); // got data from simulator, now activate the sending thread @@ -873,7 +869,7 @@ int Simulator::publish_flow_topic(mavlink_hil_optical_flow_t *flow_mavlink) flow.gyro_y_rate_integral = flow_mavlink->integrated_ygyro; flow.gyro_z_rate_integral = flow_mavlink->integrated_zgyro; flow.pixel_flow_x_integral = flow_mavlink->integrated_x; - flow.pixel_flow_x_integral = flow_mavlink->integrated_y; + flow.pixel_flow_y_integral = flow_mavlink->integrated_y; flow.quality = flow_mavlink->quality; int flow_multi; @@ -881,3 +877,25 @@ int Simulator::publish_flow_topic(mavlink_hil_optical_flow_t *flow_mavlink) return OK; } + +int Simulator::publish_distance_topic(mavlink_distance_sensor_t *dist_mavlink) +{ + uint64_t timestamp = hrt_absolute_time(); + + struct distance_sensor_s dist; + memset(&dist, 0, sizeof(dist)); + + dist.timestamp = timestamp; + dist.min_distance = dist_mavlink->min_distance / 100.0f; + dist.max_distance = dist_mavlink->max_distance / 100.0f; + dist.current_distance = dist_mavlink->current_distance / 100.0f; + dist.type = dist_mavlink->type; + dist.id = dist_mavlink->id; + dist.orientation = dist_mavlink->orientation; + dist.covariance = dist_mavlink->covariance / 100.0f; + + int dist_multi; + orb_publish_auto(ORB_ID(distance_sensor), &_dist_pub, &dist, &dist_multi, ORB_PRIO_HIGH); + + return OK; +} diff --git a/src/modules/systemlib/CMakeLists.txt b/src/modules/systemlib/CMakeLists.txt index 3fadc26da3..7277ee3ca1 100644 --- a/src/modules/systemlib/CMakeLists.txt +++ b/src/modules/systemlib/CMakeLists.txt @@ -49,6 +49,7 @@ set(SRCS bson/tinybson.c circuit_breaker.cpp battery.cpp + hysteresis/hysteresis.cpp ) if(${OS} STREQUAL "nuttx") @@ -56,6 +57,8 @@ if(${OS} STREQUAL "nuttx") err.c printload.c param/param.c + flashparams/flashparams.c + flashparams/flashfs.c up_cxxinitialize.c ) elseif ("${CONFIG_SHMEM}" STREQUAL "1") @@ -80,8 +83,6 @@ px4_add_module( MODULE modules__systemlib COMPILE_FLAGS -Wno-sign-compare - -Wno-attributes - -Wno-packed -Os SRCS ${SRCS} DEPENDS diff --git a/src/modules/systemlib/battery_params.c b/src/modules/systemlib/battery_params.c index 3268e7d8e5..e6d9b62030 100644 --- a/src/modules/systemlib/battery_params.c +++ b/src/modules/systemlib/battery_params.c @@ -78,9 +78,9 @@ PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.05f); * * @group Battery Calibration * @unit norm - * @decimal 2 * @min 0.12 * @max 0.4 + * @decimal 2 * @increment 0.01 */ PARAM_DEFINE_FLOAT(BAT_LOW_THR, 0.15f); @@ -94,9 +94,9 @@ PARAM_DEFINE_FLOAT(BAT_LOW_THR, 0.15f); * * @group Battery Calibration * @unit norm - * @decimal 2 * @min 0.05 * @max 0.1 + * @decimal 2 * @increment 0.01 */ PARAM_DEFINE_FLOAT(BAT_CRIT_THR, 0.07f); @@ -125,6 +125,7 @@ PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.3f); * * @group Battery Calibration * @unit S + * @value 0 Unconfigured * @value 2 2S Battery * @value 3 3S Battery * @value 4 4S Battery @@ -141,7 +142,7 @@ PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.3f); * @value 15 15S Battery * @value 16 16S Battery */ -PARAM_DEFINE_INT32(BAT_N_CELLS, 3); +PARAM_DEFINE_INT32(BAT_N_CELLS, 0); /** * Battery capacity. diff --git a/src/modules/systemlib/bson/tinybson.c b/src/modules/systemlib/bson/tinybson.c index 41ffbcf822..099dffe4bc 100644 --- a/src/modules/systemlib/bson/tinybson.c +++ b/src/modules/systemlib/bson/tinybson.c @@ -456,7 +456,9 @@ bson_encoder_fini(bson_encoder_t encoder) } /* sync file */ - BSON_FSYNC(encoder->fd); + if (encoder->fd > -1) { + BSON_FSYNC(encoder->fd); + } return 0; } diff --git a/src/modules/systemlib/circuit_breaker_params.c b/src/modules/systemlib/circuit_breaker_params.c index f75b09120d..74817bb195 100644 --- a/src/modules/systemlib/circuit_breaker_params.c +++ b/src/modules/systemlib/circuit_breaker_params.c @@ -140,7 +140,7 @@ PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953); * @max 240024 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024); +PARAM_DEFINE_INT32(CBRK_GPSFAIL, 0); /** * Circuit breaker for disabling buzzer diff --git a/src/modules/systemlib/cpuload.c b/src/modules/systemlib/cpuload.c index 77743bdd8c..7f4c967196 100644 --- a/src/modules/systemlib/cpuload.c +++ b/src/modules/systemlib/cpuload.c @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * Petri Tanskanen + * Copyright (c) 2012-2016 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 @@ -38,11 +36,10 @@ * * Measurement of CPU load of each individual task. * - * @author Lorenz Meier + * @author Lorenz Meier * @author Petri Tanskanen */ #include -//#include #include #include diff --git a/src/modules/systemlib/cpuload.h b/src/modules/systemlib/cpuload.h index e997bb3d9a..1c63964cee 100644 --- a/src/modules/systemlib/cpuload.h +++ b/src/modules/systemlib/cpuload.h @@ -38,6 +38,7 @@ __BEGIN_DECLS #include +#include struct system_load_taskinfo_s { uint64_t total_runtime; ///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load diff --git a/src/modules/systemlib/err.c b/src/modules/systemlib/err.c index b7b6f0a9c3..469c06175b 100644 --- a/src/modules/systemlib/err.c +++ b/src/modules/systemlib/err.c @@ -40,7 +40,6 @@ #include -#define __STDC_FORMAT_MACROS #include #include @@ -60,23 +59,14 @@ extern int lib_lowvprintf(const char *fmt, va_list ap); # warning Cannot output without one of CONFIG_NFILE_STREAMS or CONFIG_ARCH_LOWPUTC #endif -const char * -getprogname(void) -{ -#if CONFIG_TASK_NAME_SIZE > 0 - FAR struct tcb_s *thisproc = sched_self(); - - return thisproc->name; -#else - return "app"; -#endif -} +// XXX not used anymore +#if 0 static void warnerr_core(int errcode, const char *fmt, va_list args) { #if CONFIG_NFILE_STREAMS > 0 - fprintf(stderr, "%s: ", getprogname()); + fprintf(stderr, "%s: ", px4_get_taskname()); vfprintf(stderr, fmt, args); /* convenience as many parts of NuttX use negative errno */ @@ -90,7 +80,7 @@ warnerr_core(int errcode, const char *fmt, va_list args) fprintf(stderr, "\n"); #elif CONFIG_ARCH_LOWPUTC - lowsyslog("%s: ", getprogname()); + lowsyslog("%s: ", px4_get_taskname()); lowvsyslog(fmt, args); /* convenience as many parts of NuttX use negative errno */ @@ -201,3 +191,4 @@ vwarnx(const char *fmt, va_list args) { warnerr_core(NOCODE, fmt, args); } +#endif diff --git a/src/modules/systemlib/err.h b/src/modules/systemlib/err.h index 49f181e9b2..7c09bf7b0f 100644 --- a/src/modules/systemlib/err.h +++ b/src/modules/systemlib/err.h @@ -66,24 +66,38 @@ #define _SYSTEMLIB_ERR_H #include +#include #include +#include +#include #include "visibility.h" __BEGIN_DECLS -__EXPORT const char *getprogname(void); - -#ifdef __PX4_POSIX - -#include -#include -#define err(eval, ...) do { PX4_ERR(__VA_ARGS__); PX4_ERR("Task exited with errno=%i\n", errno); \ - px4_task_exit(eval); } while(0) -#define errx(eval, ...) do { PX4_ERR(__VA_ARGS__); px4_task_exit(eval); } while(0) -#define warn(...) PX4_WARN(__VA_ARGS__) -#define warnx(...) PX4_WARN(__VA_ARGS__) +#ifdef __PX4_NUTTX +#define EXIT(eval) exit(eval) #else +#define EXIT(eval) px4_task_exit(eval) +#endif + + +#define err(eval, ...) do { \ + PX4_ERR(__VA_ARGS__); \ + PX4_ERR("Task exited with errno=%i\n", errno); \ + EXIT(eval); \ + } while(0) + +#define errx(eval, ...) do { \ + PX4_ERR(__VA_ARGS__); \ + EXIT(eval); \ + } while(0) + +#define warn(...) PX4_WARN(__VA_ARGS__) +#define warnx(...) PX4_WARN(__VA_ARGS__) + +// XXX not used anymore +#if 0 __EXPORT void err(int eval, const char *fmt, ...) __attribute__((noreturn, format(printf, 2, 3))); __EXPORT void verr(int eval, const char *fmt, va_list) __attribute__((noreturn, format(printf, 2, 0))); __EXPORT void errc(int eval, int code, const char *fmt, ...) __attribute__((noreturn, format(printf, 3, 4))); diff --git a/src/modules/systemlib/flashparams/flashfs.c b/src/modules/systemlib/flashparams/flashfs.c new file mode 100644 index 0000000000..c1c05ad321 --- /dev/null +++ b/src/modules/systemlib/flashparams/flashfs.c @@ -0,0 +1,1122 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file flashparam.c + * + * Global flash based parameter store. + * + * This provides the mechanisms to interface to the PX4 + * parameter system but replace the IO with non file based flash + * i/o routines. So that the code my be implemented on a SMALL memory + * foot print device. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "flashfs.h" +#include +#include + + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +typedef uint32_t h_magic_t; +typedef uint16_t h_size_t; +typedef uint16_t h_flag_t; +typedef uint16_t data_size_t; + +typedef enum flash_config_t { + LargestBlock = 2 * 1024, // This represents the size need for backing store + MagicSig = 0xaa553cc3, + BlankSig = 0xffffffff +} flash_config_t; + +typedef enum flash_flags_t { + SizeMask = 0x0003, + MaskEntry = ~SizeMask, + BlankEntry = (h_flag_t)BlankSig, + ValidEntry = (0xa5ac & ~SizeMask), + ErasedEntry = 0x0000, +} flash_flags_t; + + +/* File flash_entry_header_t will be sizeof(h_magic_t) aligned + * The Size will be the actual length of the header plus the data + * and any padding needed to have the size be an even multiple of + * sizeof(h_magic_t) + * The + */ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wattributes" +typedef struct packed_struct flash_entry_header_t { + h_magic_t magic; /* Used to ID files*/ + h_flag_t flag; /* Used to erase this entry */ + uint32_t crc; /* Calculated over the size - end of data */ + h_size_t size; /* When added to a byte pointer to flash_entry_header_t + * Will result the offset of the next active file or + * free space. */ + flash_file_token_t file_token; /* file token type - essentially the name/type */ +} flash_entry_header_t; +#pragma GCC diagnostic pop + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ +static uint8_t *working_buffer; +static uint16_t working_buffer_size; +static bool working_buffer_static; +static sector_descriptor_t *sector_map; +static int last_erased; + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +const flash_file_token_t parameters_token = { + .n = {'p', 'a', 'r', 'm'}, +}; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: parameter_flashfs_free + * + * Description: + * Frees dynamicaly allocated memory + * + * + ****************************************************************************/ + +static void parameter_flashfs_free(void) +{ + if (!working_buffer_static && working_buffer != NULL) { + free(working_buffer); + working_buffer = NULL; + working_buffer_size = 0; + } +} + +/**************************************************************************** + * Name: blank_flash + * + * Description: + * This helper function returns true if the pointer points to a blank_flash + * location + * + * Input Parameters: + * pf - A pointer to memory aligned on sizeof(uint32_t) boundaries + * + * Returned value: + * true if blank + * + * + ****************************************************************************/ + +static inline int blank_flash(uint32_t *pf) +{ + return *pf == BlankSig; +} + +/**************************************************************************** + * Name: blank_check + * + * Description: + * Given a pointer to a flash entry header and a new size + * + * Input Parameters: + * pf - A pointer to the current flash entry header + * new_size - The total number of bytes to be written + * + * Returned value: + * true if space is blank, If it is not blank it returns false + * + ****************************************************************************/ + +static bool blank_check(flash_entry_header_t *pf, + size_t new_size) +{ + bool rv = true; + uint32_t *pm = (uint32_t *) pf; + new_size /= sizeof(uint32_t); + + while (new_size-- && rv) { + if (!blank_flash(pm++)) { + rv = false; + } + } + + return rv; +} + +/**************************************************************************** + * Name: valid_magic + * + * Description: + * This helper function returns true if the pointer points to a valid + * magic signature + * + * Input Parameters: + * pm - A pointer to memory aligned on sizeof(h_magic_t) boundaries + * + * Returned value: + * true if magic is valid + * + * + ****************************************************************************/ + +static inline int valid_magic(h_magic_t *pm) +{ + return *pm == MagicSig; +} + +/**************************************************************************** + * Name: blank_magic + * + * Description: + * This helper function returns true if the pointer points to a valid + * blank magic signature + * + * Input Parameters: + * pm - A pointer to memory aligned on sizeof(h_magic_t) boundaries + * + * Returned value: + * true if magic is valid + * + * + ****************************************************************************/ + +static inline int blank_magic(h_magic_t *pm) +{ + return *pm == BlankSig; +} +/**************************************************************************** + * Name: erased_entry + * + * Description: + * This helper function returns true if the entry is Erased + * + * Input Parameters: + * fi - A pointer to the current flash entry header + * + * Returned value: + * true if erased + * + * + ****************************************************************************/ + +static inline int erased_entry(flash_entry_header_t *fi) +{ + return (fi->flag & MaskEntry) == ErasedEntry; +} + +/**************************************************************************** + * Name: blank_entry + * + * Description: + * This helper function returns true if the entry is Blank + * + * Input Parameters: + * fi - A pointer to the current flash entry header + * + * Returned value: + * true if Blank + * + * + ****************************************************************************/ + +static inline int blank_entry(flash_entry_header_t *fi) +{ + return fi->magic == BlankSig && fi->flag == BlankEntry; +} + +/**************************************************************************** + * Name: valid_entry + * + * Description: + * This helper function returns true if the entry is Blank + * + * Input Parameters: + * fi - A pointer to the current flash entry header + * + * Returned value: + * true if valid_entry + * + * + ****************************************************************************/ + +static inline int valid_entry(flash_entry_header_t *fi) +{ + return (fi->flag & MaskEntry) == ValidEntry; +} + +/**************************************************************************** + * Name: entry_size_adjust + * + * Description: + * This helper function returns the size adjust + * + * Input Parameters: + * fi - A pointer to the current flash entry header + * + * Returned value: + * true if valid_entry + * + * + ****************************************************************************/ + +static inline int entry_size_adjust(flash_entry_header_t *fi) +{ + return fi->flag & SizeMask; +} + +/**************************************************************************** + * Name: next_entry + * + * Description: + * This helper function advances the flash entry header pointer to the + * locations of the next entry. + * + * Input Parameters: + * fh - A pointer to the current file header + * + * Returned value: + * - A pointer to the next file header location + * + * + ****************************************************************************/ + +static inline flash_entry_header_t *next_entry(flash_entry_header_t *fi) +{ + uint8_t *pb = (uint8_t *)fi; + return (flash_entry_header_t *) &pb[fi->size]; +} + +/**************************************************************************** + * Name: entry_data + * + * Description: + * This helper function returns a pointer the the data in the entry + * + * Input Parameters: + * fh - A pointer to the current file header + * + * Returned value: + * - A pointer to the next file header location + * + * + ****************************************************************************/ + +static inline uint8_t *entry_data(flash_entry_header_t *fi) +{ + return ((uint8_t *)fi) + sizeof(flash_entry_header_t); +} + +/**************************************************************************** + * Name: entry_data_length + * + * Description: + * This helper function returns the size of the user data + * + * Input Parameters: + * fh - A pointer to the current file header + * + * Returned value: + * - The length of the data in the entry + * + * + ****************************************************************************/ + +static inline data_size_t entry_data_length(flash_entry_header_t *fi) +{ + return fi->size - (sizeof(flash_entry_header_t) + entry_size_adjust(fi)); +} + +/**************************************************************************** + * Name: entry_crc_start + * + * Description: + * This helper function returns a const byte pointer to the location + * where the CRC is calculated over + * + * Input Parameters: + * fi - A pointer to the current file header + * + * Returned value: + * A pointer to the point at which the crc is calculated from. + * + * + ****************************************************************************/ + +static inline const uint8_t *entry_crc_start(flash_entry_header_t *fi) +{ + return (const uint8_t *)&fi->size; +} + +/**************************************************************************** + * Name: entry_crc_length + * + * Description: + * This helper function returns the length of the regone where the CRC is + * calculated over + * + * Input Parameters: + * fi - A pointer to the current file header + * + * Returned value: + * Number of bytes to to crc + * + * + ****************************************************************************/ + +static inline data_size_t entry_crc_length(flash_entry_header_t *fi) +{ + return fi->size - offsetof(flash_entry_header_t, size); +} + +/**************************************************************************** + * Name: find_entry + * + * Description: + * This helper function locates an "file" from the the file token + * + * Input Parameters: + * token - A flash file token, the pseudo file name + * + * Returned value: + * On Success a pointer to flash entry header or NULL on failure + * + * + ****************************************************************************/ + +static flash_entry_header_t *find_entry(flash_file_token_t token) +{ + for (int s = 0; sector_map[s].address; s++) { + + h_magic_t *pmagic = (h_magic_t *) sector_map[s].address; + h_magic_t *pe = pmagic + (sector_map[s].size / sizeof(h_magic_t)) - 1; + + /* Hunt for Magic Signature */ +cont: + + while (pmagic != pe && !valid_magic(pmagic)) { + pmagic++; + } + + /* Did we reach the end + * if so try the next sector */ + + if (pmagic == pe) { continue; } + + /* Found a magic So assume it is a file header */ + + flash_entry_header_t *pf = (flash_entry_header_t *) pmagic; + + /* Test the CRC */ + + if (pf->crc == crc32(entry_crc_start(pf), entry_crc_length(pf))) { + + /* Good CRC is it the one we are looking for ?*/ + + if (valid_entry(pf) && pf->file_token.t == token.t) { + + return pf; + + } else { + + /* Not the one we wanted but we can trust the size */ + + pf = next_entry(pf); + pmagic = (h_magic_t *) pf; + + /* If the next one is erased */ + + if (blank_entry(pf)) { + continue; + } + } + + goto cont; + + } else { + + /* in valid CRC so keep looking */ + + pmagic++; + } + } + + return NULL; +} + +/**************************************************************************** + * Name: find_free + * + * Description: + * This helper function locates free space for the number of bytes required + * + * Input Parameters: + * required - Number of bytes required + * + * Returned value: + * On Success a pointer to flash entry header or NULL on failure + * + * + ****************************************************************************/ + +static flash_entry_header_t *find_free(data_size_t required) +{ + for (int s = 0; sector_map[s].address; s++) { + + h_magic_t *pmagic = (h_magic_t *) sector_map[s].address; + h_magic_t *pe = pmagic + (sector_map[s].size / sizeof(h_magic_t)) - 1; + + /* Hunt for Magic Signature */ + + do { + + if (valid_magic(pmagic)) { + + flash_entry_header_t *pf = (flash_entry_header_t *) pmagic; + + /* Test the CRC */ + + if (pf->crc == crc32(entry_crc_start(pf), entry_crc_length(pf))) { + + /* Valid Magic and CRC look for the next record*/ + + pmagic = ((uint32_t *) next_entry(pf)); + + } else { + + pmagic++; + } + } + + if (blank_magic(pmagic)) { + + flash_entry_header_t *pf = (flash_entry_header_t *) pmagic; + + if (blank_entry(pf) && blank_check(pf, required)) { + return pf; + } + + } + } while (++pmagic != pe); + } + + return NULL; +} + +/**************************************************************************** + * Name: get_next_sector_descriptor + * + * Description: + * Given a pointer to sector_descriptor_t, this helper function + * returns a pointer to the next sector_descriptor_t + * + * Input Parameters: + * current - A pointer to the current sector_descriptor_t + * + * Returned value: + * On Success A pointer to the next sector_descriptor_t, + * otherwise NULL + * + * + ****************************************************************************/ + +static sector_descriptor_t *get_next_sector_descriptor(sector_descriptor_t * + current) +{ + for (int s = 0; sector_map[s].address; s++) { + if (current == §or_map[s]) { + if (sector_map[s + 1].address) { + s++; + + } else { + s = 0; + } + + return §or_map[s]; + } + } + + return NULL; +} + +/**************************************************************************** + * Name: get_next_sector + * + * Description: + * Given a pointer to a flash entry header returns the sector descriptor + * for the file is located in + * + * Input Parameters: + * current - A pointer to the current flash entry header + * + * Returned value: + * On Success A pointer to the next sector_descriptor_t, + * otherwise NULL + * + * + ****************************************************************************/ + +static sector_descriptor_t *get_sector_info(flash_entry_header_t *current) +{ + for (int s = 0; sector_map[s].address != 0; s++) { + uint8_t *pb = (uint8_t *) sector_map[s].address; + uint8_t *pe = pb + sector_map[s].size - 1; + uint8_t *pc = (uint8_t *) current; + + if (pc >= pb && pc <= pe) { + return §or_map[s]; + } + } + + return 0; +} + +/**************************************************************************** + * Name: erase_sector + * + * Description: + * Given a pointer to sector_descriptor_t, this function + * erases the sector and updates the last_erased using + * the pointer to the flash_entry_header_t as a sanity check + * + * Input Parameters: + * sm - A pointer to the current sector_descriptor_t + * pf - A pointer to the current flash entry header + * + * Returned value: + * O On Success or a negative errno + * + * + ****************************************************************************/ + +static int erase_sector(sector_descriptor_t *sm, flash_entry_header_t *pf) +{ + int rv = 0; + ssize_t page = up_progmem_getpage((size_t)pf); + + if (page > 0 && page == sm->page) { + last_erased = sm->page; + ssize_t size = up_progmem_erasepage(page); + + if (size < 0 || size != sm->size) { + rv = size; + } + } + + return rv; +} + +/**************************************************************************** + * Name: erase_entry + * + * Description: + * Given a pointer to a flash entry header erases the entry + * + * Input Parameters: + * pf - A pointer to the current flash entry header + * + * + * Returned value: + * >0 On Success or a negative errno + * + * + ****************************************************************************/ + +static int erase_entry(flash_entry_header_t *pf) +{ + h_flag_t data = ErasedEntry; + size_t size = sizeof(h_flag_t); + int rv = up_progmem_write((size_t) &pf->flag, &data, size); + return rv; +} + +/**************************************************************************** + * Name: check_free_space_in_sector + * + * Description: + * Given a pointer to a flash entry header and a new size + * + * Input Parameters: +* pf - A pointer to the current flash entry header + * new_size - The total number of bytes to be written + * + * Returned value: + * 0 if there is enough space left to write new size + * If not it returns the flash_file_sector_t * that needs to be erased. + * + ****************************************************************************/ + +static sector_descriptor_t *check_free_space_in_sector(flash_entry_header_t + *pf, size_t new_size) +{ + sector_descriptor_t *sm = get_sector_info(pf); + uint8_t *psector_first = (uint8_t *) sm->address; + uint8_t *psector_last = psector_first + sm->size - 1; + uint8_t *pnext_end = (uint8_t *)(valid_magic((h_magic_t *)pf) ? next_entry(pf) : pf) + new_size; + + if (pnext_end >= psector_first && pnext_end <= psector_last) { + sm = 0; + } + + return sm; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ +/**************************************************************************** + * Name: parameter_flashfs_read + * + * Description: + * This function returns a pointer to the locations of the data associated + * with the file token. On successful return *buffer will be set to Flash + * location and *buf_size the length of the user data. + * + * Input Parameters: + * token - File Token File to read + * buffer - A pointer to a pointer that will receive the address + * in flash of the data of this "files" data + * buf_size - A pointer to receive the number of bytes in the "file" + * + * Returned value: + * On success number of bytes read or a negative errno value, + * + * + ****************************************************************************/ + +int parameter_flashfs_read(flash_file_token_t token, uint8_t **buffer, size_t + *buf_size) +{ + int rv = -ENXIO; + + if (sector_map) { + + rv = -ENOENT; + flash_entry_header_t *pf = find_entry(token); + + if (pf) { + (*buffer) = entry_data(pf); + rv = entry_data_length(pf); + *buf_size = rv; + } + } + + return rv; +} + +/**************************************************************************** + * Name: parameter_flashfs_write + * + * Description: + * This function writes user data from the buffer allocated with a previous call + * to parameter_flashfs_alloc. flash starting at the given address + * + * Input Parameters: + * token - File Token File to read + * buffer - A pointer to a buffer with buf_size bytes to be written + * to the flash. This buffer must be allocated + * with a previous call to flash_alloc_buffer + * buf_size - Number of bytes to write + * + * Returned value: + * On success the number of bytes written On Error a negative value of errno + * + ****************************************************************************/ + +int +parameter_flashfs_write(flash_file_token_t token, uint8_t *buffer, size_t buf_size) +{ + int rv = -ENXIO; + + if (sector_map) { + + rv = 0; + + /* Calculate the total space needed */ + + size_t total_size = buf_size + sizeof(flash_entry_header_t); + size_t alignment = sizeof(h_magic_t) - 1; + size_t size_adjust = ((total_size + alignment) & ~alignment) - total_size; + total_size += size_adjust; + + /* Is this and existing entry */ + + flash_entry_header_t *pf = find_entry(token); + + if (!pf) { + + /* No Entry exists for this token so find a place for it */ + + pf = find_free(total_size); + + /* No Space */ + + if (pf == 0) { + parameter_flashfs_free(); + return -ENOSPC; + } + + } else { + + /* Do we have space after the entry in the sector for the update */ + + sector_descriptor_t *current_sector = check_free_space_in_sector(pf, + total_size); + + + if (current_sector == 0) { + + /* Mark the last entry erased */ + + /* todo:consider a 2 stage erase or write before erase and do a fs check + * at start up + */ + + rv = erase_entry(pf); + + if (rv < 0) { + parameter_flashfs_free(); + return rv; + } + + /* We had space and marked the last entry erased so use the Next Free */ + + pf = next_entry(pf); + + } else { + + /* + * We did not have space in the current sector so select the next sector + */ + + current_sector = get_next_sector_descriptor(current_sector); + + /* Will the data fit */ + + if (current_sector->size < total_size) { + parameter_flashfs_free(); + return -ENOSPC; + } + + /* Mark the last entry erased */ + + /* todo:consider a 2 stage erase or write before erase and do a fs check + * at start up + */ + + rv = erase_entry(pf); + + if (rv < 0) { + parameter_flashfs_free(); + return rv; + } + + pf = (flash_entry_header_t *) current_sector->address; + } + + if (!blank_check(pf, total_size)) { + rv = erase_sector(current_sector, pf); + } + } + + flash_entry_header_t *pn = (flash_entry_header_t *)(buffer - sizeof(flash_entry_header_t)); + pn->magic = MagicSig; + pn->file_token.t = token.t; + pn->flag = ValidEntry + size_adjust; + pn->size = total_size; + + for (size_t a = 0; a < size_adjust; a++) { + buffer[buf_size + a] = (uint8_t)BlankSig; + } + + pn->crc = crc32(entry_crc_start(pn), entry_crc_length(pn)); + rv = up_progmem_write((size_t) pf, pn, pn->size); + int system_bytes = (sizeof(flash_entry_header_t) + size_adjust); + + if (rv >= system_bytes) { + rv -= system_bytes; + } + } + + parameter_flashfs_free(); + return rv; +} + +/**************************************************************************** + * Name: parameter_flashfs_alloc + * + * Description: + * This function is called to get a buffer to use in a subsequent call + * to parameter_flashfs_write. The address returned is advanced into the + * buffer to reserve space for the flash entry header. + * + * Input Parameters: + * token - File Token File to read (not used) + * buffer - A pointer to return a pointer to Memory of buf_size length + * suitable for calling parameter_flashfs_write + * buf_size - In: the size needed for the write operation. + * Out: The maximum number of bytes that can be written to + * the buffer + * + * Returned value: + * On success the number of bytes written On Error a negative value of errno + * + ****************************************************************************/ + +int parameter_flashfs_alloc(flash_file_token_t token, uint8_t **buffer, size_t *buf_size) +{ + int rv = -ENXIO; + + if (sector_map) { + + rv = -ENOMEM; + + if (!working_buffer_static) { + + working_buffer_size = *buf_size + sizeof(flash_entry_header_t); + working_buffer = malloc(working_buffer_size); + + } + + /* Allocation failed or not provided */ + + if (working_buffer == NULL) { + + working_buffer_size = 0; + + } else { + + /* We have a buffer reserve space and init it */ + *buffer = &working_buffer[sizeof(flash_entry_header_t)]; + *buf_size = working_buffer_size - sizeof(flash_entry_header_t); + memset(working_buffer, 0xff , working_buffer_size); + rv = 0; + + } + + } + + return rv; +} + +/**************************************************************************** + * Name: parameter_flashfs_erase + * + * Description: + * This function erases the sectors that were passed to parameter_flashfs_init + * + * Input Parameters: + * + * Returned value: + * On success the number of bytes erased + * On Error a negative value of errno + * + ****************************************************************************/ + +int parameter_flashfs_erase(void) +{ + int rv = -ENXIO; + + if (sector_map) { + rv = 0; + + for (int s = 0; sector_map[s].address; s++) { + int sz = erase_sector(§or_map[s], (flash_entry_header_t *)sector_map[s].address); + + if (sz != 0) { + return sz; + } + + rv += sector_map[s].size; + } + } + + return rv; +} + +/**************************************************************************** + * Name: parameter_flashfs_init + * + * Description: + * This helper function advances the flash entry header pointer to the + * locations of the next entry. + * + * Input Parameters: + * fconfig - A pointer to an null entry terminated array of + * flash_file_sector_t + * buffer - A pointer to a memory to make available to callers + * for write operations. When allocated to the caller + * space is reserved in the front for the + * flash_entry_header_t. + * If this is passes as NULL. The buffer will be + * allocated from the heap on callse to + * parameter_flashfs_alloc and fread on calls calls + * to parameter_flashfs_write + * + * size - The size of the buffer in bytes. Should be be 0 if buffer + * is NULL + * + * Returned value: + * - A pointer to the next file header location + * + * + ****************************************************************************/ + +int parameter_flashfs_init(sector_descriptor_t *fconfig, uint8_t *buffer, uint16_t size) +{ + int rv = 0; + sector_map = fconfig; + working_buffer_static = buffer != NULL; + + if (!working_buffer_static) { + size = 0; + } + + working_buffer = buffer; + working_buffer_size = size; + last_erased = -1; + + /* Sanity check */ + + flash_entry_header_t *pf = find_entry(parameters_token); + + /* No paramaters */ + + if (pf == NULL) { + size_t total_size = size + sizeof(flash_entry_header_t); + size_t alignment = sizeof(h_magic_t) - 1; + size_t size_adjust = ((total_size + alignment) & ~alignment) - total_size; + total_size += size_adjust; + + /* Do we have free space ?*/ + + if (find_free(total_size) == NULL) { + + /* No paramates and no free space => neeed erase */ + + rv = parameter_flashfs_erase(); + } + } + + return rv; +} + +#if defined(FLASH_UNIT_TEST) + +static sector_descriptor_t test_sector_map[] = { + {1, 16 * 1024, 0x08004000}, + {2, 16 * 1024, 0x08008000}, + {0, 0, 0}, +}; + +__EXPORT void test(void); + +uint8_t test_buf[32 * 1024]; + +__EXPORT void test(void) +{ + uint16_t largest_block = (32 * 1024) + 32; + uint8_t *buffer = malloc(largest_block); + + parameter_flashfs_init(test_sector_map, buffer, largest_block); + + for (int t = 0; t < sizeof(test_buf); t++) { + test_buf[t] = (uint8_t) t; + } + + int er = parameter_flashfs_erase(); + uint8_t *fbuffer; + size_t buf_size; + int written = 0; + int read = 0; + int rv = 0; + + for (int a = 0; a <= 4; a++) { + rv = parameter_flashfs_alloc(parameters_token, &fbuffer, &buf_size); + memcpy(fbuffer, test_buf, a); + buf_size = a; + written = parameter_flashfs_write(parameters_token, fbuffer, buf_size); + read = parameter_flashfs_read(parameters_token, &fbuffer, &buf_size); + + if (read != written) { + static volatile int j; + j++; + } + } + + int block = 2048; + + for (int a = 0; a <= 8; a++) { + rv = parameter_flashfs_alloc(parameters_token, &fbuffer, &buf_size); + memcpy(fbuffer, test_buf, block); + buf_size = block; + written = parameter_flashfs_write(parameters_token, fbuffer, buf_size); + read = parameter_flashfs_read(parameters_token, &fbuffer, &buf_size); + + if (read != written) { + static volatile int j; + j++; + } + + block += 2048; + } + + rv++; + er++; + free(buffer); +} +#endif /* FLASH_UNIT_TEST */ diff --git a/src/modules/systemlib/flashparams/flashfs.h b/src/modules/systemlib/flashparams/flashfs.h new file mode 100644 index 0000000000..65bcb48d00 --- /dev/null +++ b/src/modules/systemlib/flashparams/flashfs.h @@ -0,0 +1,220 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file param.h + * + * Global flash based parameter store. + * + * This provides the mechanisms to interface to the PX4 + * parameter system but replace the IO with non file based flash + * i/o routines. So that the code my be implemented on a SMALL memory + * foot print device. + * + */ + +#ifndef _SYSTEMLIB_FLASHPARAMS_NUTTX_PARAM_H +#define _SYSTEMLIB_FLASHPARAMS_NUTTX_PARAM_H + +#include +#include + + + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ +/* + * PARAMETER_BUFFER_SIZE must be defined larger then the maximum parameter + * memory needed to commit the recored + ~20 bytes. For the syslib's parameter + * this would be the size of the bson representations of the data + */ +#if !defined(PARAMETER_BUFFER_SIZE) +#define PARAMETER_BUFFER_SIZE 512 +#endif + +__BEGIN_DECLS + +/* + * Define the interface data a flash_file_token_t + * is like a file name + * + */ +typedef uint32_t flash_file_tokens_t; + +typedef struct flash_file_token_t { + union { + flash_file_tokens_t t; + uint8_t n[sizeof(flash_file_tokens_t)]; + }; +} flash_file_token_t; + +/* + * Define the parameter "file name" Currently there is only + * and it is hard coded. If more are added the + * parameter_flashfs_write would need to support a backing buffer + * for when a sector is erased. + */ +__EXPORT extern const flash_file_token_t parameters_token; + +/* Define the elements of the array passed to the + * parameter_flashfs_init function + * + * For example + * static sector_descriptor_t sector_map[] = { + * {1, 16 * 1024, 0x08004000}, + * {2, 16 * 1024, 0x08008000}, + * {0, 0, 0}, + * + */ +typedef struct sector_descriptor_t { + uint8_t page; + uint16_t size; + uint32_t address; +} sector_descriptor_t; + + +/**************************************************************************** + * Name: parameter_flashfs_init + * + * Description: + * This helper function advances the flash entry header pointer to the + * locations of the next entry. + * + * Input Parameters: + * fconfig - A pointer to an null entry terminated array of + * flash_file_sector_t + * buffer - A pointer to a memory to make available to callers + * for write operations. When allocated to the caller + * space is reserved in the front for the + * flash_entry_header_t. + * If this is passes as NULL. The buffer will be + * allocated from the heap on callse to + * parameter_flashfs_alloc and fread on calls calls + * to parameter_flashfs_write + * + * size - The size of the buffer in bytes. Should be be 0 if buffer + * is NULL + * + * Returned value: + * - A pointer to the next file header location + * + * + ****************************************************************************/ + +__EXPORT int parameter_flashfs_init(sector_descriptor_t *fconfig, uint8_t *buffer, uint16_t size); + +/**************************************************************************** + * Name: parameter_flashfs_read + * + * Description: + * This function returns a pointer to the locations of the data associated + * with the file token. On successful return *buffer will be set to Flash + * location and *buf_size the length of the user data. + * + * Input Parameters: + * token - File Token File to read + * buffer - A pointer to a pointer that will receive the address + * in flash of the data of this "files" data + * buf_size - A pointer to receive the number of bytes in the "file" + * + * Returned value: + * On success number of bytes read or a negative errno value, + * + * + ****************************************************************************/ + +__EXPORT int parameter_flashfs_read(flash_file_token_t ft, uint8_t **buffer, size_t *buf_size); + +/**************************************************************************** + * Name: parameter_flashfs_write + * + * Description: + * This function writes user data from the buffer allocated with a previous call + * to parameter_flashfs_alloc. flash starting at the given address + * + * Input Parameters: + * token - File Token File to read + * buffer - A pointer to a buffer with buf_size bytes to be written + * to the flash. This buffer must be allocated + * with a previous call to flash_alloc_buffer + * buf_size - Number of bytes to write + * + * Returned value: + * On success the number of bytes written On Error a negative value of errno + * If static buffer was not provided to parameter_flashfs_init the + * buffer will be freed. + * + ****************************************************************************/ + +__EXPORT int parameter_flashfs_write(flash_file_token_t ft, uint8_t *buffer, size_t buf_size); + +/**************************************************************************** + * Name: parameter_flashfs_erase + * + * Description: + * This function erases the sectors that were passed to parameter_flashfs_init + * + * Input Parameters: + * + * Returned value: + * On success the number of bytes erased + * On Error a negative value of errno + * + ****************************************************************************/ + +__EXPORT int parameter_flashfs_erase(void); + +/**************************************************************************** + * Name: parameter_flashfs_alloc + * + * Description: + * This function is called to get a buffer to use in a subsequent call + * to parameter_flashfs_write. The address returned is advanced into the + * buffer to reserve space for the flash entry header. + * + * Input Parameters: + * token - File Token File to read (not used) + * buffer - Memory of buf_size length suitable for calling + * parameter_flashfs_write + * buf_size - The maximum number of bytes that can be written to + * the buffer + * + * Returned value: + * On success the number of bytes written On Error a negative value of errno + * + ****************************************************************************/ + +__EXPORT int parameter_flashfs_alloc(flash_file_token_t ft, uint8_t **buffer, size_t *buf_size); +__END_DECLS +#endif /* _SYSTEMLIB_FLASHPARAMS_NUTTX_PARAM_H */ diff --git a/src/modules/systemlib/flashparams/flashparams.c b/src/modules/systemlib/flashparams/flashparams.c new file mode 100644 index 0000000000..fa474dffdf --- /dev/null +++ b/src/modules/systemlib/flashparams/flashparams.c @@ -0,0 +1,377 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file flashparam.c + * + * Global flash based parameter store. + * + * This provides the mechanisms to interface to the PX4 + * parameter system but replace the IO with non file based flash + * i/o routines. So that the code my be implemented on a SMALL memory + * foot print device. + */ + +#include +#include +#include +#include +#include + +#include "systemlib/param/param.h" +#include "systemlib/uthash/utarray.h" +#include "systemlib/bson/tinybson.h" +#include "flashparams.h" +#include "flashfs.h" + +#if 0 +# define debug(fmt, args...) do { warnx(fmt, ##args); } while(0) +#else +# define debug(fmt, args...) do { } while(0) +#endif + + +/** + * Storage for modified parameters. + */ +struct param_wbuf_s { + param_t param; + union param_value_u val; + bool unsaved; +}; + + +/** lock the parameter store */ +static void +param_lock(void) +{ + //do {} while (sem_wait(¶m_sem) != 0); +} + +/** unlock the parameter store */ +static void +param_unlock(void) +{ + //sem_post(¶m_sem); +} + + +static int +param_export_internal(bool only_unsaved) +{ + struct param_wbuf_s *s = NULL; + struct bson_encoder_s encoder; + int result = -1; + + param_lock(); + + /* Use realloc */ + + bson_encoder_init_buf(&encoder, NULL, 0); + + /* no modified parameters -> we are done */ + if (param_values == NULL) { + result = 0; + goto out; + } + + while ((s = (struct param_wbuf_s *)utarray_next(param_values, s)) != NULL) { + + int32_t i; + float f; + + /* + * If we are only saving values changed since last save, and this + * one hasn't, then skip it + */ + if (only_unsaved && !s->unsaved) { + continue; + } + + s->unsaved = false; + + /* append the appropriate BSON type object */ + + switch (param_type(s->param)) { + + case PARAM_TYPE_INT32: + param_get(s->param, &i); + + if (bson_encoder_append_int(&encoder, param_name(s->param), i)) { + debug("BSON append failed for '%s'", param_name(s->param)); + goto out; + } + + break; + + case PARAM_TYPE_FLOAT: + param_get(s->param, &f); + + if (bson_encoder_append_double(&encoder, param_name(s->param), f)) { + debug("BSON append failed for '%s'", param_name(s->param)); + goto out; + } + + break; + + case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: + if (bson_encoder_append_binary(&encoder, + param_name(s->param), + BSON_BIN_BINARY, + param_size(s->param), + param_get_value_ptr_external(s->param))) { + debug("BSON append failed for '%s'", param_name(s->param)); + goto out; + } + + break; + + default: + debug("unrecognized parameter type"); + goto out; + } + } + + result = 0; + +out: + param_unlock(); + + if (result == 0) { + + /* Finalize the bison encoding*/ + + bson_encoder_fini(&encoder); + + /* Get requiered space */ + + size_t buf_size = bson_encoder_buf_size(&encoder); + + /* Get a buffer from the flash driver with enough space */ + + uint8_t *buffer; + result = parameter_flashfs_alloc(parameters_token, &buffer, &buf_size); + + if (result == OK) { + + /* Check for a write that has no changes */ + + uint8_t *was_buffer; + size_t was_buf_size; + int was_result = parameter_flashfs_read(parameters_token, &was_buffer, &was_buf_size); + + void *enc_buff = bson_encoder_buf_data(&encoder); + + bool commit = was_result < OK || was_buf_size != buf_size || 0 != memcmp(was_buffer, enc_buff, was_buf_size); + + if (commit) { + + memcpy(buffer, enc_buff, buf_size); + result = parameter_flashfs_write(parameters_token, buffer, buf_size); + result = result == buf_size ? OK : -EFBIG; + + } + + free(enc_buff); + } + } + + return result; +} + +struct param_import_state { + bool mark_saved; +}; + +static int +param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node) +{ + float f; + int32_t i; + void *v, *tmp = NULL; + int result = -1; + struct param_import_state *state = (struct param_import_state *)private; + + /* + * EOO means the end of the parameter object. (Currently not supporting + * nested BSON objects). + */ + if (node->type == BSON_EOO) { + debug("end of parameters"); + return 0; + } + + /* + * Find the parameter this node represents. If we don't know it, + * ignore the node. + */ + param_t param = param_find_no_notification(node->name); + + if (param == PARAM_INVALID) { + debug("ignoring unrecognised parameter '%s'", node->name); + return 1; + } + + /* + * Handle setting the parameter from the node + */ + + switch (node->type) { + case BSON_INT32: + if (param_type(param) != PARAM_TYPE_INT32) { + debug("unexpected type for '%s", node->name); + goto out; + } + + i = node->i; + v = &i; + break; + + case BSON_DOUBLE: + if (param_type(param) != PARAM_TYPE_FLOAT) { + debug("unexpected type for '%s", node->name); + goto out; + } + + f = node->d; + v = &f; + break; + + case BSON_BINDATA: + if (node->subtype != BSON_BIN_BINARY) { + debug("unexpected subtype for '%s", node->name); + goto out; + } + + if (bson_decoder_data_pending(decoder) != param_size(param)) { + debug("bad size for '%s'", node->name); + goto out; + } + + /* XXX check actual file data size? */ + tmp = malloc(param_size(param)); + + if (tmp == NULL) { + debug("failed allocating for '%s'", node->name); + goto out; + } + + if (bson_decoder_copy_data(decoder, tmp)) { + debug("failed copying data for '%s'", node->name); + goto out; + } + + v = tmp; + break; + + default: + debug("unrecognised node type"); + goto out; + } + + if (param_set_external(param, v, state->mark_saved, true, false)) { + + debug("error setting value for '%s'", node->name); + goto out; + } + + if (tmp != NULL) { + free(tmp); + tmp = NULL; + } + + /* don't return zero, that means EOF */ + result = 1; + +out: + + if (tmp != NULL) { + free(tmp); + } + + return result; +} + +static int +param_import_internal(bool mark_saved) +{ + struct bson_decoder_s decoder; + int result = -1; + struct param_import_state state; + + uint8_t *buffer = 0; + size_t buf_size; + parameter_flashfs_read(parameters_token, &buffer, &buf_size); + + if (bson_decoder_init_buf(&decoder, buffer, buf_size, param_import_callback, &state)) { + debug("decoder init failed"); + goto out; + } + + state.mark_saved = mark_saved; + + do { + result = bson_decoder_next(&decoder); + + } while (result > 0); + +out: + + if (result < 0) { + debug("BSON error decoding parameters"); + } + + return result; +} + +int flash_param_save(void) +{ + return param_export_internal(false); +} + + +int flash_param_save_default(void) +{ + return param_export_internal(false); +} + + +int flash_param_load(void) +{ + param_reset_all(); + return param_import_internal(true); +} + +int flash_param_import(void) +{ + return 0; +} diff --git a/src/modules/systemlib/flashparams/flashparams.h b/src/modules/systemlib/flashparams/flashparams.h new file mode 100644 index 0000000000..8300669bd4 --- /dev/null +++ b/src/modules/systemlib/flashparams/flashparams.h @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file param.h + * + * Global flash based parameter store. + * + * This provides the mechanisms to interface to the PX4 + * parameter system but replace the IO with non file based flash + * i/o routines. So that the code my be implemented on a SMALL memory + * foot print device. + * + */ + +#ifndef _SYSTEMLIB_FLASHPARAMS_FLASHPARAMS_H +#define _SYSTEMLIB_FLASHPARAMS_FLASHPARAMS_H + +#include +#include +#include +#include "systemlib/uthash/utarray.h" + +__BEGIN_DECLS + +/* + * When using the flash based parameter store we have to force + * the param_values and 2 functions to be global + */ + +#define FLASH_PARAMS_EXPOSE __EXPORT + +__EXPORT extern UT_array *param_values; +__EXPORT int param_set_external(param_t param, const void *val, bool mark_saved, bool notify_changes, bool is_saved); +__EXPORT const void *param_get_value_ptr_external(param_t param); + +/* The interface hooks to the Flash based storage */ +__EXPORT int flash_param_save(void); +__EXPORT int flash_param_save_default(void); +__EXPORT int flash_param_load(void); +__EXPORT int flash_param_import(void); +__END_DECLS +#endif /* _SYSTEMLIB_FLASHPARAMS_FLASHPARAMS_H */ diff --git a/src/modules/systemlib/hysteresis/hysteresis.cpp b/src/modules/systemlib/hysteresis/hysteresis.cpp new file mode 100644 index 0000000000..33aee834c1 --- /dev/null +++ b/src/modules/systemlib/hysteresis/hysteresis.cpp @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * Copyright (c) 2016 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 hysteresis.cpp + * + * @author Julian Oes + */ + +#include +#include "systemlib/hysteresis/hysteresis.h" + + +namespace systemlib +{ + + +void +Hysteresis::set_state_and_update(const bool new_state) +{ + if (new_state != _state) { + if (new_state != _requested_state) { + _requested_state = new_state; + _last_time_to_change_state = hrt_absolute_time(); + } + + } else { + _requested_state = _state; + } + + update(); +} + +void +Hysteresis::update() +{ + if (_requested_state != _state) { + + if (hrt_elapsed_time(&_last_time_to_change_state) >= (_state ? + _hysteresis_time_from_true_us : + _hysteresis_time_from_false_us)) { + _state = _requested_state; + } + } +} + +} // namespace systemlib diff --git a/src/modules/systemlib/hysteresis/hysteresis.h b/src/modules/systemlib/hysteresis/hysteresis.h new file mode 100644 index 0000000000..97cd6c049e --- /dev/null +++ b/src/modules/systemlib/hysteresis/hysteresis.h @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * Copyright (c) 2016 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 hysteresis.h + * + * Hysteresis of a boolean value. + * + * @author Julian Oes + */ + +#pragma once + +#include + +namespace systemlib +{ + +class Hysteresis +{ +public: + Hysteresis(bool init_state) : + _state(init_state), + _requested_state(init_state), + _hysteresis_time_from_true_us(0), + _hysteresis_time_from_false_us(0), + _last_time_to_change_state(0) + {} + + ~Hysteresis() + {} + + void set_hysteresis_time_from(const bool from_state, const hrt_abstime new_hysteresis_time_us) + { + if (from_state == true) { + _hysteresis_time_from_true_us = new_hysteresis_time_us; + + } else { + _hysteresis_time_from_false_us = new_hysteresis_time_us; + } + } + + bool get_state() const + { + return _state; + } + + void set_state_and_update(const bool new_state); + + void update(); + +private: + + bool _state; + bool _requested_state; + hrt_abstime _hysteresis_time_from_true_us; + hrt_abstime _hysteresis_time_from_false_us; + hrt_abstime _last_time_to_change_state; +}; + +} // namespace systemlib diff --git a/src/modules/systemlib/mavlink_log.c b/src/modules/systemlib/mavlink_log.c index 4d4b0062aa..33d463f502 100644 --- a/src/modules/systemlib/mavlink_log.c +++ b/src/modules/systemlib/mavlink_log.c @@ -47,6 +47,7 @@ #include #include "mavlink_log.h" +#define MAVLINK_LOG_QUEUE_SIZE 5 __EXPORT void mavlink_vasprintf(int severity, orb_advert_t *mavlink_log_pub, const char *fmt, ...) @@ -65,6 +66,8 @@ __EXPORT void mavlink_vasprintf(int severity, orb_advert_t *mavlink_log_pub, con log_msg.severity = severity; + log_msg.timestamp = hrt_absolute_time(); + va_list ap; va_start(ap, fmt); @@ -77,7 +80,9 @@ __EXPORT void mavlink_vasprintf(int severity, orb_advert_t *mavlink_log_pub, con orb_publish(ORB_ID(mavlink_log), *mavlink_log_pub, &log_msg); } else { - *mavlink_log_pub = orb_advertise(ORB_ID(mavlink_log), &log_msg); + *mavlink_log_pub = orb_advertise_queue(ORB_ID(mavlink_log), + &log_msg, + MAVLINK_LOG_QUEUE_SIZE); } } diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index f23a9dd4e3..2f7f1befed 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -427,7 +427,7 @@ public: protected: private: - mixer_simple_s *_info; + mixer_simple_s *_pinfo; static int parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler); static int parse_control_scaler(const char *buf, diff --git a/src/modules/systemlib/mixer/mixer_simple.cpp b/src/modules/systemlib/mixer/mixer_simple.cpp index 4ad20c8aee..31108c1a67 100644 --- a/src/modules/systemlib/mixer/mixer_simple.cpp +++ b/src/modules/systemlib/mixer/mixer_simple.cpp @@ -61,14 +61,14 @@ SimpleMixer::SimpleMixer(ControlCallback control_cb, uintptr_t cb_handle, mixer_simple_s *mixinfo) : Mixer(control_cb, cb_handle), - _info(mixinfo) + _pinfo(mixinfo) { } SimpleMixer::~SimpleMixer() { - if (_info != nullptr) { - free(_info); + if (_pinfo != nullptr) { + free(_pinfo); } } @@ -279,7 +279,7 @@ SimpleMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) { float sum = 0.0f; - if (_info == nullptr) { + if (_pinfo == nullptr) { return 0; } @@ -287,26 +287,26 @@ SimpleMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) return 0; } - for (unsigned i = 0; i < _info->control_count; i++) { + for (unsigned i = 0; i < _pinfo->control_count; i++) { float input; _control_cb(_cb_handle, - _info->controls[i].control_group, - _info->controls[i].control_index, + _pinfo->controls[i].control_group, + _pinfo->controls[i].control_index, input); - sum += scale(_info->controls[i].scaler, input); + sum += scale(_pinfo->controls[i].scaler, input); } - *outputs = scale(_info->output_scaler, sum); + *outputs = scale(_pinfo->output_scaler, sum); return 1; } void SimpleMixer::groups_required(uint32_t &groups) { - for (unsigned i = 0; i < _info->control_count; i++) { - groups |= 1 << _info->controls[i].control_group; + for (unsigned i = 0; i < _pinfo->control_count; i++) { + groups |= 1 << _pinfo->controls[i].control_group; } } @@ -318,30 +318,30 @@ SimpleMixer::check() /* sanity that presumes that a mixer includes a control no more than once */ /* max of 32 groups due to groups_required API */ - if (_info->control_count > 32) { + if (_pinfo->control_count > 32) { return -2; } /* validate the output scaler */ - ret = scale_check(_info->output_scaler); + ret = scale_check(_pinfo->output_scaler); if (ret != 0) { return ret; } /* validate input scalers */ - for (unsigned i = 0; i < _info->control_count; i++) { + for (unsigned i = 0; i < _pinfo->control_count; i++) { /* verify that we can fetch the control */ if (_control_cb(_cb_handle, - _info->controls[i].control_group, - _info->controls[i].control_index, + _pinfo->controls[i].control_group, + _pinfo->controls[i].control_index, junk) != 0) { return -3; } /* validate the scaler */ - ret = scale_check(_info->controls[i].scaler); + ret = scale_check(_pinfo->controls[i].scaler); if (ret != 0) { return (10 * i + ret); diff --git a/src/modules/systemlib/mixer/module.mk b/src/modules/systemlib/mixer/module.mk deleted file mode 100644 index 3fd07f5ba1..0000000000 --- a/src/modules/systemlib/mixer/module.mk +++ /dev/null @@ -1,47 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - - -# -# mixer library -# -LIBNAME = mixerlib - -SRCS = mixer.cpp \ - mixer_group.cpp \ - mixer_multirotor.cpp \ - mixer_simple.cpp \ - mixer_load.c - -SELF_DIR := $(dir $(lastword $(MAKEFILE_LIST))) -include $(SELF_DIR)multi_tables.mk diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index fdfcd729e7..2033fe36b4 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -63,12 +63,21 @@ #include "systemlib/uthash/utarray.h" #include "systemlib/bson/tinybson.h" -#include "uORB/uORB.h" -#include "uORB/topics/parameter_update.h" -#include "px4_parameters.h" +#if !defined(PARAM_NO_ORB) +# include "uORB/uORB.h" +# include "uORB/topics/parameter_update.h" +#endif +#if !defined(FLASH_BASED_PARAMS) +# define FLASH_PARAMS_EXPOSE +#else +# include "systemlib/flashparams/flashparams.h" +#endif + +#include "px4_parameters.h" #include + #if 0 # define debug(fmt, args...) do { warnx(fmt, ##args); } while(0) #else @@ -90,12 +99,11 @@ extern struct param_info_s param_array[]; extern struct param_info_s *param_info_base; extern struct param_info_s *param_info_limit; +#define param_info_count (param_info_limit - param_info_base) #else -// FIXME - start and end are reversed static const struct param_info_s *param_info_base = (const struct param_info_s *) &px4_parameters; -#endif - #define param_info_count px4_parameters.param_count +#endif /* _UNIT_TEST */ /** * Storage for modified parameters. @@ -132,16 +140,16 @@ get_param_info_count(void) } /** flexible array holding modified parameter values */ -UT_array *param_values; +FLASH_PARAMS_EXPOSE UT_array *param_values; /** array info for the modified parameters array */ -const UT_icd param_icd = {sizeof(struct param_wbuf_s), NULL, NULL, NULL}; +FLASH_PARAMS_EXPOSE const UT_icd param_icd = {sizeof(struct param_wbuf_s), NULL, NULL, NULL}; -/** parameter update topic */ -ORB_DEFINE(parameter_update, struct parameter_update_s); +#if !defined(PARAM_NO_ORB) /** parameter update topic handle */ static orb_advert_t param_topic = NULL; +#endif static void param_set_used_internal(param_t param); @@ -177,7 +185,7 @@ param_assert_locked(void) static bool handle_in_range(param_t param) { - int count = get_param_info_count(); + unsigned count = get_param_info_count(); return (count && param < count); } @@ -239,7 +247,11 @@ param_find_changed(param_t param) static void param_notify_changes(bool is_saved) { - struct parameter_update_s pup = { .timestamp = hrt_absolute_time(), .saved = is_saved}; +#if !defined(PARAM_NO_ORB) + struct parameter_update_s pup = { + .timestamp = hrt_absolute_time(), + .saved = is_saved + }; /* * If we don't have a handle to our topic, create one now; otherwise @@ -251,6 +263,8 @@ param_notify_changes(bool is_saved) } else { orb_publish(ORB_ID(parameter_update), param_topic, &pup); } + +#endif } param_t @@ -438,6 +452,7 @@ param_size(param_t param) return 0; } + /** * Obtain a pointer to the storage allocated for a parameter. * @@ -540,10 +555,12 @@ param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_ switch (param_type(param)) { case PARAM_TYPE_INT32: + params_changed = s->val.i != *(int32_t *)val; s->val.i = *(int32_t *)val; break; case PARAM_TYPE_FLOAT: + params_changed = fabsf(s->val.f - * (float *)val) > FLT_EPSILON; s->val.f = *(float *)val; break; @@ -558,6 +575,7 @@ param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_ } memcpy(s->val.p, val, param_size(param)); + params_changed = true; break; default: @@ -565,7 +583,6 @@ param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_ } s->unsaved = !mark_saved; - params_changed = true; result = 0; } @@ -583,6 +600,19 @@ out: return result; } +#if defined(FLASH_BASED_PARAMS) +int param_set_external(param_t param, const void *val, bool mark_saved, bool notify_changes, bool is_saved) +{ + return param_set_internal(param, val, mark_saved, notify_changes, is_saved); +} + +const void *param_get_value_ptr_external(param_t param) +{ + return param_get_value_ptr(param); +} + +#endif + int param_set(param_t param, const void *val) { @@ -734,6 +764,7 @@ int param_save_default(void) { int res; +#if !defined(FLASH_BASED_PARAMS) int fd; const char *filename = param_get_default_file(); @@ -759,6 +790,9 @@ param_save_default(void) } PARAM_CLOSE(fd); +#else + res = flash_param_save(); +#endif return res; } @@ -809,17 +843,17 @@ param_bus_lock(bool lock) // XXX this would be the preferred locking method // if (dev == nullptr) { - // dev = up_spiinitialize(PX4_SPI_BUS_BARO); + // dev = px4_spibus_initialize(PX4_SPI_BUS_BARO); // } // SPI_LOCK(dev, lock); // we lock like this for Pixracer for now if (lock) { - irq_state = irqsave(); + irq_state = px4_enter_critical_section(); } else { - irqrestore(irq_state); + px4_leave_critical_section(irq_state); } #endif @@ -861,49 +895,63 @@ param_export(int fd, bool only_unsaved) /* append the appropriate BSON type object */ - /* lock as short as possible */ - param_bus_lock(true); switch (param_type(s->param)) { - case PARAM_TYPE_INT32: - param_get(s->param, &i); + case PARAM_TYPE_INT32: { + param_get(s->param, &i); + const char *name = param_name(s->param); - if (bson_encoder_append_int(&encoder, param_name(s->param), i)) { - debug("BSON append failed for '%s'", param_name(s->param)); - param_bus_lock(false); - goto out; + /* lock as short as possible */ + param_bus_lock(true); + + if (bson_encoder_append_int(&encoder, name, i)) { + param_bus_lock(false); + debug("BSON append failed for '%s'", name); + goto out; + } } - break; - case PARAM_TYPE_FLOAT: - param_get(s->param, &f); + case PARAM_TYPE_FLOAT: { - if (bson_encoder_append_double(&encoder, param_name(s->param), f)) { - debug("BSON append failed for '%s'", param_name(s->param)); - param_bus_lock(false); - goto out; + param_get(s->param, &f); + const char *name = param_name(s->param); + + /* lock as short as possible */ + param_bus_lock(true); + + if (bson_encoder_append_double(&encoder, name, f)) { + param_bus_lock(false); + debug("BSON append failed for '%s'", name); + goto out; + } } - break; - case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: - if (bson_encoder_append_binary(&encoder, - param_name(s->param), - BSON_BIN_BINARY, - param_size(s->param), - param_get_value_ptr(s->param))) { - debug("BSON append failed for '%s'", param_name(s->param)); - param_bus_lock(false); - goto out; - } + case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: { + const char *name = param_name(s->param); + const size_t size = param_size(s->param); + const void *value_ptr = param_get_value_ptr(s->param); + + /* lock as short as possible */ + param_bus_lock(true); + + if (bson_encoder_append_binary(&encoder, + name, + BSON_BIN_BINARY, + size, + value_ptr)) { + param_bus_lock(false); + debug("BSON append failed for '%s'", name); + goto out; + } + } break; default: debug("unrecognized parameter type"); - param_bus_lock(false); goto out; } diff --git a/src/modules/systemlib/param/param_shmem.c b/src/modules/systemlib/param/param_shmem.c index 333fa3e885..ea64b7e6ae 100644 --- a/src/modules/systemlib/param/param_shmem.c +++ b/src/modules/systemlib/param/param_shmem.c @@ -86,11 +86,11 @@ extern struct param_info_s param_array[]; extern struct param_info_s *param_info_base; extern struct param_info_s *param_info_limit; +#define param_info_count (param_info_limit - param_info_base) #else static struct param_info_s *param_info_base = (struct param_info_s *) &px4_parameters; -#endif - #define param_info_count px4_parameters.param_count +#endif /* _UNIT_TEST */ /** * Storage for modified parameters. @@ -109,7 +109,7 @@ const int bits_per_allocation_unit = (sizeof(*param_changed_storage) * 8); //#define ENABLE_SHMEM_DEBUG extern int get_shmem_lock(const char *caller_file_name, int caller_line_number); -extern void release_shmem_lock(void); +extern void release_shmem_lock(const char *caller_file_name, int caller_line_number); struct param_wbuf_s *param_find_changed(param_t param); @@ -158,9 +158,6 @@ UT_array *param_values; /** array info for the modified parameters array */ const UT_icd param_icd = {sizeof(struct param_wbuf_s), NULL, NULL, NULL}; -/** parameter update topic */ -ORB_DEFINE(parameter_update, struct parameter_update_s); - /** parameter update topic handle */ static orb_advert_t param_topic = NULL; @@ -831,18 +828,9 @@ param_save_default(void) { int res = OK; int fd = -1; - bool is_locked = false; const char *filename = param_get_default_file(); - if (get_shmem_lock(__FILE__, __LINE__) != 0) { - PX4_ERR("Could not get shmem lock"); - res = ERROR; - goto exit; - } - - is_locked = true; - fd = PARAM_OPEN(filename, O_WRONLY | O_CREAT, PX4_O_MODE_666); if (fd < 0) { @@ -872,16 +860,12 @@ param_save_default(void) exit: - if (is_locked) { - release_shmem_lock(); - } - if (fd >= 0) { close(fd); } if (res == OK) { - PX4_INFO("saving params completed successfully"); + PX4_DEBUG("saving params completed successfully"); } return res; @@ -926,7 +910,9 @@ param_load_default_no_notify(void) int fd_load = open(param_get_default_file(), O_RDONLY); if (fd_load < 0) { - release_shmem_lock(); +#ifdef __PX4_QURT + release_shmem_lock(__FILE__, __LINE__); +#endif /* no parameter file is OK, otherwise this is an error */ if (errno != ENOENT) { @@ -941,7 +927,7 @@ param_load_default_no_notify(void) close(fd_load); - PX4_INFO("param loading done"); + PX4_DEBUG("param loading done"); if (result != 0) { PX4_WARN("error reading parameters from '%s'", param_get_default_file()); @@ -1244,8 +1230,10 @@ uint32_t param_hash_check(void) void init_params(void) { +#ifdef __PX4_QURT //copy params to shared memory init_shared_memory(); +#endif /*load params automatically*/ #ifdef __PX4_POSIX @@ -1253,6 +1241,7 @@ void init_params(void) #endif param_import_done = 1; +#ifdef __PX4_QURT copy_params_to_shmem(param_info_base); @@ -1263,6 +1252,6 @@ void init_params(void) (unsigned char *)&shmem_info_p->krait_changed_index - (unsigned char *)shmem_info_p, (unsigned char *)&shmem_info_p->adsp_changed_index - (unsigned char *)shmem_info_p); #endif - +#endif } diff --git a/src/modules/systemlib/print_load_posix.c b/src/modules/systemlib/print_load_posix.c index 5e12dc3323..f47c466103 100644 --- a/src/modules/systemlib/print_load_posix.c +++ b/src/modules/systemlib/print_load_posix.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2015-2016 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 @@ -39,13 +39,26 @@ * @author Lorenz Meier */ +#include + +#include #include #include +#include #include #include #include +#ifdef __PX4_DARWIN +#include +#endif + +#ifdef __PX4_QURT +// dprintf is not available on QURT. Use the usual output to mini-dm. +#define dprintf(_fd, _text, ...) ((_fd) == 1 ? PX4_INFO((_text), ##__VA_ARGS__) : (void)(_fd)) +#endif + extern struct system_load_s system_load; #define CL "\033[K" // clear line @@ -70,5 +83,103 @@ void init_print_load_s(uint64_t t, struct print_load_s *s) void print_load(uint64_t t, int fd, struct print_load_s *print_state) { + char *clear_line = ""; + + /* print system information */ + if (fd == 1) { + dprintf(fd, "\033[H"); /* move cursor home and clear screen */ + clear_line = CL; + } + +#if defined (__PX4_LINUX) + dprintf(fd, "%sTOP NOT IMPLEMENTED ON LINUX\n", + clear_line); + +#elif defined (__PX4_QURT) + dprintf(fd, "%sTOP NOT IMPLEMENTED ON QURT\n", + clear_line); + +#elif defined (__PX4_DARWIN) + pid_t pid = getpid(); //-- this is the process id you need info for + task_t task_handle; + task_for_pid(mach_task_self(), pid, &task_handle); + + task_info_data_t tinfo; + mach_msg_type_number_t th_info_cnt; + + th_info_cnt = TASK_INFO_MAX; + kern_return_t kr = task_info(task_handle, TASK_BASIC_INFO, (task_info_t)tinfo, &th_info_cnt); + + if (kr != KERN_SUCCESS) { + return; + } + + task_basic_info_t basic_info; + thread_array_t thread_list; + mach_msg_type_number_t th_cnt; + + thread_info_data_t th_info_data; + mach_msg_type_number_t thread_info_count; + + thread_basic_info_t basic_info_th; + uint32_t stat_thread = 0; + + basic_info = (task_basic_info_t)tinfo; + + // get all threads of the PX4 main task + kr = task_threads(task_handle, &thread_list, &th_cnt); + + if (kr != KERN_SUCCESS) { + PX4_WARN("ERROR getting thread list"); + return; + } + + if (th_cnt > 0) { + stat_thread += th_cnt; + } + + long tot_sec = 0; + long tot_usec = 0; + long tot_cpu = 0; + + dprintf(fd, "%sThreads: %d total\n", + clear_line, + th_cnt); + + for (int j = 0; j < th_cnt; j++) { + thread_info_count = THREAD_INFO_MAX; + kr = thread_info(thread_list[j], THREAD_BASIC_INFO, + (thread_info_t)th_info_data, &thread_info_count); + + if (kr != KERN_SUCCESS) { + PX4_WARN("ERROR getting thread info"); + continue; + } + + basic_info_th = (thread_basic_info_t)th_info_data; + + if (!(basic_info_th->flags & TH_FLAGS_IDLE)) { + tot_sec = tot_sec + basic_info_th->user_time.seconds + basic_info_th->system_time.seconds; + tot_usec = tot_usec + basic_info_th->system_time.microseconds + basic_info_th->system_time.microseconds; + tot_cpu = tot_cpu + basic_info_th->cpu_usage; + } + + // char tname[128]; + + // int ret = pthread_getname_np(pthread_t *thread, + // const char *name, size_t len); + + dprintf(fd, "thread %d\t\t %d\n", j, basic_info_th->cpu_usage); + } + + kr = vm_deallocate(mach_task_self(), (vm_offset_t)thread_list, + th_cnt * sizeof(thread_t)); + + if (kr != KERN_SUCCESS) { + PX4_WARN("ERROR cleaning up thread info"); + return; + } + +#endif } diff --git a/src/modules/systemlib/printload.c b/src/modules/systemlib/printload.c index a179ddc51b..fa15ae99fc 100644 --- a/src/modules/systemlib/printload.c +++ b/src/modules/systemlib/printload.c @@ -215,12 +215,25 @@ void print_load(uint64_t t, int fd, struct print_load_s *print_state) (double)(task_load * 100.f), (double)(sched_load * 100.f), (double)(idle * 100.f)); +#if defined(BOARD_DMA_ALLOC_POOL_SIZE) + uint16_t dma_total; + uint16_t dma_used; + uint16_t dma_peak_used; + + if (board_get_dma_usage(&dma_total, &dma_used, &dma_peak_used) >= 0) { + dprintf(fd, "%sDMA Memory: %d total, %d used %d peak\n", + clear_line, + dma_total, + dma_used, + dma_peak_used); + } + +#endif dprintf(fd, "%sUptime: %.3fs total, %.3fs idle\n%s\n", clear_line, (double)curr_time_us / 1000000.d, (double)idle_time_us / 1000000.d, clear_line); - /* header for task list */ dprintf(fd, "%s%4s %*-s %8s %6s %11s %10s %-6s\n", clear_line, diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.h b/src/modules/systemlib/pwm_limit/pwm_limit.h index 23adc9ee3a..644059e5a4 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.h +++ b/src/modules/systemlib/pwm_limit/pwm_limit.h @@ -51,11 +51,11 @@ __BEGIN_DECLS * time for the ESCs to initialize * (this is not actually needed if PWM is sent right after boot) */ -#define INIT_TIME_US 500000 +#define INIT_TIME_US 50000 /* * time to slowly ramp up the ESCs */ -#define RAMP_TIME_US 2500000 +#define RAMP_TIME_US 500000 enum pwm_limit_state { PWM_LIMIT_STATE_OFF = 0, diff --git a/src/modules/systemlib/px4_macros.h b/src/modules/systemlib/px4_macros.h new file mode 100644 index 0000000000..fc8079fea6 --- /dev/null +++ b/src/modules/systemlib/px4_macros.h @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * 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_macros.h + * + * 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 + +#ifndef _PX4_MACROS_H +#define _PX4_MACROS_H + + +#if !defined(arraySize) +#define arraySize(a) (sizeof((a))/sizeof((a[0]))) +#endif + +#if !defined(NO_BLOAT) +#if defined(__BASE_FILE__) +#define _FILE_NAME_ __BASE_FILE__ +#else +#define _FILE_NAME_ __FILE__ +#endif +#else +#define _FILE_NAME_ "" +#endif + +#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 + +#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 + +#define DO_PRAGMA(x) _Pragma (#x) +#define TODO(x) DO_PRAGMA(message ("TODO - " #x)) + +#endif /* _PX4_MACROS_H */ diff --git a/src/modules/systemlib/state_table.h b/src/modules/systemlib/state_table.h index f28b9180c1..7f657f4fc8 100644 --- a/src/modules/systemlib/state_table.h +++ b/src/modules/systemlib/state_table.h @@ -51,7 +51,10 @@ public: }; StateTable(Tran const *table, unsigned nStates, unsigned nSignals) - : myTable(table), myNsignals(nSignals) {} + : myState(0), myTable(table), myNsignals(nSignals) {} + + StateTable(const StateTable &) = delete; + StateTable &operator=(const StateTable &) = delete; #define NO_ACTION &StateTable::doNothing #define ACTION(_target) StateTable::Action(_target) diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c index 1db83fa260..f6ea3d045b 100644 --- a/src/modules/systemlib/system_params.c +++ b/src/modules/systemlib/system_params.c @@ -43,6 +43,8 @@ * CHANGING THIS VALUE REQUIRES A RESTART. Defines the auto-start script used to bootstrap the system. * * @reboot_required true + * @min 0 + * @max 99999 * @group System */ PARAM_DEFINE_INT32(SYS_AUTOSTART, 0); @@ -56,6 +58,8 @@ PARAM_DEFINE_INT32(SYS_AUTOSTART, 0); * * @min 0 * @max 1 + * @value 0 Keep parameters + * @value 1 Reset parameters * @group System */ PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0); @@ -65,6 +69,7 @@ PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0); * * Can be used to use a standard startup script but with a FMU only set-up. Set to 0 to force the FMU only set-up. * + * @boolean * @min 0 * @max 1 * @group System @@ -78,6 +83,9 @@ PARAM_DEFINE_INT32(SYS_USE_IO, 1); * * @min 0 * @max 2 + * @value 0 Data survives resets + * @value 1 Data survives in-flight resets only + * @value 2 Data does not survive reset * @group System */ PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2); @@ -96,14 +104,13 @@ PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2); * @reboot_required true * @group System */ -PARAM_DEFINE_INT32(SYS_MC_EST_GROUP, 0); +PARAM_DEFINE_INT32(SYS_MC_EST_GROUP, 1); /** - * Enable TELEM2 as companion computer link + * TELEM2 as companion computer link * * CHANGING THIS VALUE REQUIRES A RESTART. Configures the baud rate of the TELEM2 connector as * companion computer interface. - * Set to zero to disable, set to these values to enable (NO OTHER VALUES SUPPORTED!). * * @value 0 Disabled * @value 10 FrSky Telemetry @@ -132,3 +139,15 @@ PARAM_DEFINE_INT32(SYS_COMPANION, 157600); * @group System */ PARAM_DEFINE_INT32(SYS_PARAM_VER, 1); + +/** + * SD logger + * + * @value 0 sdlog2 (default) + * @value 1 new logger + * @min 0 + * @max 1 + * @reboot_required true + * @group System + */ +PARAM_DEFINE_INT32(SYS_LOGGER, 0); diff --git a/src/modules/systemlib/uthash/uthash.h b/src/modules/systemlib/uthash/uthash.h index a8642833f6..9f83fc34f1 100644 --- a/src/modules/systemlib/uthash/uthash.h +++ b/src/modules/systemlib/uthash/uthash.h @@ -61,7 +61,6 @@ do { typedef unsigned int uint32_t; typedef unsigned char uint8_t; #else -#define __STDC_FORMAT_MACROS #include /* uint32_t */ #endif diff --git a/src/modules/uORB/CMakeLists.txt b/src/modules/uORB/CMakeLists.txt index 894ffc63e4..508318c7d1 100644 --- a/src/modules/uORB/CMakeLists.txt +++ b/src/modules/uORB/CMakeLists.txt @@ -36,8 +36,9 @@ include_directories( ${CMAKE_CURRENT_BINARY_DIR} ) +link_libraries(msg_gen) + set(SRCS - objects_common.cpp uORBUtils.cpp uORB.cpp uORBMain.cpp @@ -49,17 +50,14 @@ set(SRCS if(${OS} STREQUAL "nuttx") list(APPEND SRCS uORBDevices_nuttx.cpp - uORBTest_UnitTest.cpp ) elseif(${OS} STREQUAL "posix") list(APPEND SRCS uORBDevices_posix.cpp - uORBTest_UnitTest.cpp ) elseif(${OS} STREQUAL "posix-arm") list(APPEND SRCS uORBDevices_posix.cpp - uORBTest_UnitTest.cpp ) elseif(${OS} STREQUAL "qurt") list(APPEND SRCS @@ -70,7 +68,7 @@ endif() px4_add_module( MODULE modules__uORB MAIN uorb - STACK_MAIN 2048 + STACK_MAIN 2100 COMPILE_FLAGS -Os SRCS ${SRCS} diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index 9990d8d543..fdfb22300c 100644 --- a/src/modules/uORB/Publication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -47,10 +47,10 @@ #include "topics/vehicle_rates_setpoint.h" #include "topics/actuator_outputs.h" #include "topics/actuator_direct.h" -#include "topics/encoders.h" #include "topics/tecs_status.h" #include "topics/rc_channels.h" #include "topics/filtered_bottom_flow.h" +#include "topics/ekf2_innovations.h" #include @@ -118,9 +118,9 @@ template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; -template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; +template class __EXPORT Publication; } diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index 0106d81a41..071254c80e 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -45,10 +45,10 @@ #include "topics/hil_sensor.h" #include "topics/vehicle_attitude.h" #include "topics/vehicle_global_position.h" -#include "topics/encoders.h" #include "topics/position_setpoint_triplet.h" #include "topics/vehicle_status.h" #include "topics/manual_control_setpoint.h" +#include "topics/mavlink_log.h" #include "topics/vehicle_local_position_setpoint.h" #include "topics/vehicle_local_position.h" #include "topics/vehicle_attitude_setpoint.h" @@ -63,6 +63,7 @@ #include "topics/att_pos_mocap.h" #include "topics/vision_position_estimate.h" #include "topics/control_state.h" +#include "topics/vehicle_land_detected.h" #include @@ -144,6 +145,12 @@ void Subscription::update() SubscriptionBase::update((void *)(&_data)); } +template +bool Subscription::check_updated() +{ + return SubscriptionBase::updated(); +} + template const T &Subscription::get() { return _data; } @@ -155,10 +162,10 @@ template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; -template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; +template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; @@ -173,5 +180,6 @@ template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; +template class __EXPORT Subscription; } // namespace uORB diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp index 1d547be842..7b55b4a2a0 100644 --- a/src/modules/uORB/Subscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -185,6 +185,10 @@ public: */ void update(); + /** + * Create an update function that uses the embedded struct. + */ + bool check_updated(); /* * This function gets the T struct data * */ diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp deleted file mode 100644 index a91d6ad45d..0000000000 --- a/src/modules/uORB/objects_common.cpp +++ /dev/null @@ -1,302 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2016 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 objects_common.cpp - * - * Common object definitions without a better home. - */ - -/** - * @defgroup topics List of all uORB topics. - */ - -#include - -#include - -#include "topics/sensor_mag.h" -ORB_DEFINE(sensor_mag, struct sensor_mag_s); - -#include "topics/sensor_accel.h" -ORB_DEFINE(sensor_accel, struct sensor_accel_s); - -#include "topics/sensor_gyro.h" -ORB_DEFINE(sensor_gyro, struct sensor_gyro_s); - -#include "topics/sensor_baro.h" -ORB_DEFINE(sensor_baro, struct sensor_baro_s); - -#include "topics/output_pwm.h" -ORB_DEFINE(output_pwm, struct output_pwm_s); - -#include "topics/input_rc.h" -ORB_DEFINE(input_rc, struct input_rc_s); - -#include "topics/pwm_input.h" -ORB_DEFINE(pwm_input, struct pwm_input_s); - -#include "topics/vehicle_attitude.h" -ORB_DEFINE(vehicle_attitude, struct vehicle_attitude_s); - -#include "topics/control_state.h" -ORB_DEFINE(control_state, struct control_state_s); - -#include "topics/sensor_combined.h" -ORB_DEFINE(sensor_combined, struct sensor_combined_s); - -#include "topics/hil_sensor.h" -ORB_DEFINE(hil_sensor, struct hil_sensor_s); - -#include "topics/vehicle_gps_position.h" -ORB_DEFINE(vehicle_gps_position, struct vehicle_gps_position_s); - -#include "topics/vehicle_land_detected.h" -ORB_DEFINE(vehicle_land_detected, struct vehicle_land_detected_s); - -#include "topics/satellite_info.h" -ORB_DEFINE(satellite_info, struct satellite_info_s); - -#include "topics/home_position.h" -ORB_DEFINE(home_position, struct home_position_s); - -#include "topics/vehicle_status.h" -ORB_DEFINE(vehicle_status, struct vehicle_status_s); - -#include "topics/vtol_vehicle_status.h" -ORB_DEFINE(vtol_vehicle_status, struct vtol_vehicle_status_s); - -#include "topics/safety.h" -ORB_DEFINE(safety, struct safety_s); - -#include "topics/battery_status.h" -ORB_DEFINE(battery_status, struct battery_status_s); - -#include "topics/servorail_status.h" -ORB_DEFINE(servorail_status, struct servorail_status_s); - -#include "topics/system_power.h" -ORB_DEFINE(system_power, struct system_power_s); - -#include "topics/vehicle_global_position.h" -ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s); - -#include "topics/vehicle_local_position.h" -ORB_DEFINE(vehicle_local_position, struct vehicle_local_position_s); - -#include "topics/att_pos_mocap.h" -ORB_DEFINE(att_pos_mocap, struct att_pos_mocap_s); - -#include "topics/vehicle_rates_setpoint.h" -ORB_DEFINE(vehicle_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); - -#include "topics/vehicle_command.h" -ORB_DEFINE(vehicle_command, struct vehicle_command_s); - -#include "topics/vehicle_control_mode.h" -ORB_DEFINE(vehicle_control_mode, struct vehicle_control_mode_s); - -#include "topics/vehicle_local_position_setpoint.h" -ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoint_s); - -#include "topics/position_setpoint_triplet.h" -ORB_DEFINE(position_setpoint_triplet, struct position_setpoint_triplet_s); - -#include "topics/vehicle_global_velocity_setpoint.h" -ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s); - -#include "topics/mission.h" -ORB_DEFINE(mission, struct mission_s); -// XXX onboard and offboard mission are still declared here until this is -// generator supported -#include -ORB_DEFINE(offboard_mission, struct mission_s); -ORB_DEFINE(onboard_mission, struct mission_s); - -#include "topics/mission_result.h" -ORB_DEFINE(mission_result, struct mission_result_s); - -#include "topics/geofence_result.h" -ORB_DEFINE(geofence_result, struct geofence_result_s); - -#include "topics/fence.h" -ORB_DEFINE(fence, struct fence_s); - -#include "topics/fence_vertex.h" -ORB_DEFINE(fence_vertex, struct fence_vertex_s); - -#include "topics/vehicle_attitude_setpoint.h" -ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s); - -#include "topics/mc_virtual_attitude_setpoint.h" -ORB_DEFINE(mc_virtual_attitude_setpoint, struct mc_virtual_attitude_setpoint_s); - -#include "topics/fw_virtual_attitude_setpoint.h" -ORB_DEFINE(fw_virtual_attitude_setpoint, struct fw_virtual_attitude_setpoint_s); - -#include "topics/manual_control_setpoint.h" -ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s); - -#include "topics/offboard_control_mode.h" -ORB_DEFINE(offboard_control_mode, struct offboard_control_mode_s); - -#include "topics/optical_flow.h" -ORB_DEFINE(optical_flow, struct optical_flow_s); - -#include "topics/filtered_bottom_flow.h" -ORB_DEFINE(filtered_bottom_flow, struct filtered_bottom_flow_s); - -#include "topics/airspeed.h" -ORB_DEFINE(airspeed, struct airspeed_s); - -#include "topics/differential_pressure.h" -ORB_DEFINE(differential_pressure, struct differential_pressure_s); - -#include "topics/subsystem_info.h" -ORB_DEFINE(subsystem_info, struct subsystem_info_s); - -/* actuator controls, as requested by controller */ -#include "topics/actuator_controls.h" -#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 -#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); - -#include "topics/actuator_outputs.h" -ORB_DEFINE(actuator_outputs, struct actuator_outputs_s); - -#include "topics/actuator_direct.h" -ORB_DEFINE(actuator_direct, struct actuator_direct_s); - -#include "topics/multirotor_motor_limits.h" -ORB_DEFINE(multirotor_motor_limits, struct multirotor_motor_limits_s); - -#include "topics/telemetry_status.h" -ORB_DEFINE(telemetry_status, struct telemetry_status_s); - -#include "topics/test_motor.h" -ORB_DEFINE(test_motor, struct test_motor_s); - -#include "topics/debug_key_value.h" -ORB_DEFINE(debug_key_value, struct debug_key_value_s); - -#include "topics/navigation_capabilities.h" -ORB_DEFINE(navigation_capabilities, struct navigation_capabilities_s); - -#include "topics/esc_status.h" -ORB_DEFINE(esc_status, struct esc_status_s); - -#include "topics/esc_report.h" -ORB_DEFINE(esc_report, struct esc_report_s); - -#include "topics/encoders.h" -ORB_DEFINE(encoders, struct encoders_s); - -#include "topics/estimator_status.h" -ORB_DEFINE(estimator_status, struct estimator_status_s); - -#include "topics/vision_position_estimate.h" -ORB_DEFINE(vision_position_estimate, struct vision_position_estimate_s); - -#include "topics/vehicle_force_setpoint.h" -ORB_DEFINE(vehicle_force_setpoint, struct vehicle_force_setpoint_s); - -#include "topics/tecs_status.h" -ORB_DEFINE(tecs_status, struct tecs_status_s); - -#include "topics/wind_estimate.h" -ORB_DEFINE(wind_estimate, struct wind_estimate_s); - -#include "topics/rc_parameter_map.h" -ORB_DEFINE(rc_parameter_map, struct rc_parameter_map_s); - -#include "topics/time_offset.h" -ORB_DEFINE(time_offset, struct time_offset_s); - -#include "topics/mc_att_ctrl_status.h" -ORB_DEFINE(mc_att_ctrl_status, struct mc_att_ctrl_status_s); - -#include "topics/distance_sensor.h" -ORB_DEFINE(distance_sensor, struct distance_sensor_s); - -#include "topics/camera_trigger.h" -ORB_DEFINE(camera_trigger, struct camera_trigger_s); - -#include "topics/vehicle_command_ack.h" -ORB_DEFINE(vehicle_command_ack, struct vehicle_command_ack_s); - -#include "topics/ekf2_innovations.h" -ORB_DEFINE(ekf2_innovations, struct ekf2_innovations_s); - -#include "topics/ekf2_replay.h" -ORB_DEFINE(ekf2_replay, struct ekf2_replay_s); - -#include "topics/qshell_req.h" -ORB_DEFINE(qshell_req, struct qshell_req_s); - -#include "topics/mavlink_log.h" -ORB_DEFINE(mavlink_log, struct mavlink_log_s); - -#include "topics/follow_target.h" -ORB_DEFINE(follow_target, struct follow_target_s); - -#include "topics/commander_state.h" -ORB_DEFINE(commander_state, struct commander_state_s); - -#include "topics/transponder_report.h" -ORB_DEFINE(transponder_report, struct transponder_report_s); - -#include "topics/gps_inject_data.h" -ORB_DEFINE(gps_inject_data, struct gps_inject_data_s); - -#include "topics/adc_report.h" -ORB_DEFINE(adc_report, struct adc_report_s); diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index 375f261ca8..8a0bf0bdfc 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -45,12 +45,23 @@ orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) return uORB::Manager::get_instance()->orb_advertise(meta, data); } +orb_advert_t orb_advertise_queue(const struct orb_metadata *meta, const void *data, unsigned int queue_size) +{ + return uORB::Manager::get_instance()->orb_advertise(meta, data, queue_size); +} + orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority) { return uORB::Manager::get_instance()->orb_advertise_multi(meta, data, instance, priority); } +orb_advert_t orb_advertise_multi_queue(const struct orb_metadata *meta, const void *data, int *instance, + int priority, unsigned int queue_size) +{ + return uORB::Manager::get_instance()->orb_advertise_multi(meta, data, instance, priority, queue_size); +} + int orb_unadvertise(orb_advert_t handle) { return uORB::Manager::get_instance()->orb_unadvertise(handle); @@ -129,8 +140,12 @@ int orb_priority(int handle, int32_t *priority) return uORB::Manager::get_instance()->orb_priority(handle, priority); } -int orb_set_interval(int handle, unsigned interval) +int orb_set_interval(int handle, unsigned interval) { return uORB::Manager::get_instance()->orb_set_interval(handle, interval); } +int orb_get_interval(int handle, unsigned *interval) +{ + return uORB::Manager::get_instance()->orb_get_interval(handle, interval); +} diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index 807da7b75b..9f7e92f65d 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -52,6 +52,8 @@ struct orb_metadata { const char *o_name; /**< unique object name */ const size_t o_size; /**< object size */ + const size_t o_size_no_padding; /**< object size w/o padding at the end (for logger) */ + const char *o_fields; /**< semicolon separated list of fields (with type) */ }; typedef const struct orb_metadata *orb_id_t; @@ -87,16 +89,14 @@ enum ORB_PRIO { #define ORB_ID(_name) &__orb_##_name /** - * Declare (prototype) the uORB metadata for a topic. + * Declare (prototype) the uORB metadata for a topic (used by code generators). * * @param _name The name of the topic. */ #if defined(__cplusplus) # define ORB_DECLARE(_name) extern "C" const struct orb_metadata __orb_##_name __EXPORT -# define ORB_DECLARE_OPTIONAL(_name) extern "C" const struct orb_metadata __orb_##_name __EXPORT #else # define ORB_DECLARE(_name) extern const struct orb_metadata __orb_##_name __EXPORT -# define ORB_DECLARE_OPTIONAL(_name) extern const struct orb_metadata __orb_##_name __EXPORT #endif /** @@ -110,11 +110,15 @@ enum ORB_PRIO { * * @param _name The name of the topic. * @param _struct The structure the topic provides. + * @param _size_no_padding Struct size w/o padding at the end + * @param _fields All fields in a semicolon separated list e.g: "float[3] position;bool armed" */ -#define ORB_DEFINE(_name, _struct) \ +#define ORB_DEFINE(_name, _struct, _size_no_padding, _fields) \ const struct orb_metadata __orb_##_name = { \ #_name, \ - sizeof(_struct) \ + sizeof(_struct), \ + _size_no_padding, \ + _fields \ }; struct hack __BEGIN_DECLS @@ -136,12 +140,24 @@ typedef void *orb_advert_t; */ extern orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) __EXPORT; +/** + * @see uORB::Manager::orb_advertise() + */ +extern orb_advert_t orb_advertise_queue(const struct orb_metadata *meta, const void *data, + unsigned int queue_size) __EXPORT; + /** * @see uORB::Manager::orb_advertise_multi() */ extern orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority) __EXPORT; +/** + * @see uORB::Manager::orb_advertise_multi() + */ +extern orb_advert_t orb_advertise_multi_queue(const struct orb_metadata *meta, const void *data, int *instance, + int priority, unsigned int queue_size) __EXPORT; + /** * @see uORB::Manager::orb_unadvertise() */ @@ -184,7 +200,7 @@ extern int orb_unsubscribe(int handle) __EXPORT; extern int orb_copy(const struct orb_metadata *meta, int handle, void *buffer) __EXPORT; /** - * @see uORB::Manager::orb_advertise() + * @see uORB::Manager::orb_check() */ extern int orb_check(int handle, bool *updated) __EXPORT; @@ -216,6 +232,11 @@ extern int orb_priority(int handle, int32_t *priority) __EXPORT; */ extern int orb_set_interval(int handle, unsigned interval) __EXPORT; +/** + * @see uORB::Manager::orb_get_interval() + */ +extern int orb_get_interval(int handle, unsigned *interval) __EXPORT; + __END_DECLS /* Diverse uORB header defines */ //XXX: move to better location diff --git a/src/modules/uORB/uORBDevices_nuttx.cpp b/src/modules/uORB/uORBDevices_nuttx.cpp index 19b814e699..93eeaa8d92 100644 --- a/src/modules/uORB/uORBDevices_nuttx.cpp +++ b/src/modules/uORB/uORBDevices_nuttx.cpp @@ -50,7 +50,8 @@ uORB::DeviceNode::DeviceNode const struct orb_metadata *meta, const char *name, const char *path, - int priority + int priority, + unsigned int queue_size ) : CDev(name, path), _meta(meta), @@ -60,6 +61,7 @@ uORB::DeviceNode::DeviceNode _publisher(0), _priority(priority), _published(false), + _queue_size(queue_size), _IsRemoteSubscriberPresent(false), _subscriber_count(0) { @@ -125,7 +127,7 @@ uORB::DeviceNode::open(struct file *filp) sd->generation = _generation; /* set priority */ - sd->priority = _priority; + sd->set_priority(_priority); filp->f_priv = (void *)sd; @@ -155,7 +157,10 @@ uORB::DeviceNode::close(struct file *filp) SubscriberData *sd = filp_to_sd(filp); if (sd != nullptr) { - hrt_cancel(&sd->update_call); + if (sd->update_interval) { + hrt_cancel(&sd->update_interval->update_call); + } + remove_internal_subscriber(); delete sd; sd = nullptr; @@ -183,26 +188,39 @@ uORB::DeviceNode::read(struct file *filp, char *buffer, size_t buflen) /* * Perform an atomic copy & state update */ - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); + + if (_generation > sd->generation + _queue_size) { + /* Reader is too far behind: some messages are lost */ + sd->generation = _generation - _queue_size; + } + + if (_generation == sd->generation && sd->generation > 0) { + /* The subscriber already read the latest message, but nothing new was published yet. + * Return the previous message + */ + --sd->generation; + } /* if the caller doesn't want the data, don't give it to them */ if (nullptr != buffer) { - memcpy(buffer, _data, _meta->o_size); + memcpy(buffer, _data + (_meta->o_size * (sd->generation % _queue_size)), _meta->o_size); } - /* track the last generation that the file has seen */ - sd->generation = _generation; + if (sd->generation < _generation) { + ++sd->generation; + } /* set priority */ - sd->priority = _priority; + sd->set_priority(_priority); /* * Clear the flag that indicates that an update has been reported, as * we have just collected it. */ - sd->update_reported = false; + sd->set_update_reported(false); - irqrestore(flags); + px4_leave_critical_section(flags); return _meta->o_size; } @@ -226,7 +244,7 @@ uORB::DeviceNode::write(struct file *filp, const char *buffer, size_t buflen) /* re-check size */ if (nullptr == _data) { - _data = new uint8_t[_meta->o_size]; + _data = new uint8_t[_meta->o_size * _queue_size]; } unlock(); @@ -244,16 +262,17 @@ uORB::DeviceNode::write(struct file *filp, const char *buffer, size_t buflen) } /* Perform an atomic copy. */ - irqstate_t flags = irqsave(); - memcpy(_data, buffer, _meta->o_size); + irqstate_t flags = px4_enter_critical_section(); + memcpy(_data + (_meta->o_size * (_generation % _queue_size)), buffer, _meta->o_size); /* update the timestamp and generation count */ _last_update = hrt_absolute_time(); + /* wrap-around happens after ~49 days, assuming a publisher rate of 1 kHz */ _generation++; _published = true; - irqrestore(flags); + px4_leave_critical_section(flags); /* notify any poll waiters */ poll_notify(POLLIN); @@ -268,9 +287,9 @@ uORB::DeviceNode::ioctl(struct file *filp, int cmd, unsigned long arg) switch (cmd) { case ORBIOCLASTUPDATE: { - irqstate_t state = irqsave(); + irqstate_t state = px4_enter_critical_section(); *(hrt_abstime *)arg = _last_update; - irqrestore(state); + px4_leave_critical_section(state); return OK; } @@ -278,16 +297,58 @@ uORB::DeviceNode::ioctl(struct file *filp, int cmd, unsigned long arg) *(bool *)arg = appears_updated(sd); return OK; - case ORBIOCSETINTERVAL: - sd->update_interval = arg; - return OK; + case ORBIOCSETINTERVAL: { + int ret = PX4_OK; + lock(); + + if (arg == 0) { + if (sd->update_interval) { + delete(sd->update_interval); + sd->update_interval = nullptr; + } + + } else { + if (sd->update_interval) { + sd->update_interval->interval = arg; + + } else { + sd->update_interval = new UpdateIntervalData(); + + if (sd->update_interval) { + memset(&sd->update_interval->update_call, 0, sizeof(hrt_call)); + sd->update_interval->interval = arg; + + } else { + ret = -ENOMEM; + } + } + } + + unlock(); + return ret; + } case ORBIOCGADVERTISER: *(uintptr_t *)arg = (uintptr_t)this; return OK; case ORBIOCGPRIORITY: - *(int *)arg = sd->priority; + *(int *)arg = sd->priority(); + return OK; + + case ORBIOCSETQUEUESIZE: + //no need for locking here, since this is used only during the advertisement call, + //and only one advertiser is allowed to open the DeviceNode at the same time. + return update_queue_size(arg); + + case ORBIOCGETINTERVAL: + if (sd->update_interval) { + *(unsigned *)arg = sd->update_interval->interval; + + } else { + *(unsigned *)arg = 0; + } + return OK; default: @@ -400,7 +461,7 @@ uORB::DeviceNode::appears_updated(SubscriberData *sd) bool ret = false; /* avoid racing between interrupt and non-interrupt context calls */ - irqstate_t state = irqsave(); + irqstate_t state = px4_enter_critical_section(); /* check if this topic has been published yet, if not bail out */ if (_data == nullptr) { @@ -418,7 +479,7 @@ uORB::DeviceNode::appears_updated(SubscriberData *sd) /* * Handle non-rate-limited subscribers. */ - if (sd->update_interval == 0) { + if (sd->update_interval == nullptr) { ret = true; break; } @@ -430,7 +491,7 @@ uORB::DeviceNode::appears_updated(SubscriberData *sd) * behaviour where checking / polling continues to report an update * until the topic is read. */ - if (sd->update_reported) { + if (sd->update_reported()) { ret = true; break; } @@ -442,7 +503,7 @@ uORB::DeviceNode::appears_updated(SubscriberData *sd) * must have collected the update we reported, otherwise * update_reported would still be true. */ - if (!hrt_called(&sd->update_call)) { + if (!hrt_called(&sd->update_interval->update_call)) { break; } @@ -451,22 +512,22 @@ uORB::DeviceNode::appears_updated(SubscriberData *sd) * until the interval has passed once more by restarting the interval * timer and thereby re-scheduling a poll notification at that time. */ - hrt_call_after(&sd->update_call, - sd->update_interval, + hrt_call_after(&sd->update_interval->update_call, + sd->update_interval->interval, &uORB::DeviceNode::update_deferred_trampoline, (void *)this); /* * Remember that we have told the subscriber that there is data. */ - sd->update_reported = true; + sd->set_update_reported(true); ret = true; break; } out: - irqrestore(state); + px4_leave_critical_section(state); /* consider it updated */ return ret; @@ -522,6 +583,20 @@ bool uORB::DeviceNode::is_published() return _published; } +int uORB::DeviceNode::update_queue_size(unsigned int queue_size) +{ + if (_queue_size == queue_size) { + return PX4_OK; + } + + if (_data || _queue_size > queue_size) { + return ERROR; + } + + _queue_size = queue_size; + return PX4_OK; +} + //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- int16_t uORB::DeviceNode::process_add_subscription(int32_t rateInHz) @@ -634,18 +709,12 @@ uORB::DeviceMaster::ioctl(struct file *filp, int cmd, unsigned long arg) *(adv->instance) = group_tries; } - /* driver wants a permanent copy of the node name, so make one here */ - objname = strdup(meta->o_name); - - if (objname == nullptr) { - return -ENOMEM; - } + objname = meta->o_name; //no need for a copy, meta->o_name will never be freed or changed /* driver wants a permanent copy of the path, so make one here */ devpath = strdup(nodepath); if (devpath == nullptr) { - free((void *)objname); return -ENOMEM; } @@ -654,7 +723,6 @@ uORB::DeviceMaster::ioctl(struct file *filp, int cmd, unsigned long arg) /* if we didn't get a device, that's bad */ if (node == nullptr) { - free((void *)objname); free((void *)devpath); return -ENOMEM; } @@ -681,7 +749,6 @@ uORB::DeviceMaster::ioctl(struct file *filp, int cmd, unsigned long arg) } /* also discard the name now */ - free((void *)objname); free((void *)devpath); } else { diff --git a/src/modules/uORB/uORBDevices_nuttx.hpp b/src/modules/uORB/uORBDevices_nuttx.hpp index 1519b06a82..2ad16ee3c7 100644 --- a/src/modules/uORB/uORBDevices_nuttx.hpp +++ b/src/modules/uORB/uORBDevices_nuttx.hpp @@ -61,7 +61,8 @@ public: const struct orb_metadata *meta, const char *name, const char *path, - int priority + int priority, + unsigned int queue_size = 1 ); /** @@ -168,18 +169,36 @@ public: * and publish to this node or if another node should be tried. */ bool is_published(); + /** + * Try to change the size of the queue. This can only be done as long as nobody published yet. + * This is the case, for example when orb_subscribe was called before an orb_advertise. + * The queue size can only be increased. + * @param queue_size new size of the queue + * @return PX4_OK if queue size successfully set + */ + int update_queue_size(unsigned int queue_size); + protected: virtual pollevent_t poll_state(struct file *filp); virtual void poll_notify_one(struct pollfd *fds, pollevent_t events); private: - struct SubscriberData { - unsigned generation; /**< last generation the subscriber has seen */ - unsigned update_interval; /**< if nonzero minimum interval between updates */ + struct UpdateIntervalData { + unsigned interval; /**< if nonzero minimum interval between updates */ struct hrt_call update_call; /**< deferred wakeup call if update_period is nonzero */ - void *poll_priv; /**< saved copy of fds->f_priv while poll is active */ - bool update_reported; /**< true if we have reported the update via poll/check */ - int priority; /**< priority of publisher */ + }; + struct SubscriberData { + ~SubscriberData() { if (update_interval) { delete(update_interval); } } + + unsigned generation; /**< last generation the subscriber has seen */ + int flags; /**< lowest 8 bits: priority of publisher, 9. bit: update_reported bit */ + UpdateIntervalData *update_interval; /**< if null, no update interval */ + + int priority() const { return flags & 0xff; } + void set_priority(uint8_t prio) { flags = (flags & ~0xff) | prio; } + + bool update_reported() const { return flags & (1 << 8); } + void set_update_reported(bool update_reported_flag) { flags = (flags & ~(1 << 8)) | (((int)update_reported_flag) << 8); } }; const struct orb_metadata *_meta; /**< object metadata information */ @@ -190,6 +209,7 @@ private: We allow one publisher to have an open file descriptor at the same time. */ const int _priority; /**< priority of topic */ bool _published; /**< has ever data been published */ + unsigned int _queue_size; /**< maximum number of elements in the queue */ private: // private class methods. diff --git a/src/modules/uORB/uORBDevices_posix.cpp b/src/modules/uORB/uORBDevices_posix.cpp index d641859875..a7c7d3cfe6 100644 --- a/src/modules/uORB/uORBDevices_posix.cpp +++ b/src/modules/uORB/uORBDevices_posix.cpp @@ -61,7 +61,8 @@ uORB::DeviceNode::SubscriberData *uORB::DeviceNode::filp_to_sd(device::file_t * return sd; } -uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const char *name, const char *path, int priority) : +uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const char *name, const char *path, + int priority, unsigned int queue_size) : VDev(name, path), _meta(meta), _data(nullptr), @@ -70,6 +71,7 @@ uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const char *name, _publisher(0), _priority(priority), _published(false), + _queue_size(queue_size), _subscriber_count(0) { // enable debug() calls @@ -134,7 +136,7 @@ uORB::DeviceNode::open(device::file_t *filp) sd->generation = _generation; /* set priority */ - sd->priority = _priority; + sd->set_priority(_priority); filp->priv = (void *)sd; @@ -167,7 +169,10 @@ uORB::DeviceNode::close(device::file_t *filp) SubscriberData *sd = filp_to_sd(filp); if (sd != nullptr) { - hrt_cancel(&sd->update_call); + if (sd->update_interval) { + hrt_cancel(&sd->update_interval->update_call); + } + remove_internal_subscriber(); delete sd; sd = nullptr; @@ -198,22 +203,35 @@ uORB::DeviceNode::read(device::file_t *filp, char *buffer, size_t buflen) */ lock(); - /* if the caller doesn't want the data, don't give it to them */ - if (nullptr != buffer) { - memcpy(buffer, _data, _meta->o_size); + if (_generation > sd->generation + _queue_size) { + /* Reader is too far behind: some messages are lost */ + sd->generation = _generation - _queue_size; } - /* track the last generation that the file has seen */ - sd->generation = _generation; + if (_generation == sd->generation && sd->generation > 0) { + /* The subscriber already read the latest message, but nothing new was published yet. + * Return the previous message + */ + --sd->generation; + } + + /* if the caller doesn't want the data, don't give it to them */ + if (nullptr != buffer) { + memcpy(buffer, _data + (_meta->o_size * (sd->generation % _queue_size)), _meta->o_size); + } + + if (sd->generation < _generation) { + ++sd->generation; + } /* set priority */ - sd->priority = _priority; + sd->set_priority(_priority); /* * Clear the flag that indicates that an update has been reported, as * we have just collected it. */ - sd->update_reported = false; + sd->set_update_reported(false); unlock(); @@ -238,7 +256,7 @@ uORB::DeviceNode::write(device::file_t *filp, const char *buffer, size_t buflen) /* re-check size */ if (nullptr == _data) { - _data = new uint8_t[_meta->o_size]; + _data = new uint8_t[_meta->o_size * _queue_size]; } unlock(); @@ -255,10 +273,11 @@ uORB::DeviceNode::write(device::file_t *filp, const char *buffer, size_t buflen) } lock(); - memcpy(_data, buffer, _meta->o_size); + memcpy(_data + (_meta->o_size * (_generation % _queue_size)), buffer, _meta->o_size); /* update the timestamp and generation count */ _last_update = hrt_absolute_time(); + /* wrap-around happens after ~49 days, assuming a publisher rate of 1 kHz */ _generation++; _published = true; @@ -290,21 +309,62 @@ uORB::DeviceNode::ioctl(device::file_t *filp, int cmd, unsigned long arg) unlock(); return PX4_OK; - case ORBIOCSETINTERVAL: - lock(); - sd->update_interval = arg; - sd->last_update = hrt_absolute_time(); - unlock(); - return PX4_OK; + case ORBIOCSETINTERVAL: { + int ret = PX4_OK; + lock(); + + if (arg == 0) { + if (sd->update_interval) { + delete(sd->update_interval); + sd->update_interval = nullptr; + } + + } else { + if (sd->update_interval) { + sd->update_interval->interval = arg; + sd->update_interval->last_update = hrt_absolute_time(); + + } else { + sd->update_interval = new UpdateIntervalData(); + + if (sd->update_interval) { + memset(&sd->update_interval->update_call, 0, sizeof(hrt_call)); + sd->update_interval->interval = arg; + sd->update_interval->last_update = hrt_absolute_time(); + + } else { + ret = -ENOMEM; + } + } + } + + unlock(); + return ret; + } case ORBIOCGADVERTISER: *(uintptr_t *)arg = (uintptr_t)this; return PX4_OK; case ORBIOCGPRIORITY: - *(int *)arg = sd->priority; + *(int *)arg = sd->priority(); return PX4_OK; + case ORBIOCSETQUEUESIZE: + //no need for locking here, since this is used only during the advertisement call, + //and only one advertiser is allowed to open the DeviceNode at the same time. + return update_queue_size(arg); + + case ORBIOCGETINTERVAL: + if (sd->update_interval) { + *(unsigned *)arg = sd->update_interval->interval; + + } else { + *(unsigned *)arg = 0; + } + + return OK; + default: /* give it to the superclass */ return VDev::ioctl(filp, cmd, arg); @@ -441,7 +501,7 @@ uORB::DeviceNode::appears_updated(SubscriberData *sd) /* * Handle non-rate-limited subscribers. */ - if (sd->update_interval == 0) { + if (sd->update_interval == nullptr) { ret = true; break; } @@ -453,22 +513,22 @@ uORB::DeviceNode::appears_updated(SubscriberData *sd) * behaviour where checking / polling continues to report an update * until the topic is read. */ - if (sd->update_reported) { + if (sd->update_reported()) { ret = true; break; } // If we have not yet reached the deadline, then assume that we can ignore any // newly received data. - if (sd->last_update + sd->update_interval > hrt_absolute_time()) { + if (sd->update_interval->last_update + sd->update_interval->interval > hrt_absolute_time()) { break; } /* * Remember that we have told the subscriber that there is data. */ - sd->update_reported = true; - sd->last_update = hrt_absolute_time(); + sd->set_update_reported(true); + sd->update_interval->last_update = hrt_absolute_time(); ret = true; break; @@ -527,6 +587,20 @@ bool uORB::DeviceNode::is_published() return _published; } +int uORB::DeviceNode::update_queue_size(unsigned int queue_size) +{ + if (_queue_size == queue_size) { + return PX4_OK; + } + + if (_data || _queue_size > queue_size) { + return ERROR; + } + + _queue_size = queue_size; + return PX4_OK; +} + //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- int16_t uORB::DeviceNode::process_add_subscription(int32_t rateInHz) @@ -640,18 +714,12 @@ uORB::DeviceMaster::ioctl(device::file_t *filp, int cmd, unsigned long arg) *(adv->instance) = group_tries; } - /* driver wants a permanent copy of the node name, so make one here */ - objname = strdup(meta->o_name); - - if (objname == nullptr) { - return -ENOMEM; - } + objname = meta->o_name; //no need for a copy, meta->o_name will never be freed or changed /* driver wants a permanent copy of the path, so make one here */ devpath = strdup(nodepath); if (devpath == nullptr) { - free((void *)objname); return -ENOMEM; } @@ -660,7 +728,6 @@ uORB::DeviceMaster::ioctl(device::file_t *filp, int cmd, unsigned long arg) /* if we didn't get a device, that's bad */ if (node == nullptr) { - free((void *)objname); free((void *)devpath); return -ENOMEM; } @@ -687,7 +754,6 @@ uORB::DeviceMaster::ioctl(device::file_t *filp, int cmd, unsigned long arg) } /* also discard the name now */ - free((void *)objname); free((void *)devpath); } else { diff --git a/src/modules/uORB/uORBDevices_posix.hpp b/src/modules/uORB/uORBDevices_posix.hpp index 963ed4a79d..7fad8b5e7f 100644 --- a/src/modules/uORB/uORBDevices_posix.hpp +++ b/src/modules/uORB/uORBDevices_posix.hpp @@ -48,7 +48,8 @@ class DeviceMaster; class uORB::DeviceNode : public device::VDev { public: - DeviceNode(const struct orb_metadata *meta, const char *name, const char *path, int priority); + DeviceNode(const struct orb_metadata *meta, const char *name, const char *path, + int priority, unsigned int queue_size = 1); ~DeviceNode(); virtual int open(device::file_t *filp); @@ -105,19 +106,37 @@ public: * and publish to this node or if another node should be tried. */ bool is_published(); + /** + * Try to change the size of the queue. This can only be done as long as nobody published yet. + * This is the case, for example when orb_subscribe was called before an orb_advertise. + * The queue size can only be increased. + * @param queue_size new size of the queue + * @return PX4_OK if queue size successfully set + */ + int update_queue_size(unsigned int queue_size); + protected: virtual pollevent_t poll_state(device::file_t *filp); virtual void poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events); private: - struct SubscriberData { - unsigned generation; /**< last generation the subscriber has seen */ - unsigned update_interval; /**< if nonzero minimum interval between updates */ + struct UpdateIntervalData { + unsigned interval; /**< if nonzero minimum interval between updates */ uint64_t last_update; /**< time at which the last update was provided, used when update_interval is nonzero */ struct hrt_call update_call; /**< deferred wakeup call if update_period is nonzero */ - void *poll_priv; /**< saved copy of fds->f_priv while poll is active */ - bool update_reported; /**< true if we have reported the update via poll/check */ - int priority; /**< priority of publisher */ + }; + struct SubscriberData { + ~SubscriberData() { if (update_interval) { delete(update_interval); } } + + unsigned generation; /**< last generation the subscriber has seen */ + int flags; /**< lowest 8 bits: priority of publisher, 9. bit: update_reported bit */ + UpdateIntervalData *update_interval; /**< if null, no update interval */ + + int priority() const { return flags & 0xff; } + void set_priority(uint8_t prio) { flags = (flags & ~0xff) | prio; } + + bool update_reported() const { return flags & (1 << 8); } + void set_update_reported(bool update_reported_flag) { flags = (flags & ~(1 << 8)) | (((int)update_reported_flag) << 8); } }; const struct orb_metadata *_meta; /**< object metadata information */ @@ -128,6 +147,7 @@ private: We allow one publisher to have an open file descriptor at the same time. */ const int _priority; /**< priority of topic */ bool _published; /**< has ever data been published */ + unsigned int _queue_size; /**< maximum number of elements in the queue */ static SubscriberData *filp_to_sd(device::file_t *filp); diff --git a/src/modules/uORB/uORBMain.cpp b/src/modules/uORB/uORBMain.cpp index cdc18e23bd..66b98debf2 100644 --- a/src/modules/uORB/uORBMain.cpp +++ b/src/modules/uORB/uORBMain.cpp @@ -37,19 +37,14 @@ #include "uORB.h" #include "uORBCommon.hpp" -#ifndef __PX4_QURT -#include "uORBTest_UnitTest.hpp" -#endif - extern "C" { __EXPORT int uorb_main(int argc, char *argv[]); } static uORB::DeviceMaster *g_dev = nullptr; static void usage() { - PX4_INFO("Usage: uorb 'start', 'test', 'latency_test' or 'status'"); + PX4_INFO("Usage: uorb 'start', 'status'"); } - int uorb_main(int argc, char *argv[]) { @@ -94,41 +89,6 @@ uorb_main(int argc, char *argv[]) return OK; } -#ifndef __PX4_QURT - - /* - * Test the driver/device. - */ - if (!strcmp(argv[1], "test")) { - if (!g_dev) { - PX4_WARN("orb is not running! start it first"); - return -ESRCH; - } - - uORBTest::UnitTest &t = uORBTest::UnitTest::instance(); - return t.test(); - } - - /* - * Test the latency. - */ - if (!strcmp(argv[1], "latency_test")) { - - uORBTest::UnitTest &t = uORBTest::UnitTest::instance(); - - if (argc > 2 && !strcmp(argv[2], "medium")) { - return t.latency_test(ORB_ID(orb_test_medium), true); - - } else if (argc > 2 && !strcmp(argv[2], "large")) { - return t.latency_test(ORB_ID(orb_test_large), true); - - } else { - return t.latency_test(ORB_ID(orb_test), true); - } - } - -#endif - /* * Print driver information. */ diff --git a/src/modules/uORB/uORBManager.cpp b/src/modules/uORB/uORBManager.cpp index c1266ba04a..05f88dd6f2 100644 --- a/src/modules/uORB/uORBManager.cpp +++ b/src/modules/uORB/uORBManager.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include "uORBUtils.hpp" #include "uORBManager.hpp" #include "px4_config.h" @@ -63,6 +64,21 @@ bool uORB::Manager::initialize() uORB::Manager::Manager() : _comm_channel(nullptr) { + +#ifdef ORB_USE_PUBLISHER_RULES + const char *file_name = "./rootfs/orb_publisher.rules"; + int ret = readPublisherRulesFromFile(file_name, _publisher_rule); + + if (ret == PX4_OK) { + _has_publisher_rules = true; + PX4_INFO("Using orb rules from %s", file_name); + + } else { + PX4_ERR("Failed to read publisher rules file %s (%s)", file_name, strerror(-ret)); + } + +#endif /* ORB_USE_PUBLISHER_RULES */ + } int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance) @@ -87,15 +103,39 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance) #endif } -orb_advert_t uORB::Manager::orb_advertise(const struct orb_metadata *meta, const void *data) +orb_advert_t uORB::Manager::orb_advertise(const struct orb_metadata *meta, const void *data, unsigned int queue_size) { //warnx("orb_advertise meta = %p", meta); - return orb_advertise_multi(meta, data, nullptr, ORB_PRIO_DEFAULT); + return orb_advertise_multi(meta, data, nullptr, ORB_PRIO_DEFAULT, queue_size); } orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, - int priority) + int priority, unsigned int queue_size) { +#ifdef ORB_USE_PUBLISHER_RULES + + // check publisher rule + if (_has_publisher_rules) { + const char *prog_name = px4_get_taskname(); + + if (strcmp(_publisher_rule.module_name, prog_name) == 0) { + if (_publisher_rule.ignore_other_topics) { + if (!findTopic(_publisher_rule, meta->o_name)) { + PX4_DEBUG("not allowing %s to publish topic %s", prog_name, meta->o_name); + return (orb_advert_t)_Instance; + } + } + + } else { + if (findTopic(_publisher_rule, meta->o_name)) { + PX4_DEBUG("not allowing %s to publish topic %s", prog_name, meta->o_name); + return (orb_advert_t)_Instance; + } + } + } + +#endif /* ORB_USE_PUBLISHER_RULES */ + int result, fd; orb_advert_t advertiser; @@ -109,6 +149,15 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, return nullptr; } + /* Set the queue size. This must be done before the first publication; thus it fails if + * this is not the first advertiser. + */ + result = px4_ioctl(fd, ORBIOCSETQUEUESIZE, (unsigned long)queue_size); + + if (result < 0 && queue_size > 1) { + PX4_WARN("orb_advertise_multi: failed to set queue size"); + } + /* get the advertiser handle and close the node */ result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser); px4_close(fd); @@ -131,6 +180,14 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, int uORB::Manager::orb_unadvertise(orb_advert_t handle) { +#ifdef ORB_USE_PUBLISHER_RULES + + if (handle == _Instance) { + return PX4_OK; //pretend success + } + +#endif /* ORB_USE_PUBLISHER_RULES */ + return uORB::DeviceNode::unadvertise(handle); } @@ -152,6 +209,14 @@ int uORB::Manager::orb_unsubscribe(int fd) int uORB::Manager::orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) { +#ifdef ORB_USE_PUBLISHER_RULES + + if (handle == _Instance) { + return PX4_OK; //pretend success + } + +#endif /* ORB_USE_PUBLISHER_RULES */ + return uORB::DeviceNode::publish(meta, handle, data); } @@ -195,6 +260,14 @@ int uORB::Manager::orb_set_interval(int handle, unsigned interval) return px4_ioctl(handle, ORBIOCSETINTERVAL, interval * 1000); } +int uORB::Manager::orb_get_interval(int handle, unsigned *interval) +{ + ASSERT(interval); + int ret = px4_ioctl(handle, ORBIOCGETINTERVAL, (unsigned long)interval); + *interval /= 1000; + return ret; +} + int uORB::Manager::node_advertise ( @@ -335,8 +408,8 @@ uORBCommunicator::IChannel *uORB::Manager::get_uorb_communicator(void) int16_t uORB::Manager::process_add_subscription(const char *messageName, int32_t msgRateInHz) { - warnx("[posix-uORB::Manager::process_add_subscription(%d)] entering Manager_process_add_subscription: name: %s", - __LINE__, messageName); + PX4_DEBUG("[posix-uORB::Manager::process_add_subscription(%d)] entering Manager_process_add_subscription: name: %s", + __LINE__, messageName); int16_t rc = 0; _remote_subscriber_topics.insert(messageName); char nodepath[orb_maxpath]; @@ -347,8 +420,8 @@ int16_t uORB::Manager::process_add_subscription(const char *messageName, uORB::DeviceNode *node = uORB::DeviceMaster::GetDeviceNode(nodepath); if (node == nullptr) { - warnx("[posix-uORB::Manager::process_add_subscription(%d)]DeviceNode(%s) not created yet", - __LINE__, messageName); + PX4_DEBUG("[posix-uORB::Manager::process_add_subscription(%d)]DeviceNode(%s) not created yet", + __LINE__, messageName); } else { // node is present. @@ -379,8 +452,8 @@ int16_t uORB::Manager::process_remove_subscription( // get the node name. if (node == nullptr) { - warnx("[posix-uORB::Manager::process_remove_subscription(%d)]Error No existing subscriber found for message: [%s]", - __LINE__, messageName); + PX4_DEBUG("[posix-uORB::Manager::process_remove_subscription(%d)]Error No existing subscriber found for message: [%s]", + __LINE__, messageName); } else { // node is present. @@ -408,8 +481,8 @@ int16_t uORB::Manager::process_received_message(const char *messageName, // get the node name. if (node == nullptr) { - warnx("[uORB::Manager::process_received_message(%d)]Error No existing subscriber found for message: [%s] nodepath:[%s]", - __LINE__, messageName, nodepath); + PX4_DEBUG("[uORB::Manager::process_received_message(%d)]Error No existing subscriber found for message: [%s] nodepath:[%s]", + __LINE__, messageName, nodepath); } else { // node is present. @@ -429,3 +502,143 @@ bool uORB::Manager::is_remote_subscriber_present(const char *messageName) return (_remote_subscriber_topics.find(messageName) != _remote_subscriber_topics.end()); #endif } + + +#ifdef ORB_USE_PUBLISHER_RULES + +bool uORB::Manager::startsWith(const char *pre, const char *str) +{ + size_t lenpre = strlen(pre), + lenstr = strlen(str); + return lenstr < lenpre ? false : strncmp(pre, str, lenpre) == 0; +} + +bool uORB::Manager::findTopic(const PublisherRule &rule, const char *topic_name) +{ + const char **topics_ptr = rule.topics; + + while (*topics_ptr) { + if (strcmp(*topics_ptr, topic_name) == 0) { + return true; + } + + ++topics_ptr; + } + + return false; +} + +void uORB::Manager::strTrim(const char **str) +{ + while (**str == ' ' || **str == '\t') { ++(*str); } +} + +int uORB::Manager::readPublisherRulesFromFile(const char *file_name, PublisherRule &rule) +{ + FILE *fp; + static const int line_len = 1024; + int ret = PX4_OK; + char *line = new char[line_len]; + + if (!line) { + return -ENOMEM; + } + + fp = fopen(file_name, "r"); + + if (fp == NULL) { + delete[](line); + return -errno; + } + + const char *restrict_topics_str = "restrict_topics:"; + const char *module_str = "module:"; + const char *ignore_others = "ignore_others:"; + + rule.ignore_other_topics = false; + rule.module_name = nullptr; + rule.topics = nullptr; + + while (fgets(line, line_len, fp) && ret == PX4_OK) { + + if (strlen(line) < 2 || line[0] == '#') { + continue; + } + + if (startsWith(restrict_topics_str, line)) { + //read topics list + char *start = line + strlen(restrict_topics_str); + strTrim((const char **)&start); + char *topics = strdup(start); + int topic_len = 0, num_topics = 0; + + for (int i = 0; topics[i]; ++i) { + if (topics[i] == ',' || topics[i] == '\n') { + if (topic_len > 0) { + topics[i] = 0; + ++num_topics; + } + + topic_len = 0; + + } else { + ++topic_len; + } + } + + if (num_topics > 0) { + rule.topics = new const char *[num_topics + 1]; + int topic = 0; + strTrim((const char **)&topics); + rule.topics[topic++] = topics; + + while (topic < num_topics) { + if (*topics == 0) { + ++topics; + strTrim((const char **)&topics); + rule.topics[topic++] = topics; + + } else { + ++topics; + } + } + + rule.topics[num_topics] = nullptr; + } + + } else if (startsWith(module_str, line)) { + //read module name + char *start = line + strlen(module_str); + strTrim((const char **)&start); + int len = strlen(start); + + if (len > 0 && start[len - 1] == '\n') { + start[len - 1] = 0; + } + + rule.module_name = strdup(start); + + } else if (startsWith(ignore_others, line)) { + const char *start = line + strlen(ignore_others); + strTrim(&start); + + if (startsWith("true", start)) { + rule.ignore_other_topics = true; + } + + } else { + PX4_ERR("orb rules file: wrong format: %s", line); + ret = -EINVAL; + } + } + + if (ret == PX4_OK && (!rule.module_name || !rule.topics)) { + PX4_ERR("Wrong format in orb publisher rules file"); + ret = -EINVAL; + } + + delete[](line); + fclose(fp); + return ret; +} +#endif /* ORB_USE_PUBLISHER_RULES */ diff --git a/src/modules/uORB/uORBManager.hpp b/src/modules/uORB/uORBManager.hpp index eb02a42b39..d0e82caa76 100644 --- a/src/modules/uORB/uORBManager.hpp +++ b/src/modules/uORB/uORBManager.hpp @@ -96,13 +96,15 @@ public: * @param data A pointer to the initial data to be published. * For topics updated by interrupt handlers, the advertisement * must be performed from non-interrupt context. + * @param queue_size Maximum number of buffered elements. If this is 1, no queuing is + * used. * @return nullptr on error, otherwise returns an object pointer * that can be used to publish to the topic. * If the topic in question is not known (due to an * ORB_DEFINE with no corresponding ORB_DECLARE) * this function will return nullptr and set errno to ENOENT. */ - orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data); + orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data, unsigned int queue_size = 1); /** * Advertise as the publisher of a topic. @@ -130,6 +132,8 @@ public: * instances, the priority allows the subscriber to prioritize the best * data source as long as its available. The subscriber is responsible to check * and handle different priorities (@see orb_priority()). + * @param queue_size Maximum number of buffered elements. If this is 1, no queuing is + * used. * @return ERROR on error, otherwise returns a handle * that can be used to publish to the topic. * If the topic in question is not known (due to an @@ -137,7 +141,7 @@ public: * this function will return -1 and set errno to ENOENT. */ orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, - int priority) ; + int priority, unsigned int queue_size = 1) ; /** @@ -187,9 +191,6 @@ public: * for the topic. * @return ERROR on error, otherwise returns a handle * that can be used to read and update the topic. - * If the topic in question is not known (due to an - * ORB_DEFINE_OPTIONAL with no corresponding ORB_DECLARE) - * this function will return -1 and set errno to ENOENT. */ int orb_subscribe(const struct orb_metadata *meta) ; @@ -275,7 +276,8 @@ public: int orb_check(int handle, bool *updated) ; /** - * Return the last time that the topic was updated. + * Return the last time that the topic was updated. If a queue is used, it returns + * the timestamp of the latest element in the queue. * * @param handle A handle returned from orb_subscribe. * @param time Returns the absolute time that the topic was updated, or zero if it has @@ -325,6 +327,18 @@ public: */ int orb_set_interval(int handle, unsigned interval) ; + + /** + * Get the minimum interval between which updates are seen for a subscription. + * + * @see orb_set_interval() + * + * @param handle A handle returned from orb_subscribe. + * @param interval The returned interval period in milliseconds. + * @return OK on success, ERROR otherwise with ERRNO set accordingly. + */ + int orb_get_interval(int handle, unsigned *interval); + /** * Method to set the uORBCommunicator::IChannel instance. * @param comm_channel @@ -432,6 +446,46 @@ private: //class methods virtual int16_t process_received_message(const char *messageName, int32_t length, uint8_t *data); + +#ifdef ORB_USE_PUBLISHER_RULES + + struct PublisherRule { + const char **topics; //null-terminated list of topic names + const char *module_name; //only this module is allowed to publish one of the topics + bool ignore_other_topics; + }; + + /** + * test if str starts with pre + */ + bool startsWith(const char *pre, const char *str); + + /** + * find a topic in a rule + */ + bool findTopic(const PublisherRule &rule, const char *topic_name); + + /** + * trim whitespace from the beginning of a string + */ + void strTrim(const char **str); + + /** + * Read publisher rules from a file. It has the format: + * + * restrict_topics: , , + * module: + * [ignore_others:true] + * + * @return 0 on success, <0 otherwise + */ + int readPublisherRulesFromFile(const char *file_name, PublisherRule &rule); + + PublisherRule _publisher_rule; + bool _has_publisher_rules = false; + +#endif /* ORB_USE_PUBLISHER_RULES */ + }; #endif /* _uORBManager_hpp_ */ diff --git a/src/modules/uORB/uORBTopics.h b/src/modules/uORB/uORBTopics.h new file mode 100644 index 0000000000..f7c3a3c865 --- /dev/null +++ b/src/modules/uORB/uORBTopics.h @@ -0,0 +1,50 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * 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. + * + ****************************************************************************/ + +#ifndef MODULES_UORB_UORBTOPICS_H_ +#define MODULES_UORB_UORBTOPICS_H_ + +#include + +/* + * Returns count of all declared topics. + * It is equal to size of array from orb_get_topics() + */ +extern size_t orb_topics_count() __EXPORT; + +/* + * Returns array of topics metadata + */ +extern const struct orb_metadata **orb_get_topics() __EXPORT; + +#endif /* MODULES_UORB_UORBTOPICS_H_ */ diff --git a/src/modules/uORB/uORB_tests/CMakeLists.txt b/src/modules/uORB/uORB_tests/CMakeLists.txt new file mode 100644 index 0000000000..5f88743d3a --- /dev/null +++ b/src/modules/uORB/uORB_tests/CMakeLists.txt @@ -0,0 +1,53 @@ +############################################################################ +# +# Copyright (c) 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. +# +############################################################################ + +# this includes the generated topics directory +include_directories(${CMAKE_CURRENT_BINARY_DIR}) + +set(SRCS uORB_tests_main.cpp) + +if(NOT ${OS} STREQUAL "qurt") + list(APPEND SRCS uORBTest_UnitTest.cpp) +endif() + +px4_add_module( + MODULE modules__uORB__uORB_tests + MAIN uorb_tests + STACK_MAIN 2048 + COMPILE_FLAGS + -Os + SRCS ${SRCS} + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/uORB/uORBTest_UnitTest.cpp b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp similarity index 72% rename from src/modules/uORB/uORBTest_UnitTest.cpp rename to src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp index c91172c107..0ff04ed402 100644 --- a/src/modules/uORB/uORBTest_UnitTest.cpp +++ b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp @@ -32,10 +32,12 @@ ****************************************************************************/ #include "uORBTest_UnitTest.hpp" -#include "uORBCommon.hpp" +#include "../uORBCommon.hpp" #include #include #include +#include +#include uORBTest::UnitTest &uORBTest::UnitTest::instance() { @@ -128,7 +130,7 @@ int uORBTest::UnitTest::pubsublatency_main(void) pubsubtest_passed = true; - if (static_cast(latency_integral / maxruns) > 30.0f) { + if (static_cast(latency_integral / maxruns) > 80.0f) { pubsubtest_res = uORB::ERROR; } else { @@ -164,7 +166,19 @@ int uORBTest::UnitTest::test() return ret; } - return test_multi2(); + ret = test_multi2(); + + if (ret != OK) { + return ret; + } + + ret = test_queue(); + + if (ret != OK) { + return ret; + } + + return test_queue_poll_notify(); } int uORBTest::UnitTest::test_unadvertise() @@ -557,6 +571,227 @@ int uORBTest::UnitTest::test_multi_reversed() return test_note("PASS multi-topic reversed"); } +int uORBTest::UnitTest::test_queue() +{ + test_note("Testing orb queuing"); + + struct orb_test_medium t, u; + int sfd; + orb_advert_t ptopic; + bool updated; + + sfd = orb_subscribe(ORB_ID(orb_test_medium_queue)); + + if (sfd < 0) { + return test_fail("subscribe failed: %d", errno); + } + + + const unsigned int queue_size = 11; + t.val = 0; + ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue), &t, queue_size); + + if (ptopic == nullptr) { + return test_fail("advertise failed: %d", errno); + } + + orb_check(sfd, &updated); + + if (!updated) { + return test_fail("update flag not set"); + } + + if (PX4_OK != orb_copy(ORB_ID(orb_test_medium_queue), sfd, &u)) { + return test_fail("copy(1) failed: %d", errno); + } + + if (u.val != t.val) { + return test_fail("copy(1) mismatch: %d expected %d", u.val, t.val); + } + + orb_check(sfd, &updated); + + if (updated) { + return test_fail("spurious updated flag"); + } + +#define CHECK_UPDATED(element) \ + orb_check(sfd, &updated); \ + if (!updated) { \ + return test_fail("update flag not set, element %i", element); \ + } +#define CHECK_NOT_UPDATED(element) \ + orb_check(sfd, &updated); \ + if (updated) { \ + return test_fail("update flag set, element %i", element); \ + } +#define CHECK_COPY(i_got, i_correct) \ + orb_copy(ORB_ID(orb_test_medium_queue), sfd, &u); \ + if (i_got != i_correct) { \ + return test_fail("got wrong element from the queue (got %i, should be %i)", i_got, i_correct); \ + } + + //no messages in the queue anymore + + test_note(" Testing to write some elements..."); + + for (unsigned int i = 0; i < queue_size - 2; ++i) { + t.val = i; + orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t); + } + + for (unsigned int i = 0; i < queue_size - 2; ++i) { + CHECK_UPDATED(i); + CHECK_COPY(u.val, i); + } + + CHECK_NOT_UPDATED(queue_size); + + test_note(" Testing overflow..."); + int overflow_by = 3; + + for (unsigned int i = 0; i < queue_size + overflow_by; ++i) { + t.val = i; + orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t); + } + + for (unsigned int i = 0; i < queue_size; ++i) { + CHECK_UPDATED(i); + CHECK_COPY(u.val, i + overflow_by); + } + + CHECK_NOT_UPDATED(queue_size); + + test_note(" Testing underflow..."); + + for (unsigned int i = 0; i < queue_size; ++i) { + CHECK_NOT_UPDATED(i); + CHECK_COPY(u.val, queue_size + overflow_by - 1); + } + + t.val = 943; + orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t); + CHECK_UPDATED(-1); + CHECK_COPY(u.val, t.val); + +#undef CHECK_COPY +#undef CHECK_UPDATED +#undef CHECK_NOT_UPDATED + + orb_unadvertise(ptopic); + + return test_note("PASS orb queuing"); +} + + +int uORBTest::UnitTest::pub_test_queue_entry(char *const argv[]) +{ + uORBTest::UnitTest &t = uORBTest::UnitTest::instance(); + return t.pub_test_queue_main(); +} + +int uORBTest::UnitTest::pub_test_queue_main() +{ + struct orb_test_medium t; + orb_advert_t ptopic; + const unsigned int queue_size = 50; + t.val = 0; + + if ((ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue_poll), &t, queue_size)) == nullptr) { + _thread_should_exit = true; + return test_fail("advertise failed: %d", errno); + } + + int message_counter = 0, num_messages = 20 * queue_size; + ++t.val; + + while (message_counter < num_messages) { + + //simulate burst + int burst_counter = 0; + + while (burst_counter++ < queue_size / 2 + 7) { //make interval non-boundary aligned + orb_publish(ORB_ID(orb_test_medium_queue_poll), ptopic, &t); + ++t.val; + } + + message_counter += burst_counter; + usleep(20 * 1000); //give subscriber a chance to catch up + } + + _num_messages_sent = t.val; + usleep(100 * 1000); + _thread_should_exit = true; + orb_unadvertise(ptopic); + + return 0; +} + +int uORBTest::UnitTest::test_queue_poll_notify() +{ + test_note("Testing orb queuing (poll & notify)"); + + struct orb_test_medium t; + int sfd; + + if ((sfd = orb_subscribe(ORB_ID(orb_test_medium_queue_poll))) < 0) { + return test_fail("subscribe failed: %d", errno); + } + + _thread_should_exit = false; + + char *const args[1] = { NULL }; + int pubsub_task = px4_task_spawn_cmd("uorb_test_queue", + SCHED_DEFAULT, + SCHED_PRIORITY_MIN + 5, + 1500, + (px4_main_t)&uORBTest::UnitTest::pub_test_queue_entry, + args); + + if (pubsub_task < 0) { + return test_fail("failed launching task"); + } + + int next_expected_val = 0; + px4_pollfd_struct_t fds[1]; + fds[0].fd = sfd; + fds[0].events = POLLIN; + + while (!_thread_should_exit) { + + int poll_ret = px4_poll(fds, 1, 500); + + if (poll_ret == 0) { + if (_thread_should_exit) { + break; + } + + return test_fail("poll timeout"); + + } else if (poll_ret < 0) { + return test_fail("poll error (%d, %d)", poll_ret, errno); + } + + if (fds[0].revents & POLLIN) { + orb_copy(ORB_ID(orb_test_medium_queue_poll), sfd, &t); + + if (next_expected_val != t.val) { + return test_fail("copy mismatch: %d expected %d", t.val, next_expected_val); + } + + ++next_expected_val; + } + } + + if (_num_messages_sent != next_expected_val) { + return test_fail("number of sent and received messages mismatch (sent: %i, received: %i)", + _num_messages_sent, next_expected_val); + } + + return test_note("PASS orb queuing (poll & notify), got %i messages", next_expected_val); +} + + int uORBTest::UnitTest::test_fail(const char *fmt, ...) { va_list ap; diff --git a/src/modules/uORB/uORBTest_UnitTest.hpp b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp similarity index 76% rename from src/modules/uORB/uORBTest_UnitTest.hpp rename to src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp index 5aae62c725..8016b9c8a2 100644 --- a/src/modules/uORB/uORBTest_UnitTest.hpp +++ b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp @@ -33,31 +33,39 @@ #ifndef _uORBTest_UnitTest_hpp_ #define _uORBTest_UnitTest_hpp_ -#include "uORBCommon.hpp" -#include "uORB.h" +#include "../uORBCommon.hpp" +#include "../uORB.h" #include struct orb_test { int val; hrt_abstime time; }; -ORB_DEFINE(orb_test, struct orb_test); -ORB_DEFINE(orb_multitest, struct orb_test); +ORB_DEFINE(orb_test, struct orb_test, sizeof(orb_test), "ORB_TEST:int val;hrt_abstime time;"); +ORB_DEFINE(orb_multitest, struct orb_test, sizeof(orb_test), "ORB_MULTITEST:int val;hrt_abstime time;"); + struct orb_test_medium { int val; hrt_abstime time; char junk[64]; }; -ORB_DEFINE(orb_test_medium, struct orb_test_medium); -ORB_DEFINE(orb_test_medium_multi, struct orb_test_medium); +ORB_DEFINE(orb_test_medium, struct orb_test_medium, sizeof(orb_test_medium), + "ORB_TEST_MEDIUM:int val;hrt_abstime time;char[64] junk;"); +ORB_DEFINE(orb_test_medium_multi, struct orb_test_medium, sizeof(orb_test_medium), + "ORB_TEST_MEDIUM_MULTI:int val;hrt_abstime time;char[64] junk;"); +ORB_DEFINE(orb_test_medium_queue, struct orb_test_medium, sizeof(orb_test_medium), + "ORB_TEST_MEDIUM_MULTI:int val;hrt_abstime time;char[64] junk;"); +ORB_DEFINE(orb_test_medium_queue_poll, struct orb_test_medium, sizeof(orb_test_medium), + "ORB_TEST_MEDIUM_MULTI:int val;hrt_abstime time;char[64] junk;"); struct orb_test_large { int val; hrt_abstime time; char junk[512]; }; -ORB_DEFINE(orb_test_large, struct orb_test_large); +ORB_DEFINE(orb_test_large, struct orb_test_large, sizeof(orb_test_large), + "ORB_TEST_LARGE:int val;hrt_abstime time;char[512] junk;"); namespace uORBTest @@ -93,13 +101,23 @@ private: bool pubsubtest_print; int pubsubtest_res = OK; - int test_unadvertise(); orb_advert_t _pfd[4]; ///< used for test_multi and test_multi_reversed int test_single(); + + /* These 3 depend on each other and must be called in this order */ int test_multi(); - int test_multi2(); int test_multi_reversed(); + int test_unadvertise(); + + int test_multi2(); + + /* queuing tests */ + int test_queue(); + static int pub_test_queue_entry(char *const argv[]); + int pub_test_queue_main(); + int test_queue_poll_notify(); + volatile int _num_messages_sent = 0; int test_fail(const char *fmt, ...); int test_note(const char *fmt, ...); diff --git a/src/modules/uORB/uORB_tests/uORB_tests_main.cpp b/src/modules/uORB/uORB_tests/uORB_tests_main.cpp new file mode 100644 index 0000000000..1f0162f15d --- /dev/null +++ b/src/modules/uORB/uORB_tests/uORB_tests_main.cpp @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include "../uORBDevices.hpp" +#include "../uORB.h" +#include "../uORBCommon.hpp" + +#ifndef __PX4_QURT +#include "uORBTest_UnitTest.hpp" +#endif + +extern "C" { __EXPORT int uorb_tests_main(int argc, char *argv[]); } + +static void usage() +{ + PX4_INFO("Usage: uorb_test 'latency_test'"); +} + +int +uorb_tests_main(int argc, char *argv[]) +{ + +#ifndef __PX4_QURT + + /* + * Test the driver/device. + */ + if (argc == 1) { + uORBTest::UnitTest &t = uORBTest::UnitTest::instance(); + int rc = t.test(); + + if (rc == OK) { + fprintf(stdout, " [uORBTest] \t\tPASS\n"); + fflush(stdout); + return 0; + + } else { + fprintf(stderr, " [uORBTest] \t\tFAIL\n"); + fflush(stderr); + return -1; + } + } + + /* + * Test the latency. + */ + if (argc > 1 && !strcmp(argv[1], "latency_test")) { + + uORBTest::UnitTest &t = uORBTest::UnitTest::instance(); + + if (argc > 2 && !strcmp(argv[2], "medium")) { + return t.latency_test(ORB_ID(orb_test_medium), true); + + } else if (argc > 2 && !strcmp(argv[2], "large")) { + return t.latency_test(ORB_ID(orb_test_large), true); + + } else { + return t.latency_test(ORB_ID(orb_test), true); + } + } + +#endif + + usage(); + return -EINVAL; +} diff --git a/src/modules/uavcan/CMakeLists.txt b/src/modules/uavcan/CMakeLists.txt index c485fc8da6..8aea69743b 100644 --- a/src/modules/uavcan/CMakeLists.txt +++ b/src/modules/uavcan/CMakeLists.txt @@ -59,9 +59,6 @@ px4_add_module( STACK_MAIN 3200 STACK_MAX 1500 COMPILE_FLAGS - -Wno-attributes - -Wno-packed - -Wno-deprecated-declarations -Os SRCS # Main diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index 36f28a423c..022bb23f23 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -123,14 +123,14 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; } - irqstate_t flags = irqsave(); + irqstate_t flags = px4_enter_critical_section(); if (!_reports.resize(arg)) { - irqrestore(flags); + px4_leave_critical_section(flags); return -ENOMEM; } - irqrestore(flags); + px4_leave_critical_section(flags); return OK; } diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index d80b4b425d..b1cb428c5b 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -104,13 +104,11 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructureinit(bitrate); if (can_init_res < 0) { warnx("CAN driver init failed %i", can_init_res); return can_init_res; } - - can_initialized = true; } /* * Node init */ - _instance = new UavcanNode(can.driver, uavcan_stm32::SystemClock::instance()); + _instance = new UavcanNode(can->driver, uavcan_stm32::SystemClock::instance()); if (_instance == nullptr) { warnx("Out of memory"); @@ -1115,7 +1123,7 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg) break; - case UAVCANIOC_HARDPOINT_SET: { + case UAVCAN_IOCS_HARDPOINT_SET: { const auto &hp_cmd = *reinterpret_cast(arg); _hardpoint_controller.set_command(hp_cmd.hardpoint_id, hp_cmd.command); } diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 46195a3203..dbb74b39af 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -73,10 +73,6 @@ // we add two to allow for actuator_direct and busevent #define UAVCAN_NUM_POLL_FDS (NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN+2) -// IOCTL control codes -static constexpr unsigned UAVCANIOCBASE = 0x7800; -static constexpr unsigned UAVCANIOC_HARDPOINT_SET = _PX4_IOC(UAVCANIOCBASE, 0x10); - /** * A UAVCAN node. */ diff --git a/src/modules/uavcan/uavcan_module.hpp b/src/modules/uavcan/uavcan_module.hpp index 49c5437e62..7e54f1785f 100644 --- a/src/modules/uavcan/uavcan_module.hpp +++ b/src/modules/uavcan/uavcan_module.hpp @@ -65,7 +65,18 @@ // ioctl interface #define _UAVCAN_IOC(_n) (_IOC(_UAVCAN_IOCBASE, _n)) #define _UAVCAN_IOCBASE (0x4000) // IOCTL base for module UAVCAN -#define UAVCAN_IOCG_NODEID_INPROGRESS _UAVCAN_IOC(1) // query if node identification is in progress +/* + * Query if node identification is in progress. Returns: + * EINVAL - not applicable in the current operating mode + * ETIME - network discovery complete + * OK (0) - network discovery in progress + */ +#define UAVCAN_IOCG_NODEID_INPROGRESS _UAVCAN_IOC(1) +/* + * Set hardpoint command. Accepts a pointer to uavcan::equipment::hardpoint::Command; returns nothing. + * The pointer may be invalidated once the call returns. + */ +#define UAVCAN_IOCS_HARDPOINT_SET _UAVCAN_IOC(10) // public prototypes extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); diff --git a/src/modules/uavcan/uavcan_params.c b/src/modules/uavcan/uavcan_params.c index e269202d99..2196a5fb30 100644 --- a/src/modules/uavcan/uavcan_params.c +++ b/src/modules/uavcan/uavcan_params.c @@ -39,9 +39,8 @@ #include /** - * Enable UAVCAN. + * UAVCAN mode * - * Allowed values: * 0 - UAVCAN disabled. * 1 - Enabled support for UAVCAN actuators and sensors. * 2 - Enabled support for dynamic node ID allocation and firmware update. diff --git a/src/modules/unit_test/unit_test.cpp b/src/modules/unit_test/unit_test.cpp index c45d7888ab..7b3f3a41d1 100644 --- a/src/modules/unit_test/unit_test.cpp +++ b/src/modules/unit_test/unit_test.cpp @@ -50,21 +50,27 @@ UnitTest::~UnitTest() void UnitTest::print_results(void) { - warnx(_tests_failed ? "SOME TESTS FAILED" : "ALL TESTS PASSED"); - warnx(" Tests passed : %d", _tests_passed); - warnx(" Tests failed : %d", _tests_failed); - warnx(" Assertions : %d", _assertions); + if (_tests_failed) { + PX4_ERR("SOME TESTS FAILED"); + + } else { + PX4_INFO("ALL TESTS PASSED"); + } + + PX4_INFO(" Tests passed : %d", _tests_passed); + PX4_INFO(" Tests failed : %d", _tests_failed); + PX4_INFO(" Assertions : %d", _assertions); } /// @brief Used internally to the ut_assert macro to print assert failures. void UnitTest::_print_assert(const char *msg, const char *test, const char *file, int line) { - warnx("Assertion failed: %s - %s (%s:%d)", msg, test, file, line); + PX4_ERR("Assertion failed: %s - %s (%s:%d)", msg, test, file, line); } /// @brief Used internally to the ut_compare macro to print assert failures. void UnitTest::_print_compare(const char *msg, const char *v1_text, int v1, const char *v2_text, int v2, const char *file, int line) { - warnx("Compare failed: %s - (%s:%d) (%s:%d) (%s:%d)", msg, v1_text, v1, v2_text, v2, file, line); + PX4_ERR("Compare failed: %s - (%s:%d) (%s:%d) (%s:%d)", msg, v1_text, v1, v2_text, v2, file, line); } diff --git a/src/modules/unit_test/unit_test.h b/src/modules/unit_test/unit_test.h index bd021ff54a..42be09c165 100644 --- a/src/modules/unit_test/unit_test.h +++ b/src/modules/unit_test/unit_test.h @@ -37,6 +37,17 @@ #include +#define ut_declare_test_c(test_function, test_class) \ + extern "C" { \ + int test_function(int argc, char *argv[]) \ + { \ + test_class* test = new test_class(); \ + bool success = test->run_tests(); \ + test->print_results(); \ + return success ? 0 : -1; \ + } \ + } + /// @brief Base class to be used for unit tests. class __EXPORT UnitTest { @@ -68,14 +79,14 @@ protected: /// test should return true if it succeeded, false for fail. #define ut_run_test(test) \ do { \ - warnx("RUNNING TEST: %s", #test); \ + PX4_INFO("RUNNING TEST: %s", #test); \ _tests_run++; \ _init(); \ if (!test()) { \ - warnx("TEST FAILED: %s", #test); \ + PX4_ERR("TEST FAILED: %s", #test); \ _tests_failed++; \ } else { \ - warnx("TEST PASSED: %s", #test); \ + PX4_INFO("TEST PASSED: %s", #test); \ _tests_passed++; \ } \ _cleanup(); \ @@ -92,6 +103,31 @@ protected: } \ } while (0) +/// @brief Used to assert a value within a unit test. +#define ut_test(test) ut_assert("test", test) + +/// @brief To assert specifically to true. +#define ut_assert_true(test) \ + do { \ + if (test != true) { \ + _print_assert("result not true", #test, __FILE__, __LINE__); \ + return false; \ + } else { \ + _assertions++; \ + } \ + } while (0) + +/// @brief To assert specifically to true. +#define ut_assert_false(test) \ + do { \ + if (test != false) { \ + _print_assert("result not false", #test, __FILE__, __LINE__); \ + return false; \ + } else { \ + _assertions++; \ + } \ + } while (0) + /// @brief Used to compare two integer values within a unit test. If possible use ut_compare instead of ut_assert /// since it will give you better error reporting of the actual values being compared. #define ut_compare(message, v1, v2) \ diff --git a/src/modules/vtol_att_control/CMakeLists.txt b/src/modules/vtol_att_control/CMakeLists.txt index b7dcd83ed0..24b2b2c6f0 100644 --- a/src/modules/vtol_att_control/CMakeLists.txt +++ b/src/modules/vtol_att_control/CMakeLists.txt @@ -33,9 +33,8 @@ px4_add_module( MODULE modules__vtol_att_control MAIN vtol_att_control - COMPILE_FLAGS - -Wno-write-strings - + STACK_MAIN 1300 + COMPILE_FLAGS -Os SRCS vtol_att_control_main.cpp tiltrotor.cpp diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 3bf8080e63..85f48dfcf8 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -67,7 +67,8 @@ Standard::Standard(VtolAttitudeControl *attc) : _params_handles_standard.front_trans_timeout = param_find("VT_TRANS_TIMEOUT"); _params_handles_standard.front_trans_time_min = param_find("VT_TRANS_MIN_TM"); _params_handles_standard.down_pitch_max = param_find("VT_DWN_PITCH_MAX"); - _params_handles_standard.forward_thurst_scale = param_find("VT_FWD_THRUST_SC"); + _params_handles_standard.forward_thrust_scale = param_find("VT_FWD_THRUST_SC"); + _params_handles_standard.airspeed_mode = param_find("FW_ARSP_MODE"); } Standard::~Standard() @@ -78,6 +79,7 @@ int Standard::parameters_update() { float v; + int i; /* duration of a forwards transition to fw mode */ param_get(_params_handles_standard.front_trans_dur, &v); @@ -112,7 +114,12 @@ Standard::parameters_update() _params_standard.down_pitch_max = math::radians(v); /* scale for fixed wing thrust used for forward acceleration in multirotor mode */ - param_get(_params_handles_standard.forward_thurst_scale, &_params_standard.forward_thurst_scale); + param_get(_params_handles_standard.forward_thrust_scale, &_params_standard.forward_thrust_scale); + + /* airspeed mode */ + param_get(_params_handles_standard.airspeed_mode, &i); + _params_standard.airspeed_mode = math::constrain(i, 0, 2); + return OK; @@ -139,9 +146,17 @@ void Standard::update_vtol_state() } else if (_vtol_schedule.flight_mode == FW_MODE) { // transition to mc mode - _vtol_schedule.flight_mode = TRANSITION_TO_MC; - _flag_enable_mc_motors = true; - _vtol_schedule.transition_start = hrt_absolute_time(); + if (_vtol_vehicle_status->vtol_transition_failsafe == true) { + // Failsafe event, engage mc motors immediately + _vtol_schedule.flight_mode = MC_MODE; + _flag_enable_mc_motors = true; + + } else { + // Regular backtransition + _vtol_schedule.flight_mode = TRANSITION_TO_MC; + _flag_enable_mc_motors = true; + _vtol_schedule.transition_start = hrt_absolute_time(); + } } else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { // failsafe back to mc mode @@ -155,7 +170,7 @@ void Standard::update_vtol_state() // transition to MC mode if transition time has passed // XXX: base this on XY hold velocity of MC if (hrt_elapsed_time(&_vtol_schedule.transition_start) > - (_params_standard.back_trans_dur * 1000000.0f)) { + (_params_standard.back_trans_dur * 1000000.0f)) { _vtol_schedule.flight_mode = MC_MODE; } } @@ -180,10 +195,11 @@ void Standard::update_vtol_state() } else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { // continue the transition to fw mode while monitoring airspeed for a final switch to fw mode - if ((_airspeed->indicated_airspeed_m_s >= _params_standard.airspeed_trans && + if (((_params_standard.airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED || + _airspeed->indicated_airspeed_m_s >= _params_standard.airspeed_trans) && (float)hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params_standard.front_trans_time_min * 1000000.0f)) || - !_armed->armed) { + can_transition_on_ground()) { _vtol_schedule.flight_mode = FW_MODE; // we can turn off the multirotor motors now _flag_enable_mc_motors = false; @@ -216,6 +232,8 @@ void Standard::update_vtol_state() void Standard::update_transition_state() { + VtolType::update_transition_state(); + // copy virtual attitude setpoint to real attitude setpoint memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); @@ -236,12 +254,25 @@ void Standard::update_transition_state() (float)hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params_standard.front_trans_time_min * 1000000.0f) ) { float weight = 1.0f - fabsf(_airspeed->indicated_airspeed_m_s - _params_standard.airspeed_blend) / - _airspeed_trans_blend_margin; + _airspeed_trans_blend_margin; _mc_roll_weight = weight; _mc_pitch_weight = weight; _mc_yaw_weight = weight; _mc_throttle_weight = weight; + // time based blending when no airspeed sensor is set + + } else if (_params_standard.airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED && + (float)hrt_elapsed_time(&_vtol_schedule.transition_start) < (_params_standard.front_trans_time_min * 1000000.0f) + ) { + float weight = 1.0f - (float)(hrt_elapsed_time(&_vtol_schedule.transition_start) / + (_params_standard.front_trans_time_min * 1000000.0f)); + _mc_roll_weight = weight; + _mc_pitch_weight = weight; + _mc_yaw_weight = weight; + _mc_throttle_weight = weight; + + } else { // at low speeds give full weight to mc _mc_roll_weight = 1.0f; @@ -254,7 +285,7 @@ void Standard::update_transition_state() if (_params_standard.front_trans_timeout > FLT_EPSILON) { if ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params_standard.front_trans_timeout * 1000000.0f)) { // transition timeout occured, abort transition - _attc->abort_front_transition(); + _attc->abort_front_transition("Transition timeout"); } } @@ -293,36 +324,70 @@ void Standard::update_mc_state() { VtolType::update_mc_state(); + // enable MC motors here in case we transitioned directly to MC mode + if (_flag_enable_mc_motors) { + set_max_mc(2000); + set_idle_mc(); + _flag_enable_mc_motors = false; + } + // if the thrust scale param is zero then the pusher-for-pitch strategy is disabled and we can return - if (_params_standard.forward_thurst_scale < FLT_EPSILON) { + if (_params_standard.forward_thrust_scale < FLT_EPSILON) { return; } - // get projection of thrust vector on body x axis. This is used to - // determine the desired forward acceleration which we want to achieve with the pusher - math::Matrix<3,3> R(&_v_att->R[0]); - math::Matrix<3,3> R_sp(&_v_att_sp->R_body[0]); - math::Vector<3> thrust_sp_axis(-R_sp(0,2), -R_sp(1,2), -R_sp(2,2)); - math::Vector<3> euler = R.to_euler(); - R.from_euler(0, 0, euler(2)); - math::Vector<3> body_x_zero_tilt(R(0,0), R(1,0), R(2,0)); + matrix::Dcmf R(matrix::Quatf(_v_att->q)); + matrix::Dcmf R_sp(&_v_att_sp->R_body[0]); + matrix::Eulerf euler(R); + matrix::Eulerf euler_sp(R_sp); + _pusher_throttle = 0.0f; + + // direction of desired body z axis represented in earth frame + matrix::Vector3f body_z_sp(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); + + // rotate desired body z axis into new frame which is rotated in z by the current + // heading of the vehicle. we refer to this as the heading frame. + matrix::Dcmf R_yaw = matrix::Eulerf(0.0f, 0.0f, -euler(2)); + body_z_sp = R_yaw * body_z_sp; + body_z_sp.normalize(); + + // calculate the desired pitch seen in the heading frame + // this value corresponds to the amount the vehicle would try to pitch forward + float pitch_forward = asinf(body_z_sp(0)); + + // only allow pitching forward up to threshold, the rest of the desired + // forward acceleration will be compensated by the pusher + if (pitch_forward < -_params_standard.down_pitch_max) { + // desired roll angle in heading frame stays the same + float roll_new = -atan2f(body_z_sp(1), body_z_sp(2)); + + _pusher_throttle = (sinf(-pitch_forward) - sinf(_params_standard.down_pitch_max)) + * _v_att_sp->thrust * _params_standard.forward_thrust_scale; + + // limit desired pitch + float pitch_new = -_params_standard.down_pitch_max; + + // create corrected desired body z axis in heading frame + matrix::Dcmf R_tmp = matrix::Eulerf(roll_new, pitch_new, 0.0f); + matrix::Vector3f tilt_new(R_tmp(0, 2), R_tmp(1, 2), R_tmp(2, 2)); + + // rotate the vector into a new frame which is rotated in z by the desired heading + // with respect to the earh frame. + float yaw_error = _wrap_pi(euler_sp(2) - euler(2)); + matrix::Dcmf R_yaw_correction = matrix::Eulerf(0.0f, 0.0f, -yaw_error); + tilt_new = R_yaw_correction * tilt_new; + + // now extract roll and pitch setpoints + float pitch = asinf(tilt_new(0)); + float roll = -atan2f(tilt_new(1), tilt_new(2)); + R_sp = matrix::Eulerf(roll, pitch, euler_sp(2)); + matrix::Quatf q_sp(R_sp); + memcpy(&_v_att_sp->R_body[0], &R_sp._data[0], sizeof(_v_att_sp->R_body)); + memcpy(&_v_att_sp->q_d[0], &q_sp._data[0], sizeof(_v_att_sp->q_d)); + } - // we are using a parameter to scale the thrust value in order to compensate for highly over/under-powered motors - _pusher_throttle = body_x_zero_tilt * thrust_sp_axis * _v_att_sp->thrust * _params_standard.forward_thurst_scale; _pusher_throttle = _pusher_throttle < 0.0f ? 0.0f : _pusher_throttle; - float pitch_sp_corrected = _v_att_sp->pitch_body < -_params_standard.down_pitch_max ? -_params_standard.down_pitch_max : _v_att_sp->pitch_body; - - // compute new desired rotation matrix with corrected pitch angle - // and copy data to attitude setpoint topic - euler = R_sp.to_euler(); - euler(1) = pitch_sp_corrected; - R_sp.from_euler(euler(0), euler(1), euler(2)); - memcpy(&_v_att_sp->R_body[0], R_sp.data, sizeof(_v_att_sp->R_body)); - _v_att_sp->pitch_body = pitch_sp_corrected; - math::Quaternion q_sp; - q_sp.from_dcm(R_sp); - memcpy(&_v_att_sp->q_d[0], &q_sp.data[0], sizeof(_v_att_sp->q_d)); } void Standard::update_fw_state() @@ -395,10 +460,12 @@ Standard::set_max_mc(unsigned pwm_value) { int ret; unsigned servo_count; - char *dev = PWM_OUTPUT0_DEVICE_PATH; + const char *dev = PWM_OUTPUT0_DEVICE_PATH; int fd = px4_open(dev, 0); - if (fd < 0) {PX4_WARN("can't open %s", dev);} + if (fd < 0) { + PX4_WARN("can't open %s", dev); + } ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); struct pwm_output_values pwm_values; @@ -411,7 +478,9 @@ Standard::set_max_mc(unsigned pwm_value) ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); - if (ret != OK) {PX4_WARN("failed setting max values");} + if (ret != OK) { + PX4_WARN("failed setting max values"); + } px4_close(fd); } diff --git a/src/modules/vtol_att_control/standard.h b/src/modules/vtol_att_control/standard.h index ea578bf2bf..33fbd5d0f9 100644 --- a/src/modules/vtol_att_control/standard.h +++ b/src/modules/vtol_att_control/standard.h @@ -75,7 +75,8 @@ private: float front_trans_timeout; float front_trans_time_min; float down_pitch_max; - float forward_thurst_scale; + float forward_thrust_scale; + int airspeed_mode; } _params_standard; struct { @@ -87,7 +88,8 @@ private: param_t front_trans_timeout; param_t front_trans_time_min; param_t down_pitch_max; - param_t forward_thurst_scale; + param_t forward_thrust_scale; + param_t airspeed_mode; } _params_handles_standard; enum vtol_mode { @@ -103,7 +105,7 @@ private: } _vtol_schedule; bool _flag_enable_mc_motors; - float _pusher_throttle; + float _pusher_throttle; float _airspeed_trans_blend_margin; void set_max_mc(unsigned pwm_value); diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index b8be7ff8d2..ec707ce27d 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -180,7 +180,7 @@ void Tailsitter::update_vtol_state() // check if we have reached airspeed and pitch angle to switch to TRANSITION P2 mode if ((_airspeed->indicated_airspeed_m_s >= _params_tailsitter.airspeed_trans - && _v_att->pitch <= PITCH_TRANSITION_FRONT_P1) || !_armed->armed) { + && _v_att->pitch <= PITCH_TRANSITION_FRONT_P1) || can_transition_on_ground()) { _vtol_schedule.flight_mode = FW_MODE; //_vtol_schedule.transition_start = hrt_absolute_time(); } @@ -250,9 +250,9 @@ void Tailsitter::update_transition_state() /** create time dependant throttle signal higher than in MC and growing untill P2 switch speed reached */ if (_airspeed->indicated_airspeed_m_s <= _params_tailsitter.airspeed_trans) { _thrust_transition = _thrust_transition_start + (fabsf(THROTTLE_TRANSITION_MAX * _thrust_transition_start) * - (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur * 1000000.0f)); + (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur * 1000000.0f)); _thrust_transition = math::constrain(_thrust_transition , _thrust_transition_start , - (1.0f + THROTTLE_TRANSITION_MAX) * _thrust_transition_start); + (1.0f + THROTTLE_TRANSITION_MAX) * _thrust_transition_start); _v_att_sp->thrust = _thrust_transition; } diff --git a/src/modules/vtol_att_control/tailsitter.h b/src/modules/vtol_att_control/tailsitter.h index 17fdcae965..456efab568 100644 --- a/src/modules/vtol_att_control/tailsitter.h +++ b/src/modules/vtol_att_control/tailsitter.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** -* @file tiltrotor.h +* @file tailsitter.h * * @author Roman Bapst * @author David Vorsin diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 9adcb6a9ff..65f14b1e7c 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -208,7 +208,7 @@ void Tiltrotor::update_vtol_state() // check if we have reached airspeed to switch to fw mode // also allow switch if we are not armed for the sake of bench testing - if (_airspeed->indicated_airspeed_m_s >= _params_tiltrotor.airspeed_trans || !_armed->armed) { + if (_airspeed->indicated_airspeed_m_s >= _params_tiltrotor.airspeed_trans || can_transition_on_ground()) { _vtol_schedule.flight_mode = TRANSITION_FRONT_P2; _vtol_schedule.transition_start = hrt_absolute_time(); } @@ -433,10 +433,12 @@ void Tiltrotor::set_rear_motor_state(rear_motor_state state) int ret; unsigned servo_count; - char *dev = PWM_OUTPUT0_DEVICE_PATH; + const char *dev = PWM_OUTPUT0_DEVICE_PATH; int fd = px4_open(dev, 0); - if (fd < 0) {PX4_WARN("can't open %s", dev);} + if (fd < 0) { + PX4_WARN("can't open %s", dev); + } ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); struct pwm_output_values pwm_max_values; 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 b247df4a3f..4d0083bf99 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -81,6 +81,7 @@ VtolAttitudeControl::VtolAttitudeControl() : _vehicle_cmd_sub(-1), _vehicle_status_sub(-1), _tecs_status_sub(-1), + _land_detected_sub(-1), //init publication handlers _actuators_0_pub(nullptr), @@ -88,7 +89,7 @@ VtolAttitudeControl::VtolAttitudeControl() : _vtol_vehicle_status_pub(nullptr), _v_rates_sp_pub(nullptr), _v_att_sp_pub(nullptr), - + _transition_command(vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC), _abort_front_transition(false) { @@ -103,7 +104,6 @@ VtolAttitudeControl::VtolAttitudeControl() : memset(&_fw_virtual_v_rates_sp, 0, sizeof(_fw_virtual_v_rates_sp)); memset(&_manual_control_sp, 0, sizeof(_manual_control_sp)); memset(&_v_control_mode, 0, sizeof(_v_control_mode)); - memset(&_vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status)); memset(&_actuators_out_0, 0, sizeof(_actuators_out_0)); memset(&_actuators_out_1, 0, sizeof(_actuators_out_1)); memset(&_actuators_mc_in, 0, sizeof(_actuators_mc_in)); @@ -115,6 +115,7 @@ VtolAttitudeControl::VtolAttitudeControl() : memset(&_vehicle_cmd, 0, sizeof(_vehicle_cmd)); memset(&_vehicle_status, 0, sizeof(_vehicle_status)); memset(&_tecs_status, 0, sizeof(_tecs_status)); + memset(&_land_detected, 0, sizeof(_land_detected)); _params.idle_pwm_mc = PWM_DEFAULT_MIN; _params.vtol_motor_count = 0; @@ -132,6 +133,7 @@ VtolAttitudeControl::VtolAttitudeControl() : _params_handles.arsp_lp_gain = param_find("VT_ARSP_LP_GAIN"); _params_handles.vtol_type = param_find("VT_TYPE"); _params_handles.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK"); + _params_handles.fw_min_alt = param_find("VT_FW_MIN_ALT"); /* fetch initial parameter values */ parameters_update(); @@ -444,6 +446,21 @@ VtolAttitudeControl::tecs_status_poll() } } +/** +* Check for land detector updates. +*/ +void +VtolAttitudeControl::land_detected_poll() +{ + bool updated; + + orb_check(_land_detected_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_land_detected), _land_detected_sub , &_land_detected); + } +} + /** * Check received command */ @@ -463,17 +480,22 @@ VtolAttitudeControl::handle_command() bool VtolAttitudeControl::is_fixed_wing_requested() { - bool to_fw = _manual_control_sp.aux1 > 0.0f; + bool to_fw = false; - // listen to transition commands if not in manual - if (!_v_control_mode.flag_control_manual_enabled) { - to_fw = _transition_command == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW; + if (_manual_control_sp.transition_switch != manual_control_setpoint_s::SWITCH_POS_NONE && + _v_control_mode.flag_control_manual_enabled) { + to_fw = (_manual_control_sp.transition_switch == manual_control_setpoint_s::SWITCH_POS_ON); + + } else { + // listen to transition commands if not in manual or mode switch is not mapped + to_fw = (_transition_command == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); } // handle abort request if (_abort_front_transition) { if (to_fw) { to_fw = false; + } else { // the state changed to mc mode, reset the abort request _abort_front_transition = false; @@ -488,10 +510,10 @@ VtolAttitudeControl::is_fixed_wing_requested() * Abort front transition */ void -VtolAttitudeControl::abort_front_transition() +VtolAttitudeControl::abort_front_transition(const char *reason) { - if(!_abort_front_transition) { - mavlink_log_critical(&_mavlink_log_pub, "Front transition timeout occured, aborting"); + if (!_abort_front_transition) { + mavlink_log_critical(&_mavlink_log_pub, "Abort: %s", reason); _abort_front_transition = true; _vtol_vehicle_status.vtol_transition_failsafe = true; } @@ -549,6 +571,11 @@ VtolAttitudeControl::parameters_update() param_get(_params_handles.elevons_mc_lock, &l); _params.elevons_mc_lock = l; + /* minimum relative altitude for FW mode (QuadChute) */ + param_get(_params_handles.fw_min_alt, &v); + _params.fw_min_alt = v; + + return OK; } @@ -557,6 +584,7 @@ VtolAttitudeControl::parameters_update() */ void VtolAttitudeControl::fill_mc_att_rates_sp() { + _v_rates_sp.timestamp = _mc_virtual_v_rates_sp.timestamp; _v_rates_sp.roll = _mc_virtual_v_rates_sp.roll; _v_rates_sp.pitch = _mc_virtual_v_rates_sp.pitch; _v_rates_sp.yaw = _mc_virtual_v_rates_sp.yaw; @@ -568,6 +596,7 @@ void VtolAttitudeControl::fill_mc_att_rates_sp() */ void VtolAttitudeControl::fill_fw_att_rates_sp() { + _v_rates_sp.timestamp = _fw_virtual_v_rates_sp.timestamp; _v_rates_sp.roll = _fw_virtual_v_rates_sp.roll; _v_rates_sp.pitch = _fw_virtual_v_rates_sp.pitch; _v_rates_sp.yaw = _fw_virtual_v_rates_sp.yaw; @@ -614,6 +643,7 @@ void VtolAttitudeControl::task_main() _vehicle_cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); _tecs_status_sub = orb_subscribe(ORB_ID(tecs_status)); + _land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); _actuator_inputs_mc = orb_subscribe(ORB_ID(actuator_controls_virtual_mc)); _actuator_inputs_fw = orb_subscribe(ORB_ID(actuator_controls_virtual_fw)); @@ -638,11 +668,12 @@ void VtolAttitudeControl::task_main() while (!_task_should_exit) { /*Advertise/Publish vtol vehicle status*/ + _vtol_vehicle_status.timestamp = hrt_absolute_time(); + if (_vtol_vehicle_status_pub != nullptr) { orb_publish(ORB_ID(vtol_vehicle_status), _vtol_vehicle_status_pub, &_vtol_vehicle_status); } else { - _vtol_vehicle_status.timestamp = hrt_absolute_time(); _vtol_vehicle_status_pub = orb_advertise(ORB_ID(vtol_vehicle_status), &_vtol_vehicle_status); } @@ -692,6 +723,7 @@ void VtolAttitudeControl::task_main() vehicle_cmd_poll(); vehicle_status_poll(); tecs_status_poll(); + land_detected_poll(); // update the vtol state machine which decides which mode we are in _vtol_type->update_vtol_state(); @@ -770,8 +802,8 @@ void VtolAttitudeControl::task_main() /* Only publish if the proper mode(s) are enabled */ if (_v_control_mode.flag_control_attitude_enabled || - _v_control_mode.flag_control_rates_enabled || - _v_control_mode.flag_control_manual_enabled) { + _v_control_mode.flag_control_rates_enabled || + _v_control_mode.flag_control_manual_enabled) { if (_actuators_0_pub != nullptr) { orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); @@ -810,7 +842,7 @@ VtolAttitudeControl::start() _control_task = px4_task_spawn_cmd("vtol_att_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, - 2048, + 1100, (px4_main_t)&VtolAttitudeControl::task_main_trampoline, nullptr); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 5a5b5eed57..ab9545fb9e 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -69,8 +69,6 @@ #include #include #include -#include -#include #include #include #include @@ -85,6 +83,8 @@ #include #include #include +#include +#include #include #include #include @@ -110,7 +110,7 @@ public: int start(); /* start the task and return OK on success */ bool is_fixed_wing_requested(); - void abort_front_transition(); + void abort_front_transition(const char *reason); struct vehicle_attitude_s *get_att() {return &_v_att;} struct vehicle_attitude_setpoint_s *get_att_sp() {return &_v_att_sp;} @@ -132,6 +132,7 @@ public: struct battery_status_s *get_batt_status() {return &_batt_status;} struct vehicle_status_s *get_vehicle_status() {return &_vehicle_status;} struct tecs_status_s *get_tecs_status() {return &_tecs_status;} + struct vehicle_land_detected_s *get_land_detected() {return &_land_detected;} struct Params *get_params() {return &_params;} @@ -159,6 +160,7 @@ private: int _vehicle_cmd_sub; int _vehicle_status_sub; int _tecs_status_sub; + int _land_detected_sub; int _actuator_inputs_mc; //topic on which the mc_att_controller publishes actuator inputs int _actuator_inputs_fw; //topic on which the fw_att_controller publishes actuator inputs @@ -191,6 +193,7 @@ private: struct vehicle_command_s _vehicle_cmd; struct vehicle_status_s _vehicle_status; struct tecs_status_s _tecs_status; + struct vehicle_land_detected_s _land_detected; Params _params; // struct holding the parameters @@ -207,6 +210,7 @@ private: param_t arsp_lp_gain; param_t vtol_type; param_t elevons_mc_lock; + param_t fw_min_alt; } _params_handles; /* for multicopters it is usual to have a non-zero idle speed of the engines @@ -241,6 +245,7 @@ private: void vehicle_battery_poll(); // Check for battery updates void vehicle_cmd_poll(); void tecs_status_poll(); + void land_detected_poll(); void parameters_update_poll(); //Check if parameters have changed void vehicle_status_poll(); int parameters_update(); //Update local paraemter cache diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index 2abaadb1cc..a8326e2cb1 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -249,21 +249,13 @@ PARAM_DEFINE_FLOAT(VT_ARSP_BLEND, 8.0f); PARAM_DEFINE_FLOAT(VT_ARSP_TRANS, 10.0f); /** - * Enable optimal recovery strategy for pitch-weak tailsitters + * Optimal recovery strategy for pitch-weak tailsitters * * @boolean * @group VTOL Attitude Control */ PARAM_DEFINE_INT32(VT_OPT_RECOV_EN, 0); -/** - * Enable weather-vane mode landings for missions - * - * @boolean - * @group VTOL Attitude Control - */ -PARAM_DEFINE_INT32(VT_WV_LND_EN, 0); - /** * Weather-vane yaw rate scale. * @@ -278,14 +270,6 @@ PARAM_DEFINE_INT32(VT_WV_LND_EN, 0); */ PARAM_DEFINE_FLOAT(VT_WV_YAWR_SCL, 0.15f); -/** - * Enable weather-vane mode for loiter - * - * @boolean - * @group VTOL Attitude Control - */ -PARAM_DEFINE_INT32(VT_WV_LTR_EN, 0); - /** * Front transition timeout * @@ -320,3 +304,14 @@ PARAM_DEFINE_FLOAT(VT_TRANS_MIN_TM, 2.0f); * @group VTOL Attitude Control */ PARAM_DEFINE_INT32(VT_NAV_FORCE_VT, 1); + +/** + * QuadChute + * + * Minimum altitude for fixed wing flight, when in fixed wing the altitude drops below this altitude + * the vehicle will transition back to MC mode and enter failsafe RTL + * @min 0.0 + * @max 200.0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_FW_MIN_ALT, 0.0f); diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 681c782815..138fc45637 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -68,6 +68,7 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) : _batt_status = _attc->get_batt_status(); _vehicle_status = _attc->get_vehicle_status(); _tecs_status = _attc->get_tecs_status(); + _land_detected = _attc->get_land_detected(); _params = _attc->get_params(); flag_idle_mc = true; @@ -85,10 +86,12 @@ void VtolType::set_idle_mc() { int ret; unsigned servo_count; - char *dev = PWM_OUTPUT0_DEVICE_PATH; + const char *dev = PWM_OUTPUT0_DEVICE_PATH; int fd = px4_open(dev, 0); - if (fd < 0) {PX4_WARN("can't open %s", dev);} + if (fd < 0) { + PX4_WARN("can't open %s", dev); + } ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); unsigned pwm_value = _params->idle_pwm_mc; @@ -102,7 +105,9 @@ void VtolType::set_idle_mc() ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); - if (ret != OK) {PX4_WARN("failed setting min values");} + if (ret != OK) { + PX4_WARN("failed setting min values"); + } px4_close(fd); @@ -115,12 +120,15 @@ void VtolType::set_idle_mc() void VtolType::set_idle_fw() { int ret; - char *dev = PWM_OUTPUT0_DEVICE_PATH; + const char *dev = PWM_OUTPUT0_DEVICE_PATH; int fd = px4_open(dev, 0); - if (fd < 0) {PX4_WARN("can't open %s", dev);} + if (fd < 0) { + PX4_WARN("can't open %s", dev); + } struct pwm_output_values pwm_values; + memset(&pwm_values, 0, sizeof(pwm_values)); for (int i = 0; i < _params->vtol_motor_count; i++) { @@ -131,7 +139,9 @@ void VtolType::set_idle_fw() ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); - if (ret != OK) {PX4_WARN("failed setting min values");} + if (ret != OK) { + PX4_WARN("failed setting min values"); + } px4_close(fd); } @@ -163,8 +173,32 @@ void VtolType::update_fw_state() _tecs_running_ts = hrt_absolute_time(); } - // tecs didn't publish yet or the position controller didn't publish yet AFTER tecs - if (!_tecs_running || (_tecs_running && _fw_virtual_att_sp->timestamp <= _tecs_running_ts)) { + // TECS didn't publish yet or the position controller didn't publish yet AFTER tecs + // only wait on TECS we're in a mode where it is actually running + if ((!_tecs_running || (_tecs_running && _fw_virtual_att_sp->timestamp <= _tecs_running_ts)) + && _v_control_mode->flag_control_altitude_enabled) { waiting_on_tecs(); } + + check_quadchute_condition(); +} + +void VtolType::update_transition_state() +{ + check_quadchute_condition(); +} + +bool VtolType::can_transition_on_ground() +{ + return !_armed->armed || _land_detected->landed; +} + +void VtolType::check_quadchute_condition() +{ + // fixed-wing minimum altitude + if (_params->fw_min_alt > FLT_EPSILON && _armed->armed) { + if (-(_local_pos->z) < _params->fw_min_alt) { + _attc->abort_front_transition("Minimum altitude"); + } + } } diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index ddfed44f57..96f11fd51e 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -59,6 +59,7 @@ struct Params { float arsp_lp_gain; // total airspeed estimate low pass gain int vtol_type; int elevons_mc_lock; // lock elevons in multicopter mode + float fw_min_alt; // minimum relative altitude for FW mode (QuadChute) }; enum mode { @@ -81,6 +82,8 @@ class VtolType public: VtolType(VtolAttitudeControl *att_controller); + VtolType(const VtolType &) = delete; + VtolType &operator=(const VtolType &) = delete; virtual ~VtolType(); @@ -120,6 +123,16 @@ public: */ virtual void waiting_on_tecs() {}; + /** + * Checks for fixed-wing failsafe condition and issues abort request if needed. + */ + void check_quadchute_condition(); + + /** + * Returns true if we're allowed to do a mode transition on the ground. + */ + bool can_transition_on_ground(); + void set_idle_mc(); void set_idle_fw(); @@ -149,6 +162,7 @@ protected: struct battery_status_s *_batt_status; // battery status struct vehicle_status_s *_vehicle_status; // vehicle status from commander app struct tecs_status_s *_tecs_status; + struct vehicle_land_detected_s *_land_detected; struct Params *_params; diff --git a/src/platforms/common/module.mk b/src/platforms/common/module.mk deleted file mode 100644 index 0e5f463e69..0000000000 --- a/src/platforms/common/module.mk +++ /dev/null @@ -1,6 +0,0 @@ -# -# Common OS porting APIs -# - -SRCS = px4_getopt.c - diff --git a/src/platforms/common/px4_getopt.c b/src/platforms/common/px4_getopt.c index 863b5816b4..d446a1fadc 100644 --- a/src/platforms/common/px4_getopt.c +++ b/src/platforms/common/px4_getopt.c @@ -143,10 +143,12 @@ __EXPORT int px4_getopt(int argc, char *argv[], const char *options, int *myopti char c; int takesarg; - if (*myoptind == 1) + if (*myoptind == 1) { if (reorder(argc, argv, options) != 0) { + *myoptind += 1; return (int)'?'; } + } p = argv[*myoptind]; @@ -158,6 +160,7 @@ __EXPORT int px4_getopt(int argc, char *argv[], const char *options, int *myopti c = isvalidopt(p[1], options, &takesarg); if (c == '?') { + *myoptind += 1; return (int)c; } @@ -165,6 +168,11 @@ __EXPORT int px4_getopt(int argc, char *argv[], const char *options, int *myopti if (takesarg) { *myoptarg = argv[*myoptind]; + + if (!*myoptarg) { //Error: option takes an argument, but there is no more argument + return -1; + } + *myoptind += 1; } diff --git a/src/platforms/nuttx/px4_layer/module.mk b/src/platforms/nuttx/px4_layer/module.mk deleted file mode 100644 index 93b55e67ab..0000000000 --- a/src/platforms/nuttx/px4_layer/module.mk +++ /dev/null @@ -1,41 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015 Mark Charlebois. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# NuttX porting layer -# - -SRCS = px4_nuttx_tasks.c \ - ../../posix/px4_layer/px4_log.c - -MAXOPTIMIZATION = -Os diff --git a/src/platforms/nuttx/px4_layer/px4_nuttx_tasks.c b/src/platforms/nuttx/px4_layer/px4_nuttx_tasks.c index bb4dea7d4a..84eed74b53 100644 --- a/src/platforms/nuttx/px4_layer/px4_nuttx_tasks.c +++ b/src/platforms/nuttx/px4_layer/px4_nuttx_tasks.c @@ -38,6 +38,7 @@ */ #include +#include #include #include #include @@ -106,3 +107,15 @@ int px4_task_delete(int pid) { return task_delete(pid); } + +const char *px4_get_taskname(void) +{ +#if CONFIG_TASK_NAME_SIZE > 0 + FAR struct tcb_s *thisproc = sched_self(); + + return thisproc->name; +#else + return "app"; +#endif +} + diff --git a/src/platforms/posix/drivers/accelsim/CMakeLists.txt b/src/platforms/posix/drivers/accelsim/CMakeLists.txt index d4146b8129..253fd3c62d 100644 --- a/src/platforms/posix/drivers/accelsim/CMakeLists.txt +++ b/src/platforms/posix/drivers/accelsim/CMakeLists.txt @@ -34,7 +34,6 @@ px4_add_module( MODULE platforms__posix__drivers__accelsim MAIN accelsim COMPILE_FLAGS - -Wno-packed SRCS accelsim.cpp DEPENDS diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index 664ea8ea8c..481eb57788 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -802,9 +802,21 @@ ACCELSIM::stop() void ACCELSIM::_measure() { - //PX4_INFO("ACCELSIM::_measure"); - /* status register and data as read back from the device */ +#if 0 + static int x = 0; + // Verify the samples are being taken at the expected rate + if (x == 99) { + x = 0; + PX4_INFO("ACCELSIM::measure %" PRIu64, hrt_absolute_time()); + + } else { + x++; + } + +#endif + + /* status register and data as read back from the device */ #pragma pack(push, 1) struct { uint8_t cmd; diff --git a/src/platforms/posix/drivers/airspeedsim/CMakeLists.txt b/src/platforms/posix/drivers/airspeedsim/CMakeLists.txt index cecefb204e..8c2a410862 100644 --- a/src/platforms/posix/drivers/airspeedsim/CMakeLists.txt +++ b/src/platforms/posix/drivers/airspeedsim/CMakeLists.txt @@ -34,7 +34,6 @@ px4_add_module( MODULE platforms__posix__drivers__airspeedsim MAIN measairspeedsim COMPILE_FLAGS - -Wno-packed -Os SRCS airspeedsim.cpp diff --git a/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp b/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp index 58f6aa9929..e3283f2fe7 100644 --- a/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp +++ b/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp @@ -251,13 +251,13 @@ AirspeedSim::ioctl(device::file_t *filp, int cmd, unsigned long arg) return -EINVAL; } - //irqstate_t flags = irqsave(); + //irqstate_t flags = px4_enter_critical_section(); if (!_reports->resize(arg)) { - //irqrestore(flags); + //px4_leave_critical_section(flags); return -ENOMEM; } - //irqrestore(flags); + //px4_leave_critical_section(flags); return OK; } @@ -371,12 +371,11 @@ AirspeedSim::update_status() { if (_sensor_ok != _last_published_sensor_ok) { /* notify about state change */ - struct subsystem_info_s info = { - true, - true, - _sensor_ok, - subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE - }; + struct subsystem_info_s info = {}; + info.present = true; + info.enabled = true; + info.ok = _sensor_ok; + info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE; if (_subsys_pub != nullptr) { orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info); diff --git a/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp b/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp index 123a22b69e..94d8d5ce7b 100644 --- a/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp +++ b/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp @@ -291,61 +291,6 @@ MEASAirspeedSim::cycle() void MEASAirspeedSim::voltage_correction(float &diff_press_pa, float &temperature) { -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) - - if (_t_system_power == -1) { - _t_system_power = orb_subscribe(ORB_ID(system_power)); - } - - if (_t_system_power == -1) { - // not available - return; - } - - bool updated = false; - orb_check(_t_system_power, &updated); - - if (updated) { - orb_copy(ORB_ID(system_power), _t_system_power, &system_power); - } - - if (system_power.voltage5V_v < 3.0f || system_power.voltage5V_v > 6.0f) { - // not valid, skip correction - return; - } - - const float slope = 65.0f; - /* - apply a piecewise linear correction, flattening at 0.5V from 5V - */ - float voltage_diff = system_power.voltage5V_v - 5.0f; - - if (voltage_diff > 0.5f) { - voltage_diff = 0.5f; - } - - if (voltage_diff < -0.5f) { - voltage_diff = -0.5f; - } - - diff_press_pa -= voltage_diff * slope; - - /* - the temperature masurement varies as well - */ - const float temp_slope = 0.887f; - voltage_diff = system_power.voltage5V_v - 5.0f; - - if (voltage_diff > 0.5f) { - voltage_diff = 0.5f; - } - - if (voltage_diff < -1.0f) { - voltage_diff = -1.0f; - } - - temperature -= voltage_diff * temp_slope; -#endif // CONFIG_ARCH_BOARD_PX4FMU_V2 } /** diff --git a/src/platforms/posix/drivers/barosim/CMakeLists.txt b/src/platforms/posix/drivers/barosim/CMakeLists.txt index 6103737bfb..6e3a105204 100644 --- a/src/platforms/posix/drivers/barosim/CMakeLists.txt +++ b/src/platforms/posix/drivers/barosim/CMakeLists.txt @@ -34,7 +34,6 @@ px4_add_module( MODULE platforms__posix__drivers__barosim MAIN barosim COMPILE_FLAGS - -Wno-packed -Os SRCS baro.cpp diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index 0ab376ed3d..248a20fef8 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -36,7 +36,6 @@ * Driver for the simulated barometric pressure sensor */ -#define __STDC_FORMAT_MACROS #include #include #include diff --git a/src/platforms/posix/drivers/df_ak8963_wrapper/CMakeLists.txt b/src/platforms/posix/drivers/df_ak8963_wrapper/CMakeLists.txt new file mode 100644 index 0000000000..9ac1b2ef75 --- /dev/null +++ b/src/platforms/posix/drivers/df_ak8963_wrapper/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# Copyright (c) 2016 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +include_directories(../../../../lib/DriverFramework/drivers) + +px4_add_module( + MODULE platforms__posix__drivers__df_ak8963_wrapper + MAIN df_ak8963_wrapper + SRCS + df_ak8963_wrapper.cpp + DEPENDS + platforms__common + df_driver_framework + df_ak8963 + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/posix/drivers/df_ak8963_wrapper/df_ak8963_wrapper.cpp b/src/platforms/posix/drivers/df_ak8963_wrapper/df_ak8963_wrapper.cpp new file mode 100644 index 0000000000..473a53e031 --- /dev/null +++ b/src/platforms/posix/drivers/df_ak8963_wrapper/df_ak8963_wrapper.cpp @@ -0,0 +1,461 @@ +/**************************************************************************** + * + * Copyright (c) 2016 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 df_ak8963_wrapper.cpp + * Lightweight driver to access the AK8963 of the DriverFramework. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include + +#include + +#include + +#include +#include + + +extern "C" { __EXPORT int df_ak8963_wrapper_main(int argc, char *argv[]); } + +using namespace DriverFramework; + + +class DfAK8963Wrapper : public AK8963 +{ +public: + DfAK8963Wrapper(enum Rotation rotation); + ~DfAK8963Wrapper(); + + + /** + * Start automatic measurement. + * + * @return 0 on success + */ + int start(); + + /** + * Stop automatic measurement. + * + * @return 0 on success + */ + int stop(); + + void print_calibration(); + +private: + int _publish(struct mag_sensor_data &data); + + void _update_mag_calibration(); + + orb_advert_t _mag_topic; + + int _param_update_sub; + + struct mag_calibration_s { + float x_offset; + float x_scale; + float y_offset; + float y_scale; + float z_offset; + float z_scale; + } _mag_calibration; + + math::Matrix<3, 3> _rotation_matrix; + + int _mag_orb_class_instance; + + perf_counter_t _mag_sample_perf; + +}; + +DfAK8963Wrapper::DfAK8963Wrapper(enum Rotation rotation) : + AK8963(MAG_DEVICE_PATH), + _mag_topic(nullptr), + _param_update_sub(-1), + _mag_calibration{}, + _mag_orb_class_instance(-1), + _mag_sample_perf(perf_alloc(PC_ELAPSED, "df_mag_read")) +{ + // Set sane default calibration values + _mag_calibration.x_scale = 1.0f; + _mag_calibration.y_scale = 1.0f; + _mag_calibration.z_scale = 1.0f; + _mag_calibration.x_offset = 0.0f; + _mag_calibration.y_offset = 0.0f; + _mag_calibration.z_offset = 0.0f; + + // Get sensor rotation matrix + get_rot_matrix(rotation, &_rotation_matrix); +} + +DfAK8963Wrapper::~DfAK8963Wrapper() +{ + perf_free(_mag_sample_perf); +} + +int DfAK8963Wrapper::start() +{ + /* Subscribe to param update topic. */ + if (_param_update_sub < 0) { + _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); + } + + /* Init device and start sensor. */ + int ret = init(); + + if (ret != 0) { + PX4_ERR("AK8963 init fail: %d", ret); + return ret; + } + + ret = AK8963::start(); + + if (ret != 0) { + PX4_ERR("AK8963 start fail: %d", ret); + return ret; + } + + /* Force getting the calibration values. */ + _update_mag_calibration(); + + return 0; +} + +int DfAK8963Wrapper::stop() +{ + /* Stop sensor. */ + int ret = AK8963::stop(); + + if (ret != 0) { + PX4_ERR("AK8963 stop fail: %d", ret); + return ret; + } + + return 0; +} + +void DfAK8963Wrapper::_update_mag_calibration() +{ + // TODO: replace magic number + for (unsigned i = 0; i < 3; ++i) { + + // TODO: remove printfs and add error counter + + char str[30]; + (void)sprintf(str, "CAL_MAG%u_ID", i); + int32_t device_id; + int res = param_get(param_find(str), &device_id); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + continue; + } + + if ((uint32_t)device_id != m_id.dev_id) { + continue; + } + + (void)sprintf(str, "CAL_MAG%u_XSCALE", i); + res = param_get(param_find(str), &_mag_calibration.x_scale); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_MAG%u_YSCALE", i); + res = param_get(param_find(str), &_mag_calibration.y_scale); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_MAG%u_ZSCALE", i); + res = param_get(param_find(str), &_mag_calibration.z_scale); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_MAG%u_XOFF", i); + res = param_get(param_find(str), &_mag_calibration.x_offset); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_MAG%u_YOFF", i); + res = param_get(param_find(str), &_mag_calibration.y_offset); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_MAG%u_ZOFF", i); + res = param_get(param_find(str), &_mag_calibration.z_offset); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + } +} + +void DfAK8963Wrapper::print_calibration() +{ + PX4_INFO("calibration x_offset: %f", (double)_mag_calibration.x_offset); + PX4_INFO("calibration x_scale: %f", (double)_mag_calibration.x_scale); + PX4_INFO("calibration y_offset: %f", (double)_mag_calibration.y_offset); + PX4_INFO("calibration y_scale: %f", (double)_mag_calibration.y_scale); + PX4_INFO("calibration z_offset: %f", (double)_mag_calibration.z_offset); + PX4_INFO("calibration z_scale: %f", (double)_mag_calibration.z_scale); +} + +int DfAK8963Wrapper::_publish(struct mag_sensor_data &data) +{ + /* Check if calibration values are still up-to-date. */ + bool updated; + orb_check(_param_update_sub, &updated); + + if (updated) { + parameter_update_s parameter_update; + orb_copy(ORB_ID(parameter_update), _param_update_sub, ¶meter_update); + + _update_mag_calibration(); + } + + /* Publish mag first. */ + perf_begin(_mag_sample_perf); + + mag_report mag_report = {}; + mag_report.timestamp = hrt_absolute_time(); + + // TODO: remove these (or get the values) + mag_report.x_raw = NAN; + mag_report.y_raw = NAN; + mag_report.z_raw = NAN; + + math::Vector<3> mag_val(data.field_x_ga, data.field_y_ga, data.field_z_ga); + + // apply sensor rotation on the accel measurement + mag_val = _rotation_matrix * mag_val; + + mag_report.x = (mag_val(0) - _mag_calibration.x_offset) * _mag_calibration.x_scale; + mag_report.y = (mag_val(1) - _mag_calibration.y_offset) * _mag_calibration.y_scale; + mag_report.z = (mag_val(2) - _mag_calibration.z_offset) * _mag_calibration.z_scale; + + + // TODO: get these right + //mag_report.scaling = -1.0f; + //mag_report.range_m_s2 = -1.0f; + + mag_report.device_id = m_id.dev_id; + + // TODO: when is this ever blocked? + if (!(m_pub_blocked)) { + + if (_mag_topic == nullptr) { + _mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report, + &_mag_orb_class_instance, ORB_PRIO_HIGH); + + } else { + orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report); + } + + } + + perf_end(_mag_sample_perf); + + /* Notify anyone waiting for data. */ + DevMgr::updateNotify(*this); + + return 0; +}; + + +namespace df_ak8963_wrapper +{ + +DfAK8963Wrapper *g_dev = nullptr; + +int start(enum Rotation rotation); +int stop(); +int info(); +void usage(); + +int start(enum Rotation rotation) +{ + g_dev = new DfAK8963Wrapper(rotation); + + if (g_dev == nullptr) { + PX4_ERR("failed instantiating DfAK8963Wrapper object"); + return -1; + } + + int ret = g_dev->start(); + + if (ret != 0) { + PX4_ERR("DfAK8963Wrapper start failed"); + return ret; + } + + // Open the MAG sensor + DevHandle h; + DevMgr::getHandle(MAG_DEVICE_PATH, h); + + if (!h.isValid()) { + DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)", + MAG_DEVICE_PATH, h.getError()); + return -1; + } + + DevMgr::releaseHandle(h); + + return 0; +} + +int stop() +{ + if (g_dev == nullptr) { + PX4_ERR("driver not running"); + return 1; + } + + int ret = g_dev->stop(); + + if (ret != 0) { + PX4_ERR("driver could not be stopped"); + return ret; + } + + delete g_dev; + g_dev = nullptr; + return 0; +} + +/** + * Print a little info about the driver. + */ +int +info() +{ + if (g_dev == nullptr) { + PX4_ERR("driver not running"); + return 1; + } + + PX4_DEBUG("state @ %p", g_dev); + + g_dev->print_calibration(); + + return 0; +} + +void +usage() +{ + PX4_INFO("Usage: df_ak8963_wrapper 'start', 'info', 'stop'"); + PX4_INFO("options:"); + PX4_INFO(" -R rotation"); +} + +} // namespace df_ak8963_wrapper + + +int +df_ak8963_wrapper_main(int argc, char *argv[]) +{ + int ch; + enum Rotation rotation = ROTATION_NONE; + int ret = 0; + int myoptind = 1; + const char *myoptarg = NULL; + + /* jump over start/off/etc and look at options first */ + while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'R': + rotation = (enum Rotation)atoi(myoptarg); + break; + + default: + df_ak8963_wrapper::usage(); + return 0; + } + } + + if (argc <= 1) { + df_ak8963_wrapper::usage(); + return 1; + } + + const char *verb = argv[myoptind]; + + + if (!strcmp(verb, "start")) { + ret = df_ak8963_wrapper::start(rotation); + } + + else if (!strcmp(verb, "stop")) { + ret = df_ak8963_wrapper::stop(); + } + + else if (!strcmp(verb, "info")) { + ret = df_ak8963_wrapper::info(); + } + + else { + df_ak8963_wrapper::usage(); + return 1; + } + + return ret; +} diff --git a/src/platforms/posix/drivers/df_bmp280_wrapper/df_bmp280_wrapper.cpp b/src/platforms/posix/drivers/df_bmp280_wrapper/df_bmp280_wrapper.cpp index 3f69985cf6..b773a225a2 100644 --- a/src/platforms/posix/drivers/df_bmp280_wrapper/df_bmp280_wrapper.cpp +++ b/src/platforms/posix/drivers/df_bmp280_wrapper/df_bmp280_wrapper.cpp @@ -115,16 +115,6 @@ DfBmp280Wrapper::~DfBmp280Wrapper() int DfBmp280Wrapper::start() { - // TODO: don't publish garbage here - baro_report baro_report = {}; - _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &baro_report, - &_baro_orb_class_instance, ORB_PRIO_DEFAULT); - - if (_baro_topic == nullptr) { - PX4_ERR("sensor_baro advert fail"); - return -1; - } - /* Init device and start sensor. */ int ret = init(); @@ -169,7 +159,7 @@ int DfBmp280Wrapper::_publish(struct baro_sensor_data &data) // TODO: verify this, it's just copied from the MS5611 driver. // Constant for now - const float MSL_PRESSURE = 101325.0f; + const double MSL_PRESSURE_KPA = 101325.0 / 1000.0; /* tropospheric properties (0-11km) for standard atmosphere */ const double T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */ @@ -178,10 +168,10 @@ int DfBmp280Wrapper::_publish(struct baro_sensor_data &data) const double R = 287.05; /* ideal gas constant in J/kg/K */ /* current pressure at MSL in kPa */ - double p1 = MSL_PRESSURE / 1000.0; + double p1 = MSL_PRESSURE_KPA; /* measured pressure in kPa */ - double p = data.pressure_pa / 1000.0; + double p = static_cast(data.pressure_pa) / 1000.0; /* * Solve: @@ -197,7 +187,11 @@ int DfBmp280Wrapper::_publish(struct baro_sensor_data &data) // TODO: when is this ever blocked? if (!(m_pub_blocked)) { - if (_baro_topic != nullptr) { + if (_baro_topic == nullptr) { + _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &baro_report, + &_baro_orb_class_instance, ORB_PRIO_DEFAULT); + + } else { orb_publish(ORB_ID(sensor_baro), _baro_topic, &baro_report); } } diff --git a/src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp b/src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp index e354176f8c..978977048a 100644 --- a/src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp +++ b/src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp @@ -59,8 +59,8 @@ #include #include -//#include -//#include + +#include #include #include @@ -74,7 +74,7 @@ using namespace DriverFramework; class DfHmc9250Wrapper : public HMC5883 { public: - DfHmc9250Wrapper(/*enum Rotation rotation*/); + DfHmc9250Wrapper(enum Rotation rotation); ~DfHmc9250Wrapper(); @@ -97,8 +97,6 @@ private: void _update_mag_calibration(); - //enum Rotation _rotation; - orb_advert_t _mag_topic; int _param_update_sub; @@ -112,20 +110,21 @@ private: float z_scale; } _mag_calibration; + math::Matrix<3, 3> _rotation_matrix; + int _mag_orb_class_instance; perf_counter_t _mag_sample_perf; }; -DfHmc9250Wrapper::DfHmc9250Wrapper(/*enum Rotation rotation*/) : +DfHmc9250Wrapper::DfHmc9250Wrapper(enum Rotation rotation) : HMC5883(MAG_DEVICE_PATH), _mag_topic(nullptr), _param_update_sub(-1), _mag_calibration{}, _mag_orb_class_instance(-1), _mag_sample_perf(perf_alloc(PC_ELAPSED, "df_mag_read")) - /*_rotation(rotation)*/ { // Set sane default calibration values _mag_calibration.x_scale = 1.0f; @@ -134,6 +133,9 @@ DfHmc9250Wrapper::DfHmc9250Wrapper(/*enum Rotation rotation*/) : _mag_calibration.x_offset = 0.0f; _mag_calibration.y_offset = 0.0f; _mag_calibration.z_offset = 0.0f; + + // Get sensor rotation matrix + get_rot_matrix(rotation, &_rotation_matrix); } DfHmc9250Wrapper::~DfHmc9250Wrapper() @@ -143,16 +145,6 @@ DfHmc9250Wrapper::~DfHmc9250Wrapper() int DfHmc9250Wrapper::start() { - // TODO: don't publish garbage here - mag_report mag_report = {}; - _mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report, - &_mag_orb_class_instance, ORB_PRIO_DEFAULT); - - if (_mag_topic == nullptr) { - PX4_ERR("sensor_mag advert fail"); - return -1; - } - /* Subscribe to param update topic. */ if (_param_update_sub < 0) { _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -288,9 +280,19 @@ int DfHmc9250Wrapper::_publish(struct mag_sensor_data &data) mag_report.x_raw = NAN; mag_report.y_raw = NAN; mag_report.z_raw = NAN; - mag_report.x = (data.field_x_ga - _mag_calibration.x_offset) * _mag_calibration.x_scale; - mag_report.y = (data.field_y_ga - _mag_calibration.y_offset) * _mag_calibration.y_scale; - mag_report.z = (data.field_z_ga - _mag_calibration.z_offset) * _mag_calibration.z_scale; + + + math::Vector<3> mag_val((data.field_x_ga - _mag_calibration.x_offset) * _mag_calibration.x_scale, + (data.field_y_ga - _mag_calibration.y_offset) * _mag_calibration.y_scale, + (data.field_z_ga - _mag_calibration.z_offset) * _mag_calibration.z_scale); + + // apply sensor rotation on the accel measurement + mag_val = _rotation_matrix * mag_val; + + mag_report.x = mag_val(0); + mag_report.y = mag_val(1); + mag_report.z = mag_val(2); + // TODO: get these right //mag_report.scaling = -1.0f; @@ -301,7 +303,11 @@ int DfHmc9250Wrapper::_publish(struct mag_sensor_data &data) // TODO: when is this ever blocked? if (!(m_pub_blocked)) { - if (_mag_topic != nullptr) { + if (_mag_topic == nullptr) { + _mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report, + &_mag_orb_class_instance, ORB_PRIO_HIGH); + + } else { orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report); } @@ -321,14 +327,14 @@ namespace df_hmc5883_wrapper DfHmc9250Wrapper *g_dev = nullptr; -int start(/* enum Rotation rotation */); +int start(enum Rotation rotation); int stop(); int info(); void usage(); -int start(/*enum Rotation rotation*/) +int start(enum Rotation rotation) { - g_dev = new DfHmc9250Wrapper(/*rotation*/); + g_dev = new DfHmc9250Wrapper(rotation); if (g_dev == nullptr) { PX4_ERR("failed instantiating DfHmc9250Wrapper object"); @@ -395,9 +401,9 @@ info() void usage() { - PX4_WARN("Usage: df_hmc5883_wrapper 'start', 'info', 'stop'"); - PX4_WARN("options:"); - //PX4_WARN(" -R rotation"); + PX4_INFO("Usage: df_hmc5883_wrapper 'start', 'info', 'stop'"); + PX4_INFO("options:"); + PX4_INFO(" -R rotation"); } } // namespace df_hmc5883_wrapper @@ -407,7 +413,7 @@ int df_hmc5883_wrapper_main(int argc, char *argv[]) { int ch; - // enum Rotation rotation = ROTATION_NONE; + enum Rotation rotation = ROTATION_NONE; int ret = 0; int myoptind = 1; const char *myoptarg = NULL; @@ -415,9 +421,9 @@ df_hmc5883_wrapper_main(int argc, char *argv[]) /* jump over start/off/etc and look at options first */ while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { switch (ch) { - //case 'R': - // rotation = (enum Rotation)atoi(myoptarg); - // break; + case 'R': + rotation = (enum Rotation)atoi(myoptarg); + break; default: df_hmc5883_wrapper::usage(); @@ -434,7 +440,7 @@ df_hmc5883_wrapper_main(int argc, char *argv[]) if (!strcmp(verb, "start")) { - ret = df_hmc5883_wrapper::start(/*rotation*/); + ret = df_hmc5883_wrapper::start(rotation); } else if (!strcmp(verb, "stop")) { diff --git a/src/platforms/posix/drivers/df_isl29501_wrapper/df_isl29501_wrapper.cpp b/src/platforms/posix/drivers/df_isl29501_wrapper/df_isl29501_wrapper.cpp index 43bb25205e..3aa4fb88e2 100644 --- a/src/platforms/posix/drivers/df_isl29501_wrapper/df_isl29501_wrapper.cpp +++ b/src/platforms/posix/drivers/df_isl29501_wrapper/df_isl29501_wrapper.cpp @@ -118,11 +118,6 @@ DfISL29501Wrapper::~DfISL29501Wrapper() int DfISL29501Wrapper::start() { - - struct distance_sensor_s d; - _range_topic = orb_advertise_multi(ORB_ID(distance_sensor), &d, - &_orb_class_instance, ORB_PRIO_DEFAULT); - int ret; ret = ISL29501::init(); @@ -156,10 +151,6 @@ int DfISL29501Wrapper::stop() int DfISL29501Wrapper::_publish(struct range_sensor_data &data) { - if (!_range_topic) { - return 1; - } - struct distance_sensor_s d; memset(&d, 0, sizeof(d)); @@ -178,7 +169,13 @@ int DfISL29501Wrapper::_publish(struct range_sensor_data &data) d.covariance = 0.0f; - orb_publish(ORB_ID(distance_sensor), _range_topic, &d); + if (_range_topic == nullptr) { + _range_topic = orb_advertise_multi(ORB_ID(distance_sensor), &d, + &_orb_class_instance, ORB_PRIO_DEFAULT); + + } else { + orb_publish(ORB_ID(distance_sensor), _range_topic, &d); + } /* Notify anyone waiting for data. */ DevMgr::updateNotify(*this); diff --git a/src/platforms/posix/drivers/df_lsm9ds1_wrapper/CMakeLists.txt b/src/platforms/posix/drivers/df_lsm9ds1_wrapper/CMakeLists.txt new file mode 100644 index 0000000000..8a514ad8e3 --- /dev/null +++ b/src/platforms/posix/drivers/df_lsm9ds1_wrapper/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# Copyright (c) 2016 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +include_directories(../../../../lib/DriverFramework/drivers) + +px4_add_module( + MODULE platforms__posix__drivers__df_lsm9ds1_wrapper + MAIN df_lsm9ds1_wrapper + SRCS + df_lsm9ds1_wrapper.cpp + DEPENDS + platforms__common + df_driver_framework + df_lsm9ds1 + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/posix/drivers/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp b/src/platforms/posix/drivers/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp new file mode 100644 index 0000000000..490423f976 --- /dev/null +++ b/src/platforms/posix/drivers/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp @@ -0,0 +1,884 @@ +/**************************************************************************** + * + * Copyright (c) 2016 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 df_lsm9ds1_wrapper.cpp + * Lightweight driver to access the MPU9250 of the DriverFramework. + * + * @author Miguel Arroyo + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include + +#include +#include + +// We don't want to auto publish, therefore set this to 0. +#define LSM9DS1_NEVER_AUTOPUBLISH_US 0 + + +extern "C" { __EXPORT int df_lsm9ds1_wrapper_main(int argc, char *argv[]); } + +using namespace DriverFramework; + + +class DfLsm9ds1Wrapper : public LSM9DS1 +{ +public: + DfLsm9ds1Wrapper(bool mag_enabled, enum Rotation rotation); + ~DfLsm9ds1Wrapper(); + + + /** + * Start automatic measurement. + * + * @return 0 on success + */ + int start(); + + /** + * Stop automatic measurement. + * + * @return 0 on success + */ + int stop(); + + /** + * Print some debug info. + */ + void info(); + +private: + int _publish(struct imu_sensor_data &data); + + void _update_accel_calibration(); + void _update_gyro_calibration(); + void _update_mag_calibration(); + + orb_advert_t _accel_topic; + orb_advert_t _gyro_topic; + orb_advert_t _mag_topic; + orb_advert_t _mavlink_log_pub; + + int _param_update_sub; + + struct accel_calibration_s { + float x_offset; + float x_scale; + float y_offset; + float y_scale; + float z_offset; + float z_scale; + } _accel_calibration; + + struct gyro_calibration_s { + float x_offset; + float x_scale; + float y_offset; + float y_scale; + float z_offset; + float z_scale; + } _gyro_calibration; + + struct mag_calibration_s { + float x_offset; + float x_scale; + float y_offset; + float y_scale; + float z_offset; + float z_scale; + } _mag_calibration; + + math::Matrix<3, 3> _rotation_matrix; + + int _accel_orb_class_instance; + int _gyro_orb_class_instance; + int _mag_orb_class_instance; + + Integrator _accel_int; + Integrator _gyro_int; + + unsigned _publish_count; + + perf_counter_t _read_counter; + perf_counter_t _error_counter; + perf_counter_t _fifo_overflow_counter; + perf_counter_t _fifo_corruption_counter; + perf_counter_t _gyro_range_hit_counter; + perf_counter_t _accel_range_hit_counter; + perf_counter_t _publish_perf; + + hrt_abstime _last_accel_range_hit_time; + uint64_t _last_accel_range_hit_count; + + bool _mag_enabled; +}; + +DfLsm9ds1Wrapper::DfLsm9ds1Wrapper(bool mag_enabled, enum Rotation rotation) : + LSM9DS1(IMU_DEVICE_ACC_GYRO, IMU_DEVICE_MAG), + _accel_topic(nullptr), + _gyro_topic(nullptr), + _mag_topic(nullptr), + _mavlink_log_pub(nullptr), + _param_update_sub(-1), + _accel_calibration{}, + _gyro_calibration{}, + _mag_calibration{}, + _accel_orb_class_instance(-1), + _gyro_orb_class_instance(-1), + _mag_orb_class_instance(-1), + _accel_int(LSM9DS1_NEVER_AUTOPUBLISH_US, false), + _gyro_int(LSM9DS1_NEVER_AUTOPUBLISH_US, true), + _publish_count(0), + _read_counter(perf_alloc(PC_COUNT, "lsm9ds1_reads")), + _error_counter(perf_alloc(PC_COUNT, "lsm9ds1_errors")), + _fifo_overflow_counter(perf_alloc(PC_COUNT, "lsm9ds1_fifo_overflows")), + _fifo_corruption_counter(perf_alloc(PC_COUNT, "lsm9ds1_fifo_corruptions")), + _gyro_range_hit_counter(perf_alloc(PC_COUNT, "lsm9ds1_gyro_range_hits")), + _accel_range_hit_counter(perf_alloc(PC_COUNT, "lsm9ds1_accel_range_hits")), + _publish_perf(perf_alloc(PC_ELAPSED, "lsm9ds1_publish")), + _last_accel_range_hit_time(0), + _last_accel_range_hit_count(0), + _mag_enabled(mag_enabled) +{ + // Set sane default calibration values + _accel_calibration.x_scale = 1.0f; + _accel_calibration.y_scale = 1.0f; + _accel_calibration.z_scale = 1.0f; + _accel_calibration.x_offset = 0.0f; + _accel_calibration.y_offset = 0.0f; + _accel_calibration.z_offset = 0.0f; + + _gyro_calibration.x_scale = 1.0f; + _gyro_calibration.y_scale = 1.0f; + _gyro_calibration.z_scale = 1.0f; + _gyro_calibration.x_offset = 0.0f; + _gyro_calibration.y_offset = 0.0f; + _gyro_calibration.z_offset = 0.0f; + + if (_mag_enabled) { + _mag_calibration.x_scale = 1.0f; + _mag_calibration.y_scale = 1.0f; + _mag_calibration.z_scale = 1.0f; + _mag_calibration.x_offset = 0.0f; + _mag_calibration.y_offset = 0.0f; + _mag_calibration.z_offset = 0.0f; + } + + // Get sensor rotation matrix + get_rot_matrix(rotation, &_rotation_matrix); +} + +DfLsm9ds1Wrapper::~DfLsm9ds1Wrapper() +{ + perf_free(_read_counter); + perf_free(_error_counter); + perf_free(_fifo_overflow_counter); + perf_free(_fifo_corruption_counter); + perf_free(_gyro_range_hit_counter); + perf_free(_accel_range_hit_counter); + + perf_free(_publish_perf); +} + +int DfLsm9ds1Wrapper::start() +{ + // TODO: don't publish garbage here + accel_report accel_report = {}; + _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &accel_report, + &_accel_orb_class_instance, ORB_PRIO_DEFAULT); + + if (_accel_topic == nullptr) { + PX4_ERR("sensor_accel advert fail"); + return -1; + } + + // TODO: don't publish garbage here + gyro_report gyro_report = {}; + _gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &gyro_report, + &_gyro_orb_class_instance, ORB_PRIO_DEFAULT); + + if (_gyro_topic == nullptr) { + PX4_ERR("sensor_gyro advert fail"); + return -1; + } + + if (_mag_enabled) { + mag_report mag_report = {}; + _mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report, + &_mag_orb_class_instance, ORB_PRIO_DEFAULT); + + if (_mag_topic == nullptr) { + PX4_ERR("sensor_mag advert fail"); + return -1; + } + } + + /* Subscribe to param update topic. */ + if (_param_update_sub < 0) { + _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); + } + + /* Init device and start sensor. */ + int ret = init(); + + if (ret != 0) { + PX4_ERR("LSM9DS1 init fail: %d", ret); + return ret; + } + + ret = LSM9DS1::start(); + + if (ret != 0) { + PX4_ERR("LSM9DS1 start fail: %d", ret); + return ret; + } + + PX4_DEBUG("LSM9DS1 device id is: %d", m_id.dev_id); + + /* Force getting the calibration values. */ + _update_accel_calibration(); + _update_gyro_calibration(); + _update_mag_calibration(); + + return 0; +} + +int DfLsm9ds1Wrapper::stop() +{ + /* Stop sensor. */ + int ret = LSM9DS1::stop(); + + if (ret != 0) { + PX4_ERR("LSM9DS1 stop fail: %d", ret); + return ret; + } + + return 0; +} + +void DfLsm9ds1Wrapper::info() +{ + perf_print_counter(_read_counter); + perf_print_counter(_error_counter); + perf_print_counter(_fifo_overflow_counter); + perf_print_counter(_fifo_corruption_counter); + perf_print_counter(_gyro_range_hit_counter); + perf_print_counter(_accel_range_hit_counter); + + perf_print_counter(_publish_perf); +} + +void DfLsm9ds1Wrapper::_update_gyro_calibration() +{ + // TODO: replace magic number + for (unsigned i = 0; i < 3; ++i) { + + // TODO: remove printfs and add error counter + + char str[30]; + (void)sprintf(str, "CAL_GYRO%u_ID", i); + int32_t device_id; + int res = param_get(param_find(str), &device_id); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + continue; + } + + if ((uint32_t)device_id != m_id.dev_id) { + continue; + } + + (void)sprintf(str, "CAL_GYRO%u_XSCALE", i); + res = param_get(param_find(str), &_gyro_calibration.x_scale); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_GYRO%u_YSCALE", i); + res = param_get(param_find(str), &_gyro_calibration.y_scale); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_GYRO%u_ZSCALE", i); + res = param_get(param_find(str), &_gyro_calibration.z_scale); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_GYRO%u_XOFF", i); + res = param_get(param_find(str), &_gyro_calibration.x_offset); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_GYRO%u_YOFF", i); + res = param_get(param_find(str), &_gyro_calibration.y_offset); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_GYRO%u_ZOFF", i); + res = param_get(param_find(str), &_gyro_calibration.z_offset); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + // We got calibration values, let's exit. + return; + } + + _gyro_calibration.x_scale = 1.0f; + _gyro_calibration.y_scale = 1.0f; + _gyro_calibration.z_scale = 1.0f; + _gyro_calibration.x_offset = 0.0f; + _gyro_calibration.y_offset = 0.0f; + _gyro_calibration.z_offset = 0.0f; +} + +void DfLsm9ds1Wrapper::_update_accel_calibration() +{ + // TODO: replace magic number + for (unsigned i = 0; i < 3; ++i) { + + // TODO: remove printfs and add error counter + + char str[30]; + (void)sprintf(str, "CAL_ACC%u_ID", i); + int32_t device_id; + int res = param_get(param_find(str), &device_id); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + continue; + } + + if ((uint32_t)device_id != m_id.dev_id) { + continue; + } + + (void)sprintf(str, "CAL_ACC%u_XSCALE", i); + res = param_get(param_find(str), &_accel_calibration.x_scale); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_ACC%u_YSCALE", i); + res = param_get(param_find(str), &_accel_calibration.y_scale); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_ACC%u_ZSCALE", i); + res = param_get(param_find(str), &_accel_calibration.z_scale); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_ACC%u_XOFF", i); + res = param_get(param_find(str), &_accel_calibration.x_offset); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_ACC%u_YOFF", i); + res = param_get(param_find(str), &_accel_calibration.y_offset); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_ACC%u_ZOFF", i); + res = param_get(param_find(str), &_accel_calibration.z_offset); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + // We got calibration values, let's exit. + return; + } + + // Set sane default calibration values + _accel_calibration.x_scale = 1.0f; + _accel_calibration.y_scale = 1.0f; + _accel_calibration.z_scale = 1.0f; + _accel_calibration.x_offset = 0.0f; + _accel_calibration.y_offset = 0.0f; + _accel_calibration.z_offset = 0.0f; +} + +void DfLsm9ds1Wrapper::_update_mag_calibration() +{ + if (!_mag_enabled) { + return; + } + + // TODO: replace magic number + for (unsigned i = 0; i < 3; ++i) { + + // TODO: remove printfs and add error counter + + char str[30]; + (void)sprintf(str, "CAL_MAG%u_ID", i); + int32_t device_id; + int res = param_get(param_find(str), &device_id); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + continue; + } + + if ((uint32_t)device_id != m_id.dev_id) { + continue; + } + + (void)sprintf(str, "CAL_MAG%u_XSCALE", i); + res = param_get(param_find(str), &_mag_calibration.x_scale); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_MAG%u_YSCALE", i); + res = param_get(param_find(str), &_mag_calibration.y_scale); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_MAG%u_ZSCALE", i); + res = param_get(param_find(str), &_mag_calibration.z_scale); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_MAG%u_XOFF", i); + res = param_get(param_find(str), &_mag_calibration.x_offset); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_MAG%u_YOFF", i); + res = param_get(param_find(str), &_mag_calibration.y_offset); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_MAG%u_ZOFF", i); + res = param_get(param_find(str), &_mag_calibration.z_offset); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + // We got calibration values, let's exit. + return; + } + + // Set sane default calibration values + _mag_calibration.x_scale = 1.0f; + _mag_calibration.y_scale = 1.0f; + _mag_calibration.z_scale = 1.0f; + _mag_calibration.x_offset = 0.0f; + _mag_calibration.y_offset = 0.0f; + _mag_calibration.z_offset = 0.0f; +} + +int DfLsm9ds1Wrapper::_publish(struct imu_sensor_data &data) +{ + /* Check if calibration values are still up-to-date. */ + bool updated; + orb_check(_param_update_sub, &updated); + + if (updated) { + parameter_update_s parameter_update; + orb_copy(ORB_ID(parameter_update), _param_update_sub, ¶meter_update); + + _update_accel_calibration(); + _update_gyro_calibration(); + } + + math::Vector<3> vec_integrated_unused; + uint64_t integral_dt_unused; + + math::Vector<3> accel_val((data.accel_m_s2_x - _accel_calibration.x_offset) * _accel_calibration.x_scale, + (data.accel_m_s2_y - _accel_calibration.y_offset) * _accel_calibration.y_scale, + (data.accel_m_s2_z - _accel_calibration.z_offset) * _accel_calibration.z_scale); + + // apply sensor rotation on the accel measurement + accel_val = _rotation_matrix * accel_val; + + _accel_int.put_with_interval(data.fifo_sample_interval_us, + accel_val, + vec_integrated_unused, + integral_dt_unused); + + math::Vector<3> gyro_val((data.gyro_rad_s_x - _gyro_calibration.x_offset) * _gyro_calibration.x_scale, + (data.gyro_rad_s_y - _gyro_calibration.y_offset) * _gyro_calibration.y_scale, + (data.gyro_rad_s_z - _gyro_calibration.z_offset) * _gyro_calibration.z_scale); + + // apply sensor rotation on the gyro measurement + gyro_val = _rotation_matrix * gyro_val; + + _gyro_int.put_with_interval(data.fifo_sample_interval_us, + gyro_val, + vec_integrated_unused, + integral_dt_unused); + + // The driver empties the FIFO buffer at 1kHz, however we only need to publish at 250Hz. + // Therefore, only publish every forth time. + ++_publish_count; + + if (_publish_count < 4) { + return 0; + } + + _publish_count = 0; + + // Update all the counters. + perf_set_count(_read_counter, data.read_counter); + perf_set_count(_error_counter, data.error_counter); + perf_set_count(_fifo_overflow_counter, data.fifo_overflow_counter); + perf_set_count(_fifo_corruption_counter, data.fifo_overflow_counter); + perf_set_count(_gyro_range_hit_counter, data.gyro_range_hit_counter); + perf_set_count(_accel_range_hit_counter, data.accel_range_hit_counter); + + perf_begin(_publish_perf); + + accel_report accel_report = {}; + gyro_report gyro_report = {}; + mag_report mag_report = {}; + + accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time(); + mag_report.timestamp = accel_report.timestamp; + + // TODO: get these right + gyro_report.scaling = -1.0f; + gyro_report.range_rad_s = -1.0f; + gyro_report.device_id = m_id.dev_id; + + accel_report.scaling = -1.0f; + accel_report.range_m_s2 = -1.0f; + accel_report.device_id = m_id.dev_id; + + if (_mag_enabled) { + mag_report.scaling = -1.0f; + mag_report.range_ga = -1.0f; + mag_report.device_id = m_id.dev_id; + } + + // TODO: remove these (or get the values) + gyro_report.x_raw = NAN; + gyro_report.y_raw = NAN; + gyro_report.z_raw = NAN; + + accel_report.x_raw = NAN; + accel_report.y_raw = NAN; + accel_report.z_raw = NAN; + + if (_mag_enabled) { + mag_report.x_raw = NAN; + mag_report.y_raw = NAN; + mag_report.z_raw = NAN; + } + + math::Vector<3> gyro_val_filt; + math::Vector<3> accel_val_filt; + + // Read and reset. + math::Vector<3> gyro_val_integ = _gyro_int.get_and_filtered(true, gyro_report.integral_dt, gyro_val_filt); + math::Vector<3> accel_val_integ = _accel_int.get_and_filtered(true, accel_report.integral_dt, accel_val_filt); + + // Use the filtered (by integration) values to get smoother / less noisy data. + gyro_report.x = gyro_val_filt(0); + gyro_report.y = gyro_val_filt(1); + gyro_report.z = gyro_val_filt(2); + + accel_report.x = accel_val_filt(0); + accel_report.y = accel_val_filt(1); + accel_report.z = accel_val_filt(2); + + if (_mag_enabled) { + + math::Vector<3> mag_val((data.mag_ga_x - _mag_calibration.x_offset) * _mag_calibration.x_scale, + (data.mag_ga_y - _mag_calibration.y_offset) * _mag_calibration.y_scale, + (data.mag_ga_z - _mag_calibration.z_offset) * _mag_calibration.z_scale); + + mag_val = _rotation_matrix * mag_val; + + mag_report.x = mag_val(0); + mag_report.y = mag_val(1); + mag_report.z = mag_val(2); + } + + gyro_report.x_integral = gyro_val_integ(0); + gyro_report.y_integral = gyro_val_integ(1); + gyro_report.z_integral = gyro_val_integ(2); + + accel_report.x_integral = accel_val_integ(0); + accel_report.y_integral = accel_val_integ(1); + accel_report.z_integral = accel_val_integ(2); + + // TODO: when is this ever blocked? + if (!(m_pub_blocked)) { + + + if (_gyro_topic != nullptr) { + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report); + } + + if (_accel_topic != nullptr) { + orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); + } + + if (_mag_topic != nullptr) { + orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report); + } + + /* Notify anyone waiting for data. */ + DevMgr::updateNotify(*this); + + // Report if there are high vibrations, every 10 times it happens. + const bool threshold_reached = (data.accel_range_hit_counter - _last_accel_range_hit_count > 10); + + // Report every 5s. + const bool due_to_report = (hrt_elapsed_time(&_last_accel_range_hit_time) > 5000000); + + if (threshold_reached && due_to_report) { + mavlink_log_critical(&_mavlink_log_pub, + "High accelerations, range exceeded %llu times", + data.accel_range_hit_counter); + + _last_accel_range_hit_time = hrt_absolute_time(); + _last_accel_range_hit_count = data.accel_range_hit_counter; + } + } + + perf_end(_publish_perf); + + // TODO: check the return codes of this function + return 0; +}; + + +namespace df_lsm9ds1_wrapper +{ + +DfLsm9ds1Wrapper *g_dev = nullptr; + +int start(bool mag_enabled, enum Rotation rotation); +int stop(); +int info(); +void usage(); + +int start(bool mag_enabled, enum Rotation rotation) +{ + g_dev = new DfLsm9ds1Wrapper(mag_enabled, rotation); + + if (g_dev == nullptr) { + PX4_ERR("failed instantiating DfLsm9ds1Wrapper object"); + return -1; + } + + int ret = g_dev->start(); + + if (ret != 0) { + PX4_ERR("DfLsm9ds1Wrapper start failed"); + return ret; + } + + // Open the IMU sensor + DevHandle h; + DevMgr::getHandle(IMU_DEVICE_ACC_GYRO, h); + + if (!h.isValid()) { + DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)", + IMU_DEVICE_ACC_GYRO, h.getError()); + return -1; + } + + DevMgr::releaseHandle(h); + + DevMgr::getHandle(IMU_DEVICE_MAG, h); + + if (!h.isValid()) { + DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)", + IMU_DEVICE_MAG, h.getError()); + return -1; + } + + DevMgr::releaseHandle(h); + return 0; +} + +int stop() +{ + if (g_dev == nullptr) { + PX4_ERR("driver not running"); + return 1; + } + + int ret = g_dev->stop(); + + if (ret != 0) { + PX4_ERR("driver could not be stopped"); + return ret; + } + + delete g_dev; + g_dev = nullptr; + return 0; +} + +/** + * Print a little info about the driver. + */ +int +info() +{ + if (g_dev == nullptr) { + PX4_ERR("driver not running"); + return 1; + } + + PX4_INFO("state @ %p", g_dev); + g_dev->info(); + + return 0; +} + +void +usage() +{ + PX4_INFO("Usage: df_lsm9ds1_wrapper 'start', 'info', 'stop', 'start_without_mag'"); + PX4_INFO("options:"); + PX4_INFO(" -R rotation"); +} + +} // namespace df_lsm9ds1_wrapper + + +int +df_lsm9ds1_wrapper_main(int argc, char *argv[]) +{ + int ch; + enum Rotation rotation = ROTATION_NONE; + int ret = 0; + int myoptind = 1; + const char *myoptarg = NULL; + + /* jump over start/off/etc and look at options first */ + while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'R': + rotation = (enum Rotation)atoi(myoptarg); + break; + + default: + df_lsm9ds1_wrapper::usage(); + return 0; + } + } + + if (argc <= 1) { + df_lsm9ds1_wrapper::usage(); + return 1; + } + + const char *verb = argv[myoptind]; + + if (!strcmp(verb, "start_without_mag")) { + ret = df_lsm9ds1_wrapper::start(false, rotation); + } + + else if (!strcmp(verb, "start")) { + ret = df_lsm9ds1_wrapper::start(true, rotation); + } + + else if (!strcmp(verb, "stop")) { + ret = df_lsm9ds1_wrapper::stop(); + } + + else if (!strcmp(verb, "info")) { + ret = df_lsm9ds1_wrapper::info(); + } + + else { + df_lsm9ds1_wrapper::usage(); + return 1; + } + + return ret; +} diff --git a/src/platforms/posix/drivers/df_mpu6050_wrapper/CMakeLists.txt b/src/platforms/posix/drivers/df_mpu6050_wrapper/CMakeLists.txt new file mode 100644 index 0000000000..3c5514e600 --- /dev/null +++ b/src/platforms/posix/drivers/df_mpu6050_wrapper/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# Copyright (c) 2016 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +include_directories(../../../../lib/DriverFramework/drivers) + +px4_add_module( + MODULE platforms__posix__drivers__df_mpu6050_wrapper + MAIN df_mpu6050_wrapper + SRCS + df_mpu6050_wrapper.cpp + DEPENDS + platforms__common + df_driver_framework + df_mpu6050 + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/posix/drivers/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp b/src/platforms/posix/drivers/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp new file mode 100644 index 0000000000..08fcb50217 --- /dev/null +++ b/src/platforms/posix/drivers/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp @@ -0,0 +1,716 @@ +/**************************************************************************** + * + * Copyright (c) 2016 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 df_mpu6050_wrapper.cpp + * Lightweight driver to access the MPU6050 of the DriverFramework. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include + +#include +#include + +// We don't want to auto publish, therefore set this to 0. +#define MPU6050_NEVER_AUTOPUBLISH_US 0 + + +extern "C" { __EXPORT int df_mpu6050_wrapper_main(int argc, char *argv[]); } + +using namespace DriverFramework; + + +class DfMPU6050Wrapper : public MPU6050 +{ +public: + DfMPU6050Wrapper(enum Rotation rotation); + ~DfMPU6050Wrapper(); + + + /** + * Start automatic measurement. + * + * @return 0 on success + */ + int start(); + + /** + * Stop automatic measurement. + * + * @return 0 on success + */ + int stop(); + + /** + * Print some debug info. + */ + void info(); + +private: + int _publish(struct imu_sensor_data &data); + + void _update_accel_calibration(); + void _update_gyro_calibration(); + + orb_advert_t _accel_topic; + orb_advert_t _gyro_topic; + + orb_advert_t _mavlink_log_pub; + + int _param_update_sub; + + struct accel_calibration_s { + float x_offset; + float x_scale; + float y_offset; + float y_scale; + float z_offset; + float z_scale; + } _accel_calibration; + + struct gyro_calibration_s { + float x_offset; + float x_scale; + float y_offset; + float y_scale; + float z_offset; + float z_scale; + } _gyro_calibration; + + math::Matrix<3, 3> _rotation_matrix; + + int _accel_orb_class_instance; + int _gyro_orb_class_instance; + + Integrator _accel_int; + Integrator _gyro_int; + + unsigned _publish_count; + + perf_counter_t _read_counter; + perf_counter_t _error_counter; + perf_counter_t _fifo_overflow_counter; + perf_counter_t _fifo_corruption_counter; + perf_counter_t _gyro_range_hit_counter; + perf_counter_t _accel_range_hit_counter; + perf_counter_t _publish_perf; + + hrt_abstime _last_accel_range_hit_time; + uint64_t _last_accel_range_hit_count; + +}; + +DfMPU6050Wrapper::DfMPU6050Wrapper(enum Rotation rotation) : + MPU6050(IMU_DEVICE_PATH), + _accel_topic(nullptr), + _gyro_topic(nullptr), + _mavlink_log_pub(nullptr), + _param_update_sub(-1), + _accel_calibration{}, + _gyro_calibration{}, + _accel_orb_class_instance(-1), + _gyro_orb_class_instance(-1), + _accel_int(MPU6050_NEVER_AUTOPUBLISH_US, false), + _gyro_int(MPU6050_NEVER_AUTOPUBLISH_US, true), + _publish_count(0), + _read_counter(perf_alloc(PC_COUNT, "mpu6050_reads")), + _error_counter(perf_alloc(PC_COUNT, "mpu6050_errors")), + _fifo_overflow_counter(perf_alloc(PC_COUNT, "mpu6050_fifo_overflows")), + _fifo_corruption_counter(perf_alloc(PC_COUNT, "mpu6050_fifo_corruptions")), + _gyro_range_hit_counter(perf_alloc(PC_COUNT, "mpu6050_gyro_range_hits")), + _accel_range_hit_counter(perf_alloc(PC_COUNT, "mpu6050_accel_range_hits")), + _publish_perf(perf_alloc(PC_ELAPSED, "mpu6050_publish")), + _last_accel_range_hit_time(0), + _last_accel_range_hit_count(0) +{ + // Set sane default calibration values + _accel_calibration.x_scale = 1.0f; + _accel_calibration.y_scale = 1.0f; + _accel_calibration.z_scale = 1.0f; + _accel_calibration.x_offset = 0.0f; + _accel_calibration.y_offset = 0.0f; + _accel_calibration.z_offset = 0.0f; + + _gyro_calibration.x_scale = 1.0f; + _gyro_calibration.y_scale = 1.0f; + _gyro_calibration.z_scale = 1.0f; + _gyro_calibration.x_offset = 0.0f; + _gyro_calibration.y_offset = 0.0f; + _gyro_calibration.z_offset = 0.0f; + + // Get sensor rotation matrix + get_rot_matrix(rotation, &_rotation_matrix); +} + +DfMPU6050Wrapper::~DfMPU6050Wrapper() +{ + perf_free(_read_counter); + perf_free(_error_counter); + perf_free(_fifo_overflow_counter); + perf_free(_fifo_corruption_counter); + perf_free(_gyro_range_hit_counter); + perf_free(_accel_range_hit_counter); + + perf_free(_publish_perf); +} + +int DfMPU6050Wrapper::start() +{ + /* Subscribe to param update topic. */ + if (_param_update_sub < 0) { + _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); + } + + /* Init device and start sensor. */ + int ret = init(); + + if (ret != 0) { + PX4_ERR("MPU6050 init fail: %d", ret); + return ret; + } + + ret = MPU6050::start(); + + if (ret != 0) { + PX4_ERR("MPU6050 start fail: %d", ret); + return ret; + } + + PX4_DEBUG("MPU6050 device id is: %d", m_id.dev_id); + + /* Force getting the calibration values. */ + _update_accel_calibration(); + _update_gyro_calibration(); + + return 0; +} + +int DfMPU6050Wrapper::stop() +{ + /* Stop sensor. */ + int ret = MPU6050::stop(); + + if (ret != 0) { + PX4_ERR("MPU6050 stop fail: %d", ret); + return ret; + } + + return 0; +} + +void DfMPU6050Wrapper::info() +{ + perf_print_counter(_read_counter); + perf_print_counter(_error_counter); + perf_print_counter(_fifo_overflow_counter); + perf_print_counter(_fifo_corruption_counter); + perf_print_counter(_gyro_range_hit_counter); + perf_print_counter(_accel_range_hit_counter); + + perf_print_counter(_publish_perf); +} + +void DfMPU6050Wrapper::_update_gyro_calibration() +{ + // TODO: replace magic number + for (unsigned i = 0; i < 3; ++i) { + + // TODO: remove printfs and add error counter + + char str[30]; + (void)sprintf(str, "CAL_GYRO%u_ID", i); + int32_t device_id; + int res = param_get(param_find(str), &device_id); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + continue; + } + + if ((uint32_t)device_id != m_id.dev_id) { + continue; + } + + (void)sprintf(str, "CAL_GYRO%u_XSCALE", i); + res = param_get(param_find(str), &_gyro_calibration.x_scale); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_GYRO%u_YSCALE", i); + res = param_get(param_find(str), &_gyro_calibration.y_scale); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_GYRO%u_ZSCALE", i); + res = param_get(param_find(str), &_gyro_calibration.z_scale); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_GYRO%u_XOFF", i); + res = param_get(param_find(str), &_gyro_calibration.x_offset); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_GYRO%u_YOFF", i); + res = param_get(param_find(str), &_gyro_calibration.y_offset); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_GYRO%u_ZOFF", i); + res = param_get(param_find(str), &_gyro_calibration.z_offset); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + // We got calibration values, let's exit. + return; + } + + _gyro_calibration.x_scale = 1.0f; + _gyro_calibration.y_scale = 1.0f; + _gyro_calibration.z_scale = 1.0f; + _gyro_calibration.x_offset = 0.0f; + _gyro_calibration.y_offset = 0.0f; + _gyro_calibration.z_offset = 0.0f; +} + +void DfMPU6050Wrapper::_update_accel_calibration() +{ + // TODO: replace magic number + for (unsigned i = 0; i < 3; ++i) { + + // TODO: remove printfs and add error counter + + char str[30]; + (void)sprintf(str, "CAL_ACC%u_ID", i); + int32_t device_id; + int res = param_get(param_find(str), &device_id); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + continue; + } + + if ((uint32_t)device_id != m_id.dev_id) { + continue; + } + + (void)sprintf(str, "CAL_ACC%u_XSCALE", i); + res = param_get(param_find(str), &_accel_calibration.x_scale); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_ACC%u_YSCALE", i); + res = param_get(param_find(str), &_accel_calibration.y_scale); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_ACC%u_ZSCALE", i); + res = param_get(param_find(str), &_accel_calibration.z_scale); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_ACC%u_XOFF", i); + res = param_get(param_find(str), &_accel_calibration.x_offset); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_ACC%u_YOFF", i); + res = param_get(param_find(str), &_accel_calibration.y_offset); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_ACC%u_ZOFF", i); + res = param_get(param_find(str), &_accel_calibration.z_offset); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + // We got calibration values, let's exit. + return; + } + + // Set sane default calibration values + _accel_calibration.x_scale = 1.0f; + _accel_calibration.y_scale = 1.0f; + _accel_calibration.z_scale = 1.0f; + _accel_calibration.x_offset = 0.0f; + _accel_calibration.y_offset = 0.0f; + _accel_calibration.z_offset = 0.0f; +} + +int DfMPU6050Wrapper::_publish(struct imu_sensor_data &data) +{ + /* Check if calibration values are still up-to-date. */ + bool updated; + orb_check(_param_update_sub, &updated); + + if (updated) { + parameter_update_s parameter_update; + orb_copy(ORB_ID(parameter_update), _param_update_sub, ¶meter_update); + + _update_accel_calibration(); + _update_gyro_calibration(); + } + + math::Vector<3> vec_integrated_unused; + uint64_t integral_dt_unused; + + math::Vector<3> accel_val(data.accel_m_s2_x, data.accel_m_s2_y, data.accel_m_s2_z); + + // apply sensor rotation on the accel measurement + accel_val = _rotation_matrix * accel_val; + + accel_val(0) = (accel_val(0) - _accel_calibration.x_offset) * _accel_calibration.x_scale; + accel_val(1) = (accel_val(1) - _accel_calibration.y_offset) * _accel_calibration.y_scale; + accel_val(2) = (accel_val(2) - _accel_calibration.z_offset) * _accel_calibration.z_scale; + + _accel_int.put_with_interval(data.fifo_sample_interval_us, + accel_val, + vec_integrated_unused, + integral_dt_unused); + + math::Vector<3> gyro_val(data.gyro_rad_s_x, data.gyro_rad_s_y, data.gyro_rad_s_z); + + // apply sensor rotation on the gyro measurement + gyro_val = _rotation_matrix * gyro_val; + + gyro_val(0) = (gyro_val(0) - _gyro_calibration.x_offset) * _gyro_calibration.x_scale; + gyro_val(1) = (gyro_val(1) - _gyro_calibration.y_offset) * _gyro_calibration.y_scale; + gyro_val(2) = (gyro_val(2) - _gyro_calibration.z_offset) * _gyro_calibration.z_scale; + + _gyro_int.put_with_interval(data.fifo_sample_interval_us, + gyro_val, + vec_integrated_unused, + integral_dt_unused); + + // If we are not receiving the last sample from the FIFO buffer yet, let's stop here + // and wait for more packets. + if (!data.is_last_fifo_sample) { + return 0; + } + + // The driver empties the FIFO buffer at 1kHz, however we only need to publish at 250Hz. + // Therefore, only publish every forth time. + ++_publish_count; + + if (_publish_count < 4) { + return 0; + } + + _publish_count = 0; + + // Update all the counters. + perf_set_count(_read_counter, data.read_counter); + perf_set_count(_error_counter, data.error_counter); + perf_set_count(_fifo_overflow_counter, data.fifo_overflow_counter); + perf_set_count(_fifo_corruption_counter, data.fifo_overflow_counter); + perf_set_count(_gyro_range_hit_counter, data.gyro_range_hit_counter); + perf_set_count(_accel_range_hit_counter, data.accel_range_hit_counter); + + perf_begin(_publish_perf); + + accel_report accel_report = {}; + gyro_report gyro_report = {}; + + accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time(); + + // TODO: get these right + gyro_report.scaling = -1.0f; + gyro_report.range_rad_s = -1.0f; + gyro_report.device_id = m_id.dev_id; + + accel_report.scaling = -1.0f; + accel_report.range_m_s2 = -1.0f; + accel_report.device_id = m_id.dev_id; + + // TODO: remove these (or get the values) + gyro_report.x_raw = NAN; + gyro_report.y_raw = NAN; + gyro_report.z_raw = NAN; + + accel_report.x_raw = NAN; + accel_report.y_raw = NAN; + accel_report.z_raw = NAN; + + math::Vector<3> gyro_val_filt; + math::Vector<3> accel_val_filt; + + // Read and reset. + math::Vector<3> gyro_val_integ = _gyro_int.get_and_filtered(true, gyro_report.integral_dt, gyro_val_filt); + math::Vector<3> accel_val_integ = _accel_int.get_and_filtered(true, accel_report.integral_dt, accel_val_filt); + + // Use the filtered (by integration) values to get smoother / less noisy data. + gyro_report.x = gyro_val_filt(0); + gyro_report.y = gyro_val_filt(1); + gyro_report.z = gyro_val_filt(2); + + accel_report.x = accel_val_filt(0); + accel_report.y = accel_val_filt(1); + accel_report.z = accel_val_filt(2); + + gyro_report.x_integral = gyro_val_integ(0); + gyro_report.y_integral = gyro_val_integ(1); + gyro_report.z_integral = gyro_val_integ(2); + + accel_report.x_integral = accel_val_integ(0); + accel_report.y_integral = accel_val_integ(1); + accel_report.z_integral = accel_val_integ(2); + + // TODO: when is this ever blocked? + if (!(m_pub_blocked)) { + + if (_gyro_topic == nullptr) { + _gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &gyro_report, + &_gyro_orb_class_instance, ORB_PRIO_DEFAULT); + + } else { + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report); + } + + if (_accel_topic == nullptr) { + _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &accel_report, + &_accel_orb_class_instance, ORB_PRIO_DEFAULT); + + } else { + orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); + } + + /* Notify anyone waiting for data. */ + DevMgr::updateNotify(*this); + + // Report if there are high vibrations, every 10 times it happens. + const bool threshold_reached = (data.accel_range_hit_counter - _last_accel_range_hit_count > 10); + + // Report every 5s. + const bool due_to_report = (hrt_elapsed_time(&_last_accel_range_hit_time) > 5000000); + + if (threshold_reached && due_to_report) { + mavlink_log_critical(&_mavlink_log_pub, + "High accelerations, range exceeded %llu times", + data.accel_range_hit_counter); + + _last_accel_range_hit_time = hrt_absolute_time(); + _last_accel_range_hit_count = data.accel_range_hit_counter; + } + } + + perf_end(_publish_perf); + + // TODO: check the return codes of this function + return 0; +}; + + +namespace df_mpu6050_wrapper +{ + +DfMPU6050Wrapper *g_dev = nullptr; + +int start(enum Rotation rotation); +int stop(); +int info(); +void usage(); + +int start(enum Rotation rotation) +{ + g_dev = new DfMPU6050Wrapper(rotation); + + if (g_dev == nullptr) { + PX4_ERR("failed instantiating DfMPU6050Wrapper object"); + return -1; + } + + int ret = g_dev->start(); + + if (ret != 0) { + PX4_ERR("DfMPU6050Wrapper start failed"); + return ret; + } + + // Open the IMU sensor + DevHandle h; + DevMgr::getHandle(IMU_DEVICE_PATH, h); + + if (!h.isValid()) { + DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)", + IMU_DEVICE_PATH, h.getError()); + return -1; + } + + DevMgr::releaseHandle(h); + + return 0; +} + +int stop() +{ + if (g_dev == nullptr) { + PX4_ERR("driver not running"); + return 1; + } + + int ret = g_dev->stop(); + + if (ret != 0) { + PX4_ERR("driver could not be stopped"); + return ret; + } + + delete g_dev; + g_dev = nullptr; + return 0; +} + +/** + * Print a little info about the driver. + */ +int +info() +{ + if (g_dev == nullptr) { + PX4_ERR("driver not running"); + return 1; + } + + PX4_INFO("state @ %p", g_dev); + g_dev->info(); + + return 0; +} + +void +usage() +{ + PX4_INFO("Usage: df_mpu6050_wrapper 'start', 'info', 'stop'"); + PX4_INFO("options:"); + PX4_INFO(" -R rotation"); +} + +} // namespace df_mpu6050_wrapper + + +int +df_mpu6050_wrapper_main(int argc, char *argv[]) +{ + int ch; + enum Rotation rotation = ROTATION_NONE; + int ret = 0; + int myoptind = 1; + const char *myoptarg = NULL; + + /* jump over start/off/etc and look at options first */ + while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'R': + rotation = (enum Rotation)atoi(myoptarg); + break; + + default: + df_mpu6050_wrapper::usage(); + return 0; + } + } + + if (argc <= 1) { + df_mpu6050_wrapper::usage(); + return 1; + } + + const char *verb = argv[myoptind]; + + if (!strcmp(verb, "start")) { + ret = df_mpu6050_wrapper::start(rotation); + } + + else if (!strcmp(verb, "stop")) { + ret = df_mpu6050_wrapper::stop(); + } + + else if (!strcmp(verb, "info")) { + ret = df_mpu6050_wrapper::info(); + } + + else { + df_mpu6050_wrapper::usage(); + return 1; + } + + return ret; +} diff --git a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp index ae8c9b65ca..368e120031 100644 --- a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp +++ b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp @@ -58,8 +58,11 @@ #include #include #include +#include #include +#include + #include #include @@ -77,7 +80,7 @@ using namespace DriverFramework; class DfMpu9250Wrapper : public MPU9250 { public: - DfMpu9250Wrapper(/*enum Rotation rotation*/); + DfMpu9250Wrapper(bool mag_enabled, enum Rotation rotation); ~DfMpu9250Wrapper(); @@ -105,11 +108,11 @@ private: void _update_accel_calibration(); void _update_gyro_calibration(); - - //enum Rotation _rotation; + void _update_mag_calibration(); orb_advert_t _accel_topic; orb_advert_t _gyro_topic; + orb_advert_t _mag_topic; orb_advert_t _mavlink_log_pub; @@ -133,8 +136,20 @@ private: float z_scale; } _gyro_calibration; + math::Matrix<3, 3> _rotation_matrix; + + struct mag_calibration_s { + float x_offset; + float x_scale; + float y_offset; + float y_scale; + float z_offset; + float z_scale; + } _mag_calibration; + int _accel_orb_class_instance; int _gyro_orb_class_instance; + int _mag_orb_class_instance; Integrator _accel_int; Integrator _gyro_int; @@ -147,25 +162,30 @@ private: perf_counter_t _fifo_corruption_counter; perf_counter_t _gyro_range_hit_counter; perf_counter_t _accel_range_hit_counter; + perf_counter_t _mag_fifo_overflow_counter; perf_counter_t _publish_perf; hrt_abstime _last_accel_range_hit_time; uint64_t _last_accel_range_hit_count; + + bool _mag_enabled; }; -DfMpu9250Wrapper::DfMpu9250Wrapper(/*enum Rotation rotation*/) : - MPU9250(IMU_DEVICE_PATH), +DfMpu9250Wrapper::DfMpu9250Wrapper(bool mag_enabled, enum Rotation rotation) : + MPU9250(IMU_DEVICE_PATH, mag_enabled), _accel_topic(nullptr), _gyro_topic(nullptr), + _mag_topic(nullptr), _mavlink_log_pub(nullptr), _param_update_sub(-1), _accel_calibration{}, _gyro_calibration{}, + _mag_calibration{}, _accel_orb_class_instance(-1), _gyro_orb_class_instance(-1), + _mag_orb_class_instance(-1), _accel_int(MPU9250_NEVER_AUTOPUBLISH_US, false), _gyro_int(MPU9250_NEVER_AUTOPUBLISH_US, true), - /*_rotation(rotation)*/ _publish_count(0), _read_counter(perf_alloc(PC_COUNT, "mpu9250_reads")), _error_counter(perf_alloc(PC_COUNT, "mpu9250_errors")), @@ -173,9 +193,11 @@ DfMpu9250Wrapper::DfMpu9250Wrapper(/*enum Rotation rotation*/) : _fifo_corruption_counter(perf_alloc(PC_COUNT, "mpu9250_fifo_corruptions")), _gyro_range_hit_counter(perf_alloc(PC_COUNT, "mpu9250_gyro_range_hits")), _accel_range_hit_counter(perf_alloc(PC_COUNT, "mpu9250_accel_range_hits")), + _mag_fifo_overflow_counter(perf_alloc(PC_COUNT, "mpu9250_mag_fifo_overflows")), _publish_perf(perf_alloc(PC_ELAPSED, "mpu9250_publish")), _last_accel_range_hit_time(0), - _last_accel_range_hit_count(0) + _last_accel_range_hit_count(0), + _mag_enabled(mag_enabled) { // Set sane default calibration values _accel_calibration.x_scale = 1.0f; @@ -191,6 +213,18 @@ DfMpu9250Wrapper::DfMpu9250Wrapper(/*enum Rotation rotation*/) : _gyro_calibration.x_offset = 0.0f; _gyro_calibration.y_offset = 0.0f; _gyro_calibration.z_offset = 0.0f; + + if (_mag_enabled) { + _mag_calibration.x_scale = 1.0f; + _mag_calibration.y_scale = 1.0f; + _mag_calibration.z_scale = 1.0f; + _mag_calibration.x_offset = 0.0f; + _mag_calibration.y_offset = 0.0f; + _mag_calibration.z_offset = 0.0f; + } + + // Get sensor rotation matrix + get_rot_matrix(rotation, &_rotation_matrix); } DfMpu9250Wrapper::~DfMpu9250Wrapper() @@ -201,6 +235,11 @@ DfMpu9250Wrapper::~DfMpu9250Wrapper() perf_free(_fifo_corruption_counter); perf_free(_gyro_range_hit_counter); perf_free(_accel_range_hit_counter); + + if (_mag_enabled) { + perf_free(_mag_fifo_overflow_counter); + } + perf_free(_publish_perf); } @@ -226,6 +265,9 @@ int DfMpu9250Wrapper::start() return -1; } + if (_mag_enabled) { + } + /* Subscribe to param update topic. */ if (_param_update_sub < 0) { _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -246,11 +288,12 @@ int DfMpu9250Wrapper::start() return ret; } - PX4_INFO("MPU9250 device id is: %d", m_id.dev_id); + PX4_DEBUG("MPU9250 device id is: %d", m_id.dev_id); /* Force getting the calibration values. */ _update_accel_calibration(); _update_gyro_calibration(); + _update_mag_calibration(); return 0; } @@ -276,6 +319,11 @@ void DfMpu9250Wrapper::info() perf_print_counter(_fifo_corruption_counter); perf_print_counter(_gyro_range_hit_counter); perf_print_counter(_accel_range_hit_counter); + + if (_mag_enabled) { + perf_print_counter(_mag_fifo_overflow_counter); + } + perf_print_counter(_publish_perf); } @@ -430,6 +478,86 @@ void DfMpu9250Wrapper::_update_accel_calibration() _accel_calibration.z_offset = 0.0f; } +void DfMpu9250Wrapper::_update_mag_calibration() +{ + if (!_mag_enabled) { + return; + } + + // TODO: replace magic number + for (unsigned i = 0; i < 3; ++i) { + + // TODO: remove printfs and add error counter + + char str[30]; + (void)sprintf(str, "CAL_MAG%u_ID", i); + int32_t device_id; + int res = param_get(param_find(str), &device_id); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + continue; + } + + if ((uint32_t)device_id != m_id.dev_id) { + continue; + } + + (void)sprintf(str, "CAL_MAG%u_XSCALE", i); + res = param_get(param_find(str), &_mag_calibration.x_scale); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_MAG%u_YSCALE", i); + res = param_get(param_find(str), &_mag_calibration.y_scale); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_MAG%u_ZSCALE", i); + res = param_get(param_find(str), &_mag_calibration.z_scale); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_MAG%u_XOFF", i); + res = param_get(param_find(str), &_mag_calibration.x_offset); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_MAG%u_YOFF", i); + res = param_get(param_find(str), &_mag_calibration.y_offset); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_MAG%u_ZOFF", i); + res = param_get(param_find(str), &_mag_calibration.z_offset); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + // We got calibration values, let's exit. + return; + } + + // Set sane default calibration values + _mag_calibration.x_scale = 1.0f; + _mag_calibration.y_scale = 1.0f; + _mag_calibration.z_scale = 1.0f; + _mag_calibration.x_offset = 0.0f; + _mag_calibration.y_offset = 0.0f; + _mag_calibration.z_offset = 0.0f; +} + int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data) { /* Check if calibration values are still up-to-date. */ @@ -442,38 +570,35 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data) _update_accel_calibration(); _update_gyro_calibration(); + _update_mag_calibration(); } - accel_report accel_report = {}; - gyro_report gyro_report = {}; + math::Vector<3> vec_integrated_unused; + uint64_t integral_dt_unused; - accel_report.x = (data.accel_m_s2_x - _accel_calibration.x_offset) * _accel_calibration.x_scale; - accel_report.y = (data.accel_m_s2_y - _accel_calibration.y_offset) * _accel_calibration.y_scale; - accel_report.z = (data.accel_m_s2_z - _accel_calibration.z_offset) * _accel_calibration.z_scale; + math::Vector<3> accel_val((data.accel_m_s2_x - _accel_calibration.x_offset) * _accel_calibration.x_scale, + (data.accel_m_s2_y - _accel_calibration.y_offset) * _accel_calibration.y_scale, + (data.accel_m_s2_z - _accel_calibration.z_offset) * _accel_calibration.z_scale); - math::Vector<3> accel_val(accel_report.x, - accel_report.y, - accel_report.z); - math::Vector<3> accel_val_integrated_unused; + // apply sensor rotation on the accel measurement + accel_val = _rotation_matrix * accel_val; _accel_int.put_with_interval(data.fifo_sample_interval_us, accel_val, - accel_val_integrated_unused, - accel_report.integral_dt); + vec_integrated_unused, + integral_dt_unused); - gyro_report.x = (data.gyro_rad_s_x - _gyro_calibration.x_offset) * _gyro_calibration.x_scale; - gyro_report.y = (data.gyro_rad_s_y - _gyro_calibration.y_offset) * _gyro_calibration.y_scale; - gyro_report.z = (data.gyro_rad_s_z - _gyro_calibration.z_offset) * _gyro_calibration.z_scale; + math::Vector<3> gyro_val((data.gyro_rad_s_x - _gyro_calibration.x_offset) * _gyro_calibration.x_scale, + (data.gyro_rad_s_y - _gyro_calibration.y_offset) * _gyro_calibration.y_scale, + (data.gyro_rad_s_z - _gyro_calibration.z_offset) * _gyro_calibration.z_scale); - math::Vector<3> gyro_val(gyro_report.x, - gyro_report.y, - gyro_report.z); - math::Vector<3> gyro_val_integrated_unused; + // apply sensor rotation on the gyro measurement + gyro_val = _rotation_matrix * gyro_val; _gyro_int.put_with_interval(data.fifo_sample_interval_us, gyro_val, - gyro_val_integrated_unused, - gyro_report.integral_dt); + vec_integrated_unused, + integral_dt_unused); // If we are not receiving the last sample from the FIFO buffer yet, let's stop here // and wait for more packets. @@ -499,10 +624,22 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data) perf_set_count(_gyro_range_hit_counter, data.gyro_range_hit_counter); perf_set_count(_accel_range_hit_counter, data.accel_range_hit_counter); + if (_mag_enabled) { + perf_set_count(_mag_fifo_overflow_counter, data.mag_fifo_overflow_counter); + } + perf_begin(_publish_perf); + accel_report accel_report = {}; + gyro_report gyro_report = {}; + mag_report mag_report = {}; + accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time(); + if (_mag_enabled) { + mag_report.timestamp = accel_report.timestamp; + } + // TODO: get these right gyro_report.scaling = -1.0f; gyro_report.range_rad_s = -1.0f; @@ -512,6 +649,12 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data) accel_report.range_m_s2 = -1.0f; accel_report.device_id = m_id.dev_id; + if (_mag_enabled) { + mag_report.scaling = -1.0f; + mag_report.range_ga = -1.0f; + mag_report.device_id = m_id.dev_id; + } + // TODO: remove these (or get the values) gyro_report.x_raw = NAN; gyro_report.y_raw = NAN; @@ -521,17 +664,48 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data) accel_report.y_raw = NAN; accel_report.z_raw = NAN; + if (_mag_enabled) { + mag_report.x_raw = NAN; + mag_report.y_raw = NAN; + mag_report.z_raw = NAN; + } + + math::Vector<3> gyro_val_filt; + math::Vector<3> accel_val_filt; + // Read and reset. - math::Vector<3> gyro_val_integrated = _gyro_int.get(true, gyro_report.integral_dt); - math::Vector<3> accel_val_integrated = _accel_int.get(true, accel_report.integral_dt); + math::Vector<3> gyro_val_integ = _gyro_int.get_and_filtered(true, gyro_report.integral_dt, gyro_val_filt); + math::Vector<3> accel_val_integ = _accel_int.get_and_filtered(true, accel_report.integral_dt, accel_val_filt); - gyro_report.x_integral = gyro_val_integrated(0); - gyro_report.y_integral = gyro_val_integrated(1); - gyro_report.z_integral = gyro_val_integrated(2); + // Use the filtered (by integration) values to get smoother / less noisy data. + gyro_report.x = gyro_val_filt(0); + gyro_report.y = gyro_val_filt(1); + gyro_report.z = gyro_val_filt(2); - accel_report.x_integral = accel_val_integrated(0); - accel_report.y_integral = accel_val_integrated(1); - accel_report.z_integral = accel_val_integrated(2); + accel_report.x = accel_val_filt(0); + accel_report.y = accel_val_filt(1); + accel_report.z = accel_val_filt(2); + + if (_mag_enabled) { + + math::Vector<3> mag_val((data.mag_ga_x - _mag_calibration.x_offset) * _mag_calibration.x_scale, + (data.mag_ga_y - _mag_calibration.y_offset) * _mag_calibration.y_scale, + (data.mag_ga_z - _mag_calibration.z_offset) * _mag_calibration.z_scale); + + mag_val = _rotation_matrix * mag_val; + + mag_report.x = mag_val(0); + mag_report.y = mag_val(1); + mag_report.z = mag_val(2); + } + + gyro_report.x_integral = gyro_val_integ(0); + gyro_report.y_integral = gyro_val_integ(1); + gyro_report.z_integral = gyro_val_integ(2); + + accel_report.x_integral = accel_val_integ(0); + accel_report.y_integral = accel_val_integ(1); + accel_report.z_integral = accel_val_integ(2); // TODO: when is this ever blocked? if (!(m_pub_blocked)) { @@ -544,6 +718,17 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data) orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); } + if (_mag_enabled) { + + if (_mag_topic == nullptr) { + _mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report, + &_mag_orb_class_instance, ORB_PRIO_LOW); + + } else { + orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report); + } + } + /* Notify anyone waiting for data. */ DevMgr::updateNotify(*this); @@ -575,14 +760,14 @@ namespace df_mpu9250_wrapper DfMpu9250Wrapper *g_dev = nullptr; -int start(/* enum Rotation rotation */); +int start(bool mag_enabled, enum Rotation rotation); int stop(); int info(); void usage(); -int start(/*enum Rotation rotation*/) +int start(bool mag_enabled, enum Rotation rotation) { - g_dev = new DfMpu9250Wrapper(/*rotation*/); + g_dev = new DfMpu9250Wrapper(mag_enabled, rotation); if (g_dev == nullptr) { PX4_ERR("failed instantiating DfMpu9250Wrapper object"); @@ -650,9 +835,9 @@ info() void usage() { - PX4_WARN("Usage: df_mpu9250_wrapper 'start', 'info', 'stop'"); - PX4_WARN("options:"); - //PX4_WARN(" -R rotation"); + PX4_INFO("Usage: df_mpu9250_wrapper 'start', 'start_without_mag', 'info', 'stop'"); + PX4_INFO("options:"); + PX4_INFO(" -R rotation"); } } // namespace df_mpu9250_wrapper @@ -662,7 +847,7 @@ int df_mpu9250_wrapper_main(int argc, char *argv[]) { int ch; - // enum Rotation rotation = ROTATION_NONE; + enum Rotation rotation = ROTATION_NONE; int ret = 0; int myoptind = 1; const char *myoptarg = NULL; @@ -670,9 +855,9 @@ df_mpu9250_wrapper_main(int argc, char *argv[]) /* jump over start/off/etc and look at options first */ while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { switch (ch) { - //case 'R': - // rotation = (enum Rotation)atoi(myoptarg); - // break; + case 'R': + rotation = (enum Rotation)atoi(myoptarg); + break; default: df_mpu9250_wrapper::usage(); @@ -687,9 +872,12 @@ df_mpu9250_wrapper_main(int argc, char *argv[]) const char *verb = argv[myoptind]; + if (!strcmp(verb, "start_without_mag")) { + ret = df_mpu9250_wrapper::start(false, rotation); + } - if (!strcmp(verb, "start")) { - ret = df_mpu9250_wrapper::start(/*rotation*/); + else if (!strcmp(verb, "start")) { + ret = df_mpu9250_wrapper::start(true, rotation); } else if (!strcmp(verb, "stop")) { diff --git a/src/platforms/posix/drivers/df_ms5607_wrapper/CMakeLists.txt b/src/platforms/posix/drivers/df_ms5607_wrapper/CMakeLists.txt new file mode 100644 index 0000000000..d75c46884d --- /dev/null +++ b/src/platforms/posix/drivers/df_ms5607_wrapper/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# Copyright (c) 2016 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +include_directories(../../../../lib/DriverFramework/drivers) + +px4_add_module( + MODULE platforms__posix__drivers__df_ms5607_wrapper + MAIN df_ms5607_wrapper + SRCS + df_ms5607_wrapper.cpp + DEPENDS + platforms__common + df_driver_framework + df_ms5607 + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/posix/drivers/df_ms5607_wrapper/df_ms5607_wrapper.cpp b/src/platforms/posix/drivers/df_ms5607_wrapper/df_ms5607_wrapper.cpp new file mode 100644 index 0000000000..c02c708f5d --- /dev/null +++ b/src/platforms/posix/drivers/df_ms5607_wrapper/df_ms5607_wrapper.cpp @@ -0,0 +1,322 @@ +/**************************************************************************** + * + * Copyright (c) 2016 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 df_ms5607_wrapper.cpp + * Lightweight driver to access the MS5607 of the DriverFramework. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include + +#include +#include + + +extern "C" { __EXPORT int df_ms5607_wrapper_main(int argc, char *argv[]); } + +using namespace DriverFramework; + + +class DfMS5607Wrapper : public MS5607 +{ +public: + DfMS5607Wrapper(); + ~DfMS5607Wrapper(); + + + /** + * Start automatic measurement. + * + * @return 0 on success + */ + int start(); + + /** + * Stop automatic measurement. + * + * @return 0 on success + */ + int stop(); + +private: + int _publish(struct baro_sensor_data &data); + + orb_advert_t _baro_topic; + + int _baro_orb_class_instance; + + perf_counter_t _baro_sample_perf; + +}; + +DfMS5607Wrapper::DfMS5607Wrapper() : + MS5607(BARO_DEVICE_PATH), + _baro_topic(nullptr), + _baro_orb_class_instance(-1), + _baro_sample_perf(perf_alloc(PC_ELAPSED, "df_baro_read")) +{ +} + +DfMS5607Wrapper::~DfMS5607Wrapper() +{ + perf_free(_baro_sample_perf); +} + +int DfMS5607Wrapper::start() +{ + /* Init device and start sensor. */ + int ret = init(); + + if (ret != 0) { + PX4_ERR("MS5607 init fail: %d", ret); + return ret; + } + + ret = MS5607::start(); + + if (ret != 0) { + PX4_ERR("MS5607 start fail: %d", ret); + return ret; + } + + return 0; +} + +int DfMS5607Wrapper::stop() +{ + /* Stop sensor. */ + int ret = MS5607::stop(); + + if (ret != 0) { + PX4_ERR("MS5607 stop fail: %d", ret); + return ret; + } + + return 0; +} + +int DfMS5607Wrapper::_publish(struct baro_sensor_data &data) +{ + perf_begin(_baro_sample_perf); + + baro_report baro_report = {}; + baro_report.timestamp = hrt_absolute_time(); + + baro_report.pressure = data.pressure_pa; + baro_report.temperature = data.temperature_c; + + // TODO: verify this, it's just copied from the MS5611 driver. + + // Constant for now + const double MSL_PRESSURE_KPA = 101325.0 / 1000.0; + + /* tropospheric properties (0-11km) for standard atmosphere */ + const double T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */ + const double a = -6.5 / 1000; /* temperature gradient in degrees per metre */ + const double g = 9.80665; /* gravity constant in m/s/s */ + const double R = 287.05; /* ideal gas constant in J/kg/K */ + + /* current pressure at MSL in kPa */ + double p1 = MSL_PRESSURE_KPA; + + /* measured pressure in kPa */ + double p = static_cast(data.pressure_pa) / 1000.0; + + /* + * Solve: + * + * / -(aR / g) \ + * | (p / p1) . T1 | - T1 + * \ / + * h = ------------------------------- + h1 + * a + */ + baro_report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; + + // TODO: when is this ever blocked? + if (!(m_pub_blocked)) { + + if (_baro_topic == nullptr) { + _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &baro_report, + &_baro_orb_class_instance, ORB_PRIO_DEFAULT); + + } else { + orb_publish(ORB_ID(sensor_baro), _baro_topic, &baro_report); + } + } + + /* Notify anyone waiting for data. */ + DevMgr::updateNotify(*this); + + perf_end(_baro_sample_perf); + + return 0; +}; + + +namespace df_ms5607_wrapper +{ + +DfMS5607Wrapper *g_dev = nullptr; + +int start(/* enum Rotation rotation */); +int stop(); +int info(); +void usage(); + +int start(/*enum Rotation rotation*/) +{ + g_dev = new DfMS5607Wrapper(/*rotation*/); + + if (g_dev == nullptr) { + PX4_ERR("failed instantiating DfMS5607Wrapper object"); + return -1; + } + + int ret = g_dev->start(); + + if (ret != 0) { + PX4_ERR("DfMS5607Wrapper start failed"); + return ret; + } + + // Open the IMU sensor + DevHandle h; + DevMgr::getHandle(BARO_DEVICE_PATH, h); + + if (!h.isValid()) { + DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)", + BARO_DEVICE_PATH, h.getError()); + return -1; + } + + DevMgr::releaseHandle(h); + + return 0; +} + +int stop() +{ + if (g_dev == nullptr) { + PX4_ERR("driver not running"); + return 1; + } + + int ret = g_dev->stop(); + + if (ret != 0) { + PX4_ERR("driver could not be stopped"); + return ret; + } + + delete g_dev; + g_dev = nullptr; + return 0; +} + +/** + * Print a little info about the driver. + */ +int +info() +{ + if (g_dev == nullptr) { + PX4_ERR("driver not running"); + return 1; + } + + PX4_DEBUG("state @ %p", g_dev); + + return 0; +} + +void +usage() +{ + PX4_WARN("Usage: df_ms5607_wrapper 'start', 'info', 'stop'"); +} + +} // namespace df_ms5607_wrapper + + +int +df_ms5607_wrapper_main(int argc, char *argv[]) +{ + int ret = 0; + int myoptind = 1; + + if (argc <= 1) { + df_ms5607_wrapper::usage(); + return 1; + } + + const char *verb = argv[myoptind]; + + + if (!strcmp(verb, "start")) { + ret = df_ms5607_wrapper::start(); + } + + else if (!strcmp(verb, "stop")) { + ret = df_ms5607_wrapper::stop(); + } + + else if (!strcmp(verb, "info")) { + ret = df_ms5607_wrapper::info(); + } + + else { + df_ms5607_wrapper::usage(); + return 1; + } + + return ret; +} diff --git a/src/platforms/posix/drivers/df_ms5611_wrapper/CMakeLists.txt b/src/platforms/posix/drivers/df_ms5611_wrapper/CMakeLists.txt new file mode 100644 index 0000000000..a84bd5114b --- /dev/null +++ b/src/platforms/posix/drivers/df_ms5611_wrapper/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# Copyright (c) 2016 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +include_directories(../../../../lib/DriverFramework/drivers) + +px4_add_module( + MODULE platforms__posix__drivers__df_ms5611_wrapper + MAIN df_ms5611_wrapper + SRCS + df_ms5611_wrapper.cpp + DEPENDS + platforms__common + df_driver_framework + df_ms5611 + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/posix/drivers/df_ms5611_wrapper/df_ms5611_wrapper.cpp b/src/platforms/posix/drivers/df_ms5611_wrapper/df_ms5611_wrapper.cpp new file mode 100644 index 0000000000..2938f17495 --- /dev/null +++ b/src/platforms/posix/drivers/df_ms5611_wrapper/df_ms5611_wrapper.cpp @@ -0,0 +1,322 @@ +/**************************************************************************** + * + * Copyright (c) 2016 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 df_ms5611_wrapper.cpp + * Lightweight driver to access the MS5611 of the DriverFramework. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include + +#include +#include + + +extern "C" { __EXPORT int df_ms5611_wrapper_main(int argc, char *argv[]); } + +using namespace DriverFramework; + + +class DfMS5611Wrapper : public MS5611 +{ +public: + DfMS5611Wrapper(); + ~DfMS5611Wrapper(); + + + /** + * Start automatic measurement. + * + * @return 0 on success + */ + int start(); + + /** + * Stop automatic measurement. + * + * @return 0 on success + */ + int stop(); + +private: + int _publish(struct baro_sensor_data &data); + + orb_advert_t _baro_topic; + + int _baro_orb_class_instance; + + perf_counter_t _baro_sample_perf; + +}; + +DfMS5611Wrapper::DfMS5611Wrapper() : + MS5611(BARO_DEVICE_PATH), + _baro_topic(nullptr), + _baro_orb_class_instance(-1), + _baro_sample_perf(perf_alloc(PC_ELAPSED, "df_baro_read")) +{ +} + +DfMS5611Wrapper::~DfMS5611Wrapper() +{ + perf_free(_baro_sample_perf); +} + +int DfMS5611Wrapper::start() +{ + /* Init device and start sensor. */ + int ret = init(); + + if (ret != 0) { + PX4_ERR("MS5611 init fail: %d", ret); + return ret; + } + + ret = MS5611::start(); + + if (ret != 0) { + PX4_ERR("MS5611 start fail: %d", ret); + return ret; + } + + return 0; +} + +int DfMS5611Wrapper::stop() +{ + /* Stop sensor. */ + int ret = MS5611::stop(); + + if (ret != 0) { + PX4_ERR("MS5611 stop fail: %d", ret); + return ret; + } + + return 0; +} + +int DfMS5611Wrapper::_publish(struct baro_sensor_data &data) +{ + perf_begin(_baro_sample_perf); + + baro_report baro_report = {}; + baro_report.timestamp = hrt_absolute_time(); + + baro_report.pressure = data.pressure_pa; + baro_report.temperature = data.temperature_c; + + // TODO: verify this, it's just copied from the MS5611 driver. + + // Constant for now + const double MSL_PRESSURE_KPA = 101325.0 / 1000.0; + + /* tropospheric properties (0-11km) for standard atmosphere */ + const double T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */ + const double a = -6.5 / 1000; /* temperature gradient in degrees per metre */ + const double g = 9.80665; /* gravity constant in m/s/s */ + const double R = 287.05; /* ideal gas constant in J/kg/K */ + + /* current pressure at MSL in kPa */ + double p1 = MSL_PRESSURE_KPA; + + /* measured pressure in kPa */ + double p = static_cast(data.pressure_pa) / 1000.0; + + /* + * Solve: + * + * / -(aR / g) \ + * | (p / p1) . T1 | - T1 + * \ / + * h = ------------------------------- + h1 + * a + */ + baro_report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; + + // TODO: when is this ever blocked? + if (!(m_pub_blocked)) { + + if (_baro_topic == nullptr) { + _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &baro_report, + &_baro_orb_class_instance, ORB_PRIO_DEFAULT); + + } else { + orb_publish(ORB_ID(sensor_baro), _baro_topic, &baro_report); + } + } + + /* Notify anyone waiting for data. */ + DevMgr::updateNotify(*this); + + perf_end(_baro_sample_perf); + + return 0; +}; + + +namespace df_ms5611_wrapper +{ + +DfMS5611Wrapper *g_dev = nullptr; + +int start(/* enum Rotation rotation */); +int stop(); +int info(); +void usage(); + +int start(/*enum Rotation rotation*/) +{ + g_dev = new DfMS5611Wrapper(/*rotation*/); + + if (g_dev == nullptr) { + PX4_ERR("failed instantiating DfMS5611Wrapper object"); + return -1; + } + + int ret = g_dev->start(); + + if (ret != 0) { + PX4_ERR("DfMS5611Wrapper start failed"); + return ret; + } + + // Open the IMU sensor + DevHandle h; + DevMgr::getHandle(BARO_DEVICE_PATH, h); + + if (!h.isValid()) { + DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)", + BARO_DEVICE_PATH, h.getError()); + return -1; + } + + DevMgr::releaseHandle(h); + + return 0; +} + +int stop() +{ + if (g_dev == nullptr) { + PX4_ERR("driver not running"); + return 1; + } + + int ret = g_dev->stop(); + + if (ret != 0) { + PX4_ERR("driver could not be stopped"); + return ret; + } + + delete g_dev; + g_dev = nullptr; + return 0; +} + +/** + * Print a little info about the driver. + */ +int +info() +{ + if (g_dev == nullptr) { + PX4_ERR("driver not running"); + return 1; + } + + PX4_DEBUG("state @ %p", g_dev); + + return 0; +} + +void +usage() +{ + PX4_WARN("Usage: df_ms5611_wrapper 'start', 'info', 'stop'"); +} + +} // namespace df_ms5611_wrapper + + +int +df_ms5611_wrapper_main(int argc, char *argv[]) +{ + int ret = 0; + int myoptind = 1; + + if (argc <= 1) { + df_ms5611_wrapper::usage(); + return 1; + } + + const char *verb = argv[myoptind]; + + + if (!strcmp(verb, "start")) { + ret = df_ms5611_wrapper::start(); + } + + else if (!strcmp(verb, "stop")) { + ret = df_ms5611_wrapper::stop(); + } + + else if (!strcmp(verb, "info")) { + ret = df_ms5611_wrapper::info(); + } + + else { + df_ms5611_wrapper::usage(); + return 1; + } + + return ret; +} diff --git a/src/platforms/posix/drivers/df_trone_wrapper/df_trone_wrapper.cpp b/src/platforms/posix/drivers/df_trone_wrapper/df_trone_wrapper.cpp index 4bc4fdf531..af90241849 100644 --- a/src/platforms/posix/drivers/df_trone_wrapper/df_trone_wrapper.cpp +++ b/src/platforms/posix/drivers/df_trone_wrapper/df_trone_wrapper.cpp @@ -117,10 +117,6 @@ DfTROneWrapper::~DfTROneWrapper() int DfTROneWrapper::start() { - struct distance_sensor_s d; - _range_topic = orb_advertise_multi(ORB_ID(distance_sensor), &d, - &_orb_class_instance, ORB_PRIO_DEFAULT); - int ret; /* Init device and start sensor. */ @@ -156,10 +152,6 @@ int DfTROneWrapper::stop() int DfTROneWrapper::_publish(struct range_sensor_data &data) { - if (!_range_topic) { - return 1; - } - struct distance_sensor_s d; memset(&d, 0, sizeof(d)); @@ -180,7 +172,13 @@ int DfTROneWrapper::_publish(struct range_sensor_data &data) d.covariance = 0.0f; - orb_publish(ORB_ID(distance_sensor), _range_topic, &d); + if (_range_topic == nullptr) { + _range_topic = orb_advertise_multi(ORB_ID(distance_sensor), &d, + &_orb_class_instance, ORB_PRIO_DEFAULT); + + } else { + orb_publish(ORB_ID(distance_sensor), _range_topic, &d); + } /* Notify anyone waiting for data. */ DevMgr::updateNotify(*this); diff --git a/src/platforms/posix/drivers/gpssim/CMakeLists.txt b/src/platforms/posix/drivers/gpssim/CMakeLists.txt index 60cd2de7a8..2c580d7757 100644 --- a/src/platforms/posix/drivers/gpssim/CMakeLists.txt +++ b/src/platforms/posix/drivers/gpssim/CMakeLists.txt @@ -34,7 +34,6 @@ px4_add_module( MODULE platforms__posix__drivers__gpssim MAIN gpssim COMPILE_FLAGS - -Wno-packed -Os SRCS gpssim.cpp diff --git a/src/platforms/posix/drivers/gpssim/gpssim.cpp b/src/platforms/posix/drivers/gpssim/gpssim.cpp index 7a86abf356..8eb462585a 100644 --- a/src/platforms/posix/drivers/gpssim/gpssim.cpp +++ b/src/platforms/posix/drivers/gpssim/gpssim.cpp @@ -37,7 +37,6 @@ */ #include -#define __STDC_FORMAT_MACROS #include #include #include @@ -278,14 +277,12 @@ GPSSIM::receive(int timeout) simulator::RawGPSData gps; sim->getGPSSample((uint8_t *)&gps, sizeof(gps)); - _report_gps_pos.timestamp_position = hrt_absolute_time(); + _report_gps_pos.timestamp = hrt_absolute_time(); _report_gps_pos.lat = gps.lat; _report_gps_pos.lon = gps.lon; _report_gps_pos.alt = gps.alt; - _report_gps_pos.timestamp_variance = _report_gps_pos.timestamp_position; _report_gps_pos.eph = (float)gps.eph * 1e-2f; _report_gps_pos.epv = (float)gps.epv * 1e-2f; - _report_gps_pos.timestamp_velocity = _report_gps_pos.timestamp_position; _report_gps_pos.vel_m_s = (float)(gps.vel) / 100.0f; _report_gps_pos.vel_n_m_s = (float)(gps.vn) / 100.0f; _report_gps_pos.vel_e_m_s = (float)(gps.ve) / 100.0f; @@ -306,17 +303,15 @@ GPSSIM::task_main() while (!_task_should_exit) { if (_fake_gps) { - _report_gps_pos.timestamp_position = hrt_absolute_time(); + _report_gps_pos.timestamp = hrt_absolute_time(); _report_gps_pos.lat = (int32_t)47.378301e7f; _report_gps_pos.lon = (int32_t)8.538777e7f; _report_gps_pos.alt = (int32_t)1200e3f; - _report_gps_pos.timestamp_variance = _report_gps_pos.timestamp_position; _report_gps_pos.s_variance_m_s = 10.0f; _report_gps_pos.c_variance_rad = 0.1f; _report_gps_pos.fix_type = 3; _report_gps_pos.eph = 0.9f; _report_gps_pos.epv = 1.8f; - _report_gps_pos.timestamp_velocity = _report_gps_pos.timestamp_position; _report_gps_pos.vel_n_m_s = 0.0f; _report_gps_pos.vel_e_m_s = 0.0f; _report_gps_pos.vel_d_m_s = 0.0f; @@ -342,10 +337,8 @@ GPSSIM::task_main() //Publish initial report that we have access to a GPS //Make sure to clear any stale data in case driver is reset memset(&_report_gps_pos, 0, sizeof(_report_gps_pos)); - _report_gps_pos.timestamp_position = hrt_absolute_time(); - _report_gps_pos.timestamp_variance = hrt_absolute_time(); - _report_gps_pos.timestamp_velocity = hrt_absolute_time(); - _report_gps_pos.timestamp_time = hrt_absolute_time(); + _report_gps_pos.timestamp = hrt_absolute_time(); + _report_gps_pos.timestamp_time_relative = 0; if (!(m_pub_blocked)) { if (_report_gps_pos_pub != nullptr) { @@ -413,9 +406,9 @@ GPSSIM::print_info() _report_gps_pos.noise_per_ms, _report_gps_pos.jamming_indicator == 255 ? "YES" : "NO"); - if (_report_gps_pos.timestamp_position != 0) { + if (_report_gps_pos.timestamp != 0) { PX4_INFO("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type, - _report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0); + _report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp) / 1000.0); PX4_INFO("lat: %d, lon: %d, alt: %d", _report_gps_pos.lat, _report_gps_pos.lon, _report_gps_pos.alt); PX4_INFO("vel: %.2fm/s, %.2fm/s, %.2fm/s", (double)_report_gps_pos.vel_n_m_s, (double)_report_gps_pos.vel_e_m_s, (double)_report_gps_pos.vel_d_m_s); @@ -442,6 +435,7 @@ void stop(); void test(); void reset(); void info(); +void usage(const char *reason); /** * Start the driver. @@ -535,9 +529,22 @@ info() g_dev->print_info(); } +void +usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s", reason); + } + + PX4_INFO("usage:"); + PX4_INFO("gpssim {start|stop|test|reset|status}"); + PX4_INFO(" [-d /dev/ttyS0-n][-f (for enabling fake)][-s (to enable sat info)]"); +} + } // namespace + int gpssim_main(int argc, char *argv[]) { @@ -547,22 +554,25 @@ gpssim_main(int argc, char *argv[]) bool fake_gps = false; bool enable_sat_info = false; + + if (argc < 2) { + + gpssim::usage("not enough arguments supplied"); + return 1; + } + /* * Start/load the driver. */ if (!strcmp(argv[1], "start")) { if (g_dev != nullptr) { - PX4_WARN("gpssim already started"); + PX4_WARN("already started"); return 0; } - /* work around getopt unreliability */ if (argc > 3) { if (!strcmp(argv[2], "-d")) { device_name = argv[3]; - - } else { - goto out; } } @@ -581,6 +591,13 @@ gpssim_main(int argc, char *argv[]) } gpssim::start(device_name, fake_gps, enable_sat_info); + return 0; + } + + /* The following need gpssim running. */ + if (g_dev == nullptr) { + PX4_WARN("not running"); + return 1; } if (!strcmp(argv[1], "stop")) { @@ -609,8 +626,4 @@ gpssim_main(int argc, char *argv[]) } return 0; - -out: - PX4_INFO("unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status'\n [-d /dev/ttyS0-n][-f (for enabling fake)][-s (to enable sat info)]"); - return 1; } diff --git a/src/platforms/posix/drivers/gyrosim/CMakeLists.txt b/src/platforms/posix/drivers/gyrosim/CMakeLists.txt index 88571e41b6..bac977afbb 100644 --- a/src/platforms/posix/drivers/gyrosim/CMakeLists.txt +++ b/src/platforms/posix/drivers/gyrosim/CMakeLists.txt @@ -35,7 +35,6 @@ px4_add_module( MAIN gyrosim STACK_MAIN 1200 COMPILE_FLAGS - -Wno-packed -Os SRCS gyrosim.cpp diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 012db15942..349fe461c6 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -41,7 +41,6 @@ * @author Mark Charlebois */ -#define __STDC_FORMAT_MACROS #include #include @@ -388,7 +387,7 @@ GYROSIM::~GYROSIM() int GYROSIM::init() { - PX4_INFO("GYROSIM::init"); + PX4_DEBUG("init"); int ret = 1; struct accel_report arp = {}; @@ -535,7 +534,7 @@ GYROSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) void GYROSIM::_set_sample_rate(unsigned desired_sample_rate_hz) { - PX4_INFO("GYROSIM::_set_sample_rate %u Hz", desired_sample_rate_hz); + PX4_DEBUG("_set_sample_rate %u Hz", desired_sample_rate_hz); if (desired_sample_rate_hz == 0 || desired_sample_rate_hz == GYRO_SAMPLERATE_DEFAULT || @@ -554,7 +553,7 @@ GYROSIM::_set_sample_rate(unsigned desired_sample_rate_hz) write_reg(MPUREG_SMPLRT_DIV, div - 1); unsigned sample_rate = 1000 / div; - PX4_INFO("GYROSIM: Changed sample rate to %uHz", sample_rate); + PX4_DEBUG("Changed sample rate to %uHz", sample_rate); setSampleInterval(1000000 / sample_rate); _gyro->setSampleInterval(1000000 / sample_rate); } @@ -1213,7 +1212,7 @@ int GYROSIM_gyro::init() { int ret = VirtDevObj::init(); - PX4_INFO("GYROSIM_gyro::init base class ret: %d", ret); + PX4_DEBUG("GYROSIM_gyro::init base class ret: %d", ret); return ret; } diff --git a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp index cc05e73fcb..961ece334d 100644 --- a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp +++ b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp @@ -249,6 +249,7 @@ ToneAlarm::ToneAlarm() : _tune_names[TONE_EKF_WARNING_TUNE] = "ekf_warning"; // ekf warning _tune_names[TONE_BARO_WARNING_TUNE] = "baro_warning"; // baro warning _tune_names[TONE_SINGLE_BEEP_TUNE] = "beep"; // single beep + _tune_names[TONE_HOME_SET] = "home_set"; } ToneAlarm::~ToneAlarm() @@ -656,8 +657,6 @@ ToneAlarm::devIOCTL(unsigned long cmd, unsigned long arg) /* decide whether to increase the alarm level to cmd or leave it alone */ switch (cmd) { case TONE_SET_ALARM: - PX4_INFO("TONE_SET_ALARM %lu", arg); - if (arg < TONE_NUMBER_OF_TUNES) { if (arg == TONE_STOP_TUNE) { // stop the tune @@ -672,6 +671,7 @@ ToneAlarm::devIOCTL(unsigned long cmd, unsigned long arg) /* play the selected tune */ _default_tune_number = arg; start_tune(_default_tunes[arg]); + PX4_INFO("%s", _tune_names[arg]); } } diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index fe12f233d3..646b94610f 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -92,7 +92,7 @@ static void print_prompt() cout.flush(); } -static void run_cmd(const vector &appargs, bool exit_on_fail) +static void run_cmd(const vector &appargs, bool exit_on_fail, bool silently_fail = false) { // command is appargs[0] string command = appargs[0]; @@ -109,8 +109,6 @@ static void run_cmd(const vector &appargs, bool exit_on_fail) arg[i] = (char *)0; - cout << endl; - int retval = apps[command](i, (char **)arg); if (retval) { @@ -121,16 +119,14 @@ static void run_cmd(const vector &appargs, bool exit_on_fail) } } - - } else if (command.compare("help") == 0) { list_builtins(); - } else if (command.length() == 0) { + } else if (command.length() == 0 || command[0] == '#') { // Do nothing - } else { - cout << endl << "Invalid command: " << command << "\ntype 'help' for a list of commands" << endl; + } else if (!silently_fail) { + cout << "Invalid command: " << command << "\ntype 'help' for a list of commands" << endl; } } @@ -138,20 +134,16 @@ static void run_cmd(const vector &appargs, bool exit_on_fail) static void usage() { - cout << "./mainapp [-d] [startup_config] -h" << std::endl; + cout << "./px4 [-d] [startup_config] -h" << std::endl; cout << " -d - Optional flag to run the app in daemon mode and does not listen for user input." << std::endl; - cout << " This is needed if mainapp is intended to be run as a upstart job on linux" << std::endl; + cout << " This is needed if px4 is intended to be run as a upstart job on linux" << std::endl; cout << " - config file for starting/stopping px4 modules" << std::endl; cout << " -h - help/usage information" << std::endl; } static void process_line(string &line, bool exit_on_fail) { - if (line.length() == 0) { - printf("\n"); - } - vector appargs(10); stringstream(line) >> appargs[0] >> appargs[1] >> appargs[2] >> appargs[3] >> appargs[4] >> appargs[5] >> appargs[6] >> @@ -170,6 +162,21 @@ bool px4_exit_requested(void) return _ExitFlag; } +static void set_cpu_scaling() +{ +#ifdef __PX4_POSIX_EAGLE + // On Snapdragon we miss updates in sdlog2 unless all 4 CPUs are run + // at the maximum frequency all the time. + // Interestingely, cpu0 and cpu3 set the scaling for all 4 CPUs on Snapdragon. + system("echo performance > /sys/devices/system/cpu/cpu0/cpufreq/scaling_governor"); + system("echo performance > /sys/devices/system/cpu/cpu3/cpufreq/scaling_governor"); + + // Alternatively we could also raise the minimum frequency to save some power, + // unfortunately this still lead to some drops. + //system("echo 1190400 > /sys/devices/system/cpu/cpu0/cpufreq/scaling_min_freq"); +#endif +} + int main(int argc, char **argv) { bool daemon_mode = false; @@ -192,6 +199,8 @@ int main(int argc, char **argv) //sigaction(SIGTERM, &sig_int, NULL); sigaction(SIGFPE, &sig_fpe, NULL); + set_cpu_scaling(); + int index = 1; char *commands_file = nullptr; @@ -234,7 +243,7 @@ int main(int argc, char **argv) DriverFramework::Framework::initialize(); px4::init_once(); - px4::init(argc, argv, "mainapp"); + px4::init(argc, argv, "px4"); // if commandfile is present, process the commands from the file if (commands_file != nullptr) { @@ -329,18 +338,19 @@ int main(int argc, char **argv) } if (buf_ptr_write > 0) { - if (mystr != string_buffer[buf_ptr_write - 1]) { + if (!mystr.empty() && mystr != string_buffer[buf_ptr_write - 1]) { string_buffer[buf_ptr_write] = mystr; buf_ptr_write++; } } else { - if (mystr != string_buffer[CMD_BUFF_SIZE - 1]) { + if (!mystr.empty() && mystr != string_buffer[CMD_BUFF_SIZE - 1]) { string_buffer[buf_ptr_write] = mystr; buf_ptr_write++; } } + cout << endl; process_line(mystr, false); mystr = ""; buf_ptr_read = buf_ptr_write; @@ -380,8 +390,11 @@ int main(int argc, char **argv) } default: // any other input - cout << c; - mystr += c; + if (c > 3) { + cout << c; + mystr += c; + } + break; } } @@ -397,7 +410,7 @@ int main(int argc, char **argv) //if (px4_task_is_running("muorb")) { // sending muorb stop is needed if it is running to exit cleanly vector muorb_stop_cmd = { "muorb", "stop" }; - run_cmd(muorb_stop_cmd, !daemon_mode); + run_cmd(muorb_stop_cmd, !daemon_mode, true); } vector shutdown_cmd = { "shutdown" }; diff --git a/src/platforms/posix/px4_layer/CMakeLists.txt b/src/platforms/posix/px4_layer/CMakeLists.txt index abccca4c59..344621c694 100644 --- a/src/platforms/posix/px4_layer/CMakeLists.txt +++ b/src/platforms/posix/px4_layer/CMakeLists.txt @@ -32,6 +32,11 @@ ############################################################################ if("${CONFIG_SHMEM}" STREQUAL "1") + set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon") + include(hexagon_sdk) + + include_directories(${CMAKE_BINARY_DIR}/src/firmware/posix) + include_directories(${HEXAGON_SDK_INCLUDES}) list(APPEND SHMEM_SRCS shmem_posix.c ) @@ -43,8 +48,6 @@ px4_add_module( MODULE platforms__posix__px4_layer COMPILE_FLAGS -Os - -Wno-attributes - -Wno-packed SRCS px4_posix_impl.cpp px4_posix_tasks.cpp diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c index bdd8dfd786..70a9c5f02d 100644 --- a/src/platforms/posix/px4_layer/drv_hrt.c +++ b/src/platforms/posix/px4_layer/drv_hrt.c @@ -45,7 +45,6 @@ #include #include #include -#define __STDC_FORMAT_MACROS #include #include #include "hrt_work.h" diff --git a/src/platforms/posix/px4_layer/module.mk b/src/platforms/posix/px4_layer/module.mk deleted file mode 100644 index efa942c626..0000000000 --- a/src/platforms/posix/px4_layer/module.mk +++ /dev/null @@ -1,48 +0,0 @@ -############################################################################ -# -# 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. -# -############################################################################ - -# -# POSIX platform dependent files -# - -SRCS = \ - px4_posix_impl.cpp \ - px4_posix_tasks.cpp \ - lib_crc32.c \ - drv_hrt.c \ - px4_log.c -ifeq ($(CONFIG_SHMEM), 1) -SRCS += shmem_posix.c -endif - -MAXOPTIMIZATION = -Os diff --git a/src/platforms/posix/px4_layer/px4_log.c b/src/platforms/posix/px4_layer/px4_log.c index 074ae8b466..792caa3012 100644 --- a/src/platforms/posix/px4_layer/px4_log.c +++ b/src/platforms/posix/px4_layer/px4_log.c @@ -7,6 +7,8 @@ __EXPORT int __px4_log_level_current = PX4_LOG_LEVEL_AT_RUN_TIME; __EXPORT const char *__px4_log_level_str[_PX4_LOG_LEVEL_PANIC + 1] = { "INFO", "DEBUG", "WARN", "ERROR", "PANIC" }; +__EXPORT const char *__px4_log_level_color[_PX4_LOG_LEVEL_PANIC + 1] = +{ PX4_ANSI_COLOR_RESET, PX4_ANSI_COLOR_GREEN, PX4_ANSI_COLOR_YELLOW, PX4_ANSI_COLOR_RED, PX4_ANSI_COLOR_RED }; void px4_backtrace() { @@ -28,3 +30,21 @@ void px4_backtrace() free(callstack); #endif } + +__EXPORT void px4_log_modulename(int level, const char *moduleName, const char *fmt, ...) +{ + if (level <= __px4_log_level_current) { + PX4_LOG_COLOR_START + printf(__px4__log_level_fmt __px4__log_level_arg(level)); + PX4_LOG_COLOR_MODULE + printf(__px4__log_modulename_pfmt, moduleName); + PX4_LOG_COLOR_MESSAGE + va_list argptr; + va_start(argptr, fmt); + vprintf(fmt, argptr); + va_end(argptr); + PX4_LOG_COLOR_END + printf("\n"); + } +} + diff --git a/src/platforms/posix/px4_layer/px4_posix_impl.cpp b/src/platforms/posix/px4_layer/px4_posix_impl.cpp index 6adeb39731..7f8a08a5d1 100644 --- a/src/platforms/posix/px4_layer/px4_posix_impl.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_impl.cpp @@ -72,20 +72,19 @@ void init_once(void); void init_once(void) { _shell_task_id = pthread_self(); - printf("[init] shell id: %lu\n", (unsigned long)_shell_task_id); + //printf("[init] shell id: %lu\n", (unsigned long)_shell_task_id); work_queues_init(); hrt_work_queue_init(); hrt_init(); #ifdef CONFIG_SHMEM - PX4_INFO("Syncing params to shared memory\n"); + PX4_DEBUG("Syncing params to shared memory\n"); init_params(); #endif } void init(int argc, char *argv[], const char *app_name) { - printf("[init] task name: %s\n", app_name); printf("\n"); printf("______ __ __ ___ \n"); printf("| ___ \\ \\ \\ / / / |\n"); @@ -94,8 +93,7 @@ void init(int argc, char *argv[], const char *app_name) printf("| | / /^\\ \\ \\___ |\n"); printf("\\_| \\/ \\/ |_/\n"); printf("\n"); - printf("Ready to fly.\n"); - printf("\n"); + printf("%s starting.\n", app_name); printf("\n"); // set the threads name diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp index 34761b0952..908a444802 100644 --- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp @@ -58,6 +58,7 @@ #include #include +#include #define MAX_CMD_LEN 100 @@ -89,11 +90,15 @@ static void *entry_adapter(void *ptr) pthdata_t *data = (pthdata_t *) ptr; int rv; + // set the threads name #ifdef __PX4_DARWIN rv = pthread_setname_np(data->name); #else - rv = pthread_setname_np(pthread_self(), data->name); + char buf[17]; + snprintf(buf, 16, "%s", data->name); + buf[16] = '0'; + rv = pthread_setname_np(pthread_self(), buf); #endif if (rv) { @@ -169,7 +174,7 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int rv = pthread_attr_init(&attr); if (rv != 0) { - PX4_WARN("px4_task_spawn_cmd: failed to init thread attrs"); + PX4_ERR("px4_task_spawn_cmd: failed to init thread attrs"); free(taskdata); return (rv < 0) ? rv : -rv; } @@ -184,6 +189,7 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int if (rv != 0) { PX4_ERR("pthread_attr_setstacksize to %d returned error (%d)", stack_size, rv); + pthread_attr_destroy(&attr); free(taskdata); return (rv < 0) ? rv : -rv; } @@ -193,7 +199,8 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int rv = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED); if (rv != 0) { - PX4_WARN("px4_task_spawn_cmd: failed to set inherit sched"); + PX4_ERR("px4_task_spawn_cmd: failed to set inherit sched"); + pthread_attr_destroy(&attr); free(taskdata); return (rv < 0) ? rv : -rv; } @@ -201,7 +208,8 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int rv = pthread_attr_setschedpolicy(&attr, scheduler); if (rv != 0) { - PX4_WARN("px4_task_spawn_cmd: failed to set sched policy"); + PX4_ERR("px4_task_spawn_cmd: failed to set sched policy"); + pthread_attr_destroy(&attr); free(taskdata); return (rv < 0) ? rv : -rv; } @@ -211,7 +219,8 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int rv = pthread_attr_setschedparam(&attr, ¶m); if (rv != 0) { - PX4_WARN("px4_task_spawn_cmd: failed to set sched param"); + PX4_ERR("px4_task_spawn_cmd: failed to set sched param"); + pthread_attr_destroy(&attr); free(taskdata); return (rv < 0) ? rv : -rv; } @@ -229,6 +238,13 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int } } + if (i >= PX4_MAX_TASKS) { + pthread_attr_destroy(&attr); + pthread_mutex_unlock(&task_mutex); + free(taskdata); + return -ENOSPC; + } + rv = pthread_create(&taskmap[taskid].pid, &attr, &entry_adapter, (void *) taskdata); if (rv != 0) { @@ -240,24 +256,23 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int if (rv != 0) { PX4_ERR("px4_task_spawn_cmd: failed to create thread %d %d\n", rv, errno); taskmap[taskid].isused = false; + pthread_attr_destroy(&attr); pthread_mutex_unlock(&task_mutex); free(taskdata); return (rv < 0) ? rv : -rv; } } else { + pthread_attr_destroy(&attr); pthread_mutex_unlock(&task_mutex); free(taskdata); return (rv < 0) ? rv : -rv; } } + pthread_attr_destroy(&attr); pthread_mutex_unlock(&task_mutex); - if (i >= PX4_MAX_TASKS) { - return -ENOSPC; - } - return i; } @@ -371,27 +386,28 @@ bool px4_task_is_running(const char *taskname) return false; } -__BEGIN_DECLS unsigned long px4_getpid() { return (unsigned long)pthread_self(); } -const char *getprogname(); -const char *getprogname() +const char *px4_get_taskname() { pthread_t pid = pthread_self(); + const char *prog_name = "UnknownApp"; + + pthread_mutex_lock(&task_mutex); for (int i = 0; i < PX4_MAX_TASKS; i++) { if (taskmap[i].isused && taskmap[i].pid == pid) { - pthread_mutex_lock(&task_mutex); - return taskmap[i].name.c_str(); - pthread_mutex_unlock(&task_mutex); + prog_name = taskmap[i].name.c_str(); } } - return "Unknown App"; + pthread_mutex_unlock(&task_mutex); + + return prog_name; } int px4_prctl(int option, const char *arg2, unsigned pid) @@ -417,4 +433,3 @@ int px4_prctl(int option, const char *arg2, unsigned pid) return rv; } -__END_DECLS diff --git a/src/platforms/posix/px4_layer/shmem_posix.c b/src/platforms/posix/px4_layer/shmem_posix.c index c9ec801ca9..66cf5ba9bf 100644 --- a/src/platforms/posix/px4_layer/shmem_posix.c +++ b/src/platforms/posix/px4_layer/shmem_posix.c @@ -51,236 +51,47 @@ #include "systemlib/param/param.h" #include +#include "px4muorb.h" //#define SHMEM_DEBUG - -int mem_fd; -unsigned char *map_base, *virt_addr; -struct shmem_info *shmem_info_p; -static void *map_memory(off_t target); - -int get_shmem_lock(const char *caller_file_name, int caller_line_number); -void release_shmem_lock(void); -void init_shared_memory(void); -void copy_params_to_shmem(struct param_info_s *); void update_to_shmem(param_t param, union param_value_u value); int update_from_shmem(param_t param, union param_value_u *value); void update_index_from_shmem(void); uint64_t update_from_shmem_prev_time = 0, update_from_shmem_current_time = 0; -static unsigned char adsp_changed_index[MAX_SHMEM_PARAMS / 8 + 1]; +extern unsigned char *adsp_changed_index; struct param_wbuf_s { param_t param; union param_value_u val; bool unsaved; }; -extern struct param_wbuf_s *param_find_changed(param_t param); - -#define MEMDEVICE "/dev/mem" - -static void *map_memory(off_t target) -{ - - if ((mem_fd = open(MEMDEVICE, O_RDWR | O_SYNC)) == -1) { - PX4_ERR("Cannot open %s", MEMDEVICE); - exit(1); - } - - /* Map one page */ - map_base = (unsigned char *) mmap(0, MAP_SIZE, PROT_READ | PROT_WRITE, - MAP_SHARED, mem_fd, target & ~MAP_MASK); - - if (map_base == (void *) - 1) { - PX4_ERR("Cannot mmap /dev/atl_mem"); - exit(1); - } - - PX4_DEBUG("Initializing map memory: mem_fd: %d, 0x%X", mem_fd, map_base + (target & MAP_MASK) + LOCK_SIZE); - - return (map_base + (target & MAP_MASK) + LOCK_SIZE); - -} - -int get_shmem_lock(const char *caller_file_name, int caller_line_number) -{ - // TODO FIXME: just say this went through - return 0; - - int i = 0; - - /* TODO: make this comment so somebody can understand it: ioctl calls cmpxchg */ - while (ioctl(mem_fd, LOCK_MEM) != 0) { - - usleep(10000); //sleep for 10 msec - i++; - - if (i > 10) { - PX4_ERR("Could not get lock, file name: %s, line number: %d, errno: %d", - caller_file_name, caller_line_number, errno); - return -1; - } - } - - return 0; //got the lock -} - -void release_shmem_lock(void) -{ - // TODO FIXME: just say this went through - return; - - int ret = ioctl(mem_fd, UNLOCK_MEM); - - if (ret != 0) { - PX4_INFO("unlock failed, ret: %d, errno %d", ret, errno); - } -} - -void init_shared_memory(void) -{ - - virt_addr = map_memory(MAP_ADDRESS); //16K space - shmem_info_p = (struct shmem_info *) virt_addr; - - PX4_DEBUG("linux memory mapped"); -} - -void copy_params_to_shmem(struct param_info_s *param_info_base) -{ - param_t param; - unsigned int i; - - if (get_shmem_lock(__FILE__, __LINE__) != 0) { - PX4_ERR("Could not get shmem lock"); - return; - } - - PX4_DEBUG("%d krait params allocated", param_count()); - - for (param = 0; param < param_count(); param++) { - struct param_wbuf_s *s = param_find_changed(param); - - if (s == NULL) { - shmem_info_p->params_val[param] = param_info_base[param].val; - - } else { - shmem_info_p->params_val[param] = s->val; - } - -#ifdef SHMEM_DEBUG - - if (param_type(param) == PARAM_TYPE_INT32) { - { - PX4_INFO("%d: written %d for param %s to shared mem", - param, shmem_info_p->params_val[param].i, param_name(param)); - } - - } else if (param_type(param) == PARAM_TYPE_FLOAT) { - { - PX4_INFO("%d: written %f for param %s to shared mem", - param, (double)shmem_info_p->params_val[param].f, param_name(param)); - } - } - -#endif - } - - PX4_DEBUG("written %u params to shmem", param_count()); - - for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) { - shmem_info_p->krait_changed_index[i] = 0; - adsp_changed_index[i] = 0; - } - - release_shmem_lock(); -} /*update value and param's change bit in shared memory*/ void update_to_shmem(param_t param, union param_value_u value) { - unsigned int byte_changed, bit_changed; - - if (get_shmem_lock(__FILE__, __LINE__) != 0) { - PX4_ERR("Could not get shmem lock"); - return; + if (px4muorb_param_update_to_shmem(param, (unsigned char *) &value, + sizeof(value))) { + PX4_ERR("krait update param %u failed", param); } - - shmem_info_p->params_val[param] = value; - - byte_changed = param / 8; - bit_changed = 1 << param % 8; - shmem_info_p->krait_changed_index[byte_changed] |= bit_changed; - - PX4_DEBUG("set %d bit on krait changed index[%d] to %d", bit_changed, byte_changed, - shmem_info_p->krait_changed_index[byte_changed]); - -#ifdef SHMEM_DEBUG - - if (param_type(param) == PARAM_TYPE_INT32) { - PX4_INFO("Set value %d for param %s to shmem, set krait index %d:%d", - value.i, param_name(param), byte_changed, bit_changed); - - } else if (param_type(param) == PARAM_TYPE_FLOAT) { - PX4_INFO("Set value %f for param %s to shmem, set krait index %d:%d", - (double)value.f, param_name(param), byte_changed, bit_changed); - } - -#endif - - release_shmem_lock(); - } void update_index_from_shmem(void) { - unsigned int i; - - if (get_shmem_lock(__FILE__, __LINE__) != 0) { - PX4_ERR("Could not get shmem lock"); + if (!adsp_changed_index) { + PX4_ERR("%s no param buffer", __FUNCTION__); return; } - PX4_DEBUG("Updating index from shmem"); - - for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) { - adsp_changed_index[i] = shmem_info_p->adsp_changed_index[i]; - } - - release_shmem_lock(); + px4muorb_param_update_index_from_shmem(adsp_changed_index, + PARAM_BUFFER_SIZE); } static void update_value_from_shmem(param_t param, union param_value_u *value) { - unsigned int byte_changed, bit_changed; - - if (get_shmem_lock(__FILE__, __LINE__) != 0) { - PX4_ERR("Could not get shmem lock"); - return; + if (px4muorb_param_update_value_from_shmem(param, (unsigned char *) value, + sizeof(union param_value_u))) { + PX4_ERR("%s get param failed", __FUNCTION__); } - - *value = shmem_info_p->params_val[param]; - - /*also clear the index since we are holding the lock*/ - byte_changed = param / 8; - bit_changed = 1 << param % 8; - shmem_info_p->adsp_changed_index[byte_changed] &= ~bit_changed; - - release_shmem_lock(); - -#ifdef SHMEM_DEBUG - - if (param_type(param) == PARAM_TYPE_INT32) { - PX4_INFO( - "Got value %d for param %s from shmem, cleared adsp index %d:%d", - value->i, param_name(param), byte_changed, bit_changed); - - } else if (param_type(param) == PARAM_TYPE_FLOAT) { - PX4_INFO( - "Got value %f for param %s from shmem, cleared adsp index %d:%d", - (double)value->f, param_name(param), byte_changed, bit_changed); - } - -#endif } int update_from_shmem(param_t param, union param_value_u *value) @@ -288,6 +99,11 @@ int update_from_shmem(param_t param, union param_value_u *value) unsigned int byte_changed, bit_changed; unsigned int retval = 0; + if (!adsp_changed_index) { + PX4_ERR("%s no param buffer", __FUNCTION__); + return 0; + } + update_from_shmem_current_time = hrt_absolute_time(); if ((update_from_shmem_current_time - update_from_shmem_prev_time) @@ -307,7 +123,8 @@ int update_from_shmem(param_t param, union param_value_u *value) //else {PX4_INFO("no change to param %s", param_name(param));} - PX4_DEBUG("%s %d bit on adsp index[%d]", (retval) ? "cleared" : "unchanged", bit_changed, byte_changed); + PX4_DEBUG("%s %d bit on adsp index[%d]", + (retval) ? "cleared" : "unchanged", bit_changed, byte_changed); return retval; } diff --git a/src/platforms/posix/tests/hello/module.mk b/src/platforms/posix/tests/hello/module.mk deleted file mode 100644 index 294c0ad7fc..0000000000 --- a/src/platforms/posix/tests/hello/module.mk +++ /dev/null @@ -1,43 +0,0 @@ -############################################################################ -# -# Copyright (c) 2014 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Publisher Example Application -# - -MODULE_COMMAND = hello - -SRCS = hello_main.cpp \ - hello_start_posix.cpp \ - hello_example.cpp - diff --git a/src/platforms/posix/tests/hrt_test/module.mk b/src/platforms/posix/tests/hrt_test/module.mk deleted file mode 100644 index e1bd89ef7f..0000000000 --- a/src/platforms/posix/tests/hrt_test/module.mk +++ /dev/null @@ -1,43 +0,0 @@ -############################################################################ -# -# Copyright (c) 2014 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Publisher Example Application -# - -MODULE_COMMAND = hrttest - -SRCS = hrt_test_main.cpp \ - hrt_test_start_posix.cpp \ - hrt_test.cpp - diff --git a/src/platforms/posix/tests/muorb/module.mk b/src/platforms/posix/tests/muorb/module.mk deleted file mode 100644 index 880fe82007..0000000000 --- a/src/platforms/posix/tests/muorb/module.mk +++ /dev/null @@ -1,47 +0,0 @@ -############################################################################ -# -# Copyright (c) 2014 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Publisher Example Application -# - -MODULE_COMMAND = muorb_test - -INCLUDE_DIRS += \ - $(EXT_MUORB_LIB_ROOT)/krait/include \ - $(PX4_BASE)src/modules/muorb/krait - -SRCS = muorb_test_main.cpp \ - muorb_test_start_posix.cpp \ - muorb_test_example.cpp - diff --git a/src/platforms/posix/tests/vcdev_test/module.mk b/src/platforms/posix/tests/vcdev_test/module.mk deleted file mode 100644 index 81920c860c..0000000000 --- a/src/platforms/posix/tests/vcdev_test/module.mk +++ /dev/null @@ -1,43 +0,0 @@ -############################################################################ -# -# Copyright (c) 2014 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Publisher Example Application -# - -MODULE_COMMAND = vcdevtest - -SRCS = vcdevtest_main.cpp \ - vcdevtest_start_posix.cpp \ - vcdevtest_example.cpp - diff --git a/src/platforms/posix/tests/wqueue/module.mk b/src/platforms/posix/tests/wqueue/module.mk deleted file mode 100644 index 4c3b6550c6..0000000000 --- a/src/platforms/posix/tests/wqueue/module.mk +++ /dev/null @@ -1,43 +0,0 @@ -############################################################################ -# -# Copyright (c) 2014 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Publisher Example Application -# - -MODULE_COMMAND = wqueue_test - -SRCS = wqueue_main.cpp \ - wqueue_start_posix.cpp \ - wqueue_test.cpp - diff --git a/src/platforms/posix/work_queue/module.mk b/src/platforms/posix/work_queue/module.mk deleted file mode 100644 index 2a493ab7b6..0000000000 --- a/src/platforms/posix/work_queue/module.mk +++ /dev/null @@ -1,54 +0,0 @@ -############################################################################ -# -# 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. -# -############################################################################ - -# -# POSIX compatible queue and work_queue implementation -# - -SRCS = \ - hrt_thread.c \ - hrt_queue.c \ - hrt_work_cancel.c \ - work_thread.c \ - work_lock.c \ - work_queue.c \ - work_cancel.c \ - queue.c \ - dq_addlast.c \ - dq_remfirst.c \ - sq_addlast.c \ - sq_remfirst.c \ - sq_addafter.c \ - dq_rem.c - -MAXOPTIMIZATION = -Os diff --git a/src/platforms/px4_app.h b/src/platforms/px4_app.h index df3a8d8294..c2310899ac 100644 --- a/src/platforms/px4_app.h +++ b/src/platforms/px4_app.h @@ -72,11 +72,6 @@ private: }; } -// PX4_MAIN is defined if module.mk sets MODULE_COMMAND -// For ROS and NuttX it is "main" and for Linux it is -// $(MODULE_COMMAND)_app_main since some apps already -// define $(MODULE_COMMAND)_main - // Task/process based build #if defined(__PX4_ROS) || defined(__PX4_NUTTX) diff --git a/src/platforms/px4_config.h b/src/platforms/px4_config.h index e44fd4afce..22879330fd 100644 --- a/src/platforms/px4_config.h +++ b/src/platforms/px4_config.h @@ -43,6 +43,7 @@ #include #include +#include "px4_micro_hal.h" #elif defined (__PX4_POSIX) diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 38ea682943..b41dc7b0eb 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -120,6 +120,10 @@ typedef param_t px4_param_t; #define PRId64 "lld" #endif +#if !defined(offsetof) +# define offsetof(TYPE, MEMBER) __builtin_offsetof (TYPE, MEMBER) +#endif + /* * POSIX Specific defines */ @@ -160,6 +164,10 @@ __END_DECLS #if defined(__PX4_QURT) #define PX4_ROOTFSDIR +#elif defined(__PX4_POSIX_EAGLE) +#define PX4_ROOTFSDIR "/home/linaro" +#elif defined(__PX4_POSIX_BEBOP) +#define PX4_ROOTFSDIR "/home/root" #else #define PX4_ROOTFSDIR "rootfs" #endif @@ -227,7 +235,6 @@ __END_DECLS #if defined(__PX4_QURT) -#define PX4_ROOTFSDIR #define DEFAULT_PARAM_FILE "/fs/eeprom/parameters" #define SIOCDEVPRIVATE 999999 diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index a38b1e7cb3..434bd5e78b 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -53,8 +53,6 @@ #include #include #include -#include -#include #include #include #include @@ -82,10 +80,6 @@ #include #include #include -#include -#include -#include -#include #include #include #include @@ -117,10 +111,6 @@ #include #include #include -#include -#include -#include -#include #include #include #include @@ -148,10 +138,6 @@ #include #include #include -#include -#include -#include -#include #include #include #include diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index 953b766f4d..1eb13a0e77 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * Copyright (C) 2015-2016 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 @@ -50,6 +50,7 @@ static inline void do_nothing(int level, ...) (void)level; } + /**************************************************************************** * __px4_log_omit: * Compile out the message @@ -59,11 +60,11 @@ static inline void do_nothing(int level, ...) #if defined(__PX4_ROS) #include -#define PX4_PANIC(...) ROS_WARN(__VA_ARGS__) -#define PX4_ERR(...) ROS_WARN(__VA_ARGS__) +#define PX4_PANIC(...) ROS_FATAL(__VA_ARGS__) +#define PX4_ERR(...) ROS_ERROR(__VA_ARGS__) #define PX4_WARN(...) ROS_WARN(__VA_ARGS__) -#define PX4_INFO(...) ROS_WARN(__VA_ARGS__) -#define PX4_DEBUG(...) +#define PX4_INFO(...) ROS_INFO(__VA_ARGS__) +#define PX4_DEBUG(...) ROS_DEBUG(__VA_ARGS__) #define PX4_BACKTRACE() #elif defined(__PX4_QURT) @@ -117,19 +118,23 @@ static inline void do_nothing(int level, ...) #else -#define __STDC_FORMAT_MACROS #include #include #include #include +#include + #include __BEGIN_DECLS __EXPORT extern uint64_t hrt_absolute_time(void); -__EXPORT extern const char *__px4_log_level_str[5]; +__EXPORT extern const char *__px4_log_level_str[_PX4_LOG_LEVEL_PANIC + 1]; +__EXPORT extern const char *__px4_log_level_color[_PX4_LOG_LEVEL_PANIC + 1]; __EXPORT extern int __px4_log_level_current; __EXPORT extern void px4_backtrace(void); +__EXPORT void px4_log_modulename(int level, const char *moduleName, const char *fmt, ...); + __END_DECLS #define PX4_BACKTRACE() px4_backtrace() @@ -150,17 +155,56 @@ __END_DECLS #define __px4__log_printcond(cond, ...) if (cond) printf(__VA_ARGS__) #define __px4__log_printline(level, ...) if (level <= __px4_log_level_current) printf(__VA_ARGS__) + +#ifndef MODULE_NAME +#define MODULE_NAME "Unknown" +#endif + #define __px4__log_timestamp_fmt "%-10" PRIu64 " " #define __px4__log_timestamp_arg ,hrt_absolute_time() #define __px4__log_level_fmt "%-5s " #define __px4__log_level_arg(level) ,__px4_log_level_str[level] #define __px4__log_thread_fmt "%#X " #define __px4__log_thread_arg ,(unsigned int)pthread_self() +#define __px4__log_modulename_fmt "%-10s " +#define __px4__log_modulename_pfmt "[%s] " +#define __px4__log_modulename_arg ,"[" MODULE_NAME "]" #define __px4__log_file_and_line_fmt " (file %s line %u)" #define __px4__log_file_and_line_arg , __FILE__, __LINE__ #define __px4__log_end_fmt "\n" -#define __px4__log_endline ) + +#define PX4_ANSI_COLOR_RED "\x1b[31m" +#define PX4_ANSI_COLOR_GREEN "\x1b[32m" +#define PX4_ANSI_COLOR_YELLOW "\x1b[33m" +#define PX4_ANSI_COLOR_BLUE "\x1b[34m" +#define PX4_ANSI_COLOR_MAGENTA "\x1b[35m" +#define PX4_ANSI_COLOR_CYAN "\x1b[36m" +#define PX4_ANSI_COLOR_GRAY "\x1B[37m" +#define PX4_ANSI_COLOR_RESET "\x1b[0m" + +#ifdef __PX4_POSIX +#define PX4_LOG_COLORIZED_OUTPUT //if defined and output is a tty, colorize the output according to the log level +#endif /* __PX4_POSIX */ + + +#ifdef PX4_LOG_COLORIZED_OUTPUT +#include +#define PX4_LOG_COLOR_START \ + int use_color = isatty(STDOUT_FILENO); \ + if (use_color) printf("%s", __px4_log_level_color[level]); +#define PX4_LOG_COLOR_MODULE \ + if (use_color) printf(PX4_ANSI_COLOR_GRAY); +#define PX4_LOG_COLOR_MESSAGE \ + if (use_color) printf("%s", __px4_log_level_color[level]); +#define PX4_LOG_COLOR_END \ + if (use_color) printf(PX4_ANSI_COLOR_RESET); +#else +#define PX4_LOG_COLOR_START +#define PX4_LOG_COLOR_MODULE +#define PX4_LOG_COLOR_MESSAGE +#define PX4_LOG_COLOR_END +#endif /* PX4_LOG_COLORIZED_OUTPUT */ /**************************************************************************** * Output format macros @@ -199,6 +243,19 @@ __END_DECLS __px4__log_level_arg(level), ##__VA_ARGS__\ ) +/**************************************************************************** + * __px4_log_modulename: + * Convert a message in the form: + * PX4_WARN("val is %d", val); + * to + * printf("%-5s [%s] val is %d\n", __px4_log_level_str[3], + * MODULENAME, val); + ****************************************************************************/ + +#define __px4_log_modulename(level, fmt, ...) \ + do { \ + px4_log_modulename(level, MODULE_NAME, fmt, ##__VA_ARGS__); \ + } while(0) /**************************************************************************** * __px4_log_timestamp: * Convert a message in the form: @@ -338,7 +395,7 @@ __END_DECLS * Messages that should never be filtered or compiled out ****************************************************************************/ #define PX4_LOG(FMT, ...) __px4_log(_PX4_LOG_LEVEL_ALWAYS, FMT, ##__VA_ARGS__) -#define PX4_INFO(FMT, ...) __px4_log(_PX4_LOG_LEVEL_ALWAYS, FMT, ##__VA_ARGS__) +#define PX4_INFO(FMT, ...) __px4_log_modulename(_PX4_LOG_LEVEL_ALWAYS, FMT, ##__VA_ARGS__) #if defined(TRACE_BUILD) /**************************************************************************** @@ -362,8 +419,8 @@ __END_DECLS /**************************************************************************** * Non-verbose settings for a Release build to minimize strings in build ****************************************************************************/ -#define PX4_PANIC(FMT, ...) __px4_log(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__) -#define PX4_ERR(FMT, ...) __px4_log(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__) +#define PX4_PANIC(FMT, ...) __px4_log_modulename(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__) +#define PX4_ERR(FMT, ...) __px4_log_modulename(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__) #define PX4_WARN(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_WARN, FMT, ##__VA_ARGS__) #define PX4_DEBUG(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__) @@ -371,9 +428,9 @@ __END_DECLS /**************************************************************************** * Medium verbose settings for a default build ****************************************************************************/ -#define PX4_PANIC(FMT, ...) __px4_log(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__) -#define PX4_ERR(FMT, ...) __px4_log(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__) -#define PX4_WARN(FMT, ...) __px4_log(_PX4_LOG_LEVEL_WARN, FMT, ##__VA_ARGS__) +#define PX4_PANIC(FMT, ...) __px4_log_modulename(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__) +#define PX4_ERR(FMT, ...) __px4_log_modulename(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__) +#define PX4_WARN(FMT, ...) __px4_log_modulename(_PX4_LOG_LEVEL_WARN, FMT, ##__VA_ARGS__) #define PX4_DEBUG(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__) #endif diff --git a/src/platforms/px4_micro_hal.h b/src/platforms/px4_micro_hal.h new file mode 100644 index 0000000000..25017b3f0e --- /dev/null +++ b/src/platforms/px4_micro_hal.h @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * Copyright (c) 2016 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. + * + ****************************************************************************/ +#pragma once +/* + * This file is a shim to bridge to nuttx_v3 + */ +#ifdef __PX4_NUTTX + +# define px4_enter_critical_section() irqsave() +# define px4_leave_critical_section(flags) irqrestore(flags) + +# define px4_spibus_initialize(port_1based) up_spiinitialize(port_1based) + +# define px4_i2cbus_initialize(bus_num_1based) up_i2cinitialize(bus_num_1based) +# define px4_i2cbus_uninitialize(pdev) up_i2cuninitialize(pdev) + +# if defined(CONFIG_STM32_VALUELINE) || defined(CONFIG_STM32_STM32F40XX) +# define px4_arch_configgpio(pinset) stm32_configgpio(pinset) +# define px4_arch_unconfiggpio(pinset) stm32_unconfiggpio(pinset) +# define px4_arch_gpioread(pinset) stm32_gpioread(pinset) +# define px4_arch_gpiowrite(pinset, value) stm32_gpiowrite(pinset, value) +# endif +#endif diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 31afbfd031..9ae11474de 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -48,7 +48,6 @@ /* includes when building for ros */ #include "ros/ros.h" #include -#define __STDC_FORMAT_MACROS #include #include #else diff --git a/src/platforms/px4_tasks.h b/src/platforms/px4_tasks.h index 6b6fdbddf4..6fa3619ae5 100644 --- a/src/platforms/px4_tasks.h +++ b/src/platforms/px4_tasks.h @@ -134,5 +134,8 @@ __EXPORT bool px4_task_is_running(const char *taskname); __EXPORT int px4_prctl(int option, const char *arg2, unsigned pid); #endif +/** return the name of the current task */ +__EXPORT const char *px4_get_taskname(void); + __END_DECLS diff --git a/src/platforms/qurt/tests/muorb/module.mk b/src/platforms/qurt/fc_addon/mpu_spi/CMakeLists.txt similarity index 63% rename from src/platforms/qurt/tests/muorb/module.mk rename to src/platforms/qurt/fc_addon/mpu_spi/CMakeLists.txt index e21ee3c60b..d227c1f1cb 100644 --- a/src/platforms/qurt/tests/muorb/module.mk +++ b/src/platforms/qurt/fc_addon/mpu_spi/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2014 PX4 Development Team. All rights reserved. +# Copyright (C) 2015 Mark Charlebois. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -31,18 +31,40 @@ # ############################################################################ -# -# Publisher Example Application -# +set(TOOLS_ERROR_MSG + "The FC_ADDON must be installed and the environment variable FC_ADDON must be set" + "(e.g. export FC_ADDON=${HOME}/Qualcomm/addon)") -MODULE_COMMAND = muorb_test +if ("$ENV{FC_ADDON}" STREQUAL "") + message(FATAL_ERROR ${TOOLS_ERROR_MSG}) +else() + set(FC_ADDON $ENV{FC_ADDON}) +endif() -SRCS = \ - muorb_test_start_qurt.cpp \ - muorb_test_example.cpp +add_library(libmpu9x50 SHARED IMPORTED GLOBAL) +set_target_properties(libmpu9x50 PROPERTIES IMPORTED_LOCATION ${FC_ADDON}/flight_controller/hexagon/libs/libmpu9x50.so) -INCLUDE_DIRS += $(PX4_BASE)/src/modules/uORB \ - $(PX4_BASE)/src/platforms \ - $(PX4_BASE)/src/modules +include_directories( + ${FC_ADDON}/flight_controller/hexagon/inc + ) +px4_add_module( + MODULE platforms__qurt__fc_addon__mpu_spi + MAIN mpu9x50 + STACK_MAIN 1200 + COMPILE_FLAGS + -Os -Weffc++ + SRCS + mpu9x50_main.cpp + mpu9x50_params.c + DEPENDS + platforms__common + ) +set(module_external_libraries + ${module_external_libraries} + libmpu9x50 + CACHE INTERNAL "module_external_libraries" + ) + +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/qurt/fc_addon/mpu_spi/mpu9x50_main.cpp b/src/platforms/qurt/fc_addon/mpu_spi/mpu9x50_main.cpp new file mode 100644 index 0000000000..246e91d548 --- /dev/null +++ b/src/platforms/qurt/fc_addon/mpu_spi/mpu9x50_main.cpp @@ -0,0 +1,667 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include +#include + +#include + +#include +#include + +// TODO-JYW: +// Include references to the driver framework. This will produce a duplicate definition +// of qurt_log. Use a #define for the qurt_log function to avoid redefinition. This code +// change must still be made. Document the latter change to be clear. +// #include +// #include + +#include +#include +#include +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include +#ifdef __cplusplus +} +#endif + +/** driver 'main' command */ +extern "C" { __EXPORT int mpu9x50_main(int argc, char *argv[]); } + +#define MAX_LEN_DEV_PATH 32 + +// TODO - need to load this from parameter file +#define SPI_INT_GPIO 65 // GPIO pin for Data Ready Interrupt +namespace mpu9x50 +{ + +/** SPI device path that mpu9x50 is connected to */ +static char _device[MAX_LEN_DEV_PATH]; + +/** flag indicating if mpu9x50 app is running */ +static bool _is_running = false; + +/** flag indicating if measurement thread should exit */ +static bool _task_should_exit = false; + +/** handle to the task main thread */ +static px4_task_t _task_handle = -1; + +/** IMU measurement data */ +static struct mpu9x50_data _data; + +static orb_advert_t _gyro_pub = nullptr; /**< gyro data publication */ +static int _gyro_orb_class_instance; /**< instance handle for gyro devices */ +static orb_advert_t _accel_pub = nullptr; /**< accelerameter data publication */ +static int _accel_orb_class_instance; /**< instance handle for accel devices */ +static orb_advert_t _mag_pub = nullptr; /**< compass data publication */ +static int _mag_orb_class_instance; /**< instance handle for mag devices */ +static int _params_sub; /**< parameter update subscription */ +static struct gyro_report _gyro; /**< gyro report */ +static struct accel_report _accel; /**< accel report */ +static struct mag_report _mag; /**< mag report */ +static struct gyro_calibration_s _gyro_sc; /**< gyro scale */ +static struct accel_calibration_s _accel_sc; /**< accel scale */ +static struct mag_calibration_s _mag_sc; /**< mag scale */ +static enum gyro_lpf_e _gyro_lpf = MPU9X50_GYRO_LPF_20HZ; /**< gyro lpf enum value */ +static enum acc_lpf_e _accel_lpf = MPU9X50_ACC_LPF_20HZ; /**< accel lpf enum value */ +static enum gyro_sample_rate_e _imu_sample_rate = MPU9x50_SAMPLE_RATE_500HZ; /**< IMU sample rate enum */ + +struct { + param_t accel_x_offset; + param_t accel_x_scale; + param_t accel_y_offset; + param_t accel_y_scale; + param_t accel_z_offset; + param_t accel_z_scale; + param_t gyro_x_offset; + param_t gyro_x_scale; + param_t gyro_y_offset; + param_t gyro_y_scale; + param_t gyro_z_offset; + param_t gyro_z_scale; + param_t mag_x_offset; + param_t mag_x_scale; + param_t mag_y_offset; + param_t mag_y_scale; + param_t mag_z_offset; + param_t mag_z_scale; + param_t gyro_lpf_enum; + param_t accel_lpf_enum; + param_t imu_sample_rate_enum; +} _params_handles; /**< parameter handles */ + +bool exit_mreasurement = false; + + +/** Print out the usage information */ +static void usage(); + +/** mpu9x50 stop measurement */ +static void stop(); + +/** task main trampoline function */ +static void task_main_trampoline(int argc, char *argv[]); + +/** mpu9x50 measurement thread primary entry point */ +static void task_main(int argc, char *argv[]); + +/** + * create and advertise all publicatoins + * @return true on success, false otherwise + */ +static bool create_pubs(); + +/** update all sensor reports with the latest sensor reading */ +static void update_reports(); + +/** publish all sensor reports */ +static void publish_reports(); + +/** update all parameters */ +static void parameters_update(); + +/** initialize all parameter handles and load the initial parameter values */ +static void parameters_init(); + +/** poll parameter update */ +static void parameter_update_poll(); + +static int64_t _isr_data_ready_timestamp = 0; + +/** + * MPU9x50 data ready interrupt service routine + * @param[out] context address of the context data provided by user whill + * registering the interrupt servcie routine + */ +static void *data_ready_isr(void *context); + +void *data_ready_isr(void *context) +{ + if (exit_mreasurement) { + return NULL; + } + + _isr_data_ready_timestamp = hrt_absolute_time(); + // send signal to measurement thread + px4_task_kill(_task_handle, SIGRTMIN); + + return NULL; +} + +void parameter_update_poll() +{ + bool updated; + + /* Check if parameters have changed */ + orb_check(_params_sub, &updated); + + if (updated) { + struct parameter_update_s param_update; + orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update); + parameters_update(); + } +} + +void parameters_update() +{ + PX4_DEBUG("mpu9x50_parameters_update"); + float v; + int v_int; + + // accel params + if (param_get(_params_handles.accel_x_offset, &v) == 0) { + _accel_sc.x_offset = v; + PX4_DEBUG("mpu9x50_parameters_update accel_x_offset %f", v); + } + + if (param_get(_params_handles.accel_x_scale, &v) == 0) { + _accel_sc.x_scale = v; + PX4_DEBUG("mpu9x50_parameters_update accel_x_scale %f", v); + } + + if (param_get(_params_handles.accel_y_offset, &v) == 0) { + _accel_sc.y_offset = v; + PX4_DEBUG("mpu9x50_parameters_update accel_y_offset %f", v); + } + + if (param_get(_params_handles.accel_y_scale, &v) == 0) { + _accel_sc.y_scale = v; + PX4_DEBUG("mpu9x50_parameters_update accel_y_scale %f", v); + } + + if (param_get(_params_handles.accel_z_offset, &v) == 0) { + _accel_sc.z_offset = v; + PX4_DEBUG("mpu9x50_parameters_update accel_z_offset %f", v); + } + + if (param_get(_params_handles.accel_z_scale, &v) == 0) { + _accel_sc.z_scale = v; + PX4_DEBUG("mpu9x50_parameters_update accel_z_scale %f", v); + } + + // gyro params + if (param_get(_params_handles.gyro_x_offset, &v) == 0) { + _gyro_sc.x_offset = v; + PX4_DEBUG("mpu9x50_parameters_update gyro_x_offset %f", v); + } + + if (param_get(_params_handles.gyro_x_scale, &v) == 0) { + _gyro_sc.x_scale = v; + PX4_DEBUG("mpu9x50_parameters_update gyro_x_scale %f", v); + } + + if (param_get(_params_handles.gyro_y_offset, &v) == 0) { + _gyro_sc.y_offset = v; + PX4_DEBUG("mpu9x50_parameters_update gyro_y_offset %f", v); + } + + if (param_get(_params_handles.gyro_y_scale, &v) == 0) { + _gyro_sc.y_scale = v; + PX4_DEBUG("mpu9x50_parameters_update gyro_y_scale %f", v); + } + + if (param_get(_params_handles.gyro_z_offset, &v) == 0) { + _gyro_sc.z_offset = v; + PX4_DEBUG("mpu9x50_parameters_update gyro_z_offset %f", v); + } + + if (param_get(_params_handles.gyro_z_scale, &v) == 0) { + _gyro_sc.z_scale = v; + PX4_DEBUG("mpu9x50_parameters_update gyro_z_scale %f", v); + } + + // mag params + if (param_get(_params_handles.mag_x_offset, &v) == 0) { + _mag_sc.x_offset = v; + PX4_DEBUG("mpu9x50_parameters_update mag_x_offset %f", v); + } + + if (param_get(_params_handles.mag_x_scale, &v) == 0) { + _mag_sc.x_scale = v; + PX4_DEBUG("mpu9x50_parameters_update mag_x_scale %f", v); + } + + if (param_get(_params_handles.mag_y_offset, &v) == 0) { + _mag_sc.y_offset = v; + PX4_DEBUG("mpu9x50_parameters_update mag_y_offset %f", v); + } + + if (param_get(_params_handles.mag_y_scale, &v) == 0) { + _mag_sc.y_scale = v; + PX4_DEBUG("mpu9x50_parameters_update mag_y_scale %f", v); + } + + if (param_get(_params_handles.mag_z_offset, &v) == 0) { + _mag_sc.z_offset = v; + PX4_DEBUG("mpu9x50_parameters_update mag_z_offset %f", v); + } + + if (param_get(_params_handles.mag_z_scale, &v) == 0) { + _mag_sc.z_scale = v; + PX4_DEBUG("mpu9x50_parameters_update mag_z_scale %f", v); + } + + // LPF params + if (param_get(_params_handles.gyro_lpf_enum, &v_int) == 0) { + if (v_int >= NUM_MPU9X50_GYRO_LPF) { + PX4_WARN("invalid gyro_lpf_enum %d use default %d", v_int, _gyro_lpf); + + } else { + _gyro_lpf = (enum gyro_lpf_e)v_int; + PX4_DEBUG("mpu9x50_parameters_update gyro_lpf_enum %d", _gyro_lpf); + } + } + + if (param_get(_params_handles.accel_lpf_enum, &v_int) == 0) { + if (v_int >= NUM_MPU9X50_ACC_LPF) { + PX4_WARN("invalid accel_lpf_enum %d use default %d", v_int, _accel_lpf); + + } else { + _accel_lpf = (enum acc_lpf_e)v_int; + PX4_DEBUG("mpu9x50_parameters_update accel_lpf_enum %d", _accel_lpf); + } + } + + if (param_get(_params_handles.imu_sample_rate_enum, &v_int) == 0) { + if (v_int >= NUM_MPU9X50_SAMPLE_RATE) { + PX4_WARN("invalid imu_sample_rate %d use default %d", v_int, _imu_sample_rate); + + } else { + _imu_sample_rate = (enum gyro_sample_rate_e)v_int; + PX4_DEBUG("mpu9x50_parameters_update imu_sample_rate %d", _imu_sample_rate); + } + } +} + +void parameters_init() +{ + _params_handles.accel_x_offset = param_find("CAL_ACC0_XOFF"); + _params_handles.accel_x_scale = param_find("CAL_ACC0_XSCALE"); + _params_handles.accel_y_offset = param_find("CAL_ACC0_YOFF"); + _params_handles.accel_y_scale = param_find("CAL_ACC0_YSCALE"); + _params_handles.accel_z_offset = param_find("CAL_ACC0_ZOFF"); + _params_handles.accel_z_scale = param_find("CAL_ACC0_ZSCALE"); + + _params_handles.gyro_x_offset = param_find("CAL_GYRO0_XOFF"); + _params_handles.gyro_x_scale = param_find("CAL_GYRO0_XSCALE"); + _params_handles.gyro_y_offset = param_find("CAL_GYRO0_YOFF"); + _params_handles.gyro_y_scale = param_find("CAL_GYRO0_YSCALE"); + _params_handles.gyro_z_offset = param_find("CAL_GYRO0_ZOFF"); + _params_handles.gyro_z_scale = param_find("CAL_GYRO0_ZSCALE"); + + _params_handles.mag_x_offset = param_find("CAL_MAG0_XOFF"); + _params_handles.mag_x_scale = param_find("CAL_MAG0_XSCALE"); + _params_handles.mag_y_offset = param_find("CAL_MAG0_YOFF"); + _params_handles.mag_y_scale = param_find("CAL_MAG0_YSCALE"); + _params_handles.mag_z_offset = param_find("CAL_MAG0_ZOFF"); + _params_handles.mag_z_scale = param_find("CAL_MAG0_ZSCALE"); + + _params_handles.gyro_lpf_enum = param_find("MPU_GYRO_LPF_ENM"); + _params_handles.accel_lpf_enum = param_find("MPU_ACC_LPF_ENM"); + + _params_handles.imu_sample_rate_enum = param_find("MPU_SAMPLE_R_ENM"); + + parameters_update(); +} + +bool create_pubs() +{ + // initialize the reports + memset(&_gyro, 0, sizeof(struct gyro_report)); + memset(&_accel, 0, sizeof(struct accel_report)); + memset(&_mag, 0, sizeof(struct mag_report)); + + _gyro_pub = orb_advertise_multi(ORB_ID(sensor_gyro), &_gyro, + &_gyro_orb_class_instance, ORB_PRIO_HIGH - 1); + + if (_gyro_pub == nullptr) { + PX4_ERR("sensor_gyro advert fail"); + return false; + } + + _accel_pub = orb_advertise_multi(ORB_ID(sensor_accel), &_accel, + &_accel_orb_class_instance, ORB_PRIO_HIGH - 1); + + if (_accel_pub == nullptr) { + PX4_ERR("sensor_accel advert fail"); + return false; + } + + _mag_pub = orb_advertise_multi(ORB_ID(sensor_mag), &_mag, + &_mag_orb_class_instance, ORB_PRIO_HIGH - 1); + + if (_mag_pub == nullptr) { + PX4_ERR("sensor_mag advert fail"); + return false; + } + + return true; +} + +void update_reports() +{ + _gyro.timestamp = _data.timestamp; + _gyro.x = ((_data.gyro_raw[0] * _data.gyro_scaling) - _gyro_sc.x_offset) * _gyro_sc.x_scale; + _gyro.y = ((_data.gyro_raw[1] * _data.gyro_scaling) - _gyro_sc.y_offset) * _gyro_sc.y_scale; + _gyro.z = ((_data.gyro_raw[2] * _data.gyro_scaling) - _gyro_sc.z_offset) * _gyro_sc.z_scale; + _gyro.temperature = _data.temperature; + _gyro.range_rad_s = _data.gyro_range_rad_s; + _gyro.scaling = _data.gyro_scaling; + _gyro.x_raw = _data.gyro_raw[0]; + _gyro.y_raw = _data.gyro_raw[1]; + _gyro.z_raw = _data.gyro_raw[2]; + _gyro.temperature_raw = _data.temperature_raw; + + _accel.timestamp = _data.timestamp; + _accel.x = ((_data.accel_raw[0] * _data.accel_scaling) - _accel_sc.x_offset) * _accel_sc.x_scale; + _accel.y = ((_data.accel_raw[1] * _data.accel_scaling) - _accel_sc.y_offset) * _accel_sc.y_scale; + _accel.z = ((_data.accel_raw[2] * _data.accel_scaling) - _accel_sc.z_offset) * _accel_sc.z_scale; + _accel.temperature = _data.temperature; + _accel.range_m_s2 = _data.accel_range_m_s2; + _accel.scaling = _data.accel_scaling; + _accel.x_raw = _data.accel_raw[0]; + _accel.y_raw = _data.accel_raw[1]; + _accel.z_raw = _data.accel_raw[2]; + _accel.temperature_raw = _data.temperature_raw; + + if (_data.mag_data_ready) { + _mag.timestamp = _data.timestamp; + _mag.x = ((_data.mag_raw[0] * _data.mag_scaling) - _mag_sc.x_offset) * _mag_sc.x_scale; + _mag.y = ((_data.mag_raw[1] * _data.mag_scaling) - _mag_sc.y_offset) * _mag_sc.y_scale; + _mag.z = ((_data.mag_raw[2] * _data.mag_scaling) - _mag_sc.z_offset) * _mag_sc.z_scale; + _mag.range_ga = _data.mag_range_ga; + _mag.scaling = _data.mag_scaling; + _mag.temperature = _data.temperature; + _mag.x_raw = _data.mag_raw[0]; + _mag.y_raw = _data.mag_raw[1]; + _mag.z_raw = _data.mag_raw[2]; + } +} + +void publish_reports() +{ + if (orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &_gyro) != OK) { + PX4_WARN("failed to publish gyro report"); + + } else { + //PX4_DEBUG("MPU_GYRO_RAW: %d %d %d", _gyro.x_raw, _gyro.y_raw, _gyro.z_raw) + //PX4_DEBUG("MPU_GYRO: %f %f %f", _gyro.x, _gyro.y, _gyro.z) + } + + if (orb_publish(ORB_ID(sensor_accel), _accel_pub, &_accel) != OK) { + PX4_WARN("failed to publish accel report"); + + } else { + //PX4_DEBUG("MPU_ACCEL_RAW: %d %d %d", _accel.x_raw, _accel.y_raw, _accel.z_raw) + //PX4_DEBUG("MPU_ACCEL: %f %f %f", _accel.x, _accel.y, _accel.z) + } + + if (_data.mag_data_ready) { + if (orb_publish(ORB_ID(sensor_mag), _mag_pub, &_mag) != OK) { + PX4_WARN("failed to publish mag report"); + + } else { + //PX4_DEBUG("MPU_MAG_RAW: %d %d %d", _mag.x_raw, _mag.y_raw, _mag.z_raw) + //PX4_DEBUG("MPU_MAG: %f %f %f", _mag.x, _mag.y, _mag.z) + } + } +} + +void task_main(int argc, char *argv[]) +{ + PX4_WARN("enter mpu9x50 task_main"); + + sigset_t set; + int sig = 0; + int rv; + exit_mreasurement = false; + + parameters_init(); + + // create the mpu9x50 default configuration structure + struct mpu9x50_config config = { + .gyro_lpf = _gyro_lpf, + .acc_lpf = _accel_lpf, + .gyro_fsr = MPU9X50_GYRO_FSR_500DPS, + .acc_fsr = MPU9X50_ACC_FSR_4G, + .gyro_sample_rate = _imu_sample_rate, + .compass_enabled = true, + .compass_sample_rate = MPU9x50_COMPASS_SAMPLE_RATE_100HZ, + .spi_dev_path = _device, + }; + + // TODO-JYW: + // manually register with the DriverFramework to allow this driver to + // be found by other modules +// DriverFramework::StubSensor stub_sensor("mpu9x50", "/dev/accel0", "/dev/accel"); + + if (mpu9x50_initialize(&config) != 0) { + PX4_WARN("error initializing mpu9x50. Quit!"); + goto exit; + } + + if (mpu9x50_register_interrupt(SPI_INT_GPIO, &mpu9x50::data_ready_isr, NULL) + //if (mpu9x50_register_interrupt(SPI_INT_GPIO, &mpu9x50::data_ready_isr, NULL) + + != 0) { + PX4_WARN("error registering data ready interrupt. Quit!"); + goto exit; + } + + // create all uorb publications + if (!create_pubs()) { + goto exit; + } + + // subscribe to parameter_update topic + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + + // initialize signal + sigemptyset(&set); + sigaddset(&set, SIGRTMIN); + + while (!_task_should_exit) { + // wait on signal + rv = sigwait(&set, &sig); + + // check if we are waken up by the proper signal + if (rv != 0 || sig != SIGRTMIN) { + PX4_WARN("mpu9x50 sigwait failed rv %d sig %d", rv, sig); + continue; + } + + // read new IMU data and store the results in data + if (mpu9x50_get_data(&_data) != 0) { + PX4_WARN("mpu9x50_get_data() failed"); + continue; + } + + // since the context switch takes long time, override the timestamp provided by mpu9x50_get_data() + // with the ts of isr. + // Note: This is ok for MPU sample rate of < 1000; Higer than 1000 Sample rates will be an issue + // as the data is not consistent. + _data.timestamp = _isr_data_ready_timestamp; + + // poll parameter update + parameter_update_poll(); + + // data is ready + update_reports(); + + // publish all sensor reports + publish_reports(); + + } + + exit_mreasurement = true; + +exit: + PX4_WARN("closing mpu9x50"); + mpu9x50_close(); +} + +/** mpu9x50 main entrance */ +void task_main_trampoline(int argc, char *argv[]) +{ + PX4_WARN("task_main_trampoline"); + task_main(argc, argv); +} + +void start() +{ + ASSERT(_task_handle == -1); + + /* start the task */ + _task_handle = px4_task_spawn_cmd("mpu9x50_main", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX, + 1500, + (px4_main_t)&task_main_trampoline, + nullptr); + + if (_task_handle < 0) { + warn("mpu9x50_main task start failed"); + return; + } + + _is_running = true; +} + +void stop() +{ + // TODO - set thread exit signal to terminate the task main thread + + _is_running = false; + _task_handle = -1; +} + +void usage() +{ + PX4_WARN("missing command: try 'start', 'stop', 'status'"); + PX4_WARN("options:"); + PX4_WARN(" -D device"); +} + +}; // namespace mpu9x50 + + +int mpu9x50_main(int argc, char *argv[]) +{ + const char *device = NULL; + int ch; + int myoptind = 1; + const char *myoptarg = NULL; + + while ((ch = px4_getopt(argc, argv, "D:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'D': + device = myoptarg; + break; + + default: + mpu9x50::usage(); + return 1; + } + } + + // Check on required arguments + if (device == NULL || strlen(device) == 0) { + mpu9x50::usage(); + return 1; + } + + memset(mpu9x50::_device, 0, MAX_LEN_DEV_PATH); + strncpy(mpu9x50::_device, device, strlen(device)); + + const char *verb = argv[myoptind]; + + /* + * Start/load the driver. + */ + if (!strcmp(verb, "start")) { + if (mpu9x50::_is_running) { + PX4_WARN("mpu9x50 already running"); + return 1; + } + + mpu9x50::start(); + + } else if (!strcmp(verb, "stop")) { + if (mpu9x50::_is_running) { + PX4_WARN("mpu9x50 is not running"); + return 1; + } + + mpu9x50::stop(); + + } else if (!strcmp(verb, "status")) { + PX4_WARN("mpu9x50 is %s", mpu9x50::_is_running ? "running" : "stopped"); + return 0; + + } else { + mpu9x50::usage(); + return 1; + } + + return 0; +} diff --git a/src/platforms/qurt/fc_addon/mpu_spi/mpu9x50_params.c b/src/platforms/qurt/fc_addon/mpu_spi/mpu9x50_params.c new file mode 100644 index 0000000000..4352c10185 --- /dev/null +++ b/src/platforms/qurt/fc_addon/mpu_spi/mpu9x50_params.c @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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 mpu9x50_params.c + * + * Parameters defined by the mpu9x50 driver + */ + +#include +#include + +/** + * Low pass filter frequency for Gyro + * + * @value 0 MPU9X50_GYRO_LPF_250HZ + * @value 1 MPU9X50_GYRO_LPF_184HZ + * @value 2 MPU9X50_GYRO_LPF_92HZ + * @value 3 MPU9X50_GYRO_LPF_41HZ + * @value 4 MPU9X50_GYRO_LPF_20HZ + * @value 5 MPU9X50_GYRO_LPF_10HZ + * @value 6 MPU9X50_GYRO_LPF_5HZ + * @value 7 MPU9X50_GYRO_LPF_3600HZ_NOLPF + * + * @group MPU9x50 Configuration + */ +PARAM_DEFINE_INT32(MPU_GYRO_LPF_ENM, 4); + +/** + * Low pass filter frequency for Accelerometer + * + * + * @value 0 MPU9X50_ACC_LPF_460HZ + * @value 1 MPU9X50_ACC_LPF_184HZ + * @value 2 MPU9X50_ACC_LPF_92HZ + * @value 3 MPU9X50_ACC_LPF_41HZ + * @value 4 MPU9X50_ACC_LPF_20HZ + * @value 5 MPU9X50_ACC_LPF_10HZ + * @value 6 MPU9X50_ACC_LPF_5HZ + * @value 7 MPU9X50_ACC_LPF_460HZ_NOLPF + * + * @group MPU9x50 Configuration + */ +PARAM_DEFINE_INT32(MPU_ACC_LPF_ENM, 4); + +/** + * Sample rate in Hz + * + * @value 0 MPU9x50_SAMPLE_RATE_100HZ + * @value 1 MPU9x50_SAMPLE_RATE_200HZ + * @value 2 MPU9x50_SAMPLE_RATE_500HZ + * @value 3 MPU9x50_SAMPLE_RATE_1000HZ + * + * @group MPU9x50 Configuration + */ +PARAM_DEFINE_INT32(MPU_SAMPLE_R_ENM, 2); diff --git a/src/platforms/qurt/fc_addon/rc_receiver/CMakeLists.txt b/src/platforms/qurt/fc_addon/rc_receiver/CMakeLists.txt new file mode 100644 index 0000000000..3b7399f492 --- /dev/null +++ b/src/platforms/qurt/fc_addon/rc_receiver/CMakeLists.txt @@ -0,0 +1,71 @@ +############################################################################ +# +# Copyright (c) 2016 Mark Charlebois. 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 AtlFlight 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. +# +############################################################################ + +set(TOOLS_ERROR_MSG + "The FC_ADDON must be installed and the environment variable FC_ADDON must be set" + "(e.g. export FC_ADDON=${HOME}/Qualcomm/addon)") + +if ("$ENV{FC_ADDON}" STREQUAL "") + message(FATAL_ERROR ${TOOLS_ERROR_MSG}) +else() + set(FC_ADDON $ENV{FC_ADDON}) +endif() + +include_directories( + ${FC_ADDON}/flight_controller/hexagon/inc + ) + +add_library(librc_receiver SHARED IMPORTED GLOBAL) +set_target_properties(librc_receiver PROPERTIES IMPORTED_LOCATION ${FC_ADDON}/flight_controller/hexagon/libs/librc_receiver.so) + +px4_add_module( + MODULE platforms__qurt__fc_addon__rc_receiver + MAIN rc_receiver + STACK_MAIN 1200 + COMPILE_FLAGS + -Os -Weffc++ + SRCS + rc_receiver_main.cpp + rc_receiver_params.c + + DEPENDS + platforms__common + ) + +set(module_external_libraries + ${module_external_libraries} + librc_receiver + CACHE INTERNAL "module_external_libraries" + ) + +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/qurt/fc_addon/rc_receiver/rc_receiver_main.cpp b/src/platforms/qurt/fc_addon/rc_receiver/rc_receiver_main.cpp new file mode 100644 index 0000000000..8f3e194ec9 --- /dev/null +++ b/src/platforms/qurt/fc_addon/rc_receiver/rc_receiver_main.cpp @@ -0,0 +1,347 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include "rc_receiver_api.h" +#ifdef __cplusplus +} +#endif + +/** driver 'main' command */ +extern "C" { __EXPORT int rc_receiver_main(int argc, char *argv[]); } + +#define MAX_LEN_DEV_PATH 32 + +namespace rc_receiver +{ + +/** Threshold value to detect rc receiver signal lost in millisecond */ +#define SIGNAL_LOST_THRESHOLD_MS 500 + +/** serial device path that rc receiver is connected to */ +static char _device[MAX_LEN_DEV_PATH]; + +/** flag indicating if rc_receiver module is running */ +static bool _is_running = false; + +/** flag indicating if task thread should exit */ +static bool _task_should_exit = false; + +/** handle to the task main thread */ +static px4_task_t _task_handle = -1; + +/** RC receiver type */ +static enum RC_RECEIVER_TYPES _rc_receiver_type; + +/** RC input channel value array accomondating up to RC_INPUT_MAX_CHANNELS */ +static uint16_t rc_inputs[input_rc_s::RC_INPUT_MAX_CHANNELS]; + +/** rc_input uorb topic publication handle */ +static orb_advert_t _input_rc_pub = nullptr; + +/** rc_input uorb topic data */ +static struct input_rc_s _rc_in; + +/**< parameter update subscription */ +static int _params_sub; + +struct { + param_t rc_receiver_type; +} _params_handles; /**< parameter handles */ + +/** Print out the usage information */ +static void usage(); + +/** mpu9x50 start measurement */ +static void start(); + +/** mpu9x50 stop measurement */ +static void stop(); + +/** task main trampoline function */ +static void task_main_trampoline(int argc, char *argv[]); + +/** mpu9x50 measurement thread primary entry point */ +static void task_main(int argc, char *argv[]); + +/** update all parameters */ +static void parameters_update(); + +/** poll parameter update */ +static void parameter_update_poll(); + +void parameters_update() +{ + PX4_DEBUG("rc_receiver_parameters_update"); + float v; + + // accel params + if (param_get(_params_handles.rc_receiver_type, &v) == 0) { + _rc_receiver_type = (enum RC_RECEIVER_TYPES)v; + PX4_DEBUG("rc_receiver_parameters_update rc_receiver_type %f", v); + } +} + +void parameters_init() +{ + _params_handles.rc_receiver_type = param_find("RC_RECEIVER_TYPE"); + + parameters_update(); +} + +void parameter_update_poll() +{ + bool updated; + + /* Check if parameters have changed */ + orb_check(_params_sub, &updated); + + if (updated) { + struct parameter_update_s param_update; + orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update); + parameters_update(); + } +} + +void start() +{ + ASSERT(_task_handle == -1); + + /* start the task */ + _task_handle = px4_task_spawn_cmd("rc_receiver_main", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX, + 1500, + (px4_main_t)&task_main_trampoline, + nullptr); + + if (_task_handle < 0) { + warn("task start failed"); + return; + } + + _is_running = true; +} + +void stop() +{ + // TODO - set thread exit signal to terminate the task main thread + + _is_running = false; + _task_handle = -1; +} + +void task_main_trampoline(int argc, char *argv[]) +{ + PX4_WARN("task_main_trampoline"); + task_main(argc, argv); +} + +void task_main(int argc, char *argv[]) +{ + PX4_WARN("enter rc_receiver task_main"); + uint32_t fd; + + // clear the rc_input report for topic advertise + memset(&_rc_in, 0, sizeof(struct input_rc_s)); + + _input_rc_pub = orb_advertise(ORB_ID(input_rc), &_rc_in); + + if (_input_rc_pub == nullptr) { + PX4_WARN("failed to advertise input_rc uorb topic. Quit!"); + return ; + } + + // subscribe to parameter_update topic + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + + // Open the RC receiver device on the specified serial port + fd = rc_receiver_open(_rc_receiver_type, _device); + + if (fd <= 0) { + PX4_WARN("failed to open rc receiver type %d dev %s. Quit!", + _rc_receiver_type, _device); + return ; + } + + // Continuously receive RC packet from serial device, until task is signaled + // to exit + uint32_t num_channels; + uint64_t ts = hrt_absolute_time(); + int ret; + int counter = 0; + + _rc_in.timestamp_last_signal = ts; + + while (!_task_should_exit) { + // poll parameter update + parameter_update_poll(); + + // read RC packet from serial device in blocking mode. + num_channels = input_rc_s::RC_INPUT_MAX_CHANNELS; + + ret = rc_receiver_get_packet(fd, rc_inputs, &num_channels); + ts = hrt_absolute_time(); + + _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_QURT; + + if (ret < 0) { + // enum RC_RECEIVER_ERRORS error_code = rc_receiver_get_last_error(fd); + // PX4_WARN("RC packet receiving timed out. error code %d", error_code); + + uint64_t time_diff_us = ts - _rc_in.timestamp_last_signal; + + if (time_diff_us > SIGNAL_LOST_THRESHOLD_MS * 1000) { + _rc_in.rc_lost = true; + + if (++counter == 500) { + PX4_WARN("RC signal lost for %u ms", time_diff_us / 1000); + counter = 0; + } + + } else { + continue; + } + } + + // populate the input_rc_s structure + if (ret == 0) { + _rc_in.timestamp_publication = ts; + _rc_in.timestamp_last_signal = _rc_in.timestamp_publication; + _rc_in.channel_count = num_channels; + + // TODO - need to add support for RSSI, failsafe mode + _rc_in.rssi = RC_INPUT_RSSI_MAX; + _rc_in.rc_failsafe = false; + _rc_in.rc_lost = false; + _rc_in.rc_lost_frame_count = 0; + _rc_in.rc_total_frame_count = 1; + } + + for (uint32_t i = 0; i < num_channels; i++) { + // Scale the Spektrum DSM value to ppm encoding. This is for the + // consistency with PX4 which internally translates DSM to PPM. + // See modules/px4iofirmware/dsm.c for details + // NOTE: rc_receiver spektrum driver outputs the data in 10bit DSM + // format, so we need to double the channel value before the scaling + _rc_in.values[i] = ((((int)(rc_inputs[i] * 2) - 1024) * 1000) / 1700) + 1500; + } + + orb_publish(ORB_ID(input_rc), _input_rc_pub, &_rc_in); + } + + rc_receiver_close(fd); +} + +void usage() +{ + PX4_WARN("missing command: try 'start', 'stop', 'status'"); + PX4_WARN("options:"); + PX4_WARN(" -D device"); +} + +} // namespace rc_receiver + +int rc_receiver_main(int argc, char *argv[]) +{ + const char *device = NULL; + int ch; + int myoptind = 1; + const char *myoptarg = NULL; + + /* jump over start/off/etc and look at options first */ + while ((ch = px4_getopt(argc, argv, "D:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'D': + device = myoptarg; + break; + + default: + rc_receiver::usage(); + return 1; + } + } + + // Check on required arguments + if (device == NULL || strlen(device) == 0) { + rc_receiver::usage(); + return 1; + } + + memset(rc_receiver::_device, 0, MAX_LEN_DEV_PATH); + strncpy(rc_receiver::_device, device, strlen(device)); + + const char *verb = argv[myoptind]; + + /* + * Start/load the driver. + */ + if (!strcmp(verb, "start")) { + if (rc_receiver::_is_running) { + PX4_WARN("rc_receiver already running"); + return 1; + } + + rc_receiver::start(); + + } else if (!strcmp(verb, "stop")) { + if (rc_receiver::_is_running) { + PX4_WARN("rc_receiver is not running"); + return 1; + } + + rc_receiver::stop(); + + } else if (!strcmp(verb, "status")) { + PX4_WARN("rc_receiver is %s", rc_receiver::_is_running ? "running" : "stopped"); + return 0; + + } else { + rc_receiver::usage(); + return 1; + } + + return 0; +} diff --git a/src/platforms/qurt/fc_addon/rc_receiver/rc_receiver_params.c b/src/platforms/qurt/fc_addon/rc_receiver/rc_receiver_params.c new file mode 100644 index 0000000000..d7a43550e0 --- /dev/null +++ b/src/platforms/qurt/fc_addon/rc_receiver/rc_receiver_params.c @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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 rc_receiver_params.c + * + * Parameters defined by the rc_receiver driver + */ + +#include +#include + +/** + * RC receiver type + * + * Acceptable values: + * + * - RC_RECEIVER_SPEKTRUM = 1, + * - RC_RECEIVER_LEMONRX = 2, + * + * @group RC Receiver Configuration + */ +PARAM_DEFINE_INT32(RC_RECEIVER_TYPE, 1); diff --git a/src/platforms/qurt/fc_addon/uart_esc/CMakeLists.txt b/src/platforms/qurt/fc_addon/uart_esc/CMakeLists.txt new file mode 100644 index 0000000000..0bc92f856d --- /dev/null +++ b/src/platforms/qurt/fc_addon/uart_esc/CMakeLists.txt @@ -0,0 +1,71 @@ +############################################################################ +# +# Copyright (C) 2015 Mark Charlebois. 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. +# +############################################################################ + +set(TOOLS_ERROR_MSG + "The FC_ADDON must be installed and the environment variable FC_ADDON must be set" + "(e.g. export FC_ADDON=${HOME}/Qualcomm/addon)") + +if ("$ENV{FC_ADDON}" STREQUAL "") + message(FATAL_ERROR ${TOOLS_ERROR_MSG}) +else() + set(FC_ADDON $ENV{FC_ADDON}) +endif() + +include_directories( + ${FC_ADDON}/flight_controller/hexagon/inc + ) + +add_library(libuart_esc SHARED IMPORTED GLOBAL) +set_target_properties(libuart_esc PROPERTIES IMPORTED_LOCATION ${FC_ADDON}/flight_controller/hexagon/libs/libuart_esc.so) + +px4_add_module( + MODULE platforms__qurt__fc_addon__uart_esc + MAIN uart_esc + STACK_MAIN 1200 + COMPILE_FLAGS + -Os -Weffc++ + SRCS + uart_esc_main.cpp + uart_esc_params.c + + DEPENDS + platforms__common + ) + +set(module_external_libraries + ${module_external_libraries} + libuart_esc + CACHE INTERNAL "module_external_libraries" + ) + +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/qurt/fc_addon/uart_esc/uart_esc_main.cpp b/src/platforms/qurt/fc_addon/uart_esc/uart_esc_main.cpp new file mode 100644 index 0000000000..8890c6d1e9 --- /dev/null +++ b/src/platforms/qurt/fc_addon/uart_esc/uart_esc_main.cpp @@ -0,0 +1,523 @@ +/**************************************************************************** +* +* Copyright (c) 2015 Mark Charlebois. All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in +* the documentation and/or other materials provided with the +* distribution. +* 3. Neither the name PX4 nor the names of its contributors may be +* used to endorse or promote products derived from this software +* without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif +#include +#ifdef __cplusplus +} +#endif + +/** driver 'main' command */ +extern "C" { __EXPORT int uart_esc_main(int argc, char *argv[]); } + +#define MAX_LEN_DEV_PATH 32 + +namespace uart_esc +{ +#define UART_ESC_MAX_MOTORS 4 + +volatile bool _task_should_exit = false; // flag indicating if uart_esc task should exit +static char _device[MAX_LEN_DEV_PATH]; +static bool _is_running = false; // flag indicating if uart_esc app is running +static px4_task_t _task_handle = -1; // handle to the task main thread +UartEsc *esc; // esc instance +void uart_esc_rotate_motors(int16_t *motor_rpm, int num_rotors); // motor re-mapping + +// subscriptions +int _controls_sub; +int _armed_sub; +int _param_sub; + +// filenames +// /dev/fs/ is mapped to /usr/share/data/adsp/ +static const char *MIXER_FILENAME = "/dev/fs/mixer_config.mix"; + + +// publications +orb_advert_t _outputs_pub; + +// topic structures +actuator_controls_s _controls; +actuator_armed_s _armed; +parameter_update_s _param_update; +actuator_outputs_s _outputs; + +/** Print out the usage information */ +static void usage(); + +/** uart_esc start */ +static void start(const char *device) __attribute__((unused)); + +/** uart_esc stop */ +static void stop(); + +/** task main trampoline function */ +static void task_main_trampoline(int argc, char *argv[]); + +/** uart_esc thread primary entry point */ +static void task_main(int argc, char *argv[]); + +/** mixer initialization */ +static MultirotorMixer *mixer; +static int initialize_mixer(const char *mixer_filename); +static int mixer_control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input); + +static void parameters_init(); +static void parameters_update(); + +struct { + int model; + int baudrate; + int px4_motor_mapping[UART_ESC_MAX_MOTORS]; +} _parameters; + +struct { + param_t model; + param_t baudrate; + param_t px4_motor_mapping[UART_ESC_MAX_MOTORS]; +} _parameter_handles; + +void parameters_init() +{ + _parameter_handles.model = param_find("UART_ESC_MODEL"); + _parameter_handles.baudrate = param_find("UART_ESC_BAUD"); + + /* PX4 motor mapping parameters */ + for (unsigned int i = 0; i < UART_ESC_MAX_MOTORS; i++) { + char nbuf[20]; + + /* min values */ + sprintf(nbuf, "UART_ESC_MOTOR%d", i + 1); + _parameter_handles.px4_motor_mapping[i] = param_find(nbuf); + } + + parameters_update(); +} + +void parameters_update() +{ + PX4_WARN("uart_esc_main parameters_update"); + int v_int; + + if (param_get(_parameter_handles.model, &v_int) == 0) { + _parameters.model = v_int; + PX4_WARN("UART_ESC_MODEL %d", _parameters.model); + } + + if (param_get(_parameter_handles.baudrate, &v_int) == 0) { + _parameters.baudrate = v_int; + PX4_WARN("UART_ESC_BAUD %d", _parameters.baudrate); + } + + for (unsigned int i = 0; i < UART_ESC_MAX_MOTORS; i++) { + if (param_get(_parameter_handles.px4_motor_mapping[i], &v_int) == 0) { + _parameters.px4_motor_mapping[i] = v_int; + PX4_WARN("UART_ESC_MOTOR%d %d", i + 1, _parameters.px4_motor_mapping[i]); + } + } +} + +int mixer_control_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &input) +{ + const actuator_controls_s *controls = (actuator_controls_s *)handle; + + input = controls[control_group].control[control_index]; + + /* motor spinup phase - lock throttle to zero * + if (_pwm_limit.state == PWM_LIMIT_STATE_RAMP) { + if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && + control_index == actuator_controls_s::INDEX_THROTTLE) { + * limit the throttle output to zero during motor spinup, + * as the motors cannot follow any demand yet + * + input = 0.0f; + } + } + */ + return 0; +} + + +int initialize_mixer(const char *mixer_filename) +{ + mixer = nullptr; + + int mixer_initialized = -1; + + char buf[2048]; + unsigned int buflen = sizeof(buf); + + PX4_INFO("Initializing mixer from config file in %s", mixer_filename); + + int fd_load = open(mixer_filename, O_RDONLY); + + if (fd_load != -1) { + int nRead = read(fd_load, buf, buflen); + close(fd_load); + + if (nRead > 0) { + mixer = MultirotorMixer::from_text(mixer_control_callback, (uintptr_t)&_controls, buf, buflen); + + if (mixer != nullptr) { + PX4_INFO("Successfully initialized mixer from config file"); + mixer_initialized = 0; + + } else { + PX4_WARN("Unable to parse from mixer config file"); + } + + } else { + PX4_WARN("Unable to read from mixer config file"); + } + + } else { + PX4_WARN("Unable to open mixer config file"); + } + + // mixer file loading failed, fall back to default mixer configuration for + // QUAD_X airframe + if (mixer_initialized < 0) { + float roll_scale = 1; + float pitch_scale = 1; + float yaw_scale = 1; + float deadband = 0; + + mixer = new MultirotorMixer(mixer_control_callback, (uintptr_t)&_controls, + MultirotorGeometry::QUAD_X, + roll_scale, pitch_scale, yaw_scale, deadband); + + if (mixer == nullptr) { + PX4_ERR("mixer initialization failed"); + mixer_initialized = -1; + return mixer_initialized; + } + + PX4_WARN("mixer config file not found, successfully initialized default quad x mixer"); + mixer_initialized = 0; + } + + return mixer_initialized; +} + +/** +* Rotate the motor rpm values based on the motor mappings configuration stored +* in motor_mapping +*/ +void uart_esc_rotate_motors(int16_t *motor_rpm, int num_rotors) +{ + ASSERT(num_rotors <= UART_ESC_MAX_MOTORS); + int i; + int16_t motor_rpm_copy[UART_ESC_MAX_MOTORS]; + + for (i = 0; i < num_rotors; i++) { + motor_rpm_copy[i] = motor_rpm[i]; + } + + for (i = 0; i < num_rotors; i++) { + motor_rpm[_parameters.px4_motor_mapping[i] - 1] = motor_rpm_copy[i]; + } +} + +void task_main(int argc, char *argv[]) +{ + PX4_INFO("enter uart_esc task_main"); + + _outputs_pub = nullptr; + + parameters_init(); + + esc = UartEsc::get_instance(); + + if (esc == NULL) { + PX4_ERR("failed to new UartEsc instance"); + + } else if (esc->initialize((enum esc_model_t)_parameters.model, + _device, _parameters.baudrate) < 0) { + PX4_ERR("failed to initialize UartEsc"); + + } else { + // Subscribe for orb topics + _controls_sub = orb_subscribe(ORB_ID(actuator_controls_0)); // single group for now + _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + _param_sub = orb_subscribe(ORB_ID(parameter_update)); + + // initialize publication structures + memset(&_outputs, 0, sizeof(_outputs)); + + // set up poll topic and limit poll interval + px4_pollfd_struct_t fds[1]; + fds[0].fd = _controls_sub; + fds[0].events = POLLIN; + //orb_set_interval(_controls_sub, 10); // max actuator update period, ms + + // set up mixer + if (initialize_mixer(MIXER_FILENAME) < 0) { + PX4_ERR("Mixer initialization failed."); + _task_should_exit = true; + } + + // Main loop + while (!_task_should_exit) { + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + + /* timed out - periodic check for _task_should_exit */ + if (pret == 0) { + continue; + } + + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + PX4_WARN("poll error %d, %d", pret, errno); + /* sleep a bit before next try */ + usleep(100000); + continue; + } + + // Handle new actuator controls data + if (fds[0].revents & POLLIN) { + + // Grab new controls data + orb_copy(ORB_ID(actuator_controls_0), _controls_sub, &_controls); + // Mix to the outputs + _outputs.timestamp = hrt_absolute_time(); + int16_t motor_rpms[UART_ESC_MAX_MOTORS]; + + if (_armed.armed) { + _outputs.noutputs = mixer->mix(&_outputs.output[0], + actuator_controls_s::NUM_ACTUATOR_CONTROLS, + NULL); + + // Make sure we support only up to UART_ESC_MAX_MOTORS motors + if (_outputs.noutputs > UART_ESC_MAX_MOTORS) { + PX4_ERR("Unsupported motors %d, up to %d motors supported", + _outputs.noutputs, UART_ESC_MAX_MOTORS); + continue; + } + + // Send outputs to the ESCs + for (unsigned outIdx = 0; outIdx < _outputs.noutputs; outIdx++) { + // map -1.0 - 1.0 outputs to RPMs + motor_rpms[outIdx] = (int16_t)(((_outputs.output[outIdx] + 1.0) / 2.0) * + (esc->max_rpm() - esc->min_rpm()) + esc->min_rpm()); + } + + uart_esc_rotate_motors(motor_rpms, _outputs.noutputs); + + } else { + _outputs.noutputs = UART_ESC_MAX_MOTORS; + + for (unsigned outIdx = 0; outIdx < _outputs.noutputs; outIdx++) { + motor_rpms[outIdx] = 0; + _outputs.output[outIdx] = -1.0; + } + } + + esc->send_rpms(&motor_rpms[0], _outputs.noutputs); + + // TODO-JYW: TESTING-TESTING + // MAINTAIN FOR REFERENCE, COMMENT OUT FOR RELEASE BUILDS +// static int count=0; +// count++; +// if (!(count % 100)) { +// PX4_DEBUG(" "); +// PX4_DEBUG("Time t: %13lu, Armed: %d ",(unsigned long)_outputs.timestamp,_armed.armed); +// PX4_DEBUG("Act Controls: 0: %+8.4f, 1: %+8.4f, 2: %+8.4f, 3: %+8.4f ",_controls.control[0],_controls.control[1],_controls.control[2],_controls.control[3]); +// PX4_DEBUG("Act Outputs : 0: %+8.4f, 1: %+8.4f, 2: %+8.4f, 3: %+8.4f ",_outputs.output[0],_outputs.output[1],_outputs.output[2],_outputs.output[3]); +// } + // TODO-JYW: TESTING-TESTING + + + /* Publish mixed control outputs */ + if (_outputs_pub != nullptr) { + orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs); + + } else { + _outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs); + } + } + + // Check for updates in other subscripions + bool updated = false; + orb_check(_armed_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); + PX4_DEBUG("arming status updated, _armed.armed: %d", _armed.armed); + } + + orb_check(_param_sub, &updated); + + if (updated) { + // Even though we are only interested in the update status of the parameters, copy + // the subscription to clear the update status. + orb_copy(ORB_ID(parameter_update), _param_sub, &_param_update); + parameters_update(); + } + } + } + + PX4_WARN("closing uart_esc"); + + delete esc; +} + +/** uart_esc main entrance */ +void task_main_trampoline(int argc, char *argv[]) +{ + PX4_WARN("task_main_trampoline"); + task_main(argc, argv); +} + +void start() +{ + ASSERT(_task_handle == -1); + + /* start the task */ + _task_handle = px4_task_spawn_cmd("uart_esc_main", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX, + 1500, + (px4_main_t)&task_main_trampoline, + nullptr); + + if (_task_handle < 0) { + warn("task start failed"); + return; + } + + _is_running = true; +} + +void stop() +{ + // TODO - set thread exit signal to terminate the task main thread + + _is_running = false; + _task_handle = -1; +} + +void usage() +{ + PX4_WARN("missing command: try 'start', 'stop', 'status'"); + PX4_WARN("options:"); + PX4_WARN(" -D device"); +} + +}; // namespace uart_esc + +int uart_esc_main(int argc, char *argv[]) +{ + const char *device = NULL; + int ch; + int myoptind = 1; + const char *myoptarg = NULL; + + while ((ch = px4_getopt(argc, argv, "D:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'D': + device = myoptarg; + break; + + default: + uart_esc::usage(); + return 1; + } + } + + // Check on required arguments + if (device == NULL || strlen(device) == 0) { + uart_esc::usage(); + return 1; + } + + memset(uart_esc::_device, 0, MAX_LEN_DEV_PATH); + strncpy(uart_esc::_device, device, strlen(device)); + + const char *verb = argv[myoptind]; + + /* + * Start/load the driver. + */ + if (!strcmp(verb, "start")) { + if (uart_esc::_is_running) { + PX4_WARN("uart_esc already running"); + return 1; + } + + uart_esc::start(); + } + + else if (!strcmp(verb, "stop")) { + if (uart_esc::_is_running) { + PX4_WARN("uart_esc is not running"); + return 1; + } + + uart_esc::stop(); + } + + else if (!strcmp(verb, "status")) { + PX4_WARN("uart_esc is %s", uart_esc::_is_running ? "running" : "stopped"); + return 0; + + } else { + uart_esc::usage(); + return 1; + } + + return 0; +} diff --git a/src/platforms/qurt/fc_addon/uart_esc/uart_esc_params.c b/src/platforms/qurt/fc_addon/uart_esc/uart_esc_params.c new file mode 100644 index 0000000000..ca1ca273b7 --- /dev/null +++ b/src/platforms/qurt/fc_addon/uart_esc/uart_esc_params.c @@ -0,0 +1,139 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Ramakrishna Kintada. 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 uart_esc_params.c + * + * Parameters defined for the uart esc driver + */ +#include +#include + +/** + * ESC model + * + * See esc_model_t enum definition in uart_esc_dev.h for all supported ESC + * model enum values. + * + * @value 0 ESC_200QX + * @value 1 ESC_350QX + * @value 2 ESC_210QC + * + * @group Snapdragon UART ESC + */ +PARAM_DEFINE_INT32(UART_ESC_MODEL, 2); + +/** + * ESC UART baud rate + * + * Default rate is 250Kbps, whic is used in off-the-shelf QRP ESC products. + * @group Snapdragon UART ESC + */ +PARAM_DEFINE_INT32(UART_ESC_BAUD, 250000); + +/** + * The PX4 default motor mappings are + * 1 4 + * [front] + * 3 2 + * + * The following paramters define the motor mappings in reference to the + * PX4 motor mapping convention. + */ +/** + * Default PX4 motor mappings + * 1 4 + * [front] + * 3 2 + */ +// PARAM_DEFINE_INT32(UART_ESC_MOTOR1, 1); +// PARAM_DEFINE_INT32(UART_ESC_MOTOR2, 2); +// PARAM_DEFINE_INT32(UART_ESC_MOTOR3, 3); +// PARAM_DEFINE_INT32(UART_ESC_MOTOR4, 4); + +/** + * Motor mappings for 350QX + * 4 3 + * [front] + * 1 2 + */ +// PARAM_DEFINE_INT32(UART_ESC_MOTOR1, 4); +// PARAM_DEFINE_INT32(UART_ESC_MOTOR2, 2); +// PARAM_DEFINE_INT32(UART_ESC_MOTOR3, 1); +// PARAM_DEFINE_INT32(UART_ESC_MOTOR4, 3); + +/** + * Motor mappings for 200QX + * 2 3 + * [front] + * 1 4 + */ +// PARAM_DEFINE_INT32(UART_ESC_MOTOR1, 2); +// PARAM_DEFINE_INT32(UART_ESC_MOTOR2, 4); +// PARAM_DEFINE_INT32(UART_ESC_MOTOR3, 1); +// PARAM_DEFINE_INT32(UART_ESC_MOTOR4, 3); + +/** + * Motor mappings for 210QC + * 4 3 + * [front] + * 1 2 + */ + +/** + * Motor 1 Mapping + * + * @group Snapdragon UART ESC + */ +PARAM_DEFINE_INT32(UART_ESC_MOTOR1, 4); + +/** + * Motor 2 Mapping + * + * @group Snapdragon UART ESC + */ +PARAM_DEFINE_INT32(UART_ESC_MOTOR2, 2); + +/** + * Motor 3 Mapping + * + * @group Snapdragon UART ESC + */ +PARAM_DEFINE_INT32(UART_ESC_MOTOR3, 1); + +/** + * Motor 4 Mapping + * + * @group Snapdragon UART ESC + */ +PARAM_DEFINE_INT32(UART_ESC_MOTOR4, 3); diff --git a/src/platforms/qurt/px4_layer/CMakeLists.txt b/src/platforms/qurt/px4_layer/CMakeLists.txt index 25ada03442..38dbd8a46f 100644 --- a/src/platforms/qurt/px4_layer/CMakeLists.txt +++ b/src/platforms/qurt/px4_layer/CMakeLists.txt @@ -30,8 +30,10 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ +set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon") +include(hexagon_sdk) -include_directories(${HEXAGON_SDK_ROOT}/lib/common/qurt/ADSPv5MP/include) +include_directories(${HEXAGON_8074_INCLUDES}) set(QURT_LAYER_SRCS px4_qurt_impl.cpp @@ -55,7 +57,6 @@ px4_add_module( MODULE platforms__qurt__px4_layer COMPILE_FLAGS -Os - -Wno-packed SRCS ${QURT_LAYER_SRCS} ${CONFIG_SRC} diff --git a/src/platforms/qurt/px4_layer/main.cpp b/src/platforms/qurt/px4_layer/main.cpp index 6b424d8239..39006cc1f1 100644 --- a/src/platforms/qurt/px4_layer/main.cpp +++ b/src/platforms/qurt/px4_layer/main.cpp @@ -82,7 +82,7 @@ static void run_cmd(map &apps, const vector &appargs while (i < appargs.size() && appargs[i].c_str()[0] != '\0') { arg[i] = (char *)appargs[i].c_str(); - PX4_WARN(" arg%d = '%s'\n", i, arg[i]); + PX4_DEBUG(" arg%d = '%s'\n", i, arg[i]); ++i; } @@ -128,10 +128,10 @@ static void process_commands(map &apps, const char *cmds) // If we have a command to run if (appargs.size() > 0) { - PX4_WARN("Processing command: %s", appargs[0].c_str()); + PX4_DEBUG("Processing command: %s", appargs[0].c_str()); for (int ai = 1; ai < (int)appargs.size(); ai++) { - PX4_WARN(" > arg: %s", appargs[ai].c_str()); + PX4_DEBUG(" > arg: %s", appargs[ai].c_str()); } run_cmd(apps, appargs); @@ -211,12 +211,12 @@ const char *get_commands() int dspal_entry(int argc, char *argv[]) { - PX4_INFO("In main\n"); + PX4_INFO("In dspal_entry"); map apps; init_app_map(apps); DriverFramework::Framework::initialize(); px4::init_once(); - px4::init(argc, (char **)argv, "mainapp"); + px4::init(argc, (char **)argv, "px4"); process_commands(apps, get_commands()); sleep(1); // give time for all commands to execute before starting external function @@ -233,7 +233,7 @@ int dspal_entry(int argc, char *argv[]) static void usage() { - PX4_WARN("Usage: dspal {start |stop}"); + PX4_INFO("Usage: dspal {start |stop}"); } diff --git a/src/platforms/qurt/px4_layer/module.mk b/src/platforms/qurt/px4_layer/module.mk deleted file mode 100644 index 23d10df6b2..0000000000 --- a/src/platforms/qurt/px4_layer/module.mk +++ /dev/null @@ -1,51 +0,0 @@ -############################################################################ -# -# Copyright (c) 2014 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# NuttX / uORB adapter library -# - -MODULE_NAME = dspal - -SRCDIR=$(dir $(MODULE_MK)) - -SRCS = \ - px4_qurt_impl.cpp \ - px4_qurt_tasks.cpp \ - lib_crc32.c \ - drv_hrt.c \ - qurt_stubs.c \ - main.cpp \ - shmem_qurt.c - -MAXOPTIMIZATION = -Os diff --git a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp index b905ac439c..e956b9da94 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp @@ -99,16 +99,13 @@ void init_once(void) { // Required for QuRT //_posix_init(); - PX4_WARN("Before calling work_queue_init"); // _shell_task_id = pthread_self(); // PX4_INFO("Shell id is %lu", _shell_task_id); work_queues_init(); - PX4_WARN("Before calling hrt_init"); hrt_work_queue_init(); hrt_init(); - PX4_WARN("after calling hrt_init"); /* Shared memory param sync*/ init_params(); diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index 1d53ed61c6..6cf342076a 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -64,6 +64,7 @@ #include #include +#include #define MAX_CMD_LEN 100 @@ -196,11 +197,11 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int #endif size_t fixed_stacksize = -1; pthread_attr_getstacksize(&attr, &fixed_stacksize); - PX4_INFO("stack size: %d passed stacksize(%d)", fixed_stacksize, stack_size); + PX4_DEBUG("stack size: %d passed stacksize(%d)", fixed_stacksize, stack_size); fixed_stacksize = 8 * 1024; fixed_stacksize = (fixed_stacksize < (size_t)stack_size) ? (size_t)stack_size : fixed_stacksize; - PX4_INFO("setting the thread[%s] stack size to[%d]", name, fixed_stacksize); + PX4_DEBUG("setting the thread[%s] stack size to[%d]", name, fixed_stacksize); pthread_attr_setstacksize(&attr, fixed_stacksize); PX4_DEBUG("stack address after pthread_attr_setstacksize: 0x%X", attr.stackaddr); @@ -325,8 +326,6 @@ void px4_show_tasks() } -__BEGIN_DECLS - unsigned long px4_getpid() { pthread_t pid = pthread_self(); @@ -341,13 +340,15 @@ unsigned long px4_getpid() } } +#ifndef __PX4_QURT + // XXX This doesn't seem to be working on QURT PX4_ERR("px4_getpid() called from unknown thread context!"); +#endif return ~0; } -const char *getprogname(); -const char *getprogname() +const char *px4_get_taskname() { pthread_t pid = pthread_self(); @@ -404,4 +405,3 @@ int px4_prctl(int option, const char *arg2, unsigned pid) return rv; } -__END_DECLS diff --git a/src/platforms/qurt/px4_layer/shmem_qurt.c b/src/platforms/qurt/px4_layer/shmem_qurt.c index ca29ba77db..780a4cc7cf 100644 --- a/src/platforms/qurt/px4_layer/shmem_qurt.c +++ b/src/platforms/qurt/px4_layer/shmem_qurt.c @@ -1,4 +1,3 @@ - /**************************************************************************** * * Copyright (c) 2015 Vijay Venkatraman. All rights reserved. @@ -48,13 +47,14 @@ #include //#define SHMEM_DEBUG +//#define PARAM_LOCK_DEBUG int mem_fd; unsigned char *map_base, *virt_addr; struct shmem_info *shmem_info_p; static void *map_memory(off_t target); int get_shmem_lock(const char *caller_file_name, int caller_line_number); -void release_shmem_lock(void); +void release_shmem_lock(const char *caller_file_name, int caller_line_number); void init_shared_memory(void); void copy_params_to_shmem(struct param_info_s *); void update_to_shmem(param_t param, union param_value_u value); @@ -76,9 +76,9 @@ static unsigned log2_for_int(unsigned v) } struct param_wbuf_s { - param_t param; - union param_value_u val; - bool unsaved; + param_t param; + union param_value_u val; + bool unsaved; }; extern struct param_wbuf_s *param_find_changed(param_t param); @@ -91,56 +91,77 @@ static void *map_memory(off_t target) int get_shmem_lock(const char *caller_file_name, int caller_line_number) { - // TODO: don't do this for now - return 0; - unsigned char *lock = (unsigned char *)(MAP_ADDRESS + LOCK_OFFSET); unsigned int i = 0; +#ifdef PARAM_LOCK_DEBUG + PX4_INFO("lock value %d before get from %s, line: %d\n", *(unsigned int *)0xfbfc000, strrchr(caller_file_name, '/'), + caller_line_number); +#endif + while (!atomic_compare_and_set(lock, 1, 0)) { - PX4_INFO("Could not get lock, file name: %s, line number: %d.\n", - caller_file_name, caller_line_number); i++; usleep(1000); - if (i > 100) { break; } + if (i > 100) { + break; + } } if (i > 100) { + PX4_INFO("Could not get lock, file name: %s, line number: %d.\n", + strrchr(caller_file_name, '/'), caller_line_number); return -1; } else { - PX4_DEBUG("Lock acquired, file name: %s, line number: %d\n", caller_file_name, caller_line_number); + PX4_DEBUG("Lock acquired, file name: %s, line number: %d\n", + caller_file_name, caller_line_number); } return 0; //got the lock } -void release_shmem_lock(void) +void release_shmem_lock(const char *caller_file_name, int caller_line_number) { - // TODO: don't do this either - return; - unsigned char *lock = (unsigned char *)(MAP_ADDRESS + LOCK_OFFSET); *lock = 1; + +#ifdef PARAM_LOCK_DEBUG + PX4_INFO("release lock, file name: %s, line number: %d.\n", + strrchr(caller_file_name, '/'), caller_line_number); +#endif + return; } void init_shared_memory(void) { //PX4_INFO("Value at lock address is %d\n", *(unsigned int*)0xfbfc000); + int i; + + if (shmem_info_p) { + return; + } virt_addr = map_memory(MAP_ADDRESS); - shmem_info_p = (struct shmem_info *)virt_addr; + shmem_info_p = (struct shmem_info *) virt_addr; + + //init lock as 1 + unsigned char *lock = (unsigned char *)(MAP_ADDRESS + LOCK_OFFSET); + *lock = 1; + + for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) { + shmem_info_p->krait_changed_index[i] = 0; + } //PX4_INFO("adsp memory mapped\n"); } void copy_params_to_shmem(struct param_info_s *param_info_base) { - param_t param; + param_t param; unsigned int i; if (get_shmem_lock(__FILE__, __LINE__) != 0) { @@ -154,9 +175,13 @@ void copy_params_to_shmem(struct param_info_s *param_info_base) //{PX4_INFO("writing to offset %d\n", (unsigned char*)(shmem_info_p->adsp_params[param].name)-(unsigned char*)shmem_info_p);} struct param_wbuf_s *s = param_find_changed(param); - if (s == NULL) { shmem_info_p->params_val[param] = param_info_base[param].val; } + if (s == NULL) { + shmem_info_p->params_val[param] = param_info_base[param].val; + } - else { shmem_info_p->params_val[param] = s->val; } + else { + shmem_info_p->params_val[param] = s->val; + } #ifdef SHMEM_DEBUG @@ -175,12 +200,11 @@ void copy_params_to_shmem(struct param_info_s *param_info_base) krait_changed_index[i] = 0; } - release_shmem_lock(); + release_shmem_lock(__FILE__, __LINE__); //PX4_INFO("Released lock\n"); } - /*update value and param's change bit in shared memory*/ void update_to_shmem(param_t param, union param_value_u value) { @@ -217,43 +241,51 @@ void update_to_shmem(param_t param, union param_value_u value) #endif - release_shmem_lock(); + release_shmem_lock(__FILE__, __LINE__); } void update_index_from_shmem(void) { unsigned int i; + param_t params[MAX_SHMEM_PARAMS / 8 + 1]; if (get_shmem_lock(__FILE__, __LINE__) != 0) { PX4_ERR("Could not get shmem lock\n"); return; } - PX4_DEBUG("Updating index from shmem\n"); - for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) { // Check if any param has been changed. if (krait_changed_index[i] != shmem_info_p->krait_changed_index[i]) { // If a param has changed, we need to find out which one. // From the byte and bit that is different, we can resolve the param number. - unsigned bit = log2_for_int(krait_changed_index[i] ^ shmem_info_p->krait_changed_index[i]); + unsigned bit = log2_for_int( + krait_changed_index[i] + ^ shmem_info_p->krait_changed_index[i]); param_t param_to_get = i * 8 + bit; // Update our krait_changed_index as well. krait_changed_index[i] = shmem_info_p->krait_changed_index[i]; + params[i] = param_to_get; - // FIXME: this is a hack but it gets the param so that it gets added - // to the local list param_values in param_shmem.c. - int32_t dummy; - param_get(param_to_get, &dummy); + } else { + params[i] = 0xFFFF; } } - release_shmem_lock(); -} + release_shmem_lock(__FILE__, __LINE__); + // FIXME: this is a hack but it gets the param so that it gets added + // to the local list param_values in param_shmem.c. + for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) { + if (params[i] != 0xFFFF) { + int32_t dummy; + param_get(params[i], &dummy); + } + } +} static void update_value_from_shmem(param_t param, union param_value_u *value) { @@ -271,7 +303,7 @@ static void update_value_from_shmem(param_t param, union param_value_u *value) bit_changed = 1 << param % 8; shmem_info_p->krait_changed_index[byte_changed] &= ~bit_changed; - release_shmem_lock(); + release_shmem_lock(__FILE__, __LINE__); #ifdef SHMEM_DEBUG @@ -299,7 +331,8 @@ int update_from_shmem(param_t param, union param_value_u *value) update_from_shmem_current_time = hrt_absolute_time(); - if ((update_from_shmem_current_time - update_from_shmem_prev_time) > 1000000) { //update every 1 second + if ((update_from_shmem_current_time - update_from_shmem_prev_time) + > 1000000) { //update every 1 second update_from_shmem_prev_time = update_from_shmem_current_time; update_index_from_shmem(); } @@ -315,9 +348,9 @@ int update_from_shmem(param_t param, union param_value_u *value) //else {PX4_INFO("no change to param %s\n", param_name(param));} - PX4_DEBUG("%s %d bit on krait changed index[%d]\n", (retval) ? "cleared" : "unchanged", bit_changed, byte_changed); + PX4_DEBUG("%s %d bit on krait changed index[%d]\n", + (retval) ? "cleared" : "unchanged", bit_changed, byte_changed); return retval; } - diff --git a/src/platforms/qurt/stubs/stubs_posix.c b/src/platforms/qurt/stubs/stubs_posix.c index 16514aa72f..24b22f13f9 100644 --- a/src/platforms/qurt/stubs/stubs_posix.c +++ b/src/platforms/qurt/stubs/stubs_posix.c @@ -148,6 +148,11 @@ int pthread_mutexattr_settype(pthread_mutexattr_t *attr, int type) return -1; } +int pthread_condattr_init(pthread_condattr_t *attr) +{ + return -1; +} + int fsync(int fd) { return -1; diff --git a/src/platforms/qurt/tests/hello/module.mk b/src/platforms/qurt/tests/hello/module.mk deleted file mode 100644 index 6bed2aea67..0000000000 --- a/src/platforms/qurt/tests/hello/module.mk +++ /dev/null @@ -1,43 +0,0 @@ -############################################################################ -# -# Copyright (c) 2014 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Publisher Example Application -# - -MODULE_COMMAND = hello - -SRCS = hello_main.cpp \ - hello_start_qurt.cpp \ - hello_example.cpp - diff --git a/src/platforms/shmem.h b/src/platforms/shmem.h index 45f26684fa..bed181c267 100644 --- a/src/platforms/shmem.h +++ b/src/platforms/shmem.h @@ -33,6 +33,8 @@ #define MAX_SHMEM_PARAMS 2000 //MAP_SIZE - (LOCK_SIZE - sizeof(struct shmem_info)) +#define PARAM_BUFFER_SIZE (MAX_SHMEM_PARAMS / 8 + 1) + struct shmem_info { union param_value_u params_val[MAX_SHMEM_PARAMS]; unsigned char krait_changed_index[MAX_SHMEM_PARAMS / 8 + 1]; /*bit map of all params changed by krait*/ diff --git a/src/systemcmds/bl_update/module.mk b/src/systemcmds/bl_update/module.mk deleted file mode 100644 index e8eea045e5..0000000000 --- a/src/systemcmds/bl_update/module.mk +++ /dev/null @@ -1,43 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Bootloader updater (flashes the FMU USB bootloader software) -# - -MODULE_COMMAND = bl_update -SRCS = bl_update.c - -MODULE_STACKSIZE = 4096 - -MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/config/module.mk b/src/systemcmds/config/module.mk deleted file mode 100644 index 0a75810b0c..0000000000 --- a/src/systemcmds/config/module.mk +++ /dev/null @@ -1,44 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Build the config tool. -# - -MODULE_COMMAND = config -SRCS = config.c - -MODULE_STACKSIZE = 4096 - -MAXOPTIMIZATION = -Os - diff --git a/src/systemcmds/dumpfile/module.mk b/src/systemcmds/dumpfile/module.mk deleted file mode 100644 index 36461f4772..0000000000 --- a/src/systemcmds/dumpfile/module.mk +++ /dev/null @@ -1,41 +0,0 @@ -############################################################################ -# -# 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. -# -############################################################################ - -# -# Dump file utility -# - -MODULE_COMMAND = dumpfile -SRCS = dumpfile.c - -MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c index 86939ff686..161fe7923b 100644 --- a/src/systemcmds/esc_calib/esc_calib.c +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -60,10 +60,6 @@ #include "drivers/drv_pwm_output.h" #include -#include -#include -#include -#include static void usage(const char *reason); __EXPORT int esc_calib_main(int argc, char *argv[]); diff --git a/src/systemcmds/esc_calib/module.mk b/src/systemcmds/esc_calib/module.mk deleted file mode 100644 index ce87eb3e2c..0000000000 --- a/src/systemcmds/esc_calib/module.mk +++ /dev/null @@ -1,43 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Build the esc_calib tool. -# - -MODULE_COMMAND = esc_calib -SRCS = esc_calib.c - -MODULE_STACKSIZE = 4096 - -MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/i2c/i2c.c b/src/systemcmds/i2c/i2c.c index 1fa60a7e47..c686591bf6 100644 --- a/src/systemcmds/i2c/i2c.c +++ b/src/systemcmds/i2c/i2c.c @@ -73,7 +73,7 @@ static struct i2c_dev_s *i2c; int i2c_main(int argc, char *argv[]) { /* find the right I2C */ - i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD); + i2c = px4_i2cbus_initialize(PX4_I2C_BUS_ONBOARD); if (i2c == NULL) { errx(1, "failed to locate I2C bus"); diff --git a/src/systemcmds/i2c/module.mk b/src/systemcmds/i2c/module.mk deleted file mode 100644 index ab2357c7d0..0000000000 --- a/src/systemcmds/i2c/module.mk +++ /dev/null @@ -1,41 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Build the i2c test tool. -# - -MODULE_COMMAND = i2c -SRCS = i2c.c - -MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/topic_listener/module.mk b/src/systemcmds/motor_ramp/CMakeLists.txt similarity index 86% rename from src/systemcmds/topic_listener/module.mk rename to src/systemcmds/motor_ramp/CMakeLists.txt index 4048c6b318..21642c136a 100644 --- a/src/systemcmds/topic_listener/module.mk +++ b/src/systemcmds/motor_ramp/CMakeLists.txt @@ -30,17 +30,15 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ - -# -# Build the topic listener tool. -# - -./topic_listener.cpp : $(PX4_BASE)/Tools/generate_listener.py - $(PX4_BASE)/Tools/generate_listener.py $(PX4_BASE) > $@ - -MODULE_COMMAND = listener -SRCS = topic_listener.cpp - -MODULE_STACKSIZE = 1800 - -MAXOPTIMIZATION = -Os +px4_add_module( + MODULE systemcmds__motor_ramp + MAIN motor_ramp + STACK_MAIN 1200 + COMPILE_FLAGS + -Wno-write-strings + SRCS + motor_ramp.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/systemcmds/motor_ramp/motor_ramp.cpp b/src/systemcmds/motor_ramp/motor_ramp.cpp new file mode 100644 index 0000000000..a7b6968cf1 --- /dev/null +++ b/src/systemcmds/motor_ramp/motor_ramp.cpp @@ -0,0 +1,410 @@ +/**************************************************************************** + * + * Copyright (c) 2016 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 motor_ramp.cpp + * Application to test motor ramp up. + * + * @author Andreas Antener + * @author Roman Bapst + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "systemlib/systemlib.h" +#include "systemlib/err.h" + +enum RampState { + RAMP_INIT, + RAMP_MIN, + RAMP_RAMP, + RAMP_WAIT +}; + +enum Mode { + RAMP, + SINE, + SQUARE +}; + +static bool _thread_should_exit = false; /**< motor_ramp exit flag */ +static bool _thread_running = false; /**< motor_ramp status flag */ +static int _motor_ramp_task; /**< Handle of motor_ramp task / thread */ +static float _ramp_time; +static int _min_pwm; +static int _max_pwm; +static Mode _mode; +static char *_mode_c; + +/** + * motor_ramp management function. + */ +extern "C" __EXPORT int motor_ramp_main(int argc, char *argv[]); + +/** + * Mainloop of motor_ramp. + */ +int motor_ramp_thread_main(int argc, char *argv[]); + +bool min_pwm_valid(unsigned pwm_value); + +bool max_pwm_valid(unsigned pwm_value); + +int set_min_pwm(int fd, unsigned long max_channels, unsigned pwm_value); + +int set_out(int fd, unsigned long max_channels, float output); + +int prepare(int fd, unsigned long *max_channels); + +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +static void +usage(const char *reason) +{ + if (reason) { + PX4_ERR("%s", reason); + } + + PX4_WARN("\n\nWARNING: motors will ramp up to full speed!\n\n" + "Usage: motor_ramp