diff --git a/.gitignore b/.gitignore index 1a6e278539..14183ed177 100644 --- a/.gitignore +++ b/.gitignore @@ -55,3 +55,5 @@ src/platforms/posix-arm/px4_messages/ src/platforms/qurt/px4_messages/ ROMFS/*/*/rc.autostart rootfs/ +*.autosave +CMakeLists.txt.user diff --git a/.gitmodules b/.gitmodules index 2820e68539..178cf86771 100644 --- a/.gitmodules +++ b/.gitmodules @@ -13,12 +13,15 @@ [submodule "Tools/gencpp"] path = Tools/gencpp url = https://github.com/ros/gencpp.git -[submodule "unittests/gtest"] - path = unittests/gtest - url = https://github.com/sjwilks/gtest.git -[submodule "src/lib/eigen"] - path = src/lib/eigen - url = https://github.com/PX4/eigen.git [submodule "src/lib/dspal"] path = src/lib/dspal url = https://github.com/mcharleb/dspal.git +[submodule "Tools/jMAVSim"] + path = Tools/jMAVSim + url = https://github.com/PX4/jMAVSim.git +[submodule "Tools/sitl_gazebo"] + path = Tools/sitl_gazebo + url = https://github.com/PX4/sitl_gazebo.git +[submodule "unittests/googletest"] + path = unittests/googletest + url = https://github.com/google/googletest.git diff --git a/.travis.yml b/.travis.yml index 5806f9d232..3786602b09 100644 --- a/.travis.yml +++ b/.travis.yml @@ -4,11 +4,13 @@ language: cpp matrix: + fast_finish: true include: - os: linux sudo: false - os: osx osx_image: beta-xcode6.3 + sudo: true cache: directories: @@ -19,16 +21,18 @@ addons: sources: - kubuntu-backports - ubuntu-toolchain-r-test + - george-edison55-precise-backports packages: - build-essential - ccache - - cmake - clang-3.5 + - cmake - g++-4.8 - gcc-4.8 - genromfs - libc6-i386 - libncurses5-dev + - ninja-build - python-argparse - python-empy - python-serial @@ -54,10 +58,11 @@ before_install: elif [ "${TRAVIS_OS_NAME}" = "osx" ]; then brew tap PX4/homebrew-px4 && brew update + && brew install astyle + && brew install gcc-arm-none-eabi && brew install genromfs && brew install kconfig-frontends - && brew install gcc-arm-none-eabi - && brew install astyle + && brew install ninja && sudo easy_install pip && sudo pip install pyserial empy ; @@ -80,6 +85,7 @@ before_script: env: global: + - NINJA_BUILD=1 # AWS KEY: $PX4_AWS_KEY - secure: "XknnZHWBbpHbN4f3fuAVwUztdLIu8ej4keC3aQSDofo3uw8AFEzojfsQsN9u77ShWSIV4iYJWh9C9ALkCx7TocJ+xYjiboo10YhM9lH/8u+EXjYWG6GHS8ua0wkir+cViSxoLNaMtmcb/rPTicJecAGANxLsIHyBAgTL3fkbLSA=" # AWS SECRET: $PX4_AWS_SECRET @@ -89,53 +95,29 @@ env: script: - make check_format - arm-none-eabi-gcc --version - - echo 'Building POSIX Firmware..' && echo -en 'travis_fold:start:script.1\\r' - - make posix_sitl_simple - - echo -en 'travis_fold:end:script.1\\r' - - echo 'Running Tests..' && echo -en 'travis_fold:start:script.2\\r' - - make posix_sitl_simple test - - cat build_posix_sitl_simple/src/modules/systemlib/mixer/mixer_multirotor.generated.h - - echo -en 'travis_fold:end:script.2\\r' - - echo 'Building NuttX Firmware..' && echo -en 'travis_fold:start:script.3\\r' - - make px4fmu-v1_default - - make px4fmu-v2_default - - echo -en 'travis_fold:end:script.3\\r' - - echo 'Running Tests..' && echo -en 'travis_fold:start:script.4\\r' - - make px4fmu-v2_default test - - echo -en 'travis_fold:end:script.4\\r' - #- make px4fmu-v2_default package - #- make posix -j4 - #- ccache -s - #- echo -en 'travis_fold:end:script.1\\r' - #- echo 'Running Tests..' && echo -en 'travis_fold:start:script.2\\r' - #- make tests - #- cat src/modules/systemlib/mixer/mixer_multirotor.generated.h - #- echo -en 'travis_fold:end:script.2\\r' - #- echo 'Building NuttX..' && echo -en 'travis_fold:start:script.3\\r' - #- make archives - #- ccache -s - #- echo -en 'travis_fold:end:script.3\\r' - #- echo 'Building NuttX Firmware..' && echo -en 'travis_fold:start:script.4\\r' - #- make -j4 - #- make size - #- ccache -s - #- echo -en 'travis_fold:end:script.4\\r' - #- zip Firmware.zip Images/*.px4 + - echo 'Building POSIX Firmware..' && make posix_sitl_simple + - echo 'Running Tests..' && make posix_sitl_simple test && cat build_posix_sitl_simple/src/modules/systemlib/mixer/mixer_multirotor.generated.h + - echo 'Building NuttX px4fmu-v1 Firmware..' && make px4fmu-v1_default + - echo 'Building NuttX px4fmu-v2 Firmware..' && make px4fmu-v2_default + - echo 'Running Tests..' && make px4fmu-v2_default test -#after_script: - #- git clone git://github.com/PX4/CI-Tools.git - #- ./CI-Tools/s3cmd-configure -## upload newest build for this branch with s3 index - #- ./CI-Tools/s3cmd-put Images/px4*.px4 CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/ -## archive newest build by date with s3 index - #- ./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/ -## upload top level index.html and timestamp.html - #- ./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:" - #- echo https://px4-travis.s3.amazonaws.com/archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/Firmware.zip +after_success: + - if [ "${TRAVIS_OS_NAME}" = "linux" ]; then + cp build_px4fmu-v1_default/src/firmware/nuttx/nuttx-px4fmu-v1-default.px4 px4fmu-v1_default.px4 + && cp build_px4fmu-v2_default/src/firmware/nuttx/nuttx-px4fmu-v2-default.px4 px4fmu-v2_default.px4 + && zip Firmware.zip px4fmu-v1_default.px4 px4fmu-v2_default.px4 + && git clone git://github.com/PX4/CI-Tools.git + && ./CI-Tools/s3cmd-configure + && ./CI-Tools/s3cmd-put px4fmu-v1_default.px4 px4fmu-v2_default.px4 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:" + && echo https://px4-travis.s3.amazonaws.com/archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/Firmware.zip + ; + fi deploy: provider: releases diff --git a/CMakeLists.txt b/CMakeLists.txt index 6aec7f2e0c..05a3cd59b3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -117,6 +117,8 @@ # #============================================================================= +# Warning: Changing this modifies CMake's internal workings +# and leads to wrong toolchain detection cmake_minimum_required(VERSION 2.8 FATAL_ERROR) #============================================================================= @@ -184,6 +186,7 @@ project(px4 CXX C ASM) if (NOT ${CMAKE_VERSION} VERSION_LESS 3.0.0) cmake_policy(SET CMP0045 NEW) # error on non-existent target in get prop cmake_policy(SET CMP0046 NEW) # no non-existent targets as dependencies + cmake_policy(SET CMP0025 OLD) # still report AppleClang as Clang endif() if (NOT ${CMAKE_VERSION} VERSION_LESS 3.1.0) cmake_policy(SET CMP0054 NEW) # don't dereference quoted variables @@ -225,9 +228,10 @@ 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_uavcan PATH "src/modules/uavcan/libuavcan") -px4_add_git_submodule(TARGET git_eigen PATH "src/lib/eigen") px4_add_git_submodule(TARGET git_nuttx PATH "NuttX") px4_add_git_submodule(TARGET git_dspal PATH "src/lib/dspal") +px4_add_git_submodule(TARGET git_jmavsim PATH "Tools/jMAVSim") +px4_add_git_submodule(TARGET git_gazebo PATH "Tools/sitl_gazebo") add_custom_target(submodule_clean WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} @@ -235,6 +239,19 @@ add_custom_target(submodule_clean COMMAND rm -rf .git/modules/* ) +#============================================================================= +# misc targets +# +add_custom_target(check_format + COMMAND Tools/check_code_style.sh + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + ) + +add_custom_target(config + COMMAND cmake-gui . + WORKING_DIRECTORY ${CMAKE_BINARY_DIR} + ) + #============================================================================= # external libraries # @@ -274,7 +291,7 @@ px4_generate_messages(TARGET msg_gen px4_generate_parameters_xml(OUT parameters.xml BOARD ${BOARD}) px4_generate_airframes_xml(OUT airframes.xml BOARD ${BOARD}) add_custom_target(xml_gen - DEPENDS git_eigen parameters.xml airframes.xml) + DEPENDS parameters.xml airframes.xml) #============================================================================= # external projects diff --git a/Makefile b/Makefile index e2f1814f0c..e67c21e1af 100644 --- a/Makefile +++ b/Makefile @@ -36,7 +36,19 @@ # We depend on our submodules, so we have to prevent attempts to # compile without it being present. ifeq ($(wildcard .git),) - $(error YOU HAVE TO USE GIT TO DOWNLOAD THIS REPOSITORY. ABORTING.) + $(error YOU HAVE TO USE GIT TO DOWNLOAD THIS REPOSITORY. ABORTING.) +endif + +CMAKE_VER := $(shell Tools/check_cmake.sh; echo $$?) +ifneq ($(CMAKE_VER),0) + $(warning Not a valid CMake version or CMake not installed.) + $(warning On Ubuntu, install or upgrade via:) + $(warning ) + $(warning sudo add-apt-repository ppa:george-edison55/cmake-3.x -y) + $(warning sudo apt-get update) + $(warning sudo apt-get install cmake) + $(warning ) + $(error Fatal) endif # Help @@ -67,12 +79,11 @@ all: px4fmu-v2_default ARGS := $(wordlist 2,$(words $(MAKECMDGOALS)),$(MAKECMDGOALS)) j ?= 4 -# disable ninja by default for now because it hides upload progress -#NINJA_BUILD := $(shell ninja --version 2>/dev/null) +NINJA_BUILD := $(shell ninja --version 2>/dev/null) ifdef NINJA_BUILD PX4_CMAKE_GENERATOR ?= "Ninja" PX4_MAKE = ninja - PX4_MAKE_ARGS = + PX4_MAKE_ARGS = else ifdef SYSTEMROOT @@ -89,6 +100,7 @@ endif # -------------------------------------------------------------------- # 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 git submodule update --init --recursive --force && mkdir -p $(PWD)/build_$@ && cd $(PWD)/build_$@ && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1); fi +$(PX4_MAKE) -C $(PWD)/build_$@ $(PX4_MAKE_ARGS) $(ARGS) endef @@ -113,57 +125,40 @@ px4fmu-v2_default: px4fmu-v2_simple: $(call cmake-build,nuttx_px4fmu-v2_simple) +px4fmu-v2_lpe: + $(call cmake-build,nuttx_px4fmu-v2_lpe) + nuttx_sim_simple: $(call cmake-build,$@) posix_sitl_simple: $(call cmake-build,$@) +posix_sitl_lpe: + $(call cmake-build,$@) + ros_sitl_simple: $(call cmake-build,$@) qurt_eagle_travis: $(call cmake-build,$@) +posix_eagle_release: + $(call cmake-build,$@) + posix: posix_sitl_simple +posix_sitl_default: posix_sitl_simple + ros: ros_sitl_simple -run_sitl_quad: posix - Tools/sitl_run.sh posix-configs/SITL/init/rcS +sitl_deprecation: + @echo "Deprecated. Use 'make posix_sitl_default run_sitl' instead." + @echo "Change init script with 'make posix_sitl_default config'" -run_sitl_plane: posix - Tools/sitl_run.sh posix-configs/SITL/init/rc.fixed_wing - -run_sitl_ros: posix - Tools/sitl_run.sh posix-configs/SITL/init/rc_iris_ros - -lldb_sitl_quad: posix - Tools/sitl_run.sh posix-configs/SITL/init/rcS lldb - -lldb_sitl_plane: posix - Tools/sitl_run.sh posix-configs/SITL/init/rc.fixed_wing lldb - -lldb_sitl_ros: posix - Tools/sitl_run.sh posix-configs/SITL/init/rc_iris_ros lldb - -gdb_sitl_quad: posix - Tools/sitl_run.sh posix-configs/SITL/init/rcS gdb - -gdb_sitl_plane: posix - Tools/sitl_run.sh posix-configs/SITL/init/rc.fixed_wing lldb - -gdb_sitl_ros: posix - Tools/sitl_run.sh posix-configs/SITL/init/rc_iris_ros lldb - -sitl_quad: - @echo "Deprecated. Use 'run_sitl_quad' instead." - -sitl_plane: - @echo "Deprecated. Use 'run_sitl_plane' instead." - -sitl_ros: - @echo "Deprecated. Use 'run_sitl_ros' instead." +sitl_quad: sitl_deprecation +sitl_plane: sitl_deprecation +sitl_ros: sitl_deprecation # Other targets # -------------------------------------------------------------------- @@ -176,7 +171,9 @@ clean: @(cd src/modules/uavcan/libuavcan && git clean -d -f -x) # 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 libuavcan +cmake_targets = 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 \ + jmavsim_gdb jmavsim_lldb $(foreach targ,$(cmake_targets),$(eval $(call cmake-targ,$(targ)))) .PHONY: clean diff --git a/README.md b/README.md index 7cb07907ff..eb33fff02e 100644 --- a/README.md +++ b/README.md @@ -4,7 +4,7 @@ [![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) -This repository contains the PX4 Flight Core, with the main applications located in the src/modules directory. It also contains the PX4 Drone Platform, which contains drivers and middleware to run drones. +This repository contains the [PX4 Flight Core](http://px4.io), with the main applications located in the src/modules directory. It also contains the PX4 Drone Platform, which contains drivers and middleware to run drones. * Official Website: http://px4.io * License: BSD 3-clause (see [LICENSE.md](https://github.com/PX4/Firmware/blob/master/LICENSE.md)) @@ -32,7 +32,7 @@ Software in the Loop guide: Use software in the loop [to get started with the codebase](https://pixhawk.org/dev/simulation/native_sitl). Developer guide: -http://px4.io/dev/ +http://dev.px4.io Testing guide: http://px4.io/dev/unit_tests diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index 0acb4088f4..97beb6831a 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -28,11 +28,3 @@ fi set MIXER quad_w set PWM_OUT 1234 -set PWM_MIN 1200 - -set MIXER_AUX pass -set PWM_AUX_RATE 50 -set PWM_AUX_OUT 1234 -set PWM_AUX_DISARMED 1000 -set PWM_AUX_MIN 1000 -set PWM_AUX_MAX 2000 diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index ffaa935055..30af86730e 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -32,6 +32,3 @@ fi set MIXER quad_w set PWM_OUT 1234 - -set PWM_MIN 1200 -set PWM_MAX 1950 diff --git a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d index 7e27043156..7accc4f6e9 100644 --- a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d +++ b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d @@ -30,7 +30,4 @@ fi set MIXER quad_w -set PWM_MIN 1210 -set PWM_MAX 2100 - set PWM_OUT 1234 diff --git a/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat b/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat index 4f22ddc436..0b1e3b2dfa 100644 --- a/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat +++ b/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat @@ -30,7 +30,6 @@ fi set MIXER sk450_deadcat set PWM_OUT 1234 -set PWM_MIN 1050 set PWM_AUX_OUT 1234 # set PWM_AUX_MIN 900 diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index 55e5237945..5e7ba1bda5 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -15,13 +15,12 @@ sh /etc/init.d/4001_quad_x if [ $AUTOCNF == yes ] then - # TODO REVIEW param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.16 + param set MC_ROLLRATE_P 0.15 param set MC_ROLLRATE_I 0.05 param set MC_ROLLRATE_D 0.01 param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.16 + param set MC_PITCHRATE_P 0.15 param set MC_PITCHRATE_I 0.05 param set MC_PITCHRATE_D 0.01 param set MC_YAW_P 2.8 diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 1dfe6c09bc..4c6cdbd7cd 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -12,7 +12,13 @@ then attitude_estimator_q start position_estimator_inav start else - ekf_att_pos_estimator start + if param compare LPE_ENABLED 1 + then + attitude_estimator_q start + local_position_estimator start + else + ekf_att_pos_estimator start + fi fi if mc_att_control start diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f14dd3015d..45128dae52 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -260,11 +260,11 @@ then # # Check if PX4IO present and update firmware if needed # - if [ -f /etc/extras/px4io-v2_default.bin ] + if [ -f /etc/extras/px4io-v2.bin ] then - set IO_FILE /etc/extras/px4io-v2_default.bin + set IO_FILE /etc/extras/px4io-v2.bin else - set IO_FILE /etc/extras/px4io-v1_default.bin + set IO_FILE /etc/extras/px4io-v1.bin fi if px4io checkcrc ${IO_FILE} diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS index 6d72ecc6c7..0df6c29071 100644 --- a/ROMFS/px4fmu_test/init.d/rcS +++ b/ROMFS/px4fmu_test/init.d/rcS @@ -55,11 +55,11 @@ else echo "[param] FAILED loading $PARAM_FILE" fi -if [ -f /etc/extras/px4io-v2_default.bin ] +if [ -f /etc/extras/px4io-v2.bin ] then - set io_file /etc/extras/px4io-v2_default.bin + set io_file /etc/extras/px4io-v2.bin else - set io_file /etc/extras/px4io-v1_default.bin + set io_file /etc/extras/px4io-v1.bin fi if px4io start diff --git a/Tools/check_cmake.sh b/Tools/check_cmake.sh new file mode 100755 index 0000000000..423b790dbd --- /dev/null +++ b/Tools/check_cmake.sh @@ -0,0 +1,14 @@ +#!/bin/bash +cmake_ver=`cmake --version` + +if [[ $cmake_ver == "" ]] +then + exit 1; +fi + +if [[ $cmake_ver == *"2.8"* ]] || [[ $cmake_ver == *"2.9"* ]] || [[ $cmake_ver == *"3.0"* ]] || [[ $cmake_ver == *"3.1"* ]] +then + exit 1; +fi + +exit 0; diff --git a/Tools/check_code_style.sh b/Tools/check_code_style.sh index bc59128317..c3dd2a3f32 100755 --- a/Tools/check_code_style.sh +++ b/Tools/check_code_style.sh @@ -4,22 +4,32 @@ failed=0 for fn in $(find src/examples \ src/systemcmds \ src/include \ - src/drivers/blinkm \ - src/drivers/bma180 \ - src/drivers/pca9685 \ - src/drivers/pca8574 \ - src/drivers/md25 \ - src/drivers/ms5611 \ - src/drivers/stm32 \ - src/drivers/px4io \ - src/drivers/px4fmu \ + 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/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/fixedwing_backside \ src/modules/segway \ + src/modules/local_position_estimator \ src/modules/unit_test \ src/modules/systemlib \ + src/modules/controllib \ -path './Build' -prune -o \ -path './mavlink' -prune -o \ -path './NuttX' -prune -o \ diff --git a/Tools/generate_listener.py b/Tools/generate_listener.py index 1f240ddbf6..625eda7037 100755 --- a/Tools/generate_listener.py +++ b/Tools/generate_listener.py @@ -119,8 +119,8 @@ print(""" #define PRIu64 "llu" #endif -#ifndef PRI64 -#define PRI64 "lld" +#ifndef PRId64 +#define PRId64 "lld" #endif """) @@ -184,7 +184,7 @@ for index,m in enumerate(messages[1:]): print("\t\t\t}") print("\t\t\tprintf(\"\\n\");") elif item[0] == "int64": - print("\t\t\tprintf(\"%s: %%\" PRI64 \"\\n\",container.%s);" % (item[1], item[1])) + print("\t\t\tprintf(\"%s: %%\" PRId64 \"\\n\",container.%s);" % (item[1], item[1])) elif item[0] == "int32": print("\t\t\tprintf(\"%s: %%d\\n\",container.%s);" % (item[1], item[1])) elif item[0] == "uint32": diff --git a/Tools/jMAVSim b/Tools/jMAVSim new file mode 160000 index 0000000000..96029523d1 --- /dev/null +++ b/Tools/jMAVSim @@ -0,0 +1 @@ +Subproject commit 96029523d13fc7fdac04c1a50b08c0eb0b39f9a9 diff --git a/Tools/posix.gdbinit b/Tools/posix.gdbinit index 0de1123362..23fa44b4e0 100644 --- a/Tools/posix.gdbinit +++ b/Tools/posix.gdbinit @@ -1,2 +1,2 @@ -handle SIGCONT nostop +handle SIGCONT nostop noprint nopass run diff --git a/Tools/px4params/srcparser.py b/Tools/px4params/srcparser.py index 2201fac6dd..b67ad890a9 100644 --- a/Tools/px4params/srcparser.py +++ b/Tools/px4params/srcparser.py @@ -45,6 +45,7 @@ class Parameter(object): "min": 5, "max": 4, "unit": 3, + "decimal": 2, # all others == 0 (sorted alphabetically) } @@ -106,7 +107,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"]) + valid_tags = set(["group", "board", "min", "max", "unit", "decimal"]) # Order of parameter groups priority = { diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo new file mode 160000 index 0000000000..d362e661b4 --- /dev/null +++ b/Tools/sitl_gazebo @@ -0,0 +1 @@ +Subproject commit d362e661b46474153f43f51a6eb947d4fda1bebe diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index 1a7741d6fd..6eb69eb46b 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -1,16 +1,85 @@ #!/bin/bash -cp Tools/posix_lldbinit build_posix_sitl_simple/src/firmware/posix/.lldbinit -cp Tools/posix.gdbinit build_posix_sitl_simple/src/firmware/posix/.gdbinit -cd build_posix_sitl_simple/src/firmware/posix + +rc_script=$1 +debugger=$2 +program=$3 +build_path=$4 +curr_dir=`pwd` + +echo SITL ARGS +echo rc_script: $rc_script +echo debugger: $debugger +echo program: $program +echo build_path: $build_path + +if [ "$#" != 4 ] +then + echo usage: sitl_run.sh rc_script debugger program build_path + exit 1 +fi + +# kill process names that might stil +# be running from last time +pkill gazebo +pkill mainapp +jmavsim_pid=`jps | grep Simulator | cut -d" " -f1` +if [ -n "$jmavsim_pid" ] +then + kill $jmavsim_pid +fi + +cp Tools/posix_lldbinit $build_path/src/firmware/posix/.lldbinit +cp Tools/posix.gdbinit $build_path/src/firmware/posix/.gdbinit + +SIM_PID=0 + +if [ "$program" == "jmavsim" ] +then + cd Tools/jMAVSim + ant + java -Djava.ext.dirs= -cp lib/*:out/production/jmavsim.jar me.drton.jmavsim.Simulator -udp 127.0.0.1:14560 & + SIM_PID=`echo $!` +elif [ "$3" == "gazebo" ] +then + if [ -x "$(command -v gazebo)" ] + then + # Set the plugin path so Gazebo finds our model and sim + export GAZEBO_PLUGIN_PATH=${GAZEBO_PLUGIN_PATH}:$curr_dir/Tools/sitl_gazebo/Build + # Set the model path so Gazebo finds the airframes + export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:$curr_dir/Tools/sitl_gazebo/models + # Disable online model lookup since this is quite experimental and unstable + 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 .. + make -j4 + gazebo ../worlds/iris.world & + SIM_PID=`echo $!` + else + echo "You need to have gazebo simulator installed!" + exit 1 + fi +fi +cd $build_path/src/firmware/posix mkdir -p rootfs/fs/microsd mkdir -p rootfs/eeprom touch rootfs/eeprom/parameters -if [ "$2" == "lldb" ] +# Start Java simulator +if [ "$debugger" == "lldb" ] then - lldb -- mainapp ../../../../$1 -elif [ "$2" == "gdb" ] + lldb -- mainapp ../../../../${rc_script}_${program} +elif [ "$debugger" == "gdb" ] then - gdb --args mainapp ../../../../$1 + gdb --args mainapp ../../../../${rc_script}_${program} else - ./mainapp ../../../../$1 + ./mainapp ../../../../${rc_script}_${program} +fi + +if [ "$3" == "jmavsim" ] +then + kill -9 $SIM_PID +elif [ "$3" == "gazebo" ] +then + kill -9 $SIM_PID fi diff --git a/Vagrantfile b/Vagrantfile index 775abd461f..dfa4da4d8b 100644 --- a/Vagrantfile +++ b/Vagrantfile @@ -26,7 +26,7 @@ Vagrant.configure(2) do |config| # Create a private network, which allows host-only access to the machine # using a specific IP. - # config.vm.network "private_network", ip: "192.168.33.10" + config.vm.network "private_network", ip: "192.168.33.10" # Create a public network, which generally matched to bridged network. # Bridged networks make the machine appear as another physical device on @@ -37,7 +37,7 @@ Vagrant.configure(2) do |config| # the path on the host to the actual folder. The second argument is # the path on the guest to mount the folder. And the optional third # argument is a set of non-required options. - config.vm.synced_folder ".", "/Firmware" + config.vm.synced_folder ".", "/Firmware", type: "nfs" # Provider-specific configuration so you can fine-tune various # backing providers for Vagrant. These expose provider-specific options. @@ -77,14 +77,18 @@ Vagrant.configure(2) do |config| # Ensure we start in the Firmware folder echo "cd /Firmware" >> ~/.bashrc # Install software + sudo add-apt-repository ppa:terry.guo/gcc-arm-embedded -y + sudo add-apt-repository ppa:george-edison55/cmake-3.x -y sudo apt-get update - sudo apt-get install -y build-essential ccache cmake clang-3.5 lldb-3.5 g++-4.8 gcc-4.8 genromfs libc6-i386 libncurses5-dev python-argparse python-empy python-serial s3cmd texinfo zlib1g-dev git-core zip gdb + sudo apt-get install -y build-essential ccache cmake clang-3.5 lldb-3.5 g++-4.8 gcc-4.8 genromfs libc6-i386 libncurses5-dev python-argparse python-empy python-serial s3cmd texinfo zlib1g-dev git-core zip gdb gcc-arm-linux-gnueabihf g++-arm-linux-gnueabihf pushd . cd ~ wget -q https://launchpadlibrarian.net/186124160/gcc-arm-none-eabi-4_8-2014q3-20140805-linux.tar.bz2 tar -jxf gcc-arm-none-eabi-4_8-2014q3-20140805-linux.tar.bz2 exportline="export PATH=$HOME/gcc-arm-none-eabi-4_8-2014q3/bin:\$PATH" if grep -Fxq "$exportline" ~/.profile; then echo nothing to do ; else echo $exportline >> ~/.profile; fi + exportline2="export HEXAGON_TOOLS_ROOT=$HOME/Qualcomm/HEXAGON_Tools/7.2.10/Tools" + if grep -Fxq "$exportline2" ~/.profile; then echo nothing to do ; else echo $exportline2 >> ~/.profile; fi . ~/.profile popd # setup ccache diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake index 5c9b8aefdf..26598fef70 100644 --- a/cmake/common/px4_base.cmake +++ b/cmake/common/px4_base.cmake @@ -433,6 +433,7 @@ function(px4_add_upload) WORKING_DIRECTORY ${CMAKE_BINARY_DIR} COMMENT "uploading ${BUNDLE}" VERBATIM + USES_TERMINAL ) endfunction() @@ -492,7 +493,6 @@ function(px4_add_common_flags) -Wpointer-arith -Wmissing-declarations -Wno-unused-parameter - -Wno-varargs -Werror=format-security -Werror=array-bounds -Wfatal-errors @@ -512,11 +512,12 @@ function(px4_add_common_flags) list(APPEND warnings -Wframe-larger-than=1024) endif() - if (${CMAKE_C_COMPILER_ID} STREQUAL "Clang") + if (${CMAKE_C_COMPILER_ID} MATCHES ".*Clang.*") # QuRT 6.4.X compiler identifies as Clang but does not support this option if (NOT ${OS} STREQUAL "qurt") list(APPEND warnings -Wno-unused-const-variable + -Wno-varargs ) endif() else() @@ -539,7 +540,7 @@ function(px4_add_common_flags) -fdata-sections ) - if (NOT ${CMAKE_C_COMPILER_ID} STREQUAL "Clang") + if (NOT ${CMAKE_C_COMPILER_ID} MATCHES ".*Clang.*") list(APPEND optimization_flags -fno-strength-reduce -fno-builtin-printf @@ -553,7 +554,7 @@ function(px4_add_common_flags) -Wnested-externs ) - if (NOT ${CMAKE_C_COMPILER_ID} STREQUAL "Clang") + if (NOT ${CMAKE_C_COMPILER_ID} MATCHES ".*Clang.*") list(APPEND c_warnings -Wold-style-declaration -Wmissing-parameter-type @@ -632,7 +633,7 @@ function(px4_add_common_flags) -DCONFIG_ARCH_BOARD_${board_config} ) - if (NOT ${CMAKE_C_COMPILER_ID} STREQUAL "Clang") + if (NOT ${CMAKE_C_COMPILER_ID} MATCHES ".*Clang.*") set(added_exe_link_flags -Wl,--warn-common -Wl,--gc-sections diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake index a242f4f736..f3638c5749 100644 --- a/cmake/configs/nuttx_px4fmu-v2_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake @@ -22,6 +22,7 @@ set(config_module_list drivers/hmc5883 drivers/ms5611 drivers/mb12xx + drivers/srf02 drivers/sf0x drivers/ll40ls drivers/trone diff --git a/cmake/configs/nuttx_px4fmu-v2_lpe.cmake b/cmake/configs/nuttx_px4fmu-v2_lpe.cmake new file mode 100644 index 0000000000..6e5cd10ba4 --- /dev/null +++ b/cmake/configs/nuttx_px4fmu-v2_lpe.cmake @@ -0,0 +1,5 @@ +include(cmake/configs/nuttx_px4fmu-v2_default.cmake) + +list(APPEND config_module_list + modules/local_position_estimator + ) diff --git a/cmake/configs/posix_eagle_release.cmake b/cmake/configs/posix_eagle_release.cmake index 8e3bb26078..b0b2a9171d 100644 --- a/cmake/configs/posix_eagle_release.cmake +++ b/cmake/configs/posix_eagle_release.cmake @@ -5,6 +5,15 @@ if("${DSPAL_STUBS_ENABLE}" STREQUAL "") set(DSPAL_STUBS_ENABLE "1") endif() +if ("${QRL_SDK_DIR}" STREQUAL "") + set(QRL_SDK_DIR /opt/qrlsdk) +endif() + +set(CMAKE_PROGRAM_PATH + "${QRL_SDK_DIR}/gcc-linaro-4.8-2015.06-x86_64_arm-linux-gnueabihf/bin" + ${CMAKE_PROGRAM_PATH} + ) + set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake) set(config_module_list diff --git a/cmake/configs/posix_sitl_lpe.cmake b/cmake/configs/posix_sitl_lpe.cmake new file mode 100644 index 0000000000..0598044844 --- /dev/null +++ b/cmake/configs/posix_sitl_lpe.cmake @@ -0,0 +1,9 @@ +include(cmake/configs/posix_sitl_simple.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_simple.cmake b/cmake/configs/posix_sitl_simple.cmake index 227bffeab8..b0a8158070 100644 --- a/cmake/configs/posix_sitl_simple.cmake +++ b/cmake/configs/posix_sitl_simple.cmake @@ -62,6 +62,27 @@ set(config_extra_builtin_cmds sercon ) +set(config_sitl_rcS + posix-configs/SITL/init/rcS + 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") + + + add_custom_target(sercon) set_target_properties(sercon PROPERTIES MAIN "sercon" STACK "2048") diff --git a/cmake/configs/qurt_eagle_hello.cmake b/cmake/configs/qurt_eagle_hello.cmake index c127216681..4328a3d4aa 100644 --- a/cmake/configs/qurt_eagle_hello.cmake +++ b/cmake/configs/qurt_eagle_hello.cmake @@ -1,6 +1,6 @@ include(qurt/px4_impl_qurt) -set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.4.cmake) +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake) set(config_module_list drivers/device diff --git a/cmake/configs/qurt_eagle_hil.cmake b/cmake/configs/qurt_eagle_hil.cmake index 0b48820254..4222fff249 100644 --- a/cmake/configs/qurt_eagle_hil.cmake +++ b/cmake/configs/qurt_eagle_hil.cmake @@ -1,6 +1,6 @@ include(qurt/px4_impl_qurt) -set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.4.cmake) +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake) set(config_module_list drivers/device diff --git a/cmake/configs/qurt_eagle_muorb.cmake b/cmake/configs/qurt_eagle_muorb.cmake index 492225aa40..d73d7becff 100644 --- a/cmake/configs/qurt_eagle_muorb.cmake +++ b/cmake/configs/qurt_eagle_muorb.cmake @@ -1,6 +1,6 @@ include(qurt/px4_impl_qurt) -set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.4.cmake) +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake) set(config_module_list drivers/device diff --git a/cmake/configs/qurt_eagle_release.cmake b/cmake/configs/qurt_eagle_release.cmake index 494416ed4f..945e9da199 100644 --- a/cmake/configs/qurt_eagle_release.cmake +++ b/cmake/configs/qurt_eagle_release.cmake @@ -20,7 +20,7 @@ set(target_libraries ) -set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.4.cmake) +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake) set(config_module_list # diff --git a/cmake/configs/qurt_eagle_test.cmake b/cmake/configs/qurt_eagle_test.cmake index b2935f1fda..2e6a370950 100644 --- a/cmake/configs/qurt_eagle_test.cmake +++ b/cmake/configs/qurt_eagle_test.cmake @@ -1,6 +1,6 @@ include(qurt/px4_impl_qurt) -set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.4.cmake) +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake) set(config_module_list drivers/device diff --git a/cmake/configs/qurt_eagle_travis.cmake b/cmake/configs/qurt_eagle_travis.cmake index 3b4672a115..b9fda6ec48 100644 --- a/cmake/configs/qurt_eagle_travis.cmake +++ b/cmake/configs/qurt_eagle_travis.cmake @@ -3,7 +3,7 @@ include(qurt/px4_impl_qurt) # Run a full link with build stubs to make sure qurt target isn't broken set(QURT_ENABLE_STUBS "1") -set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.4.cmake) +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake) set(config_module_list drivers/device diff --git a/cmake/qurt/px4_impl_qurt.cmake b/cmake/qurt/px4_impl_qurt.cmake index 49bd83f53b..8f9f3dc5d3 100644 --- a/cmake/qurt/px4_impl_qurt.cmake +++ b/cmake/qurt/px4_impl_qurt.cmake @@ -222,14 +222,7 @@ function(px4_os_prebuild_targets) ONE_VALUE OUT BOARD THREADS REQUIRED OUT BOARD ARGN ${ARGN}) - add_custom_target(git_eigen_patched - WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/src/lib/eigen - COMMAND git checkout . - COMMAND git checkout master - COMMAND patch -p1 -i ${CMAKE_SOURCE_DIR}/cmake/qurt/qurt_eigen.patch - DEPENDS git_eigen) - add_custom_target(${OUT} DEPENDS git_dspal git_eigen_patched) - add_custom_target(ALL DEPENDS git_eigen_patched) + add_custom_target(${OUT} DEPENDS git_dspal git_eigen) endfunction() diff --git a/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake b/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake index 61d63f8677..eb48554b16 100644 --- a/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake +++ b/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake @@ -26,13 +26,13 @@ set(CMAKE_SYSTEM_VERSION 1) # specify the cross compiler find_program(C_COMPILER arm-linux-gnueabihf-gcc) if(NOT C_COMPILER) - message(FATAL_ERROR "could not find arm-none-eabi-gcc 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++) if(NOT CXX_COMPILER) - message(FATAL_ERROR "could not find arm-none-eabi-g++ compiler") + message(FATAL_ERROR "could not find arm-linux-gnueabihf-g++ compiler") endif() cmake_force_cxx_compiler(${CXX_COMPILER} GNU) @@ -41,7 +41,7 @@ foreach(tool objcopy nm ld) string(TOUPPER ${tool} TOOL) find_program(${TOOL} arm-linux-gnueabihf-${tool}) if(NOT ${TOOL}) - message(FATAL_ERROR "could not find ${tool}") + message(FATAL_ERROR "could not find arm-linux-gnueabihf-${tool}") endif() endforeach() diff --git a/cmake/toolchains/Toolchain-hexagon-7.4.cmake b/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake similarity index 90% rename from cmake/toolchains/Toolchain-hexagon-7.4.cmake rename to cmake/toolchains/Toolchain-hexagon-7.2.10.cmake index 9c4286044d..1ab6fd4060 100644 --- a/cmake/toolchains/Toolchain-hexagon-7.4.cmake +++ b/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake @@ -34,8 +34,12 @@ include(CMakeForceCompiler) list(APPEND CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake) include(common/px4_base) -if(NOT HEXAGON_TOOLS_ROOT) - set(HEXAGON_TOOLS_ROOT /opt/7.4/Tools) +if ("$ENV{HEXAGON_TOOLS_ROOT}" STREQUAL "") + message(FATAL_ERROR + "The HexagonTools version 7.2.10 must be installed and the environment variable HEXAGON_TOOLS_ROOT must be set" + "(e.g. export HEXAGON_TOOLS_ROOT=/opt/HEXAGON_Tools/7.2.10/Tools)") +else() + set(HEXAGON_TOOLS_ROOT $ENV{HEXAGON_TOOLS_ROOT}) endif() macro (list2string out in) @@ -53,7 +57,7 @@ set(HEXAGON_LIB_DIR ${HEXAGON_TOOLS_ROOT}/gnu/hexagon/lib) set(HEXAGON_ISS_DIR ${HEXAGON_TOOLS_ROOT}/lib/iss) set(TOOLSLIB ${HEXAGON_TOOLS_ROOT}/target/hexagon/lib/${V_ARCH}/G0) -# Use the HexagonTools compiler (6.4.05) +# Use the HexagonTools compiler (7.2.10) set(CMAKE_C_COMPILER ${HEXAGON_BIN}/${CROSSDEV}clang) set(CMAKE_CXX_COMPILER ${HEXAGON_BIN}/${CROSSDEV}clang++) @@ -82,6 +86,7 @@ set(ARCHCPUFLAGS add_definitions( -D_PID_T -D_UID_T -D_TIMER_T -Dnoreturn_function= + -D_HAS_C9X -D__EXPORT= -Drestrict= -D_DEBUG @@ -238,3 +243,12 @@ 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) + +# The Hexagon compiler doesn't support the -rdynamic flag and this is set +# in the base cmake scripts. We have to redefine the __linux_compiler_gnu +# macro for cmake 2.8 to work +set(__LINUX_COMPILER_GNU 1) +macro(__linux_compiler_gnu lang) + set(CMAKE_SHARED_LIBRARY_LINK_${lang}_FLAGS "") +endmacro() + diff --git a/cmake/toolchains/Toolchain-posix-clang-native.cmake b/cmake/toolchains/Toolchain-posix-clang-native.cmake deleted file mode 100644 index 0945b2b1f5..0000000000 --- a/cmake/toolchains/Toolchain-posix-clang-native.cmake +++ /dev/null @@ -1,90 +0,0 @@ - -set(WARNINGS - -Wall - -Werror - -Wextra - -Wdouble-promotion - -Wshadow - -Wfloat-equal - -Wframe-larger-than=1024 - -Wpointer-arith - -Wlogical-op - -Wmissing-declarations - -Wno-unused-parameter - -Werror=format-security - -Werror=array-bounds - -Wfatal-errors - -Wformat=1 - -Werror=unused-but-set-variable - -Werror=unused-variable - -Werror=double-promotion - -Werror=reorder - -Werror=uninitialized - -Werror=init-self - -Werror=return-type - -Werror=deprecated - -Werror=unused-private-field - -Wno-packed - -Wno-frame-larger-than= - -Wno-varargs - #-Wcast-qual - generates spurious noreturn attribute warnings, - # try again later - #-Wconversion - would be nice, but too many "risky-but-safe" - # conversions in the code - #-Wcast-align - would help catch bad casts in some cases, - # but generates too many false positives - ) - -set(OPT_FLAGS - -Os -g3 - ) - -#============================================================================= -# c flags -# -set(C_WARNINGS - -Wbad-function-cast - -Wstrict-prototypes - -Wold-style-declaration - -Wmissing-parameter-type - -Wmissing-prototypes - -Wnested-externs - ) -set(C_FLAGS - -std=gnu99 - -fno-common - ) - -#============================================================================= -# cxx flags -# -set(CXX_WARNINGS - -Wno-missing-field-initializers - -Wno-varargs - ) -set(CXX_FLAGS - -fno-exceptions - -fno-rtti - -std=gnu++0x - -fno-threadsafe-statics - -DCONFIG_WCHAR_BUILTIN - -D__CUSTOM_FILE_IO__ - ) - -#============================================================================= -# ld flags -# -set(LD_FLAGS - -Wl,--warn-common - -Wl,--gc-sections - ) - -#============================================================================= -# misc flags -# -set(VISIBILITY_FLAGS - -fvisibility=hidden - "-include ${CMAKE_SOURCE_DIR}/src/include/visibility.h" - ) -set(EXE_LINK_FLAGS) - diff --git a/nuttx-configs/px4fmu-v1/nsh/Make.defs b/nuttx-configs/px4fmu-v1/nsh/Make.defs index 4e08d28a29..a1439476aa 100644 --- a/nuttx-configs/px4fmu-v1/nsh/Make.defs +++ b/nuttx-configs/px4fmu-v1/nsh/Make.defs @@ -124,11 +124,9 @@ ARCHWARNINGS = -Wall \ -Wextra \ -Wdouble-promotion \ -Wshadow \ - -Wfloat-equal \ -Wframe-larger-than=1024 \ -Wpointer-arith \ -Wlogical-op \ - -Wmissing-declarations \ -Wpacked \ -Wno-unused-parameter # -Wcast-qual - generates spurious noreturn attribute warnings, try again later @@ -140,7 +138,6 @@ ARCHCWARNINGS = $(ARCHWARNINGS) \ -Wstrict-prototypes \ -Wold-style-declaration \ -Wmissing-parameter-type \ - -Wmissing-prototypes \ -Wnested-externs ARCHWARNINGSXX = $(ARCHWARNINGS) \ -Wno-psabi diff --git a/nuttx-configs/px4fmu-v2/nsh/Make.defs b/nuttx-configs/px4fmu-v2/nsh/Make.defs index 5a1d5af2c4..a09e6794e3 100644 --- a/nuttx-configs/px4fmu-v2/nsh/Make.defs +++ b/nuttx-configs/px4fmu-v2/nsh/Make.defs @@ -124,11 +124,9 @@ ARCHWARNINGS = -Wall \ -Wextra \ -Wdouble-promotion \ -Wshadow \ - -Wfloat-equal \ -Wframe-larger-than=1024 \ -Wpointer-arith \ -Wlogical-op \ - -Wmissing-declarations \ -Wpacked \ -Wno-unused-parameter # -Wcast-qual - generates spurious noreturn attribute warnings, try again later @@ -140,7 +138,6 @@ ARCHCWARNINGS = $(ARCHWARNINGS) \ -Wstrict-prototypes \ -Wold-style-declaration \ -Wmissing-parameter-type \ - -Wmissing-prototypes \ -Wnested-externs ARCHWARNINGSXX = $(ARCHWARNINGS) \ -Wno-psabi diff --git a/nuttx-configs/px4io-v1/nsh/Make.defs b/nuttx-configs/px4io-v1/nsh/Make.defs index 74f38c0cb4..df1110fc61 100644 --- a/nuttx-configs/px4io-v1/nsh/Make.defs +++ b/nuttx-configs/px4io-v1/nsh/Make.defs @@ -117,11 +117,9 @@ ARCHWARNINGS = -Wall \ -Wextra \ -Wdouble-promotion \ -Wshadow \ - -Wfloat-equal \ -Wframe-larger-than=1024 \ -Wpointer-arith \ -Wlogical-op \ - -Wmissing-declarations \ -Wpacked \ -Wno-unused-parameter # -Wcast-qual - generates spurious noreturn attribute warnings, try again later @@ -133,7 +131,6 @@ ARCHCWARNINGS = $(ARCHWARNINGS) \ -Wstrict-prototypes \ -Wold-style-declaration \ -Wmissing-parameter-type \ - -Wmissing-prototypes \ -Wnested-externs ARCHWARNINGSXX = $(ARCHWARNINGS) ARCHDEFINES = diff --git a/nuttx-configs/px4io-v2/nsh/Make.defs b/nuttx-configs/px4io-v2/nsh/Make.defs index 287466db60..23ebca902a 100644 --- a/nuttx-configs/px4io-v2/nsh/Make.defs +++ b/nuttx-configs/px4io-v2/nsh/Make.defs @@ -117,11 +117,9 @@ ARCHWARNINGS = -Wall \ -Wextra \ -Wdouble-promotion \ -Wshadow \ - -Wfloat-equal \ -Wframe-larger-than=1024 \ -Wpointer-arith \ -Wlogical-op \ - -Wmissing-declarations \ -Wpacked \ -Wno-unused-parameter # -Wcast-qual - generates spurious noreturn attribute warnings, try again later @@ -133,7 +131,6 @@ ARCHCWARNINGS = $(ARCHWARNINGS) \ -Wstrict-prototypes \ -Wold-style-declaration \ -Wmissing-parameter-type \ - -Wmissing-prototypes \ -Wnested-externs ARCHWARNINGSXX = $(ARCHWARNINGS) ARCHDEFINES = diff --git a/posix-configs/SITL/init/rcS_gazebo b/posix-configs/SITL/init/rcS_gazebo new file mode 100644 index 0000000000..ca5415d110 --- /dev/null +++ b/posix-configs/SITL/init/rcS_gazebo @@ -0,0 +1,59 @@ +uorb start +simulator start -s +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 CAL_GYRO0_ID 2293760 +param set CAL_ACC0_ID 1376256 +param set CAL_ACC1_ID 1310720 +param set CAL_MAG0_ID 196608 +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.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 2 +rgbled 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 +attitude_estimator_q start +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 +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_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 diff --git a/posix-configs/SITL/init/rcS b/posix-configs/SITL/init/rcS_jmavsim similarity index 100% rename from posix-configs/SITL/init/rcS rename to posix-configs/SITL/init/rcS_jmavsim diff --git a/posix-configs/SITL/init/rcS_lpe_gazebo b/posix-configs/SITL/init/rcS_lpe_gazebo new file mode 100644 index 0000000000..6093991c13 --- /dev/null +++ b/posix-configs/SITL/init/rcS_lpe_gazebo @@ -0,0 +1,66 @@ +uorb start +simulator start -s +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 CAL_GYRO0_ID 2293760 +param set CAL_ACC0_ID 1376256 +param set CAL_ACC1_ID 1310720 +param set CAL_MAG0_ID 196608 +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_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 + +# changes for LPE +param set COM_RC_IN_MODE 1 +param set LPE_BETA_MAX 10000 + +rgbled 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 +attitude_estimator_q 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 +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_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 diff --git a/posix-configs/SITL/init/rcS_lpe_jmavsim b/posix-configs/SITL/init/rcS_lpe_jmavsim new file mode 100644 index 0000000000..9b416a0b26 --- /dev/null +++ b/posix-configs/SITL/init/rcS_lpe_jmavsim @@ -0,0 +1,65 @@ +uorb start +simulator start -s +param load +param set MAV_TYPE 2 +param set MC_PITCHRATE_P 0.15 +param set MC_ROLLRATE_P 0.15 +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 CAL_GYRO0_ID 2293760 +param set CAL_ACC0_ID 1376256 +param set CAL_ACC1_ID 1310720 +param set CAL_MAG0_ID 196608 +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 SENS_BOARD_ROT 0 + +# changes for LPE +param set COM_RC_IN_MODE 1 +param set LPE_BETA_MAX 10000 + +rgbled 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 +attitude_estimator_q 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_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_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 diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 0b6227a1be..df7f8c633a 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -133,6 +133,7 @@ Airspeed::init() /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(differential_pressure_s)); + if (_reports == nullptr) { goto out; } diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c index 1777e7f27c..8ad0ee7168 100644 --- a/src/drivers/ardrone_interface/ardrone_interface.c +++ b/src/drivers/ardrone_interface/ardrone_interface.c @@ -92,8 +92,10 @@ static void usage(const char *reason); static void usage(const char *reason) { - if (reason) + if (reason) { warnx("%s\n", reason); + } + warnx("usage: {start|stop|status} [-d ]\n\n"); exit(1); } @@ -122,11 +124,11 @@ int ardrone_interface_main(int argc, char *argv[]) thread_should_exit = false; ardrone_interface_task = px4_task_spawn_cmd("ardrone_interface", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 15, - 1100, - ardrone_interface_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 15, + 1100, + ardrone_interface_thread_main, + (argv) ? (char *const *)&argv[2] : (char *const *)NULL); exit(0); } @@ -138,9 +140,11 @@ int ardrone_interface_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { warnx("running"); + } else { warnx("not started"); } + exit(0); } @@ -212,19 +216,23 @@ int ardrone_interface_thread_main(int argc, char *argv[]) } if (strcmp(argv[i], "-m") == 0 || strcmp(argv[i], "--motor") == 0) { - if (i+1 < argc) { - int motor = atoi(argv[i+1]); + if (i + 1 < argc) { + int motor = atoi(argv[i + 1]); + if (motor > 0 && motor < 5) { test_motor = motor; + } else { thread_running = false; errx(1, "supply a motor # between 1 and 4. Example: -m 1\n %s", commandline_usage); } + } else { thread_running = false; errx(1, "missing parameter to -m 1..4\n %s", commandline_usage); } } + if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //device set if (argc > i + 1) { device = argv[i + 1]; @@ -314,6 +322,7 @@ int ardrone_interface_thread_main(int argc, char *argv[]) int motors[4] = {0, 0, 0, 0}; motors[test_motor - 1] = 10; ardrone_write_motor_commands(ardrone_write, motors[0], motors[1], motors[2], motors[3]); + } else { ardrone_write_motor_commands(ardrone_write, 10, 10, 10, 10); } @@ -338,33 +347,33 @@ int ardrone_interface_thread_main(int argc, char *argv[]) } if (counter % 24 == 0) { - if (led_counter == 0) ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 0); + if (led_counter == 0) { ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 0); } - if (led_counter == 1) ar_set_leds(ardrone_write, 1, 1, 0, 0, 0, 0, 0 , 0); + if (led_counter == 1) { ar_set_leds(ardrone_write, 1, 1, 0, 0, 0, 0, 0 , 0); } - if (led_counter == 2) ar_set_leds(ardrone_write, 1, 0, 0, 0, 0, 0, 0 , 0); + if (led_counter == 2) { ar_set_leds(ardrone_write, 1, 0, 0, 0, 0, 0, 0 , 0); } - if (led_counter == 3) ar_set_leds(ardrone_write, 0, 0, 0, 1, 0, 0, 0 , 0); + if (led_counter == 3) { ar_set_leds(ardrone_write, 0, 0, 0, 1, 0, 0, 0 , 0); } - if (led_counter == 4) ar_set_leds(ardrone_write, 0, 0, 1, 1, 0, 0, 0 , 0); + if (led_counter == 4) { ar_set_leds(ardrone_write, 0, 0, 1, 1, 0, 0, 0 , 0); } - if (led_counter == 5) ar_set_leds(ardrone_write, 0, 0, 1, 0, 0, 0, 0 , 0); + if (led_counter == 5) { ar_set_leds(ardrone_write, 0, 0, 1, 0, 0, 0, 0 , 0); } - if (led_counter == 6) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 1, 0 , 0); + if (led_counter == 6) { ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 1, 0 , 0); } - if (led_counter == 7) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 1, 0 , 0); + if (led_counter == 7) { ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 1, 0 , 0); } - if (led_counter == 8) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 0, 0 , 0); + if (led_counter == 8) { ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 0, 0 , 0); } - if (led_counter == 9) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 0 , 1); + if (led_counter == 9) { ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 0 , 1); } - if (led_counter == 10) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 1); + if (led_counter == 10) { ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 1); } - if (led_counter == 11) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 0); + if (led_counter == 11) { ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 0); } led_counter++; - if (led_counter == 12) led_counter = 0; + if (led_counter == 12) { led_counter = 0; } } /* run at approximately 200 Hz */ diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c index 46e1f86511..4991389129 100644 --- a/src/drivers/ardrone_interface/ardrone_motor_control.c +++ b/src/drivers/ardrone_interface/ardrone_motor_control.c @@ -108,7 +108,7 @@ void ar_enable_broadcast(int fd) int ar_multiplexing_init() { int fd; - + fd = open(PX4FMU_DEVICE_PATH, 0); if (fd < 0) { @@ -176,7 +176,7 @@ int ar_select_motor(int fd, uint8_t motor) ret += ioctl(fd, GPIO_CLEAR, motor_gpios); } else { - /* select reqested motor */ + /* select reqested motor */ ret += ioctl(fd, GPIO_CLEAR, motor_gpio[motor - 1]); } @@ -199,7 +199,7 @@ int ar_deselect_motor(int fd, uint8_t motor) ret += ioctl(fd, GPIO_SET, motor_gpios); } else { - /* deselect reqested motor */ + /* deselect reqested motor */ ret = ioctl(fd, GPIO_SET, motor_gpio[motor - 1]); } @@ -235,7 +235,7 @@ int ar_init_motors(int ardrone_uart, int gpios) */ write(ardrone_uart, &(initbuf[0]), 1); fsync(ardrone_uart); - usleep(UART_TRANSFER_TIME_BYTE_US*1); + usleep(UART_TRANSFER_TIME_BYTE_US * 1); /* * write 0x91 - request checksum @@ -243,7 +243,7 @@ int ar_init_motors(int ardrone_uart, int gpios) */ write(ardrone_uart, &(initbuf[1]), 1); fsync(ardrone_uart); - usleep(UART_TRANSFER_TIME_BYTE_US*120); + usleep(UART_TRANSFER_TIME_BYTE_US * 120); /* * write 0xA1 - set status OK @@ -252,7 +252,7 @@ int ar_init_motors(int ardrone_uart, int gpios) */ write(ardrone_uart, &(initbuf[2]), 1); fsync(ardrone_uart); - usleep(UART_TRANSFER_TIME_BYTE_US*1); + usleep(UART_TRANSFER_TIME_BYTE_US * 1); /* * set as motor i, where i = 1..4 @@ -268,7 +268,7 @@ int ar_init_motors(int ardrone_uart, int gpios) */ write(ardrone_uart, &(initbuf[4]), 1); fsync(ardrone_uart); - usleep(UART_TRANSFER_TIME_BYTE_US*11); + usleep(UART_TRANSFER_TIME_BYTE_US * 11); ar_deselect_motor(gpios, i); /* sleep 200 ms */ @@ -304,13 +304,15 @@ int ar_init_motors(int ardrone_uart, int gpios) warnx("Failed %d times", -errcounter); fflush(stdout); } + return errcounter; } /** * Sets the leds on the motor controllers, 1 turns led on, 0 off. */ -void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green) +void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, + uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green) { /* * 2 bytes are sent. The first 3 bits describe the command: 011 means led control @@ -322,12 +324,15 @@ void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t * 011 rrrr 0000 gggg 0 */ uint8_t leds[2]; - leds[0] = 0x60 | ((led4_red & 0x01) << 4) | ((led3_red & 0x01) << 3) | ((led2_red & 0x01) << 2) | ((led1_red & 0x01) << 1); - leds[1] = ((led4_green & 0x01) << 4) | ((led3_green & 0x01) << 3) | ((led2_green & 0x01) << 2) | ((led1_green & 0x01) << 1); + leds[0] = 0x60 | ((led4_red & 0x01) << 4) | ((led3_red & 0x01) << 3) | ((led2_red & 0x01) << 2) | (( + led1_red & 0x01) << 1); + leds[1] = ((led4_green & 0x01) << 4) | ((led3_green & 0x01) << 3) | ((led2_green & 0x01) << 2) | (( + led1_green & 0x01) << 1); write(ardrone_uart, leds, 2); } -int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4) { +int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4) +{ const unsigned int min_motor_interval = 4900; static uint64_t last_motor_time = 0; @@ -338,6 +343,7 @@ int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor outputs.output[2] = motor3; outputs.output[3] = motor4; static orb_advert_t pub = 0; + if (pub == 0) { /* advertise to channel 0 / primary */ pub = orb_advertise(ORB_ID(actuator_outputs), &outputs); @@ -355,15 +361,18 @@ int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor if (ret == sizeof(buf)) { return OK; + } else { return ret; } + } else { return -ERROR; } } -void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators) { +void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators) +{ float roll_control = actuators->control[0]; float pitch_control = actuators->control[1]; @@ -385,7 +394,8 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls /* linearly scale the control inputs from 0 to startpoint_full_control */ if (motor_thrust < startpoint_full_control) { - output_band = motor_thrust/startpoint_full_control; // linear from 0 to 1 + output_band = motor_thrust / startpoint_full_control; // linear from 0 to 1 + } else { output_band = 1.0f; } @@ -407,7 +417,7 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control); /* if one motor is saturated, reduce throttle */ - float saturation = fmaxf(fmaxf(motor_calc[0], motor_calc[1]),fmaxf(motor_calc[2], motor_calc[3])) - max_thrust; + float saturation = fmaxf(fmaxf(motor_calc[0], motor_calc[1]), fmaxf(motor_calc[2], motor_calc[3])) - max_thrust; if (saturation > 0.0f) { @@ -428,16 +438,16 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls /* set the motor values */ /* scale up from 0..1 to 10..500) */ - motor_pwm[0] = (uint16_t) (motor_calc[0] * ((float)max_gas - min_gas) + min_gas); - motor_pwm[1] = (uint16_t) (motor_calc[1] * ((float)max_gas - min_gas) + min_gas); - motor_pwm[2] = (uint16_t) (motor_calc[2] * ((float)max_gas - min_gas) + min_gas); - motor_pwm[3] = (uint16_t) (motor_calc[3] * ((float)max_gas - min_gas) + min_gas); + motor_pwm[0] = (uint16_t)(motor_calc[0] * ((float)max_gas - min_gas) + min_gas); + motor_pwm[1] = (uint16_t)(motor_calc[1] * ((float)max_gas - min_gas) + min_gas); + motor_pwm[2] = (uint16_t)(motor_calc[2] * ((float)max_gas - min_gas) + min_gas); + motor_pwm[3] = (uint16_t)(motor_calc[3] * ((float)max_gas - min_gas) + min_gas); /* scale up from 0..1 to 10..500) */ - motor_pwm[0] = (uint16_t) (motor_calc[0] * (float)((max_gas - min_gas) + min_gas)); - motor_pwm[1] = (uint16_t) (motor_calc[1] * (float)((max_gas - min_gas) + min_gas)); - motor_pwm[2] = (uint16_t) (motor_calc[2] * (float)((max_gas - min_gas) + min_gas)); - motor_pwm[3] = (uint16_t) (motor_calc[3] * (float)((max_gas - min_gas) + min_gas)); + motor_pwm[0] = (uint16_t)(motor_calc[0] * (float)((max_gas - min_gas) + min_gas)); + motor_pwm[1] = (uint16_t)(motor_calc[1] * (float)((max_gas - min_gas) + min_gas)); + motor_pwm[2] = (uint16_t)(motor_calc[2] * (float)((max_gas - min_gas) + min_gas)); + motor_pwm[3] = (uint16_t)(motor_calc[3] * (float)((max_gas - min_gas) + min_gas)); /* Failsafe logic for min values - should never be necessary */ motor_pwm[0] = (motor_pwm[0] > 0) ? motor_pwm[0] : min_gas; diff --git a/src/drivers/boards/aerocore/aerocore_init.c b/src/drivers/boards/aerocore/aerocore_init.c index 3bdb0d4238..83bc2f4234 100644 --- a/src/drivers/boards/aerocore/aerocore_init.c +++ b/src/drivers/boards/aerocore/aerocore_init.c @@ -117,12 +117,12 @@ __END_DECLS static GRAN_HANDLE dma_allocator; -/* - * The DMA heap size constrains the total number of things that can be +/* + * 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. + * 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). @@ -137,8 +137,10 @@ dma_alloc_init(void) sizeof(g_dma_heap), 7, /* 128B granule - must be > alignment (XXX bug?) */ 6); /* 64B alignment */ + if (dma_allocator == NULL) { message("[boot] DMA allocator setup FAILED"); + } else { g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations"); } @@ -262,11 +264,13 @@ __EXPORT int nsh_archinitialize(void) /* Configure Sensors on SPI bus #3 */ spi3 = up_spiinitialize(3); + if (!spi3) { message("[boot] FAILED to initialize SPI port 3\n"); up_ledon(LED_AMBER); return -ENODEV; } + /* Default: 1MHz, 8 bits, Mode 3 */ SPI_SETFREQUENCY(spi3, 10000000); SPI_SETBITS(spi3, 8); @@ -279,11 +283,13 @@ __EXPORT int nsh_archinitialize(void) /* Configure FRAM on SPI bus #4 */ spi4 = up_spiinitialize(4); + if (!spi4) { message("[boot] FAILED to initialize SPI port 4\n"); up_ledon(LED_AMBER); return -ENODEV; } + /* Default: ~10MHz, 8 bits, Mode 3 */ SPI_SETFREQUENCY(spi4, 10 * 1000 * 1000); SPI_SETBITS(spi4, 8); diff --git a/src/drivers/boards/aerocore/aerocore_led.c b/src/drivers/boards/aerocore/aerocore_led.c index 22b3a2c240..89c6367419 100644 --- a/src/drivers/boards/aerocore/aerocore_led.c +++ b/src/drivers/boards/aerocore/aerocore_led.c @@ -103,17 +103,23 @@ __EXPORT void led_toggle(int led) { switch (led) { case 0: - if (stm32_gpioread(GPIO_LED0)) + if (stm32_gpioread(GPIO_LED0)) { stm32_gpiowrite(GPIO_LED0, false); - else + + } else { stm32_gpiowrite(GPIO_LED0, true); + } + break; case 1: - if (stm32_gpioread(GPIO_LED1)) + if (stm32_gpioread(GPIO_LED1)) { stm32_gpiowrite(GPIO_LED1, false); - else + + } else { stm32_gpiowrite(GPIO_LED1, true); + } + break; default: diff --git a/src/drivers/boards/aerocore/aerocore_spi.c b/src/drivers/boards/aerocore/aerocore_spi.c index 1ce3f35f88..28103c843e 100644 --- a/src/drivers/boards/aerocore/aerocore_spi.c +++ b/src/drivers/boards/aerocore/aerocore_spi.c @@ -136,8 +136,8 @@ __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); + /* 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); break; diff --git a/src/drivers/boards/px4-stm32f4discovery/board_config.h b/src/drivers/boards/px4-stm32f4discovery/board_config.h index c4f469caa1..d06a3a317b 100644 --- a/src/drivers/boards/px4-stm32f4discovery/board_config.h +++ b/src/drivers/boards/px4-stm32f4discovery/board_config.h @@ -54,7 +54,7 @@ __BEGIN_DECLS #include #define UDID_START 0x1FFF7A10 - + /**************************************************************************************************** * Definitions ****************************************************************************************************/ @@ -66,13 +66,13 @@ __BEGIN_DECLS #define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\ - GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN12) + GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN12) #define GPIO_LED2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\ - GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13) + GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13) #define GPIO_LED3 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\ - GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) + GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) #define GPIO_LED4 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\ - GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN15) + GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN15) /* SPI chip selects */ #define GPIO_SPI_CS_MEMS (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) diff --git a/src/drivers/boards/px4-stm32f4discovery/px4discovery_led.c b/src/drivers/boards/px4-stm32f4discovery/px4discovery_led.c index 1f42843edf..6a6c6eaa0d 100644 --- a/src/drivers/boards/px4-stm32f4discovery/px4discovery_led.c +++ b/src/drivers/boards/px4-stm32f4discovery/px4discovery_led.c @@ -69,8 +69,7 @@ __EXPORT void led_init() __EXPORT void led_on(int led) { - if (led == 1) - { + if (led == 1) { /* Pull down to switch on */ stm32_gpiowrite(GPIO_LED1, false); } @@ -78,8 +77,7 @@ __EXPORT void led_on(int led) __EXPORT void led_off(int led) { - if (led == 1) - { + if (led == 1) { /* Pull up to switch off */ stm32_gpiowrite(GPIO_LED1, true); } @@ -87,11 +85,12 @@ __EXPORT void led_off(int led) __EXPORT void led_toggle(int led) { - if (led == 1) - { - if (stm32_gpioread(GPIO_LED1)) + if (led == 1) { + if (stm32_gpioread(GPIO_LED1)) { stm32_gpiowrite(GPIO_LED1, false); - else + + } else { stm32_gpiowrite(GPIO_LED1, true); + } } } diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_init.c b/src/drivers/boards/px4fmu-v1/px4fmu_init.c index b0e741017e..dea7678014 100644 --- a/src/drivers/boards/px4fmu-v1/px4fmu_init.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_init.c @@ -223,23 +223,23 @@ __EXPORT int nsh_archinitialize(void) * Keep the SPI2 init optional and conditionally initialize the ADC pins */ - #ifdef CONFIG_STM32_SPI2 - spi2 = up_spiinitialize(2); - /* Default SPI2 to 1MHz and de-assert the known chip selects. */ - SPI_SETFREQUENCY(spi2, 10000000); - SPI_SETBITS(spi2, 8); - SPI_SETMODE(spi2, SPIDEV_MODE3); - SPI_SELECT(spi2, PX4_SPIDEV_GYRO, false); - SPI_SELECT(spi2, PX4_SPIDEV_ACCEL_MAG, false); +#ifdef CONFIG_STM32_SPI2 + spi2 = up_spiinitialize(2); + /* Default SPI2 to 1MHz and de-assert the known chip selects. */ + SPI_SETFREQUENCY(spi2, 10000000); + SPI_SETBITS(spi2, 8); + SPI_SETMODE(spi2, SPIDEV_MODE3); + SPI_SELECT(spi2, PX4_SPIDEV_GYRO, false); + SPI_SELECT(spi2, PX4_SPIDEV_ACCEL_MAG, false); - message("[boot] Initialized SPI port2 (ADC IN12/13 blocked)\n"); - #else - 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 - #endif + message("[boot] Initialized SPI port2 (ADC IN12/13 blocked)\n"); +#else + 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 +#endif /* Get the SPI port for the microSD slot */ diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_led.c b/src/drivers/boards/px4fmu-v1/px4fmu_led.c index 888e4f551b..8a92cc6e3f 100644 --- a/src/drivers/boards/px4fmu-v1/px4fmu_led.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_led.c @@ -70,13 +70,12 @@ __EXPORT void led_init(void) __EXPORT void led_on(int led) { - if (led == 0) - { + if (led == 0) { /* Pull down to switch on */ stm32_gpiowrite(GPIO_LED1, false); } - if (led == 1) - { + + if (led == 1) { /* Pull down to switch on */ stm32_gpiowrite(GPIO_LED2, false); } @@ -84,13 +83,12 @@ __EXPORT void led_on(int led) __EXPORT void led_off(int led) { - if (led == 0) - { + if (led == 0) { /* Pull up to switch off */ stm32_gpiowrite(GPIO_LED1, true); } - if (led == 1) - { + + if (led == 1) { /* Pull up to switch off */ stm32_gpiowrite(GPIO_LED2, true); } @@ -98,18 +96,21 @@ __EXPORT void led_off(int led) __EXPORT void led_toggle(int led) { - if (led == 0) - { - if (stm32_gpioread(GPIO_LED1)) + if (led == 0) { + if (stm32_gpioread(GPIO_LED1)) { stm32_gpiowrite(GPIO_LED1, false); - else + + } else { stm32_gpiowrite(GPIO_LED1, true); + } } - if (led == 1) - { - if (stm32_gpioread(GPIO_LED2)) + + if (led == 1) { + if (stm32_gpioread(GPIO_LED2)) { stm32_gpiowrite(GPIO_LED2, false); - else + + } else { stm32_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 2a444796ff..e877a6dff4 100644 --- a/src/drivers/boards/px4fmu-v1/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_spi.c @@ -90,8 +90,8 @@ __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); + /* 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); break; diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c index e4f32b34c6..116a6ff556 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -118,12 +118,12 @@ __END_DECLS static GRAN_HANDLE dma_allocator; -/* - * The DMA heap size constrains the total number of things that can be +/* + * 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. + * 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). @@ -138,8 +138,10 @@ dma_alloc_init(void) sizeof(g_dma_heap), 7, /* 128B granule - must be > alignment (XXX bug?) */ 6); /* 64B alignment */ + if (dma_allocator == NULL) { message("[boot] DMA allocator setup FAILED"); + } else { g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations"); } @@ -214,12 +216,12 @@ static struct sdio_dev_s *sdio; /*#ifdef __cplusplus*/ /*__EXPORT int matherr(struct __exception *e)*/ /*{*/ - /*return 1;*/ +/*return 1;*/ /*}*/ /*#else*/ /*__EXPORT int matherr(struct exception *e)*/ /*{*/ - /*return 1;*/ +/*return 1;*/ /*}*/ /*#endif*/ @@ -326,10 +328,11 @@ __EXPORT int nsh_archinitialize(void) SPI_SELECT(spi4, PX4_SPIDEV_EXT0, false); SPI_SELECT(spi4, PX4_SPIDEV_EXT1, false); - #ifdef CONFIG_MMCSD +#ifdef CONFIG_MMCSD /* First, get an instance of the SDIO interface */ sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO); + if (!sdio) { message("[boot] Failed to initialize SDIO slot %d\n", CONFIG_NSH_MMCSDSLOTNO); @@ -338,6 +341,7 @@ __EXPORT int nsh_archinitialize(void) /* Now bind the SDIO interface to the MMC/SD driver */ int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio); + if (ret != OK) { message("[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret); return ret; @@ -346,7 +350,7 @@ __EXPORT int nsh_archinitialize(void) /* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */ sdio_mediachange(sdio, true); - #endif +#endif return OK; } diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_led.c b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c index 74f77b3ef6..09edb2ba66 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_led.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c @@ -69,8 +69,7 @@ __EXPORT void led_init() __EXPORT void led_on(int led) { - if (led == 1) - { + if (led == 1) { /* Pull down to switch on */ stm32_gpiowrite(GPIO_LED1, false); } @@ -78,8 +77,7 @@ __EXPORT void led_on(int led) __EXPORT void led_off(int led) { - if (led == 1) - { + if (led == 1) { /* Pull up to switch off */ stm32_gpiowrite(GPIO_LED1, true); } @@ -87,11 +85,12 @@ __EXPORT void led_off(int led) __EXPORT void led_toggle(int led) { - if (led == 1) - { - if (stm32_gpioread(GPIO_LED1)) + if (led == 1) { + if (stm32_gpioread(GPIO_LED1)) { stm32_gpiowrite(GPIO_LED1, false); - else + + } else { stm32_gpiowrite(GPIO_LED1, true); + } } } diff --git a/src/drivers/boards/px4io-v1/board_config.h b/src/drivers/boards/px4io-v1/board_config.h index 59c17431c7..e05e030713 100644 --- a/src/drivers/boards/px4io-v1/board_config.h +++ b/src/drivers/boards/px4io-v1/board_config.h @@ -30,10 +30,10 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ - + /** * @file board_config.h - * + * * PX4IO hardware definitions. */ @@ -59,11 +59,11 @@ /* LEDs */ #define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\ - GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14) + GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14) #define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\ - GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15) + GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15) #define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\ - GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) + GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) /* Safety switch button *************************************************************/ @@ -86,7 +86,7 @@ #define GPIO_ADC_VBATT (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4) #define GPIO_ADC_IN5 (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5) -/* +/* * High-resolution timer */ #define HRT_TIMER 1 /* use timer1 for the HRT */ diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp index a75516b716..d07ec24684 100644 --- a/src/drivers/camera_trigger/camera_trigger.cpp +++ b/src/drivers/camera_trigger/camera_trigger.cpp @@ -195,6 +195,7 @@ CameraTrigger::CameraTrigger() : // Convert number to individual channels unsigned i = 0; int single_pin; + while ((single_pin = pin_list % 10)) { _pins[i] = single_pin - 1; @@ -226,18 +227,19 @@ CameraTrigger::control(bool on) if (on) { // schedule trigger on and off calls hrt_call_every(&_engagecall, 500, (_interval * 1000), - (hrt_callout)&CameraTrigger::engage, this); + (hrt_callout)&CameraTrigger::engage, this); // schedule trigger on and off calls hrt_call_every(&_disengagecall, 500 + (_activation_time * 1000), (_interval * 1000), - (hrt_callout)&CameraTrigger::disengage, this); + (hrt_callout)&CameraTrigger::disengage, this); + } else { // cancel all calls hrt_cancel(&_engagecall); hrt_cancel(&_disengagecall); // ensure that the pin is off hrt_call_after(&_disengagecall, 500, - (hrt_callout)&CameraTrigger::disengage, this); + (hrt_callout)&CameraTrigger::disengage, this); } _trigger_enabled = on; @@ -249,7 +251,7 @@ CameraTrigger::start() for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) { stm32_configgpio(_gpios[_pins[i]]); - stm32_gpiowrite(_gpios[_pins[i]], !_polarity); + stm32_gpiowrite(_gpios[_pins[i]], !_polarity); } // enable immediate if configured that way @@ -316,7 +318,7 @@ CameraTrigger::cycle_trampoline(void *arg) } work_queue(LPWORK, &_work, (worker_t)&CameraTrigger::cycle_trampoline, - camera_trigger::g_camera_trigger, USEC2TICK(poll_interval_usec)); + camera_trigger::g_camera_trigger, USEC2TICK(poll_interval_usec)); } void @@ -361,7 +363,7 @@ 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"); + _polarity ? "ACTIVE_HIGH" : "ACTIVE_LOW"); warnx("interval : %.2f", (double)_interval); } diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp index 048357ebc8..c43ab1353e 100644 --- a/src/drivers/device/cdev.cpp +++ b/src/drivers/device/cdev.cpp @@ -100,14 +100,16 @@ CDev::CDev(const char *name, _registered(false), _open_count(0) { - for (unsigned i = 0; i < _max_pollwaiters; i++) + for (unsigned i = 0; i < _max_pollwaiters; i++) { _pollset[i] = nullptr; + } } CDev::~CDev() { - if (_registered) + if (_registered) { unregister_driver(_devname); + } } int @@ -124,13 +126,16 @@ CDev::register_class_devname(const char *class_devname) char name[32]; snprintf(name, sizeof(name), "%s%d", class_devname, class_instance); ret = register_driver(name, &fops, 0666, (void *)this); - if (ret == OK) break; + + if (ret == OK) { break; } + class_instance++; } if (class_instance == 4) { return ret; } + return class_instance; } @@ -148,15 +153,17 @@ CDev::init() // base class init first int ret = Device::init(); - if (ret != OK) + if (ret != OK) { goto out; + } // now register the driver if (_devname != nullptr) { ret = register_driver(_devname, &fops, 0666, (void *)this); - if (ret != OK) + if (ret != OK) { goto out; + } _registered = true; } @@ -182,8 +189,9 @@ CDev::open(file_t *filp) /* first-open callback may decline the open */ ret = open_first(filp); - if (ret != OK) + if (ret != OK) { _open_count--; + } } unlock(); @@ -209,8 +217,9 @@ CDev::close(file_t *filp) _open_count--; /* callback cannot decline the close */ - if (_open_count == 0) + if (_open_count == 0) { ret = close_last(filp); + } } else { ret = -EBADF; @@ -250,26 +259,30 @@ CDev::ioctl(file_t *filp, int cmd, unsigned long arg) { switch (cmd) { - /* fetch a pointer to the driver's private data */ + /* fetch a pointer to the driver's private data */ case DIOC_GETPRIV: *(void **)(uintptr_t)arg = (void *)this; return OK; break; + case DEVIOCSPUBBLOCK: _pub_blocked = (arg != 0); return OK; break; + case DEVIOCGPUBBLOCK: return _pub_blocked; break; } /* try the superclass. The different ioctl() function form - * means we need to copy arg */ - unsigned arg2 = arg; + * means we need to copy arg */ + unsigned arg2 = arg; int ret = Device::ioctl(cmd, arg2); - if (ret != -ENODEV) + + if (ret != -ENODEV) { return ret; + } return -ENOTTY; } @@ -305,8 +318,9 @@ CDev::poll(file_t *filp, struct pollfd *fds, bool setup) fds->revents |= fds->events & poll_state(filp); /* yes? post the notification */ - if (fds->revents != 0) + if (fds->revents != 0) { px4_sem_post(fds->sem); + } } } else { @@ -328,8 +342,9 @@ CDev::poll_notify(pollevent_t events) irqstate_t state = irqsave(); for (unsigned i = 0; i < _max_pollwaiters; i++) - if (nullptr != _pollset[i]) + if (nullptr != _pollset[i]) { poll_notify_one(_pollset[i], events); + } irqrestore(state); } @@ -342,8 +357,9 @@ CDev::poll_notify_one(struct pollfd *fds, pollevent_t events) /* if the state is now interesting, wake the waiter if it's still asleep */ /* XXX semcount check here is a vile hack; counting semphores should not be abused as cvars */ - if ((fds->revents != 0) && (fds->sem->semcount <= 0)) + if ((fds->revents != 0) && (fds->sem->semcount <= 0)) { px4_sem_post(fds->sem); + } } pollevent_t diff --git a/src/drivers/device/device_nuttx.cpp b/src/drivers/device/device_nuttx.cpp index 9146acb5da..12d42d5bfe 100644 --- a/src/drivers/device/device_nuttx.cpp +++ b/src/drivers/device/device_nuttx.cpp @@ -95,7 +95,7 @@ Device::Device(const char *name, _irq_attached(false) { sem_init(&_lock, 0, 1); - + /* setup a default device ID. When bus_type is UNKNOWN the other fields are invalid */ _device_id.devid = 0; @@ -109,8 +109,9 @@ Device::~Device() { sem_destroy(&_lock); - if (_irq_attached) + if (_irq_attached) { unregister_interrupt(_irq); + } } int @@ -126,8 +127,9 @@ Device::init() /* register */ ret = register_interrupt(_irq, this); - if (ret != OK) + if (ret != OK) { goto out; + } _irq_attached = true; } @@ -139,15 +141,17 @@ out: void Device::interrupt_enable() { - if (_irq_attached) + if (_irq_attached) { up_enable_irq(_irq); + } } void Device::interrupt_disable() { - if (_irq_attached) + if (_irq_attached) { up_disable_irq(_irq); + } } void @@ -224,6 +228,7 @@ Device::ioctl(unsigned operation, unsigned &arg) case DEVIOCGDEVICEID: return (int)_device_id.devid; } + return -ENODEV; } diff --git a/src/drivers/device/device_nuttx.h b/src/drivers/device/device_nuttx.h index 974d94ee45..c8f732fab1 100644 --- a/src/drivers/device/device_nuttx.h +++ b/src/drivers/device/device_nuttx.h @@ -118,7 +118,7 @@ public: * @param data The buffer from which values should be read. * @param count The number of items to write. * @return The number of items written on success, negative errno otherwise. - */ + */ virtual int write(unsigned address, void *data, unsigned count); /** @@ -147,13 +147,13 @@ public: parameter protocol without loss of information. */ struct DeviceStructure { - enum DeviceBusType bus_type:3; - uint8_t bus:5; // which instance of the bus type - uint8_t address; // address on the bus (eg. I2C address) - uint8_t devtype; // device class specific device type - }; + enum DeviceBusType bus_type : 3; + uint8_t bus: 5; // which instance of the bus type + uint8_t address; // address on the bus (eg. I2C address) + uint8_t devtype; // device class specific device type + }; - union DeviceId { + union DeviceId { struct DeviceStructure devid_s; uint32_t devid; }; @@ -189,14 +189,16 @@ protected: * * Note that we must loop as the wait may be interrupted by a signal. */ - void lock() { + void lock() + { do {} while (sem_wait(&_lock) != 0); } /** * Release the driver lock. */ - void unlock() { + void unlock() + { sem_post(&_lock); } @@ -416,7 +418,7 @@ protected: */ virtual int close_last(file_t *filp); - /** + /** * Register a class device name, automatically adding device * class instance suffix if need be. * @@ -425,7 +427,7 @@ protected: */ virtual int register_class_devname(const char *class_devname); - /** + /** * Register a class device name, automatically adding device * class instance suffix if need be. * @@ -440,7 +442,7 @@ protected: * * @return the file system string of the device handle */ - const char* get_devname() { return _devname; } + const char *get_devname() { return _devname; } bool _pub_blocked; /**< true if publishing should be blocked */ @@ -470,8 +472,8 @@ private: int remove_poll_waiter(struct pollfd *fds); /* do not allow copying this class */ - CDev(const CDev&); - CDev operator=(const CDev&); + CDev(const CDev &); + CDev operator=(const CDev &); }; /** @@ -503,7 +505,8 @@ protected: * * @param offset Register offset in bytes from the base address. */ - uint32_t reg(uint32_t offset) { + uint32_t reg(uint32_t offset) + { return *(volatile uint32_t *)(_base + offset); } @@ -513,7 +516,8 @@ protected: * @param offset Register offset in bytes from the base address. * @param value Value to write. */ - void reg(uint32_t offset, uint32_t value) { + void reg(uint32_t offset, uint32_t value) + { *(volatile uint32_t *)(_base + offset) = value; } @@ -527,7 +531,8 @@ protected: * @param clearbits Bits to clear in the register * @param setbits Bits to set in the register */ - void modify(uint32_t offset, uint32_t clearbits, uint32_t setbits) { + void modify(uint32_t offset, uint32_t clearbits, uint32_t setbits) + { uint32_t val = reg(offset); val &= ~clearbits; val |= setbits; @@ -542,9 +547,9 @@ private: // class instance for primary driver of each class enum CLASS_DEVICE { - CLASS_DEVICE_PRIMARY=0, - CLASS_DEVICE_SECONDARY=1, - CLASS_DEVICE_TERTIARY=2 + CLASS_DEVICE_PRIMARY = 0, + CLASS_DEVICE_SECONDARY = 1, + CLASS_DEVICE_TERTIARY = 2 }; #endif /* _DEVICE_DEVICE_H */ diff --git a/src/drivers/device/device_posix.cpp b/src/drivers/device/device_posix.cpp index bed97ff56d..088d7ccdf8 100644 --- a/src/drivers/device/device_posix.cpp +++ b/src/drivers/device/device_posix.cpp @@ -60,7 +60,7 @@ Device::Device(const char *name) : if (ret != 0) { PX4_WARN("SEM INIT FAIL: ret %d, %s", ret, strerror(errno)); } - + /* setup a default device ID. When bus_type is UNKNOWN the other fields are invalid */ _device_id.devid = 0; @@ -86,23 +86,24 @@ Device::init() int Device::dev_read(unsigned offset, void *data, unsigned count) { - return -ENODEV; + return -ENODEV; } int Device::dev_write(unsigned offset, void *data, unsigned count) { - return -ENODEV; + return -ENODEV; } int Device::dev_ioctl(unsigned operation, unsigned &arg) { - switch (operation) { - case DEVIOCGDEVICEID: - return (int)_device_id.devid; - } - return -ENODEV; + switch (operation) { + case DEVIOCGDEVICEID: + return (int)_device_id.devid; + } + + return -ENODEV; } } // namespace device diff --git a/src/drivers/device/i2c_nuttx.cpp b/src/drivers/device/i2c_nuttx.cpp index ad74f60c47..5efd2eac23 100644 --- a/src/drivers/device/i2c_nuttx.cpp +++ b/src/drivers/device/i2c_nuttx.cpp @@ -69,7 +69,7 @@ I2C::I2C(const char *name, _device_id.devid_s.bus = bus; _device_id.devid_s.address = address; // devtype needs to be filled in by the driver - _device_id.devid_s.devtype = 0; + _device_id.devid_s.devtype = 0; } I2C::~I2C() @@ -92,6 +92,7 @@ I2C::set_bus_clock(unsigned bus, unsigned clock_hz) if (_bus_clocks[index] > 0) { // DEVICE_DEBUG("overriding clock of %u with %u Hz\n", _bus_clocks[index], clock_hz); } + _bus_clocks[index] = clock_hz; return OK; @@ -122,7 +123,7 @@ I2C::init() (void)up_i2cuninitialize(_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); + _bus, _bus_clocks[bus_index] / 1000, _frequency / 1000); ret = -EINVAL; goto out; } @@ -162,13 +163,15 @@ I2C::init() // tell the world where we are DEVICE_LOG("on I2C bus %d at 0x%02x (bus: %u KHz, max: %u KHz)", - _bus, _address, _bus_clocks[bus_index] / 1000, _frequency / 1000); + _bus, _address, _bus_clocks[bus_index] / 1000, _frequency / 1000); out: + if ((ret != OK) && (_dev != nullptr)) { up_i2cuninitialize(_dev); _dev = nullptr; } + return ret; } @@ -208,18 +211,21 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re msgs++; } - if (msgs == 0) + if (msgs == 0) { return -EINVAL; + } ret = I2C_TRANSFER(_dev, &msgv[0], msgs); /* success */ - if (ret == OK) + if (ret == OK) { break; + } /* if we have already retried once, or we are going to give up, then reset the bus */ - if ((retry_count >= 1) || (retry_count >= _retries)) + if ((retry_count >= 1) || (retry_count >= _retries)) { up_i2creset(_dev); + } } while (retry_count++ < _retries); @@ -234,20 +240,23 @@ I2C::transfer(i2c_msg_s *msgv, unsigned msgs) unsigned retry_count = 0; /* force the device address into the message vector */ - for (unsigned i = 0; i < msgs; i++) + for (unsigned i = 0; i < msgs; i++) { msgv[i].addr = _address; + } do { ret = I2C_TRANSFER(_dev, msgv, msgs); /* success */ - if (ret == OK) + if (ret == OK) { break; + } /* if we have already retried once, or we are going to give up, then reset the bus */ - if ((retry_count >= 1) || (retry_count >= _retries)) + if ((retry_count >= 1) || (retry_count >= _retries)) { up_i2creset(_dev); + } } while (retry_count++ < _retries); diff --git a/src/drivers/device/i2c_nuttx.h b/src/drivers/device/i2c_nuttx.h index 97ab25672c..f4aa608857 100644 --- a/src/drivers/device/i2c_nuttx.h +++ b/src/drivers/device/i2c_nuttx.h @@ -134,7 +134,8 @@ protected: * * @param address The new bus address to set. */ - void set_address(uint16_t address) { + void set_address(uint16_t address) + { _address = address; _device_id.devid_s.address = _address; } diff --git a/src/drivers/device/i2c_posix.cpp b/src/drivers/device/i2c_posix.cpp index acd35c4cce..48bb3529e0 100644 --- a/src/drivers/device/i2c_posix.cpp +++ b/src/drivers/device/i2c_posix.cpp @@ -75,7 +75,7 @@ I2C::I2C(const char *name, _device_id.devid_s.bus = bus; _device_id.devid_s.address = address; // devtype needs to be filled in by the driver - _device_id.devid_s.devtype = 0; + _device_id.devid_s.devtype = 0; } I2C::~I2C() @@ -105,6 +105,7 @@ I2C::init() } _fd = px4_open(get_devname(), PX4_F_RDONLY | PX4_F_WRONLY); + if (_fd < 0) { DEVICE_DEBUG("px4_open failed of device %s", get_devname()); return PX4_ERROR; @@ -116,16 +117,18 @@ I2C::init() if (simulate) { _fd = 10000; - } - else { + + } else { #ifndef __PX4_QURT // Open the actual I2C device and map to the virtual dev name _fd = ::open(get_devname(), O_RDWR); + if (_fd < 0) { warnx("could not open %s", get_devname()); px4_errno = errno; return PX4_ERROR; } + #endif } @@ -135,9 +138,9 @@ I2C::init() int I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) { - #ifndef __PX4_LINUX +#ifndef __PX4_LINUX return 1; - #else +#else struct i2c_msg msgv[2]; unsigned msgs; struct i2c_rdwr_ioctl_data packets; @@ -145,7 +148,7 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re unsigned retry_count = 0; if (_fd < 0) { - warnx("I2C device not opened"); + warnx("I2C device not opened"); return 1; } @@ -169,8 +172,9 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re msgs++; } - if (msgs == 0) + if (msgs == 0) { return -EINVAL; + } packets.msgs = msgv; packets.nmsgs = msgs; @@ -178,9 +182,10 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re if (simulate) { //warnx("I2C SIM: transfer_4 on %s", get_devname()); ret = PX4_OK; - } - else { + + } else { ret = ::ioctl(_fd, I2C_RDWR, (unsigned long)&packets); + if (ret < 0) { warnx("I2C transfer failed"); return 1; @@ -188,28 +193,30 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re } /* success */ - if (ret == PX4_OK) + if (ret == PX4_OK) { break; + } } while (retry_count++ < _retries); return ret; - #endif +#endif } int I2C::transfer(struct i2c_msg *msgv, unsigned msgs) { - #ifndef __PX4_LINUX +#ifndef __PX4_LINUX return 1; - #else +#else struct i2c_rdwr_ioctl_data packets; int ret; unsigned retry_count = 0; /* force the device address into the message vector */ - for (unsigned i = 0; i < msgs; i++) + for (unsigned i = 0; i < msgs; i++) { msgv[i].addr = _address; + } do { packets.msgs = msgv; @@ -218,34 +225,38 @@ I2C::transfer(struct i2c_msg *msgv, unsigned msgs) if (simulate) { warnx("I2C SIM: transfer_2 on %s", get_devname()); ret = PX4_OK; - } - else { + + } else { ret = ::ioctl(_fd, I2C_RDWR, (unsigned long)&packets); } + if (ret < 0) { - warnx("I2C transfer failed"); - return 1; - } + warnx("I2C transfer failed"); + return 1; + } /* success */ - if (ret == PX4_OK) + if (ret == PX4_OK) { break; + } } while (retry_count++ < _retries); return ret; - #endif +#endif } int I2C::ioctl(device::file_t *filp, int cmd, unsigned long arg) { //struct i2c_rdwr_ioctl_data *packets = (i2c_rdwr_ioctl_data *)(void *)arg; switch (cmd) { - #ifdef __PX4_LINUX +#ifdef __PX4_LINUX + case I2C_RDWR: - warnx("Use I2C::transfer, not ioctl"); + warnx("Use I2C::transfer, not ioctl"); return 0; - #endif +#endif + default: /* give it to the superclass */ return VDev::ioctl(filp, cmd, arg); @@ -256,27 +267,28 @@ ssize_t I2C::read(file_t *filp, char *buffer, size_t buflen) { if (simulate) { // FIXME no idea what this should be - warnx ("2C SIM I2C::read"); + warnx("2C SIM I2C::read"); return 0; } + #ifndef __PX4_QURT return ::read(_fd, buffer, buflen); #else - return 0; + return 0; #endif } ssize_t I2C::write(file_t *filp, const char *buffer, size_t buflen) { if (simulate) { - warnx ("2C SIM I2C::write"); + warnx("2C SIM I2C::write"); return buflen; } #ifndef __PX4_QURT return ::write(_fd, buffer, buflen); #else - return buflen; + return buflen; #endif } diff --git a/src/drivers/device/i2c_posix.h b/src/drivers/device/i2c_posix.h index c05100ae43..bee8d6a38a 100644 --- a/src/drivers/device/i2c_posix.h +++ b/src/drivers/device/i2c_posix.h @@ -127,8 +127,8 @@ private: int _fd; std::string _dname; - I2C(const device::I2C&); - I2C operator=(const device::I2C&); + I2C(const device::I2C &); + I2C operator=(const device::I2C &); }; } // namespace device diff --git a/src/drivers/device/integrator.cpp b/src/drivers/device/integrator.cpp index bc2baacc85..d6bdfbf5a6 100644 --- a/src/drivers/device/integrator.cpp +++ b/src/drivers/device/integrator.cpp @@ -82,7 +82,7 @@ Integrator::put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integ // 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 + // Available: http://www.sage.unsw.edu.au/snap/publications/tian_etal2010b.pdf i += ((_integral_auto + _last_delta * (1.0f / 6.0f)) % i) * 0.5f; } @@ -117,6 +117,7 @@ math::Vector<3> Integrator::read(bool auto_reset) { math::Vector<3> val = _integral_read; + if (auto_reset) { _integral_read(0) = 0.0f; _integral_read(1) = 0.0f; diff --git a/src/drivers/device/integrator.h b/src/drivers/device/integrator.h index 6de5c942d9..ace96af2e2 100644 --- a/src/drivers/device/integrator.h +++ b/src/drivers/device/integrator.h @@ -43,7 +43,8 @@ #include -class Integrator { +class Integrator +{ public: Integrator(uint64_t auto_reset_interval = 4000 /* 250 Hz */, bool coning_compensation = false); virtual ~Integrator(); @@ -91,6 +92,6 @@ private: bool _coning_comp_on; /**< coning compensation */ /* we don't want this class to be copied */ - Integrator(const Integrator&); - Integrator operator=(const Integrator&); + Integrator(const Integrator &); + Integrator operator=(const Integrator &); }; diff --git a/src/drivers/device/ringbuffer.cpp b/src/drivers/device/ringbuffer.cpp index c30e0a54df..5843312915 100644 --- a/src/drivers/device/ringbuffer.cpp +++ b/src/drivers/device/ringbuffer.cpp @@ -46,15 +46,16 @@ namespace ringbuffer RingBuffer::RingBuffer(unsigned num_items, size_t item_size) : _num_items(num_items), _item_size(item_size), - _buf(new char[(_num_items+1) * item_size]), - _head(_num_items), - _tail(_num_items) + _buf(new char[(_num_items + 1) * item_size]), + _head(_num_items), + _tail(_num_items) {} RingBuffer::~RingBuffer() { - if (_buf != nullptr) + if (_buf != nullptr) { delete[] _buf; + } } unsigned @@ -84,20 +85,25 @@ RingBuffer::size() void RingBuffer::flush() { - while (!empty()) + while (!empty()) { get(NULL); + } } bool RingBuffer::put(const void *val, size_t val_size) { unsigned next = _next(_head); + if (next != _tail) { - if ((val_size == 0) || (val_size > _item_size)) + if ((val_size == 0) || (val_size > _item_size)) { val_size = _item_size; + } + memcpy(&_buf[_head * _item_size], val, val_size); _head = next; return true; + } else { return false; } @@ -169,11 +175,14 @@ RingBuffer::force(const void *val, size_t val_size) bool overwrote = false; for (;;) { - if (put(val, val_size)) + if (put(val, val_size)) { break; + } + get(NULL); overwrote = true; } + return overwrote; } @@ -246,6 +255,7 @@ static inline bool my_sync_bool_compare_and_swap(volatile unsigned *a, unsigned *a = c; return true; } + return false; } @@ -259,8 +269,9 @@ RingBuffer::get(void *val, size_t val_size) unsigned candidate; unsigned next; - if ((val_size == 0) || (val_size > _item_size)) + if ((val_size == 0) || (val_size > _item_size)) { val_size = _item_size; + } do { /* decide which element we think we're going to read */ @@ -270,13 +281,15 @@ RingBuffer::get(void *val, size_t val_size) next = _next(candidate); /* go ahead and read from this index */ - if (val != NULL) + if (val != NULL) { memcpy(val, &_buf[candidate * _item_size], val_size); + } /* if the tail pointer didn't change, we got our item */ } while (!__PX4_SBCAP(&_tail, candidate, next)); return true; + } else { return false; } @@ -377,10 +390,12 @@ bool RingBuffer::resize(unsigned new_size) { char *old_buffer; - char *new_buffer = new char [(new_size+1) * _item_size]; + char *new_buffer = new char [(new_size + 1) * _item_size]; + if (new_buffer == nullptr) { return false; } + old_buffer = _buf; _buf = new_buffer; _num_items = new_size; diff --git a/src/drivers/device/ringbuffer.h b/src/drivers/device/ringbuffer.h index 4fcdaf47fa..32077edb64 100644 --- a/src/drivers/device/ringbuffer.h +++ b/src/drivers/device/ringbuffer.h @@ -46,7 +46,8 @@ namespace ringbuffer __EXPORT { -class RingBuffer { +class RingBuffer +{ public: RingBuffer(unsigned ring_size, size_t entry_size); virtual ~RingBuffer(); @@ -171,8 +172,8 @@ private: unsigned _next(unsigned index); /* we don't want this class to be copied */ - RingBuffer(const RingBuffer&); - RingBuffer operator=(const RingBuffer&); + RingBuffer(const RingBuffer &); + RingBuffer operator=(const RingBuffer &); }; } // namespace ringbuffer diff --git a/src/drivers/device/sim.cpp b/src/drivers/device/sim.cpp index 787a3826c9..5b089af935 100644 --- a/src/drivers/device/sim.cpp +++ b/src/drivers/device/sim.cpp @@ -69,7 +69,7 @@ SIM::SIM(const char *name, _device_id.devid_s.bus = bus; _device_id.devid_s.address = address; // devtype needs to be filled in by the driver - _device_id.devid_s.devtype = 0; + _device_id.devid_s.devtype = 0; } SIM::~SIM() @@ -104,7 +104,7 @@ SIM::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re if (recv_len > 0) { PX4_DEBUG("SIM: receiving %d bytes", recv_len); - + // TODO - write data to recv; } diff --git a/src/drivers/device/sim.h b/src/drivers/device/sim.h index 139967f6e8..1e84af3863 100644 --- a/src/drivers/device/sim.h +++ b/src/drivers/device/sim.h @@ -37,7 +37,7 @@ * Base class for devices on simulation bus. */ -#pragma once +#pragma once #include "vdev.h" @@ -98,14 +98,14 @@ protected: * otherwise. */ virtual int transfer(const uint8_t *send, unsigned send_len, - uint8_t *recv, unsigned recv_len); + uint8_t *recv, unsigned recv_len); private: uint16_t _address; - const char * _devname; + const char *_devname; - SIM(const device::SIM&); - SIM operator=(const device::SIM&); + SIM(const device::SIM &); + SIM operator=(const device::SIM &); }; } // namespace device diff --git a/src/drivers/device/spi.cpp b/src/drivers/device/spi.cpp index 698fa63996..ce81878102 100644 --- a/src/drivers/device/spi.cpp +++ b/src/drivers/device/spi.cpp @@ -80,7 +80,7 @@ SPI::SPI(const char *name, _device_id.devid_s.bus = bus; _device_id.devid_s.address = (uint8_t)device; // devtype needs to be filled in by the driver - _device_id.devid_s.devtype = 0; + _device_id.devid_s.devtype = 0; } SPI::~SPI() @@ -94,8 +94,9 @@ SPI::init() int ret = OK; /* attach to the spi bus */ - if (_dev == nullptr) + if (_dev == nullptr) { _dev = up_spiinitialize(_bus); + } if (_dev == nullptr) { DEVICE_DEBUG("failed to init SPI"); @@ -141,34 +142,37 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len) { int result; - if ((send == nullptr) && (recv == nullptr)) + if ((send == nullptr) && (recv == nullptr)) { return -EINVAL; + } LockMode mode = up_interrupt_context() ? LOCK_NONE : locking_mode; /* lock the bus as required */ switch (mode) { default: - case LOCK_PREEMPTION: - { + case LOCK_PREEMPTION: { irqstate_t state = irqsave(); result = _transfer(send, recv, len); irqrestore(state); } break; + case LOCK_THREADS: SPI_LOCK(_dev, true); result = _transfer(send, recv, len); SPI_LOCK(_dev, false); break; + case LOCK_NONE: result = _transfer(send, recv, len); break; } + return result; } -void +void SPI::set_frequency(uint32_t frequency) { _frequency = frequency; diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h index 2f44f3cafa..9c3bf36f25 100644 --- a/src/drivers/device/spi.h +++ b/src/drivers/device/spi.h @@ -109,12 +109,12 @@ protected: * Set the SPI bus frequency * This is used to change frequency on the fly. Some sensors * (such as the MPU6000) need a lower frequency for setup - * registers and can handle higher frequency for sensor + * registers and can handle higher frequency for sensor * value registers * * @param frequency Frequency to set (Hz) */ - void set_frequency(uint32_t frequency); + void set_frequency(uint32_t frequency); /** * Locking modes supported by the driver. @@ -134,8 +134,8 @@ private: struct spi_dev_s *_dev; /* this class does not allow copying */ - SPI(const SPI&); - SPI operator=(const SPI&); + SPI(const SPI &); + SPI operator=(const SPI &); protected: int _bus; diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 1900f19fbe..60761ed511 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -54,8 +54,9 @@ struct px4_dev_t { char *name; void *cdev; - px4_dev_t(const char *n, void *c) : cdev(c) { - name = strdup(n); + px4_dev_t(const char *n, void *c) : cdev(c) + { + name = strdup(n); } ~px4_dev_t() { free(name); } @@ -85,21 +86,26 @@ VDev::VDev(const char *name, _open_count(0) { PX4_DEBUG("VDev::VDev"); - for (unsigned i = 0; i < _max_pollwaiters; i++) + + for (unsigned i = 0; i < _max_pollwaiters; i++) { _pollset[i] = nullptr; + } } VDev::~VDev() { PX4_DEBUG("VDev::~VDev"); - if (_registered) + + if (_registered) { unregister_driver(_devname); + } } int VDev::register_class_devname(const char *class_devname) { PX4_DEBUG("VDev::register_class_devname %s", class_devname); + if (class_devname == nullptr) { return -EINVAL; } @@ -111,13 +117,16 @@ VDev::register_class_devname(const char *class_devname) char name[32]; snprintf(name, sizeof(name), "%s%d", class_devname, class_instance); ret = register_driver(name, (void *)this); - if (ret == OK) break; + + if (ret == OK) { break; } + class_instance++; } if (class_instance == 4) { return ret; } + return class_instance; } @@ -127,17 +136,19 @@ VDev::register_driver(const char *name, void *data) PX4_DEBUG("VDev::register_driver %s", name); int ret = -ENOSPC; - if (name == NULL || data == NULL) + if (name == NULL || data == NULL) { return -EINVAL; + } // Make sure the device does not already exist // FIXME - convert this to a map for efficiency - for (int i=0;iname,name) == 0)) { + for (int i = 0; i < PX4_MAX_DEV; ++i) { + if (devmap[i] && (strcmp(devmap[i]->name, name) == 0)) { return -EEXIST; } } - for (int i=0;iname) == 0)) { delete devmap[i]; devmap[i] = NULL; @@ -169,6 +183,7 @@ VDev::unregister_driver(const char *name) break; } } + return ret; } @@ -178,14 +193,16 @@ VDev::unregister_class_devname(const char *class_devname, unsigned class_instanc PX4_DEBUG("VDev::unregister_class_devname"); char name[32]; snprintf(name, sizeof(name), "%s%u", class_devname, class_instance); - for (int i=0;iname,name) == 0) { + + for (int i = 0; i < PX4_MAX_DEV; ++i) { + if (devmap[i] && strcmp(devmap[i]->name, name) == 0) { delete devmap[i]; PX4_DEBUG("Unregistered class DEV %s", name); devmap[i] = NULL; return PX4_OK; } } + return -EINVAL; } @@ -197,15 +214,17 @@ VDev::init() // base class init first int ret = Device::init(); - if (ret != PX4_OK) + if (ret != PX4_OK) { goto out; + } // now register the driver if (_devname != nullptr) { ret = register_driver(_devname, (void *)this); - if (ret != PX4_OK) + if (ret != PX4_OK) { goto out; + } _registered = true; } @@ -232,8 +251,9 @@ VDev::open(file_t *filep) /* first-open callback may decline the open */ ret = open_first(filep); - if (ret != PX4_OK) + if (ret != PX4_OK) { _open_count--; + } } unlock(); @@ -261,8 +281,9 @@ VDev::close(file_t *filep) _open_count--; /* callback cannot decline the close */ - if (_open_count == 0) + if (_open_count == 0) { ret = close_last(filep); + } } else { ret = -EBADF; @@ -309,22 +330,26 @@ VDev::ioctl(file_t *filep, int cmd, unsigned long arg) switch (cmd) { - /* fetch a pointer to the driver's private data */ + /* fetch a pointer to the driver's private data */ case DIOC_GETPRIV: *(void **)(uintptr_t)arg = (void *)this; ret = PX4_OK; break; + case DEVIOCSPUBBLOCK: _pub_blocked = (arg != 0); ret = PX4_OK; break; + case DEVIOCGPUBBLOCK: ret = _pub_blocked; break; - case DEVIOCGDEVICEID: - ret = (int)_device_id.devid; + + case DEVIOCGDEVICEID: + ret = (int)_device_id.devid; PX4_INFO("IOCTL DEVIOCGDEVICEID %d", ret); break; + default: break; } @@ -365,8 +390,10 @@ VDev::poll(file_t *filep, px4_pollfd_struct_t *fds, bool setup) fds->revents |= fds->events & poll_state(filep); /* yes? post the notification */ - if (fds->revents != 0) + if (fds->revents != 0) { px4_sem_post(fds->sem); + } + } else { PX4_WARN("Store Poll Waiter error."); } @@ -392,8 +419,9 @@ VDev::poll_notify(pollevent_t events) lock(); for (unsigned i = 0; i < _max_pollwaiters; i++) - if (nullptr != _pollset[i]) + if (nullptr != _pollset[i]) { poll_notify_one(_pollset[i], events); + } unlock(); } @@ -408,7 +436,7 @@ VDev::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events) /* update the reported event set */ fds->revents |= fds->events & events; - PX4_DEBUG(" Events fds=%p %0x %0x %0x %d",fds, fds->revents, fds->events, events, value); + PX4_DEBUG(" Events fds=%p %0x %0x %0x %d", fds, fds->revents, fds->events, events, value); /* if the state is now interesting, wake the waiter if it's still asleep */ /* XXX semcount check here is a vile hack; counting semphores should not be abused as cvars */ @@ -432,6 +460,7 @@ VDev::store_poll_waiter(px4_pollfd_struct_t *fds) * Look for a free slot. */ PX4_DEBUG("VDev::store_poll_waiter"); + for (unsigned i = 0; i < _max_pollwaiters; i++) { if (nullptr == _pollset[i]) { @@ -449,6 +478,7 @@ int VDev::remove_poll_waiter(px4_pollfd_struct_t *fds) { PX4_DEBUG("VDev::remove_poll_waiter"); + for (unsigned i = 0; i < _max_pollwaiters; i++) { if (fds == _pollset[i]) { @@ -465,8 +495,9 @@ VDev::remove_poll_waiter(px4_pollfd_struct_t *fds) VDev *VDev::getDev(const char *path) { PX4_DEBUG("VDev::getDev"); - int i=0; - for (; iname, path); //} @@ -474,14 +505,16 @@ VDev *VDev::getDev(const char *path) return (VDev *)(devmap[i]->cdev); } } + return NULL; } void VDev::showDevices() { - int i=0; + int i = 0; PX4_INFO("Devices:"); - for (; iname, "/dev/", 5) == 0) { PX4_INFO(" %s", devmap[i]->name); } @@ -490,9 +523,10 @@ void VDev::showDevices() void VDev::showTopics() { - int i=0; + int i = 0; PX4_INFO("Devices:"); - for (; iname, "/obj/", 5) == 0) { PX4_INFO(" %s", devmap[i]->name); } @@ -501,11 +535,12 @@ void VDev::showTopics() void VDev::showFiles() { - int i=0; + int i = 0; PX4_INFO("Files:"); - for (; iname, "/obj/", 5) != 0 && - strncmp(devmap[i]->name, "/dev/", 5) != 0) { + strncmp(devmap[i]->name, "/dev/", 5) != 0) { PX4_INFO(" %s", devmap[i]->name); } } @@ -513,17 +548,21 @@ void VDev::showFiles() const char *VDev::topicList(unsigned int *next) { - for (;*nextname, "/obj/", 5) == 0) + for (; *next < PX4_MAX_DEV; (*next)++) + if (devmap[*next] && strncmp(devmap[(*next)]->name, "/obj/", 5) == 0) { return devmap[(*next)++]->name; + } + return NULL; } const char *VDev::devList(unsigned int *next) { - for (;*nextname, "/dev/", 5) == 0) + for (; *next < PX4_MAX_DEV; (*next)++) + if (devmap[*next] && strncmp(devmap[(*next)]->name, "/dev/", 5) == 0) { return devmap[(*next)++]->name; + } + return NULL; } diff --git a/src/drivers/device/vdev.h b/src/drivers/device/vdev.h index 89e06358f5..d1fb8ff8e1 100644 --- a/src/drivers/device/vdev.h +++ b/src/drivers/device/vdev.h @@ -37,7 +37,7 @@ * Definitions for the generic base classes in the virtual device framework. */ -#pragma once +#pragma once /* * Includes here should only cover the needs of the framework definitions. @@ -118,17 +118,17 @@ public: * @param data The buffer from which values should be read. * @param count The number of items to write. * @return The number of items written on success, negative errno otherwise. - */ + */ virtual int dev_write(unsigned address, void *data, unsigned count); - /** - * Perform a device-specific operation. - * - * @param operation The operation to perform. - * @param arg An argument to the operation. - * @return Negative errno on error, OK or positive value on success. - */ - virtual int dev_ioctl(unsigned operation, unsigned &arg); + /** + * Perform a device-specific operation. + * + * @param operation The operation to perform. + * @param arg An argument to the operation. + * @return Negative errno on error, OK or positive value on success. + */ + virtual int dev_ioctl(unsigned operation, unsigned &arg); /* device bus types for DEVID @@ -148,13 +148,13 @@ public: parameter protocol without loss of information. */ struct DeviceStructure { - enum DeviceBusType bus_type:3; - uint8_t bus:5; // which instance of the bus type - uint8_t address; // address on the bus (eg. I2C address) - uint8_t devtype; // device class specific device type - }; + enum DeviceBusType bus_type : 3; + uint8_t bus: 5; // which instance of the bus type + uint8_t address; // address on the bus (eg. I2C address) + uint8_t devtype; // device class specific device type + }; - union DeviceId { + union DeviceId { struct DeviceStructure devid_s; uint32_t devid; }; @@ -179,7 +179,8 @@ protected: * * Note that we must loop as the wait may be interrupted by a signal. */ - void lock() { + void lock() + { DEVICE_DEBUG("lock"); do {} while (px4_sem_wait(&_lock) != 0); } @@ -187,7 +188,8 @@ protected: /** * Release the driver lock. */ - void unlock() { + void unlock() + { DEVICE_DEBUG("unlock"); px4_sem_post(&_lock); } @@ -330,7 +332,7 @@ public: * * @return the file system string of the device handle */ - const char* get_devname() { return _devname; } + const char *get_devname() { return _devname; } protected: @@ -395,7 +397,7 @@ protected: */ virtual int close_last(file_t *filep); - /** + /** * Register a class device name, automatically adding device * class instance suffix if need be. * @@ -404,7 +406,7 @@ protected: */ virtual int register_class_devname(const char *class_devname); - /** + /** * Register a class device name, automatically adding device * class instance suffix if need be. * @@ -442,7 +444,7 @@ private: int remove_poll_waiter(px4_pollfd_struct_t *fds); /* do not allow copying this class */ - VDev(const VDev&); + VDev(const VDev &); //VDev operator=(const VDev&); }; @@ -464,7 +466,7 @@ public: PIO(const char *name, const char *devname, unsigned long base - ); + ); virtual ~PIO(); virtual int init(); @@ -476,7 +478,8 @@ protected: * * @param offset Register offset in bytes from the base address. */ - uint32_t reg(uint32_t offset) { + uint32_t reg(uint32_t offset) + { return *(volatile uint32_t *)(_base + offset); } @@ -486,7 +489,8 @@ protected: * @param offset Register offset in bytes from the base address. * @param value Value to write. */ - void reg(uint32_t offset, uint32_t value) { + void reg(uint32_t offset, uint32_t value) + { *(volatile uint32_t *)(_base + offset) = value; } @@ -500,7 +504,8 @@ protected: * @param clearbits Bits to clear in the register * @param setbits Bits to set in the register */ - void modify(uint32_t offset, uint32_t clearbits, uint32_t setbits) { + void modify(uint32_t offset, uint32_t clearbits, uint32_t setbits) + { uint32_t val = reg(offset); val &= ~clearbits; val |= setbits; @@ -516,8 +521,8 @@ private: // class instance for primary driver of each class enum CLASS_DEVICE { - CLASS_DEVICE_PRIMARY=0, - CLASS_DEVICE_SECONDARY=1, - CLASS_DEVICE_TERTIARY=2 + CLASS_DEVICE_PRIMARY = 0, + CLASS_DEVICE_SECONDARY = 1, + CLASS_DEVICE_TERTIARY = 2 }; diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index 5647bad892..a30bc8d06b 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -54,254 +54,265 @@ using namespace device; extern "C" { -static void timer_cb(void *data) -{ - px4_sem_t *p_sem = (px4_sem_t *)data; - px4_sem_post(p_sem); - PX4_DEBUG("timer_handler: Timer expired"); -} + static void timer_cb(void *data) + { + px4_sem_t *p_sem = (px4_sem_t *)data; + px4_sem_post(p_sem); + PX4_DEBUG("timer_handler: Timer expired"); + } #define PX4_MAX_FD 200 -static device::file_t *filemap[PX4_MAX_FD] = {}; + static device::file_t *filemap[PX4_MAX_FD] = {}; -int px4_errno; + int px4_errno; -inline bool valid_fd(int fd) -{ - return (fd < PX4_MAX_FD && fd >= 0 && filemap[fd] != NULL); -} - -int px4_open(const char *path, int flags, ...) -{ - PX4_DEBUG("px4_open"); - VDev *dev = VDev::getDev(path); - int ret = 0; - int i; - mode_t mode; - - if (!dev && (flags & (PX4_F_WRONLY|PX4_F_CREAT)) != 0 && - strncmp(path, "/obj/", 5) != 0 && - strncmp(path, "/dev/", 5) != 0) + inline bool valid_fd(int fd) { - va_list p; - va_start(p, flags); - mode = va_arg(p, mode_t); - va_end(p); + return (fd < PX4_MAX_FD && fd >= 0 && filemap[fd] != NULL); + } - // Create the file - PX4_DEBUG("Creating virtual file %s", path); - dev = VFile::createFile(path, mode); - } - if (dev) { - for (i=0; iopen(filemap[i]); - } - else { - PX4_WARN("exceeded maximum number of file descriptors!"); - ret = -ENOENT; - } - } - else { - ret = -EINVAL; - } - if (ret < 0) { - px4_errno = -ret; - return -1; - } - PX4_DEBUG("px4_open fd = %d", filemap[i]->fd); - return filemap[i]->fd; -} - -int px4_close(int fd) -{ - int ret; - if (valid_fd(fd)) { - VDev *dev = (VDev *)(filemap[fd]->vdev); - PX4_DEBUG("px4_close fd = %d", fd); - ret = dev->close(filemap[fd]); - filemap[fd] = NULL; - } - else { - ret = -EINVAL; - } - if (ret < 0) { - px4_errno = -ret; - ret = PX4_ERROR; - } - return ret; -} - -ssize_t px4_read(int fd, void *buffer, size_t buflen) -{ - int ret; - if (valid_fd(fd)) { - VDev *dev = (VDev *)(filemap[fd]->vdev); - PX4_DEBUG("px4_read fd = %d", fd); - ret = dev->read(filemap[fd], (char *)buffer, buflen); - } - else { - ret = -EINVAL; - } - if (ret < 0) { - px4_errno = -ret; - ret = PX4_ERROR; - } - return ret; -} - -ssize_t px4_write(int fd, const void *buffer, size_t buflen) -{ - int ret; - if (valid_fd(fd)) { - VDev *dev = (VDev *)(filemap[fd]->vdev); - PX4_DEBUG("px4_write fd = %d", fd); - ret = dev->write(filemap[fd], (const char *)buffer, buflen); - } - else { - ret = -EINVAL; - } - if (ret < 0) { - px4_errno = -ret; - ret = PX4_ERROR; - } - return ret; -} - -int px4_ioctl(int fd, int cmd, unsigned long arg) -{ - PX4_DEBUG("px4_ioctl fd = %d", fd); - int ret = 0; - if (valid_fd(fd)) { - VDev *dev = (VDev *)(filemap[fd]->vdev); - ret = dev->ioctl(filemap[fd], cmd, arg); - } - else { - ret = -EINVAL; - } - if (ret < 0) { - px4_errno = -ret; - } - - return ret; -} - -int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) -{ - px4_sem_t sem; - int count = 0; - int ret = -1; - unsigned int i; - - PX4_DEBUG("Called px4_poll timeout = %d", timeout); - px4_sem_init(&sem, 0, 0); - - // For each fd - for (i=0; ivdev);; - PX4_DEBUG("px4_poll: VDev->poll(setup) %d", fds[i].fd); - ret = dev->poll(filemap[fds[i].fd], &fds[i], true); + if (!dev && (flags & (PX4_F_WRONLY | PX4_F_CREAT)) != 0 && + strncmp(path, "/obj/", 5) != 0 && + strncmp(path, "/dev/", 5) != 0) { + va_list p; + va_start(p, flags); + mode = va_arg(p, mode_t); + va_end(p); - if (ret < 0) - break; - } - } - - if (ret >= 0) - { - if (timeout > 0) - { - // Use a work queue task - work_s _hpwork; - - hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)&sem, 1000*timeout); - px4_sem_wait(&sem); - - // Make sure timer thread is killed before sem goes - // out of scope - hrt_work_cancel(&_hpwork); - } - else if (timeout < 0) - { - px4_sem_wait(&sem); + // Create the file + PX4_DEBUG("Creating virtual file %s", path); + dev = VFile::createFile(path, mode); } - // For each fd - for (i=0; ivdev);; - PX4_DEBUG("px4_poll: VDev->poll(teardown) %d", fds[i].fd); - ret = dev->poll(filemap[fds[i].fd], &fds[i], false); - - if (ret < 0) + if (dev) { + for (i = 0; i < PX4_MAX_FD; ++i) { + if (filemap[i] == 0) { + filemap[i] = new device::file_t(flags, dev, i); break; + } + } - if (fds[i].revents) - count += 1; + if (i < PX4_MAX_FD) { + ret = dev->open(filemap[i]); + + } else { + PX4_WARN("exceeded maximum number of file descriptors!"); + ret = -ENOENT; + } + + } else { + ret = -EINVAL; + } + + if (ret < 0) { + px4_errno = -ret; + return -1; + } + + PX4_DEBUG("px4_open fd = %d", filemap[i]->fd); + return filemap[i]->fd; + } + + int px4_close(int fd) + { + int ret; + + if (valid_fd(fd)) { + VDev *dev = (VDev *)(filemap[fd]->vdev); + PX4_DEBUG("px4_close fd = %d", fd); + ret = dev->close(filemap[fd]); + filemap[fd] = NULL; + + } else { + ret = -EINVAL; + } + + if (ret < 0) { + px4_errno = -ret; + ret = PX4_ERROR; + } + + return ret; + } + + ssize_t px4_read(int fd, void *buffer, size_t buflen) + { + int ret; + + if (valid_fd(fd)) { + VDev *dev = (VDev *)(filemap[fd]->vdev); + PX4_DEBUG("px4_read fd = %d", fd); + ret = dev->read(filemap[fd], (char *)buffer, buflen); + + } else { + ret = -EINVAL; + } + + if (ret < 0) { + px4_errno = -ret; + ret = PX4_ERROR; + } + + return ret; + } + + ssize_t px4_write(int fd, const void *buffer, size_t buflen) + { + int ret; + + if (valid_fd(fd)) { + VDev *dev = (VDev *)(filemap[fd]->vdev); + PX4_DEBUG("px4_write fd = %d", fd); + ret = dev->write(filemap[fd], (const char *)buffer, buflen); + + } else { + ret = -EINVAL; + } + + if (ret < 0) { + px4_errno = -ret; + ret = PX4_ERROR; + } + + return ret; + } + + int px4_ioctl(int fd, int cmd, unsigned long arg) + { + PX4_DEBUG("px4_ioctl fd = %d", fd); + int ret = 0; + + if (valid_fd(fd)) { + VDev *dev = (VDev *)(filemap[fd]->vdev); + ret = dev->ioctl(filemap[fd], cmd, arg); + + } else { + ret = -EINVAL; + } + + if (ret < 0) { + px4_errno = -ret; + } + + return ret; + } + + int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) + { + px4_sem_t sem; + int count = 0; + int ret = -1; + unsigned int i; + + PX4_DEBUG("Called px4_poll timeout = %d", timeout); + px4_sem_init(&sem, 0, 0); + + // For each fd + for (i = 0; i < nfds; ++i) { + fds[i].sem = &sem; + fds[i].revents = 0; + fds[i].priv = NULL; + + // If fd is valid + if (valid_fd(fds[i].fd)) { + VDev *dev = (VDev *)(filemap[fds[i].fd]->vdev);; + PX4_DEBUG("px4_poll: VDev->poll(setup) %d", fds[i].fd); + ret = dev->poll(filemap[fds[i].fd], &fds[i], true); + + if (ret < 0) { + break; + } } } + + if (ret >= 0) { + if (timeout > 0) { + // Use a work queue task + work_s _hpwork; + + hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)&sem, 1000 * timeout); + px4_sem_wait(&sem); + + // Make sure timer thread is killed before sem goes + // out of scope + hrt_work_cancel(&_hpwork); + + } else if (timeout < 0) { + px4_sem_wait(&sem); + } + + // For each fd + for (i = 0; i < nfds; ++i) { + // If fd is valid + if (valid_fd(fds[i].fd)) { + VDev *dev = (VDev *)(filemap[fds[i].fd]->vdev);; + PX4_DEBUG("px4_poll: VDev->poll(teardown) %d", fds[i].fd); + ret = dev->poll(filemap[fds[i].fd], &fds[i], false); + + if (ret < 0) { + break; + } + + if (fds[i].revents) { + count += 1; + } + } + } + } + + px4_sem_destroy(&sem); + + return count; } - px4_sem_destroy(&sem); - - return count; -} - -int px4_fsync(int fd) -{ - return 0; -} - -int px4_access(const char *pathname, int mode) -{ - if (mode != F_OK) { - errno = EINVAL; - return -1; + int px4_fsync(int fd) + { + return 0; } - VDev *dev = VDev::getDev(pathname); - return (dev != nullptr) ? 0 : -1; -} -void px4_show_devices() -{ - VDev::showDevices(); -} + int px4_access(const char *pathname, int mode) + { + if (mode != F_OK) { + errno = EINVAL; + return -1; + } -void px4_show_topics() -{ - VDev::showTopics(); -} + VDev *dev = VDev::getDev(pathname); + return (dev != nullptr) ? 0 : -1; + } -void px4_show_files() -{ - VDev::showFiles(); -} + void px4_show_devices() + { + VDev::showDevices(); + } -const char * px4_get_device_names(unsigned int *handle) -{ - return VDev::devList(handle); -} + void px4_show_topics() + { + VDev::showTopics(); + } -const char * px4_get_topic_names(unsigned int *handle) -{ - return VDev::topicList(handle); -} + void px4_show_files() + { + VDev::showFiles(); + } + + const char *px4_get_device_names(unsigned int *handle) + { + return VDev::devList(handle); + } + + const char *px4_get_topic_names(unsigned int *handle) + { + return VDev::topicList(handle); + } } diff --git a/src/drivers/device/vfile.cpp b/src/drivers/device/vfile.cpp index 2d2d81c816..e142f626d1 100644 --- a/src/drivers/device/vfile.cpp +++ b/src/drivers/device/vfile.cpp @@ -48,7 +48,7 @@ VFile::VFile(const char *fname, mode_t mode) : { } -VFile * VFile::createFile(const char *fname, mode_t mode) +VFile *VFile::createFile(const char *fname, mode_t mode) { VFile *me = new VFile(fname, mode); me->register_driver(fname, me); diff --git a/src/drivers/device/vfile.h b/src/drivers/device/vfile.h index d7c5e15d7f..6bea62d1f1 100644 --- a/src/drivers/device/vfile.h +++ b/src/drivers/device/vfile.h @@ -44,7 +44,8 @@ #include #include -class VFile : public device::VDev { +class VFile : public device::VDev +{ public: static VFile *createFile(const char *fname, mode_t mode); diff --git a/src/drivers/drv_device.h b/src/drivers/drv_device.h index e0226590bc..02dbe7af67 100644 --- a/src/drivers/drv_device.h +++ b/src/drivers/drv_device.h @@ -68,7 +68,7 @@ #ifdef __PX4_POSIX #ifndef SIOCDEVPRIVATE - #define SIOCDEVPRIVATE 1 +#define SIOCDEVPRIVATE 1 #endif #define DIOC_GETPRIV SIOCDEVPRIVATE diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index fc619866be..bb04245333 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -65,7 +65,7 @@ __BEGIN_DECLS #define pwm_output_values output_pwm_s #ifndef PWM_OUTPUT_MAX_CHANNELS - #define PWM_OUTPUT_MAX_CHANNELS output_pwm_s::PWM_OUTPUT_MAX_CHANNELS +#define PWM_OUTPUT_MAX_CHANNELS output_pwm_s::PWM_OUTPUT_MAX_CHANNELS #endif /** diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index 51c009717c..83f83896c6 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -162,7 +162,7 @@ ETSAirspeed::collect() // caller could end up using this value as part of an // average perf_count(_comms_errors); - DEVICE_LOG("zero value from sensor"); + DEVICE_LOG("zero value from sensor"); return -1; } diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c index 75d1124a61..6100f5f1d0 100644 --- a/src/drivers/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -83,6 +83,7 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or /* Back up the original UART configuration to restore it after exit */ int termios_state; + if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { warnx("ERR: tcgetattr%s: %d\n", uart_name, termios_state); close(uart); @@ -120,9 +121,9 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or static void usage() { fprintf(stderr, - "usage: frsky_telemetry start [-d ]\n" - " frsky_telemetry stop\n" - " frsky_telemetry status\n"); + "usage: frsky_telemetry start [-d ]\n" + " frsky_telemetry stop\n" + " frsky_telemetry status\n"); exit(1); } @@ -139,6 +140,7 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) argv += 2; int ch; + while ((ch = getopt(argc, argv, "d:")) != EOF) { switch (ch) { case 'd': @@ -155,8 +157,9 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) struct termios uart_config_original; const int uart = frsky_open_uart(device_name, &uart_config_original); - if (uart < 0) + if (uart < 0) { err(1, "could not open %s", device_name); + } /* Subscribe to topics */ frsky_init(); @@ -165,6 +168,7 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) /* Main thread loop */ unsigned int iteration = 0; + while (!thread_should_exit) { /* Sleep 200 ms */ @@ -174,14 +178,12 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) frsky_send_frame1(uart); /* Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level */ - if (iteration % 5 == 0) - { + if (iteration % 5 == 0) { frsky_send_frame2(uart); } /* Send frame 3 (every 5000ms): date, time */ - if (iteration % 25 == 0) - { + if (iteration % 25 == 0) { frsky_send_frame3(uart); iteration = 0; @@ -212,16 +214,17 @@ int frsky_telemetry_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { /* this is not an error */ - if (thread_running) + if (thread_running) { errx(0, "frsky_telemetry already running"); + } thread_should_exit = false; frsky_task = px4_task_spawn_cmd("frsky_telemetry", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2000, - frsky_telemetry_thread_main, - (char * const *)argv); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2000, + frsky_telemetry_thread_main, + (char *const *)argv); while (!thread_running) { usleep(200); @@ -233,8 +236,9 @@ int frsky_telemetry_main(int argc, char *argv[]) if (!strcmp(argv[1], "stop")) { /* this is not an error */ - if (!thread_running) + if (!thread_running) { errx(0, "frsky_telemetry already stopped"); + } thread_should_exit = true; diff --git a/src/drivers/gimbal/gimbal.cpp b/src/drivers/gimbal/gimbal.cpp index 249fc22dac..40b86d1421 100644 --- a/src/drivers/gimbal/gimbal.cpp +++ b/src/drivers/gimbal/gimbal.cpp @@ -331,7 +331,7 @@ Gimbal::cycle() orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd); if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL - || cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT) { + || cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT) { _control_cmd = cmd; _control_cmd_set = true; @@ -358,20 +358,21 @@ Gimbal::cycle() if (_control_cmd_set) { unsigned mountMode = _control_cmd.param7; - DEVICE_DEBUG("control_cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", _control_cmd.command, mountMode, (double)_control_cmd.param1, (double)_control_cmd.param2); + DEVICE_DEBUG("control_cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", _control_cmd.command, mountMode, + (double)_control_cmd.param1, (double)_control_cmd.param2); if (_control_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL && - mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { + mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { /* Convert to range -1 ... 1, which corresponds to -180deg ... 180deg */ roll += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param1; pitch += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param2; yaw += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param3; - + updated = true; } if (_control_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT && - mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { + mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { float gimbalDirectionQuat[] = {_control_cmd.param1, _control_cmd.param2, _control_cmd.param3, _control_cmd.param4}; math::Vector<3> gimablDirectionEuler = math::Quaternion(gimbalDirectionQuat).to_dcm().to_euler(); diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index eca775745f..322d275549 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -37,7 +37,7 @@ ASHTECH::~ASHTECH() int ASHTECH::handle_message(int len) { - char * endp; + char *endp; if (len < 7) { return 0; } @@ -69,7 +69,8 @@ int ASHTECH::handle_message(int len) Fields 5 and 6 together yield the total offset. For example, if field 5 is -5 and field 6 is +15, local time is 5 hours and 15 minutes earlier than GMT. */ double ashtech_time = 0.0; - int day = 0, month = 0, year = 0, local_time_off_hour __attribute__((unused)) = 0, local_time_off_min __attribute__((unused)) = 0; + int day = 0, month = 0, year = 0, local_time_off_hour __attribute__((unused)) = 0, + local_time_off_min __attribute__((unused)) = 0; if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; } @@ -110,12 +111,14 @@ int ASHTECH::handle_message(int len) timespec ts; ts.tv_sec = epoch; ts.tv_nsec = usecs * 1000; + if (clock_settime(CLOCK_REALTIME, &ts)) { warn("failed setting clock"); } _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; _gps_position->time_utc_usec += usecs; + } else { _gps_position->time_utc_usec = 0; } @@ -266,7 +269,8 @@ int ASHTECH::handle_message(int len) double ashtech_time __attribute__((unused)) = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; int num_of_sv __attribute__((unused)) = 0, fix_quality = 0; double track_true = 0.0, ground_speed = 0.0 , age_of_corr __attribute__((unused)) = 0.0; - double hdop __attribute__((unused)) = 99.9, vdop __attribute__((unused)) = 99.9, pdop __attribute__((unused)) = 99.9, tdop __attribute__((unused)) = 99.9, vertic_vel = 0.0; + double hdop __attribute__((unused)) = 99.9, vdop __attribute__((unused)) = 99.9, pdop __attribute__((unused)) = 99.9, + tdop __attribute__((unused)) = 99.9, vertic_vel = 0.0; char ns = '?', ew = '?'; if (bufptr && *(++bufptr) != ',') { fix_quality = strtol(bufptr, &endp, 10); bufptr = endp; } @@ -281,7 +285,9 @@ int ASHTECH::handle_message(int len) * or strtod won't find anything and endp will point exactly where bufptr is. The same is for lon and alt. */ lat = strtod(bufptr, &endp); + if (bufptr != endp) {coordinatesFound++;} + bufptr = endp; } @@ -289,7 +295,9 @@ int ASHTECH::handle_message(int len) if (bufptr && *(++bufptr) != ',') { lon = strtod(bufptr, &endp); + if (bufptr != endp) {coordinatesFound++;} + bufptr = endp; } @@ -297,7 +305,9 @@ int ASHTECH::handle_message(int len) if (bufptr && *(++bufptr) != ',') { alt = strtod(bufptr, &endp); + if (bufptr != endp) {coordinatesFound++;} + bufptr = endp; } @@ -349,7 +359,8 @@ int ASHTECH::handle_message(int len) _gps_position->vel_n_m_s = velocity_north; /** GPS ground speed in m/s */ _gps_position->vel_e_m_s = velocity_east; /** GPS ground speed in m/s */ _gps_position->vel_d_m_s = static_cast(-vertic_vel); /** GPS ground speed in m/s */ - _gps_position->cog_rad = track_rad; /** Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */ + _gps_position->cog_rad = + track_rad; /** Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */ _gps_position->vel_ned_valid = true; /** Flag to indicate if NED speed is valid */ _gps_position->c_variance_rad = 0.1f; _gps_position->timestamp_velocity = hrt_absolute_time(); @@ -381,7 +392,8 @@ int ASHTECH::handle_message(int len) 9 The checksum data, always begins with * */ double ashtech_time __attribute__((unused)) = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0; - double min_err __attribute__((unused)) = 0.0, maj_err __attribute__((unused)) = 0.0, deg_from_north __attribute__((unused)) = 0.0, rms_err __attribute__((unused)) = 0.0; + double min_err __attribute__((unused)) = 0.0, maj_err __attribute__((unused)) = 0.0, + deg_from_north __attribute__((unused)) = 0.0, rms_err __attribute__((unused)) = 0.0; if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; } @@ -400,7 +412,7 @@ int ASHTECH::handle_message(int len) if (bufptr && *(++bufptr) != ',') { alt_err = strtod(bufptr, &endp); bufptr = endp; } _gps_position->eph = sqrtf(static_cast(lat_err) * static_cast(lat_err) - + static_cast(lon_err) * static_cast(lon_err)); + + static_cast(lon_err) * static_cast(lon_err)); _gps_position->epv = static_cast(alt_err); _gps_position->s_variance_m_s = 0; @@ -571,7 +583,7 @@ int ASHTECH::parse_char(uint8_t b) int iRet = 0; switch (_decode_state) { - /* First, look for sync1 */ + /* First, look for sync1 */ case NME_DECODE_UNINIT: if (b == '$') { _decode_state = NME_DECODE_GOT_SYNC1; @@ -636,13 +648,13 @@ void ASHTECH::decode_init(void) */ const char comm[] = "$PASHS,POP,20\r\n"\ - "$PASHS,NME,ZDA,B,ON,3\r\n"\ - "$PASHS,NME,GGA,B,OFF\r\n"\ - "$PASHS,NME,GST,B,ON,3\r\n"\ - "$PASHS,NME,POS,B,ON,0.05\r\n"\ - "$PASHS,NME,GSV,B,ON,3\r\n"\ - "$PASHS,SPD,A,8\r\n"\ - "$PASHS,SPD,B,9\r\n"; + "$PASHS,NME,ZDA,B,ON,3\r\n"\ + "$PASHS,NME,GGA,B,OFF\r\n"\ + "$PASHS,NME,GST,B,ON,3\r\n"\ + "$PASHS,NME,POS,B,ON,0.05\r\n"\ + "$PASHS,NME,GSV,B,ON,3\r\n"\ + "$PASHS,SPD,A,8\r\n"\ + "$PASHS,SPD,B,9\r\n"; int ASHTECH::configure(unsigned &baudrate) { diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 35ee8658a0..05ff3e5b91 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -211,8 +211,9 @@ GPS::~GPS() } /* well, kill it anyway, though this will probably crash */ - if (_task != -1) + if (_task != -1) { task_delete(_task); + } g_dev = nullptr; @@ -224,12 +225,13 @@ GPS::init() int ret = ERROR; /* do regular cdev init */ - if (CDev::init() != OK) + if (CDev::init() != OK) { goto out; + } /* start the GPS driver worker task */ _task = px4_task_spawn_cmd("gps", SCHED_DEFAULT, - SCHED_PRIORITY_SLOW_DRIVER, 1200, (main_t)&GPS::task_main_trampoline, nullptr); + SCHED_PRIORITY_SLOW_DRIVER, 1200, (main_t)&GPS::task_main_trampoline, nullptr); if (_task < 0) { warnx("task start failed: %d", errno); @@ -305,9 +307,10 @@ GPS::task_main() _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; - _report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s); + _report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * + _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s); _report_gps_pos.cog_rad = 0.0f; - _report_gps_pos.vel_ned_valid = true; + _report_gps_pos.vel_ned_valid = true; /* no time and satellite information simulated */ @@ -392,14 +395,16 @@ GPS::task_main() } int helper_ret; + while ((helper_ret = _Helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) { - // lock(); + // lock(); /* opportunistic publishing - else invalid data would end up on the bus */ if (!(_pub_blocked)) { if (helper_ret & 1) { orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); } + 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); @@ -508,7 +513,7 @@ void GPS::print_info() { //GPS Mode - if(_fake_gps) { + if (_fake_gps) { warnx("protocol: SIMULATED"); } @@ -522,27 +527,27 @@ GPS::print_info() warnx("protocol: MTK"); break; - case GPS_DRIVER_MODE_ASHTECH: - warnx("protocol: ASHTECH"); - break; + case GPS_DRIVER_MODE_ASHTECH: + warnx("protocol: ASHTECH"); + break; default: break; - } + } } warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); - warnx("sat info: %s, noise: %d, jamming detected: %s", - (_p_report_sat_info != nullptr) ? "enabled" : "disabled", - _report_gps_pos.noise_per_ms, - _report_gps_pos.jamming_indicator == 255 ? "YES" : "NO"); + warnx("sat info: %s, noise: %d, jamming detected: %s", + (_p_report_sat_info != nullptr) ? "enabled" : "disabled", + _report_gps_pos.noise_per_ms, + _report_gps_pos.jamming_indicator == 255 ? "YES" : "NO"); if (_report_gps_pos.timestamp_position != 0) { warnx("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_position) / 1000.0); warnx("lat: %d, lon: %d, alt: %d", _report_gps_pos.lat, _report_gps_pos.lon, _report_gps_pos.alt); warnx("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); + (double)_report_gps_pos.vel_e_m_s, (double)_report_gps_pos.vel_d_m_s); warnx("eph: %.2fm, epv: %.2fm", (double)_report_gps_pos.eph, (double)_report_gps_pos.epv); warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate()); warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate()); @@ -575,17 +580,20 @@ start(const char *uart_path, bool fake_gps, bool enable_sat_info) { int fd; - if (g_dev != nullptr) + if (g_dev != nullptr) { errx(1, "already started"); + } /* create the driver */ g_dev = new GPS(uart_path, fake_gps, enable_sat_info); - if (g_dev == nullptr) + if (g_dev == nullptr) { goto fail; + } - if (OK != g_dev->init()) + if (OK != g_dev->init()) { goto fail; + } /* set the poll rate to default, starts automatic data collection */ fd = open(GPS0_DEVICE_PATH, O_RDONLY); @@ -639,11 +647,13 @@ reset() { int fd = open(GPS0_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "failed "); + } - if (ioctl(fd, SENSORIOCRESET, 0) < 0) + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { err(1, "reset failed"); + } exit(0); } @@ -654,8 +664,9 @@ reset() void info() { - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "not running"); + } g_dev->print_info(); @@ -690,39 +701,45 @@ gps_main(int argc, char *argv[]) /* Detect fake gps option */ for (int i = 2; i < argc; i++) { - if (!strcmp(argv[i], "-f")) + if (!strcmp(argv[i], "-f")) { fake_gps = true; + } } /* Detect sat info option */ for (int i = 2; i < argc; i++) { - if (!strcmp(argv[i], "-s")) + if (!strcmp(argv[i], "-s")) { enable_sat_info = true; + } } gps::start(device_name, fake_gps, enable_sat_info); } - if (!strcmp(argv[1], "stop")) + if (!strcmp(argv[1], "stop")) { gps::stop(); + } /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) + if (!strcmp(argv[1], "test")) { gps::test(); + } /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) + if (!strcmp(argv[1], "reset")) { gps::reset(); + } /* * Print driver status. */ - if (!strcmp(argv[1], "status")) + if (!strcmp(argv[1], "status")) { gps::info(); + } out: errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status'\n [-d /dev/ttyS0-n][-f (for enabling fake)][-s (to enable sat info)]"); diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp index 371f17c1a9..25991b090a 100644 --- a/src/drivers/gps/mtk.cpp +++ b/src/drivers/gps/mtk.cpp @@ -66,8 +66,9 @@ int MTK::configure(unsigned &baudrate) { /* set baudrate first */ - if (GPS_Helper::set_baudrate(_fd, MTK_BAUDRATE) != 0) + if (GPS_Helper::set_baudrate(_fd, MTK_BAUDRATE) != 0) { return -1; + } baudrate = MTK_BAUDRATE; @@ -207,8 +208,9 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet) } else if (_decode_state == MTK_DECODE_GOT_CK_B) { // Add to checksum - if (_rx_count < 33) + if (_rx_count < 33) { add_byte_to_checksum(b); + } // Fill packet buffer ((uint8_t *)(&packet))[_rx_count] = b; @@ -288,12 +290,14 @@ MTK::handle_message(gps_mtk_packet_t &packet) timespec ts; ts.tv_sec = epoch; ts.tv_nsec = timeinfo_conversion_temp * 1000000ULL; + if (clock_settime(CLOCK_REALTIME, &ts)) { warn("failed setting clock"); } _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; _gps_position->time_utc_usec += timeinfo_conversion_temp * 1000ULL; + } else { _gps_position->time_utc_usec = 0; } diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index a7247a262d..643b339be3 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -199,6 +199,7 @@ UBX::configure(unsigned &baudrate) if (wait_for_ack(UBX_MSG_CFG_SBAS, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } + #endif /* configure message rates */ @@ -207,41 +208,50 @@ UBX::configure(unsigned &baudrate) /* try to set rate for NAV-PVT */ /* (implemented for ubx7+ modules only, use NAV-SOL, NAV-POSLLH, NAV-VELNED and NAV-TIMEUTC for ubx6) */ configure_message_rate(UBX_MSG_NAV_PVT, 1); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { _use_nav_pvt = false; + } else { _use_nav_pvt = true; } + UBX_DEBUG("%susing NAV-PVT", _use_nav_pvt ? "" : "not "); if (!_use_nav_pvt) { configure_message_rate(UBX_MSG_NAV_TIMEUTC, 5); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } configure_message_rate(UBX_MSG_NAV_POSLLH, 1); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } configure_message_rate(UBX_MSG_NAV_SOL, 1); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } configure_message_rate(UBX_MSG_NAV_VELNED, 1); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } } configure_message_rate(UBX_MSG_NAV_SVINFO, (_satellite_info != nullptr) ? 5 : 0); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } configure_message_rate(UBX_MSG_MON_HW, 1); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } @@ -269,9 +279,11 @@ UBX::wait_for_ack(const uint16_t msg, const unsigned timeout, const bool report) if (_ack_state == UBX_ACK_GOT_ACK) { ret = 0; // ACK received ok + } else if (report) { if (_ack_state == UBX_ACK_GOT_NAK) { UBX_DEBUG("ubx msg 0x%04x NAK", SWAP16((unsigned)msg)); + } else { UBX_DEBUG("ubx msg 0x%04x ACK timeout", SWAP16((unsigned)msg)); } @@ -359,6 +371,7 @@ UBX::parse_char(const uint8_t b) UBX_TRACE_PARSER("\nA"); _decode_state = UBX_DECODE_SYNC2; } + break; /* Expecting Sync2 */ @@ -370,6 +383,7 @@ UBX::parse_char(const uint8_t b) } else { // Sync1 not followed by Sync2: reset parser decode_init(); } + break; /* Expecting Class */ @@ -401,38 +415,48 @@ UBX::parse_char(const uint8_t b) UBX_TRACE_PARSER("F"); add_byte_to_checksum(b); _rx_payload_length |= b << 8; // calculate payload size + if (payload_rx_init() != 0) { // start payload reception // payload will not be handled, discard message decode_init(); + } else { _decode_state = (_rx_payload_length > 0) ? UBX_DECODE_PAYLOAD : UBX_DECODE_CHKSUM1; } + break; /* Expecting payload */ case UBX_DECODE_PAYLOAD: UBX_TRACE_PARSER("."); add_byte_to_checksum(b); + switch (_rx_msg) { case UBX_MSG_NAV_SVINFO: ret = payload_rx_add_nav_svinfo(b); // add a NAV-SVINFO payload byte break; + case UBX_MSG_MON_VER: ret = payload_rx_add_mon_ver(b); // add a MON-VER payload byte break; + default: ret = payload_rx_add(b); // add a payload byte break; } + if (ret < 0) { // payload not handled, discard message decode_init(); + } else if (ret > 0) { // payload complete, expecting checksum _decode_state = UBX_DECODE_CHKSUM1; + } else { // expecting more payload, stay in state UBX_DECODE_PAYLOAD } + ret = 0; break; @@ -441,18 +465,22 @@ UBX::parse_char(const uint8_t b) if (_rx_ck_a != b) { UBX_WARN("ubx checksum err"); decode_init(); + } else { _decode_state = UBX_DECODE_CHKSUM2; } + break; /* Expecting second checksum byte */ case UBX_DECODE_CHKSUM2: if (_rx_ck_b != b) { UBX_WARN("ubx checksum err"); + } else { ret = payload_rx_done(); // finish payload processing } + decode_init(); break; @@ -475,83 +503,116 @@ UBX::payload_rx_init() switch (_rx_msg) { case UBX_MSG_NAV_PVT: - if ( (_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX7) /* u-blox 7 msg format */ - && (_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX8)) /* u-blox 8+ msg format */ + if ((_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX7) /* u-blox 7 msg format */ + && (_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX8)) { /* u-blox 8+ msg format */ _rx_state = UBX_RXMSG_ERROR_LENGTH; - else if (!_configured) - _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured - else if (!_use_nav_pvt) - _rx_state = UBX_RXMSG_DISABLE; // disable if not using NAV-PVT + + } else if (!_configured) { + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + + } else if (!_use_nav_pvt) { + _rx_state = UBX_RXMSG_DISABLE; // disable if not using NAV-PVT + } + break; case UBX_MSG_NAV_POSLLH: - if (_rx_payload_length != sizeof(ubx_payload_rx_nav_posllh_t)) + if (_rx_payload_length != sizeof(ubx_payload_rx_nav_posllh_t)) { _rx_state = UBX_RXMSG_ERROR_LENGTH; - else if (!_configured) - _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured - else if (_use_nav_pvt) - _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead + + } else if (!_configured) { + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + + } else if (_use_nav_pvt) { + _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead + } + break; case UBX_MSG_NAV_SOL: - if (_rx_payload_length != sizeof(ubx_payload_rx_nav_sol_t)) + if (_rx_payload_length != sizeof(ubx_payload_rx_nav_sol_t)) { _rx_state = UBX_RXMSG_ERROR_LENGTH; - else if (!_configured) - _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured - else if (_use_nav_pvt) - _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead + + } else if (!_configured) { + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + + } else if (_use_nav_pvt) { + _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead + } + break; case UBX_MSG_NAV_TIMEUTC: - if (_rx_payload_length != sizeof(ubx_payload_rx_nav_timeutc_t)) + if (_rx_payload_length != sizeof(ubx_payload_rx_nav_timeutc_t)) { _rx_state = UBX_RXMSG_ERROR_LENGTH; - else if (!_configured) - _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured - else if (_use_nav_pvt) - _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead + + } else if (!_configured) { + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + + } else if (_use_nav_pvt) { + _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead + } + break; case UBX_MSG_NAV_SVINFO: - if (_satellite_info == nullptr) - _rx_state = UBX_RXMSG_DISABLE; // disable if sat info not requested - else if (!_configured) - _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured - else - memset(_satellite_info, 0, sizeof(*_satellite_info)); // initialize sat info + if (_satellite_info == nullptr) { + _rx_state = UBX_RXMSG_DISABLE; // disable if sat info not requested + + } else if (!_configured) { + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + + } else { + memset(_satellite_info, 0, sizeof(*_satellite_info)); // initialize sat info + } + break; case UBX_MSG_NAV_VELNED: - if (_rx_payload_length != sizeof(ubx_payload_rx_nav_velned_t)) + if (_rx_payload_length != sizeof(ubx_payload_rx_nav_velned_t)) { _rx_state = UBX_RXMSG_ERROR_LENGTH; - else if (!_configured) - _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured - else if (_use_nav_pvt) - _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead + + } else if (!_configured) { + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + + } else if (_use_nav_pvt) { + _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead + } + break; case UBX_MSG_MON_VER: break; // unconditionally handle this message case UBX_MSG_MON_HW: - if ( (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx6_t)) /* u-blox 6 msg format */ - && (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx7_t))) /* u-blox 7+ msg format */ + if ((_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx6_t)) /* u-blox 6 msg format */ + && (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx7_t))) { /* u-blox 7+ msg format */ _rx_state = UBX_RXMSG_ERROR_LENGTH; - else if (!_configured) - _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + + } else if (!_configured) { + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + } + break; case UBX_MSG_ACK_ACK: - if (_rx_payload_length != sizeof(ubx_payload_rx_ack_ack_t)) + if (_rx_payload_length != sizeof(ubx_payload_rx_ack_ack_t)) { _rx_state = UBX_RXMSG_ERROR_LENGTH; - else if (_configured) - _rx_state = UBX_RXMSG_IGNORE; // ignore if _configured + + } else if (_configured) { + _rx_state = UBX_RXMSG_IGNORE; // ignore if _configured + } + break; case UBX_MSG_ACK_NAK: - if (_rx_payload_length != sizeof(ubx_payload_rx_ack_nak_t)) + if (_rx_payload_length != sizeof(ubx_payload_rx_ack_nak_t)) { _rx_state = UBX_RXMSG_ERROR_LENGTH; - else if (_configured) - _rx_state = UBX_RXMSG_IGNORE; // ignore if _configured + + } else if (_configured) { + _rx_state = UBX_RXMSG_IGNORE; // ignore if _configured + } + break; default: @@ -624,32 +685,39 @@ UBX::payload_rx_add_nav_svinfo(const uint8_t b) if (_rx_payload_index < sizeof(ubx_payload_rx_nav_svinfo_part1_t)) { // Fill Part 1 buffer _buf.raw[_rx_payload_index] = b; + } else { if (_rx_payload_index == sizeof(ubx_payload_rx_nav_svinfo_part1_t)) { // Part 1 complete: decode Part 1 buffer _satellite_info->count = MIN(_buf.payload_rx_nav_svinfo_part1.numCh, satellite_info_s::SAT_INFO_MAX_SATELLITES); - UBX_TRACE_SVINFO("SVINFO len %u numCh %u\n", (unsigned)_rx_payload_length, (unsigned)_buf.payload_rx_nav_svinfo_part1.numCh); + UBX_TRACE_SVINFO("SVINFO len %u numCh %u\n", (unsigned)_rx_payload_length, + (unsigned)_buf.payload_rx_nav_svinfo_part1.numCh); } - if (_rx_payload_index < sizeof(ubx_payload_rx_nav_svinfo_part1_t) + _satellite_info->count * sizeof(ubx_payload_rx_nav_svinfo_part2_t)) { + + if (_rx_payload_index < sizeof(ubx_payload_rx_nav_svinfo_part1_t) + _satellite_info->count * sizeof( + ubx_payload_rx_nav_svinfo_part2_t)) { // Still room in _satellite_info: fill Part 2 buffer - unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) % sizeof(ubx_payload_rx_nav_svinfo_part2_t); + unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) % sizeof( + ubx_payload_rx_nav_svinfo_part2_t); _buf.raw[buf_index] = b; + if (buf_index == sizeof(ubx_payload_rx_nav_svinfo_part2_t) - 1) { // Part 2 complete: decode Part 2 buffer - unsigned sat_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) / sizeof(ubx_payload_rx_nav_svinfo_part2_t); + unsigned sat_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) / sizeof( + ubx_payload_rx_nav_svinfo_part2_t); _satellite_info->used[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.flags & 0x01); _satellite_info->snr[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.cno); _satellite_info->elevation[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.elev); _satellite_info->azimuth[sat_index] = (uint8_t)((float)_buf.payload_rx_nav_svinfo_part2.azim * 255.0f / 360.0f); _satellite_info->svid[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.svid); UBX_TRACE_SVINFO("SVINFO #%02u used %u snr %3u elevation %3u azimuth %3u svid %3u\n", - (unsigned)sat_index + 1, - (unsigned)_satellite_info->used[sat_index], - (unsigned)_satellite_info->snr[sat_index], - (unsigned)_satellite_info->elevation[sat_index], - (unsigned)_satellite_info->azimuth[sat_index], - (unsigned)_satellite_info->svid[sat_index] - ); + (unsigned)sat_index + 1, + (unsigned)_satellite_info->used[sat_index], + (unsigned)_satellite_info->snr[sat_index], + (unsigned)_satellite_info->elevation[sat_index], + (unsigned)_satellite_info->azimuth[sat_index], + (unsigned)_satellite_info->svid[sat_index] + ); } } } @@ -672,6 +740,7 @@ UBX::payload_rx_add_mon_ver(const uint8_t b) if (_rx_payload_index < sizeof(ubx_payload_rx_mon_ver_part1_t)) { // Fill Part 1 buffer _buf.raw[_rx_payload_index] = b; + } else { if (_rx_payload_index == sizeof(ubx_payload_rx_mon_ver_part1_t)) { // Part 1 complete: decode Part 1 buffer and calculate hash for SW&HW version strings @@ -681,9 +750,12 @@ UBX::payload_rx_add_mon_ver(const uint8_t b) UBX_DEBUG("VER hw \"%10s\"", _buf.payload_rx_mon_ver_part1.hwVersion); UBX_DEBUG("VER sw \"%30s\"", _buf.payload_rx_mon_ver_part1.swVersion); } + // fill Part 2 buffer - unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_mon_ver_part1_t)) % sizeof(ubx_payload_rx_mon_ver_part2_t); + unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_mon_ver_part1_t)) % sizeof( + ubx_payload_rx_mon_ver_part2_t); _buf.raw[buf_index] = b; + if (buf_index == sizeof(ubx_payload_rx_mon_ver_part2_t) - 1) { // Part 2 complete: decode Part 2 buffer UBX_DEBUG("VER ext \" %30s\"", _buf.payload_rx_mon_ver_part2.extension); @@ -717,13 +789,11 @@ UBX::payload_rx_done(void) UBX_TRACE_RXMSG("Rx NAV-PVT\n"); //Check if position fix flag is good - if ((_buf.payload_rx_nav_pvt.flags & UBX_RX_NAV_PVT_FLAGS_GNSSFIXOK) == 1) - { + if ((_buf.payload_rx_nav_pvt.flags & UBX_RX_NAV_PVT_FLAGS_GNSSFIXOK) == 1) { _gps_position->fix_type = _buf.payload_rx_nav_pvt.fixType; _gps_position->vel_ned_valid = true; - } - else - { + + } else { _gps_position->fix_type = 0; _gps_position->vel_ned_valid = false; } @@ -748,10 +818,9 @@ UBX::payload_rx_done(void) _gps_position->c_variance_rad = (float)_buf.payload_rx_nav_pvt.headAcc * M_DEG_TO_RAD_F * 1e-5f; //Check if time and date fix flags are good - if( (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDDATE) - && (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDTIME) - && (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_FULLYRESOLVED)) - { + if ((_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDDATE) + && (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDTIME) + && (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_FULLYRESOLVED)) { /* convert to unix timestamp */ struct tm timeinfo; timeinfo.tm_year = _buf.payload_rx_nav_pvt.year - 1900; @@ -770,12 +839,14 @@ UBX::payload_rx_done(void) timespec ts; ts.tv_sec = epoch; ts.tv_nsec = _buf.payload_rx_nav_pvt.nano; + if (clock_settime(CLOCK_REALTIME, &ts)) { warn("failed setting clock"); } _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; _gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000; + } else { _gps_position->time_utc_usec = 0; } @@ -827,8 +898,7 @@ UBX::payload_rx_done(void) case UBX_MSG_NAV_TIMEUTC: UBX_TRACE_RXMSG("Rx NAV-TIMEUTC\n"); - if(_buf.payload_rx_nav_timeutc.valid & UBX_RX_NAV_TIMEUTC_VALID_VALIDUTC) - { + if (_buf.payload_rx_nav_timeutc.valid & UBX_RX_NAV_TIMEUTC_VALID_VALIDUTC) { // convert to unix timestamp struct tm timeinfo; timeinfo.tm_year = _buf.payload_rx_nav_timeutc.year - 1900; @@ -849,12 +919,14 @@ UBX::payload_rx_done(void) timespec ts; ts.tv_sec = epoch; ts.tv_nsec = _buf.payload_rx_nav_timeutc.nano; + if (clock_settime(CLOCK_REALTIME, &ts)) { warn("failed setting clock"); } _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; _gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000; + } else { _gps_position->time_utc_usec = 0; } @@ -922,6 +994,7 @@ UBX::payload_rx_done(void) ret = 0; // don't handle message break; } + break; case UBX_MSG_ACK_ACK: @@ -999,39 +1072,44 @@ UBX::send_message(const uint16_t msg, const uint8_t *payload, const uint16_t len header.length = length; // Calculate checksum - calc_checksum(((uint8_t*)&header) + 2, sizeof(header) - 2, &checksum); // skip 2 sync bytes - if (payload != nullptr) + calc_checksum(((uint8_t *)&header) + 2, sizeof(header) - 2, &checksum); // skip 2 sync bytes + + if (payload != nullptr) { calc_checksum(payload, length, &checksum); + } // Send message write(_fd, (const void *)&header, sizeof(header)); - if (payload != nullptr) + + if (payload != nullptr) { write(_fd, (const void *)payload, length); + } + write(_fd, (const void *)&checksum, sizeof(checksum)); } uint32_t UBX::fnv1_32_str(uint8_t *str, uint32_t hval) { - uint8_t *s = str; + uint8_t *s = str; - /* - * FNV-1 hash each octet in the buffer - */ - while (*s) { + /* + * FNV-1 hash each octet in the buffer + */ + while (*s) { - /* multiply by the 32 bit FNV magic prime mod 2^32 */ + /* multiply by the 32 bit FNV magic prime mod 2^32 */ #if defined(NO_FNV_GCC_OPTIMIZATION) - hval *= FNV1_32_PRIME; + hval *= FNV1_32_PRIME; #else - hval += (hval<<1) + (hval<<4) + (hval<<7) + (hval<<8) + (hval<<24); + hval += (hval << 1) + (hval << 4) + (hval << 7) + (hval << 8) + (hval << 24); #endif - /* xor the bottom with the current octet */ - hval ^= (uint32_t)*s++; - } + /* xor the bottom with the current octet */ + hval ^= (uint32_t) * s++; + } - /* return our new hash value */ - return hval; + /* return our new hash value */ + return hval; } diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 77f26e0ca4..cdea2a4e02 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -321,25 +321,25 @@ private: * * @return 0 if calibration is ok, 1 else */ - int check_calibration(); + int check_calibration(); - /** - * Check the current scale calibration - * - * @return 0 if scale calibration is ok, 1 else - */ - int check_scale(); + /** + * Check the current scale calibration + * + * @return 0 if scale calibration is ok, 1 else + */ + int check_scale(); - /** - * Check the current offset calibration - * - * @return 0 if offset calibration is ok, 1 else - */ - int check_offset(); + /** + * Check the current offset calibration + * + * @return 0 if offset calibration is ok, 1 else + */ + int check_offset(); /* this class has pointer data members, do not allow copying it */ - HMC5883(const HMC5883&); - HMC5883 operator=(const HMC5883&); + HMC5883(const HMC5883 &); + HMC5883 operator=(const HMC5883 &); }; /* @@ -397,11 +397,13 @@ HMC5883::~HMC5883() /* make sure we are truly inactive */ stop(); - if (_reports != nullptr) + if (_reports != nullptr) { delete _reports; + } - if (_class_instance != -1) + if (_class_instance != -1) { unregister_class_devname(MAG_BASE_DEVICE_PATH, _class_instance); + } // free perf counters perf_free(_sample_perf); @@ -417,6 +419,7 @@ HMC5883::init() int ret = ERROR; ret = CDev::init(); + if (ret != OK) { DEVICE_DEBUG("CDev init failed"); goto out; @@ -424,8 +427,10 @@ HMC5883::init() /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(mag_report)); - if (_reports == nullptr) + + if (_reports == nullptr) { goto out; + } /* reset the device configuration */ reset(); @@ -489,14 +494,16 @@ int HMC5883::set_range(unsigned range) */ ret = write_reg(ADDR_CONF_B, (_range_bits << 5)); - if (OK != ret) + if (OK != ret) { perf_count(_comms_errors); + } uint8_t range_bits_in = 0; ret = read_reg(ADDR_CONF_B, range_bits_in); - if (OK != ret) + if (OK != ret) { perf_count(_comms_errors); + } return !(range_bits_in == (_range_bits << 5)); } @@ -512,15 +519,19 @@ void HMC5883::check_range(void) uint8_t range_bits_in = 0; ret = read_reg(ADDR_CONF_B, range_bits_in); + if (OK != ret) { perf_count(_comms_errors); return; } - if (range_bits_in != (_range_bits<<5)) { + + if (range_bits_in != (_range_bits << 5)) { perf_count(_range_errors); ret = write_reg(ADDR_CONF_B, (_range_bits << 5)); - if (OK != ret) + + if (OK != ret) { perf_count(_comms_errors); + } } } @@ -535,15 +546,19 @@ void HMC5883::check_conf(void) uint8_t conf_reg_in = 0; ret = read_reg(ADDR_CONF_A, conf_reg_in); + if (OK != ret) { perf_count(_comms_errors); return; } + if (conf_reg_in != _conf_reg) { perf_count(_conf_errors); ret = write_reg(ADDR_CONF_A, _conf_reg); - if (OK != ret) + + if (OK != ret) { perf_count(_comms_errors); + } } } @@ -555,8 +570,9 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen) int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is enabled */ if (_measure_ticks > 0) { @@ -611,77 +627,84 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) switch (cmd) { case SENSORIOCSPOLLRATE: { - switch (arg) { + switch (arg) { /* switching to manual polling */ - case SENSOR_POLLRATE_MANUAL: - stop(); - _measure_ticks = 0; - return OK; + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; /* external signalling (DRDY) not supported */ - case SENSOR_POLLRATE_EXTERNAL: + case SENSOR_POLLRATE_EXTERNAL: /* zero would be bad */ - case 0: - return -EINVAL; + 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); + 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(HMC5883_CONVERSION_INTERVAL); + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL); - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } - return OK; - } + return OK; + } /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); + 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); + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); - /* check against maximum rate */ - if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL)) - return -EINVAL; + /* check against maximum rate */ + if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL)) { + return -EINVAL; + } - /* update interval for next measurement */ - _measure_ticks = ticks; + /* update interval for next measurement */ + _measure_ticks = ticks; - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } - return OK; + return OK; + } } } - } case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) + if (_measure_ticks == 0) { return SENSOR_POLLRATE_MANUAL; + } - return 1000000/TICK2USEC(_measure_ticks); + return 1000000 / TICK2USEC(_measure_ticks); case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) + if ((arg < 1) || (arg > 100)) { return -EINVAL; + } irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { irqrestore(flags); return -ENOMEM; } + irqrestore(flags); return OK; @@ -699,7 +722,7 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) case MAGIOCGSAMPLERATE: /* same as pollrate because device is in single measurement mode*/ - return 1000000/TICK2USEC(_measure_ticks); + return 1000000 / TICK2USEC(_measure_ticks); case MAGIOCSRANGE: return set_range(arg); @@ -815,8 +838,9 @@ HMC5883::cycle() } /* measurement phase */ - if (OK != measure()) + if (OK != measure()) { DEVICE_DEBUG("measure error"); + } /* next phase is collection */ _collect_phase = true; @@ -839,8 +863,9 @@ HMC5883::measure() */ ret = write_reg(ADDR_MODE, MODE_REG_SINGLE_MODE); - if (OK != ret) + if (OK != ret) { perf_count(_comms_errors); + } return ret; } @@ -872,7 +897,7 @@ HMC5883::collect() /* this should be fairly close to the end of the measurement, so the best approximation of the time */ new_report.timestamp = hrt_absolute_time(); - new_report.error_count = perf_event_count(_comms_errors); + new_report.error_count = perf_event_count(_comms_errors); /* * @note We could read the status register here, which could tell us that @@ -908,6 +933,7 @@ HMC5883::collect() /* get measurements from the device */ new_report.temperature = 0; + if (_conf_reg & HMC5983_TEMP_SENSOR_ENABLE) { /* if temperature compensation is enabled read the @@ -923,13 +949,16 @@ HMC5883::collect() ret = _interface->read(ADDR_TEMP_OUT_MSB, raw_temperature, sizeof(raw_temperature)); + if (ret == OK) { int16_t temp16 = (((int16_t)raw_temperature[0]) << 8) + - raw_temperature[1]; - new_report.temperature = 25 + (temp16 / (16*8.0f)); + raw_temperature[1]; + new_report.temperature = 25 + (temp16 / (16 * 8.0f)); _temperature_error_count = 0; + } else { _temperature_error_count++; + if (_temperature_error_count == 10) { /* it probably really is an old HMC5883, @@ -940,6 +969,7 @@ HMC5883::collect() set_temperature_compensation(0); } } + } else { new_report.temperature = _last_report.temperature; } @@ -961,17 +991,18 @@ HMC5883::collect() // XXX revisit for SPI part, might require a bus type IOCTL unsigned dummy; sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy); + if (sensor_is_onboard) { // convert onboard so it matches offboard for the // scaling below report.y = -report.y; report.x = -report.x; - } + } - /* the standard external mag by 3DR has x pointing to the + /* the standard external mag by 3DR has x pointing to the * right, y pointing backwards, and z down, therefore switch x * and y and invert y */ - xraw_f = -report.y; + xraw_f = -report.y; yraw_f = report.x; zraw_f = report.z; @@ -989,12 +1020,14 @@ HMC5883::collect() if (_mag_topic != nullptr) { /* publish it */ orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); + } else { _mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_report, - &_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX); + &_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX); - if (_mag_topic == nullptr) + if (_mag_topic == nullptr) { DEVICE_DEBUG("ADVERT FAIL"); + } } } @@ -1016,9 +1049,11 @@ HMC5883::collect() vehicles have it is worth checking for. */ check_counter = perf_event_count(_sample_perf) % 256; + if (check_counter == 0) { check_range(); } + if (check_counter == 128) { check_conf(); } @@ -1075,7 +1110,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) } /* Set to 2.5 Gauss. We ask for 3 to get the right part of - * the chained if statement above. */ + * the chained if statement above. */ if (OK != ioctl(filp, MAGIOCSRANGE, 3)) { warnx("FAILED: MAGIOCSRANGE 3.3 Ga"); ret = 1; @@ -1146,9 +1181,11 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) ret = -EIO; goto out; } + float cal[3] = {fabsf(expected_cal[0] / report.x), fabsf(expected_cal[1] / report.y), - fabsf(expected_cal[2] / report.z)}; + fabsf(expected_cal[2] / report.z) + }; if (cal[0] > 0.7f && cal[0] < 1.35f && cal[1] > 0.7f && cal[1] < 1.35f && @@ -1211,10 +1248,11 @@ int HMC5883::check_scale() bool scale_valid; if ((-FLT_EPSILON + 1.0f < _scale.x_scale && _scale.x_scale < FLT_EPSILON + 1.0f) && - (-FLT_EPSILON + 1.0f < _scale.y_scale && _scale.y_scale < FLT_EPSILON + 1.0f) && - (-FLT_EPSILON + 1.0f < _scale.z_scale && _scale.z_scale < FLT_EPSILON + 1.0f)) { + (-FLT_EPSILON + 1.0f < _scale.y_scale && _scale.y_scale < FLT_EPSILON + 1.0f) && + (-FLT_EPSILON + 1.0f < _scale.z_scale && _scale.z_scale < FLT_EPSILON + 1.0f)) { /* scale is one */ scale_valid = false; + } else { scale_valid = true; } @@ -1228,10 +1266,11 @@ int HMC5883::check_offset() bool offset_valid; if ((-2.0f * FLT_EPSILON < _scale.x_offset && _scale.x_offset < 2.0f * FLT_EPSILON) && - (-2.0f * FLT_EPSILON < _scale.y_offset && _scale.y_offset < 2.0f * FLT_EPSILON) && - (-2.0f * FLT_EPSILON < _scale.z_offset && _scale.z_offset < 2.0f * FLT_EPSILON)) { + (-2.0f * FLT_EPSILON < _scale.y_offset && _scale.y_offset < 2.0f * FLT_EPSILON) && + (-2.0f * FLT_EPSILON < _scale.z_offset && _scale.z_offset < 2.0f * FLT_EPSILON)) { /* offset is zero */ offset_valid = false; + } else { offset_valid = true; } @@ -1247,7 +1286,7 @@ int HMC5883::check_calibration() if (_calibrated != (offset_valid && scale_valid)) { warnx("mag cal status changed %s%s", (scale_valid) ? "" : "scale invalid ", - (offset_valid) ? "" : "offset invalid"); + (offset_valid) ? "" : "offset invalid"); _calibrated = (offset_valid && scale_valid); } @@ -1261,10 +1300,12 @@ int HMC5883::set_excitement(unsigned enable) /* arm the excitement strap */ ret = read_reg(ADDR_CONF_A, _conf_reg); - if (OK != ret) + if (OK != ret) { perf_count(_comms_errors); + } _conf_reg &= ~0x03; // reset previous excitement mode + if (((int)enable) < 0) { _conf_reg |= 0x01; @@ -1273,12 +1314,13 @@ int HMC5883::set_excitement(unsigned enable) } - // ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)_conf_reg); + // ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)_conf_reg); ret = write_reg(ADDR_CONF_A, _conf_reg); - if (OK != ret) + if (OK != ret) { perf_count(_comms_errors); + } uint8_t conf_reg_ret = 0; read_reg(ADDR_CONF_A, conf_reg_ret); @@ -1317,11 +1359,12 @@ int HMC5883::set_temperature_compensation(unsigned enable) if (OK != ret) { perf_count(_comms_errors); - return -EIO; - } + return -EIO; + } if (enable != 0) { _conf_reg |= HMC5983_TEMP_SENSOR_ENABLE; + } else { _conf_reg &= ~HMC5983_TEMP_SENSOR_ENABLE; } @@ -1334,6 +1377,7 @@ int HMC5883::set_temperature_compensation(unsigned enable) } uint8_t conf_reg_ret = 0; + if (read_reg(ADDR_CONF_A, conf_reg_ret) != OK) { perf_count(_comms_errors); return -EIO; @@ -1383,7 +1427,7 @@ HMC5883::print_info() printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset); printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n", (double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale, - (double)(1.0f/_range_scale), (double)_range_ga); + (double)(1.0f / _range_scale), (double)_range_ga); printf("temperature %.2f\n", (double)_last_report.temperature); _reports->print_info("report queue"); } @@ -1436,16 +1480,20 @@ void usage(); bool start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation) { - if (bus.dev != nullptr) - errx(1,"bus option already started"); + if (bus.dev != nullptr) { + errx(1, "bus option already started"); + } device::Device *interface = bus.interface_constructor(bus.busnum); + if (interface->init() != OK) { delete interface; warnx("no device on bus %u", (unsigned)bus.busid); return false; } + bus.dev = new HMC5883(interface, bus.devpath, rotation); + if (bus.dev != nullptr && OK != bus.dev->init()) { delete bus.dev; bus.dev = NULL; @@ -1453,14 +1501,16 @@ start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation) } int fd = open(bus.devpath, O_RDONLY); + if (fd < 0) { return false; } if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { close(fd); - errx(1,"Failed to setup poll rate"); + errx(1, "Failed to setup poll rate"); } + close(fd); return true; @@ -1483,10 +1533,12 @@ start(enum HMC5883_BUS busid, enum Rotation rotation) // this device is already started continue; } + if (busid != HMC5883_BUS_ALL && bus_options[i].busid != busid) { // not the one that is asked for continue; } + started |= start_bus(bus_options[i], rotation); } @@ -1506,6 +1558,7 @@ struct hmc5883_bus_option &find_bus(enum HMC5883_BUS busid) return bus_options[i]; } } + errx(1, "bus %u not started", (unsigned)busid); } @@ -1526,31 +1579,37 @@ test(enum HMC5883_BUS busid) int fd = open(path, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "%s open failed (try 'hmc5883 start')", path); + } /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); - if (sz != sizeof(report)) + if (sz != sizeof(report)) { err(1, "immediate read failed"); + } warnx("single read"); warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); warnx("time: %lld", report.timestamp); /* check if mag is onboard or external */ - if ((ret = ioctl(fd, MAGIOCGEXTERNAL, 0)) < 0) + if ((ret = ioctl(fd, MAGIOCGEXTERNAL, 0)) < 0) { errx(1, "failed to get if mag is onboard or external"); + } + warnx("device active: %s", ret ? "external" : "onboard"); /* set the queue depth to 5 */ - if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 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)) + 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++) { @@ -1561,14 +1620,16 @@ test(enum HMC5883_BUS busid) fds.events = POLLIN; ret = poll(&fds, 1, 2000); - if (ret != 1) + 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)) + if (sz != sizeof(report)) { err(1, "periodic read failed"); + } warnx("periodic read %u", i); warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); @@ -1628,8 +1689,9 @@ int calibrate(enum HMC5883_BUS busid) int fd = open(path, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", path); + } if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) { warnx("failed to enable sensor calibration mode"); @@ -1651,14 +1713,17 @@ reset(enum HMC5883_BUS busid) int fd = open(path, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "failed "); + } - if (ioctl(fd, SENSORIOCRESET, 0) < 0) + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { err(1, "driver reset failed"); + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "driver poll restart failed"); + } exit(0); } @@ -1675,11 +1740,13 @@ temp_enable(enum HMC5883_BUS busid, bool enable) int fd = open(path, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "failed "); + } - if (ioctl(fd, MAGIOCSTEMPCOMP, (unsigned)enable) < 0) + if (ioctl(fd, MAGIOCSTEMPCOMP, (unsigned)enable) < 0) { err(1, "set temperature compensation failed"); + } close(fd); return 0; @@ -1719,7 +1786,7 @@ hmc5883_main(int argc, char *argv[]) int ch; enum HMC5883_BUS busid = HMC5883_BUS_ALL; enum Rotation rotation = ROTATION_NONE; - bool calibrate = false; + bool calibrate = false; bool temp_compensation = false; while ((ch = getopt(argc, argv, "XISR:CT")) != EOF) { @@ -1728,22 +1795,28 @@ hmc5883_main(int argc, char *argv[]) rotation = (enum Rotation)atoi(optarg); break; #if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC) + case 'I': busid = HMC5883_BUS_I2C_INTERNAL; break; #endif + case 'X': busid = HMC5883_BUS_I2C_EXTERNAL; break; + case 'S': busid = HMC5883_BUS_SPI; break; + case 'C': calibrate = true; break; + case 'T': temp_compensation = true; break; + default: hmc5883::usage(); exit(0); @@ -1757,42 +1830,51 @@ hmc5883_main(int argc, char *argv[]) */ if (!strcmp(verb, "start")) { hmc5883::start(busid, rotation); + if (calibrate && hmc5883::calibrate(busid) != 0) { errx(1, "calibration failed"); } + if (temp_compensation) { // we consider failing to setup temperature // compensation as non-fatal hmc5883::temp_enable(busid, true); } + exit(0); } /* * Test the driver/device. */ - if (!strcmp(verb, "test")) + if (!strcmp(verb, "test")) { hmc5883::test(busid); + } /* * Reset the driver. */ - if (!strcmp(verb, "reset")) + if (!strcmp(verb, "reset")) { hmc5883::reset(busid); + } /* * enable/disable temperature compensation */ - if (!strcmp(verb, "tempoff")) + if (!strcmp(verb, "tempoff")) { hmc5883::temp_enable(busid, false); - if (!strcmp(verb, "tempon")) + } + + if (!strcmp(verb, "tempon")) { hmc5883::temp_enable(busid, true); + } /* * Print driver information. */ - if (!strcmp(verb, "info") || !strcmp(verb, "status")) + if (!strcmp(verb, "info") || !strcmp(verb, "status")) { hmc5883::info(busid); + } /* * Autocalibrate the scaling diff --git a/src/drivers/hmc5883/hmc5883_i2c.cpp b/src/drivers/hmc5883/hmc5883_i2c.cpp index 6d5d36864b..d29ffbe0b4 100644 --- a/src/drivers/hmc5883/hmc5883_i2c.cpp +++ b/src/drivers/hmc5883/hmc5883_i2c.cpp @@ -31,11 +31,11 @@ * ****************************************************************************/ - /** - * @file HMC5883_I2C.cpp - * - * I2C interface for HMC5883 / HMC 5983 - */ +/** + * @file HMC5883_I2C.cpp + * + * I2C interface for HMC5883 / HMC 5983 + */ /* XXX trim includes */ #include @@ -72,7 +72,7 @@ public: virtual ~HMC5883_I2C(); virtual int init(); - virtual int read(unsigned address, void *data, unsigned count); + virtual int read(unsigned address, void *data, unsigned count); virtual int write(unsigned address, void *data, unsigned count); virtual int ioctl(unsigned operation, unsigned &arg); @@ -118,11 +118,13 @@ HMC5883_I2C::ioctl(unsigned operation, unsigned &arg) #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 if (_bus == PX4_I2C_BUS_EXPANSION) { return 1; + } else { return 0; } + #else - return 1; + return 1; #endif case DEVIOCGDEVICEID: diff --git a/src/drivers/hmc5883/hmc5883_spi.cpp b/src/drivers/hmc5883/hmc5883_spi.cpp index 2aae792f7c..2710f35802 100644 --- a/src/drivers/hmc5883/hmc5883_spi.cpp +++ b/src/drivers/hmc5883/hmc5883_spi.cpp @@ -31,11 +31,11 @@ * ****************************************************************************/ - /** - * @file HMC5883_SPI.cpp - * - * SPI interface for HMC5983 - */ +/** + * @file HMC5883_SPI.cpp + * + * SPI interface for HMC5983 + */ /* XXX trim includes */ #include @@ -77,7 +77,7 @@ public: virtual ~HMC5883_SPI(); virtual int init(); - virtual int read(unsigned address, void *data, unsigned count); + virtual int read(unsigned address, void *data, unsigned count); virtual int write(unsigned address, void *data, unsigned count); virtual int ioctl(unsigned operation, unsigned &arg); @@ -91,7 +91,7 @@ HMC5883_SPI_interface(int bus) } HMC5883_SPI::HMC5883_SPI(int bus, spi_dev_e device) : - SPI("HMC5883_SPI", nullptr, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz */) + SPI("HMC5883_SPI", nullptr, bus, device, SPIDEV_MODE3, 11 * 1000 * 1000 /* will be rounded to 10.4 MHz */) { _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; } @@ -106,6 +106,7 @@ HMC5883_SPI::init() int ret; ret = SPI::init(); + if (ret != OK) { DEVICE_DEBUG("SPI init failed"); return -EIO; @@ -148,10 +149,9 @@ HMC5883_SPI::ioctl(unsigned operation, unsigned &arg) case DEVIOCGDEVICEID: return CDev::ioctl(nullptr, operation, arg); - default: - { - ret = -EINVAL; - } + default: { + ret = -EINVAL; + } } return ret; diff --git a/src/drivers/hott/comms.cpp b/src/drivers/hott/comms.cpp index 60a49b559c..378b5b75d6 100644 --- a/src/drivers/hott/comms.cpp +++ b/src/drivers/hott/comms.cpp @@ -57,10 +57,11 @@ open_uart(const char *device) if (uart < 0) { err(1, "ERR: opening %s", device); } - - /* Back up the original uart configuration to restore it after exit */ + + /* Back up the original uart configuration to restore it after exit */ int termios_state; struct termios uart_config_original; + if ((termios_state = tcgetattr(uart, &uart_config_original)) < 0) { close(uart); err(1, "ERR: %s: %d", device, termios_state); @@ -77,7 +78,7 @@ open_uart(const char *device) if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { close(uart); err(1, "ERR: %s: %d (cfsetispeed, cfsetospeed)", - device, termios_state); + device, termios_state); } if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { diff --git a/src/drivers/hott/hott_sensors/hott_sensors.cpp b/src/drivers/hott/hott_sensors/hott_sensors.cpp index 29680a279f..f5c947d4f6 100644 --- a/src/drivers/hott/hott_sensors/hott_sensors.cpp +++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp @@ -104,15 +104,16 @@ int recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id) { static const int timeout_ms = 1000; - + struct pollfd fds; fds.fd = uart; fds.events = POLLIN; - + // XXX should this poll be inside the while loop??? if (poll(&fds, 1, timeout_ms) > 0) { int i = 0; bool stop_byte_read = false; + while (true) { read(uart, &buffer[i], sizeof(buffer[i])); @@ -121,14 +122,17 @@ recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id) *size = ++i; return OK; } + // XXX can some other field not have the STOP BYTE value? if (buffer[i] == STOP_BYTE) { *id = buffer[1]; stop_byte_read = true; } + i++; } } + return ERROR; } @@ -156,6 +160,7 @@ hott_sensors_thread_main(int argc, char *argv[]) /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ const int uart = open_uart(device); + if (uart < 0) { errx(1, "Open fail, exiting."); thread_running = false; @@ -166,6 +171,7 @@ hott_sensors_thread_main(int argc, char *argv[]) uint8_t buffer[MAX_MESSAGE_BUFFER_SIZE]; size_t size = 0; uint8_t id = 0; + while (!thread_should_exit) { // Currently we only support a General Air Module sensor. build_gam_request(&buffer[0], &size); @@ -179,6 +185,7 @@ hott_sensors_thread_main(int argc, char *argv[]) // Determine which moduel sent it and process accordingly. if (id == GAM_SENSOR_ID) { publish_gam_message(buffer); + } else { warnx("Unknown sensor ID: %d", id); } @@ -210,11 +217,11 @@ hott_sensors_main(int argc, char *argv[]) thread_should_exit = false; deamon_task = px4_task_spawn_cmd(daemon_name, - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 1024, - hott_sensors_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 1024, + hott_sensors_thread_main, + (argv) ? (char *const *)&argv[2] : (char *const *)NULL); exit(0); } diff --git a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp index a1a5b080c5..f5aa0ec58a 100644 --- a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp +++ b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp @@ -90,7 +90,7 @@ recv_req_id(int uart, uint8_t *id) static const int timeout_ms = 1000; // TODO make it a define uint8_t mode; - + struct pollfd fds; fds.fd = uart; fds.events = POLLIN; @@ -106,6 +106,7 @@ recv_req_id(int uart, uint8_t *id) /* Read the device ID being polled */ read(uart, id, sizeof(*id)); + } else { warnx("UART timeout on TX/RX port"); return ERROR; @@ -120,6 +121,7 @@ send_data(int uart, uint8_t *buffer, size_t size) usleep(POST_READ_DELAY_IN_USECS); uint16_t checksum = 0; + for (size_t i = 0; i < size; i++) { if (i == size - 1) { /* Set the checksum: the first uint8_t is taken as the checksum. */ @@ -167,6 +169,7 @@ hott_telemetry_thread_main(int argc, char *argv[]) /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ const int uart = open_uart(device); + if (uart < 0) { errx(1, "Failed opening HoTT UART, exiting."); thread_running = false; @@ -178,6 +181,7 @@ hott_telemetry_thread_main(int argc, char *argv[]) size_t size = 0; uint8_t id = 0; bool connected = true; + while (!thread_should_exit) { // Listen for and serve poll from the receiver. if (recv_req_id(uart, &id) == OK) { @@ -190,9 +194,11 @@ hott_telemetry_thread_main(int argc, char *argv[]) case EAM_SENSOR_ID: build_eam_response(buffer, &size); break; + case GAM_SENSOR_ID: build_gam_response(buffer, &size); break; + case GPS_SENSOR_ID: build_gps_response(buffer, &size); break; @@ -202,6 +208,7 @@ hott_telemetry_thread_main(int argc, char *argv[]) } send_data(uart, buffer, size); + } else { connected = false; warnx("syncing"); @@ -236,11 +243,11 @@ hott_telemetry_main(int argc, char *argv[]) thread_should_exit = false; deamon_task = px4_task_spawn_cmd(daemon_name, - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2048, - hott_telemetry_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + hott_telemetry_thread_main, + (argv) ? (char *const *)&argv[2] : (char *const *)NULL); exit(0); } diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp index 61daa302fb..db225a2b98 100644 --- a/src/drivers/hott/messages.cpp +++ b/src/drivers/hott/messages.cpp @@ -69,7 +69,7 @@ static bool _home_position_set = false; static double _home_lat = 0.0d; static double _home_lon = 0.0d; -void +void init_sub_messages(void) { _battery_sub = orb_subscribe(ORB_ID(battery_status)); @@ -80,7 +80,7 @@ init_sub_messages(void) _esc_sub = orb_subscribe(ORB_ID(esc_status)); } -void +void init_pub_messages(void) { } @@ -122,12 +122,13 @@ publish_gam_message(const uint8_t *buffer) /* announce the esc if needed, just publish else */ if (_esc_pub != nullptr) { orb_publish(ORB_ID(esc_status), _esc_pub, &esc); + } else { _esc_pub = orb_advertise(ORB_ID(esc_status), &esc); } } -void +void build_eam_response(uint8_t *buffer, size_t *size) { /* get a local copy of the current sensor values */ @@ -147,7 +148,7 @@ build_eam_response(uint8_t *buffer, size_t *size) msg.start = START_BYTE; 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.temperature2 = msg.temperature1 - BOARD_TEMP_OFFSET_DEG; @@ -170,7 +171,7 @@ build_eam_response(uint8_t *buffer, size_t *size) memcpy(buffer, &msg, *size); } -void +void build_gam_response(uint8_t *buffer, size_t *size) { /* get a local copy of the ESC Status values */ @@ -185,7 +186,7 @@ build_gam_response(uint8_t *buffer, size_t *size) msg.start = START_BYTE; msg.gam_sensor_id = GAM_SENSOR_ID; msg.sensor_text_id = GAM_SENSOR_TEXT_ID; - + msg.temperature1 = (uint8_t)(esc.esc[0].esc_temperature + 20.0F); msg.temperature2 = 20; // 0 deg. C. @@ -205,7 +206,7 @@ build_gam_response(uint8_t *buffer, size_t *size) memcpy(buffer, &msg, *size); } -void +void build_gps_response(uint8_t *buffer, size_t *size) { /* get a local copy of the current sensor values */ @@ -213,7 +214,7 @@ build_gps_response(uint8_t *buffer, size_t *size) memset(&raw, 0, sizeof(raw)); orb_copy(ORB_ID(sensor_combined), _sensor_sub, &raw); - /* get a local copy of the battery data */ + /* get a local copy of the battery data */ struct vehicle_gps_position_s gps; memset(&gps, 0, sizeof(gps)); orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &gps); @@ -241,12 +242,13 @@ build_gps_response(uint8_t *buffer, size_t *size) uint16_t speed = (uint16_t)(gps.vel_m_s * 3.6f); msg.gps_speed_L = (uint8_t)speed & 0xff; msg.gps_speed_H = (uint8_t)(speed >> 8) & 0xff; - - /* Get latitude in degrees, minutes and seconds */ - double lat = ((double)(gps.lat))*1e-7d; - /* Set the N or S specifier */ + /* Get latitude in degrees, minutes and seconds */ + double lat = ((double)(gps.lat)) * 1e-7d; + + /* Set the N or S specifier */ msg.latitude_ns = 0; + if (lat < 0) { msg.latitude_ns = 1; lat = abs(lat); @@ -265,32 +267,34 @@ build_gps_response(uint8_t *buffer, size_t *size) msg.latitude_sec_H = (uint8_t)(lat_sec >> 8) & 0xff; /* Get longitude in degrees, minutes and seconds */ - double lon = ((double)(gps.lon))*1e-7d; + double lon = ((double)(gps.lon)) * 1e-7d; /* Set the E or W specifier */ msg.longitude_ew = 0; + if (lon < 0) { msg.longitude_ew = 1; lon = abs(lon); } convert_to_degrees_minutes_seconds(lon, °, &min, &sec); - + uint16_t lon_min = (uint16_t)(deg * 100 + min); msg.longitude_min_L = (uint8_t)lon_min & 0xff; msg.longitude_min_H = (uint8_t)(lon_min >> 8) & 0xff; uint16_t lon_sec = (uint16_t)(sec); msg.longitude_sec_L = (uint8_t)lon_sec & 0xff; msg.longitude_sec_H = (uint8_t)(lon_sec >> 8) & 0xff; - + /* Altitude */ - uint16_t alt = (uint16_t)(gps.alt*1e-3f + 500.0f); + uint16_t alt = (uint16_t)(gps.alt * 1e-3f + 500.0f); msg.altitude_L = (uint8_t)alt & 0xff; msg.altitude_H = (uint8_t)(alt >> 8) & 0xff; /* Get any (and probably only ever one) _home_sub postion report */ bool updated; orb_check(_home_sub, &updated); + if (updated) { /* get a local copy of the home position data */ struct home_position_s home; diff --git a/src/drivers/hott/messages.h b/src/drivers/hott/messages.h index a116a50dd6..504c9f19ec 100644 --- a/src/drivers/hott/messages.h +++ b/src/drivers/hott/messages.h @@ -125,63 +125,63 @@ struct eam_module_msg { #define GAM_SENSOR_TEXT_ID 0xd0 struct gam_module_msg { - uint8_t start; /**< Start byte */ - uint8_t gam_sensor_id; /**< GAM sensor id */ - uint8_t warning_beeps; - uint8_t sensor_text_id; - uint8_t alarm_invers1; - uint8_t alarm_invers2; - uint8_t cell1; /**< Lipo cell voltages. Not supported. */ - uint8_t cell2; - uint8_t cell3; - uint8_t cell4; - uint8_t cell5; - uint8_t cell6; - uint8_t batt1_L; /**< Battery 1 voltage LSB value. 0.1V steps. 50 = 5.5V */ - uint8_t batt1_H; - uint8_t batt2_L; /**< Battery 2 voltage LSB value. 0.1V steps. 50 = 5.5V */ - uint8_t batt2_H; - uint8_t temperature1; /**< Temperature 1. offset of 20. a value of 20 = 0°C */ - uint8_t temperature2; /**< Temperature 2. offset of 20. a value of 20 = 0°C */ - uint8_t fuel_procent; /**< Fuel capacity in %. Values 0 - 100 */ - /**< Graphical display ranges: 0 25% 50% 75% 100% */ - uint8_t fuel_ml_L; /**< Fuel in ml scale. Full = 65535 */ - uint8_t fuel_ml_H; - uint8_t rpm_L; /**< RPM in 10 RPM steps. 300 = 3000rpm */ - uint8_t rpm_H; - uint8_t altitude_L; /**< Altitude in meters. offset of 500, 500 = 0m */ - uint8_t altitude_H; - uint8_t climbrate_L; /**< Climb rate in 0.01m/s. Value of 30000 = 0.00 m/s */ - uint8_t climbrate_H; - uint8_t climbrate3s; /**< Climb rate in m/3sec. Value of 120 = 0m/3sec */ - uint8_t current_L; /**< Current in 0.1A steps */ - uint8_t current_H; - uint8_t main_voltage_L; /**< Main power voltage using 0.1V steps */ - uint8_t main_voltage_H; - uint8_t batt_cap_L; /**< Used battery capacity in 10mAh steps */ - uint8_t batt_cap_H; - uint8_t speed_L; /**< Speed in km/h */ - uint8_t speed_H; - uint8_t min_cell_volt; /**< Minimum cell voltage in 2mV steps. 124 = 2,48V */ - uint8_t min_cell_volt_num; /**< Number of the cell with the lowest voltage */ - uint8_t rpm2_L; /**< RPM in 10 RPM steps. 300 = 3000rpm */ - uint8_t rpm2_H; - uint8_t general_error_number; /**< Voice error == 12. TODO: more docu */ - uint8_t pressure; /**< Pressure up to 16bar. 0,1bar scale. 20 = 2bar */ - uint8_t version; - uint8_t stop; /**< Stop byte */ - uint8_t checksum; /**< Lower 8-bits of all bytes summed */ + uint8_t start; /**< Start byte */ + uint8_t gam_sensor_id; /**< GAM sensor id */ + uint8_t warning_beeps; + uint8_t sensor_text_id; + uint8_t alarm_invers1; + uint8_t alarm_invers2; + uint8_t cell1; /**< Lipo cell voltages. Not supported. */ + uint8_t cell2; + uint8_t cell3; + uint8_t cell4; + uint8_t cell5; + uint8_t cell6; + uint8_t batt1_L; /**< Battery 1 voltage LSB value. 0.1V steps. 50 = 5.5V */ + uint8_t batt1_H; + uint8_t batt2_L; /**< Battery 2 voltage LSB value. 0.1V steps. 50 = 5.5V */ + uint8_t batt2_H; + uint8_t temperature1; /**< Temperature 1. offset of 20. a value of 20 = 0°C */ + uint8_t temperature2; /**< Temperature 2. offset of 20. a value of 20 = 0°C */ + uint8_t fuel_procent; /**< Fuel capacity in %. Values 0 - 100 */ + /**< Graphical display ranges: 0 25% 50% 75% 100% */ + uint8_t fuel_ml_L; /**< Fuel in ml scale. Full = 65535 */ + uint8_t fuel_ml_H; + uint8_t rpm_L; /**< RPM in 10 RPM steps. 300 = 3000rpm */ + uint8_t rpm_H; + uint8_t altitude_L; /**< Altitude in meters. offset of 500, 500 = 0m */ + uint8_t altitude_H; + uint8_t climbrate_L; /**< Climb rate in 0.01m/s. Value of 30000 = 0.00 m/s */ + uint8_t climbrate_H; + uint8_t climbrate3s; /**< Climb rate in m/3sec. Value of 120 = 0m/3sec */ + uint8_t current_L; /**< Current in 0.1A steps */ + uint8_t current_H; + uint8_t main_voltage_L; /**< Main power voltage using 0.1V steps */ + uint8_t main_voltage_H; + uint8_t batt_cap_L; /**< Used battery capacity in 10mAh steps */ + uint8_t batt_cap_H; + uint8_t speed_L; /**< Speed in km/h */ + uint8_t speed_H; + uint8_t min_cell_volt; /**< Minimum cell voltage in 2mV steps. 124 = 2,48V */ + uint8_t min_cell_volt_num; /**< Number of the cell with the lowest voltage */ + uint8_t rpm2_L; /**< RPM in 10 RPM steps. 300 = 3000rpm */ + uint8_t rpm2_H; + uint8_t general_error_number; /**< Voice error == 12. TODO: more docu */ + uint8_t pressure; /**< Pressure up to 16bar. 0,1bar scale. 20 = 2bar */ + uint8_t version; + uint8_t stop; /**< Stop byte */ + uint8_t checksum; /**< Lower 8-bits of all bytes summed */ }; /* GPS sensor constants. */ #define GPS_SENSOR_ID 0x8a #define GPS_SENSOR_TEXT_ID 0xa0 -/** +/** * The GPS sensor message - * Struct based on: https://code.google.com/p/diy-hott-gps/downloads + * Struct based on: https://code.google.com/p/diy-hott-gps/downloads */ -struct gps_module_msg { +struct gps_module_msg { uint8_t start; /**< Start byte */ uint8_t sensor_id; /**< GPS sensor ID*/ uint8_t warning; /**< 0…= warning beeps */ @@ -191,19 +191,19 @@ struct gps_module_msg { uint8_t flight_direction; /**< 119 = Flightdir./dir. 1 = 2°; 0° (North), 9 0° (East), 180° (South), 270° (West) */ uint8_t gps_speed_L; /**< 8 = /GPS speed low byte 8km/h */ uint8_t gps_speed_H; /**< 0 = /GPS speed high byte */ - + uint8_t latitude_ns; /**< 000 = N = 48°39’988 */ uint8_t latitude_min_L; /**< 231 0xE7 = 0x12E7 = 4839 */ uint8_t latitude_min_H; /**< 018 18 = 0x12 */ uint8_t latitude_sec_L; /**< 171 220 = 0xDC = 0x03DC =0988 */ uint8_t latitude_sec_H; /**< 016 3 = 0x03 */ - + uint8_t longitude_ew; /**< 000 = E= 9° 25’9360 */ uint8_t longitude_min_L; /**< 150 157 = 0x9D = 0x039D = 0925 */ uint8_t longitude_min_H; /**< 003 3 = 0x03 */ uint8_t longitude_sec_L; /**< 056 144 = 0x90 0x2490 = 9360 */ uint8_t longitude_sec_H; /**< 004 36 = 0x24 */ - + uint8_t distance_L; /**< 027 123 = /distance low byte 6 = 6 m */ uint8_t distance_H; /**< 036 35 = /distance high byte */ uint8_t altitude_L; /**< 243 244 = /Altitude low byte 500 = 0m */ diff --git a/src/drivers/irlock/irlock.cpp b/src/drivers/irlock/irlock.cpp index 19b0de6d5f..8be2f8429b 100644 --- a/src/drivers/irlock/irlock.cpp +++ b/src/drivers/irlock/irlock.cpp @@ -388,8 +388,8 @@ int IRLOCK::read_device_block(struct irlock_s *block) /** 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->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; diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 43a24359eb..266c5283be 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -206,7 +206,7 @@ extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); } class L3GD20 : public device::SPI { public: - L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotation); + L3GD20(int bus, const char *path, spi_dev_e device, enum Rotation rotation); virtual ~L3GD20(); virtual int init(); @@ -389,11 +389,11 @@ private: * * @return 0 on success, 1 on failure */ - int self_test(); + int self_test(); /* this class does not allow copying */ - L3GD20(const L3GD20&); - L3GD20 operator=(const L3GD20&); + L3GD20(const L3GD20 &); + L3GD20 operator=(const L3GD20 &); }; /* @@ -401,16 +401,18 @@ private: that ADDR_WHO_AM_I must be first in the list. */ const uint8_t L3GD20::_checked_registers[L3GD20_NUM_CHECKED_REGISTERS] = { ADDR_WHO_AM_I, - ADDR_CTRL_REG1, - ADDR_CTRL_REG2, - ADDR_CTRL_REG3, - ADDR_CTRL_REG4, - ADDR_CTRL_REG5, - ADDR_FIFO_CTRL_REG, - ADDR_LOW_ODR }; + ADDR_CTRL_REG1, + ADDR_CTRL_REG2, + ADDR_CTRL_REG3, + ADDR_CTRL_REG4, + ADDR_CTRL_REG5, + ADDR_FIFO_CTRL_REG, + ADDR_LOW_ODR + }; -L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotation) : - SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */), +L3GD20::L3GD20(int bus, const char *path, spi_dev_e device, enum Rotation rotation) : + SPI("L3GD20", path, bus, device, SPIDEV_MODE3, + 11 * 1000 * 1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */), _call{}, _call_interval(0), _reports(nullptr), @@ -456,11 +458,13 @@ L3GD20::~L3GD20() stop(); /* free any existing reports */ - if (_reports != nullptr) + if (_reports != nullptr) { delete _reports; + } - if (_class_instance != -1) + if (_class_instance != -1) { unregister_class_devname(GYRO_BASE_DEVICE_PATH, _class_instance); + } /* delete the perf counter */ perf_free(_sample_perf); @@ -475,14 +479,16 @@ L3GD20::init() int ret = ERROR; /* do SPI init (and probe) first */ - if (SPI::init() != OK) + if (SPI::init() != OK) { goto out; + } /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report)); - if (_reports == nullptr) + if (_reports == nullptr) { goto out; + } _class_instance = register_class_devname(GYRO_BASE_DEVICE_PATH); @@ -495,7 +501,7 @@ L3GD20::init() _reports->get(&grp); _gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, - &_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); + &_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); if (_gyro_topic == nullptr) { DEVICE_DEBUG("failed to create sensor_gyro publication"); @@ -517,13 +523,15 @@ L3GD20::probe() /* verify that the device is attached and functioning, accept * L3GD20, L3GD20H and L3G4200D */ - if ((v=read_reg(ADDR_WHO_AM_I)) == WHO_I_AM) { + if ((v = read_reg(ADDR_WHO_AM_I)) == WHO_I_AM) { _orientation = SENSOR_BOARD_ROTATION_DEFAULT; success = true; - } else if ((v=read_reg(ADDR_WHO_AM_I)) == WHO_I_AM_H) { + + } else if ((v = read_reg(ADDR_WHO_AM_I)) == WHO_I_AM_H) { _orientation = SENSOR_BOARD_ROTATION_180_DEG; success = true; - } else if ((v=read_reg(ADDR_WHO_AM_I)) == WHO_I_AM_L3G4200D) { + + } else if ((v = read_reg(ADDR_WHO_AM_I)) == WHO_I_AM_L3G4200D) { /* Detect the L3G4200D used on AeroCore */ _is_l3g4200d = true; _orientation = SENSOR_BOARD_ROTATION_DEFAULT; @@ -546,8 +554,9 @@ L3GD20::read(struct file *filp, char *buffer, size_t buflen) int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is enabled */ if (_call_interval > 0) { @@ -588,28 +597,29 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCSPOLLRATE: { switch (arg) { - /* switching to manual polling */ + /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _call_interval = 0; return OK; - /* external signalling not supported */ + /* external signalling not supported */ case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ + /* zero would be bad */ case 0: return -EINVAL; - /* set default/max polling rate */ + /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: if (_is_l3g4200d) { return ioctl(filp, SENSORIOCSPOLLRATE, L3G4200D_DEFAULT_RATE); } + return ioctl(filp, SENSORIOCSPOLLRATE, L3GD20_DEFAULT_RATE); - /* adjust to a legal polling interval in Hz */ + /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ bool want_start = (_call_interval == 0); @@ -618,23 +628,25 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) unsigned ticks = 1000000 / arg; /* check against maximum sane rate */ - if (ticks < 1000) + if (ticks < 1000) { return -EINVAL; + } /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ _call_interval = ticks; - _call.period = _call_interval - L3GD20_TIMER_REDUCTION; + _call.period = _call_interval - L3GD20_TIMER_REDUCTION; /* adjust filters */ float cutoff_freq_hz = _gyro_filter_x.get_cutoff_freq(); - float sample_rate = 1.0e6f/ticks; + float sample_rate = 1.0e6f / ticks; set_driver_lowpass_filter(sample_rate, cutoff_freq_hz); /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } @@ -642,25 +654,29 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_call_interval == 0) + if (_call_interval == 0) { return SENSOR_POLLRATE_MANUAL; + } return 1000000 / _call_interval; case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } - irqstate_t flags = irqsave(); - if (!_reports->resize(arg)) { irqrestore(flags); - return -ENOMEM; - } - irqrestore(flags); - return OK; - } + return OK; + } case SENSORIOCGQUEUEDEPTH: return _reports->size(); @@ -676,12 +692,12 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) return _current_rate; case GYROIOCSLOWPASS: { - float cutoff_freq_hz = arg; - float sample_rate = 1.0e6f / _call_interval; - set_driver_lowpass_filter(sample_rate, cutoff_freq_hz); + float cutoff_freq_hz = arg; + float sample_rate = 1.0e6f / _call_interval; + set_driver_lowpass_filter(sample_rate, cutoff_freq_hz); - return OK; - } + return OK; + } case GYROIOCGLOWPASS: return static_cast(_gyro_filter_x.get_cutoff_freq()); @@ -741,7 +757,8 @@ void L3GD20::write_checked_reg(unsigned reg, uint8_t value) { write_reg(reg, value); - for (uint8_t i=0; iprint_info("report queue"); - ::printf("checked_next: %u\n", _checked_next); - for (uint8_t i=0; i zero or larger than 25 dps */ - if (fabsf(_gyro_scale.x_offset) > L3GD20_MAX_OFFSET || fabsf(_gyro_scale.x_offset) < 0.000001f) - return 1; - if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f) + if (fabsf(_gyro_scale.x_offset) > L3GD20_MAX_OFFSET || fabsf(_gyro_scale.x_offset) < 0.000001f) { return 1; + } - if (fabsf(_gyro_scale.y_offset) > L3GD20_MAX_OFFSET || fabsf(_gyro_scale.y_offset) < 0.000001f) - return 1; - if (fabsf(_gyro_scale.y_scale - 1.0f) > 0.3f) + if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f) { return 1; + } - if (fabsf(_gyro_scale.z_offset) > L3GD20_MAX_OFFSET || fabsf(_gyro_scale.z_offset) < 0.000001f) + if (fabsf(_gyro_scale.y_offset) > L3GD20_MAX_OFFSET || fabsf(_gyro_scale.y_offset) < 0.000001f) { return 1; - if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f) + } + + if (fabsf(_gyro_scale.y_scale - 1.0f) > 0.3f) { return 1; + } + + if (fabsf(_gyro_scale.z_offset) > L3GD20_MAX_OFFSET || fabsf(_gyro_scale.z_offset) < 0.000001f) { + return 1; + } + + if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f) { + return 1; + } return 0; } @@ -1165,36 +1204,42 @@ start(bool external_bus, enum Rotation rotation) { int fd; - if (g_dev != nullptr) + if (g_dev != nullptr) { errx(0, "already started"); + } /* create the driver */ - if (external_bus) { + if (external_bus) { #ifdef PX4_SPI_BUS_EXT g_dev = new L3GD20(PX4_SPI_BUS_EXT, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_EXT_GYRO, rotation); #else errx(0, "External SPI not available"); #endif + } else { g_dev = new L3GD20(PX4_SPI_BUS_SENSORS, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO, rotation); } - if (g_dev == nullptr) + if (g_dev == nullptr) { goto fail; + } - if (OK != g_dev->init()) + if (OK != g_dev->init()) { goto fail; + } /* set the poll rate to default, starts automatic data collection */ fd = open(L3GD20_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { goto fail; + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { goto fail; + } - close(fd); + close(fd); exit(0); fail: @@ -1222,18 +1267,21 @@ test() /* get the driver */ fd_gyro = open(L3GD20_DEVICE_PATH, O_RDONLY); - if (fd_gyro < 0) + if (fd_gyro < 0) { err(1, "%s open failed", L3GD20_DEVICE_PATH); + } /* reset to manual polling */ - if (ioctl(fd_gyro, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) + if (ioctl(fd_gyro, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) { err(1, "reset to manual polling"); + } /* do a simple demand read */ sz = read(fd_gyro, &g_report, sizeof(g_report)); - if (sz != sizeof(g_report)) + if (sz != sizeof(g_report)) { err(1, "immediate gyro read failed"); + } warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x); warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y); @@ -1246,10 +1294,11 @@ test() warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s, (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f)); - if (ioctl(fd_gyro, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd_gyro, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "reset to default polling"); + } - close(fd_gyro); + close(fd_gyro); /* XXX add poll-rate tests here too */ errx(0, "PASS"); @@ -1263,16 +1312,19 @@ reset() { int fd = open(L3GD20_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "failed "); + } - if (ioctl(fd, SENSORIOCRESET, 0) < 0) + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { err(1, "driver reset failed"); + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "accel pollrate reset failed"); + } - close(fd); + close(fd); exit(0); } @@ -1283,8 +1335,9 @@ reset() void info() { - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "driver not running\n"); + } printf("state @ %p\n", g_dev); g_dev->print_info(); @@ -1298,8 +1351,9 @@ info() void regdump(void) { - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "driver not running"); + } printf("regdump @ %p\n", g_dev); g_dev->print_registers(); @@ -1313,8 +1367,9 @@ regdump(void) void test_error(void) { - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "driver not running"); + } printf("regdump @ %p\n", g_dev); g_dev->test_error(); @@ -1346,9 +1401,11 @@ l3gd20_main(int argc, char *argv[]) case 'X': external_bus = true; break; + case 'R': rotation = (enum Rotation)atoi(optarg); break; + default: l3gd20::usage(); exit(0); @@ -1361,38 +1418,44 @@ l3gd20_main(int argc, char *argv[]) * Start/load the driver. */ - if (!strcmp(verb, "start")) + if (!strcmp(verb, "start")) { l3gd20::start(external_bus, rotation); + } /* * Test the driver/device. */ - if (!strcmp(verb, "test")) + if (!strcmp(verb, "test")) { l3gd20::test(); + } /* * Reset the driver. */ - if (!strcmp(verb, "reset")) + if (!strcmp(verb, "reset")) { l3gd20::reset(); + } /* * Print driver information. */ - if (!strcmp(verb, "info")) + if (!strcmp(verb, "info")) { l3gd20::info(); + } /* * Print register information. */ - if (!strcmp(verb, "regdump")) + if (!strcmp(verb, "regdump")) { l3gd20::regdump(); + } /* * trigger an error */ - if (!strcmp(verb, "testerror")) + if (!strcmp(verb, "testerror")) { l3gd20::test_error(); + } errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'testerror' or 'regdump'"); } diff --git a/src/drivers/led/led.cpp b/src/drivers/led/led.cpp index 831e8f4634..f74253fa37 100644 --- a/src/drivers/led/led.cpp +++ b/src/drivers/led/led.cpp @@ -125,6 +125,7 @@ LED::ioctl(device::file_t *filp, int cmd, unsigned long arg) result = VDev::ioctl(filp, cmd, arg); #endif } + return result; } @@ -138,7 +139,9 @@ drv_led_start(void) { if (gLED == nullptr) { gLED = new LED; - if (gLED != nullptr) + + if (gLED != nullptr) { gLED->init(); + } } } diff --git a/src/drivers/ll40ls/LidarLiteI2C.cpp b/src/drivers/ll40ls/LidarLiteI2C.cpp index 9a05890cd0..150465ec5b 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.cpp +++ b/src/drivers/ll40ls/LidarLiteI2C.cpp @@ -132,7 +132,7 @@ int LidarLiteI2C::init() measure(); _reports->get(&ds_report); _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, - &_orb_class_instance, ORB_PRIO_LOW); + &_orb_class_instance, ORB_PRIO_LOW); if (_distance_sensor_topic == nullptr) { DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?"); @@ -165,12 +165,16 @@ int LidarLiteI2C::lidar_transfer(const uint8_t *send, unsigned send_len, uint8_t { if (send != NULL && send_len > 0) { int ret = transfer(send, send_len, NULL, 0); - if (ret != OK) + + if (ret != OK) { return ret; + } } + if (recv != NULL && recv_len > 0) { return transfer(NULL, 0, recv, recv_len); } + return OK; } @@ -345,8 +349,10 @@ int LidarLiteI2C::measure() int LidarLiteI2C::reset_sensor() { int ret = write_reg(LL40LS_MEASURE_REG, LL40LS_MSRREG_RESET); - if (ret != OK) + + if (ret != OK) { return ret; + } // wait for sensor reset to complete usleep(1000); @@ -447,7 +453,7 @@ int LidarLiteI2C::collect() } _last_distance = distance_cm; - + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ report.timestamp = hrt_absolute_time(); diff --git a/src/drivers/ll40ls/LidarLiteI2C.h b/src/drivers/ll40ls/LidarLiteI2C.h index 51cca0862f..1c6de3c9b1 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.h +++ b/src/drivers/ll40ls/LidarLiteI2C.h @@ -51,7 +51,7 @@ #include #include - + /* Configuration Constants */ #define LL40LS_BUS PX4_I2C_BUS_EXPANSION @@ -117,8 +117,8 @@ private: uint16_t _zero_counter; uint64_t _acquire_time_usec; volatile bool _pause_measurements; - uint8_t _hw_version; - uint8_t _sw_version; + uint8_t _hw_version; + uint8_t _sw_version; /**< the bus the device is connected to */ int _bus; diff --git a/src/drivers/ll40ls/LidarLitePWM.cpp b/src/drivers/ll40ls/LidarLitePWM.cpp index 3d17d5b36b..e2a993ad14 100644 --- a/src/drivers/ll40ls/LidarLitePWM.cpp +++ b/src/drivers/ll40ls/LidarLitePWM.cpp @@ -114,7 +114,7 @@ int LidarLitePWM::init() measure(); _reports->get(&ds_report); _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, - &_orb_class_instance, ORB_PRIO_LOW); + &_orb_class_instance, ORB_PRIO_LOW); if (_distance_sensor_topic == nullptr) { DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?"); diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index a4c8720dbc..8591c4dee3 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -80,7 +80,7 @@ LidarLiteI2C *g_dev_int; LidarLiteI2C *g_dev_ext; LidarLitePWM *g_dev_pwm; -LidarLite * get_dev(const bool use_i2c, const int bus); +LidarLite *get_dev(const bool use_i2c, const int bus); void start(const bool use_i2c, const int bus); void stop(const bool use_i2c, const int bus); void test(const bool use_i2c, const int bus); @@ -92,19 +92,25 @@ void usage(); /** * Get the correct device pointer */ -LidarLite * get_dev(const bool use_i2c, const int bus) { - LidarLite * g_dev = nullptr; +LidarLite *get_dev(const bool use_i2c, const int bus) +{ + LidarLite *g_dev = nullptr; + if (use_i2c) { - g_dev = static_cast(bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext); + g_dev = static_cast(bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext); + if (g_dev == nullptr) { errx(1, "i2c driver not running"); } + } else { - g_dev = static_cast(g_dev_pwm); + g_dev = static_cast(g_dev_pwm); + if (g_dev == nullptr) { errx(1, "pwm driver not running"); } } + return g_dev; }; @@ -114,7 +120,7 @@ LidarLite * get_dev(const bool use_i2c, const int bus) { void start(const bool use_i2c, const int bus) { if (g_dev_int != nullptr || g_dev_ext != nullptr || g_dev_pwm != nullptr) { - errx(1,"driver already started"); + errx(1, "driver already started"); } if (use_i2c) { @@ -254,18 +260,21 @@ void stop(const bool use_i2c, const int bus) delete g_dev_int; g_dev_int = nullptr; } + } else { if (g_dev_ext != nullptr) { delete g_dev_ext; g_dev_ext = nullptr; } } + } else { if (g_dev_pwm != nullptr) { delete g_dev_pwm; g_dev_pwm = nullptr; } } + exit(0); } @@ -334,7 +343,7 @@ test(const bool use_i2c, const int bus) warnx("periodic read %u", i); warnx("valid %u", (float)report.current_distance > report.min_distance - && (float)report.current_distance < report.max_distance ? 1 : 0); + && (float)report.current_distance < report.max_distance ? 1 : 0); warnx("measurement: %0.3f m", (double)report.current_distance); warnx("time: %lld", report.timestamp); } @@ -386,7 +395,7 @@ reset(const bool use_i2c, const int bus) void info(const bool use_i2c, const int bus) { - LidarLite * g_dev = get_dev(use_i2c, bus); + LidarLite *g_dev = get_dev(use_i2c, bus); printf("state @ %p\n", g_dev); g_dev->print_info(); exit(0); @@ -398,7 +407,7 @@ info(const bool use_i2c, const int bus) void regdump(const bool use_i2c, const int bus) { - LidarLite * g_dev = get_dev(use_i2c, bus); + LidarLite *g_dev = get_dev(use_i2c, bus); printf("regdump @ %p\n", g_dev); g_dev->print_registers(); exit(0); diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 4235ecc6f5..8821c60c82 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -66,7 +66,6 @@ #include #include #include -#include #include #include @@ -236,7 +235,7 @@ class LSM303D_mag; class LSM303D : public device::SPI { public: - LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rotation); + LSM303D(int bus, const char *path, spi_dev_e device, enum Rotation rotation); virtual ~LSM303D(); virtual int init(); @@ -497,8 +496,8 @@ private: int mag_set_samplerate(unsigned frequency); /* this class cannot be copied */ - LSM303D(const LSM303D&); - LSM303D operator=(const LSM303D&); + LSM303D(const LSM303D &); + LSM303D operator=(const LSM303D &); }; /* @@ -512,7 +511,8 @@ const uint8_t LSM303D::_checked_registers[LSM303D_NUM_CHECKED_REGISTERS] = { ADD ADDR_CTRL_REG4, ADDR_CTRL_REG5, ADDR_CTRL_REG6, - ADDR_CTRL_REG7 }; + ADDR_CTRL_REG7 + }; /** * Helper class implementing the mag driver node. @@ -544,13 +544,14 @@ private: void measure_trampoline(void *arg); /* this class does not allow copying due to ptr data members */ - LSM303D_mag(const LSM303D_mag&); - LSM303D_mag operator=(const LSM303D_mag&); + LSM303D_mag(const LSM303D_mag &); + LSM303D_mag operator=(const LSM303D_mag &); }; -LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rotation) : - SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within safety margins for LSM303D */), +LSM303D::LSM303D(int bus, const char *path, spi_dev_e device, enum Rotation rotation) : + SPI("LSM303D", path, bus, device, SPIDEV_MODE3, + 11 * 1000 * 1000 /* will be rounded to 10.4 MHz, within safety margins for LSM303D */), _mag(new LSM303D_mag(this)), _accel_call{}, _mag_call{}, @@ -621,13 +622,17 @@ LSM303D::~LSM303D() stop(); /* free any existing reports */ - if (_accel_reports != nullptr) + if (_accel_reports != nullptr) { delete _accel_reports; - if (_mag_reports != nullptr) - delete _mag_reports; + } - if (_accel_class_instance != -1) + if (_mag_reports != nullptr) { + delete _mag_reports; + } + + if (_accel_class_instance != -1) { unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance); + } delete _mag; @@ -653,18 +658,21 @@ LSM303D::init() /* allocate basic report buffers */ _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); - if (_accel_reports == nullptr) + if (_accel_reports == nullptr) { goto out; + } _mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report)); - if (_mag_reports == nullptr) + if (_mag_reports == nullptr) { goto out; + } reset(); /* do CDev init for the mag device node */ ret = _mag->init(); + if (ret != OK) { warnx("MAG init failed"); goto out; @@ -679,7 +687,7 @@ LSM303D::init() /* measurement will have generated a report, publish */ _mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp, - &_mag->_mag_orb_class_instance, ORB_PRIO_LOW); + &_mag->_mag_orb_class_instance, ORB_PRIO_LOW); if (_mag->_mag_topic == nullptr) { warnx("ADVERT ERR"); @@ -694,7 +702,7 @@ LSM303D::init() /* measurement will have generated a report, publish */ _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, - &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); + &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); if (_accel_topic == nullptr) { warnx("ADVERT ERR"); @@ -762,7 +770,7 @@ LSM303D::probe() if (success) { _checked_values[0] = WHO_I_AM; return OK; - } + } return -EIO; } @@ -775,8 +783,9 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is enabled */ if (_call_accel_interval > 0) { @@ -798,8 +807,9 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) measure(); /* measurement will have generated a report, copy it out */ - if (_accel_reports->get(arb)) + if (_accel_reports->get(arb)) { ret = sizeof(*arb); + } return ret; } @@ -812,8 +822,9 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is enabled */ if (_call_mag_interval > 0) { @@ -837,8 +848,9 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) _mag->measure(); /* measurement will have generated a report, copy it out */ - if (_mag_reports->get(mrb)) + if (_mag_reports->get(mrb)) { ret = sizeof(*mrb); + } return ret; } @@ -849,7 +861,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) switch (cmd) { case SENSORIOCSPOLLRATE: { - switch (arg) { + switch (arg) { /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: @@ -871,56 +883,62 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSOR_POLLRATE_DEFAULT: return ioctl(filp, SENSORIOCSPOLLRATE, LSM303D_ACCEL_DEFAULT_RATE); - /* adjust to a legal polling interval in Hz */ + /* adjust to a legal polling interval in Hz */ default: { - /* do we need to start internal polling? */ - bool want_start = (_call_accel_interval == 0); + /* do we need to start internal polling? */ + bool want_start = (_call_accel_interval == 0); - /* convert hz to hrt interval via microseconds */ - unsigned ticks = 1000000 / arg; + /* convert hz to hrt interval via microseconds */ + unsigned ticks = 1000000 / arg; - /* check against maximum sane rate */ - if (ticks < 500) - return -EINVAL; + /* check against maximum sane rate */ + if (ticks < 500) { + return -EINVAL; + } - /* adjust filters */ - accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq()); + /* adjust filters */ + accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq()); - /* update interval for next measurement */ - /* XXX this is a bit shady, but no other way to adjust... */ - _call_accel_interval = ticks; + /* update interval for next measurement */ + /* XXX this is a bit shady, but no other way to adjust... */ + _call_accel_interval = ticks; - _accel_call.period = _call_accel_interval - LSM303D_TIMER_REDUCTION; + _accel_call.period = _call_accel_interval - LSM303D_TIMER_REDUCTION; - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } - return OK; + return OK; + } } } - } case SENSORIOCGPOLLRATE: - if (_call_accel_interval == 0) + if (_call_accel_interval == 0) { return SENSOR_POLLRATE_MANUAL; + } return 1000000 / _call_accel_interval; case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_accel_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } - irqstate_t flags = irqsave(); - if (!_accel_reports->resize(arg)) { irqrestore(flags); - return -ENOMEM; - } - irqrestore(flags); - return OK; - } + return OK; + } case SENSORIOCGQUEUEDEPTH: return _accel_reports->size(); @@ -936,23 +954,25 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) return _accel_samplerate; case ACCELIOCSLOWPASS: { - return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)arg); - } + return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)arg); + } case ACCELIOCGLOWPASS: return static_cast(_accel_filter_x.get_cutoff_freq()); case ACCELIOCSSCALE: { - /* copy scale, but only if off by a few percent */ - struct accel_scale *s = (struct accel_scale *) arg; - float sum = s->x_scale + s->y_scale + s->z_scale; - if (sum > 2.0f && sum < 4.0f) { - memcpy(&_accel_scale, s, sizeof(_accel_scale)); - return OK; - } else { - return -EINVAL; + /* copy scale, but only if off by a few percent */ + struct accel_scale *s = (struct accel_scale *) arg; + float sum = s->x_scale + s->y_scale + s->z_scale; + + if (sum > 2.0f && sum < 4.0f) { + memcpy(&_accel_scale, s, sizeof(_accel_scale)); + return OK; + + } else { + return -EINVAL; + } } - } case ACCELIOCSRANGE: /* arg needs to be in G */ @@ -960,7 +980,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCGRANGE: /* convert to m/s^2 and return rounded in G */ - return (unsigned long)((_accel_range_m_s2)/LSM303D_ONE_G + 0.5f); + return (unsigned long)((_accel_range_m_s2) / LSM303D_ONE_G + 0.5f); case ACCELIOCGSCALE: /* copy scale out */ @@ -982,7 +1002,7 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) switch (cmd) { case SENSORIOCSPOLLRATE: { - switch (arg) { + switch (arg) { /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: @@ -1012,16 +1032,18 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) unsigned ticks = 1000000 / arg; /* check against maximum sane rate */ - if (ticks < 1000) + if (ticks < 1000) { return -EINVAL; + } /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ _mag_call.period = _call_mag_interval = ticks; /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } @@ -1029,25 +1051,29 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_call_mag_interval == 0) + if (_call_mag_interval == 0) { return SENSOR_POLLRATE_MANUAL; + } return 1000000 / _call_mag_interval; case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_mag_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } - irqstate_t flags = irqsave(); - if (!_mag_reports->resize(arg)) { irqrestore(flags); - return -ENOMEM; - } - irqrestore(flags); - return OK; - } + return OK; + } case SENSORIOCGQUEUEDEPTH: return _mag_reports->size(); @@ -1102,24 +1128,34 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) int LSM303D::accel_self_test() { - if (_accel_read == 0) + if (_accel_read == 0) { return 1; + } /* inspect accel offsets */ - if (fabsf(_accel_scale.x_offset) < 0.000001f) - return 1; - if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f) + if (fabsf(_accel_scale.x_offset) < 0.000001f) { return 1; + } - if (fabsf(_accel_scale.y_offset) < 0.000001f) - return 1; - if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f) + if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f) { return 1; + } - if (fabsf(_accel_scale.z_offset) < 0.000001f) + if (fabsf(_accel_scale.y_offset) < 0.000001f) { return 1; - if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) + } + + if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f) { return 1; + } + + if (fabsf(_accel_scale.z_offset) < 0.000001f) { + return 1; + } + + if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) { + return 1; + } return 0; } @@ -1127,21 +1163,25 @@ LSM303D::accel_self_test() int LSM303D::mag_self_test() { - if (_mag_read == 0) + if (_mag_read == 0) { return 1; + } /** * inspect mag offsets * don't check mag scale because it seems this is calibrated on chip */ - if (fabsf(_mag_scale.x_offset) < 0.000001f) + if (fabsf(_mag_scale.x_offset) < 0.000001f) { return 1; + } - if (fabsf(_mag_scale.y_offset) < 0.000001f) + if (fabsf(_mag_scale.y_offset) < 0.000001f) { return 1; + } - if (fabsf(_mag_scale.z_offset) < 0.000001f) + if (fabsf(_mag_scale.z_offset) < 0.000001f) { return 1; + } return 0; } @@ -1174,7 +1214,8 @@ void LSM303D::write_checked_reg(unsigned reg, uint8_t value) { write_reg(reg, value); - for (uint8_t i=0; i 20 && fabsf(z_in_new) > 20) { _constant_accel_count += 1; + } else { _constant_accel_count = 0; } + if (_constant_accel_count > 100) { // we've had 100 constant accel readings with large // values. The sensor is almost certainly dead. We @@ -1712,16 +1762,19 @@ LSM303D::print_info() perf_print_counter(_accel_duplicates); _accel_reports->print_info("accel reports"); _mag_reports->print_info("mag reports"); - ::printf("checked_next: %u\n", _checked_next); - for (uint8_t i=0; imag_ioctl(filp, cmd, arg); + case DEVIOCGDEVICEID: + return (int)CDev::ioctl(filp, cmd, arg); + break; + + default: + return _parent->mag_ioctl(filp, cmd, arg); } } @@ -1881,17 +1939,19 @@ void start(bool external_bus, enum Rotation rotation, unsigned range) { int fd, fd_mag; + if (g_dev != nullptr) { errx(0, "already started"); } /* create the driver */ - if (external_bus) { - #ifdef PX4_SPI_BUS_EXT + if (external_bus) { +#ifdef PX4_SPI_BUS_EXT g_dev = new LSM303D(PX4_SPI_BUS_EXT, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_EXT_ACCEL_MAG, rotation); - #else +#else errx(0, "External SPI not available"); - #endif +#endif + } else { g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG, rotation); } @@ -1901,8 +1961,9 @@ start(bool external_bus, enum Rotation rotation, unsigned range) goto fail; } - if (OK != g_dev->init()) + if (OK != g_dev->init()) { goto fail; + } /* set the poll rate to default, starts automatic data collection */ fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY); @@ -1928,8 +1989,8 @@ start(bool external_bus, enum Rotation rotation, unsigned range) } } - close(fd); - close(fd_mag); + close(fd); + close(fd_mag); exit(0); fail: @@ -1958,14 +2019,16 @@ test() /* get the driver */ fd_accel = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY); - if (fd_accel < 0) + if (fd_accel < 0) { err(1, "%s open failed", LSM303D_DEVICE_PATH_ACCEL); + } /* do a simple demand read */ sz = read(fd_accel, &accel_report, sizeof(accel_report)); - if (sz != sizeof(accel_report)) + if (sz != sizeof(accel_report)) { err(1, "immediate read failed"); + } warnx("accel x: \t% 9.5f\tm/s^2", (double)accel_report.x); @@ -1976,10 +2039,13 @@ test() warnx("accel z: \t%d\traw", (int)accel_report.z_raw); warnx("accel range: %8.4f m/s^2", (double)accel_report.range_m_s2); - if (ERROR == (ret = ioctl(fd_accel, ACCELIOCGLOWPASS, 0))) + + if (ERROR == (ret = ioctl(fd_accel, ACCELIOCGLOWPASS, 0))) { warnx("accel antialias filter bandwidth: fail"); - else + + } else { warnx("accel antialias filter bandwidth: %d Hz", ret); + } int fd_mag = -1; struct mag_report m_report; @@ -1987,19 +2053,23 @@ test() /* get the driver */ fd_mag = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY); - if (fd_mag < 0) + if (fd_mag < 0) { err(1, "%s open failed", LSM303D_DEVICE_PATH_MAG); + } /* check if mag is onboard or external */ - if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0) + if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0) { errx(1, "failed to get if mag is onboard or external"); + } + warnx("mag device active: %s", ret ? "external" : "onboard"); /* do a simple demand read */ sz = read(fd_mag, &m_report, sizeof(m_report)); - if (sz != sizeof(m_report)) + if (sz != sizeof(m_report)) { err(1, "immediate read failed"); + } warnx("mag x: \t% 9.5f\tga", (double)m_report.x); warnx("mag y: \t% 9.5f\tga", (double)m_report.y); @@ -2014,8 +2084,8 @@ test() err(1, "reset to default polling"); } - close(fd_accel); - close(fd_mag); + close(fd_accel); + close(fd_mag); reset(); errx(0, "PASS"); @@ -2029,28 +2099,33 @@ reset() { int fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "failed "); + } - if (ioctl(fd, SENSORIOCRESET, 0) < 0) + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { err(1, "driver reset failed"); + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "accel pollrate reset failed"); + } - close(fd); + close(fd); fd = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY); if (fd < 0) { warnx("mag could not be opened, external mag might be used"); + } else { /* no need to reset the mag as well, the reset() is the same */ - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "mag pollrate reset failed"); + } } - close(fd); + close(fd); exit(0); } @@ -2061,8 +2136,9 @@ reset() void info() { - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "driver not running\n"); + } printf("state @ %p\n", g_dev); g_dev->print_info(); @@ -2076,8 +2152,9 @@ info() void regdump() { - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "driver not running\n"); + } printf("regdump @ %p\n", g_dev); g_dev->print_registers(); @@ -2091,8 +2168,9 @@ regdump() void test_error() { - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "driver not running\n"); + } g_dev->test_error(); @@ -2124,12 +2202,15 @@ lsm303d_main(int argc, char *argv[]) case 'X': external_bus = true; break; + case 'R': rotation = (enum Rotation)atoi(optarg); break; + case 'a': accel_range = atoi(optarg); break; + default: lsm303d::usage(); exit(0); @@ -2142,38 +2223,44 @@ lsm303d_main(int argc, char *argv[]) * Start/load the driver. */ - if (!strcmp(verb, "start")) + if (!strcmp(verb, "start")) { lsm303d::start(external_bus, rotation, accel_range); + } /* * Test the driver/device. */ - if (!strcmp(verb, "test")) + if (!strcmp(verb, "test")) { lsm303d::test(); + } /* * Reset the driver. */ - if (!strcmp(verb, "reset")) + if (!strcmp(verb, "reset")) { lsm303d::reset(); + } /* * Print driver information. */ - if (!strcmp(verb, "info")) + if (!strcmp(verb, "info")) { lsm303d::info(); + } /* * dump device registers */ - if (!strcmp(verb, "regdump")) + if (!strcmp(verb, "regdump")) { lsm303d::regdump(); + } /* * trigger an error */ - if (!strcmp(verb, "testerror")) + if (!strcmp(verb, "testerror")) { lsm303d::test_error(); + } errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'testerror' or 'regdump'"); } diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index ecd2ea04a6..f3cfa0e267 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -143,7 +143,8 @@ private: int _cycling_rate; /* */ uint8_t _index_counter; /* temporary sonar i2c address */ std::vector addr_ind; /* temp sonar i2c address vector */ - std::vector _latest_sonar_measurements; /* vector to store latest sonar measurements in before writing to report */ + std::vector + _latest_sonar_measurements; /* vector to store latest sonar measurements in before writing to report */ /** @@ -274,7 +275,7 @@ MB12XX::init() struct distance_sensor_s ds_report = {}; _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, - &_orb_class_instance, ORB_PRIO_LOW); + &_orb_class_instance, ORB_PRIO_LOW); if (_distance_sensor_topic == nullptr) { DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); @@ -862,7 +863,7 @@ test() warnx("periodic read %u", i); warnx("valid %u", (float)report.current_distance > report.min_distance - && (float)report.current_distance < report.max_distance ? 1 : 0); + && (float)report.current_distance < report.max_distance ? 1 : 0); warnx("measurement: %0.3f", (double)report.current_distance); warnx("time: %llu", report.timestamp); } diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index de37f0d089..fd0ac59551 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -180,9 +180,9 @@ private: void task_main(); static int control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input); + uint8_t control_group, + uint8_t control_index, + float &input); int pwm_ioctl(file *filp, int cmd, unsigned long arg); int mk_servo_arm(bool status); @@ -191,7 +191,7 @@ private: int mk_servo_locate(); short scaling(float val, float inMin, float inMax, float outMin, float outMax); void play_beep(int count); - + }; @@ -266,8 +266,9 @@ MK::~MK() } /* clean up the alternate device node */ - if (_primary_pwm_device) + if (_primary_pwm_device) { unregister_driver(_device); + } g_mk = nullptr; } @@ -276,7 +277,7 @@ int MK::init(unsigned motors) { _param_indicate_esc = param_find("MKBLCTRL_TEST"); - + _num_outputs = motors; debugCounter = 0; int ret; @@ -303,11 +304,11 @@ MK::init(unsigned motors) /* start the IO interface task */ _task = px4_task_spawn_cmd("mkblctrl", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 20, - 1500, - (main_t)&MK::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 20, + 1500, + (main_t)&MK::task_main_trampoline, + nullptr); if (_task < 0) { @@ -448,7 +449,7 @@ void MK::play_beep(int count) { int buzzer = ::open(TONEALARM0_DEVICE_PATH, O_WRONLY); - + for (int i = 0; i < count; i++) { ::ioctl(buzzer, TONE_SET_ALARM, TONE_SINGLE_BEEP_TUNE); usleep(300000); @@ -467,7 +468,7 @@ MK::task_main() */ _t_actuators = orb_subscribe(ORB_ID(actuator_controls_0)); orb_set_interval(_t_actuators, int(1000 / _update_rate)); /* set the topic update rate (200Hz)*/ - + /* * Subscribe to actuator_armed topic. */ @@ -481,7 +482,7 @@ MK::task_main() memset(&outputs, 0, sizeof(outputs)); int dummy; _t_outputs = orb_advertise_multi(ORB_ID(actuator_outputs), - &outputs, &dummy, ORB_PRIO_HIGH); + &outputs, &dummy, ORB_PRIO_HIGH); /* * advertise the blctrl status. @@ -504,9 +505,11 @@ MK::task_main() /* loop until killed */ while (!_task_should_exit) { - param_get(_param_indicate_esc ,¶m_mkblctrl_test); + param_get(_param_indicate_esc , ¶m_mkblctrl_test); + if (param_mkblctrl_test > 0) { _indicate_esc = true; + } else { _indicate_esc = false; } @@ -575,7 +578,8 @@ MK::task_main() /* output to BLCtrl's */ if (_motortest != true && _indicate_esc != true) { Motor[i].SetPoint_PX4 = outputs.output[i]; - mk_servo_set(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 2047)); // scale the output to 0 - 2047 and sent to output routine + mk_servo_set(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, + 2047)); // scale the output to 0 - 2047 and sent to output routine } } } @@ -615,6 +619,7 @@ MK::task_main() if (Motor[i].Version == 1) { // BLCtrl 2.0 (11Bit) esc.esc[i].esc_setpoint_raw = (uint16_t)(Motor[i].SetPoint << 3) | Motor[i].SetPointLowerBits; + } else { // BLCtrl < 2.0 (8Bit) esc.esc[i].esc_setpoint_raw = (uint16_t) Motor[i].SetPoint; @@ -628,7 +633,7 @@ MK::task_main() if (_motortest == true) { mk_servo_test(i); } - + // if esc locate is requested if (_indicate_esc == true) { mk_servo_locate(); @@ -719,7 +724,8 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C) fprintf(stderr, "[mkblctrl] MotorsFound: %i\n", foundMotorCount); for (unsigned i = 0; i < foundMotorCount; i++) { - fprintf(stderr, "[mkblctrl] blctrl[%i] : found=%i\tversion=%i\tcurrent=%i\tmaxpwm=%i\ttemperature=%i\n", i, Motor[i].State, Motor[i].Version, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature); + fprintf(stderr, "[mkblctrl] blctrl[%i] : found=%i\tversion=%i\tcurrent=%i\tmaxpwm=%i\ttemperature=%i\n", i, + Motor[i].State, Motor[i].Version, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature); } @@ -776,14 +782,14 @@ MK::mk_servo_set(unsigned int chan, short val) Motor[chan].Temperature = 255;; } else { - if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error + if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) { Motor[chan].State++; } // error } Motor[chan].RoundCount = 0; } else { if (OK != transfer(&msg[0], 1, nullptr, 0)) { - if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error + if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) { Motor[chan].State++; } // error } } @@ -806,14 +812,14 @@ MK::mk_servo_set(unsigned int chan, short val) Motor[chan].Temperature = result[2]; } else { - if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error + if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) { Motor[chan].State++; } // error } Motor[chan].RoundCount = 0; } else { if (OK != transfer(&msg[0], bytesToSendBL2, nullptr, 0)) { - if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error + if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) { Motor[chan].State++; } // error } } @@ -830,7 +836,8 @@ MK::mk_servo_set(unsigned int chan, short val) for (unsigned int i = 0; i < _num_outputs; i++) { if (Motor[i].State & MOTOR_STATE_PRESENT_MASK) { - fprintf(stderr, "[mkblctrl] #%i:\tVer: %i\tVal: %i\tCurr: %i\tMaxPWM: %i\tTemp: %i\tState: %i\n", i, Motor[i].Version, Motor[i].SetPoint, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature, Motor[i].State); + fprintf(stderr, "[mkblctrl] #%i:\tVer: %i\tVal: %i\tCurr: %i\tMaxPWM: %i\tTemp: %i\tState: %i\n", i, Motor[i].Version, + Motor[i].SetPoint, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature, Motor[i].State); } } @@ -924,12 +931,12 @@ MK::mk_servo_locate() set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan])); chan++; - + if (chan <= _num_outputs) { fprintf(stderr, "[mkblctrl] ESC Locate - #%i:\tgreen\n", chan); play_beep(chan); } - + if (chan > _num_outputs) { chan = 0; } @@ -962,8 +969,9 @@ MK::ioctl(file *filp, int cmd, unsigned long arg) ret = pwm_ioctl(filp, cmd, arg); /* if nobody wants it, let CDev have it */ - if (ret == -ENOTTY) + if (ret == -ENOTTY) { ret = CDev::ioctl(filp, cmd, arg); + } return ret; } @@ -1064,8 +1072,9 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) const char *buf = (const char *)arg; unsigned buflen = strnlen(buf, 1024); - if (_mixers == nullptr) + if (_mixers == nullptr) { _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); + } if (_mixers == nullptr) { ret = -ENOMEM; @@ -1086,30 +1095,36 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) } case PWM_SERVO_SET_MIN_PWM: { - struct pwm_output_values* pwm = (struct pwm_output_values*)arg; - if (pwm->channel_count > _max_actuators) - /* fail with error */ - return -E2BIG; + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - set_rc_min_value((unsigned)pwm->values[0]); - ret = OK; - break; - } + if (pwm->channel_count > _max_actuators) + /* fail with error */ + { + return -E2BIG; + } + + set_rc_min_value((unsigned)pwm->values[0]); + ret = OK; + break; + } case PWM_SERVO_GET_MIN_PWM: ret = OK; break; case PWM_SERVO_SET_MAX_PWM: { - struct pwm_output_values* pwm = (struct pwm_output_values*)arg; - if (pwm->channel_count > _max_actuators) - /* fail with error */ - return -E2BIG; + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - set_rc_max_value((unsigned)pwm->values[0]); - ret = OK; - break; - } + if (pwm->channel_count > _max_actuators) + /* fail with error */ + { + return -E2BIG; + } + + set_rc_max_value((unsigned)pwm->values[0]); + ret = OK; + break; + } case PWM_SERVO_GET_MAX_PWM: ret = OK; @@ -1168,7 +1183,8 @@ enum FrameType { int -mk_new_mode(int motorcount, bool motortest, int px4mode, int frametype, bool overrideSecurityChecks, unsigned rcmin, unsigned rcmax) +mk_new_mode(int motorcount, bool motortest, int px4mode, int frametype, bool overrideSecurityChecks, unsigned rcmin, + unsigned rcmax) { int shouldStop = 0; @@ -1215,8 +1231,9 @@ mk_start(unsigned motors, const char *device_path) // try i2c3 first g_mk = new MK(3, device_path); - if (!g_mk) + if (!g_mk) { return -ENOMEM; + } if (OK == g_mk->init(motors)) { warnx("[mkblctrl] scanning i2c3...\n"); @@ -1233,8 +1250,9 @@ mk_start(unsigned motors, const char *device_path) // fallback to bus 1 g_mk = new MK(1, device_path); - if (!g_mk) + if (!g_mk) { return -ENOMEM; + } if (OK == g_mk->init(motors)) { warnx("[mkblctrl] scanning i2c1...\n"); @@ -1330,13 +1348,15 @@ mkblctrl_main(int argc, char *argv[]) } /* look for the optional -rc_min parameter */ - if (strcmp(argv[i], "-rc_min") == 0 ) { + if (strcmp(argv[i], "-rc_min") == 0) { if (argc > i + 1) { rc_min_value = strtoul(argv[i + 1], &ep, 0); + if (*ep != '\0') { errx(1, "bad pwm val (-rc_min)"); return 1; } + } else { errx(1, "missing value (-rc_min)"); return 1; @@ -1344,13 +1364,15 @@ mkblctrl_main(int argc, char *argv[]) } /* look for the optional -rc_max parameter */ - if (strcmp(argv[i], "-rc_max") == 0 ) { + if (strcmp(argv[i], "-rc_max") == 0) { if (argc > i + 1) { rc_max_value = strtoul(argv[i + 1], &ep, 0); + if (*ep != '\0') { errx(1, "bad pwm val (-rc_max)"); return 1; } + } else { errx(1, "missing value (-rc_max)"); return 1; @@ -1365,7 +1387,8 @@ mkblctrl_main(int argc, char *argv[]) fprintf(stderr, " [-mkmode {+/x}] [-b i2c_bus_number] [-d devicename] [--override-security-checks] [-h / --help]\n\n"); fprintf(stderr, "\t -mkmode {+/x} \t\t Type of frame, if Mikrokopter motor order is used.\n"); fprintf(stderr, "\t -d {devicepath & name}\t\t Create alternate pwm device.\n"); - fprintf(stderr, "\t --override-security-checks \t\t Disable all security checks (arming and number of ESCs). Used to test single Motors etc. (DANGER !!!)\n"); + fprintf(stderr, + "\t --override-security-checks \t\t Disable all security checks (arming and number of ESCs). Used to test single Motors etc. (DANGER !!!)\n"); fprintf(stderr, "\t -rcmin {pwn-value}\t\t Set RC_MIN Value.\n"); fprintf(stderr, "\t -rcmax {pwn-value}\t\t Set RC_MAX Value.\n"); fprintf(stderr, "\n"); diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 4af1bec586..ab2eede349 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -348,7 +348,7 @@ private: * @param The register to read. * @return The value that was read. */ - uint8_t read_reg(unsigned reg, uint32_t speed=MPU6000_LOW_BUS_SPEED); + uint8_t read_reg(unsigned reg, uint32_t speed = MPU6000_LOW_BUS_SPEED); uint16_t read_reg16(unsigned reg); /** @@ -403,7 +403,7 @@ private: * * @return 0 on success, 1 on failure */ - int self_test(); + int self_test(); /** * Accel self test @@ -417,7 +417,7 @@ private: * * @return 0 on success, 1 on failure */ - int gyro_self_test(); + int gyro_self_test(); /* set low pass filter frequency @@ -435,8 +435,8 @@ private: void check_registers(void); /* do not allow to copy this class due to pointer data members */ - MPU6000(const MPU6000&); - MPU6000 operator=(const MPU6000&); + MPU6000(const MPU6000 &); + MPU6000 operator=(const MPU6000 &); #pragma pack(push, 1) /** @@ -465,11 +465,12 @@ const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPU MPUREG_PWR_MGMT_1, MPUREG_USER_CTRL, MPUREG_SMPLRT_DIV, - MPUREG_CONFIG, + MPUREG_CONFIG, MPUREG_GYRO_CONFIG, MPUREG_ACCEL_CONFIG, MPUREG_INT_ENABLE, - MPUREG_INT_PIN_CFG }; + MPUREG_INT_PIN_CFG + }; @@ -499,8 +500,8 @@ private: int _gyro_class_instance; /* do not allow to copy this class due to pointer data members */ - MPU6000_gyro(const MPU6000_gyro&); - MPU6000_gyro operator=(const MPU6000_gyro&); + MPU6000_gyro(const MPU6000_gyro &); + MPU6000_gyro operator=(const MPU6000_gyro &); }; /** driver 'main' command */ @@ -588,13 +589,17 @@ MPU6000::~MPU6000() delete _gyro; /* free any existing reports */ - if (_accel_reports != nullptr) + if (_accel_reports != nullptr) { delete _accel_reports; - if (_gyro_reports != nullptr) - delete _gyro_reports; + } - if (_accel_class_instance != -1) + if (_gyro_reports != nullptr) { + delete _gyro_reports; + } + + if (_accel_class_instance != -1) { unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance); + } /* delete the perf counter */ perf_free(_sample_perf); @@ -623,15 +628,20 @@ MPU6000::init() /* allocate basic report buffers */ _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); - if (_accel_reports == nullptr) + + if (_accel_reports == nullptr) { goto out; + } _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report)); - if (_gyro_reports == nullptr) - goto out; - if (reset() != OK) + if (_gyro_reports == nullptr) { goto out; + } + + if (reset() != OK) { + goto out; + } /* Initialize offsets and scales */ _accel_scale.x_offset = 0; @@ -651,6 +661,7 @@ MPU6000::init() /* do CDev init for the gyro device node, keep it optional */ ret = _gyro->init(); + /* if probe/setup failed, bail now */ if (ret != OK) { DEVICE_DEBUG("gyro init failed"); @@ -667,7 +678,7 @@ MPU6000::init() /* measurement will have generated a report, publish */ _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, - &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH); + &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH); if (_accel_topic == nullptr) { warnx("ADVERT FAIL"); @@ -679,7 +690,7 @@ MPU6000::init() _gyro_reports->get(&grp); _gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, - &_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH); + &_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH); if (_gyro->_gyro_topic == nullptr) { warnx("ADVERT FAIL"); @@ -696,6 +707,7 @@ int MPU6000::reset() // frequenctly comes up in a bad state where all transfers // come as zero uint8_t tries = 5; + while (--tries != 0) { irqstate_t state; state = irqsave(); @@ -716,9 +728,11 @@ int MPU6000::reset() if (read_reg(MPUREG_PWR_MGMT_1) == MPU_CLK_SEL_PLLGYROZ) { break; } + perf_count(_reset_retries); usleep(2000); } + if (read_reg(MPUREG_PWR_MGMT_1) != MPU_CLK_SEL_PLLGYROZ) { return -EIO; } @@ -768,6 +782,7 @@ MPU6000::probe() { uint8_t whoami; whoami = read_reg(MPUREG_WHOAMI); + if (whoami != MPU_WHOAMI_6000) { DEVICE_DEBUG("unexpected WHOAMI 0x%02x", whoami); return -EIO; @@ -807,15 +822,18 @@ void MPU6000::_set_sample_rate(unsigned desired_sample_rate_hz) { if (desired_sample_rate_hz == 0 || - desired_sample_rate_hz == GYRO_SAMPLERATE_DEFAULT || - desired_sample_rate_hz == ACCEL_SAMPLERATE_DEFAULT) { + desired_sample_rate_hz == GYRO_SAMPLERATE_DEFAULT || + desired_sample_rate_hz == ACCEL_SAMPLERATE_DEFAULT) { desired_sample_rate_hz = MPU6000_GYRO_DEFAULT_RATE; } uint8_t div = 1000 / desired_sample_rate_hz; - if(div>200) div=200; - if(div<1) div=1; - write_checked_reg(MPUREG_SMPLRT_DIV, div-1); + + if (div > 200) { div = 200; } + + if (div < 1) { div = 1; } + + write_checked_reg(MPUREG_SMPLRT_DIV, div - 1); _sample_rate = 1000 / div; } @@ -830,25 +848,34 @@ MPU6000::_set_dlpf_filter(uint16_t frequency_hz) /* choose next highest filter frequency available */ - if (frequency_hz == 0) { + if (frequency_hz == 0) { filter = BITS_DLPF_CFG_2100HZ_NOLPF; - } else if (frequency_hz <= 5) { + + } else if (frequency_hz <= 5) { filter = BITS_DLPF_CFG_5HZ; + } else if (frequency_hz <= 10) { filter = BITS_DLPF_CFG_10HZ; + } else if (frequency_hz <= 20) { filter = BITS_DLPF_CFG_20HZ; + } else if (frequency_hz <= 42) { filter = BITS_DLPF_CFG_42HZ; + } else if (frequency_hz <= 98) { filter = BITS_DLPF_CFG_98HZ; + } else if (frequency_hz <= 188) { filter = BITS_DLPF_CFG_188HZ; + } else if (frequency_hz <= 256) { filter = BITS_DLPF_CFG_256HZ_NOLPF2; + } else { filter = BITS_DLPF_CFG_2100HZ_NOLPF; } + write_checked_reg(MPUREG_CONFIG, filter); } @@ -858,8 +885,9 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen) unsigned count = buflen / sizeof(accel_report); /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { @@ -868,17 +896,21 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen) } /* if no data, error (we could block here) */ - if (_accel_reports->empty()) + if (_accel_reports->empty()) { return -EAGAIN; + } perf_count(_accel_reads); /* copy reports out of our buffer to the caller */ accel_report *arp = reinterpret_cast(buffer); int transferred = 0; + while (count--) { - if (!_accel_reports->get(arp)) + if (!_accel_reports->get(arp)) { break; + } + transferred++; arp++; } @@ -901,24 +933,34 @@ MPU6000::self_test() int MPU6000::accel_self_test() { - if (self_test()) + if (self_test()) { return 1; + } /* inspect accel offsets */ - if (fabsf(_accel_scale.x_offset) < 0.000001f) - return 1; - if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f) + if (fabsf(_accel_scale.x_offset) < 0.000001f) { return 1; + } - if (fabsf(_accel_scale.y_offset) < 0.000001f) - return 1; - if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f) + if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f) { return 1; + } - if (fabsf(_accel_scale.z_offset) < 0.000001f) + if (fabsf(_accel_scale.y_offset) < 0.000001f) { return 1; - if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) + } + + if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f) { return 1; + } + + if (fabsf(_accel_scale.z_offset) < 0.000001f) { + return 1; + } + + if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) { + return 1; + } return 0; } @@ -926,8 +968,9 @@ MPU6000::accel_self_test() int MPU6000::gyro_self_test() { - if (self_test()) + if (self_test()) { return 1; + } /* * Maximum deviation of 20 degrees, according to @@ -947,27 +990,35 @@ MPU6000::gyro_self_test() const float max_scale = 0.3f; /* evaluate gyro offsets, complain if offset -> zero or larger than 20 dps. */ - if (fabsf(_gyro_scale.x_offset) > max_offset) + if (fabsf(_gyro_scale.x_offset) > max_offset) { return 1; + } /* evaluate gyro scale, complain if off by more than 30% */ - if (fabsf(_gyro_scale.x_scale - 1.0f) > max_scale) + if (fabsf(_gyro_scale.x_scale - 1.0f) > max_scale) { return 1; + } - if (fabsf(_gyro_scale.y_offset) > max_offset) - return 1; - if (fabsf(_gyro_scale.y_scale - 1.0f) > max_scale) + if (fabsf(_gyro_scale.y_offset) > max_offset) { return 1; + } - if (fabsf(_gyro_scale.z_offset) > max_offset) + if (fabsf(_gyro_scale.y_scale - 1.0f) > max_scale) { return 1; - if (fabsf(_gyro_scale.z_scale - 1.0f) > max_scale) + } + + if (fabsf(_gyro_scale.z_offset) > max_offset) { return 1; + } + + if (fabsf(_gyro_scale.z_scale - 1.0f) > max_scale) { + return 1; + } /* check if all scales are zero */ if ((fabsf(_gyro_scale.x_offset) < 0.000001f) && - (fabsf(_gyro_scale.y_offset) < 0.000001f) && - (fabsf(_gyro_scale.z_offset) < 0.000001f)) { + (fabsf(_gyro_scale.y_offset) < 0.000001f) && + (fabsf(_gyro_scale.z_offset) < 0.000001f)) { /* if all are zero, this device is not calibrated */ return 1; } @@ -1003,14 +1054,14 @@ MPU6000::factory_self_test() float gyro_ftrim[3]; // get baseline values without self-test enabled - set_frequency(MPU6000_HIGH_BUS_SPEED); + set_frequency(MPU6000_HIGH_BUS_SPEED); memset(accel_baseline, 0, sizeof(accel_baseline)); memset(gyro_baseline, 0, sizeof(gyro_baseline)); memset(accel, 0, sizeof(accel)); memset(gyro, 0, sizeof(gyro)); - for (uint8_t i=0; i>3)&0x1C) | ((trims[3]>>4)&0x03); - atrim[1] = ((trims[1]>>3)&0x1C) | ((trims[3]>>2)&0x03); - atrim[2] = ((trims[2]>>3)&0x1C) | ((trims[3]>>0)&0x03); + atrim[0] = ((trims[0] >> 3) & 0x1C) | ((trims[3] >> 4) & 0x03); + atrim[1] = ((trims[1] >> 3) & 0x1C) | ((trims[3] >> 2) & 0x03); + atrim[2] = ((trims[2] >> 3) & 0x1C) | ((trims[3] >> 0) & 0x03); gtrim[0] = trims[0] & 0x1F; gtrim[1] = trims[1] & 0x1F; gtrim[2] = trims[2] & 0x1F; // convert factory trims to right units - for (uint8_t i=0; i<3; i++) { - accel_ftrim[i] = 4096 * 0.34f * powf(0.92f/0.34f, (atrim[i]-1)/30.0f); - gyro_ftrim[i] = 25 * 131.0f * powf(1.046f, gtrim[i]-1); + for (uint8_t i = 0; i < 3; i++) { + accel_ftrim[i] = 4096 * 0.34f * powf(0.92f / 0.34f, (atrim[i] - 1) / 30.0f); + gyro_ftrim[i] = 25 * 131.0f * powf(1.046f, gtrim[i] - 1); } + // Y gyro trim is negative gyro_ftrim[1] *= -1; - for (uint8_t i=0; i<3; i++) { - float diff = accel[i]-accel_baseline[i]; - float err = 100*(diff - accel_ftrim[i]) / accel_ftrim[i]; + for (uint8_t i = 0; i < 3; i++) { + float diff = accel[i] - accel_baseline[i]; + float err = 100 * (diff - accel_ftrim[i]) / accel_ftrim[i]; ::printf("ACCEL[%u] baseline=%d accel=%d diff=%d ftrim=%d err=%d\n", (unsigned)i, - (int)(1000*accel_baseline[i]), - (int)(1000*accel[i]), - (int)(1000*diff), - (int)(1000*accel_ftrim[i]), + (int)(1000 * accel_baseline[i]), + (int)(1000 * accel[i]), + (int)(1000 * diff), + (int)(1000 * accel_ftrim[i]), (int)err); + if (fabsf(err) > 14) { ::printf("FAIL\n"); ret = -EIO; } } - for (uint8_t i=0; i<3; i++) { - float diff = gyro[i]-gyro_baseline[i]; - float err = 100*(diff - gyro_ftrim[i]) / gyro_ftrim[i]; + + for (uint8_t i = 0; i < 3; i++) { + float diff = gyro[i] - gyro_baseline[i]; + float err = 100 * (diff - gyro_ftrim[i]) / gyro_ftrim[i]; ::printf("GYRO[%u] baseline=%d gyro=%d diff=%d ftrim=%d err=%d\n", (unsigned)i, - (int)(1000*gyro_baseline[i]), - (int)(1000*gyro[i]), - (int)(1000*(gyro[i]-gyro_baseline[i])), - (int)(1000*gyro_ftrim[i]), + (int)(1000 * gyro_baseline[i]), + (int)(1000 * gyro[i]), + (int)(1000 * (gyro[i] - gyro_baseline[i])), + (int)(1000 * gyro_ftrim[i]), (int)err); + if (fabsf(err) > 14) { ::printf("FAIL\n"); ret = -EIO; @@ -1118,6 +1173,7 @@ MPU6000::factory_self_test() write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config); _in_factory_test = false; + if (ret == OK) { ::printf("PASSED\n"); } @@ -1149,8 +1205,9 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) unsigned count = buflen / sizeof(gyro_report); /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { @@ -1159,17 +1216,21 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) } /* if no data, error (we could block here) */ - if (_gyro_reports->empty()) + if (_gyro_reports->empty()) { return -EAGAIN; + } perf_count(_gyro_reads); /* copy reports out of our buffer to the caller */ gyro_report *grp = reinterpret_cast(buffer); int transferred = 0; + while (count--) { - if (!_gyro_reports->get(grp)) + if (!_gyro_reports->get(grp)) { break; + } + transferred++; grp++; } @@ -1189,27 +1250,27 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCSPOLLRATE: { switch (arg) { - /* switching to manual polling */ + /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _call_interval = 0; return OK; - /* external signalling not supported */ + /* external signalling not supported */ case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ + /* zero would be bad */ case 0: return -EINVAL; - /* set default/max polling rate */ + /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: return ioctl(filp, SENSORIOCSPOLLRATE, 1000); case SENSOR_POLLRATE_DEFAULT: return ioctl(filp, SENSORIOCSPOLLRATE, MPU6000_ACCEL_DEFAULT_RATE); - /* adjust to a legal polling interval in Hz */ + /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ bool want_start = (_call_interval == 0); @@ -1218,12 +1279,13 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) unsigned ticks = 1000000 / arg; /* check against maximum sane rate */ - if (ticks < 1000) + if (ticks < 1000) { return -EINVAL; + } // adjust filters float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); - float sample_rate = 1.0e6f/ticks; + float sample_rate = 1.0e6f / ticks; _set_dlpf_filter(cutoff_freq_hz); _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); @@ -1240,17 +1302,18 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) /* XXX this is a bit shady, but no other way to adjust... */ _call_interval = ticks; - /* - set call interval faster then the sample time. We - then detect when we have duplicate samples and reject - them. This prevents aliasing due to a beat between the - stm32 clock and the mpu6000 clock - */ - _call.period = _call_interval - MPU6000_TIMER_REDUCTION; + /* + set call interval faster then the sample time. We + then detect when we have duplicate samples and reject + them. This prevents aliasing due to a beat between the + stm32 clock and the mpu6000 clock + */ + _call.period = _call_interval - MPU6000_TIMER_REDUCTION; /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } @@ -1258,25 +1321,29 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_call_interval == 0) + if (_call_interval == 0) { return SENSOR_POLLRATE_MANUAL; + } return 1000000 / _call_interval; case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_accel_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } - irqstate_t flags = irqsave(); - if (!_accel_reports->resize(arg)) { irqrestore(flags); - return -ENOMEM; - } - irqrestore(flags); - return OK; - } + return OK; + } case SENSORIOCGQUEUEDEPTH: return _accel_reports->size(); @@ -1300,14 +1367,15 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) _accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); return OK; - case ACCELIOCSSCALE: - { + case ACCELIOCSSCALE: { /* copy scale, but only if off by a few percent */ struct accel_scale *s = (struct accel_scale *) arg; float sum = s->x_scale + s->y_scale + s->z_scale; + if (sum > 2.0f && sum < 4.0f) { memcpy(&_accel_scale, s, sizeof(_accel_scale)); return OK; + } else { return -EINVAL; } @@ -1322,7 +1390,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) return set_accel_range(arg); case ACCELIOCGRANGE: - return (unsigned long)((_accel_range_m_s2)/MPU6000_ONE_G + 0.5f); + return (unsigned long)((_accel_range_m_s2) / MPU6000_ONE_G + 0.5f); case ACCELIOCSELFTEST: return accel_self_test(); @@ -1338,26 +1406,29 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { - /* these are shared with the accel side */ + /* these are shared with the accel side */ case SENSORIOCSPOLLRATE: case SENSORIOCGPOLLRATE: case SENSORIOCRESET: return ioctl(filp, cmd, arg); case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_gyro_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } - irqstate_t flags = irqsave(); - if (!_gyro_reports->resize(arg)) { irqrestore(flags); - return -ENOMEM; - } - irqrestore(flags); - return OK; - } + return OK; + } case SENSORIOCGQUEUEDEPTH: return _gyro_reports->size(); @@ -1371,6 +1442,7 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCGLOWPASS: return _gyro_filter_x.get_cutoff_freq(); + case GYROIOCSLOWPASS: // set hardware filtering _set_dlpf_filter(arg); @@ -1395,6 +1467,7 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) // _gyro_range_scale = xx // _gyro_range_rad_s = xx return -EINVAL; + case GYROIOCGRANGE: return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f); @@ -1412,8 +1485,8 @@ MPU6000::read_reg(unsigned reg, uint32_t speed) { uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0}; - // general register transfer at low clock speed - set_frequency(speed); + // general register transfer at low clock speed + set_frequency(speed); transfer(cmd, cmd, sizeof(cmd)); @@ -1425,8 +1498,8 @@ MPU6000::read_reg16(unsigned reg) { uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 }; - // general register transfer at low clock speed - set_frequency(MPU6000_LOW_BUS_SPEED); + // general register transfer at low clock speed + set_frequency(MPU6000_LOW_BUS_SPEED); transfer(cmd, cmd, sizeof(cmd)); @@ -1441,8 +1514,8 @@ MPU6000::write_reg(unsigned reg, uint8_t value) cmd[0] = reg | DIR_WRITE; cmd[1] = value; - // general register transfer at low clock speed - set_frequency(MPU6000_LOW_BUS_SPEED); + // general register transfer at low clock speed + set_frequency(MPU6000_LOW_BUS_SPEED); transfer(cmd, nullptr, sizeof(cmd)); } @@ -1462,7 +1535,8 @@ void MPU6000::write_checked_reg(unsigned reg, uint8_t value) { write_reg(reg, value); - for (uint8_t i=0; i 4) { // 8g - AFS_SEL = 2 afs_sel = 2; lsb_per_g = 4096; max_accel_g = 8; + } else if (max_g_in > 2) { // 4g - AFS_SEL = 1 afs_sel = 1; lsb_per_g = 8192; max_accel_g = 4; + } else { // 2g - AFS_SEL = 0 afs_sel = 0; lsb_per_g = 16384; @@ -1525,9 +1602,9 @@ MPU6000::start() /* start polling at the specified rate */ hrt_call_every(&_call, - 1000, - _call_interval-MPU6000_TIMER_REDUCTION, - (hrt_callout)&MPU6000::measure_trampoline, this); + 1000, + _call_interval - MPU6000_TIMER_REDUCTION, + (hrt_callout)&MPU6000::measure_trampoline, this); } void @@ -1565,7 +1642,8 @@ MPU6000::check_registers(void) the data registers. */ uint8_t v; - if ((v=read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) != + + if ((v = read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) != _checked_values[_checked_next]) { /* if we get the wrong value then we know the SPI bus @@ -1591,7 +1669,8 @@ MPU6000::check_registers(void) // register values but large offsets on the // accel axes _reset_wait = hrt_absolute_time() + 10000; - _checked_next = 0; + _checked_next = 0; + } else { write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); // waiting 3ms between register writes seems @@ -1599,9 +1678,11 @@ MPU6000::check_registers(void) // recovering considerably _reset_wait = hrt_absolute_time() + 3000; } + _register_wait = 20; } - _checked_next = (_checked_next+1) % MPU6000_NUM_CHECKED_REGISTERS; + + _checked_next = (_checked_next + 1) % MPU6000_NUM_CHECKED_REGISTERS; } void @@ -1618,6 +1699,7 @@ MPU6000::measure() } struct MPUReport mpu_report; + struct Report { int16_t accel_x; int16_t accel_y; @@ -1636,13 +1718,14 @@ MPU6000::measure() */ mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; - // sensor transfer at high clock speed - set_frequency(MPU6000_HIGH_BUS_SPEED); + // sensor transfer at high clock speed + set_frequency(MPU6000_HIGH_BUS_SPEED); - if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) + if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) { return; + } - check_registers(); + check_registers(); /* see if this is duplicate accelerometer data. Note that we @@ -1652,13 +1735,14 @@ MPU6000::measure() sampled at 8kHz, so we would incorrectly think we have new data when we are in fact getting duplicate accelerometer data. */ - if (!_got_duplicate && memcmp(&mpu_report.accel_x[0], &_last_accel[0], 6) == 0) { + if (!_got_duplicate && memcmp(&mpu_report.accel_x[0], &_last_accel[0], 6) == 0) { // it isn't new data - wait for next timer perf_end(_sample_perf); perf_count(_duplicates); _got_duplicate = true; return; } + memcpy(&_last_accel[0], &mpu_report.accel_x[0], 6); _got_duplicate = false; @@ -1686,10 +1770,10 @@ MPU6000::measure() // all zero data - probably a SPI bus error perf_count(_bad_transfers); perf_end(_sample_perf); - // note that we don't call reset() here as a reset() - // costs 20ms with interrupts disabled. That means if - // the mpu6k does go bad it would cause a FMU failure, - // regardless of whether another sensor is available, + // note that we don't call reset() here as a reset() + // costs 20ms with interrupts disabled. That means if + // the mpu6k does go bad it would cause a FMU failure, + // regardless of whether another sensor is available, return; } @@ -1736,7 +1820,7 @@ MPU6000::measure() // transfers and bad register reads. This allows the higher // level code to decide if it should use this sensor based on // whether it has had failures - grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); + grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); /* * 1) Scale raw value to SI units using scaling from datasheet. @@ -1866,16 +1950,19 @@ MPU6000::print_info() perf_print_counter(_duplicates); _accel_reports->print_info("accel queue"); _gyro_reports->print_info("gyro queue"); - ::printf("checked_next: %u\n", _checked_next); - for (uint8_t i=0; igyro_ioctl(filp, cmd, arg); + case DEVIOCGDEVICEID: + return (int)CDev::ioctl(filp, cmd, arg); + break; + + default: + return _parent->gyro_ioctl(filp, cmd, arg); } } @@ -1982,51 +2074,59 @@ void start(bool external_bus, enum Rotation rotation, int range) { int fd; - MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; - const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL; - const char *path_gyro = external_bus?MPU_DEVICE_PATH_GYRO_EXT:MPU_DEVICE_PATH_GYRO; + MPU6000 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + const char *path_accel = external_bus ? MPU_DEVICE_PATH_ACCEL_EXT : MPU_DEVICE_PATH_ACCEL; + const char *path_gyro = external_bus ? MPU_DEVICE_PATH_GYRO_EXT : MPU_DEVICE_PATH_GYRO; if (*g_dev_ptr != nullptr) /* if already started, the still command succeeded */ + { errx(0, "already started"); + } /* create the driver */ - if (external_bus) { + if (external_bus) { #ifdef PX4_SPI_BUS_EXT *g_dev_ptr = new MPU6000(PX4_SPI_BUS_EXT, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_EXT_MPU, rotation); #else errx(0, "External SPI not available"); #endif + } else { *g_dev_ptr = new MPU6000(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_MPU, rotation); } - if (*g_dev_ptr == nullptr) + if (*g_dev_ptr == nullptr) { goto fail; + } - if (OK != (*g_dev_ptr)->init()) + if (OK != (*g_dev_ptr)->init()) { goto fail; + } /* set the poll rate to default, starts automatic data collection */ fd = open(path_accel, O_RDONLY); - if (fd < 0) + if (fd < 0) { goto fail; + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { goto fail; + } - if (ioctl(fd, ACCELIOCSRANGE, range) < 0) + if (ioctl(fd, ACCELIOCSRANGE, range) < 0) { goto fail; + } - close(fd); + close(fd); exit(0); fail: if (*g_dev_ptr != nullptr) { - delete (*g_dev_ptr); - *g_dev_ptr = nullptr; + delete(*g_dev_ptr); + *g_dev_ptr = nullptr; } errx(1, "driver start failed"); @@ -2035,14 +2135,17 @@ fail: void stop(bool external_bus) { - MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; + MPU6000 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + if (*g_dev_ptr != nullptr) { delete *g_dev_ptr; *g_dev_ptr = nullptr; + } else { /* warn, but not an error */ warnx("already stopped."); } + exit(0); } @@ -2054,8 +2157,8 @@ stop(bool external_bus) void test(bool external_bus) { - const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL; - const char *path_gyro = external_bus?MPU_DEVICE_PATH_GYRO_EXT:MPU_DEVICE_PATH_GYRO; + const char *path_accel = external_bus ? MPU_DEVICE_PATH_ACCEL_EXT : MPU_DEVICE_PATH_ACCEL; + const char *path_gyro = external_bus ? MPU_DEVICE_PATH_GYRO_EXT : MPU_DEVICE_PATH_GYRO; accel_report a_report; gyro_report g_report; ssize_t sz; @@ -2070,12 +2173,14 @@ test(bool external_bus) /* get the driver */ int fd_gyro = open(path_gyro, O_RDONLY); - if (fd_gyro < 0) + if (fd_gyro < 0) { err(1, "%s open failed", path_gyro); + } /* reset to manual polling */ - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) { err(1, "reset to manual polling"); + } /* do a simple demand read */ sz = read(fd, &a_report, sizeof(a_report)); @@ -2117,8 +2222,9 @@ test(bool external_bus) warnx("temp: \t%d\traw 0x%0x", (short)a_report.temperature_raw, (unsigned short)a_report.temperature_raw); /* reset to default polling */ - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "reset to default polling"); + } close(fd); close(fd_gyro); @@ -2135,19 +2241,22 @@ test(bool external_bus) void reset(bool external_bus) { - const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL; + const char *path_accel = external_bus ? MPU_DEVICE_PATH_ACCEL_EXT : MPU_DEVICE_PATH_ACCEL; int fd = open(path_accel, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "failed "); + } - if (ioctl(fd, SENSORIOCRESET, 0) < 0) + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { err(1, "driver reset failed"); + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "driver poll restart failed"); + } - close(fd); + close(fd); exit(0); } @@ -2158,9 +2267,11 @@ reset(bool external_bus) void info(bool external_bus) { - MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; - if (*g_dev_ptr == nullptr) + MPU6000 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + + if (*g_dev_ptr == nullptr) { errx(1, "driver not running"); + } printf("state @ %p\n", *g_dev_ptr); (*g_dev_ptr)->print_info(); @@ -2174,9 +2285,11 @@ info(bool external_bus) void regdump(bool external_bus) { - MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; - if (*g_dev_ptr == nullptr) + MPU6000 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + + if (*g_dev_ptr == nullptr) { errx(1, "driver not running"); + } printf("regdump @ %p\n", *g_dev_ptr); (*g_dev_ptr)->print_registers(); @@ -2190,9 +2303,11 @@ regdump(bool external_bus) void testerror(bool external_bus) { - MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; - if (*g_dev_ptr == nullptr) + MPU6000 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + + if (*g_dev_ptr == nullptr) { errx(1, "driver not running"); + } (*g_dev_ptr)->test_error(); @@ -2205,9 +2320,11 @@ testerror(bool external_bus) void factorytest(bool external_bus) { - MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; - if (*g_dev_ptr == nullptr) + MPU6000 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + + if (*g_dev_ptr == nullptr) { errx(1, "driver not running"); + } (*g_dev_ptr)->factory_self_test(); @@ -2240,12 +2357,15 @@ mpu6000_main(int argc, char *argv[]) case 'X': external_bus = true; break; + case 'R': rotation = (enum Rotation)atoi(optarg); break; + case 'a': accel_range = atoi(optarg); break; + default: mpu6000::usage(); exit(0); diff --git a/src/drivers/mpu9250/mpu9250.cpp b/src/drivers/mpu9250/mpu9250.cpp index 248c4b038f..bff57865c8 100644 --- a/src/drivers/mpu9250/mpu9250.cpp +++ b/src/drivers/mpu9250/mpu9250.cpp @@ -337,7 +337,7 @@ private: * @param The register to read. * @return The value that was read. */ - uint8_t read_reg(unsigned reg, uint32_t speed=MPU9250_LOW_BUS_SPEED); + uint8_t read_reg(unsigned reg, uint32_t speed = MPU9250_LOW_BUS_SPEED); uint16_t read_reg16(unsigned reg); /** @@ -392,7 +392,7 @@ private: * * @return 0 on success, 1 on failure */ - int self_test(); + int self_test(); /** * Accel self test @@ -406,7 +406,7 @@ private: * * @return 0 on success, 1 on failure */ - int gyro_self_test(); + int gyro_self_test(); /* set low pass filter frequency @@ -424,8 +424,8 @@ private: void check_registers(void); /* do not allow to copy this class due to pointer data members */ - MPU9250(const MPU9250&); - MPU9250 operator=(const MPU9250&); + MPU9250(const MPU9250 &); + MPU9250 operator=(const MPU9250 &); #pragma pack(push, 1) /** @@ -455,12 +455,13 @@ const uint8_t MPU9250::_checked_registers[MPU9250_NUM_CHECKED_REGISTERS] = { MPU MPUREG_PWR_MGMT_2, MPUREG_USER_CTRL, MPUREG_SMPLRT_DIV, - MPUREG_CONFIG, + MPUREG_CONFIG, MPUREG_GYRO_CONFIG, MPUREG_ACCEL_CONFIG, MPUREG_ACCEL_CONFIG2, MPUREG_INT_ENABLE, - MPUREG_INT_PIN_CFG }; + MPUREG_INT_PIN_CFG + }; @@ -490,8 +491,8 @@ private: int _gyro_class_instance; /* do not allow to copy this class due to pointer data members */ - MPU9250_gyro(const MPU9250_gyro&); - MPU9250_gyro operator=(const MPU9250_gyro&); + MPU9250_gyro(const MPU9250_gyro &); + MPU9250_gyro operator=(const MPU9250_gyro &); }; /** driver 'main' command */ @@ -577,13 +578,17 @@ MPU9250::~MPU9250() delete _gyro; /* free any existing reports */ - if (_accel_reports != nullptr) + if (_accel_reports != nullptr) { delete _accel_reports; - if (_gyro_reports != nullptr) - delete _gyro_reports; + } - if (_accel_class_instance != -1) + if (_gyro_reports != nullptr) { + delete _gyro_reports; + } + + if (_accel_class_instance != -1) { unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance); + } /* delete the perf counter */ perf_free(_sample_perf); @@ -612,15 +617,20 @@ MPU9250::init() /* allocate basic report buffers */ _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); - if (_accel_reports == nullptr) + + if (_accel_reports == nullptr) { goto out; + } _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report)); - if (_gyro_reports == nullptr) - goto out; - if (reset() != OK) + if (_gyro_reports == nullptr) { goto out; + } + + if (reset() != OK) { + goto out; + } /* Initialize offsets and scales */ _accel_scale.x_offset = 0; @@ -640,6 +650,7 @@ MPU9250::init() /* do CDev init for the gyro device node, keep it optional */ ret = _gyro->init(); + /* if probe/setup failed, bail now */ if (ret != OK) { DEVICE_DEBUG("gyro init failed"); @@ -656,7 +667,7 @@ MPU9250::init() /* measurement will have generated a report, publish */ _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, - &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH); + &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH); if (_accel_topic == nullptr) { warnx("ADVERT FAIL"); @@ -668,7 +679,7 @@ MPU9250::init() _gyro_reports->get(&grp); _gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, - &_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH); + &_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH); if (_gyro->_gyro_topic == nullptr) { warnx("ADVERT FAIL"); @@ -722,16 +733,20 @@ int MPU9250::reset() usleep(1000); uint8_t retries = 10; + while (retries--) { bool all_ok = true; - for (uint8_t i=0; i200) div=200; - if(div<1) div=1; - write_checked_reg(MPUREG_SMPLRT_DIV, div-1); + + if (div > 200) { div = 200; } + + if (div < 1) { div = 1; } + + write_checked_reg(MPUREG_SMPLRT_DIV, div - 1); _sample_rate = 1000 / div; } @@ -790,31 +808,40 @@ MPU9250::_set_dlpf_filter(uint16_t frequency_hz) if (frequency_hz == 0) { _dlpf_freq = 0; filter = BITS_DLPF_CFG_3600HZ; + } else if (frequency_hz <= 5) { _dlpf_freq = 5; filter = BITS_DLPF_CFG_5HZ; + } else if (frequency_hz <= 10) { _dlpf_freq = 10; filter = BITS_DLPF_CFG_10HZ; + } else if (frequency_hz <= 20) { _dlpf_freq = 20; filter = BITS_DLPF_CFG_20HZ; + } else if (frequency_hz <= 41) { _dlpf_freq = 41; filter = BITS_DLPF_CFG_41HZ; + } else if (frequency_hz <= 92) { _dlpf_freq = 92; filter = BITS_DLPF_CFG_92HZ; + } else if (frequency_hz <= 184) { _dlpf_freq = 184; filter = BITS_DLPF_CFG_184HZ; + } else if (frequency_hz <= 250) { _dlpf_freq = 250; filter = BITS_DLPF_CFG_250HZ; + } else { _dlpf_freq = 0; filter = BITS_DLPF_CFG_3600HZ; } + write_checked_reg(MPUREG_CONFIG, filter); } @@ -824,8 +851,9 @@ MPU9250::read(struct file *filp, char *buffer, size_t buflen) unsigned count = buflen / sizeof(accel_report); /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { @@ -834,17 +862,21 @@ MPU9250::read(struct file *filp, char *buffer, size_t buflen) } /* if no data, error (we could block here) */ - if (_accel_reports->empty()) + if (_accel_reports->empty()) { return -EAGAIN; + } perf_count(_accel_reads); /* copy reports out of our buffer to the caller */ accel_report *arp = reinterpret_cast(buffer); int transferred = 0; + while (count--) { - if (!_accel_reports->get(arp)) + if (!_accel_reports->get(arp)) { break; + } + transferred++; arp++; } @@ -867,24 +899,34 @@ MPU9250::self_test() int MPU9250::accel_self_test() { - if (self_test()) + if (self_test()) { return 1; + } /* inspect accel offsets */ - if (fabsf(_accel_scale.x_offset) < 0.000001f) - return 1; - if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f) + if (fabsf(_accel_scale.x_offset) < 0.000001f) { return 1; + } - if (fabsf(_accel_scale.y_offset) < 0.000001f) - return 1; - if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f) + if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f) { return 1; + } - if (fabsf(_accel_scale.z_offset) < 0.000001f) + if (fabsf(_accel_scale.y_offset) < 0.000001f) { return 1; - if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) + } + + if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f) { return 1; + } + + if (fabsf(_accel_scale.z_offset) < 0.000001f) { + return 1; + } + + if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) { + return 1; + } return 0; } @@ -892,8 +934,9 @@ MPU9250::accel_self_test() int MPU9250::gyro_self_test() { - if (self_test()) + if (self_test()) { return 1; + } /* * Maximum deviation of 20 degrees, according to @@ -909,27 +952,35 @@ MPU9250::gyro_self_test() const float max_scale = 0.3f; /* evaluate gyro offsets, complain if offset -> zero or larger than 20 dps. */ - if (fabsf(_gyro_scale.x_offset) > max_offset) + if (fabsf(_gyro_scale.x_offset) > max_offset) { return 1; + } /* evaluate gyro scale, complain if off by more than 30% */ - if (fabsf(_gyro_scale.x_scale - 1.0f) > max_scale) + if (fabsf(_gyro_scale.x_scale - 1.0f) > max_scale) { return 1; + } - if (fabsf(_gyro_scale.y_offset) > max_offset) - return 1; - if (fabsf(_gyro_scale.y_scale - 1.0f) > max_scale) + if (fabsf(_gyro_scale.y_offset) > max_offset) { return 1; + } - if (fabsf(_gyro_scale.z_offset) > max_offset) + if (fabsf(_gyro_scale.y_scale - 1.0f) > max_scale) { return 1; - if (fabsf(_gyro_scale.z_scale - 1.0f) > max_scale) + } + + if (fabsf(_gyro_scale.z_offset) > max_offset) { return 1; + } + + if (fabsf(_gyro_scale.z_scale - 1.0f) > max_scale) { + return 1; + } /* check if all scales are zero */ if ((fabsf(_gyro_scale.x_offset) < 0.000001f) && - (fabsf(_gyro_scale.y_offset) < 0.000001f) && - (fabsf(_gyro_scale.z_offset) < 0.000001f)) { + (fabsf(_gyro_scale.y_offset) < 0.000001f) && + (fabsf(_gyro_scale.z_offset) < 0.000001f)) { /* if all are zero, this device is not calibrated */ return 1; } @@ -960,8 +1011,9 @@ MPU9250::gyro_read(struct file *filp, char *buffer, size_t buflen) unsigned count = buflen / sizeof(gyro_report); /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { @@ -970,17 +1022,21 @@ MPU9250::gyro_read(struct file *filp, char *buffer, size_t buflen) } /* if no data, error (we could block here) */ - if (_gyro_reports->empty()) + if (_gyro_reports->empty()) { return -EAGAIN; + } perf_count(_gyro_reads); /* copy reports out of our buffer to the caller */ gyro_report *grp = reinterpret_cast(buffer); int transferred = 0; + while (count--) { - if (!_gyro_reports->get(grp)) + if (!_gyro_reports->get(grp)) { break; + } + transferred++; grp++; } @@ -1000,27 +1056,27 @@ MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCSPOLLRATE: { switch (arg) { - /* switching to manual polling */ + /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _call_interval = 0; return OK; - /* external signalling not supported */ + /* external signalling not supported */ case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ + /* zero would be bad */ case 0: return -EINVAL; - /* set default/max polling rate */ + /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: return ioctl(filp, SENSORIOCSPOLLRATE, 1000); case SENSOR_POLLRATE_DEFAULT: return ioctl(filp, SENSORIOCSPOLLRATE, MPU9250_ACCEL_DEFAULT_RATE); - /* adjust to a legal polling interval in Hz */ + /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ bool want_start = (_call_interval == 0); @@ -1029,12 +1085,13 @@ MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg) unsigned ticks = 1000000 / arg; /* check against maximum sane rate */ - if (ticks < 1000) + if (ticks < 1000) { return -EINVAL; + } // adjust filters float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); - float sample_rate = 1.0e6f/ticks; + float sample_rate = 1.0e6f / ticks; _set_dlpf_filter(cutoff_freq_hz); _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); @@ -1051,17 +1108,18 @@ MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg) /* XXX this is a bit shady, but no other way to adjust... */ _call_interval = ticks; - /* - set call interval faster then the sample time. We - then detect when we have duplicate samples and reject - them. This prevents aliasing due to a beat between the - stm32 clock and the mpu9250 clock - */ - _call.period = _call_interval - MPU9250_TIMER_REDUCTION; + /* + set call interval faster then the sample time. We + then detect when we have duplicate samples and reject + them. This prevents aliasing due to a beat between the + stm32 clock and the mpu9250 clock + */ + _call.period = _call_interval - MPU9250_TIMER_REDUCTION; /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } @@ -1069,25 +1127,29 @@ MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_call_interval == 0) + if (_call_interval == 0) { return SENSOR_POLLRATE_MANUAL; + } return 1000000 / _call_interval; case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_accel_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } - irqstate_t flags = irqsave(); - if (!_accel_reports->resize(arg)) { irqrestore(flags); - return -ENOMEM; - } - irqrestore(flags); - return OK; - } + return OK; + } case SENSORIOCGQUEUEDEPTH: return _accel_reports->size(); @@ -1109,14 +1171,15 @@ MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg) _accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); return OK; - case ACCELIOCSSCALE: - { + case ACCELIOCSSCALE: { /* copy scale, but only if off by a few percent */ struct accel_scale *s = (struct accel_scale *) arg; float sum = s->x_scale + s->y_scale + s->z_scale; + if (sum > 2.0f && sum < 4.0f) { memcpy(&_accel_scale, s, sizeof(_accel_scale)); return OK; + } else { return -EINVAL; } @@ -1131,18 +1194,20 @@ MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg) return set_accel_range(arg); case ACCELIOCGRANGE: - return (unsigned long)((_accel_range_m_s2)/MPU9250_ONE_G + 0.5f); + return (unsigned long)((_accel_range_m_s2) / MPU9250_ONE_G + 0.5f); case ACCELIOCSELFTEST: return accel_self_test(); #ifdef ACCELIOCSHWLOWPASS + case ACCELIOCSHWLOWPASS: _set_dlpf_filter(arg); return OK; #endif #ifdef ACCELIOCGHWLOWPASS + case ACCELIOCGHWLOWPASS: return _dlpf_freq; #endif @@ -1159,26 +1224,29 @@ MPU9250::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { - /* these are shared with the accel side */ + /* these are shared with the accel side */ case SENSORIOCSPOLLRATE: case SENSORIOCGPOLLRATE: case SENSORIOCRESET: return ioctl(filp, cmd, arg); case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_gyro_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } - irqstate_t flags = irqsave(); - if (!_gyro_reports->resize(arg)) { irqrestore(flags); - return -ENOMEM; - } - irqrestore(flags); - return OK; - } + return OK; + } case SENSORIOCGQUEUEDEPTH: return _gyro_reports->size(); @@ -1216,6 +1284,7 @@ MPU9250::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) // _gyro_range_scale = xx // _gyro_range_rad_s = xx return -EINVAL; + case GYROIOCGRANGE: return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f); @@ -1223,12 +1292,14 @@ MPU9250::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) return gyro_self_test(); #ifdef GYROIOCSHWLOWPASS + case GYROIOCSHWLOWPASS: _set_dlpf_filter(arg); return OK; #endif #ifdef GYROIOCGHWLOWPASS + case GYROIOCGHWLOWPASS: return _dlpf_freq; #endif @@ -1244,8 +1315,8 @@ MPU9250::read_reg(unsigned reg, uint32_t speed) { uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0}; - // general register transfer at low clock speed - set_frequency(speed); + // general register transfer at low clock speed + set_frequency(speed); transfer(cmd, cmd, sizeof(cmd)); @@ -1257,8 +1328,8 @@ MPU9250::read_reg16(unsigned reg) { uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 }; - // general register transfer at low clock speed - set_frequency(MPU9250_LOW_BUS_SPEED); + // general register transfer at low clock speed + set_frequency(MPU9250_LOW_BUS_SPEED); transfer(cmd, cmd, sizeof(cmd)); @@ -1273,8 +1344,8 @@ MPU9250::write_reg(unsigned reg, uint8_t value) cmd[0] = reg | DIR_WRITE; cmd[1] = value; - // general register transfer at low clock speed - set_frequency(MPU9250_LOW_BUS_SPEED); + // general register transfer at low clock speed + set_frequency(MPU9250_LOW_BUS_SPEED); transfer(cmd, nullptr, sizeof(cmd)); } @@ -1294,7 +1365,8 @@ void MPU9250::write_checked_reg(unsigned reg, uint8_t value) { write_reg(reg, value); - for (uint8_t i=0; i 4) { // 8g - AFS_SEL = 2 afs_sel = 2; lsb_per_g = 4096; max_accel_g = 8; + } else if (max_g_in > 2) { // 4g - AFS_SEL = 1 afs_sel = 1; lsb_per_g = 8192; max_accel_g = 4; + } else { // 2g - AFS_SEL = 0 afs_sel = 0; lsb_per_g = 16384; @@ -1346,9 +1421,9 @@ MPU9250::start() /* start polling at the specified rate */ hrt_call_every(&_call, - 1000, - _call_interval-MPU9250_TIMER_REDUCTION, - (hrt_callout)&MPU9250::measure_trampoline, this); + 1000, + _call_interval - MPU9250_TIMER_REDUCTION, + (hrt_callout)&MPU9250::measure_trampoline, this); } void @@ -1379,7 +1454,8 @@ MPU9250::check_registers(void) the data registers. */ uint8_t v; - if ((v=read_reg(_checked_registers[_checked_next], MPU9250_HIGH_BUS_SPEED)) != + + if ((v = read_reg(_checked_registers[_checked_next], MPU9250_HIGH_BUS_SPEED)) != _checked_values[_checked_next]) { _checked_bad[_checked_next] = v; @@ -1408,7 +1484,8 @@ MPU9250::check_registers(void) // register values but large offsets on the // accel axes _reset_wait = hrt_absolute_time() + 10000; - _checked_next = 0; + _checked_next = 0; + } else { write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); // waiting 3ms between register writes seems @@ -1416,9 +1493,11 @@ MPU9250::check_registers(void) // recovering considerably _reset_wait = hrt_absolute_time() + 3000; } + _register_wait = 20; } - _checked_next = (_checked_next+1) % MPU9250_NUM_CHECKED_REGISTERS; + + _checked_next = (_checked_next + 1) % MPU9250_NUM_CHECKED_REGISTERS; } void @@ -1430,6 +1509,7 @@ MPU9250::measure() } struct MPUReport mpu_report; + struct Report { int16_t accel_x; int16_t accel_y; @@ -1448,13 +1528,14 @@ MPU9250::measure() */ mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; - // sensor transfer at high clock speed - set_frequency(MPU9250_HIGH_BUS_SPEED); + // sensor transfer at high clock speed + set_frequency(MPU9250_HIGH_BUS_SPEED); - if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) + if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) { return; + } - check_registers(); + check_registers(); /* see if this is duplicate accelerometer data. Note that we @@ -1464,13 +1545,14 @@ MPU9250::measure() sampled at 8kHz, so we would incorrectly think we have new data when we are in fact getting duplicate accelerometer data. */ - if (!_got_duplicate && memcmp(&mpu_report.accel_x[0], &_last_accel[0], 6) == 0) { + if (!_got_duplicate && memcmp(&mpu_report.accel_x[0], &_last_accel[0], 6) == 0) { // it isn't new data - wait for next timer perf_end(_sample_perf); perf_count(_duplicates); _got_duplicate = true; return; } + memcpy(&_last_accel[0], &mpu_report.accel_x[0], 6); _got_duplicate = false; @@ -1498,10 +1580,10 @@ MPU9250::measure() // all zero data - probably a SPI bus error perf_count(_bad_transfers); perf_end(_sample_perf); - // note that we don't call reset() here as a reset() - // costs 20ms with interrupts disabled. That means if - // the mpu6k does go bad it would cause a FMU failure, - // regardless of whether another sensor is available, + // note that we don't call reset() here as a reset() + // costs 20ms with interrupts disabled. That means if + // the mpu6k does go bad it would cause a FMU failure, + // regardless of whether another sensor is available, return; } @@ -1548,7 +1630,7 @@ MPU9250::measure() // transfers and bad register reads. This allows the higher // level code to decide if it should use this sensor based on // whether it has had failures - grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); + grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); /* * 1) Scale raw value to SI units using scaling from datasheet. @@ -1657,22 +1739,26 @@ MPU9250::print_info() perf_print_counter(_duplicates); _accel_reports->print_info("accel queue"); _gyro_reports->print_info("gyro queue"); - ::printf("checked_next: %u\n", _checked_next); - for (uint8_t i=0; igyro_ioctl(filp, cmd, arg); + case DEVIOCGDEVICEID: + return (int)CDev::ioctl(filp, cmd, arg); + break; + + default: + return _parent->gyro_ioctl(filp, cmd, arg); } } @@ -1778,48 +1869,55 @@ void start(bool external_bus, enum Rotation rotation) { int fd; - MPU9250 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; - const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL; - const char *path_gyro = external_bus?MPU_DEVICE_PATH_GYRO_EXT:MPU_DEVICE_PATH_GYRO; + MPU9250 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + const char *path_accel = external_bus ? MPU_DEVICE_PATH_ACCEL_EXT : MPU_DEVICE_PATH_ACCEL; + const char *path_gyro = external_bus ? MPU_DEVICE_PATH_GYRO_EXT : MPU_DEVICE_PATH_GYRO; if (*g_dev_ptr != nullptr) /* if already started, the still command succeeded */ + { errx(0, "already started"); + } /* create the driver */ - if (external_bus) { + if (external_bus) { #ifdef PX4_SPI_BUS_EXT *g_dev_ptr = new MPU9250(PX4_SPI_BUS_EXT, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_EXT_MPU, rotation); #else errx(0, "External SPI not available"); #endif + } else { *g_dev_ptr = new MPU9250(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_MPU, rotation); } - if (*g_dev_ptr == nullptr) + if (*g_dev_ptr == nullptr) { goto fail; + } - if (OK != (*g_dev_ptr)->init()) + if (OK != (*g_dev_ptr)->init()) { goto fail; + } /* set the poll rate to default, starts automatic data collection */ fd = open(path_accel, O_RDONLY); - if (fd < 0) + if (fd < 0) { goto fail; + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { goto fail; + } - close(fd); + close(fd); exit(0); fail: if (*g_dev_ptr != nullptr) { - delete (*g_dev_ptr); - *g_dev_ptr = nullptr; + delete(*g_dev_ptr); + *g_dev_ptr = nullptr; } errx(1, "driver start failed"); @@ -1828,14 +1926,17 @@ fail: void stop(bool external_bus) { - MPU9250 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; + MPU9250 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + if (*g_dev_ptr != nullptr) { delete *g_dev_ptr; *g_dev_ptr = nullptr; + } else { /* warn, but not an error */ warnx("already stopped."); } + exit(0); } @@ -1847,8 +1948,8 @@ stop(bool external_bus) void test(bool external_bus) { - const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL; - const char *path_gyro = external_bus?MPU_DEVICE_PATH_GYRO_EXT:MPU_DEVICE_PATH_GYRO; + const char *path_accel = external_bus ? MPU_DEVICE_PATH_ACCEL_EXT : MPU_DEVICE_PATH_ACCEL; + const char *path_gyro = external_bus ? MPU_DEVICE_PATH_GYRO_EXT : MPU_DEVICE_PATH_GYRO; accel_report a_report; gyro_report g_report; ssize_t sz; @@ -1863,12 +1964,14 @@ test(bool external_bus) /* get the driver */ int fd_gyro = open(path_gyro, O_RDONLY); - if (fd_gyro < 0) + if (fd_gyro < 0) { err(1, "%s open failed", path_gyro); + } /* reset to manual polling */ - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) { err(1, "reset to manual polling"); + } /* do a simple demand read */ sz = read(fd, &a_report, sizeof(a_report)); @@ -1922,19 +2025,22 @@ test(bool external_bus) void reset(bool external_bus) { - const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL; + const char *path_accel = external_bus ? MPU_DEVICE_PATH_ACCEL_EXT : MPU_DEVICE_PATH_ACCEL; int fd = open(path_accel, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "failed "); + } - if (ioctl(fd, SENSORIOCRESET, 0) < 0) + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { err(1, "driver reset failed"); + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "driver poll restart failed"); + } - close(fd); + close(fd); exit(0); } @@ -1945,9 +2051,11 @@ reset(bool external_bus) void info(bool external_bus) { - MPU9250 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; - if (*g_dev_ptr == nullptr) + MPU9250 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + + if (*g_dev_ptr == nullptr) { errx(1, "driver not running"); + } printf("state @ %p\n", *g_dev_ptr); (*g_dev_ptr)->print_info(); @@ -1961,9 +2069,11 @@ info(bool external_bus) void regdump(bool external_bus) { - MPU9250 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; - if (*g_dev_ptr == nullptr) + MPU9250 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + + if (*g_dev_ptr == nullptr) { errx(1, "driver not running"); + } printf("regdump @ %p\n", *g_dev_ptr); (*g_dev_ptr)->print_registers(); @@ -1977,9 +2087,11 @@ regdump(bool external_bus) void testerror(bool external_bus) { - MPU9250 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; - if (*g_dev_ptr == nullptr) + MPU9250 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + + if (*g_dev_ptr == nullptr) { errx(1, "driver not running"); + } (*g_dev_ptr)->test_error(); @@ -2010,9 +2122,11 @@ mpu9250_main(int argc, char *argv[]) case 'X': external_bus = true; break; + case 'R': rotation = (enum Rotation)atoi(optarg); break; + default: mpu9250::usage(); exit(0); diff --git a/src/drivers/pwm_input/pwm_input.cpp b/src/drivers/pwm_input/pwm_input.cpp index 06d660d92c..4d73f6204d 100644 --- a/src/drivers/pwm_input/pwm_input.cpp +++ b/src/drivers/pwm_input/pwm_input.cpp @@ -302,6 +302,7 @@ PWMIN::init() CDev::init(); _reports = new ringbuffer::RingBuffer(2, sizeof(struct pwm_input_s)); + if (_reports == nullptr) { return -ENOMEM; } @@ -490,6 +491,7 @@ PWMIN::read(struct file *filp, char *buffer, size_t buflen) buf++; } } + /* if there was no data, warn the caller */ return ret ? ret : -EAGAIN; } diff --git a/src/drivers/pwm_out_sim/pwm_out_sim.cpp b/src/drivers/pwm_out_sim/pwm_out_sim.cpp index f42d166535..8040cc4d95 100644 --- a/src/drivers/pwm_out_sim/pwm_out_sim.cpp +++ b/src/drivers/pwm_out_sim/pwm_out_sim.cpp @@ -133,9 +133,9 @@ private: void task_main(); static int control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input); + uint8_t control_group, + uint8_t control_index, + float &input); int pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg); @@ -194,6 +194,7 @@ PWMSim::~PWMSim() _task_should_exit = true; unsigned i = 10; + do { /* wait 50ms - it should wake every 100ms or so worst-case */ usleep(50000); @@ -226,17 +227,18 @@ PWMSim::init() if (ret != OK) { return ret; + } else { _primary_pwm_device = true; } /* start the PWMSim interface task */ _task = px4_task_spawn_cmd("pwm_out_sim", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 1000, - (px4_main_t)&PWMSim::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 1000, + (px4_main_t)&PWMSim::task_main_trampoline, + nullptr); if (_task < 0) { DEVICE_DEBUG("task start failed: %d", errno); @@ -277,7 +279,7 @@ PWMSim::set_mode(Mode mode) _num_outputs = 4; break; - case MODE_8PWM: + case MODE_8PWM: DEVICE_DEBUG("MODE_8PWM"); /* multi-port as 8 PWM outs */ _update_rate = 50; /* default output rate */ @@ -308,6 +310,7 @@ PWMSim::set_mode(Mode mode) default: return -EINVAL; } + _mode = mode; return OK; } @@ -315,8 +318,9 @@ PWMSim::set_mode(Mode mode) int PWMSim::set_pwm_rate(unsigned rate) { - if ((rate > 500) || (rate < 10)) + if ((rate > 500) || (rate < 10)) { return -EINVAL; + } _update_rate = rate; return OK; @@ -383,8 +387,11 @@ PWMSim::task_main() /* handle update rate changes */ if (_current_update_rate != _update_rate) { int update_rate_in_ms = int(1000 / _update_rate); - if (update_rate_in_ms < 2) + + if (update_rate_in_ms < 2) { update_rate_in_ms = 2; + } + orb_set_interval(_t_actuators, update_rate_in_ms); // up_pwm_servo_set_rate(_update_rate); _current_update_rate = _update_rate; @@ -403,7 +410,7 @@ PWMSim::task_main() if (fds[0].revents & POLLIN) { /* get controls - must always do this to avoid spinning */ orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : - ORB_ID(actuator_controls_1), _t_actuators, &_controls); + ORB_ID(actuator_controls_1), _t_actuators, &_controls); /* can we mix? */ if (_armed && _mixers != nullptr) { @@ -415,11 +422,12 @@ PWMSim::task_main() 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) { + PX4_ISFINITE(outputs.output[i]) && + outputs.output[i] >= -1.0f && + outputs.output[i] <= 1.0f) { /* scale for PWM output 900 - 2100us */ outputs.output[i] = 1500 + (600 * outputs.output[i]); + } else { /* * Value is NaN, INF or out of band - set to the minimum value. @@ -460,18 +468,20 @@ PWMSim::task_main() int PWMSim::control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input) + uint8_t control_group, + uint8_t control_index, + float &input) { const actuator_controls_s *controls = (actuator_controls_s *)handle; if (_armed) { input = controls->control[control_index]; + } else { /* clamp actuator to zero if not armed */ input = 0.0f; } + return 0; } @@ -481,7 +491,7 @@ PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg) int ret; /* if we are in valid PWM mode, try it as a PWM ioctl as well */ - switch(_mode) { + switch (_mode) { case MODE_2PWM: case MODE_4PWM: case MODE_8PWM: @@ -489,6 +499,7 @@ PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg) case MODE_16PWM: ret = PWMSim::pwm_ioctl(filp, cmd, arg); break; + default: ret = -ENOTTY; DEVICE_DEBUG("not in a PWM mode"); @@ -596,7 +607,7 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) break; } - /* FALLTHROUGH */ + /* FALLTHROUGH */ case PWM_SERVO_SET(0): case PWM_SERVO_SET(1): if (arg < 2100) { @@ -625,7 +636,7 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) break; } - /* FALLTHROUGH */ + /* FALLTHROUGH */ case PWM_SERVO_GET(1): case PWM_SERVO_GET(0): { *(servo_position_t *)arg = 1500; @@ -633,12 +644,12 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) } case PWM_SERVO_GET_RATEGROUP(0) ... PWM_SERVO_GET_RATEGROUP(PWM_OUTPUT_MAX_CHANNELS - 1): { - // no restrictions on output grouping - unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0); + // no restrictions on output grouping + unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0); - *(uint32_t *)arg = (1 << channel); - break; - } + *(uint32_t *)arg = (1 << channel); + break; + } case PWM_SERVO_GET_COUNT: case MIXERIOCGETOUTPUTCOUNT: @@ -648,6 +659,7 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) } else if (_mode == MODE_4PWM) { *(unsigned *)arg = 4; + } else { *(unsigned *)arg = 2; @@ -688,8 +700,9 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) const char *buf = (const char *)arg; unsigned buflen = strnlen(buf, 1024); - if (_mixers == nullptr) + if (_mixers == nullptr) { _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); + } if (_mixers == nullptr) { ret = -ENOMEM; @@ -705,6 +718,7 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) ret = -EINVAL; } } + break; } @@ -752,7 +766,7 @@ hil_new_mode(PortMode new_mode) switch (new_mode) { case PORT_MODE_UNDEFINED: case PORT1_MODE_UNSET: - case PORT2_MODE_UNSET: + case PORT2_MODE_UNSET: /* nothing more to do here */ break; @@ -852,7 +866,8 @@ fake(int argc, char *argv[]) extern "C" __EXPORT int pwm_out_sim_main(int argc, char *argv[]); static void -usage() { +usage() +{ PX4_WARN("pwm_out_sim: 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"); } @@ -868,10 +883,12 @@ pwm_out_sim_main(int argc, char *argv[]) usage(); return -EINVAL; } + verb = argv[1]; if (g_pwm_sim == nullptr) { g_pwm_sim = new PWMSim; + if (g_pwm_sim == nullptr) { return -ENOMEM; } @@ -881,7 +898,7 @@ pwm_out_sim_main(int argc, char *argv[]) * Mode switches. */ - // this was all cut-and-pasted from the FMU driver; it's junk + // this was all cut-and-pasted from the FMU driver; it's junk if (!strcmp(verb, "mode_pwm")) { new_mode = PORT1_FULL_PWM; @@ -905,13 +922,14 @@ pwm_out_sim_main(int argc, char *argv[]) if (new_mode != PORT_MODE_UNDEFINED) { /* yes but it's the same mode */ - if (new_mode == g_port_mode) + if (new_mode == g_port_mode) { return OK; + } /* switch modes */ ret = hil_new_mode(new_mode); - } - else if (!strcmp(verb, "test")) { + + } else if (!strcmp(verb, "test")) { ret = test(); } @@ -924,13 +942,15 @@ pwm_out_sim_main(int argc, char *argv[]) ret = -EINVAL; } - if ( ret == OK && g_pwm_sim->_task == -1 ) { + if (ret == OK && g_pwm_sim->_task == -1) { ret = g_pwm_sim->init(); + if (ret != OK) { warnx("failed to start the pwm_out_sim driver"); delete g_pwm_sim; g_pwm_sim = nullptr; } } + return ret; } diff --git a/src/drivers/px4flow/i2c_frame.h b/src/drivers/px4flow/i2c_frame.h index 43e10e417c..f73d4a92cc 100644 --- a/src/drivers/px4flow/i2c_frame.h +++ b/src/drivers/px4flow/i2c_frame.h @@ -44,38 +44,36 @@ #define __STDC_FORMAT_MACROS #include -typedef struct i2c_frame -{ - uint16_t frame_count; - int16_t pixel_flow_x_sum; - int16_t pixel_flow_y_sum; - int16_t flow_comp_m_x; - int16_t flow_comp_m_y; - int16_t qual; - int16_t gyro_x_rate; - int16_t gyro_y_rate; - int16_t gyro_z_rate; - uint8_t gyro_range; - uint8_t sonar_timestamp; - int16_t ground_distance; +typedef struct i2c_frame { + uint16_t frame_count; + int16_t pixel_flow_x_sum; + int16_t pixel_flow_y_sum; + int16_t flow_comp_m_x; + int16_t flow_comp_m_y; + int16_t qual; + int16_t gyro_x_rate; + int16_t gyro_y_rate; + int16_t gyro_z_rate; + uint8_t gyro_range; + uint8_t sonar_timestamp; + int16_t ground_distance; } i2c_frame; #define I2C_FRAME_SIZE (sizeof(i2c_frame)) -typedef struct i2c_integral_frame -{ - uint16_t frame_count_since_last_readout; - int16_t pixel_flow_x_integral; - int16_t pixel_flow_y_integral; - int16_t gyro_x_rate_integral; - int16_t gyro_y_rate_integral; - int16_t gyro_z_rate_integral; - uint32_t integration_timespan; - uint32_t sonar_timestamp; - uint16_t ground_distance; - int16_t gyro_temperature; - uint8_t qual; +typedef struct i2c_integral_frame { + uint16_t frame_count_since_last_readout; + int16_t pixel_flow_x_integral; + int16_t pixel_flow_y_integral; + int16_t gyro_x_rate_integral; + int16_t gyro_y_rate_integral; + int16_t gyro_z_rate_integral; + uint32_t integration_timespan; + uint32_t sonar_timestamp; + uint16_t ground_distance; + int16_t gyro_temperature; + uint8_t qual; } i2c_integral_frame; #define I2C_INTEGRAL_FRAME_SIZE (sizeof(i2c_integral_frame)) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 134ce6a52b..92cac76312 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -246,7 +246,7 @@ PX4FLOW::init() struct distance_sensor_s ds_report = {}; _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, - &_orb_class_instance, ORB_PRIO_HIGH); + &_orb_class_instance, ORB_PRIO_HIGH); if (_distance_sensor_topic == nullptr) { DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); @@ -503,22 +503,34 @@ PX4FLOW::collect() struct optical_flow_s report; report.timestamp = hrt_absolute_time(); + report.pixel_flow_x_integral = static_cast(f_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians + report.pixel_flow_y_integral = static_cast(f_integral.pixel_flow_y_integral) / 10000.0f;//convert to radians + report.frame_count_since_last_readout = f_integral.frame_count_since_last_readout; + report.ground_distance_m = static_cast(f_integral.ground_distance) / 1000.0f;//convert to meters + report.quality = f_integral.qual; //0:bad ; 255 max quality + report.gyro_x_rate_integral = static_cast(f_integral.gyro_x_rate_integral) / 10000.0f; //convert to radians + report.gyro_y_rate_integral = static_cast(f_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians + report.gyro_z_rate_integral = static_cast(f_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians + report.integration_timespan = f_integral.integration_timespan; //microseconds + report.time_since_last_sonar_update = f_integral.sonar_timestamp;//microseconds + report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius report.sensor_id = 0; /* rotate measurements according to parameter */ float zeroval = 0.0f; + rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval); if (_px4flow_topic == nullptr) { @@ -659,12 +671,13 @@ int start() { int fd; - + /* entry check: */ if (start_in_progress) { warnx("start already in progress"); return 1; } + start_in_progress = true; if (g_dev != nullptr) { @@ -676,22 +689,24 @@ start() warnx("scanning I2C buses for device.."); int retry_nr = 0; + while (1) { const int busses_to_try[] = { PX4_I2C_BUS_EXPANSION, - #ifdef PX4_I2C_BUS_ESC +#ifdef PX4_I2C_BUS_ESC PX4_I2C_BUS_ESC, - #endif +#endif PX4_I2C_BUS_ONBOARD, -1 }; const int *cur_bus = busses_to_try; - while(*cur_bus != -1) { + while (*cur_bus != -1) { /* create the driver */ /* warnx("trying bus %d", *cur_bus); */ g_dev = new PX4FLOW(*cur_bus); + if (g_dev == nullptr) { /* this is a fatal error */ break; @@ -713,7 +728,7 @@ start() /* check whether we found it: */ if (*cur_bus != -1) { - + /* check for failure: */ if (g_dev == nullptr) { break; @@ -740,16 +755,17 @@ start() // warnx("PX4FLOW not found on I2C busses. Retrying in %d ms. Giving up in %d retries.", START_RETRY_TIMEOUT, START_RETRY_COUNT - retry_nr); usleep(START_RETRY_TIMEOUT * 1000); retry_nr++; + } else { break; } } - + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; } - + start_in_progress = false; return 1; } @@ -793,8 +809,7 @@ test() /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); - if (sz != sizeof(report)) - { + if (sz != sizeof(report)) { warnx("immediate read failed"); } diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 3c7c0d2994..9a6581437e 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -3468,12 +3468,12 @@ px4io_main(int argc, char *argv[]) } else { #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) - fn[0] = "/etc/extras/px4io-v1_default.bin"; + fn[0] = "/etc/extras/px4io-v1.bin"; fn[1] = "/fs/microsd/px4io1.bin"; fn[2] = "/fs/microsd/px4io.bin"; fn[3] = nullptr; #elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) - fn[0] = "/etc/extras/px4io-v2_default.bin"; + fn[0] = "/etc/extras/px4io-v2.bin"; fn[1] = "/fs/microsd/px4io2.bin"; fn[2] = "/fs/microsd/px4io.bin"; fn[3] = nullptr; diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index d379b20b60..3cf33e8eca 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -135,9 +135,9 @@ extern "C" __EXPORT int rgbled_main(int argc, char *argv[]); RGBLED::RGBLED(int bus, int rgbled) : I2C("rgbled", RGBLED0_DEVICE_PATH, bus, rgbled #ifdef __PX4_NUTTX - ,100000 /* maximum speed supported */ + , 100000 /* maximum speed supported */ #endif - ), + ), _mode(RGBLED_MODE_OFF), _r(0), _g(0), @@ -195,9 +195,9 @@ RGBLED::probe() unsigned prevretries = _retries; _retries = 4; - if ((ret=get(on, powersave, r, g, b)) != OK || - (ret=send_led_enable(false) != OK) || - (ret=send_led_enable(false) != OK)) { + if ((ret = get(on, powersave, r, g, b)) != OK || + (ret = send_led_enable(false) != OK) || + (ret = send_led_enable(false) != OK)) { return ret; } @@ -297,6 +297,7 @@ RGBLED::led() if (_param_sub >= 0) { bool updated = false; orb_check(_param_sub, &updated); + if (updated) { parameter_update_s pupdate; orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate); @@ -310,8 +311,9 @@ RGBLED::led() case RGBLED_MODE_BLINK_SLOW: case RGBLED_MODE_BLINK_NORMAL: case RGBLED_MODE_BLINK_FAST: - if (_counter >= 2) + if (_counter >= 2) { _counter = 0; + } send_led_enable(_counter == 0); @@ -319,8 +321,9 @@ RGBLED::led() case RGBLED_MODE_BREATHE: - if (_counter >= 62) + if (_counter >= 62) { _counter = 0; + } int n; @@ -338,8 +341,9 @@ RGBLED::led() case RGBLED_MODE_PATTERN: /* don't run out of the pattern array and stop if the next frame is 0 */ - if (_counter >= RGBLED_PATTERN_LENGTH || _pattern.duration[_counter] <= 0) + if (_counter >= RGBLED_PATTERN_LENGTH || _pattern.duration[_counter] <= 0) { _counter = 0; + } set_color(_pattern.color[_counter]); send_led_rgb(); @@ -549,8 +553,9 @@ RGBLED::send_led_enable(bool enable) { uint8_t settings_byte = 0; - if (enable) + if (enable) { settings_byte |= SETTING_ENABLE; + } settings_byte |= SETTING_NOT_POWERSAVE; @@ -606,6 +611,7 @@ RGBLED::update_params() if (maxbrt == 1) { maxbrt = 2; } + _max_brightness = maxbrt / 15.0f; } @@ -629,6 +635,7 @@ rgbled_main(int argc, char *argv[]) /* jump over start/off/etc and look at options first */ int myoptind = 1; const char *myoptarg = NULL; + while ((ch = px4_getopt(argc, argv, "a:b:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'a': @@ -645,10 +652,10 @@ rgbled_main(int argc, char *argv[]) } } - if (myoptind >= argc) { - rgbled_usage(); - return 1; - } + if (myoptind >= argc) { + rgbled_usage(); + return 1; + } const char *verb = argv[myoptind]; @@ -677,6 +684,7 @@ rgbled_main(int argc, char *argv[]) warnx("no RGB led on bus #%d", i2cdevice); return 1; } + i2cdevice = PX4_I2C_BUS_LED; } } @@ -741,12 +749,14 @@ rgbled_main(int argc, char *argv[]) ret = px4_ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF); px4_close(fd); + /* delete the rgbled object if stop was requested, in addition to turning off the LED. */ if (!strcmp(verb, "stop")) { delete g_rgbled; g_rgbled = nullptr; return 0; } + return ret; } diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index 5b945e452d..f9116f1d45 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -61,7 +61,7 @@ uint8_t RoboClaw::checksum_mask = 0x7f; RoboClaw::RoboClaw(const char *deviceName, uint16_t address, - uint16_t pulsesPerRev): + uint16_t pulsesPerRev): _address(address), _pulsesPerRev(pulsesPerRev), _uart(0), @@ -80,18 +80,27 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address, // start serial port _uart = open(deviceName, O_RDWR | O_NOCTTY); - if (_uart < 0) err(1, "could not open %s", deviceName); + + if (_uart < 0) { err(1, "could not open %s", deviceName); } + int ret = 0; struct termios uart_config; ret = tcgetattr(_uart, &uart_config); - if (ret < 0) err (1, "failed to get attr"); + + if (ret < 0) { err(1, "failed to get attr"); } + uart_config.c_oflag &= ~ONLCR; // no CR for every LF ret = cfsetispeed(&uart_config, B38400); - if (ret < 0) err (1, "failed to set input speed"); + + if (ret < 0) { err(1, "failed to set input speed"); } + ret = cfsetospeed(&uart_config, B38400); - if (ret < 0) err (1, "failed to set output speed"); + + if (ret < 0) { err(1, "failed to set output speed"); } + ret = tcsetattr(_uart, TCSANOW, &uart_config); - if (ret < 0) err (1, "failed to set attr"); + + if (ret < 0) { err(1, "failed to set attr"); } // setup default settings, reset encoders resetEncoders(); @@ -107,25 +116,30 @@ RoboClaw::~RoboClaw() int RoboClaw::readEncoder(e_motor motor) { uint16_t sum = 0; + if (motor == MOTOR_1) { _sendCommand(CMD_READ_ENCODER_1, nullptr, 0, sum); + } else if (motor == MOTOR_2) { _sendCommand(CMD_READ_ENCODER_2, nullptr, 0, sum); } + uint8_t rbuf[50]; usleep(5000); int nread = read(_uart, rbuf, 50); + if (nread < 6) { printf("failed to read\n"); return -1; } + //printf("received: \n"); //for (int i=0;i 1) value = 1; - if (value < -1) value = -1; - uint8_t speed = fabs(value)*127; + if (value > 1) { value = 1; } + + if (value < -1) { value = -1; } + + uint8_t speed = fabs(value) * 127; + // send command if (motor == MOTOR_1) { if (value > 0) { return _sendCommand(CMD_DRIVE_FWD_1, &speed, 1, sum); + } else { return _sendCommand(CMD_DRIVE_REV_1, &speed, 1, sum); } + } else if (motor == MOTOR_2) { if (value > 0) { return _sendCommand(CMD_DRIVE_FWD_2, &speed, 1, sum); + } else { return _sendCommand(CMD_DRIVE_REV_2, &speed, 1, sum); } } + return -1; } int RoboClaw::setMotorDutyCycle(e_motor motor, float value) { uint16_t sum = 0; + // bound - if (value > 1) value = 1; - if (value < -1) value = -1; - int16_t duty = 1500*value; + if (value > 1) { value = 1; } + + if (value < -1) { value = -1; } + + int16_t duty = 1500 * value; + // send command if (motor == MOTOR_1) { return _sendCommand(CMD_SIGNED_DUTYCYCLE_1, - (uint8_t *)(&duty), 2, sum); + (uint8_t *)(&duty), 2, sum); + } else if (motor == MOTOR_2) { return _sendCommand(CMD_SIGNED_DUTYCYCLE_2, - (uint8_t *)(&duty), 2, sum); + (uint8_t *)(&duty), 2, sum); } + return -1; } @@ -247,7 +285,7 @@ int RoboClaw::resetEncoders() { uint16_t sum = 0; return _sendCommand(CMD_RESET_ENCODERS, - nullptr, 0, sum); + nullptr, 0, sum); } int RoboClaw::update() @@ -256,52 +294,57 @@ int RoboClaw::update() // check for exit condition every second // note "::poll" is required to distinguish global // poll from member function for driver - if (::poll(&_controlPoll, 1, 1000) < 0) return -1; // poll error + if (::poll(&_controlPoll, 1, 1000) < 0) { return -1; } // poll error // if new data, send to motors if (_actuators.updated()) { _actuators.update(); - setMotorDutyCycle(MOTOR_1,_actuators.control[CH_VOLTAGE_LEFT]); - setMotorDutyCycle(MOTOR_2,_actuators.control[CH_VOLTAGE_RIGHT]); + setMotorDutyCycle(MOTOR_1, _actuators.get().control[CH_VOLTAGE_LEFT]); + setMotorDutyCycle(MOTOR_2, _actuators.get().control[CH_VOLTAGE_RIGHT]); } + return 0; } -uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n) +uint16_t RoboClaw::_sumBytes(uint8_t *buf, size_t n) { uint16_t sum = 0; + //printf("sum\n"); - for (size_t i=0;i 5 * 1000 * 1000LL && _consecutive_fail_count < 5) { DEVICE_LOG("collection error #%u", _consecutive_fail_count); } + _consecutive_fail_count++; /* restart the measurement state machine */ start(); return; + } else { /* apparently success */ _consecutive_fail_count = 0; @@ -886,7 +892,7 @@ test() warnx("read #%u", i); warnx("valid %u", (float)report.current_distance > report.min_distance - && (float)report.current_distance < report.max_distance ? 1 : 0); + && (float)report.current_distance < report.max_distance ? 1 : 0); warnx("measurement: %0.3f m", (double)report.current_distance); warnx("time: %llu", report.timestamp); } diff --git a/src/drivers/srf02/CMakeLists.txt b/src/drivers/srf02/CMakeLists.txt new file mode 100644 index 0000000000..da5b878696 --- /dev/null +++ b/src/drivers/srf02/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# 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. +# +############################################################################ +px4_add_module( + MODULE drivers__srf02 + MAIN srf02 + COMPILE_FLAGS + -Os + SRCS + srf02.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/srf02/srf02.cpp b/src/drivers/srf02/srf02.cpp new file mode 100644 index 0000000000..92fda5d41f --- /dev/null +++ b/src/drivers/srf02/srf02.cpp @@ -0,0 +1,961 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file srf02.cpp + * + * Driver for the SRF02 sonar range finder adapted from the Maxbotix sonar range finder driver (mb12xx). + */ + +#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 + +/* Configuration Constants */ +#define SRF02_BUS PX4_I2C_BUS_EXPANSION +#define SRF02_BASEADDR 0x70 /* 7-bit address. 8-bit address is 0xE0 */ +#define SRF02_DEVICE_PATH "/dev/srf02" + +/* MB12xx Registers addresses */ + +#define SRF02_TAKE_RANGE_REG 0x51 /* Measure range Register */ +#define SRF02_SET_ADDRESS_0 0xA0 /* Change address 0 Register */ +#define SRF02_SET_ADDRESS_1 0xAA /* Change address 1 Register */ +#define SRF02_SET_ADDRESS_2 0xA5 /* Change address 2 Register */ + +/* Device limits */ +#define SRF02_MIN_DISTANCE (0.20f) +#define SRF02_MAX_DISTANCE (7.65f) + +#define SRF02_CONVERSION_INTERVAL 100000 /* 60ms for one sonar */ +#define TICKS_BETWEEN_SUCCESIVE_FIRES 100000 /* 30ms between each sonar measurement (watch out for interference!) */ + + +/* 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 + +class SRF02 : public device::I2C +{ +public: + SRF02(int bus = SRF02_BUS, int address = SRF02_BASEADDR); + virtual ~SRF02(); + + 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(); + +protected: + virtual int probe(); + +private: + float _min_distance; + float _max_distance; + work_s _work; + ringbuffer::RingBuffer *_reports; + bool _sensor_ok; + int _measure_ticks; + bool _collect_phase; + int _class_instance; + int _orb_class_instance; + + orb_advert_t _distance_sensor_topic; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + + uint8_t _cycle_counter; /* counter in cycle to change i2c adresses */ + int _cycling_rate; /* */ + uint8_t _index_counter; /* temporary sonar i2c address */ + std::vector addr_ind; /* temp sonar i2c address vector */ + std::vector + _latest_sonar_measurements; /* vector to store latest sonar measurements in before writing to report */ + + + /** + * Test whether the device supported by the driver is present at a + * specific address. + * + * @param address The I2C bus address to probe. + * @return True if the device is present. + */ + int probe_address(uint8_t address); + + /** + * Initialise 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(); + + /** + * Stop the automatic measurement state machine. + */ + void stop(); + + /** + * 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 SRF02_MIN_DISTANCE + * and SRF02_MAX_DISTANCE + */ + void set_minimum_distance(float min); + void set_maximum_distance(float max); + float get_minimum_distance(); + float get_maximum_distance(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void cycle(); + int measure(); + int collect(); + /** + * 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); + + +}; + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int srf02_main(int argc, char *argv[]); + +SRF02::SRF02(int bus, int address) : + I2C("MB12xx", SRF02_DEVICE_PATH, bus, address, 100000), + _min_distance(SRF02_MIN_DISTANCE), + _max_distance(SRF02_MAX_DISTANCE), + _reports(nullptr), + _sensor_ok(false), + _measure_ticks(0), + _collect_phase(false), + _class_instance(-1), + _orb_class_instance(-1), + _distance_sensor_topic(nullptr), + _sample_perf(perf_alloc(PC_ELAPSED, "srf02_read")), + _comms_errors(perf_alloc(PC_COUNT, "srf02_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "srf02_buffer_overflows")), + _cycle_counter(0), /* initialising counter for cycling function to zero */ + _cycling_rate(0), /* initialising cycling rate (which can differ depending on one sonar or multiple) */ + _index_counter(0) /* initialising temp sonar i2c address to zero */ + +{ + /* enable debug() calls */ + _debug_enabled = false; + + /* work_cancel in the dtor will explode if we don't do this... */ + memset(&_work, 0, sizeof(_work)); +} + +SRF02::~SRF02() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) { + delete _reports; + } + + if (_class_instance != -1) { + unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance); + } + + /* free perf counters */ + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_buffer_overflows); +} + +int +SRF02::init() +{ + int ret = ERROR; + + /* do I2C init (and probe) first */ + if (I2C::init() != OK) { + goto out; + } + + /* allocate basic report buffers */ + _reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s)); + + _index_counter = SRF02_BASEADDR; /* set temp sonar i2c address to base adress */ + set_address(_index_counter); /* set I2c port to temp sonar i2c adress */ + + if (_reports == nullptr) { + goto out; + } + + _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); + + if (_class_instance == CLASS_DEVICE_PRIMARY) { + /* get a publish handle on the range finder topic */ + struct distance_sensor_s ds_report = {}; + + _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, + &_orb_class_instance, ORB_PRIO_LOW); + + if (_distance_sensor_topic == nullptr) { + DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); + } + } + + // XXX we should find out why we need to wait 200 ms here + usleep(200000); + + /* check for connected rangefinders on each i2c port: + We start from i2c base address (0x70 = 112) and count downwards + So second iteration it uses i2c address 111, third iteration 110 and so on*/ + for (unsigned counter = 0; counter <= MB12XX_MAX_RANGEFINDERS; counter++) { + _index_counter = SRF02_BASEADDR - counter; /* set temp sonar i2c address to base adress - counter */ + set_address(_index_counter); /* set I2c port to temp sonar i2c adress */ + int ret2 = measure(); + + if (ret2 == 0) { /* sonar is present -> store address_index in array */ + addr_ind.push_back(_index_counter); + DEVICE_DEBUG("sonar added"); + _latest_sonar_measurements.push_back(200); + } + } + + _index_counter = SRF02_BASEADDR; + set_address(_index_counter); /* set i2c port back to base adress for rest of driver */ + + /* if only one sonar detected, no special timing is required between firing, so use default */ + if (addr_ind.size() == 1) { + _cycling_rate = SRF02_CONVERSION_INTERVAL; + + } else { + _cycling_rate = TICKS_BETWEEN_SUCCESIVE_FIRES; + } + + /* show the connected sonars in terminal */ + for (unsigned i = 0; i < addr_ind.size(); i++) { + DEVICE_LOG("sonar %d with address %d added", (i + 1), addr_ind[i]); + } + + DEVICE_DEBUG("Number of sonars connected: %d", addr_ind.size()); + + ret = OK; + /* sensor is ok, but we don't really know if it is within range */ + _sensor_ok = true; +out: + return ret; +} + +int +SRF02::probe() +{ + return measure(); +} + +void +SRF02::set_minimum_distance(float min) +{ + _min_distance = min; +} + +void +SRF02::set_maximum_distance(float max) +{ + _max_distance = max; +} + +float +SRF02::get_minimum_distance() +{ + return _min_distance; +} + +float +SRF02::get_maximum_distance() +{ + return _max_distance; +} + +int +SRF02::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; + + /* external signalling (DRDY) 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(_cycling_rate); + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + + } + + 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 */ + int ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(_cycling_rate)) { + 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(); + } + + 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; + } + + irqstate_t flags = irqsave(); + + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } + + irqrestore(flags); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _reports->size(); + + case SENSORIOCRESET: + /* XXX implement this */ + return -EINVAL; + + case RANGEFINDERIOCSETMINIUMDISTANCE: { + set_minimum_distance(*(float *)arg); + return 0; + } + break; + + case RANGEFINDERIOCSETMAXIUMDISTANCE: { + set_maximum_distance(*(float *)arg); + return 0; + } + break; + + default: + /* give it to the superclass */ + return I2C::ioctl(filp, cmd, arg); + } +} + +ssize_t +SRF02::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; + + /* 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(rbuf)) { + ret += sizeof(*rbuf); + rbuf++; + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement - run one conversion */ + do { + _reports->flush(); + + /* trigger a measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + /* wait for it to complete */ + usleep(_cycling_rate * 2); + + /* run the collection phase */ + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + if (_reports->get(rbuf)) { + ret = sizeof(*rbuf); + } + + } while (0); + + return ret; +} + +int +SRF02::measure() +{ + + int ret; + + /* + * Send the command to begin a measurement. + */ + + uint8_t cmd[2]; + cmd[0] = 0x00; + cmd[1] = SRF02_TAKE_RANGE_REG; + ret = transfer(cmd, 2, nullptr, 0); + + if (OK != ret) { + perf_count(_comms_errors); + DEVICE_DEBUG("i2c::transfer returned %d", ret); + return ret; + } + + ret = OK; + + return ret; +} + +int +SRF02::collect() +{ + int ret = -EIO; + + /* read from the sensor */ + uint8_t val[2] = {0, 0}; + uint8_t cmd = 0x02; + perf_begin(_sample_perf); + + ret = transfer(&cmd, 1, nullptr, 0); + ret = transfer(nullptr, 0, &val[0], 2); + + if (ret < 0) { + DEVICE_DEBUG("error reading from sensor: %d", ret); + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; + } + + uint16_t distance_cm = val[0] << 8 | val[1]; + float distance_m = float(distance_cm) * 1e-2f; + + struct distance_sensor_s report; + report.timestamp = hrt_absolute_time(); + report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND; + report.orientation = 8; + report.current_distance = distance_m; + report.min_distance = get_minimum_distance(); + report.max_distance = get_maximum_distance(); + report.covariance = 0.0f; + /* TODO: set proper ID */ + report.id = 0; + + /* publish it, if we are the primary */ + if (_distance_sensor_topic != nullptr) { + orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); + } + + if (_reports->force(&report)) { + perf_count(_buffer_overflows); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + ret = OK; + + perf_end(_sample_perf); + return ret; +} + +void +SRF02::start() +{ + + /* reset the report ring and state machine */ + _collect_phase = false; + _reports->flush(); + + /* schedule a cycle to start things */ + 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 + }; + 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); + + } +} + +void +SRF02::stop() +{ + work_cancel(HPWORK, &_work); +} + +void +SRF02::cycle_trampoline(void *arg) +{ + + SRF02 *dev = (SRF02 *)arg; + + dev->cycle(); + +} + +void +SRF02::cycle() +{ + if (_collect_phase) { + _index_counter = addr_ind[_cycle_counter]; /*sonar from previous iteration collect is now read out */ + set_address(_index_counter); + + /* perform collection */ + if (OK != collect()) { + DEVICE_DEBUG("collection error"); + /* if error restart the measurement state machine */ + start(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* change i2c adress to next sonar */ + _cycle_counter = _cycle_counter + 1; + + if (_cycle_counter >= addr_ind.size()) { + _cycle_counter = 0; + } + + /* Is there a collect->measure gap? Yes, and the timing is set equal to the cycling_rate + Otherwise the next sonar would fire without the first one having received its reflected sonar pulse */ + + if (_measure_ticks > USEC2TICK(_cycling_rate)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&SRF02::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(_cycling_rate)); + return; + } + } + + /* Measurement (firing) phase */ + + /* ensure sonar i2c adress is still correct */ + _index_counter = addr_ind[_cycle_counter]; + set_address(_index_counter); + + /* Perform measurement */ + if (OK != measure()) { + DEVICE_DEBUG("measure error sonar adress %d", _index_counter); + } + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&SRF02::cycle_trampoline, + this, + USEC2TICK(_cycling_rate)); + +} + +void +SRF02::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"); +} + +/** + * Local functions in support of the shell command. + */ +namespace srf02 +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +SRF02 *g_dev; + +void start(); +void stop(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start() +{ + int fd; + + if (g_dev != nullptr) { + errx(1, "already started"); + } + + /* create the driver */ + g_dev = new SRF02(SRF02_BUS); + + if (g_dev == nullptr) { + goto fail; + } + + if (OK != g_dev->init()) { + goto fail; + } + + /* set the poll rate to default, starts automatic data collection */ + fd = open(SRF02_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + goto fail; + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + goto fail; + } + + exit(0); + +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Stop the driver + */ +void stop() +{ + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + + } else { + errx(1, "driver not running"); + } + + exit(0); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + struct distance_sensor_s report; + ssize_t sz; + int ret; + + int fd = open(SRF02_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + err(1, "%s open failed (try 'srf02 start' if the driver is not running", SRF02_DEVICE_PATH); + } + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + err(1, "immediate read failed"); + } + + warnx("single read"); + warnx("measurement: %0.2f m", (double)report.current_distance); + warnx("time: %llu", report.timestamp); + + /* 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("valid %u", (float)report.current_distance > report.min_distance + && (float)report.current_distance < report.max_distance ? 1 : 0); + warnx("measurement: %0.3f", (double)report.current_distance); + warnx("time: %llu", report.timestamp); + } + + /* reset the sensor polling to default rate */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { + errx(1, "failed to set default poll rate"); + } + + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(SRF02_DEVICE_PATH, 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() +{ + if (g_dev == nullptr) { + errx(1, "driver not running"); + } + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + +} /* namespace */ + +int +srf02_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) { + srf02::start(); + } + + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) { + srf02::stop(); + } + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) { + srf02::test(); + } + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) { + srf02::reset(); + } + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { + srf02::info(); + } + + errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); +} diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index 55b36f8bb4..1a54e11809 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -116,6 +116,7 @@ #include #include +#include /* Check that tone alarm and HRT timers are different */ #if defined(TONE_ALARM_TIMER) && defined(HRT_TIMER) @@ -273,6 +274,8 @@ # define rDMAR REG(STM32_GTIM_DMAR_OFFSET) #endif +#define CBRK_BUZZER_KEY 782097 + class ToneAlarm : public device::CDev { public: @@ -288,6 +291,12 @@ public: return _tune_names[tune]; } + enum { + CBRK_OFF = 0, + CBRK_ON, + CBRK_UNINIT + }; + private: static const unsigned _tune_max = 1024 * 8; // be reasonable about user tunes const char *_default_tunes[TONE_NUMBER_OF_TUNES]; @@ -307,6 +316,7 @@ private: unsigned _octave; unsigned _silence_length; // if nonzero, silence before next note bool _repeat; // if true, tune restarts at end + int _cbrk; //if true, no audio output hrt_call _note_call; // HRT callout for note completion @@ -375,7 +385,8 @@ ToneAlarm::ToneAlarm() : _default_tune_number(0), _user_tune(nullptr), _tune(nullptr), - _next(nullptr) + _next(nullptr), + _cbrk(CBRK_UNINIT) { // enable debug() calls //_debug_enabled = true; @@ -537,6 +548,13 @@ ToneAlarm::rest_duration(unsigned rest_length, unsigned dots) void ToneAlarm::start_note(unsigned note) { + // check if circuit breaker is enabled + if (_cbrk == CBRK_UNINIT) { + _cbrk = circuit_breaker_enabled("CBRK_BUZZER", CBRK_BUZZER_KEY); + } + + if (_cbrk != CBRK_OFF) { return; } + // compute the divisor unsigned divisor = note_to_divisor(note); diff --git a/src/drivers/trone/trone.cpp b/src/drivers/trone/trone.cpp index 83834b7f84..e3b77fb19c 100644 --- a/src/drivers/trone/trone.cpp +++ b/src/drivers/trone/trone.cpp @@ -212,7 +212,8 @@ static const uint8_t crc_table[] = { 0xfa, 0xfd, 0xf4, 0xf3 }; - static uint8_t crc8(uint8_t *p, uint8_t len) { +static uint8_t crc8(uint8_t *p, uint8_t len) +{ uint16_t i; uint16_t crc = 0x0; @@ -301,7 +302,7 @@ TRONE::init() _reports->get(&ds_report); _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, - &_orb_class_instance, ORB_PRIO_LOW); + &_orb_class_instance, ORB_PRIO_LOW); if (_distance_sensor_topic == nullptr) { DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); @@ -318,23 +319,23 @@ out: int TRONE::probe() { - uint8_t who_am_i=0; + uint8_t who_am_i = 0; const uint8_t cmd = TRONE_WHO_AM_I_REG; - - // set the I2C bus address - set_address(TRONE_BASEADDR); - - // can't use a single transfer as TROne need a bit of time for internal processing + + // set the I2C bus address + set_address(TRONE_BASEADDR); + + // can't use a single transfer as TROne need a bit of time for internal processing if (transfer(&cmd, 1, nullptr, 0) == OK) { - if ( transfer(nullptr, 0, &who_am_i, 1) == OK && who_am_i == TRONE_WHO_AM_I_REG_VAL){ - return measure(); - } - } + if (transfer(nullptr, 0, &who_am_i, 1) == OK && who_am_i == TRONE_WHO_AM_I_REG_VAL) { + return measure(); + } + } DEVICE_DEBUG("WHO_AM_I byte mismatch 0x%02x should be 0x%02x\n", - (unsigned)who_am_i, - TRONE_WHO_AM_I_REG_VAL); + (unsigned)who_am_i, + TRONE_WHO_AM_I_REG_VAL); // not found on any address return -EIO; @@ -596,7 +597,7 @@ TRONE::collect() // This validation check can be used later _valid = crc8(val, 2) == val[2] && (float)report.current_distance > report.min_distance - && (float)report.current_distance < report.max_distance ? 1 : 0; + && (float)report.current_distance < report.max_distance ? 1 : 0; /* publish it, if we are the primary */ if (_distance_sensor_topic != nullptr) { @@ -680,10 +681,10 @@ TRONE::cycle() if (_measure_ticks > USEC2TICK(TRONE_CONVERSION_INTERVAL)) { /* schedule a fresh cycle call when we are ready to measure again */ work_queue(HPWORK, - &_work, - (worker_t)&TRONE::cycle_trampoline, - this, - _measure_ticks - USEC2TICK(TRONE_CONVERSION_INTERVAL)); + &_work, + (worker_t)&TRONE::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(TRONE_CONVERSION_INTERVAL)); return; } @@ -699,10 +700,10 @@ TRONE::cycle() /* schedule a fresh cycle call when the measurement is done */ work_queue(HPWORK, - &_work, - (worker_t)&TRONE::cycle_trampoline, - this, - USEC2TICK(TRONE_CONVERSION_INTERVAL)); + &_work, + (worker_t)&TRONE::cycle_trampoline, + this, + USEC2TICK(TRONE_CONVERSION_INTERVAL)); } void diff --git a/src/firmware/nuttx/CMakeLists.txt b/src/firmware/nuttx/CMakeLists.txt index 6304ab2642..be6f9b1062 100644 --- a/src/firmware/nuttx/CMakeLists.txt +++ b/src/firmware/nuttx/CMakeLists.txt @@ -5,13 +5,6 @@ px4_nuttx_generate_builtin_commands( ${config_extra_builtin_cmds} ) -px4_nuttx_add_romfs(OUT romfs - ROOT ROMFS/px4fmu_common - EXTRAS ${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}_${LABEL}.bin - ) - -add_dependencies(romfs fw_io) - # add executable add_executable(firmware_nuttx builtin_commands.c) @@ -33,8 +26,6 @@ if(NOT ${BOARD} STREQUAL "sim") set_target_properties(firmware_nuttx PROPERTIES LINK_FLAGS ${main_link_flags}) endif() -set(fw_file ${CMAKE_CURRENT_BINARY_DIR}/${target_name}.px4) - target_link_libraries(firmware_nuttx -Wl,--start-group ${module_libraries} @@ -49,6 +40,14 @@ add_custom_target(check_weak ) if(NOT ${BOARD} STREQUAL "sim") + + + px4_nuttx_add_romfs(OUT romfs + ROOT ROMFS/px4fmu_common + EXTRAS ${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}.bin + ) + add_dependencies(romfs fw_io) + set(fw_file ${CMAKE_CURRENT_BINARY_DIR}/${OS}-${BOARD}-${LABEL}.px4) @@ -81,21 +80,21 @@ if(NOT ${BOARD} STREQUAL "sim") add_custom_target(debug_io COMMAND ${GDB} - ${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}_${LABEL} + ${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board} DEPENDS firmware_nuttx ${CMAKE_CURRENT_BINARY_DIR}/.gdbinit ) add_custom_target(debug_io_tui COMMAND ${GDBTUI} - ${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}_${LABEL} + ${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board} DEPENDS firmware_nuttx ${CMAKE_CURRENT_BINARY_DIR}/.gdbinit ) add_custom_target(debug_io_ddd COMMAND ${DDD} --debugger ${GDB} - ${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}_${LABEL} + ${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board} DEPENDS firmware_nuttx ${CMAKE_CURRENT_BINARY_DIR}/.gdbinit ) diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt index 7c30f1dfa6..3417f49d18 100644 --- a/src/firmware/posix/CMakeLists.txt +++ b/src/firmware/posix/CMakeLists.txt @@ -10,7 +10,7 @@ add_executable(mainapp apps.h ) -if (NOT ${CMAKE_C_COMPILER_ID} STREQUAL "Clang") +if (NOT ${CMAKE_C_COMPILER_ID} STREQUAL "Clang" OR NOT APPLE) target_link_libraries(mainapp -Wl,--start-group ${module_libraries} @@ -18,17 +18,36 @@ if (NOT ${CMAKE_C_COMPILER_ID} STREQUAL "Clang") -Wl,--end-group ) else() - if (APPLE) - target_link_libraries(mainapp - ${module_libraries} - pthread m - ) - else() - target_link_libraries(mainapp - ${module_libraries} - pthread m rt - ) - endif() + target_link_libraries(mainapp + ${module_libraries} + pthread m + ) endif() +add_custom_target(run_config + COMMAND Tools/sitl_run.sh "${config_sitl_rcS}" "${config_sitl_debugger}" + "${config_sitl_viewer}" "${CMAKE_BINARY_DIR}" + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + USES_TERMINAL + ) +add_dependencies(run_config mainapp) + +foreach(viewer jmavsim gazebo) + foreach(debugger none gdb lldb) + if (debugger STREQUAL "none") + set(_targ_name "${viewer}") + else() + set(_targ_name "${viewer}_${debugger}") + endif() + add_custom_target(${_targ_name} + COMMAND Tools/sitl_run.sh "${config_sitl_rcS}" + "${debugger}" + "${viewer}" "${CMAKE_BINARY_DIR}" + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + USES_TERMINAL + ) + add_dependencies(${_targ_name} mainapp) + endforeach() +endforeach() + # vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index ca82ddd5e8..f56513e209 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -55,154 +55,183 @@ get_rot_matrix(enum Rotation rot, math::Matrix<3, 3> *rot_matrix) __EXPORT void rotate_3f(enum Rotation rot, float &x, float &y, float &z) { - float tmp; - switch (rot) { - case ROTATION_NONE: - case ROTATION_MAX: - return; - case ROTATION_YAW_45: { - tmp = HALF_SQRT_2*(x - y); - y = HALF_SQRT_2*(x + y); - x = tmp; - return; - } - case ROTATION_YAW_90: { - tmp = x; x = -y; y = tmp; - return; - } - case ROTATION_YAW_135: { - tmp = -HALF_SQRT_2*(x + y); - y = HALF_SQRT_2*(x - y); - x = tmp; - return; - } - case ROTATION_YAW_180: - x = -x; y = -y; - return; - case ROTATION_YAW_225: { - tmp = HALF_SQRT_2*(y - x); - y = -HALF_SQRT_2*(x + y); - x = tmp; - return; - } - case ROTATION_YAW_270: { - tmp = x; x = y; y = -tmp; - return; - } - case ROTATION_YAW_315: { - tmp = HALF_SQRT_2*(x + y); - y = HALF_SQRT_2*(y - x); - x = tmp; - return; - } - case ROTATION_ROLL_180: { - y = -y; z = -z; - return; - } - case ROTATION_ROLL_180_YAW_45: { - tmp = HALF_SQRT_2*(x + y); - y = HALF_SQRT_2*(x - y); - x = tmp; z = -z; - return; - } - case ROTATION_ROLL_180_YAW_90: { - tmp = x; x = y; y = tmp; z = -z; - return; - } - case ROTATION_ROLL_180_YAW_135: { - tmp = HALF_SQRT_2*(y - x); - y = HALF_SQRT_2*(y + x); - x = tmp; z = -z; - return; - } - case ROTATION_PITCH_180: { - x = -x; z = -z; - return; - } - case ROTATION_ROLL_180_YAW_225: { - tmp = -HALF_SQRT_2*(x + y); - y = HALF_SQRT_2*(y - x); - x = tmp; z = -z; - return; - } - case ROTATION_ROLL_180_YAW_270: { - tmp = x; x = -y; y = -tmp; z = -z; - return; - } - case ROTATION_ROLL_180_YAW_315: { - tmp = HALF_SQRT_2*(x - y); - y = -HALF_SQRT_2*(x + y); - x = tmp; z = -z; - return; - } - case ROTATION_ROLL_90: { - tmp = z; z = y; y = -tmp; - return; - } - case ROTATION_ROLL_90_YAW_45: { - tmp = z; z = y; y = -tmp; - tmp = HALF_SQRT_2*(x - y); - y = HALF_SQRT_2*(x + y); - x = tmp; - return; - } - case ROTATION_ROLL_90_YAW_90: { - tmp = z; z = y; y = -tmp; - tmp = x; x = -y; y = tmp; - return; - } - case ROTATION_ROLL_90_YAW_135: { - tmp = z; z = y; y = -tmp; - tmp = -HALF_SQRT_2*(x + y); - y = HALF_SQRT_2*(x - y); - x = tmp; - return; - } - case ROTATION_ROLL_270: { - tmp = z; z = -y; y = tmp; - return; - } - case ROTATION_ROLL_270_YAW_45: { - tmp = z; z = -y; y = tmp; - tmp = HALF_SQRT_2*(x - y); - y = HALF_SQRT_2*(x + y); - x = tmp; - return; - } - case ROTATION_ROLL_270_YAW_90: { - tmp = z; z = -y; y = tmp; - tmp = x; x = -y; y = tmp; - return; - } - case ROTATION_ROLL_270_YAW_135: { - tmp = z; z = -y; y = tmp; - tmp = -HALF_SQRT_2*(x + y); - y = HALF_SQRT_2*(x - y); - x = tmp; - return; - } - case ROTATION_ROLL_270_YAW_270: { - tmp = z; z = -y; y = tmp; - tmp = x; x = y; y = -tmp; - return; - } - case ROTATION_PITCH_90: { - tmp = z; z = -x; x = tmp; - return; - } - case ROTATION_PITCH_270: { - tmp = z; z = x; x = -tmp; - return; - } - case ROTATION_ROLL_180_PITCH_270: { - tmp = z; z = x; x = tmp; - y = -y; - return; - } - case ROTATION_PITCH_90_YAW_180: { - tmp = x; x = z; z = tmp; - y = -y; - return; - } - } + float tmp; + + switch (rot) { + case ROTATION_NONE: + case ROTATION_MAX: + return; + + case ROTATION_YAW_45: { + tmp = HALF_SQRT_2 * (x - y); + y = HALF_SQRT_2 * (x + y); + x = tmp; + return; + } + + case ROTATION_YAW_90: { + tmp = x; x = -y; y = tmp; + return; + } + + case ROTATION_YAW_135: { + tmp = -HALF_SQRT_2 * (x + y); + y = HALF_SQRT_2 * (x - y); + x = tmp; + return; + } + + case ROTATION_YAW_180: + x = -x; y = -y; + return; + + case ROTATION_YAW_225: { + tmp = HALF_SQRT_2 * (y - x); + y = -HALF_SQRT_2 * (x + y); + x = tmp; + return; + } + + case ROTATION_YAW_270: { + tmp = x; x = y; y = -tmp; + return; + } + + case ROTATION_YAW_315: { + tmp = HALF_SQRT_2 * (x + y); + y = HALF_SQRT_2 * (y - x); + x = tmp; + return; + } + + case ROTATION_ROLL_180: { + y = -y; z = -z; + return; + } + + case ROTATION_ROLL_180_YAW_45: { + tmp = HALF_SQRT_2 * (x + y); + y = HALF_SQRT_2 * (x - y); + x = tmp; z = -z; + return; + } + + case ROTATION_ROLL_180_YAW_90: { + tmp = x; x = y; y = tmp; z = -z; + return; + } + + case ROTATION_ROLL_180_YAW_135: { + tmp = HALF_SQRT_2 * (y - x); + y = HALF_SQRT_2 * (y + x); + x = tmp; z = -z; + return; + } + + case ROTATION_PITCH_180: { + x = -x; z = -z; + return; + } + + case ROTATION_ROLL_180_YAW_225: { + tmp = -HALF_SQRT_2 * (x + y); + y = HALF_SQRT_2 * (y - x); + x = tmp; z = -z; + return; + } + + case ROTATION_ROLL_180_YAW_270: { + tmp = x; x = -y; y = -tmp; z = -z; + return; + } + + case ROTATION_ROLL_180_YAW_315: { + tmp = HALF_SQRT_2 * (x - y); + y = -HALF_SQRT_2 * (x + y); + x = tmp; z = -z; + return; + } + + case ROTATION_ROLL_90: { + tmp = z; z = y; y = -tmp; + return; + } + + case ROTATION_ROLL_90_YAW_45: { + tmp = z; z = y; y = -tmp; + tmp = HALF_SQRT_2 * (x - y); + y = HALF_SQRT_2 * (x + y); + x = tmp; + return; + } + + case ROTATION_ROLL_90_YAW_90: { + tmp = z; z = y; y = -tmp; + tmp = x; x = -y; y = tmp; + return; + } + + case ROTATION_ROLL_90_YAW_135: { + tmp = z; z = y; y = -tmp; + tmp = -HALF_SQRT_2 * (x + y); + y = HALF_SQRT_2 * (x - y); + x = tmp; + return; + } + + case ROTATION_ROLL_270: { + tmp = z; z = -y; y = tmp; + return; + } + + case ROTATION_ROLL_270_YAW_45: { + tmp = z; z = -y; y = tmp; + tmp = HALF_SQRT_2 * (x - y); + y = HALF_SQRT_2 * (x + y); + x = tmp; + return; + } + + case ROTATION_ROLL_270_YAW_90: { + tmp = z; z = -y; y = tmp; + tmp = x; x = -y; y = tmp; + return; + } + + case ROTATION_ROLL_270_YAW_135: { + tmp = z; z = -y; y = tmp; + tmp = -HALF_SQRT_2 * (x + y); + y = HALF_SQRT_2 * (x - y); + x = tmp; + return; + } + + case ROTATION_ROLL_270_YAW_270: { + tmp = z; z = -y; y = tmp; + tmp = x; x = y; y = -tmp; + return; + } + + case ROTATION_PITCH_90: { + tmp = z; z = -x; x = tmp; + return; + } + + case ROTATION_PITCH_270: { + tmp = z; z = x; x = -tmp; + return; + } + + case ROTATION_ROLL_180_PITCH_270: { + tmp = z; z = x; x = tmp; + y = -y; + return; + } + + case ROTATION_PITCH_90_YAW_180: { + tmp = x; x = z; z = tmp; + y = -y; + return; + } + } } diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index 8128551e51..280b9cad0c 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -122,7 +122,7 @@ const rot_lookup_t rot_lookup[] = { * Get the rotation matrix */ __EXPORT void -get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix); +get_rot_matrix(enum Rotation rot, math::Matrix<3, 3> *rot_matrix); /** diff --git a/src/lib/dspal b/src/lib/dspal index 95e91546f4..c8e885aac5 160000 --- a/src/lib/dspal +++ b/src/lib/dspal @@ -1 +1 @@ -Subproject commit 95e91546f42e6d88d34a2bb29d0f428a8706c9e4 +Subproject commit c8e885aac51aa34855bb3880d3dc916b9e278083 diff --git a/src/lib/eigen b/src/lib/eigen deleted file mode 160000 index 240599e148..0000000000 --- a/src/lib/eigen +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 240599e1488da540c2ae8753dea0907f6cfac8ee diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 30b9d9ec28..a4b1599a9d 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -83,16 +83,19 @@ __EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference return ref->timestamp; } -__EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +__EXPORT int map_projection_global_init(double lat_0, double lon_0, + uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { if (strcmp("commander", getprogname()) == 0) { return map_projection_init_timestamped(&mp_ref, lat_0, lon_0, timestamp); + } else { return -1; } } -__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, + uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { ref->lat_rad = lat_0 * M_DEG_TO_RAD; @@ -106,7 +109,8 @@ __EXPORT int map_projection_init_timestamped(struct map_projection_reference_s * return 0; } -__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, + double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { return map_projection_init_timestamped(ref, lat_0, lon_0, hrt_absolute_time()); } @@ -116,7 +120,8 @@ __EXPORT int map_projection_global_reference(double *ref_lat_rad, double *ref_lo return map_projection_reference(&mp_ref, ref_lat_rad, ref_lon_rad); } -__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad) +__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, + double *ref_lon_rad) { if (!map_projection_initialized(ref)) { return -1; @@ -134,7 +139,8 @@ __EXPORT int map_projection_global_project(double lat, double lon, float *x, flo } -__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y) +__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, + float *y) { if (!map_projection_initialized(ref)) { return -1; @@ -161,7 +167,8 @@ __EXPORT int map_projection_global_reproject(float x, float y, double *lat, doub return map_projection_reproject(&mp_ref, x, y, lat, lon); } -__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon) +__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, + double *lon) { if (!map_projection_initialized(ref)) { return -1; @@ -212,14 +219,16 @@ __EXPORT int globallocalconverter_init(double lat_0, double lon_0, float alt_0, { if (strcmp("commander", getprogname()) == 0) { gl_ref.alt = alt_0; - if (!map_projection_global_init(lat_0, lon_0, timestamp)) - { + + if (!map_projection_global_init(lat_0, lon_0, timestamp)) { gl_ref.init_done = true; return 0; + } else { gl_ref.init_done = false; return -1; } + } else { return -1; } @@ -260,8 +269,7 @@ __EXPORT int globallocalconverter_getref(double *lat_0, double *lon_0, float *al return -1; } - if (map_projection_global_getref(lat_0, lon_0)) - { + if (map_projection_global_getref(lat_0, lon_0)) { return -1; } @@ -283,7 +291,8 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou double d_lat = lat_next_rad - lat_now_rad; double d_lon = lon_next_rad - lon_now_rad; - double a = sin(d_lat / (double)2.0) * sin(d_lat / (double)2.0) + sin(d_lon / (double)2.0) * sin(d_lon / (double)2.0) * cos(lat_now_rad) * cos(lat_next_rad); + double a = sin(d_lat / (double)2.0) * sin(d_lat / (double)2.0) + sin(d_lon / (double)2.0) * sin(d_lon / + (double)2.0) * cos(lat_now_rad) * cos(lat_next_rad); double c = (double)2.0 * atan2(sqrt(a), sqrt((double)1.0 - a)); return CONSTANTS_RADIUS_OF_EARTH * c; @@ -299,14 +308,16 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub double d_lon = lon_next_rad - lon_now_rad; /* conscious mix of double and float trig function to maximize speed and efficiency */ - float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)); + float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , + cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)); theta = _wrap_pi(theta); return theta; } -__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e) +__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, + float *v_e) { double lat_now_rad = lat_now * M_DEG_TO_RAD; double lon_now_rad = lon_now * M_DEG_TO_RAD; @@ -316,11 +327,13 @@ __EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double double d_lon = lon_next_rad - lon_now_rad; /* conscious mix of double and float trig function to maximize speed and efficiency */ - *v_n = CONSTANTS_RADIUS_OF_EARTH * (cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)); + *v_n = CONSTANTS_RADIUS_OF_EARTH * (cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos( + d_lon)); *v_e = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad); } -__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e) +__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, + float *v_n, float *v_e) { double lat_now_rad = lat_now * M_DEG_TO_RAD; double lon_now_rad = lon_now * M_DEG_TO_RAD; @@ -335,7 +348,8 @@ __EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, d *v_e = CONSTANTS_RADIUS_OF_EARTH * d_lon * cos(lat_now_rad); } -__EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res) +__EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, + double *lon_res) { double lat_now_rad = lat_now * M_DEG_TO_RAD; double lon_now_rad = lon_now * M_DEG_TO_RAD; @@ -346,7 +360,8 @@ __EXPORT void add_vector_to_global_position(double lat_now, double lon_now, floa // Additional functions - @author Doug Weibel -__EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end) +__EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, + double lat_start, double lon_start, double lat_end, double lon_end) { // This function returns the distance to the nearest point on the track line. Distance is positive if current // position is right of the track and negative if left of the track as seen from a point on the track line @@ -397,7 +412,8 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, d } -__EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, +__EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, + double lat_center, double lon_center, float radius, float arc_start_bearing, float arc_sweep) { // This function returns the distance to the nearest point on the track arc. Distance is positive if current @@ -436,10 +452,12 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do in_sector = false; // Case where sector does not span zero - if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) { in_sector = true; } + if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start + && bearing_now <= bearing_sector_end) { in_sector = true; } // Case where sector does span zero - if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start || bearing_now < bearing_sector_end)) { in_sector = true; } + if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start + || bearing_now < bearing_sector_end)) { in_sector = true; } // If in the sector then calculate distance and bearing to closest point if (in_sector) { @@ -479,6 +497,7 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do if (dist_to_start < dist_to_end) { crosstrack_error->distance = dist_to_start; crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); + } else { crosstrack_error->past_end = true; crosstrack_error->distance = dist_to_end; @@ -539,6 +558,7 @@ __EXPORT float _wrap_pi(float bearing) } int c = 0; + while (bearing >= M_PI_F) { bearing -= M_TWOPI_F; @@ -548,6 +568,7 @@ __EXPORT float _wrap_pi(float bearing) } c = 0; + while (bearing < -M_PI_F) { bearing += M_TWOPI_F; @@ -567,6 +588,7 @@ __EXPORT float _wrap_2pi(float bearing) } int c = 0; + while (bearing >= M_TWOPI_F) { bearing -= M_TWOPI_F; @@ -576,6 +598,7 @@ __EXPORT float _wrap_2pi(float bearing) } c = 0; + while (bearing < 0.0f) { bearing += M_TWOPI_F; @@ -595,6 +618,7 @@ __EXPORT float _wrap_180(float bearing) } int c = 0; + while (bearing >= 180.0f) { bearing -= 360.0f; @@ -604,6 +628,7 @@ __EXPORT float _wrap_180(float bearing) } c = 0; + while (bearing < -180.0f) { bearing += 360.0f; @@ -623,6 +648,7 @@ __EXPORT float _wrap_360(float bearing) } int c = 0; + while (bearing >= 360.0f) { bearing -= 360.0f; @@ -632,6 +658,7 @@ __EXPORT float _wrap_360(float bearing) } c = 0; + while (bearing < 0.0f) { bearing += 360.0f; diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index ca3587b934..f77a8b58a4 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -113,7 +113,8 @@ __EXPORT int map_projection_global_reference(double *ref_lat_rad, double *ref_lo * Writes the reference values of the projection given by the argument to ref_lat and ref_lon * @return 0 if map_projection_init was called before, -1 else */ -__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad); +__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, + double *ref_lon_rad); /** * Initializes the global map transformation. @@ -158,15 +159,16 @@ __EXPORT int map_projection_init(struct map_projection_reference_s *ref, double __EXPORT int map_projection_global_project(double lat, double lon, float *x, float *y); - /* Transforms a point in the geographic coordinate system to the local - * azimuthal equidistant plane using the projection given by the argument - * @param x north - * @param y east - * @param lat in degrees (47.1234567°, not 471234567°) - * @param lon in degrees (8.1234567°, not 81234567°) - * @return 0 if map_projection_init was called before, -1 else - */ -__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y); +/* Transforms a point in the geographic coordinate system to the local + * azimuthal equidistant plane using the projection given by the argument +* @param x north +* @param y east +* @param lat in degrees (47.1234567°, not 471234567°) +* @param lon in degrees (8.1234567°, not 81234567°) +* @return 0 if map_projection_init was called before, -1 else +*/ +__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, + float *y); /** * Transforms a point in the local azimuthal equidistant plane to the @@ -190,7 +192,8 @@ __EXPORT int map_projection_global_reproject(float x, float y, double *lat, doub * @param lon in degrees (8.1234567°, not 81234567°) * @return 0 if map_projection_init was called before, -1 else */ -__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon); +__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, + double *lon); /** * Get reference position of the global map projection @@ -243,15 +246,20 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou */ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); -__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e); +__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, + float *v_e); -__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e); +__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, + float *v_n, float *v_e); -__EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res); +__EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, + double *lon_res); -__EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end); +__EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, + double lat_start, double lon_start, double lat_end, double lon_end); -__EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, +__EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, + double lat_center, double lon_center, float radius, float arc_start_bearing, float arc_sweep); /* diff --git a/src/lib/geo_lookup/geo_mag_declination.c b/src/lib/geo_lookup/geo_mag_declination.c index c41d526061..58a3a351d9 100644 --- a/src/lib/geo_lookup/geo_mag_declination.c +++ b/src/lib/geo_lookup/geo_mag_declination.c @@ -35,7 +35,7 @@ * @file geo_mag_declination.c * * Calculation / lookup table for earth magnetic field declination. -* +* * Lookup table from Scott Ferguson * * XXX Lookup table currently too coarse in resolution (only full degrees) @@ -55,18 +55,18 @@ static const int8_t declination_table[13][37] = \ { { 46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, -66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46 }, - { 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, -3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, 29, 30 }, - { 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, -40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21 }, - { 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, 8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, 10, 13, 15, 16 }, - { 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, -10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12 }, - { 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, -14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10 }, - { 9, 9, 9, 9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, 7, 8, 9 }, - { 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, 0, 0, 0, 1, 3, 5, 7, 8 }, - { 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, 0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8 }, - { 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, 0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6 }, - { 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, -16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5 }, - { 4, 8, 12, 15, 17, 18, 16, 12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, 0, 4 }, - { 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, 13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3 }, + { 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, -3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, 29, 30 }, + { 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, -40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21 }, + { 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, 8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, 10, 13, 15, 16 }, + { 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, -10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12 }, + { 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, -14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10 }, + { 9, 9, 9, 9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, 7, 8, 9 }, + { 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, 0, 0, 0, 1, 3, 5, 7, 8 }, + { 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, 0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8 }, + { 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, 0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6 }, + { 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, -16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5 }, + { 4, 8, 12, 15, 17, 18, 16, 12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, 0, 4 }, + { 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, 13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3 }, }; static float get_lookup_table_val(unsigned lat, unsigned lon); diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index b81f8868ec..38322e56a7 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -49,8 +49,7 @@ #ifdef CONFIG_ARCH_ARM #include "../CMSIS/Include/arm_math.h" #else -#include -#include +#include "modules/local_position_estimator/matrix/src/Matrix.hpp" #endif #include @@ -308,11 +307,9 @@ public: arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat); return res; #else - Eigen::Matrix Me = Eigen::Map > - (this->arm_mat.pData); - Eigen::Matrix Him = Eigen::Map > - (m.arm_mat.pData); - Eigen::Matrix Product = Me * Him; + matrix::Matrix Me(this->arm_mat.pData); + matrix::Matrix Him(m.arm_mat.pData); + matrix::Matrix Product = Me * Him; Matrix res(Product.data()); return res; #endif @@ -327,10 +324,8 @@ public: arm_mat_trans_f32(&this->arm_mat, &res.arm_mat); return res; #else - Eigen::Matrix Me = Eigen::Map > - (this->arm_mat.pData); - Me.transposeInPlace(); - Matrix res(Me.data()); + matrix::Matrix Me(this->arm_mat.pData); + Matrix res(Me.transpose().data()); return res; #endif } @@ -344,9 +339,8 @@ public: arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat); return res; #else - Eigen::Matrix Me = Eigen::Map > - (this->arm_mat.pData); - Eigen::Matrix MyInverse = Me.inverse(); //not sure if A = A.inverse() is a good idea + matrix::Matrix Me(this->arm_mat.pData); + matrix::Matrix MyInverse = Me.inverse(); //not sure if A = A.inverse() is a good idea Matrix res(MyInverse.data()); return res; #endif @@ -412,11 +406,9 @@ public: Vector res; arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col); #else - //probably nicer if this could go into a function like "eigen_mat_mult" or so - Eigen::Matrix Me = Eigen::Map > - (this->arm_mat.pData); - Eigen::VectorXf Vec = Eigen::Map(v.arm_col.pData, N); - Eigen::VectorXf Product = Me * Vec; + matrix::Matrix Me(this->arm_mat.pData); + matrix::Matrix Vec(v.arm_col.pData); + matrix::Matrix Product = Me * Vec; Vector res(Product.data()); #endif return res; diff --git a/src/lib/rc/sumd.c b/src/lib/rc/sumd.c index cea7790ec1..1d99f1d433 100644 --- a/src/lib/rc/sumd.c +++ b/src/lib/rc/sumd.c @@ -95,9 +95,11 @@ uint16_t sumd_crc16(uint16_t crc, uint8_t value) { int i; crc ^= (uint16_t)value << 8; + for (i = 0; i < 8; i++) { crc = (crc & 0x8000) ? (crc << 1) ^ 0x1021 : (crc << 1); } + return crc; } @@ -108,15 +110,17 @@ uint8_t sumd_crc8(uint8_t crc, uint8_t value) } int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, uint16_t *channels, - uint16_t max_chan_count) + uint16_t max_chan_count) { int ret = 1; + switch (_decode_state) { case SUMD_DECODE_STATE_UNSYNCED: - if(_debug) - printf( " SUMD_DECODE_STATE_UNSYNCED \n") ; - + if (_debug) { + printf(" SUMD_DECODE_STATE_UNSYNCED \n") ; + } + if (byte == SUMD_HEADER_ID) { _rxpacket.header = byte; _sumd = true; @@ -127,9 +131,11 @@ int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe _crc16 = sumd_crc16(_crc16, byte); _crc8 = sumd_crc8(_crc8, byte); _decode_state = SUMD_DECODE_STATE_GOT_HEADER; - if(_debug) - printf( " SUMD_DECODE_STATE_GOT_HEADER: %x \n", byte) ; - + + if (_debug) { + printf(" SUMD_DECODE_STATE_GOT_HEADER: %x \n", byte) ; + } + } else { ret = 3; } @@ -139,17 +145,23 @@ int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe case SUMD_DECODE_STATE_GOT_HEADER: if (byte == SUMD_ID_SUMD || byte == SUMD_ID_SUMH) { _rxpacket.status = byte; + if (byte == SUMD_ID_SUMH) { _sumd = false; } + if (_sumd) { _crc16 = sumd_crc16(_crc16, byte); + } else { - _crc8 = sumd_crc8(_crc8, byte); + _crc8 = sumd_crc8(_crc8, byte); } + _decode_state = SUMD_DECODE_STATE_GOT_STATE; - if(_debug) - printf( " SUMD_DECODE_STATE_GOT_STATE: %x \n", byte) ; + + if (_debug) { + printf(" SUMD_DECODE_STATE_GOT_STATE: %x \n", byte) ; + } } else { _decode_state = SUMD_DECODE_STATE_UNSYNCED; @@ -160,50 +172,65 @@ int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe case SUMD_DECODE_STATE_GOT_STATE: if (byte >= 2 && byte <= SUMD_MAX_CHANNELS) { _rxpacket.length = byte; + if (_sumd) { _crc16 = sumd_crc16(_crc16, byte); + } else { - _crc8 = sumd_crc8(_crc8, byte); + _crc8 = sumd_crc8(_crc8, byte); } + _rxlen++; _decode_state = SUMD_DECODE_STATE_GOT_LEN; - if(_debug) - printf( " SUMD_DECODE_STATE_GOT_LEN: %x (%d) \n", byte, byte) ; - + + if (_debug) { + printf(" SUMD_DECODE_STATE_GOT_LEN: %x (%d) \n", byte, byte) ; + } + } else { _decode_state = SUMD_DECODE_STATE_UNSYNCED; } + break; case SUMD_DECODE_STATE_GOT_LEN: _rxpacket.sumd_data[_rxlen] = byte; + if (_sumd) { _crc16 = sumd_crc16(_crc16, byte); + } else { - _crc8 = sumd_crc8(_crc8, byte); + _crc8 = sumd_crc8(_crc8, byte); } + _rxlen++; - - if (_rxlen <= ((_rxpacket.length*2) )) { - if(_debug) - printf( " SUMD_DECODE_STATE_GOT_DATA[%d]: %x\n", _rxlen-2, byte) ; + + if (_rxlen <= ((_rxpacket.length * 2))) { + if (_debug) { + printf(" SUMD_DECODE_STATE_GOT_DATA[%d]: %x\n", _rxlen - 2, byte) ; + } } else { _decode_state = SUMD_DECODE_STATE_GOT_DATA; - if(_debug) - printf( " SUMD_DECODE_STATE_GOT_DATA -- finish --\n") ; - + + if (_debug) { + printf(" SUMD_DECODE_STATE_GOT_DATA -- finish --\n") ; + } + } break; case SUMD_DECODE_STATE_GOT_DATA: _rxpacket.crc16_high = byte; - if(_debug) - printf( " SUMD_DECODE_STATE_GOT_CRC16[1]: %x [%x]\n", byte, ((_crc16 >> 8) & 0xff)) ; - + + if (_debug) { + printf(" SUMD_DECODE_STATE_GOT_CRC16[1]: %x [%x]\n", byte, ((_crc16 >> 8) & 0xff)) ; + } + if (_sumd) { _decode_state = SUMD_DECODE_STATE_GOT_CRC; + } else { _decode_state = SUMD_DECODE_STATE_GOT_CRC16_BYTE_1; } @@ -212,91 +239,108 @@ int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe case SUMD_DECODE_STATE_GOT_CRC16_BYTE_1: _rxpacket.crc16_low = byte; - if(_debug) - printf( " SUMD_DECODE_STATE_GOT_CRC16[2]: %x [%x]\n", byte, (_crc16 & 0xff)) ; - + + if (_debug) { + printf(" SUMD_DECODE_STATE_GOT_CRC16[2]: %x [%x]\n", byte, (_crc16 & 0xff)) ; + } + _decode_state = SUMD_DECODE_STATE_GOT_CRC16_BYTE_2; break; case SUMD_DECODE_STATE_GOT_CRC16_BYTE_2: _rxpacket.telemetry = byte; - if(_debug) - printf( " SUMD_DECODE_STATE_GOT_SUMH_TELEMETRY: %x\n", byte) ; - + + if (_debug) { + printf(" SUMD_DECODE_STATE_GOT_SUMH_TELEMETRY: %x\n", byte) ; + } + _decode_state = SUMD_DECODE_STATE_GOT_CRC; break; - + case SUMD_DECODE_STATE_GOT_CRC: if (_sumd) { _rxpacket.crc16_low = byte; - if(_debug) - printf( " SUMD_DECODE_STATE_GOT_CRC[2]: %x [%x]\n\n", byte, (_crc16 & 0xff)) ; - - if (_crc16 == (uint16_t)(_rxpacket.crc16_high<<8)+_rxpacket.crc16_low) { + + if (_debug) { + printf(" SUMD_DECODE_STATE_GOT_CRC[2]: %x [%x]\n\n", byte, (_crc16 & 0xff)) ; + } + + if (_crc16 == (uint16_t)(_rxpacket.crc16_high << 8) + _rxpacket.crc16_low) { _crcOK = true; } + } else { _rxpacket.crc8 = byte; - if(_debug) - printf( " SUMD_DECODE_STATE_GOT_CRC8_SUMH: %x [%x]\n\n", byte, _crc8) ; - + + if (_debug) { + printf(" SUMD_DECODE_STATE_GOT_CRC8_SUMH: %x [%x]\n\n", byte, _crc8) ; + } + if (_crc8 == _rxpacket.crc8) { _crcOK = true; } } - + if (_crcOK) { - if(_debug) - printf( " CRC - OK \n") ; + if (_debug) { + printf(" CRC - OK \n") ; + } if (_sumd) { - if(_debug) - printf( " Got valid SUMD Packet\n") ; - + if (_debug) { + printf(" Got valid SUMD Packet\n") ; + } + } else { - if(_debug) - printf( " Got valid SUMH Packet\n") ; - + if (_debug) { + printf(" Got valid SUMH Packet\n") ; + } + + } + + if (_debug) { + printf(" RXLEN: %d [Chans: %d] \n\n", _rxlen - 1, (_rxlen - 1) / 2) ; } - - if(_debug) - printf( " RXLEN: %d [Chans: %d] \n\n", _rxlen-1, (_rxlen-1)/2) ; ret = 0; unsigned i; uint8_t _cnt = *rx_count + 1; *rx_count = _cnt; - + *rssi = 100; /* received Channels */ if ((uint16_t)_rxpacket.length > max_chan_count) { _rxpacket.length = (uint8_t) max_chan_count; } + *channel_count = (uint16_t)_rxpacket.length; /* decode the actual packet */ /* reorder first 4 channels */ /* ch1 = roll -> sumd = ch2 */ - channels[0] = (uint16_t)((_rxpacket.sumd_data[1*2+1]<<8) | _rxpacket.sumd_data[1*2+2])>>3; + channels[0] = (uint16_t)((_rxpacket.sumd_data[1 * 2 + 1] << 8) | _rxpacket.sumd_data[1 * 2 + 2]) >> 3; /* ch2 = pitch -> sumd = ch2 */ - channels[1] = (uint16_t)((_rxpacket.sumd_data[2*2+1]<<8) | _rxpacket.sumd_data[2*2+2])>>3; + channels[1] = (uint16_t)((_rxpacket.sumd_data[2 * 2 + 1] << 8) | _rxpacket.sumd_data[2 * 2 + 2]) >> 3; /* ch3 = throttle -> sumd = ch2 */ - channels[2] = (uint16_t)((_rxpacket.sumd_data[0*2+1]<<8) | _rxpacket.sumd_data[0*2+2])>>3; + channels[2] = (uint16_t)((_rxpacket.sumd_data[0 * 2 + 1] << 8) | _rxpacket.sumd_data[0 * 2 + 2]) >> 3; /* ch4 = yaw -> sumd = ch2 */ - channels[3] = (uint16_t)((_rxpacket.sumd_data[3*2+1]<<8) | _rxpacket.sumd_data[3*2+2])>>3; + channels[3] = (uint16_t)((_rxpacket.sumd_data[3 * 2 + 1] << 8) | _rxpacket.sumd_data[3 * 2 + 2]) >> 3; /* we start at channel 5(index 4) */ unsigned chan_index = 4; for (i = 4; i < _rxpacket.length; i++) { - if(_debug) - printf( "ch[%d] : %x %x [ %x %d ]\n", i+1, _rxpacket.sumd_data[i*2+1], _rxpacket.sumd_data[i*2+2], ((_rxpacket.sumd_data[i*2+1]<<8) | _rxpacket.sumd_data[i*2+2])>>3, ((_rxpacket.sumd_data[i*2+1]<<8) | _rxpacket.sumd_data[i*2+2])>>3); - - channels[chan_index] = (uint16_t)((_rxpacket.sumd_data[i*2+1]<<8) | _rxpacket.sumd_data[i*2+2])>>3; + if (_debug) { + printf("ch[%d] : %x %x [ %x %d ]\n", i + 1, _rxpacket.sumd_data[i * 2 + 1], _rxpacket.sumd_data[i * 2 + 2], + ((_rxpacket.sumd_data[i * 2 + 1] << 8) | _rxpacket.sumd_data[i * 2 + 2]) >> 3, + ((_rxpacket.sumd_data[i * 2 + 1] << 8) | _rxpacket.sumd_data[i * 2 + 2]) >> 3); + } + + channels[chan_index] = (uint16_t)((_rxpacket.sumd_data[i * 2 + 1] << 8) | _rxpacket.sumd_data[i * 2 + 2]) >> 3; /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ //channels[chan_index] = (uint16_t)(channels[chan_index] * SUMD_SCALE_FACTOR + .5f) + SUMD_SCALE_OFFSET; @@ -306,8 +350,10 @@ int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe } else { /* decoding failed */ ret = 4; - if(_debug) - printf( " CRC - fail \n") ; + + if (_debug) { + printf(" CRC - fail \n") ; + } } diff --git a/src/lib/rc/sumd.h b/src/lib/rc/sumd.h index f4793edfc8..ba9e830b91 100644 --- a/src/lib/rc/sumd.h +++ b/src/lib/rc/sumd.h @@ -60,7 +60,7 @@ typedef struct { uint8_t header; ///< 0xA8 for a valid packet uint8_t status; ///< 0x01 valid and live SUMD data frame / 0x00 = SUMH / 0x81 = Failsafe uint8_t length; ///< Channels - uint8_t sumd_data[SUMD_MAX_CHANNELS*2]; ///< ChannelData (High Byte/ Low Byte) + uint8_t sumd_data[SUMD_MAX_CHANNELS * 2]; ///< ChannelData (High Byte/ Low Byte) uint8_t crc16_high; ///< High Byte of 16 Bit CRC uint8_t crc16_low; ///< Low Byte of 16 Bit CRC uint8_t telemetry; ///< Telemetry request @@ -102,7 +102,7 @@ __EXPORT int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_ uint16_t *channels, uint16_t max_chan_count); */ int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, - uint16_t *channels, uint16_t max_chan_count); + uint16_t *channels, uint16_t max_chan_count); + - __END_DECLS 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 5d3de32c0f..dcab4cde69 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -79,12 +79,14 @@ using math::Quaternion; class AttitudeEstimatorQ; -namespace attitude_estimator_q { +namespace attitude_estimator_q +{ AttitudeEstimatorQ *instance; } -class AttitudeEstimatorQ { +class AttitudeEstimatorQ +{ public: /** * Constructor @@ -160,6 +162,7 @@ private: /* Low pass filter for attitude rates */ math::LowPassFilter2p _lp_roll_rate; math::LowPassFilter2p _lp_pitch_rate; + math::LowPassFilter2p _lp_yaw_rate; hrt_abstime _vel_prev_t = 0; @@ -187,11 +190,12 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() : _voter_gyro(3), _voter_accel(3), _voter_mag(3), - _lp_roll_rate(250.0f, 20.0f), - _lp_pitch_rate(250.0f, 20.0f) + _lp_roll_rate(250.0f, 18.0f), + _lp_pitch_rate(250.0f, 18.0f), + _lp_yaw_rate(250, 10.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_gyro_bias = param_find("ATT_W_GYRO_BIAS"); @@ -235,11 +239,11 @@ int AttitudeEstimatorQ::start() /* start the task */ _control_task = px4_task_spawn_cmd("attitude_estimator_q", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2100, - (px4_main_t)&AttitudeEstimatorQ::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2100, + (px4_main_t)&AttitudeEstimatorQ::task_main_trampoline, + nullptr); if (_control_task < 0) { warn("task start failed"); @@ -289,6 +293,7 @@ void AttitudeEstimatorQ::task_main() // Poll error, sleep and try again usleep(10000); continue; + } else if (ret == 0) { // Poll timeout, do nothing continue; @@ -298,6 +303,7 @@ void AttitudeEstimatorQ::task_main() // Update sensors sensor_combined_s sensors; + if (!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)) { // Feed validator with recent sensor data @@ -311,6 +317,7 @@ void AttitudeEstimatorQ::task_main() 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]; @@ -319,10 +326,11 @@ void AttitudeEstimatorQ::task_main() _voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]); } + _voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3], - sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]); + sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]); _voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3], - sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]); + sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]); } int best_gyro, best_accel, best_mag; @@ -341,26 +349,28 @@ void AttitudeEstimatorQ::task_main() _data_good = true; if (!_failsafe && (_voter_gyro.failover_count() > 0 || - _voter_accel.failover_count() > 0 || - _voter_mag.failover_count() > 0)) { + _voter_accel.failover_count() > 0 || + _voter_mag.failover_count() > 0)) { _failsafe = true; mavlink_and_console_log_emergency(_mavlink_fd, "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)) { + _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_fd, "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))); + (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; } @@ -368,8 +378,10 @@ void AttitudeEstimatorQ::task_main() bool gpos_updated; orb_check(_global_pos_sub, &gpos_updated); + if (gpos_updated) { orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_gpos); + if (_mag_decl_auto && _gpos.eph < 20.0f && hrt_elapsed_time(&_gpos.timestamp) < 1000000) { /* set magnetic declination automatically */ _mag_decl = math::radians(get_mag_declination(_gpos.lat, _gpos.lon)); @@ -387,6 +399,7 @@ void AttitudeEstimatorQ::task_main() /* calculate acceleration in body frame */ _pos_acc = _q.conjugate_inversed((vel - _vel_prev) / vel_dt); } + _vel_prev_t = _gpos.timestamp; _vel_prev = vel; } @@ -400,7 +413,7 @@ void AttitudeEstimatorQ::task_main() // Time from previous iteration hrt_abstime now = hrt_absolute_time(); - float dt = (last_time > 0) ? ((now - last_time) / 1000000.0f) : 0.0f; + float dt = (last_time > 0) ? ((now - last_time) / 1000000.0f) : 0.00001f; last_time = now; if (dt > _dt_max) { @@ -420,9 +433,12 @@ void AttitudeEstimatorQ::task_main() att.pitch = euler(1); att.yaw = euler(2); - att.rollspeed = _rates(0); - att.pitchspeed = _rates(1); - att.yawspeed = _rates(2); + /* the complimentary filter should reflect the true system + * state, but we need smoother outputs for the control system + */ + att.rollspeed = _lp_roll_rate.apply(_rates(0)); + att.pitchspeed = _lp_pitch_rate.apply(_rates(1)); + att.yawspeed = _lp_yaw_rate.apply(_rates(2)); for (int i = 0; i < 3; i++) { att.g_comp[i] = _accel(i) - _pos_acc(i); @@ -443,6 +459,7 @@ void AttitudeEstimatorQ::task_main() if (_att_pub == nullptr) { _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); + } else { orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att); } @@ -472,11 +489,14 @@ void AttitudeEstimatorQ::task_main() } } -void AttitudeEstimatorQ::update_parameters(bool force) { +void AttitudeEstimatorQ::update_parameters(bool force) +{ bool updated = force; + if (!updated) { orb_check(_params_sub, &updated); } + if (updated) { parameter_update_s param_update; orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update); @@ -498,7 +518,8 @@ void AttitudeEstimatorQ::update_parameters(bool force) { } } -bool AttitudeEstimatorQ::init() { +bool AttitudeEstimatorQ::init() +{ // Rotation matrix can be easily constructed from acceleration and mag field vectors // 'k' is Earth Z axis (Down) unit vector in body frame Vector<3> k = -_accel; @@ -522,9 +543,10 @@ bool AttitudeEstimatorQ::init() { _q.normalize(); if (PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) && - PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)) && - _q.length() > 0.95f && _q.length() < 1.05f) { + PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)) && + _q.length() > 0.95f && _q.length() < 1.05f) { _inited = true; + } else { _inited = false; } @@ -532,7 +554,8 @@ bool AttitudeEstimatorQ::init() { return _inited; } -bool AttitudeEstimatorQ::update(float dt) { +bool AttitudeEstimatorQ::update(float dt) +{ if (!_inited) { if (!_data_good) { @@ -559,18 +582,20 @@ bool AttitudeEstimatorQ::update(float dt) { // Vector<3> k = _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, 1.0f)); // Optimized version with dropped zeros Vector<3> k( - 2.0f * (_q(1) * _q(3) - _q(0) * _q(2)), - 2.0f * (_q(2) * _q(3) + _q(0) * _q(1)), - (_q(0) * _q(0) - _q(1) * _q(1) - _q(2) * _q(2) + _q(3) * _q(3)) + 2.0f * (_q(1) * _q(3) - _q(0) * _q(2)), + 2.0f * (_q(2) * _q(3) + _q(0) * _q(1)), + (_q(0) * _q(0) - _q(1) * _q(1) - _q(2) * _q(2) + _q(3) * _q(3)) ); corr += (k % (_accel - _pos_acc).normalized()) * _w_accel; // Gyro bias estimation _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); } + _rates = _gyro + _gyro_bias; // Feed forward gyro @@ -583,7 +608,7 @@ bool AttitudeEstimatorQ::update(float dt) { _q.normalize(); if (!(PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) && - PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)))) { + PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)))) { // Reset quaternion to last good state _q = q_last; _rates.zero(); @@ -594,7 +619,9 @@ bool AttitudeEstimatorQ::update(float dt) { return true; } -int attitude_estimator_q_main(int argc, char *argv[]) { + +int attitude_estimator_q_main(int argc, char *argv[]) +{ if (argc < 1) { warnx("usage: attitude_estimator_q {start|stop|status}"); return 1; 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 d72f62dc61..d3959db4cf 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c @@ -47,6 +47,7 @@ * @group Attitude Q estimator * @min 0 * @max 1 + * @decimal 2 */ PARAM_DEFINE_FLOAT(ATT_W_ACC, 0.2f); @@ -56,6 +57,7 @@ PARAM_DEFINE_FLOAT(ATT_W_ACC, 0.2f); * @group Attitude Q estimator * @min 0 * @max 1 + * @decimal 2 */ PARAM_DEFINE_FLOAT(ATT_W_MAG, 0.1f); @@ -65,6 +67,7 @@ PARAM_DEFINE_FLOAT(ATT_W_MAG, 0.1f); * @group Attitude Q estimator * @min 0 * @max 1 + * @decimal 2 */ PARAM_DEFINE_FLOAT(ATT_W_GYRO_BIAS, 0.1f); @@ -77,6 +80,7 @@ PARAM_DEFINE_FLOAT(ATT_W_GYRO_BIAS, 0.1f); * * @group Attitude Q estimator * @unit degrees + * @decimal 2 */ PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f); @@ -106,6 +110,7 @@ PARAM_DEFINE_INT32(ATT_ACC_COMP, 2); * @min 0 * @max 2 * @unit rad/s + * @decimal 3 */ PARAM_DEFINE_FLOAT(ATT_BIAS_MAX, 0.05f); @@ -113,7 +118,8 @@ PARAM_DEFINE_FLOAT(ATT_BIAS_MAX, 0.05f); * Threshold (of RMS) to warn about high vibration levels * * @group Attitude Q estimator - * @min 0.001 - * @max 100 + * @min 0.01 + * @max 10 + * @decimal 2 */ PARAM_DEFINE_FLOAT(ATT_VIBE_THRESH, 0.2f); diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 0ffaea5bec..f411e1c128 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -387,7 +387,7 @@ float battery_remaining_estimate_voltage(float voltage, float discharged, float // XXX this time constant needs to become tunable // but really, the right fix are smart batteries. float val = throttle_lowpassed * 0.97f + throttle_normalized * 0.03f; - if (isfinite(val)) { + if (PX4_ISFINITE(val)) { throttle_lowpassed = val; } diff --git a/src/modules/controllib/block/Block.hpp b/src/modules/controllib/block/Block.hpp index bb2a0e2657..e74258b523 100644 --- a/src/modules/controllib/block/Block.hpp +++ b/src/modules/controllib/block/Block.hpp @@ -58,8 +58,8 @@ static const uint16_t maxPublicationsPerBlock = 100; static const uint8_t blockNameLengthMax = 80; // forward declaration -class SuperBlock; class BlockParamBase; +class SuperBlock; /** */ @@ -81,9 +81,9 @@ public: protected: // accessors SuperBlock *getParent() { return _parent; } - List & getSubscriptions() { return _subscriptions; } - List & getPublications() { return _publications; } - List & getParams() { return _params; } + List &getSubscriptions() { return _subscriptions; } + List &getPublications() { return _publications; } + List &getParams() { return _params; } // attributes const char *_name; SuperBlock *_parent; @@ -94,8 +94,8 @@ protected: private: /* this class has pointer data members and should not be copied (private constructor) */ - Block(const control::Block&); - Block operator=(const control::Block&); + Block(const control::Block &); + Block operator=(const control::Block &); }; class __EXPORT SuperBlock : @@ -106,28 +106,32 @@ public: // methods SuperBlock(SuperBlock *parent, const char *name) : Block(parent, name), - _children() { + _children() + { } virtual ~SuperBlock() {}; virtual void setDt(float dt); - virtual void updateParams() { + virtual void updateParams() + { Block::updateParams(); - if (getChildren().getHead() != NULL) updateChildParams(); + if (getChildren().getHead() != NULL) { updateChildParams(); } } - virtual void updateSubscriptions() { + virtual void updateSubscriptions() + { Block::updateSubscriptions(); - if (getChildren().getHead() != NULL) updateChildSubscriptions(); + if (getChildren().getHead() != NULL) { updateChildSubscriptions(); } } - virtual void updatePublications() { + virtual void updatePublications() + { Block::updatePublications(); - if (getChildren().getHead() != NULL) updateChildPublications(); + if (getChildren().getHead() != NULL) { updateChildPublications(); } } protected: // methods - List & getChildren() { return _children; } + List &getChildren() { return _children; } void updateChildParams(); void updateChildSubscriptions(); void updateChildPublications(); diff --git a/src/modules/controllib/block/BlockParam.cpp b/src/modules/controllib/block/BlockParam.cpp index 80f1bae1b0..b560d7999e 100644 --- a/src/modules/controllib/block/BlockParam.cpp +++ b/src/modules/controllib/block/BlockParam.cpp @@ -65,6 +65,7 @@ BlockParamBase::BlockParamBase(Block *parent, const char *name, bool parent_pref } else if (parent_prefix) { snprintf(fullname, blockNameLengthMax, "%s_%s", parentName, name); + } else { strncpy(fullname, name, blockNameLengthMax); } @@ -74,15 +75,17 @@ BlockParamBase::BlockParamBase(Block *parent, const char *name, bool parent_pref _handle = param_find(fullname); - if (_handle == PARAM_INVALID) + if (_handle == PARAM_INVALID) { printf("error finding param: %s\n", fullname); + } }; template BlockParam::BlockParam(Block *block, const char *name, - bool parent_prefix) : + bool parent_prefix) : BlockParamBase(block, name, parent_prefix), - _val() { + _val() +{ update(); } @@ -93,13 +96,15 @@ template void BlockParam::set(T val) { _val = val; } template -void BlockParam::update() { - if (_handle != PARAM_INVALID) param_get(_handle, &_val); +void BlockParam::update() +{ + if (_handle != PARAM_INVALID) { param_get(_handle, &_val); } } template -void BlockParam::commit() { - if (_handle != PARAM_INVALID) param_set(_handle, &_val); +void BlockParam::commit() +{ + if (_handle != PARAM_INVALID) { param_set(_handle, &_val); } } template diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp index 6a0b615910..3915ecc5e1 100644 --- a/src/modules/controllib/blocks.cpp +++ b/src/modules/controllib/blocks.cpp @@ -123,9 +123,10 @@ int blockLimitSymTest() float BlockLowPass::update(float input) { - if (!isfinite(getState())) { + if (!PX4_ISFINITE(getState())) { setState(input); } + float b = 2 * float(M_PI) * getFCut() * getDt(); float a = b / (1 + b); setState(a * input + (1 - a)*getState()); @@ -202,13 +203,14 @@ int blockHighPassTest() float BlockLowPass2::update(float input) { - if (!isfinite(getState())) { + if (!PX4_ISFINITE(getState())) { setState(input); } if (fabsf(_lp.get_cutoff_freq() - getFCutParam()) > FLT_EPSILON) { _lp.set_cutoff_frequency(_fs, getFCutParam()); } + _state = _lp.apply(input); return _state; } @@ -340,8 +342,10 @@ int blockIntegralTrapTest() float BlockDerivative::update(float input) { float output; + if (_initialized) { output = _lowPass.update((input - getU()) / getDt()); + } else { // if this is the first call to update // we have no valid derivative @@ -351,6 +355,7 @@ float BlockDerivative::update(float input) output = 0.0f; _initialized = true; } + setU(input); return output; } @@ -534,6 +539,7 @@ int blockRandGaussTest() } float stdDev = sqrt(sum / (n - 1)); + (void)(stdDev); ASSERT(equal(mean, blockRandGauss.getMean(), 1e-1)); ASSERT(equal(stdDev, blockRandGauss.getStdDev(), 1e-1)); printf("PASS\n"); diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp index 786bfc06d3..2b571de590 100644 --- a/src/modules/controllib/blocks.hpp +++ b/src/modules/controllib/blocks.hpp @@ -317,7 +317,8 @@ public: _kP(this, "") // only one param, no need to name {}; virtual ~BlockP() {}; - float update(float input) { + float update(float input) + { return getKP() * input; } // accessors @@ -343,7 +344,8 @@ public: _kI(this, "I") {}; virtual ~BlockPI() {}; - float update(float input) { + float update(float input) + { return getKP() * input + getKI() * getIntegral().update(input); } @@ -374,7 +376,8 @@ public: _kD(this, "D") {}; virtual ~BlockPD() {}; - float update(float input) { + float update(float input) + { return getKP() * input + getKD() * getDerivative().update(input); } @@ -407,7 +410,8 @@ public: _kD(this, "D") {}; virtual ~BlockPID() {}; - float update(float input) { + float update(float input) + { return getKP() * input + getKI() * getIntegral().update(input) + getKD() * getDerivative().update(input); @@ -440,11 +444,13 @@ public: SuperBlock(parent, name), _trim(this, "TRIM"), _limit(this, ""), - _val(0) { + _val(0) + { update(0); }; virtual ~BlockOutput() {}; - void update(float input) { + void update(float input) + { _val = _limit.update(input + getTrim()); } // accessors @@ -472,13 +478,15 @@ public: const char *name) : Block(parent, name), _min(this, "MIN"), - _max(this, "MAX") { + _max(this, "MAX") + { // seed should be initialized somewhere // in main program for all calls to rand // XXX currently in nuttx if you seed to 0, rand breaks }; virtual ~BlockRandUniform() {}; - float update() { + float update() + { static float rand_max = MAX_RAND; float rand_val = rand(); float bounds = getMax() - getMin(); @@ -503,13 +511,15 @@ public: const char *name) : Block(parent, name), _mean(this, "MEAN"), - _stdDev(this, "DEV") { + _stdDev(this, "DEV") + { // seed should be initialized somewhere // in main program for all calls to rand // XXX currently in nuttx if you seed to 0, rand breaks }; virtual ~BlockRandGauss() {}; - float update() { + float update() + { static float V1, V2, S; static int phase = 0; float X; diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp index 4f651777ba..3a99617e4d 100644 --- a/src/modules/controllib/uorb/blocks.cpp +++ b/src/modules/controllib/uorb/blocks.cpp @@ -53,10 +53,11 @@ BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *nam BlockWaypointGuidance::~BlockWaypointGuidance() {}; -void BlockWaypointGuidance::update(vehicle_global_position_s &pos, - vehicle_attitude_s &att, - position_setpoint_s &missionCmd, - position_setpoint_s &lastMissionCmd) +void BlockWaypointGuidance::update( + const vehicle_global_position_s &pos, + const vehicle_attitude_s &att, + const position_setpoint_s &missionCmd, + const position_setpoint_s &lastMissionCmd) { // heading to waypoint @@ -83,16 +84,16 @@ void BlockWaypointGuidance::update(vehicle_global_position_s &pos, BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) : SuperBlock(parent, name), // subscriptions - _att(ORB_ID(vehicle_attitude), 20, &getSubscriptions()), - _attCmd(ORB_ID(vehicle_attitude_setpoint), 20, &getSubscriptions()), - _ratesCmd(ORB_ID(vehicle_rates_setpoint), 20, &getSubscriptions()), - _pos(ORB_ID(vehicle_global_position), 20, &getSubscriptions()), - _missionCmd(ORB_ID(position_setpoint_triplet), 20, &getSubscriptions()), - _manual(ORB_ID(manual_control_setpoint), 20, &getSubscriptions()), - _status(ORB_ID(vehicle_status), 20, &getSubscriptions()), - _param_update(ORB_ID(parameter_update), 1000, &getSubscriptions()), // limit to 1 Hz + _att(ORB_ID(vehicle_attitude), 20, 0, &getSubscriptions()), + _attCmd(ORB_ID(vehicle_attitude_setpoint), 20, 0, &getSubscriptions()), + _ratesCmd(ORB_ID(vehicle_rates_setpoint), 20, 0, &getSubscriptions()), + _pos(ORB_ID(vehicle_global_position), 20, 0, &getSubscriptions()), + _missionCmd(ORB_ID(position_setpoint_triplet), 20, 0, &getSubscriptions()), + _manual(ORB_ID(manual_control_setpoint), 20, 0, &getSubscriptions()), + _status(ORB_ID(vehicle_status), 20, 0, &getSubscriptions()), + _param_update(ORB_ID(parameter_update), 1000, 0, &getSubscriptions()), // limit to 1 Hz // publications - _actuators(ORB_ID(actuator_controls_0), &getPublications()) + _actuators(ORB_ID(actuator_controls_0), -1, &getPublications()) { } diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp index e3c0811116..7766b67f6b 100644 --- a/src/modules/controllib/uorb/blocks.hpp +++ b/src/modules/controllib/uorb/blocks.hpp @@ -80,10 +80,10 @@ private: public: BlockWaypointGuidance(SuperBlock *parent, const char *name); virtual ~BlockWaypointGuidance(); - void update(vehicle_global_position_s &pos, - vehicle_attitude_s &att, - position_setpoint_s &missionCmd, - position_setpoint_s &lastMissionCmd); + void update(const vehicle_global_position_s &pos, + const vehicle_attitude_s &att, + const position_setpoint_s &missionCmd, + const position_setpoint_s &lastMissionCmd); float getPsiCmd() { return _psiCmd; } }; diff --git a/src/modules/ekf_att_pos_estimator/CMakeLists.txt b/src/modules/ekf_att_pos_estimator/CMakeLists.txt index fc7fefd2bf..13eaeb6d2b 100644 --- a/src/modules/ekf_att_pos_estimator/CMakeLists.txt +++ b/src/modules/ekf_att_pos_estimator/CMakeLists.txt @@ -32,7 +32,7 @@ ############################################################################ set(MODULE_CFLAGS ) -if(NOT ${OS} STREQUAL "qurt") +if(${OS} STREQUAL "nuttx") list(APPEND MODULE_CFLAGS -Wframe-larger-than=3400) endif() 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 4edcd565bf..006b272841 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 @@ -1194,7 +1194,7 @@ int AttitudePositionEstimatorEKF::start() _estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 20, - 3900, + 4600, (px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline, nullptr); @@ -1317,9 +1317,9 @@ void AttitudePositionEstimatorEKF::pollData() _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 { - _ekf->dAngIMU.x = 0.5f * (_ekf->angRate.x + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0]); - _ekf->dAngIMU.y = 0.5f * (_ekf->angRate.y + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 1]); - _ekf->dAngIMU.z = 0.5f * (_ekf->angRate.z + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2]); + _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]; @@ -1337,9 +1337,9 @@ void AttitudePositionEstimatorEKF::pollData() _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->dVelIMU.y = 0.5f * (_ekf->accel.y + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1]); - _ekf->dVelIMU.z = 0.5f * (_ekf->accel.z + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]); + _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]; diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index 839918728c..9e6afb3caf 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -43,7 +43,6 @@ #include #include #include -#include #ifndef M_PI_F #define M_PI_F static_cast(M_PI) @@ -1853,7 +1852,7 @@ void AttPosEKF::FuseOptFlow() Vector3f relVelSensor; // Perform sequential fusion of optical flow measurements only with valid tilt and height - flowStates[1] = std::max(flowStates[1], statesAtFlowTime[9] + minFlowRng); + flowStates[1] = fmax(flowStates[1], statesAtFlowTime[9] + minFlowRng); float heightAboveGndEst = flowStates[1] - statesAtFlowTime[9]; bool validTilt = Tnb.z.z > 0.71f; if (validTilt) @@ -2113,7 +2112,7 @@ void AttPosEKF::OpticalFlowEKF() } else { return; } - distanceTravelledSq = std::min(distanceTravelledSq, 100.0f); + distanceTravelledSq = fmin(distanceTravelledSq, 100.0f); Popt[1][1] += (distanceTravelledSq * sq(gndHgtSigma)); } @@ -2153,7 +2152,7 @@ void AttPosEKF::OpticalFlowEKF() varInnovRng = 1.0f/SK_RNG[1]; // constrain terrain height to be below the vehicle - flowStates[1] = std::max(flowStates[1], statesAtRngTime[9] + minFlowRng); + flowStates[1] = fmax(flowStates[1], statesAtRngTime[9] + minFlowRng); // estimate range to centre of image range = (flowStates[1] - statesAtRngTime[9]) * SK_RNG[2]; @@ -2173,7 +2172,7 @@ void AttPosEKF::OpticalFlowEKF() } // constrain the states flowStates[0] = ConstrainFloat(flowStates[0], 0.1f, 10.0f); - flowStates[1] = std::max(flowStates[1], statesAtRngTime[9] + minFlowRng); + flowStates[1] = fmax(flowStates[1], statesAtRngTime[9] + minFlowRng); // correct the covariance matrix float nextPopt[2][2]; @@ -2182,8 +2181,8 @@ void AttPosEKF::OpticalFlowEKF() nextPopt[1][0] = -Popt[1][0]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f); nextPopt[1][1] = -Popt[1][1]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f); // prevent the state variances from becoming negative and maintain symmetry - Popt[0][0] = std::max(nextPopt[0][0],0.0f); - Popt[1][1] = std::max(nextPopt[1][1],0.0f); + Popt[0][0] = fmax(nextPopt[0][0],0.0f); + Popt[1][1] = fmax(nextPopt[1][1],0.0f); Popt[0][1] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]); Popt[1][0] = Popt[0][1]; } @@ -2222,7 +2221,7 @@ void AttPosEKF::OpticalFlowEKF() vel.z = statesAtFlowTime[6]; // constrain terrain height to be below the vehicle - flowStates[1] = std::max(flowStates[1], statesAtFlowTime[9] + minFlowRng); + flowStates[1] = fmax(flowStates[1], statesAtFlowTime[9] + minFlowRng); // estimate range to centre of image range = (flowStates[1] - statesAtFlowTime[9]) / Tnb_flow.z.z; @@ -2290,7 +2289,7 @@ void AttPosEKF::OpticalFlowEKF() } // constrain the states flowStates[0] = ConstrainFloat(flowStates[0], 0.1f, 10.0f); - flowStates[1] = std::max(flowStates[1], statesAtFlowTime[9] + minFlowRng); + flowStates[1] = fmax(flowStates[1], statesAtFlowTime[9] + minFlowRng); // correct the covariance matrix for (uint8_t i = 0; i < 2 ; i++) { @@ -2306,8 +2305,8 @@ void AttPosEKF::OpticalFlowEKF() } // prevent the state variances from becoming negative and maintain symmetry - Popt[0][0] = std::max(nextPopt[0][0],0.0f); - Popt[1][1] = std::max(nextPopt[1][1],0.0f); + Popt[0][0] = fmax(nextPopt[0][0],0.0f); + Popt[1][1] = fmax(nextPopt[1][1],0.0f); Popt[0][1] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]); Popt[1][0] = Popt[0][1]; } diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index 7984bb12af..39b624f24a 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -142,7 +142,7 @@ void BlockMultiModeBacksideAutopilot::update() // store old position command before update if new command sent if (_missionCmd.updated()) { - _lastMissionCmd = _missionCmd.getData(); + _lastMissionCmd = _missionCmd.get(); } // check for new updates @@ -152,15 +152,17 @@ void BlockMultiModeBacksideAutopilot::update() updateSubscriptions(); // default all output to zero unless handled by mode - for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++) { - _actuators.control[i] = 0.0f; + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROLS; i++) { + _actuators.get().control[i] = 0.0f; } // only update guidance in auto mode - if (_status.main_state == MAIN_STATE_AUTO) { + if (_status.get().main_state == vehicle_status_s::MAIN_STATE_AUTO_MISSION) { // TODO use vehicle_control_mode here? // update guidance - _guide.update(_pos, _att, _missionCmd.current, _lastMissionCmd.current); + _guide.update(_pos.get(), _att.get(), + _missionCmd.get().current, + _lastMissionCmd.current); } // XXX handle STABILIZED (loiter on spot) as well @@ -168,32 +170,32 @@ void BlockMultiModeBacksideAutopilot::update() // the setpoint should update to loitering around this position // handle autopilot modes - if (_status.main_state == MAIN_STATE_AUTO) { + if (_status.get().main_state == vehicle_status_s::MAIN_STATE_AUTO) { // calculate velocity, XXX should be airspeed, // but using ground speed for now for the purpose // of control we will limit the velocity feedback between // the min/max velocity float v = _vLimit.update(sqrtf( - _pos.vel_n * _pos.vel_n + - _pos.vel_e * _pos.vel_e + - _pos.vel_d * _pos.vel_d)); + _pos.get().vel_n * _pos.get().vel_n + + _pos.get().vel_e * _pos.get().vel_e + + _pos.get().vel_d * _pos.get().vel_d)); // limit velocity command between min/max velocity float vCmd = _vLimit.update(_vCmd.get()); // altitude hold - float dThrottle = _h2Thr.update(_missionCmd.current.alt - _pos.alt); + float dThrottle = _h2Thr.update(_missionCmd.get().current.alt - _pos.get().alt); // heading hold - float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw); + float psiError = _wrap_pi(_guide.getPsiCmd() - _att.get().yaw); float phiCmd = _phiLimit.update(_psi2Phi.update(psiError)); - float pCmd = _phi2P.update(phiCmd - _att.roll); + float pCmd = _phi2P.update(phiCmd - _att.get().roll); // velocity hold // negative sign because nose over to increase speed float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v)); - float qCmd = _theta2Q.update(thetaCmd - _att.pitch); + float qCmd = _theta2Q.update(thetaCmd - _att.get().pitch); // yaw rate cmd float rCmd = 0; @@ -203,14 +205,16 @@ void BlockMultiModeBacksideAutopilot::update() float outputScale = velocityRatio * velocityRatio; // this term scales the output based on the dynamic pressure change from trim _stabilization.update(pCmd, qCmd, rCmd, - _att.rollspeed, _att.pitchspeed, _att.yawspeed, + _att.get().rollspeed, + _att.get().pitchspeed, + _att.get().yawspeed, outputScale); // output - _actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get(); - _actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get(); - _actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get(); - _actuators.control[CH_THR] = dThrottle + _trimThr.get(); + _actuators.get().control[CH_AIL] = _stabilization.getAileron() + _trimAil.get(); + _actuators.get().control[CH_ELV] = _stabilization.getElevator() + _trimElv.get(); + _actuators.get().control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get(); + _actuators.get().control[CH_THR] = dThrottle + _trimThr.get(); // XXX limit throttle to manual setting (safety) for now. // If it turns out to be confusing, it can be removed later once @@ -218,28 +222,29 @@ void BlockMultiModeBacksideAutopilot::update() // This is not a hack, but a design choice. // do not limit in HIL - if (_status.hil_state != HIL_STATE_ON) { + if (_status.get().hil_state != _vehicle_status::HIL_STATE_ON) { /* limit to value of manual throttle */ - _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? - _actuators.control[CH_THR] : _manual.throttle; + _actuators.get().control[CH_THR] = + (_actuators.get().control[CH_THR] < _manual.get().z) ? + _actuators.get().control[CH_THR] : _manual.get().z; } - } else if (_status.main_state == MAIN_STATE_MANUAL) { - _actuators.control[CH_AIL] = _manual.roll; - _actuators.control[CH_ELV] = _manual.pitch; - _actuators.control[CH_RDR] = _manual.yaw; - _actuators.control[CH_THR] = _manual.throttle; + } else if (_status.get().main_state == vehicle_status_s::MAIN_STATE_MANUAL) { + _actuators.get().control[CH_AIL] = _manual.get().y; + _actuators.get().control[CH_ELV] = _manual.get().x; + _actuators.get().control[CH_RDR] = _manual.get().r; + _actuators.get().control[CH_THR] = _manual.get().z; - } else if (_status.main_state == MAIN_STATE_ALTCTL || - _status.main_state == MAIN_STATE_POSCTL /* TODO, implement pos control */) { + } else if (_status.get().main_state == vehicle_status_s::MAIN_STATE_ALTCTL || + _status.get().main_state == vehicle_status_s::MAIN_STATE_POSCTL /* TODO, implement pos control */) { // calculate velocity, XXX should be airspeed, but using ground speed for now // for the purpose of control we will limit the velocity feedback between // the min/max velocity float v = _vLimit.update(sqrtf( - _pos.vel_n * _pos.vel_n + - _pos.vel_e * _pos.vel_e + - _pos.vel_d * _pos.vel_d)); + _pos.get().vel_n * _pos.get().vel_n + + _pos.get().vel_e * _pos.get().vel_e + + _pos.get().vel_d * _pos.get().vel_d)); // pitch channel -> rate of climb // TODO, might want to put a gain on this, otherwise commanding @@ -248,33 +253,35 @@ void BlockMultiModeBacksideAutopilot::update() //_crMax.get()*_manual.pitch - _pos.vz); // roll channel -> bank angle - float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax()); - float pCmd = _phi2P.update(phiCmd - _att.roll); + float phiCmd = _phiLimit.update(_manual.get().y * _phiLimit.getMax()); + float pCmd = _phi2P.update(phiCmd - _att.get().roll); // throttle channel -> velocity // negative sign because nose over to increase speed - float vCmd = _vLimit.update(_manual.throttle * + float vCmd = _vLimit.update(_manual.get().z * (_vLimit.getMax() - _vLimit.getMin()) + _vLimit.getMin()); float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v)); - float qCmd = _theta2Q.update(thetaCmd - _att.pitch); + float qCmd = _theta2Q.update(thetaCmd - _att.get().pitch); // yaw rate cmd float rCmd = 0; // stabilization _stabilization.update(pCmd, qCmd, rCmd, - _att.rollspeed, _att.pitchspeed, _att.yawspeed); + _att.get().rollspeed, + _att.get().pitchspeed, + _att.get().yawspeed); // output - _actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get(); - _actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get(); - _actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get(); + _actuators.get().control[CH_AIL] = _stabilization.getAileron() + _trimAil.get(); + _actuators.get().control[CH_ELV] = _stabilization.getElevator() + _trimElv.get(); + _actuators.get().control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get(); // currently using manual throttle // XXX if you enable this watch out, vz might be very noisy //_actuators.control[CH_THR] = dThrottle + _trimThr.get(); - _actuators.control[CH_THR] = _manual.throttle; + _actuators.get().control[CH_THR] = _manual.get().z; // XXX limit throttle to manual setting (safety) for now. // If it turns out to be confusing, it can be removed later once @@ -282,10 +289,11 @@ void BlockMultiModeBacksideAutopilot::update() // This is not a hack, but a design choice. /* do not limit in HIL */ - if (_status.hil_state != HIL_STATE_ON) { + if (_status.get().hil_state != vehicle_status_s::HIL_STATE_ON) { /* limit to value of manual throttle */ - _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? - _actuators.control[CH_THR] : _manual.throttle; + _actuators.get().control[CH_THR] = + (_actuators.get().control[CH_THR] < _manual.get().z) ? + _actuators.get().control[CH_THR] : _manual.get().z; } // body rates controller, disabled for now @@ -294,13 +302,13 @@ void BlockMultiModeBacksideAutopilot::update() } else if ( 0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) { // TODO use vehicle_control_mode here? - _stabilization.update(_manual.roll, _manual.pitch, _manual.yaw, - _att.rollspeed, _att.pitchspeed, _att.yawspeed); + _stabilization.update(_manual.get().y, _manual.get().x, _manual.get().r, + _att.get().rollspeed, _att.get().pitchspeed, _att.get().yawspeed); - _actuators.control[CH_AIL] = _stabilization.getAileron(); - _actuators.control[CH_ELV] = _stabilization.getElevator(); - _actuators.control[CH_RDR] = _stabilization.getRudder(); - _actuators.control[CH_THR] = _manual.throttle; + _actuators.get().control[CH_AIL] = _stabilization.getAileron(); + _actuators.get().control[CH_ELV] = _stabilization.getElevator(); + _actuators.get().control[CH_RDR] = _stabilization.getRudder(); + _actuators.get().control[CH_THR] = _manual.get().z; } // update all publications @@ -311,8 +319,8 @@ BlockMultiModeBacksideAutopilot::~BlockMultiModeBacksideAutopilot() { // send one last publication when destroyed, setting // all output to zero - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { - _actuators.control[i] = 0.0f; + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROLS; i++) { + _actuators.get().control[i] = 0.0f; } updatePublications(); 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 cfd1988f44..5870039c24 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 @@ -179,7 +179,7 @@ PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); * @max 150.0 * @group L1 Control */ -PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 25.0f); +PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 10.0f); /** * Maximum climb rate diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 49e1d735e0..7a045fb1bb 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -53,7 +53,7 @@ mTecs::mTecs() : _airspeedMin(this, "FW_AIRSPD_MIN", false), /* Publications */ #if defined(__PX4_NUTTX) - _status(ORB_ID(tecs_status), &getPublications()), + _status(ORB_ID(tecs_status), -1, &getPublications()), #endif // defined(__PX4_NUTTX) /* control blocks */ _controlTotalEnergy(this, "THR"), @@ -88,8 +88,8 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti float airspeedSp, unsigned mode, LimitOverride limitOverride) { /* check if all input arguments are numbers and abort if not so */ - if (!isfinite(flightPathAngle) || !isfinite(altitude) || - !isfinite(altitudeSp) || !isfinite(airspeed) || !isfinite(airspeedSp) || !isfinite(mode)) { + if (!PX4_ISFINITE(flightPathAngle) || !PX4_ISFINITE(altitude) || + !PX4_ISFINITE(altitudeSp) || !PX4_ISFINITE(airspeed) || !PX4_ISFINITE(airspeedSp) || !PX4_ISFINITE(mode)) { return -1; } @@ -111,8 +111,8 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti #if defined(__PX4_NUTTX) /* Write part of the status message */ - _status.altitudeSp = altitudeSp; - _status.altitude_filtered = altitudeFiltered; + _status.get().altitudeSp = altitudeSp; + _status.get().altitude_filtered = altitudeFiltered; #endif // defined(__PX4_NUTTX) @@ -125,8 +125,8 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng float airspeedSp, unsigned mode, LimitOverride limitOverride) { /* check if all input arguments are numbers and abort if not so */ - if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) || - !isfinite(airspeed) || !isfinite(airspeedSp) || !isfinite(mode)) { + if (!PX4_ISFINITE(flightPathAngle) || !PX4_ISFINITE(flightPathAngleSp) || + !PX4_ISFINITE(airspeed) || !PX4_ISFINITE(airspeedSp) || !PX4_ISFINITE(mode)) { return -1; } @@ -149,8 +149,8 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng #if defined(__PX4_NUTTX) /* Write part of the status message */ - _status.airspeedSp = airspeedSp; - _status.airspeed_filtered = airspeedFiltered; + _status.get().airspeedSp = airspeedSp; + _status.get().airspeed_filtered = airspeedFiltered; #endif // defined(__PX4_NUTTX) @@ -163,8 +163,8 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight float accelerationLongitudinalSp, unsigned mode, LimitOverride limitOverride) { /* check if all input arguments are numbers and abort if not so */ - if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) || - !isfinite(airspeedFiltered) || !isfinite(accelerationLongitudinalSp) || !isfinite(mode)) { + if (!PX4_ISFINITE(flightPathAngle) || !PX4_ISFINITE(flightPathAngleSp) || + !PX4_ISFINITE(airspeedFiltered) || !PX4_ISFINITE(accelerationLongitudinalSp) || !PX4_ISFINITE(mode)) { return -1; } /* time measurement */ diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 7cfe65bb7f..f8066241c5 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -49,18 +49,18 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), _paramHandle(), _params(), _vehicleLocalPositionSub(-1), - _vehicleLocalPosition({}), - _airspeedSub(-1), - _vehicleStatusSub(-1), - _armingSub(-1), - _airspeed{}, - _vehicleStatus{}, - _arming{}, - _parameterSub(-1), - _velocity_xy_filtered(0.0f), - _velocity_z_filtered(0.0f), - _airspeed_filtered(0.0f), - _landDetectTrigger(0) + _vehicleLocalPosition( {}), + _airspeedSub(-1), + _vehicleStatusSub(-1), + _armingSub(-1), + _airspeed{}, + _vehicleStatus{}, + _arming{}, + _parameterSub(-1), + _velocity_xy_filtered(0.0f), + _velocity_z_filtered(0.0f), + _airspeed_filtered(0.0f), + _landDetectTrigger(0) { _paramHandle.maxVelocity = param_find("LNDFW_VEL_XY_MAX"); _paramHandle.maxClimbRate = param_find("LNDFW_VEL_Z_MAX"); @@ -101,10 +101,12 @@ bool FixedwingLandDetector::update() if (hrt_elapsed_time(&_vehicleLocalPosition.timestamp) < 500 * 1000) { float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicleLocalPosition.vx * - _vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy); + _vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy); + if (PX4_ISFINITE(val)) { _velocity_xy_filtered = val; } + val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_vehicleLocalPosition.vz); if (PX4_ISFINITE(val)) { diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index ae6a1b5037..3040e23407 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -47,12 +47,11 @@ LandDetector::LandDetector() : _landDetectedPub(0), - _landDetected({0, false}), - _arming_time(0), - _taskShouldExit(false), - _taskIsRunning(false), - _work{} -{ + _landDetected( {0, false}), + _arming_time(0), + _taskShouldExit(false), + _taskIsRunning(false), +_work{} { // ctor } @@ -111,7 +110,8 @@ void LandDetector::cycle() } if (!_taskShouldExit) { - work_queue(LPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, USEC2TICK(1000000 / LAND_DETECTOR_UPDATE_RATE)); + work_queue(LPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, + USEC2TICK(1000000 / LAND_DETECTOR_UPDATE_RATE)); } } diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index d28863b499..b86e431150 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -99,7 +99,8 @@ protected: 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 = 1000000; /**< time interval in which wider acceptance thresholds are used after arming */ + static constexpr uint64_t LAND_DETECTOR_ARM_PHASE_TIME = + 1000000; /**< time interval in which wider acceptance thresholds are used after arming */ protected: orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 1d0a6b92dc..fa636e496c 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -99,13 +99,14 @@ bool MulticopterLandDetector::update() if (!_arming.armed) { _arming_time = 0; return true; + } else if (_arming_time == 0) { _arming_time = hrt_absolute_time(); } // return status based on armed state if no position lock is available if (_vehicleGlobalPosition.timestamp == 0 || - hrt_elapsed_time(&_vehicleGlobalPosition.timestamp) > 500000) { + hrt_elapsed_time(&_vehicleGlobalPosition.timestamp) > 500000) { // no position lock - not landed if armed return !_arming.armed; @@ -129,14 +130,14 @@ bool MulticopterLandDetector::update() // check if we are moving horizontally bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n + _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > _params.maxVelocity - && _vehicleStatus.condition_global_position_valid; + && _vehicleStatus.condition_global_position_valid; // next look if all rotation angles are not moving float maxRotationScaled = _params.maxRotation * armThresholdFactor; bool rotating = (fabsf(_vehicleAttitude.rollspeed) > maxRotationScaled) || - (fabsf(_vehicleAttitude.pitchspeed) > maxRotationScaled) || - (fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled); + (fabsf(_vehicleAttitude.pitchspeed) > maxRotationScaled) || + (fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled); // check if thrust output is minimal (about half of default) bool minimalThrust = _actuators.control[3] <= _params.maxThrottle; diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index c4d0e5194d..0aa6e1fad2 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -152,6 +152,7 @@ static int land_detector_start(const char *mode) return 1; } } + printf("\n"); } @@ -176,6 +177,7 @@ int land_detector_main(int argc, char *argv[]) warnx("land_detector start failed"); return 1; } + return 0; } diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp new file mode 100644 index 0000000000..f3295e1fc9 --- /dev/null +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -0,0 +1,1378 @@ +#include "BlockLocalPositionEstimator.hpp" +#include +#include +#include + +static const int MIN_FLOW_QUALITY = 100; +static const int REQ_INIT_COUNT = 100; + +static const uint32_t VISION_POSITION_TIMEOUT = 500000; +static const uint32_t MOCAP_TIMEOUT = 200000; + +static const uint32_t XY_SRC_TIMEOUT = 2000000; + +using namespace std; + +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_distance(ORB_ID(distance_sensor), + 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()), + + // publications + _pub_lpos(ORB_ID(vehicle_local_position), -1, &getPublications()), + _pub_gpos(ORB_ID(vehicle_global_position), -1, &getPublications()), + _pub_filtered_flow(ORB_ID(filtered_bottom_flow), -1, &getPublications()), + _pub_est_status(ORB_ID(estimator_status), -1, &getPublications()), + + // map projection + _map_ref(), + + // block parameters + _integrate(this, "INTEGRATE"), + _flow_xy_stddev(this, "FLW_XY"), + _sonar_z_stddev(this, "SNR_Z"), + _lidar_z_stddev(this, "LDR_Z"), + _accel_xy_stddev(this, "ACC_XY"), + _accel_z_stddev(this, "ACC_Z"), + _baro_stddev(this, "BAR_Z"), + _gps_xy_stddev(this, "GPS_XY"), + _gps_z_stddev(this, "GPS_Z"), + _gps_vxy_stddev(this, "GPS_VXY"), + _gps_vz_stddev(this, "GPS_VZ"), + _gps_eph_max(this, "EPH_MAX"), + _vision_xy_stddev(this, "VIS_XY"), + _vision_z_stddev(this, "VIS_Z"), + _no_vision(this, "NO_VISION"), + _beta_max(this, "BETA_MAX"), + _mocap_p_stddev(this, "VIC_P"), + _pn_p_noise_power(this, "PN_P"), + _pn_v_noise_power(this, "PN_V"), + _pn_b_noise_power(this, "PN_B"), + + // misc + _polls(), + _timeStamp(hrt_absolute_time()), + _time_last_xy(0), + _time_last_flow(0), + _time_last_baro(0), + _time_last_gps(0), + _time_last_lidar(0), + _time_last_sonar(0), + _time_last_vision_p(0), + _time_last_mocap(0), + + // mavlink log + _mavlink_fd(open(MAVLINK_LOG_DEVICE, 0)), + + // initialization flags + _baroInitialized(false), + _gpsInitialized(false), + _lidarInitialized(false), + _sonarInitialized(false), + _flowInitialized(false), + _visionInitialized(false), + _mocapInitialized(false), + + // init counts + _baroInitCount(0), + _gpsInitCount(0), + _lidarInitCount(0), + _sonarInitCount(0), + _flowInitCount(0), + _visionInitCount(0), + _mocapInitCount(0), + + // reference altitudes + _altHome(0), + _altHomeInitialized(false), + _baroAltHome(0), + _gpsAltHome(0), + _lidarAltHome(0), + _sonarAltHome(0), + _visionHome(), + _mocapHome(), + + // flow integration + _flowX(0), + _flowY(0), + _flowMeanQual(0), + + // reference lat/lon + _gpsLatHome(0), + _gpsLonHome(0), + + // status + _canEstimateXY(false), + _canEstimateZ(false), + _xyTimeout(false), + + // faults + _baroFault(FAULT_NONE), + _gpsFault(FAULT_NONE), + _lidarFault(FAULT_NONE), + _flowFault(FAULT_NONE), + _sonarFault(FAULT_NONE), + _visionFault(FAULT_NONE), + _mocapFault(FAULT_NONE), + + //timeouts + _visionTimeout(true), + _mocapTimeout(true), + + // loop performance + _loop_perf(), + _interval_perf(), + _err_perf(), + + // kf matrices + _x(), _u(), _P() +{ + // setup event triggering based on new flow messages to integrate + _polls[POLL_FLOW].fd = _sub_flow.getHandle(); + _polls[POLL_FLOW].events = POLLIN; + + _polls[POLL_PARAM].fd = _sub_param_update.getHandle(); + _polls[POLL_PARAM].events = POLLIN; + + _polls[POLL_SENSORS].fd = _sub_sensor.getHandle(); + _polls[POLL_SENSORS].events = POLLIN; + + // initialize P to identity*0.1 + initP(); + + _x.setZero(); + _u.setZero(); + + // perf counters + _loop_perf = perf_alloc(PC_ELAPSED, + "local_position_estimator_runtime"); + //_interval_perf = perf_alloc(PC_INTERVAL, + //"local_position_estimator_interval"); + _err_perf = perf_alloc(PC_COUNT, "local_position_estimator_err"); + + // map + _map_ref.init_done = false; + + // intialize parameter dependent matrices + updateParams(); +} + +BlockLocalPositionEstimator::~BlockLocalPositionEstimator() +{ +} + +void BlockLocalPositionEstimator::update() +{ + + // wait for a sensor update, check for exit condition every 100 ms + int ret = poll(_polls, 3, 100); + + if (ret < 0) { + /* poll error, count it in perf */ + perf_count(_err_perf); + return; + } + + uint64_t newTimeStamp = hrt_absolute_time(); + float dt = (newTimeStamp - _timeStamp) / 1.0e6f; + _timeStamp = newTimeStamp; + + //printf("dt: %0.5g\n", double(dt)); + + // set dt for all child blocks + setDt(dt); + + // see which updates are available + bool flowUpdated = _sub_flow.updated(); + bool paramsUpdated = _sub_param_update.updated(); + bool baroUpdated = _sub_sensor.updated(); + bool lidarUpdated = false; + bool sonarUpdated = false; + + if (_sub_distance.updated()) { + if (_sub_distance.get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_LASER) { + lidarUpdated = true; + } + + if (_sub_distance.get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND) { + sonarUpdated = true; + } + + if (_sub_distance.get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_INFRARED) { + mavlink_log_info(_mavlink_fd, "[lpe] no support to short-range infrared sensors "); + warnx("[lpe] short-range infrared detected. Ignored... "); + } + } + + bool gpsUpdated = _sub_gps.updated(); + bool homeUpdated = _sub_home.updated(); + bool visionUpdated = _sub_vision_pos.updated(); + bool mocapUpdated = _sub_mocap.updated(); + + // get new data + updateSubscriptions(); + + // update parameters + if (paramsUpdated) { + updateParams(); + } + + // update home position projection + if (homeUpdated) { + updateHome(); + } + + // check for timeouts on external sources + if ((hrt_absolute_time() - _time_last_vision_p > VISION_POSITION_TIMEOUT) && _visionInitialized) { + if (!_visionTimeout) { + _visionTimeout = true; + mavlink_log_info(_mavlink_fd, "[lpe] vision position timeout "); + warnx("[lpe] vision position timeout "); + } + + } else { + _visionTimeout = false; + } + + if ((hrt_absolute_time() - _time_last_mocap > MOCAP_TIMEOUT) && _mocapInitialized) { + if (!_mocapTimeout) { + _mocapTimeout = true; + mavlink_log_info(_mavlink_fd, "[lpe] mocap timeout "); + warnx("[lpe] mocap timeout "); + } + + } else { + _mocapTimeout = false; + } + + // determine if we should start estimating + _canEstimateZ = _baroInitialized && !_baroFault; + _canEstimateXY = + (_gpsInitialized && !_gpsFault) || + (_flowInitialized && !_flowFault) || + (_visionInitialized && !_visionTimeout && !_visionFault) || + (_mocapInitialized && !_mocapTimeout && !_mocapFault); + + if (_canEstimateXY) { + _time_last_xy = hrt_absolute_time(); + } + + // if we have no lat, lon initialized projection at 0,0 + if (_canEstimateXY && !_map_ref.init_done) { + map_projection_init(&_map_ref, 0, 0); + } + + // reinitialize x if necessary + bool reinit_x = false; + + for (int i = 0; i < n_x; i++) { + // should we do a reinit + // of sensors here? + // don't want it to take too long + if (!isfinite(_x(i))) { + reinit_x = true; + break; + } + } + + if (reinit_x) { + for (int i = 0; i < n_x; i++) { + _x(i) = 0; + } + + mavlink_log_info(_mavlink_fd, "[lpe] reinit x"); + warnx("[lpe] reinit x"); + } + + // reinitialize P if necessary + bool reinit_P = false; + + for (int i = 0; i < n_x; i++) { + for (int j = 0; j < n_x; j++) { + if (!isfinite(_P(i, j))) { + reinit_P = true; + break; + } + } + + if (reinit_P) { break; } + } + + if (reinit_P) { + mavlink_log_info(_mavlink_fd, "[lpe] reinit P"); + warnx("[lpe] reinit P"); + initP(); + } + + // do prediction + predict(); + + // sensor corrections/ initializations + if (gpsUpdated) { + if (!_gpsInitialized) { + initGps(); + + } else { + correctGps(); + } + } + + if (baroUpdated) { + if (!_baroInitialized) { + initBaro(); + + } else { + correctBaro(); + } + } + + if (lidarUpdated) { + if (!_lidarInitialized) { + initLidar(); + + } else { + correctLidar(); + } + } + + if (sonarUpdated) { + if (!_sonarInitialized) { + initSonar(); + + } else { + correctSonar(); + } + } + + if (flowUpdated) { + if (!_flowInitialized) { + initFlow(); + + } else { + perf_begin(_loop_perf);// TODO + correctFlow(); + //perf_count(_interval_perf); + perf_end(_loop_perf); + } + } + + if (_no_vision.get() != CBRK_NO_VISION_KEY) { // check if no vision circuit breaker is set + if (visionUpdated) { + if (!_visionInitialized) { + initVision(); + + } else { + correctVision(); + } + } + } + + if (mocapUpdated) { + if (!_mocapInitialized) { + initmocap(); + + } else { + correctmocap(); + } + } + + _xyTimeout = (hrt_absolute_time() - _time_last_xy > XY_SRC_TIMEOUT); + + if (!_xyTimeout && _altHomeInitialized) { + // update all publications if possible + publishLocalPos(); + publishEstimatorStatus(); + publishGlobalPos(); + publishFilteredFlow(); + + } else if (_altHomeInitialized) { + // publish only Z estimate + publishLocalPos(); + publishEstimatorStatus(); + } + +} + +void BlockLocalPositionEstimator::updateHome() +{ + double lat = _sub_home.get().lat; + double lon = _sub_home.get().lon; + float alt = _sub_home.get().alt; + + mavlink_log_info(_mavlink_fd, "[lpe] home: lat %5.0f, lon %5.0f, alt %5.0f", lat, lon, double(alt)); + warnx("[lpe] home: lat %5.0f, lon %5.0f, alt %5.0f", 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; + _lidarAltHome += delta_alt; + _sonarAltHome += delta_alt; +} + +void BlockLocalPositionEstimator::initBaro() +{ + // collect baro data + if (!_baroInitialized && + (_sub_sensor.get().baro_timestamp[0] != _time_last_baro)) { + _time_last_baro = _sub_sensor.get().baro_timestamp[0]; + _baroAltHome += _sub_sensor.get().baro_alt_meter[0]; + + if (_baroInitCount++ > REQ_INIT_COUNT) { + _baroAltHome /= _baroInitCount; + mavlink_log_info(_mavlink_fd, + "[lpe] baro offs: %d m", (int)_baroAltHome); + warnx("[lpe] baro offs: %d m", (int)_baroAltHome); + _baroInitialized = true; + + if (!_altHomeInitialized) { + _altHomeInitialized = true; + _altHome = _baroAltHome; + } + } + } +} + + +void BlockLocalPositionEstimator::initGps() +{ + // collect gps data + if (!_gpsInitialized && _sub_gps.get().fix_type > 2) { + double lat = _sub_gps.get().lat * 1e-7; + double lon = _sub_gps.get().lon * 1e-7; + float alt = _sub_gps.get().alt * 1e-3f; + // increament sums for mean + _gpsLatHome += lat; + _gpsLonHome += lon; + _gpsAltHome += alt; + _time_last_gps = _sub_gps.get().timestamp_position; + + if (_gpsInitCount++ > REQ_INIT_COUNT) { + _gpsLatHome /= _gpsInitCount; + _gpsLonHome /= _gpsInitCount; + _gpsAltHome /= _gpsInitCount; + map_projection_init(&_map_ref, lat, lon); + mavlink_log_info(_mavlink_fd, "[lpe] gps init: " + "lat %d, lon %d, alt %d m", + int(_gpsLatHome), int(_gpsLonHome), int(_gpsAltHome)); + warnx("[lpe] gps init: lat %d, lon %d, alt %d m", + int(_gpsLatHome), int(_gpsLonHome), int(_gpsAltHome)); + _gpsInitialized = true; + + if (!_altHomeInitialized) { + _altHomeInitialized = true; + _altHome = _gpsAltHome; + } + } + } +} + +void BlockLocalPositionEstimator::initLidar() +{ + + if (_sub_distance.get().type != distance_sensor_s::MAV_DISTANCE_SENSOR_LASER) { return; } + + // collect lidar data + bool valid = false; + float d = _sub_distance.get().current_distance; + + if (d < _sub_distance.get().max_distance && + d > _sub_distance.get().min_distance) { + valid = true; + } + + if (!_lidarInitialized && valid) { + // increament sums for mean + _lidarAltHome += _sub_distance.get().current_distance; + + if (_lidarInitCount++ > REQ_INIT_COUNT) { + _lidarAltHome /= _lidarInitCount; + mavlink_log_info(_mavlink_fd, "[lpe] lidar init: " + "alt %d cm", + int(100 * _lidarAltHome)); + warnx("[lpe] lidar init: alt %d cm", + int(100 * _lidarAltHome)); + _lidarInitialized = true; + } + } +} + +void BlockLocalPositionEstimator::initSonar() +{ + + if (_sub_distance.get().type != distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND) { return; } + + // collect sonar data + bool valid = false; + float d = _sub_distance.get().current_distance; + + if (d < _sub_distance.get().max_distance && + d > _sub_distance.get().min_distance) { + valid = true; + } + + if (!_sonarInitialized && valid) { + // increament sums for mean + _sonarAltHome += _sub_distance.get().current_distance; + + if (_sonarInitCount++ > REQ_INIT_COUNT) { + _sonarAltHome /= _sonarInitCount; + mavlink_log_info(_mavlink_fd, "[lpe] sonar init: " + "alt %d cm", + int(100 * _sonarAltHome)); + warnx("[lpe] sonar init: alt %d cm", + int(100 * _sonarAltHome)); + _sonarInitialized = true; + } + } +} + +void BlockLocalPositionEstimator::initFlow() +{ + + // collect pixel flow data + if (!_flowInitialized) { + // increament sums for mean + _flowMeanQual += _sub_flow.get().quality; + + if (_flowInitCount++ > REQ_INIT_COUNT) { + _flowMeanQual /= _flowInitCount; + + if (_flowMeanQual < MIN_FLOW_QUALITY) { + // retry initialisation till we have better flow data + warnx("[lpe] flow quality bad, retrying init : %d", + int(_flowMeanQual)); + _flowMeanQual = 0; + _flowInitCount = 0; + return; + } + + mavlink_log_info(_mavlink_fd, "[lpe] flow init: " + "quality %d", + int(_flowMeanQual)); + warnx("[lpe] flow init: quality %d", + int(_flowMeanQual)); + _flowInitialized = true; + } + } +} + +void BlockLocalPositionEstimator::initVision() +{ + // collect vision position data + if (!_visionInitialized) { + // increament sums for mean + Vector3f pos; + pos(0) = _sub_vision_pos.get().x; + pos(1) = _sub_vision_pos.get().y; + pos(2) = _sub_vision_pos.get().z; + _visionHome += pos; + + if (_visionInitCount++ > REQ_INIT_COUNT) { + _visionHome /= _visionInitCount; + mavlink_log_info(_mavlink_fd, "[lpe] vision position init: " + "%f, %f, %f m", double(pos(0)), double(pos(1)), double(pos(2))); + warnx("[lpe] vision position init: " + "%f, %f, %f m", double(pos(0)), double(pos(1)), double(pos(2))); + _visionInitialized = true; + } + } +} + +void BlockLocalPositionEstimator::initmocap() +{ + // collect mocap data + if (!_mocapInitialized) { + // increament sums for mean + Vector3f pos; + pos(0) = _sub_mocap.get().x; + pos(1) = _sub_mocap.get().y; + pos(2) = _sub_mocap.get().z; + _mocapHome += pos; + + if (_mocapInitCount++ > REQ_INIT_COUNT) { + _mocapHome /= _mocapInitCount; + mavlink_log_info(_mavlink_fd, "[lpe] mocap init: " + "%f, %f, %f m", double(pos(0)), double(pos(1)), double(pos(2))); + warnx("[lpe] mocap init: " + "%f, %f, %f m", double(pos(0)), double(pos(1)), double(pos(2))); + _mocapInitialized = true; + } + } +} + +void BlockLocalPositionEstimator::publishLocalPos() +{ + // publish local position + if (isfinite(_x(X_x)) && isfinite(_x(X_y)) && isfinite(_x(X_z)) && + isfinite(_x(X_vx)) && isfinite(_x(X_vy)) + && 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().yaw = _sub_att.get().yaw; + _pub_lpos.get().xy_global = _sub_home.get().timestamp != 0; // need home for reference + _pub_lpos.get().z_global = _baroInitialized; + _pub_lpos.get().ref_timestamp = _sub_home.get().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; + // TODO, terrain alt + _pub_lpos.get().dist_bottom = -_x(X_z); + _pub_lpos.get().dist_bottom_rate = -_x(X_vz); + _pub_lpos.get().surface_bottom_timestamp = 0; + _pub_lpos.get().dist_bottom_valid = true; + _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(); + } +} + +void BlockLocalPositionEstimator::publishEstimatorStatus() +{ + if (isfinite(_x(X_x)) && isfinite(_x(X_y)) && isfinite(_x(X_z)) && + isfinite(_x(X_vx)) && isfinite(_x(X_vy)) + && isfinite(_x(X_vz))) { + _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 > 0) << SENSOR_BARO) + + ((_gpsFault > 0) << SENSOR_GPS) + + ((_lidarFault > 0) << SENSOR_LIDAR) + + ((_flowFault > 0) << SENSOR_FLOW) + + ((_sonarFault > 0) << SENSOR_SONAR) + + ((_visionFault > 0) << SENSOR_VISION) + + ((_mocapFault > 0) << SENSOR_MOCAP); + _pub_est_status.get().timeout_flags = + (_xyTimeout << 0) + + (_visionTimeout << 1) + + (_mocapTimeout << 2); + _pub_est_status.update(); + } +} + +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; + + if (isfinite(lat) && isfinite(lon) && isfinite(alt) && + isfinite(_x(X_vx)) && isfinite(_x(X_vy)) && + isfinite(_x(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().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 = 0; + _pub_gpos.get().terrain_alt_valid = false; + _pub_gpos.get().dead_reckoning = !_canEstimateXY && !_xyTimeout; + _pub_gpos.update(); + } +} + +void BlockLocalPositionEstimator::publishFilteredFlow() +{ + // publish filtered flow + if (isfinite(_pub_filtered_flow.get().sumx) && + isfinite(_pub_filtered_flow.get().sumy) && + isfinite(_pub_filtered_flow.get().vx) && + isfinite(_pub_filtered_flow.get().vy)) { + _pub_filtered_flow.update(); + } +} + +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_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; +} + +void BlockLocalPositionEstimator::predict() +{ + // if can't update anything, don't propagate + // state or covariance + if (!_canEstimateXY && !_canEstimateZ) { return; } + + if (_integrate.get() && _sub_att.get().R_valid) { + Matrix3f R_att(_sub_att.get().R); + Vector3f a(_sub_sensor.get().accelerometer_m_s2); + _u = R_att * a; + _u(U_az) += 9.81f; // add g + + } else { + _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(); + Q(X_x, X_x) = _pn_p_noise_power.get(); + Q(X_y, X_y) = _pn_p_noise_power.get(); + Q(X_z, X_z) = _pn_p_noise_power.get(); + Q(X_vx, X_vx) = _pn_v_noise_power.get(); + Q(X_vy, X_vy) = _pn_v_noise_power.get(); + Q(X_vz, X_vz) = _pn_v_noise_power.get(); + + // technically, the noise is in the body frame, + // but the components are all the same, so + // ignoring for now + Q(X_bx, X_bx) = _pn_b_noise_power.get(); + Q(X_by, X_by) = _pn_b_noise_power.get(); + Q(X_bz, X_bz) = _pn_b_noise_power.get(); + + // continuous time kalman filter prediction + Matrix 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; + } + + // propagate + _x += dx; + _P += (A * _P + _P * A.transpose() + + B * R * B.transpose() + Q) * getDt(); +} + +void BlockLocalPositionEstimator::correctFlow() +{ + + // flow measurement matrix and noise matrix + Matrix C; + C.setZero(); + C(Y_flow_x, X_x) = 1; + C(Y_flow_y, X_y) = 1; + + Matrix R; + R.setZero(); + R(Y_flow_x, Y_flow_x) = + _flow_xy_stddev.get() * _flow_xy_stddev.get(); + R(Y_flow_y, Y_flow_y) = + _flow_xy_stddev.get() * _flow_xy_stddev.get(); + + float flow_speed[3] = {0.0f, 0.0f, 0.0f}; + float global_speed[3] = {0.0f, 0.0f, 0.0f}; + + /* calc dt between flow timestamps */ + /* ignore first flow msg */ + if (_time_last_flow == 0) { + _time_last_flow = _sub_flow.get().timestamp; + return; + } + + float dt = (_sub_flow.get().timestamp - _time_last_flow) * 1.0e-6f ; + _time_last_flow = _sub_flow.get().timestamp; + + // calculate velocity over ground + if (_sub_flow.get().integration_timespan > 0) { + flow_speed[0] = (_sub_flow.get().pixel_flow_x_integral / + (_sub_flow.get().integration_timespan / 1e6f) - + _sub_att.get().pitchspeed) * // Body rotation correction TODO check this + _x(X_z); + flow_speed[1] = (_sub_flow.get().pixel_flow_y_integral / + (_sub_flow.get().integration_timespan / 1e6f) - + _sub_att.get().rollspeed) * // Body rotation correction + _x(X_z); + + } else { + flow_speed[0] = 0; + flow_speed[1] = 0; + } + + flow_speed[2] = 0.0f; + + /* update filtered flow */ + _pub_filtered_flow.get().sumx += flow_speed[0] * dt; + _pub_filtered_flow.get().sumy += flow_speed[1] * dt; + _pub_filtered_flow.get().vx = flow_speed[0]; + _pub_filtered_flow.get().vy = flow_speed[1]; + + // TODO add yaw rotation correction (with distance to vehicle zero) + + // convert to globalframe velocity + for (uint8_t i = 0; i < 3; i++) { + float sum = 0.0f; + + for (uint8_t j = 0; j < 3; j++) { + sum += flow_speed[j] * PX4_R(_sub_att.get().R, i, j); + } + + global_speed[i] = sum; + } + + // flow integral + _flowX += global_speed[0] * dt; + _flowY += global_speed[1] * dt; + + // measurement + Vector2f y; + y(0) = _flowX; + y(1) = _flowY; + + // residual + Vector2f r = y - C * _x; + + // residual covariance, (inverse) + Matrix S_I = + (C * _P * C.transpose() + R).inverse(); + + // fault detection + float beta = sqrtf((r.transpose() * (S_I * r))(0, 0)); + + if (_sub_flow.get().quality < MIN_FLOW_QUALITY) { + if (!_flowFault) { + mavlink_log_info(_mavlink_fd, "[lpe] bad flow data "); + warnx("[lpe] bad flow data "); + _flowFault = FAULT_SEVERE; + } + + } else if (beta > _beta_max.get()) { + if (!_flowFault) { + mavlink_log_info(_mavlink_fd, "[lpe] flow fault, beta %5.2f", double(beta)); + warnx("[lpe] flow fault, beta %5.2f", double(beta)); + _flowFault = FAULT_MINOR; + } + + } else if (_flowFault) { + _flowFault = FAULT_NONE; + mavlink_log_info(_mavlink_fd, "[lpe] flow OK"); + warnx("[lpe] flow OK"); + } + + // kalman filter correction if no fault + if (_flowFault == FAULT_NONE) { + Matrix K = + _P * C.transpose() * S_I; + _x += K * r; + _P -= K * C * _P; + // reset flow integral to current estimate of position + // if a fault occurred + + } else { + _flowX = _x(X_x); + _flowY = _x(X_y); + } + +} + +void BlockLocalPositionEstimator::correctSonar() +{ + + if (_sub_distance.get().type != distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND) { + return; + } + + float d = _sub_distance.get().current_distance; + + // sonar measurement matrix and noise matrix + Matrix C; + C.setZero(); + C(Y_sonar_z, X_z) = -1; + + // use parameter covariance unless sensor provides reasonable value + Matrix R; + R.setZero(); + float cov = _sub_distance.get().covariance; + + if (cov < 1.0e-3f) { + R(0, 0) = _sonar_z_stddev.get() * _sonar_z_stddev.get(); + + } else { + R(0, 0) = cov; + } + + // measurement + Matrix y; + y(0) = (d - _sonarAltHome) * + cosf(_sub_att.get().roll) * + cosf(_sub_att.get().pitch); + + // residual + Matrix r = y - C * _x; + + // residual covariance, (inverse) + Matrix S_I = + (C * _P * C.transpose() + R).inverse(); + + // fault detection + float beta = sqrtf((r.transpose() * (S_I * r))(0, 0)); + + if (d < _sub_distance.get().min_distance || + d > _sub_distance.get().max_distance) { + if (!_sonarFault) { + mavlink_log_info(_mavlink_fd, "[lpe] sonar out of range"); + warnx("[lpe] sonar out of range"); + _sonarFault = FAULT_SEVERE; + } + + } else if (beta > _beta_max.get()) { + if (!_sonarFault) { + mavlink_log_info(_mavlink_fd, "[lpe] sonar fault, beta %5.2f", double(beta)); + warnx("[lpe] sonar fault, beta %5.2f", double(beta)); + _sonarFault = FAULT_MINOR; + } + + } else if (_sonarFault) { + _sonarFault = FAULT_NONE; + mavlink_log_info(_mavlink_fd, "[lpe] sonar OK"); + warnx("[lpe] sonar OK"); + } + + // kalman filter correction if no fault + if (_sonarFault == FAULT_NONE) { + Matrix K = + _P * C.transpose() * S_I; + _x += K * r; + _P -= K * C * _P; + } + + _time_last_sonar = _sub_distance.get().timestamp; + +} + +void BlockLocalPositionEstimator::correctBaro() +{ + + Matrix y; + y(0) = _sub_sensor.get().baro_alt_meter[0] - _baroAltHome; + + // baro measurement matrix + Matrix C; + C.setZero(); + C(Y_baro_z, X_z) = -1; // measured altitude, negative down dir. + + Matrix R; + R.setZero(); + R(0, 0) = _baro_stddev.get() * _baro_stddev.get(); + + // residual + Matrix S_I = + ((C * _P * C.transpose()) + R).inverse(); + Matrix r = y - (C * _x); + + // fault detection + float beta = sqrtf((r.transpose() * (S_I * r))(0, 0)); + + if (beta > _beta_max.get()) { + if (!_baroFault) { + mavlink_log_info(_mavlink_fd, "[lpe] baro fault, beta %5.2f", double(beta)); + warnx("[lpe] baro fault, beta %5.2f", double(beta)); + _baroFault = FAULT_MINOR; + } + + // lower baro trust + S_I = ((C * _P * C.transpose()) + R * 10).inverse(); + + } else if (_baroFault) { + _baroFault = FAULT_NONE; + mavlink_log_info(_mavlink_fd, "[lpe] baro OK"); + warnx("[lpe] baro OK"); + } + + // kalman filter correction if no fault + if (_baroFault == FAULT_NONE) { + Matrix K = _P * C.transpose() * S_I; + _x += K * r; + _P -= K * C * _P; + } + + _time_last_baro = _sub_sensor.get().baro_timestamp[0]; +} + +void BlockLocalPositionEstimator::correctLidar() +{ + + if (_sub_distance.get().type != distance_sensor_s::MAV_DISTANCE_SENSOR_LASER) { + return; + } + + float d = _sub_distance.get().current_distance; + + Matrix C; + C.setZero(); + C(Y_lidar_z, X_z) = -1; // measured altitude, + // negative down dir. + + // use parameter covariance unless sensor provides reasonable value + Matrix R; + R.setZero(); + float cov = _sub_distance.get().covariance; + + if (cov < 1.0e-3f) { + R(0, 0) = _lidar_z_stddev.get() * _lidar_z_stddev.get(); + + } else { + R(0, 0) = cov; + } + + Matrix y; + y.setZero(); + y(0) = (d - _lidarAltHome) * + cosf(_sub_att.get().roll) * + cosf(_sub_att.get().pitch); + + // residual + Matrix S_I = ((C * _P * C.transpose()) + R).inverse(); + Matrix r = y - C * _x; + + // fault detection + float beta = sqrtf((r.transpose() * (S_I * r))(0, 0)); + + // zero is an error code for the lidar + if (d < _sub_distance.get().min_distance || + d > _sub_distance.get().max_distance) { + if (!_lidarFault) { + mavlink_log_info(_mavlink_fd, "[lpe] lidar out of range"); + warnx("[lpe] lidar out of range"); + _lidarFault = FAULT_SEVERE; + } + + } else if (beta > _beta_max.get()) { + if (!_lidarFault) { + mavlink_log_info(_mavlink_fd, "[lpe] lidar fault, beta %5.2f", double(beta)); + warnx("[lpe] lidar fault, beta %5.2f", double(beta)); + _lidarFault = FAULT_MINOR; + } + + } else if (_lidarFault) { // disable fault if ok + _lidarFault = FAULT_NONE; + mavlink_log_info(_mavlink_fd, "[lpe] lidar OK"); + warnx("[lpe] lidar OK"); + } + + // kalman filter correction if no fault + if (_lidarFault == FAULT_NONE) { + Matrix K = _P * C.transpose() * S_I; + _x += K * r; + _P -= K * C * _P; + } + + _time_last_lidar = _sub_distance.get().timestamp; +} + +void BlockLocalPositionEstimator::correctGps() // TODO : use another other metric for glitch detection +{ + + // gps measurement in local frame + double lat = _sub_gps.get().lat * 1.0e-7; + double lon = _sub_gps.get().lon * 1.0e-7; + float alt = _sub_gps.get().alt * 1.0e-3f; + + float px = 0; + float py = 0; + float pz = alt - _gpsAltHome; + map_projection_project(&_map_ref, lat, lon, &px, &py); + + //printf("gps: lat %10g, lon, %10g alt %10g\n", lat, lon, double(alt)); + //printf("home: lat %10g, lon, %10g alt %10g\n", _sub_home.lat, _sub_home.lon, double(_sub_home.alt)); + //printf("local: x %10g y %10g z %10g\n", double(px), double(py), double(pz)); + + Matrix y; + y.setZero(); + y(0) = px; + y(1) = py; + y(2) = pz; + y(3) = _sub_gps.get().vel_n_m_s; + y(4) = _sub_gps.get().vel_e_m_s; + y(5) = _sub_gps.get().vel_d_m_s; + + // gps measurement matrix, measures position and velocity + Matrix C; + C.setZero(); + C(Y_gps_x, X_x) = 1; + C(Y_gps_y, X_y) = 1; + C(Y_gps_z, X_z) = 1; + C(Y_gps_vx, X_vx) = 1; + C(Y_gps_vy, X_vy) = 1; + C(Y_gps_vz, X_vz) = 1; + + // gps covariance matrix + Matrix R; + R.setZero(); + + // default to parameter, use gps cov if provided + float var_xy = _gps_xy_stddev.get() * _gps_xy_stddev.get(); + float var_z = _gps_z_stddev.get() * _gps_z_stddev.get(); + 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) { + var_xy = _sub_gps.get().eph * _sub_gps.get().eph; + } + + if (_sub_gps.get().epv > 1e-3f) { + var_z = _sub_gps.get().epv * _sub_gps.get().epv; + } + + // TODO is velocity covariance provided from gps sub + R(0, 0) = var_xy; + R(1, 1) = var_xy; + R(2, 2) = var_z; + R(3, 3) = var_vxy; + R(4, 4) = var_vxy; + R(5, 5) = var_vz; + + // residual + Matrix r = y - C * _x; + Matrix S_I = (C * _P * C.transpose() + R).inverse(); + + // fault detection + float beta = sqrtf((r.transpose() * (S_I * r))(0, 0)); + uint8_t nSat = _sub_gps.get().satellites_used; + float eph = _sub_gps.get().eph; + + if (nSat < 6 || eph > _gps_eph_max.get()) { + if (!_gpsFault) { + mavlink_log_info(_mavlink_fd, "[lpe] gps fault nSat: %d eph: %5.2f", nSat, double(eph)); + warnx("[lpe] gps fault nSat: %d eph: %5.2f", nSat, double(eph)); + _gpsFault = FAULT_SEVERE; + } + + } else if (beta > _beta_max.get()) { + if (!_gpsFault) { + mavlink_log_info(_mavlink_fd, "[lpe] gps fault, beta: %5.2f", double(beta)); + warnx("[lpe] gps fault, beta: %5.2f", double(beta)); + mavlink_log_info(_mavlink_fd, "[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_log_info(_mavlink_fd, "[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))); + mavlink_log_info(_mavlink_fd, "[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))); + _gpsFault = FAULT_MINOR; + } + + // trust GPS less + S_I = ((C * _P * C.transpose()) + R * 10).inverse(); + + } else if (_gpsFault) { + _gpsFault = FAULT_NONE; + mavlink_log_info(_mavlink_fd, "[lpe] GPS OK"); + warnx("[lpe] GPS OK"); + } + + // kalman filter correction if no hard fault + if (_gpsFault == FAULT_NONE) { + Matrix K = _P * C.transpose() * S_I; + _x += K * r; + _P -= K * C * _P; + } + + _time_last_gps = _timeStamp; +} + +void BlockLocalPositionEstimator::correctVision() +{ + + Matrix y; + y.setZero(); + y(0) = _sub_vision_pos.get().x - _visionHome(0); + y(1) = _sub_vision_pos.get().y - _visionHome(1); + y(2) = _sub_vision_pos.get().z - _visionHome(2); + + // vision measurement matrix, measures position + Matrix C; + C.setZero(); + C(Y_vision_x, X_x) = 1; + C(Y_vision_y, X_y) = 1; + C(Y_vision_z, X_z) = 1; + + // noise matrix + Matrix R; + R.setZero(); + R(Y_vision_x, Y_vision_x) = _vision_xy_stddev.get() * _vision_xy_stddev.get(); + R(Y_vision_y, Y_vision_y) = _vision_xy_stddev.get() * _vision_xy_stddev.get(); + R(Y_vision_z, Y_vision_z) = _vision_z_stddev.get() * _vision_z_stddev.get(); + + // residual + Matrix S_I = ((C * _P * C.transpose()) + R).inverse(); + Matrix r = y - C * _x; + + // fault detection + float beta = sqrtf((r.transpose() * (S_I * r))(0, 0)); + + if (beta > _beta_max.get()) { + if (!_visionFault) { + mavlink_log_info(_mavlink_fd, "[lpe] vision position fault, beta %5.2f", double(beta)); + warnx("[lpe] vision position fault, beta %5.2f", double(beta)); + _visionFault = FAULT_MINOR; + } + + // trust less + S_I = ((C * _P * C.transpose()) + R * 10).inverse(); + + } else if (_visionFault) { + _visionFault = FAULT_NONE; + mavlink_log_info(_mavlink_fd, "[lpe] vision position OK"); + warnx("[lpe] vision position OK"); + } + + // kalman filter correction if no fault + if (_visionFault == FAULT_NONE) { + Matrix K = _P * C.transpose() * S_I; + _x += K * r; + _P -= K * C * _P; + } + + _time_last_vision_p = _sub_vision_pos.get().timestamp_boot; +} + +void BlockLocalPositionEstimator::correctmocap() +{ + + Matrix y; + y.setZero(); + y(Y_mocap_x) = _sub_mocap.get().x - _mocapHome(0); + y(Y_mocap_y) = _sub_mocap.get().y - _mocapHome(1); + y(Y_mocap_z) = _sub_mocap.get().z - _mocapHome(2); + + // mocap measurement matrix, measures position + Matrix C; + C.setZero(); + C(Y_mocap_x, X_x) = 1; + C(Y_mocap_y, X_y) = 1; + C(Y_mocap_z, X_z) = 1; + + // noise matrix + Matrix R; + R.setZero(); + float mocap_p_var = _mocap_p_stddev.get()* \ + _mocap_p_stddev.get(); + R(Y_mocap_x, Y_mocap_x) = mocap_p_var; + R(Y_mocap_y, Y_mocap_y) = mocap_p_var; + R(Y_mocap_z, Y_mocap_z) = mocap_p_var; + + // residual + Matrix S_I = ((C * _P * C.transpose()) + R).inverse(); + Matrix r = y - C * _x; + + // fault detection + float beta = sqrtf((r.transpose() * (S_I * r))(0, 0)); + + if (beta > _beta_max.get()) { + if (!_mocapFault) { + mavlink_log_info(_mavlink_fd, "[lpe] mocap fault, beta %5.2f", double(beta)); + warnx("[lpe] mocap fault, beta %5.2f", double(beta)); + _mocapFault = FAULT_MINOR; + } + + // trust less + S_I = ((C * _P * C.transpose()) + R * 10).inverse(); + + } else if (_mocapFault) { + _mocapFault = FAULT_NONE; + mavlink_log_info(_mavlink_fd, "[lpe] mocap OK"); + warnx("[lpe] mocap OK"); + } + + // kalman filter correction if no fault + if (_mocapFault == FAULT_NONE) { + Matrix K = _P * C.transpose() * S_I; + _x += K * r; + _P -= K * C * _P; + } + + _time_last_mocap = _sub_mocap.get().timestamp_boot; +} diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp new file mode 100644 index 0000000000..66f301efe3 --- /dev/null +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -0,0 +1,310 @@ +#pragma once + +#include +#include +#include +#include + +#ifdef USE_MATRIX_LIB +#include "matrix/src/Matrix.hpp" +using namespace matrix; +#else +#include +using namespace Eigen; +#endif + +// uORB Subscriptions +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// uORB Publications +#include +#include +#include +#include +#include + +#define CBRK_NO_VISION_KEY 328754 + +using namespace control; + +enum fault_t { + FAULT_NONE = 0, + FAULT_MINOR, + FAULT_SEVERE +}; + +enum sensor_t { + SENSOR_BARO = 0, + SENSOR_GPS, + SENSOR_LIDAR, + SENSOR_FLOW, + SENSOR_SONAR, + SENSOR_VISION, + SENSOR_MOCAP +}; + +class BlockLocalPositionEstimator : public control::SuperBlock +{ +// +// The purpose of this estimator is to provide a robust solution for +// indoor flight. +// +// dynamics: +// +// x(+) = A * x(-) + B * u(+) +// y_i = C_i*x +// +// kalman filter +// +// E[xx'] = P +// E[uu'] = W +// E[y_iy_i'] = R_i +// +// prediction +// x(+|-) = A*x(-|-) + B*u(+) +// P(+|-) = A*P(-|-)*A' + B*W*B' +// +// correction +// x(+|+) = x(+|-) + K_i * (y_i - H_i * x(+|-) ) +// +// +// input: +// ax, ay, az (acceleration NED) +// +// states: +// px, py, pz , ( position NED) +// vx, vy, vz ( vel NED), +// bx, by, bz ( TODO accelerometer bias) +// tz (TODO terrain altitude) +// +// measurements: +// +// sonar: pz (measured d*cos(phi)*cos(theta)) +// +// baro: pz +// +// flow: vx, vy (flow is in body x, y frame) +// +// gps: px, py, pz, vx, vy, vz (flow is in body x, y frame) +// +// lidar: px (actual measured d*cos(phi)*cos(theta)) +// +// vision: px, py, pz, vx, vy, vz +// +// mocap: px, py, pz +// +public: + BlockLocalPositionEstimator(); + void update(); + virtual ~BlockLocalPositionEstimator(); + +private: + // prevent copy and assignment + BlockLocalPositionEstimator(const BlockLocalPositionEstimator &); + BlockLocalPositionEstimator operator=(const BlockLocalPositionEstimator &); + + // constants + static const uint8_t n_x = 9; + static const uint8_t n_u = 3; // 3 accelerations + static const uint8_t n_y_flow = 2; + static const uint8_t n_y_sonar = 1; + static const uint8_t n_y_baro = 1; + static const uint8_t n_y_lidar = 1; + static const uint8_t n_y_gps = 6; + static const uint8_t n_y_vision = 3; + static const uint8_t n_y_mocap = 3; + enum {X_x = 0, X_y, X_z, X_vx, X_vy, X_vz, X_bx, X_by, X_bz}; + enum {U_ax = 0, U_ay, U_az}; + enum {Y_baro_z = 0}; + enum {Y_lidar_z = 0}; + enum {Y_flow_x = 0, Y_flow_y}; + enum {Y_sonar_z = 0}; + enum {Y_gps_x = 0, Y_gps_y, Y_gps_z, Y_gps_vx, Y_gps_vy, Y_gps_vz}; + enum {Y_vision_x = 0, Y_vision_y, Y_vision_z, Y_vision_vx, Y_vision_vy, Y_vision_vz}; + enum {Y_mocap_x = 0, Y_mocap_y, Y_mocap_z}; + enum {POLL_FLOW, POLL_SENSORS, POLL_PARAM}; + + // methods + // ---------------------------- + void initP(); + + // predict the next state + void predict(); + + // correct the state prediction wtih a measurement + void correctBaro(); + void correctGps(); + void correctLidar(); + void correctFlow(); + void correctSonar(); + void correctVision(); + void correctmocap(); + + // sensor initialization + void updateHome(); + void initBaro(); + void initGps(); + void initLidar(); + void initSonar(); + void initFlow(); + void initVision(); + void initmocap(); + + // publications + void publishLocalPos(); + void publishGlobalPos(); + void publishFilteredFlow(); + void publishEstimatorStatus(); + + // attributes + // ---------------------------- + + // 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_distance; + 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; + + // publications + uORB::Publication _pub_lpos; + uORB::Publication _pub_gpos; + uORB::Publication _pub_filtered_flow; + uORB::Publication _pub_est_status; + + // map projection + struct map_projection_reference_s _map_ref; + + // parameters + BlockParamInt _integrate; + + BlockParamFloat _flow_xy_stddev; + BlockParamFloat _sonar_z_stddev; + + BlockParamFloat _lidar_z_stddev; + + BlockParamFloat _accel_xy_stddev; + BlockParamFloat _accel_z_stddev; + + BlockParamFloat _baro_stddev; + + BlockParamFloat _gps_xy_stddev; + BlockParamFloat _gps_z_stddev; + + BlockParamFloat _gps_vxy_stddev; + BlockParamFloat _gps_vz_stddev; + + BlockParamFloat _gps_eph_max; + + BlockParamFloat _vision_xy_stddev; + BlockParamFloat _vision_z_stddev; + BlockParamInt _no_vision; + BlockParamFloat _beta_max; + + BlockParamFloat _mocap_p_stddev; + + // process noise + BlockParamFloat _pn_p_noise_power; + BlockParamFloat _pn_v_noise_power; + BlockParamFloat _pn_b_noise_power; + + // misc + struct pollfd _polls[3]; + uint64_t _timeStamp; + uint64_t _time_last_xy; + uint64_t _time_last_flow; + uint64_t _time_last_baro; + uint64_t _time_last_gps; + uint64_t _time_last_lidar; + uint64_t _time_last_sonar; + uint64_t _time_last_vision_p; + uint64_t _time_last_mocap; + int _mavlink_fd; + + // initialization flags + bool _baroInitialized; + bool _gpsInitialized; + bool _lidarInitialized; + bool _sonarInitialized; + bool _flowInitialized; + bool _visionInitialized; + bool _mocapInitialized; + + // init counts + int _baroInitCount; + int _gpsInitCount; + int _lidarInitCount; + int _sonarInitCount; + int _flowInitCount; + int _visionInitCount; + int _mocapInitCount; + + // reference altitudes + float _altHome; + bool _altHomeInitialized; + float _baroAltHome; + float _gpsAltHome; + float _lidarAltHome; + float _sonarAltHome; + float _flowAltHome; + Vector3f _visionHome; + Vector3f _mocapHome; + + // flow integration + float _flowX; + float _flowY; + float _flowMeanQual; + + // referene lat/lon + double _gpsLatHome; + double _gpsLonHome; + + // status + bool _canEstimateXY; + bool _canEstimateZ; + bool _xyTimeout; + + // sensor faults + fault_t _baroFault; + fault_t _gpsFault; + fault_t _lidarFault; + fault_t _flowFault; + fault_t _sonarFault; + fault_t _visionFault; + fault_t _mocapFault; + + bool _visionTimeout; + bool _mocapTimeout; + + perf_counter_t _loop_perf; + perf_counter_t _interval_perf; + perf_counter_t _err_perf; + + // state space + Matrix _x; // state vector + Matrix _u; // input vector + Matrix _P; // state covariance matrix +}; diff --git a/src/modules/local_position_estimator/CMakeLists.txt b/src/modules/local_position_estimator/CMakeLists.txt new file mode 100644 index 0000000000..e0d1be2d0a --- /dev/null +++ b/src/modules/local_position_estimator/CMakeLists.txt @@ -0,0 +1,57 @@ +############################################################################ +# +# 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. +# +############################################################################ +if(${OS} STREQUAL "nuttx") + list(APPEND MODULE_CFLAGS -Wframe-larger-than=6500) +elseif(${OS} STREQUAL "posix") + list(APPEND MODULE_CFLAGS -Wno-error) + # add matrix tests + add_subdirectory(matrix) +endif() + +# use custom matrix lib instead of Eigen +add_definitions(-DUSE_MATRIX_LIB) + + +px4_add_module( + MODULE modules__local_position_estimator + MAIN local_position_estimator + STACK 9216 + COMPILE_FLAGS ${MODULE_CFLAGS} + SRCS + local_position_estimator_main.cpp + BlockLocalPositionEstimator.cpp + params.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/local_position_estimator/local_position_estimator_main.cpp b/src/modules/local_position_estimator/local_position_estimator_main.cpp new file mode 100644 index 0000000000..b0cd25554e --- /dev/null +++ b/src/modules/local_position_estimator/local_position_estimator_main.cpp @@ -0,0 +1,169 @@ +/**************************************************************************** + * + * 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 local_position_estimator.cpp + * @author James Goppert + * @author Mohammed Kabir + * @author Nuno Marques + * + * Local position estimator + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "BlockLocalPositionEstimator.hpp" + +static volatile bool thread_should_exit = false; /**< Deamon exit flag */ +static volatile bool thread_running = false; /**< Deamon status flag */ +static int deamon_task; /**< Handle of deamon task / thread */ + +/** + * Deamon management function. + */ +extern "C" __EXPORT int local_position_estimator_main(int argc, char *argv[]); + +/** + * Mainloop of deamon. + */ +int local_position_estimator_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static int usage(const char *reason); + +static int +usage(const char *reason) +{ + if (reason) { + fprintf(stderr, "%s\n", reason); + } + + fprintf(stderr, "usage: local_position_estimator {start|stop|status} [-p ]\n\n"); + return 1; +} + +/** + * The deamon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ +int local_position_estimator_main(int argc, char *argv[]) +{ + + if (argc < 1) { + usage("missing command"); + } + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("already running"); + /* this is not an error */ + return 0; + } + + thread_should_exit = false; + + deamon_task = px4_task_spawn_cmd("lp_estimator", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 10240, + local_position_estimator_thread_main, + (argv && argc > 2) ? (char *const *) &argv[2] : (char *const *) NULL); + return 0; + } + + if (!strcmp(argv[1], "stop")) { + if (thread_running) { + warnx("stop"); + thread_should_exit = true; + + } else { + warnx("not started"); + } + + return 0; + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("is running"); + + } else { + warnx("not started"); + } + + return 0; + } + + usage("unrecognized command"); + return 1; +} + +int local_position_estimator_thread_main(int argc, char *argv[]) +{ + + warnx("starting"); + + using namespace control; + + + thread_running = true; + + BlockLocalPositionEstimator est; + + while (!thread_should_exit) { + est.update(); + } + + warnx("exiting."); + + thread_running = false; + + return 0; +} diff --git a/src/modules/local_position_estimator/matrix/.gitignore b/src/modules/local_position_estimator/matrix/.gitignore new file mode 100644 index 0000000000..a5309e6b90 --- /dev/null +++ b/src/modules/local_position_estimator/matrix/.gitignore @@ -0,0 +1 @@ +build*/ diff --git a/src/modules/local_position_estimator/matrix/CMakeLists.txt b/src/modules/local_position_estimator/matrix/CMakeLists.txt new file mode 100644 index 0000000000..5a16ced707 --- /dev/null +++ b/src/modules/local_position_estimator/matrix/CMakeLists.txt @@ -0,0 +1,13 @@ +cmake_minimum_required(VERSION 2.8) + +project(matrix CXX) + +set(CMAKE_BUILD_TYPE "RelWithDebInfo") + +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Weffc++") + +enable_testing() + +include_directories(src) + +add_subdirectory(test) diff --git a/src/modules/local_position_estimator/matrix/src/Matrix.hpp b/src/modules/local_position_estimator/matrix/src/Matrix.hpp new file mode 100644 index 0000000000..ae1111894c --- /dev/null +++ b/src/modules/local_position_estimator/matrix/src/Matrix.hpp @@ -0,0 +1,464 @@ +#pragma once + +#include +#include +#include +#include +#include + +namespace matrix +{ + +template +class Matrix +{ + +private: + T _data[M][N]; + size_t _rows; + size_t _cols; + +public: + + Matrix() : + _data(), + _rows(M), + _cols(N) + { + } + + Matrix(const T *data) : + _data(), + _rows(M), + _cols(N) + { + memcpy(_data, data, sizeof(_data)); + } + + Matrix(const Matrix &other) : + _data(), + _rows(M), + _cols(N) + { + memcpy(_data, other._data, sizeof(_data)); + } + + Matrix(T x, T y, T z) : + _data(), + _rows(M), + _cols(N) + { + // TODO, limit to only 3x1 matrices + _data[0][0] = x; + _data[1][0] = y; + _data[2][0] = z; + } + + /** + * Accessors/ Assignment etc. + */ + + T *data() + { + return _data[0]; + } + + inline size_t rows() const + { + return _rows; + } + + inline size_t cols() const + { + return _rows; + } + + inline T operator()(size_t i) const + { + return _data[i][0]; + } + + inline T operator()(size_t i, size_t j) const + { + return _data[i][j]; + } + + inline T &operator()(size_t i) + { + return _data[i][0]; + } + + inline T &operator()(size_t i, size_t j) + { + return _data[i][j]; + } + + /** + * Matrix Operations + */ + + // this might use a lot of programming memory + // since it instantiates a class for every + // required mult pair, but it provides + // compile time size_t checking + template + Matrix operator*(const Matrix &other) const + { + const Matrix &self = *this; + Matrix res; + res.setZero(); + + for (size_t i = 0; i < M; i++) { + for (size_t k = 0; k < P; k++) { + for (size_t j = 0; j < N; j++) { + res(i, k) += self(i, j) * other(j, k); + } + } + } + + return res; + } + + Matrix operator+(const Matrix &other) const + { + Matrix res; + const Matrix &self = *this; + + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + res(i , j) = self(i, j) + other(i, j); + } + } + + return res; + } + + bool operator==(const Matrix &other) const + { + Matrix res; + const Matrix &self = *this; + + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + if (self(i , j) != other(i, j)) { + return false; + } + } + } + + return true; + } + + Matrix operator-(const Matrix &other) const + { + Matrix res; + const Matrix &self = *this; + + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + res(i , j) = self(i, j) - other(i, j); + } + } + + return res; + } + + void operator+=(const Matrix &other) + { + Matrix &self = *this; + self = self + other; + } + + void operator-=(const Matrix &other) + { + Matrix &self = *this; + self = self - other; + } + + void operator*=(const Matrix &other) + { + Matrix &self = *this; + self = self * other; + } + + /** + * Scalar Operations + */ + + Matrix operator*(T scalar) const + { + Matrix res; + const Matrix &self = *this; + + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + res(i , j) = self(i, j) * scalar; + } + } + + return res; + } + + Matrix operator+(T scalar) const + { + Matrix res; + Matrix &self = *this; + + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + res(i , j) = self(i, j) + scalar; + } + } + + return res; + } + + void operator*=(T scalar) + { + Matrix &self = *this; + + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + self(i, j) = self(i, j) * scalar; + } + } + } + + void operator/=(T scalar) + { + Matrix &self = *this; + self = self * (1.0f / scalar); + } + + /** + * Misc. Functions + */ + + void print() + { + Matrix &self = *this; + printf("\n"); + + for (size_t i = 0; i < M; i++) { + printf("["); + + for (size_t j = 0; j < N; j++) { + printf("%10g\t", double(self(i, j))); + } + + printf("]\n"); + } + } + + Matrix transpose() const + { + Matrix res; + const Matrix &self = *this; + + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + res(j, i) = self(i, j); + } + } + + return res; + } + + Matrix expm(float dt, size_t n) const + { + Matrix res; + res.setIdentity(); + Matrix A_pow = *this; + float k_fact = 1; + size_t k = 1; + + while (k < n) { + res += A_pow * (float(pow(dt, k)) / k_fact); + + if (k == n) { break; } + + A_pow *= A_pow; + k_fact *= k; + k++; + } + + return res; + } + + Matrix diagonal() const + { + Matrix res; + // force square for now + const Matrix &self = *this; + + for (size_t i = 0; i < M; i++) { + res(i) = self(i, i); + } + + return res; + } + + void setZero() + { + memset(_data, 0, sizeof(_data)); + } + + void setIdentity() + { + setZero(); + Matrix &self = *this; + + for (size_t i = 0; i < M and i < N; i++) { + self(i, i) = 1; + } + } + + inline void swapRows(size_t a, size_t b) + { + if (a == b) { return; } + + Matrix &self = *this; + + for (size_t j = 0; j < cols(); j++) { + T tmp = self(a, j); + self(a, j) = self(b, j); + self(b, j) = tmp; + } + } + + inline void swapCols(size_t a, size_t b) + { + if (a == b) { return; } + + Matrix &self = *this; + + for (size_t i = 0; i < rows(); i++) { + T tmp = self(i, a); + self(i, a) = self(i, b); + self(i, b) = tmp; + } + } + + /** + * inverse based on LU factorization with partial pivotting + */ + Matrix inverse() const + { + Matrix L; + L.setIdentity(); + const Matrix &A = (*this); + Matrix U = A; + Matrix P; + P.setIdentity(); + + //printf("A:\n"); A.print(); + + // for all diagonal elements + for (size_t n = 0; n < N; n++) { + + // if diagonal is zero, swap with row below + if (fabsf(U(n, n)) < 1e-8f) { + //printf("trying pivot for row %d\n",n); + for (size_t i = 0; i < N; i++) { + if (i == n) { continue; } + + //printf("\ttrying row %d\n",i); + if (fabsf(U(i, n)) > 1e-8f) { + //printf("swapped %d\n",i); + U.swapRows(i, n); + P.swapRows(i, n); + } + } + } + +#ifdef MATRIX_ASSERT + //printf("A:\n"); A.print(); + //printf("U:\n"); U.print(); + //printf("P:\n"); P.print(); + //fflush(stdout); + ASSERT(fabsf(U(n, n)) > 1e-8f); +#endif + + // failsafe, return zero matrix + if (fabsf(U(n, n)) < 1e-8f) { + Matrix zero; + zero.setZero(); + return zero; + } + + // for all rows below diagonal + for (size_t i = (n + 1); i < N; i++) { + L(i, n) = U(i, n) / U(n, n); + + // add i-th row and n-th row + // multiplied by: -a(i,n)/a(n,n) + for (size_t k = n; k < N; k++) { + U(i, k) -= L(i, n) * U(n, k); + } + } + } + + //printf("L:\n"); L.print(); + //printf("U:\n"); U.print(); + + // solve LY=P*I for Y by forward subst + Matrix Y = P; + + // for all columns of Y + for (size_t c = 0; c < N; c++) { + // for all rows of L + for (size_t i = 0; i < N; i++) { + // for all columns of L + for (size_t j = 0; j < i; j++) { + // for all existing y + // subtract the component they + // contribute to the solution + Y(i, c) -= L(i, j) * Y(j, c); + } + + // divide by the factor + // on current + // term to be solved + // Y(i,c) /= L(i,i); + // but L(i,i) = 1.0 + } + } + + //printf("Y:\n"); Y.print(); + + // solve Ux=y for x by back subst + Matrix X = Y; + + // for all columns of X + for (size_t c = 0; c < N; c++) { + // for all rows of U + for (size_t k = 0; k < N; k++) { + // have to go in reverse order + size_t i = N - 1 - k; + + // for all columns of U + for (size_t j = i + 1; j < N; j++) { + // for all existing x + // subtract the component they + // contribute to the solution + X(i, c) -= U(i, j) * X(j, c); + } + + // divide by the factor + // on current + // term to be solved + X(i, c) /= U(i, i); + } + } + + //printf("X:\n"); X.print(); + return X; + } + +}; + +typedef Matrix Vector2f; +typedef Matrix Vector3f; +typedef Matrix Matrix3f; + +}; // namespace matrix diff --git a/src/modules/local_position_estimator/matrix/test/CMakeLists.txt b/src/modules/local_position_estimator/matrix/test/CMakeLists.txt new file mode 100644 index 0000000000..cf280e287b --- /dev/null +++ b/src/modules/local_position_estimator/matrix/test/CMakeLists.txt @@ -0,0 +1,15 @@ +set(tests + setIdentity + inverse + matrixMult + vectorAssignment + matrixAssignment + matrixScalarMult + transpose + ) + +foreach(test ${tests}) + add_executable(${test} + ${test}.cpp) + add_test(${test} ${test}) +endforeach() diff --git a/src/modules/local_position_estimator/matrix/test/inverse.cpp b/src/modules/local_position_estimator/matrix/test/inverse.cpp new file mode 100644 index 0000000000..73f7c0b432 --- /dev/null +++ b/src/modules/local_position_estimator/matrix/test/inverse.cpp @@ -0,0 +1,30 @@ +#include "Matrix.hpp" +#include +#include + +using namespace matrix; + +int main() +{ + float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1}; + Matrix3f A(data); + Matrix3f A_I = A.inverse(); + float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1}; + Matrix3f A_I_check(data_check); + (void)A_I; + assert(A_I == A_I_check); + + // stess test + static const size_t n = 100; + Matrix A_large; + A_large.setIdentity(); + Matrix A_large_I; + A_large_I.setZero(); + + for (size_t i = 0; i < 100; i++) { + A_large_I = A_large.inverse(); + assert(A_large == A_large_I); + } + + return 0; +} diff --git a/src/modules/local_position_estimator/matrix/test/matrixAssignment.cpp b/src/modules/local_position_estimator/matrix/test/matrixAssignment.cpp new file mode 100644 index 0000000000..5a625d0cc1 --- /dev/null +++ b/src/modules/local_position_estimator/matrix/test/matrixAssignment.cpp @@ -0,0 +1,29 @@ +#include "Matrix.hpp" +#include + +using namespace matrix; + +int main() +{ + Matrix3f m; + m.setZero(); + m(0, 0) = 1; + m(0, 1) = 2; + m(0, 2) = 3; + m(1, 0) = 4; + m(1, 1) = 5; + m(1, 2) = 6; + m(2, 0) = 7; + m(2, 1) = 8; + m(2, 2) = 9; + + m.print(); + + float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9}; + Matrix3f m2(data); + m2.print(); + + assert(m == m2); + + return 0; +} diff --git a/src/modules/local_position_estimator/matrix/test/matrixMult.cpp b/src/modules/local_position_estimator/matrix/test/matrixMult.cpp new file mode 100644 index 0000000000..7b42d947d4 --- /dev/null +++ b/src/modules/local_position_estimator/matrix/test/matrixMult.cpp @@ -0,0 +1,26 @@ +#include "Matrix.hpp" +#include +#include + +using namespace matrix; + +int main() +{ + float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1}; + Matrix3f A(data); + float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1}; + Matrix3f A_I(data_check); + Matrix3f I; + I.setIdentity(); + A.print(); + A_I.print(); + Matrix3f R = A * A_I; + R.print(); + assert(R == I); + + Matrix3f R2 = A; + R2 *= A_I; + R2.print(); + assert(R2 == I); + return 0; +} diff --git a/src/modules/local_position_estimator/matrix/test/matrixScalarMult.cpp b/src/modules/local_position_estimator/matrix/test/matrixScalarMult.cpp new file mode 100644 index 0000000000..78bdb5b27f --- /dev/null +++ b/src/modules/local_position_estimator/matrix/test/matrixScalarMult.cpp @@ -0,0 +1,18 @@ +#include "Matrix.hpp" +#include +#include + +using namespace matrix; + +int main() +{ + float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9}; + Matrix3f A(data); + A = A * 2; + float data_check[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18}; + Matrix3f A_check(data_check); + A.print(); + A_check.print(); + assert(A == A_check); + return 0; +} diff --git a/src/modules/local_position_estimator/matrix/test/setIdentity.cpp b/src/modules/local_position_estimator/matrix/test/setIdentity.cpp new file mode 100644 index 0000000000..f80e437939 --- /dev/null +++ b/src/modules/local_position_estimator/matrix/test/setIdentity.cpp @@ -0,0 +1,25 @@ +#include "Matrix.hpp" +#include + +using namespace matrix; + +int main() +{ + Matrix3f A; + A.setIdentity(); + assert(A.rows() == 3); + assert(A.cols() == 3); + + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + if (i == j) { + assert(A(i, j) == 1); + + } else { + assert(A(i, j) == 0); + } + } + } + + return 0; +} diff --git a/src/modules/local_position_estimator/matrix/test/transpose.cpp b/src/modules/local_position_estimator/matrix/test/transpose.cpp new file mode 100644 index 0000000000..3623fc1f9a --- /dev/null +++ b/src/modules/local_position_estimator/matrix/test/transpose.cpp @@ -0,0 +1,18 @@ +#include "Matrix.hpp" +#include +#include + +using namespace matrix; + +int main() +{ + float data[9] = {1, 2, 3, 4, 5, 6}; + Matrix A(data); + Matrix A_T = A.transpose(); + float data_check[9] = {1, 4, 2, 5, 3, 6}; + Matrix A_T_check(data_check); + A_T.print(); + A_T_check.print(); + assert(A_T == A_T_check); + return 0; +} diff --git a/src/modules/local_position_estimator/matrix/test/vectorAssignment.cpp b/src/modules/local_position_estimator/matrix/test/vectorAssignment.cpp new file mode 100644 index 0000000000..68f5070194 --- /dev/null +++ b/src/modules/local_position_estimator/matrix/test/vectorAssignment.cpp @@ -0,0 +1,28 @@ +#include "Matrix.hpp" +#include + +using namespace matrix; + +int main() +{ + Vector3f v; + v(0) = 1; + v(1) = 2; + v(2) = 3; + + v.print(); + + assert(v(0) == 1); + assert(v(1) == 2); + assert(v(2) == 3); + + Vector3f v2(4, 5, 6); + + v2.print(); + + assert(v2(0) == 4); + assert(v2(1) == 5); + assert(v2(2) == 6); + + return 0; +} diff --git a/src/modules/local_position_estimator/params.c b/src/modules/local_position_estimator/params.c new file mode 100644 index 0000000000..8fbde4ab69 --- /dev/null +++ b/src/modules/local_position_estimator/params.c @@ -0,0 +1,222 @@ +#include + +// 16 is max name length + + +/** + * Enable local position estimator. + * + * @group Local Position Estimator + */ +PARAM_DEFINE_INT32(LPE_ENABLED, 1); + +/** + * Enable accelerometer integration for prediction. + * + * @group Local Position Estimator + */ +PARAM_DEFINE_INT32(LPE_INTEGRATE, 1); + +/** + * Optical flow xy standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 1 + */ +PARAM_DEFINE_FLOAT(LPE_FLW_XY, 0.01f); + +/** + * Sonar z standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 1 + */ +PARAM_DEFINE_FLOAT(LPE_SNR_Z, 0.2f); + +/** + * Lidar z standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 1 + */ +PARAM_DEFINE_FLOAT(LPE_LDR_Z, 0.03f); + +/** + * Accelerometer xy standard deviation + * + * 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. + * + * should be 0.0464 + * + * @group Local Position Estimator + * @unit m/s^2 + * @min 0.00001 + * @max 2 + */ +PARAM_DEFINE_FLOAT(LPE_ACC_XY, 0.0454f); + +/** + * Accelerometer z standard deviation + * + * (see Accel x comments) + * + * @group Local Position Estimator + * @unit m/s^2 + * @min 0.00001 + * @max 2 + */ +PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.0454f); + +/** + * Barometric presssure altitude z standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 3 + */ +PARAM_DEFINE_FLOAT(LPE_BAR_Z, 1.0f); + +/** + * GPS xy standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 5 + */ +PARAM_DEFINE_FLOAT(LPE_GPS_XY, 2.0f); + +/** + * GPS z standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 20 + */ +PARAM_DEFINE_FLOAT(LPE_GPS_Z, 10.0f); + +/** + * GPS xy velocity standard deviation. + * + * @group Local Position Estimator + * @unit m/s + * @min 0.01 + * @max 2 + */ +PARAM_DEFINE_FLOAT(LPE_GPS_VXY, 0.275f); + +/** + * GPS z velocity standard deviation. + * + * @group Local Position Estimator + * @unit m/s + * @min 0.01 + * @max 2 + */ +PARAM_DEFINE_FLOAT(LPE_GPS_VZ, 0.237f); + +/** + * GPS max eph + * + * @group Local Position Estimator + * @unit m + * @min 1.0 + * @max 5.0 + */ +PARAM_DEFINE_FLOAT(LPE_EPH_MAX, 3.0f); + + + +/** + * Vision xy standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 1 + */ +PARAM_DEFINE_FLOAT(LPE_VIS_XY, 0.5f); + +/** + * Vision z standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 2 + */ +PARAM_DEFINE_FLOAT(LPE_VIS_Z, 0.5f); + +/** + * Circuit breaker to disable vision input. + * + * Set to the appropriate key (328754) to disable vision input. + * + * @group Local Position Estimator + * @min 0 + * @max 1 + */ +PARAM_DEFINE_INT32(LPE_NO_VISION, 0); + +/** + * Vicon position standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 1 + */ +PARAM_DEFINE_FLOAT(LPE_VIC_P, 0.05f); + +/** + * Position propagation process noise power (variance*sampling rate). + * + * @group Local Position Estimator + * @unit (m/s^2)-s + * @min 0 + * @max 1 + */ +PARAM_DEFINE_FLOAT(LPE_PN_P, 0.0f); + +/** + * Velocity propagation process noise power (variance*sampling rate). + * + * @group Local Position Estimator + * @unit (m/s)-s + * @min 0 + * @max 5 + */ +PARAM_DEFINE_FLOAT(LPE_PN_V, 0.0f); + +/** + * Accel bias propagation process noise power (variance*sampling rate). + * + * @group Local Position Estimator + * @unit (m/s)-s + * @min 0 + * @max 1 + */ +PARAM_DEFINE_FLOAT(LPE_PN_B, 1e-8f); + +/** + * Fault detection threshold, for chi-squared dist. + * + * TODO add separate params for 1 dof, 3 dof, and 6 dof beta + * or false alarm rate in false alarms/hr + * + * @group Local Position Estimator + * @unit + * @min 3 + * @max 1000 + */ +PARAM_DEFINE_FLOAT(LPE_BETA_MAX, 1000.0f); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 297a76239b..a12133c021 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1676,7 +1676,6 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("GPS_RAW_INT", 1.0f); configure_stream("GLOBAL_POSITION_INT", 3.0f); configure_stream("LOCAL_POSITION_NED", 3.0f); - configure_stream("NAMED_VALUE_FLOAT", 2.0f); configure_stream("RC_CHANNELS", 1.0f); configure_stream("SERVO_OUTPUT_RAW_0", 1.0f); configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index fed42bef2b..be3ab850f4 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -73,6 +73,7 @@ #include #include #include +#include #include #include #include @@ -1271,6 +1272,78 @@ protected: }; +class MavlinkStreamLocalPositionNEDCOV : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamLocalPositionNEDCOV::get_name_static(); + } + + static const char *get_name_static() + { + return "LOCAL_POSITION_NED_COV"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamLocalPositionNEDCOV(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_est_sub; + uint64_t _est_time; + + /* do not allow top copying this class */ + MavlinkStreamLocalPositionNEDCOV(MavlinkStreamLocalPositionNEDCOV &); + MavlinkStreamLocalPositionNEDCOV& operator = (const MavlinkStreamLocalPositionNEDCOV &); + +protected: + explicit MavlinkStreamLocalPositionNEDCOV(Mavlink *mavlink) : MavlinkStream(mavlink), + _est_sub(_mavlink->add_orb_subscription(ORB_ID(estimator_status))), + _est_time(0) + {} + + void send(const hrt_abstime t) + { + struct estimator_status_s est; + + if (_est_sub->update(&_est_time, &est)) { + mavlink_local_position_ned_cov_t msg; + + msg.time_boot_ms = est.timestamp / 1000; + msg.x = est.states[0]; + msg.y = est.states[1]; + msg.z = est.states[2]; + msg.vx = est.states[3]; + msg.vy = est.states[4]; + msg.vz = est.states[5]; + msg.ax = est.states[6]; + msg.ay = est.states[7]; + msg.az = est.states[8]; + for (int i=0;i<9;i++) { + msg.covariance[i] = est.covariances[i]; + } + msg.covariance[9] = est.nan_flags; + msg.covariance[10] = est.health_flags; + 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); + } + } +}; + class MavlinkStreamAttPosMocap : public MavlinkStream { public: @@ -2478,6 +2551,7 @@ const StreamListItem *streams_list[] = { 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(&MavlinkStreamLocalPositionNEDCOV::new_instance, &MavlinkStreamLocalPositionNEDCOV::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), 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 95ba66361a..ab8030e471 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -155,6 +155,10 @@ private: control::BlockParamFloat _manual_thr_min; control::BlockParamFloat _manual_thr_max; + control::BlockDerivative _vel_x_deriv; + control::BlockDerivative _vel_y_deriv; + control::BlockDerivative _vel_z_deriv; + struct { param_t thr_min; param_t thr_max; @@ -177,6 +181,10 @@ private: param_t man_pitch_max; param_t man_yaw_max; param_t mc_att_yaw_p; + param_t hold_xy_dz; + param_t hold_z_dz; + param_t hold_max_xy; + param_t hold_max_z; } _params_handles; /**< handles for interesting parameters */ struct { @@ -189,6 +197,10 @@ private: float man_pitch_max; float man_yaw_max; float mc_att_yaw_p; + float hold_xy_dz; + float hold_z_dz; + float hold_max_xy; + float hold_max_z; math::Vector<3> pos_p; math::Vector<3> vel_p; @@ -206,6 +218,10 @@ private: bool _reset_pos_sp; bool _reset_alt_sp; bool _mode_auto; + bool _pos_hold_engaged; + bool _alt_hold_engaged; + bool _run_pos_control; + bool _run_alt_control; math::Vector<3> _pos; math::Vector<3> _pos_sp; @@ -213,7 +229,6 @@ private: math::Vector<3> _vel_sp; math::Vector<3> _vel_prev; /**< velocity on previous step */ math::Vector<3> _vel_ff; - math::Vector<3> _sp_move_rate; math::Matrix<3, 3> _R; /**< rotation matrix from attitude quaternions */ float _yaw; /**< yaw angle (euler) */ @@ -323,12 +338,19 @@ MulticopterPositionControl::MulticopterPositionControl() : _global_vel_sp_pub(nullptr), _manual_thr_min(this, "MANTHR_MIN"), _manual_thr_max(this, "MANTHR_MAX"), + _vel_x_deriv(this, "VELD"), + _vel_y_deriv(this, "VELD"), + _vel_z_deriv(this, "VELD"), _ref_alt(0.0f), _ref_timestamp(0), _reset_pos_sp(true), _reset_alt_sp(true), _mode_auto(false), + _pos_hold_engaged(false), + _alt_hold_engaged(false), + _run_pos_control(true), + _run_alt_control(true), _yaw(0.0f) { memset(&_vehicle_status, 0, sizeof(_vehicle_status)); @@ -358,7 +380,6 @@ MulticopterPositionControl::MulticopterPositionControl() : _vel_sp.zero(); _vel_prev.zero(); _vel_ff.zero(); - _sp_move_rate.zero(); _R.identity(); @@ -383,6 +404,11 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_handles.man_pitch_max = param_find("MPC_MAN_P_MAX"); _params_handles.man_yaw_max = param_find("MPC_MAN_Y_MAX"); _params_handles.mc_att_yaw_p = param_find("MC_YAW_P"); + _params_handles.hold_xy_dz = param_find("MPC_HOLD_XY_DZ"); + _params_handles.hold_z_dz = param_find("MPC_HOLD_Z_DZ"); + _params_handles.hold_max_xy = param_find("MPC_HOLD_MAX_XY"); + _params_handles.hold_max_z = param_find("MPC_HOLD_MAX_Z"); + /* fetch initial parameter values */ parameters_update(true); @@ -470,6 +496,16 @@ MulticopterPositionControl::parameters_update(bool force) param_get(_params_handles.z_ff, &v); v = math::constrain(v, 0.0f, 1.0f); _params.vel_ff(2) = v; + param_get(_params_handles.hold_xy_dz, &v); + v = math::constrain(v, 0.0f, 1.0f); + _params.hold_xy_dz = v; + param_get(_params_handles.hold_z_dz, &v); + v = math::constrain(v, 0.0f, 1.0f); + _params.hold_z_dz = v; + param_get(_params_handles.hold_max_xy, &v); + _params.hold_max_xy = (v < 0.0f ? 0.0f : v); + param_get(_params_handles.hold_max_z, &v); + _params.hold_max_z = (v < 0.0f ? 0.0f : v); _params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f; @@ -590,9 +626,9 @@ MulticopterPositionControl::reset_pos_sp() _reset_pos_sp = false; /* shift position setpoint to make attitude setpoint continuous */ _pos_sp(0) = _pos(0) + (_vel(0) - PX4_R(_att_sp.R_body, 0, 2) * _att_sp.thrust / _params.vel_p(0) - - _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0); + - _params.vel_ff(0) * _vel_sp(0)) / _params.pos_p(0); _pos_sp(1) = _pos(1) + (_vel(1) - PX4_R(_att_sp.R_body, 1, 2) * _att_sp.thrust / _params.vel_p(1) - - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1); + - _params.vel_ff(1) * _vel_sp(1)) / _params.pos_p(1); mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1)); } } @@ -602,7 +638,7 @@ MulticopterPositionControl::reset_alt_sp() { if (_reset_alt_sp) { _reset_alt_sp = false; - _pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2); + _pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _vel_sp(2)) / _params.pos_p(2); mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2)); } } @@ -633,31 +669,20 @@ MulticopterPositionControl::limit_pos_sp_offset() void MulticopterPositionControl::control_manual(float dt) { - _sp_move_rate.zero(); + math::Vector<3> req_vel_sp; // X,Y in local frame and Z in global (D), in [-1,1] normalized range + req_vel_sp.zero(); if (_control_mode.flag_control_altitude_enabled) { - /* move altitude setpoint with throttle stick */ - _sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz); + /* set vertical velocity setpoint with throttle stick */ + req_vel_sp(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz); // D } if (_control_mode.flag_control_position_enabled) { - /* move position setpoint with roll/pitch stick */ - _sp_move_rate(0) = _manual.x; - _sp_move_rate(1) = _manual.y; + /* set horizontal velocity setpoint with roll/pitch stick */ + req_vel_sp(0) = _manual.x; + req_vel_sp(1) = _manual.y; } - /* limit setpoint move rate */ - float sp_move_norm = _sp_move_rate.length(); - - if (sp_move_norm > 1.0f) { - _sp_move_rate /= sp_move_norm; - } - - /* _sp_move_rate scaled to 0..1, scale it to max speed and rotate around yaw */ - math::Matrix<3, 3> R_yaw_sp; - R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body); - _sp_move_rate = R_yaw_sp * _sp_move_rate.emult(_params.vel_max); - if (_control_mode.flag_control_altitude_enabled) { /* reset alt setpoint to current altitude if needed */ reset_alt_sp(); @@ -668,30 +693,70 @@ MulticopterPositionControl::control_manual(float dt) reset_pos_sp(); } - /* feed forward setpoint move rate with weight vel_ff */ - _vel_ff = _sp_move_rate.emult(_params.vel_ff); + /* limit velocity setpoint */ + float req_vel_sp_norm = req_vel_sp.length(); - /* move position setpoint */ - _pos_sp += _sp_move_rate * dt; - - /* check if position setpoint is too far from actual position */ - math::Vector<3> pos_sp_offs; - pos_sp_offs.zero(); - - if (_control_mode.flag_control_position_enabled) { - pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); - pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); + if (req_vel_sp_norm > 1.0f) { + req_vel_sp /= req_vel_sp_norm; } - if (_control_mode.flag_control_altitude_enabled) { - pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); + /* _req_vel_sp scaled to 0..1, scale it to max speed and rotate around yaw */ + math::Matrix<3, 3> R_yaw_sp; + R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body); + math::Vector<3> req_vel_sp_scaled = R_yaw_sp * req_vel_sp.emult(_params.vel_max); // in NED and scaled to actual velocity + + /* + * assisted velocity mode: user controls velocity, but if velocity is small enough, position + * hold is activated for the corresponding axis + */ + + /* horizontal axes */ + if (_control_mode.flag_control_position_enabled) + { + /* check for pos. hold */ + if (fabsf(req_vel_sp(0)) < _params.hold_xy_dz && fabsf(req_vel_sp(1)) < _params.hold_xy_dz) + { + if (!_pos_hold_engaged && (_params.hold_max_xy < FLT_EPSILON || + (fabsf(_vel(0)) < _params.hold_max_xy && fabsf(_vel(1)) < _params.hold_max_xy))) + { + _pos_hold_engaged = true; + } + } + else { + _pos_hold_engaged = false; + _pos_sp(0) = _pos(0); + _pos_sp(1) = _pos(1); + } + + /* set requested velocity setpoint */ + if (!_pos_hold_engaged) { + _run_pos_control = false; /* request velocity setpoint to be used, instead of position setpoint */ + _vel_sp(0) = req_vel_sp_scaled(0); + _vel_sp(1) = req_vel_sp_scaled(1); + } } - float pos_sp_offs_norm = pos_sp_offs.length(); + /* vertical axis */ + if (_control_mode.flag_control_altitude_enabled) + { + /* check for pos. hold */ + if (fabsf(req_vel_sp(2)) < _params.hold_z_dz) + { + if (!_alt_hold_engaged && (_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z)) + { + _alt_hold_engaged = true; + } + } + else { + _alt_hold_engaged = false; + _pos_sp(2) = _pos(2); + } - if (pos_sp_offs_norm > 1.0f) { - pos_sp_offs /= pos_sp_offs_norm; - _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); + /* set requested velocity setpoint */ + if (!_alt_hold_engaged) { + _run_alt_control = false; /* request velocity setpoint to be used, instead of altitude setpoint */ + _vel_sp(2) = req_vel_sp_scaled(2); + } } } @@ -716,8 +781,10 @@ MulticopterPositionControl::control_offboard(float dt) reset_pos_sp(); /* set position setpoint move rate */ - _sp_move_rate(0) = _pos_sp_triplet.current.vx; - _sp_move_rate(1) = _pos_sp_triplet.current.vy; + _vel_sp(0) = _pos_sp_triplet.current.vx; + _vel_sp(1) = _pos_sp_triplet.current.vy; + + _run_pos_control = false; /* request velocity setpoint to be used, instead of position setpoint */ } if (_pos_sp_triplet.current.yaw_valid) { @@ -734,15 +801,10 @@ MulticopterPositionControl::control_offboard(float dt) reset_alt_sp(); /* set altitude setpoint move rate */ - _sp_move_rate(2) = _pos_sp_triplet.current.vz; + _vel_sp(2) = _pos_sp_triplet.current.vz; + + _run_alt_control = false; /* request velocity setpoint to be used, instead of position setpoint */ } - - /* feed forward setpoint move rate with weight vel_ff */ - _vel_ff = _sp_move_rate.emult(_params.vel_ff); - - /* move position setpoint */ - _pos_sp += _sp_move_rate * dt; - } else { reset_pos_sp(); reset_alt_sp(); @@ -998,6 +1060,9 @@ MulticopterPositionControl::task_main() float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.0f; t_prev = t; + // set dt for control blocks + setDt(dt); + if (_control_mode.flag_armed && !was_armed) { /* reset setpoints and integrals on arming */ _reset_pos_sp = true; @@ -1034,7 +1099,11 @@ MulticopterPositionControl::task_main() _vel(2) = _local_pos.vz; _vel_ff.zero(); - _sp_move_rate.zero(); + + /* by default, run position/altitude controller. the control_* functions + * can disable this and run velocity controllers directly in this cycle */ + _run_pos_control = true; + _run_alt_control = true; /* select control source */ if (_control_mode.flag_control_manual_enabled) { @@ -1074,10 +1143,15 @@ MulticopterPositionControl::task_main() } } else { - /* run position & altitude controllers, calculate velocity setpoint */ - math::Vector<3> pos_err = _pos_sp - _pos; + /* run position & altitude controllers, if enabled (otherwise use already computed velocity setpoints) */ + if (_run_pos_control) { + _vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0); + _vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1); + } - _vel_sp = pos_err.emult(_params.pos_p) + _vel_ff; + if (_run_alt_control) { + _vel_sp(2) = (_pos_sp(2) - _pos(2)) * _params.pos_p(2); + } /* make sure velocity setpoint is saturated in xy*/ float vel_norm_xy = sqrtf(_vel_sp(0)*_vel_sp(0) + @@ -1094,17 +1168,23 @@ MulticopterPositionControl::task_main() _vel_sp(2) = _vel_sp(2)*_params.vel_max(2)/vel_norm_z; } - if (!_control_mode.flag_control_altitude_enabled) { - _reset_alt_sp = true; - _vel_sp(2) = 0.0f; - } - if (!_control_mode.flag_control_position_enabled) { _reset_pos_sp = true; + } + + if (!_control_mode.flag_control_altitude_enabled) { + _reset_alt_sp = true; + } + + if (!_control_mode.flag_control_velocity_enabled) { _vel_sp(0) = 0.0f; _vel_sp(1) = 0.0f; } + if (!_control_mode.flag_control_climb_rate_enabled) { + _vel_sp(2) = 0.0f; + } + /* use constant descend rate when landing, ignore altitude setpoint */ if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { _vel_sp(2) = _params.land_speed; @@ -1161,8 +1241,12 @@ MulticopterPositionControl::task_main() /* velocity error */ math::Vector<3> vel_err = _vel_sp - _vel; - /* derivative of velocity error, not includes setpoint acceleration */ - math::Vector<3> vel_err_d = (_sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt; + /* derivative of velocity error, / + * does not includes setpoint acceleration */ + math::Vector<3> vel_err_d; + vel_err_d(0) = _vel_x_deriv.update(-_vel(0)); + vel_err_d(1) = _vel_y_deriv.update(-_vel(1)); + vel_err_d(2) = _vel_z_deriv.update(-_vel(2)); /* thrust vector in NED frame */ math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int; 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 d29d11c65d..0fa51f6f0c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -265,3 +265,49 @@ PARAM_DEFINE_FLOAT(MPC_MAN_P_MAX, 35.0f); */ PARAM_DEFINE_FLOAT(MPC_MAN_Y_MAX, 120.0f); +/** + * Deadzone of X,Y sticks where position hold is enabled + * + * @unit % + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_HOLD_XY_DZ, 0.1f); + +/** + * Deadzone of Z stick where altitude hold is enabled + * + * @unit % + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_HOLD_Z_DZ, 0.1f); + +/** + * Maximum horizontal velocity for which position hold is enabled (use 0 to disable check) + * + * @unit m/s + * @min 0.0 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_HOLD_MAX_XY, 0.5f); + +/** + * Maximum vertical velocity for which position hold is enabled (use 0 to disable check) + * + * @unit m/s + * @min 0.0 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_HOLD_MAX_Z, 0.5f); + +/** + * Low pass filter cut freq. for numerical velocity derivative + * + * @unit Hz + * @min 0.0 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_VELD_LP, 5.0f); diff --git a/src/modules/muorb/adsp/px4muorb.hpp b/src/modules/muorb/adsp/px4muorb.hpp index d509819570..1ec6b45566 100644 --- a/src/modules/muorb/adsp/px4muorb.hpp +++ b/src/modules/muorb/adsp/px4muorb.hpp @@ -37,22 +37,22 @@ extern "C" { -int px4muorb_orb_initialize() __EXPORT; + int px4muorb_orb_initialize() __EXPORT; -int px4muorb_add_subscriber(const char *name) __EXPORT; + int px4muorb_add_subscriber(const char *name) __EXPORT; -int px4muorb_remove_subscriber(const char *name) __EXPORT; + int px4muorb_remove_subscriber(const char *name) __EXPORT; -int px4muorb_send_topic_data(const char *name, const uint8_t *data, int data_len_in_bytes) __EXPORT; + int px4muorb_send_topic_data(const char *name, const uint8_t *data, int data_len_in_bytes) __EXPORT; -int px4muorb_is_subscriber_present(const char *topic_name, int *status) __EXPORT; + int px4muorb_is_subscriber_present(const char *topic_name, int *status) __EXPORT; -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) __EXPORT; + 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) __EXPORT; -int px4muorb_receive_bulk_data(uint8_t *_BulkTransferBuffer, int max_size_in_bytes, - int *length_in_bytes, int *topic_count) __EXPORT; + int px4muorb_receive_bulk_data(uint8_t *_BulkTransferBuffer, int max_size_in_bytes, + int *length_in_bytes, int *topic_count) __EXPORT; -int px4muorb_unblock_recieve_msg(void) __EXPORT; + int px4muorb_unblock_recieve_msg(void) __EXPORT; } diff --git a/src/modules/muorb/adsp/uORBFastRpcChannel.cpp b/src/modules/muorb/adsp/uORBFastRpcChannel.cpp index ffbf2ea049..82a3301322 100644 --- a/src/modules/muorb/adsp/uORBFastRpcChannel.cpp +++ b/src/modules/muorb/adsp/uORBFastRpcChannel.cpp @@ -183,6 +183,7 @@ int16_t uORB::FastRpcChannel::send_message(const char *messageName, int32_t leng } if ((unsigned long)DataQSize() < _min_q) { _min_q = (unsigned long)DataQSize(); } + if ((unsigned long)DataQSize() > _max_q) { _max_q = (unsigned long)DataQSize(); } _count++; @@ -367,6 +368,7 @@ int16_t uORB::FastRpcChannel::get_data if (rc != 0) { topic_name[0] = '\0'; } + /* PX4_DEBUG("GetData: %30s: t1: %lu t2: %lu t3: %lu", topic_name, (unsigned long)t1, (unsigned long)t2, (unsigned long)t3); @@ -464,9 +466,13 @@ int16_t uORB::FastRpcChannel::get_bulk_data hrt_abstime t3 = hrt_absolute_time(); if ((unsigned long)(t3 - t1) > _get_bulk_max) { _get_bulk_max = (unsigned long)(t3 - t1); } + if ((unsigned long)(t3 - t1) < _get_bulk_min) { _get_bulk_min = (unsigned long)(t3 - t1); } + if ((unsigned long)(*topic_count) > _bulk_topic_count_max) { _bulk_topic_count_max = (unsigned long)(*topic_count); } + if ((unsigned long)(*topic_count) < _bulk_topic_count_min) { _bulk_topic_count_min = (unsigned long)(*topic_count); } + if ((unsigned long)(t3 - check_time) > 10000000) { //PX4_DEBUG("GetData: t1: %lu t2: %lu t3: %lu", (unsigned long)t1, (unsigned long)t2, (unsigned long)t3); //PX4_DEBUG(".... dt1: %7lu dt2: %7lu Q: %d", (unsigned long)(t2 - t1), (unsigned long)(t3 - t2), DataQSize()); diff --git a/src/modules/muorb/krait-stubs/px4muorb_KraitRpcWrapper.hpp b/src/modules/muorb/krait-stubs/px4muorb_KraitRpcWrapper.hpp index d6feab2a47..f6b64b8e54 100644 --- a/src/modules/muorb/krait-stubs/px4muorb_KraitRpcWrapper.hpp +++ b/src/modules/muorb/krait-stubs/px4muorb_KraitRpcWrapper.hpp @@ -36,41 +36,41 @@ namespace px4muorb { - class KraitRpcWrapper; +class KraitRpcWrapper; } class px4muorb::KraitRpcWrapper { public: - /** - * Constructor - */ - KraitRpcWrapper() {} - - /** - * destructor - */ - ~KraitRpcWrapper() {} + /** + * Constructor + */ + KraitRpcWrapper() {} - /** - * Initiatizes the rpc channel px4 muorb - */ - bool Initialize() { return true; } + /** + * destructor + */ + ~KraitRpcWrapper() {} - /** - * Terminate to clean up the resources. This should be called at program exit - */ - bool Terminate() { return true; } + /** + * Initiatizes the rpc channel px4 muorb + */ + bool Initialize() { return true; } - /** - * Muorb related functions to pub/sub of orb topic from krait to adsp - */ - int32_t AddSubscriber( const char* topic ) { return 1; } - int32_t RemoveSubscriber( const char* topic ) { return 1; } - int32_t SendData( const char* topic, int32_t length_in_bytes, const uint8_t* data ) { return 1; } - int32_t ReceiveData( int32_t* msg_type, char** topic, int32_t* length_in_bytes, uint8_t** data ) { return 1; } - int32_t IsSubscriberPresent( const char* topic, int32_t* status ) { return 1; } - int32_t ReceiveBulkData( uint8_t** bulk_data, int32_t* length_in_bytes, int32_t* topic_count ) { return 1; } - int32_t UnblockReceiveData() { return 1; } + /** + * Terminate to clean up the resources. This should be called at program exit + */ + bool Terminate() { return true; } + + /** + * Muorb related functions to pub/sub of orb topic from krait to adsp + */ + int32_t AddSubscriber(const char *topic) { return 1; } + int32_t RemoveSubscriber(const char *topic) { return 1; } + int32_t SendData(const char *topic, int32_t length_in_bytes, const uint8_t *data) { return 1; } + int32_t ReceiveData(int32_t *msg_type, char **topic, int32_t *length_in_bytes, uint8_t **data) { return 1; } + int32_t IsSubscriberPresent(const char *topic, int32_t *status) { return 1; } + int32_t ReceiveBulkData(uint8_t **bulk_data, int32_t *length_in_bytes, int32_t *topic_count) { return 1; } + int32_t UnblockReceiveData() { return 1; } }; #endif // _px4muorb_KraitWrapper_hpp_ diff --git a/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp b/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp index 165705ee19..6e53f0bbfd 100644 --- a/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp +++ b/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp @@ -139,11 +139,11 @@ int16_t uORB::KraitFastRpcChannel::send_message(const char *messageName, int32_t if ((t3 - t2) > _snd_msg_max) { _snd_msg_max = (t3 - t2); } _snd_msg_avg = ((double)((_snd_msg_avg * (_snd_msg_count - 1)) + - (unsigned long)(t3 - t2))) / (double)(_snd_msg_count); + (unsigned long)(t3 - t2))) / (double)(_snd_msg_count); } _overall_snd_avg = ((double)((_overall_snd_avg * (_overall_snd_count - 1)) + - (unsigned long)(t4 - t1))) / (double)(_overall_snd_count); + (unsigned long)(t4 - t1))) / (double)(_overall_snd_count); if ((t4 - _log_check_time) > _log_check_interval) { /* diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 2ce37ef94d..94ded8a4b9 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -153,7 +153,7 @@ MissionBlock::is_mission_item_reached() if (_waypoint_position_reached && !_waypoint_yaw_reached) { /* TODO: removed takeoff, why? */ - if (_navigator->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) { + if (_navigator->get_vstatus()->is_rotary_wing && 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); diff --git a/src/modules/px4iofirmware/CMakeLists.txt b/src/modules/px4iofirmware/CMakeLists.txt index 7b4c5652c6..3319bec6a2 100644 --- a/src/modules/px4iofirmware/CMakeLists.txt +++ b/src/modules/px4iofirmware/CMakeLists.txt @@ -103,7 +103,7 @@ elseif(${config_io_board} STREQUAL "px4io-v2") ) endif() -set(fw_io_name ${config_io_board}_${LABEL}) +set(fw_io_name ${config_io_board}) add_executable(${fw_io_name} ${srcs}) diff --git a/src/modules/px4iofirmware/adc.c b/src/modules/px4iofirmware/adc.c index d3db26c233..1688d53048 100644 --- a/src/modules/px4iofirmware/adc.c +++ b/src/modules/px4iofirmware/adc.c @@ -96,19 +96,22 @@ adc_init(void) rCR2 |= ADC_CR2_RSTCAL; up_udelay(1); - if (rCR2 & ADC_CR2_RSTCAL) + if (rCR2 & ADC_CR2_RSTCAL) { return -1; + } rCR2 |= ADC_CR2_CAL; up_udelay(100); - if (rCR2 & ADC_CR2_CAL) + if (rCR2 & ADC_CR2_CAL) { return -1; + } + #endif /* * Configure sampling time. - * + * * For electrical protection reasons, we want to be able to have * 10K in series with ADC inputs that leave the board. At 12MHz this * means we need 28.5 cycles of sampling time (per table 43 in the diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index b0e96b1f03..fc6fbf82b8 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -90,8 +90,9 @@ static bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value) { - if (raw == 0xffff) + if (raw == 0xffff) { return false; + } *channel = (raw >> shift) & 0xf; @@ -132,18 +133,21 @@ dsm_guess_format(bool reset) unsigned channel, value; /* if the channel decodes, remember the assigned number */ - if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31)) + if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31)) { cs10 |= (1 << channel); + } - if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31)) + if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31)) { cs11 |= (1 << channel); + } /* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-dsm_frame format */ } /* wait until we have seen plenty of frames - 5 should normally be enough */ - if (samples++ < 5) + if (samples++ < 5) { return; + } /* * Iterate the set of sensible sniffed channel sets and see whether @@ -170,11 +174,13 @@ dsm_guess_format(bool reset) for (unsigned i = 0; i < (sizeof(masks) / sizeof(masks[0])); i++) { - if (cs10 == masks[i]) + if (cs10 == masks[i]) { votes10++; + } - if (cs11 == masks[i]) + if (cs11 == masks[i]) { votes11++; + } } if ((votes11 == 1) && (votes10 == 0)) { @@ -210,8 +216,9 @@ dsm_init(const char *device) POWER_SPEKTRUM(true); #endif - if (dsm_fd < 0) + if (dsm_fd < 0) { dsm_fd = open(device, O_RDONLY | O_NONBLOCK); + } if (dsm_fd >= 0) { @@ -251,13 +258,14 @@ void dsm_bind(uint16_t cmd, int pulses) { #if !defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2) - #warning DSM BIND NOT IMPLEMENTED ON UNKNOWN PLATFORM +#warning DSM BIND NOT IMPLEMENTED ON UNKNOWN PLATFORM #else const uint32_t usart1RxAsOutp = GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10; - if (dsm_fd < 0) + if (dsm_fd < 0) { return; + } switch (cmd) { @@ -297,6 +305,7 @@ dsm_bind(uint16_t cmd, int pulses) up_udelay(120); stm32_gpiowrite(usart1RxAsOutp, true); } + break; case dsm_bind_reinit_uart: @@ -306,6 +315,7 @@ dsm_bind(uint16_t cmd, int pulses) break; } + #endif } @@ -329,8 +339,9 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) * If we have lost signal for at least a second, reset the * format guessing heuristic. */ - if (((frame_time - dsm_last_frame_time) > 1000000) && (dsm_channel_shift != 0)) + if (((frame_time - dsm_last_frame_time) > 1000000) && (dsm_channel_shift != 0)) { dsm_guess_format(true); + } /* we have received something we think is a dsm_frame */ dsm_last_frame_time = frame_time; @@ -358,20 +369,24 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) uint16_t raw = (dp[0] << 8) | dp[1]; unsigned channel, value; - if (!dsm_decode_channel(raw, dsm_channel_shift, &channel, &value)) + if (!dsm_decode_channel(raw, dsm_channel_shift, &channel, &value)) { continue; + } /* ignore channels out of range */ - if (channel >= PX4IO_RC_INPUT_CHANNELS) + if (channel >= PX4IO_RC_INPUT_CHANNELS) { continue; + } /* update the decoded channel count */ - if (channel >= *num_values) + if (channel >= *num_values) { *num_values = channel + 1; + } /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding. */ - if (dsm_channel_shift == 10) + if (dsm_channel_shift == 10) { value *= 2; + } /* * Spektrum scaling is special. There are these basic considerations @@ -421,8 +436,9 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) * lines, so if we get a channel count of 13, we'll return 12 (the last * data index that is stable). */ - if (*num_values == 13) + if (*num_values == 13) { *num_values = 12; + } if (dsm_channel_shift == 11) { /* Set the 11-bit data indicator */ @@ -482,6 +498,7 @@ dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **by /* if the read failed for any reason, just give up here */ if (ret < 1) { return false; + } else { *n_bytes = ret; *bytes = &dsm_frame[dsm_partial_frame_count]; @@ -497,8 +514,9 @@ dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **by /* * If we don't have a full dsm frame, return */ - if (dsm_partial_frame_count < DSM_FRAME_SIZE) + if (dsm_partial_frame_count < DSM_FRAME_SIZE) { return false; + } /* * Great, it looks like we might have a dsm frame. Go ahead and diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c index 6d1d1fc2dd..74692a2675 100644 --- a/src/modules/px4iofirmware/i2c.c +++ b/src/modules/px4iofirmware/i2c.c @@ -120,7 +120,7 @@ interface_init(void) rCR1 = 0; /* set for DMA operation */ - rCR2 |= I2C_CR2_ITEVFEN |I2C_CR2_ITERREN | I2C_CR2_DMAEN; + rCR2 |= I2C_CR2_ITEVFEN | I2C_CR2_ITERREN | I2C_CR2_DMAEN; /* set the frequency value in CR2 */ rCR2 &= ~I2C_CR2_FREQ_MASK; @@ -128,8 +128,11 @@ interface_init(void) /* set divisor and risetime for fast mode */ uint16_t result = STM32_PCLK1_FREQUENCY / (400000 * 25); - if (result < 1) + + if (result < 1) { result = 1; + } + result = 3; rCCR &= ~I2C_CCR_CCR_MASK; rCCR |= I2C_CCR_DUTY | I2C_CCR_FS | result; @@ -163,7 +166,7 @@ i2c_reset(void) rCR1 = 0; /* set for DMA operation */ - rCR2 |= I2C_CR2_ITEVFEN |I2C_CR2_ITERREN | I2C_CR2_DMAEN; + rCR2 |= I2C_CR2_ITEVFEN | I2C_CR2_ITERREN | I2C_CR2_DMAEN; /* set the frequency value in CR2 */ rCR2 &= ~I2C_CR2_FREQ_MASK; @@ -171,8 +174,11 @@ i2c_reset(void) /* set divisor and risetime for fast mode */ uint16_t result = STM32_PCLK1_FREQUENCY / (400000 * 25); - if (result < 1) + + if (result < 1) { result = 1; + } + result = 3; rCCR &= ~I2C_CCR_CCR_MASK; rCCR |= I2C_CCR_DUTY | I2C_CCR_FS | result; @@ -203,13 +209,16 @@ i2c_interrupt(int irq, FAR void *context) case DIR_TX: i2c_tx_complete(); break; + case DIR_RX: i2c_rx_complete(); break; + default: /* not currently transferring - must be a new txn */ break; } + direction = DIR_NONE; } @@ -232,8 +241,9 @@ i2c_interrupt(int irq, FAR void *context) } /* clear any errors that might need it (this handles AF as well */ - if (sr1 & I2C_SR1_ERRORMASK) + if (sr1 & I2C_SR1_ERRORMASK) { rSR1 = 0; + } return 0; } @@ -248,13 +258,13 @@ i2c_rx_setup(void) */ rx_len = 0; stm32_dmasetup(rx_dma, (uintptr_t)&rDR, (uintptr_t)&rx_buf[0], sizeof(rx_buf), - DMA_CCR_CIRC | - DMA_CCR_MINC | - DMA_CCR_PSIZE_32BITS | - DMA_CCR_MSIZE_8BITS | - DMA_CCR_PRIMED); + DMA_CCR_CIRC | + DMA_CCR_MINC | + DMA_CCR_PSIZE_32BITS | + DMA_CCR_MSIZE_8BITS | + DMA_CCR_PRIMED); - stm32_dmastart(rx_dma, NULL, NULL, false); + stm32_dmastart(rx_dma, NULL, NULL, false); } static void @@ -269,8 +279,10 @@ i2c_rx_complete(void) /* work out how many registers are being written */ unsigned count = (rx_len - 2) / 2; + if (count > 0) { registers_set(selected_page, selected_offset, (const uint16_t *)&rx_buf[2], count); + } else { /* no registers written, must be an address cycle */ uint16_t *regs; @@ -278,9 +290,11 @@ i2c_rx_complete(void) /* work out which registers are being addressed */ int ret = registers_get(selected_page, selected_offset, ®s, ®_count); + if (ret == 0) { tx_buf = (uint8_t *)regs; tx_len = reg_count * 2; + } else { tx_buf = junk_buf; tx_len = sizeof(junk_buf); @@ -306,16 +320,16 @@ i2c_tx_setup(void) /* * Note that we configure DMA in circular mode; this means that a too-long * transfer will copy the buffer more than once, but that avoids us having - * to deal with bailing out of a transaction while the master is still + * to deal with bailing out of a transaction while the master is still * babbling at us. */ stm32_dmasetup(tx_dma, (uintptr_t)&rDR, (uintptr_t)&tx_buf[0], tx_len, - DMA_CCR_DIR | - DMA_CCR_CIRC | - DMA_CCR_MINC | - DMA_CCR_PSIZE_8BITS | - DMA_CCR_MSIZE_8BITS | - DMA_CCR_PRIMED); + DMA_CCR_DIR | + DMA_CCR_CIRC | + DMA_CCR_MINC | + DMA_CCR_PSIZE_8BITS | + DMA_CCR_MSIZE_8BITS | + DMA_CCR_PRIMED); stm32_dmastart(tx_dma, NULL, NULL, false); } diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index b608d5fc93..b15ec09610 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -97,12 +97,13 @@ mixer_tick(void) /* check that we are receiving fresh data from the FMU */ if ((system_state.fmu_data_received_time == 0) || - hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { + hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { /* too long without FMU input, time to go to failsafe */ if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { isr_debug(1, "AP RX timeout"); } + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK); r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST; @@ -122,7 +123,7 @@ mixer_tick(void) /* do not mix if RAW_PWM mode is on and FMU is good */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { /* don't actually mix anything - we already have raw PWM values */ source = MIX_NONE; @@ -130,28 +131,29 @@ mixer_tick(void) } else { if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { /* mix from FMU controls */ 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)) { + 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)) { - /* if allowed, mix from RC inputs directly */ + /* if allowed, mix from RC inputs directly */ source = MIX_OVERRIDE; - } 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)) { + + } 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; @@ -166,31 +168,32 @@ mixer_tick(void) * */ should_arm = ( - /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) - /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) - /* and FMU is armed */ && ( - ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) - /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ) - /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) - /* or failsafe was set manually */ || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) - ) - ); + /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + /* and FMU is armed */ && ( + ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) + /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) + /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) + /* or failsafe was set manually */ || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) + && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) + ) + ); should_arm_nothrottle = ( - /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) - /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) - /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)); + /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)); should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) - && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) - && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK); + && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK); /* * Check if failsafe termination is set - if yes, * set the force failsafe flag once entering the first * failsafe condition. */ - if ( /* if we have requested flight termination style failsafe (noreturn) */ + if ( /* if we have requested flight termination style failsafe (noreturn) */ (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) && /* and we ended up in a failsafe condition */ (source == MIX_FAILSAFE) && @@ -213,6 +216,7 @@ mixer_tick(void) */ if (source == MIX_FAILSAFE) { r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE; + } else { r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE); } @@ -244,7 +248,8 @@ mixer_tick(void) in_mixer = false; /* the pwm limit call takes care of out of band errors */ - pwm_limit_calc(should_arm, should_arm_nothrottle, mixed, r_setup_pwm_reverse, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + pwm_limit_calc(should_arm, should_arm_nothrottle, mixed, r_setup_pwm_reverse, r_page_servo_disarmed, + r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); /* clamp unused outputs to zero */ for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) { @@ -291,6 +296,7 @@ mixer_tick(void) if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) { sbus2_output(r_page_servos, PX4IO_SERVO_COUNT); + } else if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) { sbus1_output(r_page_servos, PX4IO_SERVO_COUNT); } @@ -324,10 +330,11 @@ mixer_callback(uintptr_t handle, switch (source) { case MIX_FMU: - if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS ) { + if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) { control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]); break; } + return -1; case MIX_OVERRIDE: @@ -335,17 +342,21 @@ mixer_callback(uintptr_t handle, control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); break; } + return -1; case MIX_OVERRIDE_FMU_OK: + /* FMU is ok but we are in override mode, use direct rc control for the available rc channels. The remaining channels are still controlled by the fmu */ if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) { control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); break; + } else if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) { control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]); break; } + return -1; case MIX_FAILSAFE: @@ -357,15 +368,15 @@ mixer_callback(uintptr_t handle, /* apply trim offsets for override channels */ 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_index == actuator_controls_s::INDEX_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_index == actuator_controls_s::INDEX_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_index == actuator_controls_s::INDEX_YAW) { control += REG_TO_FLOAT(r_setup_trim_yaw); } } @@ -373,6 +384,7 @@ mixer_callback(uintptr_t handle, /* limit output */ if (control > 1.0f) { control = 1.0f; + } else if (control < -1.0f) { control = -1.0f; } @@ -380,7 +392,7 @@ mixer_callback(uintptr_t handle, /* motor spinup phase - lock throttle to zero */ if ((pwm_limit.state == PWM_LIMIT_STATE_RAMP) || (should_arm_nothrottle && !should_arm)) { if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && - control_index == actuator_controls_s::INDEX_THROTTLE) { + control_index == actuator_controls_s::INDEX_THROTTLE) { /* limit the throttle output to zero during motor spinup, * as the motors cannot follow any demand yet */ @@ -391,7 +403,7 @@ mixer_callback(uintptr_t handle, /* only safety off, but not armed - set throttle as invalid */ if (should_arm_nothrottle && !should_arm) { if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && - control_index == actuator_controls_s::INDEX_THROTTLE) { + control_index == actuator_controls_s::INDEX_THROTTLE) { /* mark the throttle as invalid */ control = NAN_VALUE; } @@ -414,7 +426,7 @@ mixer_handle_text(const void *buffer, size_t length) { /* do not allow a mixer change while safety off and FMU armed */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && - (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { return 1; } @@ -444,7 +456,7 @@ mixer_handle_text(const void *buffer, size_t length) mixer_group.reset(); mixer_text_length = 0; - /* FALLTHROUGH */ + /* FALLTHROUGH */ case F2I_MIXER_ACTION_APPEND: isr_debug(2, "append %d", length); @@ -470,6 +482,7 @@ mixer_handle_text(const void *buffer, size_t length) /* only set mixer ok if no residual is left over */ if (resid == 0) { r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK; + } else { /* not yet reached the end of the mixer, set as not ok */ r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; @@ -497,13 +510,13 @@ mixer_handle_text(const void *buffer, size_t length) static void mixer_set_failsafe() { - /* + /* * Check if a custom failsafe value has been written, * or if the mixer is not ok and bail out. */ if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) || - !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { + !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { return; } diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 673da0e44d..1267b48853 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -46,7 +46,7 @@ * * The first two bytes of each write select a page and offset address * respectively. Subsequent reads and writes increment the offset within - * the page. + * the page. * * Some pages are read- or write-only. * @@ -56,7 +56,7 @@ * Writes to unimplemented registers are ignored. Reads from unimplemented * registers return undefined values. * - * As convention, values that would be floating point in other parts of + * As convention, values that would be floating point in other parts of * the PX4 system are expressed as signed integer values scaled by 10000, * e.g. control values range from -10000..10000. Use the REG_TO_SIGNED and * SIGNED_TO_REG macros to convert between register representation and @@ -66,7 +66,7 @@ * readable pages to be densely packed. Page numbers do not need to be * packed. * - * Definitions marked [1] are only valid on PX4IOv1 boards. Likewise, + * Definitions marked [1] are only valid on PX4IOv1 boards. Likewise, * [2] denotes definitions specific to the PX4IOv2 board. */ @@ -218,14 +218,14 @@ enum { /* DSM bind states */ dsm_bind_send_pulses, dsm_bind_reinit_uart }; - /* 8 */ +/* 8 */ #define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ #define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */ #define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */ #define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */ - /* storage space of 12 occupied by CRC */ +/* storage space of 12 occupied by CRC */ #define PX4IO_P_SETUP_FORCE_SAFETY_OFF 12 /* force safety switch into 'armed' (PWM enabled) state - this is a non-data write and hence index 12 can safely be used. */ @@ -339,8 +339,7 @@ struct IOPacket { #define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK) #define PKT_SIZE(_p) ((size_t)((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p)))) -static const uint8_t crc8_tab[256] __attribute__((unused)) = -{ +static const uint8_t crc8_tab[256] __attribute__((unused)) = { 0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15, 0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D, 0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65, @@ -383,8 +382,9 @@ crc_packet(struct IOPacket *pkt) uint8_t *p = (uint8_t *)pkt; uint8_t c = 0; - while (p < end) - c = crc8_tab[c ^ *(p++)]; + while (p < end) { + c = crc8_tab[c ^ * (p++)]; + } return c; } diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 94f1fc11ef..1ee50d5a7f 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -97,11 +97,12 @@ isr_debug(uint8_t level, const char *fmt, ...) if (level > r_page_setup[PX4IO_P_SETUP_SET_DEBUG]) { return; } + va_list ap; va_start(ap, fmt); vsnprintf(msg[msg_next_in], sizeof(msg[0]), fmt, ap); va_end(ap); - msg_next_in = (msg_next_in+1) % NUM_MSG; + msg_next_in = (msg_next_in + 1) % NUM_MSG; msg_counter++; } @@ -113,11 +114,14 @@ show_debug_messages(void) { if (msg_counter != last_msg_counter) { uint32_t n = msg_counter - last_msg_counter; - if (n > NUM_MSG) n = NUM_MSG; + + if (n > NUM_MSG) { n = NUM_MSG; } + last_msg_counter = msg_counter; + while (n--) { debug("%s", msg[msg_next_out]); - msg_next_out = (msg_next_out+1) % NUM_MSG; + msg_next_out = (msg_next_out + 1) % NUM_MSG; } } } @@ -135,7 +139,7 @@ ring_blink(void) #ifdef GPIO_LED4 if (/* IO armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) - /* and FMU is armed */ && (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + /* and FMU is armed */ && (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { LED_RING(1); return; } @@ -151,7 +155,7 @@ ring_blink(void) if (brightness_counter < max_brightness) { - bool on = ((on_counter * 100) / brightness_counter+1) <= ((brightness * 100) / max_brightness+1); + bool on = ((on_counter * 100) / brightness_counter + 1) <= ((brightness * 100) / max_brightness + 1); // XXX once led is PWM driven, // remove the ! in the line below @@ -195,7 +199,7 @@ static uint64_t reboot_time; */ void schedule_reboot(uint32_t time_delta_usec) { - reboot_time = hrt_absolute_time() + time_delta_usec; + reboot_time = hrt_absolute_time() + time_delta_usec; } /** @@ -203,9 +207,9 @@ void schedule_reboot(uint32_t time_delta_usec) */ static void check_reboot(void) { - if (reboot_time != 0 && hrt_absolute_time() > reboot_time) { - up_systemreset(); - } + if (reboot_time != 0 && hrt_absolute_time() > reboot_time) { + up_systemreset(); + } } static void @@ -215,12 +219,14 @@ calculate_fw_crc(void) #define APP_LOAD_ADDRESS 0x08001000 // compute CRC of the current firmware uint32_t sum = 0; + for (unsigned p = 0; p < APP_SIZE_MAX; p += 4) { uint32_t bytes = *(uint32_t *)(p + APP_LOAD_ADDRESS); sum = crc32part((uint8_t *)&bytes, sizeof(bytes), sum); } + r_page_setup[PX4IO_P_SETUP_CRC] = sum & 0xFFFF; - r_page_setup[PX4IO_P_SETUP_CRC+1] = sum >> 16; + r_page_setup[PX4IO_P_SETUP_CRC + 1] = sum >> 16; } int @@ -308,7 +314,7 @@ user_start(int argc, char *argv[]) * allocations. We don't want him to be able to * get past that point. This needs to be clearly * documented in the dev guide. - * + * */ if (minfo.mxordblk < 600) { @@ -320,10 +326,12 @@ user_start(int argc, char *argv[]) if (phase) { LED_AMBER(true); LED_BLUE(false); + } else { LED_AMBER(false); LED_BLUE(true); } + up_udelay(250000); phase = !phase; @@ -338,7 +346,8 @@ user_start(int argc, char *argv[]) */ uint64_t last_debug_time = 0; - uint64_t last_heartbeat_time = 0; + uint64_t last_heartbeat_time = 0; + for (;;) { /* track the rate at which the loop is running */ @@ -354,14 +363,14 @@ user_start(int argc, char *argv[]) controls_tick(); perf_end(controls_perf); - if ((hrt_absolute_time() - last_heartbeat_time) > 250*1000) { - last_heartbeat_time = hrt_absolute_time(); - heartbeat_blink(); - } + if ((hrt_absolute_time() - last_heartbeat_time) > 250 * 1000) { + last_heartbeat_time = hrt_absolute_time(); + heartbeat_blink(); + } ring_blink(); - check_reboot(); + check_reboot(); /* check for debug activity (default: none) */ show_debug_messages(); @@ -371,7 +380,7 @@ user_start(int argc, char *argv[]) */ if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) { - isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u", + isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG], (unsigned)r_status_flags, (unsigned)r_setup_arming, diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 8adc91cda9..46852003c8 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -112,15 +112,14 @@ uint16_t r_page_servos[PX4IO_SERVO_COUNT]; * * Raw RC input */ -uint16_t r_page_raw_rc_input[] = -{ +uint16_t r_page_raw_rc_input[] = { [PX4IO_P_RAW_RC_COUNT] = 0, [PX4IO_P_RAW_RC_FLAGS] = 0, [PX4IO_P_RAW_RC_NRSSI] = 0, [PX4IO_P_RAW_RC_DATA] = 0, [PX4IO_P_RAW_FRAME_COUNT] = 0, [PX4IO_P_RAW_LOST_FRAME_COUNT] = 0, - [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0 + [PX4IO_P_RAW_RC_BASE ...(PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0 }; /** @@ -130,7 +129,7 @@ uint16_t r_page_raw_rc_input[] = */ uint16_t r_page_rc_input[] = { [PX4IO_P_RC_VALID] = 0, - [PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + PX4IO_RC_MAPPED_CONTROL_CHANNELS)] = 0 + [PX4IO_P_RC_BASE ...(PX4IO_P_RC_BASE + PX4IO_RC_MAPPED_CONTROL_CHANNELS)] = 0 }; /** @@ -146,8 +145,7 @@ uint16_t r_page_scratch[32]; * * Setup registers */ -volatile uint16_t r_page_setup[] = -{ +volatile uint16_t r_page_setup[] = { #ifdef CONFIG_ARCH_BOARD_PX4IO_V2 /* default to RSSI ADC functionality */ [PX4IO_P_SETUP_FEATURES] = PX4IO_P_SETUP_FEATURES_ADC_RSSI, @@ -171,7 +169,7 @@ volatile uint16_t r_page_setup[] = #endif [PX4IO_P_SETUP_SET_DEBUG] = 0, [PX4IO_P_SETUP_REBOOT_BL] = 0, - [PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0, + [PX4IO_P_SETUP_CRC ...(PX4IO_P_SETUP_CRC + 1)] = 0, [PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = 0, [PX4IO_P_SETUP_PWM_REVERSE] = 0, [PX4IO_P_SETUP_TRIM_ROLL] = 0, @@ -181,23 +179,23 @@ volatile uint16_t r_page_setup[] = #ifdef CONFIG_ARCH_BOARD_PX4IO_V2 #define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | \ - PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \ - PX4IO_P_SETUP_FEATURES_ADC_RSSI | \ - PX4IO_P_SETUP_FEATURES_PWM_RSSI) + PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \ + PX4IO_P_SETUP_FEATURES_ADC_RSSI | \ + PX4IO_P_SETUP_FEATURES_PWM_RSSI) #else #define PX4IO_P_SETUP_FEATURES_VALID 0 #endif #define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \ - PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \ - PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \ - PX4IO_P_SETUP_ARMING_IO_ARM_OK | \ - PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \ - PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \ - PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \ - PX4IO_P_SETUP_ARMING_LOCKDOWN | \ - PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \ - PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE | \ - PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) + PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \ + PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \ + PX4IO_P_SETUP_ARMING_IO_ARM_OK | \ + PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \ + PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \ + PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \ + PX4IO_P_SETUP_ARMING_LOCKDOWN | \ + PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \ + PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE | \ + PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) #define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1) #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) @@ -230,7 +228,7 @@ uint16_t r_page_rc_input_config[PX4IO_RC_INPUT_CHANNELS * PX4IO_P_RC_CONFIG_STR * PAGE 105 * * Failsafe servo PWM values - * + * * Disable pulses as default. */ uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 }; @@ -265,7 +263,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num switch (page) { - /* handle bulk controls input */ + /* handle bulk controls input */ case PX4IO_PAGE_CONTROLS: /* copy channel data */ @@ -282,10 +280,10 @@ 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; - /* handle raw PWM input */ + /* handle raw PWM input */ case PX4IO_PAGE_DIRECT_PWM: /* copy channel data */ @@ -306,7 +304,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num break; - /* handle setup for servo failsafe values */ + /* handle setup for servo failsafe values */ case PX4IO_PAGE_FAILSAFE_PWM: /* copy channel data */ @@ -316,8 +314,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* ignore 0 */ } else if (*values < PWM_LOWEST_MIN) { r_page_servo_failsafe[offset] = PWM_LOWEST_MIN; + } else if (*values > PWM_HIGHEST_MAX) { r_page_servo_failsafe[offset] = PWM_HIGHEST_MAX; + } else { r_page_servo_failsafe[offset] = *values; } @@ -329,6 +329,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num num_values--; values++; } + break; case PX4IO_PAGE_CONTROL_MIN_PWM: @@ -340,8 +341,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* ignore 0 */ } else if (*values > PWM_HIGHEST_MIN) { r_page_servo_control_min[offset] = PWM_HIGHEST_MIN; + } else if (*values < PWM_LOWEST_MIN) { r_page_servo_control_min[offset] = PWM_LOWEST_MIN; + } else { r_page_servo_control_min[offset] = *values; } @@ -350,8 +353,9 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num num_values--; values++; } + break; - + case PX4IO_PAGE_CONTROL_MAX_PWM: /* copy channel data */ @@ -361,8 +365,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* ignore 0 */ } else if (*values > PWM_HIGHEST_MAX) { r_page_servo_control_max[offset] = PWM_HIGHEST_MAX; + } else if (*values < PWM_LOWEST_MAX) { r_page_servo_control_max[offset] = PWM_LOWEST_MAX; + } else { r_page_servo_control_max[offset] = *values; } @@ -371,10 +377,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num num_values--; values++; } + break; - case PX4IO_PAGE_DISARMED_PWM: - { + case PX4IO_PAGE_DISARMED_PWM: { /* flag for all outputs */ bool all_disarmed_off = true; @@ -384,12 +390,15 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num if (*values == 0) { /* 0 means disabling always PWM */ r_page_servo_disarmed[offset] = 0; + } else if (*values < PWM_LOWEST_MIN) { r_page_servo_disarmed[offset] = PWM_LOWEST_MIN; all_disarmed_off = false; + } else if (*values > PWM_HIGHEST_MAX) { r_page_servo_disarmed[offset] = PWM_HIGHEST_MAX; all_disarmed_off = false; + } else { r_page_servo_disarmed[offset] = *values; all_disarmed_off = false; @@ -403,6 +412,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num if (all_disarmed_off) { /* disable PWM output if disarmed */ r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE); + } else { /* enable PWM output always */ r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE; @@ -410,7 +420,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num } break; - /* handle text going to the mixer parser */ + /* handle text going to the mixer parser */ case PX4IO_PAGE_MIXERLOAD: /* do not change the mixer if FMU is armed and IO's safety is off * this state defines an active system. This check is done in the @@ -419,19 +429,25 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num return mixer_handle_text(values, num_values * sizeof(*values)); default: + /* avoid offset wrap */ - if ((offset + num_values) > 255) + if ((offset + num_values) > 255) { num_values = 255 - offset; + } /* iterate individual registers, set each in turn */ while (num_values--) { - if (registers_set_one(page, offset, *values)) + if (registers_set_one(page, offset, *values)) { return -1; + } + offset++; values++; } + break; } + return 0; } @@ -448,19 +464,22 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; case PX4IO_P_STATUS_FLAGS: - /* + + /* * Allow FMU override of arming state (to allow in-air restores), * but only if the arming state is not in sync on the IO side. */ if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { r_status_flags = value; } + break; default: /* just ignore writes to other registers in this page */ break; } + break; case PX4IO_PAGE_SETUP: @@ -472,36 +491,37 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) /* some of the options conflict - give S.BUS out precedence, then ADC RSSI, then PWM RSSI */ /* switch S.Bus output pin as needed */ - #ifdef ENABLE_SBUS_OUT +#ifdef ENABLE_SBUS_OUT ENABLE_SBUS_OUT(value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT)); /* disable the conflicting options with SBUS 1 */ if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT)) { value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | - PX4IO_P_SETUP_FEATURES_ADC_RSSI | - PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + PX4IO_P_SETUP_FEATURES_ADC_RSSI | + PX4IO_P_SETUP_FEATURES_SBUS2_OUT); } /* disable the conflicting options with SBUS 2 */ if (value & (PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) { value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | - PX4IO_P_SETUP_FEATURES_ADC_RSSI | - PX4IO_P_SETUP_FEATURES_SBUS1_OUT); + PX4IO_P_SETUP_FEATURES_ADC_RSSI | + PX4IO_P_SETUP_FEATURES_SBUS1_OUT); } - #endif + +#endif /* disable the conflicting options with ADC RSSI */ if (value & (PX4IO_P_SETUP_FEATURES_ADC_RSSI)) { value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | - PX4IO_P_SETUP_FEATURES_SBUS1_OUT | - PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + PX4IO_P_SETUP_FEATURES_SBUS1_OUT | + PX4IO_P_SETUP_FEATURES_SBUS2_OUT); } /* disable the conflicting options with PWM RSSI (without effect here, but for completeness) */ if (value & (PX4IO_P_SETUP_FEATURES_PWM_RSSI)) { value &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI | - PX4IO_P_SETUP_FEATURES_SBUS1_OUT | - PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + PX4IO_P_SETUP_FEATURES_SBUS1_OUT | + PX4IO_P_SETUP_FEATURES_SBUS2_OUT); } /* apply changes */ @@ -551,9 +571,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (value < 25) { value = 25; } + if (value > 400) { value = 400; } + pwm_configure_rates(r_setup_pwm_rates, value, r_setup_pwm_altrate); break; @@ -561,13 +583,16 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (value < 25) { value = 25; } + if (value > 400) { value = 400; } + pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value); break; #ifdef CONFIG_ARCH_BOARD_PX4IO_V1 + case PX4IO_P_SETUP_RELAYS: value &= PX4IO_P_SETUP_RELAYS_VALID; r_setup_relays = value; @@ -598,10 +623,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; } - // we schedule a reboot rather than rebooting - // immediately to allow the IO board to ACK - // the reboot command - schedule_reboot(100000); + // we schedule a reboot rather than rebooting + // immediately to allow the IO board to ACK + // the reboot command + schedule_reboot(100000); break; case PX4IO_P_SETUP_DSM: @@ -612,18 +637,21 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (value == PX4IO_FORCE_SAFETY_MAGIC) { r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF; } + break; case PX4IO_P_SETUP_FORCE_SAFETY_OFF: if (value == PX4IO_FORCE_SAFETY_MAGIC) { r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF; } + break; case PX4IO_P_SETUP_RC_THR_FAILSAFE_US: if (value > 650 && value < 2350) { r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = value; } + break; case PX4IO_P_SETUP_PWM_REVERSE: @@ -639,93 +667,103 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) default: return -1; } + break; case PX4IO_PAGE_RC_CONFIG: { - /** - * do not allow a RC config change while safety is off - */ - if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { - break; - } - - unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE; - unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE; - uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE]; - - if (channel >= PX4IO_RC_INPUT_CHANNELS) - return -1; - - /* disable the channel until we have a chance to sanity-check it */ - conf[PX4IO_P_RC_CONFIG_OPTIONS] &= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; - - switch (index) { - - case PX4IO_P_RC_CONFIG_MIN: - case PX4IO_P_RC_CONFIG_CENTER: - case PX4IO_P_RC_CONFIG_MAX: - case PX4IO_P_RC_CONFIG_DEADZONE: - case PX4IO_P_RC_CONFIG_ASSIGNMENT: - conf[index] = value; - break; - - case PX4IO_P_RC_CONFIG_OPTIONS: - value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID; - r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; - - /* clear any existing RC disabled flag */ - r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED); - - /* set all options except the enabled option */ - conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; - - /* should the channel be enabled? */ - /* this option is normally set last */ - if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { - uint8_t count = 0; - bool disabled = false; - - /* assert min..center..max ordering */ - if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) { - count++; - } - if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) { - count++; - } - if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) { - count++; - } - if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) { - count++; - } - /* assert deadzone is sane */ - if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) { - count++; - } - - if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) { - disabled = true; - } else if ((conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) && - (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] != PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH)) { - count++; - } - - /* sanity checks pass, enable channel */ - if (count) { - isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1)); - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK; - } else if (!disabled) { - conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; - } + /** + * do not allow a RC config change while safety is off + */ + if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { + break; } - break; - /* inner switch: case PX4IO_P_RC_CONFIG_OPTIONS */ + unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE; + unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE; + uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE]; + + if (channel >= PX4IO_RC_INPUT_CHANNELS) { + return -1; + } + + /* disable the channel until we have a chance to sanity-check it */ + conf[PX4IO_P_RC_CONFIG_OPTIONS] &= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + + switch (index) { + + case PX4IO_P_RC_CONFIG_MIN: + case PX4IO_P_RC_CONFIG_CENTER: + case PX4IO_P_RC_CONFIG_MAX: + case PX4IO_P_RC_CONFIG_DEADZONE: + case PX4IO_P_RC_CONFIG_ASSIGNMENT: + conf[index] = value; + break; + + case PX4IO_P_RC_CONFIG_OPTIONS: + value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID; + r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; + + /* clear any existing RC disabled flag */ + r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED); + + /* set all options except the enabled option */ + conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + + /* should the channel be enabled? */ + /* this option is normally set last */ + if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { + uint8_t count = 0; + bool disabled = false; + + /* assert min..center..max ordering */ + if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) { + count++; + } + + if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) { + count++; + } + + if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) { + count++; + } + + if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) { + count++; + } + + /* assert deadzone is sane */ + if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) { + count++; + } + + if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) { + disabled = true; + + } else if ((conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) && + (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] != PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH)) { + count++; + } + + /* sanity checks pass, enable channel */ + if (count) { + isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1)); + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK; + + } else if (!disabled) { + conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + } + } + + break; + /* inner switch: case PX4IO_P_RC_CONFIG_OPTIONS */ + + } + + break; + /* case PX4IO_RC_PAGE_CONFIG */ } - break; - /* case PX4IO_RC_PAGE_CONFIG */ - } case PX4IO_PAGE_TEST: switch (offset) { @@ -733,11 +771,13 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) LED_AMBER(value & 1); break; } + break; default: return -1; } + return 0; } @@ -787,6 +827,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val * */ unsigned counts = adc_measure(ADC_VBATT); + if (counts != 0xffff) { unsigned mV = (166460 + (counts * 45934)) / 10000; unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000; @@ -806,6 +847,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val configuration for their sensor */ unsigned counts = adc_measure(ADC_IBATT); + if (counts != 0xffff) { r_page_status[PX4IO_P_STATUS_IBATT] = counts; } @@ -815,6 +857,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val /* PX4IO_P_STATUS_VSERVO */ { unsigned counts = adc_measure(ADC_VSERVO); + if (counts != 0xffff) { // use 3:1 scaling on 3.3V ADC input unsigned mV = counts * 9900 / 4096; @@ -826,6 +869,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val /* PX4IO_P_STATUS_VRSSI */ { unsigned counts = adc_measure(ADC_RSSI); + if (counts != 0xffff) { // use 1:1 scaling on 3.3V ADC input unsigned mV = counts * 3300 / 4096; @@ -858,8 +902,10 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val case PX4IO_PAGE_PWM_INFO: memset(r_page_scratch, 0, sizeof(r_page_scratch)); - for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) + + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { r_page_scratch[PX4IO_RATE_MAP_BASE + i] = up_pwm_servo_get_rate_group(i); + } SELECT_PAGE(r_page_scratch); break; @@ -872,15 +918,19 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val case PX4IO_PAGE_CONFIG: SELECT_PAGE(r_page_config); break; + case PX4IO_PAGE_ACTUATORS: SELECT_PAGE(r_page_actuators); break; + case PX4IO_PAGE_SERVOS: SELECT_PAGE(r_page_servos); break; + case PX4IO_PAGE_RAW_RC_INPUT: SELECT_PAGE(r_page_raw_rc_input); break; + case PX4IO_PAGE_RC_INPUT: SELECT_PAGE(r_page_rc_input); break; @@ -889,24 +939,31 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val case PX4IO_PAGE_SETUP: SELECT_PAGE(r_page_setup); break; + case PX4IO_PAGE_CONTROLS: SELECT_PAGE(r_page_controls); break; + case PX4IO_PAGE_RC_CONFIG: SELECT_PAGE(r_page_rc_input_config); break; + case PX4IO_PAGE_DIRECT_PWM: SELECT_PAGE(r_page_servos); break; + case PX4IO_PAGE_FAILSAFE_PWM: SELECT_PAGE(r_page_servo_failsafe); break; + case PX4IO_PAGE_CONTROL_MIN_PWM: SELECT_PAGE(r_page_servo_control_min); break; + case PX4IO_PAGE_CONTROL_MAX_PWM: SELECT_PAGE(r_page_servo_control_max); break; + case PX4IO_PAGE_DISARMED_PWM: SELECT_PAGE(r_page_servo_disarmed); break; @@ -918,12 +975,13 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val #undef SELECT_PAGE #undef COPY_PAGE -last_page = page; -last_offset = offset; + last_page = page; + last_offset = offset; /* if the offset is at or beyond the end of the page, we have no data */ - if (offset >= *num_values) + if (offset >= *num_values) { return -1; + } /* correct the data pointer and count for the offset */ *values += offset; @@ -943,8 +1001,10 @@ pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate) /* get the channel mask for this rate group */ uint32_t mask = up_pwm_servo_get_rate_group(group); - if (mask == 0) + + if (mask == 0) { continue; + } /* all channels in the group must be either default or alt-rate */ uint32_t alt = map & mask; @@ -956,18 +1016,23 @@ pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate) r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR; return; } + } else { /* set it - errors here are unexpected */ if (alt != 0) { - if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_altrate) != OK) + if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_altrate) != OK) { r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR; + } + } else { - if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_defaultrate) != OK) + if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_defaultrate) != OK) { r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR; + } } } } } + r_setup_pwm_rates = map; r_setup_pwm_defaultrate = defaultrate; r_setup_pwm_altrate = altrate; diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c index 06f8733a52..f4ab74a45a 100644 --- a/src/modules/px4iofirmware/safety.c +++ b/src/modules/px4iofirmware/safety.c @@ -110,7 +110,7 @@ safety_check_button(void *arg) * length in all cases of the if/else struct below. */ if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && - (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) { + (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) { if (counter < ARM_COUNTER_THRESHOLD) { counter++; @@ -149,6 +149,7 @@ safety_check_button(void *arg) } else if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) { pattern = LED_PATTERN_FMU_ARMED; + } else if (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) { pattern = LED_PATTERN_FMU_OK_TO_ARM; @@ -176,6 +177,7 @@ failsafe_blink(void *arg) /* blink the failsafe LED if we don't have FMU input */ if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { failsafe = !failsafe; + } else { failsafe = false; } diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index e9adc8cd69..300ec0b784 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -131,14 +131,19 @@ interface_init(void) rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE; #if 0 /* keep this for signal integrity testing */ + for (;;) { while (!(rSR & USART_SR_TXE)) ; + rDR = 0xfa; + while (!(rSR & USART_SR_TXE)) ; + rDR = 0xa0; } + #endif /* configure RX DMA and return to listening state */ @@ -153,6 +158,7 @@ rx_handle_packet(void) /* check packet CRC */ uint8_t crc = dma_packet.crc; dma_packet.crc = 0; + if (crc != crc_packet(&dma_packet)) { perf_count(pc_crcerr); @@ -170,11 +176,13 @@ rx_handle_packet(void) if (registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], PKT_COUNT(dma_packet))) { perf_count(pc_regerr); dma_packet.count_code = PKT_CODE_ERROR; + } else { dma_packet.count_code = PKT_CODE_SUCCESS; } + return; - } + } if (PKT_CODE(dma_packet) == PKT_CODE_READ) { @@ -185,17 +193,22 @@ rx_handle_packet(void) if (registers_get(dma_packet.page, dma_packet.offset, ®isters, &count) < 0) { perf_count(pc_regerr); dma_packet.count_code = PKT_CODE_ERROR; + } else { /* constrain reply to requested size */ - if (count > PKT_MAX_REGS) + if (count > PKT_MAX_REGS) { count = PKT_MAX_REGS; - if (count > PKT_COUNT(dma_packet)) + } + + if (count > PKT_COUNT(dma_packet)) { count = PKT_COUNT(dma_packet); + } /* copy reply registers into DMA buffer */ memcpy((void *)&dma_packet.regs[0], registers, count * 2); dma_packet.count_code = count | PKT_CODE_SUCCESS; } + return; } @@ -250,16 +263,22 @@ serial_interrupt(int irq, void *context) (void)rDR; /* required to clear any of the interrupt status that brought us here */ if (sr & (USART_SR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */ - USART_SR_NE | /* noise error - we have lost a byte due to noise */ - USART_SR_FE)) { /* framing error - start/stop bit lost or line break */ + USART_SR_NE | /* noise error - we have lost a byte due to noise */ + USART_SR_FE)) { /* framing error - start/stop bit lost or line break */ perf_count(pc_errors); - if (sr & USART_SR_ORE) + + if (sr & USART_SR_ORE) { perf_count(pc_ore); - if (sr & USART_SR_NE) + } + + if (sr & USART_SR_NE) { perf_count(pc_ne); - if (sr & USART_SR_FE) + } + + if (sr & USART_SR_FE) { perf_count(pc_fe); + } /* send a line break - this will abort transmission/reception on the other end */ rCR1 |= USART_CR1_SBK; @@ -270,7 +289,7 @@ serial_interrupt(int irq, void *context) if (sr & USART_SR_IDLE) { - /* + /* * If we saw an error, don't bother looking at the packet - it should have * been aborted by the sender and will definitely be bad. Get the DMA reconfigured * ready for their retry. @@ -284,10 +303,11 @@ serial_interrupt(int irq, void *context) /* * The sender has stopped sending - this is probably the end of a packet. - * Check the received length against the length in the header to see if + * Check the received length against the length in the header to see if * we have something that looks like a packet. */ unsigned length = sizeof(dma_packet) - stm32_dmaresidual(rx_dma); + if ((length < 1) || (length < PKT_SIZE(dma_packet))) { /* it was too short - possibly truncated */ diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index c14afb322f..eb52ce8296 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -167,7 +167,8 @@ public: int start(); private: - static const unsigned _rc_max_chan_count = input_rc_s::RC_INPUT_MAX_CHANNELS; /**< maximum number of r/c channels we handle */ + static const unsigned _rc_max_chan_count = + input_rc_s::RC_INPUT_MAX_CHANNELS; /**< maximum number of r/c channels we handle */ /** * Get and limit value for specified RC function. Returns NAN if not mapped. @@ -227,7 +228,7 @@ private: orb_advert_t _battery_pub; /**< battery status */ orb_advert_t _airspeed_pub; /**< airspeed */ orb_advert_t _diff_pres_pub; /**< differential_pressure */ - + perf_counter_t _loop_perf; /**< loop performance counter */ struct rc_channels_s _rc; /**< r/c channel data */ @@ -366,7 +367,7 @@ private: int init_sensor_class(const struct orb_metadata *meta, int *subs, - uint32_t *priorities, uint32_t *errcount); + uint32_t *priorities, uint32_t *errcount); /** * Update our local parameter cache. @@ -489,10 +490,10 @@ Sensors::Sensors() : _armed(false), /* subscriptions */ - _gyro_sub{-1, -1, -1}, - _accel_sub{-1, -1, -1}, - _mag_sub{-1, -1, -1}, - _baro_sub{-1, -1, -1}, + _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), @@ -588,7 +589,8 @@ Sensors::Sensors() : /* RC to parameter mapping for changing parameters with RC */ for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { char name[rc_parameter_map_s::PARAM_ID_LEN]; - snprintf(name, rc_parameter_map_s::PARAM_ID_LEN, "RC_MAP_PARAM%d", i + 1); // shifted by 1 because param name starts at 1 + snprintf(name, rc_parameter_map_s::PARAM_ID_LEN, "RC_MAP_PARAM%d", + i + 1); // shifted by 1 because param name starts at 1 _parameter_handles.rc_map_param[i] = param_find(name); } @@ -641,7 +643,7 @@ Sensors::Sensors() : (void)param_find("PWM_AUX_MAX"); (void)param_find("PWM_AUX_DISARMED"); (void)param_find("TRIG_MODE"); - + /* fetch initial parameter values */ parameters_update(); } @@ -825,19 +827,20 @@ 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); + } else if (_parameters.battery_voltage_scaling < 0.0f) { /* apply scaling according to defaults if set to default */ - #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 _parameters.battery_voltage_scaling = 0.0082f; - #elif CONFIG_ARCH_BOARD_AEROCORE +#elif CONFIG_ARCH_BOARD_AEROCORE _parameters.battery_voltage_scaling = 0.0063f; - #elif CONFIG_ARCH_BOARD_PX4FMU_V2 +#elif CONFIG_ARCH_BOARD_PX4FMU_V2 _parameters.battery_voltage_scaling = 0.00459340659f; - #else +#else /* ensure a missing default trips a low voltage lockdown */ _parameters.battery_voltage_scaling = 0.00001f; - #endif +#endif } /* scaling of ADC ticks to battery current */ @@ -1098,9 +1101,11 @@ Sensors::gyro_poll(struct sensor_combined_s &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; } + raw.gyro_errcount[i] = gyro_report.error_count; raw.gyro_temp[i] = gyro_report.temperature; } @@ -1183,8 +1188,8 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar[0] * 1e2f, raw.baro_pres_mbar[0] * 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 + raw.baro_pres_mbar[0] * 1e2f, + raw.baro_pres_mbar[0] * 1e2f, air_temperature_celsius)); _airspeed.air_temperature_celsius = air_temperature_celsius; @@ -1265,7 +1270,7 @@ Sensors::parameter_update_poll(bool forced) bool config_ok = false; - /* run through all stored calibrations */ + /* run through all stored calibrations */ for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) { /* initially status is ok per config */ failed = false; @@ -1297,15 +1302,19 @@ Sensors::parameter_update_poll(bool forced) if (failed) { warnx(CAL_ERROR_APPLY_CAL_MSG, "gyro", i); + } else { /* apply new scaling and offsets */ res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale); + if (res) { warnx(CAL_ERROR_APPLY_CAL_MSG, "gyro", i); + } else { config_ok = true; } } + break; } } @@ -1331,7 +1340,7 @@ Sensors::parameter_update_poll(bool forced) bool config_ok = false; - /* run through all stored calibrations */ + /* run through all stored calibrations */ for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) { /* initially status is ok per config */ failed = false; @@ -1363,15 +1372,19 @@ Sensors::parameter_update_poll(bool forced) if (failed) { warnx(CAL_ERROR_APPLY_CAL_MSG, "accel", i); + } else { /* apply new scaling and offsets */ res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&gscale); + if (res) { warnx(CAL_ERROR_APPLY_CAL_MSG, "accel", i); + } else { config_ok = true; } } + break; } } @@ -1404,7 +1417,7 @@ Sensors::parameter_update_poll(bool forced) bool config_ok = false; - /* run through all stored calibrations */ + /* run through all stored calibrations */ for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) { /* initially status is ok per config */ failed = false; @@ -1444,6 +1457,7 @@ Sensors::parameter_update_poll(bool forced) /* reset param to -1 to indicate internal mag */ int32_t minus_one = MAG_ROT_VAL_INTERNAL; param_set_no_notification(param_find(str), &minus_one); + } else { int32_t mag_rot; @@ -1488,15 +1502,19 @@ Sensors::parameter_update_poll(bool forced) if (failed) { warnx(CAL_ERROR_APPLY_CAL_MSG, "mag", i); + } else { /* apply new scaling and offsets */ res = px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&gscale); + if (res) { warnx(CAL_ERROR_APPLY_CAL_MSG, "mag", i); + } else { config_ok = true; } } + break; } } @@ -1764,6 +1782,7 @@ Sensors::set_params_from_rc() } float rc_val = get_rc_value((rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i), -1.0, 1.0); + /* Check if the value has changed, * maybe we need to introduce a more aggressive limit here */ if (rc_val > _param_rc_values[i] + FLT_EPSILON || rc_val < _param_rc_values[i] - FLT_EPSILON) { @@ -1895,24 +1914,30 @@ Sensors::rc_poll() manual.timestamp = rc_input.timestamp_last_signal; /* limit controls */ - manual.y = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0); - manual.x = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0); - manual.r = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0); - manual.z = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0); - manual.flaps = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0); - manual.aux1 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0); - manual.aux2 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0); - manual.aux3 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0); - manual.aux4 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0); - manual.aux5 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0); + manual.y = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0); + manual.x = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0); + manual.r = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0); + manual.z = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0); + manual.flaps = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0); + manual.aux1 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0); + manual.aux2 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0); + manual.aux3 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0); + manual.aux4 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0); + manual.aux5 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0); /* mode switches */ - manual.mode_switch = get_rc_sw3pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv); - manual.posctl_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv); - manual.return_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th, _parameters.rc_return_inv); - manual.loiter_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv); - manual.acro_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv); - manual.offboard_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv); + manual.mode_switch = get_rc_sw3pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th, + _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv); + manual.posctl_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th, + _parameters.rc_posctl_inv); + manual.return_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th, + _parameters.rc_return_inv); + manual.loiter_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th, + _parameters.rc_loiter_inv); + manual.acro_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th, + _parameters.rc_acro_inv); + manual.offboard_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD, + _parameters.rc_offboard_th, _parameters.rc_offboard_inv); /* publish manual_control_setpoint topic */ if (_manual_control_pub != nullptr) { @@ -1964,7 +1989,7 @@ Sensors::task_main_trampoline(int argc, char *argv[]) int Sensors::init_sensor_class(const struct orb_metadata *meta, int *subs, - uint32_t *priorities, uint32_t *errcount) + uint32_t *priorities, uint32_t *errcount) { unsigned group_count = orb_group_count(meta); @@ -1975,7 +2000,7 @@ 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]); + orb_priority(subs[i], (int32_t *)&priorities[i]); } } @@ -1988,27 +2013,40 @@ Sensors::task_main() /* start individual sensors */ int ret = 0; + do { /* create a scope to handle exit with break */ ret = accel_init(); - if (ret) break; + + if (ret) { break; } + ret = gyro_init(); - if (ret) break; + + if (ret) { break; } + ret = mag_init(); - if (ret) break; + + if (ret) { break; } + ret = baro_init(); - if (ret) break; + + if (ret) { break; } + ret = adc_init(); - if (ret) break; + + if (ret) { break; } + break; } while (0); if (ret) { warnx("sensor initialization failed"); _sensors_task = -1; - if (_fd_adc >=0) { + + if (_fd_adc >= 0) { px4_close(_fd_adc); _fd_adc = -1; } + return; } @@ -2016,28 +2054,31 @@ Sensors::task_main() /* 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"); + "SENSOR_COUNT_MAX larger than sensor_combined datastructure fields. Overflow would occur"); /* * do subscriptions */ unsigned gcount_prev = _gyro_count; + unsigned mcount_prev = _mag_count; + unsigned acount_prev = _accel_count; + 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]); + &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]); + &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]); + &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]); + &raw.baro_priority[0], &raw.baro_errcount[0]); if (gcount_prev != _gyro_count || mcount_prev != _mag_count || @@ -2130,6 +2171,7 @@ Sensors::task_main() /* if the secondary failed as well, go to the tertiary */ if (hrt_elapsed_time(&raw.gyro_timestamp[1]) > 20 * 1000) { fds[0].fd = _gyro_sub[2]; + } else { fds[0].fd = _gyro_sub[1]; } @@ -2150,16 +2192,16 @@ Sensors::task_main() */ 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]); + &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]); + &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]); + &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]); + &raw.baro_priority[0], &raw.baro_errcount[0]); _last_config_update = hrt_absolute_time(); @@ -2190,11 +2232,11 @@ Sensors::start() /* start the task */ _sensors_task = px4_task_spawn_cmd("sensors", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - (px4_main_t)&Sensors::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + (px4_main_t)&Sensors::task_main_trampoline, + nullptr); /* wait until the task is up and running or has failed */ while (_sensors_task > 0 && _task_should_exit) { diff --git a/src/modules/simulator/CMakeLists.txt b/src/modules/simulator/CMakeLists.txt index de452c5de4..0a7e221086 100644 --- a/src/modules/simulator/CMakeLists.txt +++ b/src/modules/simulator/CMakeLists.txt @@ -47,6 +47,8 @@ px4_add_module( ${SIMULATOR_SRCS} DEPENDS platforms__common + git_jmavsim + git_gazebo ) # vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index a930d78c80..f99dd20450 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -769,7 +769,7 @@ int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu) return OK; } -int Simulator::publish_flow_topic(mavlink_hil_optical_flow_t* flow_mavlink) +int Simulator::publish_flow_topic(mavlink_hil_optical_flow_t *flow_mavlink) { uint64_t timestamp = hrt_absolute_time(); @@ -794,10 +794,10 @@ int Simulator::publish_flow_topic(mavlink_hil_optical_flow_t* flow_mavlink) flow.quality = flow_mavlink->quality; if (_flow_pub == nullptr) { - _flow_pub = orb_advertise(ORB_ID(optical_flow), &flow); + _flow_pub = orb_advertise(ORB_ID(optical_flow), &flow); } else { - orb_publish(ORB_ID(optical_flow), _flow_pub, &flow); + orb_publish(ORB_ID(optical_flow), _flow_pub, &flow); } } diff --git a/src/modules/systemlib/circuit_breaker_params.c b/src/modules/systemlib/circuit_breaker_params.c index 36b35246db..003b73bb62 100644 --- a/src/modules/systemlib/circuit_breaker_params.c +++ b/src/modules/systemlib/circuit_breaker_params.c @@ -133,4 +133,17 @@ PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953); * @max 240024 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024); \ No newline at end of file +PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024); + +/** + * Circuit breaker for disabling buzzer + * + * Setting this parameter to 782097 will disable the buzzer audio notification. + * + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 782097 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_BUZZER, 0); diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 766954c64a..38074f7ae5 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -315,8 +315,13 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) // scale yaw if it violates limits. inform about yaw limit reached if (out < 0.0f) { - yaw = -((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) * - roll_pitch_scale + thrust + boost) / _rotors[i].yaw_scale; + if (fabsf(_rotors[i].yaw_scale) <= FLT_EPSILON) { + yaw = 0.0f; + + } else { + yaw = -((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) * + roll_pitch_scale + thrust + boost) / _rotors[i].yaw_scale; + } if (status_reg != NULL) { (*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT; @@ -326,8 +331,14 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) // allow to reduce thrust to get some yaw response float thrust_reduction = fminf(0.15f, out - 1.0f); thrust -= thrust_reduction; - yaw = (1.0f - ((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) * - roll_pitch_scale + thrust + boost)) / _rotors[i].yaw_scale; + + if (fabsf(_rotors[i].yaw_scale) <= FLT_EPSILON) { + yaw = 0.0f; + + } else { + yaw = (1.0f - ((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) * + roll_pitch_scale + thrust + boost)) / _rotors[i].yaw_scale; + } if (status_reg != NULL) { (*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT; diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index bd8ecf13b4..e979e3b4f4 100644 --- a/src/modules/uORB/Publication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -50,37 +50,63 @@ #include "topics/encoders.h" #include "topics/tecs_status.h" #include "topics/rc_channels.h" +#include "topics/filtered_bottom_flow.h" + +#include namespace uORB { -template -Publication::Publication( - const struct orb_metadata *meta, - List *list) : - T(), // initialize data structure to zero - PublicationNode(meta, list) +PublicationBase::PublicationBase(const struct orb_metadata *meta, + int priority) : + _meta(meta), + _priority(priority), + _instance(), + _handle(nullptr) { } -template -Publication::~Publication() {} - -template -void *Publication::getDataVoidPtr() +void PublicationBase::update(void *data) { - return (void *)(T *)(this); + if (_handle != nullptr) { + int ret = orb_publish(getMeta(), getHandle(), data); + + if (ret != PX4_OK) { warnx("publish fail"); } + + } else { + orb_advert_t handle; + + if (_priority > 0) { + handle = orb_advertise_multi( + getMeta(), data, + &_instance, _priority); + + } else { + handle = orb_advertise(getMeta(), data); + } + + if (int64_t(handle) != PX4_ERROR) { + setHandle(handle); + + } else { + warnx("advert fail"); + } + } } +PublicationBase::~PublicationBase() +{ +} PublicationNode::PublicationNode(const struct orb_metadata *meta, + int priority, List *list) : - PublicationBase(meta) + PublicationBase(meta, priority) { if (list != nullptr) { list->add(this); } } - +// explicit template instantiation template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; @@ -94,5 +120,6 @@ 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/Publication.hpp b/src/modules/uORB/Publication.hpp index f8af00e96d..37658721ca 100644 --- a/src/modules/uORB/Publication.hpp +++ b/src/modules/uORB/Publication.hpp @@ -42,13 +42,13 @@ #include #include - +#include namespace uORB { /** - * Base publication warapper class, used in list traversal + * Base publication wrapper class, used in list traversal * of various publications. */ class __EXPORT PublicationBase @@ -58,50 +58,40 @@ public: /** * Constructor * - * - * @param meta The uORB metadata (usually from the ORB_ID() - * macro) for the topic. + * @param meta The uORB metadata (usually from + * the ORB_ID() macro) for the topic. + * @param priority The priority for multi pub/sub, 0-based, -1 means + * don't publish as multi */ - PublicationBase(const struct orb_metadata *meta) : - _meta(meta), - _handle(nullptr) - { - } + PublicationBase(const struct orb_metadata *meta, + int priority = -1); /** * Update the struct * @param data The uORB message struct we are updating. */ - void update(void *data) - { - if (_handle != nullptr) { - orb_publish(getMeta(), getHandle(), data); - - } else { - setHandle(orb_advertise(getMeta(), data)); - } - } + void update(void *data); /** * Deconstructor */ - virtual ~PublicationBase() - { - } + virtual ~PublicationBase(); + // accessors const struct orb_metadata *getMeta() { return _meta; } orb_advert_t getHandle() { return _handle; } protected: + // disallow copy + PublicationBase(const PublicationBase &other); + // disallow assignment + PublicationBase &operator=(const PublicationBase &other); // accessors void setHandle(orb_advert_t handle) { _handle = handle; } // attributes const struct orb_metadata *_meta; + int _priority; + int _instance; orb_advert_t _handle; -private: - // forbid copy - PublicationBase(const PublicationBase &) : _meta(), _handle() {}; - // forbid assignment - PublicationBase &operator = (const PublicationBase &); }; /** @@ -121,13 +111,14 @@ public: /** * Constructor * - * - * @param meta The uORB metadata (usually from the ORB_ID() - * macro) for the topic. - * @param list A pointer to a list of subscriptions - * that this should be appended to. + * @param meta The uORB metadata (usually from + * the ORB_ID() macro) for the topic. + * @param priority The priority for multi pub, 0-based. + * @param list A list interface for adding to + * list during construction */ PublicationNode(const struct orb_metadata *meta, + int priority = -1, List *list = nullptr); /** @@ -142,7 +133,6 @@ public: */ template class __EXPORT Publication : - public T, // this must be first! public PublicationNode { public: @@ -151,33 +141,37 @@ public: * * @param meta The uORB metadata (usually from * the ORB_ID() macro) for the topic. + * @param priority The priority for multi pub, 0-based. * @param list A list interface for adding to * list during construction */ Publication(const struct orb_metadata *meta, - List *list = nullptr); + int priority = -1, + List *list = nullptr) : + PublicationNode(meta, priority, list), + _data() + { + } /** * Deconstructor **/ - virtual ~Publication(); + virtual ~Publication() {}; /* - * XXX - * This function gets the T struct, assuming - * the struct is the first base class, this - * should use dynamic cast, but doesn't - * seem to be available - */ - void *getDataVoidPtr(); + * This function gets the T struct + * */ + T &get() { return _data; } /** * Create an update function that uses the embedded struct. */ void update() { - PublicationBase::update(getDataVoidPtr()); + PublicationBase::update((void *)(&_data)); } +private: + T _data; }; } // namespace uORB diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index 3554b497d7..a85e8da02d 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -54,37 +54,89 @@ #include "topics/vehicle_attitude_setpoint.h" #include "topics/vehicle_rates_setpoint.h" #include "topics/rc_channels.h" +#include "topics/battery_status.h" +#include "topics/optical_flow.h" +#include "topics/distance_sensor.h" +#include "topics/home_position.h" #include "topics/vehicle_control_mode.h" #include "topics/actuator_armed.h" +#include "topics/att_pos_mocap.h" +#include "topics/vision_position_estimate.h" + +#include namespace uORB { -template -Subscription::Subscription( - const struct orb_metadata *meta, - unsigned interval, - List *list) : - T(), // initialize data structure to zero - SubscriptionNode(meta, interval, list) +SubscriptionBase::SubscriptionBase(const struct orb_metadata *meta, + unsigned interval, unsigned instance) : + _meta(meta), + _instance(instance), + _handle() +{ + if (_instance > 0) { + _handle = orb_subscribe_multi( + getMeta(), instance); + + } else { + _handle = orb_subscribe(getMeta()); + } + + if (_handle < 0) { warnx("sub failed"); } + + orb_set_interval(getHandle(), interval); +} + +bool SubscriptionBase::updated() +{ + bool isUpdated = false; + int ret = orb_check(_handle, &isUpdated); + + if (ret != PX4_OK) { warnx("orb check failed"); } + + return isUpdated; +} + +void SubscriptionBase::update(void *data) +{ + if (updated()) { + int ret = orb_copy(_meta, _handle, data); + + if (ret != PX4_OK) { warnx("orb copy failed"); } + } +} + +SubscriptionBase::~SubscriptionBase() +{ + int ret = orb_unsubscribe(_handle); + + if (ret != PX4_OK) { warnx("orb unsubscribe failed"); } +} + +template +Subscription::Subscription(const struct orb_metadata *meta, + unsigned interval, + int instance, + List *list) : + SubscriptionNode(meta, interval, instance, list), + _data() // initialize data structure to zero { } -template -Subscription::~Subscription() {} - -template -void *Subscription::getDataVoidPtr() +template +Subscription::~Subscription() { - return (void *)(T *)(this); } -template -T Subscription::getData() +template +void Subscription::update() { - return T(*this); + SubscriptionBase::update((void *)(&_data)); } +template +const T &Subscription::get() { return _data; } + template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; @@ -104,5 +156,11 @@ 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; } // namespace uORB diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp index 31842ed715..0136cfcacc 100644 --- a/src/modules/uORB/Subscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -42,6 +42,7 @@ #include #include +#include namespace uORB { @@ -60,43 +61,29 @@ public: * * @param meta The uORB metadata (usually from the ORB_ID() * macro) for the topic. - * * @param interval The minimum interval in milliseconds * between updates + * @param instance The instance for multi sub. */ SubscriptionBase(const struct orb_metadata *meta, - unsigned interval=0) : - _meta(meta), - _handle() { - setHandle(orb_subscribe(getMeta())); - orb_set_interval(getHandle(), interval); - } + unsigned interval = 0, unsigned instance = 0); /** * Check if there is a new update. * */ - bool updated() { - bool isUpdated = false; - orb_check(_handle, &isUpdated); - return isUpdated; - } + bool updated(); /** * Update the struct * @param data The uORB message struct we are updating. */ - void update(void * data) { - if (updated()) { - orb_copy(_meta, _handle, data); - } - } + void update(void *data); /** * Deconstructor */ - virtual ~SubscriptionBase() { - orb_unsubscribe(_handle); - } + virtual ~SubscriptionBase(); + // accessors const struct orb_metadata *getMeta() { return _meta; } int getHandle() { return _handle; } @@ -105,12 +92,13 @@ protected: void setHandle(int handle) { _handle = handle; } // attributes const struct orb_metadata *_meta; + int _instance; int _handle; private: - // forbid copy - SubscriptionBase(const SubscriptionBase& other); - // forbid assignment - SubscriptionBase& operator = (const SubscriptionBase &); + // disallow copy + SubscriptionBase(const SubscriptionBase &other); + // disallow assignment + SubscriptionBase &operator=(const SubscriptionBase &other); }; /** @@ -119,7 +107,7 @@ private: typedef SubscriptionBase SubscriptionTiny; /** - * The publication base class as a list node. + * The subscription base class as a list node. */ class __EXPORT SubscriptionNode : @@ -130,20 +118,22 @@ public: /** * Constructor * - * * @param meta The uORB metadata (usually from the ORB_ID() * macro) for the topic. * @param interval The minimum interval in milliseconds * between updates + * @param instance The instance for multi sub. * @param list A pointer to a list of subscriptions * that this should be appended to. */ SubscriptionNode(const struct orb_metadata *meta, - unsigned interval=0, - List * list=nullptr) : - SubscriptionBase(meta, interval), - _interval(interval) { - if (list != nullptr) list->add(this); + unsigned interval = 0, + int instance = 0, + List *list = nullptr) : + SubscriptionBase(meta, interval, instance), + _interval(interval) + { + if (list != nullptr) { list->add(this); } } /** @@ -164,7 +154,6 @@ protected: */ template class __EXPORT Subscription : - public T, // this must be first! public SubscriptionNode { public: @@ -179,8 +168,10 @@ public: * list during construction */ Subscription(const struct orb_metadata *meta, - unsigned interval=0, - List * list=nullptr); + unsigned interval = 0, + int instance = 0, + List *list = nullptr); + /** * Deconstructor */ @@ -190,19 +181,14 @@ public: /** * Create an update function that uses the embedded struct. */ - void update() { - SubscriptionBase::update(getDataVoidPtr()); - } + void update(); /* - * XXX - * This function gets the T struct, assuming - * the struct is the first base class, this - * should use dynamic cast, but doesn't - * seem to be available - */ - void *getDataVoidPtr(); - T getData(); + * This function gets the T struct data + * */ + const T &get(); +private: + T _data; }; } // namespace uORB diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index 5a13a864c1..6d6f084d24 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -269,6 +269,7 @@ int orb_exists(const struct orb_metadata *meta, int instance) int orb_group_count(const struct orb_metadata *meta) { unsigned group_count = 0; + while (!uORB::Manager::get_instance()->orb_exists(meta, group_count++)) {}; return group_count; diff --git a/src/modules/uORB/uORBDevices_nuttx.cpp b/src/modules/uORB/uORBDevices_nuttx.cpp index 5d0ce3b99e..55f9d353f3 100644 --- a/src/modules/uORB/uORBDevices_nuttx.cpp +++ b/src/modules/uORB/uORBDevices_nuttx.cpp @@ -36,7 +36,6 @@ #include #include #include -#include #include "uORBDevices_nuttx.hpp" #include "uORBUtils.hpp" #include "uORBManager.hpp" @@ -684,4 +683,3 @@ uORB::DeviceNode *uORB::DeviceMaster::GetDeviceNode(const char *nodepath) return rc; } - diff --git a/src/modules/uORB/uORBMain.cpp b/src/modules/uORB/uORBMain.cpp index 8b2930eaef..360b95a724 100644 --- a/src/modules/uORB/uORBMain.cpp +++ b/src/modules/uORB/uORBMain.cpp @@ -45,83 +45,86 @@ extern "C" { __EXPORT int uorb_main(int argc, char *argv[]); } static uORB::DeviceMaster *g_dev = nullptr; static void usage() { - warnx("Usage: uorb 'start', 'test', 'latency_test' or 'status'"); + warnx("Usage: uorb 'start', 'test', 'latency_test' or 'status'"); } int uorb_main(int argc, char *argv[]) { - if (argc < 2) { - usage(); - return -EINVAL; - } + if (argc < 2) { + usage(); + return -EINVAL; + } - /* - * Start/load the driver. - * - * XXX it would be nice to have a wrapper for this... - */ - if (!strcmp(argv[1], "start")) { + /* + * Start/load the driver. + * + * XXX it would be nice to have a wrapper for this... + */ + if (!strcmp(argv[1], "start")) { - if (g_dev != nullptr) { - warnx("already loaded"); - /* user wanted to start uorb, its already running, no error */ - return 0; - } + if (g_dev != nullptr) { + warnx("already loaded"); + /* user wanted to start uorb, its already running, no error */ + return 0; + } - /* create the driver */ - g_dev = new uORB::DeviceMaster(uORB::PUBSUB); + /* create the driver */ + g_dev = new uORB::DeviceMaster(uORB::PUBSUB); - if (g_dev == nullptr) { - warnx("driver alloc failed"); - return -ENOMEM; - } + if (g_dev == nullptr) { + warnx("driver alloc failed"); + return -ENOMEM; + } - if (OK != g_dev->init()) { - warnx("driver init failed"); - delete g_dev; - g_dev = nullptr; - return -EIO; - } + if (OK != g_dev->init()) { + warnx("driver init failed"); + delete g_dev; + g_dev = nullptr; + return -EIO; + } - return OK; - } + return OK; + } #ifndef __PX4_QURT - /* - * Test the driver/device. - */ - if (!strcmp(argv[1], "test")) - { - uORBTest::UnitTest &t = uORBTest::UnitTest::instance(); - return t.test(); - } - /* - * Test the latency. - */ - if (!strcmp(argv[1], "latency_test")) { + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) { + 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); + } + } - 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. - */ - if (!strcmp(argv[1], "status")) - { - return OK; - } + /* + * Print driver information. + */ + if (!strcmp(argv[1], "status")) { + return OK; + } - usage(); - return -EINVAL; + usage(); + return -EINVAL; } diff --git a/src/modules/uORB/uORBUtils.cpp b/src/modules/uORB/uORBUtils.cpp index a18fa1453a..4b4e76c170 100644 --- a/src/modules/uORB/uORBUtils.cpp +++ b/src/modules/uORB/uORBUtils.cpp @@ -37,45 +37,46 @@ int uORB::Utils::node_mkpath ( - char *buf, - Flavor f, - const struct orb_metadata *meta, - int *instance + char *buf, + Flavor f, + const struct orb_metadata *meta, + int *instance ) { - unsigned len; + unsigned len; - unsigned index = 0; + unsigned index = 0; - if (instance != nullptr) { - index = *instance; - } + if (instance != nullptr) { + index = *instance; + } - len = snprintf(buf, orb_maxpath, "/%s/%s%d", - (f == PUBSUB) ? "obj" : "param", - meta->o_name, index); + len = snprintf(buf, orb_maxpath, "/%s/%s%d", + (f == PUBSUB) ? "obj" : "param", + meta->o_name, index); - if (len >= orb_maxpath) { - return -ENAMETOOLONG; - } + if (len >= orb_maxpath) { + return -ENAMETOOLONG; + } - return OK; + return OK; } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- int uORB::Utils::node_mkpath(char *buf, Flavor f, - const char *orbMsgName ) + const char *orbMsgName) { - unsigned len; + unsigned len; - unsigned index = 0; + unsigned index = 0; - len = snprintf(buf, orb_maxpath, "/%s/%s%d", (f == PUBSUB) ? "obj" : "param", - orbMsgName, index ); + len = snprintf(buf, orb_maxpath, "/%s/%s%d", (f == PUBSUB) ? "obj" : "param", + orbMsgName, index); - if (len >= orb_maxpath) - return -ENAMETOOLONG; + if (len >= orb_maxpath) { + return -ENAMETOOLONG; + } - return OK; + return OK; } diff --git a/src/modules/uORB/uORBUtils.hpp b/src/modules/uORB/uORBUtils.hpp index 522d255b26..4593f1a95e 100644 --- a/src/modules/uORB/uORBUtils.hpp +++ b/src/modules/uORB/uORBUtils.hpp @@ -38,24 +38,24 @@ namespace uORB { - class Utils; +class Utils; } class uORB::Utils { public: - static int node_mkpath - ( - char *buf, - Flavor f, - const struct orb_metadata *meta, - int *instance = nullptr - ); + static int node_mkpath + ( + char *buf, + Flavor f, + const struct orb_metadata *meta, + int *instance = nullptr + ); - /** - * same as above except this generators the path based on the string. - */ - static int node_mkpath(char *buf, Flavor f, const char *orbMsgName); + /** + * same as above except this generators the path based on the string. + */ + static int node_mkpath(char *buf, Flavor f, const char *orbMsgName); }; diff --git a/src/platforms/common/CMakeLists.txt b/src/platforms/common/CMakeLists.txt index 7b50768ac1..09defae9b4 100644 --- a/src/platforms/common/CMakeLists.txt +++ b/src/platforms/common/CMakeLists.txt @@ -37,10 +37,6 @@ set(depends git_uavcan ) -if(${OS} STREQUAL "posix" OR ${BOARD} STREQUAL "sim") - list(APPEND depends git_eigen) -endif() - px4_add_module( MODULE platforms__common SRCS diff --git a/src/platforms/common/px4_getopt.c b/src/platforms/common/px4_getopt.c index d7d03ed49f..863b5816b4 100644 --- a/src/platforms/common/px4_getopt.c +++ b/src/platforms/common/px4_getopt.c @@ -44,13 +44,19 @@ static char isvalidopt(char p, const char *options, int *takesarg) { int idx = 0; *takesarg = 0; - while (options[idx] != 0 && p != options[idx]) + + while (options[idx] != 0 && p != options[idx]) { ++idx; - if (options[idx] == 0) + } + + if (options[idx] == 0) { return '?'; - if (options[idx+1] == ':') { + } + + if (options[idx + 1] == ':') { *takesarg = 1; } + return options[idx]; } @@ -67,40 +73,50 @@ static int reorder(int argc, char **argv, const char *options) while (idx < argc && argv[idx] != 0) { if (argv[idx][0] == '-') { c = isvalidopt(argv[idx][1], options, &takesarg); - if (c == '?') + + if (c == '?') { return 1; + } + tmp_argv[tmpidx] = argv[idx]; tmpidx++; + if (takesarg) { - tmp_argv[tmpidx] = argv[idx+1]; + tmp_argv[tmpidx] = argv[idx + 1]; // printf("tmp_argv[%d] = %s\n", tmpidx, tmp_argv[tmpidx]); tmpidx++; idx++; } } + idx++; } // Add non-options to the end idx = 1; + while (idx < argc && argv[idx] != 0) { if (argv[idx][0] == '-') { c = isvalidopt(argv[idx][1], options, &takesarg); - if (c == '?') + + if (c == '?') { return c; + } + if (takesarg) { idx++; } - } - else { + + } else { tmp_argv[tmpidx] = argv[idx]; tmpidx++; } + idx++; } // Reorder argv - for (idx=1; idx < argc; idx++) { + for (idx = 1; idx < argc; idx++) { argv[idx] = tmp_argv[idx]; } @@ -128,24 +144,32 @@ __EXPORT int px4_getopt(int argc, char *argv[], const char *options, int *myopti int takesarg; if (*myoptind == 1) - if (reorder(argc, argv, options) != 0) + if (reorder(argc, argv, options) != 0) { return (int)'?'; + } p = argv[*myoptind]; - if (*myoptarg == 0) + if (*myoptarg == 0) { *myoptarg = argv[*myoptind]; + } if (p && options && myoptind && p[0] == '-') { c = isvalidopt(p[1], options, &takesarg); - if (c == '?') + + if (c == '?') { return (int)c; + } + *myoptind += 1; + if (takesarg) { *myoptarg = argv[*myoptind]; *myoptind += 1; } + return (int)c; } + return -1; } diff --git a/src/platforms/nuttx/px4_layer/px4_nuttx_tasks.c b/src/platforms/nuttx/px4_layer/px4_nuttx_tasks.c index 1849374fb2..bb4dea7d4a 100644 --- a/src/platforms/nuttx/px4_layer/px4_nuttx_tasks.c +++ b/src/platforms/nuttx/px4_layer/px4_nuttx_tasks.c @@ -70,13 +70,14 @@ px4_systemreset(bool to_bootloader) /* XXX wow, this is evil - write a magic number into backup register zero */ *(uint32_t *)0x40002850 = 0xb007b007; } + up_systemreset(); /* lock up here */ - while(true); + while (true); } -int px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, char * const argv[]) +int px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, char *const argv[]) { int pid; diff --git a/src/platforms/posix/drivers/accelsim/CMakeLists.txt b/src/platforms/posix/drivers/accelsim/CMakeLists.txt index e5b1a92147..afa59e515f 100644 --- a/src/platforms/posix/drivers/accelsim/CMakeLists.txt +++ b/src/platforms/posix/drivers/accelsim/CMakeLists.txt @@ -37,5 +37,6 @@ px4_add_module( accelsim.cpp DEPENDS platforms__common + git_jmavsim ) # vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index fa0a256605..1f9fa0da05 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -99,7 +99,7 @@ class ACCELSIM_mag; class ACCELSIM : public device::VDev { public: - ACCELSIM(const char* path, enum Rotation rotation); + ACCELSIM(const char *path, enum Rotation rotation); virtual ~ACCELSIM(); virtual int init(); @@ -310,8 +310,8 @@ private: int mag_set_samplerate(unsigned frequency); /* this class cannot be copied */ - ACCELSIM(const ACCELSIM&); - ACCELSIM operator=(const ACCELSIM&); + ACCELSIM(const ACCELSIM &); + ACCELSIM operator=(const ACCELSIM &); }; /* @@ -350,12 +350,12 @@ private: void measure_trampoline(void *arg); /* this class does not allow copying due to ptr data members */ - ACCELSIM_mag(const ACCELSIM_mag&); - ACCELSIM_mag operator=(const ACCELSIM_mag&); + ACCELSIM_mag(const ACCELSIM_mag &); + ACCELSIM_mag operator=(const ACCELSIM_mag &); }; -ACCELSIM::ACCELSIM(const char* path, enum Rotation rotation) : +ACCELSIM::ACCELSIM(const char *path, enum Rotation rotation) : VDev("ACCELSIM", path), _mag(new ACCELSIM_mag(this)), _accel_call{}, @@ -397,7 +397,7 @@ ACCELSIM::ACCELSIM(const char* path, enum Rotation rotation) : _debug_enabled = false; _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_ACCELSIM; - + /* Prime _mag with parents devid. */ _mag->_device_id.devid = _device_id.devid; _mag->_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_ACCELSIM; @@ -425,13 +425,17 @@ ACCELSIM::~ACCELSIM() stop(); /* free any existing reports */ - if (_accel_reports != nullptr) + if (_accel_reports != nullptr) { delete _accel_reports; - if (_mag_reports != nullptr) - delete _mag_reports; + } - if (_accel_class_instance != -1) + if (_mag_reports != nullptr) { + delete _mag_reports; + } + + if (_accel_class_instance != -1) { unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance); + } delete _mag; @@ -460,18 +464,21 @@ ACCELSIM::init() /* allocate basic report buffers */ _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); - if (_accel_reports == nullptr) + if (_accel_reports == nullptr) { goto out; + } _mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report)); - if (_mag_reports == nullptr) + if (_mag_reports == nullptr) { goto out; + } reset(); /* do VDev init for the mag device node */ ret = _mag->init(); + if (ret != OK) { PX4_WARN("MAG init failed"); goto out; @@ -485,7 +492,7 @@ ACCELSIM::init() /* measurement will have generated a report, publish */ _mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp, - &_mag->_mag_orb_class_instance, ORB_PRIO_LOW); + &_mag->_mag_orb_class_instance, ORB_PRIO_LOW); if (_mag->_mag_topic == nullptr) { PX4_WARN("ADVERT ERR"); @@ -499,7 +506,7 @@ ACCELSIM::init() /* measurement will have generated a report, publish */ _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, - &_accel_orb_class_instance, ORB_PRIO_DEFAULT); + &_accel_orb_class_instance, ORB_PRIO_DEFAULT); if (_accel_topic == nullptr) { PX4_WARN("ADVERT ERR"); @@ -522,16 +529,20 @@ ACCELSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) if (cmd & DIR_READ) { // Get data from the simulator Simulator *sim = Simulator::getInstance(); - if (sim == NULL) + + if (sim == NULL) { return ENODEV; + } // FIXME - not sure what interrupt status should be recv[1] = 0; + // skip cmd and status bytes if (cmd & ACC_READ) { - sim->getRawAccelReport(&recv[2], len-2); + sim->getRawAccelReport(&recv[2], len - 2); + } else if (cmd & MAG_READ) { - sim->getMagReport(&recv[2], len-2); + sim->getMagReport(&recv[2], len - 2); } } @@ -546,8 +557,9 @@ ACCELSIM::read(device::file_t *filp, char *buffer, size_t buflen) int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is enabled */ if (_call_accel_interval > 0) { @@ -569,8 +581,9 @@ ACCELSIM::read(device::file_t *filp, char *buffer, size_t buflen) measure(); /* measurement will have generated a report, copy it out */ - if (_accel_reports->get(arb)) + if (_accel_reports->get(arb)) { ret = sizeof(*arb); + } return ret; } @@ -583,8 +596,9 @@ ACCELSIM::mag_read(device::file_t *filp, char *buffer, size_t buflen) int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is enabled */ if (_call_mag_interval > 0) { @@ -608,8 +622,9 @@ ACCELSIM::mag_read(device::file_t *filp, char *buffer, size_t buflen) _mag->measure(); /* measurement will have generated a report, copy it out */ - if (_mag_reports->get(mrb)) + if (_mag_reports->get(mrb)) { ret = sizeof(*mrb); + } return ret; } @@ -620,7 +635,7 @@ ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) switch (cmd) { case SENSORIOCSPOLLRATE: { - switch (arg) { + switch (arg) { /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: @@ -642,52 +657,56 @@ ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) case SENSOR_POLLRATE_DEFAULT: return ioctl(filp, SENSORIOCSPOLLRATE, ACCELSIM_ACCEL_DEFAULT_RATE); - /* adjust to a legal polling interval in Hz */ + /* adjust to a legal polling interval in Hz */ default: { - /* do we need to start internal polling? */ - bool want_start = (_call_accel_interval == 0); + /* do we need to start internal polling? */ + bool want_start = (_call_accel_interval == 0); - /* convert hz to hrt interval via microseconds */ - unsigned period = 1000000 / arg; + /* convert hz to hrt interval via microseconds */ + unsigned period = 1000000 / arg; - /* check against maximum sane rate */ - if (period < 500) - return -EINVAL; + /* check against maximum sane rate */ + if (period < 500) { + return -EINVAL; + } - /* adjust filters */ - accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq()); + /* adjust filters */ + accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq()); - /* update interval for next measurement */ - /* XXX this is a bit shady, but no other way to adjust... */ - _accel_call.period = _call_accel_interval = period; + /* update interval for next measurement */ + /* XXX this is a bit shady, but no other way to adjust... */ + _accel_call.period = _call_accel_interval = period; - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } - return OK; + return OK; + } } } - } case SENSORIOCGPOLLRATE: - if (_call_accel_interval == 0) + if (_call_accel_interval == 0) { return SENSOR_POLLRATE_MANUAL; + } return 1000000 / _call_accel_interval; case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } - if (!_accel_reports->resize(arg)) { - return -ENOMEM; + if (!_accel_reports->resize(arg)) { + return -ENOMEM; + } + + return OK; } - return OK; - } - case SENSORIOCGQUEUEDEPTH: return _accel_reports->size(); @@ -702,20 +721,22 @@ ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) return _accel_samplerate; case ACCELIOCSLOWPASS: { - return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)arg); - } + return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)arg); + } case ACCELIOCSSCALE: { - /* copy scale, but only if off by a few percent */ - struct accel_scale *s = (struct accel_scale *) arg; - float sum = s->x_scale + s->y_scale + s->z_scale; - if (sum > 2.0f && sum < 4.0f) { - memcpy(&_accel_scale, s, sizeof(_accel_scale)); - return OK; - } else { - return -EINVAL; + /* copy scale, but only if off by a few percent */ + struct accel_scale *s = (struct accel_scale *) arg; + float sum = s->x_scale + s->y_scale + s->z_scale; + + if (sum > 2.0f && sum < 4.0f) { + memcpy(&_accel_scale, s, sizeof(_accel_scale)); + return OK; + + } else { + return -EINVAL; + } } - } case ACCELIOCSRANGE: /* arg needs to be in G */ @@ -723,7 +744,7 @@ ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) case ACCELIOCGRANGE: /* convert to m/s^2 and return rounded in G */ - return (unsigned long)((_accel_range_m_s2)/ACCELSIM_ONE_G + 0.5f); + return (unsigned long)((_accel_range_m_s2) / ACCELSIM_ONE_G + 0.5f); case ACCELIOCGSCALE: /* copy scale out */ @@ -745,7 +766,7 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) switch (cmd) { case SENSORIOCSPOLLRATE: { - switch (arg) { + switch (arg) { /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: @@ -775,8 +796,9 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) unsigned period = 1000000 / arg; /* check against maximum sane rate (1ms) */ - if (period < 10000) + if (period < 10000) { return -EINVAL; + } /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ @@ -785,8 +807,9 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) //PX4_INFO("SET _call_mag_interval=%u", _call_mag_interval); /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } @@ -794,23 +817,25 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_call_mag_interval == 0) + if (_call_mag_interval == 0) { return SENSOR_POLLRATE_MANUAL; + } return 1000000 / _call_mag_interval; case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } - if (!_mag_reports->resize(arg)) { - return -ENOMEM; + if (!_mag_reports->resize(arg)) { + return -ENOMEM; + } + + return OK; } - return OK; - } - case SENSORIOCGQUEUEDEPTH: return _mag_reports->size(); @@ -854,6 +879,7 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) case MAGIOCSELFTEST: return OK; + default: /* give it to the superclass */ return VDev::ioctl(filp, cmd, arg); @@ -888,7 +914,8 @@ void ACCELSIM::write_checked_reg(unsigned reg, uint8_t value) { write_reg(reg, value); - for (uint8_t i=0; imag_ioctl(filp, cmd, arg); + case DEVIOCGDEVICEID: + return (int)VDev::ioctl(filp, cmd, arg); + break; + + default: + return _parent->mag_ioctl(filp, cmd, arg); } } @@ -1311,8 +1342,9 @@ int start(enum Rotation rotation) { int fd, fd_mag; + if (g_dev != nullptr) { - PX4_WARN( "already started"); + PX4_WARN("already started"); return 0; } @@ -1339,7 +1371,7 @@ start(enum Rotation rotation) if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); - px4_close(fd); + px4_close(fd); goto fail; } @@ -1350,14 +1382,13 @@ start(enum Rotation rotation) if (px4_ioctl(fd_mag, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); } - } - else - { + + } else { PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); } - px4_close(fd); - px4_close(fd_mag); + px4_close(fd); + px4_close(fd_mag); return 0; fail: @@ -1405,7 +1436,7 @@ accelsim_main(int argc, char *argv[]) enum Rotation rotation = ROTATION_NONE; int ret; int myoptind = 1; - const char * myoptarg = NULL; + const char *myoptarg = NULL; /* jump over start/off/etc and look at options first */ while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { @@ -1413,6 +1444,7 @@ accelsim_main(int argc, char *argv[]) case 'R': rotation = (enum Rotation)atoi(myoptarg); break; + default: accelsim::usage(); return 0; @@ -1424,18 +1456,21 @@ accelsim_main(int argc, char *argv[]) /* * Start/load the driver. */ - if (!strcmp(verb, "start")) + if (!strcmp(verb, "start")) { ret = accelsim::start(rotation); + } /* * Print driver information. */ - else if (!strcmp(verb, "info")) + else if (!strcmp(verb, "info")) { ret = accelsim::info(); + } else { accelsim::usage(); return 1; } + return ret; } diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp index b7d4071111..d9f0dd4f15 100644 --- a/src/platforms/posix/drivers/adcsim/adcsim.cpp +++ b/src/platforms/posix/drivers/adcsim/adcsim.cpp @@ -37,7 +37,7 @@ * Driver for the ADCSIM. * * This is a designed for simulating sampling things like voltages - * and so forth. + * and so forth. */ #include @@ -83,7 +83,7 @@ protected: private: static const hrt_abstime _tickrate = 10000; /**< 100Hz base rate */ - + hrt_call _call; perf_counter_t _sample_perf; @@ -126,11 +126,13 @@ ADCSIM::ADCSIM(uint32_t channels) : _channel_count++; } } + _samples = new adc_msg_s[_channel_count]; /* prefill the channel numbers in the sample array */ if (_samples != nullptr) { unsigned index = 0; + for (unsigned i = 0; i < 32; i++) { if (channels & (1 << i)) { _samples[index].am_channel = i; @@ -143,8 +145,9 @@ ADCSIM::ADCSIM(uint32_t channels) : ADCSIM::~ADCSIM() { - if (_samples != nullptr) + if (_samples != nullptr) { delete _samples; + } } int @@ -167,8 +170,9 @@ ADCSIM::read(device::file_t *filp, char *buffer, size_t len) { const size_t maxsize = sizeof(adc_msg_s) * _channel_count; - if (len > maxsize) + if (len > maxsize) { len = maxsize; + } /* block interrupts while copying samples to avoid racing with an update */ memcpy(buffer, _samples, len); @@ -205,8 +209,10 @@ void ADCSIM::_tick() { /* scan the channel set and sample each */ - for (unsigned i = 0; i < _channel_count; i++) + for (unsigned i = 0; i < _channel_count; i++) { _samples[i].am_data = _sample(_samples[i].am_channel); + } + update_system_power(); } @@ -240,6 +246,7 @@ test(void) { int fd = px4_open(ADCSIM0_DEVICE_PATH, O_RDONLY); + if (fd < 0) { PX4_ERR("can't open ADCSIM device"); return 1; @@ -290,8 +297,9 @@ adcsim_main(int argc, char *argv[]) } if (argc > 1) { - if (!strcmp(argv[1], "test")) + if (!strcmp(argv[1], "test")) { ret = test(); + } } return ret; diff --git a/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp b/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp index 9cb0ac73d2..58f6aa9929 100644 --- a/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp +++ b/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp @@ -71,7 +71,7 @@ #include "airspeedsim.h" -AirspeedSim::AirspeedSim(int bus, int address, unsigned conversion_interval, const char* path) : +AirspeedSim::AirspeedSim(int bus, int address, unsigned conversion_interval, const char *path) : VDev("AIRSPEEDSIM", path), _reports(nullptr), _buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")), @@ -101,12 +101,14 @@ AirspeedSim::~AirspeedSim() /* make sure we are truly inactive */ stop(); - if (_class_instance != -1) + if (_class_instance != -1) { unregister_class_devname(AIRSPEED_BASE_DEVICE_PATH, _class_instance); + } /* free any existing reports */ - if (_reports != nullptr) + if (_reports != nullptr) { delete _reports; + } // free perf counters perf_free(_sample_perf); @@ -127,6 +129,7 @@ AirspeedSim::init() /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(differential_pressure_s)); + if (_reports == nullptr) { goto out; } @@ -145,8 +148,9 @@ AirspeedSim::init() /* measurement will have generated a report, publish */ _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp); - if (_airspeed_pub == nullptr) + if (_airspeed_pub == nullptr) { PX4_WARN("uORB started?"); + } } ret = OK; @@ -165,7 +169,7 @@ AirspeedSim::probe() _retries = 4; int ret = measure(); - // drop back to 2 retries once initialised + // drop back to 2 retries once initialised _retries = 2; return ret; } @@ -178,20 +182,20 @@ AirspeedSim::ioctl(device::file_t *filp, int cmd, unsigned long arg) case SENSORIOCSPOLLRATE: { switch (arg) { - /* switching to manual polling */ + /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _measure_ticks = 0; return OK; - /* external signalling (DRDY) not supported */ + /* external signalling (DRDY) not supported */ case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ + /* zero would be bad */ case 0: return -EINVAL; - /* set default/max polling rate */ + /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: { /* do we need to start internal polling? */ @@ -201,13 +205,14 @@ AirspeedSim::ioctl(device::file_t *filp, int cmd, unsigned long arg) _measure_ticks = USEC2TICK(_conversion_interval); /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } - /* adjust to a legal polling interval in Hz */ + /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ bool want_start = (_measure_ticks == 0); @@ -216,15 +221,17 @@ AirspeedSim::ioctl(device::file_t *filp, int cmd, unsigned long arg) unsigned long ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ - if (ticks < USEC2TICK(_conversion_interval)) + if (ticks < USEC2TICK(_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) + if (want_start) { start(); + } return OK; } @@ -232,21 +239,24 @@ AirspeedSim::ioctl(device::file_t *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) + 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)) + if ((arg < 1) || (arg > 100)) { return -EINVAL; + } //irqstate_t flags = irqsave(); if (!_reports->resize(arg)) { //irqrestore(flags); return -ENOMEM; } + //irqrestore(flags); return OK; @@ -260,16 +270,16 @@ AirspeedSim::ioctl(device::file_t *filp, int cmd, unsigned long arg) return -EINVAL; case AIRSPEEDIOCSSCALE: { - struct airspeed_scale *s = (struct airspeed_scale*)arg; - _diff_pres_offset = s->offset_pa; - return OK; + struct airspeed_scale *s = (struct airspeed_scale *)arg; + _diff_pres_offset = s->offset_pa; + return OK; } case AIRSPEEDIOCGSCALE: { - struct airspeed_scale *s = (struct airspeed_scale*)arg; - s->offset_pa = _diff_pres_offset; - s->scale = 1.0f; - return OK; + struct airspeed_scale *s = (struct airspeed_scale *)arg; + s->offset_pa = _diff_pres_offset; + s->scale = 1.0f; + return OK; } default: @@ -287,8 +297,9 @@ AirspeedSim::read(device::file_t *filp, char *buffer, size_t buflen) int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is enabled */ if (_measure_ticks > 0) { @@ -369,6 +380,7 @@ AirspeedSim::update_status() if (_subsys_pub != nullptr) { orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info); + } else { _subsys_pub = orb_advertise(ORB_ID(subsystem_info), &info); } @@ -402,21 +414,26 @@ AirspeedSim::print_info() void AirspeedSim::new_report(const differential_pressure_s &report) { - if (!_reports->force(&report)) + if (!_reports->force(&report)) { perf_count(_buffer_overflows); + } } int -AirspeedSim::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) { +AirspeedSim::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) +{ if (recv_len > 0) { // this is equivalent to the collect phase Simulator *sim = Simulator::getInstance(); + if (sim == NULL) { PX4_ERR("Error BARO_SIM::transfer no simulator"); return -ENODEV; } + PX4_DEBUG("BARO_SIM::transfer getting sample"); sim->getAirspeedSample(recv, recv_len); + } else { // we don't need measure phase } diff --git a/src/platforms/posix/drivers/airspeedsim/airspeedsim.h b/src/platforms/posix/drivers/airspeedsim/airspeedsim.h index 2c668fc97f..91db618d9b 100644 --- a/src/platforms/posix/drivers/airspeedsim/airspeedsim.h +++ b/src/platforms/posix/drivers/airspeedsim/airspeedsim.h @@ -127,7 +127,7 @@ protected: virtual int collect() = 0; virtual int transfer(const uint8_t *send, unsigned send_len, - uint8_t *recv, unsigned recv_len); + uint8_t *recv, unsigned recv_len); /** * Update the subsystem status diff --git a/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp b/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp index 3474b0bcaa..89691f7483 100644 --- a/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp +++ b/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp @@ -153,12 +153,12 @@ MEASAirspeedSim::collect() int ret = -EIO; /* read from the sensor */ - #pragma pack(push, 1) +#pragma pack(push, 1) struct { float temperature; float diff_pressure; } airspeed_report; - #pragma pack(pop) +#pragma pack(pop) perf_begin(_sample_perf); @@ -458,7 +458,7 @@ test() if (fd < 0) { PX4_ERR("%s open failed (try 'meas_airspeed_sim start' if the driver is not running", PATH_MS4525); - return 1; + return 1; } /* do a simple demand read */ diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index f69363fd40..975fa3687c 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -89,7 +89,7 @@ enum BAROSIM_BUS { class BAROSIM : public device::VDev { public: - BAROSIM(device::Device *interface, barosim::prom_u &prom_buf, const char* path); + BAROSIM(device::Device *interface, barosim::prom_u &prom_buf, const char *path); ~BAROSIM(); virtual int init(); @@ -195,7 +195,7 @@ protected: */ extern "C" __EXPORT int barosim_main(int argc, char *argv[]); -BAROSIM::BAROSIM(device::Device *interface, barosim::prom_u &prom_buf, const char* path) : +BAROSIM::BAROSIM(device::Device *interface, barosim::prom_u &prom_buf, const char *path) : VDev("BAROSIM", path), _interface(interface), _prom(prom_buf.s), @@ -224,12 +224,14 @@ BAROSIM::~BAROSIM() /* make sure we are truly inactive */ stop_cycle(); - if (_class_instance != -1) + if (_class_instance != -1) { unregister_class_devname(get_devname(), _class_instance); + } /* free any existing reports */ - if (_reports != nullptr) + if (_reports != nullptr) { delete _reports; + } // free perf counters perf_free(_sample_perf); @@ -247,6 +249,7 @@ BAROSIM::init() DEVICE_DEBUG("BAROSIM::init"); ret = VDev::init(); + if (ret != OK) { DEVICE_DEBUG("VDev init failed"); goto out; @@ -270,7 +273,7 @@ BAROSIM::init() _reports->flush(); _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp, - &_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT); + &_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT); if (_baro_topic == nullptr) { PX4_ERR("failed to create sensor_baro publication"); @@ -329,8 +332,9 @@ BAROSIM::read(device::file_t *filp, char *buffer, size_t buflen) int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is enabled */ if (_measure_ticks > 0) { @@ -383,8 +387,9 @@ BAROSIM::read(device::file_t *filp, char *buffer, size_t buflen) } /* state machine will have generated a report, copy it out */ - if (_reports->get(brp)) + if (_reports->get(brp)) { ret = sizeof(*brp); + } } while (0); @@ -396,23 +401,23 @@ BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { - case SENSORIOCSPOLLRATE: + case SENSORIOCSPOLLRATE: switch (arg) { - /* switching to manual polling */ + /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop_cycle(); _measure_ticks = 0; return OK; - /* external signalling not supported */ + /* external signalling not supported */ case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ + /* zero would be bad */ case 0: return -EINVAL; - /* set default/max polling rate */ + /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: { /* do we need to start internal polling? */ @@ -422,13 +427,14 @@ BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) _measure_ticks = USEC2TICK(BAROSIM_CONVERSION_INTERVAL); /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start_cycle(); + } return OK; } - /* adjust to a legal polling interval in Hz */ + /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ bool want_start = (_measure_ticks == 0); @@ -437,36 +443,41 @@ BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) unsigned long ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ - if (ticks < USEC2TICK(BAROSIM_CONVERSION_INTERVAL)) + if (ticks < USEC2TICK(BAROSIM_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) + if (want_start) { start_cycle(); + } return OK; } } case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) + 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; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } - if (!_reports->resize(arg)) { - return -ENOMEM; + if (!_reports->resize(arg)) { + return -ENOMEM; + } + + return OK; } - return OK; - } case SENSORIOCGQUEUEDEPTH: return _reports->size(); @@ -481,8 +492,9 @@ BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) case BAROIOCSMSLPRESSURE: /* range-check for sanity */ - if ((arg < 80000) || (arg > 120000)) + if ((arg < 80000) || (arg > 120000)) { return -EINVAL; + } _msl_pressure = arg; return OK; @@ -537,6 +549,7 @@ BAROSIM::cycle() /* perform collection */ ret = collect(); + if (ret != OK) { /* issue a reset command to the sensor */ _interface->dev_ioctl(IOCTL_RESET, dummy); @@ -569,6 +582,7 @@ BAROSIM::cycle() /* measurement phase */ ret = measure(); + if (ret != OK) { //DEVICE_LOG("measure error %d", ret); /* issue a reset command to the sensor */ @@ -605,8 +619,10 @@ BAROSIM::measure() * Send the command to begin measuring. */ ret = _interface->dev_ioctl(IOCTL_MEASURE, addr); - if (OK != ret) + + if (OK != ret) { perf_count(_comms_errors); + } perf_end(_measure_perf); @@ -631,10 +647,11 @@ BAROSIM::collect() 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); + 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 *)&baro_report, sizeof(baro_report)); + if (ret < 0) { perf_count(_comms_errors); perf_end(_sample_perf); @@ -647,6 +664,7 @@ BAROSIM::collect() report.altitude = baro_report.altitude; report.temperature = baro_report.temperature; report.timestamp = hrt_absolute_time(); + } else { report.pressure = baro_report.pressure; report.altitude = baro_report.altitude; @@ -658,6 +676,7 @@ BAROSIM::collect() if (_baro_topic != nullptr) { /* publish it */ orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); + } else { PX4_WARN("BAROSIM::collect _baro_topic not initialized"); } @@ -792,6 +811,7 @@ start_bus(struct barosim_bus_option &bus) prom_u prom_buf; device::Device *interface = bus.interface_constructor(prom_buf, bus.busnum); + if (interface->init() != OK) { delete interface; PX4_ERR("no device on bus %u", (unsigned)bus.busid); @@ -799,13 +819,14 @@ start_bus(struct barosim_bus_option &bus) } bus.dev = new BAROSIM(interface, prom_buf, bus.devpath); + if (bus.dev != nullptr && OK != bus.dev->init()) { delete bus.dev; bus.dev = NULL; PX4_ERR("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 */ @@ -813,6 +834,7 @@ start_bus(struct barosim_bus_option &bus) PX4_ERR("can't open baro device"); return false; } + if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { px4_close(fd); PX4_ERR("failed setting default poll rate"); @@ -863,6 +885,7 @@ test() int fd; fd = px4_open(bus.devpath, O_RDONLY); + if (fd < 0) { PX4_ERR("open failed (try 'barosim start' if the driver is not running)"); return 1; @@ -940,6 +963,7 @@ reset() int fd; fd = px4_open(bus.devpath, O_RDONLY); + if (fd < 0) { PX4_ERR("failed "); return 1; @@ -954,6 +978,7 @@ reset() PX4_ERR("driver poll restart failed"); return 1; } + return 0; } @@ -963,13 +988,15 @@ reset() int info() { - for (uint8_t i=0; iprint_info(); } } + return 0; } @@ -1078,26 +1105,30 @@ barosim_main(int argc, char *argv[]) /* * Start/load the driver. */ - if (!strcmp(verb, "start")) + if (!strcmp(verb, "start")) { ret = barosim::start(); + } /* * Test the driver/device. */ - else if (!strcmp(verb, "test")) + else if (!strcmp(verb, "test")) { ret = barosim::test(); + } /* * Reset the driver. */ - else if (!strcmp(verb, "reset")) + else if (!strcmp(verb, "reset")) { ret = barosim::reset(); + } /* * Print driver information. */ - else if (!strcmp(verb, "info")) + else if (!strcmp(verb, "info")) { ret = barosim::info(); + } /* * Perform MSL pressure calibration given an altitude in metres @@ -1112,10 +1143,11 @@ barosim_main(int argc, char *argv[]) long altitude = strtol(argv[2], nullptr, 10); ret = barosim::calibrate(altitude); - } - else { + + } else { barosim::usage(); return 1; } + return ret; } diff --git a/src/platforms/posix/drivers/barosim/baro_sim.cpp b/src/platforms/posix/drivers/barosim/baro_sim.cpp index 8e66dc3ac6..252d1428c9 100644 --- a/src/platforms/posix/drivers/barosim/baro_sim.cpp +++ b/src/platforms/posix/drivers/barosim/baro_sim.cpp @@ -31,11 +31,11 @@ * ****************************************************************************/ - /** - * @file baro_sim.cpp - * - * Simulation interface for barometer - */ +/** + * @file baro_sim.cpp + * + * Simulation interface for barometer + */ /* XXX trim includes */ #include @@ -182,7 +182,7 @@ int BARO_SIM::_measure(unsigned addr) { /* - * Disable retries on this command; we can't know whether failure + * Disable retries on this command; we can't know whether failure * means the device did or did not see the command. */ _retries = 0; @@ -201,25 +201,28 @@ BARO_SIM::_read_prom() int BARO_SIM::transfer(const uint8_t *send, unsigned send_len, - uint8_t *recv, unsigned recv_len) + uint8_t *recv, unsigned recv_len) { // TODO add Simulation data connection so calls retrieve // data from the simulator if (recv_len == 0) { PX4_DEBUG("BARO_SIM measurement requested"); - } - else if (send_len != 1 || send[0] != 0 ) { + + } else if (send_len != 1 || send[0] != 0) { PX4_WARN("BARO_SIM::transfer invalid param %u %u %u", send_len, send[0], recv_len); return 1; - } - else { + + } else { Simulator *sim = Simulator::getInstance(); + if (sim == NULL) { PX4_ERR("Error BARO_SIM::transfer no simulator"); return -ENODEV; } + PX4_DEBUG("BARO_SIM::transfer getting sample"); sim->getBaroSample(recv, recv_len); } + return 0; } diff --git a/src/platforms/posix/drivers/gpssim/gpssim.cpp b/src/platforms/posix/drivers/gpssim/gpssim.cpp index d94fc35f8a..889d793486 100644 --- a/src/platforms/posix/drivers/gpssim/gpssim.cpp +++ b/src/platforms/posix/drivers/gpssim/gpssim.cpp @@ -204,8 +204,9 @@ GPSSIM::~GPSSIM() } /* well, kill it anyway, though this will probably crash */ - if (_task != -1) + if (_task != -1) { px4_task_delete(_task); + } g_dev = nullptr; } @@ -216,12 +217,13 @@ GPSSIM::init() int ret = ERROR; /* do regular cdev init */ - if (VDev::init() != OK) + if (VDev::init() != OK) { goto out; + } /* start the GPS driver worker task */ _task = px4_task_spawn_cmd("gps", SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, 1500, (px4_main_t)&GPSSIM::task_main_trampoline, nullptr); + SCHED_PRIORITY_DEFAULT, 1500, (px4_main_t)&GPSSIM::task_main_trampoline, nullptr); if (_task < 0) { PX4_ERR("task start failed: %d", errno); @@ -263,7 +265,8 @@ GPSSIM::task_main_trampoline(void *arg) } int -GPSSIM::receive(int timeout) { +GPSSIM::receive(int timeout) +{ Simulator *sim = Simulator::getInstance(); simulator::RawGPSData gps; sim->getGPSSample((uint8_t *)&gps, sizeof(gps)); @@ -275,11 +278,11 @@ GPSSIM::receive(int timeout) { _report_gps_pos.timestamp_variance = hrt_absolute_time(); _report_gps_pos.eph = (float)gps.eph * 1e-2f; _report_gps_pos.epv = (float)gps.epv * 1e-2f; - _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; - _report_gps_pos.vel_d_m_s = (float)(gps.vd)/100.0f; - _report_gps_pos.cog_rad = (float)(gps.cog)*3.1415f/(100.0f * 180.0f); + _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; + _report_gps_pos.vel_d_m_s = (float)(gps.vd) / 100.0f; + _report_gps_pos.cog_rad = (float)(gps.cog) * 3.1415f / (100.0f * 180.0f); _report_gps_pos.fix_type = gps.fix_type; _report_gps_pos.satellites_used = gps.satellites_visible; @@ -309,9 +312,10 @@ GPSSIM::task_main() _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; - _report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s); + _report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * + _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s); _report_gps_pos.cog_rad = 0.0f; - _report_gps_pos.vel_ned_valid = true; + _report_gps_pos.vel_ned_valid = true; //no time and satellite information simulated @@ -325,6 +329,7 @@ GPSSIM::task_main() } usleep(2e5); + } else { //Publish initial report that we have access to a GPS //Make sure to clear any stale data in case driver is reset @@ -350,7 +355,8 @@ GPSSIM::task_main() /* opportunistic publishing - else invalid data would end up on the bus */ if (!(_pub_blocked)) { - orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); + if (_p_report_sat_info) { if (_report_sat_info_pub != nullptr) { orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info); @@ -361,6 +367,7 @@ GPSSIM::task_main() } } } + lock(); } } @@ -383,7 +390,7 @@ void GPSSIM::print_info() { //GPS Mode - if(_fake_gps) { + if (_fake_gps) { PX4_INFO("protocol: faked"); } @@ -392,17 +399,17 @@ GPSSIM::print_info() } PX4_INFO("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); - PX4_INFO("sat info: %s, noise: %d, jamming detected: %s", - (_p_report_sat_info != nullptr) ? "enabled" : "disabled", - _report_gps_pos.noise_per_ms, - _report_gps_pos.jamming_indicator == 255 ? "YES" : "NO"); + PX4_INFO("sat info: %s, noise: %d, jamming detected: %s", + (_p_report_sat_info != nullptr) ? "enabled" : "disabled", + _report_gps_pos.noise_per_ms, + _report_gps_pos.jamming_indicator == 255 ? "YES" : "NO"); if (_report_gps_pos.timestamp_position != 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_position) / 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); + (double)_report_gps_pos.vel_e_m_s, (double)_report_gps_pos.vel_d_m_s); PX4_INFO("eph: %.2fm, epv: %.2fm", (double)_report_gps_pos.eph, (double)_report_gps_pos.epv); //PX4_INFO("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate()); //PX4_INFO("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate()); diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index cbe1dbef41..0ac023285c 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -34,7 +34,7 @@ /** * @file gyrosim.cpp * - * Driver for the simulated gyro + * Driver for the simulated gyro * * @author Andrew Tridgell * @author Pat Hickey @@ -242,7 +242,7 @@ private: * * @return 0 on success, 1 on failure */ - int self_test(); + int self_test(); /** * Accel self test @@ -256,7 +256,7 @@ private: * * @return 0 on success, 1 on failure */ - int gyro_self_test(); + int gyro_self_test(); /* set sample rate (approximate) - 1kHz to 5Hz @@ -264,8 +264,8 @@ private: void _set_sample_rate(unsigned desired_sample_rate_hz); /* do not allow to copy this class due to pointer data members */ - GYROSIM(const GYROSIM&); - GYROSIM operator=(const GYROSIM&); + GYROSIM(const GYROSIM &); + GYROSIM operator=(const GYROSIM &); #pragma pack(push, 1) /** @@ -314,8 +314,8 @@ private: int _gyro_class_instance; /* do not allow to copy this class due to pointer data members */ - GYROSIM_gyro(const GYROSIM_gyro&); - GYROSIM_gyro operator=(const GYROSIM_gyro&); + GYROSIM_gyro(const GYROSIM_gyro &); + GYROSIM_gyro operator=(const GYROSIM_gyro &); }; /** driver 'main' command */ @@ -389,13 +389,17 @@ GYROSIM::~GYROSIM() delete _gyro; /* free any existing reports */ - if (_accel_reports != nullptr) + if (_accel_reports != nullptr) { delete _accel_reports; - if (_gyro_reports != nullptr) - delete _gyro_reports; + } - if (_accel_class_instance != -1) + if (_gyro_reports != nullptr) { + delete _gyro_reports; + } + + if (_accel_class_instance != -1) { unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance); + } /* delete the perf counter */ perf_free(_sample_perf); @@ -419,16 +423,19 @@ GYROSIM::init() } struct accel_report arp = {}; + struct gyro_report grp = {}; /* allocate basic report buffers */ _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); + if (_accel_reports == nullptr) { PX4_WARN("_accel_reports creation failed"); goto out; } _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report)); + if (_gyro_reports == nullptr) { PX4_WARN("_gyro_reports creation failed"); goto out; @@ -457,6 +464,7 @@ GYROSIM::init() /* do VDev init for the gyro device node, keep it optional */ ret = _gyro->init(); + /* if probe/setup failed, bail now */ if (ret != OK) { DEVICE_DEBUG("gyro init failed"); @@ -472,12 +480,12 @@ GYROSIM::init() /* measurement will have generated a report, publish */ _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, - &_accel_orb_class_instance, ORB_PRIO_HIGH); + &_accel_orb_class_instance, ORB_PRIO_HIGH); if (_accel_topic == nullptr) { PX4_WARN("ADVERT FAIL"); - } - else { + + } else { _pub_blocked = false; } @@ -486,7 +494,7 @@ GYROSIM::init() _gyro_reports->get(&grp); _gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, - &_gyro->_gyro_orb_class_instance, ORB_PRIO_HIGH); + &_gyro->_gyro_orb_class_instance, ORB_PRIO_HIGH); if (_gyro->_gyro_topic == nullptr) { PX4_WARN("ADVERT FAIL"); @@ -511,6 +519,7 @@ GYROSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) if (cmd == MPUREAD) { // Get data from the simulator Simulator *sim = Simulator::getInstance(); + if (sim == NULL) { PX4_WARN("failed accessing simulator"); return ENODEV; @@ -519,17 +528,18 @@ GYROSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) // FIXME - not sure what interrupt status should be recv[1] = 0; // skip cmd and status bytes - sim->getMPUReport(&recv[2], len-2); - } - else if (cmd & DIR_READ) - { - PX4_DEBUG("Reading %u bytes from register %u", len-1, reg); - memcpy(&_regdata[reg-MPUREG_PRODUCT_ID], &send[1], len-1); - } - else { - PX4_DEBUG("Writing %u bytes to register %u", len-1, reg); - if (recv) - memcpy(&recv[1], &_regdata[reg-MPUREG_PRODUCT_ID], len-1); + sim->getMPUReport(&recv[2], len - 2); + + } else if (cmd & DIR_READ) { + PX4_DEBUG("Reading %u bytes from register %u", len - 1, reg); + memcpy(&_regdata[reg - MPUREG_PRODUCT_ID], &send[1], len - 1); + + } else { + PX4_DEBUG("Writing %u bytes to register %u", len - 1, reg); + + if (recv) { + memcpy(&recv[1], &_regdata[reg - MPUREG_PRODUCT_ID], len - 1); + } } return PX4_OK; @@ -542,23 +552,26 @@ void GYROSIM::_set_sample_rate(unsigned desired_sample_rate_hz) { PX4_INFO("GYROSIM::_set_sample_rate %uHz", desired_sample_rate_hz); + if (desired_sample_rate_hz == 0 || - desired_sample_rate_hz == GYRO_SAMPLERATE_DEFAULT || - desired_sample_rate_hz == ACCEL_SAMPLERATE_DEFAULT) { + desired_sample_rate_hz == GYRO_SAMPLERATE_DEFAULT || + desired_sample_rate_hz == ACCEL_SAMPLERATE_DEFAULT) { desired_sample_rate_hz = GYROSIM_GYRO_DEFAULT_RATE; } uint8_t div = 1000 / desired_sample_rate_hz; - if(div>200) div=200; - if(div<1) div=1; + + if (div > 200) { div = 200; } + + if (div < 1) { div = 1; } // This does nothing in the simulator but writes the value in the "register" so // register dumps look correct - write_reg(MPUREG_SMPLRT_DIV, div-1); + write_reg(MPUREG_SMPLRT_DIV, div - 1); _sample_rate = 1000 / div; PX4_INFO("GYROSIM: Changed sample rate to %uHz", _sample_rate); - _call_interval = 1000000/_sample_rate; + _call_interval = 1000000 / _sample_rate; hrt_cancel(&_call); hrt_call_every(&_call, _call_interval, _call_interval, (hrt_callout)&GYROSIM::measure_trampoline, this); } @@ -569,8 +582,9 @@ GYROSIM::read(device::file_t *filp, char *buffer, size_t buflen) unsigned count = buflen / sizeof(accel_report); /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { @@ -579,17 +593,21 @@ GYROSIM::read(device::file_t *filp, char *buffer, size_t buflen) } /* if no data, error (we could block here) */ - if (_accel_reports->empty()) + if (_accel_reports->empty()) { return -EAGAIN; + } perf_count(_accel_reads); /* copy reports out of our buffer to the caller */ accel_report *arp = reinterpret_cast(buffer); int transferred = 0; + while (count--) { - if (!_accel_reports->get(arp)) + if (!_accel_reports->get(arp)) { break; + } + transferred++; arp++; } @@ -614,24 +632,34 @@ GYROSIM::accel_self_test() { return OK; - if (self_test()) + if (self_test()) { return 1; + } /* inspect accel offsets */ - if (fabsf(_accel_scale.x_offset) < 0.000001f) - return 1; - if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f) + if (fabsf(_accel_scale.x_offset) < 0.000001f) { return 1; + } - if (fabsf(_accel_scale.y_offset) < 0.000001f) - return 1; - if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f) + if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f) { return 1; + } - if (fabsf(_accel_scale.z_offset) < 0.000001f) + if (fabsf(_accel_scale.y_offset) < 0.000001f) { return 1; - if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) + } + + if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f) { return 1; + } + + if (fabsf(_accel_scale.z_offset) < 0.000001f) { + return 1; + } + + if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) { + return 1; + } return 0; } @@ -641,8 +669,9 @@ GYROSIM::gyro_self_test() { return OK; - if (self_test()) + if (self_test()) { return 1; + } /* * Maximum deviation of 20 degrees, according to @@ -658,27 +687,35 @@ GYROSIM::gyro_self_test() const float max_scale = 0.3f; /* evaluate gyro offsets, complain if offset -> zero or larger than 20 dps. */ - if (fabsf(_gyro_scale.x_offset) > max_offset) + if (fabsf(_gyro_scale.x_offset) > max_offset) { return 1; + } /* evaluate gyro scale, complain if off by more than 30% */ - if (fabsf(_gyro_scale.x_scale - 1.0f) > max_scale) + if (fabsf(_gyro_scale.x_scale - 1.0f) > max_scale) { return 1; + } - if (fabsf(_gyro_scale.y_offset) > max_offset) - return 1; - if (fabsf(_gyro_scale.y_scale - 1.0f) > max_scale) + if (fabsf(_gyro_scale.y_offset) > max_offset) { return 1; + } - if (fabsf(_gyro_scale.z_offset) > max_offset) + if (fabsf(_gyro_scale.y_scale - 1.0f) > max_scale) { return 1; - if (fabsf(_gyro_scale.z_scale - 1.0f) > max_scale) + } + + if (fabsf(_gyro_scale.z_offset) > max_offset) { return 1; + } + + if (fabsf(_gyro_scale.z_scale - 1.0f) > max_scale) { + return 1; + } /* check if all scales are zero */ if ((fabsf(_gyro_scale.x_offset) < 0.000001f) && - (fabsf(_gyro_scale.y_offset) < 0.000001f) && - (fabsf(_gyro_scale.z_offset) < 0.000001f)) { + (fabsf(_gyro_scale.y_offset) < 0.000001f) && + (fabsf(_gyro_scale.z_offset) < 0.000001f)) { /* if all are zero, this device is not calibrated */ return 1; } @@ -692,8 +729,9 @@ GYROSIM::gyro_read(device::file_t *filp, char *buffer, size_t buflen) unsigned count = buflen / sizeof(gyro_report); /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { @@ -702,17 +740,21 @@ GYROSIM::gyro_read(device::file_t *filp, char *buffer, size_t buflen) } /* if no data, error (we could block here) */ - if (_gyro_reports->empty()) + if (_gyro_reports->empty()) { return -EAGAIN; + } perf_count(_gyro_reads); /* copy reports out of our buffer to the caller */ gyro_report *grp = reinterpret_cast(buffer); int transferred = 0; + while (count--) { - if (!_gyro_reports->get(grp)) + if (!_gyro_reports->get(grp)) { break; + } + transferred++; grp++; } @@ -732,34 +774,35 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) case SENSORIOCSPOLLRATE: { switch (arg) { - /* switching to manual polling */ + /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _call_interval = 0; return OK; - /* external signalling not supported */ + /* external signalling not supported */ case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ + /* zero would be bad */ case 0: return -EINVAL; - /* set default/max polling rate */ + /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: return ioctl(filp, SENSORIOCSPOLLRATE, 1000); case SENSOR_POLLRATE_DEFAULT: return ioctl(filp, SENSORIOCSPOLLRATE, GYROSIM_ACCEL_DEFAULT_RATE); - /* adjust to a legal polling interval in Hz */ + /* adjust to a legal polling interval in Hz */ default: { /* convert hz to hrt interval via microseconds */ unsigned ticks = 1000000 / arg; /* check against maximum sane rate */ - if (ticks < 1000) + if (ticks < 1000) { return -EINVAL; + } /* update interval for next measurement */ _call_interval = ticks; @@ -778,23 +821,25 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_call_interval == 0) + if (_call_interval == 0) { return SENSOR_POLLRATE_MANUAL; + } return 1000000 / _call_interval; case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } - if (!_accel_reports->resize(arg)) { - return -ENOMEM; + if (!_accel_reports->resize(arg)) { + return -ENOMEM; + } + + return OK; } - return OK; - } - case SENSORIOCGQUEUEDEPTH: return _accel_reports->size(); @@ -808,14 +853,15 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) case ACCELIOCSLOWPASS: return OK; - case ACCELIOCSSCALE: - { + case ACCELIOCSSCALE: { /* copy scale, but only if off by a few percent */ struct accel_scale *s = (struct accel_scale *) arg; float sum = s->x_scale + s->y_scale + s->z_scale; + if (sum > 2.0f && sum < 4.0f) { memcpy(&_accel_scale, s, sizeof(_accel_scale)); return OK; + } else { return -EINVAL; } @@ -830,7 +876,7 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) return set_accel_range(arg); case ACCELIOCGRANGE: - return (unsigned long)((_accel_range_m_s2)/GYROSIM_ONE_G + 0.5f); + return (unsigned long)((_accel_range_m_s2) / GYROSIM_ONE_G + 0.5f); case ACCELIOCSELFTEST: return accel_self_test(); @@ -846,24 +892,25 @@ GYROSIM::gyro_ioctl(device::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { - /* these are shared with the accel side */ + /* these are shared with the accel side */ case SENSORIOCSPOLLRATE: case SENSORIOCGPOLLRATE: case SENSORIOCRESET: return ioctl(filp, cmd, arg); case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } - if (!_gyro_reports->resize(arg)) { - return -ENOMEM; + if (!_gyro_reports->resize(arg)) { + return -ENOMEM; + } + + return OK; } - return OK; - } - case SENSORIOCGQUEUEDEPTH: return _gyro_reports->size(); @@ -893,6 +940,7 @@ GYROSIM::gyro_ioctl(device::file_t *filp, int cmd, unsigned long arg) // _gyro_range_scale = xx // _gyro_range_rad_s = xx return -EINVAL; + case GYROIOCGRANGE: return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f); @@ -924,7 +972,7 @@ GYROSIM::write_reg(unsigned reg, uint8_t value) cmd[0] = reg | DIR_WRITE; cmd[1] = value; - // general register transfer at low clock speed + // general register transfer at low clock speed transfer(cmd, nullptr, sizeof(cmd)); } @@ -933,11 +981,11 @@ GYROSIM::set_accel_range(unsigned max_g_in) { // workaround for bugged versions of MPU6k (rev C) switch (_product) { - case GYROSIMES_REV_C4: - write_reg(MPUREG_ACCEL_CONFIG, 1 << 3); - _accel_range_scale = (GYROSIM_ONE_G / 4096.0f); - _accel_range_m_s2 = 8.0f * GYROSIM_ONE_G; - return OK; + case GYROSIMES_REV_C4: + write_reg(MPUREG_ACCEL_CONFIG, 1 << 3); + _accel_range_scale = (GYROSIM_ONE_G / 4096.0f); + _accel_range_m_s2 = 8.0f * GYROSIM_ONE_G; + return OK; } uint8_t afs_sel; @@ -948,14 +996,17 @@ GYROSIM::set_accel_range(unsigned max_g_in) afs_sel = 3; lsb_per_g = 2048; max_accel_g = 16; + } else if (max_g_in > 4) { // 8g - AFS_SEL = 2 afs_sel = 2; lsb_per_g = 4096; max_accel_g = 8; + } else if (max_g_in > 2) { // 4g - AFS_SEL = 1 afs_sel = 1; lsb_per_g = 8192; max_accel_g = 4; + } else { // 2g - AFS_SEL = 0 afs_sel = 0; lsb_per_g = 16384; @@ -1011,10 +1062,11 @@ GYROSIM::measure() if (x == 99) { x = 0; PX4_INFO("GYROSIM::measure %" PRIu64, hrt_absolute_time()); - } - else { + + } else { x++; } + #endif struct MPUReport mpu_report = {}; @@ -1026,8 +1078,8 @@ GYROSIM::measure() */ mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; - // sensor transfer at high clock speed - //set_frequency(GYROSIM_HIGH_BUS_SPEED); + // sensor transfer at high clock speed + //set_frequency(GYROSIM_HIGH_BUS_SPEED); if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) { return; } @@ -1045,7 +1097,7 @@ GYROSIM::measure() // transfers and bad register reads. This allows the higher // level code to decide if it should use this sensor based on // whether it has had failures - grb.error_count = arb.error_count = 0; // FIXME + grb.error_count = arb.error_count = 0; // FIXME /* * 1) Scale raw value to SI units using scaling from datasheet. @@ -1074,21 +1126,21 @@ GYROSIM::measure() _last_temperature = mpu_report.temp; - arb.temperature_raw = (int16_t)((mpu_report.temp - 35.0f)*361.0f); + arb.temperature_raw = (int16_t)((mpu_report.temp - 35.0f) * 361.0f); arb.temperature = _last_temperature; arb.x = mpu_report.accel_x; arb.y = mpu_report.accel_y; arb.z = mpu_report.accel_z; - grb.x_raw = (int16_t)(mpu_report.gyro_x/_gyro_range_scale); - grb.y_raw = (int16_t)(mpu_report.gyro_y/_gyro_range_scale); - grb.z_raw = (int16_t)(mpu_report.gyro_z/_gyro_range_scale); + grb.x_raw = (int16_t)(mpu_report.gyro_x / _gyro_range_scale); + grb.y_raw = (int16_t)(mpu_report.gyro_y / _gyro_range_scale); + grb.z_raw = (int16_t)(mpu_report.gyro_z / _gyro_range_scale); grb.scaling = _gyro_range_scale; grb.range_rad_s = _gyro_range_rad_s; - grb.temperature_raw = (int16_t)((mpu_report.temp - 35.0f)*361.0f); + grb.temperature_raw = (int16_t)((mpu_report.temp - 35.0f) * 361.0f); grb.temperature = _last_temperature; grb.x = mpu_report.gyro_x; @@ -1102,6 +1154,7 @@ GYROSIM::measure() /* notify anyone waiting for data */ poll_notify(POLLIN); _gyro->parent_poll_notify(); + if (!(_pub_blocked)) { /* log the time of this report */ perf_begin(_controller_latency_perf); @@ -1135,22 +1188,25 @@ GYROSIM::print_info() void GYROSIM::print_registers() { - char buf[6*13+1]; - int i=0; + char buf[6 * 13 + 1]; + int i = 0; buf[0] = '\0'; PX4_INFO("GYROSIM registers"); - for (uint8_t reg=MPUREG_PRODUCT_ID; reg<=108; reg++) { + + for (uint8_t reg = MPUREG_PRODUCT_ID; reg <= 108; reg++) { uint8_t v = read_reg(reg); - sprintf(&buf[i*6], "%02x:%02x ",(unsigned)reg, (unsigned)v); + sprintf(&buf[i * 6], "%02x:%02x ", (unsigned)reg, (unsigned)v); i++; - if ((i+1) % 13 == 0) { + + if ((i + 1) % 13 == 0) { PX4_INFO("%s", buf); - i=0; + i = 0; buf[i] = '\0'; } } - PX4_INFO("%s",buf); + + PX4_INFO("%s", buf); } @@ -1165,8 +1221,9 @@ GYROSIM_gyro::GYROSIM_gyro(GYROSIM *parent, const char *path) : GYROSIM_gyro::~GYROSIM_gyro() { - if (_gyro_class_instance != -1) + if (_gyro_class_instance != -1) { unregister_class_devname(GYRO_BASE_DEVICE_PATH, _gyro_class_instance); + } } int @@ -1205,11 +1262,12 @@ GYROSIM_gyro::ioctl(device::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { - case DEVIOCGDEVICEID: - return (int)VDev::ioctl(filp, cmd, arg); - break; - default: - return _parent->gyro_ioctl(filp, cmd, arg); + case DEVIOCGDEVICEID: + return (int)VDev::ioctl(filp, cmd, arg); + break; + + default: + return _parent->gyro_ioctl(filp, cmd, arg); } } @@ -1239,7 +1297,7 @@ int start(enum Rotation rotation) { int fd; - GYROSIM **g_dev_ptr = &g_dev_sim; + GYROSIM **g_dev_ptr = &g_dev_sim; const char *path_accel = MPU_DEVICE_PATH_ACCEL; const char *path_gyro = MPU_DEVICE_PATH_GYRO; @@ -1252,17 +1310,20 @@ start(enum Rotation rotation) /* create the driver */ *g_dev_ptr = new GYROSIM(path_accel, path_gyro, rotation); - if (*g_dev_ptr == nullptr) + if (*g_dev_ptr == nullptr) { goto fail; + } - if (OK != (*g_dev_ptr)->init()) + if (OK != (*g_dev_ptr)->init()) { goto fail; + } /* set the poll rate to default, starts automatic data collection */ fd = px4_open(path_accel, O_RDONLY); - if (fd < 0) + if (fd < 0) { goto fail; + } if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { px4_close(fd); @@ -1274,8 +1335,8 @@ start(enum Rotation rotation) fail: if (*g_dev_ptr != nullptr) { - delete (*g_dev_ptr); - *g_dev_ptr = nullptr; + delete(*g_dev_ptr); + *g_dev_ptr = nullptr; } PX4_WARN("driver start failed"); @@ -1286,13 +1347,16 @@ int stop() { GYROSIM **g_dev_ptr = &g_dev_sim; + if (*g_dev_ptr != nullptr) { delete *g_dev_ptr; *g_dev_ptr = nullptr; + } else { /* warn, but not an error */ PX4_WARN("already stopped."); } + return 0; } @@ -1350,7 +1414,7 @@ test() PX4_INFO("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw); PX4_INFO("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw); PX4_INFO("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2, - (double)(a_report.range_m_s2 / GYROSIM_ONE_G)); + (double)(a_report.range_m_s2 / GYROSIM_ONE_G)); /* do a simple demand read */ sz = px4_read(fd_gyro, &g_report, sizeof(g_report)); @@ -1368,7 +1432,7 @@ test() PX4_INFO("gyro y: \t%d\traw", (int)g_report.y_raw); PX4_INFO("gyro z: \t%d\traw", (int)g_report.z_raw); PX4_INFO("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s, - (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f)); + (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f)); PX4_INFO("temp: \t%8.4f\tdeg celsius", (double)a_report.temperature); PX4_INFO("temp: \t%d\traw 0x%0x", (short)a_report.temperature_raw, (unsigned short)a_report.temperature_raw); @@ -1380,7 +1444,7 @@ test() reset(); PX4_INFO("PASS"); - + return 0; } @@ -1409,11 +1473,11 @@ reset() goto reset_fail; } - px4_close(fd); + px4_close(fd); return 0; reset_fail: - px4_close(fd); + px4_close(fd); return 1; } @@ -1423,7 +1487,8 @@ reset_fail: int info() { - GYROSIM **g_dev_ptr = &g_dev_sim; + GYROSIM **g_dev_ptr = &g_dev_sim; + if (*g_dev_ptr == nullptr) { PX4_ERR("driver not running"); return 1; @@ -1442,6 +1507,7 @@ int regdump() { GYROSIM **g_dev_ptr = &g_dev_sim; + if (*g_dev_ptr == nullptr) { PX4_ERR("driver not running"); return 1; @@ -1473,11 +1539,13 @@ gyrosim_main(int argc, char *argv[]) /* jump over start/off/etc and look at options first */ int myoptind = 1; const char *myoptarg = NULL; + while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'R': rotation = (enum Rotation)atoi(myoptarg); break; + default: gyrosim::usage(); return 0; diff --git a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp index d070017a36..b58cccb5bd 100644 --- a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp +++ b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp @@ -47,16 +47,16 @@ * From Wikibooks: * * PLAY "[string expression]" - * + * * Used to play notes and a score ... The tones are indicated by letters A through G. - * Accidentals are indicated with a "+" or "#" (for sharp) or "-" (for flat) + * Accidentals are indicated with a "+" or "#" (for sharp) or "-" (for flat) * immediately after the note letter. See this example: - * + * * PLAY "C C# C C#" * * Whitespaces are ignored inside the string expression. There are also codes that - * set the duration, octave and tempo. They are all case-insensitive. PLAY executes - * the commands or notes the order in which they appear in the string. Any indicators + * set the duration, octave and tempo. They are all case-insensitive. PLAY executes + * the commands or notes the order in which they appear in the string. Any indicators * that change the properties are effective for the notes following that indicator. * * Ln Sets the duration (length) of the notes. The variable n does not indicate an actual duration @@ -66,15 +66,15 @@ * The shorthand notation of length is also provided for a note. For example, "L4 CDE L8 FG L4 AB" * can be shortened to "L4 CDE F8G8 AB". F and G play as eighth notes while others play as quarter notes. * On Sets the current octave. Valid values for n are 0 through 6. An octave begins with C and ends with B. - * Remember that C- is equivalent to B. + * Remember that C- is equivalent to B. * < > Changes the current octave respectively down or up one level. * Nn Plays a specified note in the seven-octave range. Valid values are from 0 to 84. (0 is a pause.) * Cannot use with sharp and flat. Cannot use with the shorthand notation neither. * MN Stand for Music Normal. Note duration is 7/8ths of the length indicated by Ln. It is the default mode. * ML Stand for Music Legato. Note duration is full length of that indicated by Ln. * MS Stand for Music Staccato. Note duration is 3/4ths of the length indicated by Ln. - * Pn Causes a silence (pause) for the length of note indicated (same as Ln). - * Tn Sets the number of "L4"s per minute (tempo). Valid values are from 32 to 255. The default value is T120. + * Pn Causes a silence (pause) for the length of note indicated (same as Ln). + * Tn Sets the number of "L4"s per minute (tempo). Valid values are from 32 to 255. The default value is T120. * . When placed after a note, it causes the duration of the note to be 3/2 of the set duration. * This is how to get "dotted" notes. "L4 C#." would play C sharp as a dotted quarter note. * It can be used for a pause as well. @@ -120,14 +120,15 @@ public: virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); virtual ssize_t write(device::file_t *filp, const char *buffer, size_t len); - inline const char *name(int tune) { + inline const char *name(int tune) + { return _tune_names[tune]; } private: static const unsigned _tune_max = 1024 * 8; // be reasonable about user tunes - const char * _default_tunes[TONE_NUMBER_OF_TUNES]; - const char * _tune_names[TONE_NUMBER_OF_TUNES]; + const char *_default_tunes[TONE_NUMBER_OF_TUNES]; + const char *_tune_names[TONE_NUMBER_OF_TUNES]; static const uint8_t _note_tab[]; unsigned _default_tune_number; // number of currently playing default tune (0 for none) @@ -151,8 +152,8 @@ private: // unsigned note_to_divisor(unsigned note); - // Calculate the duration in microseconds of play and silence for a - // note given the current tempo, length and mode and the number of + // Calculate the duration in microseconds of play and silence for a + // note given the current tempo, length and mode and the number of // dots following in the play string. // unsigned note_duration(unsigned &silence, unsigned note_length, unsigned dots); @@ -257,8 +258,9 @@ ToneAlarm::init() ret = VDev::init(); - if (ret != OK) + if (ret != OK) { return ret; + } DEVICE_DEBUG("ready"); return OK; @@ -267,7 +269,7 @@ ToneAlarm::init() unsigned ToneAlarm::note_to_divisor(unsigned note) { - const int TONE_ALARM_CLOCK = 120000000ul/4; + const int TONE_ALARM_CLOCK = 120000000ul / 4; // compute the frequency first (Hz) float freq = 880.0f * expf(logf(2.0f) * ((int)note - 46) / 12.0f); @@ -285,25 +287,31 @@ ToneAlarm::note_duration(unsigned &silence, unsigned note_length, unsigned dots) { unsigned whole_note_period = (60 * 1000000 * 4) / _tempo; - if (note_length == 0) + if (note_length == 0) { note_length = 1; + } + unsigned note_period = whole_note_period / note_length; switch (_note_mode) { case MODE_NORMAL: silence = note_period / 8; break; + case MODE_STACCATO: silence = note_period / 4; break; + default: case MODE_LEGATO: silence = 0; break; } + note_period -= silence; unsigned dot_extension = note_period / 2; + while (dots--) { note_period += dot_extension; dot_extension /= 2; @@ -317,12 +325,14 @@ ToneAlarm::rest_duration(unsigned rest_length, unsigned dots) { unsigned whole_note_period = (60 * 1000000 * 4) / _tempo; - if (rest_length == 0) + if (rest_length == 0) { rest_length = 1; + } unsigned rest_period = whole_note_period / rest_length; unsigned dot_extension = rest_period / 2; + while (dots--) { rest_period += dot_extension; dot_extension /= 2; @@ -406,115 +416,155 @@ ToneAlarm::next_note() while (note == 0) { // we always need at least one character from the string int c = next_char(); - if (c == 0) + + if (c == 0) { goto tune_end; + } + _next++; switch (c) { case 'L': // select note length _note_length = next_number(); - if (_note_length < 1) + + if (_note_length < 1) { goto tune_error; + } + break; case 'O': // select octave _octave = next_number(); - if (_octave > 6) + + if (_octave > 6) { _octave = 6; + } + break; case '<': // decrease octave - if (_octave > 0) + if (_octave > 0) { _octave--; + } + break; case '>': // increase octave - if (_octave < 6) + if (_octave < 6) { _octave++; + } + break; case 'M': // select inter-note gap c = next_char(); - if (c == 0) + + if (c == 0) { goto tune_error; + } + _next++; + switch (c) { case 'N': _note_mode = MODE_NORMAL; break; + case 'L': _note_mode = MODE_LEGATO; break; + case 'S': _note_mode = MODE_STACCATO; break; + case 'F': _repeat = false; break; + case 'B': _repeat = true; break; + default: goto tune_error; } + break; case 'P': // pause for a note length stop_note(); - hrt_call_after(&_note_call, - (hrt_abstime)rest_duration(next_number(), next_dots()), - (hrt_callout)next_trampoline, - this); + hrt_call_after(&_note_call, + (hrt_abstime)rest_duration(next_number(), next_dots()), + (hrt_callout)next_trampoline, + this); return; case 'T': { // change tempo - unsigned nt = next_number(); + unsigned nt = next_number(); - if ((nt >= 32) && (nt <= 255)) { - _tempo = nt; - } else { - goto tune_error; + if ((nt >= 32) && (nt <= 255)) { + _tempo = nt; + + } else { + goto tune_error; + } + + break; } - break; - } case 'N': // play an arbitrary note note = next_number(); - if (note > 84) + + if (note > 84) { goto tune_error; + } + if (note == 0) { // this is a rest - pause for the current note length hrt_call_after(&_note_call, - (hrt_abstime)rest_duration(_note_length, next_dots()), - (hrt_callout)next_trampoline, - this); - return; + (hrt_abstime)rest_duration(_note_length, next_dots()), + (hrt_callout)next_trampoline, + this); + return; } + break; case 'A'...'G': // play a note in the current octave note = _note_tab[c - 'A'] + (_octave * 12) + 1; c = next_char(); + switch (c) { case '#': // up a semitone case '+': - if (note < 84) + if (note < 84) { note++; + } + _next++; break; + case '-': // down a semitone - if (note > 1) + if (note > 1) { note--; + } + _next++; break; + default: // 0 / no next char here is OK break; } + // shorthand length notation note_length = next_number(); - if (note_length == 0) + + if (note_length == 0) { note_length = _note_length; + } + break; default: @@ -540,12 +590,15 @@ tune_error: // stop (and potentially restart) the tune tune_end: stop_note(); + if (_repeat) { start_tune(_tune); + } else { _tune = nullptr; _default_tune_number = 0; } + return; } @@ -555,6 +608,7 @@ ToneAlarm::next_char() while (isspace(*_next)) { _next++; } + return toupper(*_next); } @@ -566,8 +620,11 @@ ToneAlarm::next_number() for (;;) { c = next_char(); - if (!isdigit(c)) + + if (!isdigit(c)) { return number; + } + _next++; number = (number * 10) + (c - '0'); } @@ -582,6 +639,7 @@ ToneAlarm::next_dots() _next++; dots++; } + return dots; } @@ -615,6 +673,7 @@ ToneAlarm::ioctl(device::file_t *filp, int cmd, unsigned long arg) _next = nullptr; _repeat = false; _default_tune_number = 0; + } else { /* always interrupt alarms, unless they are repeating and already playing */ if (!(_repeat && _default_tune_number == arg)) { @@ -623,6 +682,7 @@ ToneAlarm::ioctl(device::file_t *filp, int cmd, unsigned long arg) start_tune(_default_tunes[arg]); } } + } else { result = -EINVAL; } @@ -637,8 +697,9 @@ ToneAlarm::ioctl(device::file_t *filp, int cmd, unsigned long arg) // irqrestore(flags); /* give it to the superclass if we didn't like it */ - if (result == -ENOTTY) + if (result == -ENOTTY) { result = VDev::ioctl(filp, cmd, arg); + } return result; } @@ -647,8 +708,9 @@ ssize_t ToneAlarm::write(device::file_t *filp, const char *buffer, size_t len) { // sanity-check the buffer for length and nul-termination - if (len > _tune_max) + if (len > _tune_max) { return -EFBIG; + } // if we have an existing user tune, free it if (_user_tune != nullptr) { @@ -665,13 +727,16 @@ ToneAlarm::write(device::file_t *filp, const char *buffer, size_t len) } // if the new tune is empty, we're done - if (buffer[0] == '\0') + if (buffer[0] == '\0') { return OK; + } // allocate a copy of the new tune _user_tune = strndup(buffer, len); - if (_user_tune == nullptr) + + if (_user_tune == nullptr) { return -ENOMEM; + } // and play it start_tune(_user_tune); @@ -725,13 +790,15 @@ play_string(const char *str, bool free_buffer) ret = px4_write(fd, str, strlen(str) + 1); px4_close(fd); - if (free_buffer) + if (free_buffer) { free((void *)str); + } if (ret < 0) { PX4_WARN("play tune"); return 1; } + return ret; } @@ -766,31 +833,38 @@ tone_alarm_main(int argc, char *argv[]) ret = play_tune(TONE_STOP_TUNE); } - else if (!strcmp(argv1, "stop")) + else if (!strcmp(argv1, "stop")) { ret = play_tune(TONE_STOP_TUNE); + } - else if ((tune = strtol(argv1, nullptr, 10)) != 0) + else if ((tune = strtol(argv1, nullptr, 10)) != 0) { ret = play_tune(tune); + } /* If it is a file name then load and play it as a string */ else if (*argv1 == '/') { FILE *fd = fopen(argv1, "r"); int sz; char *buffer; + if (fd == nullptr) { PX4_WARN("couldn't open '%s'", argv1); return 1; } + fseek(fd, 0, SEEK_END); sz = ftell(fd); fseek(fd, 0, SEEK_SET); buffer = (char *)malloc(sz + 1); + if (buffer == nullptr) { PX4_WARN("not enough memory memory"); return 1; } + // FIXME - Make GCC happy if (fread(buffer, sz, 1, fd)) { } + /* terminate the string */ buffer[sz] = 0; ret = play_string(buffer, true); @@ -801,14 +875,15 @@ tone_alarm_main(int argc, char *argv[]) if (*argv1 == 'M') { ret = play_string(argv1, false); } - } - else { + + } else { /* It might be a tune name */ for (tune = 1; tune < TONE_NUMBER_OF_TUNES; tune++) if (!strcmp(g_dev->name(tune), argv1)) { ret = play_tune(tune); return ret; } + PX4_WARN("unrecognized command, try 'start', 'stop', an alarm number or name, or a file name starting with a '/'"); ret = 1; } diff --git a/src/platforms/posix/include/crc32.h b/src/platforms/posix/include/crc32.h index bf828e3e0e..34e080b1c2 100644 --- a/src/platforms/posix/include/crc32.h +++ b/src/platforms/posix/include/crc32.h @@ -63,7 +63,7 @@ extern "C" { ****************************************************************************/ EXTERN uint32_t crc32part(const uint8_t *src, size_t len, - uint32_t crc32val); + uint32_t crc32val); /**************************************************************************** * Name: crc32 diff --git a/src/platforms/posix/include/queue.h b/src/platforms/posix/include/queue.h index 4d95541e02..30dc264f3f 100644 --- a/src/platforms/posix/include/queue.h +++ b/src/platforms/posix/include/queue.h @@ -69,30 +69,26 @@ * Global Type Declarations ************************************************************************/ -struct sq_entry_s -{ - FAR struct sq_entry_s *flink; +struct sq_entry_s { + FAR struct sq_entry_s *flink; }; typedef struct sq_entry_s sq_entry_t; -struct dq_entry_s -{ - FAR struct dq_entry_s *flink; - FAR struct dq_entry_s *blink; +struct dq_entry_s { + FAR struct dq_entry_s *flink; + FAR struct dq_entry_s *blink; }; typedef struct dq_entry_s dq_entry_t; -struct sq_queue_s -{ - FAR sq_entry_t *head; - FAR sq_entry_t *tail; +struct sq_queue_s { + FAR sq_entry_t *head; + FAR sq_entry_t *tail; }; typedef struct sq_queue_s sq_queue_t; -struct dq_queue_s -{ - FAR dq_entry_t *head; - FAR dq_entry_t *tail; +struct dq_queue_s { + FAR dq_entry_t *head; + FAR dq_entry_t *tail; }; typedef struct dq_queue_s dq_queue_t; @@ -112,11 +108,11 @@ EXTERN void dq_addfirst(FAR dq_entry_t *node, dq_queue_t *queue); EXTERN void sq_addlast(FAR sq_entry_t *node, sq_queue_t *queue); EXTERN void dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue); EXTERN void sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node, - sq_queue_t *queue); + sq_queue_t *queue); EXTERN void dq_addafter(FAR dq_entry_t *prev, FAR dq_entry_t *node, - dq_queue_t *queue); + dq_queue_t *queue); EXTERN void dq_addbefore(FAR dq_entry_t *next, FAR dq_entry_t *node, - dq_queue_t *queue); + dq_queue_t *queue); EXTERN FAR sq_entry_t *sq_remafter(FAR sq_entry_t *node, sq_queue_t *queue); EXTERN void sq_rem(FAR sq_entry_t *node, sq_queue_t *queue); diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index fd9fdef5c6..1b22627721 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -66,6 +66,13 @@ extern "C" { cout.flush(); _exit(0); } + void _SigFpeHandler(int sig_num); + void _SigFpeHandler(int sig_num) + { + cout.flush(); + cout << endl << "floating point exception" << endl; + cout.flush(); + } } static void print_prompt() @@ -96,6 +103,7 @@ static void run_cmd(const vector &appargs, bool exit_on_fail) if (exit_on_fail && retval) { exit(retval); } + usleep(65000); } else if (command.compare("help") == 0) { @@ -108,6 +116,7 @@ static void run_cmd(const vector &appargs, bool exit_on_fail) cout << "Invalid command: " << command << "\ntype 'help' for a list of commands" << endl; } + print_prompt(); } @@ -135,6 +144,7 @@ int main(int argc, char **argv) { bool daemon_mode = false; signal(SIGINT, _SigIntHandler); + signal(SIGFPE, _SigFpeHandler); int index = 1; bool error_detected = false; diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c index 10792b3089..0544131705 100644 --- a/src/platforms/posix/px4_layer/drv_hrt.c +++ b/src/platforms/posix/px4_layer/drv_hrt.c @@ -237,6 +237,7 @@ void hrt_init(void) sq_init(&callout_queue); int sem_ret = px4_sem_init(&_hrt_lock, 0, 1); + if (sem_ret) { PX4_ERR("SEM INIT FAIL: %s", strerror(errno)); } @@ -365,9 +366,10 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte #if 1 // Use this to debug busy CPU that keeps rescheduling with 0 period time - if (interval < HRT_INTERVAL_MIN) { - PX4_ERR("hrt_call_internal interval too short: %" PRIu64, interval); - } + /*if (interval < HRT_INTERVAL_MIN) {*/ + /*PX4_ERR("hrt_call_internal interval too short: %" PRIu64, interval);*/ + /*PX4_BACKTRACE();*/ + /*}*/ #endif entry->deadline = deadline; diff --git a/src/platforms/posix/px4_layer/lib_crc32.c b/src/platforms/posix/px4_layer/lib_crc32.c index 4ba6fbf6df..c14ebfceeb 100644 --- a/src/platforms/posix/px4_layer/lib_crc32.c +++ b/src/platforms/posix/px4_layer/lib_crc32.c @@ -48,46 +48,45 @@ #include // Needed for Linux -#define FAR +#define FAR /************************************************************************************************ * Private Data ************************************************************************************************/ - -static const uint32_t crc32_tab[] = -{ - 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, - 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, - 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, - 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, - 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, - 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, - 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, - 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, - 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, - 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, - 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, - 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, - 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, - 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, - 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, - 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, - 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, - 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, - 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, - 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, - 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, - 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, - 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, - 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, - 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, - 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, - 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, - 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, - 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, - 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, - 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, - 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d + +static const uint32_t crc32_tab[] = { + 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, + 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, + 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, + 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, + 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, + 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, + 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, + 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, + 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, + 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, + 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, + 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, + 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, + 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, + 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, + 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, + 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, + 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, + 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, + 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, + 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, + 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, + 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, + 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, + 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, + 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, + 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, + 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, + 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, + 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, + 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, + 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d }; /************************************************************************************************ @@ -103,13 +102,13 @@ static const uint32_t crc32_tab[] = uint32_t crc32part(FAR const uint8_t *src, size_t len, uint32_t crc32val) { - size_t i; + size_t i; - for (i = 0; i < len; i++) - { - crc32val = crc32_tab[(crc32val ^ src[i]) & 0xff] ^ (crc32val >> 8); - } - return crc32val; + for (i = 0; i < len; i++) { + crc32val = crc32_tab[(crc32val ^ src[i]) & 0xff] ^ (crc32val >> 8); + } + + return crc32val; } /************************************************************************************************ @@ -122,5 +121,5 @@ uint32_t crc32part(FAR const uint8_t *src, size_t len, uint32_t crc32val) uint32_t crc32(FAR const uint8_t *src, size_t len) { - return crc32part(src, len, 0); + return crc32part(src, len, 0); } diff --git a/src/platforms/posix/px4_layer/px4_log.c b/src/platforms/posix/px4_layer/px4_log.c index 238b324194..074ae8b466 100644 --- a/src/platforms/posix/px4_layer/px4_log.c +++ b/src/platforms/posix/px4_layer/px4_log.c @@ -1,5 +1,30 @@ +#include #include +#ifdef __PX4_POSIX +#include +#endif __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_str[_PX4_LOG_LEVEL_PANIC + 1] = { "INFO", "DEBUG", "WARN", "ERROR", "PANIC" }; + +void px4_backtrace() +{ +#ifdef __PX4_POSIX + void *buffer[10]; + char **callstack; + int bt_size; + int idx; + + bt_size = backtrace(buffer, 10); + callstack = backtrace_symbols(buffer, bt_size); + + PX4_INFO("Backtrace: %d", bt_size); + + for (idx = 0; idx < bt_size; idx++) { + PX4_INFO("%s", callstack[idx]); + } + + free(callstack); +#endif +} diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp index f112290cb3..ba476b5070 100644 --- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp @@ -63,8 +63,7 @@ pthread_t _shell_task_id = 0; -struct task_entry -{ +struct task_entry { pthread_t pid; std::string name; bool isused; @@ -73,27 +72,26 @@ struct task_entry static task_entry taskmap[PX4_MAX_TASKS]; -typedef struct -{ +typedef struct { px4_main_t entry; int argc; char *argv[]; - // strings are allocated after the + // strings are allocated after the } pthdata_t; -static void *entry_adapter ( void *ptr ) +static void *entry_adapter(void *ptr) { - pthdata_t *data; - data = (pthdata_t *) ptr; + pthdata_t *data; + data = (pthdata_t *) ptr; data->entry(data->argc, data->argv); free(ptr); PX4_DEBUG("Before px4_task_exit"); - px4_task_exit(0); + px4_task_exit(0); PX4_DEBUG("After px4_task_exit"); return NULL; -} +} void px4_systemreset(bool to_bootloader) @@ -102,7 +100,8 @@ px4_systemreset(bool to_bootloader) exit(0); } -px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char * const argv[]) +px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, + char *const argv[]) { int rv; int argc = 0; @@ -110,52 +109,62 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int unsigned int len = 0; unsigned long offset; unsigned long structsize; - char * p = (char *)argv; + char *p = (char *)argv; - pthread_t task; + pthread_t task; pthread_attr_t attr; struct sched_param param; // Calculate argc while (p != (char *)0) { p = argv[argc]; - if (p == (char *)0) - break; - ++argc; - len += strlen(p)+1; - } - structsize = sizeof(pthdata_t)+(argc+1)*sizeof(char *); - pthdata_t *taskdata; - - // not safe to pass stack data to the thread creation - taskdata = (pthdata_t *)malloc(structsize+len); - offset = ((unsigned long)taskdata)+structsize; - taskdata->entry = entry; + if (p == (char *)0) { + break; + } + + ++argc; + len += strlen(p) + 1; + } + + structsize = sizeof(pthdata_t) + (argc + 1) * sizeof(char *); + pthdata_t *taskdata; + + // not safe to pass stack data to the thread creation + taskdata = (pthdata_t *)malloc(structsize + len); + offset = ((unsigned long)taskdata) + structsize; + + taskdata->entry = entry; taskdata->argc = argc; - for (i=0; iargv[i] = (char *)offset; strcpy((char *)offset, argv[i]); - offset+=strlen(argv[i])+1; + offset += strlen(argv[i]) + 1; } + // Must add NULL at end of argv taskdata->argv[argc] = (char *)0; PX4_DEBUG("starting task %s", name); rv = pthread_attr_init(&attr); + if (rv != 0) { PX4_WARN("px4_task_spawn_cmd: failed to init thread attrs"); return (rv < 0) ? rv : -rv; } + rv = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED); + if (rv != 0) { PX4_WARN("px4_task_spawn_cmd: failed to set inherit sched"); return (rv < 0) ? rv : -rv; } + rv = pthread_attr_setschedpolicy(&attr, scheduler); + if (rv != 0) { PX4_WARN("px4_task_spawn_cmd: failed to set sched policy"); return (rv < 0) ? rv : -rv; @@ -164,28 +173,31 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int param.sched_priority = priority; rv = pthread_attr_setschedparam(&attr, ¶m); + if (rv != 0) { PX4_WARN("px4_task_spawn_cmd: failed to set sched param"); return (rv < 0) ? rv : -rv; } - rv = pthread_create (&task, &attr, &entry_adapter, (void *) taskdata); + rv = pthread_create(&task, &attr, &entry_adapter, (void *) taskdata); + if (rv != 0) { if (rv == EPERM) { //printf("WARNING: NOT RUNING AS ROOT, UNABLE TO RUN REALTIME THREADS\n"); - rv = pthread_create (&task, NULL, &entry_adapter, (void *) taskdata); + rv = pthread_create(&task, NULL, &entry_adapter, (void *) taskdata); + if (rv != 0) { PX4_ERR("px4_task_spawn_cmd: failed to create thread %d %d\n", rv, errno); return (rv < 0) ? rv : -rv; } - } - else { + + } else { return (rv < 0) ? rv : -rv; } } - for (i=0; i=PX4_MAX_TASKS) { + + if (i >= PX4_MAX_TASKS) { return -ENOSPC; } - return i; + + return i; } int px4_task_delete(px4_task_t id) @@ -205,15 +219,18 @@ int px4_task_delete(px4_task_t id) pthread_t pid; PX4_DEBUG("Called px4_task_delete"); - if (id < PX4_MAX_TASKS && taskmap[id].isused) + if (id < PX4_MAX_TASKS && taskmap[id].isused) { pid = taskmap[id].pid; - else + + } else { return -EINVAL; + } // If current thread then exit, otherwise cancel - if (pthread_self() == pid) { + if (pthread_self() == pid) { taskmap[id].isused = false; pthread_exit(0); + } else { rv = pthread_cancel(pid); } @@ -225,20 +242,21 @@ int px4_task_delete(px4_task_t id) void px4_task_exit(int ret) { - int i; + int i; pthread_t pid = pthread_self(); // Get pthread ID from the opaque ID - for (i=0; i=PX4_MAX_TASKS) { + + if (i >= PX4_MAX_TASKS) { PX4_ERR("px4_task_exit: self task not found!"); - } - else { + + } else { PX4_DEBUG("px4_task_exit: %s", taskmap[i].name.c_str()); } @@ -251,10 +269,12 @@ int px4_task_kill(px4_task_t id, int sig) pthread_t pid; PX4_DEBUG("Called px4_task_kill %d", sig); - if (id < PX4_MAX_TASKS && taskmap[id].isused && taskmap[id].pid != 0) + if (id < PX4_MAX_TASKS && taskmap[id].isused && taskmap[id].pid != 0) { pid = taskmap[id].pid; - else + + } else { return -EINVAL; + } // If current thread then exit, otherwise cancel rv = pthread_kill(pid, sig); @@ -268,27 +288,30 @@ void px4_show_tasks() int count = 0; PX4_INFO("Active Tasks:"); - for (idx=0; idx < PX4_MAX_TASKS; idx++) - { + + for (idx = 0; idx < PX4_MAX_TASKS; idx++) { if (taskmap[idx].isused) { PX4_INFO(" %-10s %lu", taskmap[idx].name.c_str(), (unsigned long)taskmap[idx].pid); count++; } } - if (count == 0) + + if (count == 0) { PX4_INFO(" No running tasks"); + } } bool px4_task_is_running(const char *taskname) { int idx; - for (idx=0; idx < PX4_MAX_TASKS; idx++) - { + + for (idx = 0; idx < PX4_MAX_TASKS; idx++) { if (taskmap[idx].isused && (strcmp(taskmap[idx].name.c_str(), taskname) == 0)) { return true; } } + return false; } __BEGIN_DECLS @@ -301,14 +324,14 @@ unsigned long px4_getpid() const char *getprogname(); const char *getprogname() { - pthread_t pid = pthread_self(); - for (int i=0; ilock)); s->value--; - if(s->value < 0) { - pthread_cond_wait(&(s->wait), &(s->lock)); + + if (s->value < 0) { + pthread_cond_wait(&(s->wait), &(s->lock)); } + pthread_mutex_unlock(&(s->lock)); return 0; @@ -77,9 +79,11 @@ int px4_sem_post(px4_sem_t *s) { pthread_mutex_lock(&(s->lock)); s->value++; - if(s->value <= 0) { + + if (s->value <= 0) { pthread_cond_signal(&(s->wait)); } + pthread_mutex_unlock(&(s->lock)); return 0; diff --git a/src/platforms/posix/tests/hello/hello_example.cpp b/src/platforms/posix/tests/hello/hello_example.cpp index a30aeb57bc..5d3cb76852 100644 --- a/src/platforms/posix/tests/hello/hello_example.cpp +++ b/src/platforms/posix/tests/hello/hello_example.cpp @@ -49,8 +49,9 @@ int HelloExample::main() { appState.setRunning(true); - int i=0; - while (!appState.exitRequested() && i<5) { + int i = 0; + + while (!appState.exitRequested() && i < 5) { sleep(2); printf(" Doing work...\n"); diff --git a/src/platforms/posix/tests/hello/hello_example.h b/src/platforms/posix/tests/hello/hello_example.h index a4ae517056..bf589996d1 100644 --- a/src/platforms/posix/tests/hello/hello_example.h +++ b/src/platforms/posix/tests/hello/hello_example.h @@ -41,7 +41,8 @@ #include -class HelloExample { +class HelloExample +{ public: HelloExample() {}; diff --git a/src/platforms/posix/tests/hello/hello_start_posix.cpp b/src/platforms/posix/tests/hello/hello_start_posix.cpp index 8dde729a6e..583baaf56a 100644 --- a/src/platforms/posix/tests/hello/hello_start_posix.cpp +++ b/src/platforms/posix/tests/hello/hello_start_posix.cpp @@ -55,7 +55,7 @@ static int daemon_task; /* Handle of deamon task / thread */ extern "C" __EXPORT int hello_main(int argc, char *argv[]); int hello_main(int argc, char *argv[]) { - + if (argc < 2) { PX4_WARN("usage: hello {start|stop|status}\n"); return 1; @@ -70,11 +70,11 @@ int hello_main(int argc, char *argv[]) } daemon_task = px4_task_spawn_cmd("hello", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - PX4_MAIN, - (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + PX4_MAIN, + (argv) ? (char *const *)&argv[2] : (char *const *)NULL); return 0; } diff --git a/src/platforms/posix/tests/hrt_test/hrt_test.cpp b/src/platforms/posix/tests/hrt_test/hrt_test.cpp index 8dd1f4bde3..528980cea8 100644 --- a/src/platforms/posix/tests/hrt_test/hrt_test.cpp +++ b/src/platforms/posix/tests/hrt_test/hrt_test.cpp @@ -54,6 +54,7 @@ static void timer_expired(void *arg) { static int i = 0; PX4_INFO("Test\n"); + if (i < 5) { i++; hrt_call_after(&t1, update_interval, timer_expired, (void *)0); @@ -79,7 +80,7 @@ int HRTTest::main() memset(&t1, 0, sizeof(t1)); PX4_INFO("HRT_CALL %d\n", hrt_called(&t1)); - + hrt_call_after(&t1, update_interval, timer_expired, (void *)0); sleep(2); PX4_INFO("HRT_CALL - %d\n", hrt_called(&t1)); diff --git a/src/platforms/posix/tests/hrt_test/hrt_test.h b/src/platforms/posix/tests/hrt_test/hrt_test.h index c4c97be6d2..ac4450e919 100644 --- a/src/platforms/posix/tests/hrt_test/hrt_test.h +++ b/src/platforms/posix/tests/hrt_test/hrt_test.h @@ -41,7 +41,8 @@ #include -class HRTTest { +class HRTTest +{ public: HRTTest() {}; diff --git a/src/platforms/posix/tests/hrt_test/hrt_test_start_posix.cpp b/src/platforms/posix/tests/hrt_test/hrt_test_start_posix.cpp index 2544804496..ed718b2cc4 100644 --- a/src/platforms/posix/tests/hrt_test/hrt_test_start_posix.cpp +++ b/src/platforms/posix/tests/hrt_test/hrt_test_start_posix.cpp @@ -63,11 +63,11 @@ int hrttest_main(int argc, char *argv[]) } daemon_task = px4_task_spawn_cmd("hrttest", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - PX4_MAIN, - (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + PX4_MAIN, + (argv) ? (char *const *)&argv[2] : (char *const *)NULL); return 0; } diff --git a/src/platforms/posix/tests/muorb/muorb_test_example.cpp b/src/platforms/posix/tests/muorb/muorb_test_example.cpp index 3bae353f2f..4547c6db57 100644 --- a/src/platforms/posix/tests/muorb/muorb_test_example.cpp +++ b/src/platforms/posix/tests/muorb/muorb_test_example.cpp @@ -57,127 +57,128 @@ int MuorbTestExample::main() int MuorbTestExample::DefaultTest() { - int i=0; - orb_advert_t pub_id = orb_advertise( ORB_ID( esc_status ), & m_esc_status ); - if( pub_id == 0 ) - { - PX4_ERR( "error publishing esc_status" ); - return -1; - } - orb_advert_t pub_id_vc = orb_advertise( ORB_ID( vehicle_command ), & m_vc ); - if( pub_id_vc == 0 ) - { - PX4_ERR( "error publishing vehicle_command" ); - return -1; - } - if( orb_publish( ORB_ID( vehicle_command ), pub_id_vc, &m_vc ) == PX4_ERROR ) - { - PX4_ERR( "[%d]Error publishing the vechile command message", i ); - return -1; - } - int sub_vc = orb_subscribe( ORB_ID( vehicle_command ) ); - if ( sub_vc == PX4_ERROR ) - { - PX4_ERR( "Error subscribing to vehicle_command topic" ); - return -1; - } + int i = 0; + orb_advert_t pub_id = orb_advertise(ORB_ID(esc_status), & m_esc_status); - while (!appState.exitRequested() && i<100) { + if (pub_id == 0) { + PX4_ERR("error publishing esc_status"); + return -1; + } - PX4_DEBUG("[%d] Doing work...", i ); - if( orb_publish( ORB_ID( esc_status ), pub_id, &m_esc_status ) == PX4_ERROR ) - { - PX4_ERR( "[%d]Error publishing the esc status message for iter", i ); - break; - } - bool updated = false; - if( orb_check( sub_vc, &updated ) == 0 ) - { - if( updated ) - { - PX4_DEBUG( "[%d]Vechicle Status is updated... reading new value", i ); - if( orb_copy( ORB_ID( vehicle_command ), sub_vc, &m_vc ) != 0 ) - { - PX4_ERR( "[%d]Error calling orb copy for vechivle status... ", i ); - break; - } - if( orb_publish( ORB_ID( vehicle_command ), pub_id_vc, &m_vc ) == PX4_ERROR ) - { - PX4_ERR( "[%d]Error publishing the vechile command message", i ); - break; - } - } - else - { - PX4_DEBUG( "[%d] VC topic is not updated", i ); - } - } - else - { - PX4_ERR( "[%d]Error checking the updated status for vechile command... ", i ); - break; - } - - ++i; - } - return 0; + orb_advert_t pub_id_vc = orb_advertise(ORB_ID(vehicle_command), & m_vc); + + if (pub_id_vc == 0) { + PX4_ERR("error publishing vehicle_command"); + return -1; + } + + if (orb_publish(ORB_ID(vehicle_command), pub_id_vc, &m_vc) == PX4_ERROR) { + PX4_ERR("[%d]Error publishing the vechile command message", i); + return -1; + } + + int sub_vc = orb_subscribe(ORB_ID(vehicle_command)); + + if (sub_vc == PX4_ERROR) { + PX4_ERR("Error subscribing to vehicle_command topic"); + return -1; + } + + while (!appState.exitRequested() && i < 100) { + + PX4_DEBUG("[%d] Doing work...", i); + + if (orb_publish(ORB_ID(esc_status), pub_id, &m_esc_status) == PX4_ERROR) { + PX4_ERR("[%d]Error publishing the esc status message for iter", i); + break; + } + + bool updated = false; + + if (orb_check(sub_vc, &updated) == 0) { + if (updated) { + PX4_DEBUG("[%d]Vechicle Status is updated... reading new value", i); + + if (orb_copy(ORB_ID(vehicle_command), sub_vc, &m_vc) != 0) { + PX4_ERR("[%d]Error calling orb copy for vechivle status... ", i); + break; + } + + if (orb_publish(ORB_ID(vehicle_command), pub_id_vc, &m_vc) == PX4_ERROR) { + PX4_ERR("[%d]Error publishing the vechile command message", i); + break; + } + + } else { + PX4_DEBUG("[%d] VC topic is not updated", i); + } + + } else { + PX4_ERR("[%d]Error checking the updated status for vechile command... ", i); + break; + } + + ++i; + } + + return 0; } int MuorbTestExample::PingPongTest() { - int i=0; - orb_advert_t pub_id_vc = orb_advertise( ORB_ID( vehicle_command ), & m_vc ); - if( pub_id_vc == 0 ) - { - PX4_ERR( "error publishing vehicle_command" ); - return -1; - } - if( orb_publish( ORB_ID( vehicle_command ), pub_id_vc, &m_vc ) == PX4_ERROR ) - { - PX4_ERR( "[%d]Error publishing the vechile command message", i ); - return -1; - } - int sub_esc_status = orb_subscribe( ORB_ID( esc_status ) ); - if ( sub_esc_status == PX4_ERROR ) - { - PX4_ERR( "Error subscribing to esc_status topic" ); - return -1; - } + int i = 0; + orb_advert_t pub_id_vc = orb_advertise(ORB_ID(vehicle_command), & m_vc); - while (!appState.exitRequested() ) { + if (pub_id_vc == 0) { + PX4_ERR("error publishing vehicle_command"); + return -1; + } - PX4_INFO("[%d] Doing work...", i ); - bool updated = false; - if( orb_check( sub_esc_status, &updated ) == 0 ) - { - if( updated ) - { - PX4_INFO( "[%d]ESC status is updated... reading new value", i ); - if( orb_copy( ORB_ID( esc_status ), sub_esc_status, &m_esc_status ) != 0 ) - { - PX4_ERR( "[%d]Error calling orb copy for esc status... ", i ); - break; - } - if( orb_publish( ORB_ID( vehicle_command ), pub_id_vc, &m_vc ) == PX4_ERROR ) - { - PX4_ERR( "[%d]Error publishing the vechile command message", i ); - break; - } - } - else - { - PX4_INFO( "[%d] esc status topic is not updated", i ); - } - } - else - { - PX4_ERR( "[%d]Error checking the updated status for esc status... ", i ); - break; - } - // sleep for 1 sec. - usleep( 1000000 ); + if (orb_publish(ORB_ID(vehicle_command), pub_id_vc, &m_vc) == PX4_ERROR) { + PX4_ERR("[%d]Error publishing the vechile command message", i); + return -1; + } - ++i; - } - return 0; + int sub_esc_status = orb_subscribe(ORB_ID(esc_status)); + + if (sub_esc_status == PX4_ERROR) { + PX4_ERR("Error subscribing to esc_status topic"); + return -1; + } + + while (!appState.exitRequested()) { + + PX4_INFO("[%d] Doing work...", i); + bool updated = false; + + if (orb_check(sub_esc_status, &updated) == 0) { + if (updated) { + PX4_INFO("[%d]ESC status is updated... reading new value", i); + + if (orb_copy(ORB_ID(esc_status), sub_esc_status, &m_esc_status) != 0) { + PX4_ERR("[%d]Error calling orb copy for esc status... ", i); + break; + } + + if (orb_publish(ORB_ID(vehicle_command), pub_id_vc, &m_vc) == PX4_ERROR) { + PX4_ERR("[%d]Error publishing the vechile command message", i); + break; + } + + } else { + PX4_INFO("[%d] esc status topic is not updated", i); + } + + } else { + PX4_ERR("[%d]Error checking the updated status for esc status... ", i); + break; + } + + // sleep for 1 sec. + usleep(1000000); + + ++i; + } + + return 0; } diff --git a/src/platforms/posix/tests/muorb/muorb_test_example.h b/src/platforms/posix/tests/muorb/muorb_test_example.h index c5d699ae7d..4f4dcf02d6 100644 --- a/src/platforms/posix/tests/muorb/muorb_test_example.h +++ b/src/platforms/posix/tests/muorb/muorb_test_example.h @@ -43,7 +43,8 @@ #include "uORB/topics/esc_status.h" #include "uORB/topics/vehicle_command.h" -class MuorbTestExample { +class MuorbTestExample +{ public: MuorbTestExample() {}; @@ -53,9 +54,9 @@ public: static px4::AppState appState; /* track requests to terminate app */ private: - int DefaultTest(); - int PingPongTest(); - struct esc_status_s m_esc_status; - struct vehicle_command_s m_vc; - + int DefaultTest(); + int PingPongTest(); + struct esc_status_s m_esc_status; + struct vehicle_command_s m_vc; + }; diff --git a/src/platforms/posix/tests/muorb/muorb_test_main.cpp b/src/platforms/posix/tests/muorb/muorb_test_main.cpp index effa9ff88b..d9e9d1c6a9 100644 --- a/src/platforms/posix/tests/muorb/muorb_test_main.cpp +++ b/src/platforms/posix/tests/muorb/muorb_test_main.cpp @@ -43,14 +43,14 @@ #include "muorb_test_example.h" #include #include "uORB/uORBManager.hpp" -#include "uORBKraitFastRpcChannel.hpp" +#include "uORBKraitFastRpcChannel.hpp" int PX4_MAIN(int argc, char **argv) { px4::init(argc, argv, "muorb_test"); PX4_DEBUG("muorb_test"); - + MuorbTestExample hello; hello.main(); diff --git a/src/platforms/posix/tests/muorb/muorb_test_start_posix.cpp b/src/platforms/posix/tests/muorb/muorb_test_start_posix.cpp index 943605f531..20227ba54e 100644 --- a/src/platforms/posix/tests/muorb/muorb_test_start_posix.cpp +++ b/src/platforms/posix/tests/muorb/muorb_test_start_posix.cpp @@ -71,11 +71,11 @@ int muorb_test_main(int argc, char *argv[]) } daemon_task = px4_task_spawn_cmd("muorb_test", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 16000, - PX4_MAIN, - (char* const*)argv); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 16000, + PX4_MAIN, + (char *const *)argv); return 0; } diff --git a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp index ec709cc0ef..1276b740a2 100644 --- a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp +++ b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp @@ -60,44 +60,50 @@ static int writer_main(int argc, char *argv[]) char buf[1]; int fd = px4_open(TESTDEV, PX4_F_WRONLY); + if (fd < 0) { PX4_INFO("Writer: Open failed %d %d", fd, px4_errno); return -px4_errno; } int ret; - int i=0; + int i = 0; + while (!g_exit) { // Wait for 2 seconds PX4_INFO("Writer: Sleeping for 2 sec"); ret = sleep(2); + if (ret < 0) { PX4_INFO("Writer: sleep failed %d %d", ret, errno); return ret; } - buf[0] = 'A'+(char)(i % 26); + buf[0] = 'A' + (char)(i % 26); PX4_INFO("Writer: writing char '%c'", buf[0]); ret = px4_write(fd, buf, 1); - ++i; + ++i; } + px4_close(fd); PX4_INFO("Writer: stopped"); return ret; } -class PrivData { +class PrivData +{ public: PrivData() : _read_offset(0) {} ~PrivData() {} - + size_t _read_offset; }; - -class VCDevNode : public VDev { + +class VCDevNode : public VDev +{ public: - VCDevNode() : - VDev("vcdevtest", TESTDEV), + VCDevNode() : + VDev("vcdevtest", TESTDEV), _is_open_for_write(false), _write_offset(0) {}; @@ -120,22 +126,25 @@ int VCDevNode::open(device::file_t *handlep) errno = EBUSY; return -1; } + int ret = VDev::open(handlep); + if (ret != 0) { return ret; } + handlep->priv = new PrivData; if (_is_open_for_write && (handlep->flags & PX4_F_WRONLY)) { _is_open_for_write = true; } - + return 0; } int VCDevNode::close(device::file_t *handlep) { - delete (PrivData *)handlep->priv; + delete(PrivData *)handlep->priv; handlep->priv = nullptr; VDev::close(handlep); @@ -143,12 +152,13 @@ int VCDevNode::close(device::file_t *handlep) if ((handlep->flags & PX4_F_WRONLY) && _is_open_for_write) { _is_open_for_write = false; } + return 0; } ssize_t VCDevNode::write(device::file_t *handlep, const char *buffer, size_t buflen) { - for (size_t i=0; ipriv; ssize_t chars_read = 0; PX4_INFO("read %zu write %zu", p->_read_offset, _write_offset); - for (size_t i=0; i_read_offset < _write_offset); i++) { + + for (size_t i = 0; i < buflen && (p->_read_offset < _write_offset); i++) { buffer[i] = _buf[p->_read_offset]; p->_read_offset++; chars_read++; } - + return chars_read; } -VCDevExample::~VCDevExample() { +VCDevExample::~VCDevExample() +{ if (_node) { delete _node; _node = 0; @@ -183,16 +195,19 @@ VCDevExample::~VCDevExample() { static int test_pub_block(int fd, unsigned long blocked) { int ret = px4_ioctl(fd, DEVIOCSPUBBLOCK, blocked); + if (ret < 0) { PX4_INFO("ioctl PX4_DEVIOCSPUBBLOCK failed %d %d", ret, px4_errno); return -px4_errno; } ret = px4_ioctl(fd, DEVIOCGPUBBLOCK, 0); + if (ret < 0) { PX4_INFO("ioctl PX4_DEVIOCGPUBBLOCK failed %d %d", ret, px4_errno); return -px4_errno; } + PX4_INFO("pub_blocked = %d %s", ret, (unsigned long)ret == blocked ? "PASS" : "FAIL"); return 0; @@ -214,32 +229,39 @@ int VCDevExample::do_poll(int fd, int timeout, int iterations, int delayms_after // Test indefinte blocking poll while ((!appState.exitRequested()) && (loop_count < iterations)) { pollret = px4_poll(fds, 1, timeout); + if (pollret < 0) { PX4_ERR("Reader: px4_poll failed %d %d FAIL", pollret, px4_errno); goto fail; - } + } + PX4_INFO("Reader: px4_poll returned %d", pollret); + if (pollret) { readret = px4_read(fd, readbuf, 10); + if (readret != 1) { if (mustblock) { PX4_ERR("Reader: read failed %d FAIL", readret); goto fail; - } - else { + + } else { PX4_INFO("Reader: read failed %d FAIL", readret); } - } - else { + + } else { readbuf[readret] = '\0'; PX4_INFO("Reader: px4_poll returned %d, read '%s' PASS", pollret, readbuf); } } + if (delayms_after_poll) { - usleep(delayms_after_poll*1000); + usleep(delayms_after_poll * 1000); } + loop_count++; } + return 0; fail: return 1; @@ -269,47 +291,61 @@ int VCDevExample::main() void *p = 0; int ret = px4_ioctl(fd, DIOC_GETPRIV, (unsigned long)&p); + if (ret < 0) { PX4_INFO("ioctl DIOC_GETPRIV failed %d %d", ret, px4_errno); return -px4_errno; } + PX4_INFO("priv data = %p %s", p, p == (void *)_node ? "PASS" : "FAIL"); ret = test_pub_block(fd, 1); - if (ret < 0) + + if (ret < 0) { return ret; + } + ret = test_pub_block(fd, 0); - if (ret < 0) + + if (ret < 0) { return ret; + } // Start a task that will write something in 4 seconds - (void)px4_task_spawn_cmd("writer", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 6, - 2000, - writer_main, - (char* const*)NULL); + (void)px4_task_spawn_cmd("writer", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 6, + 2000, + writer_main, + (char *const *)NULL); ret = 0; PX4_INFO("TEST: BLOCKING POLL ---------------"); + if (do_poll(fd, -1, 3, 0)) { ret = 1; goto fail2; } + PX4_INFO("TEST: ZERO TIMEOUT POLL -----------"); - if(do_poll(fd, 0, 3, 0)) { + + if (do_poll(fd, 0, 3, 0)) { ret = 1; goto fail2; } + PX4_INFO("TEST: 100ms TIMEOUT POLL -----------"); - if(do_poll(fd, 0, 30, 100)) { + + if (do_poll(fd, 0, 30, 100)) { ret = 1; goto fail2; } + PX4_INFO("TEST: 1 SEC TIMOUT POLL ------------"); - if(do_poll(fd, 1000, 3, 0)) { + + if (do_poll(fd, 1000, 3, 0)) { ret = 1; goto fail2; } diff --git a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.h b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.h index 10befc795c..a1cc325f00 100644 --- a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.h +++ b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.h @@ -43,7 +43,8 @@ class VCDevNode; -class VCDevExample { +class VCDevExample +{ public: VCDevExample() : _node(0) {}; diff --git a/src/platforms/posix/tests/vcdev_test/vcdevtest_start_posix.cpp b/src/platforms/posix/tests/vcdev_test/vcdevtest_start_posix.cpp index 5ed9269b2d..135ca8f3f7 100644 --- a/src/platforms/posix/tests/vcdev_test/vcdevtest_start_posix.cpp +++ b/src/platforms/posix/tests/vcdev_test/vcdevtest_start_posix.cpp @@ -50,7 +50,7 @@ static int daemon_task; /* Handle of deamon task / thread */ extern "C" __EXPORT int vcdevtest_main(int argc, char *argv[]); int vcdevtest_main(int argc, char *argv[]) { - + if (argc < 2) { printf("usage: vcdevtest {start|stop|status}\n"); return 1; @@ -65,11 +65,11 @@ int vcdevtest_main(int argc, char *argv[]) } daemon_task = px4_task_spawn_cmd("vcdevtest", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - PX4_MAIN, - (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + PX4_MAIN, + (argv) ? (char *const *)&argv[2] : (char *const *)NULL); return 0; } diff --git a/src/platforms/posix/tests/wqueue/wqueue_start_posix.cpp b/src/platforms/posix/tests/wqueue/wqueue_start_posix.cpp index 2479020097..d4652f8e62 100644 --- a/src/platforms/posix/tests/wqueue/wqueue_start_posix.cpp +++ b/src/platforms/posix/tests/wqueue/wqueue_start_posix.cpp @@ -52,7 +52,7 @@ static int daemon_task; /* Handle of deamon task / thread */ extern "C" __EXPORT int wqueue_test_main(int argc, char *argv[]); int wqueue_test_main(int argc, char *argv[]) { - + if (argc < 2) { PX4_INFO("usage: wqueue_test {start|stop|status}\n"); return 1; @@ -67,11 +67,11 @@ int wqueue_test_main(int argc, char *argv[]) } daemon_task = px4_task_spawn_cmd("wqueue", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - PX4_MAIN, - (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + PX4_MAIN, + (argv) ? (char *const *)&argv[2] : (char *const *)NULL); return 0; } diff --git a/src/platforms/posix/tests/wqueue/wqueue_test.cpp b/src/platforms/posix/tests/wqueue/wqueue_test.cpp index 6c9ececc14..aec10269b8 100644 --- a/src/platforms/posix/tests/wqueue/wqueue_test.cpp +++ b/src/platforms/posix/tests/wqueue/wqueue_test.cpp @@ -65,8 +65,11 @@ void WQueueTest::do_lp_work() { static int iter = 0; printf("done lp work\n"); - if (iter > 5) + + if (iter > 5) { _lpwork_done = true; + } + ++iter; work_queue(LPWORK, &_lpwork, (worker_t)&lp_worker_cb, this, 1000); @@ -76,8 +79,11 @@ void WQueueTest::do_hp_work() { static int iter = 0; printf("done hp work\n"); - if (iter > 5) + + if (iter > 5) { _hpwork_done = true; + } + ++iter; // requeue diff --git a/src/platforms/posix/tests/wqueue/wqueue_test.h b/src/platforms/posix/tests/wqueue/wqueue_test.h index 6db3fc1e25..56e66a0b1b 100644 --- a/src/platforms/posix/tests/wqueue/wqueue_test.h +++ b/src/platforms/posix/tests/wqueue/wqueue_test.h @@ -43,11 +43,12 @@ #include #include -class WQueueTest { +class WQueueTest +{ public: - WQueueTest() : - _lpwork_done(false), - _hpwork_done(false) + WQueueTest() : + _lpwork_done(false), + _hpwork_done(false) { memset(&_lpwork, 0, sizeof(_lpwork)); memset(&_hpwork, 0, sizeof(_hpwork)); diff --git a/src/platforms/posix/work_queue/hrt_thread.c b/src/platforms/posix/work_queue/hrt_thread.c index efe09ea860..98331ca798 100644 --- a/src/platforms/posix/work_queue/hrt_thread.c +++ b/src/platforms/posix/work_queue/hrt_thread.c @@ -165,6 +165,7 @@ static void hrt_work_process() if (!worker) { PX4_ERR("MESSED UP: worker = 0"); + PX4_BACKTRACE(); } else { worker(arg); @@ -273,7 +274,7 @@ void hrt_work_queue_init(void) work_hrtthread, (char *const *)NULL); - + #ifdef __PX4_QURT signal(SIGALRM, _sighandler); #else diff --git a/src/platforms/px4_adc.h b/src/platforms/px4_adc.h index 8420f51971..c8c8ce01f0 100644 --- a/src/platforms/px4_adc.h +++ b/src/platforms/px4_adc.h @@ -52,13 +52,12 @@ // FIXME - this needs to be a px4_adc_msg_s type // Curently copied from NuttX -struct adc_msg_s -{ - uint8_t am_channel; /* The 8-bit ADC Channel */ - int32_t am_data; /* ADC convert result (4 bytes) */ -} __attribute__ ((packed)); +struct adc_msg_s { + uint8_t am_channel; /* The 8-bit ADC Channel */ + int32_t am_data; /* ADC convert result (4 bytes) */ +} __attribute__((packed)); -// Example settings +// Example settings #define ADC_BATTERY_VOLTAGE_CHANNEL 10 #define ADC_BATTERY_CURRENT_CHANNEL ((uint8_t)(-1)) #define ADC_AIRSPEED_VOLTAGE_CHANNEL 11 diff --git a/src/platforms/px4_app.h b/src/platforms/px4_app.h index 9aa285e228..df3a8d8294 100644 --- a/src/platforms/px4_app.h +++ b/src/platforms/px4_app.h @@ -40,9 +40,11 @@ #pragma once -namespace px4 { +namespace px4 +{ -class AppState { +class AppState +{ public: ~AppState() {} @@ -65,8 +67,8 @@ protected: bool _isRunning; #endif private: - AppState(const AppState&); - const AppState& operator=(const AppState&); + AppState(const AppState &); + const AppState &operator=(const AppState &); }; } diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 69dc2d64eb..4d453e5983 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -92,7 +92,7 @@ typedef param_t px4_param_t; */ #if defined(__PX4_NUTTX) -#define PX4_ROOTFSDIR +#define PX4_ROOTFSDIR /* XXX this is a hack to resolve conflicts with NuttX headers */ #if !defined(__PX4_TESTS) @@ -119,7 +119,7 @@ typedef param_t px4_param_t; #define PRId64 "lld" #endif -/* +/* * POSIX Specific defines */ #elif defined(__PX4_POSIX) @@ -138,7 +138,7 @@ typedef param_t px4_param_t; /* FIXME - Used to satisfy build */ //STM DocID018909 Rev 8 Sect 39.1 (Unique device ID Register) -#define UNIQUE_ID 0x1FFF7A10 +#define UNIQUE_ID 0x1FFF7A10 #define STM32_SYSMEM_UID "SIMULATIONID" /* FIXME - Used to satisfy build */ @@ -153,12 +153,12 @@ __END_DECLS #endif #define USEC_PER_TICK (1000000UL/PX4_TICKS_PER_SEC) -#define USEC2TICK(x) (((x)+(USEC_PER_TICK/2))/USEC_PER_TICK) +#define USEC2TICK(x) (((x)+(USEC_PER_TICK/2))/USEC_PER_TICK) #define px4_statfs_buf_f_bavail_t unsigned long #if defined(__PX4_QURT) -#define PX4_ROOTFSDIR +#define PX4_ROOTFSDIR #else #define PX4_ROOTFSDIR "rootfs" #endif @@ -166,7 +166,7 @@ __END_DECLS #endif -/* +/* * Defines for ROS and Linux */ #if defined(__PX4_ROS) || defined(__PX4_POSIX) @@ -216,6 +216,7 @@ __END_DECLS #ifndef __PX4_QURT #if defined(__cplusplus) +#include #define PX4_ISFINITE(x) std::isfinite(x) #else #define PX4_ISFINITE(x) isfinite(x) @@ -226,7 +227,7 @@ __END_DECLS #if defined(__PX4_QURT) -#define PX4_ROOTFSDIR +#define PX4_ROOTFSDIR #define DEFAULT_PARAM_FILE "/fs/eeprom/parameters" #define SIOCDEVPRIVATE 999999 @@ -234,17 +235,9 @@ __END_DECLS // Missing math.h defines #define PX4_ISFINITE(x) __builtin_isfinite(x) -// FIXME - these are missing for clang++ but not for clang -#if defined(__cplusplus) -#define isfinite(x) true -#define isnan(x) false -#define isinf(x) false -#define fminf(x, y) ((x) > (y) ? y : x) #endif -#endif - -/* +/* *Defines for all platforms */ diff --git a/src/platforms/px4_i2c.h b/src/platforms/px4_i2c.h index 2b72f37c5d..1d49600473 100644 --- a/src/platforms/px4_i2c.h +++ b/src/platforms/px4_i2c.h @@ -75,23 +75,23 @@ typedef struct i2c_dev_s px4_i2c_dev_t; // NOTE - This is a copy of the NuttX i2c_msg_s structure typedef struct { - uint16_t addr; /* Slave address */ - uint16_t flags; /* See I2C_M_* definitions */ - uint8_t *buffer; - int length; + uint16_t addr; /* Slave address */ + uint16_t flags; /* See I2C_M_* definitions */ + uint8_t *buffer; + int length; } px4_i2c_msg_t; // NOTE - This is a copy of the NuttX i2c_ops_s structure typedef struct { - const struct px4_i2c_ops_t *ops; /* I2C vtable */ + const struct px4_i2c_ops_t *ops; /* I2C vtable */ } px4_i2c_dev_t; // FIXME - Empty defines for I2C ops // Original version commented out //#define I2C_SETFREQUENCY(d,f) ((d)->ops->setfrequency(d,f)) -#define I2C_SETFREQUENCY(d,f) +#define I2C_SETFREQUENCY(d,f) //#define SPI_SELECT(d,id,s) ((d)->ops->select(d,id,s)) -#define SPI_SELECT(d,id,s) +#define SPI_SELECT(d,id,s) // FIXME - Stub implementation // Original version commented out @@ -101,8 +101,7 @@ inline int I2C_TRANSFER(px4_i2c_dev_t *dev, px4_i2c_msg_t *msg, int count) { ret #ifdef __PX4_QURT -struct i2c_msg -{ +struct i2c_msg { uint16_t addr; /* Slave address */ uint16_t flags; /* See I2C_M_* definitions */ uint8_t *buf; diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index 55bf3a55bc..953b766f4d 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -64,6 +64,7 @@ static inline void do_nothing(int level, ...) #define PX4_WARN(...) ROS_WARN(__VA_ARGS__) #define PX4_INFO(...) ROS_WARN(__VA_ARGS__) #define PX4_DEBUG(...) +#define PX4_BACKTRACE() #elif defined(__PX4_QURT) #include "qurt_log.h" @@ -72,6 +73,7 @@ static inline void do_nothing(int level, ...) ****************************************************************************/ #define PX4_LOG(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ALWAYS, __FILE__, __LINE__, FMT, ##__VA_ARGS__) #define PX4_INFO(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ALWAYS, __FILE__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_BACKTRACE() #if defined(TRACE_BUILD) /**************************************************************************** @@ -127,8 +129,11 @@ __EXPORT extern uint64_t hrt_absolute_time(void); __EXPORT extern const char *__px4_log_level_str[5]; __EXPORT extern int __px4_log_level_current; +__EXPORT extern void px4_backtrace(void); __END_DECLS +#define PX4_BACKTRACE() px4_backtrace() + // __px4_log_level_current will be initialized to PX4_LOG_LEVEL_AT_RUN_TIME #define PX4_LOG_LEVEL_AT_RUN_TIME _PX4_LOG_LEVEL_ERROR @@ -173,11 +178,11 @@ __END_DECLS ****************************************************************************/ #define __px4_log_named_cond(name, cond, FMT, ...) \ __px4__log_printcond(cond,\ - "%s " \ - FMT\ - __px4__log_end_fmt \ - ,name, ##__VA_ARGS__\ - ) + "%s " \ + FMT\ + __px4__log_end_fmt \ + ,name, ##__VA_ARGS__\ + ) /**************************************************************************** * __px4_log: @@ -188,11 +193,11 @@ __END_DECLS ****************************************************************************/ #define __px4_log(level, FMT, ...) \ __px4__log_printline(level,\ - __px4__log_level_fmt \ - FMT\ - __px4__log_end_fmt \ - __px4__log_level_arg(level), ##__VA_ARGS__\ - ) + __px4__log_level_fmt \ + FMT\ + __px4__log_end_fmt \ + __px4__log_level_arg(level), ##__VA_ARGS__\ + ) /**************************************************************************** * __px4_log_timestamp: @@ -204,14 +209,14 @@ __END_DECLS ****************************************************************************/ #define __px4_log_timestamp(level, FMT, ...) \ __px4__log_printline(level,\ - __px4__log_level_fmt\ - __px4__log_timestamp_fmt\ - FMT\ - __px4__log_end_fmt\ - __px4__log_level_arg(level)\ - __px4__log_timestamp_arg\ - , ##__VA_ARGS__\ - ) + __px4__log_level_fmt\ + __px4__log_timestamp_fmt\ + FMT\ + __px4__log_end_fmt\ + __px4__log_level_arg(level)\ + __px4__log_timestamp_arg\ + , ##__VA_ARGS__\ + ) /**************************************************************************** * __px4_log_timestamp_thread: @@ -223,16 +228,16 @@ __END_DECLS ****************************************************************************/ #define __px4_log_timestamp_thread(level, FMT, ...) \ __px4__log_printline(level,\ - __px4__log_level_fmt\ - __px4__log_timestamp_fmt\ - __px4__log_thread_fmt\ - FMT\ - __px4__log_end_fmt\ - __px4__log_level_arg(level)\ - __px4__log_timestamp_arg\ - __px4__log_thread_arg\ - , ##__VA_ARGS__\ - ) + __px4__log_level_fmt\ + __px4__log_timestamp_fmt\ + __px4__log_thread_fmt\ + FMT\ + __px4__log_end_fmt\ + __px4__log_level_arg(level)\ + __px4__log_timestamp_arg\ + __px4__log_thread_arg\ + , ##__VA_ARGS__\ + ) /**************************************************************************** * __px4_log_file_and_line: @@ -244,16 +249,16 @@ __END_DECLS ****************************************************************************/ #define __px4_log_file_and_line(level, FMT, ...) \ __px4__log_printline(level,\ - __px4__log_level_fmt\ - __px4__log_timestamp_fmt\ - FMT\ - __px4__log_file_and_line_fmt\ - __px4__log_end_fmt\ - __px4__log_level_arg(level)\ - __px4__log_timestamp_arg\ - , ##__VA_ARGS__\ - __px4__log_file_and_line_arg\ - ) + __px4__log_level_fmt\ + __px4__log_timestamp_fmt\ + FMT\ + __px4__log_file_and_line_fmt\ + __px4__log_end_fmt\ + __px4__log_level_arg(level)\ + __px4__log_timestamp_arg\ + , ##__VA_ARGS__\ + __px4__log_file_and_line_arg\ + ) /**************************************************************************** * __px4_log_timestamp_file_and_line: @@ -266,16 +271,16 @@ __END_DECLS ****************************************************************************/ #define __px4_log_timestamp_file_and_line(level, FMT, ...) \ __px4__log_printline(level,\ - __px4__log_level_fmt\ - __px4__log_timestamp_fmt\ - FMT\ - __px4__log_file_and_line_fmt\ - __px4__log_end_fmt\ - __px4__log_level_arg(level)\ - __px4__log_timestamp_arg\ - , ##__VA_ARGS__\ - __px4__log_file_and_line_arg\ - ) + __px4__log_level_fmt\ + __px4__log_timestamp_fmt\ + FMT\ + __px4__log_file_and_line_fmt\ + __px4__log_end_fmt\ + __px4__log_level_arg(level)\ + __px4__log_timestamp_arg\ + , ##__VA_ARGS__\ + __px4__log_file_and_line_arg\ + ) /**************************************************************************** * __px4_log_thread_file_and_line: @@ -288,16 +293,16 @@ __END_DECLS ****************************************************************************/ #define __px4_log_thread_file_and_line(level, FMT, ...) \ __px4__log_printline(level,\ - __px4__log_level_fmt\ - __px4__log_thread_fmt\ - FMT\ - __px4__log_file_and_line_fmt\ - __px4__log_end_fmt\ - __px4__log_level_arg(level)\ - __px4__log_thread_arg\ - , ##__VA_ARGS__\ - __px4__log_file_and_line_arg\ - ) + __px4__log_level_fmt\ + __px4__log_thread_fmt\ + FMT\ + __px4__log_file_and_line_fmt\ + __px4__log_end_fmt\ + __px4__log_level_arg(level)\ + __px4__log_thread_arg\ + , ##__VA_ARGS__\ + __px4__log_file_and_line_arg\ + ) /**************************************************************************** * __px4_log_timestamp_thread_file_and_line: @@ -310,18 +315,18 @@ __END_DECLS ****************************************************************************/ #define __px4_log_timestamp_thread_file_and_line(level, FMT, ...) \ __px4__log_printline(level,\ - __px4__log_level_fmt\ - __px4__log_timestamp_fmt\ - __px4__log_thread_fmt\ - FMT\ - __px4__log_file_and_line_fmt\ - __px4__log_end_fmt\ - __px4__log_level_arg(level)\ - __px4__log_timestamp_arg\ - __px4__log_thread_arg\ - , ##__VA_ARGS__\ - __px4__log_file_and_line_arg\ - ) + __px4__log_level_fmt\ + __px4__log_timestamp_fmt\ + __px4__log_thread_fmt\ + FMT\ + __px4__log_file_and_line_fmt\ + __px4__log_end_fmt\ + __px4__log_level_arg(level)\ + __px4__log_timestamp_arg\ + __px4__log_thread_arg\ + , ##__VA_ARGS__\ + __px4__log_file_and_line_arg\ + ) /**************************************************************************** diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 497f3be04c..dc806aa905 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -86,7 +86,7 @@ public: template Subscriber *subscribe(void(*fp)(const T &), unsigned interval) { - SubscriberBase *sub = new SubscriberROS((ros::NodeHandle*)this, std::bind(fp, std::placeholders::_1)); + SubscriberBase *sub = new SubscriberROS((ros::NodeHandle *)this, std::bind(fp, std::placeholders::_1)); _subs.push_back(sub); return (Subscriber *)sub; } @@ -99,7 +99,7 @@ public: template Subscriber *subscribe(void(C::*fp)(const T &), C *obj, unsigned interval) { - SubscriberBase *sub = new SubscriberROS((ros::NodeHandle*)this, std::bind(fp, obj, std::placeholders::_1)); + SubscriberBase *sub = new SubscriberROS((ros::NodeHandle *)this, std::bind(fp, obj, std::placeholders::_1)); _subs.push_back(sub); return (Subscriber *)sub; } @@ -110,7 +110,7 @@ public: template Subscriber *subscribe(unsigned interval) { - SubscriberBase *sub = new SubscriberROS((ros::NodeHandle*)this); + SubscriberBase *sub = new SubscriberROS((ros::NodeHandle *)this); _subs.push_back(sub); return (Subscriber *)sub; } @@ -119,11 +119,11 @@ public: * Advertise topic */ template - Publisher* advertise() + Publisher *advertise() { - PublisherROS *pub = new PublisherROS((ros::NodeHandle*)this); - _pubs.push_back((PublisherBase*)pub); - return (Publisher*)pub; + PublisherROS *pub = new PublisherROS((ros::NodeHandle *)this); + _pubs.push_back((PublisherBase *)pub); + return (Publisher *)pub; } /** @@ -242,7 +242,7 @@ public: { PublisherUORB *pub = new PublisherUORB(); _pubs.add(pub); - return (Publisher*)pub; + return (Publisher *)pub; } /** diff --git a/src/platforms/px4_posix.h b/src/platforms/px4_posix.h index bd682d9ce5..ab53d03900 100644 --- a/src/platforms/px4_posix.h +++ b/src/platforms/px4_posix.h @@ -54,8 +54,7 @@ __BEGIN_DECLS -typedef struct -{ +typedef struct { pthread_mutex_t lock; pthread_cond_t wait; int value; @@ -97,7 +96,7 @@ typedef struct pollfd px4_pollfd_struct_t; #if defined(__cplusplus) #define _GLOBAL :: #else -#define _GLOBAL +#define _GLOBAL #endif #define px4_open _GLOBAL open #define px4_close _GLOBAL close @@ -118,14 +117,14 @@ typedef struct pollfd px4_pollfd_struct_t; typedef short pollevent_t; typedef struct { - /* This part of the struct is POSIX-like */ - int fd; /* The descriptor being polled */ - pollevent_t events; /* The input event flags */ - pollevent_t revents; /* The output event flags */ + /* This part of the struct is POSIX-like */ + int fd; /* The descriptor being polled */ + pollevent_t events; /* The input event flags */ + pollevent_t revents; /* The output event flags */ - /* Required for PX4 compatability */ - px4_sem_t *sem; /* Pointer to semaphore used to post output event */ - void *priv; /* For use by drivers */ + /* Required for PX4 compatability */ + px4_sem_t *sem; /* Pointer to semaphore used to post output event */ + void *priv; /* For use by drivers */ } px4_pollfd_struct_t; __BEGIN_DECLS @@ -150,8 +149,8 @@ extern int px4_errno; __EXPORT void px4_show_devices(void); __EXPORT void px4_show_files(void); -__EXPORT const char * px4_get_device_names(unsigned int *handle); +__EXPORT const char *px4_get_device_names(unsigned int *handle); __EXPORT void px4_show_topics(void); -__EXPORT const char * px4_get_topic_names(unsigned int *handle); +__EXPORT const char *px4_get_topic_names(unsigned int *handle); __END_DECLS diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index eb8e964e79..fb9ec50e10 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -86,7 +86,8 @@ public: */ PublisherROS(ros::NodeHandle *rnh) : Publisher(), - _ros_pub(rnh->advertisedata())>::type &>(T::handle(), kQueueSizeDefault)) + _ros_pub(rnh->advertise < typename std::remove_reference < decltype(((T *)nullptr)->data()) >::type & > (T::handle(), + kQueueSizeDefault)) {} ~PublisherROS() {}; @@ -136,7 +137,8 @@ public: _uorb_pub(new uORB::PublicationBase(T::handle())) {} - ~PublisherUORB() { + ~PublisherUORB() + { delete _uorb_pub; }; @@ -145,7 +147,7 @@ public: */ int publish(const T &msg) { - _uorb_pub->update((void *)&(msg.data())); + _uorb_pub->update((void *) & (msg.data())); return 0; } @@ -154,7 +156,7 @@ public: */ void update() {} ; private: - uORB::PublicationBase * _uorb_pub; /**< Handle to the publisher */ + uORB::PublicationBase *_uorb_pub; /**< Handle to the publisher */ }; #endif diff --git a/src/platforms/px4_spi.h b/src/platforms/px4_spi.h index 17397ee7ac..78b747b775 100644 --- a/src/platforms/px4_spi.h +++ b/src/platforms/px4_spi.h @@ -3,32 +3,29 @@ #ifdef __PX4_NUTTX #include #elif defined(__PX4_POSIX) -enum spi_dev_e -{ - SPIDEV_NONE = 0, /* Not a valid value */ - SPIDEV_MMCSD, /* Select SPI MMC/SD device */ - SPIDEV_FLASH, /* Select SPI FLASH device */ - SPIDEV_ETHERNET, /* Select SPI ethernet device */ - SPIDEV_DISPLAY, /* Select SPI LCD/OLED display device */ - SPIDEV_WIRELESS, /* Select SPI Wireless device */ - SPIDEV_TOUCHSCREEN, /* Select SPI touchscreen device */ - SPIDEV_EXPANDER, /* Select SPI I/O expander device */ - SPIDEV_MUX, /* Select SPI multiplexer device */ - SPIDEV_AUDIO_DATA, /* Select SPI audio codec device data port */ - SPIDEV_AUDIO_CTRL, /* Select SPI audio codec device control port */ +enum spi_dev_e { + SPIDEV_NONE = 0, /* Not a valid value */ + SPIDEV_MMCSD, /* Select SPI MMC/SD device */ + SPIDEV_FLASH, /* Select SPI FLASH device */ + SPIDEV_ETHERNET, /* Select SPI ethernet device */ + SPIDEV_DISPLAY, /* Select SPI LCD/OLED display device */ + SPIDEV_WIRELESS, /* Select SPI Wireless device */ + SPIDEV_TOUCHSCREEN, /* Select SPI touchscreen device */ + SPIDEV_EXPANDER, /* Select SPI I/O expander device */ + SPIDEV_MUX, /* Select SPI multiplexer device */ + SPIDEV_AUDIO_DATA, /* Select SPI audio codec device data port */ + SPIDEV_AUDIO_CTRL, /* Select SPI audio codec device control port */ }; /* Certain SPI devices may required differnt clocking modes */ -enum spi_mode_e -{ - SPIDEV_MODE0 = 0, /* CPOL=0 CHPHA=0 */ - SPIDEV_MODE1, /* CPOL=0 CHPHA=1 */ - SPIDEV_MODE2, /* CPOL=1 CHPHA=0 */ - SPIDEV_MODE3 /* CPOL=1 CHPHA=1 */ +enum spi_mode_e { + SPIDEV_MODE0 = 0, /* CPOL=0 CHPHA=0 */ + SPIDEV_MODE1, /* CPOL=0 CHPHA=1 */ + SPIDEV_MODE2, /* CPOL=1 CHPHA=0 */ + SPIDEV_MODE3 /* CPOL=1 CHPHA=1 */ }; -struct spi_dev_s -{ +struct spi_dev_s { int unused; }; #else diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index e92c82fc66..59cfa5a191 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -86,12 +86,12 @@ public: /** * Get the last message value */ - virtual T& get() {return _msg_current;} + virtual T &get() {return _msg_current;} /** * Get the last native message value */ - virtual decltype(((T*)nullptr)->data()) data() + virtual decltype(((T *)nullptr)->data()) data() { return _msg_current.data(); } @@ -135,7 +135,7 @@ protected: * Called on topic update, saves the current message and then calls the provided callback function * needs to use the native type as it is called by ROS */ - void callback(const typename std::remove_referencedata())>::type &msg) + void callback(const typename std::remove_reference < decltype(((T *)nullptr)->data()) >::type &msg) { /* Store data */ this->_msg_current.data() = msg; @@ -197,7 +197,8 @@ public: _uorb_sub(new uORB::SubscriptionBase(T::handle(), interval)) {} - virtual ~SubscriberUORB() { + virtual ~SubscriberUORB() + { delete _uorb_sub; }; @@ -219,19 +220,19 @@ public: int getUORBHandle() { return _uorb_sub->getHandle(); } protected: - uORB::SubscriptionBase * _uorb_sub; /**< Handle to the subscription */ + uORB::SubscriptionBase *_uorb_sub; /**< Handle to the subscription */ #ifndef CONFIG_ARCH_BOARD_SIM - typename std::remove_referencedata())>::type getUORBData() + typename std::remove_reference < decltype(((T *)nullptr)->data()) >::type getUORBData() { - return (typename std::remove_referencedata())>::type)*_uorb_sub; + return (typename std::remove_reference < decltype(((T *)nullptr)->data()) >::type) * _uorb_sub; } #endif /** * Get void pointer to last message value */ - void *get_void_ptr() { return (void *)&(this->_msg_current.data()); } + void *get_void_ptr() { return (void *) & (this->_msg_current.data()); } }; @@ -248,9 +249,9 @@ public: */ SubscriberUORBCallback(unsigned interval #ifndef CONFIG_ARCH_BOARD_SIM - ,std::function cbf) + , std::function cbf) #else - ) + ) #endif : SubscriberUORB(interval), diff --git a/src/platforms/px4_tasks.h b/src/platforms/px4_tasks.h index 9dc237e63b..04585aaced 100644 --- a/src/platforms/px4_tasks.h +++ b/src/platforms/px4_tasks.h @@ -105,11 +105,11 @@ __EXPORT void px4_systemreset(bool to_bootloader) noreturn_function; /** Starts a task and performs any specific accounting, scheduler setup, etc. */ __EXPORT px4_task_t px4_task_spawn_cmd(const char *name, - int priority, - int scheduler, - int stack_size, - px4_main_t entry, - char * const argv[]); + int priority, + int scheduler, + int stack_size, + px4_main_t entry, + char *const argv[]); /** Deletes a task - does not do resource cleanup **/ __EXPORT int px4_task_delete(px4_task_t pid); diff --git a/src/platforms/px4_time.h b/src/platforms/px4_time.h index 22751b3a99..d57f8e9103 100644 --- a/src/platforms/px4_time.h +++ b/src/platforms/px4_time.h @@ -30,8 +30,7 @@ __BEGIN_DECLS #if 0 #if !defined(__cplusplus) -struct timespec -{ +struct timespec { time_t tv_sec; long tv_nsec; }; diff --git a/src/platforms/px4_workqueue.h b/src/platforms/px4_workqueue.h index 76e1218ba5..3e3170f2de 100644 --- a/src/platforms/px4_workqueue.h +++ b/src/platforms/px4_workqueue.h @@ -48,7 +48,7 @@ #include #ifdef __PX4_QURT - #include +#include #endif __BEGIN_DECLS @@ -57,10 +57,9 @@ __BEGIN_DECLS #define LPWORK 1 #define NWORKERS 2 -struct wqueue_s -{ - pid_t pid; /* The task ID of the worker thread */ - struct dq_queue_s q; /* The queue of pending work */ +struct wqueue_s { + pid_t pid; /* The task ID of the worker thread */ + struct dq_queue_s q; /* The queue of pending work */ }; extern struct wqueue_s g_work[NWORKERS]; @@ -69,13 +68,12 @@ extern struct wqueue_s g_work[NWORKERS]; typedef void (*worker_t)(void *arg); -struct work_s -{ - struct dq_entry_s dq; /* Implements a doubly linked list */ - worker_t worker; /* Work callback */ - void *arg; /* Callback argument */ - uint64_t qtime; /* Time work queued */ - uint32_t delay; /* Delay until work performed */ +struct work_s { + struct dq_entry_s dq; /* Implements a doubly linked list */ + worker_t worker; /* Work callback */ + void *arg; /* Callback argument */ + uint64_t qtime; /* Time work queued */ + uint32_t delay; /* Delay until work performed */ }; /**************************************************************************** @@ -144,6 +142,6 @@ int work_lpthread(int argc, char *argv[]); __END_DECLS -#else +#else #error "Unknown target OS" #endif diff --git a/src/platforms/qurt/dspal/dspal_stub.c b/src/platforms/qurt/dspal/dspal_stub.c index fc736a169c..13542cdfd3 100644 --- a/src/platforms/qurt/dspal/dspal_stub.c +++ b/src/platforms/qurt/dspal/dspal_stub.c @@ -43,25 +43,29 @@ static void do_dlopen() char *error; void (*entry_function)() = NULL; - handle = dlopen ("libdspal_client.so", RTLD_LAZY); + handle = dlopen("libdspal_client.so", RTLD_LAZY); + if (!handle) { printf("Error opening libdspal_client.so\n"); return 1; } + entry_function = dlsym(handle, "dspal_entry"); + if (((error = dlerror()) != NULL) || (entry_function == NULL)) { printf("Error dlsym for dspal_entry"); ret = 2; } + dlclose(handle); #endif } - -int main(int argc, char* argv[]) + +int main(int argc, char *argv[]) { int ret = 0; - char *builtin[]={"libgcc.so", "libc.so", "libstdc++.so"}; + char *builtin[] = {"libgcc.so", "libc.so", "libstdc++.so"}; printf("In DSPAL main\n"); dlinit(3, builtin); diff --git a/src/platforms/qurt/include/crc32.h b/src/platforms/qurt/include/crc32.h index bf828e3e0e..34e080b1c2 100644 --- a/src/platforms/qurt/include/crc32.h +++ b/src/platforms/qurt/include/crc32.h @@ -63,7 +63,7 @@ extern "C" { ****************************************************************************/ EXTERN uint32_t crc32part(const uint8_t *src, size_t len, - uint32_t crc32val); + uint32_t crc32val); /**************************************************************************** * Name: crc32 diff --git a/src/platforms/qurt/include/i2c.h b/src/platforms/qurt/include/i2c.h index ee3e4196dd..5021cf1814 100644 --- a/src/platforms/qurt/include/i2c.h +++ b/src/platforms/qurt/include/i2c.h @@ -34,10 +34,9 @@ ****************************************************************************/ #pragma once -struct i2c_msg_s -{ - uint16_t addr; /* Slave address */ - uint16_t flags; /* See I2C_M_* definitions */ - uint8_t *buffer; - int length; +struct i2c_msg_s { + uint16_t addr; /* Slave address */ + uint16_t flags; /* See I2C_M_* definitions */ + uint8_t *buffer; + int length; }; diff --git a/src/platforms/qurt/include/queue.h b/src/platforms/qurt/include/queue.h index 4d95541e02..30dc264f3f 100644 --- a/src/platforms/qurt/include/queue.h +++ b/src/platforms/qurt/include/queue.h @@ -69,30 +69,26 @@ * Global Type Declarations ************************************************************************/ -struct sq_entry_s -{ - FAR struct sq_entry_s *flink; +struct sq_entry_s { + FAR struct sq_entry_s *flink; }; typedef struct sq_entry_s sq_entry_t; -struct dq_entry_s -{ - FAR struct dq_entry_s *flink; - FAR struct dq_entry_s *blink; +struct dq_entry_s { + FAR struct dq_entry_s *flink; + FAR struct dq_entry_s *blink; }; typedef struct dq_entry_s dq_entry_t; -struct sq_queue_s -{ - FAR sq_entry_t *head; - FAR sq_entry_t *tail; +struct sq_queue_s { + FAR sq_entry_t *head; + FAR sq_entry_t *tail; }; typedef struct sq_queue_s sq_queue_t; -struct dq_queue_s -{ - FAR dq_entry_t *head; - FAR dq_entry_t *tail; +struct dq_queue_s { + FAR dq_entry_t *head; + FAR dq_entry_t *tail; }; typedef struct dq_queue_s dq_queue_t; @@ -112,11 +108,11 @@ EXTERN void dq_addfirst(FAR dq_entry_t *node, dq_queue_t *queue); EXTERN void sq_addlast(FAR sq_entry_t *node, sq_queue_t *queue); EXTERN void dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue); EXTERN void sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node, - sq_queue_t *queue); + sq_queue_t *queue); EXTERN void dq_addafter(FAR dq_entry_t *prev, FAR dq_entry_t *node, - dq_queue_t *queue); + dq_queue_t *queue); EXTERN void dq_addbefore(FAR dq_entry_t *next, FAR dq_entry_t *node, - dq_queue_t *queue); + dq_queue_t *queue); EXTERN FAR sq_entry_t *sq_remafter(FAR sq_entry_t *node, sq_queue_t *queue); EXTERN void sq_rem(FAR sq_entry_t *node, sq_queue_t *queue); diff --git a/src/platforms/qurt/include/sched.h b/src/platforms/qurt/include/sched.h index 7d7724c5fa..b67db23b1d 100644 --- a/src/platforms/qurt/include/sched.h +++ b/src/platforms/qurt/include/sched.h @@ -3,8 +3,7 @@ #define SCHED_FIFO 1 #define SCHED_RR 2 -struct sched_param -{ +struct sched_param { int sched_priority; }; diff --git a/src/platforms/qurt/px4_layer/commands_hil.c b/src/platforms/qurt/px4_layer/commands_hil.c index 3ff299b89d..87aa6479f8 100644 --- a/src/platforms/qurt/px4_layer/commands_hil.c +++ b/src/platforms/qurt/px4_layer/commands_hil.c @@ -41,7 +41,7 @@ const char *get_commands(void); const char *get_commands() { - static const char *commands = + static const char *commands = "uorb start\n" "param set CAL_GYRO0_ID 2293760\n" "param set CAL_ACC0_ID 1310720\n" @@ -104,5 +104,5 @@ const char *get_commands() ; return commands; - + } diff --git a/src/platforms/qurt/px4_layer/commands_muorb.c b/src/platforms/qurt/px4_layer/commands_muorb.c index 27318af9c6..de6381a35a 100644 --- a/src/platforms/qurt/px4_layer/commands_muorb.c +++ b/src/platforms/qurt/px4_layer/commands_muorb.c @@ -39,42 +39,42 @@ const char *get_commands() { - static const char *commands = - "uorb start\n" - "muorb_test start\n"; + static const char *commands = + "uorb start\n" + "muorb_test start\n"; -/* - "hil mode_pwm\n" - "mixer load /dev/pwm_output0 /startup/quad_x.main.mix\n"; -*/ -/* - "param show\n" - "param set CAL_GYRO_ID 2293760\n" - "param set CAL_ACC0_ID 1310720\n" - "param set CAL_ACC1_ID 1376256\n" - "param set CAL_MAG0_ID 196608\n" - "gyrosim start\n" - "accelsim start\n" - "rgbled start\n" - "tone_alarm start\n" - "simulator start -s\n" - "commander start\n" - "sensors start\n" - "ekf_att_pos_estimator start\n" - "mc_pos_control start\n" - "mc_att_control start\n" - "param set MAV_TYPE 2\n" - "param set RC1_MAX 2015\n" - "param set RC1_MIN 996\n" - "param set RC_TRIM 1502\n" -*/ + /* + "hil mode_pwm\n" + "mixer load /dev/pwm_output0 /startup/quad_x.main.mix\n"; + */ + /* + "param show\n" + "param set CAL_GYRO_ID 2293760\n" + "param set CAL_ACC0_ID 1310720\n" + "param set CAL_ACC1_ID 1376256\n" + "param set CAL_MAG0_ID 196608\n" + "gyrosim start\n" + "accelsim start\n" + "rgbled start\n" + "tone_alarm start\n" + "simulator start -s\n" + "commander start\n" + "sensors start\n" + "ekf_att_pos_estimator start\n" + "mc_pos_control start\n" + "mc_att_control start\n" + "param set MAV_TYPE 2\n" + "param set RC1_MAX 2015\n" + "param set RC1_MIN 996\n" + "param set RC_TRIM 1502\n" + */ return commands; -/*====================================== Working set -======================================*/ - - //"muorb_test start\n" - //"gyrosim start\n" - //"adcsim start\n" - + /*====================================== Working set + ======================================*/ + + //"muorb_test start\n" + //"gyrosim start\n" + //"adcsim start\n" + } diff --git a/src/platforms/qurt/px4_layer/drv_hrt.c b/src/platforms/qurt/px4_layer/drv_hrt.c index b5d6bd20ff..c15c818172 100644 --- a/src/platforms/qurt/px4_layer/drv_hrt.c +++ b/src/platforms/qurt/px4_layer/drv_hrt.c @@ -219,7 +219,7 @@ hrt_call_enter(struct hrt_call *entry) * * This routine simulates a timer interrupt handler */ -static void +static void hrt_tim_isr(void *p) { @@ -249,7 +249,7 @@ hrt_call_reschedule() uint32_t ticks = USEC2TICK(HRT_INTERVAL_MAX); //printf("hrt_call_reschedule\n"); - + /* * Determine what the next deadline will be. * @@ -275,10 +275,10 @@ hrt_call_reschedule() } } - // There is no timer ISR, so simulate one by putting an event on the + // There is no timer ISR, so simulate one by putting an event on the // high priority work queue //printf("ticks = %u\n", ticks); - work_queue(HPWORK, &_hrt_work, (worker_t)&hrt_tim_isr, NULL, ticks); + work_queue(HPWORK, &_hrt_work, (worker_t)&hrt_tim_isr, NULL, ticks); } static void @@ -286,6 +286,7 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte { //printf("hrt_call_internal\n"); hrt_lock(); + //printf("hrt_call_internal after lock\n"); /* if the entry is currently queued, remove it */ /* note that we are using a potentially uninitialised @@ -295,8 +296,9 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte queue for the uninitialised entry->link but we don't do anything actually unsafe. */ - if (entry->deadline != 0) + if (entry->deadline != 0) { sq_rem(&entry->link, &callout_queue); + } entry->deadline = deadline; entry->period = interval; @@ -360,17 +362,20 @@ hrt_call_invoke(void) hrt_abstime deadline; hrt_lock(); + while (true) { /* get the current time */ hrt_abstime now = hrt_absolute_time(); call = (struct hrt_call *)sq_peek(&callout_queue); - if (call == NULL) + if (call == NULL) { break; + } - if (call->deadline > now) + if (call->deadline > now) { break; + } sq_rem(&call->link, &callout_queue); //lldbg("call pop\n"); @@ -404,6 +409,7 @@ hrt_call_invoke(void) hrt_call_enter(call); } } + hrt_unlock(); } diff --git a/src/platforms/qurt/px4_layer/lib_crc32.c b/src/platforms/qurt/px4_layer/lib_crc32.c index 4ba6fbf6df..c14ebfceeb 100644 --- a/src/platforms/qurt/px4_layer/lib_crc32.c +++ b/src/platforms/qurt/px4_layer/lib_crc32.c @@ -48,46 +48,45 @@ #include // Needed for Linux -#define FAR +#define FAR /************************************************************************************************ * Private Data ************************************************************************************************/ - -static const uint32_t crc32_tab[] = -{ - 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, - 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, - 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, - 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, - 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, - 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, - 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, - 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, - 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, - 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, - 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, - 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, - 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, - 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, - 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, - 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, - 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, - 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, - 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, - 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, - 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, - 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, - 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, - 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, - 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, - 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, - 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, - 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, - 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, - 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, - 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, - 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d + +static const uint32_t crc32_tab[] = { + 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, + 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, + 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, + 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, + 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, + 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, + 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, + 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, + 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, + 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, + 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, + 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, + 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, + 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, + 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, + 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, + 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, + 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, + 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, + 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, + 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, + 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, + 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, + 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, + 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, + 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, + 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, + 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, + 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, + 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, + 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, + 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d }; /************************************************************************************************ @@ -103,13 +102,13 @@ static const uint32_t crc32_tab[] = uint32_t crc32part(FAR const uint8_t *src, size_t len, uint32_t crc32val) { - size_t i; + size_t i; - for (i = 0; i < len; i++) - { - crc32val = crc32_tab[(crc32val ^ src[i]) & 0xff] ^ (crc32val >> 8); - } - return crc32val; + for (i = 0; i < len; i++) { + crc32val = crc32_tab[(crc32val ^ src[i]) & 0xff] ^ (crc32val >> 8); + } + + return crc32val; } /************************************************************************************************ @@ -122,5 +121,5 @@ uint32_t crc32part(FAR const uint8_t *src, size_t len, uint32_t crc32val) uint32_t crc32(FAR const uint8_t *src, size_t len) { - return crc32part(src, len, 0); + return crc32part(src, len, 0); } diff --git a/src/platforms/qurt/px4_layer/main.cpp b/src/platforms/qurt/px4_layer/main.cpp index 15907ded78..fc8801b60e 100644 --- a/src/platforms/qurt/px4_layer/main.cpp +++ b/src/platforms/qurt/px4_layer/main.cpp @@ -50,8 +50,8 @@ using namespace std; -extern void init_app_map(map &apps); -extern void list_builtins(map &apps); +extern void init_app_map(map &apps); +extern void list_builtins(map &apps); static px4_task_t g_dspal_task = -1; __BEGIN_DECLS @@ -61,24 +61,27 @@ extern const char *get_commands(void); void qurt_external_hook(void) __attribute__((weak)); __END_DECLS -static void run_cmd(map &apps, const vector &appargs) { +static void run_cmd(map &apps, const vector &appargs) +{ // command is appargs[0] string command = appargs[0]; + if (apps.find(command) != apps.end()) { - const char *arg[2+1]; + const char *arg[2 + 1]; unsigned int i = 0; + while (i < appargs.size() && appargs[i].c_str()[0] != '\0') { arg[i] = (char *)appargs[i].c_str(); PX4_WARN(" arg = '%s'\n", arg[i]); ++i; } + arg[i] = (char *)0; //PX4_DEBUG_PRINTF(i); - apps[command](i,(char **)arg); - } - else - { + apps[command](i, (char **)arg); + + } else { PX4_WARN("NOT FOUND."); list_builtins(apps); } @@ -88,14 +91,15 @@ void eat_whitespace(const char *&b, int &i) { // Eat whitespace while (b[i] == ' ' || b[i] == '\t') { ++i; } + b = &b[i]; - i=0; + i = 0; } -static void process_commands(map &apps, const char *cmds) +static void process_commands(map &apps, const char *cmds) { vector appargs; - int i=0; + int i = 0; const char *b = cmds; bool found_first_char = false; char arg[256]; @@ -103,7 +107,7 @@ static void process_commands(map &apps, const char *cmds) // Eat leading whitespace eat_whitespace(b, i); - for(;;) { + for (;;) { // End of command line if (b[i] == '\n' || b[i] == '\0') { strncpy(arg, b, i); @@ -112,20 +116,26 @@ 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()); - for(int ai=1;ai<(int)appargs.size();ai++) - PX4_WARN(" > arg: %s",appargs[ai].c_str()); - run_cmd(apps, appargs); + PX4_WARN("Processing command: %s", appargs[0].c_str()); + + for (int ai = 1; ai < (int)appargs.size(); ai++) { + PX4_WARN(" > arg: %s", appargs[ai].c_str()); + } + + run_cmd(apps, appargs); } + appargs.clear(); + if (b[i] == '\n') { eat_whitespace(b, ++i); continue; - } - else { + + } else { break; } } + // End of arg else if (b[i] == ' ') { strncpy(arg, b, i); @@ -134,11 +144,13 @@ static void process_commands(map &apps, const char *cmds) eat_whitespace(b, ++i); continue; } + ++i; } } -namespace px4 { +namespace px4 +{ extern void init_once(void); }; @@ -147,23 +159,25 @@ int dspal_main(int argc, char *argv[]); __END_DECLS -int dspal_entry( int argc, char* argv[] ) +int dspal_entry(int argc, char *argv[]) { PX4_INFO("In main\n"); - map apps; + map apps; init_app_map(apps); px4::init_once(); px4::init(argc, (char **)argv, "mainapp"); process_commands(apps, get_commands()); sleep(1); // give time for all commands to execute before starting external function - if(qurt_external_hook) - { + + if (qurt_external_hook) { qurt_external_hook(); } - for(;;){ + + for (;;) { sleep(1); } - return 0; + + return 0; } static void usage() @@ -174,32 +188,32 @@ static void usage() extern "C" { -int dspal_main(int argc, char *argv[]) -{ - int ret = 0; - if (argc == 2 && strcmp(argv[1], "start") == 0) { + int dspal_main(int argc, char *argv[]) + { + int ret = 0; + + if (argc == 2 && strcmp(argv[1], "start") == 0) { g_dspal_task = px4_task_spawn_cmd("dspal", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 1500, - dspal_entry, - argv); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 1500, + dspal_entry, + argv); - } - else if (argc == 2 && strcmp(argv[1], "stop") == 0) { - if (g_dspal_task < 0) { - PX4_WARN("start up thread not running"); - } - else { - px4_task_delete(g_dspal_task); - g_dspal_task = -1; - } - } - else { - usage(); - ret = -1; - } + } else if (argc == 2 && strcmp(argv[1], "stop") == 0) { + if (g_dspal_task < 0) { + PX4_WARN("start up thread not running"); - return ret; -} + } else { + px4_task_delete(g_dspal_task); + g_dspal_task = -1; + } + + } else { + usage(); + ret = -1; + } + + return ret; + } } diff --git a/src/platforms/qurt/px4_layer/params.c b/src/platforms/qurt/px4_layer/params.c index 3ad35a6a12..654b07df00 100644 --- a/src/platforms/qurt/px4_layer/params.c +++ b/src/platforms/qurt/px4_layer/params.c @@ -39,5 +39,5 @@ /** * @board QuRT_App */ -PARAM_DEFINE_INT32(MAV_TYPE,2); +PARAM_DEFINE_INT32(MAV_TYPE, 2); diff --git a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp index 14ef3304bc..be419d4069 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp @@ -61,11 +61,13 @@ extern uint64_t get_ticks_per_us(); //long PX4_TICKS_PER_SEC = 1000L; -unsigned int sleep(unsigned int sec) -{ - for (unsigned int i=0; i< sec; i++) +unsigned int sleep(unsigned int sec) +{ + for (unsigned int i = 0; i < sec; i++) { usleep(1000000); - return 0; + } + + return 0; } extern void hrt_init(void); @@ -96,16 +98,16 @@ void init_once(void) { // Required for QuRT //_posix_init(); - PX4_WARN( "Before calling work_queue_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" ); + PX4_WARN("Before calling hrt_init"); hrt_work_queue_init(); hrt_init(); - PX4_WARN( "after calling hrt_init" ); + PX4_WARN("after calling hrt_init"); } void init(int argc, char *argv[], const char *app_name) @@ -142,9 +144,11 @@ dm_write( size_t strnlen(const char *s, size_t maxlen) { - size_t i=0; - while (s[i] != '\0' && i < maxlen) + size_t i = 0; + + while (s[i] != '\0' && i < maxlen) { ++i; + } return i; } @@ -154,7 +158,7 @@ int ioctl(int a, int b, unsigned long c) return -1; } -int write(int a, char const* b, int c) +int write(int a, char const *b, int c) { return -1; } diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index 0e7fdc209a..4216bbf599 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -66,8 +66,7 @@ pthread_t _shell_task_id = 0; -struct task_entry -{ +struct task_entry { pthread_t pid; std::string name; bool isused; @@ -76,31 +75,30 @@ struct task_entry static task_entry taskmap[PX4_MAX_TASKS]; -typedef struct -{ +typedef struct { px4_main_t entry; int argc; char *argv[]; - // strings are allocated after the + // strings are allocated after the } pthdata_t; -static void *entry_adapter ( void *ptr ) +static void *entry_adapter(void *ptr) { - pthdata_t *data; - data = (pthdata_t *) ptr; + pthdata_t *data; + data = (pthdata_t *) ptr; data->entry(data->argc, data->argv); - PX4_WARN( "Before waiting infinte busy loop" ); - //for( ;; ) - //{ - // volatile int x = 0; - // ++x; - // } + PX4_WARN("Before waiting infinte busy loop"); + //for( ;; ) + //{ + // volatile int x = 0; + // ++x; + // } free(ptr); - px4_task_exit(0); + px4_task_exit(0); return NULL; -} +} void px4_systemreset(bool to_bootloader) @@ -108,7 +106,8 @@ px4_systemreset(bool to_bootloader) PX4_WARN("Called px4_system_reset"); } -px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char * const argv[]) +px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, + char *const argv[]) { int rv; int argc = 0; @@ -116,83 +115,96 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int unsigned int len = 0; unsigned long offset; unsigned long structsize; - char * p = (char *)argv; + char *p = (char *)argv; PX4_DEBUG("Creating %s\n", name); - pthread_t task; + pthread_t task; pthread_attr_t attr; struct sched_param param; // Calculate argc while (p != (char *)0) { p = argv[argc]; - if (p == (char *)0) - break; - ++argc; - len += strlen(p)+1; - } - structsize = sizeof(pthdata_t)+(argc+1)*sizeof(char *); - pthdata_t *taskdata; - - // not safe to pass stack data to the thread creation - taskdata = (pthdata_t *)malloc(structsize+len); - offset = ((unsigned long)taskdata)+structsize; - taskdata->entry = entry; + if (p == (char *)0) { + break; + } + + ++argc; + len += strlen(p) + 1; + } + + structsize = sizeof(pthdata_t) + (argc + 1) * sizeof(char *); + pthdata_t *taskdata; + + // not safe to pass stack data to the thread creation + taskdata = (pthdata_t *)malloc(structsize + len); + offset = ((unsigned long)taskdata) + structsize; + + taskdata->entry = entry; taskdata->argc = argc; - for (i=0; iargv[i] = (char *)offset; strcpy((char *)offset, argv[i]); - offset+=strlen(argv[i])+1; + offset += strlen(argv[i]) + 1; } + // Must add NULL at end of argv taskdata->argv[argc] = (char *)0; rv = pthread_attr_init(&attr); + if (rv != 0) { PX4_WARN("px4_task_spawn_cmd: failed to init thread attrs"); return (rv < 0) ? rv : -rv; } + #if 0 rv = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED); + if (rv != 0) { PX4_WARN("px4_task_spawn_cmd: failed to set inherit sched"); return (rv < 0) ? rv : -rv; } + rv = pthread_attr_setschedpolicy(&attr, scheduler); + if (rv != 0) { PX4_WARN("px4_task_spawn_cmd: failed to set sched policy"); return (rv < 0) ? rv : -rv; } + #endif - size_t fixed_stacksize = -1; - pthread_attr_getstacksize(&attr, &fixed_stacksize); - PX4_WARN("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; + size_t fixed_stacksize = -1; + pthread_attr_getstacksize(&attr, &fixed_stacksize); + PX4_WARN("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_WARN("setting the thread[%s] stack size to[%d]", name, fixed_stacksize); + pthread_attr_setstacksize(&attr, fixed_stacksize); + //pthread_attr_setstacksize(&attr, stack_size); - PX4_WARN("setting the thread[%s] stack size to[%d]", name, fixed_stacksize ); - pthread_attr_setstacksize(&attr, fixed_stacksize); - //pthread_attr_setstacksize(&attr, stack_size); - param.sched_priority = priority; rv = pthread_attr_setschedparam(&attr, ¶m); + if (rv != 0) { PX4_WARN("px4_task_spawn_cmd: failed to set sched param"); return (rv < 0) ? rv : -rv; } - rv = pthread_create (&task, &attr, &entry_adapter, (void *) taskdata); + rv = pthread_create(&task, &attr, &entry_adapter, (void *) taskdata); + if (rv != 0) { return (rv < 0) ? rv : -rv; } - for (i=0; i=PX4_MAX_TASKS) { + + if (i >= PX4_MAX_TASKS) { return -ENOSPC; } - return i; + + return i; } int px4_task_delete(px4_task_t id) @@ -212,15 +226,18 @@ int px4_task_delete(px4_task_t id) pthread_t pid; PX4_WARN("Called px4_task_delete"); - if (id < PX4_MAX_TASKS && taskmap[id].isused) + if (id < PX4_MAX_TASKS && taskmap[id].isused) { pid = taskmap[id].pid; - else + + } else { return -EINVAL; + } // If current thread then exit, otherwise cancel - if (pthread_self() == pid) { + if (pthread_self() == pid) { taskmap[id].isused = false; pthread_exit(0); + } else { rv = pthread_cancel(pid); } @@ -232,20 +249,21 @@ int px4_task_delete(px4_task_t id) void px4_task_exit(int ret) { - int i; + int i; pthread_t pid = pthread_self(); // Get pthread ID from the opaque ID - for (i=0; i=PX4_MAX_TASKS) { + + if (i >= PX4_MAX_TASKS) { PX4_ERR("px4_task_exit: self task not found!"); - } - else { + + } else { PX4_DEBUG("px4_task_exit: %s", taskmap[i].name.c_str()); } @@ -258,10 +276,12 @@ int px4_task_kill(px4_task_t id, int sig) pthread_t pid; PX4_DEBUG("Called px4_task_kill %d, taskname %s", sig, taskmap[id].name.c_str()); - if (id < PX4_MAX_TASKS && taskmap[id].pid != 0) + if (id < PX4_MAX_TASKS && taskmap[id].pid != 0) { pid = taskmap[id].pid; - else + + } else { return -EINVAL; + } // If current thread then exit, otherwise cancel rv = pthread_kill(pid, sig); @@ -275,15 +295,17 @@ void px4_show_tasks() int count = 0; PX4_INFO("Active Tasks:"); - for (idx=0; idx < PX4_MAX_TASKS; idx++) - { + + for (idx = 0; idx < PX4_MAX_TASKS; idx++) { if (taskmap[idx].isused) { PX4_INFO(" %-10s %d", taskmap[idx].name.c_str(), taskmap[idx].pid); count++; } } - if (count == 0) + + if (count == 0) { PX4_INFO(" No running tasks"); + } } @@ -297,11 +319,12 @@ int px4_getpid() // return SHELL_TASK_ID; // Get pthread ID from the opaque ID - for (int i=0; i -class HelloExample { +class HelloExample +{ public: HelloExample() {}; diff --git a/src/platforms/qurt/tests/hello/hello_start_qurt.cpp b/src/platforms/qurt/tests/hello/hello_start_qurt.cpp index e4180307ce..56aa52b478 100644 --- a/src/platforms/qurt/tests/hello/hello_start_qurt.cpp +++ b/src/platforms/qurt/tests/hello/hello_start_qurt.cpp @@ -71,11 +71,11 @@ int hello_main(int argc, char *argv[]) } daemon_task = px4_task_spawn_cmd("hello", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 16000, - PX4_MAIN, - (char* const*)argv); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 16000, + PX4_MAIN, + (char *const *)argv); return 0; } diff --git a/src/platforms/qurt/tests/muorb/muorb_test_example.h b/src/platforms/qurt/tests/muorb/muorb_test_example.h index 266b0a2b42..68236eb854 100644 --- a/src/platforms/qurt/tests/muorb/muorb_test_example.h +++ b/src/platforms/qurt/tests/muorb/muorb_test_example.h @@ -37,7 +37,8 @@ #include "uORB/topics/esc_status.h" #include "uORB/topics/vehicle_command.h" -class MuorbTestExample { +class MuorbTestExample +{ public: MuorbTestExample() {}; ~MuorbTestExample() {}; diff --git a/src/platforms/qurt/tests/muorb/muorb_test_main.cpp b/src/platforms/qurt/tests/muorb/muorb_test_main.cpp index 8f8b7640e2..c6818a9270 100644 --- a/src/platforms/qurt/tests/muorb/muorb_test_main.cpp +++ b/src/platforms/qurt/tests/muorb/muorb_test_main.cpp @@ -42,7 +42,7 @@ #include #include "muorb_test_example.h" -extern "C" __EXPORT int muorb_test_entry( int argc, char** argv ); +extern "C" __EXPORT int muorb_test_entry(int argc, char **argv); int muorb_test_entry(int argc, char **argv) { diff --git a/src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp b/src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp index 6d64cfec8b..ba44169ec3 100644 --- a/src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp +++ b/src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp @@ -80,15 +80,15 @@ int muorb_test_main(int argc, char *argv[]) /* this is not an error */ return 0; } - - PX4_INFO( "before starting the muorb_test_entry task" ); + + PX4_INFO("before starting the muorb_test_entry task"); daemon_task = px4_task_spawn_cmd("muorb_test", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 8192, - muorb_test_entry, - (char* const*)argv); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 8192, + muorb_test_entry, + (char *const *)argv); return 0; } diff --git a/src/platforms/ros/geo.cpp b/src/platforms/ros/geo.cpp index 2f277d7f16..d13a5e277a 100644 --- a/src/platforms/ros/geo.cpp +++ b/src/platforms/ros/geo.cpp @@ -182,7 +182,8 @@ __EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference return ref->timestamp; } -__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, + uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { ref->lat_rad = lat_0 * M_DEG_TO_RAD; @@ -196,12 +197,14 @@ __EXPORT int map_projection_init_timestamped(struct map_projection_reference_s * return 0; } -__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, + double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { return map_projection_init_timestamped(ref, lat_0, lon_0, px4::get_time_micros()); } -__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad) +__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, + double *ref_lon_rad) { if (!map_projection_initialized(ref)) { return -1; @@ -213,7 +216,8 @@ __EXPORT int map_projection_reference(const struct map_projection_reference_s *r return 0; } -__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y) +__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, + float *y) { if (!map_projection_initialized(ref)) { return -1; @@ -235,7 +239,8 @@ __EXPORT int map_projection_project(const struct map_projection_reference_s *ref return 0; } -__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon) +__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, + double *lon) { if (!map_projection_initialized(ref)) { return -1; diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp index 54086cfd4b..38db5dad7f 100644 --- a/src/platforms/ros/nodes/commander/commander.cpp +++ b/src/platforms/ros/nodes/commander/commander.cpp @@ -109,45 +109,45 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon } void Commander::SetOffboardControl(const px4::offboard_control_mode &msg_offboard_control_mode, - px4::vehicle_control_mode &msg_vehicle_control_mode) + px4::vehicle_control_mode &msg_vehicle_control_mode) { msg_vehicle_control_mode.flag_control_manual_enabled = false; msg_vehicle_control_mode.flag_control_offboard_enabled = true; msg_vehicle_control_mode.flag_control_auto_enabled = false; msg_vehicle_control_mode.flag_control_rates_enabled = !msg_offboard_control_mode.ignore_bodyrate || - !msg_offboard_control_mode.ignore_attitude || - !msg_offboard_control_mode.ignore_position || - !msg_offboard_control_mode.ignore_velocity || - !msg_offboard_control_mode.ignore_acceleration_force; + !msg_offboard_control_mode.ignore_attitude || + !msg_offboard_control_mode.ignore_position || + !msg_offboard_control_mode.ignore_velocity || + !msg_offboard_control_mode.ignore_acceleration_force; msg_vehicle_control_mode.flag_control_attitude_enabled = !msg_offboard_control_mode.ignore_attitude || - !msg_offboard_control_mode.ignore_position || - !msg_offboard_control_mode.ignore_velocity || - !msg_offboard_control_mode.ignore_acceleration_force; + !msg_offboard_control_mode.ignore_position || + !msg_offboard_control_mode.ignore_velocity || + !msg_offboard_control_mode.ignore_acceleration_force; msg_vehicle_control_mode.flag_control_velocity_enabled = !msg_offboard_control_mode.ignore_velocity || - !msg_offboard_control_mode.ignore_position; + !msg_offboard_control_mode.ignore_position; msg_vehicle_control_mode.flag_control_climb_rate_enabled = !msg_offboard_control_mode.ignore_velocity || - !msg_offboard_control_mode.ignore_position; + !msg_offboard_control_mode.ignore_position; msg_vehicle_control_mode.flag_control_position_enabled = !msg_offboard_control_mode.ignore_position; msg_vehicle_control_mode.flag_control_altitude_enabled = !msg_offboard_control_mode.ignore_velocity || - !msg_offboard_control_mode.ignore_position; + !msg_offboard_control_mode.ignore_position; } void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, - px4::vehicle_status &msg_vehicle_status, - px4::vehicle_control_mode &msg_vehicle_control_mode) { + px4::vehicle_status &msg_vehicle_status, + px4::vehicle_control_mode &msg_vehicle_control_mode) +{ // XXX this is a minimal implementation. If more advanced functionalities are // needed consider a full port of the commander - if (msg->offboard_switch == px4::manual_control_setpoint::SWITCH_POS_ON) - { + if (msg->offboard_switch == px4::manual_control_setpoint::SWITCH_POS_ON) { SetOffboardControl(_msg_offboard_control_mode, msg_vehicle_control_mode); return; } @@ -155,7 +155,7 @@ void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, msg_vehicle_control_mode.flag_control_offboard_enabled = false; switch (msg->mode_switch) { - case px4::manual_control_setpoint::SWITCH_POS_NONE: + case px4::manual_control_setpoint::SWITCH_POS_NONE: ROS_WARN("Joystick button mapping error, main mode not set"); break; @@ -183,6 +183,7 @@ void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, msg_vehicle_control_mode.flag_control_climb_rate_enabled = true; msg_vehicle_control_mode.flag_control_position_enabled = true; msg_vehicle_control_mode.flag_control_velocity_enabled = true; + } else { msg_vehicle_status.main_state = msg_vehicle_status.MAIN_STATE_ALTCTL; msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_ALTCTL; @@ -194,6 +195,7 @@ void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, msg_vehicle_control_mode.flag_control_position_enabled = false; msg_vehicle_control_mode.flag_control_velocity_enabled = false; } + break; } @@ -206,20 +208,20 @@ void Commander::OffboardControlModeCallback(const px4::offboard_control_modeCons /* Force system into offboard control mode */ if (!_got_manual_control) { SetOffboardControl(_msg_offboard_control_mode, _msg_vehicle_control_mode); - + _msg_vehicle_status.timestamp = px4::get_time_micros(); _msg_vehicle_status.hil_state = _msg_vehicle_status.HIL_STATE_OFF; _msg_vehicle_status.hil_state = _msg_vehicle_status.VEHICLE_TYPE_QUADROTOR; _msg_vehicle_status.is_rotary_wing = true; _msg_vehicle_status.arming_state = _msg_vehicle_status.ARMING_STATE_ARMED; - + _msg_actuator_armed.armed = true; _msg_actuator_armed.timestamp = px4::get_time_micros(); _msg_vehicle_control_mode.timestamp = px4::get_time_micros(); _msg_vehicle_control_mode.flag_armed = true; - + _vehicle_control_mode_pub.publish(_msg_vehicle_control_mode); _actuator_armed_pub.publish(_msg_actuator_armed); diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h index 856144389d..67bee544ab 100644 --- a/src/platforms/ros/nodes/commander/commander.h +++ b/src/platforms/ros/nodes/commander/commander.h @@ -68,14 +68,14 @@ protected: * Set control mode flags based on stick positions (equiv to code in px4 commander) */ void EvalSwitches(const px4::manual_control_setpointConstPtr &msg, - px4::vehicle_status &msg_vehicle_status, - px4::vehicle_control_mode &msg_vehicle_control_mode); + px4::vehicle_status &msg_vehicle_status, + px4::vehicle_control_mode &msg_vehicle_control_mode); /** * Sets offboard controll flags in msg_vehicle_control_mode */ void SetOffboardControl(const px4::offboard_control_mode &msg_offboard_control_mode, - px4::vehicle_control_mode &msg_vehicle_control_mode); + px4::vehicle_control_mode &msg_vehicle_control_mode); ros::NodeHandle _n; ros::Subscriber _man_ctrl_sp_sub; diff --git a/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp index 328f545c6b..7b02a6f07e 100644 --- a/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp +++ b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp @@ -65,15 +65,18 @@ int DemoOffboardAttitudeSetpoints::main() /* Publish example offboard attitude setpoint */ geometry_msgs::PoseStamped pose; - tf::Quaternion q = tf::createQuaternionFromRPY(0.0, 0.1 * (sinf(0.5 * (float)px4::get_time_micros() / 1000000.0f)) , 0.0); + tf::Quaternion q = tf::createQuaternionFromRPY(0.0, 0.1 * (sinf(0.5 * (float)px4::get_time_micros() / 1000000.0f)) , + 0.0); quaternionTFToMsg(q, pose.pose.orientation); _attitude_sp_pub.publish(pose); std_msgs::Float64 thrust; - thrust.data = 0.4f + 0.25 * (sinf((float)px4::get_time_micros() / 1000000.0f)); // just some example throttle input that makes the quad 'jump' + thrust.data = 0.4f + 0.25 * (sinf((float)px4::get_time_micros() / + 1000000.0f)); // just some example throttle input that makes the quad 'jump' _thrust_sp_pub.publish(thrust); } + return 0; } diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp index 8488c518f5..fc8db220c3 100644 --- a/src/platforms/ros/nodes/manual_input/manual_input.cpp +++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp @@ -116,12 +116,14 @@ void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, do if (val + offset > deadzone || val + offset < -deadzone) { out = (float)((val + offset)) * scale; + } else { out = 0.0f; } } -void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_control_setpoint &msg_mc_sp) { +void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_control_setpoint &msg_mc_sp) +{ msg_mc_sp.acro_switch = px4::manual_control_setpoint::SWITCH_POS_NONE; if (_buttons_state[MAIN_STATE_MANUAL] != msg->buttons[_param_buttons_map[MAIN_STATE_MANUAL]] == true) { @@ -150,6 +152,7 @@ void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_co msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; return; + } else if (_buttons_state[MAIN_STATE_OFFBOARD] != msg->buttons[_param_buttons_map[MAIN_STATE_OFFBOARD]] == true) { msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_MIDDLE; msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp index 567acc200e..e015266809 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.cpp +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -66,7 +66,8 @@ int main(int argc, char **argv) ros::init(argc, argv, "mavlink"); ros::NodeHandle privateNh("~"); std::string mavlink_fcu_url; - privateNh.param("mavlink_fcu_url", mavlink_fcu_url, std::string("udp://localhost:14565@localhost:14560")); + privateNh.param("mavlink_fcu_url", mavlink_fcu_url, + std::string("udp://localhost:14565@localhost:14560")); Mavlink m(mavlink_fcu_url); ros::spin(); return 0; @@ -76,18 +77,18 @@ void Mavlink::VehicleAttitudeCallback(const vehicle_attitudeConstPtr &msg) { mavlink_message_t msg_m; mavlink_msg_attitude_quaternion_pack_chan( - _link->get_system_id(), - _link->get_component_id(), - _link->get_channel(), - &msg_m, - get_time_micros() / 1000, - msg->q[0], - msg->q[1], - msg->q[2], - msg->q[3], - msg->rollspeed, - msg->pitchspeed, - msg->yawspeed); + _link->get_system_id(), + _link->get_component_id(), + _link->get_channel(), + &msg_m, + get_time_micros() / 1000, + msg->q[0], + msg->q[1], + msg->q[2], + msg->q[3], + msg->rollspeed, + msg->pitchspeed, + msg->yawspeed); _link->send_message(&msg_m); } @@ -95,33 +96,36 @@ void Mavlink::VehicleLocalPositionCallback(const vehicle_local_positionConstPtr { mavlink_message_t msg_m; mavlink_msg_local_position_ned_pack_chan( - _link->get_system_id(), - _link->get_component_id(), - _link->get_channel(), - &msg_m, - get_time_micros() / 1000, - msg->x, - msg->y, - msg->z, - msg->vx, - msg->vy, - msg->vz); + _link->get_system_id(), + _link->get_component_id(), + _link->get_channel(), + &msg_m, + get_time_micros() / 1000, + msg->x, + msg->y, + msg->z, + msg->vx, + msg->vy, + msg->vz); _link->send_message(&msg_m); } -void Mavlink::handle_msg(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid) { +void Mavlink::handle_msg(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid) +{ (void)sysid; (void)compid; - switch(mmsg->msgid) { - case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: - handle_msg_set_attitude_target(mmsg); - break; - case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: - handle_msg_set_position_target_local_ned(mmsg); - break; - default: - break; + switch (mmsg->msgid) { + case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: + handle_msg_set_attitude_target(mmsg); + break; + + case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: + handle_msg_set_position_target_local_ned(mmsg); + break; + + default: + break; } } @@ -146,10 +150,12 @@ void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) /* Message want's us to ignore everything except thrust: only ignore if previously ignored */ _offboard_control_mode.ignore_bodyrate = ignore_bodyrate && _offboard_control_mode.ignore_bodyrate; _offboard_control_mode.ignore_attitude = ignore_attitude && _offboard_control_mode.ignore_attitude; + } else { _offboard_control_mode.ignore_bodyrate = ignore_bodyrate; _offboard_control_mode.ignore_attitude = ignore_attitude; } + _offboard_control_mode.ignore_position = true; _offboard_control_mode.ignore_velocity = true; _offboard_control_mode.ignore_acceleration_force = true; @@ -162,13 +168,14 @@ void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) */ _att_sp.timestamp = get_time_micros(); + if (!ignore_attitude) { mavlink_quaternion_to_euler(set_attitude_target.q, &_att_sp.roll_body, &_att_sp.pitch_body, - &_att_sp.yaw_body); + &_att_sp.yaw_body); mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])_att_sp.R_body.data()); _att_sp.R_valid = true; } - + if (!_offboard_control_mode.ignore_thrust) { _att_sp.thrust = set_attitude_target.thrust; @@ -178,6 +185,7 @@ void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) for (ssize_t i = 0; i < 4; i++) { _att_sp.q_d[i] = set_attitude_target.q[i]; } + _att_sp.q_d_valid = true; } @@ -200,9 +208,9 @@ void Mavlink::handle_msg_set_position_target_local_ned(const mavlink_message_t * /* Only accept messages which are intended for this system */ // XXX removed for sitl, makes maybe sense to re-introduce at some point // if ((mavlink_system.sysid == set_position_target_local_ned.target_system || - // set_position_target_local_ned.target_system == 0) && - // (mavlink_system.compid == set_position_target_local_ned.target_component || - // set_position_target_local_ned.target_component == 0)) { + // set_position_target_local_ned.target_system == 0) && + // (mavlink_system.compid == set_position_target_local_ned.target_component || + // set_position_target_local_ned.target_component == 0)) { /* convert mavlink type (local, NED) to uORB offboard control struct */ offboard_control_mode.ignore_position = (bool)(set_position_target_local_ned.type_mask & 0x7); @@ -223,7 +231,7 @@ void Mavlink::handle_msg_set_position_target_local_ned(const mavlink_message_t * * gets published only if in offboard mode. We leave that out for now. */ if (is_force_sp && offboard_control_mode.ignore_position && - offboard_control_mode.ignore_velocity) { + offboard_control_mode.ignore_velocity) { /* The offboard setpoint is a force setpoint only, directly writing to the force * setpoint topic and not publishing the setpoint triplet topic */ vehicle_force_setpoint force_sp; @@ -233,6 +241,7 @@ void Mavlink::handle_msg_set_position_target_local_ned(const mavlink_message_t * //XXX: yaw _force_sp_pub.publish(force_sp); + } else { /* It's not a pure force setpoint: publish to setpoint triplet topic */ position_setpoint_triplet pos_sp_triplet; @@ -247,6 +256,7 @@ void Mavlink::handle_msg_set_position_target_local_ned(const mavlink_message_t * pos_sp_triplet.current.x = set_position_target_local_ned.x; pos_sp_triplet.current.y = set_position_target_local_ned.y; pos_sp_triplet.current.z = set_position_target_local_ned.z; + } else { pos_sp_triplet.current.position_valid = false; } @@ -257,6 +267,7 @@ void Mavlink::handle_msg_set_position_target_local_ned(const mavlink_message_t * pos_sp_triplet.current.vx = set_position_target_local_ned.vx; pos_sp_triplet.current.vy = set_position_target_local_ned.vy; pos_sp_triplet.current.vz = set_position_target_local_ned.vz; + } else { pos_sp_triplet.current.velocity_valid = false; } @@ -292,6 +303,7 @@ void Mavlink::handle_msg_set_position_target_local_ned(const mavlink_message_t * } else { pos_sp_triplet.current.yawspeed_valid = false; } + //XXX handle global pos setpoints (different MAV frames) _pos_sp_triplet_pub.publish(pos_sp_triplet); diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp index 002a112b69..67084c64bb 100644 --- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -221,14 +221,19 @@ void MultirotorMixer::actuatorControlsCallback(const px4::actuator_controls_0 &m // switch mixer if necessary std::string mixer_name; _n.getParamCached("mixer", mixer_name); + if (mixer_name == "x") { _rotors = _config_index[0]; + } else if (mixer_name == "+") { _rotors = _config_index[1]; + } else if (mixer_name == "e") { _rotors = _config_index[2]; + } else if (mixer_name == "w") { _rotors = _config_index[3]; + } else if (mixer_name == "i") { _rotors = _config_index[4]; } diff --git a/unittests/googletest b/unittests/googletest new file mode 160000 index 0000000000..c99458533a --- /dev/null +++ b/unittests/googletest @@ -0,0 +1 @@ +Subproject commit c99458533a9b4c743ed51537e25989ea55944908 diff --git a/unittests/gtest b/unittests/gtest deleted file mode 160000 index cdef6e4ce0..0000000000 --- a/unittests/gtest +++ /dev/null @@ -1 +0,0 @@ -Subproject commit cdef6e4ce097f953445446e8da4cb8bb68478bc5