diff --git a/.gitignore b/.gitignore index 14183ed177..07848d7a90 100644 --- a/.gitignore +++ b/.gitignore @@ -57,3 +57,6 @@ ROMFS/*/*/rc.autostart rootfs/ *.autosave CMakeLists.txt.user +GPATH +GRTAGS +GTAGS diff --git a/.gitmodules b/.gitmodules index 744dcd2787..b84b78548e 100644 --- a/.gitmodules +++ b/.gitmodules @@ -15,7 +15,7 @@ url = https://github.com/ros/gencpp.git [submodule "src/lib/dspal"] path = src/lib/dspal - url = https://github.com/mcharleb/dspal.git + url = https://github.com/ATLFlight/dspal.git [submodule "Tools/jMAVSim"] path = Tools/jMAVSim url = https://github.com/PX4/jMAVSim.git @@ -28,3 +28,6 @@ [submodule "src/lib/matrix"] path = src/lib/matrix url = https://github.com/PX4/Matrix.git +[submodule "src/lib/DriverFramework"] + path = src/lib/DriverFramework + url = https://github.com/PX4/DriverFramework.git diff --git a/.travis.yml b/.travis.yml index e6b5239344..209bcc8795 100644 --- a/.travis.yml +++ b/.travis.yml @@ -9,7 +9,7 @@ matrix: - os: linux sudo: false - os: osx - osx_image: beta-xcode6.3 + osx_image: xcode7 sudo: true cache: @@ -59,12 +59,10 @@ 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 update; brew update + && brew install cmake ninja astyle gcc-arm-none-eabi && brew install genromfs && brew install kconfig-frontends - && brew install ninja && sudo easy_install pip && sudo pip install pyserial empy ; @@ -99,26 +97,27 @@ env: script: - make check_format - arm-none-eabi-gcc --version - - echo 'Building POSIX Firmware..' && make posix_sitl_simple - - echo 'Running Tests..' && make posix_sitl_simple test + - echo 'Building POSIX Firmware..' && make posix_sitl_default + - echo 'Running Tests..' && make posix_sitl_default test + - echo 'Running Unittests..' && cd unittests && ./run_tests.sh + - cd .. - echo 'Building UAVCAN node firmware..' && git clone https://github.com/thiemar/vectorcontrol - cd vectorcontrol - BOARD=s2740vc_1_0 make && BOARD=px4esc_1_6 make + - ../Tools/uavcan_copy.sh - cd .. - - mkdir -p ROMFS/px4fmu_common/uavcan/fw/com.thiemar.s2740vc-v1/1.0/ - - mkdir -p ROMFS/px4fmu_common/uavcan/fw/org.pixhawk.px4esc-v1/1.0/ - - cp vectorcontrol/firmware/com.thiemar.s2740vc-v1-1.0.*.bin ROMFS/px4fmu_common/uavcan/fw/com.thiemar.s2740vc-v1/1.0/. - - cp vectorcontrol/firmware/org.pixhawk.px4esc-v1-1.0.*.bin ROMFS/px4fmu_common/uavcan/fw/org.pixhawk.px4esc-v1/1.0/. - echo 'Building NuttX px4fmu-v1 Firmware..' && make px4fmu-v1_default - echo 'Building NuttX px4fmu-v2 Firmware..' && make px4fmu-v2_default + - echo 'Building NuttX px4fmu-v4 Firmware..' && make px4fmu-v4_default - echo 'Running Tests..' && make px4fmu-v2_default test 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 - && ./CI-Tools/s3cmd-put px4fmu-v1_default.px4 px4fmu-v2_default.px4 build_px4fmu-v2_default/parameters.xml build_px4fmu-v2_default/airframes.xml CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/ + && cp build_px4fmu-v4_default/src/firmware/nuttx/nuttx-px4fmu-v4-default.px4 px4fmu-v4_default.px4 + && zip Firmware.zip px4fmu-v1_default.px4 px4fmu-v2_default.px4 px4fmu-v4_default.px4 + && ./CI-Tools/s3cmd-put px4fmu-v1_default.px4 px4fmu-v2_default.px4 px4fmu-v4_default.px4 build_px4fmu-v2_default/parameters.xml build_px4fmu-v2_default/airframes.xml CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/ && ./CI-Tools/s3cmd-put Firmware.zip archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/ && ./CI-Tools/s3cmd-put CI-Tools/directory/index.html archives/Firmware/$TRAVIS_BRANCH/ && ./CI-Tools/s3cmd-put CI-Tools/index.html index.html diff --git a/CMakeLists.txt b/CMakeLists.txt index 6b395b6261..607fafb9ee 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -121,6 +121,10 @@ # and leads to wrong toolchain detection cmake_minimum_required(VERSION 2.8 FATAL_ERROR) +# Use clang +#SET (CMAKE_C_COMPILER /usr/bin/clang-3.6) +#SET (CMAKE_CXX_COMPILER /usr/bin/clang++-3.6) + #============================================================================= # parameters # @@ -146,6 +150,16 @@ list(GET config_args 1 BOARD) list(GET config_args 2 LABEL) set(target_name "${OS}-${BOARD}-${LABEL}") +if("${OS}" STREQUAL "posix") + if (APPLE) + set(DF_TARGET darwin) + else() + set(DF_TARGET linux) + endif() +else() + set(DF_TARGET ${OS}) +endif() + message(STATUS "${target_name}") # switch to ros CMake file if building ros @@ -230,6 +244,7 @@ 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_nuttx PATH "NuttX") px4_add_git_submodule(TARGET git_dspal PATH "src/lib/dspal") +px4_add_git_submodule(TARGET git_driverframework PATH "src/lib/DriverFramework") px4_add_git_submodule(TARGET git_jmavsim PATH "Tools/jMAVSim") px4_add_git_submodule(TARGET git_gazebo PATH "Tools/sitl_gazebo") px4_add_git_submodule(TARGET git_matrix PATH "src/lib/matrix") @@ -321,6 +336,11 @@ endforeach() add_subdirectory(src/firmware/${OS}) +add_subdirectory(src/lib/DriverFramework/framework/src) +#add_dependencies(df_driver_framework nuttx_export_${CONFIG}.stamp) +if (NOT "${OS}" STREQUAL "nuttx") +endif() + if (config_io_board) add_subdirectory(src/modules/px4iofirmware) endif() diff --git a/Images/px4fmu-v4.prototype b/Images/px4fmu-v4.prototype new file mode 100644 index 0000000000..24e72f868d --- /dev/null +++ b/Images/px4fmu-v4.prototype @@ -0,0 +1,12 @@ +{ + "board_id": 11, + "magic": "PX4FWv1", + "description": "Firmware for the PX4FMUv4 board", + "image": "", + "build_time": 0, + "summary": "PX4FMUv4", + "version": "0.1", + "image_size": 0, + "git_identity": "", + "board_revision": 0 +} diff --git a/Makefile b/Makefile index 096a058653..1c269a8b56 100644 --- a/Makefile +++ b/Makefile @@ -130,6 +130,9 @@ px4fmu-v1_default: px4fmu-v2_default: $(call cmake-build,nuttx_px4fmu-v2_default) +px4fmu-v4_default: + $(call cmake-build,nuttx_px4fmu-v4_default) + px4fmu-v2_simple: $(call cmake-build,nuttx_px4fmu-v2_simple) @@ -139,13 +142,13 @@ px4fmu-v2_lpe: nuttx_sim_simple: $(call cmake-build,$@) -posix_sitl_simple: +posix_sitl_default: $(call cmake-build,$@) posix_sitl_lpe: $(call cmake-build,$@) -ros_sitl_simple: +ros_sitl_default: $(call cmake-build,$@) qurt_eagle_travis: @@ -157,19 +160,15 @@ qurt_eagle_release: posix_eagle_release: $(call cmake-build,$@) -posix: posix_sitl_simple - -posix_sitl_default: posix_sitl_simple - -ros: ros_sitl_simple +posix: posix_sitl_default sitl_deprecation: @echo "Deprecated. Use 'make posix_sitl_default jmavsim' or" @echo "'make posix_sitl_default gazebo' if Gazebo is preferred." -sitl_quad: sitl_deprecation -sitl_plane: sitl_deprecation -sitl_ros: sitl_deprecation +run_sitl_quad: sitl_deprecation +run_sitl_plane: sitl_deprecation +run_sitl_ros: sitl_deprecation # Other targets # -------------------------------------------------------------------- @@ -180,11 +179,12 @@ clean: @rm -rf build_*/ @(cd NuttX && git clean -d -f -x) @(cd src/modules/uavcan/libuavcan && git clean -d -f -x) + @(git submodule sync) # 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 \ run_cmake_config config gazebo gazebo_gdb gazebo_lldb jmavsim \ - jmavsim_gdb jmavsim_lldb gazebo_gdb_iris gazebo_lldb_vtol gazebo_iris gazebo_vtol + jmavsim_gdb jmavsim_lldb gazebo_gdb_iris gazebo_lldb_tailsitter gazebo_iris gazebo_tailsitter $(foreach targ,$(cmake_targets),$(eval $(call cmake-targ,$(targ)))) .PHONY: clean diff --git a/README.md b/README.md index eb33fff02e..abbf0975a6 100644 --- a/README.md +++ b/README.md @@ -38,10 +38,10 @@ Testing guide: http://px4.io/dev/unit_tests This repository contains code supporting these boards: - * [Snapdragon Flight](https://www.intrinsyc.com/qualcomm-snapdragon-flight/) + * [Snapdragon Flight](http://dev.px4.io/hardware-snapdragon.html) * FMUv1.x - * FMUv2.x (Pixhawk) - * FMUv3.x (Pixhawk 2) + * FMUv2.x ([Pixhawk](http://dev.px4.io/hardware-pixhawk.html), Pixhawk 2 and [Pixfalcon](http://dev.px4.io/hardware-pixfalcon.html)) + * FMUv4.x (Pixhawk X and [Pixracer](http://dev.px4.io/hardware-pixracer.html)) * AeroCore (v1 and v2) * STM32F4Discovery (basic support) [Tutorial](https://pixhawk.org/modules/stm32f4discovery) diff --git a/ROMFS/px4fmu_common/init.d/13005_vtol_AAERT_quad b/ROMFS/px4fmu_common/init.d/13005_vtol_AAERT_quad index 4b8191f557..fabab37231 100644 --- a/ROMFS/px4fmu_common/init.d/13005_vtol_AAERT_quad +++ b/ROMFS/px4fmu_common/init.d/13005_vtol_AAERT_quad @@ -1,6 +1,6 @@ #!nsh # -# @name Generic AAERT tailplane airframe with Quad VTOL. +# @name Fun Cub Quad VTOL. # # @type Standard VTOL # diff --git a/ROMFS/px4fmu_common/init.d/13007_vtol_AAVVT_quad b/ROMFS/px4fmu_common/init.d/13007_vtol_AAVVT_quad new file mode 100644 index 0000000000..2d48670e71 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/13007_vtol_AAVVT_quad @@ -0,0 +1,51 @@ +#!nsh +# +# @name Generic AAVVT v-tail plane airframe with Quad VTOL. +# +# @type Standard VTOL +# +# @maintainer Sander Smeets +# + +sh /etc/init.d/rc.vtol_defaults + +if [ $AUTOCNF == yes ] +then + param set VT_TYPE 2 + param set VT_MOT_COUNT 4 + param set VT_TRANS_THR 0.75 + + param set MC_ROLL_P 7.0 + param set MC_ROLLRATE_P 0.15 + param set MC_ROLLRATE_I 0.002 + param set MC_ROLLRATE_D 0.003 + param set MC_ROLLRATE_FF 0.0 + param set MC_PITCH_P 7.0 + param set MC_PITCHRATE_P 0.12 + param set MC_PITCHRATE_I 0.002 + param set MC_PITCHRATE_D 0.003 + param set MC_PITCHRATE_FF 0.0 + param set MC_YAW_P 2.8 + param set MC_YAW_FF 0.5 + param set MC_YAWRATE_P 0.22 + param set MC_YAWRATE_I 0.02 + param set MC_YAWRATE_D 0.0 + param set MC_YAWRATE_FF 0.0 + +fi + +set MIXER vtol_quad_x +set PWM_OUT 12345678 + +set MIXER_AUX vtol_AAVVT +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 + +set MAV_TYPE 22 + +param set VT_MOT_COUNT 4 +param set VT_IDLE_PWM_MC 1080 +param set VT_TYPE 2 diff --git a/ROMFS/px4fmu_common/init.d/15001_coax_heli b/ROMFS/px4fmu_common/init.d/15001_coax_heli new file mode 100644 index 0000000000..97513b7049 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/15001_coax_heli @@ -0,0 +1,65 @@ +#!nsh +# +# @name Coaxial Helicopter (such as Esky Lama v4 or Esky Big Lama) +# +# @type Coaxial Helicopter +# +# @output MAIN1 Left swashplate servomotor, pitch axis +# @output MAIN2 Right swashplate servomotor, roll axis +# @output MAIN3 Upper rotor (CCW) +# @output MAIN4 Lower rotor (CW) +# +# @maintainer Emmanuel Roussel +# + +set VEHICLE_TYPE mc + +if [ $AUTOCNF == yes ] +then + param set PE_VELNE_NOISE 0.5 + param set PE_VELD_NOISE 0.35 + param set PE_POSNE_NOISE 0.5 + param set PE_POSD_NOISE 1.25 + + param set NAV_ACC_RAD 2.0 + param set RTL_RETURN_ALT 30.0 + param set RTL_DESCEND_ALT 10.0 + + param set PWM_DISARMED 900 + param set PWM_MIN 1075 + param set PWM_MAX 1950 + + param set MC_ROLL_P 6.5 + param set MC_ROLLRATE_P 0.17 + param set MC_ROLLRATE_I 0.05 + param set MC_ROLLRATE_D 0.005 + param set MC_ROLLRATE_FF 0 + param set MC_PITCH_P 6.5 + param set MC_PITCHRATE_P 0.17 + param set MC_PITCHRATE_I 0.05 + param set MC_PITCHRATE_D 0.005 + param set MC_PITCHRATE_FF 0 + param set MC_YAW_P 2 + param set MC_YAW_FF 0.5 + param set MC_YAWRATE_P 0.1 + param set MC_YAWRATE_I 0.1 + param set MC_YAWRATE_D 0.0 + param set MC_YAWRATE_FF 0 +fi + +set MIXER coax +# use PWM parameters for throttle channel +set PWM_OUT 34 +set PWM_RATE 400 +set PWM_DISARMED p:PWM_DISARMED +set PWM_MIN p:PWM_MIN +set PWM_MAX p:PWM_MAX + +# This is the gimbal pass mixer +set MIXER_AUX pass +set PWM_AUX_RATE 50 +set PWM_AUX_OUT 1234 +set PWM_AUX_DISARMED 1500 +set PWM_AUX_MIN 1000 +set PWM_AUX_MAX 2000 + diff --git a/ROMFS/px4fmu_common/init.d/2101_fw_AERT b/ROMFS/px4fmu_common/init.d/2101_fw_AERT index 3631a1e36c..d13514d03a 100644 --- a/ROMFS/px4fmu_common/init.d/2101_fw_AERT +++ b/ROMFS/px4fmu_common/init.d/2101_fw_AERT @@ -6,8 +6,8 @@ # # @output MAIN1 aileron # @output MAIN2 elevator -# @output MAIN4 rudder -# @output MAIN3 throttle +# @output MAIN3 rudder +# @output MAIN4 throttle # @output MAIN5 flaps # # @output AUX1 feed-through of RC AUX1 channel @@ -21,6 +21,8 @@ sh /etc/init.d/rc.fw_defaults set MIXER AERT -# The ESC requires a specific pulse to arm. +# use PWM parameters for throttle channel set PWM_OUT 4 set PWM_DISARMED p:PWM_DISARMED +set PWM_MIN p:PWM_MIN +set PWM_MAX p:PWM_MAX diff --git a/ROMFS/px4fmu_common/init.d/2104_fw_AETR b/ROMFS/px4fmu_common/init.d/2104_fw_AETR index 3113ede87a..f533ce66fe 100644 --- a/ROMFS/px4fmu_common/init.d/2104_fw_AETR +++ b/ROMFS/px4fmu_common/init.d/2104_fw_AETR @@ -21,6 +21,8 @@ sh /etc/init.d/rc.fw_defaults set MIXER AETR -# The ESC requires a specific pulse to arm. +# use PWM parameters for throttle channel set PWM_OUT 3 set PWM_DISARMED p:PWM_DISARMED +set PWM_MIN p:PWM_MIN +set PWM_MAX p:PWM_MAX diff --git a/ROMFS/px4fmu_common/init.d/2105_maja b/ROMFS/px4fmu_common/init.d/2105_maja new file mode 100644 index 0000000000..1f68428f3b --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/2105_maja @@ -0,0 +1,50 @@ +#!nsh +# +# @name Bormatec Maja +# +# @type Standard Plane +# +# @output MAIN1 aileron +# @output MAIN2 aileron +# @output MAIN3 elevator +# @output MAIN4 rudder +# @output MAIN5 throttle +# @output MAIN6 wheel +# @output MAIN7 flaps +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Andreas Antener +# + +sh /etc/init.d/rc.fw_defaults + +if [ $AUTOCNF == yes ] +then + param set FW_AIRSPD_MIN 10 + param set FW_AIRSPD_TRIM 15 + param set FW_AIRSPD_MAX 20 + + param set FW_MAN_P_MAX 55 + param set FW_MAN_R_MAX 55 + param set FW_R_LIM 55 + + param set FW_WR_FF 0.2 + param set FW_WR_I 0.2 + param set FW_WR_IMAX 0.8 + param set FW_WR_P 1 + param set FW_W_RMAX 0 + + # set disarmed value for the ESC + param set PWM_DISARMED 1000 +fi + +set MIXER AAERTWF + +# use PWM parameters for throttle channel +set PWM_OUT 5 +set PWM_DISARMED p:PWM_DISARMED +set PWM_MIN p:PWM_MIN +set PWM_MAX p:PWM_MAX diff --git a/ROMFS/px4fmu_common/init.d/2106_albatross b/ROMFS/px4fmu_common/init.d/2106_albatross new file mode 100644 index 0000000000..79e560fb5f --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/2106_albatross @@ -0,0 +1,51 @@ +#!nsh +# +# @name Applied Aeronautics Albatross +# +# @type Standard Plane +# +# @output MAIN1 aileron right +# @output MAIN2 aileron left +# @output MAIN3 v-tail right +# @output MAIN4 v-tail left +# @output MAIN5 throttle +# @output MAIN6 wheel +# @output MAIN7 flaps right +# @output MAIN8 flaps left +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Andreas Antener +# + +sh /etc/init.d/rc.fw_defaults + +if [ $AUTOCNF == yes ] +then + param set FW_AIRSPD_MIN 10 + param set FW_AIRSPD_TRIM 15 + param set FW_AIRSPD_MAX 20 + + param set FW_MAN_P_MAX 55 + param set FW_MAN_R_MAX 55 + param set FW_R_LIM 55 + + param set FW_WR_FF 0.2 + param set FW_WR_I 0.2 + param set FW_WR_IMAX 0.8 + param set FW_WR_P 1 + param set FW_W_RMAX 0 + + # set disarmed value for the ESC + param set PWM_DISARMED 1000 +fi + +set MIXER AAVVTWFF + +# use PWM parameters for throttle channel +set PWM_OUT 5 +set PWM_DISARMED p:PWM_DISARMED +set PWM_MIN p:PWM_MIN +set PWM_MAX p:PWM_MAX diff --git a/ROMFS/px4fmu_common/init.d/4009_qav250 b/ROMFS/px4fmu_common/init.d/4009_qav250 index dfd94992af..aed6aef3b7 100644 --- a/ROMFS/px4fmu_common/init.d/4009_qav250 +++ b/ROMFS/px4fmu_common/init.d/4009_qav250 @@ -30,4 +30,7 @@ then param set PWM_MIN 980 param set MPC_THR_MIN 0.06 param set MPC_MANTHR_MIN 0.06 + param set MC_ROLLRATE_MAX 360.0 + param set MC_PITCHRATE_MAX 360.0 + param set MC_YAWRATE_MAX 200.0 fi diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 08a044329f..f02fb5c650 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -3,8 +3,20 @@ # Standard startup script for PX4FMU v1, v2, v3 onboard sensor drivers. # -if ms5611 start +if ver hwcmp PX4FMU_V1 then + if ms5611 start + then + fi +else + if ms5611 -s start + then + fi + + # Blacksheep telemetry + if bst start + then + fi fi if adc start @@ -67,32 +79,55 @@ then fi fi else - # FMUv1 - if mpu6000 start + if ver hwcmp PX4FMU_V4 then - fi + # External I2C bus + if hmc5883 -C -T -X start + then + fi - if l3gd20 start - then - fi + # Internal SPI bus is rotated 90 deg yaw + if hmc5883 -C -T -S -R 2 start + then + fi - # MAG selection - if param compare SENS_EXT_MAG 2 - then - if hmc5883 -C -I start + # Internal SPI bus ICM-20608-G is rotated 90 deg yaw + if mpu6000 -R 2 -T 20608 start + then + fi + + # Internal SPI bus mpu9250 is rotated 90 deg yaw + if mpu9250 -R 2 start then fi else - # Use only external as primary - if param compare SENS_EXT_MAG 1 + # FMUv1 + if mpu6000 start then - if hmc5883 -C -X start + fi + + if l3gd20 start + then + fi + + # MAG selection + if param compare SENS_EXT_MAG 2 + then + if hmc5883 -C -I start then fi else - # auto-detect the primary, prefer external - if hmc5883 start + # Use only external as primary + if param compare SENS_EXT_MAG 1 then + if hmc5883 -C -X start + then + fi + else + # auto-detect the primary, prefer external + if hmc5883 start + then + fi fi fi fi diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps index 7d0c153f09..6462f6954d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_apps +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_apps @@ -18,4 +18,4 @@ fw_pos_control_l1 start # Start Land Detector # Multicopter for now until we have something for VTOL # -land_detector start multicopter +land_detector start vtol diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c4b25987be..a2a726bd7c 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -217,7 +217,12 @@ then # if param compare SYS_USE_IO 1 then - set USE_IO yes + if ver hwcmp PX4FMU_V4 + then + set USE_IO no + else + set USE_IO yes + fi else set USE_IO no fi @@ -233,6 +238,13 @@ then fi unset MODE + # + # Wipe incompatible settings for boards not having two outputs + if ver hwcmp PX4FMU_V4 + then + set MIXER_AUX none + fi + # # Override parameters from user configuration file # @@ -537,8 +549,14 @@ then # Exit from nsh to free port for mavlink set EXIT_ON_END yes else - # Start MAVLink on default port: ttyS1 set MAVLINK_F "-r 1200" + # Avoid using ttyS1 for MAVLink on FMUv4 + if ver hwcmp PX4FMU_V4 + then + set MAVLINK_F "-r 1200 -d /dev/ttyS2" + # Start MAVLink on Wifi (ESP8266 port) + mavlink start -r 800000 -b 921600 -d /dev/ttyS0 + fi fi fi @@ -564,6 +582,10 @@ then then mavlink start -d /dev/ttyS2 -b 57600 -m osd -r 1000 fi + if param compare SYS_COMPANION 257600 + then + mavlink start -d /dev/ttyS2 -b 57600 -m magic -r 5000 -x + fi # Sensors on the PWM interface bank # clear pins 5 and 6 if param compare SENS_EN_LL40LS 1 diff --git a/ROMFS/px4fmu_common/mixers/AAERTWF.main.mix b/ROMFS/px4fmu_common/mixers/AAERTWF.main.mix new file mode 100644 index 0000000000..2ab5bdb611 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/AAERTWF.main.mix @@ -0,0 +1,96 @@ +Aileron/rudder/elevator/throttle/wheel/flaps mixer for PX4FMU +======================================================= + +This file defines mixers suitable for controlling a fixed wing aircraft with +aileron, rudder, elevator, throttle and steerable wheel controls using PX4FMU. +The configuration assumes the aileron servo(s) are connected to PX4FMU servo +output 0 and 1, the elevator to output 2, the rudder to output 3, the throttle +to output 4 and the wheel to output 5. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps) 6 (flaperon). + +Aileron mixer (roll + flaperon) +--------------------------------- + +This mixer assumes that the aileron servos are set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 10000 10000 0 -10000 10000 +S: 0 6 -10000 -10000 0 -10000 10000 + +Elevator mixer +------------ +Two scalers total (output, roll). + +This mixer assumes that the elevator servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 1 -10000 -10000 0 -10000 10000 + +Rudder mixer +------------ +Two scalers total (output, yaw). + +This mixer assumes that the rudder servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 2 10000 10000 0 -10000 10000 + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 + +Wheel mixer +------------ +Two scalers total (output, yaw). + +This mixer assumes that the wheel servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 2 10000 10000 0 -10000 10000 + + +Flaps / gimbal / payload mixer for last three channels, +using the payload control group +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 2 0 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 2 2 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix b/ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix new file mode 100644 index 0000000000..e6520862d7 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix @@ -0,0 +1,84 @@ +Aileron/v-tail/throttle/wheel/flaps mixer for PX4FMU +======================================================= + +This file defines mixers suitable for controlling a fixed wing aircraft with +aileron, v-tail (rudder, elevator), throttle, steerable wheel and flaps +using PX4FMU. +The configuration assumes the aileron servos are connected to PX4FMU servo +output 0 and 1, the tail servos to output 2 and 3, the throttle +to output 4, the wheel to output 5 and the flaps to output 6 and 7. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps) 6 (flaperon). + +Aileron mixer (roll + flaperon) +--------------------------------- + +This mixer assumes that the aileron servos are set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 10000 10000 0 -10000 10000 +S: 0 6 -10000 -10000 0 -10000 10000 + +V-tail mixers +------------- +Three scalers total (output, roll, pitch). + +On the assumption that the two tail servos are physically reversed, the pitch +input is inverted between the two servos. + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 2 -7000 -7000 0 -10000 10000 +S: 0 1 -8000 -8000 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 2 -7000 -7000 0 -10000 10000 +S: 0 1 8000 8000 0 -10000 10000 + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 + +Wheel mixer +------------ +Two scalers total (output, yaw). + +This mixer assumes that the wheel servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 2 -10000 -10000 0 -10000 10000 + +Flaps mixer +------------ +Flap servos are physically reversed. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 0 5000 -10000 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 0 -5000 10000 -10000 10000 + diff --git a/ROMFS/px4fmu_common/mixers/coax.main.mix b/ROMFS/px4fmu_common/mixers/coax.main.mix new file mode 100644 index 0000000000..96d9afdf1a --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/coax.main.mix @@ -0,0 +1,33 @@ +Coaxial helicopter mixer +- Two servomotors act on the swashplate (90° angle on the swashplate, decoupled effect on roll and pitch). +- No collective pitch. +- One motor per rotor. +=========================== + +Left swashplate servomotor, pitch axis +------------- +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 1 -10000 -10000 0 -10000 10000 + +Right swashplate servomotor, roll axis +------------- +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 0 10000 10000 0 -10000 10000 + +Upper rotor (CCW) +Mixing between yaw and thrust +------------- +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 2 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 + +Lower rotor (CW) +Mixing between yaw and thrust +------------- +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 2 -10000 -10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/quad_h.main.mix b/ROMFS/px4fmu_common/mixers/quad_h.main.mix new file mode 100644 index 0000000000..4a177d2ff0 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/quad_h.main.mix @@ -0,0 +1,26 @@ +Multirotor mixer for PX4FMU +=========================== + +This file defines a single mixer for a quadrotor in the H configuration. All controls +are mixed 100%. + +R: 4h 10000 10000 10000 0 + +Gimbal / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix b/ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix index 4fd323353a..bd332a2688 100644 --- a/ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix +++ b/ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix @@ -9,10 +9,10 @@ R: 4x 10000 10000 10000 0 #mixer for the elevons M: 2 O: 10000 10000 0 -10000 10000 -S: 1 0 10000 10000 0 -10000 10000 -S: 1 1 10000 10000 0 -10000 10000 +S: 1 0 5000 5000 0 -10000 10000 +S: 1 1 5000 5000 0 -10000 10000 M: 2 O: 10000 10000 0 -10000 10000 -S: 1 0 10000 10000 0 -10000 10000 -S: 1 1 -10000 -10000 0 -10000 10000 +S: 1 0 5000 5000 0 -10000 10000 +S: 1 1 -5000 -5000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/vtol_AAVVT.aux.mix b/ROMFS/px4fmu_common/mixers/vtol_AAVVT.aux.mix new file mode 100644 index 0000000000..86581585c2 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/vtol_AAVVT.aux.mix @@ -0,0 +1,59 @@ +Aileron/v-tail/throttle VTOL mixer for PX4FMU +======================================================= + +This file defines mixers suitable for controlling a fixed wing aircraft with +aileron, v-tail (rudder, elevator) and throttle using PX4FMU. +The configuration assumes the aileron servos are connected to PX4FMU +AUX servo output 0 and 1, the tail servos to output 2 and 3, the throttle +to output 4. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps) 6 (flaperon). + +Aileron mixer (roll + flaperon) +--------------------------------- + +This mixer assumes that the aileron servos are set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 1 0 10000 10000 0 -10000 10000 +S: 1 6 10000 10000 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 1 0 10000 10000 0 -10000 10000 +S: 1 6 -10000 -10000 0 -10000 10000 + + +V-tail mixers +------------- +Three scalers total (output, roll, pitch). + +On the assumption that the two tail servos are physically reversed, the pitch +input is inverted between the two servos. + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 1 2 -7000 -7000 0 -10000 10000 +S: 1 1 -8000 -8000 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 1 2 -7000 -7000 0 -10000 10000 +S: 1 1 8000 8000 0 -10000 10000 + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 3 0 20000 -10000 -10000 10000 + diff --git a/Tools/check_code_style.sh b/Tools/check_code_style.sh index c3dd2a3f32..cd42b65c80 100755 --- a/Tools/check_code_style.sh +++ b/Tools/check_code_style.sh @@ -14,6 +14,7 @@ for fn in $(find src/examples \ src/lib/rc \ src/lib/version \ src/modules/attitude_estimator_q \ + src/modules/fw_att_control \ src/modules/gpio_led \ src/modules/land_detector \ src/modules/muorb \ diff --git a/Tools/decode_backtrace.py b/Tools/decode_backtrace.py new file mode 100755 index 0000000000..62a9c16b46 --- /dev/null +++ b/Tools/decode_backtrace.py @@ -0,0 +1,123 @@ +#!/usr/bin/env python +############################################################################ +# +# Copyright (C) 2015 Mark Charlebois. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +import os +import subprocess + +# The PX4_BACKTRACE() macro can be used to generate backtraces in the +# running code. These provide the addresses only. This utility can be +# used to decode the backtrace into a readable form: +# +# Example: +# If the following lines were pasted into the shell after running decode_backtrace.py +# +# INFO Backtrace: 10 +# INFO ./mainapp(px4_backtrace+0x27) [0x42b212] +# INFO ./mainapp() [0x42d608] +# INFO ./mainapp() [0x42d57e] +# INFO ./mainapp() [0x4ba48d] +# +# The output would be: +# +# 0x42b212 px4_backtrace (0x42b1eb) +# 0x42d608 ACCELSIM::devRead(void*, unsigned long) (0x42d600) +# 0x42d57e ACCELSIM::start() (0x42d578) +# 0x4ba48d DriverFramework::DevObj::addHandle(DriverFramework::DevHandle&) (0x4ba42e) +# +# The output format is: +# () + + +def usage(): + msg = """ +Usage: Tools/decode_backtrace.py + +This will load the symbols for /src/firmware/posix/mainapp +The user just needs to copy and paste the backtrace into the terminal +where decode_backtrace.py is running. + +""" + print msg + +funcaddr = [] +func = [] + +# Load the symbols from the binary +def load_symbol_map(): + output = subprocess.check_output(["nm", "-p", "-C", os.sys.argv[1]+"/src/firmware/posix/mainapp"]) + data = output.split("\n") + data.sort() + + i = 0 + for line in data: + if line.startswith("0"): + funcaddr.append(int(line[0:16],16)) + func.append(line[19:]) + #print(hex(funcaddr[i]), func[i]) + i = i+1 + +# Find the function for the specified call stack address +def lookup(addr): + i=1 + while i < len(funcaddr) and addr > funcaddr[i]: + i=i+1 + if i >= len(funcaddr): + return -1 + + # return the function before this one + return i-1 + +if len(os.sys.argv) != 2: + usage() + raise SystemExit + +load_symbol_map() + +print +print "Paste the backtrace here. Use Ctrl-C to exit." +print +while 1: + # Wait for user to paste the backtrace into the shell + line = raw_input() + tmp = line.split("[") + if len(tmp) == 1: + if len(line.split("Backtrace:")) > 1: + print + continue + addrstr = tmp[1].split("]")[0] + addr = int(addrstr, 16) + idx = lookup(addr) + if idx >= 0: + print hex(addr), func[idx], "({0})".format(hex(funcaddr[idx])) + diff --git a/Tools/generate_listener.py b/Tools/generate_listener.py index 625eda7037..f097cb741b 100755 --- a/Tools/generate_listener.py +++ b/Tools/generate_listener.py @@ -17,7 +17,7 @@ for index,m in enumerate(raw_messages): temp_list_floats = [] temp_list_uint64 = [] temp_list_bool = [] - if("actuator_control" not in m and "pwm_input" not in m and "position_setpoint" not in m): + if("pwm_input" not in m and "position_setpoint" not in m): temp_list = [] f = open(m,'r') for line in f.readlines(): @@ -55,9 +55,11 @@ for index,m in enumerate(raw_messages): f.close() (m_head, m_tail) = os.path.split(m) - messages.append(m_tail.split('.')[0]) + message = m_tail.split('.')[0] + if message != "actuator_controls": + messages.append(message) + message_elements.append(temp_list) #messages.append(m.split('/')[-1].split('.')[0]) - message_elements.append(temp_list) num_messages = len(messages); @@ -198,6 +200,7 @@ for index,m in enumerate(messages[1:]): print("\t} else {") print("\t\t printf(\" Topic did not match any known topics\\n\");") print("\t}") +print("\t\torb_unsubscribe(sub);") print("\t return 0;") print("}") diff --git a/Tools/jMAVSim b/Tools/jMAVSim index 96029523d1..4267771753 160000 --- a/Tools/jMAVSim +++ b/Tools/jMAVSim @@ -1 +1 @@ -Subproject commit 96029523d13fc7fdac04c1a50b08c0eb0b39f9a9 +Subproject commit 4267771753c4bb5d0696a678856620229523162e diff --git a/Tools/px_process_params.py b/Tools/px_process_params.py index 3a11fc5c50..fcd5b994e9 100644 --- a/Tools/px_process_params.py +++ b/Tools/px_process_params.py @@ -130,7 +130,7 @@ def main(): # Output to XML file if args.xml: print("Creating XML file " + args.xml) - out = xmlout.XMLOutput(param_groups, args.board, args.inject_xml) + out = xmlout.XMLOutput(param_groups, args.board, os.path.join(args.src_path, args.inject_xml)) out.Save(args.xml) # Output to DokuWiki tables diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 3cdd9d1a6f..4755eb24ff 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -148,6 +148,7 @@ class uploader(object): OK = b'\x10' FAILED = b'\x11' INVALID = b'\x13' # rev3+ + BAD_SILICON_REV = b'\x14' # rev5+ # command bytes NOP = b'\x00' # guaranteed to be discarded by the bootloader @@ -162,11 +163,14 @@ class uploader(object): GET_SN = b'\x2b' # rev4+ , get a word from SN area GET_CHIP = b'\x2c' # rev5+ , get chip version SET_BOOT_DELAY = b'\x2d' # rev5+ , set boot delay + GET_CHIP_DES = b'\x2e' # rev5+ , get chip description in ASCII + MAX_DES_LENGTH = 20 + REBOOT = b'\x30' INFO_BL_REV = b'\x01' # bootloader protocol revision - BL_REV_MIN = 2 # minimum supported bootloader protocol - BL_REV_MAX = 4 # maximum supported bootloader protocol + BL_REV_MIN = 2 # minimum supported bootloader protocol + BL_REV_MAX = 5 # maximum supported bootloader protocol INFO_BOARD_ID = b'\x02' # board type INFO_BOARD_REV = b'\x03' # board revision INFO_FLASH_SIZE = b'\x04' # max firmware size in bytes @@ -235,12 +239,16 @@ class uploader(object): if (self.__recv() != self.INSYNC): #print("unexpected 0x%x instead of INSYNC" % ord(c)) return False; - - if (self.__recv() != self.OK): + c = self.__recv() + if (c == self.BAD_SILICON_REV): + raise NotImplementedError() + if (c != self.OK): #print("unexpected 0x%x instead of OK" % ord(c)) return False return True + except NotImplementedError: + raise RuntimeError("Programing not supported for this version of silicon!\n See https://pixhawk.org/help/errata") except RuntimeError: #timeout, no response yet return False @@ -274,6 +282,14 @@ class uploader(object): value = self.__recv_int() self.__getSync() return value + # send the GET_CHIP command + def __getCHIPDes(self): + self.__send(uploader.GET_CHIP_DES + uploader.EOC) + length = self.__recv_int() + value = self.__recv(length) + self.__getSync() + peices = value.split(",") + return peices def __drawProgressBar(self, label, progress, maxVal): if maxVal < progress: @@ -467,6 +483,12 @@ class uploader(object): print(binascii.hexlify(x).decode('Latin-1'), end='') # show user print('') print("chip: %08x" % self.__getCHIP()) + if (self.bl_rev >= 5): + des = self.__getCHIPDes() + if (len(des) == 2): + print("family: %s" % des[0]) + print("revision: %s" % des[1]) + print("flash %d" % self.fw_maxsize) except Exception: # ignore bad character encodings pass diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index d362e661b4..03a749a3b3 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit d362e661b46474153f43f51a6eb947d4fda1bebe +Subproject commit 03a749a3b3238c014329e9e8da57d102ede6f4f0 diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index 8767f9f841..dcfd21930b 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -37,6 +37,8 @@ then kill $jmavsim_pid fi +set -e + cp Tools/posix_lldbinit $build_path/src/firmware/posix/.lldbinit cp Tools/posix.gdbinit $build_path/src/firmware/posix/.gdbinit @@ -46,14 +48,14 @@ if [ "$program" == "jmavsim" ] && [ "$no_sim" == "" ] 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 & + nice -n -10 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" ] && [ "$no_sim" == "" ] +elif [ "$program" == "gazebo" ] && [ "$no_sim" == "" ] 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 + export GAZEBO_PLUGIN_PATH=$curr_dir/Tools/sitl_gazebo/Build:${GAZEBO_PLUGIN_PATH} # Set the model path so Gazebo finds the airframes export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:$curr_dir/Tools/sitl_gazebo/models # The next line would disable online model lookup, can be commented in, in case of unstable behaviour. @@ -61,11 +63,11 @@ then export SITL_GAZEBO_PATH=$curr_dir/Tools/sitl_gazebo mkdir -p Tools/sitl_gazebo/Build cd Tools/sitl_gazebo/Build - cmake .. + cmake -Wno-dev .. make -j4 - gzserver ../worlds/${model}.world & + nice -n -10 gzserver --verbose ../worlds/${model}.world & SIM_PID=`echo $!` - gzclient & + gzclient --verbose & GUI_PID=`echo $!` else echo "You need to have gazebo simulator installed!" @@ -76,21 +78,25 @@ cd $build_path/src/firmware/posix mkdir -p rootfs/fs/microsd mkdir -p rootfs/eeprom touch rootfs/eeprom/parameters + +# Do not exit on failure now from here on because we want the complete cleanup +set +e + # Start Java simulator if [ "$debugger" == "lldb" ] then - lldb -- mainapp ../../../../${rc_script}_${program} + lldb -- mainapp ../../../../${rc_script}_${program}_${model} elif [ "$debugger" == "gdb" ] then - gdb --args mainapp ../../../../${rc_script}_${program} + gdb --args mainapp ../../../../${rc_script}_${program}_${model} elif [ "$debugger" == "ddd" ] then - ddd --debugger gdb --args mainapp ../../../../${rc_script}_${program} + ddd --debugger gdb --args mainapp ../../../../${rc_script}_${program}_${model} elif [ "$debugger" == "valgrind" ] then - valgrind ./mainapp ../../../../${rc_script}_${program} + valgrind ./mainapp ../../../../${rc_script}_${program}_${model} else - ./mainapp ../../../../${rc_script}_${program} + nice -n -10 ./mainapp ../../../../${rc_script}_${program}_${model} fi if [ "$program" == "jmavsim" ] diff --git a/Tools/uavcan_copy.sh b/Tools/uavcan_copy.sh new file mode 100755 index 0000000000..9d21e0b562 --- /dev/null +++ b/Tools/uavcan_copy.sh @@ -0,0 +1,34 @@ +#!/bin/bash +UAVCANFW=../ROMFS/px4fmu_common/uavcan/fw +ROMFS_REFIX=_ +echo Removing : $UAVCANFW +rm -fr $UAVCANFW +for f in $(find firmware -type f -name "*.*.bin") +do + UAVCAN_NAME=$(echo $f | cut -d"/" -f2 | cut -d. -f1-4 | cut -d- -f1-2) + UAVCAN_HW=$(echo $f cut -d/ -f2 | cut -d. -f1-4 | cut -d- -f3) + DST=${ROMFS_REFIX}$(echo $f | cut -d. -f3-7 | cut -d- -f1,2,3).$(echo $f | cut -d. -f6,7) + # deal with legacy non conforming naming + if [[ ${DST:(-7)} == bin.bin ]] + then + echo " WARNING: Improper name format!!!!!!!!! $f see should be -...bin" + DST=${DST%????} + fi + echo Processing file: $f Length:${#DST} + if [ ${#DST} -le 28 ] + then + if [ -d "${UAVCANFW}/${UAVCAN_NAME}/${UAVCAN_HW}" ] + then + echo " ERROR: name colision directory ${UAVCANFW}/${UAVCAN_NAME}/${UAVCAN_HW} exits!" + exit 2 + fi + echo " Creating Directory ${UAVCANFW}/${UAVCAN_NAME}/${UAVCAN_HW}" + mkdir -p ${UAVCANFW}/${UAVCAN_NAME}/${UAVCAN_HW} + echo " Copying $f to ${UAVCANFW}/${UAVCAN_NAME}/${UAVCAN_HW}/${DST}" + cp $f ${UAVCANFW}/${UAVCAN_NAME}/${UAVCAN_HW}/${DST} + else + echo " ERROR: $DST is ${#DST} charaters and needs to be less than or equal to 28" + exit 1 + fi +done +exit 0 diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake index 8a3a844886..fa34b1584d 100644 --- a/cmake/common/px4_base.cmake +++ b/cmake/common/px4_base.cmake @@ -619,6 +619,7 @@ function(px4_add_common_flags) ${CMAKE_BINARY_DIR}/src/modules/px4_messages ${CMAKE_BINARY_DIR}/src/modules ${CMAKE_SOURCE_DIR}/mavlink/include/mavlink + ${CMAKE_SOURCE_DIR}/src/lib/DriverFramework/framework/include ) list(APPEND added_include_dirs diff --git a/cmake/configs/nuttx_px4fmu-v1_default.cmake b/cmake/configs/nuttx_px4fmu-v1_default.cmake index ed48bd4f34..902447c282 100644 --- a/cmake/configs/nuttx_px4fmu-v1_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v1_default.cmake @@ -111,6 +111,8 @@ set(config_module_list lib/geo_lookup lib/conversion lib/launchdetection + lib/terrain_estimation + lib/runway_takeoff platforms/nuttx # had to add for cmake, not sure why wasn't in original config diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake index 0c6458cad1..9fafe851be 100644 --- a/cmake/configs/nuttx_px4fmu-v2_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake @@ -2,6 +2,8 @@ include(nuttx/px4_impl_nuttx) set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake) +set(config_uavcan_num_ifaces 2) + set(config_module_list # # Board support modules @@ -43,6 +45,7 @@ set(config_module_list drivers/gimbal drivers/pwm_input drivers/camera_trigger + drivers/bst # # System commands @@ -118,6 +121,8 @@ set(config_module_list lib/geo_lookup lib/conversion lib/launchdetection + lib/terrain_estimation + lib/runway_takeoff platforms/nuttx # had to add for cmake, not sure why wasn't in original config diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake new file mode 100644 index 0000000000..a6dd256a34 --- /dev/null +++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake @@ -0,0 +1,185 @@ +include(nuttx/px4_impl_nuttx) + +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake) + +set(config_uavcan_num_ifaces 1) + +set(config_module_list + # + # Board support modules + # + drivers/device + drivers/stm32 + drivers/stm32/adc + drivers/stm32/tone_alarm + drivers/led + drivers/px4fmu + drivers/boards/px4fmu-v4 + drivers/rgbled + drivers/mpu6000 + drivers/mpu9250 + drivers/hmc5883 + drivers/ms5611 + drivers/mb12xx + drivers/srf02 + drivers/sf0x + drivers/ll40ls + drivers/trone + drivers/gps + drivers/pwm_out_sim + drivers/hott + drivers/hott/hott_telemetry + drivers/hott/hott_sensors + drivers/blinkm + drivers/airspeed + drivers/ets_airspeed + drivers/meas_airspeed + drivers/frsky_telemetry + modules/sensors + drivers/mkblctrl + drivers/px4flow + drivers/oreoled + drivers/gimbal + drivers/pwm_input + drivers/camera_trigger + + # + # System commands + # + systemcmds/bl_update + systemcmds/mixer + systemcmds/param + systemcmds/perf + systemcmds/pwm + systemcmds/esc_calib + systemcmds/reboot + systemcmds/topic_listener + systemcmds/top + systemcmds/config + systemcmds/nshterm + systemcmds/mtd + systemcmds/dumpfile + systemcmds/ver + systemcmds/tests + + # + # General system control + # + modules/commander + modules/navigator + modules/mavlink + modules/gpio_led + modules/uavcan + modules/land_detector + + # + # Estimation modules (EKF/ SO3 / other filters) + # + # Too high RAM usage due to static allocations + # modules/attitude_estimator_ekf + modules/attitude_estimator_q + modules/ekf_att_pos_estimator + modules/position_estimator_inav + + # + # Vehicle Control + # + # modules/segway # XXX Needs GCC 4.7 fix + modules/fw_pos_control_l1 + modules/fw_att_control + modules/mc_att_control + modules/mc_pos_control + modules/vtol_att_control + + # + # Logging + # + modules/sdlog2 + + # + # Library modules + # + modules/param + modules/systemlib + modules/systemlib/mixer + modules/controllib + modules/uORB + modules/dataman + + # + # Libraries + # + #lib/mathlib/CMSIS + lib/mathlib + lib/mathlib/math/filter + lib/rc + lib/ecl + lib/external_lgpl + lib/geo + lib/geo_lookup + lib/conversion + lib/launchdetection + lib/terrain_estimation + lib/runway_takeoff + platforms/nuttx + + # had to add for cmake, not sure why wasn't in original config + platforms/common + platforms/nuttx/px4_layer + + # + # OBC challenge + # + modules/bottle_drop + + # + # Rover apps + # + examples/rover_steering_control + + # + # Demo apps + # + #examples/math_demo + # Tutorial code from + # https://px4.io/dev/px4_simple_app + #examples/px4_simple_app + + # Tutorial code from + # https://px4.io/dev/daemon + #examples/px4_daemon_app + + # Tutorial code from + # https://px4.io/dev/debug_values + #examples/px4_mavlink_debug + + # Tutorial code from + # https://px4.io/dev/example_fixedwing_control + #examples/fixedwing_control + + # Hardware test + #examples/hwtest +) + +set(config_extra_builtin_cmds + serdis + sercon + ) + +set(config_extra_libs + ${CMAKE_SOURCE_DIR}/src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a + uavcan + uavcan_stm32_driver + ) + +set(config_io_extra_libs + #${CMAKE_SOURCE_DIR}/src/lib/mathlib/CMSIS/libarm_cortexM3l_math.a + ) + +add_custom_target(sercon) +set_target_properties(sercon PROPERTIES + MAIN "sercon" STACK "2048") + +add_custom_target(serdis) +set_target_properties(serdis PROPERTIES + MAIN "serdis" STACK "2048") diff --git a/cmake/configs/posix_eagle_default.cmake b/cmake/configs/posix_eagle_default.cmake index fc97e70918..76d9711187 100644 --- a/cmake/configs/posix_eagle_default.cmake +++ b/cmake/configs/posix_eagle_default.cmake @@ -38,6 +38,8 @@ set(config_module_list lib/geo lib/geo_lookup lib/conversion + lib/terrain_estimation + lib/runway_takeoff platforms/common platforms/posix/px4_layer diff --git a/cmake/configs/posix_sitl_simple.cmake b/cmake/configs/posix_sitl_default.cmake similarity index 93% rename from cmake/configs/posix_sitl_simple.cmake rename to cmake/configs/posix_sitl_default.cmake index 581b82e827..b3c9205586 100644 --- a/cmake/configs/posix_sitl_simple.cmake +++ b/cmake/configs/posix_sitl_default.cmake @@ -3,11 +3,9 @@ include(posix/px4_impl_posix) set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake) set(config_module_list - drivers/led drivers/device drivers/boards/sitl drivers/pwm_out_sim - drivers/rgbled platforms/common platforms/posix/px4_layer platforms/posix/work_queue @@ -18,12 +16,15 @@ set(config_module_list platforms/posix/drivers/airspeedsim platforms/posix/drivers/barosim platforms/posix/drivers/gyrosim + platforms/posix/drivers/rgbledsim + platforms/posix/drivers/ledsim systemcmds/param systemcmds/mixer systemcmds/ver systemcmds/esc_calib systemcmds/reboot systemcmds/topic_listener + systemcmds/perf modules/uORB modules/param modules/systemlib @@ -56,6 +57,8 @@ set(config_module_list lib/geo lib/geo_lookup lib/launchdetection + lib/terrain_estimation + lib/runway_takeoff ) set(config_extra_builtin_cmds diff --git a/cmake/configs/posix_sitl_lpe.cmake b/cmake/configs/posix_sitl_lpe.cmake index 0598044844..c264466f9d 100644 --- a/cmake/configs/posix_sitl_lpe.cmake +++ b/cmake/configs/posix_sitl_lpe.cmake @@ -1,4 +1,4 @@ -include(cmake/configs/posix_sitl_simple.cmake) +include(cmake/configs/posix_sitl_default.cmake) list(APPEND config_module_list modules/local_position_estimator diff --git a/cmake/configs/qurt_eagle_hil.cmake b/cmake/configs/qurt_eagle_hil.cmake index 4222fff249..87dd29152e 100644 --- a/cmake/configs/qurt_eagle_hil.cmake +++ b/cmake/configs/qurt_eagle_hil.cmake @@ -47,6 +47,8 @@ set(config_module_list lib/geo lib/geo_lookup lib/conversion + lib/terrain_estimation + lib/runway_takeoff modules/controllib # diff --git a/cmake/configs/qurt_eagle_release.cmake b/cmake/configs/qurt_eagle_release.cmake index 945e9da199..62a20263e5 100644 --- a/cmake/configs/qurt_eagle_release.cmake +++ b/cmake/configs/qurt_eagle_release.cmake @@ -70,6 +70,8 @@ set(config_module_list lib/geo lib/geo_lookup lib/conversion + lib/terrain_estimation + lib/runway_takeoff # # QuRT port diff --git a/cmake/configs/qurt_eagle_travis.cmake b/cmake/configs/qurt_eagle_travis.cmake index b9fda6ec48..75cb5f2394 100644 --- a/cmake/configs/qurt_eagle_travis.cmake +++ b/cmake/configs/qurt_eagle_travis.cmake @@ -52,6 +52,8 @@ set(config_module_list lib/geo_lookup lib/conversion lib/ecl + lib/terrain_estimation + lib/runway_takeoff # # QuRT port diff --git a/cmake/nuttx/px4_impl_nuttx.cmake b/cmake/nuttx/px4_impl_nuttx.cmake index af7391f45c..ae3d95eaf5 100644 --- a/cmake/nuttx/px4_impl_nuttx.cmake +++ b/cmake/nuttx/px4_impl_nuttx.cmake @@ -429,6 +429,7 @@ function(px4_os_add_flags) ) set(added_definitions -D__PX4_NUTTX + -D__DF_NUTTX ) set(added_c_flags -nodefaultlibs @@ -458,6 +459,14 @@ function(px4_os_add_flags) -mfpu=fpv4-sp-d16 -mfloat-abi=hard ) + elseif (${BOARD} STREQUAL "px4fmu-v4") + set(cpu_flags + -mcpu=cortex-m4 + -mthumb + -march=armv7e-m + -mfpu=fpv4-sp-d16 + -mfloat-abi=hard + ) elseif (${BOARD} STREQUAL "aerocore") set(cpu_flags -mcpu=cortex-m4 @@ -489,6 +498,8 @@ function(px4_os_add_flags) #message(STATUS "nuttx: set(${${var}} ${${${var}}} ${added_${lower_var}} PARENT_SCOPE)") endforeach() + set(DF_TARGET "nuttx" PARENT_SCOPE) + endfunction() #============================================================================= diff --git a/cmake/posix/px4_impl_posix.cmake b/cmake/posix/px4_impl_posix.cmake index d51b5527d5..4c2dfbb709 100644 --- a/cmake/posix/px4_impl_posix.cmake +++ b/cmake/posix/px4_impl_posix.cmake @@ -170,25 +170,32 @@ if(UNIX AND APPLE) set(added_definitions -D__PX4_POSIX -D__PX4_DARWIN + -D__DF_DARWIN -DCLOCK_MONOTONIC=1 -Dnoreturn_function=__attribute__\(\(noreturn\)\) -include ${PX4_INCLUDE_DIR}visibility.h ) + set(added_exe_linker_flags + -lpthread + ) + else() set(added_definitions -D__PX4_POSIX -D__PX4_LINUX + -D__DF_LINUX -DCLOCK_MONOTONIC=1 -Dnoreturn_function=__attribute__\(\(noreturn\)\) -include ${PX4_INCLUDE_DIR}visibility.h ) -endif() set(added_exe_linker_flags - -lpthread + -lpthread -lrt ) + +endif() # Add the toolchain specific flags diff --git a/cmake/qurt/px4_impl_qurt.cmake b/cmake/qurt/px4_impl_qurt.cmake index 312c818ed5..5c40dfebd7 100644 --- a/cmake/qurt/px4_impl_qurt.cmake +++ b/cmake/qurt/px4_impl_qurt.cmake @@ -172,6 +172,7 @@ function(px4_os_add_flags) set(added_definitions -D__PX4_QURT -D__PX4_POSIX + -D__DF_QURT -include ${PX4_INCLUDE_DIR}visibility.h ) @@ -185,6 +186,8 @@ function(px4_os_add_flags) set(CMAKE_SHARED_LIBRARY_LINK_C_FLAGS "") set(CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "") + set(DF_TARGET "qurt" PARENT_SCOPE) + # output foreach(var ${inout_vars}) string(TOLOWER ${var} lower_var) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 86a2b147e4..db5cf66d1a 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 86a2b147e4cec5066efdc6d030d69d902929e66f +Subproject commit db5cf66d1a7f549ccbaa92b5476e78754e008297 diff --git a/msg/actuator_controls.msg b/msg/actuator_controls.msg index 66e12325d2..a6ebda6aae 100644 --- a/msg/actuator_controls.msg +++ b/msg/actuator_controls.msg @@ -5,6 +5,9 @@ uint8 INDEX_PITCH = 1 uint8 INDEX_YAW = 2 uint8 INDEX_THROTTLE = 3 uint8 INDEX_FLAPS = 4 +uint8 INDEX_SPOILERS = 5 +uint8 INDEX_AIRBRAKES = 6 +uint8 INDEX_LANDING_GEAR = 7 uint8 GROUP_INDEX_ATTITUDE = 0 uint64 timestamp uint64 timestamp_sample # the timestamp the data this control response is based on was sampled diff --git a/msg/control_state.msg b/msg/control_state.msg index 4f130353d3..e504a765da 100644 --- a/msg/control_state.msg +++ b/msg/control_state.msg @@ -10,6 +10,7 @@ float32 x_pos # X position in local frame float32 y_pos # Y position in local frame float32 z_pos # z position in local frame float32 airspeed # Airspeed, estimated +bool airspeed_valid # False: Non-finite values or non-updating sensor float32[3] vel_variance # Variance in body velocity estimate float32[3] pos_variance # Variance in local position estimate float32[4] q # Attitude Quaternion diff --git a/msg/fw_virtual_attitude_setpoint.msg b/msg/fw_virtual_attitude_setpoint.msg new file mode 100644 index 0000000000..7bbb670b31 --- /dev/null +++ b/msg/fw_virtual_attitude_setpoint.msg @@ -0,0 +1,23 @@ + +uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data + +float32 roll_body # body angle in NED frame +float32 pitch_body # body angle in NED frame +float32 yaw_body # body angle in NED frame + +float32 yaw_sp_move_rate # rad/s (commanded by user) + +float32[9] R_body # Rotation matrix describing the setpoint as rotation from the current body frame +bool R_valid # Set to true if rotation matrix is valid + +# For quaternion-based attitude control +float32[4] q_d # Desired quaternion for quaternion control +bool q_d_valid # Set to true if quaternion vector is valid +float32[4] q_e # Attitude error in quaternion +bool q_e_valid # Set to true if quaternion error vector is valid + +float32 thrust # Thrust in Newton the power system should generate + +bool roll_reset_integral # Reset roll integral part (navigation logic change) +bool pitch_reset_integral # Reset pitch integral part (navigation logic change) +bool yaw_reset_integral # Reset yaw integral part (navigation logic change) diff --git a/msg/mc_virtual_attitude_setpoint.msg b/msg/mc_virtual_attitude_setpoint.msg new file mode 100644 index 0000000000..7bbb670b31 --- /dev/null +++ b/msg/mc_virtual_attitude_setpoint.msg @@ -0,0 +1,23 @@ + +uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data + +float32 roll_body # body angle in NED frame +float32 pitch_body # body angle in NED frame +float32 yaw_body # body angle in NED frame + +float32 yaw_sp_move_rate # rad/s (commanded by user) + +float32[9] R_body # Rotation matrix describing the setpoint as rotation from the current body frame +bool R_valid # Set to true if rotation matrix is valid + +# For quaternion-based attitude control +float32[4] q_d # Desired quaternion for quaternion control +bool q_d_valid # Set to true if quaternion vector is valid +float32[4] q_e # Attitude error in quaternion +bool q_e_valid # Set to true if quaternion error vector is valid + +float32 thrust # Thrust in Newton the power system should generate + +bool roll_reset_integral # Reset roll integral part (navigation logic change) +bool pitch_reset_integral # Reset pitch integral part (navigation logic change) +bool yaw_reset_integral # Reset yaw integral part (navigation logic change) diff --git a/msg/navigation_capabilities.msg b/msg/navigation_capabilities.msg index 235b5df03b..6d12aaaed9 100644 --- a/msg/navigation_capabilities.msg +++ b/msg/navigation_capabilities.msg @@ -3,3 +3,4 @@ float32 turn_distance # the optimal distance to a waypoint to switch to the nex float32 landing_horizontal_slope_displacement float32 landing_slope_angle_rad float32 landing_flare_length +bool abort_landing diff --git a/msg/rc_channels.msg b/msg/rc_channels.msg index b2e08d864a..6b30794e00 100644 --- a/msg/rc_channels.msg +++ b/msg/rc_channels.msg @@ -26,3 +26,4 @@ uint8 channel_count # Number of valid channels int8[20] function # Functions mapping uint8 rssi # Receive signal strength index bool signal_lost # Control signal lost, should be checked together with topic timeout +uint32 frame_drop_count # Number of dropped frames diff --git a/msg/vehicle_attitude_setpoint.msg b/msg/vehicle_attitude_setpoint.msg index 7bbb670b31..43ea237b47 100644 --- a/msg/vehicle_attitude_setpoint.msg +++ b/msg/vehicle_attitude_setpoint.msg @@ -21,3 +21,7 @@ float32 thrust # Thrust in Newton the power system should generate bool roll_reset_integral # Reset roll integral part (navigation logic change) bool pitch_reset_integral # Reset pitch integral part (navigation logic change) bool yaw_reset_integral # Reset yaw integral part (navigation logic change) + +bool fw_control_yaw # control heading with rudder (used for auto takeoff on runway) + +bool apply_flaps diff --git a/msg/vehicle_global_position.msg b/msg/vehicle_global_position.msg index 6a38e2c9d7..cee381e871 100644 --- a/msg/vehicle_global_position.msg +++ b/msg/vehicle_global_position.msg @@ -18,3 +18,4 @@ float32 epv # Standard deviation of position vertically float32 terrain_alt # Terrain altitude in m, WGS84 bool terrain_alt_valid # Terrain altitude estimate is valid bool dead_reckoning # True if this position is estimated through dead-reckoning +float32 pressure_alt # Pressure altitude diff --git a/msg/vehicle_gps_position.msg b/msg/vehicle_gps_position.msg index 25c532ccaa..718144f327 100644 --- a/msg/vehicle_gps_position.msg +++ b/msg/vehicle_gps_position.msg @@ -3,6 +3,7 @@ uint64 timestamp_position # Timestamp for position information int32 lat # Latitude in 1E-7 degrees int32 lon # Longitude in 1E-7 degrees int32 alt # Altitude in 1E-3 meters (millimeters) above MSL +int32 alt_ellipsoid # Altitude in 1E-3 meters (millimeters) above Ellipsoid uint64 timestamp_variance float32 s_variance_m_s # speed accuracy estimate m/s diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index c91672c0be..f3a1767dde 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -9,7 +9,8 @@ uint8 MAIN_STATE_ACRO = 6 uint8 MAIN_STATE_OFFBOARD = 7 uint8 MAIN_STATE_STAB = 8 uint8 MAIN_STATE_RATTITUDE = 9 -uint8 MAIN_STATE_MAX = 10 +uint8 MAIN_STATE_AUTO_TAKEOFF = 10 +uint8 MAIN_STATE_MAX = 11 # If you change the order, add or remove arming_state_t states make sure to update the arrays # in state_machine_helper.cpp as well. @@ -43,7 +44,8 @@ uint8 NAVIGATION_STATE_TERMINATION = 13 # Termination mode uint8 NAVIGATION_STATE_OFFBOARD = 14 uint8 NAVIGATION_STATE_STAB = 15 # Stabilized mode uint8 NAVIGATION_STATE_RATTITUDE = 16 # Rattitude (aka "flip") mode -uint8 NAVIGATION_STATE_MAX = 17 +uint8 NAVIGATION_STATE_AUTO_TAKEOFF = 17 # Takeoff +uint8 NAVIGATION_STATE_MAX = 18 # VEHICLE_MODE_FLAG, same as MAV_MODE_FLAG of MAVLink 1.0 protocol uint8 VEHICLE_MODE_FLAG_SAFETY_ARMED = 128 @@ -113,6 +115,7 @@ uint32 system_id # system id, inspired by MAVLink's system ID field uint32 component_id # subsystem / component id, inspired by MAVLink's component ID field bool is_rotary_wing # True if system is in rotary wing configuration, so for a VTOL this is only true while flying as a multicopter +bool vtol_in_transition # True if VTOL is doing a transition bool is_vtol # True if the system is VTOL capable bool vtol_fw_permanent_stab # True if vtol should stabilize attitude for fw in manual mode bool in_transition_mode @@ -141,7 +144,6 @@ bool rc_signal_lost_cmd # true if RC lost mode is commanded bool rc_input_blocked # set if RC input should be ignored temporarily uint8 rc_input_mode # set to 1 to disable the RC input, 2 to enable manual control to RC in mapping. -bool data_link_found_new # new datalink to GCS found bool data_link_lost # datalink to GCS lost bool data_link_lost_cmd # datalink to GCS lost mode commanded uint8 data_link_lost_counter # counts unique data link lost events diff --git a/nuttx-configs/px4fmu-v4/include/board.h b/nuttx-configs/px4fmu-v4/include/board.h new file mode 100755 index 0000000000..2f7c1189f3 --- /dev/null +++ b/nuttx-configs/px4fmu-v4/include/board.h @@ -0,0 +1,299 @@ +/************************************************************************************ + * configs/px4fmu/include/board.h + * include/arch/board/board.h + * + * Copyright (C) 2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ************************************************************************************/ + +#ifndef __ARCH_BOARD_BOARD_H +#define __ARCH_BOARD_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include +#ifndef __ASSEMBLY__ +# include +#endif + +#include + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ +/* The PX4FMUV2 uses a 24MHz crystal connected to the HSE. + * + * This is the "standard" configuration as set up by arch/arm/src/stm32f40xx_rcc.c: + * System Clock source : PLL (HSE) + * SYSCLK(Hz) : 168000000 Determined by PLL configuration + * HCLK(Hz) : 168000000 (STM32_RCC_CFGR_HPRE) + * AHB Prescaler : 1 (STM32_RCC_CFGR_HPRE) + * APB1 Prescaler : 4 (STM32_RCC_CFGR_PPRE1) + * APB2 Prescaler : 2 (STM32_RCC_CFGR_PPRE2) + * HSE Frequency(Hz) : 24000000 (STM32_BOARD_XTAL) + * PLLM : 24 (STM32_PLLCFG_PLLM) + * PLLN : 336 (STM32_PLLCFG_PLLN) + * PLLP : 2 (STM32_PLLCFG_PLLP) + * PLLQ : 7 (STM32_PLLCFG_PPQ) + * Main regulator output voltage : Scale1 mode Needed for high speed SYSCLK + * Flash Latency(WS) : 5 + * Prefetch Buffer : OFF + * Instruction cache : ON + * Data cache : ON + * Require 48MHz for USB OTG FS, : Enabled + * SDIO and RNG clock + */ + +/* HSI - 16 MHz RC factory-trimmed + * LSI - 32 KHz RC + * HSE - On-board crystal frequency is 24MHz + * LSE - not installed + */ + +#define STM32_BOARD_XTAL 24000000ul + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL +//#define STM32_LSE_FREQUENCY 32768 + +/* Main PLL Configuration. + * + * PLL source is HSE + * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * = (25,000,000 / 25) * 336 + * = 336,000,000 + * SYSCLK = PLL_VCO / PLLP + * = 336,000,000 / 2 = 168,000,000 + * USB OTG FS, SDIO and RNG Clock + * = PLL_VCO / PLLQ + * = 48,000,000 + */ + +#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(24) +#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(336) +#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_2 +#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(7) + +#define STM32_SYSCLK_FREQUENCY 168000000ul + +/* AHB clock (HCLK) is SYSCLK (168MHz) */ + +#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK /* HCLK = SYSCLK / 1 */ +#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/4 (42MHz) */ + +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd4 /* PCLK1 = HCLK / 4 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/4) + +/* Timers driven from APB1 will be twice PCLK1 */ + +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* APB2 clock (PCLK2) is HCLK/2 (84MHz) */ + +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timers driven from APB2 will be twice PCLK2 */ + +#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM9_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK2_FREQUENCY) + +/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx + * otherwise frequency is 2xAPBx. + * Note: TIM1,8 are on APB2, others on APB1 + */ + +#define STM32_TIM18_FREQUENCY (2*STM32_PCLK2_FREQUENCY) +#define STM32_TIM27_FREQUENCY (2*STM32_PCLK1_FREQUENCY) + +/* SDIO dividers. Note that slower clocking is required when DMA is disabled + * in order to avoid RX overrun/TX underrun errors due to delayed responses + * to service FIFOs in interrupt driven mode. These values have not been + * tuned!!! + * + * HCLK=72MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(178+2)=400 KHz + */ + +#define SDIO_INIT_CLKDIV (178 << SDIO_CLKCR_CLKDIV_SHIFT) + +/* DMA ON: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(2+2)=18 MHz + * DMA OFF: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(3+2)=14.4 MHz + */ + +#ifdef CONFIG_SDIO_DMA +# define SDIO_MMCXFR_CLKDIV (2 << SDIO_CLKCR_CLKDIV_SHIFT) +#else +# define SDIO_MMCXFR_CLKDIV (3 << SDIO_CLKCR_CLKDIV_SHIFT) +#endif + +/* DMA ON: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(1+2)=24 MHz + * DMA OFF: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(3+2)=14.4 MHz + */ + +#ifdef CONFIG_SDIO_DMA +# define SDIO_SDXFR_CLKDIV (1 << SDIO_CLKCR_CLKDIV_SHIFT) +#else +# define SDIO_SDXFR_CLKDIV (3 << SDIO_CLKCR_CLKDIV_SHIFT) +#endif + +/* DMA Channl/Stream Selections *****************************************************/ +/* Stream selections are arbitrary for now but might become important in the future + * is we set aside more DMA channels/streams. + * + * SDIO DMA + *   DMAMAP_SDIO_1 = Channel 4, Stream 3 <- may later be used by SPI DMA + *   DMAMAP_SDIO_2 = Channel 4, Stream 6 + */ + +#define DMAMAP_SDIO DMAMAP_SDIO_1 + +/* Alternate function pin selections ************************************************/ + +/* + * UARTs. + */ +#define GPIO_USART1_RX GPIO_USART1_RX_2 /* ESP8266 */ +#define GPIO_USART1_TX GPIO_USART1_TX_2 + +#define GPIO_USART2_RX GPIO_USART2_RX_2 +#define GPIO_USART2_TX GPIO_USART2_TX_2 +#define GPIO_USART2_RTS GPIO_USART2_RTS_2 +#define GPIO_USART2_CTS GPIO_USART2_CTS_2 + +#define GPIO_USART3_RX GPIO_USART3_RX_3 +#define GPIO_USART3_TX GPIO_USART3_TX_3 +#define GPIO_USART3_RTS GPIO_USART3_RTS_2 +#define GPIO_USART3_CTS GPIO_USART3_CTS_2 + +#define GPIO_UART4_RX GPIO_UART4_RX_1 +#define GPIO_UART4_TX GPIO_UART4_TX_1 + +#define GPIO_USART6_RX GPIO_USART6_RX_1 +#define GPIO_USART6_TX GPIO_USART6_TX_1 + +#define GPIO_UART7_RX GPIO_UART7_RX_1 +#define GPIO_UART7_TX GPIO_UART7_TX_1 + +/* UART8 has no alternate pin config */ + +/* UART RX DMA configurations */ +#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 +#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 + +/* + * CAN + * + * CAN1 is routed to the onboard transceiver. + */ +#define GPIO_CAN1_RX GPIO_CAN1_RX_3 +#define GPIO_CAN1_TX GPIO_CAN1_TX_3 + +/* + * I2C + * + * The optional _GPIO configurations allow the I2C driver to manually + * reset the bus to clear stuck slaves. They match the pin configuration, + * but are normally-high GPIOs. + */ +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 +#define GPIO_I2C1_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN8) +#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9) + + +/* + * SPI + * + * There are sensors on SPI1, and SPI2 is connected to the FRAM. + */ +#define GPIO_SPI1_MISO (GPIO_SPI1_MISO_1|GPIO_SPEED_50MHz) +#define GPIO_SPI1_MOSI (GPIO_SPI1_MOSI_1|GPIO_SPEED_50MHz) +#define GPIO_SPI1_SCK (GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz) + +#define GPIO_SPI2_MISO (GPIO_SPI2_MISO_1|GPIO_SPEED_50MHz) +#define GPIO_SPI2_MOSI (GPIO_SPI2_MOSI_1|GPIO_SPEED_50MHz) +#define GPIO_SPI2_SCK (GPIO_SPI2_SCK_2|GPIO_SPEED_50MHz) + +/************************************************************************************ + * Public Data + ************************************************************************************/ + +#ifndef __ASSEMBLY__ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +/************************************************************************************ + * Public Function Prototypes + ************************************************************************************/ +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +EXTERN void stm32_boardinitialize(void); + +#undef EXTERN +#if defined(__cplusplus) +} +#endif + +#endif /* __ASSEMBLY__ */ +#endif /* __ARCH_BOARD_BOARD_H */ diff --git a/nuttx-configs/px4fmu-v4/include/nsh_romfsimg.h b/nuttx-configs/px4fmu-v4/include/nsh_romfsimg.h new file mode 100644 index 0000000000..15e4e7a8d5 --- /dev/null +++ b/nuttx-configs/px4fmu-v4/include/nsh_romfsimg.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * nsh_romfsetc.h + * + * This file is a stub for 'make export' purposes; the actual ROMFS + * must be supplied by the library client. + */ + +extern unsigned char romfs_img[]; +extern unsigned int romfs_img_len; diff --git a/nuttx-configs/px4fmu-v4/nsh/Make.defs b/nuttx-configs/px4fmu-v4/nsh/Make.defs new file mode 100644 index 0000000000..6177f4c36d --- /dev/null +++ b/nuttx-configs/px4fmu-v4/nsh/Make.defs @@ -0,0 +1,177 @@ +############################################################################ +# configs/px4fmu-v2/nsh/Make.defs +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name NuttX nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +include ${TOPDIR}/.config +include ${TOPDIR}/tools/Config.mk + +# +# We only support building with the ARM bare-metal toolchain from +# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. +# +CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI + +include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs + +CC = $(CROSSDEV)gcc +CXX = $(CROSSDEV)g++ +CPP = $(CROSSDEV)gcc -E +LD = $(CROSSDEV)ld +AR = $(CROSSDEV)ar rcs +NM = $(CROSSDEV)nm +OBJCOPY = $(CROSSDEV)objcopy +OBJDUMP = $(CROSSDEV)objdump + +MAXOPTIMIZATION = -Os +ARCHCPUFLAGS = -mcpu=cortex-m4 \ + -mthumb \ + -march=armv7e-m \ + -mfpu=fpv4-sp-d16 \ + -mfloat-abi=hard + + +# enable precise stack overflow tracking +ifeq ($(CONFIG_ARMV7M_STACKCHECK),y) +INSTRUMENTATIONDEFINES = -finstrument-functions -ffixed-r10 +endif + +# pull in *just* libm from the toolchain ... this is grody +LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" +EXTRA_LIBS += $(LIBM) + +# use our linker script +LDSCRIPT = ld.script + +ifeq ($(WINTOOL),y) + # Windows-native toolchains + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" + ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" + ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}" +else + ifeq ($(PX4_WINTOOL),y) + # Windows-native toolchains (MSYS) + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + else + # Linux/Cygwin-native toolchain + MKDEP = $(TOPDIR)/tools/mkdeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + endif +endif + +# tool versions +ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} +ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} + +# optimisation flags +ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ + -fno-strict-aliasing \ + -fno-strength-reduce \ + -fomit-frame-pointer \ + -funsafe-math-optimizations \ + -fno-builtin-printf \ + -ffunction-sections \ + -fdata-sections + +ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") +ARCHOPTIMIZATION += -g +endif + +ARCHCFLAGS = -std=gnu99 +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x +ARCHWARNINGS = -Wall \ + -Wno-sign-compare \ + -Wextra \ + -Wdouble-promotion \ + -Wshadow \ + -Wframe-larger-than=1024 \ + -Wpointer-arith \ + -Wlogical-op \ + -Wpacked \ + -Wno-unused-parameter +# -Wcast-qual - generates spurious noreturn attribute warnings, try again later +# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code +# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives + +ARCHCWARNINGS = $(ARCHWARNINGS) \ + -Wbad-function-cast \ + -Wstrict-prototypes \ + -Wold-style-declaration \ + -Wmissing-parameter-type \ + -Wnested-externs +ARCHWARNINGSXX = $(ARCHWARNINGS) \ + -Wno-psabi +ARCHDEFINES = +ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 + +# this seems to be the only way to add linker flags +EXTRA_LIBS += --warn-common \ + --gc-sections + +CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common +CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) +CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe +CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) +CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ + +NXFLATLDFLAGS1 = -r -d -warn-common +NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections +LDNXFLATFLAGS = -e main -s 2048 + +OBJEXT = .o +LIBEXT = .a +EXEEXT = + + +# produce partially-linked $1 from files in $2 +define PRELINK + @echo "PRELINK: $1" + $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 +endef + +HOSTCC = gcc +HOSTINCLUDES = -I. +HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe +HOSTLDFLAGS = + diff --git a/src/platforms/posix/drivers/gyrosim/module.mk b/nuttx-configs/px4fmu-v4/nsh/appconfig similarity index 73% rename from src/platforms/posix/drivers/gyrosim/module.mk rename to nuttx-configs/px4fmu-v4/nsh/appconfig index 7bd37c2e2a..0e18aa8ef1 100644 --- a/src/platforms/posix/drivers/gyrosim/module.mk +++ b/nuttx-configs/px4fmu-v4/nsh/appconfig @@ -1,6 +1,8 @@ ############################################################################ +# configs/px4fmu/nsh/appconfig # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -12,7 +14,7 @@ # notice, this list of conditions and the following disclaimer in # the documentation and/or other materials provided with the # distribution. -# 3. Neither the name PX4 nor the names of its contributors may be +# 3. Neither the name NuttX nor the names of its contributors may be # used to endorse or promote products derived from this software # without specific prior written permission. # @@ -31,16 +33,20 @@ # ############################################################################ -# -# Makefile to build the MPU6000 driver. -# +# Path to example in apps/examples containing the user_start entry point -MODULE_COMMAND = gyrosim +CONFIGURED_APPS += examples/nsh -SRCS = gyrosim.cpp +# The NSH application library +CONFIGURED_APPS += nshlib +CONFIGURED_APPS += system/readline -MODULE_STACKSIZE = 1200 +ifeq ($(CONFIG_CAN),y) +#CONFIGURED_APPS += examples/can +endif -EXTRACXXFLAGS = -Weffc++ - -MAXOPTIMIZATION = -Os +#ifeq ($(CONFIG_USBDEV),y) +#ifeq ($(CONFIG_CDCACM),y) +CONFIGURED_APPS += examples/cdcacm +#endif +#endif diff --git a/nuttx-configs/px4fmu-v4/nsh/defconfig b/nuttx-configs/px4fmu-v4/nsh/defconfig new file mode 100644 index 0000000000..5e9c276494 --- /dev/null +++ b/nuttx-configs/px4fmu-v4/nsh/defconfig @@ -0,0 +1,1068 @@ +# +# Automatically generated file; DO NOT EDIT. +# Nuttx/ Configuration +# +CONFIG_NUTTX_NEWCONFIG=y + +# +# Build Setup +# +# CONFIG_EXPERIMENTAL is not set +# CONFIG_HOST_LINUX is not set +CONFIG_HOST_OSX=y +# CONFIG_HOST_WINDOWS is not set +# CONFIG_HOST_OTHER is not set + +# +# Build Configuration +# +CONFIG_APPS_DIR="../apps" +# CONFIG_BUILD_2PASS is not set + +# +# Binary Output Formats +# +# CONFIG_RRLOAD_BINARY is not set +# CONFIG_INTELHEX_BINARY is not set +# CONFIG_MOTOROLA_SREC is not set +CONFIG_RAW_BINARY=y + +# +# Customize Header Files +# +# CONFIG_ARCH_STDBOOL_H is not set +CONFIG_ARCH_MATH_H=y +# CONFIG_ARCH_FLOAT_H is not set +# CONFIG_ARCH_STDARG_H is not set + +# +# Debug Options +# +CONFIG_DEBUG=n +CONFIG_DEBUG_VERBOSE=n + +# +# Subsystem Debug Options +# +# CONFIG_DEBUG_MM is not set +# CONFIG_DEBUG_SCHED is not set +# CONFIG_DEBUG_USB is not set +CONFIG_DEBUG_FS=y +# CONFIG_DEBUG_LIB is not set +# CONFIG_DEBUG_BINFMT is not set +# CONFIG_DEBUG_GRAPHICS is not set + +# +# Driver Debug Options +# +# CONFIG_DEBUG_ANALOG is not set +# CONFIG_DEBUG_I2C is not set +# CONFIG_DEBUG_SPI is not set +# CONFIG_DEBUG_SDIO is not set +# CONFIG_DEBUG_GPIO is not set +CONFIG_DEBUG_DMA=y +# CONFIG_DEBUG_WATCHDOG is not set +# CONFIG_DEBUG_AUDIO is not set +CONFIG_DEBUG_SYMBOLS=y + +# +# System Type +# +# CONFIG_ARCH_8051 is not set +CONFIG_ARCH_ARM=y +# CONFIG_ARCH_AVR is not set +# CONFIG_ARCH_HC is not set +# CONFIG_ARCH_MIPS is not set +# CONFIG_ARCH_RGMP is not set +# CONFIG_ARCH_SH is not set +# CONFIG_ARCH_SIM is not set +# CONFIG_ARCH_X86 is not set +# CONFIG_ARCH_Z16 is not set +# CONFIG_ARCH_Z80 is not set +CONFIG_ARCH="arm" + +# +# ARM Options +# +# CONFIG_ARCH_CHIP_C5471 is not set +# CONFIG_ARCH_CHIP_CALYPSO is not set +# CONFIG_ARCH_CHIP_DM320 is not set +# CONFIG_ARCH_CHIP_IMX is not set +# CONFIG_ARCH_CHIP_KINETIS is not set +# CONFIG_ARCH_CHIP_KL is not set +# CONFIG_ARCH_CHIP_LM is not set +# CONFIG_ARCH_CHIP_LPC17XX is not set +# CONFIG_ARCH_CHIP_LPC214X is not set +# CONFIG_ARCH_CHIP_LPC2378 is not set +# CONFIG_ARCH_CHIP_LPC31XX is not set +# CONFIG_ARCH_CHIP_LPC43XX is not set +# CONFIG_ARCH_CHIP_NUC1XX is not set +# CONFIG_ARCH_CHIP_SAM34 is not set +CONFIG_ARCH_CHIP_STM32=y +# CONFIG_ARCH_CHIP_STR71X is not set +CONFIG_ARCH_CORTEXM4=y +CONFIG_ARCH_FAMILY="armv7-m" +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARCH_HAVE_CMNVECTOR=y +CONFIG_ARMV7M_CMNVECTOR=y +CONFIG_ARCH_HAVE_FPU=y +CONFIG_ARCH_FPU=y +CONFIG_ARCH_HAVE_MPU=y +# CONFIG_ARMV7M_MPU is not set +# CONFIG_DEBUG_HARDFAULT is not set + +# +# ARMV7M Configuration Options +# +# CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set +CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y +CONFIG_ARMV7M_STACKCHECK=n +CONFIG_SERIAL_TERMIOS=y +CONFIG_SDIO_DMA=y +CONFIG_SDIO_DMAPRIO=0x00010000 +# CONFIG_SDIO_WIDTH_D1_ONLY is not set + +# +# STM32 Configuration Options +# +# CONFIG_ARCH_CHIP_STM32L151C6 is not set +# CONFIG_ARCH_CHIP_STM32L151C8 is not set +# CONFIG_ARCH_CHIP_STM32L151CB is not set +# CONFIG_ARCH_CHIP_STM32L151R6 is not set +# CONFIG_ARCH_CHIP_STM32L151R8 is not set +# CONFIG_ARCH_CHIP_STM32L151RB is not set +# CONFIG_ARCH_CHIP_STM32L151V6 is not set +# CONFIG_ARCH_CHIP_STM32L151V8 is not set +# CONFIG_ARCH_CHIP_STM32L151VB is not set +# CONFIG_ARCH_CHIP_STM32L152C6 is not set +# CONFIG_ARCH_CHIP_STM32L152C8 is not set +# CONFIG_ARCH_CHIP_STM32L152CB is not set +# CONFIG_ARCH_CHIP_STM32L152R6 is not set +# CONFIG_ARCH_CHIP_STM32L152R8 is not set +# CONFIG_ARCH_CHIP_STM32L152RB is not set +# CONFIG_ARCH_CHIP_STM32L152V6 is not set +# CONFIG_ARCH_CHIP_STM32L152V8 is not set +# CONFIG_ARCH_CHIP_STM32L152VB is not set +# CONFIG_ARCH_CHIP_STM32F100C8 is not set +# CONFIG_ARCH_CHIP_STM32F100CB is not set +# CONFIG_ARCH_CHIP_STM32F100R8 is not set +# CONFIG_ARCH_CHIP_STM32F100RB is not set +# CONFIG_ARCH_CHIP_STM32F100RC is not set +# CONFIG_ARCH_CHIP_STM32F100RD is not set +# CONFIG_ARCH_CHIP_STM32F100RE is not set +# CONFIG_ARCH_CHIP_STM32F100V8 is not set +# CONFIG_ARCH_CHIP_STM32F100VB is not set +# CONFIG_ARCH_CHIP_STM32F100VC is not set +# CONFIG_ARCH_CHIP_STM32F100VD is not set +# CONFIG_ARCH_CHIP_STM32F100VE is not set +# CONFIG_ARCH_CHIP_STM32F103C4 is not set +# CONFIG_ARCH_CHIP_STM32F103C8 is not set +# CONFIG_ARCH_CHIP_STM32F103RET6 is not set +# CONFIG_ARCH_CHIP_STM32F103VCT6 is not set +# CONFIG_ARCH_CHIP_STM32F103VET6 is not set +# CONFIG_ARCH_CHIP_STM32F103ZET6 is not set +# CONFIG_ARCH_CHIP_STM32F105VBT7 is not set +# CONFIG_ARCH_CHIP_STM32F107VC is not set +# CONFIG_ARCH_CHIP_STM32F207IG is not set +# CONFIG_ARCH_CHIP_STM32F302CB is not set +# CONFIG_ARCH_CHIP_STM32F302CC is not set +# CONFIG_ARCH_CHIP_STM32F302RB is not set +# CONFIG_ARCH_CHIP_STM32F302RC is not set +# CONFIG_ARCH_CHIP_STM32F302VB is not set +# CONFIG_ARCH_CHIP_STM32F302VC is not set +# CONFIG_ARCH_CHIP_STM32F303CB is not set +# CONFIG_ARCH_CHIP_STM32F303CC is not set +# CONFIG_ARCH_CHIP_STM32F303RB is not set +# CONFIG_ARCH_CHIP_STM32F303RC is not set +# CONFIG_ARCH_CHIP_STM32F303VB is not set +# CONFIG_ARCH_CHIP_STM32F303VC is not set +# CONFIG_ARCH_CHIP_STM32F405RG is not set +# CONFIG_ARCH_CHIP_STM32F405VG is not set +# CONFIG_ARCH_CHIP_STM32F405ZG is not set +# CONFIG_ARCH_CHIP_STM32F407VE is not set +# CONFIG_ARCH_CHIP_STM32F407VG is not set +# CONFIG_ARCH_CHIP_STM32F407ZE is not set +# CONFIG_ARCH_CHIP_STM32F407ZG is not set +# CONFIG_ARCH_CHIP_STM32F407IE is not set +# CONFIG_ARCH_CHIP_STM32F407IG is not set +CONFIG_ARCH_CHIP_STM32F427V=y +# CONFIG_ARCH_CHIP_STM32F427Z is not set +# CONFIG_ARCH_CHIP_STM32F427I is not set +# CONFIG_STM32_STM32L15XX is not set +# CONFIG_STM32_ENERGYLITE is not set +# CONFIG_STM32_STM32F10XX is not set +# CONFIG_STM32_VALUELINE is not set +# CONFIG_STM32_CONNECTIVITYLINE is not set +# CONFIG_STM32_PERFORMANCELINE is not set +# CONFIG_STM32_HIGHDENSITY is not set +# CONFIG_STM32_MEDIUMDENSITY is not set +# CONFIG_STM32_LOWDENSITY is not set +# CONFIG_STM32_STM32F20XX is not set +# CONFIG_STM32_STM32F30XX is not set +CONFIG_STM32_STM32F40XX=y +CONFIG_STM32_STM32F427=y +# CONFIG_STM32_DFU is not set + +# +# STM32 Peripheral Support +# +CONFIG_STM32_ADC1=y +# CONFIG_STM32_ADC2 is not set +# CONFIG_STM32_ADC3 is not set +CONFIG_STM32_BKPSRAM=y +# CONFIG_STM32_CAN1 is not set +# CONFIG_STM32_CAN2 is not set +CONFIG_STM32_CCMDATARAM=y +# CONFIG_STM32_CRC is not set +# CONFIG_STM32_CRYP is not set +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=y +# CONFIG_STM32_DAC1 is not set +# CONFIG_STM32_DAC2 is not set +# CONFIG_STM32_DCMI is not set +# CONFIG_STM32_ETHMAC is not set +# CONFIG_STM32_FSMC is not set +# CONFIG_STM32_HASH is not set +CONFIG_STM32_I2C1=y +# CONFIG_STM32_I2C2 is not set +# CONFIG_STM32_I2C3 is not set +CONFIG_STM32_OTGFS=y +# CONFIG_STM32_OTGHS is not set +CONFIG_STM32_PWR=y +# CONFIG_STM32_RNG is not set +CONFIG_STM32_SDIO=y +CONFIG_STM32_SPI1=y +CONFIG_STM32_SPI2=y +# CONFIG_STM32_SPI3 is not set +# CONFIG_STM32_SPI4 is not set +# CONFIG_STM32_SPI5 is not set +# CONFIG_STM32_SPI6 is not set +CONFIG_STM32_SYSCFG=y +CONFIG_STM32_TIM1=y +# CONFIG_STM32_TIM2 is not set +# CONFIG_STM32_TIM3 is not set +CONFIG_STM32_TIM4=y +# CONFIG_STM32_TIM5 is not set +# CONFIG_STM32_TIM6 is not set +# CONFIG_STM32_TIM7 is not set +CONFIG_STM32_TIM8=y +CONFIG_STM32_TIM9=y +CONFIG_STM32_TIM10=y +CONFIG_STM32_TIM11=y +# CONFIG_STM32_TIM12 is not set +# CONFIG_STM32_TIM13 is not set +# CONFIG_STM32_TIM14 is not set +CONFIG_STM32_USART1=y +CONFIG_STM32_USART2=y +CONFIG_STM32_USART3=y +CONFIG_STM32_UART4=y +# CONFIG_STM32_UART5 is not set +CONFIG_STM32_USART6=y +CONFIG_STM32_UART7=y +CONFIG_STM32_UART8=y +# CONFIG_STM32_IWDG is not set +CONFIG_STM32_WWDG=y +CONFIG_STM32_ADC=y +CONFIG_STM32_SPI=y +CONFIG_STM32_I2C=y + +# +# Alternate Pin Mapping +# +CONFIG_STM32_FLASH_PREFETCH=y +# CONFIG_STM32_JTAG_DISABLE is not set +# CONFIG_STM32_JTAG_FULL_ENABLE is not set +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set +CONFIG_STM32_JTAG_SW_ENABLE=y +CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y +# CONFIG_STM32_FORCEPOWER is not set +# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set +# CONFIG_STM32_CCMEXCLUDE is not set +CONFIG_STM32_DMACAPABLE=y +# CONFIG_STM32_TIM1_PWM is not set +# CONFIG_STM32_TIM3_PWM is not set +# CONFIG_STM32_TIM4_PWM is not set +# CONFIG_STM32_TIM9_PWM is not set +# CONFIG_STM32_TIM10_PWM is not set +# CONFIG_STM32_TIM11_PWM is not set +# CONFIG_STM32_TIM1_ADC is not set +# CONFIG_STM32_TIM3_ADC is not set +# CONFIG_STM32_TIM4_ADC is not set +CONFIG_STM32_USART=y + +# +# U[S]ART Configuration +# +# Hot fix for lost data +CONFIG_STM32_RXDMA_BUFFER_SIZE_OVERRIDE=256 +# CONFIG_USART1_RS485 is not set +CONFIG_USART1_RXDMA=y +# CONFIG_USART2_RS485 is not set +CONFIG_USART2_RXDMA=y +# CONFIG_USART3_RS485 is not set +CONFIG_USART3_RXDMA=y +# CONFIG_UART4_RS485 is not set +CONFIG_UART4_RXDMA=y +CONFIG_UART5_RXDMA=y +# CONFIG_USART6_RS485 is not set +CONFIG_USART6_RXDMA=y +# CONFIG_UART7_RS485 is not set +CONFIG_UART7_RXDMA=y +# CONFIG_UART8_RS485 is not set +CONFIG_UART8_RXDMA=y +CONFIG_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32_USART_SINGLEWIRE=y + +# +# SPI Configuration +# +# CONFIG_STM32_SPI_INTERRUPTS is not set +# CONFIG_STM32_SPI_DMA is not set + +# +# I2C Configuration +# +# CONFIG_STM32_I2C_DYNTIMEO is not set +CONFIG_STM32_I2CTIMEOSEC=0 +CONFIG_STM32_I2CTIMEOMS=10 +# CONFIG_STM32_I2C_DUTY16_9 is not set + +# +# SDIO Configuration +# +CONFIG_SDIO_PRI=128 + +# +# USB Host Configuration +# + +# +# USB Device Configuration +# + +# +# External Memory Configuration +# + +# +# Architecture Options +# +# CONFIG_ARCH_NOINTC is not set +# CONFIG_ARCH_VECNOTIRQ is not set +CONFIG_ARCH_DMA=y +# CONFIG_ARCH_IRQPRIO is not set +# CONFIG_CUSTOM_STACK is not set +# CONFIG_ADDRENV is not set +CONFIG_ARCH_HAVE_VFORK=y +CONFIG_ARCH_STACKDUMP=y +# CONFIG_ENDIAN_BIG is not set +# CONFIG_ARCH_HAVE_RAMFUNCS is not set +CONFIG_ARCH_HAVE_RAMVECTORS=y +# CONFIG_ARCH_RAMVECTORS is not set + +# +# Board Settings +# +CONFIG_BOARD_LOOPSPERMSEC=16717 +# CONFIG_ARCH_CALIBRATION is not set +CONFIG_DRAM_START=0x20000000 +CONFIG_DRAM_SIZE=262144 +CONFIG_ARCH_HAVE_INTERRUPTSTACK=y +# The actual usage is 420 bytes +CONFIG_ARCH_INTERRUPTSTACK=750 + +# +# Boot options +# +# CONFIG_BOOT_RUNFROMEXTSRAM is not set +CONFIG_BOOT_RUNFROMFLASH=y +# CONFIG_BOOT_RUNFROMISRAM is not set +# CONFIG_BOOT_RUNFROMSDRAM is not set +# CONFIG_BOOT_COPYTORAM is not set + +# +# Board Selection +# +CONFIG_ARCH_BOARD_PX4FMU_V4=y +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD="" + +# +# Common Board Options +# +CONFIG_NSH_MMCSDMINOR=0 +CONFIG_NSH_MMCSDSLOTNO=0 +CONFIG_MMCSD_HAVE_SDIOWAIT_WRCOMPLETE=y +# +# Board-Specific Options +# + +# +# RTOS Features +# +# CONFIG_BOARD_INITIALIZE is not set +CONFIG_MSEC_PER_TICK=1 +CONFIG_RR_INTERVAL=0 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_TASK_NAME_SIZE=24 +# CONFIG_SCHED_HAVE_PARENT is not set +# CONFIG_JULIAN_TIME is not set +CONFIG_START_YEAR=1970 +CONFIG_START_MONTH=1 +CONFIG_START_DAY=1 +CONFIG_DEV_CONSOLE=y +# CONFIG_MUTEX_TYPES is not set +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_NNESTPRIO=8 +# CONFIG_FDCLONE_DISABLE is not set +CONFIG_FDCLONE_STDIO=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SCHED_WAITPID=y +# CONFIG_SCHED_STARTHOOK is not set +CONFIG_SCHED_ATEXIT=y +CONFIG_SCHED_ATEXIT_MAX=1 +# CONFIG_SCHED_ONEXIT is not set +CONFIG_USER_ENTRYPOINT="nsh_main" +# CONFIG_DISABLE_OS_API is not set + +# +# Signal Numbers +# +CONFIG_SIG_SIGUSR1=1 +CONFIG_SIG_SIGUSR2=2 +CONFIG_SIG_SIGALARM=3 +CONFIG_SIG_SIGCONDTIMEDOUT=16 +CONFIG_SIG_SIGWORK=4 + +# +# Sizes of configurable things (0 disables) +# +CONFIG_MAX_TASKS=32 +CONFIG_MAX_TASK_ARGS=10 +CONFIG_NPTHREAD_KEYS=4 +CONFIG_NFILE_DESCRIPTORS=42 +CONFIG_NFILE_STREAMS=8 +CONFIG_NAME_MAX=32 +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_MQ_MAXMSGSIZE=32 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=50 +CONFIG_PREALLOC_TIMERS=50 + +# +# Stack and heap information +# +CONFIG_IDLETHREAD_STACKSIZE=1000 +CONFIG_USERMAIN_STACKSIZE=2500 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_PTHREAD_STACK_DEFAULT=2048 + +# +# Device Drivers +# +# CONFIG_DISABLE_POLL is not set +CONFIG_DEV_NULL=y +# CONFIG_DEV_ZERO is not set +# CONFIG_LOOP is not set +# CONFIG_RAMDISK is not set +# CONFIG_CAN is not set +# CONFIG_PWM is not set +CONFIG_I2C=y +# CONFIG_I2C_SLAVE is not set +CONFIG_I2C_TRANSFER=y +# CONFIG_I2C_WRITEREAD is not set +# CONFIG_I2C_POLLED is not set +# CONFIG_I2C_TRACE is not set +CONFIG_ARCH_HAVE_I2CRESET=y +CONFIG_I2C_RESET=y +CONFIG_SPI=y +# CONFIG_SPI_OWNBUS is not set +CONFIG_SPI_EXCHANGE=y +# CONFIG_SPI_CMDDATA is not set +CONFIG_RTC=y +CONFIG_RTC_DATETIME=y +CONFIG_RTC_HSECLOCK=y +CONFIG_WATCHDOG=y +# CONFIG_ANALOG is not set +# CONFIG_AUDIO_DEVICES is not set +# CONFIG_BCH is not set +# CONFIG_INPUT is not set +# CONFIG_LCD is not set +CONFIG_MMCSD=y +CONFIG_MMCSD_NSLOTS=1 +# CONFIG_MMCSD_READONLY is not set +CONFIG_MMCSD_MULTIBLOCK_DISABLE=y +# CONFIG_MMCSD_MMCSUPPORT is not set +# CONFIG_MMCSD_HAVECARDDETECT is not set +# CONFIG_MMCSD_SPI is not set +CONFIG_MMCSD_SDIO=y +# CONFIG_SDIO_MUXBUS is not set +# CONFIG_SDIO_BLOCKSETUP is not set +CONFIG_MTD=y + +# +# MTD Configuration +# +CONFIG_MTD_PARTITION=y +CONFIG_MTD_BYTE_WRITE=y + +# +# MTD Device Drivers +# +# CONFIG_RAMMTD is not set +# CONFIG_MTD_AT24XX is not set +# CONFIG_MTD_AT45DB is not set +# CONFIG_MTD_M25P is not set +# CONFIG_MTD_SMART is not set +CONFIG_MTD_RAMTRON=y +# CONFIG_MTD_SST25 is not set +# CONFIG_MTD_SST39FV is not set +# CONFIG_MTD_W25 is not set +CONFIG_PIPES=y +# CONFIG_PM is not set +# CONFIG_POWER is not set +# CONFIG_SENSORS is not set +CONFIG_SERIAL=y +# CONFIG_DEV_LOWCONSOLE is not set +CONFIG_SERIAL_REMOVABLE=y +# CONFIG_16550_UART is not set +CONFIG_ARCH_HAVE_UART4=y +CONFIG_ARCH_HAVE_UART7=y +CONFIG_ARCH_HAVE_UART8=y +CONFIG_ARCH_HAVE_USART1=y +CONFIG_ARCH_HAVE_USART2=y +CONFIG_ARCH_HAVE_USART3=y +CONFIG_ARCH_HAVE_USART6=y +CONFIG_MCU_SERIAL=y +CONFIG_STANDARD_SERIAL=y +CONFIG_SERIAL_NPOLLWAITERS=2 +# CONFIG_SERIAL_TIOCSERGSTRUCT is not set +# CONFIG_USART1_SERIAL_CONSOLE is not set +# CONFIG_USART2_SERIAL_CONSOLE is not set +# CONFIG_USART3_SERIAL_CONSOLE is not set +# CONFIG_UART4_SERIAL_CONSOLE is not set +# CONFIG_USART6_SERIAL_CONSOLE is not set +CONFIG_UART7_SERIAL_CONSOLE=y +# CONFIG_UART8_SERIAL_CONSOLE is not set +# CONFIG_NO_SERIAL_CONSOLE is not set + +# +# USART1 Configuration +# +CONFIG_USART1_RXBUFSIZE=600 +CONFIG_USART1_TXBUFSIZE=1100 +CONFIG_USART1_BAUD=115200 +CONFIG_USART1_BITS=8 +CONFIG_USART1_PARITY=0 +CONFIG_USART1_2STOP=0 +# CONFIG_USART1_IFLOWCONTROL is not set +# CONFIG_USART1_OFLOWCONTROL is not set + +# +# USART2 Configuration +# +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_TXBUFSIZE=1100 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_BITS=8 +CONFIG_USART2_PARITY=0 +CONFIG_USART2_2STOP=0 +CONFIG_USART2_IFLOWCONTROL=y +CONFIG_USART2_OFLOWCONTROL=y + +# +# USART3 Configuration +# +CONFIG_USART3_RXBUFSIZE=300 +CONFIG_USART3_TXBUFSIZE=300 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_BITS=8 +CONFIG_USART3_PARITY=0 +CONFIG_USART3_2STOP=0 +CONFIG_USART3_IFLOWCONTROL=y +CONFIG_USART3_OFLOWCONTROL=y + +# +# UART4 Configuration +# +CONFIG_UART4_RXBUFSIZE=300 +CONFIG_UART4_TXBUFSIZE=300 +CONFIG_UART4_BAUD=57600 +CONFIG_UART4_BITS=8 +CONFIG_UART4_PARITY=0 +CONFIG_UART4_2STOP=0 +# CONFIG_UART4_IFLOWCONTROL is not set +# CONFIG_UART4_OFLOWCONTROL is not set + +# +# USART6 Configuration +# +CONFIG_USART6_RXBUFSIZE=300 +CONFIG_USART6_TXBUFSIZE=300 +CONFIG_USART6_BAUD=57600 +CONFIG_USART6_BITS=8 +CONFIG_USART6_PARITY=0 +CONFIG_USART6_2STOP=0 +# CONFIG_USART6_IFLOWCONTROL is not set +# CONFIG_USART6_OFLOWCONTROL is not set + +# +# UART7 Configuration +# +CONFIG_UART7_RXBUFSIZE=300 +CONFIG_UART7_TXBUFSIZE=300 +CONFIG_UART7_BAUD=57600 +CONFIG_UART7_BITS=8 +CONFIG_UART7_PARITY=0 +CONFIG_UART7_2STOP=0 +# CONFIG_UART7_IFLOWCONTROL is not set +# CONFIG_UART7_OFLOWCONTROL is not set + +# +# UART8 Configuration +# +CONFIG_UART8_RXBUFSIZE=300 +CONFIG_UART8_TXBUFSIZE=300 +CONFIG_UART8_BAUD=57600 +CONFIG_UART8_BITS=8 +CONFIG_UART8_PARITY=0 +CONFIG_UART8_2STOP=0 +# CONFIG_UART8_IFLOWCONTROL is not set +# CONFIG_UART8_OFLOWCONTROL is not set +CONFIG_SERIAL_IFLOWCONTROL=y +CONFIG_SERIAL_OFLOWCONTROL=y +CONFIG_USBDEV=y + +# +# USB Device Controller Driver Options +# +# CONFIG_USBDEV_ISOCHRONOUS is not set +# CONFIG_USBDEV_DUALSPEED is not set +# CONFIG_USBDEV_SELFPOWERED is not set +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +# CONFIG_USBDEV_DMA is not set +# CONFIG_USBDEV_TRACE is not set + +# +# USB Device Class Driver Options +# +# CONFIG_USBDEV_COMPOSITE is not set +# CONFIG_PL2303 is not set +CONFIG_CDCACM=y +# CONFIG_CDCACM_CONSOLE is not set +CONFIG_CDCACM_EP0MAXPACKET=64 +CONFIG_CDCACM_EPINTIN=1 +CONFIG_CDCACM_EPINTIN_FSSIZE=64 +CONFIG_CDCACM_EPINTIN_HSSIZE=64 +CONFIG_CDCACM_EPBULKOUT=3 +CONFIG_CDCACM_EPBULKOUT_FSSIZE=64 +CONFIG_CDCACM_EPBULKOUT_HSSIZE=512 +CONFIG_CDCACM_EPBULKIN=2 +CONFIG_CDCACM_EPBULKIN_FSSIZE=64 +CONFIG_CDCACM_EPBULKIN_HSSIZE=512 +CONFIG_CDCACM_NWRREQS=4 +CONFIG_CDCACM_NRDREQS=4 +CONFIG_CDCACM_BULKIN_REQLEN=96 +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=4000 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_PRODUCTID=0x0011 +CONFIG_CDCACM_VENDORSTR="3D Robotics" +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v4.x" +# CONFIG_USBMSC is not set +# CONFIG_USBHOST is not set +# CONFIG_WIRELESS is not set + +# +# System Logging Device Options +# + +# +# System Logging +# +# CONFIG_RAMLOG is not set + +# +# Networking Support +# +# CONFIG_NET is not set + +# +# File Systems +# + +# +# File system configuration +# +# CONFIG_DISABLE_MOUNTPOINT is not set +# CONFIG_FS_RAMMAP is not set +CONFIG_FS_FAT=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_MAXFNAME=32 +CONFIG_FS_FATTIME=y +CONFIG_FAT_DMAMEMORY=y +CONFIG_FS_NXFFS=y +CONFIG_NXFFS_PREALLOCATED=y +CONFIG_NXFFS_ERASEDSTATE=0xff +CONFIG_NXFFS_PACKTHRESHOLD=32 +CONFIG_NXFFS_MAXNAMLEN=32 +CONFIG_NXFFS_TAILTHRESHOLD=2048 +CONFIG_FS_ROMFS=y +# CONFIG_FS_SMARTFS is not set +CONFIG_FS_BINFS=y + +# +# System Logging +# +# CONFIG_SYSLOG_ENABLE is not set +# CONFIG_SYSLOG is not set + +# +# Graphics Support +# +# CONFIG_NX is not set + +# +# Memory Management +# +# CONFIG_MM_MULTIHEAP is not set +# CONFIG_MM_SMALL is not set +CONFIG_MM_REGIONS=2 +CONFIG_GRAN=y +# CONFIG_GRAN_SINGLE is not set +# CONFIG_GRAN_INTR is not set +# CONFIG_DEBUG_GRAN is not set + +# +# Audio Support +# +# CONFIG_AUDIO is not set + +# +# Binary Formats +# +# CONFIG_BINFMT_DISABLE is not set +# CONFIG_BINFMT_EXEPATH is not set +# CONFIG_NXFLAT is not set +# CONFIG_ELF is not set +CONFIG_BUILTIN=y +# CONFIG_PIC is not set +# CONFIG_SYMTAB_ORDEREDBYNAME is not set + +# +# Library Routines +# + +# +# Standard C Library Options +# +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STDIO_LINEBUFFER=y +CONFIG_NUNGET_CHARS=2 +CONFIG_LIB_HOMEDIR="/" +# CONFIG_NOPRINTF_FIELDWIDTH is not set +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIB_RAND_ORDER=1 +# CONFIG_EOL_IS_CR is not set +# CONFIG_EOL_IS_LF is not set +# CONFIG_EOL_IS_BOTH_CRLF is not set +CONFIG_EOL_IS_EITHER_CRLF=y +# CONFIG_LIBC_EXECFUNCS is not set +CONFIG_POSIX_SPAWN_PROXY_STACKSIZE=1024 +CONFIG_TASK_SPAWN_DEFAULT_STACKSIZE=2048 +CONFIG_LIBC_STRERROR=y +# CONFIG_LIBC_STRERROR_SHORT is not set +# CONFIG_LIBC_PERROR_STDOUT is not set +CONFIG_ARCH_LOWPUTC=y +CONFIG_LIB_SENDFILE_BUFSIZE=512 +# CONFIG_ARCH_ROMGETC is not set +CONFIG_ARCH_OPTIMIZED_FUNCTIONS=y +CONFIG_ARCH_MEMCPY=y +# CONFIG_ARCH_MEMCMP is not set +# CONFIG_ARCH_MEMMOVE is not set +# CONFIG_ARCH_MEMSET is not set +# CONFIG_MEMSET_OPTSPEED is not set +# CONFIG_ARCH_STRCHR is not set +# CONFIG_ARCH_STRCMP is not set +# CONFIG_ARCH_STRCPY is not set +# CONFIG_ARCH_STRNCPY is not set +# CONFIG_ARCH_STRLEN is not set +# CONFIG_ARCH_STRNLEN is not set +# CONFIG_ARCH_BZERO is not set + +# +# Non-standard Library Support +# +CONFIG_SCHED_WORKQUEUE=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_WORKPRIORITY=192 +CONFIG_SCHED_WORKPERIOD=5000 +CONFIG_SCHED_WORKSTACKSIZE=1600 +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKPERIOD=50000 +CONFIG_SCHED_LPWORKSTACKSIZE=1600 +# CONFIG_LIB_KBDCODEC is not set +# CONFIG_LIB_SLCDCODEC is not set + +# +# Basic CXX Support +# +CONFIG_C99_BOOL8=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +# CONFIG_CXX_NEWLONG is not set + +# +# uClibc++ Standard C++ Library +# +# CONFIG_UCLIBCXX is not set + +# +# Application Configuration +# + +# +# Built-In Applications +# +CONFIG_BUILTIN_PROXY_STACKSIZE=1024 + +# +# Examples +# +# CONFIG_EXAMPLES_BUTTONS is not set +# CONFIG_EXAMPLES_CAN is not set +CONFIG_EXAMPLES_CDCACM=y +# CONFIG_EXAMPLES_COMPOSITE is not set +# CONFIG_EXAMPLES_CXXTEST is not set +# CONFIG_EXAMPLES_DHCPD is not set +# CONFIG_EXAMPLES_ELF is not set +# CONFIG_EXAMPLES_FTPC is not set +# CONFIG_EXAMPLES_FTPD is not set +# CONFIG_EXAMPLES_HELLO is not set +# CONFIG_EXAMPLES_HELLOXX is not set +# CONFIG_EXAMPLES_JSON is not set +# CONFIG_EXAMPLES_HIDKBD is not set +# CONFIG_EXAMPLES_KEYPADTEST is not set +# CONFIG_EXAMPLES_IGMP is not set +# CONFIG_EXAMPLES_LCDRW is not set +# CONFIG_EXAMPLES_MM is not set +# CONFIG_EXAMPLES_MODBUS is not set +CONFIG_EXAMPLES_MOUNT=y +# CONFIG_EXAMPLES_NRF24L01TERM is not set +CONFIG_EXAMPLES_NSH=y +# CONFIG_EXAMPLES_NULL is not set +# CONFIG_EXAMPLES_NX is not set +# CONFIG_EXAMPLES_NXCONSOLE is not set +# CONFIG_EXAMPLES_NXFFS is not set +# CONFIG_EXAMPLES_NXFLAT is not set +# CONFIG_EXAMPLES_NXHELLO is not set +# CONFIG_EXAMPLES_NXIMAGE is not set +# CONFIG_EXAMPLES_NXLINES is not set +# CONFIG_EXAMPLES_NXTEXT is not set +# CONFIG_EXAMPLES_OSTEST is not set +# CONFIG_EXAMPLES_PASHELLO is not set +# CONFIG_EXAMPLES_PIPE is not set +# CONFIG_EXAMPLES_POSIXSPAWN is not set +# CONFIG_EXAMPLES_QENCODER is not set +# CONFIG_EXAMPLES_RGMP is not set +# CONFIG_EXAMPLES_ROMFS is not set +# CONFIG_EXAMPLES_SENDMAIL is not set +# CONFIG_EXAMPLES_SERLOOP is not set +# CONFIG_EXAMPLES_SLCD is not set +# CONFIG_EXAMPLES_SMART_TEST is not set +# CONFIG_EXAMPLES_SMART is not set +# CONFIG_EXAMPLES_TCPECHO is not set +# CONFIG_EXAMPLES_TELNETD is not set +# CONFIG_EXAMPLES_THTTPD is not set +# CONFIG_EXAMPLES_TIFF is not set +# CONFIG_EXAMPLES_TOUCHSCREEN is not set +# CONFIG_EXAMPLES_UDP is not set +# CONFIG_EXAMPLES_UIP is not set +# CONFIG_EXAMPLES_USBSERIAL is not set +# CONFIG_EXAMPLES_USBMSC is not set +# CONFIG_EXAMPLES_USBTERM is not set +# CONFIG_EXAMPLES_WATCHDOG is not set + +# +# Graphics Support +# +# CONFIG_TIFF is not set + +# +# Interpreters +# +# CONFIG_INTERPRETERS_FICL is not set +# CONFIG_INTERPRETERS_PCODE is not set + +# +# Network Utilities +# + +# +# Networking Utilities +# +# CONFIG_NETUTILS_CODECS is not set +# CONFIG_NETUTILS_DHCPC is not set +# CONFIG_NETUTILS_DHCPD is not set +# CONFIG_NETUTILS_FTPC is not set +# CONFIG_NETUTILS_FTPD is not set +# CONFIG_NETUTILS_JSON is not set +# CONFIG_NETUTILS_RESOLV is not set +# CONFIG_NETUTILS_SMTP is not set +# CONFIG_NETUTILS_TELNETD is not set +# CONFIG_NETUTILS_TFTPC is not set +# CONFIG_NETUTILS_THTTPD is not set +# CONFIG_NETUTILS_UIPLIB is not set +# CONFIG_NETUTILS_WEBCLIENT is not set + +# +# FreeModBus +# +# CONFIG_MODBUS is not set + +# +# NSH Library +# +CONFIG_NSH_LIBRARY=y +CONFIG_NSH_BUILTIN_APPS=y + +# +# Disable Individual commands +# +# CONFIG_NSH_DISABLE_CAT is not set +# CONFIG_NSH_DISABLE_CD is not set +# CONFIG_NSH_DISABLE_CP is not set +# CONFIG_NSH_DISABLE_DD is not set +# CONFIG_NSH_DISABLE_ECHO is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_FREE is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_HELP is not set +# CONFIG_NSH_DISABLE_HEXDUMP is not set +# CONFIG_NSH_DISABLE_IFCONFIG is not set +# CONFIG_NSH_DISABLE_KILL is not set +# CONFIG_NSH_DISABLE_LOSETUP is not set +# CONFIG_NSH_DISABLE_LS is not set +# CONFIG_NSH_DISABLE_MB is not set +# CONFIG_NSH_DISABLE_MKDIR is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_MKFIFO is not set +# CONFIG_NSH_DISABLE_MKRD is not set +# CONFIG_NSH_DISABLE_MH is not set +# CONFIG_NSH_DISABLE_MOUNT is not set +# CONFIG_NSH_DISABLE_MW is not set +# CONFIG_NSH_DISABLE_NSFMOUNT is not set +# CONFIG_NSH_DISABLE_PS is not set +# CONFIG_NSH_DISABLE_PING is not set +# CONFIG_NSH_DISABLE_PUT is not set +# CONFIG_NSH_DISABLE_PWD is not set +# CONFIG_NSH_DISABLE_RM is not set +# CONFIG_NSH_DISABLE_RMDIR is not set +# CONFIG_NSH_DISABLE_SET is not set +# CONFIG_NSH_DISABLE_SH is not set +# CONFIG_NSH_DISABLE_SLEEP is not set +# CONFIG_NSH_DISABLE_TEST is not set +# CONFIG_NSH_DISABLE_UMOUNT is not set +# CONFIG_NSH_DISABLE_UNSET is not set +# CONFIG_NSH_DISABLE_USLEEP is not set +# CONFIG_NSH_DISABLE_WGET is not set +# CONFIG_NSH_DISABLE_XD is not set + +# +# Configure Command Options +# +# CONFIG_NSH_CMDOPT_DF_H is not set +CONFIG_NSH_CODECS_BUFSIZE=128 +CONFIG_NSH_FILEIOSIZE=512 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=12 +CONFIG_NSH_NESTDEPTH=8 +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLEBG is not set +CONFIG_NSH_ROMFSETC=y +# CONFIG_NSH_ROMFSRC is not set +CONFIG_NSH_ROMFSMOUNTPT="/etc" +CONFIG_NSH_INITSCRIPT="init.d/rcS" +CONFIG_NSH_ROMFSDEVNO=0 +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_ARCHROMFS=y +CONFIG_NSH_FATDEVNO=1 +CONFIG_NSH_FATSECTSIZE=512 +CONFIG_NSH_FATNSECTORS=1024 +CONFIG_NSH_FATMOUNTPT="/tmp" +CONFIG_NSH_CONSOLE=y +# CONFIG_NSH_USBCONSOLE is not set + +# +# USB Trace Support +# +# CONFIG_NSH_USBDEV_TRACE is not set +# CONFIG_NSH_CONDEV is not set +CONFIG_NSH_ARCHINIT=y + +# +# NxWidgets/NxWM +# + +# +# System NSH Add-Ons +# + +# +# Custom Free Memory Command +# +# CONFIG_SYSTEM_FREE is not set + +# +# I2C tool +# +# CONFIG_SYSTEM_I2CTOOL is not set + +# +# FLASH Program Installation +# +# CONFIG_SYSTEM_INSTALL is not set + +# +# FLASH Erase-all Command +# +# CONFIG_SYSTEM_FLASH_ERASEALL is not set + +# +# readline() +# +CONFIG_SYSTEM_READLINE=y +CONFIG_READLINE_ECHO=y + +# +# Power Off +# +# CONFIG_SYSTEM_POWEROFF is not set + +# +# RAMTRON +# +# CONFIG_SYSTEM_RAMTRON is not set + +# +# SD Card +# +# CONFIG_SYSTEM_SDCARD is not set + +# +# Sysinfo +# +CONFIG_SYSTEM_SYSINFO=y + +# +# USB Monitor +# + +CONFIG_NSOCKET_DESCRIPTORS=0 diff --git a/nuttx-configs/px4fmu-v4/nsh/setenv.sh b/nuttx-configs/px4fmu-v4/nsh/setenv.sh new file mode 100755 index 0000000000..265520997d --- /dev/null +++ b/nuttx-configs/px4fmu-v4/nsh/setenv.sh @@ -0,0 +1,67 @@ +#!/bin/bash +# configs/stm3240g-eval/nsh/setenv.sh +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name NuttX nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# + +if [ "$_" = "$0" ] ; then + echo "You must source this script, not run it!" 1>&2 + exit 1 +fi + +WD=`pwd` +if [ ! -x "setenv.sh" ]; then + echo "This script must be executed from the top-level NuttX build directory" + exit 1 +fi + +if [ -z "${PATH_ORIG}" ]; then + export PATH_ORIG="${PATH}" +fi + +# This the Cygwin path to the location where I installed the RIDE +# toolchain under windows. You will also have to edit this if you install +# the RIDE toolchain in any other location +#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/Raisonance/Ride/arm-gcc/bin" + +# This the Cygwin path to the location where I installed the CodeSourcery +# toolchain under windows. You will also have to edit this if you install +# the CodeSourcery toolchain in any other location +export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery G++ Lite/bin" + +# This the Cygwin path to the location where I build the buildroot +# toolchain. +#export TOOLCHAIN_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" + +# Add the path to the toolchain to the PATH varialble +export PATH="${TOOLCHAIN_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" + +echo "PATH : ${PATH}" diff --git a/nuttx-configs/px4fmu-v4/scripts/ld.script b/nuttx-configs/px4fmu-v4/scripts/ld.script new file mode 100644 index 0000000000..f0f9e0f244 --- /dev/null +++ b/nuttx-configs/px4fmu-v4/scripts/ld.script @@ -0,0 +1,158 @@ +/**************************************************************************** + * configs/px4fmu/common/ld.script + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* The STM32F427 has 2048Kb of FLASH beginning at address 0x0800:0000 and + * 256Kb of SRAM. SRAM is split up into three blocks: + * + * 1) 112Kb of SRAM beginning at address 0x2000:0000 + * 2) 16Kb of SRAM beginning at address 0x2001:c000 + * 3) 64Kb of SRAM beginning at address 0x2002:0000 + * 4) 64Kb of TCM SRAM beginning at address 0x1000:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + * + * The first 0x4000 of flash is reserved for the bootloader. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08004000, LENGTH = 2032K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K + ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K +} + +OUTPUT_ARCH(arm) + +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + /* + * This is a hack to make the newlib libm __errno() call + * use the NuttX get_errno_ptr() function. + */ + __errno = get_errno_ptr; + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + /* + * Construction data for parameters. + */ + __param ALIGN(4): { + __param_start = ABSOLUTE(.); + KEEP(*(__param*)) + __param_end = ABSOLUTE(.); + } > flash + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/nuttx-configs/px4fmu-v4/src/Makefile b/nuttx-configs/px4fmu-v4/src/Makefile new file mode 100644 index 0000000000..a93664b08b --- /dev/null +++ b/nuttx-configs/px4fmu-v4/src/Makefile @@ -0,0 +1,84 @@ +############################################################################ +# configs/px4fmu/src/Makefile +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name NuttX nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +-include $(TOPDIR)/Make.defs + +CFLAGS += -I$(TOPDIR)/sched + +ASRCS = +AOBJS = $(ASRCS:.S=$(OBJEXT)) + +CSRCS = empty.c +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ARCH_SRCDIR = $(TOPDIR)/arch/$(CONFIG_ARCH)/src +ifeq ($(WINTOOL),y) + CFLAGS += -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}" +else + CFLAGS += -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m +endif + +all: libboard$(LIBEXT) + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +libboard$(LIBEXT): $(OBJS) + $(call ARCHIVE, $@, $(OBJS)) + +.depend: Makefile $(SRCS) + $(Q) $(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + $(Q) touch $@ + +depend: .depend + +clean: + $(call DELFILE, libboard$(LIBEXT)) + $(call CLEAN) + +distclean: clean + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) + +-include Make.dep + diff --git a/nuttx-configs/px4fmu-v4/src/empty.c b/nuttx-configs/px4fmu-v4/src/empty.c new file mode 100644 index 0000000000..5de10699fb --- /dev/null +++ b/nuttx-configs/px4fmu-v4/src/empty.c @@ -0,0 +1,4 @@ +/* + * There are no source files here, but libboard.a can't be empty, so + * we have this empty source file to keep it company. + */ diff --git a/nuttx-configs/px4io-v2/nsh/defconfig b/nuttx-configs/px4io-v2/nsh/defconfig index 56892e3f93..a9b8477948 100755 --- a/nuttx-configs/px4io-v2/nsh/defconfig +++ b/nuttx-configs/px4io-v2/nsh/defconfig @@ -1,705 +1,198 @@ +############################################################################ +# configs/px4io/nsh/defconfig # -# Automatically generated file; DO NOT EDIT. -# Nuttx/ Configuration +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt # -CONFIG_NUTTX_NEWCONFIG=y - +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: # -# Build Setup +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name NuttX nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. # -# CONFIG_EXPERIMENTAL is not set -CONFIG_HOST_LINUX=y -# CONFIG_HOST_OSX is not set -# CONFIG_HOST_WINDOWS is not set -# CONFIG_HOST_OTHER is not set - +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. # -# Build Configuration +############################################################################ # -CONFIG_APPS_DIR="../apps" -# CONFIG_BUILD_2PASS is not set - +# architecture selection # -# Binary Output Formats +# CONFIG_ARCH - identifies the arch subdirectory and, hence, the +# processor architecture. +# CONFIG_ARCH_family - for use in C code. This identifies the +# particular chip family that the architecture is implemented +# in. +# CONFIG_ARCH_architecture - for use in C code. This identifies the +# specific architecture within the chip family. +# CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory +# CONFIG_ARCH_CHIP_name - For use in C code +# CONFIG_ARCH_BOARD - identifies the configs subdirectory and, hence, +# the board that supports the particular chip or SoC. +# CONFIG_ARCH_BOARD_name - for use in C code +# CONFIG_ENDIAN_BIG - define if big endian (default is little endian) +# CONFIG_BOARD_LOOPSPERMSEC - for delay loops +# CONFIG_DRAM_SIZE - Describes the installed DRAM. +# CONFIG_DRAM_START - The start address of DRAM (physical) +# CONFIG_ARCH_IRQPRIO - The ST32F100CB supports interrupt prioritization +# CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt +# stack. If defined, this symbol is the size of the interrupt +# stack in bytes. If not defined, the user task stacks will be +# used during interrupt handling. +# CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions +# CONFIG_ARCH_BOOTLOADER - Set if you are using a bootloader. +# CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture. +# CONFIG_ARCH_BUTTONS - Enable support for buttons. Unique to board architecture. +# CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that +# cause a 100 second delay during boot-up. This 100 second delay +# serves no purpose other than it allows you to calibrate +# CONFIG_BOARD_LOOPSPERMSEC. You simply use a stop watch to measure +# the 100 second delay then adjust CONFIG_BOARD_LOOPSPERMSEC until +# the delay actually is 100 seconds. +# CONFIG_ARCH_DMA - Support DMA initialization # -# CONFIG_RRLOAD_BINARY is not set -# CONFIG_INTELHEX_BINARY is not set -# CONFIG_MOTOROLA_SREC is not set -CONFIG_RAW_BINARY=y - -# -# Customize Header Files -# -# CONFIG_ARCH_STDBOOL_H is not set -CONFIG_ARCH_MATH_H=y -# CONFIG_ARCH_FLOAT_H is not set -# CONFIG_ARCH_STDARG_H is not set - -# -# Debug Options -# -# CONFIG_DEBUG is not set -CONFIG_DEBUG_SYMBOLS=y -# -# System Type -# -# CONFIG_ARCH_8051 is not set -CONFIG_ARCH_ARM=y -# CONFIG_ARCH_AVR is not set -# CONFIG_ARCH_HC is not set -# CONFIG_ARCH_MIPS is not set -# CONFIG_ARCH_RGMP is not set -# CONFIG_ARCH_SH is not set -# CONFIG_ARCH_SIM is not set -# CONFIG_ARCH_X86 is not set -# CONFIG_ARCH_Z16 is not set -# CONFIG_ARCH_Z80 is not set CONFIG_ARCH="arm" - -# -# ARM Options -# -# CONFIG_ARCH_CHIP_C5471 is not set -# CONFIG_ARCH_CHIP_CALYPSO is not set -# CONFIG_ARCH_CHIP_DM320 is not set -# CONFIG_ARCH_CHIP_IMX is not set -# CONFIG_ARCH_CHIP_KINETIS is not set -# CONFIG_ARCH_CHIP_KL is not set -# CONFIG_ARCH_CHIP_LM is not set -# CONFIG_ARCH_CHIP_LPC17XX is not set -# CONFIG_ARCH_CHIP_LPC214X is not set -# CONFIG_ARCH_CHIP_LPC2378 is not set -# CONFIG_ARCH_CHIP_LPC31XX is not set -# CONFIG_ARCH_CHIP_LPC43XX is not set -# CONFIG_ARCH_CHIP_NUC1XX is not set -# CONFIG_ARCH_CHIP_SAM34 is not set -CONFIG_ARCH_CHIP_STM32=y -# CONFIG_ARCH_CHIP_STR71X is not set +CONFIG_ARCH_ARM=y CONFIG_ARCH_CORTEXM3=y -CONFIG_ARCH_FAMILY="armv7-m" CONFIG_ARCH_CHIP="stm32" -# CONFIG_ARMV7M_USEBASEPRI is not set -CONFIG_ARCH_HAVE_CMNVECTOR=y -CONFIG_ARMV7M_CMNVECTOR=y -# CONFIG_ARCH_HAVE_FPU is not set -CONFIG_ARCH_HAVE_MPU=y -# CONFIG_ARMV7M_MPU is not set - -# -# ARMV7M Configuration Options -# -# CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set -# CONFIG_ARMV7M_TOOLCHAIN_CODEREDL is not set -# CONFIG_ARMV7M_TOOLCHAIN_CODESOURCERYL is not set -CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y -# CONFIG_ARMV7M_STACKCHECK is not set -CONFIG_SERIAL_TERMIOS=y - -# -# STM32 Configuration Options -# -# CONFIG_ARCH_CHIP_STM32L151C6 is not set -# CONFIG_ARCH_CHIP_STM32L151C8 is not set -# CONFIG_ARCH_CHIP_STM32L151CB is not set -# CONFIG_ARCH_CHIP_STM32L151R6 is not set -# CONFIG_ARCH_CHIP_STM32L151R8 is not set -# CONFIG_ARCH_CHIP_STM32L151RB is not set -# CONFIG_ARCH_CHIP_STM32L151V6 is not set -# CONFIG_ARCH_CHIP_STM32L151V8 is not set -# CONFIG_ARCH_CHIP_STM32L151VB is not set -# CONFIG_ARCH_CHIP_STM32L152C6 is not set -# CONFIG_ARCH_CHIP_STM32L152C8 is not set -# CONFIG_ARCH_CHIP_STM32L152CB is not set -# CONFIG_ARCH_CHIP_STM32L152R6 is not set -# CONFIG_ARCH_CHIP_STM32L152R8 is not set -# CONFIG_ARCH_CHIP_STM32L152RB is not set -# CONFIG_ARCH_CHIP_STM32L152V6 is not set -# CONFIG_ARCH_CHIP_STM32L152V8 is not set -# CONFIG_ARCH_CHIP_STM32L152VB is not set CONFIG_ARCH_CHIP_STM32F100C8=y -# CONFIG_ARCH_CHIP_STM32F100CB is not set -# CONFIG_ARCH_CHIP_STM32F100R8 is not set -# CONFIG_ARCH_CHIP_STM32F100RB is not set -# CONFIG_ARCH_CHIP_STM32F100RC is not set -# CONFIG_ARCH_CHIP_STM32F100RD is not set -# CONFIG_ARCH_CHIP_STM32F100RE is not set -# CONFIG_ARCH_CHIP_STM32F100V8 is not set -# CONFIG_ARCH_CHIP_STM32F100VB is not set -# CONFIG_ARCH_CHIP_STM32F100VC is not set -# CONFIG_ARCH_CHIP_STM32F100VD is not set -# CONFIG_ARCH_CHIP_STM32F100VE is not set -# CONFIG_ARCH_CHIP_STM32F102CB is not set -# CONFIG_ARCH_CHIP_STM32F103C4 is not set -# CONFIG_ARCH_CHIP_STM32F103C8 is not set -# CONFIG_ARCH_CHIP_STM32F103RET6 is not set -# CONFIG_ARCH_CHIP_STM32F103VCT6 is not set -# CONFIG_ARCH_CHIP_STM32F103VET6 is not set -# CONFIG_ARCH_CHIP_STM32F103ZET6 is not set -# CONFIG_ARCH_CHIP_STM32F105VBT7 is not set -# CONFIG_ARCH_CHIP_STM32F107VC is not set -# CONFIG_ARCH_CHIP_STM32F207IG is not set -# CONFIG_ARCH_CHIP_STM32F302CB is not set -# CONFIG_ARCH_CHIP_STM32F302CC is not set -# CONFIG_ARCH_CHIP_STM32F302RB is not set -# CONFIG_ARCH_CHIP_STM32F302RC is not set -# CONFIG_ARCH_CHIP_STM32F302VB is not set -# CONFIG_ARCH_CHIP_STM32F302VC is not set -# CONFIG_ARCH_CHIP_STM32F303CB is not set -# CONFIG_ARCH_CHIP_STM32F303CC is not set -# CONFIG_ARCH_CHIP_STM32F303RB is not set -# CONFIG_ARCH_CHIP_STM32F303RC is not set -# CONFIG_ARCH_CHIP_STM32F303VB is not set -# CONFIG_ARCH_CHIP_STM32F303VC is not set -# CONFIG_ARCH_CHIP_STM32F405RG is not set -# CONFIG_ARCH_CHIP_STM32F405VG is not set -# CONFIG_ARCH_CHIP_STM32F405ZG is not set -# CONFIG_ARCH_CHIP_STM32F407VE is not set -# CONFIG_ARCH_CHIP_STM32F407VG is not set -# CONFIG_ARCH_CHIP_STM32F407ZE is not set -# CONFIG_ARCH_CHIP_STM32F407ZG is not set -# CONFIG_ARCH_CHIP_STM32F407IE is not set -# CONFIG_ARCH_CHIP_STM32F407IG is not set -# CONFIG_ARCH_CHIP_STM32F427V is not set -# CONFIG_ARCH_CHIP_STM32F427Z is not set -# CONFIG_ARCH_CHIP_STM32F427I is not set -# CONFIG_STM32_STM32L15XX is not set -# CONFIG_STM32_ENERGYLITE is not set -CONFIG_STM32_STM32F10XX=y -CONFIG_STM32_VALUELINE=y -# CONFIG_STM32_CONNECTIVITYLINE is not set -# CONFIG_STM32_PERFORMANCELINE is not set -# CONFIG_STM32_HIGHDENSITY is not set -# CONFIG_STM32_MEDIUMDENSITY is not set -# CONFIG_STM32_LOWDENSITY is not set -# CONFIG_STM32_STM32F20XX is not set -# CONFIG_STM32_STM32F30XX is not set -# CONFIG_STM32_STM32F40XX is not set - -# -# STM32 Peripheral Support -# -CONFIG_STM32_ADC1=y -# CONFIG_STM32_BKP is not set -# CONFIG_STM32_CEC is not set -# CONFIG_STM32_CRC is not set -CONFIG_STM32_DMA1=y -# CONFIG_STM32_DAC1 is not set -# CONFIG_STM32_DAC2 is not set -# CONFIG_STM32_I2C1 is not set -# CONFIG_STM32_I2C2 is not set -# CONFIG_STM32_PWR is not set -# CONFIG_STM32_SPI1 is not set -# CONFIG_STM32_TIM1 is not set -# CONFIG_STM32_TIM2 is not set -# CONFIG_STM32_TIM3 is not set -# CONFIG_STM32_TIM4 is not set -# CONFIG_STM32_TIM5 is not set -# CONFIG_STM32_TIM6 is not set -# CONFIG_STM32_TIM7 is not set -# CONFIG_STM32_TIM12 is not set -# CONFIG_STM32_TIM13 is not set -# CONFIG_STM32_TIM14 is not set -# CONFIG_STM32_TIM15 is not set -# CONFIG_STM32_TIM16 is not set -# CONFIG_STM32_TIM17 is not set -CONFIG_STM32_USART1=y -CONFIG_STM32_USART2=y -CONFIG_STM32_USART3=y -# CONFIG_STM32_UART4 is not set -# CONFIG_STM32_UART5 is not set -# CONFIG_STM32_IWDG is not set -# CONFIG_STM32_WWDG is not set -CONFIG_STM32_ADC=y - -# -# Alternate Pin Mapping -# -# CONFIG_STM32_USART1_REMAP is not set -# CONFIG_STM32_USART2_REMAP is not set -CONFIG_STM32_USART3_NO_REMAP=y -# CONFIG_STM32_USART3_FULL_REMAP is not set -# CONFIG_STM32_USART3_PARTIAL_REMAP is not set -# CONFIG_STM32_JTAG_DISABLE is not set -# CONFIG_STM32_JTAG_FULL_ENABLE is not set -# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set -CONFIG_STM32_JTAG_SW_ENABLE=y -# CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG is not set -# CONFIG_STM32_FORCEPOWER is not set -# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set -# CONFIG_STM32_DMACAPABLE is not set -CONFIG_STM32_USART=y - -# -# U[S]ART Configuration -# -# CONFIG_USART1_RS485 is not set -# CONFIG_USART2_RS485 is not set -# CONFIG_USART2_RXDMA is not set -# CONFIG_USART3_RS485 is not set -CONFIG_USART3_RXDMA=y -# CONFIG_SERIAL_DISABLE_REORDERING is not set -# CONFIG_STM32_USART_SINGLEWIRE is not set - -# -# USB Host Configuration -# - -# -# USB Device Configuration -# - -# -# External Memory Configuration -# - -# -# Architecture Options -# -# CONFIG_ARCH_NOINTC is not set -# CONFIG_ARCH_VECNOTIRQ is not set -CONFIG_ARCH_DMA=y -CONFIG_ARCH_IRQPRIO=y -# CONFIG_CUSTOM_STACK is not set -# CONFIG_ADDRENV is not set -CONFIG_ARCH_HAVE_VFORK=y -CONFIG_ARCH_STACKDUMP=y -# CONFIG_ENDIAN_BIG is not set -# CONFIG_ARCH_HAVE_RAMFUNCS is not set -CONFIG_ARCH_HAVE_RAMVECTORS=y -# CONFIG_ARCH_RAMVECTORS is not set - -# -# Board Settings -# -CONFIG_BOARD_LOOPSPERMSEC=2000 -# CONFIG_ARCH_CALIBRATION is not set -CONFIG_DRAM_START=0x20000000 -CONFIG_DRAM_SIZE=0x2000 -CONFIG_ARCH_HAVE_INTERRUPTSTACK=y -CONFIG_ARCH_INTERRUPTSTACK=500 - -# -# Boot options -# -# CONFIG_BOOT_RUNFROMEXTSRAM is not set -CONFIG_BOOT_RUNFROMFLASH=y -# CONFIG_BOOT_RUNFROMISRAM is not set -# CONFIG_BOOT_RUNFROMSDRAM is not set -# CONFIG_BOOT_COPYTORAM is not set - -# -# Board Selection -# CONFIG_ARCH_BOARD="px4io-v2" CONFIG_ARCH_BOARD_PX4IO_V2=y +CONFIG_BOARD_LOOPSPERMSEC=2000 +CONFIG_DRAM_SIZE=0x00002000 +CONFIG_DRAM_START=0x20000000 +CONFIG_ARCH_INTERRUPTSTACK=n +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARCH_BOOTLOADER=n +CONFIG_ARCH_LEDS=n +CONFIG_ARCH_BUTTONS=n +CONFIG_ARCH_CALIBRATION=n +CONFIG_ARCH_DMA=y +CONFIG_ARCH_MATH_H=y + +CONFIG_ARMV7M_CMNVECTOR=y +CONFIG_ARMV7M_STACKCHECK=n +# +# JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): +# +# CONFIG_STM32_DFU - Use the DFU bootloader, not JTAG +# +# JTAG Enable options: +# +# CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) +# but without JNTRST. +# CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled +# +CONFIG_STM32_DFU=n +CONFIG_STM32_JTAG_FULL_ENABLE=n +CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n +CONFIG_STM32_JTAG_SW_ENABLE=y # -# Common Board Options +# Individual subsystems can be enabled: # +# AHB: +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=n +CONFIG_STM32_CRC=n +# APB1: +# Timers 2,3 and 4 are owned by the PWM driver +CONFIG_STM32_TIM2=n +CONFIG_STM32_TIM3=n +CONFIG_STM32_TIM4=n +CONFIG_STM32_TIM5=n +CONFIG_STM32_TIM6=n +CONFIG_STM32_TIM7=n +CONFIG_STM32_WWDG=n +CONFIG_STM32_SPI2=n +CONFIG_STM32_USART2=y +CONFIG_STM32_USART3=y +CONFIG_STM32_I2C1=n +CONFIG_STM32_I2C2=n +CONFIG_STM32_BKP=n +CONFIG_STM32_PWR=n +CONFIG_STM32_DAC=n +# APB2: +# We use our own ADC driver, but leave this on for clocking purposes. +CONFIG_STM32_ADC1=y +CONFIG_STM32_ADC2=n +# TIM1 is owned by the HRT +CONFIG_STM32_TIM1=n +CONFIG_STM32_SPI1=n +CONFIG_STM32_TIM8=n +CONFIG_STM32_USART1=y +CONFIG_STM32_ADC3=n + # -# Board-Specific Options +# STM32F100 specific serial device driver settings # - +# CONFIG_USARTn_SERIAL_CONSOLE - selects the USARTn for the +# console and ttys0 (default is the USART1). +# CONFIG_USARTn_RXBUFSIZE - Characters are buffered as received. +# This specific the size of the receive buffer +# CONFIG_USARTn_TXBUFSIZE - Characters are buffered before +# being sent. This specific the size of the transmit buffer +# CONFIG_USARTn_BAUD - The configure BAUD of the UART. Must be +# CONFIG_USARTn_BITS - The number of bits. Must be either 7 or 8. +# CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity +# CONFIG_USARTn_2STOP - Two stop bits # -# RTOS Features -# -# CONFIG_BOARD_INITIALIZE is not set -CONFIG_MSEC_PER_TICK=1 -CONFIG_RR_INTERVAL=0 -# CONFIG_SCHED_INSTRUMENTATION is not set -CONFIG_TASK_NAME_SIZE=8 -# CONFIG_SCHED_HAVE_PARENT is not set -# CONFIG_JULIAN_TIME is not set -CONFIG_START_YEAR=1970 -CONFIG_START_MONTH=1 -CONFIG_START_DAY=1 -CONFIG_DEV_CONSOLE=y -# CONFIG_MUTEX_TYPES is not set -# CONFIG_PRIORITY_INHERITANCE is not set -CONFIG_FDCLONE_DISABLE=y -CONFIG_FDCLONE_STDIO=y -CONFIG_SDCLONE_DISABLE=y -# CONFIG_SCHED_WAITPID is not set -# CONFIG_SCHED_STARTHOOK is not set -# CONFIG_SCHED_ATEXIT is not set -# CONFIG_SCHED_ONEXIT is not set -CONFIG_USER_ENTRYPOINT="user_start" -CONFIG_DISABLE_OS_API=y -# CONFIG_DISABLE_CLOCK is not set -CONFIG_DISABLE_POSIX_TIMERS=y -CONFIG_DISABLE_PTHREAD=y -CONFIG_DISABLE_SIGNALS=y -CONFIG_DISABLE_MQUEUE=y -CONFIG_DISABLE_ENVIRON=y - -# -# Sizes of configurable things (0 disables) -# -CONFIG_MAX_TASKS=4 -CONFIG_MAX_TASK_ARGS=4 -CONFIG_NPTHREAD_KEYS=2 -CONFIG_NFILE_DESCRIPTORS=8 -CONFIG_NFILE_STREAMS=0 -CONFIG_NAME_MAX=12 -CONFIG_PREALLOC_MQ_MSGS=4 -CONFIG_MQ_MAXMSGSIZE=32 -CONFIG_MAX_WDOGPARMS=2 -CONFIG_PREALLOC_WDOGS=4 -CONFIG_PREALLOC_TIMERS=0 - -# -# Stack and heap information -# -CONFIG_IDLETHREAD_STACKSIZE=290 -CONFIG_USERMAIN_STACKSIZE=800 -CONFIG_PTHREAD_STACK_MIN=512 -CONFIG_PTHREAD_STACK_DEFAULT=1024 - -# -# Device Drivers -# -CONFIG_DISABLE_POLL=y -CONFIG_DEV_NULL=y -# CONFIG_DEV_ZERO is not set -# CONFIG_LOOP is not set -# CONFIG_RAMDISK is not set -# CONFIG_CAN is not set -# CONFIG_PWM is not set -# CONFIG_I2C is not set -CONFIG_ARCH_HAVE_I2CRESET=y -# CONFIG_SPI is not set -# CONFIG_RTC is not set -# CONFIG_WATCHDOG is not set -# CONFIG_ANALOG is not set -# CONFIG_AUDIO_DEVICES is not set -# CONFIG_BCH is not set -# CONFIG_INPUT is not set -# CONFIG_LCD is not set -# CONFIG_MMCSD is not set -# CONFIG_MTD is not set -# CONFIG_PIPES is not set -# CONFIG_PM is not set -# CONFIG_POWER is not set -# CONFIG_SENSORS is not set -CONFIG_SERIAL=y -# CONFIG_DEV_LOWCONSOLE is not set -# CONFIG_16550_UART is not set -CONFIG_ARCH_HAVE_USART1=y -CONFIG_ARCH_HAVE_USART2=y -CONFIG_ARCH_HAVE_USART3=y -CONFIG_MCU_SERIAL=y +CONFIG_SERIAL_TERMIOS=y CONFIG_STANDARD_SERIAL=y + CONFIG_USART1_SERIAL_CONSOLE=y -# CONFIG_USART2_SERIAL_CONSOLE is not set -# CONFIG_USART3_SERIAL_CONSOLE is not set -# CONFIG_NO_SERIAL_CONSOLE is not set +CONFIG_USART2_SERIAL_CONSOLE=n +CONFIG_USART3_SERIAL_CONSOLE=n -# -# USART1 Configuration -# -CONFIG_USART1_RXBUFSIZE=64 CONFIG_USART1_TXBUFSIZE=64 -CONFIG_USART1_BAUD=115200 -CONFIG_USART1_BITS=8 -CONFIG_USART1_PARITY=0 -CONFIG_USART1_2STOP=0 -# CONFIG_USART1_IFLOWCONTROL is not set -# CONFIG_USART1_OFLOWCONTROL is not set - -# -# USART2 Configuration -# -CONFIG_USART2_RXBUFSIZE=64 CONFIG_USART2_TXBUFSIZE=64 -CONFIG_USART2_BAUD=115200 -CONFIG_USART2_BITS=8 -CONFIG_USART2_PARITY=0 -CONFIG_USART2_2STOP=0 -# CONFIG_USART2_IFLOWCONTROL is not set -# CONFIG_USART2_OFLOWCONTROL is not set - -# -# USART3 Configuration -# -CONFIG_USART3_RXBUFSIZE=64 CONFIG_USART3_TXBUFSIZE=64 + +CONFIG_USART1_RXBUFSIZE=64 +CONFIG_USART2_RXBUFSIZE=64 +CONFIG_USART3_RXBUFSIZE=64 + +CONFIG_USART1_BAUD=115200 +CONFIG_USART2_BAUD=115200 CONFIG_USART3_BAUD=115200 + +CONFIG_USART1_BITS=8 +CONFIG_USART2_BITS=8 CONFIG_USART3_BITS=8 + +CONFIG_USART1_PARITY=0 +CONFIG_USART2_PARITY=0 CONFIG_USART3_PARITY=0 + +CONFIG_USART1_2STOP=0 +CONFIG_USART2_2STOP=0 CONFIG_USART3_2STOP=0 -# CONFIG_USART3_IFLOWCONTROL is not set -# CONFIG_USART3_OFLOWCONTROL is not set -# CONFIG_SERIAL_IFLOWCONTROL is not set -# CONFIG_SERIAL_OFLOWCONTROL is not set -# CONFIG_USBDEV is not set -# CONFIG_USBHOST is not set -# CONFIG_WIRELESS is not set -# -# System Logging Device Options -# - -# -# System Logging -# -# CONFIG_RAMLOG is not set - -# -# Networking Support -# -# CONFIG_NET is not set - -# -# File Systems -# - -# -# File system configuration -# -CONFIG_DISABLE_MOUNTPOINT=y -# CONFIG_FS_RAMMAP is not set - -# -# System Logging -# -# CONFIG_SYSLOG_ENABLE is not set -# CONFIG_SYSLOG is not set - -# -# Graphics Support -# -# CONFIG_NX is not set - -# -# Memory Management -# -# CONFIG_MM_MULTIHEAP is not set -CONFIG_MM_SMALL=y -CONFIG_MM_REGIONS=1 -# CONFIG_GRAN is not set - -# -# Audio Support -# -# CONFIG_AUDIO is not set - -# -# Binary Formats -# -# CONFIG_BINFMT_DISABLE is not set -# CONFIG_NXFLAT is not set -# CONFIG_ELF is not set -CONFIG_BUILTIN=y -# CONFIG_PIC is not set -# CONFIG_SYMTAB_ORDEREDBYNAME is not set - -# -# Library Routines -# - -# -# Standard C Library Options -# -CONFIG_STDIO_BUFFER_SIZE=32 -# CONFIG_STDIO_LINEBUFFER is not set -CONFIG_NUNGET_CHARS=2 -# CONFIG_NOPRINTF_FIELDWIDTH is not set -# CONFIG_LIBC_FLOATINGPOINT is not set -CONFIG_LIB_RAND_ORDER=1 -# CONFIG_EOL_IS_CR is not set -# CONFIG_EOL_IS_LF is not set -# CONFIG_EOL_IS_BOTH_CRLF is not set -CONFIG_EOL_IS_EITHER_CRLF=y -# CONFIG_LIBC_EXECFUNCS is not set -CONFIG_POSIX_SPAWN_PROXY_STACKSIZE=1024 -CONFIG_TASK_SPAWN_DEFAULT_STACKSIZE=2048 -# CONFIG_LIBC_STRERROR is not set -# CONFIG_LIBC_PERROR_STDOUT is not set -CONFIG_ARCH_LOWPUTC=y -CONFIG_LIB_SENDFILE_BUFSIZE=512 -# CONFIG_ARCH_ROMGETC is not set -# CONFIG_ARCH_OPTIMIZED_FUNCTIONS is not set - -# -# Non-standard Library Support -# -# CONFIG_LIB_KBDCODEC is not set -# CONFIG_LIB_SLCDCODEC is not set - -# -# Basic CXX Support -# -# CONFIG_C99_BOOL8 is not set -CONFIG_HAVE_CXX=y -CONFIG_HAVE_CXXINITIALIZE=y -# CONFIG_CXX_NEWLONG is not set - -# -# uClibc++ Standard C++ Library -# -# CONFIG_UCLIBCXX is not set - -# -# Application Configuration -# - -# -# Built-In Applications -# - -# -# Examples -# -# CONFIG_EXAMPLES_BUTTONS is not set -# CONFIG_EXAMPLES_CAN is not set -# CONFIG_EXAMPLES_COMPOSITE is not set -# CONFIG_EXAMPLES_CXXTEST is not set -# CONFIG_EXAMPLES_DHCPD is not set -# CONFIG_EXAMPLES_ELF is not set -# CONFIG_EXAMPLES_FTPC is not set -# CONFIG_EXAMPLES_FTPD is not set -# CONFIG_EXAMPLES_HELLO is not set -# CONFIG_EXAMPLES_HELLOXX is not set -# CONFIG_EXAMPLES_JSON is not set -# CONFIG_EXAMPLES_HIDKBD is not set -# CONFIG_EXAMPLES_KEYPADTEST is not set -# CONFIG_EXAMPLES_IGMP is not set -# CONFIG_EXAMPLES_LCDRW is not set -# CONFIG_EXAMPLES_MM is not set -# CONFIG_EXAMPLES_MODBUS is not set -# CONFIG_EXAMPLES_MOUNT is not set -# CONFIG_EXAMPLES_NRF24L01TERM is not set -# CONFIG_EXAMPLES_NSH is not set -# CONFIG_EXAMPLES_NULL is not set -# CONFIG_EXAMPLES_NX is not set -# CONFIG_EXAMPLES_NXCONSOLE is not set -# CONFIG_EXAMPLES_NXFFS is not set -# CONFIG_EXAMPLES_NXFLAT is not set -# CONFIG_EXAMPLES_NXHELLO is not set -# CONFIG_EXAMPLES_NXIMAGE is not set -# CONFIG_EXAMPLES_NXLINES is not set -# CONFIG_EXAMPLES_NXTEXT is not set -# CONFIG_EXAMPLES_OSTEST is not set -# CONFIG_EXAMPLES_PASHELLO is not set -# CONFIG_EXAMPLES_PIPE is not set -# CONFIG_EXAMPLES_POLL is not set -# CONFIG_EXAMPLES_POSIXSPAWN is not set -# CONFIG_EXAMPLES_QENCODER is not set -# CONFIG_EXAMPLES_RGMP is not set -# CONFIG_EXAMPLES_ROMFS is not set -# CONFIG_EXAMPLES_SENDMAIL is not set -# CONFIG_EXAMPLES_SERLOOP is not set -# CONFIG_EXAMPLES_SLCD is not set -# CONFIG_EXAMPLES_SMART is not set -# CONFIG_EXAMPLES_TCPECHO is not set -# CONFIG_EXAMPLES_TELNETD is not set -# CONFIG_EXAMPLES_THTTPD is not set -# CONFIG_EXAMPLES_TIFF is not set -# CONFIG_EXAMPLES_TOUCHSCREEN is not set -# CONFIG_EXAMPLES_UDP is not set -# CONFIG_EXAMPLES_UIP is not set -# CONFIG_EXAMPLES_USBSERIAL is not set -# CONFIG_EXAMPLES_USBMSC is not set -# CONFIG_EXAMPLES_USBTERM is not set -# CONFIG_EXAMPLES_WATCHDOG is not set - -# -# Graphics Support -# -# CONFIG_TIFF is not set - -# -# Interpreters -# -# CONFIG_INTERPRETERS_FICL is not set -# CONFIG_INTERPRETERS_PCODE is not set - -# -# Network Utilities -# - -# -# Networking Utilities -# -# CONFIG_NETUTILS_CODECS is not set -# CONFIG_NETUTILS_DHCPC is not set -# CONFIG_NETUTILS_DHCPD is not set -# CONFIG_NETUTILS_FTPC is not set -# CONFIG_NETUTILS_FTPD is not set -# CONFIG_NETUTILS_JSON is not set -# CONFIG_NETUTILS_RESOLV is not set -# CONFIG_NETUTILS_SMTP is not set -# CONFIG_NETUTILS_TELNETD is not set -# CONFIG_NETUTILS_TFTPC is not set -# CONFIG_NETUTILS_THTTPD is not set -# CONFIG_NETUTILS_UIPLIB is not set -# CONFIG_NETUTILS_WEBCLIENT is not set - -# -# FreeModBus -# -# CONFIG_MODBUS is not set - -# -# NSH Library -# -# CONFIG_NSH_LIBRARY is not set - -# -# NxWidgets/NxWM -# - -# -# System NSH Add-Ons -# - -# -# Custom Free Memory Command -# -# CONFIG_SYSTEM_FREE is not set - -# -# I2C tool -# - -# -# FLASH Program Installation -# -# CONFIG_SYSTEM_INSTALL is not set - -# -# FLASH Erase-all Command -# - -# -# readline() -# -# CONFIG_SYSTEM_READLINE is not set - -# -# Power Off -# -# CONFIG_SYSTEM_POWEROFF is not set - -# -# RAMTRON -# -# CONFIG_SYSTEM_RAMTRON is not set - -# -# SD Card -# -# CONFIG_SYSTEM_SDCARD is not set - -# -# Sysinfo -# -# CONFIG_SYSTEM_SYSINFO is not set - -# -# USB Monitor -# +CONFIG_USART1_RXDMA=y +SERIAL_HAVE_CONSOLE_DMA=y +# Conflicts with I2C1 DMA +CONFIG_USART2_RXDMA=n +CONFIG_USART3_RXDMA=y # # PX4IO specific driver settings @@ -723,3 +216,332 @@ CONFIG_HAVE_CXXINITIALIZE=y CONFIG_HRT_TIMER=y CONFIG_HRT_PPM=y +# +# General build options +# +# CONFIG_RRLOAD_BINARY - make the rrload binary format used with +# BSPs from www.ridgerun.com using the tools/mkimage.sh script +# CONFIG_INTELHEX_BINARY - make the Intel HEX binary format +# used with many different loaders using the GNU objcopy program +# Should not be selected if you are not using the GNU toolchain. +# CONFIG_MOTOROLA_SREC - make the Motorola S-Record binary format +# used with many different loaders using the GNU objcopy program +# Should not be selected if you are not using the GNU toolchain. +# CONFIG_RAW_BINARY - make a raw binary format file used with many +# different loaders using the GNU objcopy program. This option +# should not be selected if you are not using the GNU toolchain. +# CONFIG_HAVE_LIBM - toolchain supports libm.a +# +CONFIG_RRLOAD_BINARY=n +CONFIG_INTELHEX_BINARY=n +CONFIG_MOTOROLA_SREC=n +CONFIG_RAW_BINARY=y +CONFIG_HAVE_LIBM=n + +# +# General OS setup +# +# CONFIG_APPS_DIR - Identifies the relative path to the directory +# that builds the application to link with NuttX. Default: ../apps +# CONFIG_DEBUG - enables built-in debug options +# CONFIG_DEBUG_VERBOSE - enables verbose debug output +# CONFIG_DEBUG_SYMBOLS - build without optimization and with +# debug symbols (needed for use with a debugger). +# CONFIG_HAVE_CXX - Enable support for C++ +# CONFIG_HAVE_CXXINITIALIZE - The platform-specific logic includes support +# for initialization of static C++ instances for this architecture +# and for the selected toolchain (via up_cxxinitialize()). +# CONFIG_MM_REGIONS - If the architecture includes multiple +# regions of memory to allocate from, this specifies the +# number of memory regions that the memory manager must +# handle and enables the API mm_addregion(start, end); +# CONFIG_ARCH_LOWPUTC - architecture supports low-level, boot +# time console output +# CONFIG_MSEC_PER_TICK - The default system timer is 100Hz +# or MSEC_PER_TICK=10. This setting may be defined to +# inform NuttX that the processor hardware is providing +# system timer interrupts at some interrupt interval other +# than 10 msec. +# CONFIG_RR_INTERVAL - The round robin timeslice will be set +# this number of milliseconds; Round robin scheduling can +# be disabled by setting this value to zero. +# CONFIG_SCHED_INSTRUMENTATION - enables instrumentation in +# scheduler to monitor system performance +# CONFIG_TASK_NAME_SIZE - Spcifies that maximum size of a +# task name to save in the TCB. Useful if scheduler +# instrumentation is selected. Set to zero to disable. +# CONFIG_START_YEAR, CONFIG_START_MONTH, CONFIG_START_DAY - +# Used to initialize the internal time logic. +# CONFIG_GREGORIAN_TIME - Enables Gregorian time conversions. +# You would only need this if you are concerned about accurate +# time conversions in the past or in the distant future. +# CONFIG_JULIAN_TIME - Enables Julian time conversions. You +# would only need this if you are concerned about accurate +# time conversion in the distand past. You must also define +# CONFIG_GREGORIAN_TIME in order to use Julian time. +# CONFIG_DEV_CONSOLE - Set if architecture-specific logic +# provides /dev/console. Enables stdout, stderr, stdin. +# CONFIG_DEV_LOWCONSOLE - Use the simple, low-level serial console +# driver (minimul support) +# CONFIG_MUTEX_TYPES: Set to enable support for recursive and +# errorcheck mutexes. Enables pthread_mutexattr_settype(). +# CONFIG_PRIORITY_INHERITANCE : Set to enable support for priority +# inheritance on mutexes and semaphores. +# CONFIG_SEM_PREALLOCHOLDERS: This setting is only used if priority +# inheritance is enabled. It defines the maximum number of +# different threads (minus one) that can take counts on a +# semaphore with priority inheritance support. This may be +# set to zero if priority inheritance is disabled OR if you +# are only using semaphores as mutexes (only one holder) OR +# if no more than two threads participate using a counting +# semaphore. +# CONFIG_SEM_NNESTPRIO. If priority inheritance is enabled, +# then this setting is the maximum number of higher priority +# threads (minus 1) than can be waiting for another thread +# to release a count on a semaphore. This value may be set +# to zero if no more than one thread is expected to wait for +# a semaphore. +# CONFIG_FDCLONE_DISABLE. Disable cloning of all file descriptors +# by task_create() when a new task is started. If set, all +# files/drivers will appear to be closed in the new task. +# CONFIG_FDCLONE_STDIO. Disable cloning of all but the first +# three file descriptors (stdin, stdout, stderr) by task_create() +# when a new task is started. If set, all files/drivers will +# appear to be closed in the new task except for stdin, stdout, +# and stderr. +# CONFIG_SDCLONE_DISABLE. Disable cloning of all socket +# desciptors by task_create() when a new task is started. If +# set, all sockets will appear to be closed in the new task. +# CONFIG_SCHED_WORKQUEUE. Create a dedicated "worker" thread to +# handle delayed processing from interrupt handlers. This feature +# is required for some drivers but, if there are not complaints, +# can be safely disabled. The worker thread also performs +# garbage collection -- completing any delayed memory deallocations +# from interrupt handlers. If the worker thread is disabled, +# then that clean will be performed by the IDLE thread instead +# (which runs at the lowest of priority and may not be appropriate +# if memory reclamation is of high priority). If CONFIG_SCHED_WORKQUEUE +# is enabled, then the following options can also be used: +# CONFIG_SCHED_WORKPRIORITY - The execution priority of the worker +# thread. Default: 50 +# CONFIG_SCHED_WORKPERIOD - How often the worker thread checks for +# work in units of microseconds. Default: 50*1000 (50 MS). +# CONFIG_SCHED_WORKSTACKSIZE - The stack size allocated for the worker +# thread. Default: CONFIG_IDLETHREAD_STACKSIZE. +# CONFIG_SIG_SIGWORK - The signal number that will be used to wake-up +# the worker thread. Default: 4 +# CONFIG_SCHED_WAITPID - Enable the waitpid() API +# CONFIG_SCHED_ATEXIT - Enabled the atexit() API +# +CONFIG_USER_ENTRYPOINT="user_start" +#CONFIG_APPS_DIR= +CONFIG_DEBUG=n +CONFIG_DEBUG_VERBOSE=n +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_FS=n +CONFIG_DEBUG_GRAPHICS=n +CONFIG_DEBUG_LCD=n +CONFIG_DEBUG_USB=n +CONFIG_DEBUG_NET=n +CONFIG_DEBUG_RTC=n +CONFIG_DEBUG_ANALOG=n +CONFIG_DEBUG_PWM=n +CONFIG_DEBUG_CAN=n +CONFIG_DEBUG_I2C=n +CONFIG_DEBUG_INPUT=n + +CONFIG_MSEC_PER_TICK=1 +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_MM_REGIONS=1 +CONFIG_MM_SMALL=y +CONFIG_ARCH_LOWPUTC=y +CONFIG_RR_INTERVAL=0 +CONFIG_SCHED_INSTRUMENTATION=n +CONFIG_TASK_NAME_SIZE=8 +CONFIG_START_YEAR=1970 +CONFIG_START_MONTH=1 +CONFIG_START_DAY=1 +CONFIG_GREGORIAN_TIME=n +CONFIG_JULIAN_TIME=n +# this eats ~1KiB of RAM ... work out why +CONFIG_DEV_CONSOLE=y +CONFIG_DEV_LOWCONSOLE=n +CONFIG_MUTEX_TYPES=n +CONFIG_PRIORITY_INHERITANCE=n +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_NNESTPRIO=0 +CONFIG_FDCLONE_DISABLE=y +CONFIG_FDCLONE_STDIO=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SCHED_WORKQUEUE=n +CONFIG_SCHED_WORKPRIORITY=50 +CONFIG_SCHED_WORKPERIOD=50000 +CONFIG_SCHED_WORKSTACKSIZE=1024 +CONFIG_SIG_SIGWORK=4 +CONFIG_SCHED_WAITPID=n +CONFIG_SCHED_ATEXIT=n + +# +# The following can be used to disable categories of +# APIs supported by the OS. If the compiler supports +# weak functions, then it should not be necessary to +# disable functions unless you want to restrict usage +# of those APIs. +# +# There are certain dependency relationships in these +# features. +# +# o mq_notify logic depends on signals to awaken tasks +# waiting for queues to become full or empty. +# o pthread_condtimedwait() depends on signals to wake +# up waiting tasks. +# +CONFIG_DISABLE_CLOCK=n +CONFIG_DISABLE_POSIX_TIMERS=y +CONFIG_DISABLE_PTHREAD=y +CONFIG_DISABLE_SIGNALS=y +CONFIG_DISABLE_MQUEUE=y +CONFIG_DISABLE_MOUNTPOINT=y +CONFIG_DISABLE_ENVIRON=y +CONFIG_DISABLE_POLL=y + +# +# Misc libc settings +# +# CONFIG_NOPRINTF_FIELDWIDTH - sprintf-related logic is a +# little smaller if we do not support fieldwidthes +# +CONFIG_NOPRINTF_FIELDWIDTH=n + +# +# Allow for architecture optimized implementations +# +# The architecture can provide optimized versions of the +# following to improve system performance +# +CONFIG_ARCH_MEMCPY=n +CONFIG_ARCH_MEMCMP=n +CONFIG_ARCH_MEMMOVE=n +CONFIG_ARCH_MEMSET=n +CONFIG_ARCH_STRCMP=n +CONFIG_ARCH_STRCPY=n +CONFIG_ARCH_STRNCPY=n +CONFIG_ARCH_STRLEN=n +CONFIG_ARCH_STRNLEN=n +CONFIG_ARCH_BZERO=n + +# +# Sizes of configurable things (0 disables) +# +# CONFIG_MAX_TASKS - The maximum number of simultaneously +# active tasks. This value must be a power of two. +# CONFIG_MAX_TASK_ARGS - This controls the maximum number of +# of parameters that a task may receive (i.e., maxmum value +# of 'argc') +# CONFIG_NPTHREAD_KEYS - The number of items of thread- +# specific data that can be retained +# CONFIG_NFILE_DESCRIPTORS - The maximum number of file +# descriptors (one for each open) +# CONFIG_NFILE_STREAMS - The maximum number of streams that +# can be fopen'ed +# CONFIG_NAME_MAX - The maximum size of a file name. +# CONFIG_STDIO_BUFFER_SIZE - Size of the buffer to allocate +# on fopen. (Only if CONFIG_NFILE_STREAMS > 0) +# CONFIG_STDIO_LINEBUFFER - If standard C buffered I/O is enabled +# (CONFIG_STDIO_BUFFER_SIZE > 0), then this option may be added +# to force automatic, line-oriented flushing the output buffer +# for putc(), fputc(), putchar(), puts(), fputs(), printf(), +# fprintf(), and vfprintf(). When a newline is encountered in +# the output string, the output buffer will be flushed. This +# (slightly) increases the NuttX footprint but supports the kind +# of behavior that people expect for printf(). +# CONFIG_NUNGET_CHARS - Number of characters that can be +# buffered by ungetc() (Only if CONFIG_NFILE_STREAMS > 0) +# CONFIG_PREALLOC_MQ_MSGS - The number of pre-allocated message +# structures. The system manages a pool of preallocated +# message structures to minimize dynamic allocations +# CONFIG_MQ_MAXMSGSIZE - Message structures are allocated with +# a fixed payload size given by this settin (does not include +# other message structure overhead. +# CONFIG_MAX_WDOGPARMS - Maximum number of parameters that +# can be passed to a watchdog handler +# CONFIG_PREALLOC_WDOGS - The number of pre-allocated watchdog +# structures. The system manages a pool of preallocated +# watchdog structures to minimize dynamic allocations +# CONFIG_PREALLOC_TIMERS - The number of pre-allocated POSIX +# timer structures. The system manages a pool of preallocated +# timer structures to minimize dynamic allocations. Set to +# zero for all dynamic allocations. +# +CONFIG_MAX_TASKS=4 +CONFIG_MAX_TASK_ARGS=4 +CONFIG_NPTHREAD_KEYS=2 +CONFIG_NFILE_DESCRIPTORS=8 +CONFIG_NFILE_STREAMS=0 +CONFIG_NAME_MAX=12 +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STDIO_LINEBUFFER=n +CONFIG_NUNGET_CHARS=2 +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_MQ_MAXMSGSIZE=32 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=4 +CONFIG_PREALLOC_TIMERS=0 + + +# +# Settings for apps/nshlib +# +# CONFIG_NSH_BUILTIN_APPS - Support external registered, +# "named" applications that can be executed from the NSH +# command line (see apps/README.txt for more information). +# CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer +# CONFIG_NSH_STRERROR - Use strerror(errno) +# CONFIG_NSH_LINELEN - Maximum length of one command line +# CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi +# CONFIG_NSH_DISABLESCRIPT - Disable scripting support +# CONFIG_NSH_DISABLEBG - Disable background commands +# CONFIG_NSH_ROMFSETC - Use startup script in /etc +# CONFIG_NSH_CONSOLE - Use serial console front end +# CONFIG_NSH_TELNET - Use telnetd console front end +# CONFIG_NSH_ARCHINIT - Platform provides architecture +# specific initialization (nsh_archinitialize()). +# + +# Disable NSH completely +CONFIG_NSH_CONSOLE=n + +# +# Stack and heap information +# +# CONFIG_BOOT_RUNFROMFLASH - Some configurations support XIP +# operation from FLASH but must copy initialized .data sections to RAM. +# (should also be =n for the STM3210E-EVAL which always runs from flash) +# CONFIG_BOOT_COPYTORAM - Some configurations boot in FLASH +# but copy themselves entirely into RAM for better performance. +# CONFIG_CUSTOM_STACK - The up_ implementation will handle +# all stack operations outside of the nuttx model. +# CONFIG_STACK_POINTER - The initial stack pointer (arm7tdmi only) +# CONFIG_IDLETHREAD_STACKSIZE - The size of the initial stack. +# This is the thread that (1) performs the inital boot of the system up +# to the point where user_start() is spawned, and (2) there after is the +# IDLE thread that executes only when there is no other thread ready to +# run. +# CONFIG_USERMAIN_STACKSIZE - The size of the stack to allocate +# for the main user thread that begins at the user_start() entry point. +# CONFIG_PTHREAD_STACK_MIN - Minimum pthread stack size +# CONFIG_PTHREAD_STACK_DEFAULT - Default pthread stack size +# CONFIG_HEAP_BASE - The beginning of the heap +# CONFIG_HEAP_SIZE - The size of the heap +# +CONFIG_BOOT_RUNFROMFLASH=n +CONFIG_BOOT_COPYTORAM=n +CONFIG_CUSTOM_STACK=n +CONFIG_STACK_POINTER= +CONFIG_IDLETHREAD_STACKSIZE=1024 +CONFIG_USERMAIN_STACKSIZE=1200 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_PTHREAD_STACK_DEFAULT=1024 +CONFIG_HEAP_BASE= +CONFIG_HEAP_SIZE= diff --git a/posix-configs/SITL/init/rc.fixed_wing b/posix-configs/SITL/init/rc.fixed_wing index bdb2fb8e9d..7aa521daf2 100644 --- a/posix-configs/SITL/init/rc.fixed_wing +++ b/posix-configs/SITL/init/rc.fixed_wing @@ -5,10 +5,10 @@ param set MAV_TYPE 1 param set SYS_AUTOSTART 3033 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_ID 2293768 +param set CAL_ACC0_ID 1376264 +param set CAL_ACC1_ID 1310728 +param set CAL_MAG0_ID 196616 param set CAL_GYRO0_XOFF 0.01 param set CAL_ACC0_XOFF 0.01 param set CAL_ACC0_YOFF -0.01 @@ -21,7 +21,7 @@ param set CAL_MAG0_XOFF 0.01 param set MPC_XY_P 0.4 param set MPC_XY_VEL_P 0.2 param set MPC_XY_VEL_D 0.005 -param set COM_RC_IN_MODE 2 +param set COM_RC_IN_MODE 1 rgbled start tone_alarm start gyrosim start diff --git a/posix-configs/SITL/init/rcS_gazebo b/posix-configs/SITL/init/rcS_gazebo_iris similarity index 85% rename from posix-configs/SITL/init/rcS_gazebo rename to posix-configs/SITL/init/rcS_gazebo_iris index ca5415d110..95be984fb4 100644 --- a/posix-configs/SITL/init/rcS_gazebo +++ b/posix-configs/SITL/init/rcS_gazebo_iris @@ -9,10 +9,10 @@ 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_ID 2293768 +param set CAL_ACC0_ID 1376264 +param set CAL_ACC1_ID 1310728 +param set CAL_MAG0_ID 196616 param set CAL_GYRO0_XOFF 0.01 param set CAL_ACC0_XOFF 0.01 param set CAL_ACC0_YOFF -0.01 @@ -27,8 +27,11 @@ 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 +param set COM_RC_IN_MODE 1 +param set NAV_ACC_RAD 2.0 +param set RTL_RETURN_ALT 30.0 +param set RTL_DESCEND_ALT 10.0 +rgbledsim start tone_alarm start gyrosim start accelsim start diff --git a/posix-configs/SITL/init/rcS_gazebo_tailsitter b/posix-configs/SITL/init/rcS_gazebo_tailsitter new file mode 100644 index 0000000000..c88948cf55 --- /dev/null +++ b/posix-configs/SITL/init/rcS_gazebo_tailsitter @@ -0,0 +1,64 @@ +uorb start +simulator start -s +param load +param set MAV_TYPE 20 +param set MC_PITCHRATE_P 0.3 +param set MC_ROLLRATE_P 0.3 +param set MC_YAW_P 2.0 +param set MC_YAWRATE_P 0.35 +param set VT_TYPE 0 +param set SYS_AUTOSTART 4010 +param set SYS_RESTART_TYPE 2 +dataman start +param set CAL_GYRO0_ID 2293768 +param set CAL_ACC0_ID 1376264 +param set CAL_ACC1_ID 1310728 +param set CAL_MAG0_ID 196616 +param set CAL_GYRO0_XOFF 0.01 +param set CAL_ACC0_XOFF 0.01 +param set CAL_ACC0_YOFF -0.01 +param set CAL_ACC0_ZOFF 0.01 +param set CAL_ACC0_XSCALE 1.01 +param set CAL_ACC0_YSCALE 1.01 +param set CAL_ACC0_ZSCALE 1.01 +param set CAL_ACC1_XOFF 0.01 +param set CAL_MAG0_XOFF 0.01 +param set MPC_XY_P 0.15 +param set MPC_XY_VEL_P 0.05 +param set MPC_XY_VEL_D 0.005 +param set MPC_XY_FF 0.1 +param set SENS_BOARD_ROT 8 +param set COM_RC_IN_MODE 1 +rgbledsim start +tone_alarm start +gyrosim start +accelsim start +barosim start +adcsim start +gpssim start +measairspeedsim start +pwm_out_sim mode_pwm +sleep 1 +sensors start +commander start +land_detector start multicopter +navigator start +attitude_estimator_q start +position_estimator_inav start +vtol_att_control start +mc_pos_control start +mc_att_control start +fw_pos_control_l1 start +fw_att_control start +mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x_vtol.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_jmavsim b/posix-configs/SITL/init/rcS_jmavsim_iris similarity index 82% rename from posix-configs/SITL/init/rcS_jmavsim rename to posix-configs/SITL/init/rcS_jmavsim_iris index 878eda3a6e..88a3f3978e 100644 --- a/posix-configs/SITL/init/rcS_jmavsim +++ b/posix-configs/SITL/init/rcS_jmavsim_iris @@ -3,16 +3,18 @@ simulator start -s param load param set MAV_TYPE 2 param set MC_PITCHRATE_P 0.15 +param set MC_PITCH_P 7 +param set MC_ROLL_P 7 param set MC_ROLLRATE_P 0.15 -param set MC_YAW_P 2.0 +param set MC_YAW_P 2.8 param set MC_YAWRATE_P 0.35 param set SYS_AUTOSTART 4010 param set SYS_RESTART_TYPE 2 dataman start -param set 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_ID 2293768 +param set CAL_ACC0_ID 1376264 +param set CAL_ACC1_ID 1310728 +param set CAL_MAG0_ID 196616 param set CAL_GYRO0_XOFF 0.01 param set CAL_ACC0_XOFF 0.01 param set CAL_ACC0_YOFF -0.01 @@ -26,8 +28,11 @@ param set MPC_XY_P 0.4 param set MPC_XY_VEL_P 0.2 param set MPC_XY_VEL_D 0.005 param set SENS_BOARD_ROT 0 -param set COM_RC_IN_MODE 2 -rgbled start +param set COM_RC_IN_MODE 1 +param set NAV_ACC_RAD 2.0 +param set RTL_RETURN_ALT 30.0 +param set RTL_DESCEND_ALT 10.0 +rgbledsim start tone_alarm start gyrosim start accelsim start diff --git a/posix-configs/SITL/init/rcS_lpe_gazebo b/posix-configs/SITL/init/rcS_lpe_gazebo_iris similarity index 93% rename from posix-configs/SITL/init/rcS_lpe_gazebo rename to posix-configs/SITL/init/rcS_lpe_gazebo_iris index 6093991c13..a6c3f5cf39 100644 --- a/posix-configs/SITL/init/rcS_lpe_gazebo +++ b/posix-configs/SITL/init/rcS_lpe_gazebo_iris @@ -9,10 +9,10 @@ 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_ID 2293768 +param set CAL_ACC0_ID 1376264 +param set CAL_ACC1_ID 1310728 +param set CAL_MAG0_ID 196616 param set CAL_GYRO0_XOFF 0.01 param set CAL_ACC0_XOFF 1 param set CAL_ACC0_YOFF 2 diff --git a/posix-configs/SITL/init/rcS_lpe_jmavsim b/posix-configs/SITL/init/rcS_lpe_jmavsim_iris similarity index 93% rename from posix-configs/SITL/init/rcS_lpe_jmavsim rename to posix-configs/SITL/init/rcS_lpe_jmavsim_iris index 9b416a0b26..7d59fe876f 100644 --- a/posix-configs/SITL/init/rcS_lpe_jmavsim +++ b/posix-configs/SITL/init/rcS_lpe_jmavsim_iris @@ -9,10 +9,10 @@ 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_ID 2293768 +param set CAL_ACC0_ID 1376264 +param set CAL_ACC1_ID 1310728 +param set CAL_MAG0_ID 196616 param set CAL_GYRO0_XOFF 0.01 param set CAL_ACC0_XOFF 0.01 param set CAL_ACC0_YOFF -0.01 diff --git a/posix-configs/SITL/init/rc_iris_ros b/posix-configs/SITL/init/rc_iris_ros index 096f659d65..af2db4d57b 100644 --- a/posix-configs/SITL/init/rc_iris_ros +++ b/posix-configs/SITL/init/rc_iris_ros @@ -9,10 +9,10 @@ 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_ID 2293768 +param set CAL_ACC0_ID 1376264 +param set CAL_ACC1_ID 1310728 +param set CAL_MAG0_ID 196616 param set CAL_GYRO0_XOFF 0.01 param set CAL_ACC0_XOFF 0.01 param set CAL_ACC0_YOFF -0.01 @@ -40,7 +40,7 @@ param set MP_ROLLRATE_I 0.001 param set MP_ROLLRATE_D 0.001 param set MP_PITCH_P 4 param set MP_PITCHRATE_P 0.3 -param set COM_RC_IN_MODE 2 +param set COM_RC_IN_MODE 1 rgbled start tone_alarm start gyrosim start diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_init.c b/src/drivers/boards/px4fmu-v1/px4fmu_init.c index dea7678014..4e969692bc 100644 --- a/src/drivers/boards/px4fmu-v1/px4fmu_init.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_init.c @@ -145,20 +145,6 @@ static struct spi_dev_s *spi3; #include -#if 0 -#ifdef __cplusplus -__EXPORT int matherr(struct __exception *e) -{ - return 1; -} -#else -__EXPORT int matherr(struct exception *e) -{ - return 1; -} -#endif -#endif - __EXPORT int nsh_archinitialize(void) { int result; diff --git a/src/drivers/boards/px4fmu-v2/CMakeLists.txt b/src/drivers/boards/px4fmu-v2/CMakeLists.txt index 27cb595c34..19b3f02754 100644 --- a/src/drivers/boards/px4fmu-v2/CMakeLists.txt +++ b/src/drivers/boards/px4fmu-v2/CMakeLists.txt @@ -44,4 +44,4 @@ px4_add_module( DEPENDS platforms__common ) -# vim: set noet ft=cmake fenc=utf-8 ff=unix : +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c index 116a6ff556..687110fa40 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -212,19 +212,6 @@ static struct sdio_dev_s *sdio; #include -/* TODO XXX commented this out to get cmake build working */ -/*#ifdef __cplusplus*/ -/*__EXPORT int matherr(struct __exception *e)*/ -/*{*/ -/*return 1;*/ -/*}*/ -/*#else*/ -/*__EXPORT int matherr(struct exception *e)*/ -/*{*/ -/*return 1;*/ -/*}*/ -/*#endif*/ - __EXPORT int nsh_archinitialize(void) { diff --git a/src/drivers/boards/px4fmu-v4/CMakeLists.txt b/src/drivers/boards/px4fmu-v4/CMakeLists.txt new file mode 100644 index 0000000000..b16a991331 --- /dev/null +++ b/src/drivers/boards/px4fmu-v4/CMakeLists.txt @@ -0,0 +1,47 @@ +############################################################################ +# +# 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__boards__px4fmu-v4 + COMPILE_FLAGS + -Os + SRCS + px4fmu_can.c + px4fmu_init.c + px4fmu_pwm_servo.c + px4fmu_spi.c + px4fmu_usb.c + px4fmu_led.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h new file mode 100644 index 0000000000..a29c961f5a --- /dev/null +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -0,0 +1,282 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file board_config.h + * + * PX4FMUv2 internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +__BEGIN_DECLS + +/* these headers are not C++ safe */ +#include +#include + +#define UDID_START 0x1FFF7A10 + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ +/* Configuration ************************************************************************************/ +//{GPIO_RSSI_IN, 0, 0}, - pio Analog used as PWM +//{0, GPIO_LED_SAFETY, 0}, pio replacement +//{GPIO_SAFETY_SWITCH_IN, 0, 0}, pio replacement +//{0, GPIO_PERIPH_3V3_EN, 0}, Owned by the 8266 driver +//{0, GPIO_SBUS_INV, 0}, https://github.com/PX4/Firmware/blob/master/src/modules/px4iofirmware/sbus.c +//{GPIO_8266_GPIO0, 0, 0}, Owned by the 8266 driver +//{0, GPIO_SPEKTRUM_POWER, 0}, Owned Spektum driver input to auto pilot +//{0, GPIO_8266_PD, 0}, Owned by the 8266 driver +//{0, GPIO_8266_RST, 0}, Owned by the 8266 driver + +/* PX4FMU GPIOs ***********************************************************************************/ +/* LEDs */ + +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN11) +#define GPIO_LED2 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) +#define GPIO_LED3 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN3) + +#define GPIO_LED_RED GPIO_LED1 +#define GPIO_LED_GREEN GPIO_LED2 +#define GPIO_LED_BLUE GPIO_LED3 + +/* Define the Chip Selects */ + +#define GPIO_SPI_CS_MPU9250 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) +#define GPIO_SPI_CS_HMC5983 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN15) +#define GPIO_SPI_CS_MS5611 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) +#define GPIO_SPI_CS_ICM_20608_G (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) + +#define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) + +/* Define the Ready interrupts */ + +#define GPIO_DRDY_MPU9250 (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15) +#define GPIO_DRDY_HMC5983 (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTE|GPIO_PIN12) +#define GPIO_DRDY_ICM_20608_G (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTC|GPIO_PIN14) + + +/* + * Define the ability to shut off off the sensor signals + * by changing the signals to inputs + */ + +#define _PIN_OFF(def) (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz)) + +#define GPIO_SPI_CS_OFF_MPU9250 _PIN_OFF(GPIO_SPI_CS_MPU9250) +#define GPIO_SPI_CS_OFF_HMC5983 _PIN_OFF(GPIO_SPI_CS_HMC5983) +#define GPIO_SPI_CS_OFF_MS5611 _PIN_OFF(GPIO_SPI_CS_MS5611) +#define GPIO_SPI_CS_OFF_ICM_20608_G _PIN_OFF(GPIO_SPI_CS_ICM_20608_G) + +#define GPIO_DRDY_OFF_MPU9250 _PIN_OFF(GPIO_DRDY_MPU9250) +#define GPIO_DRDY_OFF_HMC5983 _PIN_OFF(GPIO_DRDY_HMC5983) +#define GPIO_DRDY_OFF_ICM_20608_G _PIN_OFF(GPIO_DRDY_ICM_20608_G) + + +/* SPI1 off */ +#define GPIO_SPI1_SCK_OFF _PIN_OFF(GPIO_SPI1_SCK) +#define GPIO_SPI1_MISO_OFF _PIN_OFF(GPIO_SPI1_MISO) +#define GPIO_SPI1_MOSI_OFF _PIN_OFF(GPIO_SPI1_MOSI) + +#define PX4_SPI_BUS_SENSORS 1 +#define PX4_SPI_BUS_RAMTRON 2 + +/* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */ +#define PX4_SPIDEV_GYRO 1 +#define PX4_SPIDEV_ACCEL_MAG 2 +#define PX4_SPIDEV_BARO 3 +#define PX4_SPIDEV_MPU 4 +#define PX4_SPIDEV_HMC 5 +#define PX4_SPIDEV_ICM 6 + +/* I2C busses */ +#define PX4_I2C_BUS_EXPANSION 1 +#define PX4_I2C_BUS_LED PX4_I2C_BUS_EXPANSION + +/* Devices on the external bus. + * + * Note that these are unshifted addresses. + */ +#define PX4_I2C_OBDEV_LED 0x55 +#define PX4_I2C_OBDEV_HMC5883 0x1e + +/* + * ADC channels + * + * These are the channel numbers of the ADCs of the microcontroller that can be used by the Px4 Firmware in the adc driver + */ +#define ADC_CHANNELS (1 << 2) | (1 << 3) | (1 << 4) | (1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) | (1 << 15) + +// ADC defines to be used in sensors.cpp to read from a particular channel +#define ADC_BATTERY_VOLTAGE_CHANNEL 2 +#define ADC_BATTERY_CURRENT_CHANNEL 3 +#define ADC_5V_RAIL_SENSE 4 +#define ADC_AIRSPEED_VOLTAGE_CHANNEL 15 + +/* User GPIOs + * + * GPIO0-5 are the PWM servo outputs. + */ +#define GPIO_GPIO0_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN14) +#define GPIO_GPIO1_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN13) +#define GPIO_GPIO2_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN11) +#define GPIO_GPIO3_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN9) +#define GPIO_GPIO4_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN13) +#define GPIO_GPIO5_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN14) + +#define GPIO_GPIO0_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN14) +#define GPIO_GPIO1_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN13) +#define GPIO_GPIO2_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11) +#define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN9) +#define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13) +#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN14) + +/* Power supply control and monitoring GPIOs */ +#define GPIO_VDD_BRICK_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) +#define GPIO_VDD_3V3_SENSORS_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) + +/* Tone alarm output */ +#define TONE_ALARM_TIMER 2 /* timer 2 */ +#define TONE_ALARM_CHANNEL 1 /* channel 1 */ +#define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15) +#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15) + +/* PWM + * + * Six PWM outputs are configured. + * + * Pins: + * + * CH1 : PE14 : TIM1_CH4 + * CH2 : PE13 : TIM1_CH3 + * CH3 : PE11 : TIM1_CH2 + * CH4 : PE9 : TIM1_CH1 + * CH5 : PD13 : TIM4_CH2 + * CH6 : PD14 : TIM4_CH3 + */ +#define GPIO_TIM1_CH1OUT GPIO_TIM1_CH1OUT_2 +#define GPIO_TIM1_CH2OUT GPIO_TIM1_CH2OUT_2 +#define GPIO_TIM1_CH3OUT GPIO_TIM1_CH3OUT_2 +#define GPIO_TIM1_CH4OUT GPIO_TIM1_CH4OUT_2 +#define GPIO_TIM4_CH2OUT GPIO_TIM4_CH2OUT_2 +#define GPIO_TIM4_CH3OUT GPIO_TIM4_CH3OUT_2 + +/* USB OTG FS + * + * PA9 OTG_FS_VBUS VBUS sensing + */ +#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9) + +/* High-resolution timer */ +#define HRT_TIMER 3 /* use timer8 for the HRT */ +#define HRT_TIMER_CHANNEL 4 /* use capture/compare channel */ + +#define HRT_PPM_CHANNEL 3 /* use capture/compare channel 2 */ +#define GPIO_PPM_IN (GPIO_ALT|GPIO_AF2|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN0) + +#define SBUS_SERIAL_PORT "/dev/ttyS4" + +/* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */ +#define PWMIN_TIMER 4 +#define PWMIN_TIMER_CHANNEL 2 +#define GPIO_PWM_IN GPIO_TIM4_CH2IN_2 + +#define GPIO_RSSI_IN (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1) +#define GPIO_LED_SAFETY (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN3) +#define GPIO_SAFETY_SWITCH_IN (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4) +#define GPIO_PERIPH_3V3_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN5) +#define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) + +#define GPIO_8266_GPIO0 (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN2) +#define GPIO_SPEKTRUM_POWER (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) +#define GPIO_8266_PD (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN5) +#define GPIO_8266_RST (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN6) + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +/**************************************************************************************************** + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ****************************************************************************************************/ + +extern void stm32_spiinitialize(void); + +extern void stm32_usbinitialize(void); + +/**************************************************************************** + * Name: nsh_archinitialize + * + * Description: + * Perform architecture specific initialization for NSH. + * + * CONFIG_NSH_ARCHINIT=y : + * Called from the NSH library + * + * CONFIG_BOARD_INITIALIZE=y, CONFIG_NSH_LIBRARY=y, && + * CONFIG_NSH_ARCHINIT=n : + * Called from board_initialize(). + * + ****************************************************************************/ + +#ifdef CONFIG_NSH_LIBRARY +int nsh_archinitialize(void); +#endif + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/src/drivers/boards/px4fmu-v4/px4fmu_can.c b/src/drivers/boards/px4fmu-v4/px4fmu_can.c new file mode 100644 index 0000000000..62f170957d --- /dev/null +++ b/src/drivers/boards/px4fmu-v4/px4fmu_can.c @@ -0,0 +1,144 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4fmu_can.c + * + * Board-specific CAN functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include + +#include +#include + +#include "chip.h" +#include "up_arch.h" + +#include "stm32.h" +#include "stm32_can.h" +#include "board_config.h" + +#ifdef CONFIG_CAN + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ +/* Configuration ********************************************************************/ + +#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2) +# warning "Both CAN1 and CAN2 are enabled. Assuming only CAN1." +# undef CONFIG_STM32_CAN2 +#endif + +#ifdef CONFIG_STM32_CAN1 +# define CAN_PORT 1 +#else +# define CAN_PORT 2 +#endif + +/* Debug ***************************************************************************/ +/* Non-standard debug that may be enabled just for testing CAN */ + +#ifdef CONFIG_DEBUG_CAN +# define candbg dbg +# define canvdbg vdbg +# define canlldbg lldbg +# define canllvdbg llvdbg +#else +# define candbg(x...) +# define canvdbg(x...) +# define canlldbg(x...) +# define canllvdbg(x...) +#endif + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: can_devinit + * + * Description: + * All STM32 architectures must provide the following interface to work with + * examples/can. + * + ************************************************************************************/ + +int can_devinit(void) +{ + static bool initialized = false; + struct can_dev_s *can; + int ret; + + /* Check if we have already initialized */ + + if (!initialized) { + /* Call stm32_caninitialize() to get an instance of the CAN interface */ + + can = stm32_caninitialize(CAN_PORT); + + if (can == NULL) { + candbg("ERROR: Failed to get CAN interface\n"); + return -ENODEV; + } + + /* Register the CAN driver at "/dev/can0" */ + + ret = can_register("/dev/can0", can); + + if (ret < 0) { + candbg("ERROR: can_register failed: %d\n", ret); + return ret; + } + + /* Now we are initialized */ + + initialized = true; + } + + return OK; +} + +#endif \ No newline at end of file diff --git a/src/drivers/boards/px4fmu-v4/px4fmu_init.c b/src/drivers/boards/px4fmu-v4/px4fmu_init.c new file mode 100644 index 0000000000..7966f57165 --- /dev/null +++ b/src/drivers/boards/px4fmu-v4/px4fmu_init.c @@ -0,0 +1,335 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4fmu_init.c + * + * PX4FMU-specific early startup code. This file implements the + * nsh_archinitialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include "board_config.h" +#include + +#include + +#include +#include + +#include +#include + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* Debug ********************************************************************/ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) lowsyslog(__VA_ARGS__) +# else +# define message(...) printf(__VA_ARGS__) +# endif +#else +# ifdef CONFIG_DEBUG +# define message lowsyslog +# else +# define message printf +# endif +#endif + +/* + * Ideally we'd be able to get these from up_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +__END_DECLS + +/**************************************************************************** + * Protected Functions + ****************************************************************************/ + +#if defined(CONFIG_FAT_DMAMEMORY) +# if !defined(CONFIG_GRAN) || !defined(CONFIG_FAT_DMAMEMORY) +# error microSD DMA support requires CONFIG_GRAN +# endif + +static GRAN_HANDLE dma_allocator; + +/* + * The DMA heap size constrains the total number of things that can be + * ready to do DMA at a time. + * + * For example, FAT DMA depends on one sector-sized buffer per filesystem plus + * one sector-sized buffer per file. + * + * We use a fundamental alignment / granule size of 64B; this is sufficient + * to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits). + */ +static uint8_t g_dma_heap[8192] __attribute__((aligned(64))); +static perf_counter_t g_dma_perf; + +static void +dma_alloc_init(void) +{ + dma_allocator = gran_initialize(g_dma_heap, + sizeof(g_dma_heap), + 7, /* 128B granule - must be > alignment (XXX bug?) */ + 6); /* 64B alignment */ + + if (dma_allocator == NULL) { + message("[boot] DMA allocator setup FAILED"); + + } else { + g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations"); + } +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/* + * DMA-aware allocator stubs for the FAT filesystem. + */ + +__EXPORT void *fat_dma_alloc(size_t size); +__EXPORT void fat_dma_free(FAR void *memory, size_t size); + +void * +fat_dma_alloc(size_t size) +{ + perf_count(g_dma_perf); + return gran_alloc(dma_allocator, size); +} + +void +fat_dma_free(FAR void *memory, size_t size) +{ + gran_free(dma_allocator, memory, size); +} + +#else + +# define dma_alloc_init() + +#endif + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void +stm32_boardinitialize(void) +{ + /* configure SPI interfaces */ + stm32_spiinitialize(); + + /* configure LEDs */ + up_ledinit(); +} + +/**************************************************************************** + * Name: nsh_archinitialize + * + * Description: + * Perform architecture specific initialization + * + ****************************************************************************/ + +static struct spi_dev_s *spi1; +static struct spi_dev_s *spi2; +static struct sdio_dev_s *sdio; + +#include + +__EXPORT int nsh_archinitialize(void) +{ + + /* configure ADC pins */ + stm32_configgpio(GPIO_ADC1_IN2); /* BATT_VOLTAGE_SENS */ + stm32_configgpio(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */ + stm32_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */ + + stm32_configgpio(GPIO_ADC1_IN13); /* FMU_AUX_ADC_1 */ + stm32_configgpio(GPIO_ADC1_IN14); /* FMU_AUX_ADC_2 */ + stm32_configgpio(GPIO_ADC1_IN15); /* PRESSURE_SENS */ + + /* configure power supply control/sense pins */ + stm32_configgpio(GPIO_PERIPH_3V3_EN); + stm32_configgpio(GPIO_VDD_BRICK_VALID); + stm32_configgpio(GPIO_GPIO5_OUTPUT); + + stm32_configgpio(GPIO_SBUS_INV); + stm32_configgpio(GPIO_8266_GPIO0); + stm32_configgpio(GPIO_SPEKTRUM_POWER); + stm32_configgpio(GPIO_8266_PD); + stm32_configgpio(GPIO_8266_RST); + + /* configure the high-resolution time/callout interface */ + hrt_init(); + + /* configure the DMA allocator */ + dma_alloc_init(); + + /* configure CPU load estimation */ +#ifdef CONFIG_SCHED_INSTRUMENTATION + cpuload_initialize_once(); +#endif + + /* set up the serial DMA polling */ + static struct hrt_call serial_dma_call; + struct timespec ts; + + /* + * Poll at 1ms intervals for received bytes that have not triggered + * a DMA event. + */ + ts.tv_sec = 0; + ts.tv_nsec = 1000000; + + hrt_call_every(&serial_dma_call, + ts_to_abstime(&ts), + ts_to_abstime(&ts), + (hrt_callout)stm32_serial_dma_poll, + NULL); + + /* initial LED state */ + drv_led_start(); + led_off(LED_RED); + led_off(LED_GREEN); + led_off(LED_BLUE); + + /* Configure SPI-based devices */ + + spi1 = up_spiinitialize(1); + + if (!spi1) { + message("[boot] FAILED to initialize SPI port 1\n"); + up_ledon(LED_RED); + return -ENODEV; + } + + /* Default SPI1 to 1MHz and de-assert the known chip selects. */ + SPI_SETFREQUENCY(spi1, 10000000); + SPI_SETBITS(spi1, 8); + SPI_SETMODE(spi1, SPIDEV_MODE3); + SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false); + SPI_SELECT(spi1, PX4_SPIDEV_HMC, false); + SPI_SELECT(spi1, PX4_SPIDEV_BARO, false); + SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); + up_udelay(20); + + /* Get the SPI port for the FRAM */ + + spi2 = up_spiinitialize(2); + + if (!spi2) { + message("[boot] FAILED to initialize SPI port 2\n"); + up_ledon(LED_RED); + return -ENODEV; + } + + /* Default SPI2 to 37.5 MHz (40 MHz rounded to nearest valid divider, F4 max) + * and de-assert the known chip selects. */ + + // XXX start with 10.4 MHz in FRAM usage and go up to 37.5 once validated + SPI_SETFREQUENCY(spi2, 12 * 1000 * 1000); + SPI_SETBITS(spi2, 8); + SPI_SETMODE(spi2, SPIDEV_MODE3); + SPI_SELECT(spi2, SPIDEV_FLASH, false); + +#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); + return -ENODEV; + } + + /* 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; + } + + /* 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 + + return OK; +} diff --git a/src/drivers/boards/px4fmu-v4/px4fmu_led.c b/src/drivers/boards/px4fmu-v4/px4fmu_led.c new file mode 100644 index 0000000000..ddb34e8a56 --- /dev/null +++ b/src/drivers/boards/px4fmu-v4/px4fmu_led.c @@ -0,0 +1,106 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4fmu2_led.c + * + * PX4FMU LED backend. + */ + +#include + +#include + +#include "stm32.h" +#include "board_config.h" + +#include + +/* + * Ideally we'd be able to get these from up_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +extern void led_toggle(int led); +__END_DECLS + + + +static uint32_t g_ledmap[] = { + GPIO_LED_BLUE, // Indexed by LED_BLUE + GPIO_LED_RED, // Indexed by LED_RED, LED_AMBER + GPIO_LED_SAFETY, // Indexed by LED_SAFETY + GPIO_LED_GREEN, // Indexed by LED_GREEN +}; + +__EXPORT void led_init(void) +{ + /* Configure LED GPIOs for output */ + for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) { + stm32_configgpio(g_ledmap[l]); + } +} + +static void phy_set_led(int led, bool state) +{ + /* Pull Down to switch on */ + stm32_gpiowrite(g_ledmap[led], !state); +} + +static bool phy_get_led(int led) +{ + + return !stm32_gpioread(g_ledmap[led]); +} + +__EXPORT void led_on(int led) +{ + phy_set_led(led, true); +} + +__EXPORT void led_off(int led) +{ + phy_set_led(led, false); +} + +__EXPORT void led_toggle(int led) +{ + + phy_set_led(led, !phy_get_led(led)); +} diff --git a/src/drivers/boards/px4fmu-v4/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmu-v4/px4fmu_pwm_servo.c new file mode 100644 index 0000000000..600dfef412 --- /dev/null +++ b/src/drivers/boards/px4fmu-v4/px4fmu_pwm_servo.c @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file px4fmu_pwm_servo.c + * + * Configuration data for the stm32 pwm_servo driver. + * + * Note that these arrays must always be fully-sized. + */ + +#include + +#include +#include +#include + +#include +#include + +#include "board_config.h" + +__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { + { + .base = STM32_TIM1_BASE, + .clock_register = STM32_RCC_APB2ENR, + .clock_bit = RCC_APB2ENR_TIM1EN, + .clock_freq = STM32_APB2_TIM1_CLKIN + }, + { + .base = STM32_TIM4_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM4EN, + .clock_freq = STM32_APB1_TIM4_CLKIN + } +}; + +__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = { + { + .gpio = GPIO_TIM1_CH4OUT, + .timer_index = 0, + .timer_channel = 4, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM1_CH3OUT, + .timer_index = 0, + .timer_channel = 3, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM1_CH2OUT, + .timer_index = 0, + .timer_channel = 2, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM1_CH1OUT, + .timer_index = 0, + .timer_channel = 1, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM4_CH2OUT, + .timer_index = 1, + .timer_channel = 2, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM4_CH3OUT, + .timer_index = 1, + .timer_channel = 3, + .default_value = 1000, + } +}; diff --git a/src/drivers/boards/px4fmu-v4/px4fmu_spi.c b/src/drivers/boards/px4fmu-v4/px4fmu_spi.c new file mode 100644 index 0000000000..bdfa47f62c --- /dev/null +++ b/src/drivers/boards/px4fmu-v4/px4fmu_spi.c @@ -0,0 +1,163 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4fmu_spi.c + * + * Board-specific SPI functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include "board_config.h" + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ************************************************************************************/ + +__EXPORT void stm32_spiinitialize(void) +{ +#ifdef CONFIG_STM32_SPI1 + stm32_configgpio(GPIO_SPI_CS_MPU9250); + stm32_configgpio(GPIO_SPI_CS_HMC5983); + stm32_configgpio(GPIO_SPI_CS_MS5611); + stm32_configgpio(GPIO_SPI_CS_ICM_20608_G); + + /* De-activate all peripherals, + * required for some peripheral + * state machines + */ + stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1); + stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1); + stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1); + stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1); + + stm32_configgpio(GPIO_DRDY_MPU9250); + stm32_configgpio(GPIO_DRDY_HMC5983); + stm32_configgpio(GPIO_DRDY_ICM_20608_G); +#endif + +#ifdef CONFIG_STM32_SPI2 + stm32_configgpio(GPIO_SPI_CS_FRAM); + stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1); +#endif + +} + +__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* SPI select is active low, so write !selected to select the device */ + + switch (devid) { + case PX4_SPIDEV_ICM: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1); + stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1); + stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1); + stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, !selected); + break; + + case PX4_SPIDEV_ACCEL_MAG: + /* Making sure the other peripherals are not selected */ + break; + + case PX4_SPIDEV_BARO: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1); + stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1); + stm32_gpiowrite(GPIO_SPI_CS_MS5611, !selected); + stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1); + break; + + case PX4_SPIDEV_HMC: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1); + stm32_gpiowrite(GPIO_SPI_CS_HMC5983, !selected); + stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1); + stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1); + break; + + case PX4_SPIDEV_MPU: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_MPU9250, !selected); + stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1); + stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1); + stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1); + break; + + default: + break; + } +} + +__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return SPI_STATUS_PRESENT; +} + + +#ifdef CONFIG_STM32_SPI2 +__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* there can only be one device on this bus, so always select it */ + stm32_gpiowrite(GPIO_SPI_CS_FRAM, !selected); +} + +__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + /* FRAM is always present */ + return SPI_STATUS_PRESENT; +} +#endif diff --git a/src/drivers/boards/px4fmu-v4/px4fmu_usb.c b/src/drivers/boards/px4fmu-v4/px4fmu_usb.c new file mode 100644 index 0000000000..e4cfc27e15 --- /dev/null +++ b/src/drivers/boards/px4fmu-v4/px4fmu_usb.c @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4fmu_usb.c + * + * Board-specific USB functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include "board_config.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_usbinitialize + * + * Description: + * Called to setup USB-related GPIO pins for the PX4FMU board. + * + ************************************************************************************/ + +__EXPORT void stm32_usbinitialize(void) +{ + /* The OTG FS has an internal soft pull-up */ + + /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ + +#ifdef CONFIG_STM32_OTGFS + stm32_configgpio(GPIO_OTGFS_VBUS); + /* XXX We only support device mode + stm32_configgpio(GPIO_OTGFS_PWRON); + stm32_configgpio(GPIO_OTGFS_OVER); + */ +#endif +} + +/************************************************************************************ + * Name: stm32_usbsuspend + * + * Description: + * Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is + * used. This function is called whenever the USB enters or leaves suspend mode. + * This is an opportunity for the board logic to shutdown clocks, power, etc. + * while the USB is suspended. + * + ************************************************************************************/ + +__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + ulldbg("resume: %d\n", resume); +} + diff --git a/src/drivers/boards/px4io-v1/board_config.h b/src/drivers/boards/px4io-v1/board_config.h index e05e030713..f83f18e503 100644 --- a/src/drivers/boards/px4io-v1/board_config.h +++ b/src/drivers/boards/px4io-v1/board_config.h @@ -65,6 +65,8 @@ #define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\ GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) +#define GPIO_USART1_RX_SPEKTRUM (GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10) + /* Safety switch button *************************************************************/ #define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5) diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h index e9b17e7dc7..d97f3b203c 100644 --- a/src/drivers/boards/px4io-v2/board_config.h +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -79,6 +79,8 @@ #define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13) #define GPIO_LED4 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN11) +#define GPIO_USART1_RX_SPEKTRUM (GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10) + /* Safety switch button *******************************************************/ #define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5) diff --git a/src/platforms/posix/drivers/adcsim/module.mk b/src/drivers/bst/CMakeLists.txt similarity index 87% rename from src/platforms/posix/drivers/adcsim/module.mk rename to src/drivers/bst/CMakeLists.txt index a8ad55effc..7c312fc2e9 100644 --- a/src/platforms/posix/drivers/adcsim/module.mk +++ b/src/drivers/bst/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# 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 @@ -30,13 +30,15 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ - -# -# STM32 ADC driver -# - -MODULE_COMMAND = adcsim - -SRCS = adcsim.cpp - -MAXOPTIMIZATION = -Os +px4_add_module( + MODULE drivers__bst + MAIN bst + STACK 1200 + COMPILE_FLAGS + -Os + SRCS + bst.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/bst/bst.cpp b/src/drivers/bst/bst.cpp new file mode 100644 index 0000000000..9a8d55ac40 --- /dev/null +++ b/src/drivers/bst/bst.cpp @@ -0,0 +1,409 @@ +/**************************************************************************** + * + * 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 bst.cpp + * + * Black Sheep Telemetry driver + * + * @author Anton Babushkin + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define BST_DEVICE_PATH "/dev/bst0" + +static const char commandline_usage[] = "usage: bst start|status|stop"; + +extern "C" __EXPORT int bst_main(int argc, char *argv[]); + +#define BST_ADDR 0x76 + +namespace px4 +{ +namespace bst +{ + +#pragma pack(push, 1) + +template +struct BSTPacket { + uint8_t length; + uint8_t type; + T payload; + uint8_t crc; +}; + +struct BSTDeviceInfoRequest { + uint8_t cmd; +}; + +struct BSTDeviceInfoReply { + uint32_t hw_id; + uint16_t fw_id; + uint8_t dev_name_len; + char dev_name[32]; +}; + +struct BSTGPSPosition { + int32_t lat; + int32_t lon; + uint16_t gs; + uint16_t heading; + uint16_t alt; + uint8_t sats; +}; + +struct BSTAttitude { + int16_t pitch; + int16_t roll; + int16_t yaw; +}; + +struct BSTBattery { + uint16_t voltage; + uint16_t current; + uint8_t capacity[3]; +}; + +#pragma pack(pop) + +class BST : public device::I2C +{ +public: + BST(int bus); + + virtual ~BST(); + + virtual int init(); + + virtual int probe(); + + virtual int info() { return 0; } + + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg) { return 0; } + + work_s *work_ptr() { return &_work; } + + void stop(); + + static void start_trampoline(void *arg); + +private: + work_s _work = {}; + bool _should_run = false; + unsigned _interval = 100; + int _gps_sub; + int _attitude_sub; + int _battery_sub; + + void start(); + + static void cycle_trampoline(void *arg); + + void cycle(); + + template + void send_packet(BSTPacket &packet) + { + packet.length = sizeof(packet) - 1; // Length + packet.crc = crc8(reinterpret_cast(&packet.type), sizeof(packet) - 2); + + transfer(reinterpret_cast(&packet), sizeof(packet), nullptr, 0); + } + + template + void send_packet(BSTPacket &packet_send, BSTPacket &packet_recv) + { + packet_send.length = sizeof(packet_send) - 1; // Length + packet_send.crc = crc8(reinterpret_cast(&packet_send.type), sizeof(packet_send) - 2); + transfer(reinterpret_cast(&packet_send), sizeof(packet_send), reinterpret_cast(&packet_recv), + sizeof(packet_recv)); + } + + static uint8_t crc8(uint8_t *data, size_t len); + + //! Byte swap unsigned short + uint16_t swap_uint16(uint16_t val) + { + return (val << 8) | (val >> 8); + } + + //! Byte swap short + int16_t swap_int16(int16_t val) + { + return (val << 8) | ((val >> 8) & 0xFF); + } + + //! Byte swap unsigned int + uint32_t swap_uint32(uint32_t val) + { + val = ((val << 8) & 0xFF00FF00) | ((val >> 8) & 0xFF00FF); + return (val << 16) | (val >> 16); + } + + //! Byte swap int + int32_t swap_int32(int32_t val) + { + val = ((val << 8) & 0xFF00FF00) | ((val >> 8) & 0xFF00FF); + return (val << 16) | ((val >> 16) & 0xFFFF); + } +}; + +static BST *g_bst = nullptr; + +BST::BST(int bus) : + I2C("bst", BST_DEVICE_PATH, bus, BST_ADDR +#ifdef __PX4_NUTTX + , 100000 /* maximum speed supported */ +#endif + ) +{ +} + +BST::~BST() +{ + _should_run = false; + + work_cancel(LPWORK, &_work); +} + +int BST::probe() +{ + int retries_prev = _retries; + _retries = 3; + + BSTPacket dev_info_req = {}; + dev_info_req.type = 0x0A; + dev_info_req.payload.cmd = 0x04; + BSTPacket dev_info_reply = {}; + send_packet(dev_info_req, dev_info_reply); + + if (dev_info_reply.type != 0x05) { + warnx("no devices found"); + return -EIO; + } + + uint8_t *reply_raw = reinterpret_cast(&dev_info_reply); + uint8_t crc_calc = crc8(reinterpret_cast(&dev_info_reply.type), dev_info_reply.length - 1); + uint8_t crc_recv = reply_raw[dev_info_reply.length]; + + if (crc_recv != crc_calc) { + warnx("CRC error: got %02x, should be %02x", (int)crc_recv, (int)crc_calc); + return -EIO; + } + + dev_info_reply.payload.dev_name[dev_info_reply.payload.dev_name_len] = '\0'; + warnx("device info: hardware ID: 0x%08X, firmware ID: 0x%04X, device name: %s", + (int)swap_uint32(dev_info_reply.payload.hw_id), (int)swap_uint16(dev_info_reply.payload.fw_id), + dev_info_reply.payload.dev_name); + + _retries = retries_prev; + + return OK; +} + +int BST::init() +{ + int ret; + ret = I2C::init(); + + if (ret != OK) { + return ret; + } + + work_queue(LPWORK, &_work, BST::start_trampoline, g_bst, 0); + + return OK; +} + +void BST::start_trampoline(void *arg) +{ + reinterpret_cast(arg)->start(); +} + +void BST::start() +{ + _should_run = true; + + _attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + _battery_sub = orb_subscribe(ORB_ID(battery_status)); + + set_address(0x00); // General call address + + work_queue(LPWORK, &_work, BST::cycle_trampoline, this, 0); +} + +void BST::cycle_trampoline(void *arg) +{ + reinterpret_cast(arg)->cycle(); +} + +void BST::cycle() +{ + if (_should_run) { + bool updated = false; + + orb_check(_attitude_sub, &updated); + + if (updated) { + vehicle_attitude_s att; + orb_copy(ORB_ID(vehicle_attitude), _attitude_sub, &att); + BSTPacket bst_att = {}; + bst_att.type = 0x1E; + bst_att.payload.roll = swap_int32(att.roll * 10000); + bst_att.payload.pitch = swap_int32(att.pitch * 10000); + bst_att.payload.yaw = swap_int32(att.yaw * 10000); + send_packet(bst_att); + } + + updated = false; + orb_check(_battery_sub, &updated); + + if (updated) { + battery_status_s batt; + orb_copy(ORB_ID(battery_status), _battery_sub, &batt); + BSTPacket bst_batt = {}; + bst_batt.type = 0x08; + bst_batt.payload.voltage = swap_uint16(batt.voltage_v * 10.0f); + bst_batt.payload.current = swap_uint16(batt.current_a * 10.0f); + uint32_t discharged = batt.discharged_mah; + bst_batt.payload.capacity[0] = static_cast(discharged >> 16); + bst_batt.payload.capacity[1] = static_cast(discharged >> 8); + bst_batt.payload.capacity[2] = static_cast(discharged); + send_packet(bst_batt); + } + + updated = false; + orb_check(_gps_sub, &updated); + + if (updated) { + vehicle_gps_position_s gps; + orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &gps); + + if (gps.fix_type >= 3 && gps.eph < 50.0f) { + BSTPacket bst_gps = {}; + bst_gps.type = 0x02; + bst_gps.payload.lat = swap_int32(gps.lat); + bst_gps.payload.lon = swap_int32(gps.lon); + bst_gps.payload.alt = swap_int16(gps.alt / 1000 + 1000); + bst_gps.payload.gs = swap_int16(gps.vel_m_s * 360.0f); + bst_gps.payload.heading = swap_int16(gps.cog_rad * 18000.0f / M_PI_F); + bst_gps.payload.sats = gps.satellites_used; + send_packet(bst_gps); + } + } + + work_queue(LPWORK, &_work, BST::cycle_trampoline, this, _interval); + } +} + +uint8_t BST::crc8(uint8_t *data, size_t len) +{ + uint8_t crc = 0x00; + + while (len--) { + crc ^= *data++; + + for (int i = 0; i < 8; i++) { + crc = crc & 0x80 ? (crc << 1) ^ 0xD5 : crc << 1; + } + } + + return crc; +} + +} +} + +using namespace px4::bst; + +int bst_main(int argc, char *argv[]) +{ + if (argc < 2) { + errx(1, "missing command\n%s", commandline_usage); + } + + if (!strcmp(argv[1], "start")) { + + if (g_bst) { + warnx("already running"); + exit(0); + } + + g_bst = new BST(PX4_I2C_BUS_EXPANSION); + + if (g_bst != nullptr && OK != g_bst->init()) { + delete g_bst; + g_bst = nullptr; + } + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + if (!g_bst) { + warnx("not running"); + exit(0); + } + + delete g_bst; + g_bst = nullptr; + warnx("stopped"); + + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (g_bst) { + warnx("is running"); + + } else { + warnx("is not running"); + } + + exit(0); + } + + errx(1, "unrecognized command\n%s", commandline_usage); +} diff --git a/src/drivers/camera_trigger/CMakeLists.txt b/src/drivers/camera_trigger/CMakeLists.txt index 4f3669876e..18f4bbde36 100644 --- a/src/drivers/camera_trigger/CMakeLists.txt +++ b/src/drivers/camera_trigger/CMakeLists.txt @@ -33,6 +33,7 @@ px4_add_module( MODULE drivers__camera_trigger MAIN camera_trigger + STACK 1200 COMPILE_FLAGS -Os SRCS diff --git a/src/drivers/device/device_posix.cpp b/src/drivers/device/device_posix.cpp index 088d7ccdf8..cd3bad09ce 100644 --- a/src/drivers/device/device_posix.cpp +++ b/src/drivers/device/device_posix.cpp @@ -96,7 +96,7 @@ Device::dev_write(unsigned offset, void *data, unsigned count) } int -Device::dev_ioctl(unsigned operation, unsigned &arg) +Device::dev_ioctl(unsigned operation, unsigned arg) { switch (operation) { case DEVIOCGDEVICEID: diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 4aa58447f3..cdf01d0ea5 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -45,6 +45,10 @@ #include #include +#include "DevMgr.hpp" + +using namespace DriverFramework; + namespace device { @@ -363,7 +367,6 @@ VDev::ioctl(file_t *filep, int cmd, unsigned long arg) case DEVIOCGDEVICEID: ret = (int)_device_id.devid; - PX4_INFO("IOCTL DEVIOCGDEVICEID %d", ret); break; default: @@ -533,7 +536,7 @@ VDev *VDev::getDev(const char *path) void VDev::showDevices() { int i = 0; - PX4_INFO("Devices:"); + PX4_INFO("PX4 Devices:"); pthread_mutex_lock(&devmutex); @@ -544,6 +547,23 @@ void VDev::showDevices() } pthread_mutex_unlock(&devmutex); + +#ifndef __PX4_UNIT_TESTS + PX4_INFO("DF Devices:"); + const char *dev_path; + unsigned int index = 0; + i = 0; + + do { + // Each look increments index and returns -1 if end reached + i = DevMgr::getNextDeviceName(index, &dev_path); + + if (i == 0) { + PX4_INFO(" %s", dev_path); + } + } while (i == 0); + +#endif } void VDev::showTopics() @@ -599,4 +619,4 @@ const char *VDev::devList(unsigned int *next) return NULL; } -} // namespace device \ No newline at end of file +} // namespace device diff --git a/src/drivers/device/vdev.h b/src/drivers/device/vdev.h index d1fb8ff8e1..b15fd8209f 100644 --- a/src/drivers/device/vdev.h +++ b/src/drivers/device/vdev.h @@ -128,7 +128,7 @@ public: * @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); + virtual int dev_ioctl(unsigned operation, unsigned arg); /* device bus types for DEVID @@ -181,7 +181,7 @@ protected: */ void lock() { - DEVICE_DEBUG("lock"); + //DEVICE_DEBUG("lock"); do {} while (px4_sem_wait(&_lock) != 0); } @@ -190,7 +190,7 @@ protected: */ void unlock() { - DEVICE_DEBUG("unlock"); + //DEVICE_DEBUG("unlock"); px4_sem_post(&_lock); } diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index c5e1c9a4f5..d99c7cfc74 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -56,13 +56,6 @@ pthread_mutex_t filemutex = PTHREAD_MUTEX_INITIALIZER; 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"); - } - #define PX4_MAX_FD 200 static device::file_t *filemap[PX4_MAX_FD] = {}; @@ -106,7 +99,7 @@ extern "C" { strncmp(path, "/dev/", 5) != 0) { va_list p; va_start(p, flags); - mode = va_arg(p, mode_t); + mode = va_arg(p, int); va_end(p); // Create the file @@ -240,15 +233,30 @@ extern "C" { int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) { + if (nfds == 0) { + PX4_WARN("px4_poll with no fds"); + return -1; + } + px4_sem_t sem; int count = 0; int ret = -1; unsigned int i; + const unsigned NAMELEN = 32; + char thread_name[NAMELEN] = {}; + int nret = pthread_getname_np(pthread_self(), thread_name, NAMELEN); + + if (nret || thread_name[0] == 0) { + PX4_WARN("failed getting thread name"); + } + PX4_DEBUG("Called px4_poll timeout = %d", timeout); px4_sem_init(&sem, 0, 0); - // For each fd + // Go through all fds and check them for a pollable state + bool fd_pollable = false; + for (i = 0; i < nfds; ++i) { fds[i].sem = &sem; fds[i].revents = 0; @@ -258,42 +266,71 @@ extern "C" { // If fd is valid if (dev) { - PX4_DEBUG("px4_poll: VDev->poll(setup) %d", fds[i].fd); + PX4_DEBUG("%s: px4_poll: VDev->poll(setup) %d", thread_name, fds[i].fd); ret = dev->poll(filemap[fds[i].fd], &fds[i], true); if (ret < 0) { + PX4_WARN("%s: px4_poll() error: %s", + thread_name, strerror(errno)); break; } + + if (ret >= 0) { + fd_pollable = true; + } } } - if (ret >= 0) { + // If any FD can be polled, lock the semaphore and + // check for new data + if (fd_pollable) { 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); + // Get the current time + struct timespec ts; + px4_clock_gettime(CLOCK_REALTIME, &ts); - // Make sure timer thread is killed before sem goes - // out of scope - hrt_work_cancel(&_hpwork); + // Calculate an absolute time in the future + const unsigned billion = (1000 * 1000 * 1000); + unsigned tdiff = timeout; + uint64_t nsecs = ts.tv_nsec + (tdiff * 1000 * 1000); + ts.tv_sec += nsecs / billion; + nsecs -= (nsecs / billion) * billion; + ts.tv_nsec = nsecs; + + // Execute a blocking wait for that time in the future + errno = 0; + ret = px4_sem_timedwait(&sem, &ts); +#ifndef __PX4_DARWIN + ret = errno; +#endif + + // Ensure ret is negative on failure + if (ret > 0) { + ret = -ret; + } + + if (ret && ret != -ETIMEDOUT) { + PX4_WARN("%s: px4_poll() sem error", thread_name); + } } else if (timeout < 0) { px4_sem_wait(&sem); } - // For each fd + // We have waited now (or not, depending on timeout), + // go through all fds and count how many have data for (i = 0; i < nfds; ++i) { VDev *dev = get_vdev(fds[i].fd); // If fd is valid if (dev) { - PX4_DEBUG("px4_poll: VDev->poll(teardown) %d", fds[i].fd); + PX4_DEBUG("%s: px4_poll: VDev->poll(teardown) %d", thread_name, fds[i].fd); ret = dev->poll(filemap[fds[i].fd], &fds[i], false); if (ret < 0) { + PX4_WARN("%s: px4_poll() 2nd poll fail", thread_name); break; } @@ -306,7 +343,9 @@ extern "C" { px4_sem_destroy(&sem); - return count; + // Return the positive count if present, + // return the negative error number if failed + return (count) ? count : ret; } int px4_fsync(int fd) diff --git a/src/drivers/drv_device.h b/src/drivers/drv_device.h index 02dbe7af67..411c30f51f 100644 --- a/src/drivers/drv_device.h +++ b/src/drivers/drv_device.h @@ -46,24 +46,7 @@ #include "drv_sensor.h" #include "drv_orb_dev.h" -/* - * ioctl() definitions - */ - -#define _DEVICEIOCBASE (0x100) -#define _DEVICEIOC(_n) (_PX4_IOC(_DEVICEIOCBASE, _n)) - -/** ask device to stop publishing */ -#define DEVIOCSPUBBLOCK _DEVICEIOC(0) - -/** check publication block status */ -#define DEVIOCGPUBBLOCK _DEVICEIOC(1) - -/** - * Return device ID, to enable matching of configuration parameters - * (such as compass offsets) to specific sensors - */ -#define DEVIOCGDEVICEID _DEVICEIOC(2) +#include "DevIOCTL.h" #ifdef __PX4_POSIX diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h index acddadfdbe..971c22b2d4 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -94,6 +94,29 @@ #endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V4 +/* + * PX4FMUv3 GPIO numbers. + * + * There are no alternate functions on this board. + */ +# define GPIO_SERVO_1 (1<<0) /**< servo 1 output */ +# define GPIO_SERVO_2 (1<<1) /**< servo 2 output */ +# define GPIO_SERVO_3 (1<<2) /**< servo 3 output */ +# define GPIO_SERVO_4 (1<<3) /**< servo 4 output */ +# define GPIO_SERVO_5 (1<<4) /**< servo 5 output */ +# define GPIO_SERVO_6 (1<<5) /**< servo 6 output */ + +# define GPIO_3V3_SENSORS_EN (1<<7) /**< PE3 - VDD_3V3_SENSORS_EN */ +# define GPIO_BRICK_VALID (1<<8) /**< PB5 - !VDD_BRICK_VALID */ + +/** + * Device paths for things that support the GPIO ioctl protocol. + */ +# define PX4FMU_DEVICE_PATH "/dev/px4fmu" + +#endif + #ifdef CONFIG_ARCH_BOARD_AEROCORE /* * AeroCore GPIO numbers and configuration. @@ -121,7 +144,7 @@ #if !defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2) && \ !defined(CONFIG_ARCH_BOARD_PX4FMU_V1) && !defined(CONFIG_ARCH_BOARD_PX4FMU_V2) && \ !defined(CONFIG_ARCH_BOARD_AEROCORE) && !defined(CONFIG_ARCH_BOARD_PX4_STM32F4DISCOVERY) && \ - !defined(CONFIG_ARCH_BOARD_SITL) + !defined(CONFIG_ARCH_BOARD_PX4FMU_V4) && !defined(CONFIG_ARCH_BOARD_SITL) # error No CONFIG_ARCH_BOARD_xxxx set #endif /* diff --git a/src/drivers/drv_led.h b/src/drivers/drv_led.h index dbcfde91fa..3e191d20a6 100644 --- a/src/drivers/drv_led.h +++ b/src/drivers/drv_led.h @@ -43,6 +43,7 @@ #include #include +#define LED_BASE_DEVICE_PATH "/dev/led" #define LED0_DEVICE_PATH "/dev/led0" #define _LED_BASE 0x2800 @@ -52,6 +53,8 @@ #define LED_RED 1 /* some boards have red rather than amber */ #define LED_BLUE 0 #define LED_SAFETY 2 +#define LED_GREEN 3 + #define LED_ON _PX4_IOC(_LED_BASE, 0) #define LED_OFF _PX4_IOC(_LED_BASE, 1) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index bb04245333..06d1f10d47 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -76,7 +76,12 @@ __BEGIN_DECLS /** * Lowest minimum PWM in us */ -#define PWM_LOWEST_MIN 900 +#define PWM_LOWEST_MIN 90 + +/** + * Default value for a shutdown motor + */ +#define PWM_MOTOR_OFF 800 /** * Default minimum PWM in us @@ -101,7 +106,7 @@ __BEGIN_DECLS /** * Lowest PWM allowed as the maximum PWM */ -#define PWM_LOWEST_MAX 950 +#define PWM_LOWEST_MAX 200 /** * Do not output a channel with this value diff --git a/src/drivers/drv_rgbled.h b/src/drivers/drv_rgbled.h index 4e3de36abd..2194f8ed97 100644 --- a/src/drivers/drv_rgbled.h +++ b/src/drivers/drv_rgbled.h @@ -42,6 +42,8 @@ #include #include +#define RGBLED_BASE_DEVICE_PATH "/dev/rgbled" + /* more devices will be 1, 2, etc */ #define RGBLED0_DEVICE_PATH "/dev/rgbled0" diff --git a/src/drivers/gimbal/CMakeLists.txt b/src/drivers/gimbal/CMakeLists.txt index ac26a1876e..4a13480cd1 100644 --- a/src/drivers/gimbal/CMakeLists.txt +++ b/src/drivers/gimbal/CMakeLists.txt @@ -37,6 +37,7 @@ px4_add_module( -Os SRCS gimbal.cpp + gimbal_params.c DEPENDS platforms__common ) diff --git a/src/drivers/gimbal/gimbal.cpp b/src/drivers/gimbal/gimbal.cpp index 40b86d1421..bb76d1229b 100644 --- a/src/drivers/gimbal/gimbal.cpp +++ b/src/drivers/gimbal/gimbal.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. + * 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 @@ -35,6 +35,7 @@ * @file gimbal.cpp * @author Lorenz Meier * @author Anton Matosov + * @author Andreas Antener * * Driver to control a gimbal - relies on input via telemetry or RC * and output via the standardized control group #2 and a mixer. @@ -72,6 +73,8 @@ #include #include #include +#include +#include #include #include @@ -120,6 +123,9 @@ private: work_s _work; int _vehicle_command_sub; int _att_sub; + int _vehicle_control_mode_sub; + int _manual_control_sub; + int _params_sub; bool _attitude_compensation_roll; bool _attitude_compensation_pitch; @@ -127,6 +133,7 @@ private: bool _initialized; bool _control_cmd_set; bool _config_cmd_set; + int _mount_mode; // see MAV_MOUNT_MODE orb_advert_t _actuator_controls_2_topic; @@ -136,6 +143,18 @@ private: struct vehicle_command_s _control_cmd; struct vehicle_command_s _config_cmd; + struct manual_control_setpoint_s _manual_control; + struct vehicle_control_mode_s _control_mode; + + struct { + int aux_mnt_chn; + int use_mnt; + } _parameters; + + struct { + param_t aux_mnt_chn; + param_t use_mnt; + } _params_handles; /** * Initialise the automatic measurement state machine and start it. @@ -150,6 +169,11 @@ private: */ void stop(); + /** + * Update parameters + */ + void update_params(); + /** * Perform a poll cycle; collect from the previous measurement * and start a new one. @@ -176,6 +200,9 @@ Gimbal::Gimbal() : CDev("Gimbal", GIMBAL_DEVICE_PATH), _vehicle_command_sub(-1), _att_sub(-1), + _vehicle_control_mode_sub(-1), + _manual_control_sub(-1), + _params_sub(-1), _attitude_compensation_roll(true), _attitude_compensation_pitch(true), _attitude_compensation_yaw(true), @@ -188,6 +215,10 @@ Gimbal::Gimbal() : // disable debug() calls _debug_enabled = false; + _params_handles.aux_mnt_chn = param_find("GMB_AUX_MNT_CHN"); + _params_handles.use_mnt = param_find("GMB_USE_MNT"); + update_params(); + // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); } @@ -267,12 +298,23 @@ Gimbal::cycle_trampoline(void *arg) dev->cycle(); } +void +Gimbal::update_params() +{ + param_get(_params_handles.aux_mnt_chn, &_parameters.aux_mnt_chn); + param_get(_params_handles.use_mnt, &_parameters.use_mnt); +} + void Gimbal::cycle() { if (!_initialized) { - /* get a subscription handle on the vehicle command topic */ + /* get subscription handles */ _vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command)); + _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); /* get a publication handle on actuator output topic */ struct actuator_controls_s zero_report; @@ -284,6 +326,8 @@ Gimbal::cycle() warnx("advert err"); } + _mount_mode = vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT; + _initialized = true; } @@ -294,13 +338,31 @@ Gimbal::cycle() float roll = 0.0f; float pitch = 0.0f; float yaw = 0.0f; + float out_mount_mode = 0.0f; + /* Parameter update */ + bool params_updated; - if (_att_sub < 0) { - _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + orb_check(_params_sub, ¶ms_updated); + + if (params_updated) { + struct parameter_update_s param_update; + orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update); // XXX: is this actually necessary? + + update_params(); } - vehicle_attitude_s att; + /* Control mode update */ + bool vehicle_control_mode_updated; + + orb_check(_vehicle_control_mode_sub, &vehicle_control_mode_updated); + + if (vehicle_control_mode_updated) { + orb_copy(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_control_mode); + } + + /* Check attitude */ + struct vehicle_attitude_s att; orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att); @@ -319,7 +381,45 @@ Gimbal::cycle() updated = true; } + /* Check manual input */ + bool manual_updated; + orb_check(_manual_control_sub, &manual_updated); + + if (manual_updated) { + orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &_manual_control); + + /* only check manual input for mount mode when not in offboard and aux chan is configured */ + if (!_control_mode.flag_control_offboard_enabled && _parameters.aux_mnt_chn > 0) { + float aux = 0.0f; + + switch (_parameters.aux_mnt_chn) { + case 1: + aux = _manual_control.aux1; + break; + + case 2: + aux = _manual_control.aux2; + break; + + case 3: + aux = _manual_control.aux3; + break; + } + + if (aux < 0.0f && _mount_mode != vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT) { + _mount_mode = vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT; + updated = true; + } + + if (aux > 0.0f && _mount_mode != vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL) { + _mount_mode = vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL; + updated = true; + } + } + } + + /* Check command input */ struct vehicle_command_s cmd; bool cmd_updated; @@ -353,6 +453,13 @@ Gimbal::cycle() _attitude_compensation_pitch = (int)_config_cmd.param3 == 1; _attitude_compensation_yaw = (int)_config_cmd.param4 == 1; + /* only check commanded mount mode when in offboard */ + if (_control_mode.flag_control_offboard_enabled && + fabsf(_config_cmd.param1 - _mount_mode) > FLT_EPSILON) { + _mount_mode = int(_config_cmd.param1 + 0.5f); + updated = true; + } + } if (_control_cmd_set) { @@ -385,6 +492,28 @@ Gimbal::cycle() } + /* consider mount mode if parameter is set */ + if (_parameters.use_mnt > 0) { + switch (_mount_mode) { + case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT: + out_mount_mode = -1.0f; + roll = 0.0f; + pitch = 0.0f; + yaw = 0.0f; + break; + + case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL: + case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING: + case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING: + case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT: + out_mount_mode = 1.0f; + break; + + default: + out_mount_mode = -1.0f; + } + } + if (updated) { struct actuator_controls_s controls; @@ -396,6 +525,8 @@ Gimbal::cycle() controls.control[0] = roll; controls.control[1] = pitch; controls.control[2] = yaw; + //controls.control[3] = ; // camera shutter + controls.control[4] = out_mount_mode; /* publish it */ orb_publish(ORB_ID(actuator_controls_2), _actuator_controls_2_topic, &controls); diff --git a/src/drivers/gimbal/gimbal_params.c b/src/drivers/gimbal/gimbal_params.c new file mode 100644 index 0000000000..955bfa8b23 --- /dev/null +++ b/src/drivers/gimbal/gimbal_params.c @@ -0,0 +1,71 @@ +/**************************************************************************** + * + * 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 gimbal_params.c + * + * Parameters for the gimbal controller. + * + * @author Andreas Antener + */ + +#include + +#include + +/** + * Consider mount operation mode. + * + * If set to 1, mount mode will be enforced. + * + * @min 0 + * @max 1 + * @group Gimbal + */ +PARAM_DEFINE_INT32(GMB_USE_MNT, 0); + +/** + * Auxilary switch to set mount operation mode. + * + * Set to 0 to disable manual mode control. + * + * If set to an auxilary switch: + * Switch off means the gimbal is put into safe/locked position. + * Switch on means the gimbal can move freely, and landing gear + * will be retracted if applicable. + * + * @min 0 + * @max 3 + * @group Gimbal + */ +PARAM_DEFINE_INT32(GMB_AUX_MNT_CHN, 0); diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 643b339be3..f6c3fd7882 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -874,6 +874,7 @@ UBX::payload_rx_done(void) _gps_position->alt = _buf.payload_rx_nav_posllh.hMSL; _gps_position->eph = (float)_buf.payload_rx_nav_posllh.hAcc * 1e-3f; // from mm to m _gps_position->epv = (float)_buf.payload_rx_nav_posllh.vAcc * 1e-3f; // from mm to m + _gps_position->alt_ellipsoid = _buf.payload_rx_nav_posllh.height; _gps_position->timestamp_position = hrt_absolute_time(); diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index 8591c4dee3..5e50d34934 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -97,7 +97,7 @@ 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_EXPANSION ? g_dev_ext : g_dev_int); if (g_dev == nullptr) { errx(1, "i2c driver not running"); @@ -231,11 +231,15 @@ void start(const bool use_i2c, const int bus) fail: +#ifdef PX4_I2C_BUS_ONBOARD + if (g_dev_int != nullptr && (bus == -1 || bus == PX4_I2C_BUS_ONBOARD)) { delete g_dev_int; g_dev_int = nullptr; } +#endif + if (g_dev_ext != nullptr && (bus == -1 || bus == PX4_I2C_BUS_EXPANSION)) { delete g_dev_ext; g_dev_ext = nullptr; @@ -255,17 +259,17 @@ fail: void stop(const bool use_i2c, const int bus) { if (use_i2c) { - if (bus == PX4_I2C_BUS_ONBOARD) { - if (g_dev_int != nullptr) { - delete g_dev_int; - g_dev_int = nullptr; - } - - } else { + if (bus == PX4_I2C_BUS_EXPANSION) { if (g_dev_ext != nullptr) { delete g_dev_ext; g_dev_ext = nullptr; } + + } else { + if (g_dev_int != nullptr) { + delete g_dev_int; + g_dev_int = nullptr; + } } } else { @@ -293,7 +297,7 @@ test(const bool use_i2c, const int bus) const char *path; if (use_i2c) { - path = ((bus == PX4_I2C_BUS_ONBOARD) ? LL40LS_DEVICE_PATH_INT : LL40LS_DEVICE_PATH_EXT); + path = ((bus == PX4_I2C_BUS_EXPANSION) ? LL40LS_DEVICE_PATH_EXT : LL40LS_DEVICE_PATH_INT); } else { path = LL40LS_DEVICE_PATH_PWM; @@ -366,7 +370,7 @@ reset(const bool use_i2c, const int bus) const char *path; if (use_i2c) { - path = ((bus == PX4_I2C_BUS_ONBOARD) ? LL40LS_DEVICE_PATH_INT : LL40LS_DEVICE_PATH_EXT); + path = ((bus == PX4_I2C_BUS_EXPANSION) ? LL40LS_DEVICE_PATH_EXT : LL40LS_DEVICE_PATH_INT); } else { path = LL40LS_DEVICE_PATH_PWM; diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index ae77a07fd0..eaf68a3191 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -335,7 +335,7 @@ MEASAirspeed::cycle() void MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) { -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) if (_t_system_power == -1) { _t_system_power = orb_subscribe(ORB_ID(system_power)); @@ -389,7 +389,7 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) } temperature -= voltage_diff * temp_slope; -#endif // CONFIG_ARCH_BOARD_PX4FMU_V2 +#endif // defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) } /** diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index ab2eede349..1d948bb7aa 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -149,6 +149,21 @@ #define BIT_INT_STATUS_DATA 0x01 #define MPU_WHOAMI_6000 0x68 +#define ICM_WHOAMI_20608 0xaf + +// ICM2608 specific registers + +/* this is an undocumented register which + if set incorrectly results in getting a 2.7m/s/s offset + on the Y axis of the accelerometer +*/ +#define MPUREG_ICM_UNDOC1 0x11 +#define MPUREG_ICM_UNDOC1_VALUE 0xc9 + +// Product ID Description for ICM2608 +// There is none + +#define ICM20608_REV_00 0 // Product ID Description for MPU6000 // high 4 bits low 4 bits @@ -208,7 +223,8 @@ class MPU6000_gyro; class MPU6000 : public device::SPI { public: - MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation); + MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation, + int device_type); virtual ~MPU6000(); virtual int init(); @@ -242,6 +258,7 @@ protected: virtual int gyro_ioctl(struct file *filp, int cmd, unsigned long arg); private: + int _device_type; MPU6000_gyro *_gyro; uint8_t _product; /** product code */ @@ -272,7 +289,6 @@ private: perf_counter_t _good_transfers; perf_counter_t _reset_retries; perf_counter_t _duplicates; - perf_counter_t _system_latency_perf; perf_counter_t _controller_latency_perf; uint8_t _register_wait; @@ -293,7 +309,7 @@ private: // this is used to support runtime checking of key // configuration registers to detect SPI bus errors and sensor // reset -#define MPU6000_NUM_CHECKED_REGISTERS 9 +#define MPU6000_NUM_CHECKED_REGISTERS 10 static const uint8_t _checked_registers[MPU6000_NUM_CHECKED_REGISTERS]; uint8_t _checked_values[MPU6000_NUM_CHECKED_REGISTERS]; uint8_t _checked_next; @@ -326,6 +342,15 @@ private: */ int reset(); + /** + * is_icm_device + */ + bool is_icm_device() { return _device_type == 20608;} + /** + * is_mpu_device + */ + bool is_mpu_device() { return _device_type == 6000;} + /** * Static trampoline from the hrt_call context; because we don't have a * generic hrt wrapper yet. @@ -469,7 +494,8 @@ const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPU MPUREG_GYRO_CONFIG, MPUREG_ACCEL_CONFIG, MPUREG_INT_ENABLE, - MPUREG_INT_PIN_CFG + MPUREG_INT_PIN_CFG, + MPUREG_ICM_UNDOC1 }; @@ -507,8 +533,10 @@ private: /** driver 'main' command */ extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); } -MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation) : +MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation, + int device_type) : SPI("MPU6000", path_accel, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED), + _device_type(device_type), _gyro(new MPU6000_gyro(this, path_gyro)), _product(0), _call{}, @@ -533,7 +561,6 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _good_transfers(perf_alloc(PC_COUNT, "mpu6000_good_transfers")), _reset_retries(perf_alloc(PC_COUNT, "mpu6000_reset_retries")), _duplicates(perf_alloc(PC_COUNT, "mpu6000_duplicates")), - _system_latency_perf(perf_alloc_once(PC_ELAPSED, "sys_latency")), _controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")), _register_wait(0), _reset_wait(0), @@ -770,6 +797,10 @@ int MPU6000::reset() write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read usleep(1000); + if (is_icm_device()) { + write_checked_reg(MPUREG_ICM_UNDOC1, MPUREG_ICM_UNDOC1_VALUE); + } + // Oscillator set // write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ); usleep(1000); @@ -780,16 +811,15 @@ int MPU6000::reset() int MPU6000::probe() { - uint8_t whoami; - whoami = read_reg(MPUREG_WHOAMI); + uint8_t whoami = read_reg(MPUREG_WHOAMI); + uint8_t expected = is_mpu_device() ? MPU_WHOAMI_6000 : ICM_WHOAMI_20608; - if (whoami != MPU_WHOAMI_6000) { + if (whoami != expected) { DEVICE_DEBUG("unexpected WHOAMI 0x%02x", whoami); return -EIO; - } - /* look for a product ID we recognise */ + /* look for a product ID we recognize */ _product = read_reg(MPUREG_PRODUCT_ID); // verify product revision @@ -806,6 +836,7 @@ MPU6000::probe() case MPU6000_REV_D8: case MPU6000_REV_D9: case MPU6000_REV_D10: + case ICM20608_REV_00: DEVICE_DEBUG("ID 0x%02x", _product); _checked_values[0] = _product; return OK; @@ -1485,6 +1516,13 @@ MPU6000::read_reg(unsigned reg, uint32_t speed) { uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0}; + // There is no MPUREG_PRODUCT_ID on the icm device + // so lets make dummy it up and allow the rest of the + // code to run as is + if (reg == MPUREG_PRODUCT_ID && is_icm_device()) { + return ICM20608_REV_00; + } + // general register transfer at low clock speed set_frequency(speed); @@ -1643,6 +1681,11 @@ MPU6000::check_registers(void) */ uint8_t v; + // the MPUREG_ICM_UNDOC1 is specific to the ICM20608 (and undocumented) + if (_checked_registers[_checked_next] == MPUREG_ICM_UNDOC1 && !is_icm_device()) { + _checked_next = (_checked_next + 1) % MPU6000_NUM_CHECKED_REGISTERS; + } + if ((v = read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) != _checked_values[_checked_next]) { /* @@ -1870,7 +1913,12 @@ MPU6000::measure() arb.scaling = _accel_range_scale; arb.range_m_s2 = _accel_range_m_s2; - _last_temperature = (report.temp) / 361.0f + 35.0f; + if (is_icm_device()) { // if it is an ICM20608 + _last_temperature = (report.temp) / 326.8f + 25.0f; + + } else { // If it is an MPU6000 + _last_temperature = (report.temp) / 361.0f + 35.0f; + } arb.temperature_raw = report.temp; arb.temperature = _last_temperature; @@ -1923,7 +1971,6 @@ MPU6000::measure() if (accel_notify && !(_pub_blocked)) { /* log the time of this report */ perf_begin(_controller_latency_perf); - perf_begin(_system_latency_perf); /* publish it */ orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); } @@ -2054,7 +2101,7 @@ namespace mpu6000 MPU6000 *g_dev_int; // on internal bus MPU6000 *g_dev_ext; // on external bus -void start(bool, enum Rotation, int range); +void start(bool, enum Rotation, int range, int device_type); void stop(bool); void test(bool); void reset(bool); @@ -2071,7 +2118,7 @@ void usage(); * or failed to detect the sensor. */ void -start(bool external_bus, enum Rotation rotation, int range) +start(bool external_bus, enum Rotation rotation, int range, int device_type) { int fd; MPU6000 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; @@ -2087,13 +2134,23 @@ start(bool external_bus, enum Rotation rotation, int range) /* create the driver */ 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); +# if defined(PX4_SPIDEV_EXT_ICM) + spi_dev_e cs = (spi_dev_e)(device_type == 6000 ? PX4_SPIDEV_EXT_MPU : PX4_SPIDEV_EXT_ICM); +# else + spi_dev_e cs = (spi_dev_e) PX4_SPIDEV_EXT_MPU; +# endif + *g_dev_ptr = new MPU6000(PX4_SPI_BUS_EXT, path_accel, path_gyro, cs, rotation, device_type); #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 defined(PX4_SPIDEV_ICM) + spi_dev_e cs = (spi_dev_e)(device_type == 6000 ? PX4_SPIDEV_MPU : PX4_SPIDEV_ICM); +#else + spi_dev_e cs = (spi_dev_e) PX4_SPIDEV_MPU; +#endif + *g_dev_ptr = new MPU6000(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, cs, rotation, device_type); } if (*g_dev_ptr == nullptr) { @@ -2337,6 +2394,7 @@ usage() warnx("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump', 'factorytest', 'testerror'"); warnx("options:"); warnx(" -X (external bus)"); + warnx(" -M 6000|20608 (default 6000)"); warnx(" -R rotation"); warnx(" -a accel range (in g)"); } @@ -2347,17 +2405,22 @@ int mpu6000_main(int argc, char *argv[]) { bool external_bus = false; + int device_type = 6000; int ch; enum Rotation rotation = ROTATION_NONE; int accel_range = 8; /* jump over start/off/etc and look at options first */ - while ((ch = getopt(argc, argv, "XR:a:")) != EOF) { + while ((ch = getopt(argc, argv, "T:XR:a:")) != EOF) { switch (ch) { case 'X': external_bus = true; break; + case 'T': + device_type = atoi(optarg); + break; + case 'R': rotation = (enum Rotation)atoi(optarg); break; @@ -2379,7 +2442,7 @@ mpu6000_main(int argc, char *argv[]) */ if (!strcmp(verb, "start")) { - mpu6000::start(external_bus, rotation, accel_range); + mpu6000::start(external_bus, rotation, accel_range, device_type); } if (!strcmp(verb, "stop")) { diff --git a/src/drivers/mpu9250/mpu9250.cpp b/src/drivers/mpu9250/mpu9250.cpp index bff57865c8..d34b8ba651 100644 --- a/src/drivers/mpu9250/mpu9250.cpp +++ b/src/drivers/mpu9250/mpu9250.cpp @@ -70,6 +70,7 @@ #include #include +#include #include #include #include @@ -173,12 +174,16 @@ #define MPU_WHOAMI_9250 0x71 -#define MPU9250_DEFAULT_ONCHIP_FILTER_FREQ 41 #define MPU9250_ACCEL_DEFAULT_RATE 1000 +#define MPU9250_ACCEL_MAX_OUTPUT_RATE 280 #define MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 #define MPU9250_GYRO_DEFAULT_RATE 1000 +/* rates need to be the same between accel and gyro */ +#define MPU9250_GYRO_MAX_OUTPUT_RATE MPU9250_ACCEL_MAX_OUTPUT_RATE #define MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30 +#define MPU9250_DEFAULT_ONCHIP_FILTER_FREQ 41 + #define MPU9250_ONE_G 9.80665f #ifdef PX4_SPI_BUS_EXT @@ -267,7 +272,6 @@ private: perf_counter_t _good_transfers; perf_counter_t _reset_retries; perf_counter_t _duplicates; - perf_counter_t _system_latency_perf; perf_counter_t _controller_latency_perf; uint8_t _register_wait; @@ -280,6 +284,9 @@ private: math::LowPassFilter2p _gyro_filter_y; math::LowPassFilter2p _gyro_filter_z; + Integrator _accel_int; + Integrator _gyro_int; + enum Rotation _rotation; // this is used to support runtime checking of key @@ -525,7 +532,6 @@ MPU9250::MPU9250(int bus, const char *path_accel, const char *path_gyro, spi_dev _good_transfers(perf_alloc(PC_COUNT, "mpu9250_good_transfers")), _reset_retries(perf_alloc(PC_COUNT, "mpu9250_reset_retries")), _duplicates(perf_alloc(PC_COUNT, "mpu9250_duplicates")), - _system_latency_perf(perf_alloc_once(PC_ELAPSED, "sys_latency")), _controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")), _register_wait(0), _reset_wait(0), @@ -535,6 +541,8 @@ MPU9250::MPU9250(int bus, const char *path_accel, const char *path_gyro, spi_dev _gyro_filter_x(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _gyro_filter_y(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _gyro_filter_z(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ), + _accel_int(1000000 / MPU9250_ACCEL_MAX_OUTPUT_RATE), + _gyro_int(1000000 / MPU9250_GYRO_MAX_OUTPUT_RATE, true), _rotation(rotation), _checked_next(0), _last_temperature(0), @@ -667,7 +675,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 - 1 : ORB_PRIO_HIGH - 1); if (_accel_topic == nullptr) { warnx("ADVERT FAIL"); @@ -679,7 +687,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 - 1 : ORB_PRIO_HIGH - 1); if (_gyro->_gyro_topic == nullptr) { warnx("ADVERT FAIL"); @@ -1669,6 +1677,14 @@ MPU9250::measure() arb.y = _accel_filter_y.apply(y_in_new); arb.z = _accel_filter_z.apply(z_in_new); + math::Vector<3> aval(x_in_new, y_in_new, z_in_new); + math::Vector<3> aval_integrated; + + bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt); + arb.x_integral = aval_integrated(0); + arb.y_integral = aval_integrated(1); + arb.z_integral = aval_integrated(2); + arb.scaling = _accel_range_scale; arb.range_m_s2 = _accel_range_m_s2; @@ -1696,6 +1712,14 @@ MPU9250::measure() grb.y = _gyro_filter_y.apply(y_gyro_in_new); grb.z = _gyro_filter_z.apply(z_gyro_in_new); + math::Vector<3> gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new); + math::Vector<3> gval_integrated; + + bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt); + grb.x_integral = gval_integrated(0); + grb.y_integral = gval_integrated(1); + grb.z_integral = gval_integrated(2); + grb.scaling = _gyro_range_scale; grb.range_rad_s = _gyro_range_rad_s; @@ -1706,18 +1730,22 @@ MPU9250::measure() _gyro_reports->force(&grb); /* notify anyone waiting for data */ - poll_notify(POLLIN); - _gyro->parent_poll_notify(); + if (accel_notify) { + poll_notify(POLLIN); + } - if (!(_pub_blocked)) { + if (gyro_notify) { + _gyro->parent_poll_notify(); + } + + if (accel_notify && !(_pub_blocked)) { /* log the time of this report */ perf_begin(_controller_latency_perf); - perf_begin(_system_latency_perf); /* publish it */ orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); } - if (!(_pub_blocked)) { + if (gyro_notify && !(_pub_blocked)) { /* publish it */ orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); } @@ -2012,6 +2040,13 @@ test(bool external_bus) warnx("temp: \t%8.4f\tdeg celsius", (double)a_report.temperature); 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) { + err(1, "reset to default polling"); + } + + close(fd); + close(fd_gyro); /* XXX add poll-rate tests here too */ diff --git a/src/drivers/pwm_input/pwm_input.cpp b/src/drivers/pwm_input/pwm_input.cpp index 4d73f6204d..1a609a91f0 100644 --- a/src/drivers/pwm_input/pwm_input.cpp +++ b/src/drivers/pwm_input/pwm_input.cpp @@ -599,6 +599,10 @@ static void pwmin_test(void) (unsigned)buf.period, (unsigned)buf.pulse_width, (unsigned)buf.error_count); + + } else { + /* no data, retry in 2 ms */ + ::usleep(2000); } } diff --git a/src/drivers/pwm_out_sim/pwm_out_sim.cpp b/src/drivers/pwm_out_sim/pwm_out_sim.cpp index 993958a050..b67ba077df 100644 --- a/src/drivers/pwm_out_sim/pwm_out_sim.cpp +++ b/src/drivers/pwm_out_sim/pwm_out_sim.cpp @@ -187,6 +187,7 @@ PWMSim::PWMSim() : _update_rate(50), _current_update_rate(0), _control_subs { -1}, + _poll_fds{}, _poll_fds_num(0), _armed_sub(-1), _outputs_pub(0), @@ -366,7 +367,6 @@ PWMSim::subscribe() } if (_control_subs[i] > 0) { - printf("valid\n"); _poll_fds[_poll_fds_num].fd = _control_subs[i]; _poll_fds[_poll_fds_num].events = POLLIN; _poll_fds_num++; @@ -381,7 +381,6 @@ PWMSim::task_main() _current_update_rate = 0; _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_armed_sub, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ actuator_outputs_s outputs; @@ -391,8 +390,6 @@ PWMSim::task_main() _outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &outputs); - DEVICE_LOG("starting"); - /* loop until killed */ while (!_task_should_exit) { @@ -420,7 +417,19 @@ PWMSim::task_main() } /* sleep waiting for data, but no more than a second */ - int ret = px4_poll(&_poll_fds[0], _poll_fds_num, 1000); + int ret = 0; + + if (_poll_fds_num == 0) { + usleep(1000 * 1000); + + /* this can happen during boot, but after the sleep its likely resolved */ + if (_poll_fds_num == 0) { + PX4_WARN("pwm_out_sim: No valid fds"); + } + + } else { + ret = px4_poll(&_poll_fds[0], _poll_fds_num, 1000); + } /* this would be bad... */ if (ret < 0) { @@ -474,8 +483,9 @@ PWMSim::task_main() } /* do mixing */ - actuator_outputs_s outputs; + actuator_outputs_s outputs = {}; num_outputs = _mixers->mix(&outputs.output[0], num_outputs, NULL); + outputs.noutputs = num_outputs; outputs.timestamp = hrt_absolute_time(); /* disable unused ports by setting their output to NaN */ @@ -548,7 +558,7 @@ PWMSim::control_callback(uintptr_t handle, const actuator_controls_s *controls = (actuator_controls_s *)handle; if (_armed) { - input = controls->control[control_index]; + input = controls[control_group].control[control_index]; } else { /* clamp actuator to zero if not armed */ diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 18acccc00c..5b7f1e5557 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -694,7 +694,9 @@ start() #ifdef PX4_I2C_BUS_ESC PX4_I2C_BUS_ESC, #endif +#ifdef PX4_I2C_BUS_ONBOARD PX4_I2C_BUS_ONBOARD, +#endif -1 }; diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 64d9dced4a..8892cfedeb 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -71,6 +71,11 @@ #include #include +#include +#include +#include +#include + #include #include #include @@ -89,7 +94,7 @@ * This is the analog to FMU_INPUT_DROP_LIMIT_US on the IO side */ -#define CONTROL_INPUT_DROP_LIMIT_MS 20 +#define CONTROL_INPUT_DROP_LIMIT_US 2000 #define NAN_VALUE (0.0f/0.0f) class PX4FMU : public device::CDev @@ -121,7 +126,7 @@ private: #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) static const unsigned _max_actuators = 4; #endif -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) static const unsigned _max_actuators = 6; #endif #if defined(CONFIG_ARCH_BOARD_AEROCORE) @@ -141,6 +146,8 @@ private: orb_advert_t _outputs_pub; unsigned _num_outputs; int _class_instance; + int _sbus_fd; + int _dsm_fd; volatile bool _initialized; bool _servo_armed; @@ -232,6 +239,17 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { {GPIO_VDD_5V_HIPOWER_OC, 0, 0}, {GPIO_VDD_5V_PERIPH_OC, 0, 0}, #endif +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V4) + {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, + {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, + {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0}, + {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0}, + {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0}, + {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0}, + + {0, GPIO_VDD_3V3_SENSORS_EN, 0}, + {GPIO_VDD_BRICK_VALID, 0, 0}, +#endif #if defined(CONFIG_ARCH_BOARD_AEROCORE) /* AeroCore breaks out User GPIOs on J11 */ {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, @@ -274,6 +292,8 @@ PX4FMU::PX4FMU() : _outputs_pub(nullptr), _num_outputs(0), _class_instance(0), + _sbus_fd(-1), + _dsm_fd(-1), _initialized(false), _servo_armed(false), _pwm_on(false), @@ -307,6 +327,11 @@ PX4FMU::PX4FMU() : _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM; #endif +#ifdef GPIO_SBUS_INV + // this board has a GPIO to control SBUS inversion + stm32_configgpio(GPIO_SBUS_INV); +#endif + /* only enable this during development */ _debug_enabled = false; } @@ -569,7 +594,7 @@ PX4FMU::update_pwm_rev_mask() void PX4FMU::publish_pwm_outputs(uint16_t *values, size_t numvalues) { - actuator_outputs_s outputs; + actuator_outputs_s outputs = {}; outputs.noutputs = numvalues; outputs.timestamp = hrt_absolute_time(); @@ -620,6 +645,15 @@ PX4FMU::cycle() update_pwm_rev_mask(); +#ifdef SBUS_SERIAL_PORT + _sbus_fd = sbus_init(SBUS_SERIAL_PORT, true); +#endif + +#ifdef DSM_SERIAL_PORT + // XXX rather than opening it we need to cycle between protocols until one is locked in + //_dsm_fd = dsm_init(DSM_SERIAL_PORT); +#endif + _initialized = true; } @@ -668,6 +702,9 @@ PX4FMU::cycle() /* check if anything updated */ int ret = ::poll(_poll_fds, _poll_fds_num, 0); + /* log if main actuator updated and sync */ + int main_out_latency = 0; + /* this would be bad... */ if (ret < 0) { DEVICE_LOG("poll error %d", errno); @@ -685,6 +722,20 @@ PX4FMU::cycle() if (_control_subs[i] > 0) { if (_poll_fds[poll_id].revents & POLLIN) { orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); + + /* main outputs */ + if (i == 0) { + //main_out_latency = hrt_absolute_time() - _controls[i].timestamp - 250; + + /* do only correct within the current phase */ + if (abs(main_out_latency) > CONTROL_INPUT_DROP_LIMIT_US) { + main_out_latency = CONTROL_INPUT_DROP_LIMIT_US; + } + + if (main_out_latency < 250) { + main_out_latency = 0; + } + } } poll_id++; @@ -776,10 +827,47 @@ PX4FMU::cycle() update_pwm_rev_mask(); } + bool rc_updated = false; + +#ifdef SBUS_SERIAL_PORT + bool sbus_failsafe, sbus_frame_drop; + uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS]; + uint16_t raw_rc_count; + bool sbus_updated = sbus_input(_sbus_fd, &raw_rc_values[0], &raw_rc_count, &sbus_failsafe, &sbus_frame_drop, + input_rc_s::RC_INPUT_MAX_CHANNELS); + + if (sbus_updated) { + // we have a new PPM frame. Publish it. + _rc_in.channel_count = raw_rc_count; + + if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) { + _rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS; + } + + for (uint8_t i = 0; i < _rc_in.channel_count; i++) { + _rc_in.values[i] = raw_rc_values[i]; + } + + _rc_in.timestamp_publication = hrt_absolute_time(); + _rc_in.timestamp_last_signal = _rc_in.timestamp_publication; + + _rc_in.rc_ppm_frame_length = 0; + _rc_in.rssi = (!sbus_frame_drop) ? RC_INPUT_RSSI_MAX : (RC_INPUT_RSSI_MAX / 2); + _rc_in.rc_failsafe = sbus_failsafe; + _rc_in.rc_lost = false; + _rc_in.rc_lost_frame_count = sbus_dropped_frames(); + _rc_in.rc_total_frame_count = 0; + + rc_updated = true; + } + +#endif + #ifdef HRT_PPM_CHANNEL // see if we have new PPM input data - if (ppm_last_valid_decode != _rc_in.timestamp_last_signal) { + if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) && + ppm_decoded_channels > 3) { // we have a new PPM frame. Publish it. _rc_in.channel_count = ppm_decoded_channels; @@ -801,6 +889,12 @@ PX4FMU::cycle() _rc_in.rc_lost_frame_count = 0; _rc_in.rc_total_frame_count = 0; + rc_updated = true; + } + +#endif + + if (rc_updated) { /* lazily advertise on first publication */ if (_to_input_rc == nullptr) { _to_input_rc = orb_advertise(ORB_ID(input_rc), &_rc_in); @@ -810,8 +904,8 @@ PX4FMU::cycle() } } -#endif - work_queue(HPWORK, &_work, (worker_t)&PX4FMU::cycle_trampoline, this, USEC2TICK(CONTROL_INPUT_DROP_LIMIT_MS * 1000)); + work_queue(HPWORK, &_work, (worker_t)&PX4FMU::cycle_trampoline, this, + USEC2TICK(CONTROL_INPUT_DROP_LIMIT_US - main_out_latency)); } void PX4FMU::work_stop() @@ -1266,7 +1360,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) set_mode(MODE_4PWM); break; -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) case 6: set_mode(MODE_6PWM); @@ -1479,6 +1573,79 @@ PX4FMU::sensor_reset(int ms) // stm32_configgpio(GPIO_ACCEL_DRDY); // stm32_configgpio(GPIO_EXTI_MPU_DRDY); +#endif +#endif +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V4) + + if (ms < 1) { + ms = 1; + } + + /* disable SPI bus */ + stm32_configgpio(GPIO_SPI_CS_OFF_MPU9250); + stm32_configgpio(GPIO_SPI_CS_OFF_HMC5983); + stm32_configgpio(GPIO_SPI_CS_OFF_MS5611); + stm32_configgpio(GPIO_SPI_CS_OFF_ICM_20608_G); + + stm32_gpiowrite(GPIO_SPI_CS_OFF_MPU9250, 0); + stm32_gpiowrite(GPIO_SPI_CS_OFF_HMC5983, 0); + stm32_gpiowrite(GPIO_SPI_CS_OFF_MS5611, 0); + stm32_gpiowrite(GPIO_SPI_CS_OFF_ICM_20608_G, 0); + + stm32_configgpio(GPIO_SPI1_SCK_OFF); + stm32_configgpio(GPIO_SPI1_MISO_OFF); + stm32_configgpio(GPIO_SPI1_MOSI_OFF); + + stm32_gpiowrite(GPIO_SPI1_SCK_OFF, 0); + stm32_gpiowrite(GPIO_SPI1_MISO_OFF, 0); + stm32_gpiowrite(GPIO_SPI1_MOSI_OFF, 0); + + stm32_configgpio(GPIO_DRDY_OFF_MPU9250); + stm32_configgpio(GPIO_DRDY_OFF_HMC5983); + stm32_configgpio(GPIO_DRDY_OFF_ICM_20608_G); + + stm32_gpiowrite(GPIO_DRDY_OFF_MPU9250, 0); + stm32_gpiowrite(GPIO_DRDY_OFF_HMC5983, 0); + stm32_gpiowrite(GPIO_DRDY_OFF_ICM_20608_G, 0); + + /* set the sensor rail off */ + stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); + stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0); + + /* wait for the sensor rail to reach GND */ + usleep(ms * 1000); + warnx("reset done, %d ms", ms); + + /* re-enable power */ + + /* switch the sensor rail back on */ + stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1); + + /* wait a bit before starting SPI, different times didn't influence results */ + usleep(100); + + /* reconfigure the SPI pins */ +#ifdef CONFIG_STM32_SPI1 + stm32_configgpio(GPIO_SPI_CS_MPU9250); + stm32_configgpio(GPIO_SPI_CS_HMC5983); + stm32_configgpio(GPIO_SPI_CS_MS5611); + stm32_configgpio(GPIO_SPI_CS_ICM_20608_G); + + /* De-activate all peripherals, + * required for some peripheral + * state machines + */ + stm32_gpiowrite(GPIO_SPI_CS_OFF_MPU9250, 1); + stm32_gpiowrite(GPIO_SPI_CS_OFF_HMC5983, 1); + stm32_gpiowrite(GPIO_SPI_CS_OFF_MS5611, 1); + stm32_gpiowrite(GPIO_SPI_CS_OFF_ICM_20608_G, 1); + + // // XXX bring up the EXTI pins again + // stm32_configgpio(GPIO_GYRO_DRDY); + // stm32_configgpio(GPIO_MAG_DRDY); + // stm32_configgpio(GPIO_ACCEL_DRDY); + // stm32_configgpio(GPIO_EXTI_MPU_DRDY); + #endif #endif } @@ -1505,6 +1672,31 @@ PX4FMU::peripheral_reset(int ms) /* switch the peripheral rail back on */ stm32_gpiowrite(GPIO_VDD_5V_PERIPH_EN, 0); #endif +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V4) + + if (ms < 1) { + ms = 10; + } + + /* set the peripheral rails off */ + stm32_configgpio(GPIO_PERIPH_3V3_EN); + + stm32_gpiowrite(GPIO_PERIPH_3V3_EN, 0); + + bool last = stm32_gpioread(GPIO_SPEKTRUM_POWER); + /* Keep Spektum on to discharge rail*/ + stm32_gpiowrite(GPIO_SPEKTRUM_POWER, 1); + + /* wait for the peripheral rail to reach GND */ + usleep(ms * 1000); + warnx("reset done, %d ms", ms); + + /* re-enable power */ + + /* switch the peripheral rail back on */ + stm32_gpiowrite(GPIO_SPEKTRUM_POWER, last); + stm32_gpiowrite(GPIO_PERIPH_3V3_EN, 1); +#endif } void @@ -1696,7 +1888,7 @@ fmu_new_mode(PortMode new_mode) /* select 4-pin PWM mode */ servo_mode = PX4FMU::MODE_4PWM; #endif -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) servo_mode = PX4FMU::MODE_6PWM; #endif #if defined(CONFIG_ARCH_BOARD_AEROCORE) @@ -1704,7 +1896,7 @@ fmu_new_mode(PortMode new_mode) #endif break; -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) case PORT_PWM4: /* select 4-pin PWM mode */ @@ -1998,7 +2190,6 @@ fmu_main(int argc, char *argv[]) (unsigned)id[6], (unsigned)id[7], (unsigned)id[8], (unsigned)id[9], (unsigned)id[10], (unsigned)id[11]); } - if (fmu_start() != OK) { errx(1, "failed to start the FMU driver"); } @@ -2012,7 +2203,7 @@ fmu_main(int argc, char *argv[]) } else if (!strcmp(verb, "mode_pwm")) { new_mode = PORT_FULL_PWM; -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) } else if (!strcmp(verb, "mode_pwm4")) { new_mode = PORT_PWM4; @@ -2050,6 +2241,13 @@ fmu_main(int argc, char *argv[]) test(); } + if (!strcmp(verb, "info")) { +#ifdef SBUS_SERIAL_PORT + warnx("frame drops: %u", sbus_dropped_frames()); +#endif + return 0; + } + if (!strcmp(verb, "fake")) { fake(argc - 1, argv + 1); } @@ -2101,7 +2299,7 @@ fmu_main(int argc, char *argv[]) #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test, fake, sensor_reset, id\n"); -#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_AEROCORE) +#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) || defined(CONFIG_ARCH_BOARD_AEROCORE) fprintf(stderr, " mode_gpio, mode_pwm, mode_pwm4, test, sensor_reset [milliseconds], i2c \n"); #endif exit(1); diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 2cabe98a23..9db91d28b8 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1844,7 +1844,7 @@ int PX4IO::io_publish_pwm_outputs() { /* data we are going to fetch */ - actuator_outputs_s outputs; + actuator_outputs_s outputs = {}; multirotor_motor_limits_s motor_limits; outputs.timestamp = hrt_absolute_time(); diff --git a/src/drivers/px4io/uploader.h b/src/drivers/px4io/uploader.h index def10c0392..2855ea4ad6 100644 --- a/src/drivers/px4io/uploader.h +++ b/src/drivers/px4io/uploader.h @@ -54,22 +54,29 @@ public: private: enum { - PROTO_NOP = 0x00, - PROTO_OK = 0x10, - PROTO_FAILED = 0x11, - PROTO_INSYNC = 0x12, - PROTO_EOC = 0x20, - PROTO_GET_SYNC = 0x21, - PROTO_GET_DEVICE = 0x22, - PROTO_CHIP_ERASE = 0x23, - PROTO_CHIP_VERIFY = 0x24, - PROTO_PROG_MULTI = 0x27, - PROTO_READ_MULTI = 0x28, - PROTO_GET_CRC = 0x29, - PROTO_REBOOT = 0x30, + PROTO_NOP = 0x00, + PROTO_OK = 0x10, + PROTO_FAILED = 0x11, + PROTO_INSYNC = 0x12, + PROTO_INVALID = 0x13, + PROTO_BAD_SILICON_REV = 0x14, + PROTO_EOC = 0x20, + PROTO_GET_SYNC = 0x21, + PROTO_GET_DEVICE = 0x22, + PROTO_CHIP_ERASE = 0x23, + PROTO_CHIP_VERIFY = 0x24, + PROTO_PROG_MULTI = 0x27, + PROTO_READ_MULTI = 0x28, + PROTO_GET_CRC = 0x29, + PROTO_GET_OTP = 0x2a, + PROTO_GET_SN = 0x2b, + PROTO_GET_CHIP = 0x2c, + PROTO_SET_DELAY = 0x2d, + PROTO_GET_CHIP_DES = 0x2e, + PROTO_REBOOT = 0x30, - INFO_BL_REV = 1, /**< bootloader protocol revision */ - BL_REV = 4, /**< supported bootloader protocol */ + INFO_BL_REV = 1, /**< bootloader protocol revision */ + BL_REV = 5, /**< supported bootloader protocol */ INFO_BOARD_ID = 2, /**< board type */ INFO_BOARD_REV = 3, /**< board revision */ INFO_FLASH_SIZE = 4, /**< max firmware size in bytes */ diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index ab458895a6..9c6e60947a 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -348,6 +348,40 @@ ADC::update_system_power(void) } #endif // CONFIG_ARCH_BOARD_PX4FMU_V2 +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V4 + system_power_s system_power; + system_power.timestamp = hrt_absolute_time(); + + system_power.voltage5V_v = 0; + + for (unsigned i = 0; i < _channel_count; i++) { + if (_samples[i].am_channel == 4) { + // it is 2:1 scaled + system_power.voltage5V_v = _samples[i].am_data * (6.6f / 4096); + } + } + + // these are not ADC related, but it is convenient to + // publish these to the same topic + system_power.usb_connected = stm32_gpioread(GPIO_OTGFS_VBUS); + + // note that the valid pins are active High + system_power.brick_valid = stm32_gpioread(GPIO_VDD_BRICK_VALID); + system_power.servo_valid = 1; + + // OC pins are not supported + system_power.periph_5V_OC = 0; + system_power.hipower_5V_OC = 0; + + /* lazily publish */ + if (_to_system_power != nullptr) { + orb_publish(ORB_ID(system_power), _to_system_power, &system_power); + + } else { + _to_system_power = orb_advertise(ORB_ID(system_power), &system_power); + } + +#endif // CONFIG_ARCH_BOARD_PX4FMU_V4 } uint16_t diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index ba148c1335..475c9c9c75 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -221,6 +221,7 @@ /* * Specific registers and bits used by HRT sub-functions */ +/* FIXME! There is an interaction in the CCMR registers that prevents using Chan 1 as the timer and chan 2 as the PPM*/ #if HRT_TIMER_CHANNEL == 1 # define rCCR_HRT rCCR1 /* compare register for HRT */ # define DIER_HRT GTIM_DIER_CC1IE /* interrupt enable for HRT */ @@ -294,7 +295,7 @@ static void hrt_call_invoke(void); # define GTIM_CCER_CC4NP 0 # define PPM_EDGE_FLIP # endif - +/* FIXME! There is an interaction in the CCMR registers that prevents using Chan 1 as the timer and chan 2 as the PPM*/ # if HRT_PPM_CHANNEL == 1 # define rCCR_PPM rCCR1 /* capture register for PPM */ # define DIER_PPM GTIM_DIER_CC1IE /* capture interrupt (non-DMA mode) */ diff --git a/src/firmware/nuttx/CMakeLists.txt b/src/firmware/nuttx/CMakeLists.txt index be6f9b1062..895eec044a 100644 --- a/src/firmware/nuttx/CMakeLists.txt +++ b/src/firmware/nuttx/CMakeLists.txt @@ -13,7 +13,7 @@ add_executable(firmware_nuttx set(nuttx_export_dir ${CMAKE_BINARY_DIR}/${BOARD}/NuttX/nuttx-export) set(link_libs - romfs apps nuttx m gcc + romfs apps nuttx m gcc df_driver_framework ) if(NOT ${BOARD} STREQUAL "sim") @@ -41,13 +41,17 @@ add_custom_target(check_weak if(NOT ${BOARD} STREQUAL "sim") + if (config_io_board) + set(extras "${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}.bin") + endif() px4_nuttx_add_romfs(OUT romfs ROOT ROMFS/px4fmu_common - EXTRAS ${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}.bin + EXTRAS ${extras} ) - add_dependencies(romfs fw_io) - + if (config_io_board) + add_dependencies(romfs fw_io) + endif() set(fw_file ${CMAKE_CURRENT_BINARY_DIR}/${OS}-${BOARD}-${LABEL}.px4) diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt index fb7490ffe2..2ed7c4b441 100644 --- a/src/firmware/posix/CMakeLists.txt +++ b/src/firmware/posix/CMakeLists.txt @@ -10,10 +10,12 @@ add_executable(mainapp apps.h ) -if (NOT ${CMAKE_C_COMPILER_ID} STREQUAL "Clang" OR NOT APPLE) +if (NOT ${CMAKE_C_COMPILER_ID} STREQUAL "Clang" AND NOT APPLE) target_link_libraries(mainapp -Wl,--start-group ${module_libraries} + df_driver_framework + ${df_driver_libraries} pthread m rt -Wl,--end-group ) @@ -34,7 +36,7 @@ add_dependencies(run_config mainapp) foreach(viewer none jmavsim gazebo) foreach(debugger none gdb lldb ddd valgrind) - foreach(model none iris vtol) + foreach(model none iris tailsitter) if (debugger STREQUAL "none") if (model STREQUAL "none") set(_targ_name "${viewer}") diff --git a/src/firmware/qurt/CMakeLists.txt b/src/firmware/qurt/CMakeLists.txt index b6d743186f..bb55e8dc22 100644 --- a/src/firmware/qurt/CMakeLists.txt +++ b/src/firmware/qurt/CMakeLists.txt @@ -24,6 +24,7 @@ target_link_libraries(mainapp -Wl,--whole-archive ${module_libraries} ${target_libraries} + df_driver_framework m -Wl,--no-whole-archive -Wl,${TOOLSLIB}/pic/libstdc++.a) diff --git a/src/include/mavlink/mavlink_log.h b/src/include/mavlink/mavlink_log.h index 13a6b16b34..f0141d8a17 100644 --- a/src/include/mavlink/mavlink_log.h +++ b/src/include/mavlink/mavlink_log.h @@ -106,10 +106,10 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...); * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); * @param _text The text to log; */ -#define mavlink_and_console_log_emergency(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__); \ - fprintf(stderr, "telem> "); \ - fprintf(stderr, _text, ##__VA_ARGS__); \ - fprintf(stderr, "\n"); +#define mavlink_and_console_log_emergency(_fd, _text, ...) do { mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__); \ + fprintf(stderr, "telem> "); \ + fprintf(stderr, _text, ##__VA_ARGS__); \ + fprintf(stderr, "\n"); } while(0); /** * Send a mavlink critical message and print to console. @@ -117,10 +117,10 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...); * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); * @param _text The text to log; */ -#define mavlink_and_console_log_critical(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__); \ - fprintf(stderr, "telem> "); \ - fprintf(stderr, _text, ##__VA_ARGS__); \ - fprintf(stderr, "\n"); +#define mavlink_and_console_log_critical(_fd, _text, ...) do { mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__); \ + fprintf(stderr, "telem> "); \ + fprintf(stderr, _text, ##__VA_ARGS__); \ + fprintf(stderr, "\n"); } while(0); /** * Send a mavlink emergency message and print to console. @@ -128,10 +128,10 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...); * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); * @param _text The text to log; */ -#define mavlink_and_console_log_info(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__); \ - fprintf(stderr, "telem> "); \ - fprintf(stderr, _text, ##__VA_ARGS__); \ - fprintf(stderr, "\n"); +#define mavlink_and_console_log_info(_fd, _text, ...) do { mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__); \ + fprintf(stderr, "telem> "); \ + fprintf(stderr, _text, ##__VA_ARGS__); \ + fprintf(stderr, "\n"); } while(0); struct mavlink_logmessage { char text[MAVLINK_LOG_MAXLEN + 1]; diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework new file mode 160000 index 0000000000..2ae8d8118d --- /dev/null +++ b/src/lib/DriverFramework @@ -0,0 +1 @@ +Subproject commit 2ae8d8118db0e95867cd1946d5b8b85d7e4ef9d3 diff --git a/src/lib/dspal b/src/lib/dspal index c8e885aac5..94188fb83a 160000 --- a/src/lib/dspal +++ b/src/lib/dspal @@ -1 +1 @@ -Subproject commit c8e885aac51aa34855bb3880d3dc916b9e278083 +Subproject commit 94188fb83ab6b2da59eb9a8c8596ab49b67c42f6 diff --git a/src/lib/ecl/CMakeLists.txt b/src/lib/ecl/CMakeLists.txt index 0fd5b5f5e9..9f8e02ba2e 100644 --- a/src/lib/ecl/CMakeLists.txt +++ b/src/lib/ecl/CMakeLists.txt @@ -39,6 +39,7 @@ px4_add_module( attitude_fw/ecl_pitch_controller.cpp attitude_fw/ecl_roll_controller.cpp attitude_fw/ecl_yaw_controller.cpp + attitude_fw/ecl_wheel_controller.cpp l1/ecl_l1_pos_controller.cpp validation/data_validator.cpp validation/data_validator_group.cpp diff --git a/src/lib/ecl/attitude_fw/ecl_controller.h b/src/lib/ecl/attitude_fw/ecl_controller.h index ac1ac88d04..fed7d1d8fb 100644 --- a/src/lib/ecl/attitude_fw/ecl_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_controller.h @@ -75,6 +75,8 @@ struct ECL_ControlData { float airspeed_max; float airspeed; float scaler; + float groundspeed; + float groundspeed_scaler; bool lock_integrator; }; diff --git a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp new file mode 100644 index 0000000000..7e34ff2a61 --- /dev/null +++ b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp @@ -0,0 +1,153 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_wheel_controller.cpp + * Implementation of a simple PID wheel controller for heading tracking. + * + * Authors and acknowledgements in header. + */ + +#include "ecl_wheel_controller.h" +#include +#include +#include +#include +#include +#include +#include + +ECL_WheelController::ECL_WheelController() : + ECL_Controller("wheel") +{ +} + +ECL_WheelController::~ECL_WheelController() +{ +} + +float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_data) +{ + /* Do not calculate control signal with bad inputs */ + if (!(PX4_ISFINITE(ctl_data.yaw_rate) && + PX4_ISFINITE(ctl_data.groundspeed) && + PX4_ISFINITE(ctl_data.groundspeed_scaler))) { + perf_count(_nonfinite_input_perf); + return math::constrain(_last_output, -1.0f, 1.0f); + } + + /* get the usual dt estimate */ + uint64_t dt_micros = ecl_elapsed_time(&_last_run); + _last_run = ecl_absolute_time(); + float dt = (float)dt_micros * 1e-6f; + + /* lock integral for long intervals */ + bool lock_integrator = ctl_data.lock_integrator; + + if (dt_micros > 500000) { + lock_integrator = true; + } + + /* input conditioning */ + float min_speed = 1.0f; + + /* Calculate body angular rate error */ + _rate_error = _rate_setpoint - ctl_data.yaw_rate; //body angular rate error + + if (!lock_integrator && _k_i > 0.0f && ctl_data.groundspeed > min_speed) { + + float id = _rate_error * dt * ctl_data.groundspeed_scaler; + + /* + * anti-windup: do not allow integrator to increase if actuator is at limit + */ + if (_last_output < -1.0f) { + /* only allow motion to center: increase value */ + id = math::max(id, 0.0f); + + } else if (_last_output > 1.0f) { + /* only allow motion to center: decrease value */ + id = math::min(id, 0.0f); + } + + _integrator += id; + } + + /* integrator limit */ + //xxx: until start detection is available: integral part in control signal is limited here + float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); + + /* Apply PI rate controller and store non-limited output */ + _last_output = _rate_setpoint * _k_ff * ctl_data.groundspeed_scaler + + _rate_error * _k_p * ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler + + integrator_constrained; + /*warnx("wheel: _last_output: %.4f, _integrator: %.4f, scaler %.4f", + (double)_last_output, (double)_integrator, (double)ctl_data.groundspeed_scaler);*/ + + + return math::constrain(_last_output, -1.0f, 1.0f); +} + + +float ECL_WheelController::control_attitude(const struct ECL_ControlData &ctl_data) +{ + /* Do not calculate control signal with bad inputs */ + if (!(PX4_ISFINITE(ctl_data.yaw_setpoint) && + PX4_ISFINITE(ctl_data.yaw))) { + perf_count(_nonfinite_input_perf); + warnx("not controlling wheel"); + return _rate_setpoint; + } + + /* Calculate the error */ + float yaw_error = ctl_data.yaw_setpoint - ctl_data.yaw; + /* shortest angle (wrap around) */ + yaw_error = (float)fmod((float)fmod((yaw_error + M_PI_F), M_TWOPI_F) + M_TWOPI_F, M_TWOPI_F) - M_PI_F; + /*warnx("yaw_error: %.4f", (double)yaw_error);*/ + + /* Apply P controller: rate setpoint from current error and time constant */ + _rate_setpoint = yaw_error / _tc; + + /* limit the rate */ + if (_max_rate > 0.01f) { + if (_rate_setpoint > 0.0f) { + _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; + + } else { + _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; + } + + } + + return _rate_setpoint; +} diff --git a/src/lib/ecl/attitude_fw/ecl_wheel_controller.h b/src/lib/ecl/attitude_fw/ecl_wheel_controller.h new file mode 100644 index 0000000000..f153a8f8f1 --- /dev/null +++ b/src/lib/ecl/attitude_fw/ecl_wheel_controller.h @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_wheel_controller.h + * Definition of a simple orthogonal coordinated turn yaw PID controller. + * + * @author Lorenz Meier + * @author Thomas Gubler + * @author Andreas Antener + * + * Acknowledgements: + * + * The control design is based on a design + * by Paul Riseborough and Andrew Tridgell, 2013, + * which in turn is based on initial work of + * Jonathan Challinger, 2012. + */ +#ifndef ECL_HEADING_CONTROLLER_H +#define ECL_HEADING_CONTROLLER_H + +#include +#include + +#include "ecl_controller.h" + +class __EXPORT ECL_WheelController : + public ECL_Controller +{ +public: + ECL_WheelController(); + + ~ECL_WheelController(); + + float control_attitude(const struct ECL_ControlData &ctl_data); + + float control_bodyrate(const struct ECL_ControlData &ctl_data); +}; + +#endif // ECL_HEADING_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index 81d8210549..511b5fd36f 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -76,14 +76,15 @@ public: _coordinated_method = coordinated_method; } -protected: - float _coordinated_min_speed; - enum { COORD_METHOD_OPEN = 0, - COORD_METHOD_CLOSEACC = 1, + COORD_METHOD_CLOSEACC = 1 }; +protected: + float _coordinated_min_speed; + float _max_rate; + int32_t _coordinated_method; float control_bodyrate_impl(const struct ECL_ControlData &ctl_data); diff --git a/src/lib/ecl/validation/data_validator.cpp b/src/lib/ecl/validation/data_validator.cpp index 299811ea43..4e55849df0 100644 --- a/src/lib/ecl/validation/data_validator.cpp +++ b/src/lib/ecl/validation/data_validator.cpp @@ -118,34 +118,34 @@ DataValidator::confidence(uint64_t timestamp) if (_time_last == 0) { _error_mask |= ERROR_FLAG_NO_DATA; ret = 0.0f; - } - + /* timed out - that's it */ - if (timestamp - _time_last > _timeout_interval) { + } else if (timestamp - _time_last > _timeout_interval) { _error_mask |= ERROR_FLAG_TIMEOUT; ret = 0.0f; - } /* we got the exact same sensor value N times in a row */ - if (_value_equal_count > VALUE_EQUAL_COUNT_MAX) { + } else if (_value_equal_count > VALUE_EQUAL_COUNT_MAX) { _error_mask |= ERROR_FLAG_STALE_DATA; ret = 0.0f; - } - + /* check error count limit */ - if (_error_count > NORETURN_ERRCOUNT) { + } else if (_error_count > NORETURN_ERRCOUNT) { _error_mask |= ERROR_FLAG_HIGH_ERRCOUNT; ret = 0.0f; - } /* cap error density counter at window size */ - if (_error_density > ERROR_DENSITY_WINDOW) { + } else if (_error_density > ERROR_DENSITY_WINDOW) { _error_mask |= ERROR_FLAG_HIGH_ERRDENSITY; _error_density = ERROR_DENSITY_WINDOW; + + /* no error */ + } else { + _error_mask = ERROR_FLAG_NO_ERROR; } /* no critical errors */ - if(ret > 0.0f) { + if (ret > 0.0f) { /* return local error density for last N measurements */ ret = 1.0f - (_error_density / ERROR_DENSITY_WINDOW); } diff --git a/src/lib/ecl/validation/data_validator.h b/src/lib/ecl/validation/data_validator.h index 31f6d9d989..c313b1cb04 100644 --- a/src/lib/ecl/validation/data_validator.h +++ b/src/lib/ecl/validation/data_validator.h @@ -97,7 +97,7 @@ public: * Get the error state of this validator * @return the bitmask with the error status */ - uint32_t state() { return (_error_mask); } + uint32_t state() { return _error_mask; } /** * Reset the error state of this validator diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index a4b1599a9d..6912f19070 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -86,12 +86,7 @@ __EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference __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; - } + return map_projection_init_timestamped(&mp_ref, lat_0, lon_0, timestamp); } __EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, @@ -153,7 +148,16 @@ __EXPORT int map_projection_project(const struct map_projection_reference_s *ref double cos_lat = cos(lat_rad); double cos_d_lon = cos(lon_rad - ref->lon_rad); - double c = acos(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon); + double arg = ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon; + + if (arg > 1.0) { + arg = 1.0; + + } else if (arg < -1.0) { + arg = -1.0; + } + + double c = acos(arg); double k = (fabs(c) < DBL_EPSILON) ? 1.0 : (c / sin(c)); *x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH; @@ -217,19 +221,14 @@ __EXPORT int map_projection_global_getref(double *lat_0, double *lon_0) } __EXPORT int globallocalconverter_init(double lat_0, double lon_0, float alt_0, uint64_t timestamp) { - if (strcmp("commander", getprogname()) == 0) { - gl_ref.alt = alt_0; + gl_ref.alt = alt_0; - 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; - } + 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; } } @@ -298,6 +297,41 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou return CONSTANTS_RADIUS_OF_EARTH * c; } +__EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist, + double *lat_target, double *lon_target) +{ + if (fabsf(dist) < FLT_EPSILON) { + *lat_target = lat_A; + *lon_target = lon_A; + + } else if (dist >= FLT_EPSILON) { + float heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); + waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target); + + } else { + float heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); + heading = _wrap_2pi(heading + M_PI_F); + waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target); + } +} + +__EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, + double *lat_target, double *lon_target) +{ + bearing = _wrap_2pi(bearing); + double radius_ratio = (double)(fabs(dist) / CONSTANTS_RADIUS_OF_EARTH); + + double lat_start_rad = lat_start * M_DEG_TO_RAD; + double lon_start_rad = lon_start * M_DEG_TO_RAD; + + *lat_target = asin(sin(lat_start_rad) * cos(radius_ratio) + cos(lat_start_rad) * sin(radius_ratio) * cos(( + double)bearing)); + *lon_target = lon_start_rad + atan2(sin((double)bearing) * sin(radius_ratio) * cos(lat_start_rad), + cos(radius_ratio) - sin(lat_start_rad) * sin(*lat_target)); + + *lat_target *= M_RAD_TO_DEG; + *lon_target *= M_RAD_TO_DEG; +} __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) { double lat_now_rad = lat_now * M_DEG_TO_RAD; diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index f77a8b58a4..11451fcf19 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -236,6 +236,36 @@ __EXPORT int globallocalconverter_getref(double *lat_0, double *lon_0, float *al */ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); + +/** + * Creates a new waypoint C on the line of two given waypoints (A, B) at certain distance + * from waypoint A + * + * @param lat_A waypoint A latitude in degrees (47.1234567°, not 471234567°) + * @param lon_A waypoint A longitude in degrees (8.1234567°, not 81234567°) + * @param lat_B waypoint B latitude in degrees (47.1234567°, not 471234567°) + * @param lon_B waypoint B longitude in degrees (8.1234567°, not 81234567°) + * @param dist distance of target waypoint from waypoint A in meters (can be negative) + * @param lat_target latitude of target waypoint C in degrees (47.1234567°, not 471234567°) + * @param lon_target longitude of target waypoint C in degrees (47.1234567°, not 471234567°) + */ +__EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist, + double *lat_target, double *lon_target); + +/** + * Creates a waypoint from given waypoint, distance and bearing + * see http://www.movable-type.co.uk/scripts/latlong.html + * + * @param lat_start latitude of starting waypoint in degrees (47.1234567°, not 471234567°) + * @param lon_start longitude of starting waypoint in degrees (8.1234567°, not 81234567°) + * @param bearing in rad + * @param distance in meters + * @param lat_target latitude of target waypoint in degrees (47.1234567°, not 471234567°) + * @param lon_target longitude of target waypoint in degrees (47.1234567°, not 471234567°) + */ +__EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, + double *lat_target, double *lon_target); + /** * Returns the bearing to the next waypoint in radians. * diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index b7cb068dd4..4f5ccc623b 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -211,6 +211,16 @@ public: data[3] = static_cast(cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2); } + /** + * simplified version of the above method to create quaternion representing rotation only by yaw + */ + void from_yaw(float yaw) { + data[0] = cosf(yaw / 2.0f); + data[1] = 0.0f; + data[2] = 0.0f; + data[3] = sinf(yaw / 2.0f); + } + /** * create Euler angles vector from the quaternion */ diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp index c3d44588fd..fb929c9906 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp @@ -74,7 +74,7 @@ float LowPassFilter2p::apply(float sample) // do the filtering float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2; - if (isnan(delay_element_0) || isinf(delay_element_0)) { + if (!PX4_ISFINITE(delay_element_0)) { // don't allow bad values to propagate via the filter delay_element_0 = sample; } diff --git a/src/lib/matrix b/src/lib/matrix index 7656385ea1..9cd6ac3dd9 160000 --- a/src/lib/matrix +++ b/src/lib/matrix @@ -1 +1 @@ -Subproject commit 7656385ea1d3f0a374a8146430bc63cf02e66d6b +Subproject commit 9cd6ac3dd9a255a1a2ef299da413c38a9040b8c7 diff --git a/src/lib/rc/CMakeLists.txt b/src/lib/rc/CMakeLists.txt index 686f398e27..15bbd71bb9 100644 --- a/src/lib/rc/CMakeLists.txt +++ b/src/lib/rc/CMakeLists.txt @@ -37,6 +37,8 @@ px4_add_module( SRCS st24.c sumd.c + sbus.c + dsm.c DEPENDS platforms__common ) diff --git a/src/modules/px4iofirmware/dsm.c b/src/lib/rc/dsm.c similarity index 93% rename from src/modules/px4iofirmware/dsm.c rename to src/lib/rc/dsm.c index fc6fbf82b8..5d7186ec1b 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/lib/rc/dsm.c @@ -46,9 +46,16 @@ #include #include +#include "dsm.h" #include -#include "px4io.h" +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 +#include +#endif + +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 +#include +#endif #define DSM_FRAME_SIZE 16 /**= PX4IO_RC_INPUT_CHANNELS) { + if (channel >= max_values) { continue; } @@ -473,7 +480,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) * @return true=decoded raw channel values updated, false=no update */ bool -dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes) +dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes, unsigned max_values) { ssize_t ret; hrt_abstime now; @@ -523,5 +530,5 @@ dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **by * decode it. */ dsm_partial_frame_count = 0; - return dsm_decode(now, values, num_values); + return dsm_decode(now, values, num_values, max_values); } diff --git a/src/lib/rc/dsm.h b/src/lib/rc/dsm.h new file mode 100644 index 0000000000..80614d066e --- /dev/null +++ b/src/lib/rc/dsm.h @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file dsm.h + * + * RC protocol definition for Spektrum RC + * + * @author Lorenz Meier + */ + +#pragma once + +#include + +__BEGIN_DECLS + +__EXPORT int dsm_init(const char *device); +__EXPORT bool dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes, unsigned max_values); +__EXPORT void dsm_bind(uint16_t cmd, int pulses); + +__END_DECLS diff --git a/src/modules/px4iofirmware/sbus.c b/src/lib/rc/sbus.c similarity index 50% rename from src/modules/px4iofirmware/sbus.c rename to src/lib/rc/sbus.c index 11f336a286..f911648416 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/lib/rc/sbus.c @@ -42,23 +42,25 @@ #include #include #include +#include -#include - +#include "sbus.h" #include -#define DEBUG -#include "px4io.h" -#include "protocol.h" -#include "debug.h" +#define SBUS_START_SYMBOL 0x0f -#define SBUS_FRAME_SIZE 25 #define SBUS_INPUT_CHANNELS 16 #define SBUS_FLAGS_BYTE 23 #define SBUS_FAILSAFE_BIT 3 #define SBUS_FRAMELOST_BIT 2 #define SBUS1_FRAME_DELAY 14000 +#define SBUS_SINGLE_CHAR_LEN_US (1/((100000/10)) * 1000 * 1000) + +#define SBUS_FRAME_INTERVAL_US 2500 +#define SBUS_MIN_CALL_INTERVAL_US (SBUS_FRAME_GAP_US / 3) +#define SBUS_EPSILON_US 2500 + /* Measured values with Futaba FX-30/R6108SB: -+100% on TX: PCM 1.100/1.520/1.950ms -> SBus raw values: 350/1024/1700 (100% ATV) @@ -73,31 +75,52 @@ #define SBUS_TARGET_MIN 1000.0f #define SBUS_TARGET_MAX 2000.0f +#ifdef SBUS_DEBUG +#include +#endif + /* pre-calculate the floating point stuff as far as possible at compile time */ #define SBUS_SCALE_FACTOR ((SBUS_TARGET_MAX - SBUS_TARGET_MIN) / (SBUS_RANGE_MAX - SBUS_RANGE_MIN)) #define SBUS_SCALE_OFFSET (int)(SBUS_TARGET_MIN - (SBUS_SCALE_FACTOR * SBUS_RANGE_MIN + 0.5f)) -static int sbus_fd = -1; - static hrt_abstime last_rx_time; static hrt_abstime last_frame_time; static hrt_abstime last_txframe_time = 0; -static uint8_t frame[SBUS_FRAME_SIZE]; +#define SBUS2_FRAME_SIZE_RX_VOLTAGE 3 +#define SBUS2_FRAME_SIZE_GPS_DIGIT 3 + +static enum SBUS2_DECODE_STATE { + SBUS2_DECODE_STATE_DESYNC = 0xFFF, + SBUS2_DECODE_STATE_SBUS_START = 0x2FF, + SBUS2_DECODE_STATE_SBUS1_SYNC = 0x00, + SBUS2_DECODE_STATE_SBUS2_SYNC = 0x1FF, + SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE = 0x04, + SBUS2_DECODE_STATE_SBUS2_GPS = 0x14, + SBUS2_DECODE_STATE_SBUS2_DATA1 = 0x24, + SBUS2_DECODE_STATE_SBUS2_DATA2 = 0x34 +} sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; + +static uint8_t sbus_frame[SBUS_FRAME_SIZE + (SBUS_FRAME_SIZE / 2)]; static unsigned partial_frame_count; -unsigned sbus_frame_drops; +static unsigned sbus_frame_drops; -static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, - bool *sbus_frame_drop, uint16_t max_channels); +unsigned +sbus_dropped_frames() +{ + return sbus_frame_drops; +} + +static bool +sbus_decode(uint64_t frame_time, uint8_t *frame, uint16_t *values, uint16_t *num_values, + bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values); int -sbus_init(const char *device) +sbus_init(const char *device, bool singlewire) { - if (sbus_fd < 0) { - sbus_fd = open(device, O_RDWR | O_NONBLOCK); - } + int sbus_fd = open(device, O_RDWR | O_NONBLOCK); if (sbus_fd >= 0) { struct termios t; @@ -108,21 +131,25 @@ sbus_init(const char *device) t.c_cflag |= (CSTOPB | PARENB); tcsetattr(sbus_fd, TCSANOW, &t); + if (singlewire) { + /* only defined in configs capable of IOCTL */ +#ifdef SBUS_SERIAL_PORT + //ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); +#endif + } + /* initialise the decoder */ partial_frame_count = 0; last_rx_time = hrt_absolute_time(); - - debug("S.Bus: ready"); - - } else { - debug("S.Bus: open failed"); + last_frame_time = last_rx_time; + sbus_frame_drops = 0; } return sbus_fd; } void -sbus1_output(uint16_t *values, uint16_t num_values) +sbus1_output(int sbus_fd, uint16_t *values, uint16_t num_values) { uint8_t byteindex = 1; /*Data starts one byte into the sbus frame. */ uint8_t offset = 0; @@ -161,20 +188,20 @@ sbus1_output(uint16_t *values, uint16_t num_values) } } void -sbus2_output(uint16_t *values, uint16_t num_values) +sbus2_output(int sbus_fd, uint16_t *values, uint16_t num_values) { - char b = 'B'; - write(sbus_fd, &b, 1); + sbus1_output(sbus_fd, values, num_values); } bool -sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels) +sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, + uint16_t max_channels) { - ssize_t ret; + int ret = 1; hrt_abstime now; /* - * The S.bus protocol doesn't provide reliable framing, + * The S.BUS protocol doesn't provide reliable framing, * so we detect frame boundaries by the inter-frame delay. * * The minimum frame spacing is 7ms; with 25 bytes at 100000bps @@ -190,44 +217,238 @@ sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sb */ now = hrt_absolute_time(); - if ((now - last_rx_time) > 3000) { - if (partial_frame_count > 0) { - sbus_frame_drops++; - partial_frame_count = 0; - } - } - /* * Fetch bytes, but no more than we would need to complete - * the current frame. + * a complete frame. */ - ret = read(sbus_fd, &frame[partial_frame_count], SBUS_FRAME_SIZE - partial_frame_count); + uint8_t buf[SBUS_FRAME_SIZE * 2]; + bool sbus_decoded = false; + + ret = read(sbus_fd, &buf[0], SBUS_FRAME_SIZE); /* if the read failed for any reason, just give up here */ if (ret < 1) { return false; } - last_rx_time = now; - /* - * Add bytes to the current frame + * Try to decode something with what we got */ - partial_frame_count += ret; + if (sbus_parse(now, &buf[0], ret, values, num_values, sbus_failsafe, + sbus_frame_drop, &sbus_frame_drops, max_channels)) { - /* - * If we don't have a full frame, return - */ - if (partial_frame_count < SBUS_FRAME_SIZE) { - return false; + sbus_decoded = true; } - /* - * Great, it looks like we might have a frame. Go ahead and - * decode it. + return sbus_decoded; +} + +bool +sbus_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values, + uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, unsigned *frame_drops, uint16_t max_channels) +{ + + last_rx_time = now; + + /* this is set by the decoding state machine and will default to false + * once everything that was decodable has been decoded. */ - partial_frame_count = 0; - return sbus_decode(now, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels); + bool decode_ret = false; + + /* keep decoding until we have consumed the buffer */ + for (unsigned d = 0; d < len; d++) { + + /* overflow check */ + if (partial_frame_count == sizeof(sbus_frame) / sizeof(sbus_frame[0])) { + partial_frame_count = 0; + sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; +#ifdef SBUS_DEBUG + printf("SBUS2: RESET (BUF LIM)\n"); +#endif + } + + if (partial_frame_count == SBUS_FRAME_SIZE) { + partial_frame_count = 0; + sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; +#ifdef SBUS_DEBUG + printf("SBUS2: RESET (PACKET LIM)\n"); +#endif + } + +#ifdef SBUS_DEBUG +#if 0 + printf("sbus state: %s%s%s%s%s%s, count: %d, val: %02x\n", + (sbus_decode_state == SBUS2_DECODE_STATE_DESYNC) ? "SBUS2_DECODE_STATE_DESYNC" : "", + (sbus_decode_state == SBUS2_DECODE_STATE_SBUS_START) ? "SBUS2_DECODE_STATE_SBUS_START" : "", + (sbus_decode_state == SBUS2_DECODE_STATE_SBUS1_SYNC) ? "SBUS2_DECODE_STATE_SBUS1_SYNC" : "", + (sbus_decode_state == SBUS2_DECODE_STATE_SBUS2_SYNC) ? "SBUS2_DECODE_STATE_SBUS2_SYNC" : "", + (sbus_decode_state == SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE) ? "SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE" : "", + (sbus_decode_state == SBUS2_DECODE_STATE_SBUS2_GPS) ? "SBUS2_DECODE_STATE_SBUS2_GPS" : "", + partial_frame_count, + (unsigned)frame[d]); +#endif +#endif + + switch (sbus_decode_state) { + case SBUS2_DECODE_STATE_DESYNC: + + /* we are de-synced and only interested in the frame marker */ + if (frame[d] == SBUS_START_SYMBOL) { + sbus_decode_state = SBUS2_DECODE_STATE_SBUS_START; + partial_frame_count = 0; + sbus_frame[partial_frame_count++] = frame[d]; + } + + break; + + /* fall through */ + case SBUS2_DECODE_STATE_SBUS_START: + case SBUS2_DECODE_STATE_SBUS1_SYNC: + + /* fall through */ + case SBUS2_DECODE_STATE_SBUS2_SYNC: { + sbus_frame[partial_frame_count++] = frame[d]; + + /* decode whatever we got and expect */ + if (partial_frame_count < SBUS_FRAME_SIZE) { + break; + } + + /* + * Great, it looks like we might have a frame. Go ahead and + * decode it. + */ + decode_ret = sbus_decode(now, sbus_frame, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels); + + /* + * Offset recovery: If decoding failed, check if there is a second + * start marker in the packet. + */ + unsigned start_index = 0; + + if (!decode_ret && sbus_decode_state == SBUS2_DECODE_STATE_DESYNC) { + + for (unsigned i = 1; i < partial_frame_count; i++) { + if (sbus_frame[i] == SBUS_START_SYMBOL) { + start_index = i; + break; + } + } + + /* we found a second start marker */ + if (start_index != 0) { + /* shift everything in the buffer and reset the state machine */ + for (unsigned i = 0; i < partial_frame_count - start_index; i++) { + sbus_frame[i] = sbus_frame[i + start_index]; + } + + partial_frame_count -= start_index; + sbus_decode_state = SBUS2_DECODE_STATE_SBUS_START; + +#ifdef SBUS_DEBUG + printf("DECODE RECOVERY: %d\n", start_index); +#endif + } + } + + /* if there has been no successful attempt at saving a failed + * decoding run, reset the frame count for successful and + * unsuccessful decode runs. + */ + if (start_index == 0) { + partial_frame_count = 0; + } + + } + break; + + case SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE: { + sbus_frame[partial_frame_count++] = frame[d]; + + if (partial_frame_count == 1 && sbus_frame[0] == SBUS_START_SYMBOL) { + /* this slot is unused and in fact S.BUS2 sync */ + sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC; + } + + if (partial_frame_count < SBUS2_FRAME_SIZE_RX_VOLTAGE) { + break; + } + + /* find out which payload we're dealing with in this slot */ + switch (sbus_frame[0]) { + case 0x03: { + // Observed values: + // (frame[0] == 0x3 && frame[1] == 0x84 && frame[2] == 0x0) + // (frame[0] == 0x3 && frame[1] == 0xc4 && frame[2] == 0x0) + // (frame[0] == 0x3 && frame[1] == 0x80 && frame[2] == 0x2f) + // (frame[0] == 0x3 && frame[1] == 0xc0 && frame[2] == 0x2f) +#ifdef SBUS_DEBUG + //uint16_t rx_voltage = (sbus_frame[1] << 8) | sbus_frame[2]; + //printf("rx_voltage %d\n", (int)rx_voltage); +#endif + } + + partial_frame_count = 0; + break; + + default: + /* this is not what we expect it to be, go back to sync */ + sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; + sbus_frame_drops++; + } + + } + break; + + case SBUS2_DECODE_STATE_SBUS2_GPS: { + sbus_frame[partial_frame_count++] = frame[d]; + + if (partial_frame_count == 1 && sbus_frame[0] == SBUS_START_SYMBOL) { + /* this slot is unused and in fact S.BUS2 sync */ + sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC; + } + + if (partial_frame_count < 24) { + break; + } + + /* find out which payload we're dealing with in this slot */ + switch (sbus_frame[0]) { + case 0x13: { +#ifdef SBUS_DEBUG + uint16_t gps_something = (frame[1] << 8) | frame[2]; + printf("gps_something %d\n", (int)gps_something); +#endif + } + + partial_frame_count = 0; + break; + + default: + /* this is not what we expect it to be, go back to sync */ + sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; + sbus_frame_drops++; + /* throw unknown bytes away */ + } + } + break; + + default: +#ifdef SBUS_DEBUG + printf("UNKNOWN PROTO STATE"); +#endif + decode_ret = false; + } + + + } + + if (frame_drops) { + *frame_drops = sbus_frame_drops; + } + + /* return false as default */ + return decode_ret; } /* @@ -266,40 +487,59 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { /* 15 */ { {20, 5, 0x07, 0}, {21, 0, 0xff, 3}, { 0, 0, 0x00, 0} } }; -static bool -sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, - uint16_t max_values) +bool +sbus_decode(uint64_t frame_time, uint8_t *frame, uint16_t *values, uint16_t *num_values, + bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values) { + /* check frame boundary markers to avoid out-of-sync cases */ - if ((frame[0] != 0x0f)) { + if ((frame[0] != SBUS_START_SYMBOL)) { sbus_frame_drops++; +#ifdef SBUS_DEBUG + printf("DECODE FAIL: "); + + for (unsigned i = 0; i < SBUS_FRAME_SIZE; i++) { + printf("%0x ", frame[i]); + } + + printf("\n"); +#endif + sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; return false; } switch (frame[24]) { case 0x00: /* this is S.BUS 1 */ + sbus_decode_state = SBUS2_DECODE_STATE_SBUS1_SYNC; break; - case 0x03: - /* S.BUS 2 SLOT0: RX battery and external voltage */ + case 0x04: + /* receiver voltage */ + sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE; break; - case 0x83: - /* S.BUS 2 SLOT1 */ + case 0x14: + /* GPS / baro */ + sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_GPS; break; - case 0x43: - case 0xC3: - case 0x23: - case 0xA3: - case 0x63: - case 0xE3: + case 0x24: + /* Unknown SBUS2 data */ + sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC; + break; + + case 0x34: + /* Unknown SBUS2 data */ + sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC; break; default: - /* we expect one of the bits above, but there are some we don't know yet */ - break; +#ifdef SBUS_DEBUG + printf("DECODE FAIL: END MARKER\n"); +#endif + sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; + return false; } /* we have received something we think is a frame */ @@ -331,7 +571,7 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool } /* decode switch channels if data fields are wide enough */ - if (PX4IO_RC_INPUT_CHANNELS > 17 && chancount > 15) { + if (max_values > 17 && chancount > 15) { chancount = 18; /* channel 17 (index 16) */ diff --git a/src/lib/rc/sbus.h b/src/lib/rc/sbus.h new file mode 100644 index 0000000000..0da5ea2935 --- /dev/null +++ b/src/lib/rc/sbus.h @@ -0,0 +1,84 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file sbus.h + * + * RC protocol definition for S.BUS + * + * @author Lorenz Meier + */ + +#pragma once + +#include +#include + +__BEGIN_DECLS + +#define SBUS_FRAME_SIZE 25 +#define SBUS_BUFFER_SIZE (SBUS_FRAME_SIZE + SBUS_FRAME_SIZE / 2) + +__EXPORT int sbus_init(const char *device, bool singlewire); + +/** + * Parse serial bytes on the S.BUS bus + * + * The S.bus protocol doesn't provide reliable framing, + * so we detect frame boundaries by the inter-frame delay. + * + * The minimum frame spacing is 7ms; with 25 bytes at 100000bps + * frame transmission time is ~2ms. + * + * If an interval of more than 4ms (7 ms frame spacing plus margin) + * passes between calls, the first byte we read will be the first + * byte of a frame. + * + * In the case where byte(s) are dropped from a frame, this also + * provides a degree of protection. Of course, it would be better + * if we didn't drop bytes... + */ +__EXPORT bool sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, + bool *sbus_frame_drop, + uint16_t max_channels); +__EXPORT bool sbus_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values, + uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, unsigned *frame_drops, uint16_t max_channels); +__EXPORT void sbus1_output(int sbus_fd, uint16_t *values, uint16_t num_values); +__EXPORT void sbus2_output(int sbus_fd, uint16_t *values, uint16_t num_values); + +/** + * The number of incomplete frames we encountered + */ +__EXPORT unsigned sbus_dropped_frames(void); + +__END_DECLS diff --git a/src/lib/rc/sumd.h b/src/lib/rc/sumd.h index ba9e830b91..03d7364186 100644 --- a/src/lib/rc/sumd.h +++ b/src/lib/rc/sumd.h @@ -101,8 +101,8 @@ uint8_t sumd_crc8(uint8_t crc, uint8_t value); __EXPORT 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); */ -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); +__EXPORT 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); __END_DECLS diff --git a/src/platforms/posix/drivers/airspeedsim/module.mk b/src/lib/runway_takeoff/CMakeLists.txt similarity index 87% rename from src/platforms/posix/drivers/airspeedsim/module.mk rename to src/lib/runway_takeoff/CMakeLists.txt index fb12fa04a2..f9f51dd543 100644 --- a/src/platforms/posix/drivers/airspeedsim/module.mk +++ b/src/lib/runway_takeoff/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2013 PX4 Development Team. All rights reserved. +# 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 @@ -30,12 +30,13 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ - -# -# Makefile to build the generic airspeed driver. -# -MODULE_COMMAND = measairspeedsim - -SRCS = airspeedsim.cpp meas_airspeed_sim.cpp - -MAXOPTIMIZATION = -Os +px4_add_module( + MODULE lib__runway_takeoff + COMPILE_FLAGS + -Os + SRCS + RunwayTakeoff.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp new file mode 100644 index 0000000000..7089889858 --- /dev/null +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -0,0 +1,286 @@ +/**************************************************************************** + * + * 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 RunwayTakeoff.cpp + * Runway takeoff handling for fixed-wing UAVs with steerable wheels. + * + * @author Roman Bapst + * @author Andreas Antener + */ + +#include +#include +#include + +#include "RunwayTakeoff.h" +#include +#include +#include +#include + +namespace runwaytakeoff +{ + +RunwayTakeoff::RunwayTakeoff() : + SuperBlock(NULL, "RWTO"), + _state(), + _initialized(false), + _initialized_time(0), + _init_yaw(0), + _climbout(false), + _throttle_ramp_time(2 * 1e6), + _start_wp(), + _runway_takeoff_enabled(this, "TKOFF"), + _heading_mode(this, "HDG"), + _nav_alt(this, "NAV_ALT"), + _takeoff_throttle(this, "MAX_THR"), + _runway_pitch_sp(this, "PSP"), + _max_takeoff_pitch(this, "MAX_PITCH"), + _max_takeoff_roll(this, "MAX_ROLL"), + _min_airspeed_scaling(this, "AIRSPD_SCL"), + _airspeed_min(this, "FW_AIRSPD_MIN", false), + _climbout_diff(this, "FW_CLMBOUT_DIFF", false) +{ + + updateParams(); +} + +RunwayTakeoff::~RunwayTakeoff() +{ + +} + +void RunwayTakeoff::init(float yaw, double current_lat, double current_lon) +{ + _init_yaw = yaw; + _initialized = true; + _state = RunwayTakeoffState::THROTTLE_RAMP; + _initialized_time = hrt_absolute_time(); + _climbout = true; // this is true until climbout is finished + _start_wp(0) = (float)current_lat; + _start_wp(1) = (float)current_lon; +} + +void RunwayTakeoff::update(float airspeed, float alt_agl, + double current_lat, double current_lon, int mavlink_fd) +{ + + switch (_state) { + case RunwayTakeoffState::THROTTLE_RAMP: + if (hrt_elapsed_time(&_initialized_time) > _throttle_ramp_time) { + _state = RunwayTakeoffState::CLAMPED_TO_RUNWAY; + } + + break; + + case RunwayTakeoffState::CLAMPED_TO_RUNWAY: + if (airspeed > _airspeed_min.get() * _min_airspeed_scaling.get()) { + _state = RunwayTakeoffState::TAKEOFF; + mavlink_log_info(mavlink_fd, "#Takeoff airspeed reached"); + } + + break; + + case RunwayTakeoffState::TAKEOFF: + if (alt_agl > _nav_alt.get()) { + _state = RunwayTakeoffState::CLIMBOUT; + + /* + * If we started in heading hold mode, move the navigation start WP to the current location now. + * The navigator will take this as starting point to navigate towards the takeoff WP. + */ + if (_heading_mode.get() == 0) { + _start_wp(0) = (float)current_lat; + _start_wp(1) = (float)current_lon; + } + + mavlink_log_info(mavlink_fd, "#Climbout"); + } + + break; + + case RunwayTakeoffState::CLIMBOUT: + if (alt_agl > _climbout_diff.get()) { + _climbout = false; + _state = RunwayTakeoffState::FLY; + mavlink_log_info(mavlink_fd, "#Navigating to waypoint"); + } + + break; + + default: + break; + } +} + +/* + * Returns true as long as we're below navigation altitude + */ +bool RunwayTakeoff::controlYaw() +{ + // keep controlling yaw directly until we start navigation + return _state < RunwayTakeoffState::CLIMBOUT; +} + +/* + * Returns pitch setpoint to use. + * + * Limited (parameter) as long as the plane is on runway. Otherwise + * use the one from TECS + */ +float RunwayTakeoff::getPitch(float tecsPitch) +{ + if (_state <= RunwayTakeoffState::CLAMPED_TO_RUNWAY) { + return math::radians(_runway_pitch_sp.get()); + } + + return tecsPitch; +} + +/* + * Returns the roll setpoint to use. + */ +float RunwayTakeoff::getRoll(float navigatorRoll) +{ + // until we have enough ground clearance, set roll to 0 + if (_state < RunwayTakeoffState::CLIMBOUT) { + return 0.0f; + } + + // allow some roll during climbout + else if (_state < RunwayTakeoffState::FLY) { + return math::constrain(navigatorRoll, + math::radians(-_max_takeoff_roll.get()), + math::radians(_max_takeoff_roll.get())); + } + + return navigatorRoll; +} + +/* + * Returns the yaw setpoint to use. + * + * In heading hold mode (_heading_mode == 0), it returns initial yaw as long as it's on the + * runway. When it has enough ground clearance we start navigation towards WP. + */ +float RunwayTakeoff::getYaw(float navigatorYaw) +{ + if (_heading_mode.get() == 0 && _state < RunwayTakeoffState::CLIMBOUT) { + return _init_yaw; + + } else { + return navigatorYaw; + } +} + +/* + * Returns the throttle setpoint to use. + * + * Ramps up in the beginning, until it lifts off the runway it is set to + * parameter value, then it returns the TECS throttle. + */ +float RunwayTakeoff::getThrottle(float tecsThrottle) +{ + switch (_state) { + case RunwayTakeoffState::THROTTLE_RAMP: { + float throttle = hrt_elapsed_time(&_initialized_time) / (float)_throttle_ramp_time * + _takeoff_throttle.get(); + return throttle < _takeoff_throttle.get() ? + throttle : + _takeoff_throttle.get(); + } + + case RunwayTakeoffState::CLAMPED_TO_RUNWAY: + return _takeoff_throttle.get(); + + default: + return tecsThrottle; + } +} + +bool RunwayTakeoff::resetIntegrators() +{ + // reset integrators if we're still on runway + return _state < RunwayTakeoffState::TAKEOFF; +} + +/* + * Returns the minimum pitch for TECS to use. + * + * In climbout we either want what was set on the waypoint (sp_min) but at least + * the climbtout minimum pitch (parameter). + * Otherwise use the minimum that is enforced generally (parameter). + */ +float RunwayTakeoff::getMinPitch(float sp_min, float climbout_min, float min) +{ + if (_state < RunwayTakeoffState::FLY) { + return math::max(sp_min, climbout_min); + } + + else { + return min; + } +} + +/* + * Returns the maximum pitch for TECS to use. + * + * Limited by parameter (if set) until climbout is done. + */ +float RunwayTakeoff::getMaxPitch(float max) +{ + // use max pitch from parameter if set (> 0.1) + if (_state < RunwayTakeoffState::FLY && _max_takeoff_pitch.get() > 0.1f) { + return _max_takeoff_pitch.get(); + } + + else { + return max; + } +} + +/* + * Returns the "previous" (start) WP for navigation. + */ +math::Vector<2> RunwayTakeoff::getStartWP() +{ + return _start_wp; +} + +void RunwayTakeoff::reset() +{ + _initialized = false; + _state = RunwayTakeoffState::THROTTLE_RAMP; +} + +} diff --git a/src/lib/runway_takeoff/RunwayTakeoff.h b/src/lib/runway_takeoff/RunwayTakeoff.h new file mode 100644 index 0000000000..0e1c5ed14b --- /dev/null +++ b/src/lib/runway_takeoff/RunwayTakeoff.h @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * 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 RunwayTakeoff.h + * Runway takeoff handling for fixed-wing UAVs with steerable wheels. + * + * @author Roman Bapst + * @author Andreas Antener + */ + +#ifndef RUNWAYTAKEOFF_H +#define RUNWAYTAKEOFF_H + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace runwaytakeoff +{ + +enum RunwayTakeoffState { + THROTTLE_RAMP = 0, /**< ramping up throttle */ + CLAMPED_TO_RUNWAY = 1, /**< clamped to runway, controlling yaw directly (wheel or rudder) */ + TAKEOFF = 2, /**< taking off, get ground clearance, roll 0 */ + CLIMBOUT = 3, /**< climbout to safe height before navigation, roll limited */ + FLY = 4 /**< fly towards takeoff waypoint */ +}; + +class __EXPORT RunwayTakeoff : public control::SuperBlock +{ +public: + RunwayTakeoff(); + ~RunwayTakeoff(); + + void init(float yaw, double current_lat, double current_lon); + void update(float airspeed, float alt_agl, double current_lat, double current_lon, int mavlink_fd); + + RunwayTakeoffState getState() { return _state; }; + bool isInitialized() { return _initialized; }; + + bool runwayTakeoffEnabled() { return (bool)_runway_takeoff_enabled.get(); }; + float getMinAirspeedScaling() { return _min_airspeed_scaling.get(); }; + float getInitYaw() { return _init_yaw; }; + + bool controlYaw(); + bool climbout() { return _climbout; }; + float getPitch(float tecsPitch); + float getRoll(float navigatorRoll); + float getYaw(float navigatorYaw); + float getThrottle(float tecsThrottle); + bool resetIntegrators(); + float getMinPitch(float sp_min, float climbout_min, float min); + float getMaxPitch(float max); + math::Vector<2> getStartWP(); + + void reset(); + +protected: +private: + /** state variables **/ + RunwayTakeoffState _state; + bool _initialized; + hrt_abstime _initialized_time; + float _init_yaw; + bool _climbout; + unsigned _throttle_ramp_time; + math::Vector<2> _start_wp; + + /** parameters **/ + control::BlockParamInt _runway_takeoff_enabled; + control::BlockParamInt _heading_mode; + control::BlockParamFloat _nav_alt; + control::BlockParamFloat _takeoff_throttle; + control::BlockParamFloat _runway_pitch_sp; + control::BlockParamFloat _max_takeoff_pitch; + control::BlockParamFloat _max_takeoff_roll; + control::BlockParamFloat _min_airspeed_scaling; + control::BlockParamFloat _airspeed_min; + control::BlockParamFloat _climbout_diff; + +}; + +} + +#endif // RUNWAYTAKEOFF_H diff --git a/src/lib/runway_takeoff/runway_takeoff_params.c b/src/lib/runway_takeoff/runway_takeoff_params.c new file mode 100644 index 0000000000..62c5e54fac --- /dev/null +++ b/src/lib/runway_takeoff/runway_takeoff_params.c @@ -0,0 +1,133 @@ +/**************************************************************************** + * + * 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 runway_takeoff_params.c + * + * Parameters for runway takeoff + * + * @author Andreas Antener + */ + +/** + * Enable or disable runway takeoff with landing gear + * + * 0: disabled, 1: enabled + * + * @min 0 + * @max 1 + * @group Runway Takeoff + */ +PARAM_DEFINE_INT32(RWTO_TKOFF, 0); + +/** + * Specifies which heading should be held during runnway takeoff. + * + * 0: airframe heading, 1: heading towards takeoff waypoint + * + * @min 0 + * @max 1 + * @group Runway Takeoff + */ +PARAM_DEFINE_INT32(RWTO_HDG, 0); + +/** + * Altitude AGL at which we have enough ground clearance to allow some roll. + * Until RWTO_NAV_ALT is reached the plane is held level and only + * rudder is used to keep the heading (see RWTO_HDG). This should be below + * FW_CLMBOUT_DIFF if FW_CLMBOUT_DIFF > 0. + * + * @unit m + * @min 0.0 + * @max 100.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_NAV_ALT, 5.0); + +/** + * Max throttle during runway takeoff. + * (Can be used to test taxi on runway) + * + * @min 0.0 + * @max 1.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_MAX_THR, 1.0); + +/** + * Pitch setpoint during taxi / before takeoff airspeed is reached. + * A taildragger with stearable wheel might need to pitch up + * a little to keep it's wheel on the ground before airspeed + * to takeoff is reached. + * + * @unit deg + * @min 0.0 + * @max 20.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0); + +/** + * Max pitch during takeoff. + * Fixed-wing settings are used if set to 0. Note that there is also a minimum + * pitch of 10 degrees during takeoff, so this must be larger if set. + * + * @unit deg + * @min 0.0 + * @max 60.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0); + +/** + * Max roll during climbout. + * Roll is limited during climbout to ensure enough lift and prevents aggressive + * navigation before we're on a safe height. + * + * @unit deg + * @min 0.0 + * @max 60.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_MAX_ROLL, 25.0); + +/** + * Min. airspeed scaling factor for takeoff. + * Pitch up will be commanded when the following airspeed is reached: + * FW_AIRSPD_MIN * RWTO_AIRSPD_SCL + * + * @min 0.0 + * @max 2.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_AIRSPD_SCL, 1.3); diff --git a/src/platforms/posix/drivers/barosim/module.mk b/src/lib/terrain_estimation/CMakeLists.txt similarity index 87% rename from src/platforms/posix/drivers/barosim/module.mk rename to src/lib/terrain_estimation/CMakeLists.txt index 5a4cb4ebb9..0c1177b96b 100644 --- a/src/platforms/posix/drivers/barosim/module.mk +++ b/src/lib/terrain_estimation/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# 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 @@ -30,13 +30,13 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ - -# -# MS5611 driver -# - -MODULE_COMMAND = barosim - -SRCS = baro.cpp baro_sim.cpp - -MAXOPTIMIZATION = -Os +px4_add_module( + MODULE lib__terrain_estimation + COMPILE_FLAGS + -Os + SRCS + terrain_estimator.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/lib/terrain_estimation/terrain_estimator.cpp b/src/lib/terrain_estimation/terrain_estimator.cpp new file mode 100644 index 0000000000..64a747f532 --- /dev/null +++ b/src/lib/terrain_estimation/terrain_estimator.cpp @@ -0,0 +1,201 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Roman Bapst. 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 terrain_estimator.cpp + * A terrain estimation kalman filter. + */ + +#include "terrain_estimator.h" + +#define DISTANCE_TIMEOUT 100000 // time in usec after which laser is considered dead + +TerrainEstimator::TerrainEstimator() : + _distance_last(0.0f), + _terrain_valid(false), + _time_last_distance(0), + _time_last_gps(0) +{ + memset(&_x._data[0], 0, sizeof(_x._data)); + _u_z = 0.0f; + _P.setIdentity(); +} + +bool TerrainEstimator::is_distance_valid(float distance) +{ + if (distance > 40.0f || distance < 0.00001f) { + return false; + + } else { + return true; + } +} + +void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitude, + const struct sensor_combined_s *sensor, + const struct distance_sensor_s *distance) +{ + if (attitude->R_valid) { + matrix::Matrix R_att(attitude->R); + matrix::Vector a(&sensor->accelerometer_m_s2[0]); + matrix::Vector u; + u = R_att * a; + _u_z = u(2) + 9.81f; // compensate for gravity + + } else { + _u_z = 0.0f; + } + + // dynamics matrix + matrix::Matrix A; + A.setZero(); + A(0, 1) = 1; + A(1, 2) = 1; + + // input matrix + matrix::Matrix B; + B.setZero(); + B(1, 0) = 1; + + // input noise variance + float R = 0.135f; + + // process noise convariance + matrix::Matrix Q; + Q(0, 0) = 0; + Q(1, 1) = 0; + + // do prediction + matrix::Vector dx = (A * _x) * dt; + dx(1) += B(1, 0) * _u_z * dt; + + // propagate state and covariance matrix + _x += dx; + _P += (A * _P + _P * A.transpose() + + B * R * B.transpose() + Q) * dt; +} + +void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps, + const struct distance_sensor_s *distance, + const struct vehicle_attitude_s *attitude) +{ + // terrain estimate is invalid if we have range sensor timeout + if (time_ref - distance->timestamp > DISTANCE_TIMEOUT) { + _terrain_valid = false; + } + + if (distance->timestamp > _time_last_distance) { + + float d = distance->current_distance; + + matrix::Matrix C; + C(0, 0) = -1; // measured altitude, + + float R = 0.009f; + + matrix::Vector y; + y(0) = d * cosf(attitude->roll) * cosf(attitude->pitch); + + // residual + matrix::Matrix S_I = (C * _P * C.transpose()); + S_I(0, 0) += R; + S_I = matrix::inv (S_I); + matrix::Vector r = y - C * _x; + + matrix::Matrix K = _P * C.transpose() * S_I; + + // some sort of outlayer rejection + if (fabsf(distance->current_distance - _distance_last) < 1.0f) { + _x += K * r; + _P -= K * C * _P; + } + + // if the current and the last range measurement are bad then we consider the terrain + // estimate to be invalid + if (!is_distance_valid(distance->current_distance) && !is_distance_valid(_distance_last)) { + _terrain_valid = false; + + } else { + _terrain_valid = true; + } + + _time_last_distance = distance->timestamp; + _distance_last = distance->current_distance; + } + + if (gps->timestamp_position > _time_last_gps && gps->fix_type >= 3) { + matrix::Matrix C; + C(0, 1) = 1; + + float R = 0.056f; + + matrix::Vector y; + y(0) = gps->vel_d_m_s; + + // residual + matrix::Matrix S_I = (C * _P * C.transpose()); + S_I(0, 0) += R; + S_I = matrix::inv(S_I); + matrix::Vector r = y - C * _x; + + matrix::Matrix K = _P * C.transpose() * S_I; + _x += K * r; + _P -= K * C * _P; + + _time_last_gps = gps->timestamp_position; + } + + // reinitialise filter if we find bad data + bool reinit = false; + + for (int i = 0; i < n_x; i++) { + if (!PX4_ISFINITE(_x(i))) { + reinit = true; + } + } + + for (int i = 0; i < n_x; i++) { + for (int j = 0; j < n_x; j++) { + if (!PX4_ISFINITE(_P(i, j))) { + reinit = true; + } + } + } + + if (reinit) { + memset(&_x._data[0], 0, sizeof(_x._data)); + _P.setZero(); + _P(0, 0) = _P(1, 1) = _P(2, 2) = 0.1f; + } + +} diff --git a/src/lib/terrain_estimation/terrain_estimator.h b/src/lib/terrain_estimation/terrain_estimator.h new file mode 100644 index 0000000000..ed9e9741f3 --- /dev/null +++ b/src/lib/terrain_estimation/terrain_estimator.h @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Roman Bapst. 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 terrain_estimator.h + */ + +#include +#include "matrix/Matrix.hpp" +#include +#include +#include +#include + + +/* +* This class can be used to estimate distance to the ground using a laser range finder. +* It's assumed that the laser points down vertically if the vehicle is in it's neutral pose. +* The predict(...) function will do a state prediciton based on accelerometer inputs. It also +* considers accelerometer bias. +* The measurement_update(...) function does a measurement update based on range finder and gps +* velocity measurements. Both functions should always be called together when there is new +* acceleration data available. +* The is_valid() function provides information whether the estimate is valid. +*/ + +class __EXPORT TerrainEstimator +{ +public: + TerrainEstimator(); + ~TerrainEstimator() {}; + + bool is_valid() {return _terrain_valid;} + float get_distance_to_ground() {return -_x(0);} + float get_velocity() {return _x(1);}; + + void predict(float dt, const struct vehicle_attitude_s *attitude, const struct sensor_combined_s *sensor, + const struct distance_sensor_s *distance); + void measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps, + const struct distance_sensor_s *distance, + const struct vehicle_attitude_s *attitude); + +private: + enum {n_x = 3}; + + float _distance_last; + bool _terrain_valid; + + // kalman filter variables + matrix::Vector _x; // state: ground distance, velocity, accel bias in z direction + float _u_z; // acceleration in earth z direction + matrix::Matrix _P; // covariance matrix + + // timestamps + uint64_t _time_last_distance; + uint64_t _time_last_gps; + + /* + struct { + float var_acc_z; + float var_p_z; + float var_p_vz; + float var_lidar; + float var_gps_vz; + } _params; + */ + + bool is_distance_valid(float distance); + +}; \ No newline at end of file diff --git a/src/lib/version/version.h b/src/lib/version/version.h index b71ba1d0d9..9492cec90f 100644 --- a/src/lib/version/version.h +++ b/src/lib/version/version.h @@ -51,6 +51,10 @@ #define HW_ARCH "PX4FMU_V2" #endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V4 +#define HW_ARCH "PX4FMU_V4" +#endif + #ifdef CONFIG_ARCH_BOARD_AEROCORE #define HW_ARCH "AEROCORE" #endif diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index e408fb3c99..273d159f2e 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -284,8 +284,6 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) /* subscribe to raw data */ int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); - /* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */ - orb_set_interval(sub_raw, 3); /* subscribe to GPS */ int sub_gps = orb_subscribe(ORB_ID(vehicle_gps_position)); 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 4b4fba54c8..3e44579c66 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -59,6 +59,7 @@ #include #include #include +#include #include #include @@ -122,6 +123,7 @@ private: int _params_sub = -1; int _vision_sub = -1; int _mocap_sub = -1; + int _airspeed_sub = -1; int _global_pos_sub = -1; orb_advert_t _att_pub = nullptr; orb_advert_t _ctrl_state_pub = nullptr; @@ -161,6 +163,8 @@ private: att_pos_mocap_s _mocap = {}; Vector<3> _mocap_hdg; + airspeed_s _airspeed = {}; + Quaternion _q; Vector<3> _rates; Vector<3> _gyro_bias; @@ -198,16 +202,21 @@ private: bool init(); bool update(float dt); + + // Update magnetic declination (in rads) immediately changing yaw rotation + void update_mag_declination(float new_declination); }; AttitudeEstimatorQ::AttitudeEstimatorQ() : + _vel_prev(0, 0, 0), + _pos_acc(0, 0, 0), _voter_gyro(3), _voter_accel(3), _voter_mag(3), - _lp_roll_rate(250.0f, 18.0f), - _lp_pitch_rate(250.0f, 18.0f), - _lp_yaw_rate(250.0f, 10.0f) + _lp_roll_rate(250.0f, 30.0f), + _lp_pitch_rate(250.0f, 30.0f), + _lp_yaw_rate(250.0f, 20.0f) { _voter_mag.set_timeout(200000); @@ -258,7 +267,7 @@ int AttitudeEstimatorQ::start() _control_task = px4_task_spawn_cmd("attitude_estimator_q", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 2100, + 2400, (px4_main_t)&AttitudeEstimatorQ::task_main_trampoline, nullptr); @@ -287,11 +296,20 @@ void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[]) void AttitudeEstimatorQ::task_main() { + +#ifdef __PX4_POSIX + perf_counter_t _perf_accel(perf_alloc_once(PC_ELAPSED, "sim_accel_delay")); + perf_counter_t _perf_mpu(perf_alloc_once(PC_ELAPSED, "sim_mpu_delay")); + perf_counter_t _perf_mag(perf_alloc_once(PC_ELAPSED, "sim_mag_delay")); +#endif + _sensors_sub = orb_subscribe(ORB_ID(sensor_combined)); _vision_sub = orb_subscribe(ORB_ID(vision_position_estimate)); _mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap)); + _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); @@ -299,7 +317,7 @@ void AttitudeEstimatorQ::task_main() hrt_abstime last_time = 0; - px4_pollfd_struct_t fds[1]; + px4_pollfd_struct_t fds[1] = {}; fds[0].fd = _sensors_sub; fds[0].events = POLLIN; @@ -313,10 +331,12 @@ void AttitudeEstimatorQ::task_main() if (ret < 0) { // Poll error, sleep and try again usleep(10000); + PX4_WARN("Q POLL ERROR"); continue; } else if (ret == 0) { // Poll timeout, do nothing + PX4_WARN("Q POLL TIMEOUT"); continue; } @@ -325,6 +345,10 @@ void AttitudeEstimatorQ::task_main() // Update sensors sensor_combined_s sensors; + int best_gyro = 0; + int best_accel = 0; + int best_mag = 0; + if (!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)) { // Feed validator with recent sensor data @@ -361,8 +385,6 @@ void AttitudeEstimatorQ::task_main() } } - int best_gyro, best_accel, best_mag; - // Get best measurement values hrt_abstime curr_time = hrt_absolute_time(); _gyro.set(_voter_gyro.get_best(curr_time, &best_gyro)); @@ -379,6 +401,12 @@ void AttitudeEstimatorQ::task_main() if (!_failsafe) { uint32_t flags = DataValidator::ERROR_FLAG_NO_ERROR; +#ifdef __PX4_POSIX + perf_end(_perf_accel); + perf_end(_perf_mpu); + perf_end(_perf_mag); +#endif + if (_voter_gyro.failover_count() > 0) { _failsafe = true; flags = _voter_gyro.failover_state(); @@ -473,6 +501,14 @@ void AttitudeEstimatorQ::task_main() _mocap_hdg = Rmoc.transposed() * v; } + // Update airspeed + bool airspeed_updated = false; + orb_check(_airspeed_sub, &airspeed_updated); + + if (airspeed_updated) { + orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); + } + // Check for timeouts on data if (_ext_hdg_mode == 1) { _ext_hdg_good = _vision.timestamp_boot > 0 && (hrt_elapsed_time(&_vision.timestamp_boot) < 500000); @@ -489,7 +525,7 @@ void AttitudeEstimatorQ::task_main() 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)); + update_mag_declination(math::radians(get_mag_declination(_gpos.lat, _gpos.lon))); } } @@ -586,6 +622,16 @@ void AttitudeEstimatorQ::task_main() ctrl_state.yaw_rate = _lp_yaw_rate.apply(_rates(2)); + /* Airspeed - take airspeed measurement directly here as no wind is estimated */ + if (PX4_ISFINITE(_airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - _airspeed.timestamp < 1e6 + && _airspeed.timestamp > 0) { + ctrl_state.airspeed = _airspeed.indicated_airspeed_m_s; + ctrl_state.airspeed_valid = true; + + } else { + ctrl_state.airspeed_valid = false; + } + /* Publish to control state topic */ if (_ctrl_state_pub == nullptr) { _ctrl_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state); @@ -614,7 +660,7 @@ void AttitudeEstimatorQ::update_parameters(bool force) param_get(_params_handles.w_gyro_bias, &_w_gyro_bias); float mag_decl_deg = 0.0f; param_get(_params_handles.mag_decl, &mag_decl_deg); - _mag_decl = math::radians(mag_decl_deg); + update_mag_declination(math::radians(mag_decl_deg)); int32_t mag_decl_auto_int; param_get(_params_handles.mag_decl_auto, &mag_decl_auto_int); _mag_decl_auto = mag_decl_auto_int != 0; @@ -649,6 +695,12 @@ bool AttitudeEstimatorQ::init() // Convert to quaternion _q.from_dcm(R); + + // Compensate for magnetic declination + Quaternion decl_rotation; + decl_rotation.from_yaw(_mag_decl); + _q = decl_rotation * _q; + _q.normalize(); if (PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) && @@ -708,6 +760,8 @@ bool AttitudeEstimatorQ::update(float dt) corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag; } + _q.normalize(); + // Accelerometer correction // Project 'k' unit vector of earth frame to body frame // Vector<3> k = _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, 1.0f)); @@ -750,6 +804,20 @@ bool AttitudeEstimatorQ::update(float dt) return true; } +void AttitudeEstimatorQ::update_mag_declination(float new_declination) +{ + // Apply initial declination or trivial rotations without changing estimation + if (!_inited || fabsf(new_declination - _mag_decl) < 0.0001f) { + _mag_decl = new_declination; + + } else { + // Immediately rotate current estimation to avoid gyro bias growth + Quaternion decl_rotation; + decl_rotation.from_yaw(new_declination - _mag_decl); + _q = decl_rotation * _q; + _mag_decl = new_declination; + } +} int attitude_estimator_q_main(int argc, char *argv[]) { diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index c1ec35b5ee..4239797e0a 100644 --- a/src/modules/commander/CMakeLists.txt +++ b/src/modules/commander/CMakeLists.txt @@ -37,7 +37,7 @@ endif() px4_add_module( MODULE modules__commander MAIN commander - STACK 1200 + STACK 4096 COMPILE_FLAGS ${MODULE_CFLAGS} -Os diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 8093ef56ba..3ca741da9b 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -69,21 +69,23 @@ #include "PreflightCheck.h" +#include "DevMgr.hpp" + +using namespace DriverFramework; + namespace Commander { -static int check_calibration(int fd, const char* param_template, int &devid); - -int check_calibration(int fd, const char* param_template, int &devid) +static int check_calibration(DevHandle &h, const char* param_template, int &devid) { bool calibration_found; /* new style: ask device for calibration state */ - int ret = px4_ioctl(fd, SENSORIOCCALTEST, 0); + int ret = h.ioctl(SENSORIOCCALTEST, 0); calibration_found = (ret == OK); - devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0); + devid = h.ioctl(DEVIOCGDEVICEID, 0); char s[20]; int instance = 0; @@ -120,37 +122,44 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in char s[30]; sprintf(s, "%s%u", MAG_BASE_DEVICE_PATH, instance); - int fd = px4_open(s, 0); + DevHandle h; + DevMgr::getHandle(s, h); - if (fd < 0) { + if (!h.isValid()) { if (!optional) { - if(report_fail) mavlink_and_console_log_critical(mavlink_fd, + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: NO MAG SENSOR #%u", instance); + } } return false; } - int ret = check_calibration(fd, "CAL_MAG%u_ID", device_id); + int ret = check_calibration(h, "CAL_MAG%u_ID", device_id); if (ret) { - if(report_fail) mavlink_and_console_log_critical(mavlink_fd, + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance); + } success = false; goto out; } - ret = px4_ioctl(fd, MAGIOCSELFTEST, 0); + ret = h.ioctl(MAGIOCSELFTEST, 0); if (ret != OK) { - if(report_fail) mavlink_and_console_log_critical(mavlink_fd, + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: MAG #%u SELFTEST FAILED", instance); + } success = false; goto out; } out: - px4_close(fd); + DevMgr::releaseHandle(h); return success; } @@ -160,31 +169,38 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, char s[30]; sprintf(s, "%s%u", ACCEL_BASE_DEVICE_PATH, instance); - int fd = px4_open(s, O_RDONLY); + DevHandle h; + DevMgr::getHandle(s, h); - if (fd < 0) { + if (!h.isValid()) { if (!optional) { - if(report_fail) mavlink_and_console_log_critical(mavlink_fd, + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance); + } } return false; } - int ret = check_calibration(fd, "CAL_ACC%u_ID", device_id); + int ret = check_calibration(h, "CAL_ACC%u_ID", device_id); if (ret) { - if(report_fail) mavlink_and_console_log_critical(mavlink_fd, + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance); + } success = false; goto out; } - ret = px4_ioctl(fd, ACCELIOCSELFTEST, 0); + ret = h.ioctl(ACCELIOCSELFTEST, 0); if (ret != OK) { - if(report_fail) mavlink_and_console_log_critical(mavlink_fd, + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL #%u SELFTEST FAILED", instance); + } success = false; goto out; } @@ -193,14 +209,16 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, if (dynamic) { /* check measurement result range */ struct accel_report acc; - ret = px4_read(fd, &acc, sizeof(acc)); + ret = h.read(&acc, sizeof(acc)); if (ret == sizeof(acc)) { /* evaluate values */ float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) { - if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming"); + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming"); + } /* this is frickin' fatal */ success = false; goto out; @@ -217,7 +235,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, #endif out: - px4_close(fd); + DevMgr::releaseHandle(h); return success; } @@ -227,9 +245,10 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev char s[30]; sprintf(s, "%s%u", GYRO_BASE_DEVICE_PATH, instance); - int fd = px4_open(s, 0); + DevHandle h; + DevMgr::getHandle(s, h); - if (fd < 0) { + if (!h.isValid()) { if (!optional) { if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, @@ -240,7 +259,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev return false; } - int ret = check_calibration(fd, "CAL_GYRO%u_ID", device_id); + int ret = check_calibration(h, "CAL_GYRO%u_ID", device_id); if (ret) { if (report_fail) { @@ -251,7 +270,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev goto out; } - ret = px4_ioctl(fd, GYROIOCSELFTEST, 0); + ret = h.ioctl(GYROIOCSELFTEST, 0); if (ret != OK) { if (report_fail) { @@ -263,7 +282,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev } out: - px4_close(fd); + DevMgr::releaseHandle(h); return success; } @@ -273,9 +292,10 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev char s[30]; sprintf(s, "%s%u", BARO_BASE_DEVICE_PATH, instance); - int fd = px4_open(s, 0); + DevHandle h; + DevMgr::getHandle(s, h); - if (fd < 0) { + if (!h.isValid()) { if (!optional) { if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, @@ -300,7 +320,7 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev //out: - px4_close(fd); + DevMgr::releaseHandle(h); return success; } @@ -506,7 +526,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro /* ---- Global Navigation Satellite System receiver ---- */ if (checkGNSS) { - if(!gnssCheck(mavlink_fd, reportFailures)) { + if (!gnssCheck(mavlink_fd, reportFailures)) { failed = true; } } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 83e79b77fc..1f5453aed5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -153,7 +153,7 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage #define PRINT_INTERVAL 5000000 #define PRINT_MODE_REJECT_INTERVAL 10000000 -#define INAIR_RESTART_HOLDOFF_INTERVAL 1000000 +#define INAIR_RESTART_HOLDOFF_INTERVAL 500000 #define HIL_ID_MIN 1000 #define HIL_ID_MAX 1999 @@ -300,8 +300,8 @@ int commander_main(int argc, char *argv[]) thread_should_exit = false; daemon_task = px4_task_spawn_cmd("commander", SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 40, - 3500, + SCHED_PRIORITY_DEFAULT + 40, + 3600, commander_thread_main, (char * const *)&argv[0]); @@ -382,9 +382,9 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "check")) { int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0); int checkres = 0; - checkres = preflight_check(&status, mavlink_fd_local, false); + checkres = preflight_check(&status, mavlink_fd_local, false, true); warnx("Preflight check: %s", (checkres == 0) ? "OK" : "FAILED"); - checkres = preflight_check(&status, mavlink_fd_local, true); + checkres = preflight_check(&status, mavlink_fd_local, true, true); warnx("Prearm check: %s", (checkres == 0) ? "OK" : "FAILED"); px4_close(mavlink_fd_local); return 0; @@ -410,6 +410,39 @@ int commander_main(int argc, char *argv[]) return 0; } + if (!strcmp(argv[1], "takeoff")) { + int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0); + if (TRANSITION_CHANGED == arm_disarm(true, mavlink_fd_local, "command line")) { + warnx("note: not updating home position on commandline arming!"); + + /* see if we got a home position */ + if (status.condition_home_position_valid) { + vehicle_command_s cmd = {}; + cmd.target_system = status.system_id; + cmd.target_component = status.component_id; + + cmd.command = vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF; + // cmd.param1 = 0.25f; /* minimum pitch */ + // /* param 2-3 unused */ + // cmd.param4 = home_position.yaw; + // cmd.param5 = home_position.lat; + // cmd.param6 = home_position.lon; + // cmd.param7 = home_position.alt; + + // XXX inspect use of publication handle + (void)orb_advertise(ORB_ID(vehicle_command), &cmd); + + } else { + warnx("rejecting takeoff, no position lock yet"); + } + + } else { + warnx("arming failed"); + } + px4_close(mavlink_fd_local); + return 0; + } + usage("unrecognized command"); return 1; } @@ -526,6 +559,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s case vehicle_command_s::VEHICLE_CMD_DO_SET_MODE: { uint8_t base_mode = (uint8_t)cmd->param1; uint8_t custom_main_mode = (uint8_t)cmd->param2; + uint8_t custom_sub_mode = (uint8_t)cmd->param3; transition_result_t arming_ret = TRANSITION_NOT_CHANGED; @@ -540,7 +574,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s arming_ret = arm_disarm(cmd_arm, mavlink_fd, "set mode command"); - /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ + /* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */ if (cmd_arm && (arming_ret == TRANSITION_CHANGED) && (hrt_absolute_time() > (commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL))) { @@ -563,7 +597,30 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ - main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION); + if (custom_sub_mode > 0) { + switch(custom_sub_mode) { + case PX4_CUSTOM_SUB_MODE_AUTO_LOITER: + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_LOITER); + break; + case PX4_CUSTOM_SUB_MODE_AUTO_MISSION: + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION); + break; + case PX4_CUSTOM_SUB_MODE_AUTO_RTL: + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_RTL); + break; + case PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF: + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF); + break; + + default: + main_ret = TRANSITION_DENIED; + mavlink_log_critical(mavlink_fd, "Unsupported auto mode"); + break; + } + + } else { + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION); + } } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) { /* ACRO */ @@ -763,8 +820,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } if (cmd_result == vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED) { - warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home->lat, home->lon, (double)home->alt); - mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home->lat, home->lon, (double)home->alt); + mavlink_and_console_log_info(mavlink_fd, "Home position: %.7f, %.7f, %.2f", home->lat, home->lon, (double)home->alt); /* announce new home position */ if (*home_pub != nullptr) { @@ -809,6 +865,17 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } break; + case vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF: { + /* ok, home set, use it to take off */ + if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF)) { + warnx("taking off!"); + } else { + warnx("takeoff denied"); + } + + } + break; + case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION: case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: @@ -878,8 +945,7 @@ static void commander_set_home_position(orb_advert_t &homePub, home_position_s & home.yaw = attitude.yaw; - warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); - mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); + mavlink_and_console_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); /* announce new home position */ if (homePub != nullptr) { @@ -945,6 +1011,9 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_disarm_land = param_find("COM_DISARM_LAND"); param_t _param_map_mode_sw = param_find("RC_MAP_MODE_SW"); + // These are too verbose, but we will retain them a little longer + // until we are sure we really don't need them. + // const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX]; // main_states_str[vehicle_status_s::MAIN_STATE_MANUAL] = "MANUAL"; // main_states_str[vehicle_status_s::MAIN_STATE_ALTCTL] = "ALTCTL"; @@ -956,33 +1025,34 @@ int commander_thread_main(int argc, char *argv[]) // main_states_str[vehicle_status_s::MAIN_STATE_STAB] = "STAB"; // main_states_str[vehicle_status_s::MAIN_STATE_OFFBOARD] = "OFFBOARD"; - const char *arming_states_str[vehicle_status_s::ARMING_STATE_MAX]; - arming_states_str[vehicle_status_s::ARMING_STATE_INIT] = "INIT"; - arming_states_str[vehicle_status_s::ARMING_STATE_STANDBY] = "STANDBY"; - arming_states_str[vehicle_status_s::ARMING_STATE_ARMED] = "ARMED"; - arming_states_str[vehicle_status_s::ARMING_STATE_ARMED_ERROR] = "ARMED_ERROR"; - arming_states_str[vehicle_status_s::ARMING_STATE_STANDBY_ERROR] = "STANDBY_ERROR"; - arming_states_str[vehicle_status_s::ARMING_STATE_REBOOT] = "REBOOT"; - arming_states_str[vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE"; + // const char *arming_states_str[vehicle_status_s::ARMING_STATE_MAX]; + // arming_states_str[vehicle_status_s::ARMING_STATE_INIT] = "INIT"; + // arming_states_str[vehicle_status_s::ARMING_STATE_STANDBY] = "STANDBY"; + // arming_states_str[vehicle_status_s::ARMING_STATE_ARMED] = "ARMED"; + // arming_states_str[vehicle_status_s::ARMING_STATE_ARMED_ERROR] = "ARMED_ERROR"; + // arming_states_str[vehicle_status_s::ARMING_STATE_STANDBY_ERROR] = "STANDBY_ERROR"; + // arming_states_str[vehicle_status_s::ARMING_STATE_REBOOT] = "REBOOT"; + // arming_states_str[vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE"; - const char *nav_states_str[vehicle_status_s::NAVIGATION_STATE_MAX]; - nav_states_str[vehicle_status_s::NAVIGATION_STATE_MANUAL] = "MANUAL"; - nav_states_str[vehicle_status_s::NAVIGATION_STATE_STAB] = "STAB"; - nav_states_str[vehicle_status_s::NAVIGATION_STATE_RATTITUDE] = "RATTITUDE"; - nav_states_str[vehicle_status_s::NAVIGATION_STATE_ALTCTL] = "ALTCTL"; - nav_states_str[vehicle_status_s::NAVIGATION_STATE_POSCTL] = "POSCTL"; - nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; - nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; - nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL"; - nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER] = "AUTO_RCRECOVER"; - nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS"; - nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL] = "AUTO_LANDENGFAIL"; - nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL] = "AUTO_LANDGPSFAIL"; - nav_states_str[vehicle_status_s::NAVIGATION_STATE_ACRO] = "ACRO"; - nav_states_str[vehicle_status_s::NAVIGATION_STATE_LAND] = "LAND"; - nav_states_str[vehicle_status_s::NAVIGATION_STATE_DESCEND] = "DESCEND"; - nav_states_str[vehicle_status_s::NAVIGATION_STATE_TERMINATION] = "TERMINATION"; - nav_states_str[vehicle_status_s::NAVIGATION_STATE_OFFBOARD] = "OFFBOARD"; + // const char *nav_states_str[vehicle_status_s::NAVIGATION_STATE_MAX]; + // nav_states_str[vehicle_status_s::NAVIGATION_STATE_MANUAL] = "MANUAL"; + // nav_states_str[vehicle_status_s::NAVIGATION_STATE_STAB] = "STAB"; + // nav_states_str[vehicle_status_s::NAVIGATION_STATE_RATTITUDE] = "RATTITUDE"; + // nav_states_str[vehicle_status_s::NAVIGATION_STATE_ALTCTL] = "ALTCTL"; + // nav_states_str[vehicle_status_s::NAVIGATION_STATE_POSCTL] = "POSCTL"; + // nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; + // nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; + // nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL"; + // nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF] = "AUTO_TAKEOFF"; + // nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER] = "AUTO_RCRECOVER"; + // nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS"; + // nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL] = "AUTO_LANDENGFAIL"; + // nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL] = "AUTO_LANDGPSFAIL"; + // nav_states_str[vehicle_status_s::NAVIGATION_STATE_ACRO] = "ACRO"; + // nav_states_str[vehicle_status_s::NAVIGATION_STATE_LAND] = "LAND"; + // nav_states_str[vehicle_status_s::NAVIGATION_STATE_DESCEND] = "DESCEND"; + // nav_states_str[vehicle_status_s::NAVIGATION_STATE_TERMINATION] = "TERMINATION"; + // nav_states_str[vehicle_status_s::NAVIGATION_STATE_OFFBOARD] = "OFFBOARD"; /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -1034,7 +1104,7 @@ int commander_thread_main(int argc, char *argv[]) // XXX for now just set sensors as initialized status.condition_system_sensors_initialized = true; - status.condition_system_prearm_error_reported = true; + status.condition_system_prearm_error_reported = false; status.condition_system_hotplug_timeout = false; status.counter++; @@ -1117,8 +1187,6 @@ int commander_thread_main(int argc, char *argv[]) bool updated = false; - rc_calibration_check(mavlink_fd, true); - /* Subscribe to safety topic */ int safety_sub = orb_subscribe(ORB_ID(safety)); memset(&safety, 0, sizeof(safety)); @@ -1304,7 +1372,7 @@ int commander_thread_main(int argc, char *argv[]) /* initialize low priority thread */ pthread_attr_t commander_low_prio_attr; pthread_attr_init(&commander_low_prio_attr); - pthread_attr_setstacksize(&commander_low_prio_attr, 2600); + pthread_attr_setstacksize(&commander_low_prio_attr, 2880); struct sched_param param; (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); @@ -1367,11 +1435,8 @@ int commander_thread_main(int argc, char *argv[]) if (map_mode_sw == 0 && map_mode_sw != map_mode_sw_new && map_mode_sw_new < input_rc_s::RC_INPUT_MAX_CHANNELS && map_mode_sw_new > 0) { status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, - !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout); + (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout); } - - /* re-check RC calibration */ - rc_calibration_check(mavlink_fd, true); } /* Safety parameters */ @@ -1471,11 +1536,11 @@ int commander_thread_main(int argc, char *argv[]) if (is_hil_setup(autostart_id)) { /* HIL configuration: check only RC input */ (void)Commander::preflightCheck(mavlink_fd, false, false, false, false, false, - !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), false, true); + (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), false, true); } else { /* check sensors also */ (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, - !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout); + (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout); } } @@ -1599,7 +1664,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status); status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab; - /* Make sure that this is only adjusted if vehicle really is of type vtol*/ + /* Make sure that this is only adjusted if vehicle really is of type vtol */ if (is_vtol(&status)) { status.is_rotary_wing = vtol_status.vtol_in_rw_mode; status.in_transition_mode = vtol_status.vtol_in_trans_mode; @@ -2015,12 +2080,12 @@ int commander_thread_main(int argc, char *argv[]) static bool flight_termination_printed = false; if (!flight_termination_printed) { - warnx("Flight termination because of navigator request"); + mavlink_and_console_log_critical(mavlink_fd, "Geofence violation: flight termination"); flight_termination_printed = true; } if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - mavlink_log_critical(mavlink_fd, "Flight termination active"); + mavlink_and_console_log_critical(mavlink_fd, "Flight termination active"); } } @@ -2043,7 +2108,6 @@ int commander_thread_main(int argc, char *argv[]) } else { /* the mission is valid */ tune_mission_ok(true); - warnx("mission ok"); } /* prevent further feedback until the mission changes */ @@ -2195,12 +2259,14 @@ int commander_thread_main(int argc, char *argv[]) /* report a regain */ if (telemetry_last_dl_loss[i] > 0) { mavlink_and_console_log_info(mavlink_fd, "data link #%i regained", i); - } else if (telemetry_last_dl_loss[i] == 0){ - /* do not report a new data link in order to not spam the user */ - status.data_link_found_new = true; - status_changed = true; + } else if (telemetry_last_dl_loss[i] == 0) { + /* new link */ } + /* got link again or new */ + status.condition_system_prearm_error_reported = false; + status_changed = true; + telemetry_lost[i] = false; have_link = true; @@ -2208,10 +2274,6 @@ int commander_thread_main(int argc, char *argv[]) /* telemetry was healthy also in last iteration * we don't have to check a timeout */ have_link = true; - if(status.data_link_found_new) { - status.data_link_found_new = false; - status_changed = true; - } } } else { @@ -2322,7 +2384,7 @@ int commander_thread_main(int argc, char *argv[]) if ((status.main_state ==vehicle_status_s::MAIN_STATE_ACRO || status.main_state ==vehicle_status_s::MAIN_STATE_RATTITUDE || status.main_state ==vehicle_status_s::MAIN_STATE_MANUAL || - status.main_state !=vehicle_status_s::MAIN_STATE_STAB || + status.main_state ==vehicle_status_s::MAIN_STATE_STAB || status.main_state ==vehicle_status_s::MAIN_STATE_ALTCTL || status.main_state ==vehicle_status_s::MAIN_STATE_POSCTL) && ((status.rc_signal_lost && status.gps_failure) || @@ -2350,7 +2412,7 @@ int commander_thread_main(int argc, char *argv[]) commander_set_home_position(home_pub, _home, local_position, global_position, attitude); } - /* update home position on arming if at least 1s from commander start spent to avoid setting home on in-air restart */ + /* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */ else if (((!was_armed && armed.armed) || (was_landed && !status.condition_landed)) && (now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) { commander_set_home_position(home_pub, _home, local_position, global_position, attitude); @@ -2362,7 +2424,6 @@ int commander_thread_main(int argc, char *argv[]) /* print new state */ if (arming_state_changed) { status_changed = true; - mavlink_log_info(mavlink_fd, "[cmd] arming state: %s", arming_states_str[status.arming_state]); arming_state_changed = false; } @@ -2387,7 +2448,6 @@ int commander_thread_main(int argc, char *argv[]) // TODO handle mode changes by commands if (main_state_changed || nav_state_changed) { status_changed = true; - mavlink_log_info(mavlink_fd, "Flight mode: %s", nav_states_str[status.nav_state]); main_state_changed = false; } @@ -2888,6 +2948,7 @@ set_control_mode() case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: + case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; @@ -3072,10 +3133,8 @@ void answer_command(struct vehicle_command_s &cmd, unsigned result) void *commander_low_prio_loop(void *arg) { -#if defined(__PX4_LINUX) || defined(__PX4_NUTTX) /* Set thread name */ - prctl(PR_SET_NAME, "commander_low_prio", getpid()); -#endif + px4_prctl(PR_SET_NAME, "commander_low_prio", getpid()); /* Subscribe to command topic */ int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -3250,7 +3309,7 @@ void *commander_low_prio_loop(void *arg) } status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, - !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout); + !(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout); arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */, mavlink_fd); diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index f411e1c128..e1a517538c 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -63,6 +63,9 @@ #include #include "commander_helper.h" +#include "DevMgr.hpp" + +using namespace DriverFramework; /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -93,12 +96,15 @@ bool is_vtol(const struct vehicle_status_s * current_status) { current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_OCTOROTOR); } -static int buzzer = -1; static hrt_abstime blink_msg_end = 0; // end time for currently blinking LED message, 0 if no blink message static hrt_abstime tune_end = 0; // end time of currently played tune, 0 for repeating tunes or silence static int tune_current = TONE_STOP_TUNE; // currently playing tune, can be interrupted after tune_end static unsigned int tune_durations[TONE_NUMBER_OF_TUNES]; +static DevHandle h_leds; +static DevHandle h_rgbleds; +static DevHandle h_buzzer; + static param_t bat_v_empty_h; static param_t bat_v_full_h; static param_t bat_n_cells_h; @@ -133,10 +139,10 @@ int buzzer_init() tune_durations[TONE_NOTIFY_NEUTRAL_TUNE] = 500000; tune_durations[TONE_ARMING_WARNING_TUNE] = 3000000; - buzzer = px4_open(TONEALARM0_DEVICE_PATH, O_WRONLY); + DevMgr::getHandle(TONEALARM0_DEVICE_PATH, h_buzzer); - if (buzzer < 0) { - warnx("Buzzer: px4_open fail\n"); + if (!h_buzzer.isValid()) { + PX4_WARN("Buzzer: px4_open fail\n"); return ERROR; } @@ -145,12 +151,12 @@ int buzzer_init() void buzzer_deinit() { - px4_close(buzzer); + DevMgr::releaseHandle(h_buzzer); } void set_tune_override(int tune) { - px4_ioctl(buzzer, TONE_SET_ALARM, tune); + h_buzzer.ioctl(TONE_SET_ALARM, tune); } void set_tune(int tune) @@ -161,7 +167,7 @@ void set_tune(int tune) if (tune_end == 0 || new_tune_duration != 0 || hrt_absolute_time() > tune_end) { /* allow interrupting current non-repeating tune by the same tune */ if (tune != tune_current || new_tune_duration != 0) { - px4_ioctl(buzzer, TONE_SET_ALARM, tune); + h_buzzer.ioctl(TONE_SET_ALARM, tune); } tune_current = tune; @@ -264,30 +270,27 @@ int blink_msg_state() } } -static int leds = -1; -static int rgbleds = -1; - int led_init() { blink_msg_end = 0; /* first open normal LEDs */ - leds = px4_open(LED0_DEVICE_PATH, 0); + DevMgr::getHandle(LED0_DEVICE_PATH, h_leds); - if (leds < 0) { - warnx("LED: px4_open fail\n"); + if (!h_leds.isValid()) { + PX4_WARN("LED: getHandle fail\n"); return ERROR; } /* the blue LED is only available on FMUv1 & AeroCore but not FMUv2 */ - (void)px4_ioctl(leds, LED_ON, LED_BLUE); + (void)h_leds.ioctl(LED_ON, LED_BLUE); /* switch blue off */ led_off(LED_BLUE); /* we consider the amber led mandatory */ - if (px4_ioctl(leds, LED_ON, LED_AMBER)) { - warnx("Amber LED: px4_ioctl fail\n"); + if (h_leds.ioctl(LED_ON, LED_AMBER)) { + PX4_WARN("Amber LED: ioctl fail\n"); return ERROR; } @@ -295,10 +298,11 @@ int led_init() led_off(LED_AMBER); /* then try RGB LEDs, this can fail on FMUv1*/ - rgbleds = px4_open(RGBLED0_DEVICE_PATH, 0); + DevHandle h; + DevMgr::getHandle(RGBLED0_DEVICE_PATH, h_rgbleds); - if (rgbleds < 0) { - warnx("No RGB LED found at " RGBLED0_DEVICE_PATH); + if (!h_rgbleds.isValid()) { + PX4_WARN("No RGB LED found at " RGBLED0_DEVICE_PATH); } return 0; @@ -306,64 +310,41 @@ int led_init() void led_deinit() { - if (leds >= 0) { - px4_close(leds); - } - - if (rgbleds >= 0) { - px4_close(rgbleds); - } + DevMgr::releaseHandle(h_leds); + DevMgr::releaseHandle(h_rgbleds); } int led_toggle(int led) { - if (leds < 0) { - return leds; - } - return px4_ioctl(leds, LED_TOGGLE, led); + return h_leds.ioctl(LED_TOGGLE, led); } int led_on(int led) { - if (leds < 0) { - return leds; - } - return px4_ioctl(leds, LED_ON, led); + return h_leds.ioctl(LED_ON, led); } int led_off(int led) { - if (leds < 0) { - return leds; - } - return px4_ioctl(leds, LED_OFF, led); + return h_leds.ioctl(LED_OFF, led); } void rgbled_set_color(rgbled_color_t color) { - if (rgbleds < 0) { - return; - } - px4_ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color); + h_rgbleds.ioctl(RGBLED_SET_COLOR, (unsigned long)color); } void rgbled_set_mode(rgbled_mode_t mode) { - if (rgbleds < 0) { - return; - } - px4_ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode); + h_rgbleds.ioctl(RGBLED_SET_MODE, (unsigned long)mode); } void rgbled_set_pattern(rgbled_pattern_t *pattern) { - if (rgbleds < 0) { - return; - } - px4_ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); + h_rgbleds.ioctl(RGBLED_SET_PATTERN, (unsigned long)pattern); } unsigned battery_get_n_cells() { diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 1c4108017f..4b06ecbdf4 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -55,6 +55,7 @@ * @group Radio Calibration * @min -0.25 * @max 0.25 + * @decimal 2 */ PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); @@ -69,6 +70,7 @@ PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); * @group Radio Calibration * @min -0.25 * @max 0.25 + * @decimal 2 */ PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); @@ -83,6 +85,7 @@ PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); * @group Radio Calibration * @min -0.25 * @max 0.25 + * @decimal 2 */ PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); @@ -93,6 +96,7 @@ PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); * * @group Battery Calibration * @unit V + * @decimal 2 */ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); @@ -103,6 +107,7 @@ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); * * @group Battery Calibration * @unit V + * @decimal 2 */ PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f); @@ -115,6 +120,8 @@ PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f); * @group Battery Calibration * @unit V * @min 0.0 + * @max 1.5 + * @decimal 2 */ PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f); @@ -137,6 +144,7 @@ PARAM_DEFINE_INT32(BAT_N_CELLS, 3); * * @group Battery Calibration * @unit mA + * @decimal 0 */ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); @@ -184,6 +192,7 @@ PARAM_DEFINE_INT32(COM_DL_REG_T, 0); * @group Commander * @min 0.0 * @max 1.0 + * @decimal 1 */ PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f); @@ -196,6 +205,7 @@ PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f); * @min 0.0 * @max 30.0 * @unit ampere + * @decimal 2 */ PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f); @@ -209,6 +219,7 @@ PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f); * @unit second * @min 0.0 * @max 60.0 + * @decimal 1 */ PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f); @@ -221,6 +232,7 @@ PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f); * @unit second * @min 0 * @max 35 + * @decimal 1 */ PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5f); @@ -233,6 +245,7 @@ PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5f); * @unit meter * @min 2 * @max 15 + * @decimal 2 */ PARAM_DEFINE_FLOAT(COM_HOME_H_T, 5.0f); @@ -245,6 +258,7 @@ PARAM_DEFINE_FLOAT(COM_HOME_H_T, 5.0f); * @unit meter * @min 5 * @max 25 + * @decimal 2 */ PARAM_DEFINE_FLOAT(COM_HOME_V_T, 10.0f); diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 54866d8cb3..1fcd48b088 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -122,7 +122,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data) } if (s == 0 && calibration_counter[0] % (calibration_count / 20) == 0) { - mavlink_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter[0] * 100) / calibration_count); + mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter[0] * 100) / calibration_count); } } @@ -131,14 +131,14 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data) } if (poll_errcount > 1000) { - mavlink_log_critical(worker_data->mavlink_fd, CAL_ERROR_SENSOR_MSG); + mavlink_and_console_log_critical(worker_data->mavlink_fd, CAL_ERROR_SENSOR_MSG); return calibrate_return_error; } } for (unsigned s = 0; s < max_gyros; s++) { if (worker_data->device_id[s] != 0 && calibration_counter[s] < calibration_count / 2) { - mavlink_log_critical(worker_data->mavlink_fd, "[cal] ERROR: missing data, sensor %d", s) + mavlink_and_console_log_critical(worker_data->mavlink_fd, "[cal] ERROR: missing data, sensor %d", s) return calibrate_return_error; } @@ -155,7 +155,7 @@ int do_gyro_calibration(int mavlink_fd) int res = OK; gyro_worker_data_t worker_data = {}; - mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); worker_data.mavlink_fd = mavlink_fd; @@ -179,7 +179,7 @@ int do_gyro_calibration(int mavlink_fd) (void)sprintf(str, "CAL_GYRO%u_ID", s); res = param_set_no_notification(param_find(str), &(worker_data.device_id[s])); if (res != OK) { - mavlink_log_critical(mavlink_fd, "[cal] Unable to reset CAL_GYRO%u_ID", s); + mavlink_and_console_log_critical(mavlink_fd, "[cal] Unable to reset CAL_GYRO%u_ID", s); return ERROR; } @@ -193,7 +193,7 @@ int do_gyro_calibration(int mavlink_fd) px4_close(fd); if (res != OK) { - mavlink_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s); return ERROR; } } @@ -238,7 +238,7 @@ int do_gyro_calibration(int mavlink_fd) float zdiff = worker_data.gyro_report_0.z - worker_data.gyro_scale[0].z_offset; /* maximum allowable calibration error in radians */ - const float maxoff = 0.0055f; + const float maxoff = 0.01f; if (!PX4_ISFINITE(worker_data.gyro_scale[0].x_offset) || !PX4_ISFINITE(worker_data.gyro_scale[0].y_offset) || @@ -302,7 +302,7 @@ int do_gyro_calibration(int mavlink_fd) px4_close(fd); if (res != OK) { - mavlink_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, 1); } } } @@ -325,7 +325,7 @@ int do_gyro_calibration(int mavlink_fd) res = param_save_default(); if (res != OK) { - mavlink_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG); } } @@ -333,9 +333,9 @@ int do_gyro_calibration(int mavlink_fd) usleep(200000); if (res == OK) { - mavlink_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name); } else { - mavlink_log_info(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name); } /* give this message enough time to propagate */ diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index bcdb5d3f9d..1bde387c63 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -65,6 +65,10 @@ #include "state_machine_helper.h" #include "commander_helper.h" #include "PreflightCheck.h" +#ifndef __PX4_NUTTX +#include "DevMgr.hpp" +using namespace DriverFramework; +#endif // This array defines the arming state transitions. The rows are the new state, and the columns // are the current state. Using new state and current state you can index into the array which @@ -123,6 +127,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s /* only perform the pre-arm check if we have to */ if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { + prearm_ret = preflight_check(status, mavlink_fd, true /* pre-arm */ ); } /* re-run the pre-flight check as long as sensors are failing */ @@ -130,6 +135,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { + prearm_ret = preflight_check(status, mavlink_fd, false /* pre-flight */); status->condition_system_sensors_initialized = !prearm_ret; } @@ -197,7 +203,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s // are measured but are insufficient if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f)) { // Check avionics rail voltages - if (status->avionics_power_rail_voltage < 4.75f) { + if (status->avionics_power_rail_voltage < 4.5f) { mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); feedback_provided = true; valid_transition = false; @@ -269,13 +275,10 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s /* reset feedback state */ if (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR && + status->arming_state != vehicle_status_s::ARMING_STATE_INIT && valid_transition) { status->condition_system_prearm_error_reported = false; } - if(status->data_link_found_new == true) - { - status->condition_system_prearm_error_reported = false; - } /* end of atomic state update */ #ifdef __PX4_NUTTX @@ -348,6 +351,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta case vehicle_status_s::MAIN_STATE_AUTO_MISSION: case vehicle_status_s::MAIN_STATE_AUTO_RTL: + case vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF: /* need global position and home position */ if (status->condition_global_position_valid && status->condition_home_position_valid) { ret = TRANSITION_CHANGED; @@ -475,6 +479,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta #else + // Handle VDev devices const char *devname; unsigned int handle = 0; for(;;) { @@ -482,9 +487,9 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta if (devname == NULL) break; - /* skip mavlink */ + /* skip mavlink */ if (!strcmp("/dev/mavlink", devname)) { - continue; + continue; } @@ -492,7 +497,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta if (sensfd < 0) { warn("failed opening device %s", devname); - continue; + continue; } int block_ret = px4_ioctl(sensfd, DEVIOCSPUBBLOCK, 1); @@ -501,6 +506,29 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR"); } + + // Handle DF devices + const char *df_dev_path; + unsigned int index = 0; + for(;;) { + if (DevMgr::getNextDeviceName(index, &df_dev_path) < 0) { + break; + } + + DevHandle h; + DevMgr::getHandle(df_dev_path, h); + + if (!h.isValid()) { + warn("failed opening device %s", df_dev_path); + continue; + } + + int block_ret = h.ioctl(DEVIOCSPUBBLOCK, 1); + DevMgr::releaseHandle(h); + + printf("Disabling %s: %s\n", df_dev_path, (block_ret == OK) ? "OK" : "ERROR"); + } + ret = TRANSITION_CHANGED; mavlink_and_console_log_critical(mavlink_fd, "Switched to ON hil state"); #endif @@ -717,6 +745,27 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } break; + case vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF: + /* require global position and home */ + + if (status->engine_failure) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; + } else if ((!status->condition_global_position_valid || + !status->condition_home_position_valid)) { + status->failsafe = true; + + if (status->condition_local_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + } + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF; + } + break; + case vehicle_status_s::MAIN_STATE_OFFBOARD: /* require offboard control, otherwise stay where you are */ if (status->offboard_control_signal_lost && !status->rc_signal_lost) { @@ -743,12 +792,12 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en return status->nav_state != nav_state_old; } -int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, bool prearm) +int preflight_check(struct vehicle_status_s *status, const int mavlink_fd, bool prearm, bool force_report) { /* */ - bool reportFailures = false; - reportFailures = !status->condition_system_prearm_error_reported && status->condition_system_hotplug_timeout; + bool reportFailures = force_report || (!status->condition_system_prearm_error_reported && + status->condition_system_hotplug_timeout); bool checkAirspeed = false; /* Perform airspeed check only if circuit breaker is not @@ -757,11 +806,20 @@ int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, checkAirspeed = true; } - bool preflight_ok = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status->circuit_breaker_engaged_gpsfailure_check, true, reportFailures); + bool preflight_ok = Commander::preflightCheck(mavlink_fd, true, true, true, true, + checkAirspeed, (status->rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), + !status->circuit_breaker_engaged_gpsfailure_check, true, reportFailures); if (!status->cb_usb && status->usb_connected && prearm) { preflight_ok = false; - if(reportFailures) mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited"); + if (reportFailures) { + mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited"); + } + } + + /* report once, then set the flag */ + if (mavlink_fd >= 0 && reportFailures && !preflight_ok) { + status->condition_system_prearm_error_reported = true; } return !preflight_ok; diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index bdb7b13f28..7c209cf07f 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -65,6 +65,6 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, const bool stay_in_failsafe); -int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, bool prearm); +int preflight_check(struct vehicle_status_s *status, const int mavlink_fd, bool prearm, bool force_report=false); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index a29a29c735..d5e39dbfd3 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -89,7 +89,7 @@ typedef enum { } dm_reset_reason; /** Maximum size in bytes of a single item instance */ -#define DM_MAX_DATA_SIZE 124 +#define DM_MAX_DATA_SIZE 126 /** Retrieve from the data manager store */ __EXPORT ssize_t diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index 1429979d6d..fc11e0f505 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -69,6 +69,7 @@ #include #include +#include #include #include #include "estimator_22states.h" @@ -278,6 +279,8 @@ private: AttPosEKF *_ekf; + TerrainEstimator *_terrain_estimator; + /* Low pass filter for attitude rates */ math::LowPassFilter2p _LP_att_P; math::LowPassFilter2p _LP_att_Q; 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 f4fcf78e25..f9b0b1cbca 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 @@ -74,9 +74,12 @@ static uint64_t IMUusec = 0; static constexpr float rc = 10.0f; // RC time constant of 1st order LPF in seconds static constexpr uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; ///< units: microseconds static constexpr float POS_RESET_THRESHOLD = 5.0f; ///< Seconds before we signal a total GPS failure -static constexpr unsigned MAG_SWITCH_HYSTERESIS = 10; ///< Ignore the first few mag failures (which amounts to a few milliseconds) -static constexpr unsigned GYRO_SWITCH_HYSTERESIS = 5; ///< Ignore the first few gyro failures (which amounts to a few milliseconds) -static constexpr unsigned ACCEL_SWITCH_HYSTERESIS = 5; ///< Ignore the first few accel failures (which amounts to a few milliseconds) +static constexpr unsigned MAG_SWITCH_HYSTERESIS = + 10; ///< Ignore the first few mag failures (which amounts to a few milliseconds) +static constexpr unsigned GYRO_SWITCH_HYSTERESIS = + 5; ///< Ignore the first few gyro failures (which amounts to a few milliseconds) +static constexpr unsigned ACCEL_SWITCH_HYSTERESIS = + 5; ///< Ignore the first few accel failures (which amounts to a few milliseconds) static constexpr float EPH_LARGE_VALUE = 1000.0f; static constexpr float EPV_LARGE_VALUE = 1000.0f; @@ -118,7 +121,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _task_running(false), _estimator_task(-1), -/* subscriptions */ + /* subscriptions */ _sensor_combined_sub(-1), _distance_sub(-1), _airspeed_sub(-1), @@ -163,7 +166,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _sensor_combined{}, - _pos_ref{}, + _pos_ref{}, _filter_ref_offset(0.0f), _baro_gps_offset(0.0f), @@ -177,7 +180,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")), _perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")), - /* states */ + /* states */ _gps_alt_filt(0.0f), _baro_alt_filt(0.0f), _covariancePredictionDt(0.0f), @@ -212,6 +215,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _parameters{}, _parameter_handles{}, _ekf(nullptr), + _terrain_estimator(nullptr), _LP_att_P(250.0f, 20.0f), _LP_att_Q(250.0f, 20.0f), @@ -219,6 +223,8 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : { _voter_mag.set_timeout(200000); + _terrain_estimator = new TerrainEstimator(); + _parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS"); _parameter_handles.pos_delay_ms = param_find("PE_POS_DELAY_MS"); _parameter_handles.height_delay_ms = param_find("PE_HGT_DELAY_MS"); @@ -268,6 +274,7 @@ AttitudePositionEstimatorEKF::~AttitudePositionEstimatorEKF() } while (_estimator_task != -1); } + delete _terrain_estimator; delete _ekf; estimator::g_estimator = nullptr; @@ -321,13 +328,13 @@ int AttitudePositionEstimatorEKF::parameters_update() _ekf->accelProcessNoise = _parameters.acc_pnoise; _ekf->airspeedMeasurementSigma = _parameters.eas_noise; _ekf->rngFinderPitch = 0.0f; // XXX base on SENS_BOARD_Y_OFF - #if 0 +#if 0 // Initially disable loading until // convergence is flight-test proven _ekf->magBias.x = _mag_offset_x.get(); _ekf->magBias.y = _mag_offset_y.get(); _ekf->magBias.z = _mag_offset_z.get(); - #endif +#endif } return OK; @@ -450,18 +457,18 @@ int AttitudePositionEstimatorEKF::check_filter_state() if (rep.health_flags < ((1 << 0) | (1 << 1) | (1 << 2) | (1 << 3))) { PX4_INFO("health: VEL:%s POS:%s HGT:%s OFFS:%s", - ((rep.health_flags & (1 << 0)) ? "OK" : "ERR"), - ((rep.health_flags & (1 << 1)) ? "OK" : "ERR"), - ((rep.health_flags & (1 << 2)) ? "OK" : "ERR"), - ((rep.health_flags & (1 << 3)) ? "OK" : "ERR")); + ((rep.health_flags & (1 << 0)) ? "OK" : "ERR"), + ((rep.health_flags & (1 << 1)) ? "OK" : "ERR"), + ((rep.health_flags & (1 << 2)) ? "OK" : "ERR"), + ((rep.health_flags & (1 << 3)) ? "OK" : "ERR")); } if (rep.timeout_flags) { PX4_INFO("timeout: %s%s%s%s", - ((rep.timeout_flags & (1 << 0)) ? "VEL " : ""), - ((rep.timeout_flags & (1 << 1)) ? "POS " : ""), - ((rep.timeout_flags & (1 << 2)) ? "HGT " : ""), - ((rep.timeout_flags & (1 << 3)) ? "IMU " : "")); + ((rep.timeout_flags & (1 << 0)) ? "VEL " : ""), + ((rep.timeout_flags & (1 << 1)) ? "POS " : ""), + ((rep.timeout_flags & (1 << 2)) ? "HGT " : ""), + ((rep.timeout_flags & (1 << 3)) ? "IMU " : "")); } } @@ -473,7 +480,7 @@ int AttitudePositionEstimatorEKF::check_filter_state() // Copy diagonal elemnts of covariance matrix float covariances[28]; _ekf->get_covariance(covariances); - + for (size_t i = 0; i < rep.n_states; i++) { rep.states[i] = ekf_report.states[i]; rep.covariances[i] = covariances[i]; @@ -523,9 +530,6 @@ void AttitudePositionEstimatorEKF::task_main() _landDetectorSub = orb_subscribe(ORB_ID(vehicle_land_detected)); _armedSub = orb_subscribe(ORB_ID(actuator_armed)); - /* rate limit vehicle status updates to 5Hz */ - orb_set_interval(_vstatus_sub, 200); - _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); /* sets also parameters in the EKF object */ @@ -645,6 +649,7 @@ void AttitudePositionEstimatorEKF::task_main() if (_gpsIsGood) { _baro_gps_offset = _baro_alt_filt - _gps_alt_filt; } + // if (hrt_elapsed_time(&_last_debug_print) >= 5e6) { // _last_debug_print = hrt_absolute_time(); // perf_print_counter(_perf_baro); @@ -697,6 +702,10 @@ void AttitudePositionEstimatorEKF::task_main() // Run EKF data fusion steps updateSensorFusion(_gpsIsGood, _newDataMag, _newRangeData, _newHgtData, _newAdsData); + // Run separate terrain estimator + _terrain_estimator->predict(_ekf->dtIMU, &_att, &_sensor_combined, &_distance); + _terrain_estimator->measurement_update(hrt_absolute_time(), &_gps, &_distance, &_att); + // Publish attitude estimations publishAttitude(); @@ -729,13 +738,15 @@ void AttitudePositionEstimatorEKF::task_main() } void AttitudePositionEstimatorEKF::initReferencePosition(hrt_abstime timestamp, - bool gps_valid, double lat, double lon, float gps_alt, float baro_alt) + bool gps_valid, double lat, double lon, float gps_alt, float baro_alt) { // Reference altitude if (PX4_ISFINITE(_ekf->states[9])) { _filter_ref_offset = _ekf->states[9]; + } else if (PX4_ISFINITE(-_ekf->hgtMea)) { _filter_ref_offset = -_ekf->hgtMea; + } else { _filter_ref_offset = -_baro.altitude; } @@ -791,11 +802,11 @@ void AttitudePositionEstimatorEKF::initializeGPS() #if 0 PX4_INFO("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, - (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); + (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); PX4_INFO("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, - (double)_filter_ref_offset); + (double)_filter_ref_offset); PX4_INFO("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, - (double)math::degrees(declination)); + (double)math::degrees(declination)); #endif _gps_initialized = true; @@ -852,7 +863,9 @@ void AttitudePositionEstimatorEKF::publishControlState() /* Accelerations in Body Frame */ _ctrl_state.x_acc = _ekf->accel.x; _ctrl_state.y_acc = _ekf->accel.y; - _ctrl_state.z_acc = _ekf->accel.z; + _ctrl_state.z_acc = _ekf->accel.z - _ekf->states[13]; + + _ctrl_state.horz_acc_mag = _ekf->getAccNavMagHorizontal(); /* Velocity in Body Frame */ Vector3f v_n(_ekf->states[4], _ekf->states[5], _ekf->states[6]); @@ -887,7 +900,17 @@ void AttitudePositionEstimatorEKF::publishControlState() _ctrl_state.q[3] = _ekf->states[3]; /* Airspeed (Groundspeed - Windspeed) */ - _ctrl_state.airspeed = sqrt(pow(_ekf->states[4] - _ekf->states[14], 2) + pow(_ekf->states[5] - _ekf->states[15], 2) + pow(_ekf->states[6], 2)); + //_ctrl_state.airspeed = sqrt(pow(_ekf->states[4] - _ekf->states[14], 2) + pow(_ekf->states[5] - _ekf->states[15], 2) + pow(_ekf->states[6], 2)); + // the line above was introduced by the control state PR. The airspeed it gives is totally wrong and leads to horrible flight performance in SITL + // and in outdoor tests + if (PX4_ISFINITE(_airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - _airspeed.timestamp < 1e6 + && _airspeed.timestamp > 0) { + _ctrl_state.airspeed = _airspeed.indicated_airspeed_m_s; + _ctrl_state.airspeed_valid = true; + + } else { + _ctrl_state.airspeed_valid = false; + } /* Attitude Rates */ _ctrl_state.roll_rate = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt; @@ -896,12 +919,11 @@ void AttitudePositionEstimatorEKF::publishControlState() /* Guard from bad data */ if (!PX4_ISFINITE(_ctrl_state.x_vel) || - !PX4_ISFINITE(_ctrl_state.y_vel) || - !PX4_ISFINITE(_ctrl_state.z_vel) || - !PX4_ISFINITE(_ctrl_state.x_pos) || - !PX4_ISFINITE(_ctrl_state.y_pos) || - !PX4_ISFINITE(_ctrl_state.z_pos)) - { + !PX4_ISFINITE(_ctrl_state.y_vel) || + !PX4_ISFINITE(_ctrl_state.z_vel) || + !PX4_ISFINITE(_ctrl_state.x_pos) || + !PX4_ISFINITE(_ctrl_state.y_pos) || + !PX4_ISFINITE(_ctrl_state.z_pos)) { // bad data, abort publication return; } @@ -941,12 +963,11 @@ void AttitudePositionEstimatorEKF::publishLocalPosition() _local_pos.yaw = _att.yaw; if (!PX4_ISFINITE(_local_pos.x) || - !PX4_ISFINITE(_local_pos.y) || - !PX4_ISFINITE(_local_pos.z) || - !PX4_ISFINITE(_local_pos.vx) || - !PX4_ISFINITE(_local_pos.vy) || - !PX4_ISFINITE(_local_pos.vz)) - { + !PX4_ISFINITE(_local_pos.y) || + !PX4_ISFINITE(_local_pos.z) || + !PX4_ISFINITE(_local_pos.vx) || + !PX4_ISFINITE(_local_pos.vy) || + !PX4_ISFINITE(_local_pos.vz)) { // bad data, abort publication return; } @@ -972,6 +993,7 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() _global_pos.lat = est_lat; _global_pos.lon = est_lon; _global_pos.time_utc_usec = _gps.time_utc_usec; + } else { _global_pos.lat = 0.0; _global_pos.lon = 0.0; @@ -992,14 +1014,22 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() if (_local_pos.v_z_valid) { _global_pos.vel_d = _local_pos.vz; + } else { _global_pos.vel_d = 0.0f; } /* terrain altitude */ - _global_pos.terrain_alt = _ekf->hgtRef - _ekf->flowStates[1]; - _global_pos.terrain_alt_valid = (_distance_last_valid > 0) && - (hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000); + if (_terrain_estimator->is_valid()) { + _global_pos.terrain_alt = _global_pos.alt - _terrain_estimator->get_distance_to_ground(); + _global_pos.terrain_alt_valid = true; + + } else { + _global_pos.terrain_alt_valid = false; + } + + /* baro altitude */ + _global_pos.pressure_alt = _ekf->baroHgt; _global_pos.yaw = _local_pos.yaw; @@ -1007,8 +1037,8 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() if (!_local_pos.xy_global || !_local_pos.v_xy_valid || - _gps.timestamp_position == 0 || - (dtLastGoodGPS >= POS_RESET_THRESHOLD)) { + _gps.timestamp_position == 0 || + (dtLastGoodGPS >= POS_RESET_THRESHOLD)) { _global_pos.eph = EPH_LARGE_VALUE; _global_pos.epv = EPV_LARGE_VALUE; @@ -1019,12 +1049,11 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() } if (!PX4_ISFINITE(_global_pos.lat) || - !PX4_ISFINITE(_global_pos.lon) || - !PX4_ISFINITE(_global_pos.alt) || - !PX4_ISFINITE(_global_pos.vel_n) || - !PX4_ISFINITE(_global_pos.vel_e) || - !PX4_ISFINITE(_global_pos.vel_d)) - { + !PX4_ISFINITE(_global_pos.lon) || + !PX4_ISFINITE(_global_pos.alt) || + !PX4_ISFINITE(_global_pos.vel_n) || + !PX4_ISFINITE(_global_pos.vel_e) || + !PX4_ISFINITE(_global_pos.vel_d)) { // bad data, abort publication return; } @@ -1081,6 +1110,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const if (_prediction_steps < MAX_PREDICITION_STEPS && ((hrt_absolute_time() - _prediction_last) < 20 * 1000)) { _prediction_steps++; return; + } else { _prediction_steps = 0; _prediction_last = hrt_absolute_time(); @@ -1200,11 +1230,11 @@ int AttitudePositionEstimatorEKF::start() /* start the task */ _estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 20, - 4600, - (px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 20, + 4600, + (px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline, + nullptr); if (_estimator_task < 0) { warn("task start failed"); @@ -1221,7 +1251,7 @@ void AttitudePositionEstimatorEKF::print_status() math::Vector<3> euler = R.to_euler(); PX4_INFO("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n", - (double)math::degrees(euler(0)), (double)math::degrees(euler(1)), (double)math::degrees(euler(2))); + (double)math::degrees(euler(0)), (double)math::degrees(euler(1)), (double)math::degrees(euler(2))); // State vector: // 0-3: quaternions (q0, q1, q2, q3) @@ -1237,51 +1267,51 @@ void AttitudePositionEstimatorEKF::print_status() PX4_INFO("alt RAW: baro alt: %8.4f GPS alt: %8.4f", (double)_baro.altitude, (double)_ekf->gpsHgt); PX4_INFO("alt EST: local alt: %8.4f (NED), AMSL alt: %8.4f (ENU)", (double)(_local_pos.z), (double)_global_pos.alt); PX4_INFO("filter ref offset: %8.4f baro GPS offset: %8.4f", (double)_filter_ref_offset, - (double)_baro_gps_offset); + (double)_baro_gps_offset); PX4_INFO("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, - (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); + (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); PX4_INFO("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, - (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, - (double)_ekf->correctedDelAng.z); + (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, + (double)_ekf->correctedDelAng.z); PX4_INFO("states (quat) [0-3]: %8.4f, %8.4f, %8.4f, %8.4f", (double)_ekf->states[0], (double)_ekf->states[1], - (double)_ekf->states[2], (double)_ekf->states[3]); + (double)_ekf->states[2], (double)_ekf->states[3]); PX4_INFO("states (vel m/s) [4-6]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[4], (double)_ekf->states[5], - (double)_ekf->states[6]); + (double)_ekf->states[6]); PX4_INFO("states (pos m) [7-9]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[7], (double)_ekf->states[8], - (double)_ekf->states[9]); + (double)_ekf->states[9]); PX4_INFO("states (delta ang) [10-12]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[10], (double)_ekf->states[11], - (double)_ekf->states[12]); + (double)_ekf->states[12]); if (EKF_STATE_ESTIMATES == 23) { PX4_INFO("states (accel offs) [13]: %8.4f", (double)_ekf->states[13]); PX4_INFO("states (wind) [14-15]: %8.4f, %8.4f", (double)_ekf->states[14], (double)_ekf->states[15]); PX4_INFO("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[16], (double)_ekf->states[17], - (double)_ekf->states[18]); + (double)_ekf->states[18]); PX4_INFO("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[19], (double)_ekf->states[20], - (double)_ekf->states[21]); + (double)_ekf->states[21]); PX4_INFO("states (terrain) [22]: %8.4f", (double)_ekf->states[22]); } else { PX4_INFO("states (accel offs) [13]: %8.4f", (double)_ekf->states[13]); PX4_INFO("states (wind) [14-15]: %8.4f, %8.4f", (double)_ekf->states[14], (double)_ekf->states[15]); PX4_INFO("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[16], (double)_ekf->states[17], - (double)_ekf->states[18]); + (double)_ekf->states[18]); PX4_INFO("states (mag bias) [19-21]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[19], (double)_ekf->states[20], - (double)_ekf->states[21]); + (double)_ekf->states[21]); } PX4_INFO("states: %s %s %s %s %s %s %s %s %s %s", - (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT", - (_landDetector.landed) ? "ON_GROUND" : "AIRBORNE", - (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL", - (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS", - (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT", - (_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG", - (_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", - (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", - (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS", - (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); + (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT", + (_landDetector.landed) ? "ON_GROUND" : "AIRBORNE", + (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL", + (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS", + (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT", + (_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG", + (_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", + (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", + (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS", + (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); PX4_INFO("gyro status:"); _voter_gyro.print(); @@ -1312,6 +1342,7 @@ void AttitudePositionEstimatorEKF::pollData() if (PX4_ISFINITE(_ekf->dtIMUfilt) && _ekf->dtIMUfilt < 0.5f && _ekf->dtIMUfilt > 0.0001f) { deltaT = _ekf->dtIMUfilt; + } else { deltaT = 0.01f; } @@ -1324,16 +1355,17 @@ void AttitudePositionEstimatorEKF::pollData() for (unsigned i = 0; i < (sizeof(_sensor_combined.gyro_timestamp) / sizeof(_sensor_combined.gyro_timestamp[0])); i++) { _voter_gyro.put(i, _sensor_combined.gyro_timestamp[i], &_sensor_combined.gyro_rad_s[i * 3], - _sensor_combined.gyro_errcount[i], _sensor_combined.gyro_priority[i]); + _sensor_combined.gyro_errcount[i], _sensor_combined.gyro_priority[i]); _voter_accel.put(i, _sensor_combined.accelerometer_timestamp[i], &_sensor_combined.accelerometer_m_s2[i * 3], - _sensor_combined.accelerometer_errcount[i], _sensor_combined.accelerometer_priority[i]); + _sensor_combined.accelerometer_errcount[i], _sensor_combined.accelerometer_priority[i]); _voter_mag.put(i, _sensor_combined.magnetometer_timestamp[i], &_sensor_combined.magnetometer_ga[i * 3], - _sensor_combined.magnetometer_errcount[i], _sensor_combined.magnetometer_priority[i]); + _sensor_combined.magnetometer_errcount[i], _sensor_combined.magnetometer_priority[i]); } // Get best measurement values hrt_abstime curr_time = hrt_absolute_time(); (void)_voter_gyro.get_best(curr_time, &_gyro_main); + if (_gyro_main >= 0) { // Use pre-integrated values if possible @@ -1341,12 +1373,15 @@ void AttitudePositionEstimatorEKF::pollData() _ekf->dAngIMU.x = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 0]; _ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 1]; _ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 2]; + } else { float dt_gyro = _sensor_combined.gyro_integral_dt[_gyro_main] / 1e6f; + if (PX4_ISFINITE(dt_gyro) && (dt_gyro < 0.5f) && (dt_gyro > 0.00001f)) { deltaT = dt_gyro; _ekf->dtIMU = deltaT; } + _ekf->dAngIMU.x = 0.5f * (_ekf->angRate.x + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0]) * _ekf->dtIMU; _ekf->dAngIMU.y = 0.5f * (_ekf->angRate.y + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 1]) * _ekf->dtIMU; _ekf->dAngIMU.z = 0.5f * (_ekf->angRate.z + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2]) * _ekf->dtIMU; @@ -1359,6 +1394,7 @@ void AttitudePositionEstimatorEKF::pollData() } (void)_voter_accel.get_best(curr_time, &_accel_main); + if (_accel_main >= 0 && (_last_accel != _sensor_combined.accelerometer_timestamp[_accel_main])) { // Use pre-integrated values if possible @@ -1366,6 +1402,7 @@ void AttitudePositionEstimatorEKF::pollData() _ekf->dVelIMU.x = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 0]; _ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 1]; _ekf->dVelIMU.z = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 2]; + } else { _ekf->dVelIMU.x = 0.5f * (_ekf->accel.x + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]) * _ekf->dtIMU; _ekf->dVelIMU.y = 0.5f * (_ekf->accel.y + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1]) * _ekf->dtIMU; @@ -1379,9 +1416,10 @@ void AttitudePositionEstimatorEKF::pollData() } (void)_voter_mag.get_best(curr_time, &_mag_main); + if (_mag_main >= 0) { Vector3f mag(_sensor_combined.magnetometer_ga[_mag_main * 3 + 0], _sensor_combined.magnetometer_ga[_mag_main * 3 + 1], - _sensor_combined.magnetometer_ga[_mag_main * 3 + 2]); + _sensor_combined.magnetometer_ga[_mag_main * 3 + 2]); /* fail over to the 2nd mag if we know the first is down */ if (mag.length() > 0.1f && (_last_mag != _sensor_combined.magnetometer_timestamp[_mag_main])) { @@ -1395,26 +1433,28 @@ void AttitudePositionEstimatorEKF::pollData() } 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; } @@ -1423,7 +1463,7 @@ void AttitudePositionEstimatorEKF::pollData() // XXX this is for assessing the filter performance // leave this in as long as larger improvements are still being made. - #if 0 +#if 0 float deltaTIntegral = (_sensor_combined.gyro_integral_dt[0]) / 1e6f; float deltaTIntAcc = (_sensor_combined.accelerometer_integral_dt[0]) / 1e6f; @@ -1434,32 +1474,32 @@ void AttitudePositionEstimatorEKF::pollData() if (hrt_elapsed_time(&lastprint) > 1000000 || _sensor_combined.gyro_rad_s[0] > 4.0f) { PX4_WARN("dt: %8.6f, dtg: %8.6f, dta: %8.6f, dt > 5 ms: %u, dt > 10 ms: %u", - (double)deltaT, (double)deltaTIntegral, (double)deltaTIntAcc, - dtoverflow5, dtoverflow10); + (double)deltaT, (double)deltaTIntegral, (double)deltaTIntAcc, + dtoverflow5, dtoverflow10); PX4_WARN("EKF: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f", - (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.z, - (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z); + (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.z, + (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z); PX4_WARN("INT: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f", - (double)(_sensor_combined.gyro_integral_rad[0]), (double)(_sensor_combined.gyro_integral_rad[2]), - (double)(_sensor_combined.accelerometer_integral_m_s[0]), - (double)(_sensor_combined.accelerometer_integral_m_s[1]), - (double)(_sensor_combined.accelerometer_integral_m_s[2])); + (double)(_sensor_combined.gyro_integral_rad[0]), (double)(_sensor_combined.gyro_integral_rad[2]), + (double)(_sensor_combined.accelerometer_integral_m_s[0]), + (double)(_sensor_combined.accelerometer_integral_m_s[1]), + (double)(_sensor_combined.accelerometer_integral_m_s[2])); PX4_WARN("DRV: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f", - (double)(_sensor_combined.gyro_rad_s[0] * deltaT), (double)(_sensor_combined.gyro_rad_s[2] * deltaT), - (double)(_sensor_combined.accelerometer_m_s2[0] * deltaT), - (double)(_sensor_combined.accelerometer_m_s2[1] * deltaT), - (double)(_sensor_combined.accelerometer_m_s2[2] * deltaT)); + (double)(_sensor_combined.gyro_rad_s[0] * deltaT), (double)(_sensor_combined.gyro_rad_s[2] * deltaT), + (double)(_sensor_combined.accelerometer_m_s2[0] * deltaT), + (double)(_sensor_combined.accelerometer_m_s2[1] * deltaT), + (double)(_sensor_combined.accelerometer_m_s2[2] * deltaT)); PX4_WARN("EKF rate: %8.4f, %8.4f, %8.4f", - (double)_att.rollspeed, (double)_att.pitchspeed, (double)_att.yawspeed); + (double)_att.rollspeed, (double)_att.pitchspeed, (double)_att.yawspeed); PX4_WARN("DRV rate: %8.4f, %8.4f, %8.4f", - (double)_sensor_combined.gyro_rad_s[0], - (double)_sensor_combined.gyro_rad_s[1], - (double)_sensor_combined.gyro_rad_s[2]); + (double)_sensor_combined.gyro_rad_s[0], + (double)_sensor_combined.gyro_rad_s[1], + (double)_sensor_combined.gyro_rad_s[2]); lastprint = hrt_absolute_time(); } @@ -1471,7 +1511,8 @@ void AttitudePositionEstimatorEKF::pollData() if (deltaT > 0.01f) { dtoverflow10++; } - #endif + +#endif _data_good = true; @@ -1631,6 +1672,7 @@ void AttitudePositionEstimatorEKF::pollData() } baro_last = _baro.timestamp; + if (!_baro_init) { _baro_init = true; _baro_alt_filt = _baro.altitude; @@ -1653,8 +1695,9 @@ void AttitudePositionEstimatorEKF::pollData() if (_newRangeData) { orb_copy(ORB_ID(distance_sensor), _distance_sub, &_distance); + if ((_distance.current_distance > _distance.min_distance) - && (_distance.current_distance < _distance.max_distance)) { + && (_distance.current_distance < _distance.max_distance)) { _ekf->rngMea = _distance.current_distance; _distance_last_valid = _distance.timestamp; diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c index 45e2f24939..208449790b 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c @@ -202,7 +202,7 @@ PARAM_DEFINE_FLOAT(PE_GYRO_PNOISE, 0.015f); * @max 1.0 * @group Position Estimator */ -PARAM_DEFINE_FLOAT(PE_ACC_PNOISE, 0.25f); +PARAM_DEFINE_FLOAT(PE_ACC_PNOISE, 0.125f); /** * Gyro bias estimate process noise diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index 6eb325d531..c0a51afc0a 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -382,6 +382,8 @@ public: void get_covariance(float c[28]); + float getAccNavMagHorizontal() { return _accNavMagHorizontal; } + protected: /** diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 3ed5b3a5a3..b5e49d67c8 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -44,7 +44,7 @@ #include #include #include -#include +#include #include #include #include @@ -59,6 +59,7 @@ #include #include #include +#include #include #include #include @@ -82,6 +83,7 @@ #include #include #include +#include #include /** @@ -141,6 +143,7 @@ private: orb_id_t _rates_sp_id; // pointer to correct rates setpoint uORB metadata structure orb_id_t _actuators_id; // pointer to correct actuator controls0 uORB metadata structure + orb_id_t _attitude_setpoint_id; struct control_state_s _ctrl_state; /**< control state */ struct accel_report _accel; /**< body frame accelerations */ @@ -160,14 +163,19 @@ private: bool _setpoint_valid; /**< flag if the position control setpoint is valid */ bool _debug; /**< if set to true, print debug output */ + float _flaps_cmd_last; + float _flaperons_cmd_last; + + struct { - float tconst; + float p_tc; float p_p; float p_i; float p_ff; float p_rmax_pos; float p_rmax_neg; float p_integrator_max; + float r_tc; float r_p; float r_i; float r_ff; @@ -181,6 +189,11 @@ private: float y_coordinated_min_speed; int32_t y_coordinated_method; float y_rmax; + float w_p; + float w_i; + float w_ff; + float w_integrator_max; + float w_rmax; float airspeed_min; float airspeed_trim; @@ -196,19 +209,23 @@ private: float man_roll_max; /**< Max Roll in rad */ float man_pitch_max; /**< Max Pitch in rad */ + float flaps_scale; /**< Scale factor for flaps */ + float flaperon_scale; /**< Scale factor for flaperons */ + int vtol_type; /**< VTOL type: 0 = tailsitter, 1 = tiltrotor */ } _parameters; /**< local copies of interesting parameters */ struct { - param_t tconst; + param_t p_tc; param_t p_p; param_t p_i; param_t p_ff; param_t p_rmax_pos; param_t p_rmax_neg; param_t p_integrator_max; + param_t r_tc; param_t r_p; param_t r_i; param_t r_ff; @@ -222,6 +239,11 @@ private: param_t y_coordinated_min_speed; param_t y_coordinated_method; param_t y_rmax; + param_t w_p; + param_t w_i; + param_t w_ff; + param_t w_integrator_max; + param_t w_rmax; param_t airspeed_min; param_t airspeed_trim; @@ -235,6 +257,9 @@ private: param_t man_roll_max; param_t man_pitch_max; + param_t flaps_scale; + param_t flaperon_scale; + param_t vtol_type; } _parameter_handles; /**< handles for interesting parameters */ @@ -248,6 +273,7 @@ private: ECL_RollController _roll_ctrl; ECL_PitchController _pitch_ctrl; ECL_YawController _yaw_ctrl; + ECL_WheelController _wheel_ctrl; /** @@ -321,7 +347,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _task_running(false), _control_task(-1), -/* subscriptions */ + /* subscriptions */ _ctrl_state_sub(-1), _accel_sub(-1), _vcontrol_mode_sub(-1), @@ -330,7 +356,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _global_pos_sub(-1), _vehicle_status_sub(-1), -/* publications */ + /* publications */ _rate_sp_pub(nullptr), _attitude_sp_pub(nullptr), _actuators_0_pub(nullptr), @@ -338,14 +364,17 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _rates_sp_id(0), _actuators_id(0), + _attitude_setpoint_id(0), -/* performance counters */ + /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")), _nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")), -/* states */ + /* states */ _setpoint_valid(false), - _debug(false) + _debug(false), + _flaps_cmd_last(0), + _flaperons_cmd_last(0) { /* safely initialize structs */ _ctrl_state = {}; @@ -360,7 +389,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _vehicle_status = {}; - _parameter_handles.tconst = param_find("FW_ATT_TC"); + _parameter_handles.p_tc = param_find("FW_P_TC"); _parameter_handles.p_p = param_find("FW_PR_P"); _parameter_handles.p_i = param_find("FW_PR_I"); _parameter_handles.p_ff = param_find("FW_PR_FF"); @@ -368,6 +397,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG"); _parameter_handles.p_integrator_max = param_find("FW_PR_IMAX"); + _parameter_handles.r_tc = param_find("FW_R_TC"); _parameter_handles.r_p = param_find("FW_RR_P"); _parameter_handles.r_i = param_find("FW_RR_I"); _parameter_handles.r_ff = param_find("FW_RR_FF"); @@ -380,6 +410,12 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.y_integrator_max = param_find("FW_YR_IMAX"); _parameter_handles.y_rmax = param_find("FW_Y_RMAX"); + _parameter_handles.w_p = param_find("FW_WR_P"); + _parameter_handles.w_i = param_find("FW_WR_I"); + _parameter_handles.w_ff = param_find("FW_WR_FF"); + _parameter_handles.w_integrator_max = param_find("FW_WR_IMAX"); + _parameter_handles.w_rmax = param_find("FW_W_RMAX"); + _parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN"); _parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM"); _parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX"); @@ -396,6 +432,9 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.man_roll_max = param_find("FW_MAN_R_MAX"); _parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX"); + _parameter_handles.flaps_scale = param_find("FW_FLAPS_SCL"); + _parameter_handles.flaperon_scale = param_find("FW_FLAPERON_SCL"); + _parameter_handles.vtol_type = param_find("VT_TYPE"); /* fetch initial parameter values */ @@ -435,7 +474,7 @@ int FixedwingAttitudeControl::parameters_update() { - param_get(_parameter_handles.tconst, &(_parameters.tconst)); + param_get(_parameter_handles.p_tc, &(_parameters.p_tc)); param_get(_parameter_handles.p_p, &(_parameters.p_p)); param_get(_parameter_handles.p_i, &(_parameters.p_i)); param_get(_parameter_handles.p_ff, &(_parameters.p_ff)); @@ -443,6 +482,7 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg)); param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max)); + param_get(_parameter_handles.r_tc, &(_parameters.r_tc)); param_get(_parameter_handles.r_p, &(_parameters.r_p)); param_get(_parameter_handles.r_i, &(_parameters.r_i)); param_get(_parameter_handles.r_ff, &(_parameters.r_ff)); @@ -458,6 +498,12 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.y_coordinated_method, &(_parameters.y_coordinated_method)); param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax)); + param_get(_parameter_handles.w_p, &(_parameters.w_p)); + param_get(_parameter_handles.w_i, &(_parameters.w_i)); + param_get(_parameter_handles.w_ff, &(_parameters.w_ff)); + param_get(_parameter_handles.w_integrator_max, &(_parameters.w_integrator_max)); + param_get(_parameter_handles.w_rmax, &(_parameters.w_rmax)); + param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min)); param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim)); param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max)); @@ -474,10 +520,13 @@ FixedwingAttitudeControl::parameters_update() _parameters.man_roll_max = math::radians(_parameters.man_roll_max); _parameters.man_pitch_max = math::radians(_parameters.man_pitch_max); + param_get(_parameter_handles.flaps_scale, &_parameters.flaps_scale); + param_get(_parameter_handles.flaperon_scale, &_parameters.flaperon_scale); + param_get(_parameter_handles.vtol_type, &_parameters.vtol_type); /* pitch control parameters */ - _pitch_ctrl.set_time_constant(_parameters.tconst); + _pitch_ctrl.set_time_constant(_parameters.p_tc); _pitch_ctrl.set_k_p(_parameters.p_p); _pitch_ctrl.set_k_i(_parameters.p_i); _pitch_ctrl.set_k_ff(_parameters.p_ff); @@ -486,7 +535,7 @@ FixedwingAttitudeControl::parameters_update() _pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg)); /* roll control parameters */ - _roll_ctrl.set_time_constant(_parameters.tconst); + _roll_ctrl.set_time_constant(_parameters.r_tc); _roll_ctrl.set_k_p(_parameters.r_p); _roll_ctrl.set_k_i(_parameters.r_i); _roll_ctrl.set_k_ff(_parameters.r_ff); @@ -502,6 +551,13 @@ FixedwingAttitudeControl::parameters_update() _yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method); _yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax)); + /* wheel control parameters */ + _wheel_ctrl.set_k_p(_parameters.w_p); + _wheel_ctrl.set_k_i(_parameters.w_i); + _wheel_ctrl.set_k_ff(_parameters.w_ff); + _wheel_ctrl.set_integrator_max(_parameters.w_integrator_max); + _wheel_ctrl.set_max_rate(math::radians(_parameters.w_rmax)); + return OK; } @@ -579,14 +635,18 @@ FixedwingAttitudeControl::vehicle_status_poll() if (vehicle_status_updated) { orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + /* set correct uORB ID, depending on if vehicle is VTOL or not */ if (!_rates_sp_id) { if (_vehicle_status.is_vtol) { _rates_sp_id = ORB_ID(fw_virtual_rates_setpoint); _actuators_id = ORB_ID(actuator_controls_virtual_fw); + _attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint); + } else { _rates_sp_id = ORB_ID(vehicle_rates_setpoint); _actuators_id = ORB_ID(actuator_controls_0); + _attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint); } } } @@ -613,12 +673,6 @@ FixedwingAttitudeControl::task_main() _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); - /* rate limit vehicle status updates to 5Hz */ - orb_set_interval(_vcontrol_mode_sub, 200); - /* do not limit the attitude updates in order to minimize latency. - * actuator outputs are still limited by the individual drivers - * properly to not saturate IO or physical limitations */ - parameters_update(); /* get an initial update for all sensor and status data */ @@ -675,8 +729,9 @@ FixedwingAttitudeControl::task_main() last_run = hrt_absolute_time(); /* guard against too large deltaT's */ - if (deltaT > 1.0f) + if (deltaT > 1.0f) { deltaT = 0.01f; + } /* load local copies */ orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); @@ -751,6 +806,10 @@ FixedwingAttitudeControl::task_main() vehicle_status_poll(); + // the position controller will not emit attitude setpoints in some modes + // we need to make sure that this flag is reset + _att_sp.fw_control_yaw = _att_sp.fw_control_yaw && _vcontrol_mode.flag_control_auto_enabled; + /* lock integrator until control is started */ bool lock_integrator; @@ -765,6 +824,7 @@ FixedwingAttitudeControl::task_main() if (_vcontrol_mode.flag_control_termination_enabled) { _actuators_airframe.control[7] = 1.0f; //warnx("_actuators_airframe.control[1] = 1.0f;"); + } else { _actuators_airframe.control[7] = 0.0f; //warnx("_actuators_airframe.control[1] = -1.0f;"); @@ -778,9 +838,58 @@ FixedwingAttitudeControl::task_main() /* default flaps to center */ float flaps_control = 0.0f; + static float delta_flaps = 0; + /* map flaps by default to manual if valid */ - if (PX4_ISFINITE(_manual.flaps)) { - flaps_control = _manual.flaps; + if (PX4_ISFINITE(_manual.flaps) && _vcontrol_mode.flag_control_manual_enabled) { + flaps_control = 0.5f * (_manual.flaps + 1.0f) * _parameters.flaps_scale; + + } else if (_vcontrol_mode.flag_control_auto_enabled) { + flaps_control = _att_sp.apply_flaps ? 1.0f * _parameters.flaps_scale : 0.0f; + } + + // move the actual control value continuous with time + static hrt_abstime t_flaps_changed = 0; + + if (fabsf(flaps_control - _flaps_cmd_last) > 0.01f) { + t_flaps_changed = hrt_absolute_time(); + delta_flaps = flaps_control - _flaps_cmd_last; + _flaps_cmd_last = flaps_control; + } + + static float flaps_applied = 0.0f; + + if (fabsf(flaps_applied - flaps_control) > 0.01f) { + flaps_applied = (flaps_control - delta_flaps) + (float)hrt_elapsed_time(&t_flaps_changed) * (delta_flaps) / 1000000; + } + + /* default flaperon to center */ + float flaperon = 0.0f; + + static float delta_flaperon = 0.0f; + + /* map flaperons by default to manual if valid */ + if (PX4_ISFINITE(_manual.aux2) && _vcontrol_mode.flag_control_manual_enabled) { + flaperon = 0.5f * (_manual.aux2 + 1.0f) * _parameters.flaperon_scale; + + } else if (_vcontrol_mode.flag_control_auto_enabled) { + flaperon = _att_sp.apply_flaps ? 1.0f * _parameters.flaperon_scale : 0.0f; + } + + // move the actual control value continuous with time + static hrt_abstime t_flaperons_changed = 0; + + if (fabsf(flaperon - _flaperons_cmd_last) > 0.01f) { + t_flaperons_changed = hrt_absolute_time(); + delta_flaperon = flaperon - _flaperons_cmd_last; + _flaperons_cmd_last = flaperon; + } + + static float flaperon_applied = 0.0f; + + if (fabsf(flaperon_applied - flaperon) > 0.01f) { + flaperon_applied = (flaperon - delta_flaperon) + (float)hrt_elapsed_time(&t_flaperons_changed) * + (delta_flaperon) / 1000000; } /* decide if in stabilized or full manual control */ @@ -789,11 +898,13 @@ FixedwingAttitudeControl::task_main() float airspeed; /* if airspeed is not updating, we assume the normal average speed */ - if (bool nonfinite = !PX4_ISFINITE(_ctrl_state.airspeed)) { + if (bool nonfinite = !PX4_ISFINITE(_ctrl_state.airspeed) || !_ctrl_state.airspeed_valid) { airspeed = _parameters.airspeed_trim; + if (nonfinite) { perf_count(_nonfinite_input_perf); } + } else { /* prevent numerical drama by requiring 0.5 m/s minimal speed */ airspeed = math::max(0.5f, _ctrl_state.airspeed); @@ -806,11 +917,20 @@ FixedwingAttitudeControl::task_main() * * Forcing the scaling to this value allows reasonable handheld tests. */ + float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : + airspeed); - float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed); + /* Use min airspeed to calculate ground speed scaling region. + * Don't scale below gspd_scaling_trim + */ + float groundspeed = sqrtf(_global_pos.vel_n * _global_pos.vel_n + + _global_pos.vel_e * _global_pos.vel_e); + float gspd_scaling_trim = (_parameters.airspeed_min * 0.6f); + float groundspeed_scaler = gspd_scaling_trim / ((groundspeed < gspd_scaling_trim) ? gspd_scaling_trim : groundspeed); float roll_sp = _parameters.rollsp_offset_rad; float pitch_sp = _parameters.pitchsp_offset_rad; + float yaw_sp = 0.0f; float yaw_manual = 0.0f; float throttle_sp = 0.0f; @@ -820,22 +940,27 @@ FixedwingAttitudeControl::task_main() * for sure not be set from the remote control values) */ if (_vcontrol_mode.flag_control_auto_enabled || - !_vcontrol_mode.flag_control_manual_enabled) { + !_vcontrol_mode.flag_control_manual_enabled) { /* read in attitude setpoint from attitude setpoint uorb topic */ roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; + yaw_sp = _att_sp.yaw_body; throttle_sp = _att_sp.thrust; /* reset integrals where needed */ if (_att_sp.roll_reset_integral) { _roll_ctrl.reset_integrator(); } + if (_att_sp.pitch_reset_integral) { _pitch_ctrl.reset_integrator(); } + if (_att_sp.yaw_reset_integral) { _yaw_ctrl.reset_integrator(); + _wheel_ctrl.reset_integrator(); } + } else if (_vcontrol_mode.flag_control_velocity_enabled) { /* the pilot does not want to change direction, @@ -843,9 +968,10 @@ FixedwingAttitudeControl::task_main() */ if (fabsf(_manual.y) < 0.01f && fabsf(_roll) < 0.2f) { roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; + } else { roll_sp = (_manual.y * _parameters.man_roll_max) - + _parameters.rollsp_offset_rad; + + _parameters.rollsp_offset_rad; } pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; @@ -855,15 +981,18 @@ FixedwingAttitudeControl::task_main() if (_att_sp.roll_reset_integral) { _roll_ctrl.reset_integrator(); } + if (_att_sp.pitch_reset_integral) { _pitch_ctrl.reset_integrator(); } + if (_att_sp.yaw_reset_integral) { _yaw_ctrl.reset_integrator(); + _wheel_ctrl.reset_integrator(); } } else if (_vcontrol_mode.flag_control_altitude_enabled) { - /* + /* * Velocity should be controlled and manual is enabled. */ roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad; @@ -874,12 +1003,16 @@ FixedwingAttitudeControl::task_main() if (_att_sp.roll_reset_integral) { _roll_ctrl.reset_integrator(); } + if (_att_sp.pitch_reset_integral) { _pitch_ctrl.reset_integrator(); } + if (_att_sp.yaw_reset_integral) { _yaw_ctrl.reset_integrator(); + _wheel_ctrl.reset_integrator(); } + } else { /* * Scale down roll and pitch as the setpoints are radians @@ -912,15 +1045,13 @@ FixedwingAttitudeControl::task_main() att_sp.thrust = throttle_sp; /* lazily publish the setpoint only once available */ - if (!_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) { - if (_attitude_sp_pub != nullptr) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp); + if (_attitude_sp_pub != nullptr) { + /* publish the attitude setpoint */ + orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &att_sp); - } else { - /* advertise and publish */ - _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - } + } else if (_attitude_setpoint_id) { + /* advertise and publish */ + _attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &att_sp); } } @@ -929,6 +1060,7 @@ FixedwingAttitudeControl::task_main() _roll_ctrl.reset_integrator(); _pitch_ctrl.reset_integrator(); _yaw_ctrl.reset_integrator(); + _wheel_ctrl.reset_integrator(); } /* Prepare speed_body_u and speed_body_w */ @@ -952,17 +1084,23 @@ FixedwingAttitudeControl::task_main() control_input.acc_body_z = _accel.z; control_input.roll_setpoint = roll_sp; control_input.pitch_setpoint = pitch_sp; + control_input.yaw_setpoint = yaw_sp; control_input.airspeed_min = _parameters.airspeed_min; control_input.airspeed_max = _parameters.airspeed_max; control_input.airspeed = airspeed; control_input.scaler = airspeed_scaling; control_input.lock_integrator = lock_integrator; + control_input.groundspeed = groundspeed; + control_input.groundspeed_scaler = groundspeed_scaler; + + _yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method); /* Run attitude controllers */ if (PX4_ISFINITE(roll_sp) && PX4_ISFINITE(pitch_sp)) { _roll_ctrl.control_attitude(control_input); _pitch_ctrl.control_attitude(control_input); _yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude + _wheel_ctrl.control_attitude(control_input); /* Update input data for rate controllers */ control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate(); @@ -972,6 +1110,7 @@ FixedwingAttitudeControl::task_main() /* Run attitude RATE controllers which need the desired attitudes from above, add trim */ float roll_u = _roll_ctrl.control_bodyrate(control_input); _actuators.control[0] = (PX4_ISFINITE(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll; + if (!PX4_ISFINITE(roll_u)) { _roll_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); @@ -983,33 +1122,47 @@ FixedwingAttitudeControl::task_main() float pitch_u = _pitch_ctrl.control_bodyrate(control_input); _actuators.control[1] = (PX4_ISFINITE(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch; + if (!PX4_ISFINITE(pitch_u)) { _pitch_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); + if (_debug && loop_counter % 10 == 0) { warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f," - " airspeed %.4f, airspeed_scaling %.4f," - " roll_sp %.4f, pitch_sp %.4f," - " _roll_ctrl.get_desired_rate() %.4f," - " _pitch_ctrl.get_desired_rate() %.4f" - " att_sp.roll_body %.4f", - (double)pitch_u, (double)_yaw_ctrl.get_desired_rate(), - (double)airspeed, (double)airspeed_scaling, - (double)roll_sp, (double)pitch_sp, - (double)_roll_ctrl.get_desired_rate(), - (double)_pitch_ctrl.get_desired_rate(), - (double)_att_sp.roll_body); + " airspeed %.4f, airspeed_scaling %.4f," + " roll_sp %.4f, pitch_sp %.4f," + " _roll_ctrl.get_desired_rate() %.4f," + " _pitch_ctrl.get_desired_rate() %.4f" + " att_sp.roll_body %.4f", + (double)pitch_u, (double)_yaw_ctrl.get_desired_rate(), + (double)airspeed, (double)airspeed_scaling, + (double)roll_sp, (double)pitch_sp, + (double)_roll_ctrl.get_desired_rate(), + (double)_pitch_ctrl.get_desired_rate(), + (double)_att_sp.roll_body); } } - float yaw_u = _yaw_ctrl.control_bodyrate(control_input); + float yaw_u = 0.0f; + + if (_att_sp.fw_control_yaw == true) { + yaw_u = _wheel_ctrl.control_bodyrate(control_input); + } + + else { + yaw_u = _yaw_ctrl.control_bodyrate(control_input); + } + _actuators.control[2] = (PX4_ISFINITE(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; /* add in manual rudder control */ _actuators.control[2] += yaw_manual; + if (!PX4_ISFINITE(yaw_u)) { _yaw_ctrl.reset_integrator(); + _wheel_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); + if (_debug && loop_counter % 10 == 0) { warnx("yaw_u %.4f", (double)yaw_u); } @@ -1018,16 +1171,19 @@ FixedwingAttitudeControl::task_main() /* throttle passed through if it is finite and if no engine failure was * detected */ _actuators.control[3] = (PX4_ISFINITE(throttle_sp) && - !(_vehicle_status.engine_failure || - _vehicle_status.engine_failure_cmd)) ? - throttle_sp : 0.0f; + !(_vehicle_status.engine_failure || + _vehicle_status.engine_failure_cmd)) ? + throttle_sp : 0.0f; + if (!PX4_ISFINITE(throttle_sp)) { if (_debug && loop_counter % 10 == 0) { warnx("throttle_sp %.4f", (double)throttle_sp); } } + } else { perf_count(_nonfinite_input_perf); + if (_debug && loop_counter % 10 == 0) { warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp); } @@ -1046,6 +1202,7 @@ FixedwingAttitudeControl::task_main() if (_rate_sp_pub != nullptr) { /* publish the attitude rates setpoint */ orb_publish(_rates_sp_id, _rate_sp_pub, &_rates_sp); + } else if (_rates_sp_id) { /* advertise the attitude rates setpoint */ _rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp); @@ -1059,9 +1216,9 @@ FixedwingAttitudeControl::task_main() _actuators.control[actuator_controls_s::INDEX_THROTTLE] = _manual.z; } - _actuators.control[actuator_controls_s::INDEX_FLAPS] = flaps_control; + _actuators.control[actuator_controls_s::INDEX_FLAPS] = flaps_applied; _actuators.control[5] = _manual.aux1; - _actuators.control[6] = _manual.aux2; + _actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = flaperon_applied; _actuators.control[7] = _manual.aux3; /* lazily publish the setpoint only once available */ @@ -1071,15 +1228,15 @@ FixedwingAttitudeControl::task_main() _actuators_airframe.timestamp_sample = _ctrl_state.timestamp; /* Only publish if any of the proper modes are enabled */ - if(_vcontrol_mode.flag_control_rates_enabled || - _vcontrol_mode.flag_control_attitude_enabled || - _vcontrol_mode.flag_control_manual_enabled) - { + if (_vcontrol_mode.flag_control_rates_enabled || + _vcontrol_mode.flag_control_attitude_enabled || + _vcontrol_mode.flag_control_manual_enabled) { /* publish the actuator controls */ if (_actuators_0_pub != nullptr) { orb_publish(_actuators_id, _actuators_0_pub, &_actuators); + } else if (_actuators_id) { - _actuators_0_pub= orb_advertise(_actuators_id, &_actuators); + _actuators_0_pub = orb_advertise(_actuators_id, &_actuators); } if (_actuators_2_pub != nullptr) { @@ -1110,11 +1267,11 @@ FixedwingAttitudeControl::start() /* start the task */ _control_task = px4_task_spawn_cmd("fw_att_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 1300, - (px4_main_t)&FixedwingAttitudeControl::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 1300, + (px4_main_t)&FixedwingAttitudeControl::task_main_trampoline, + nullptr); if (_control_task < 0) { warn("task start failed"); @@ -1161,13 +1318,15 @@ int fw_att_control_main(int argc, char *argv[]) printf("."); fflush(stdout); } + printf("\n"); } + return 0; } if (!strcmp(argv[1], "stop")) { - if (att_control::g_control == nullptr){ + if (att_control::g_control == nullptr) { warnx("not running"); return 1; } diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 51c06fabb9..2782c5b542 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -46,9 +46,9 @@ */ /** - * Attitude Time Constant + * Attitude Roll Time Constant * - * This defines the latency between a step input and the achieved setpoint + * This defines the latency between a roll step input and the achieved setpoint * (inverse to a P gain). Half a second is a good start value and fits for * most average systems. Smaller systems may require smaller values, but as * this will wear out servos faster, the value should only be decreased as @@ -59,7 +59,23 @@ * @max 1.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.4f); +PARAM_DEFINE_FLOAT(FW_R_TC, 0.4f); + +/** + * Attitude Pitch Time Constant + * + * This defines the latency between a pitch step input and the achieved setpoint + * (inverse to a P gain). Half a second is a good start value and fits for + * most average systems. Smaller systems may require smaller values, but as + * this will wear out servos faster, the value should only be decreased as + * needed. + * + * @unit seconds + * @min 0.2 + * @max 1.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_P_TC, 0.4f); /** * Pitch rate proportional gain. @@ -220,6 +236,55 @@ PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f); */ PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f); +/** + * Wheel steering rate proportional gain + * + * This defines how much the wheel steering input will be commanded depending on the + * current body angular rate error. + * + * @min 0.005 + * @max 1.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_WR_P, 0.5f); + +/** + * Wheel steering rate integrator gain + * + * This gain defines how much control response will result out of a steady + * state error. It trims any constant error. + * + * @min 0.0 + * @max 50.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_WR_I, 0.1f); + +/** + * Wheel steering rate integrator limit + * + * The portion of the integrator part in the control surface deflection is + * limited to this value + * + * @min 0.0 + * @max 1.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_WR_IMAX, 1.0f); + +/** + * Maximum wheel steering rate + * + * This limits the maximum wheel steering rate the controller will output (in degrees per + * second). Setting a value of zero disables the limit. + * + * @unit deg/s + * @min 0.0 + * @max 90.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_W_RMAX, 0.0f); + /** * Roll rate feed forward * @@ -255,6 +320,17 @@ PARAM_DEFINE_FLOAT(FW_PR_FF, 0.5f); */ PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f); +/** + * Wheel steering rate feed forward + * + * Direct feed forward from rate setpoint to control surface output + * + * @min 0.0 + * @max 10.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_WR_FF, 0.2f); + /** * Minimal speed for yaw coordination * @@ -374,3 +450,21 @@ PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f); * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 45.0f); + +/** + * Scale factor for flaps + * + * @min 0.0 + * @max 1.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_FLAPS_SCL, 1.0f); + +/** + * Scale factor for flaperons + * + * @min 0.0 + * @max 1.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_FLAPERON_SCL, 0.0f); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index f21bfc5f2f..d7aebbe0a8 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -49,6 +49,7 @@ * * @author Lorenz Meier * @author Thomas Gubler + * @author Andreas Antener */ #include @@ -71,6 +72,7 @@ #include #include #include +#include #include #include #include @@ -95,6 +97,7 @@ #include "landingslope.h" #include "mtecs/mTecs.h" #include +#include static int _control_task = -1; /**< task handle for sensor task */ #define HDG_HOLD_DIST_NEXT 3000.0f // initial distance of waypoint in front of plane in heading hold mode @@ -103,9 +106,12 @@ static int _control_task = -1; /**< task handle for sensor task */ #define HDG_HOLD_YAWRATE_THRESH 0.15f // max yawrate at which plane locks yaw for heading hold mode #define HDG_HOLD_MAN_INPUT_THRESH 0.01f // max manual roll input from user which does not change the locked heading #define TAKEOFF_IDLE 0.2f // idle speed for POSCTRL/ATTCTRL (when landed and throttle stick > 0) +#define T_ALT_TIMEOUT 1 // time after which we abort landing if terrain estimate is not valid -static constexpr float THROTTLE_THRESH = 0.05f; ///< max throttle from user which will not lead to motors spinning up in altitude controlled modes -static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH = 0.85f; ///< a throttle / pitch input above this value leads to the system switching to climbout mode +static constexpr float THROTTLE_THRESH = + 0.05f; ///< max throttle from user which will not lead to motors spinning up in altitude controlled modes +static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH = + 0.85f; ///< a throttle / pitch input above this value leads to the system switching to climbout mode static constexpr float ALTHOLD_EPV_RESET_THRESH = 5.0f; /** @@ -163,6 +169,8 @@ private: orb_advert_t _tecs_status_pub; /**< TECS status publication */ orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */ + orb_id_t _attitude_setpoint_id; + struct control_state_s _ctrl_state; /**< control state */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */ @@ -194,9 +202,17 @@ private: bool land_onslope; bool land_useterrain; + // landing relevant states + float _t_alt_prev_valid; //**< last terrain estimate which was valid */ + hrt_abstime _time_last_t_alt; //*< time at which we had last valid terrain alt */ + hrt_abstime _time_started_landing; //*< time at which landing started */ + float height_flare; //*< estimated height to ground at which flare started */ + bool _was_in_air; /**< indicated wether the plane was in the air in the previous interation*/ hrt_abstime _time_went_in_air; /**< time at which the plane went in the air */ + runwaytakeoff::RunwayTakeoff _runway_takeoff; + /* takeoff/launch states */ LaunchDetectionResult launch_detection_state; @@ -215,7 +231,7 @@ private: /* throttle and airspeed states */ float _airspeed_error; ///< airspeed error to setpoint in m/s bool _airspeed_valid; ///< flag if a valid airspeed estimate exists - uint64_t _airspeed_last_valid; ///< last time airspeed was valid. Used to detect sensor failures + uint64_t _airspeed_last_received; ///< last time airspeed was received. Used to detect timeouts. float _groundspeed_undershoot; ///< ground speed error to min. speed in m/s bool _global_pos_valid; ///< global position is valid math::Matrix<3, 3> _R_nb; ///< current attitude @@ -277,6 +293,7 @@ private: float land_flare_pitch_min_deg; float land_flare_pitch_max_deg; int land_use_terrain_estimate; + float land_airspeed_scale; } _parameters; /**< local copies of interesting parameters */ @@ -325,6 +342,7 @@ private: param_t land_flare_pitch_min_deg; param_t land_flare_pitch_max_deg; param_t land_use_terrain_estimate; + param_t land_airspeed_scale; } _parameter_handles; /**< handles for interesting parameters */ @@ -384,13 +402,18 @@ private: * @param waypoint_next the waypoint in the heading direction */ void get_waypoint_heading_distance(float heading, float distance, - struct position_setpoint_s &waypoint_prev, struct position_setpoint_s &waypoint_next, bool flag_init); + struct position_setpoint_s &waypoint_prev, struct position_setpoint_s &waypoint_next, bool flag_init); /** * Return the terrain estimate during landing: uses the wp altitude value or the terrain estimate if available */ float get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos); + /** + * Return the terrain estimate during takeoff or takeoff_alt if terrain estimate is not available + */ + float get_terrain_altitude_takeoff(float takeoff_alt, const struct vehicle_global_position_s &global_pos); + /** * Check if we are in a takeoff situation */ @@ -418,7 +441,8 @@ private: const struct position_setpoint_triplet_s &_pos_sp_triplet); float calculate_target_airspeed(float airspeed_demand); - void calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet); + void calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed_2d, + const struct position_setpoint_triplet_s &pos_sp_triplet); /** * Shim for calling task_main from task_create. @@ -445,13 +469,13 @@ private: * XXX need to clean up/remove this function once mtecs fully replaces TECS */ void tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas, - float pitch_min_rad, float pitch_max_rad, - float throttle_min, float throttle_max, float throttle_cruise, - bool climbout_mode, float climbout_pitch_min_rad, - float altitude, - const math::Vector<3> &ground_speed, - unsigned mode = tecs_status_s::TECS_MODE_NORMAL, - bool pitch_max_special = false); + float pitch_min_rad, float pitch_max_rad, + float throttle_min, float throttle_max, float throttle_cruise, + bool climbout_mode, float climbout_pitch_min_rad, + float altitude, + const math::Vector<3> &ground_speed, + unsigned mode = tecs_status_s::TECS_MODE_NORMAL, + bool pitch_max_special = false); }; @@ -473,7 +497,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _task_should_exit(false), _task_running(false), -/* subscriptions */ + /* subscriptions */ _global_pos_sub(-1), _pos_sp_triplet_sub(-1), _ctrl_state_sub(-1), @@ -483,12 +507,15 @@ FixedwingPositionControl::FixedwingPositionControl() : _manual_control_sub(-1), _sensor_combined_sub(-1), -/* publications */ + /* publications */ _attitude_sp_pub(nullptr), _tecs_status_pub(nullptr), _nav_capabilities_pub(nullptr), -/* states */ + /* publication ID */ + _attitude_setpoint_id(0), + + /* states */ _ctrl_state(), _att_sp(), _nav_capabilities(), @@ -499,7 +526,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _pos_sp_triplet(), _sensor_combined(), -/* performance counters */ + /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), _hold_alt(0.0f), @@ -519,8 +546,13 @@ FixedwingPositionControl::FixedwingPositionControl() : land_motor_lim(false), land_onslope(false), land_useterrain(false), + _t_alt_prev_valid(0), + _time_last_t_alt(0), + _time_started_landing(0), + height_flare(0.0f), _was_in_air(false), _time_went_in_air(0), + _runway_takeoff(), launch_detection_state(LAUNCHDETECTION_RES_NONE), last_manual(false), landingslope(), @@ -529,7 +561,7 @@ FixedwingPositionControl::FixedwingPositionControl() : launchDetector(), _airspeed_error(0.0f), _airspeed_valid(false), - _airspeed_last_valid(0), + _airspeed_last_received(0), _groundspeed_undershoot(0.0f), _global_pos_valid(false), _l1_control(), @@ -561,7 +593,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST"); _parameter_handles.land_flare_pitch_min_deg = param_find("FW_FLARE_PMIN"); _parameter_handles.land_flare_pitch_max_deg = param_find("FW_FLARE_PMAX"); - _parameter_handles.land_use_terrain_estimate= param_find("FW_LND_USETER"); + _parameter_handles.land_use_terrain_estimate = param_find("FW_LND_USETER"); + _parameter_handles.land_airspeed_scale = param_find("FW_AIRSPD_SCALE"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST"); @@ -661,10 +694,12 @@ FixedwingPositionControl::parameters_update() _parameters.land_thrust_lim_alt_relative = 0.66f * _parameters.land_flare_alt_relative; } - param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance)); + param_get(_parameter_handles.land_heading_hold_horizontal_distance, + &(_parameters.land_heading_hold_horizontal_distance)); param_get(_parameter_handles.land_flare_pitch_min_deg, &(_parameters.land_flare_pitch_min_deg)); param_get(_parameter_handles.land_flare_pitch_max_deg, &(_parameters.land_flare_pitch_max_deg)); param_get(_parameter_handles.land_use_terrain_estimate, &(_parameters.land_use_terrain_estimate)); + param_get(_parameter_handles.land_airspeed_scale, &(_parameters.land_airspeed_scale)); _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); @@ -701,7 +736,8 @@ FixedwingPositionControl::parameters_update() } /* Update the landing slope */ - landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative, _parameters.land_thrust_lim_alt_relative, _parameters.land_H1_virt); + landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative, + _parameters.land_thrust_lim_alt_relative, _parameters.land_H1_virt); /* Update and publish the navigation capabilities */ _nav_capabilities.landing_slope_angle_rad = landingslope.landing_slope_angle_rad(); @@ -715,6 +751,8 @@ FixedwingPositionControl::parameters_update() /* Update the mTecs */ _mTecs.updateParams(); + _runway_takeoff.updateParams(); + return OK; } @@ -739,6 +777,16 @@ FixedwingPositionControl::vehicle_status_poll() if (updated) { orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + + /* set correct uORB ID, depending on if vehicle is VTOL or not */ + if (!_attitude_setpoint_id) { + if (_vehicle_status.is_vtol) { + _attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint); + + } else { + _attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint); + } + } } } @@ -766,13 +814,13 @@ FixedwingPositionControl::control_state_poll() if (ctrl_state_updated) { orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); - _airspeed_valid = true; - _airspeed_last_valid = hrt_absolute_time(); + _airspeed_valid = _ctrl_state.airspeed_valid; + _airspeed_last_received = hrt_absolute_time(); } else { /* no airspeed updates for one second */ - if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_valid) > 1e6) { + if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_received) > 1e6) { _airspeed_valid = false; } } @@ -786,7 +834,7 @@ FixedwingPositionControl::control_state_poll() _roll = euler_angles(0); _pitch = euler_angles(1); _yaw = euler_angles(2); - + /* update TECS state */ _tecs.enable_airspeed(_airspeed_valid); } @@ -863,7 +911,8 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand) } void -FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet) +FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, + const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet) { if (pos_sp_triplet.current.valid && !(pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER)) { @@ -876,11 +925,15 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &c /* The minimum desired ground speed is the minimum airspeed projected on to the ground using the altitude and horizontal difference between the waypoints if available*/ float distance = 0.0f; float delta_altitude = 0.0f; + if (pos_sp_triplet.previous.valid) { - distance = get_distance_to_next_waypoint(pos_sp_triplet.previous.lat, pos_sp_triplet.previous.lon, pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); + distance = get_distance_to_next_waypoint(pos_sp_triplet.previous.lat, pos_sp_triplet.previous.lon, + pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); delta_altitude = pos_sp_triplet.current.alt - pos_sp_triplet.previous.alt; + } else { - distance = get_distance_to_next_waypoint(current_position(0), current_position(1), pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); + distance = get_distance_to_next_waypoint(current_position(0), current_position(1), pos_sp_triplet.current.lat, + pos_sp_triplet.current.lon); delta_altitude = pos_sp_triplet.current.alt - _global_pos.alt; } @@ -909,13 +962,14 @@ void FixedwingPositionControl::navigation_capabilities_publish() if (_nav_capabilities_pub != nullptr) { orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities); + } else { _nav_capabilities_pub = orb_advertise(ORB_ID(navigation_capabilities), &_nav_capabilities); } } void FixedwingPositionControl::get_waypoint_heading_distance(float heading, float distance, - struct position_setpoint_s &waypoint_prev, struct position_setpoint_s &waypoint_next, bool flag_init) + struct position_setpoint_s &waypoint_prev, struct position_setpoint_s &waypoint_next, bool flag_init) { waypoint_prev.valid = true; waypoint_prev.alt = _hold_alt; @@ -924,6 +978,7 @@ void FixedwingPositionControl::get_waypoint_heading_distance(float heading, floa // on init set first waypoint to momentary position waypoint_prev.lat = _global_pos.lat - cos(heading) * (double)(HDG_HOLD_SET_BACK_DIST) / 1e6; waypoint_prev.lon = _global_pos.lon - sin(heading) * (double)(HDG_HOLD_SET_BACK_DIST) / 1e6; + } else { /* for previous waypoint use the one still in front of us but shift it such that it is @@ -939,7 +994,8 @@ void FixedwingPositionControl::get_waypoint_heading_distance(float heading, floa waypoint_next.alt = _hold_alt; } -float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos) +float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint_alt, + const struct vehicle_global_position_s &global_pos) { if (!PX4_ISFINITE(global_pos.terrain_alt)) { return land_setpoint_alt; @@ -947,17 +1003,29 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint /* Decide if the terrain estimation can be used, once we switched to using the terrain we stick with it * for the whole landing */ - if (_parameters.land_use_terrain_estimate && (global_pos.terrain_alt_valid || land_useterrain)) { - if(!land_useterrain) { - mavlink_log_info(_mavlink_fd, "#audio: Landing, using terrain estimate"); + if (_parameters.land_use_terrain_estimate && global_pos.terrain_alt_valid) { + if (!land_useterrain) { + mavlink_log_info(_mavlink_fd, "Landing, using terrain estimate"); land_useterrain = true; } + return global_pos.terrain_alt; + } else { return land_setpoint_alt; } } +float FixedwingPositionControl::get_terrain_altitude_takeoff(float takeoff_alt, + const struct vehicle_global_position_s &global_pos) +{ + if (PX4_ISFINITE(global_pos.terrain_alt) && global_pos.terrain_alt_valid) { + return global_pos.terrain_alt; + } + + return takeoff_alt; +} + bool FixedwingPositionControl::update_desired_altitude(float dt) { /* @@ -998,33 +1066,39 @@ bool FixedwingPositionControl::update_desired_altitude(float dt) float pitch = -(_manual.x - deadBand) / factor; _hold_alt += (_parameters.max_sink_rate * dt) * pitch; _was_in_deadband = false; + } else if (_manual.x < - deadBand) { /* pitching up */ float pitch = -(_manual.x + deadBand) / factor; _hold_alt += (_parameters.max_climb_rate * dt) * pitch; _was_in_deadband = false; climbout_mode = (pitch > MANUAL_THROTTLE_CLIMBOUT_THRESH); + } else if (!_was_in_deadband) { - /* store altitude at which manual.x was inside deadBand - * The aircraft should immediately try to fly at this altitude - * as this is what the pilot expects when he moves the stick to the center */ + /* store altitude at which manual.x was inside deadBand + * The aircraft should immediately try to fly at this altitude + * as this is what the pilot expects when he moves the stick to the center */ _hold_alt = _global_pos.alt; _althold_epv = _global_pos.epv; _was_in_deadband = true; } + if (_vehicle_status.is_vtol) { if (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode) { _hold_alt = _global_pos.alt; } } + return climbout_mode; } -bool FixedwingPositionControl::in_takeoff_situation() { +bool FixedwingPositionControl::in_takeoff_situation() +{ const hrt_abstime delta_takeoff = 10000000; const float throttle_threshold = 0.1f; - if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold && _global_pos.alt <= _takeoff_ground_alt + _parameters.climbout_diff) { + if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold + && _global_pos.alt <= _takeoff_ground_alt + _parameters.climbout_diff) { return true; } @@ -1037,6 +1111,7 @@ void FixedwingPositionControl::do_takeoff_help(float *hold_altitude, float *pitc if (in_takeoff_situation()) { *hold_altitude = _takeoff_ground_alt + _parameters.climbout_diff; *pitch_limit_min = math::radians(10.0f); + } else { *pitch_limit_min = _parameters.pitch_limit_min; } @@ -1047,13 +1122,17 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi const struct position_setpoint_triplet_s &pos_sp_triplet) { float dt = 0.01; // Using non zero value to a avoid division by zero + if (_control_position_last_called > 0) { dt = (float)hrt_elapsed_time(&_control_position_last_called) * 1e-6f; } + _control_position_last_called = hrt_absolute_time(); bool setpoint = true; + _att_sp.fw_control_yaw = false; // by default we don't want yaw to be contoller directly with rudder + _att_sp.apply_flaps = false; // by default we don't use flaps float eas2tas = 1.0f; // XXX calculate actual number based on current measurements /* filter speed and altitude for controller */ @@ -1062,14 +1141,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* tell TECS to update its state, but let it know when it cannot actually control the plane */ bool in_air_alt_control = (!_vehicle_status.condition_landed && - (_control_mode.flag_control_auto_enabled || - _control_mode.flag_control_velocity_enabled || - _control_mode.flag_control_altitude_enabled)); + (_control_mode.flag_control_auto_enabled || + _control_mode.flag_control_velocity_enabled || + _control_mode.flag_control_altitude_enabled)); /* update TECS filters */ if (!_mTecs.getEnabled()) { _tecs.update_state(_global_pos.alt, _ctrl_state.airspeed, _R_nb, - accel_body, accel_earth, (_global_pos.timestamp > 0), in_air_alt_control); + accel_body, accel_earth, (_global_pos.timestamp > 0), in_air_alt_control); } math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)}; @@ -1087,13 +1166,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _time_went_in_air = hrt_absolute_time(); _takeoff_ground_alt = _global_pos.alt; } + /* reset flag when airplane landed */ if (_vehicle_status.condition_landed) { _was_in_air = false; } if (_control_mode.flag_control_auto_enabled && - pos_sp_triplet.current.valid) { + pos_sp_triplet.current.valid) { /* AUTONOMOUS FLIGHT */ /* Reset integrators if switching to this mode from a other mode in which posctl was not active */ @@ -1102,10 +1182,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_mTecs.getEnabled()) { _mTecs.resetIntegrators(); _mTecs.resetDerivatives(_ctrl_state.airspeed); + } else { _tecs.reset_state(); } } + _control_mode_current = FW_POSCTRL_MODE_AUTO; /* reset hold altitude */ @@ -1159,44 +1241,83 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.yaw_body = _l1_control.nav_bearing(); tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { /* waypoint is a loiter waypoint */ _l1_control.navigate_loiter(curr_wp, current_position, pos_sp_triplet.current.loiter_radius, - pos_sp_triplet.current.loiter_direction, ground_speed_2d); + pos_sp_triplet.current.loiter_direction, ground_speed_2d); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - if (in_takeoff_situation()) { - /* limit roll motion to ensure enough lift */ - _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), - math::radians(15.0f)); + float alt_sp; + + if (_nav_capabilities.abort_landing == true) { + // if we entered loiter due to an aborted landing, demand + // altitude setpoint well above landing waypoint + alt_sp = _pos_sp_triplet.current.alt + 2.0f * _parameters.climbout_diff; + + } else { + // use altitude given by wapoint + alt_sp = _pos_sp_triplet.current.alt; } - tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); + if (in_takeoff_situation() || + ((_global_pos.alt < _pos_sp_triplet.current.alt + _parameters.climbout_diff) + && _nav_capabilities.abort_landing == true)) { + /* limit roll motion to ensure enough lift */ + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), + math::radians(15.0f)); + } + + tecs_update_pitch_throttle(alt_sp, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { + // apply full flaps for landings. this flag will also trigger the use of flaperons + // if they have been enabled using the corresponding parameter + _att_sp.apply_flaps = true; + + // save time at which we started landing + if (_time_started_landing == 0) { + _time_started_landing = hrt_absolute_time(); + } + float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); - float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); + float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), + curr_wp(1)); /* Horizontal landing control */ /* switch to heading hold for the last meters, continue heading hold after */ float wp_distance = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); /* calculate a waypoint distance value which is 0 when the aircraft is behind the waypoint */ float wp_distance_save = wp_distance; + if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) >= math::radians(90.0f)) { wp_distance_save = 0.0f; } - //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); + // create virtual waypoint which is on the desired flight path but + // some distance behind landing waypoint. This will make sure that the plane + // will always follow the desired flight path even if we get close or past + // the landing waypoint + math::Vector<2> curr_wp_shifted; + double lat; + double lon; + create_waypoint_from_line_and_dist(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon, pos_sp_triplet.previous.lat, + pos_sp_triplet.previous.lon, -1000.0f, &lat, &lon); + curr_wp_shifted(0) = (float)lat; + curr_wp_shifted(1) = (float)lon; + + // we want the plane to keep tracking the desired flight path until we start flaring + // if we go into heading hold mode earlier then we risk to be pushed away from the runway by cross winds + //if (land_noreturn_vertical) { if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) { /* heading hold, along the line connecting this and the last waypoint */ @@ -1204,10 +1325,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (!land_noreturn_horizontal) {//set target_bearing in first occurrence if (pos_sp_triplet.previous.valid) { target_bearing = bearing_lastwp_currwp; + } else { target_bearing = _yaw; } - mavlink_log_info(_mavlink_fd, "#audio: Landing, heading hold"); + + mavlink_log_info(_mavlink_fd, "#Landing, heading hold"); } // warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_yaw)); @@ -1236,26 +1359,66 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // XXX this could make a great param float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; - float airspeed_land = 1.3f * _parameters.airspeed_min; - float airspeed_approach = 1.3f * _parameters.airspeed_min; + float airspeed_land = _parameters.land_airspeed_scale * _parameters.airspeed_min; + float airspeed_approach = _parameters.land_airspeed_scale * _parameters.airspeed_min; /* Get an estimate of the terrain altitude if available, otherwise terrain_alt will be * equal to _pos_sp_triplet.current.alt */ - float terrain_alt = get_terrain_altitude_landing(_pos_sp_triplet.current.alt, _global_pos); + float terrain_alt; + + if (_parameters.land_use_terrain_estimate) { + if (_global_pos.terrain_alt_valid) { + // all good, have valid terrain altitude + terrain_alt = _global_pos.terrain_alt; + _t_alt_prev_valid = terrain_alt; + _time_last_t_alt = hrt_absolute_time(); + + } else if (_time_last_t_alt == 0) { + // we have started landing phase but don't have valid terrain + // wait for some time, maybe we will soon get a valid estimate + // until then just use the altitude of the landing waypoint + if (hrt_elapsed_time(&_time_started_landing) < 10 * 1000 * 1000) { + terrain_alt = _pos_sp_triplet.current.alt; + + } else { + // still no valid terrain, abort landing + terrain_alt = _pos_sp_triplet.current.alt; + _nav_capabilities.abort_landing = true; + } + + } else if ((!_global_pos.terrain_alt_valid && hrt_elapsed_time(&_time_last_t_alt) < T_ALT_TIMEOUT * 1000 * 1000) + || land_noreturn_vertical) { + // use previous terrain estimate for some time and hope to recover + // if we are already flaring (land_noreturn_vertical) then just + // go with the old estimate + terrain_alt = _t_alt_prev_valid; + + } else { + // terrain alt was not valid for long time, abort landing + terrain_alt = _t_alt_prev_valid; + _nav_capabilities.abort_landing = true; + } + + } else { + // no terrain estimation, just use landing waypoint altitude + terrain_alt = _pos_sp_triplet.current.alt; + } + /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */ float L_altitude_rel = _pos_sp_triplet.previous.valid ? - _pos_sp_triplet.previous.alt - terrain_alt : 0.0f; + _pos_sp_triplet.previous.alt - terrain_alt : 0.0f; - float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp); + float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, + bearing_lastwp_currwp, bearing_airplane_currwp); /* Check if we should start flaring with a vertical and a * horizontal limit (with some tolerance) * The horizontal limit is only applied when we are in front of the wp */ if (((_global_pos.alt < terrain_alt + landingslope.flare_relative_alt()) && - (wp_distance_save < landingslope.flare_length() + 5.0f)) || - land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out + (wp_distance_save < landingslope.flare_length() + 5.0f)) || + land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ // /* force TECS to only control speed with pitch, altitude is only implicitely controlled now */ @@ -1264,168 +1427,258 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* kill the throttle if param requests it */ throttle_max = _parameters.throttle_max; - if (_global_pos.alt < terrain_alt + landingslope.motor_lim_relative_alt() || land_motor_lim) { + /* enable direct yaw control using rudder/wheel */ + _att_sp.yaw_body = target_bearing; + _att_sp.fw_control_yaw = true; + + if (_global_pos.alt < terrain_alt + landingslope.motor_lim_relative_alt() || land_motor_lim) { throttle_max = math::min(throttle_max, _parameters.throttle_land_max); + if (!land_motor_lim) { land_motor_lim = true; - mavlink_log_info(_mavlink_fd, "#audio: Landing, limiting throttle"); + mavlink_log_info(_mavlink_fd, "#Landing, limiting throttle"); } - } + } - float flare_curve_alt_rel = landingslope.getFlareCurveRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp); + float flare_curve_alt_rel = landingslope.getFlareCurveRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, + bearing_airplane_currwp); /* avoid climbout */ - if ((flare_curve_alt_rel_last < flare_curve_alt_rel && land_noreturn_vertical) || land_stayonground) - { + if ((flare_curve_alt_rel_last < flare_curve_alt_rel && land_noreturn_vertical) || land_stayonground) { flare_curve_alt_rel = 0.0f; // stay on ground land_stayonground = true; } tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel, - calculate_target_airspeed(airspeed_land), eas2tas, - math::radians(_parameters.land_flare_pitch_min_deg), - math::radians(_parameters.land_flare_pitch_max_deg), - 0.0f, throttle_max, throttle_land, - false, land_motor_lim ? math::radians(_parameters.land_flare_pitch_min_deg) : math::radians(_parameters.pitch_limit_min), - _global_pos.alt, ground_speed, - land_motor_lim ? tecs_status_s::TECS_MODE_LAND_THROTTLELIM : tecs_status_s::TECS_MODE_LAND); + calculate_target_airspeed(airspeed_land), eas2tas, + math::radians(_parameters.land_flare_pitch_min_deg), + math::radians(_parameters.land_flare_pitch_max_deg), + 0.0f, throttle_max, throttle_land, + false, land_motor_lim ? math::radians(_parameters.land_flare_pitch_min_deg) : math::radians( + _parameters.pitch_limit_min), + _global_pos.alt, ground_speed, + land_motor_lim ? tecs_status_s::TECS_MODE_LAND_THROTTLELIM : tecs_status_s::TECS_MODE_LAND); if (!land_noreturn_vertical) { - mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring"); + // just started with the flaring phase + _att_sp.pitch_body = 0.0f; + height_flare = _global_pos.alt - terrain_alt; + mavlink_log_info(_mavlink_fd, "#Landing, flaring"); land_noreturn_vertical = true; + + } else { + if (_global_pos.vel_d > 0.1f) { + _att_sp.pitch_body = math::radians(_parameters.land_flare_pitch_min_deg) * + math::constrain((height_flare - (_global_pos.alt - terrain_alt)) / height_flare, 0.0f, 1.0f); + + } else { + _att_sp.pitch_body = _att_sp.pitch_body; + } } - //warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); flare_curve_alt_rel_last = flare_curve_alt_rel; + } else { - /* intersect glide slope: - * minimize speed to approach speed - * if current position is higher than the slope follow the glide slope (sink to the - * glide slope) - * also if the system captures the slope it should stay - * on the slope (bool land_onslope) - * if current position is below the slope continue at previous wp altitude - * until the intersection with slope - * */ + /* intersect glide slope: + * minimize speed to approach speed + * if current position is higher than the slope follow the glide slope (sink to the + * glide slope) + * also if the system captures the slope it should stay + * on the slope (bool land_onslope) + * if current position is below the slope continue at previous wp altitude + * until the intersection with slope + * */ float altitude_desired_rel; + if (_global_pos.alt > terrain_alt + landing_slope_alt_rel_desired || land_onslope) { /* stay on slope */ altitude_desired_rel = landing_slope_alt_rel_desired; + if (!land_onslope) { - mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope"); + mavlink_log_info(_mavlink_fd, "#Landing, on slope"); land_onslope = true; } + } else { /* continue horizontally */ altitude_desired_rel = _pos_sp_triplet.previous.valid ? L_altitude_rel : - _global_pos.alt - terrain_alt; + _global_pos.alt - terrain_alt; } tecs_update_pitch_throttle(terrain_alt + altitude_desired_rel, - calculate_target_airspeed(airspeed_approach), eas2tas, - math::radians(_parameters.pitch_limit_min), - math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, - _parameters.throttle_max, - _parameters.throttle_cruise, - false, - math::radians(_parameters.pitch_limit_min), - _global_pos.alt, - ground_speed); + calculate_target_airspeed(airspeed_approach), eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + _parameters.throttle_max, + _parameters.throttle_cruise, + false, + math::radians(_parameters.pitch_limit_min), + _global_pos.alt, + ground_speed); } } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { - /* Perform launch detection */ - if (launchDetector.launchDetectionEnabled() && - launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { - /* Inform user that launchdetection is running */ - static hrt_abstime last_sent = 0; - if(hrt_absolute_time() - last_sent > 4e6) { - mavlink_log_critical(_mavlink_fd, "Launchdetection running"); - last_sent = hrt_absolute_time(); + if (_runway_takeoff.runwayTakeoffEnabled()) { + if (!_runway_takeoff.isInitialized()) { + math::Quaternion q(&_ctrl_state.q[0]); + math::Vector<3> euler = q.to_euler(); + _runway_takeoff.init(euler(2), _global_pos.lat, _global_pos.lon); + + /* need this already before takeoff is detected + * doesn't matter if it gets reset when takeoff is detected eventually */ + _takeoff_ground_alt = _global_pos.alt; + + mavlink_log_info(_mavlink_fd, "#Takeoff on runway"); } - /* Detect launch */ - launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); + float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt, _global_pos); - /* update our copy of the launch detection state */ - launch_detection_state = launchDetector.getLaunchDetected(); - } else { - /* no takeoff detection --> fly */ - launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; - } + // update runway takeoff helper + _runway_takeoff.update( + _ctrl_state.airspeed, + _global_pos.alt - terrain_alt, + _global_pos.lat, + _global_pos.lon, + _mavlink_fd); - /* Set control values depending on the detection state */ - if (launch_detection_state != LAUNCHDETECTION_RES_NONE) { - /* Launch has been detected, hence we have to control the plane. */ + /* + * Update navigation: _runway_takeoff returns the start WP according to mode and phase. + * If we use the navigator heading or not is decided later. + */ + _l1_control.navigate_waypoints(_runway_takeoff.getStartWP(), curr_wp, current_position, ground_speed_2d); - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); - - /* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use - * full throttle, otherwise we use the preTakeOff Throttle */ - float takeoff_throttle = launch_detection_state != - LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ? - launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max; - - /* select maximum pitch: the launchdetector may impose another limit for the pitch - * depending on the state of the launch */ - float takeoff_pitch_max_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max); + // update tecs + float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_parameters.pitch_limit_max); float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); - /* apply minimum pitch and limit roll if target altitude is not within climbout_diff - * meters */ - if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) { + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, + calculate_target_airspeed( + _runway_takeoff.getMinAirspeedScaling() * _parameters.airspeed_min), + eas2tas, + math::radians(_parameters.pitch_limit_min), + takeoff_pitch_max_rad, + _parameters.throttle_min, + _parameters.throttle_max, // XXX should we also set runway_takeoff_throttle here? + _parameters.throttle_cruise, + _runway_takeoff.climbout(), + math::radians(_runway_takeoff.getMinPitch( + _pos_sp_triplet.current.pitch_min, + 10.0f, + _parameters.pitch_limit_min)), + _global_pos.alt, + ground_speed, + tecs_status_s::TECS_MODE_TAKEOFF, + takeoff_pitch_max_deg != _parameters.pitch_limit_max); - /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ - tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, - calculate_target_airspeed(1.3f * _parameters.airspeed_min), - eas2tas, - math::radians(_parameters.pitch_limit_min), - takeoff_pitch_max_rad, - _parameters.throttle_min, takeoff_throttle, - _parameters.throttle_cruise, - true, - math::max(math::radians(pos_sp_triplet.current.pitch_min), - math::radians(10.0f)), - _global_pos.alt, - ground_speed, - tecs_status_s::TECS_MODE_TAKEOFF, - takeoff_pitch_max_deg != _parameters.pitch_limit_max); + // assign values + _att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.nav_roll()); + _att_sp.yaw_body = _runway_takeoff.getYaw(_l1_control.nav_bearing()); + _att_sp.fw_control_yaw = _runway_takeoff.controlYaw(); + _att_sp.pitch_body = _runway_takeoff.getPitch(_tecs.get_pitch_demand()); - /* limit roll motion to ensure enough lift */ - _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), - math::radians(15.0f)); + // reset integrals except yaw (which also counts for the wheel controller) + _att_sp.roll_reset_integral = _runway_takeoff.resetIntegrators(); + _att_sp.pitch_reset_integral = _runway_takeoff.resetIntegrators(); + + /*warnx("yaw: %.4f, roll: %.4f, pitch: %.4f", (double)_att_sp.yaw_body, + (double)_att_sp.roll_body, (double)_att_sp.pitch_body);*/ + + } else { + /* Perform launch detection */ + if (launchDetector.launchDetectionEnabled() && + launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { + /* Inform user that launchdetection is running */ + static hrt_abstime last_sent = 0; + + if (hrt_absolute_time() - last_sent > 4e6) { + mavlink_log_critical(_mavlink_fd, "#Launchdetection running"); + last_sent = hrt_absolute_time(); + } + + /* Detect launch */ + launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); + + /* update our copy of the launch detection state */ + launch_detection_state = launchDetector.getLaunchDetected(); + + } else { + /* no takeoff detection --> fly */ + launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; + } + + /* Set control values depending on the detection state */ + if (launch_detection_state != LAUNCHDETECTION_RES_NONE) { + /* Launch has been detected, hence we have to control the plane. */ + + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + + /* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use + * full throttle, otherwise we use the preTakeOff Throttle */ + float takeoff_throttle = launch_detection_state != + LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ? + launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max; + + /* select maximum pitch: the launchdetector may impose another limit for the pitch + * depending on the state of the launch */ + float takeoff_pitch_max_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max); + float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); + + /* apply minimum pitch and limit roll if target altitude is not within climbout_diff + * meters */ + if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) { + /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, + calculate_target_airspeed(1.3f * _parameters.airspeed_min), + eas2tas, + math::radians(_parameters.pitch_limit_min), + takeoff_pitch_max_rad, + _parameters.throttle_min, takeoff_throttle, + _parameters.throttle_cruise, + true, + math::max(math::radians(_pos_sp_triplet.current.pitch_min), + math::radians(10.0f)), + _global_pos.alt, + ground_speed, + tecs_status_s::TECS_MODE_TAKEOFF, + takeoff_pitch_max_deg != _parameters.pitch_limit_max); + + /* limit roll motion to ensure enough lift */ + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), + math::radians(15.0f)); + + } else { + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, + calculate_target_airspeed(_parameters.airspeed_trim), + eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + takeoff_throttle, + _parameters.throttle_cruise, + false, + math::radians(_parameters.pitch_limit_min), + _global_pos.alt, + ground_speed); + } } else { - tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, - calculate_target_airspeed(_parameters.airspeed_trim), - eas2tas, - math::radians(_parameters.pitch_limit_min), - math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, - takeoff_throttle, - _parameters.throttle_cruise, - false, - math::radians(_parameters.pitch_limit_min), - _global_pos.alt, - ground_speed); - } - } else { - /* Tell the attitude controller to stop integrating while we are waiting - * for the launch */ - _att_sp.roll_reset_integral = true; - _att_sp.pitch_reset_integral = true; - _att_sp.yaw_reset_integral = true; + /* Tell the attitude controller to stop integrating while we are waiting + * for the launch */ + _att_sp.roll_reset_integral = true; + _att_sp.pitch_reset_integral = true; + _att_sp.yaw_reset_integral = true; - /* Set default roll and pitch setpoints during detection phase */ - _att_sp.roll_body = 0.0f; - _att_sp.pitch_body = math::max(math::radians(pos_sp_triplet.current.pitch_min), - math::radians(10.0f)); + /* Set default roll and pitch setpoints during detection phase */ + _att_sp.roll_body = 0.0f; + _att_sp.pitch_body = math::max(math::radians(pos_sp_triplet.current.pitch_min), + math::radians(10.0f)); + } } } @@ -1446,7 +1699,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } } else if (_control_mode.flag_control_velocity_enabled && - _control_mode.flag_control_altitude_enabled) { + _control_mode.flag_control_altitude_enabled) { /* POSITION CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed, heading is set to a distant waypoint */ @@ -1457,27 +1710,30 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _hdg_hold_enabled = false; // this makes sure the waypoints are reset below _yaw_lock_engaged = false; } + /* Reset integrators if switching to this mode from a other mode in which posctl was not active */ if (_control_mode_current == FW_POSCTRL_MODE_OTHER) { /* reset integrators */ if (_mTecs.getEnabled()) { _mTecs.resetIntegrators(); _mTecs.resetDerivatives(_ctrl_state.airspeed); + } else { _tecs.reset_state(); } } + _control_mode_current = FW_POSCTRL_MODE_POSITION; /* Get demanded airspeed */ float altctrl_airspeed = _parameters.airspeed_min + - (_parameters.airspeed_max - _parameters.airspeed_min) * - _manual.z; + (_parameters.airspeed_max - _parameters.airspeed_min) * + _manual.z; /* update desired altitude based on user pitch stick input */ bool climbout_requested = update_desired_altitude(dt); - /* if we assume that user is taking off then help by demanding altitude setpoint well above ground + /* if we assume that user is taking off then help by demanding altitude setpoint well above ground * and set limit to pitch angle to prevent stearing into ground */ float pitch_limit_min; @@ -1486,23 +1742,24 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* throttle limiting */ throttle_max = _parameters.throttle_max; + if (fabsf(_manual.z) < THROTTLE_THRESH) { throttle_max = 0.0f; } tecs_update_pitch_throttle(_hold_alt, - altctrl_airspeed, - eas2tas, - math::radians(_parameters.pitch_limit_min), - math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, - throttle_max, - _parameters.throttle_cruise, - climbout_requested, - ((climbout_requested) ? math::radians(10.0f) : pitch_limit_min), - _global_pos.alt, - ground_speed, - tecs_status_s::TECS_MODE_NORMAL); + altctrl_airspeed, + eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + throttle_max, + _parameters.throttle_cruise, + climbout_requested, + ((climbout_requested) ? math::radians(10.0f) : pitch_limit_min), + _global_pos.alt, + ground_speed, + tecs_status_s::TECS_MODE_NORMAL); /* heading control */ @@ -1534,7 +1791,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* we have a valid heading hold position, are we too close? */ if (get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, - _hdg_hold_curr_wp.lat, _hdg_hold_curr_wp.lon) < HDG_HOLD_REACHED_DIST) { + _hdg_hold_curr_wp.lat, _hdg_hold_curr_wp.lon) < HDG_HOLD_REACHED_DIST) { get_waypoint_heading_distance(_hdg_hold_yaw, HDG_HOLD_DIST_NEXT, _hdg_hold_prev_wp, _hdg_hold_curr_wp, false); } @@ -1555,9 +1812,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (in_takeoff_situation()) { /* limit roll motion to ensure enough lift */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), - math::radians(15.0f)); + math::radians(15.0f)); } } + } else { _hdg_hold_enabled = false; _yaw_lock_engaged = false; @@ -1570,27 +1828,30 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Need to init because last loop iteration was in a different mode */ _hold_alt = _global_pos.alt; } + /* Reset integrators if switching to this mode from a other mode in which posctl was not active */ if (_control_mode_current == FW_POSCTRL_MODE_OTHER) { /* reset integrators */ if (_mTecs.getEnabled()) { _mTecs.resetIntegrators(); _mTecs.resetDerivatives(_ctrl_state.airspeed); + } else { _tecs.reset_state(); } } + _control_mode_current = FW_POSCTRL_MODE_ALTITUDE; /* Get demanded airspeed */ float altctrl_airspeed = _parameters.airspeed_min + - (_parameters.airspeed_max - _parameters.airspeed_min) * - _manual.z; + (_parameters.airspeed_max - _parameters.airspeed_min) * + _manual.z; /* update desired altitude based on user pitch stick input */ bool climbout_requested = update_desired_altitude(dt); - /* if we assume that user is taking off then help by demanding altitude setpoint well above ground + /* if we assume that user is taking off then help by demanding altitude setpoint well above ground * and set limit to pitch angle to prevent stearing into ground */ float pitch_limit_min; @@ -1598,31 +1859,40 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* throttle limiting */ throttle_max = _parameters.throttle_max; + if (fabsf(_manual.z) < THROTTLE_THRESH) { throttle_max = 0.0f; } tecs_update_pitch_throttle(_hold_alt, - altctrl_airspeed, - eas2tas, - math::radians(_parameters.pitch_limit_min), - math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, - throttle_max, - _parameters.throttle_cruise, - climbout_requested, - ((climbout_requested) ? math::radians(10.0f) : pitch_limit_min), - _global_pos.alt, - ground_speed, - tecs_status_s::TECS_MODE_NORMAL); + altctrl_airspeed, + eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + throttle_max, + _parameters.throttle_cruise, + climbout_requested, + ((climbout_requested) ? math::radians(10.0f) : pitch_limit_min), + _global_pos.alt, + ground_speed, + tecs_status_s::TECS_MODE_NORMAL); + } else { _control_mode_current = FW_POSCTRL_MODE_OTHER; /** MANUAL FLIGHT **/ - /* reset hold altitude */ + // reset hold altitude _hold_alt = _global_pos.alt; + // reset terrain estimation relevant values + _time_started_landing = 0; + _time_last_t_alt = 0; + + // reset lading abort state + _nav_capabilities.abort_landing = false; + /* no flight mode applies, do not publish an attitude setpoint */ setpoint = false; @@ -1637,25 +1907,36 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { /* Set thrust to 0 to minimize damage */ _att_sp.thrust = 0.0f; + } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto - pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && - launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { - /* making sure again that the correct thrust is used, - * without depending on library calls for safety reasons */ + pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && + launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS && + !_runway_takeoff.runwayTakeoffEnabled()) { + /* making sure again that the correct thrust is used, + * without depending on library calls for safety reasons */ _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); + } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && - pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { + pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && + _runway_takeoff.runwayTakeoffEnabled()) { + _att_sp.thrust = _runway_takeoff.getThrottle( + math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : + _tecs.get_throttle_demand(), throttle_max)); + + } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && + pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { _att_sp.thrust = 0.0f; + } else { /* Copy thrust and pitch values from tecs */ if (_vehicle_status.condition_landed && - (_control_mode_current == FW_POSCTRL_MODE_POSITION || _control_mode_current == FW_POSCTRL_MODE_ALTITUDE)) - { + (_control_mode_current == FW_POSCTRL_MODE_POSITION || _control_mode_current == FW_POSCTRL_MODE_ALTITUDE)) { // when we are landed in these modes we want the motor to spin _att_sp.thrust = math::min(TAKEOFF_IDLE, throttle_max); + } else { _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : - _tecs.get_throttle_demand(), throttle_max); + _tecs.get_throttle_demand(), throttle_max); } @@ -1663,14 +1944,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* During a takeoff waypoint while waiting for launch the pitch sp is set * already (not by tecs) */ - if (!(_control_mode_current == FW_POSCTRL_MODE_AUTO && - pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && - launch_detection_state == LAUNCHDETECTION_RES_NONE)) { + if (!(_control_mode_current == FW_POSCTRL_MODE_AUTO && ( + (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && + (launch_detection_state == LAUNCHDETECTION_RES_NONE || + _runway_takeoff.runwayTakeoffEnabled())) || + (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND && + land_noreturn_vertical)) + )) { _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); } if (_control_mode.flag_control_position_enabled) { last_manual = false; + } else { last_manual = true; } @@ -1785,20 +2071,21 @@ FixedwingPositionControl::task_main() _att_sp.timestamp = hrt_absolute_time(); /* lazily publish the setpoint only once available */ - if (_attitude_sp_pub != nullptr && !_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) { + if (_attitude_sp_pub != nullptr) { /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &_att_sp); + orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &_att_sp); - } else if (_attitude_sp_pub == nullptr && !_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) { + } else if (_attitude_setpoint_id) { /* advertise and publish */ - _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + _attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp); } /* XXX check if radius makes sense here */ float turn_distance = _l1_control.switch_distance(100.0f); /* lazily publish navigation capabilities */ - if ((hrt_elapsed_time(&_nav_capabilities.timestamp) > 1000000) || (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON + if ((hrt_elapsed_time(&_nav_capabilities.timestamp) > 1000000) + || (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON && turn_distance > 0)) { /* set new turn distance */ @@ -1809,6 +2096,7 @@ FixedwingPositionControl::task_main() } } + perf_end(_loop_perf); } @@ -1825,6 +2113,7 @@ void FixedwingPositionControl::reset_takeoff_state() { launch_detection_state = LAUNCHDETECTION_RES_NONE; launchDetector.reset(); + _runway_takeoff.reset(); } void FixedwingPositionControl::reset_landing_state() @@ -1854,10 +2143,13 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ /* Using mtecs library: prepare arguments for mtecs call */ float flightPathAngle = 0.0f; float ground_speed_length = ground_speed.length(); + if (ground_speed_length > FLT_EPSILON) { - flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); + flightPathAngle = -asinf(ground_speed(2) / ground_speed_length); } + fwPosctrl::LimitOverride limitOverride; + if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { /* Force the slow downwards spiral */ limitOverride.enablePitchMinOverride(-1.0f); @@ -1865,6 +2157,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ } else if (climbout_mode) { limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); + } else { limitOverride.disablePitchMinOverride(); } @@ -1872,12 +2165,15 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ if (pitch_max_special) { /* Use the maximum pitch from the argument */ limitOverride.enablePitchMaxOverride(M_RAD_TO_DEG_F * pitch_max_rad); + } else { /* use pitch max set by MT param */ limitOverride.disablePitchMaxOverride(); } + _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _ctrl_state.airspeed, v_sp, mode, - limitOverride); + limitOverride); + } else { if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { /* Force the slow downwards spiral */ @@ -1885,8 +2181,9 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ pitch_max_rad = M_DEG_TO_RAD_F * 5.0f; } -/* No underspeed protection in landing mode */ - _tecs.set_detect_underspeed_enabled(!(mode == tecs_status_s::TECS_MODE_LAND || mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM)); + /* No underspeed protection in landing mode */ + _tecs.set_detect_underspeed_enabled(!(mode == tecs_status_s::TECS_MODE_LAND + || mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM)); /* Using tecs library */ _tecs.update_pitch_throttle(_R_nb, _pitch, altitude, alt_sp, v_sp, @@ -1903,18 +2200,21 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ t.timestamp = s.timestamp; switch (s.mode) { - case TECS::ECL_TECS_MODE_NORMAL: - t.mode = tecs_status_s::TECS_MODE_NORMAL; - break; - case TECS::ECL_TECS_MODE_UNDERSPEED: - t.mode = tecs_status_s::TECS_MODE_UNDERSPEED; - break; - case TECS::ECL_TECS_MODE_BAD_DESCENT: - t.mode = tecs_status_s::TECS_MODE_BAD_DESCENT; - break; - case TECS::ECL_TECS_MODE_CLIMBOUT: - t.mode = tecs_status_s::TECS_MODE_CLIMBOUT; - break; + case TECS::ECL_TECS_MODE_NORMAL: + t.mode = tecs_status_s::TECS_MODE_NORMAL; + break; + + case TECS::ECL_TECS_MODE_UNDERSPEED: + t.mode = tecs_status_s::TECS_MODE_UNDERSPEED; + break; + + case TECS::ECL_TECS_MODE_BAD_DESCENT: + t.mode = tecs_status_s::TECS_MODE_BAD_DESCENT; + break; + + case TECS::ECL_TECS_MODE_CLIMBOUT: + t.mode = tecs_status_s::TECS_MODE_CLIMBOUT; + break; } t.altitudeSp = s.altitude_sp; @@ -1933,12 +2233,13 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ t.totalEnergyRateError = s.total_energy_rate_error; t.energyDistributionError = s.energy_distribution_error; t.energyDistributionRateError = s.energy_distribution_rate_error; - + t.throttle_integ = s.throttle_integ; t.pitch_integ = s.pitch_integ; if (_tecs_status_pub != nullptr) { orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t); + } else { _tecs_status_pub = orb_advertise(ORB_ID(tecs_status), &t); } @@ -1952,11 +2253,11 @@ FixedwingPositionControl::start() /* start the task */ _control_task = px4_task_spawn_cmd("fw_pos_control_l1", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 1300, - (px4_main_t)&FixedwingPositionControl::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 1300, + (px4_main_t)&FixedwingPositionControl::task_main_trampoline, + nullptr); if (_control_task < 0) { warn("task start failed"); @@ -1991,13 +2292,14 @@ int fw_pos_control_l1_main(int argc, char *argv[]) printf("."); fflush(stdout); } + printf("\n"); return 0; } if (!strcmp(argv[1], "stop")) { - if (l1_control::g_control == nullptr){ + if (l1_control::g_control == nullptr) { warnx("not running"); return 1; } 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 5870039c24..14b73038a0 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 @@ -443,3 +443,15 @@ PARAM_DEFINE_FLOAT(FW_FLARE_PMIN, 2.5f); * */ PARAM_DEFINE_FLOAT(FW_FLARE_PMAX, 15.0f); + +/** + * Takeoff and landing airspeed scale factor + * + * Multiplying this factor with the minimum airspeed of the plane + * gives the target airspeed for takeoff and landing approach. + * + * @min 1.0 + * @max 1.5 + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_AIRSPD_SCALE, 1.3f); diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 2ff3fc2767..531a921b83 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -89,7 +89,7 @@ int gpio_led_main(int argc, char *argv[]) "\t\tr2\tPX4IO RELAY2" ); #endif -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) errx(1, "usage: gpio_led {start|stop} [-p ]\n" "\t-p \tUse specified AUX OUT pin number (default: 1)" ); @@ -111,7 +111,7 @@ int gpio_led_main(int argc, char *argv[]) #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 char *pin_name = "PX4FMU GPIO_EXT1"; #endif -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) char pin_name[] = "AUX OUT 1"; #endif @@ -154,7 +154,7 @@ int gpio_led_main(int argc, char *argv[]) } #endif -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) unsigned int n = strtoul(argv[3], NULL, 10); if (n >= 1 && n <= 6) { @@ -207,6 +207,8 @@ void gpio_led_start(FAR void *arg) char *gpio_dev; +#if defined(PX4IO_DEVICE_PATH) + if (priv->use_io) { gpio_dev = PX4IO_DEVICE_PATH; @@ -214,6 +216,10 @@ void gpio_led_start(FAR void *arg) gpio_dev = PX4FMU_DEVICE_PATH; } +#else + gpio_dev = PX4FMU_DEVICE_PATH; +#endif + /* open GPIO device */ priv->gpio_fd = open(gpio_dev, 0); diff --git a/src/modules/land_detector/CMakeLists.txt b/src/modules/land_detector/CMakeLists.txt index 3882f246c2..9ef684d3b1 100644 --- a/src/modules/land_detector/CMakeLists.txt +++ b/src/modules/land_detector/CMakeLists.txt @@ -41,6 +41,7 @@ px4_add_module( LandDetector.cpp MulticopterLandDetector.cpp FixedwingLandDetector.cpp + VtolLandDetector.cpp DEPENDS platforms__common ) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index f8066241c5..cff46c1a3f 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -36,6 +36,7 @@ * Land detection algorithm for fixedwings * * @author Johan Jansen + * @author Lorenz Meier */ #include "FixedwingLandDetector.h" @@ -48,42 +49,44 @@ 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) + _controlStateSub(-1), + _vehicleStatusSub(-1), + _armingSub(-1), + _airspeedSub(-1), + _controlState{}, + _vehicleStatus{}, + _arming{}, + _airspeed{}, + _parameterSub(-1), + _velocity_xy_filtered(0.0f), + _velocity_z_filtered(0.0f), + _airspeed_filtered(0.0f), + _accel_horz_lp(0.0f), + _landDetectTrigger(0) { _paramHandle.maxVelocity = param_find("LNDFW_VEL_XY_MAX"); _paramHandle.maxClimbRate = param_find("LNDFW_VEL_Z_MAX"); _paramHandle.maxAirSpeed = param_find("LNDFW_AIRSPD_MAX"); + _paramHandle.maxIntVelocity = param_find("LNDFW_VELI_MAX"); } void FixedwingLandDetector::initialize() { // Subscribe to local position and airspeed data - _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position)); - _airspeedSub = orb_subscribe(ORB_ID(airspeed)); + _controlStateSub = orb_subscribe(ORB_ID(control_state)); _vehicleStatusSub = orb_subscribe(ORB_ID(vehicle_status)); _armingSub = orb_subscribe(ORB_ID(actuator_armed)); + _airspeedSub = orb_subscribe(ORB_ID(airspeed)); updateParameterCache(true); } void FixedwingLandDetector::updateSubscriptions() { - orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition); - orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); + orb_update(ORB_ID(control_state), _controlStateSub, &_controlState); orb_update(ORB_ID(vehicle_status), _vehicleStatusSub, &_vehicleStatus); orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); + orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); } bool FixedwingLandDetector::update() @@ -99,29 +102,33 @@ bool FixedwingLandDetector::update() const uint64_t now = hrt_absolute_time(); bool landDetected = false; - 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); + if (hrt_elapsed_time(&_controlState.timestamp) < 500 * 1000) { + float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_controlState.x_vel * + _controlState.x_vel + _controlState.y_vel * _controlState.y_vel); if (PX4_ISFINITE(val)) { _velocity_xy_filtered = val; } - val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_vehicleLocalPosition.vz); + val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_controlState.z_vel); if (PX4_ISFINITE(val)) { _velocity_z_filtered = val; } - } - if (hrt_elapsed_time(&_airspeed.timestamp) < 500 * 1000) { _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; + + // a leaking lowpass prevents biases from building up, but + // gives a mostly correct response for short impulses + _accel_horz_lp = _accel_horz_lp * 0.8f + _controlState.horz_acc_mag * 0.18f; + } // crude land detector for fixedwing if (_velocity_xy_filtered < _params.maxVelocity && _velocity_z_filtered < _params.maxClimbRate - && _airspeed_filtered < _params.maxAirSpeed) { + && _airspeed_filtered < _params.maxAirSpeed + && _accel_horz_lp < _params.maxIntVelocity) { // these conditions need to be stable for a period of time before we trust them if (now > _landDetectTrigger) { @@ -151,5 +158,6 @@ void FixedwingLandDetector::updateParameterCache(const bool force) param_get(_paramHandle.maxVelocity, &_params.maxVelocity); param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate); param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed); + param_get(_paramHandle.maxIntVelocity, &_params.maxIntVelocity); } } diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index 325faee794..5c5b18a9e5 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -42,11 +42,11 @@ #define __FIXED_WING_LAND_DETECTOR_H__ #include "LandDetector.h" -#include -#include +#include #include #include #include +#include #include class FixedwingLandDetector : public LandDetector @@ -83,28 +83,31 @@ private: param_t maxVelocity; param_t maxClimbRate; param_t maxAirSpeed; + param_t maxIntVelocity; } _paramHandle; struct { float maxVelocity; float maxClimbRate; float maxAirSpeed; + float maxIntVelocity; } _params; private: - int _vehicleLocalPositionSub; /**< notification of local position */ - struct vehicle_local_position_s _vehicleLocalPosition; /**< the result from local position subscription */ - int _airspeedSub; + int _controlStateSub; /**< notification of local position */ int _vehicleStatusSub; int _armingSub; - struct airspeed_s _airspeed; + int _airspeedSub; + struct control_state_s _controlState; /**< the result from local position subscription */ struct vehicle_status_s _vehicleStatus; struct actuator_armed_s _arming; + struct airspeed_s _airspeed; int _parameterSub; float _velocity_xy_filtered; float _velocity_z_filtered; float _airspeed_filtered; + float _accel_horz_lp; uint64_t _landDetectTrigger; }; diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 3040e23407..5140a2809c 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -57,14 +57,14 @@ _work{} { LandDetector::~LandDetector() { - work_cancel(LPWORK, &_work); + work_cancel(HPWORK, &_work); _taskShouldExit = true; } int LandDetector::start() { /* schedule a cycle to start things */ - work_queue(LPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, 0); + work_queue(HPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, 0); return 0; } @@ -107,10 +107,11 @@ void LandDetector::cycle() // publish the land detected broadcast orb_publish(ORB_ID(vehicle_land_detected), (orb_advert_t)_landDetectedPub, &_landDetected); + //warnx("in air status changed: %s", (_landDetected.landed) ? "LANDED" : "TAKEOFF"); } if (!_taskShouldExit) { - work_queue(LPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, + work_queue(HPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, USEC2TICK(1000000 / LAND_DETECTOR_UPDATE_RATE)); } } diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index fa636e496c..2f46c21f3f 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -95,6 +95,13 @@ bool MulticopterLandDetector::update() // first poll for new data from our subscriptions updateSubscriptions(); + updateParameterCache(false); + + return get_landed_state(); +} + +bool MulticopterLandDetector::get_landed_state() +{ // only trigger flight conditions if we are armed if (!_arming.armed) { _arming_time = 0; diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index d001be4e7f..3ee697a468 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -60,24 +60,29 @@ protected: /** * @brief polls all subscriptions and pulls any data that has changed **/ - void updateSubscriptions(); + virtual void updateSubscriptions(); /** * @brief Runs one iteration of the land detection algorithm **/ - bool update() override; + virtual bool update() override; /** * @brief Initializes the land detection algorithm **/ - void initialize() override; + virtual void initialize() override; - -private: /** * @brief download and update local parameter cache **/ - void updateParameterCache(const bool force); + virtual void updateParameterCache(const bool force); + + /** + * @brief get multicopter landed state + **/ + bool get_landed_state(); + +private: /** * @brief Handles for interesting parameters diff --git a/src/modules/land_detector/VtolLandDetector.cpp b/src/modules/land_detector/VtolLandDetector.cpp new file mode 100644 index 0000000000..3643993b18 --- /dev/null +++ b/src/modules/land_detector/VtolLandDetector.cpp @@ -0,0 +1,116 @@ +/**************************************************************************** + * + * 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 VtolLandDetector.cpp + * Land detection algorithm for vtol + * + * @author Roman Bapst + */ + +#include "VtolLandDetector.h" +#include + +VtolLandDetector::VtolLandDetector() : MulticopterLandDetector(), + _paramHandle(), + _params(), + _airspeedSub(-1), + _parameterSub(-1), + _airspeed{}, + _was_in_air(false), + _airspeed_filtered(0) +{ + _paramHandle.maxAirSpeed = param_find("LNDFW_AIRSPD_MAX"); +} + +void VtolLandDetector::initialize() +{ + MulticopterLandDetector::initialize(); + _airspeedSub = orb_subscribe(ORB_ID(airspeed)); + + // download parameters + updateParameterCache(true); +} + +void VtolLandDetector::updateSubscriptions() +{ + MulticopterLandDetector::updateSubscriptions(); + + orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); +} + +bool VtolLandDetector::update() +{ + updateSubscriptions(); + updateParameterCache(false); + + // this is returned from the mutlicopter land detector + bool landed = get_landed_state(); + + // for vtol we additionally consider airspeed + if (hrt_elapsed_time(&_airspeed.timestamp) < 500 * 1000) { + _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; + + } else { + // if airspeed does not update, set it to zero and rely on multicopter land detector + _airspeed_filtered = 0.0f; + } + + // only consider airspeed if we have been in air before to avoid false + // detections in the case of wind on the ground + if (_was_in_air && _airspeed_filtered > _params.maxAirSpeed) { + landed = false; + } + + _was_in_air = !landed; + + return landed; +} + +void VtolLandDetector::updateParameterCache(const bool force) +{ + MulticopterLandDetector::updateParameterCache(force); + + bool updated; + parameter_update_s paramUpdate; + + orb_check(_parameterSub, &updated); + + if (updated) { + orb_copy(ORB_ID(parameter_update), _parameterSub, ¶mUpdate); + } + + if (updated || force) { + param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed); + } +} diff --git a/src/modules/land_detector/VtolLandDetector.h b/src/modules/land_detector/VtolLandDetector.h new file mode 100644 index 0000000000..d36140d64f --- /dev/null +++ b/src/modules/land_detector/VtolLandDetector.h @@ -0,0 +1,94 @@ +/**************************************************************************** + * + * 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 VtolLandDetector.h + * Land detection algorithm for vtol + * + * @author Roman Bapst + */ + +#ifndef __VTOL_LAND_DETECTOR_H__ +#define __VTOL_LAND_DETECTOR_H__ + +#include "MulticopterLandDetector.h" +#include + +class VtolLandDetector : public MulticopterLandDetector +{ +public: + VtolLandDetector(); + + +private: + /** + * @brief polls all subscriptions and pulls any data that has changed + **/ + void updateSubscriptions() override; + + /** + * @brief Runs one iteration of the land detection algorithm + **/ + bool update() override; + + /** + * @brief Initializes the land detection algorithm + **/ + void initialize() override; + + /** + * @brief download and update local parameter cache + **/ + void updateParameterCache(const bool force) override; + + /** + * @brief Handles for interesting parameters + **/ + struct { + param_t maxAirSpeed; + } _paramHandle; + + struct { + float maxAirSpeed; + } _params; + + int _airspeedSub; + int _parameterSub; + + struct airspeed_s _airspeed; + + bool _was_in_air; /**< indicates whether the vehicle was in the air in the previous iteration */ + float _airspeed_filtered; /**< low pass filtered airspeed */ +}; + +#endif diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index 0aa6e1fad2..1646bf89da 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -53,6 +53,7 @@ #include "FixedwingLandDetector.h" #include "MulticopterLandDetector.h" +#include "VtolLandDetector.h" //Function prototypes static int land_detector_start(const char *mode); @@ -113,6 +114,9 @@ static int land_detector_start(const char *mode) } else if (!strcmp(mode, "multicopter")) { land_detector_task = new MulticopterLandDetector(); + } else if (!strcmp(mode, "vtol")) { + land_detector_task = new VtolLandDetector(); + } else { warnx("[mode] must be either 'fixedwing' or 'multicopter'"); return -1; diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index d9201eb18a..690467b97c 100644 --- a/src/modules/land_detector/land_detector_params.c +++ b/src/modules/land_detector/land_detector_params.c @@ -109,6 +109,19 @@ PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 5.0f); */ PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 10.0f); +/** + * Fixedwing max short-term velocity + * + * Maximum velocity integral in flight direction allowed in the landed state (m/s) + * + * @min 2 + * @max 10 + * @unit m/s + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LNDFW_VELI_MAX, 4.0f); + /** * Airspeed max * diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index d1262f9177..8b457a68db 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -711,6 +711,7 @@ void BlockLocalPositionEstimator::publishGlobalPos() _pub_gpos.get().terrain_alt = 0; _pub_gpos.get().terrain_alt_valid = false; _pub_gpos.get().dead_reckoning = !_canEstimateXY && !_xyTimeout; + _pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter[0]; _pub_gpos.update(); } } diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 3a2f1d583b..07b768354f 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1232,7 +1232,7 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate) usleep(MAIN_LOOP_DELAY / 2); } while (_subscribe_to_stream != nullptr); - delete s; + delete[] s; } } @@ -1523,6 +1523,9 @@ Mavlink::task_main(int argc, char *argv[]) } else if (strcmp(myoptarg, "osd") == 0) { _mode = MAVLINK_MODE_OSD; + } else if (strcmp(myoptarg, "magic") == 0) { + _mode = MAVLINK_MODE_MAGIC; + } else if (strcmp(myoptarg, "config") == 0) { _mode = MAVLINK_MODE_CONFIG; } @@ -1686,6 +1689,8 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("DISTANCE_SENSOR", 0.5f); configure_stream("OPTICAL_FLOW_RAD", 5.0f); configure_stream("EXTENDED_SYS_STATE", 1.0f); + configure_stream("ALTITUDE", 1.0f); + configure_stream("VISION_POSITION_NED", 10.0f); break; case MAVLINK_MODE_ONBOARD: @@ -1709,9 +1714,11 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("SYSTEM_TIME", 1.0f); configure_stream("TIMESYNC", 10.0f); configure_stream("ACTUATOR_CONTROL_TARGET0", 10.0f); - /* camera trigger is rate limited at the source, do not limit here */ + //camera trigger is rate limited at the source, do not limit here configure_stream("CAMERA_TRIGGER", 500.0f); configure_stream("EXTENDED_SYS_STATE", 2.0f); + configure_stream("ALTITUDE", 10.0f); + configure_stream("VISION_POSITION_NED", 10.0f); break; case MAVLINK_MODE_OSD: @@ -1727,6 +1734,11 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("RC_CHANNELS", 5.0f); configure_stream("SERVO_OUTPUT_RAW_0", 1.0f); configure_stream("EXTENDED_SYS_STATE", 1.0f); + configure_stream("ALTITUDE", 1.0f); + break; + + case MAVLINK_MODE_MAGIC: + //stream nothing break; case MAVLINK_MODE_CONFIG: @@ -1743,7 +1755,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("VFR_HUD", 20.0f); configure_stream("ATTITUDE", 100.0f); configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f); - configure_stream("RC_CHANNELS", 5.0f); + configure_stream("RC_CHANNELS", 10.0f); configure_stream("SERVO_OUTPUT_RAW_0", 20.0f); configure_stream("SERVO_OUTPUT_RAW_1", 20.0f); configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f); @@ -1753,6 +1765,8 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("GPS_RAW_INT", 20.0f); configure_stream("CAMERA_TRIGGER", 500.0f); configure_stream("EXTENDED_SYS_STATE", 2.0f); + configure_stream("ALTITUDE", 10.0f); + configure_stream("VISION_POSITION_NED", 10.0f); default: break; diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 95e0fdb91a..37c66b5963 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -149,6 +149,7 @@ public: MAVLINK_MODE_CUSTOM, MAVLINK_MODE_ONBOARD, MAVLINK_MODE_OSD, + MAVLINK_MODE_MAGIC, MAVLINK_MODE_CONFIG }; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index be3ab850f4..ea8d2fae84 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -59,6 +59,7 @@ #include #include #include +#include #include #include #include @@ -79,6 +80,8 @@ #include #include +#include + #include "mavlink_messages.h" #include "mavlink_main.h" @@ -155,6 +158,14 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL; break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF; + break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED @@ -909,7 +920,7 @@ protected: if (updated) { mavlink_vfr_hud_t msg; - msg.airspeed = airspeed.true_airspeed_m_s; + msg.airspeed = airspeed.indicated_airspeed_m_s; msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f; @@ -969,7 +980,7 @@ protected: struct vehicle_gps_position_s gps; if (_gps_sub->update(&_gps_time, &gps)) { - mavlink_gps_raw_int_t msg; + mavlink_gps_raw_int_t msg = {}; msg.time_usec = gps.timestamp_position; msg.fix_type = gps.fix_type; @@ -1208,6 +1219,69 @@ protected: } }; +class MavlinkStreamVisionPositionNED : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamVisionPositionNED::get_name_static(); + } + + static const char *get_name_static() + { + return "VISION_POSITION_NED"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamVisionPositionNED(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } +private: + + MavlinkOrbSubscription *_pos_sub; + uint64_t _pos_time; + + /* do not allow top copying this class */ + MavlinkStreamVisionPositionNED(MavlinkStreamVisionPositionNED &); + MavlinkStreamVisionPositionNED& operator = (const MavlinkStreamVisionPositionNED &); + +protected: + explicit MavlinkStreamVisionPositionNED(Mavlink *mavlink) : MavlinkStream(mavlink), + _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vision_position_estimate))), + _pos_time(0) + {} + + void send(const hrt_abstime t) + { + struct vision_position_estimate_s vpos; + memset(&vpos, 0, sizeof(vpos)); + + if (_pos_sub->update(&_pos_time, &vpos)) { + mavlink_vision_position_estimate_t vmsg; + vmsg.usec = vpos.timestamp_boot / 1000; + vmsg.x = vpos.x; + vmsg.y = vpos.y; + vmsg.z = vpos.z; + math::Quaternion q(vpos.q); + math::Vector<3> rpy = q.to_euler(); + vmsg.roll = rpy(0); + vmsg.pitch = rpy(1); + vmsg.yaw = rpy(2); + + _mavlink->send_message(MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, &vmsg); + } + } +}; class MavlinkStreamLocalPositionNED : public MavlinkStream { @@ -1727,7 +1801,7 @@ protected: float out[8]; - const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; + const float pwm_center = (PWM_DEFAULT_MAX + PWM_DEFAULT_MIN) / 2; unsigned system_type = _mavlink->get_system_type(); @@ -1765,14 +1839,14 @@ protected: } for (unsigned i = 0; i < 8; i++) { - if (act.output[i] > PWM_LOWEST_MIN / 2) { + if (act.output[i] > PWM_DEFAULT_MIN / 2) { if (i < n) { /* scale PWM out 900..2100 us to 0..1 for rotors */ - out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + out[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN); } else { /* scale PWM out 900..2100 us to -1..1 for other channels */ - out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); + out[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2); } } else { @@ -1785,14 +1859,14 @@ protected: /* fixed wing: scale throttle to 0..1 and other channels to -1..1 */ for (unsigned i = 0; i < 8; i++) { - if (act.output[i] > PWM_LOWEST_MIN / 2) { + if (act.output[i] > PWM_DEFAULT_MIN / 2) { if (i != 3) { /* scale PWM out 900..2100 us to -1..1 for normal channels */ - out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); + out[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2); } else { /* scale PWM out 900..2100 us to 0..1 for throttle */ - out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + out[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN); } } else { @@ -1939,6 +2013,9 @@ protected: msg.vx = pos_sp.vx; msg.vy = pos_sp.vy; msg.vz = pos_sp.vz; + msg.afx = pos_sp.acc_x; + msg.afy = pos_sp.acc_y; + msg.afz = pos_sp.acc_z; _mavlink->send_message(MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, &msg); } @@ -2537,6 +2614,91 @@ protected: } }; +class MavlinkStreamAltitude : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamAltitude::get_name_static(); + } + + static const char *get_name_static() + { + return "ALTITUDE"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_ALTITUDE; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamAltitude(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_ALTITUDE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_global_pos_sub; + uint64_t _global_pos_time; + + MavlinkOrbSubscription *_local_pos_sub; + uint64_t _local_pos_time; + + MavlinkOrbSubscription *_home_sub; + uint64_t _home_time; + + /* do not allow top copying this class */ + MavlinkStreamAltitude(MavlinkStreamAltitude &); + MavlinkStreamAltitude& operator = (const MavlinkStreamAltitude &); + +protected: + explicit MavlinkStreamAltitude(Mavlink *mavlink) : MavlinkStream(mavlink), + _global_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))), + _global_pos_time(0), + _local_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))), + _local_pos_time(0), + _home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))), + _home_time(0) + {} + + void send(const hrt_abstime t) + { + struct vehicle_global_position_s global_pos; + struct vehicle_local_position_s local_pos; + struct home_position_s home; + + bool updated = _global_pos_sub->update(&_global_pos_time, &global_pos); + updated |= _local_pos_sub->update(&_local_pos_time, &local_pos); + updated |= _home_sub->update(&_home_time, &home); + + if (updated) { + mavlink_altitude_t msg; + + msg.time_usec = hrt_absolute_time(); + + msg.altitude_monotonic = global_pos.pressure_alt; + msg.altitude_amsl = global_pos.alt; + msg.altitude_local = -local_pos.z; + msg.altitude_relative = home.alt; + + if (global_pos.terrain_alt_valid) { + msg.altitude_terrain = global_pos.terrain_alt; + msg.bottom_clearance = global_pos.alt - global_pos.terrain_alt; + } else { + msg.altitude_terrain = -1001; + msg.bottom_clearance = -1; + } + + _mavlink->send_message(MAVLINK_MSG_ID_ALTITUDE, &msg); + } + } +}; + const StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static), new StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static), @@ -2551,6 +2713,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(&MavlinkStreamVisionPositionNED::new_instance, &MavlinkStreamVisionPositionNED::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), @@ -2574,5 +2737,6 @@ const StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamCameraTrigger::new_instance, &MavlinkStreamCameraTrigger::get_name_static), new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static), new StreamListItem(&MavlinkStreamExtendedSysState::new_instance, &MavlinkStreamExtendedSysState::get_name_static), + new StreamListItem(&MavlinkStreamAltitude::new_instance, &MavlinkStreamAltitude::get_name_static), nullptr }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 0c2180a4d8..f4a3fbd208 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -43,6 +43,8 @@ /* XXX trim includes */ #include #include +#include +#include #include #include #include @@ -537,7 +539,7 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg) /* copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ vcmd.param1 = new_mode.base_mode; vcmd.param2 = custom_mode.main_mode; - vcmd.param3 = 0; + vcmd.param3 = custom_mode.sub_mode; vcmd.param4 = 0; vcmd.param5 = 0; vcmd.param6 = 0; @@ -719,7 +721,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t } /* set the yaw sp value */ - if (!offboard_control_mode.ignore_attitude && !isnan(set_position_target_local_ned.yaw)) { + if (!offboard_control_mode.ignore_attitude && PX4_ISFINITE(set_position_target_local_ned.yaw)) { pos_sp_triplet.current.yaw_valid = true; pos_sp_triplet.current.yaw = set_position_target_local_ned.yaw; @@ -728,7 +730,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t } /* set the yawrate sp value */ - if (!offboard_control_mode.ignore_bodyrate && !isnan(set_position_target_local_ned.yaw)) { + if (!offboard_control_mode.ignore_bodyrate && PX4_ISFINITE(set_position_target_local_ned.yaw)) { pos_sp_triplet.current.yawspeed_valid = true; pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate; @@ -1147,13 +1149,6 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) manual.r = man.r / 1000.0f; manual.z = man.z / 1000.0f; - manual.mode_switch = decode_switch_pos(man.buttons, 0); - manual.return_switch = decode_switch_pos(man.buttons, 1); - manual.posctl_switch = decode_switch_pos(man.buttons, 2); - manual.loiter_switch = decode_switch_pos(man.buttons, 3); - manual.acro_switch = decode_switch_pos(man.buttons, 4); - manual.offboard_switch = decode_switch_pos(man.buttons, 5); - if (_manual_pub == nullptr) { _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); @@ -1183,8 +1178,7 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) tstatus.heartbeat_time = tstatus.timestamp; if (_telemetry_status_pub == nullptr) { - int multi_instance; - _telemetry_status_pub = orb_advertise_multi(ORB_ID(telemetry_status), &tstatus, &multi_instance, ORB_PRIO_HIGH); + _telemetry_status_pub = orb_advertise_multi(ORB_ID(telemetry_status), &tstatus, NULL, ORB_PRIO_HIGH); } else { orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); @@ -1325,8 +1319,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) struct airspeed_s airspeed = {}; float ias = calc_indicated_airspeed(imu.diff_pressure * 1e2f); - // XXX need to fix this - float tas = ias; + float tas = calc_true_airspeed_from_indicated(ias, imu.abs_pressure * 100, imu.temperature); airspeed.timestamp = timestamp; airspeed.indicated_airspeed_m_s = ias; @@ -1751,10 +1744,15 @@ void * MavlinkReceiver::receive_thread(void *arg) { + /* set thread name */ + char thread_name[24]; + sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id()); + px4_prctl(PR_SET_NAME, thread_name, getpid()); + const int timeout = 500; #ifdef __PX4_POSIX /* 1500 is the Wifi MTU, so we make sure to fit a full packet */ - uint8_t buf[1600]; + uint8_t buf[1600 * 5]; #else /* the serial port buffers internally as well, we just need to fit a small chunk */ uint8_t buf[64]; @@ -1767,12 +1765,6 @@ MavlinkReceiver::receive_thread(void *arg) if (_mavlink->get_protocol() == SERIAL) { uart_fd = _mavlink->get_uart_fd(); -#ifndef __PX4_POSIX - /* set thread name */ - char thread_name[24]; - sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id()); - prctl(PR_SET_NAME, thread_name, getpid()); -#endif fds[0].fd = uart_fd; fds[0].events = POLLIN; @@ -1837,8 +1829,10 @@ MavlinkReceiver::receive_thread(void *arg) } } - /* count received bytes */ - _mavlink->count_rxbytes(nread); + /* count received bytes (nread will be -1 on read error) */ + if (nread > 0) { + _mavlink->count_rxbytes(nread); + } } } @@ -1872,6 +1866,7 @@ void MavlinkReceiver::smooth_time_offset(uint64_t offset_ns) void *MavlinkReceiver::start_helper(void *context) { + MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink *)context); rcv->receive_thread(NULL); 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 c36ba5bfc6..a49714ff52 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013 - 2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -69,6 +69,7 @@ #include #include #include +#include #include #include #include @@ -90,6 +91,7 @@ #define SIGMA 0.000001f #define MIN_DIST 0.01f #define MANUAL_THROTTLE_MAX_MULTICOPTER 0.9f +#define ONE_G 9.8066f /** * Multicopter position control app start / stop handling function @@ -141,6 +143,8 @@ private: orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */ orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint publication */ + orb_id_t _attitude_setpoint_id; + struct vehicle_status_s _vehicle_status; /**< vehicle status */ struct control_state_s _ctrl_state; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ @@ -185,6 +189,7 @@ private: param_t hold_z_dz; param_t hold_max_xy; param_t hold_max_z; + param_t acc_hor_max; } _params_handles; /**< handles for interesting parameters */ struct { @@ -201,6 +206,7 @@ private: float hold_z_dz; float hold_max_xy; float hold_max_z; + float acc_hor_max; math::Vector<3> pos_p; math::Vector<3> vel_p; @@ -229,6 +235,9 @@ private: math::Vector<3> _vel_sp; math::Vector<3> _vel_prev; /**< velocity on previous step */ math::Vector<3> _vel_ff; + math::Vector<3> _vel_sp_prev; + math::Vector<3> _thrust_sp_prev; + math::Vector<3> _vel_err_d; /**< derivative of current velocity */ math::Matrix<3, 3> _R; /**< rotation matrix from attitude quaternions */ float _yaw; /**< yaw angle (euler) */ @@ -255,12 +264,21 @@ private: */ void update_ref(); /** - * Reset position setpoint to current position + * Reset position setpoint to current position. + * + * This reset will only occur if the _reset_pos_sp flag has been set. + * The general logic is to first "activate" the flag in the flight + * regime where a switch to a position control mode should hold the + * very last position. Once switching to a position control mode + * the last position is stored once. */ void reset_pos_sp(); /** - * Reset altitude setpoint to current altitude + * Reset altitude setpoint to current altitude. + * + * This reset will only occur if the _reset_alt_sp flag has been set. + * The general logic follows the reset_pos_sp() architecture. */ void reset_alt_sp(); @@ -279,8 +297,8 @@ private: */ void control_offboard(float dt); - bool cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r, - const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res); + bool cross_sphere_line(const math::Vector<3> &sphere_c, float sphere_r, + const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3> &res); /** * Set position setpoint for AUTO @@ -321,7 +339,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _control_task(-1), _mavlink_fd(-1), -/* subscriptions */ + /* subscriptions */ _ctrl_state_sub(-1), _att_sp_sub(-1), _control_mode_sub(-1), @@ -332,10 +350,11 @@ MulticopterPositionControl::MulticopterPositionControl() : _pos_sp_triplet_sub(-1), _global_vel_sp_sub(-1), -/* publications */ + /* publications */ _att_sp_pub(nullptr), _local_pos_sp_pub(nullptr), _global_vel_sp_pub(nullptr), + _attitude_setpoint_id(0), _manual_thr_min(this, "MANTHR_MIN"), _manual_thr_max(this, "MANTHR_MAX"), _vel_x_deriv(this, "VELD"), @@ -380,6 +399,8 @@ MulticopterPositionControl::MulticopterPositionControl() : _vel_sp.zero(); _vel_prev.zero(); _vel_ff.zero(); + _vel_sp_prev.zero(); + _vel_err_d.zero(); _R.identity(); @@ -408,6 +429,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _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"); + _params_handles.acc_hor_max = param_find("MPC_ACC_HOR_MAX"); /* fetch initial parameter values */ @@ -506,6 +528,8 @@ MulticopterPositionControl::parameters_update(bool force) _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); + param_get(_params_handles.acc_hor_max, &v); + _params.acc_hor_max = v; _params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f; @@ -517,7 +541,7 @@ MulticopterPositionControl::parameters_update(bool force) _params.man_roll_max = math::radians(_params.man_roll_max); _params.man_pitch_max = math::radians(_params.man_pitch_max); _params.man_yaw_max = math::radians(_params.man_yaw_max); - param_get(_params_handles.mc_att_yaw_p,&v); + param_get(_params_handles.mc_att_yaw_p, &v); _params.mc_att_yaw_p = v; } @@ -533,6 +557,16 @@ MulticopterPositionControl::poll_subscriptions() if (updated) { orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + + /* set correct uORB ID, depending on if vehicle is VTOL or not */ + if (!_attitude_setpoint_id) { + if (_vehicle_status.is_vtol) { + _attitude_setpoint_id = ORB_ID(mc_virtual_attitude_setpoint); + + } else { + _attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint); + } + } } orb_check(_ctrl_state_sub, &updated); @@ -626,10 +660,11 @@ 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) * _vel_sp(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) * _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)); + - _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)); } } @@ -639,7 +674,7 @@ MulticopterPositionControl::reset_alt_sp() if (_reset_alt_sp) { _reset_alt_sp = false; _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)); + //mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2)); } } @@ -703,7 +738,8 @@ MulticopterPositionControl::control_manual(float dt) /* _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 + 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 @@ -711,20 +747,20 @@ MulticopterPositionControl::control_manual(float dt) */ /* horizontal axes */ - if (_control_mode.flag_control_position_enabled) - { + 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 (fabsf(req_vel_sp(0)) < _params.hold_xy_dz && fabsf(req_vel_sp(1)) < _params.hold_xy_dz) { if (!_pos_hold_engaged) { - if (_params.hold_max_xy < FLT_EPSILON || (fabsf(_vel(0)) < _params.hold_max_xy && fabsf(_vel(1)) < _params.hold_max_xy)) { + if (_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; } } - } - else { + + } else { _pos_hold_engaged = false; } @@ -739,25 +775,27 @@ MulticopterPositionControl::control_manual(float dt) } /* vertical axis */ - if (_control_mode.flag_control_altitude_enabled) - { + 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; + if (fabsf(req_vel_sp(2)) < _params.hold_z_dz) { + if (!_alt_hold_engaged) { + if (_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z) { + _alt_hold_engaged = true; + + } else { + _alt_hold_engaged = false; + } } - } - else { + + } else { _alt_hold_engaged = false; - _pos_sp(2) = _pos(2); } /* 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); + _pos_sp(2) = _pos(2); } } } @@ -777,6 +815,7 @@ MulticopterPositionControl::control_offboard(float dt) /* control position */ _pos_sp(0) = _pos_sp_triplet.current.x; _pos_sp(1) = _pos_sp_triplet.current.y; + } else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) { /* control velocity */ /* reset position setpoint to current position if needed */ @@ -791,6 +830,7 @@ MulticopterPositionControl::control_offboard(float dt) if (_pos_sp_triplet.current.yaw_valid) { _att_sp.yaw_body = _pos_sp_triplet.current.yaw; + } else if (_pos_sp_triplet.current.yawspeed_valid) { _att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt; } @@ -798,6 +838,7 @@ MulticopterPositionControl::control_offboard(float dt) if (_control_mode.flag_control_altitude_enabled && _pos_sp_triplet.current.position_valid) { /* Control altitude */ _pos_sp(2) = _pos_sp_triplet.current.z; + } else if (_control_mode.flag_control_climb_rate_enabled && _pos_sp_triplet.current.velocity_valid) { /* reset alt setpoint to current altitude if needed */ reset_alt_sp(); @@ -807,6 +848,7 @@ MulticopterPositionControl::control_offboard(float dt) _run_alt_control = false; /* request velocity setpoint to be used, instead of position setpoint */ } + } else { reset_pos_sp(); reset_alt_sp(); @@ -814,8 +856,8 @@ MulticopterPositionControl::control_offboard(float dt) } bool -MulticopterPositionControl::cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r, - const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res) +MulticopterPositionControl::cross_sphere_line(const math::Vector<3> &sphere_c, float sphere_r, + const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3> &res) { /* project center of sphere on line */ /* normalized AB */ @@ -845,18 +887,21 @@ void MulticopterPositionControl::control_auto(float dt) /* reset position setpoint on AUTO mode activation */ reset_pos_sp(); reset_alt_sp(); + /* set current velocity as last target velocity to smooth out transition */ + _vel_sp_prev = _vel; } //Poll position setpoint bool updated; orb_check(_pos_sp_triplet_sub, &updated); + if (updated) { orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); //Make sure that the position setpoint is valid if (!PX4_ISFINITE(_pos_sp_triplet.current.lat) || - !PX4_ISFINITE(_pos_sp_triplet.current.lon) || - !PX4_ISFINITE(_pos_sp_triplet.current.alt)) { + !PX4_ISFINITE(_pos_sp_triplet.current.lon) || + !PX4_ISFINITE(_pos_sp_triplet.current.alt)) { _pos_sp_triplet.current.valid = false; } } @@ -876,21 +921,21 @@ void MulticopterPositionControl::control_auto(float dt) curr_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt); if (PX4_ISFINITE(curr_sp(0)) && - PX4_ISFINITE(curr_sp(1)) && - PX4_ISFINITE(curr_sp(2))) { + PX4_ISFINITE(curr_sp(1)) && + PX4_ISFINITE(curr_sp(2))) { current_setpoint_valid = true; } } if (_pos_sp_triplet.previous.valid) { map_projection_project(&_ref_pos, - _pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, - &prev_sp.data[0], &prev_sp.data[1]); + _pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, + &prev_sp.data[0], &prev_sp.data[1]); prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt); if (PX4_ISFINITE(prev_sp(0)) && - PX4_ISFINITE(prev_sp(1)) && - PX4_ISFINITE(prev_sp(2))) { + PX4_ISFINITE(prev_sp(1)) && + PX4_ISFINITE(prev_sp(2))) { previous_setpoint_valid = true; } } @@ -920,14 +965,15 @@ void MulticopterPositionControl::control_auto(float dt) math::Vector<3> prev_curr_s = curr_sp_s - prev_sp_s; math::Vector<3> curr_pos_s = pos_s - curr_sp_s; float curr_pos_s_len = curr_pos_s.length(); + if (curr_pos_s_len < 1.0f) { /* copter is closer to waypoint than unit radius */ /* check next waypoint and use it to avoid slowing down when passing via waypoint */ if (_pos_sp_triplet.next.valid) { math::Vector<3> next_sp; map_projection_project(&_ref_pos, - _pos_sp_triplet.next.lat, _pos_sp_triplet.next.lon, - &next_sp.data[0], &next_sp.data[1]); + _pos_sp_triplet.next.lat, _pos_sp_triplet.next.lon, + &next_sp.data[0], &next_sp.data[1]); next_sp(2) = -(_pos_sp_triplet.next.alt - _ref_alt); if ((next_sp - curr_sp).length() > MIN_DIST) { @@ -945,6 +991,7 @@ void MulticopterPositionControl::control_auto(float dt) if (cos_a_curr_next > 0.0f && cos_b > 0.0f) { float curr_next_s_len = curr_next_s.length(); + /* if curr - next distance is larger than unit radius, limit it */ if (curr_next_s_len > 1.0f) { cos_a_curr_next /= curr_next_s_len; @@ -952,8 +999,8 @@ void MulticopterPositionControl::control_auto(float dt) /* feed forward position setpoint offset */ math::Vector<3> pos_ff = prev_curr_s_norm * - cos_a_curr_next * cos_b * cos_b * (1.0f - curr_pos_s_len) * - (1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f)); + cos_a_curr_next * cos_b * cos_b * (1.0f - curr_pos_s_len) * + (1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f)); pos_sp_s += pos_ff; } } @@ -961,6 +1008,7 @@ void MulticopterPositionControl::control_auto(float dt) } else { bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, pos_sp_s); + if (near) { /* unit sphere crosses trajectory */ @@ -988,6 +1036,7 @@ void MulticopterPositionControl::control_auto(float dt) /* difference between current and desired position setpoints, 1 = max speed */ math::Vector<3> d_pos_m = (pos_sp_s - pos_sp_old_s).edivide(_params.pos_p); float d_pos_m_len = d_pos_m.length(); + if (d_pos_m_len > dt) { pos_sp_s = pos_sp_old_s + (d_pos_m / d_pos_m_len * dt).emult(_params.pos_p); } @@ -1091,6 +1140,7 @@ MulticopterPositionControl::task_main() /* reset setpoints and integrals on arming */ _reset_pos_sp = true; _reset_alt_sp = true; + _vel_sp_prev.zero(); reset_int_z = true; reset_int_xy = true; reset_yaw_sp = true; @@ -1109,19 +1159,39 @@ MulticopterPositionControl::task_main() update_ref(); + /* Update velocity derivative, + * independent of the current flight mode + */ + if (_local_pos.timestamp > 0) { + + if (PX4_ISFINITE(_local_pos.x) && + PX4_ISFINITE(_local_pos.y) && + PX4_ISFINITE(_local_pos.z)) { + + _pos(0) = _local_pos.x; + _pos(1) = _local_pos.y; + _pos(2) = _local_pos.z; + } + + if (PX4_ISFINITE(_local_pos.vx) && + PX4_ISFINITE(_local_pos.vy) && + PX4_ISFINITE(_local_pos.vz)) { + + _vel(0) = _local_pos.vx; + _vel(1) = _local_pos.vy; + _vel(2) = _local_pos.vz; + } + + _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)); + } + if (_control_mode.flag_control_altitude_enabled || _control_mode.flag_control_position_enabled || _control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) { - _pos(0) = _local_pos.x; - _pos(1) = _local_pos.y; - _pos(2) = _local_pos.z; - - _vel(0) = _local_pos.vx; - _vel(1) = _local_pos.vy; - _vel(2) = _local_pos.vz; - _vel_ff.zero(); /* by default, run position/altitude controller. the control_* functions @@ -1129,6 +1199,16 @@ MulticopterPositionControl::task_main() _run_pos_control = true; _run_alt_control = true; + // reset the horizontal and vertical position hold flags for non-manual modes + // or if position is not controlled + if (!_control_mode.flag_control_position_enabled || !_control_mode.flag_control_manual_enabled) { + _pos_hold_engaged = false; + } + + if (!_control_mode.flag_control_altitude_enabled || !_control_mode.flag_control_manual_enabled) { + _alt_hold_engaged = false; + } + /* select control source */ if (_control_mode.flag_control_manual_enabled) { /* manual control */ @@ -1145,7 +1225,8 @@ MulticopterPositionControl::task_main() control_auto(dt); } - if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { + if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid + && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { /* idle state, don't run controller and set zero thrust */ R.identity(); memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); @@ -1160,10 +1241,10 @@ MulticopterPositionControl::task_main() /* publish attitude setpoint */ if (_att_sp_pub != nullptr) { - orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); + orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp); - } else { - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + } else if (_attitude_setpoint_id) { + _att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp); } } else { @@ -1178,18 +1259,20 @@ MulticopterPositionControl::task_main() } /* make sure velocity setpoint is saturated in xy*/ - float vel_norm_xy = sqrtf(_vel_sp(0)*_vel_sp(0) + - _vel_sp(1)*_vel_sp(1)); - if (vel_norm_xy > _params.vel_max(0)) { + float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) + + _vel_sp(1) * _vel_sp(1)); + + if (vel_norm_xy > _params.vel_max(0)) { /* note assumes vel_max(0) == vel_max(1) */ - _vel_sp(0) = _vel_sp(0)*_params.vel_max(0)/vel_norm_xy; - _vel_sp(1) = _vel_sp(1)*_params.vel_max(1)/vel_norm_xy; + _vel_sp(0) = _vel_sp(0) * _params.vel_max(0) / vel_norm_xy; + _vel_sp(1) = _vel_sp(1) * _params.vel_max(1) / vel_norm_xy; } /* make sure velocity setpoint is saturated in z*/ - float vel_norm_z = sqrtf(_vel_sp(2)*_vel_sp(2)); + float vel_norm_z = sqrtf(_vel_sp(2) * _vel_sp(2)); + if (vel_norm_z > _params.vel_max(2)) { - _vel_sp(2) = _vel_sp(2)*_params.vel_max(2)/vel_norm_z; + _vel_sp(2) = _vel_sp(2) * _params.vel_max(2) / vel_norm_z; } if (!_control_mode.flag_control_position_enabled) { @@ -1201,6 +1284,8 @@ MulticopterPositionControl::task_main() } if (!_control_mode.flag_control_velocity_enabled) { + _vel_sp_prev(0) = _vel(0); + _vel_sp_prev(1) = _vel(1); _vel_sp(0) = 0.0f; _vel_sp(1) = 0.0f; } @@ -1210,10 +1295,27 @@ MulticopterPositionControl::task_main() } /* use constant descend rate when landing, ignore altitude setpoint */ - if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { + if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid + && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { _vel_sp(2) = _params.land_speed; } + // limit total horizontal acceleration + math::Vector<2> acc_hor; + acc_hor(0) = (_vel_sp(0) - _vel_sp_prev(0)) / dt; + acc_hor(1) = (_vel_sp(1) - _vel_sp_prev(1)) / dt; + + if (acc_hor.length() > _params.acc_hor_max) { + acc_hor.normalize(); + acc_hor *= _params.acc_hor_max; + math::Vector<2> vel_sp_hor_prev(_vel_sp_prev(0), _vel_sp_prev(1)); + math::Vector<2> vel_sp_hor = acc_hor * dt + vel_sp_hor_prev; + _vel_sp(0) = vel_sp_hor(0); + _vel_sp(1) = vel_sp_hor(1); + } + + _vel_sp_prev = _vel_sp; + _global_vel_sp.vx = _vel_sp(0); _global_vel_sp.vy = _vel_sp(1); _global_vel_sp.vz = _vel_sp(2); @@ -1265,15 +1367,9 @@ MulticopterPositionControl::task_main() /* velocity error */ math::Vector<3> vel_err = _vel_sp - _vel; - /* 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; + // TODO?: + _vel_sp.emult(_params.vel_ff) + math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + _vel_err_d.emult(_params.vel_d) + thrust_int; if (!_control_mode.flag_control_velocity_enabled) { thrust_sp(0) = 0.0f; @@ -1300,7 +1396,7 @@ MulticopterPositionControl::task_main() /* adjust limits for landing mode */ if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && - _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { + _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { /* limit max tilt and min lift when landing */ tilt_max = _params.tilt_max_land; @@ -1316,6 +1412,7 @@ MulticopterPositionControl::task_main() } if (_control_mode.flag_control_velocity_enabled) { + /* limit max tilt */ if (thr_min >= 0.0f && tilt_max < M_PI_F / 2 - 0.05f) { /* absolute horizontal thrust */ @@ -1333,9 +1430,10 @@ MulticopterPositionControl::task_main() } } } + } - } else { - /* thrust compensation for altitude only control mode */ + if (_control_mode.flag_control_altitude_enabled) { + /* thrust compensation for altitude only control modes */ float att_comp; if (_R(2, 2) > TILT_COS_MAX) { @@ -1485,9 +1583,9 @@ MulticopterPositionControl::task_main() _att_sp.thrust = thrust_abs; /* save thrust setpoint for logging */ - _local_pos_sp.acc_x = thrust_sp(0); - _local_pos_sp.acc_y = thrust_sp(1); - _local_pos_sp.acc_z = thrust_sp(2); + _local_pos_sp.acc_x = thrust_sp(0) * ONE_G; + _local_pos_sp.acc_y = thrust_sp(1) * ONE_G; + _local_pos_sp.acc_z = thrust_sp(2) * ONE_G; _att_sp.timestamp = hrt_absolute_time(); @@ -1510,6 +1608,7 @@ MulticopterPositionControl::task_main() /* publish local position setpoint */ if (_local_pos_sp_pub != nullptr) { orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp); + } else { _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp); } @@ -1521,10 +1620,13 @@ MulticopterPositionControl::task_main() _mode_auto = false; reset_int_z = true; reset_int_xy = true; + + /* store last velocity in case a mode switch to position control occurs */ + _vel_sp_prev = _vel; } /* generate attitude setpoint from manual controls */ - if(_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) { + if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) { /* reset yaw setpoint to current position if needed */ if (reset_yaw_sp) { @@ -1533,8 +1635,7 @@ MulticopterPositionControl::task_main() } /* do not move yaw while arming */ - else if (_manual.z > 0.1f) - { + else if (_manual.z > 0.1f) { const float yaw_offset_max = _params.man_yaw_max / _params.mc_att_yaw_p; _att_sp.yaw_sp_move_rate = _manual.r * _params.man_yaw_max; @@ -1564,18 +1665,27 @@ MulticopterPositionControl::task_main() } } + math::Matrix<3, 3> R_sp; + /* construct attitude setpoint rotation matrix */ - math::Matrix<3,3> R_sp; - R_sp.from_euler(_att_sp.roll_body,_att_sp.pitch_body,_att_sp.yaw_body); + R_sp.from_euler(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body); memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body)); + /* reset the acceleration set point for all non-attitude flight modes */ + if (!(_control_mode.flag_control_offboard_enabled && + !(_control_mode.flag_control_position_enabled || + _control_mode.flag_control_velocity_enabled))) { + + _thrust_sp_prev = R_sp * math::Vector<3>(0, 0, -_att_sp.thrust); + } + /* copy quaternion setpoint to attitude setpoint topic */ math::Quaternion q_sp; q_sp.from_dcm(R_sp); memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d)); _att_sp.timestamp = hrt_absolute_time(); - } - else { + + } else { reset_yaw_sp = true; } @@ -1584,20 +1694,25 @@ MulticopterPositionControl::task_main() /* publish attitude setpoint * Do not publish if offboard is enabled but position/velocity control is disabled, - * in this case the attitude setpoint is published by the mavlink app + * in this case the attitude setpoint is published by the mavlink app. Also do not publish + * if the vehicle is a VTOL and it's just doing a transition (the VTOL attitude control module will generate + * attitude setpoints for the transition). */ if (!(_control_mode.flag_control_offboard_enabled && - !(_control_mode.flag_control_position_enabled || - _control_mode.flag_control_velocity_enabled))) { - if (_att_sp_pub != nullptr && (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode)) { - orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); - } else if (_att_sp_pub == nullptr && (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode)) { - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + !(_control_mode.flag_control_position_enabled || + _control_mode.flag_control_velocity_enabled))) { + + if (_att_sp_pub != nullptr) { + orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp); + + } else if (_attitude_setpoint_id) { + _att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp); } } /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ - reset_int_z_manual = _control_mode.flag_armed && _control_mode.flag_control_manual_enabled && !_control_mode.flag_control_climb_rate_enabled; + reset_int_z_manual = _control_mode.flag_armed && _control_mode.flag_control_manual_enabled + && !_control_mode.flag_control_climb_rate_enabled; } mavlink_log_info(_mavlink_fd, "[mpc] stopped"); @@ -1612,11 +1727,11 @@ MulticopterPositionControl::start() /* start the task */ _control_task = px4_task_spawn_cmd("mc_pos_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 1500, - (px4_main_t)&MulticopterPositionControl::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 1500, + (px4_main_t)&MulticopterPositionControl::task_main_trampoline, + nullptr); if (_control_task < 0) { warn("task start failed"); 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 7c63a82afa..65673a8284 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -309,3 +309,13 @@ PARAM_DEFINE_FLOAT(MPC_HOLD_MAX_Z, 0.5f); * @group Multicopter Position Control */ PARAM_DEFINE_FLOAT(MPC_VELD_LP, 5.0f); + +/** + * Maximum horizonal acceleration in velocity controlled modes + * + * @unit m/s/s + * @min 2.0 + * @max 10.0 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_ACC_HOR_MAX, 4.0f); diff --git a/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp b/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp index 6e53f0bbfd..71d052b4c6 100644 --- a/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp +++ b/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp @@ -36,6 +36,7 @@ #include "px4_tasks.h" #include #include +#include #define LOG_TAG "uORBKraitFastRpcChannel.cpp" diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index d8399ccafd..6a5c4c15d9 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -44,6 +44,7 @@ px4_add_module( mission.cpp loiter.cpp rtl.cpp + takeoff.cpp mission_feasibility_checker.cpp geofence.cpp datalinkloss.cpp diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index a19e13ca2f..deaf2dadb8 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -119,7 +119,7 @@ private: float _altitude_min; float _altitude_max; - uint8_t _vertices_count; + unsigned _vertices_count; /* Params */ control::BlockParamInt _param_action; @@ -129,7 +129,7 @@ private: control::BlockParamInt _param_max_hor_distance; control::BlockParamInt _param_max_ver_distance; - uint8_t _outside_counter; + unsigned _outside_counter; int _mavlinkFd; diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index 070fde6174..14902fdd6d 100644 --- a/src/modules/navigator/geofence_params.c +++ b/src/modules/navigator/geofence_params.c @@ -95,6 +95,8 @@ PARAM_DEFINE_INT32(GF_COUNT, -1); * * Set to > 0 to activate a geofence action if horizontal distance to home exceeds this value. * + * @unit meters + * @min -1 * @group Geofence */ PARAM_DEFINE_INT32(GF_MAX_HOR_DIST, -1); @@ -104,6 +106,8 @@ PARAM_DEFINE_INT32(GF_MAX_HOR_DIST, -1); * * Set to > 0 to activate a geofence action if vertical distance to home exceeds this value. * + * @unit meters + * @min -1 * @group Geofence */ PARAM_DEFINE_INT32(GF_MAX_VER_DIST, -1); diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 94ded8a4b9..e2f454d4cf 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -143,7 +143,14 @@ MissionBlock::is_mission_item_reached() } } else { /* for normal mission items used their acceptance radius */ - if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.acceptance_radius)) { + float mission_acceptance_radius = _navigator->get_acceptance_radius(_mission_item.acceptance_radius); + + /* if set to zero use the default instead */ + if (mission_acceptance_radius < NAV_EPSILON_POSITION) { + mission_acceptance_radius = _navigator->get_acceptance_radius(); + } + + if (dist >= 0.0f && dist <= mission_acceptance_radius) { _waypoint_position_reached = true; } } @@ -203,7 +210,8 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite sp->lon = item->lon; sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude; sp->yaw = item->yaw; - sp->loiter_radius = item->loiter_radius; + sp->loiter_radius = (item->loiter_radius > NAV_EPSILON_POSITION) ? item->loiter_radius : + _navigator->get_loiter_radius(); sp->loiter_direction = item->loiter_direction; sp->pitch_min = item->pitch_min; @@ -289,3 +297,29 @@ MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance) item->origin = ORIGIN_ONBOARD; } } + +void +MissionBlock::set_takeoff_item(struct mission_item_s *item, float min_clearance, float min_pitch) +{ + item->nav_cmd = NAV_CMD_LOITER_UNLIMITED; + + /* use current position and use return altitude as clearance */ + item->lat = _navigator->get_global_position()->lat; + item->lon = _navigator->get_global_position()->lon; + item->altitude = _navigator->get_global_position()->alt; + + if (min_clearance > 0.0f) { + item->altitude += min_clearance; + } + + item->altitude_is_relative = false; + item->yaw = NAN; + item->loiter_radius = _navigator->get_loiter_radius(); + item->loiter_direction = 1; + item->acceptance_radius = (_navigator->get_acceptance_radius() > min_clearance / 2.0f) ? + (min_clearance / 2) : _navigator->get_acceptance_radius(); + item->time_inside = 0.0f; + item->pitch_min = min_pitch; + item->autocontinue = false; + item->origin = ORIGIN_ONBOARD; +} diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index eb17a329ee..5490b4c6b8 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -96,6 +96,11 @@ protected: */ void set_loiter_item(struct mission_item_s *item, float min_clearance = -1.0f); + /** + * Set a takeoff mission item + */ + void set_takeoff_item(struct mission_item_s *item, float min_clearance = -1.0f, float min_pitch = 0.0f); + mission_item_s _mission_item; bool _waypoint_position_reached; bool _waypoint_yaw_reached; diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 880d4bd98e..ece75c89f7 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -92,10 +92,6 @@ bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRota failed = failed || !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid); } - if (!failed) { - mavlink_log_info(_mavlink_fd, "Mission checked and ready."); - } - return !failed; } @@ -201,7 +197,6 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s missionitem.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && missionitem.nav_cmd != NAV_CMD_LAND && missionitem.nav_cmd != NAV_CMD_TAKEOFF && - missionitem.nav_cmd != NAV_CMD_ROI && missionitem.nav_cmd != NAV_CMD_PATHPLANNING && missionitem.nav_cmd != NAV_CMD_DO_JUMP && missionitem.nav_cmd != NAV_CMD_DO_SET_SERVO) { diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index d23a20c5ff..b0fabc08f5 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -47,6 +47,8 @@ #define NUM_MISSIONS_SUPPORTED 256 +#define NAV_EPSILON_POSITION 0.001f /**< Anything smaller than this is considered zero */ + /* compatible to mavlink MAV_CMD */ enum NAV_CMD { NAV_CMD_IDLE = 0, @@ -81,6 +83,7 @@ enum ORIGIN { * This is the position the MAV is heading towards. If it of type loiter, * the MAV is circling around it with the given loiter radius in meters. */ +#pragma pack(push, 1) struct mission_item_s { bool altitude_is_relative; /**< true if altitude is relative from start point */ double lat; /**< latitude in degrees */ @@ -89,19 +92,19 @@ struct mission_item_s { float yaw; /**< in radians NED -PI..+PI, NAN means don't change yaw */ float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover */ int8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */ - enum NAV_CMD nav_cmd; /**< navigation command */ + unsigned nav_cmd; /**< navigation command */ float acceptance_radius; /**< default radius in which the mission is accepted as reached in meters */ float time_inside; /**< time that the MAV should stay inside the radius before advancing in seconds */ float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ bool autocontinue; /**< true if next waypoint should follow after this one */ - enum ORIGIN origin; /**< where the waypoint has been generated */ + unsigned origin; /**< where the waypoint has been generated */ int do_jump_mission_index; /**< index where the do jump will go to */ unsigned do_jump_repeat_count; /**< how many times do jump needs to be done */ unsigned do_jump_current_count; /**< count how many times the jump has been done */ int actuator_num; /**< actuator number to be set 0..5 ( corresponds to AUX outputs 1..6 */ int actuator_value; /**< new value for selected actuator in ms 900...2000 */ }; - +#pragma pack(pop) #include /** diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 6ecf73a606..b154d21769 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -61,6 +61,7 @@ #include "navigator_mode.h" #include "mission.h" #include "loiter.h" +#include "takeoff.h" #include "rtl.h" #include "datalinkloss.h" #include "enginefailure.h" @@ -71,7 +72,7 @@ /** * Number of navigation modes that need on_active/on_inactive calls */ -#define NAVIGATOR_MODE_ARRAY_SIZE 7 +#define NAVIGATOR_MODE_ARRAY_SIZE 8 class Navigator : public control::SuperBlock { @@ -183,6 +184,7 @@ private: int _onboard_mission_sub; /**< onboard mission subscription */ int _offboard_mission_sub; /**< offboard mission subscription */ int _param_update_sub; /**< param update subscription */ + int _vehicle_command_sub; /**< vehicle commands (onboard and offboard) */ orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ orb_advert_t _mission_result_pub; @@ -215,9 +217,15 @@ private: bool _inside_fence; /**< vehicle is inside fence */ + bool _can_loiter_at_sp; /**< flags if current position SP can be used to loiter */ + bool _pos_sp_triplet_updated; /**< flags if position SP triplet needs to be published */ + bool _pos_sp_triplet_published_invalid_once; /**< flags if position SP triplet has been published once to UORB */ + bool _mission_result_updated; /**< flags if mission result has seen an update */ + NavigatorMode *_navigation_mode; /**< abstract pointer to current navigation mode class */ Mission _mission; /**< class that handles the missions */ Loiter _loiter; /**< class that handles loiter */ + Takeoff _takeoff; /**< class for handling takeoff commands */ RTL _rtl; /**< class that handles RTL */ RCLoss _rcLoss; /**< class that handles RTL according to OBC rules (rc loss mode) */ @@ -228,11 +236,6 @@ private: NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */ - bool _can_loiter_at_sp; /**< flags if current position SP can be used to loiter */ - bool _pos_sp_triplet_updated; /**< flags if position SP triplet needs to be published */ - bool _pos_sp_triplet_published_invalid_once; /**< flags if position SP triplet has been published once to UORB */ - bool _mission_result_updated; /**< flags if mission result has seen an update */ - control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */ control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */ control::BlockParamInt _param_datalinkloss_obc; /**< if true: obc mode on data link loss enabled */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e44b8d0be5..cd6021a971 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -71,6 +71,7 @@ #include #include #include +#include #include #include @@ -111,6 +112,7 @@ Navigator::Navigator() : _onboard_mission_sub(-1), _offboard_mission_sub(-1), _param_update_sub(-1), + _vehicle_command_sub(-1), _pos_sp_triplet_pub(nullptr), _mission_result_pub(nullptr), _geofence_result_pub(nullptr), @@ -132,18 +134,19 @@ Navigator::Navigator() : _geofence{}, _geofence_violation_warning_sent(false), _inside_fence(true), + _can_loiter_at_sp(false), + _pos_sp_triplet_updated(false), + _pos_sp_triplet_published_invalid_once(false), + _mission_result_updated(false), _navigation_mode(nullptr), _mission(this, "MIS"), _loiter(this, "LOI"), + _takeoff(this, "TKF"), _rtl(this, "RTL"), _rcLoss(this, "RCL"), _dataLinkLoss(this, "DLL"), _engineFailure(this, "EF"), _gpsFailure(this, "GPSF"), - _can_loiter_at_sp(false), - _pos_sp_triplet_updated(false), - _pos_sp_triplet_published_invalid_once(false), - _mission_result_updated(false), _param_loiter_radius(this, "LOITER_RAD"), _param_acceptance_radius(this, "ACC_RAD"), _param_datalinkloss_obc(this, "DLL_OBC"), @@ -157,6 +160,7 @@ Navigator::Navigator() : _navigation_mode_array[4] = &_engineFailure; _navigation_mode_array[5] = &_gpsFailure; _navigation_mode_array[6] = &_rcLoss; + _navigation_mode_array[7] = &_takeoff; updateParams(); } @@ -259,6 +263,8 @@ Navigator::task_main() _mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); _geofence.setMavlinkFd(_mavlink_fd); + bool have_geofence_position_data = false; + /* Try to load the geofence: * if /fs/microsd/etc/geofence.txt load from this file * else clear geofence data in datamanager */ @@ -286,6 +292,7 @@ Navigator::task_main() _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); + _vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command)); /* copy all topics first time */ vehicle_status_update(); @@ -297,42 +304,28 @@ Navigator::task_main() navigation_capabilities_update(); params_update(); - /* rate limit position and sensor updates to 50 Hz */ - orb_set_interval(_global_pos_sub, 20); - orb_set_interval(_sensor_combined_sub, 20); - hrt_abstime mavlink_open_time = 0; const hrt_abstime mavlink_open_interval = 500000; /* wakeup source(s) */ - px4_pollfd_struct_t fds[8]; + px4_pollfd_struct_t fds[1] = {}; /* Setup of loop */ fds[0].fd = _global_pos_sub; fds[0].events = POLLIN; - fds[1].fd = _home_pos_sub; - fds[1].events = POLLIN; - fds[2].fd = _capabilities_sub; - fds[2].events = POLLIN; - fds[3].fd = _vstatus_sub; - fds[3].events = POLLIN; - fds[4].fd = _control_mode_sub; - fds[4].events = POLLIN; - fds[5].fd = _param_update_sub; - fds[5].events = POLLIN; - fds[6].fd = _sensor_combined_sub; - fds[6].events = POLLIN; - fds[7].fd = _gps_pos_sub; - fds[7].events = POLLIN; + + bool global_pos_available_once = false; while (!_task_should_exit) { /* wait for up to 200ms for data */ - int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200); + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000); if (pret == 0) { /* timed out - periodic check for _task_should_exit, etc. */ - PX4_WARN("timed out"); + if (global_pos_available_once) { + PX4_WARN("navigator timed out"); + } continue; } else if (pret < 0) { @@ -341,6 +334,8 @@ Navigator::task_main() continue; } + global_pos_available_once = true; + perf_begin(_loop_perf); if (_mavlink_fd < 0 && hrt_absolute_time() > mavlink_open_time) { @@ -349,10 +344,11 @@ Navigator::task_main() _mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); } - static bool have_geofence_position_data = false; + bool updated; /* gps updated */ - if (fds[7].revents & POLLIN) { + orb_check(_gps_pos_sub, &updated); + if (updated) { gps_position_update(); if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) { have_geofence_position_data = true; @@ -360,36 +356,52 @@ Navigator::task_main() } /* sensors combined updated */ - if (fds[6].revents & POLLIN) { + orb_check(_sensor_combined_sub, &updated); + if (updated) { sensor_combined_update(); } /* parameters updated */ - if (fds[5].revents & POLLIN) { + orb_check(_param_update_sub, &updated); + if (updated) { params_update(); updateParams(); } /* vehicle control mode updated */ - if (fds[4].revents & POLLIN) { + orb_check(_control_mode_sub, &updated); + if (updated) { vehicle_control_mode_update(); } /* vehicle status updated */ - if (fds[3].revents & POLLIN) { + orb_check(_vstatus_sub, &updated); + if (updated) { vehicle_status_update(); } /* navigation capabilities updated */ - if (fds[2].revents & POLLIN) { + orb_check(_capabilities_sub, &updated); + if (updated) { navigation_capabilities_update(); } /* home position updated */ - if (fds[1].revents & POLLIN) { + orb_check(_home_pos_sub, &updated); + if (updated) { home_position_update(); } + orb_check(_vehicle_command_sub, &updated); + if (updated) { + vehicle_command_s cmd; + orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd); + + if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) { + warnx("navigator: got takeoff coordinates"); + } + } + /* global position updated */ if (fds[0].revents & POLLIN) { global_position_update(); @@ -440,8 +452,15 @@ Navigator::task_main() _can_loiter_at_sp = false; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: - _pos_sp_triplet_published_invalid_once = false; - _navigation_mode = &_mission; + if (_nav_caps.abort_landing) { + // pos controller aborted landing, requests loiter + // above landing waypoint + _navigation_mode = &_loiter; + _pos_sp_triplet_published_invalid_once = false; + } else { + _pos_sp_triplet_published_invalid_once = false; + _navigation_mode = &_mission; + } break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: _pos_sp_triplet_published_invalid_once = false; @@ -459,6 +478,10 @@ Navigator::task_main() _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_rtl; break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: + _pos_sp_triplet_published_invalid_once = false; + _navigation_mode = &_takeoff; + break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: /* Use complex data link loss mode only when enabled via param * otherwise use rtl */ @@ -671,7 +694,7 @@ void Navigator::publish_mission_result() { _mission_result.instance_count = _mission_instance_count; - + /* lazily publish the mission result only once available */ if (_mission_result_pub != nullptr) { /* publish mission result */ diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp new file mode 100644 index 0000000000..f669429449 --- /dev/null +++ b/src/modules/navigator/takeoff.cpp @@ -0,0 +1,94 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file Takeoff.cpp + * + * Helper class to Takeoff + * + * @author Lorenz Meier +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include "takeoff.h" +#include "navigator.h" + +Takeoff::Takeoff(Navigator *navigator, const char *name) : + MissionBlock(navigator, name), + _param_min_alt(this, "MIS_TAKEOFF_ALT", false) +{ + /* load initial params */ + updateParams(); +} + +Takeoff::~Takeoff() +{ +} + +void +Takeoff::on_inactive() +{ +} + +void +Takeoff::on_activation() +{ + /* set current mission item to Takeoff */ + set_takeoff_item(&_mission_item, _param_min_alt.get()); + + /* convert mission item to current setpoint */ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + pos_sp_triplet->previous.valid = false; + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; + pos_sp_triplet->next.valid = false; + + _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER); + + _navigator->set_position_setpoint_triplet_updated(); +} + +void +Takeoff::on_active() +{ +} diff --git a/src/modules/navigator/takeoff.h b/src/modules/navigator/takeoff.h new file mode 100644 index 0000000000..2ac50b8748 --- /dev/null +++ b/src/modules/navigator/takeoff.h @@ -0,0 +1,67 @@ +/*************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file Takeoff.h + * + * Helper class to take off + * + * @author Lorenz Meier + */ + +#ifndef NAVIGATOR_TAKEOFF_H +#define NAVIGATOR_TAKEOFF_H + +#include +#include + +#include "navigator_mode.h" +#include "mission_block.h" + +class Takeoff : public MissionBlock +{ +public: + Takeoff(Navigator *navigator, const char *name); + + ~Takeoff(); + + virtual void on_inactive(); + + virtual void on_activation(); + + virtual void on_active(); + +private: + control::BlockParamFloat _param_min_alt; +}; + +#endif diff --git a/src/modules/position_estimator_inav/CMakeLists.txt b/src/modules/position_estimator_inav/CMakeLists.txt index 2d5315b92b..2f14727509 100644 --- a/src/modules/position_estimator_inav/CMakeLists.txt +++ b/src/modules/position_estimator_inav/CMakeLists.txt @@ -30,18 +30,21 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ + +set(MODULE_CFLAGS ) if(${OS} STREQUAL "nuttx") - list(APPEND MODULE_CFLAGS -Wframe-larger-than=3890) + list(APPEND MODULE_CFLAGS -Wframe-larger-than=4000) endif() + px4_add_module( MODULE modules__position_estimator_inav MAIN position_estimator_inav STACK 1200 COMPILE_FLAGS ${MODULE_CFLAGS} SRCS - position_estimator_inav_main.c - position_estimator_inav_params.c - inertial_filter.c + position_estimator_inav_main.cpp + position_estimator_inav_params.cpp + inertial_filter.cpp DEPENDS platforms__common ) diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.cpp similarity index 100% rename from src/modules/position_estimator_inav/inertial_filter.c rename to src/modules/position_estimator_inav/inertial_filter.cpp diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp similarity index 95% rename from src/modules/position_estimator_inav/position_estimator_inav_main.c rename to src/modules/position_estimator_inav/position_estimator_inav_main.cpp index aa7fe0d972..1da3ff305c 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -74,6 +74,7 @@ #include #include +#include #include "position_estimator_inav_params.h" #include "inertial_filter.h" @@ -95,7 +96,7 @@ static const hrt_abstime lidar_valid_timeout = 1000000; // estimate lidar distan static const unsigned updates_counter_len = 1000000; static const float max_flow = 1.0f; // max flow value that can be used, rad/s -__EXPORT int position_estimator_inav_main(int argc, char *argv[]); +extern "C" __EXPORT int position_estimator_inav_main(int argc, char *argv[]); int position_estimator_inav_thread_main(int argc, char *argv[]); @@ -153,7 +154,7 @@ int position_estimator_inav_main(int argc, char *argv[]) thread_should_exit = false; position_estimator_inav_task = px4_task_spawn_cmd("position_estimator_inav", - SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5000, + SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5300, position_estimator_inav_thread_main, (argv && argc > 2) ? (char * const *) &argv[2] : (char * const *) NULL); return 0; @@ -216,7 +217,7 @@ static void write_debug_log(const char *msg, float dt, float x_est[2], float y_e fclose(f); } #else -#define write_debug_log(...) +#define write_debug_log(...) #endif /**************************************************************************** @@ -380,6 +381,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_advert_t vehicle_global_position_pub = NULL; struct position_estimator_inav_params params; + memset(¶ms, 0, sizeof(params)); struct position_estimator_inav_param_handles pos_inav_param_handles; /* initialize parameter handles */ inav_parameters_init(&pos_inav_param_handles); @@ -390,21 +392,24 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* first parameters update */ inav_parameters_update(&pos_inav_param_handles, ¶ms); - px4_pollfd_struct_t fds_init[1] = { - { .fd = sensor_combined_sub, .events = POLLIN }, - }; + px4_pollfd_struct_t fds_init[1] = {}; + fds_init[0].fd = sensor_combined_sub; + fds_init[0].events = POLLIN; /* wait for initial baro value */ bool wait_baro = true; + TerrainEstimator *terrain_estimator = new TerrainEstimator(); + thread_running = true; while (wait_baro && !thread_should_exit) { - int ret = px4_poll(fds_init, 1, 1000); + int ret = px4_poll(&fds_init[0], 1, 1000); if (ret < 0) { /* poll error */ mavlink_log_info(mavlink_fd, "[inav] poll error on init"); + PX4_WARN("INAV poll error"); } else if (ret > 0) { if (fds_init[0].revents & POLLIN) { @@ -423,20 +428,20 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else { wait_baro = false; baro_offset /= (float) baro_init_cnt; - warnx("baro offset: %d m", (int)baro_offset); - mavlink_log_info(mavlink_fd, "[inav] baro offset: %d m", (int)baro_offset); local_pos.z_valid = true; local_pos.v_z_valid = true; } } } + } else { + PX4_WARN("INAV poll timeout"); } } /* main loop */ - px4_pollfd_struct_t fds[1] = { - { .fd = vehicle_attitude_sub, .events = POLLIN }, - }; + px4_pollfd_struct_t fds[1]; + fds[0].fd = vehicle_attitude_sub; + fds[0].events = POLLIN; while (!thread_should_exit) { int ret = px4_poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate @@ -523,9 +528,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_check(optical_flow_sub, &updated); orb_check(distance_sensor_sub, &updated2); + /* update lidar separately, needed by terrain estimator */ + if (updated2) { + orb_copy(ORB_ID(distance_sensor), distance_sensor_sub, &lidar); + } + if (updated && updated2) { orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); - orb_copy(ORB_ID(distance_sensor), distance_sensor_sub, &lidar); /* calculate time from previous update */ // float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f; @@ -586,15 +595,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* set this flag if flow should be accurate according to current velocity and attitude rate estimate */ flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow && fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow; - //this flag is not working --> - flow_accurate = true; //already checked if flow_q > 0.3 /*calculate offset of flow-gyro using already calibrated gyro from autopilot*/ flow_gyrospeed[0] = flow.gyro_x_rate_integral/(float)flow.integration_timespan*1000000.0f; flow_gyrospeed[1] = flow.gyro_y_rate_integral/(float)flow.integration_timespan*1000000.0f; flow_gyrospeed[2] = flow.gyro_z_rate_integral/(float)flow.integration_timespan*1000000.0f; - + //moving average if (n_flow >= 100) { gyro_offset_filtered[0] = flow_gyrospeed_filtered[0] - att_gyrospeed_filtered[0]; @@ -616,7 +623,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) att_gyrospeed_filtered[2] = (att.yawspeed + n_flow * att_gyrospeed_filtered[2]) / (n_flow + 1); n_flow++; } - + /*yaw compensation (flow sensor is not in center of rotation) -> params in QGC*/ yaw_comp[0] = params.flow_module_offset_x * (flow_gyrospeed[2] - gyro_offset_filtered[2]); @@ -953,9 +960,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) t_prev = t; /* increase EPH/EPV on each step */ + if (eph < 0.000001f) { //get case where eph is 0 -> would stay 0 + eph = 0.001; + } if (eph < max_eph_epv) { eph *= 1.0f + dt; } + if (epv < 0.000001f) { //get case where epv is 0 -> would stay 0 + epv = 0.001; + } if (epv < max_eph_epv) { epv += 0.005f * dt; // add 1m to EPV each 200s (baro drift) } @@ -968,9 +981,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) bool use_vision_z = vision_valid && params.w_z_vision_p > MIN_VALID_W; /* use MOCAP if it's valid and has a valid weight parameter */ bool use_mocap = mocap_valid && params.w_mocap_p > MIN_VALID_W; + if(params.disable_mocap) { //disable mocap if fake gps is used + use_mocap = false; + } /* use flow if it's valid and (accurate or no GPS available) */ bool use_flow = flow_valid && (flow_accurate || !use_gps_xy); + bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy || use_mocap; bool dist_bottom_valid = (t < lidar_valid_time + lidar_valid_timeout); @@ -1033,7 +1050,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) c += R_gps[j][i] * accel_bias_corr[j]; } - if (isfinite(c)) { + if (PX4_ISFINITE(c)) { acc_bias[i] += c * params.w_acc_bias * dt; } } @@ -1073,7 +1090,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) c += PX4_R(att.R, j, i) * accel_bias_corr[j]; } - if (isfinite(c)) { + if (PX4_ISFINITE(c)) { acc_bias[i] += c * params.w_acc_bias * dt; } } @@ -1098,7 +1115,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) c += PX4_R(att.R, j, i) * accel_bias_corr[j]; } - if (isfinite(c)) { + if (PX4_ISFINITE(c)) { acc_bias[i] += c * params.w_acc_bias * dt; } } @@ -1106,7 +1123,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* inertial filter prediction for altitude */ inertial_filter_predict(dt, z_est, acc[2]); - if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) { + if (!(PX4_ISFINITE(z_est[0]) && PX4_ISFINITE(z_est[1]))) { write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); @@ -1133,10 +1150,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(corr_mocap[2][0], dt, z_est, 0, w_mocap_p); } - if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) { + if (!(PX4_ISFINITE(z_est[0]) && PX4_ISFINITE(z_est[1]))) { write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, - corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); + corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); memcpy(z_est, z_est_prev, sizeof(z_est)); memset(corr_gps, 0, sizeof(corr_gps)); memset(corr_vision, 0, sizeof(corr_vision)); @@ -1152,7 +1169,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_predict(dt, x_est, acc[0]); inertial_filter_predict(dt, y_est, acc[1]); - if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) { + if (!(PX4_ISFINITE(x_est[0]) && PX4_ISFINITE(x_est[1]) && PX4_ISFINITE(y_est[0]) && PX4_ISFINITE(y_est[1]))) { write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); @@ -1166,7 +1183,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow); inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow); - //mavlink_log_info(mavlink_fd, "w_flow = %2.4f\t w_xy_flow = %2.4f\n", (double)w_flow, (double)params.w_xy_flow); } if (use_gps_xy) { @@ -1200,7 +1216,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(corr_mocap[1][0], dt, y_est, 0, w_mocap_p); } - if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) { + if (!(PX4_ISFINITE(x_est[0]) && PX4_ISFINITE(x_est[1]) && PX4_ISFINITE(y_est[0]) && PX4_ISFINITE(y_est[1]))) { write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); @@ -1221,6 +1237,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v); } + /* run terrain estimator */ + terrain_estimator->predict(dt, &att, &sensor, &lidar); + terrain_estimator->measurement_update(hrt_absolute_time(), &gps, &lidar, &att); + if (inav_verbose_mode) { /* print updates rate */ if (t > updates_counter_start + updates_counter_len) { @@ -1264,6 +1284,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) buf_ptr = 0; } + /* publish local position */ local_pos.xy_valid = can_estimate_xy; local_pos.v_xy_valid = can_estimate_xy; @@ -1310,6 +1331,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) global_pos.eph = eph; global_pos.epv = epv; + if (terrain_estimator->is_valid()) { + global_pos.terrain_alt = global_pos.alt - terrain_estimator->get_distance_to_ground(); + global_pos.terrain_alt_valid = true; + } else { + global_pos.terrain_alt_valid = false; + } + + global_pos.pressure_alt = sensor.baro_alt_meter[0]; + if (vehicle_global_position_pub == NULL) { vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.cpp similarity index 96% rename from src/modules/position_estimator_inav/position_estimator_inav_params.c rename to src/modules/position_estimator_inav/position_estimator_inav_params.cpp index be8a136c4d..9178a9d659 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.cpp @@ -158,10 +158,10 @@ PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_V, 0.0f); * Weight (cutoff frequency) for optical flow (velocity) measurements. * * @min 0.0 - * @max 30.0 + * @max 10.0 * @group Position Estimator INAV */ -PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 10.0f); +PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 9.0f); /** * XY axis weight for resetting velocity @@ -313,6 +313,17 @@ PARAM_DEFINE_FLOAT(INAV_FLOW_DIST_X, 0.0f); */ PARAM_DEFINE_FLOAT(INAV_FLOW_DIST_Y, 0.0f); +/** + * Disable mocap (set 0 if using fake gps) + * + * Disable mocap + * + * @min 0 + * @max 1 + * @group Position Estimator INAV + */ +PARAM_DEFINE_FLOAT(INAV_DISAB_MOCAP, 0); + /** * Disable vision input * @@ -364,6 +375,7 @@ int inav_parameters_init(struct position_estimator_inav_param_handles *h) h->delay_gps = param_find("INAV_DELAY_GPS"); h->flow_module_offset_x = param_find("INAV_FLOW_DIST_X"); h->flow_module_offset_y = param_find("INAV_FLOW_DIST_Y"); + h->disable_mocap = param_find("INAV_DISAB_MOCAP"); return 0; } @@ -372,6 +384,7 @@ int inav_parameters_update(const struct position_estimator_inav_param_handles *h { param_get(h->w_z_baro, &(p->w_z_baro)); param_get(h->w_z_gps_p, &(p->w_z_gps_p)); + param_get(h->w_z_gps_v, &(p->w_z_gps_v)); param_get(h->w_z_vision_p, &(p->w_z_vision_p)); param_get(h->w_z_lidar, &(p->w_z_lidar)); param_get(h->w_xy_gps_p, &(p->w_xy_gps_p)); @@ -394,6 +407,7 @@ int inav_parameters_update(const struct position_estimator_inav_param_handles *h param_get(h->delay_gps, &(p->delay_gps)); param_get(h->flow_module_offset_x, &(p->flow_module_offset_x)); param_get(h->flow_module_offset_y, &(p->flow_module_offset_y)); + param_get(h->disable_mocap, &(p->disable_mocap)); return 0; } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index 43601568f8..07b78104bf 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -67,6 +67,7 @@ struct position_estimator_inav_params { float delay_gps; float flow_module_offset_x; float flow_module_offset_y; + int32_t disable_mocap; }; struct position_estimator_inav_param_handles { @@ -95,6 +96,7 @@ struct position_estimator_inav_param_handles { param_t delay_gps; param_t flow_module_offset_x; param_t flow_module_offset_y; + param_t disable_mocap; }; #define CBRK_NO_VISION_KEY 328754 diff --git a/src/modules/px4iofirmware/CMakeLists.txt b/src/modules/px4iofirmware/CMakeLists.txt index 3319bec6a2..854bbdcc80 100644 --- a/src/modules/px4iofirmware/CMakeLists.txt +++ b/src/modules/px4iofirmware/CMakeLists.txt @@ -64,6 +64,7 @@ px4_join(OUT CMAKE_CXX_FLAGS LIST "${cxx_flags}" GLUE " ") include_directories( ${include_dirs} ${CMAKE_BINARY_DIR}/src/modules/systemlib/mixer + . ) link_directories(${link_dirs}) add_definitions(${definitions}) @@ -71,11 +72,9 @@ add_definitions(${definitions}) set(srcs adc.c controls.c - dsm.c px4io.c registers.c safety.c - sbus.c ../systemlib/up_cxxinitialize.c ../systemlib/perf_counter.c mixer.cpp @@ -86,6 +85,8 @@ set(srcs ../systemlib/pwm_limit/pwm_limit.c ../../lib/rc/st24.c ../../lib/rc/sumd.c + ../../lib/rc/sbus.c + ../../lib/rc/dsm.c ../../drivers/stm32/drv_hrt.c ../../drivers/stm32/drv_pwm_servo.c ../../drivers/boards/${config_io_board}/px4io_init.c diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 65d49a388a..f7321a966d 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -46,6 +46,8 @@ #include #include #include +#include +#include #include "px4io.h" @@ -60,7 +62,8 @@ static perf_counter_t c_gather_dsm; static perf_counter_t c_gather_sbus; static perf_counter_t c_gather_ppm; -static int _dsm_fd; +static int _dsm_fd = -1; +int _sbus_fd = -1; static uint16_t rc_value_override = 0; @@ -74,7 +77,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool uint16_t temp_count = r_raw_rc_count; uint8_t n_bytes = 0; uint8_t *bytes; - *dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes); + *dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes, PX4IO_RC_INPUT_CHANNELS); if (*dsm_updated) { r_raw_rc_count = temp_count & 0x7fff; @@ -158,7 +161,7 @@ controls_init(void) _dsm_fd = dsm_init("/dev/ttyS0"); /* S.bus input (USART3) */ - sbus_init("/dev/ttyS2"); + _sbus_fd = sbus_init("/dev/ttyS2", false); /* default to a 1:1 input map, all enabled */ for (unsigned i = 0; i < PX4IO_RC_INPUT_CHANNELS; i++) { @@ -240,7 +243,7 @@ controls_tick() perf_begin(c_gather_sbus); bool sbus_failsafe, sbus_frame_drop; - bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, + bool sbus_updated = sbus_input(_sbus_fd, r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS); if (sbus_updated) { @@ -484,7 +487,7 @@ controls_tick() * Override is enabled if either the hardcoded channel / value combination * is selected, or the AP has requested it. */ - if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && + if ((!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK)) && (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && !(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 0a0962367d..4ff95bd5af 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -48,6 +48,7 @@ #include #include +#include #include #include @@ -71,6 +72,8 @@ static bool should_arm_nothrottle = false; static bool should_always_enable_pwm = false; static volatile bool in_mixer = false; +extern int _sbus_fd; + /* selected control values and count for mixing */ enum mixer_source { MIX_NONE, @@ -292,10 +295,10 @@ mixer_tick(void) /* set S.BUS1 or S.BUS2 outputs */ if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) { - sbus2_output(r_page_servos, PX4IO_SERVO_COUNT); + sbus2_output(_sbus_fd, 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); + sbus1_output(_sbus_fd, r_page_servos, PX4IO_SERVO_COUNT); } } else if (mixer_servos_armed && should_always_enable_pwm) { @@ -306,11 +309,11 @@ mixer_tick(void) /* set S.BUS1 or S.BUS2 outputs */ if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) { - sbus1_output(r_page_servo_disarmed, PX4IO_SERVO_COUNT); + sbus1_output(_sbus_fd, r_page_servo_disarmed, PX4IO_SERVO_COUNT); } if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) { - sbus2_output(r_page_servo_disarmed, PX4IO_SERVO_COUNT); + sbus2_output(_sbus_fd, r_page_servo_disarmed, PX4IO_SERVO_COUNT); } } } diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 51ed9c6e40..2197b7b776 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -224,14 +224,6 @@ extern uint16_t adc_measure(unsigned channel); */ extern void controls_init(void); extern void controls_tick(void); -extern int dsm_init(const char *device); -extern bool dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes); -extern void dsm_bind(uint16_t cmd, int pulses); -extern int sbus_init(const char *device); -extern bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, - uint16_t max_channels); -extern void sbus1_output(uint16_t *values, uint16_t num_values); -extern void sbus2_output(uint16_t *values, uint16_t num_values); /** global debug level for isr_debug() */ extern volatile uint8_t debug_level; diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 18f5db5645..951de1f044 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -47,6 +47,7 @@ #include #include #include +#include #include "px4io.h" #include "protocol.h" @@ -152,7 +153,7 @@ volatile uint16_t r_page_setup[] = { #else [PX4IO_P_SETUP_FEATURES] = 0, #endif - [PX4IO_P_SETUP_ARMING] = 0, + [PX4IO_P_SETUP_ARMING] = (PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE), [PX4IO_P_SETUP_PWM_RATES] = 0, [PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50, [PX4IO_P_SETUP_PWM_ALTRATE] = 200, diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index f15ec8969a..57aec762e6 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -311,7 +311,7 @@ int sdlog2_main(int argc, char *argv[]) deamon_task = px4_task_spawn_cmd("sdlog2", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT - 30, - 3000, + 3300, sdlog2_thread_main, (char * const *)argv); @@ -555,7 +555,7 @@ int open_perf_file(const char* str) static void *logwriter_thread(void *arg) { /* set name */ - prctl(PR_SET_NAME, "sdlog2_writer", 0); + px4_prctl(PR_SET_NAME, "sdlog2_writer", 0); int log_fd = open_log_file(); @@ -686,9 +686,12 @@ void sdlog2_start_log() pthread_attr_init(&logwriter_attr); struct sched_param param; + (void)pthread_attr_getschedparam(&logwriter_attr, ¶m); /* low priority, as this is expensive disk I/O */ param.sched_priority = SCHED_PRIORITY_DEFAULT - 40; - (void)pthread_attr_setschedparam(&logwriter_attr, ¶m); + if (pthread_attr_setschedparam(&logwriter_attr, ¶m)) { + warnx("sdlog2: failed setting sched params"); + } pthread_attr_setstacksize(&logwriter_attr, 2048); @@ -1674,11 +1677,12 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- RC CHANNELS --- */ if (copy_if_updated(ORB_ID(rc_channels), &subs.rc_sub, &buf.rc)) { log_msg.msg_type = LOG_RC_MSG; - /* Copy only the first 8 channels of 14 */ + /* Copy only the first 12 channels of 18 */ memcpy(log_msg.body.log_RC.channel, buf.rc.channels, sizeof(log_msg.body.log_RC.channel)); log_msg.body.log_RC.rssi = buf.rc.rssi; log_msg.body.log_RC.channel_count = buf.rc.channel_count; log_msg.body.log_RC.signal_lost = buf.rc.signal_lost; + log_msg.body.log_RC.frame_drop = buf.rc.frame_drop_count; LOGBUFFER_WRITE_AND_COUNT(RC); } @@ -1790,7 +1794,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_EST1_MSG; unsigned maxcopy1 = ((sizeof(buf.estimator_status.states) - maxcopy0) < sizeof(log_msg.body.log_EST1.s)) ? (sizeof(buf.estimator_status.states) - maxcopy0) : sizeof(log_msg.body.log_EST1.s); memset(&(log_msg.body.log_EST1.s), 0, sizeof(log_msg.body.log_EST1.s)); - memcpy(&(log_msg.body.log_EST1.s), buf.estimator_status.states + maxcopy0, maxcopy1); + memcpy(&(log_msg.body.log_EST1.s), ((char*)buf.estimator_status.states) + maxcopy0, maxcopy1); LOGBUFFER_WRITE_AND_COUNT(EST1); log_msg.msg_type = LOG_EST2_MSG; @@ -1802,7 +1806,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_EST3_MSG; unsigned maxcopy3 = ((sizeof(buf.estimator_status.covariances) - maxcopy2) < sizeof(log_msg.body.log_EST3.cov)) ? (sizeof(buf.estimator_status.covariances) - maxcopy2) : sizeof(log_msg.body.log_EST3.cov); memset(&(log_msg.body.log_EST3.cov), 0, sizeof(log_msg.body.log_EST3.cov)); - memcpy(&(log_msg.body.log_EST3.cov), buf.estimator_status.covariances + maxcopy2, maxcopy3); + memcpy(&(log_msg.body.log_EST3.cov), ((char*)buf.estimator_status.covariances) + maxcopy2, maxcopy3); LOGBUFFER_WRITE_AND_COUNT(EST3); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 6ccbec2fa9..86c2c58868 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -188,10 +188,11 @@ struct log_STAT_s { /* --- RC - RC INPUT CHANNELS --- */ #define LOG_RC_MSG 11 struct log_RC_s { - float channel[8]; + float channel[12]; uint8_t rssi; uint8_t channel_count; uint8_t signal_lost; + uint32_t frame_drop; }; /* --- OUT0 - ACTUATOR_0 OUTPUT --- */ @@ -533,7 +534,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(STAT, "BBBfBBf", "MainState,ArmS,Failsafe,BatRem,BatWarn,Landed,Load"), LOG_FORMAT(VTOL, "f", "Arsp"), LOG_FORMAT(CTS, "fffffff", "Vx_b,Vy_b,Vz_b,Vinf,P,Q,R"), - LOG_FORMAT(RC, "ffffffffBBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,RSSI,Count,SignalLost"), + LOG_FORMAT(RC, "ffffffffffffBBBL", "C0,C1,C2,C3,C4,C5,C6,C7,C8,C9,C10,C11,RSSI,CNT,Lost,Drop"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 3d4dfd62c1..9543306cda 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -41,9 +41,6 @@ * @author Thomas Gubler */ -#include -#include - /** * ID of the board this parameter set was calibrated on. * @@ -1805,7 +1802,6 @@ PARAM_DEFINE_FLOAT(RC18_REV, 1.0f); */ PARAM_DEFINE_FLOAT(RC18_DZ, 0.0f); -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 /** * Enable relay control of relay 1 mapped to the Spektrum receiver power supply * @@ -1814,7 +1810,6 @@ PARAM_DEFINE_FLOAT(RC18_DZ, 0.0f); * @group Radio Calibration */ PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ -#endif /** * DSM binding trigger. diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index ab246e14a1..b20b33011e 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -65,7 +65,6 @@ #include #include -//#include #include #include @@ -97,6 +96,10 @@ #include #include +#include "DevMgr.hpp" + +using namespace DriverFramework; + /** * Analog layout: * FMU: @@ -119,7 +122,7 @@ #define BATT_V_LOWPASS 0.001f -#define BATT_V_IGNORE_THRESHOLD 2.5f +#define BATT_V_IGNORE_THRESHOLD 2.1f /** * HACK - true temperature is much less than indicated temperature in baro, @@ -195,7 +198,7 @@ private: void rc_poll(); /* XXX should not be here - should be own driver */ - int _fd_adc; /**< ADC driver handle */ + DevHandle _h_adc; /**< ADC driver handle */ hrt_abstime _last_adc; /**< last time we took input from the ADC */ bool _task_should_exit; /**< if true, sensor task should exit */ @@ -485,7 +488,7 @@ Sensors *g_sensors = nullptr; } Sensors::Sensors() : - _fd_adc(-1), + _h_adc(), _last_adc(0), _task_should_exit(true), @@ -538,6 +541,7 @@ Sensors::Sensors() : memset(&_rc, 0, sizeof(_rc)); memset(&_diff_pres, 0, sizeof(_diff_pres)); + memset(&_parameters, 0, sizeof(_parameters)); memset(&_rc_parameter_map, 0, sizeof(_rc_parameter_map)); /* basic r/c parameters */ @@ -846,7 +850,7 @@ Sensors::parameters_update() } else if (_parameters.battery_voltage_scaling < 0.0f) { /* apply scaling according to defaults if set to default */ -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +#if defined (CONFIG_ARCH_BOARD_PX4FMU_V2) || defined (CONFIG_ARCH_BOARD_PX4FMU_V4) _parameters.battery_voltage_scaling = 0.0082f; #elif CONFIG_ARCH_BOARD_AEROCORE _parameters.battery_voltage_scaling = 0.0063f; @@ -898,23 +902,20 @@ Sensors::parameters_update() /* update barometer qnh setting */ param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh)); - int barofd; - barofd = px4_open(BARO0_DEVICE_PATH, 0); + DevHandle h_baro; + DevMgr::getHandle(BARO0_DEVICE_PATH, h_baro); - if (barofd < 0) { - warnx("ERROR: no barometer found on %s", BARO0_DEVICE_PATH); + if (!h_baro.isValid()) { + warnx("ERROR: no barometer found on %s (%d)", BARO0_DEVICE_PATH, h_baro.getError()); return ERROR; } else { - int baroret = px4_ioctl(barofd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100)); + int baroret = h_baro.ioctl(BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100)); if (baroret) { warnx("qnh could not be set"); - px4_close(barofd); return ERROR; } - - px4_close(barofd); } return OK; @@ -923,23 +924,20 @@ Sensors::parameters_update() int Sensors::accel_init() { - int fd; + DevHandle h_accel; + DevMgr::getHandle(ACCEL0_DEVICE_PATH, h_accel); - fd = px4_open(ACCEL0_DEVICE_PATH, 0); - - if (fd < 0) { - warnx("FATAL: no accelerometer found: %s", ACCEL0_DEVICE_PATH); + if (!h_accel.isValid()) { + warnx("FATAL: no accelerometer found: %s (%d)", ACCEL0_DEVICE_PATH, h_accel.getError()); return ERROR; } else { /* set the accel internal sampling rate to default rate */ - px4_ioctl(fd, ACCELIOCSSAMPLERATE, ACCEL_SAMPLERATE_DEFAULT); + h_accel.ioctl(ACCELIOCSSAMPLERATE, ACCEL_SAMPLERATE_DEFAULT); /* set the driver to poll at default rate */ - px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); - - px4_close(fd); + h_accel.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); } return OK; @@ -948,23 +946,20 @@ Sensors::accel_init() int Sensors::gyro_init() { - int fd; + DevHandle h_gyro; + DevMgr::getHandle(GYRO0_DEVICE_PATH, h_gyro); - fd = px4_open(GYRO0_DEVICE_PATH, 0); - - if (fd < 0) { - warnx("FATAL: no gyro found: %s", GYRO0_DEVICE_PATH); + if (!h_gyro.isValid()) { + warnx("FATAL: no gyro found: %s (%d)", GYRO0_DEVICE_PATH, h_gyro.getError()); return ERROR; } /* set the gyro internal sampling rate to default rate */ - px4_ioctl(fd, GYROIOCSSAMPLERATE, GYRO_SAMPLERATE_DEFAULT); + h_gyro.ioctl(GYROIOCSSAMPLERATE, GYRO_SAMPLERATE_DEFAULT); /* set the driver to poll at default rate */ - px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); - - px4_close(fd); + h_gyro.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); return OK; } @@ -972,32 +967,32 @@ Sensors::gyro_init() int Sensors::mag_init() { - int fd; int ret; - fd = px4_open(MAG0_DEVICE_PATH, 0); + DevHandle h_mag; + DevMgr::getHandle(MAG0_DEVICE_PATH, h_mag); - if (fd < 0) { - warnx("FATAL: no magnetometer found: %s", MAG0_DEVICE_PATH); + if (!h_mag.isValid()) { + warnx("FATAL: no magnetometer found: %s (%d)", MAG0_DEVICE_PATH, h_mag.getError()); return ERROR; } /* try different mag sampling rates */ - ret = px4_ioctl(fd, MAGIOCSSAMPLERATE, 150); + ret = h_mag.ioctl(MAGIOCSSAMPLERATE, 150); if (ret == OK) { /* set the pollrate accordingly */ - px4_ioctl(fd, SENSORIOCSPOLLRATE, 150); + h_mag.ioctl(SENSORIOCSPOLLRATE, 150); } else { - ret = px4_ioctl(fd, MAGIOCSSAMPLERATE, 100); + ret = h_mag.ioctl(MAGIOCSSAMPLERATE, 100); /* if the slower sampling rate still fails, something is wrong */ if (ret == OK) { /* set the driver to poll also at the slower rate */ - px4_ioctl(fd, SENSORIOCSPOLLRATE, 100); + h_mag.ioctl(SENSORIOCSPOLLRATE, 100); } else { warnx("FATAL: mag sampling rate could not be set"); @@ -1005,27 +1000,22 @@ Sensors::mag_init() } } - px4_close(fd); - return OK; } int Sensors::baro_init() { - int fd; + DevHandle h_baro; + DevMgr::getHandle(BARO0_DEVICE_PATH, h_baro); - fd = px4_open(BARO0_DEVICE_PATH, 0); - - if (fd < 0) { - warnx("FATAL: No barometer found: %s", BARO0_DEVICE_PATH); + if (!h_baro.isValid()) { + warnx("FATAL: No barometer found: %s (%d)", BARO0_DEVICE_PATH, h_baro.getError()); return ERROR; } /* set the driver to poll at 150Hz */ - px4_ioctl(fd, SENSORIOCSPOLLRATE, 150); - - px4_close(fd); + h_baro.ioctl(SENSORIOCSPOLLRATE, 150); return OK; } @@ -1034,10 +1024,10 @@ int Sensors::adc_init() { - _fd_adc = px4_open(ADC0_DEVICE_PATH, O_RDONLY | O_NONBLOCK); + DevMgr::getHandle(ADC0_DEVICE_PATH, _h_adc); - if (_fd_adc < 0) { - warnx("FATAL: no ADC found: %s", ADC0_DEVICE_PATH); + if (!_h_adc.isValid()) { + warnx("FATAL: no ADC found: %s (%d)", ADC0_DEVICE_PATH, _h_adc.getError()); return ERROR; } @@ -1277,9 +1267,10 @@ Sensors::parameter_update_poll(bool forced) res = ERROR; (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); - int fd = px4_open(str, 0); + DevHandle h; + DevMgr::getHandle(str, h); - if (fd < 0) { + if (!h.isValid()) { continue; } @@ -1295,12 +1286,15 @@ Sensors::parameter_update_poll(bool forced) failed = failed || (OK != param_get(param_find(str), &device_id)); if (failed) { - px4_close(fd); + DevMgr::releaseHandle(h); continue; } + //int id = h.ioctl(DEVIOCGDEVICEID, 0); + //PX4_WARN("sensors: device ID: %s: %d, %u", str, id, (unsigned)id); + /* if the calibration is for this device, apply it */ - if (device_id == px4_ioctl(fd, DEVIOCGDEVICEID, 0)) { + if (device_id == h.ioctl(DEVIOCGDEVICEID, 0)) { struct gyro_scale gscale = {}; (void)sprintf(str, "CAL_GYRO%u_XOFF", i); failed = failed || (OK != param_get(param_find(str), &gscale.x_offset)); @@ -1320,7 +1314,7 @@ Sensors::parameter_update_poll(bool forced) } else { /* apply new scaling and offsets */ - res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale); + res = h.ioctl(GYROIOCSSCALE, (long unsigned int)&gscale); if (res) { warnx(CAL_ERROR_APPLY_CAL_MSG, "gyro", i); @@ -1337,8 +1331,6 @@ Sensors::parameter_update_poll(bool forced) if (config_ok) { gyro_count++; } - - px4_close(fd); } /* run through all accel sensors */ @@ -1347,9 +1339,10 @@ Sensors::parameter_update_poll(bool forced) res = ERROR; (void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s); - int fd = px4_open(str, 0); + DevHandle h; + DevMgr::getHandle(str, h); - if (fd < 0) { + if (!h.isValid()) { continue; } @@ -1365,12 +1358,15 @@ Sensors::parameter_update_poll(bool forced) failed = failed || (OK != param_get(param_find(str), &device_id)); if (failed) { - px4_close(fd); + DevMgr::releaseHandle(h); continue; } + // int id = h.ioctl(DEVIOCGDEVICEID, 0); + // PX4_WARN("sensors: device ID: %s: %d, %u", str, id, (unsigned)id); + /* if the calibration is for this device, apply it */ - if (device_id == px4_ioctl(fd, DEVIOCGDEVICEID, 0)) { + if (device_id == h.ioctl(DEVIOCGDEVICEID, 0)) { struct accel_scale gscale = {}; (void)sprintf(str, "CAL_ACC%u_XOFF", i); failed = failed || (OK != param_get(param_find(str), &gscale.x_offset)); @@ -1390,7 +1386,7 @@ Sensors::parameter_update_poll(bool forced) } else { /* apply new scaling and offsets */ - res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&gscale); + res = h.ioctl(ACCELIOCSSCALE, (long unsigned int)&gscale); if (res) { warnx(CAL_ERROR_APPLY_CAL_MSG, "accel", i); @@ -1407,8 +1403,6 @@ Sensors::parameter_update_poll(bool forced) if (config_ok) { accel_count++; } - - px4_close(fd); } /* run through all mag sensors */ @@ -1423,9 +1417,10 @@ Sensors::parameter_update_poll(bool forced) res = ERROR; (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s); - int fd = px4_open(str, 0); + DevHandle h; + DevMgr::getHandle(str, h); - if (fd < 0) { + if (!h.isValid()) { /* the driver is not running, abort */ continue; } @@ -1444,12 +1439,15 @@ Sensors::parameter_update_poll(bool forced) (void)param_find(str); if (failed) { - px4_close(fd); + DevMgr::releaseHandle(h); continue; } + // int id = h.ioctl(DEVIOCGDEVICEID, 0); + // PX4_WARN("sensors: device ID: %s: %d, %u", str, id, (unsigned)id); + /* if the calibration is for this device, apply it */ - if (device_id == px4_ioctl(fd, DEVIOCGDEVICEID, 0)) { + if (device_id == h.ioctl(DEVIOCGDEVICEID, 0)) { struct mag_scale gscale = {}; (void)sprintf(str, "CAL_MAG%u_XOFF", i); failed = failed || (OK != param_get(param_find(str), &gscale.x_offset)); @@ -1466,7 +1464,7 @@ Sensors::parameter_update_poll(bool forced) (void)sprintf(str, "CAL_MAG%u_ROT", i); - if (px4_ioctl(fd, MAGIOCGEXTERNAL, 0) <= 0) { + if (h.ioctl(MAGIOCGEXTERNAL, 0) <= 0) { /* mag is internal */ _mag_rotation[s] = _board_rotation; /* reset param to -1 to indicate internal mag */ @@ -1520,7 +1518,7 @@ Sensors::parameter_update_poll(bool forced) } else { /* apply new scaling and offsets */ - res = px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&gscale); + res = h.ioctl(MAGIOCSSCALE, (long unsigned int)&gscale); if (res) { warnx(CAL_ERROR_APPLY_CAL_MSG, "mag", i); @@ -1537,8 +1535,6 @@ Sensors::parameter_update_poll(bool forced) if (config_ok) { mag_count++; } - - px4_close(fd); } int fd = px4_open(AIRSPEED0_DEVICE_PATH, 0); @@ -1621,7 +1617,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) /* make space for a maximum of twelve channels (to ensure reading all channels at once) */ struct adc_msg_s buf_adc[12]; /* read all channels available */ - int ret = px4_read(_fd_adc, &buf_adc, sizeof(buf_adc)); + int ret = _h_adc.read(&buf_adc, sizeof(buf_adc)); if (ret >= (int)sizeof(buf_adc[0])) { @@ -1912,6 +1908,7 @@ Sensors::rc_poll() _rc.rssi = rc_input.rssi; _rc.signal_lost = signal_lost; _rc.timestamp = rc_input.timestamp_last_signal; + _rc.frame_drop_count = rc_input.rc_lost_frame_count; /* publish rc_channels topic even if signal is invalid, for debug */ if (_rc_pub != nullptr) { @@ -2060,10 +2057,7 @@ Sensors::task_main() warnx("sensor initialization failed"); _sensors_task = -1; - if (_fd_adc >= 0) { - px4_close(_fd_adc); - _fd_adc = -1; - } + DevMgr::releaseHandle(_h_adc); return; } @@ -2114,9 +2108,6 @@ Sensors::task_main() _rc_parameter_map_sub = orb_subscribe(ORB_ID(rc_parameter_map)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - /* rate limit vehicle status updates to 5Hz */ - orb_set_interval(_vcontrol_mode_sub, 200); - /* * do advertisements */ diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index 1656a3451f..bcd188b0f0 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -136,9 +136,13 @@ int Simulator::start(int argc, char *argv[]) _instance->pollForMAVLinkMessages(false); #endif - } else { + } else if (argv[2][1] == 'p') { // Update sensor data _instance->pollForMAVLinkMessages(true); + + } else { + _instance->initializeSensorData(); + _instance->_initialized = true; } } else { @@ -151,9 +155,10 @@ int Simulator::start(int argc, char *argv[]) static void usage() { - PX4_WARN("Usage: simulator {start -[sc] |stop}"); + PX4_WARN("Usage: simulator {start -[spt] |stop}"); PX4_WARN("Simulate raw sensors: simulator start -s"); PX4_WARN("Publish sensors combined: simulator start -p"); + PX4_WARN("Dummy unit test data: simulator start -t"); } __BEGIN_DECLS @@ -167,15 +172,17 @@ extern "C" { int ret = 0; if (argc == 3 && strcmp(argv[1], "start") == 0) { - if (strcmp(argv[2], "-s") == 0 || strcmp(argv[2], "-p") == 0) { + if (strcmp(argv[2], "-s") == 0 || + strcmp(argv[2], "-p") == 0 || + strcmp(argv[2], "-t") == 0) { if (g_sim_task >= 0) { warnx("Simulator already started"); return 0; } - g_sim_task = px4_task_spawn_cmd("Simulator", + g_sim_task = px4_task_spawn_cmd("simulator", SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, + SCHED_PRIORITY_MAX, 1500, Simulator::start, argv); diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 6f9c35bb95..182be7916b 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -50,6 +50,7 @@ #include #include #include +#include #include #include #include @@ -222,6 +223,13 @@ private: _mag(1), _gps(1), _airspeed(1), + _perf_accel(perf_alloc_once(PC_ELAPSED, "sim_accel_delay")), + _perf_mpu(perf_alloc_once(PC_ELAPSED, "sim_mpu_delay")), + _perf_baro(perf_alloc_once(PC_ELAPSED, "sim_baro_delay")), + _perf_mag(perf_alloc_once(PC_ELAPSED, "sim_mag_delay")), + _perf_gps(perf_alloc_once(PC_ELAPSED, "sim_gps_delay")), + _perf_airspeed(perf_alloc_once(PC_ELAPSED, "sim_airspeed_delay")), + _perf_sim_delay(perf_alloc_once(PC_ELAPSED, "sim_network_delay")), _accel_pub(nullptr), _baro_pub(nullptr), _gyro_pub(nullptr), @@ -255,6 +263,14 @@ private: simulator::Report _gps; simulator::Report _airspeed; + perf_counter_t _perf_accel; + perf_counter_t _perf_mpu; + perf_counter_t _perf_baro; + perf_counter_t _perf_mag; + perf_counter_t _perf_gps; + perf_counter_t _perf_airspeed; + perf_counter_t _perf_sim_delay; + // uORB publisher handlers orb_advert_t _accel_pub; orb_advert_t _baro_pub; diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index ca45cdf2b8..507f220a43 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -39,6 +39,7 @@ #include #include #include +#include extern "C" __EXPORT hrt_abstime hrt_reset(void); @@ -65,7 +66,7 @@ static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f; static int openUart(const char *uart_name, int baud); static int _fd; -static unsigned char _buf[200]; +static unsigned char _buf[1024]; sockaddr_in _srcaddr; static socklen_t _addrlen = sizeof(_srcaddr); @@ -75,21 +76,21 @@ void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) { float out[8] = {}; - const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; + const float pwm_center = (PWM_DEFAULT_MAX + PWM_DEFAULT_MIN) / 2; // for now we only support quadrotors unsigned n = 4; - if (_vehicle_status.is_rotary_wing) { + if (_vehicle_status.is_rotary_wing || _vehicle_status.is_vtol) { for (unsigned i = 0; i < 8; i++) { - if (_actuators.output[i] > PWM_LOWEST_MIN / 2) { + if (_actuators.output[i] > PWM_DEFAULT_MIN / 2) { if (i < n) { // scale PWM out 900..2100 us to 0..1 for rotors */ - out[i] = (_actuators.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + out[i] = (_actuators.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN); } else { // scale PWM out 900..2100 us to -1..1 for other channels */ - out[i] = (_actuators.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); + out[i] = (_actuators.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2); } } else { @@ -113,7 +114,7 @@ void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) actuator_msg.time_usec = hrt_absolute_time(); actuator_msg.roll_ailerons = out[0]; - actuator_msg.pitch_elevator = _vehicle_status.is_rotary_wing ? out[1] : -out[1]; + actuator_msg.pitch_elevator = (_vehicle_status.is_rotary_wing || _vehicle_status.is_vtol) ? out[1] : -out[1]; actuator_msg.yaw_rudder = out[2]; actuator_msg.throttle = out[3]; actuator_msg.aux1 = out[4]; @@ -182,6 +183,7 @@ void Simulator::update_sensors(mavlink_hil_sensor_t *imu) mpu.gyro_z = imu->zgyro; write_MPU_data((void *)&mpu); + perf_begin(_perf_mpu); RawAccelData accel = {}; accel.x = imu->xacc; @@ -189,6 +191,7 @@ void Simulator::update_sensors(mavlink_hil_sensor_t *imu) accel.z = imu->zacc; write_accel_data((void *)&accel); + perf_begin(_perf_accel); RawMagData mag = {}; mag.x = imu->xmag; @@ -196,6 +199,7 @@ void Simulator::update_sensors(mavlink_hil_sensor_t *imu) mag.z = imu->zmag; write_mag_data((void *)&mag); + perf_begin(_perf_mag); RawBaroData baro = {}; // calculate air pressure from altitude (valid for low altitude) @@ -234,15 +238,23 @@ void Simulator::update_gps(mavlink_hil_gps_t *gps_sim) void Simulator::handle_message(mavlink_message_t *msg, bool publish) { switch (msg->msgid) { - case MAVLINK_MSG_ID_HIL_SENSOR: - mavlink_hil_sensor_t imu; - mavlink_msg_hil_sensor_decode(msg, &imu); + case MAVLINK_MSG_ID_HIL_SENSOR: { + mavlink_hil_sensor_t imu; + mavlink_msg_hil_sensor_decode(msg, &imu); - if (publish) { - publish_sensor_topics(&imu); + uint64_t sim_timestamp = imu.time_usec; + struct timespec ts; + px4_clock_gettime(CLOCK_REALTIME, &ts); + uint64_t timestamp = ts.tv_sec * 1000 * 1000 + ts.tv_nsec / 1000; + + perf_set(_perf_sim_delay, timestamp - sim_timestamp); + + if (publish) { + publish_sensor_topics(&imu); + } + + update_sensors(&imu); } - - update_sensors(&imu); break; case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW: @@ -337,15 +349,23 @@ void Simulator::poll_topics() void *Simulator::sending_trampoline(void *) { _instance->send(); - return 0; // why do I have to put this??? + return nullptr; } void Simulator::send() { - px4_pollfd_struct_t fds[1]; + px4_pollfd_struct_t fds[1] = {}; fds[0].fd = _actuator_outputs_sub; fds[0].events = POLLIN; + + // set the threads name +#ifdef __PX4_DARWIN + pthread_setname_np("sim_send"); +#else + pthread_setname_np(pthread_self(), "sim_send"); +#endif + int pret; while (true) { @@ -360,8 +380,6 @@ void Simulator::send() // this is undesirable but not much we can do if (pret < 0) { PX4_WARN("poll error %d, %d", pret, errno); - // sleep a bit before next try - usleep(100000); continue; } @@ -379,12 +397,12 @@ void Simulator::initializeSensorData() RawMPUData mpu = {}; mpu.accel_z = 9.81f; - write_MPU_data((void *)&mpu); + write_MPU_data(&mpu); RawAccelData accel = {}; accel.z = 9.81f; - write_accel_data((void *)&accel); + write_accel_data(&accel); RawMagData mag = {}; mag.x = 0.4f; @@ -399,11 +417,11 @@ void Simulator::initializeSensorData() baro.altitude = 0.0f; baro.temperature = 25.0f; - write_baro_data((void *)&baro); + write_baro_data(&baro); RawAirspeedData airspeed {}; - write_airspeed_data((void *)&airspeed); + write_airspeed_data(&airspeed); } void Simulator::pollForMAVLinkMessages(bool publish) @@ -440,7 +458,7 @@ void Simulator::pollForMAVLinkMessages(bool publish) (void)pthread_attr_getschedparam(&sender_thread_attr, ¶m); /* low priority */ - param.sched_priority = SCHED_PRIORITY_DEFAULT; + param.sched_priority = SCHED_PRIORITY_DEFAULT + 40; (void)pthread_attr_setschedparam(&sender_thread_attr, ¶m); // setup serial connection to autopilot (used to get manual controls) @@ -483,7 +501,7 @@ void Simulator::pollForMAVLinkMessages(bool publish) PX4_INFO("Waiting for initial data on UDP. Please start the flight simulator to proceed.."); while (pret <= 0) { - pret = ::poll(&fds[0], (sizeof(fds[0]) / sizeof(fds[0])), 100); + pret = ::poll(&fds[0], fd_count, 100); } PX4_INFO("Found initial message, pret = %d", pret); @@ -505,13 +523,22 @@ void Simulator::pollForMAVLinkMessages(bool publish) pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, NULL); pthread_attr_destroy(&sender_thread_attr); + + // set the threads name +#ifdef __PX4_DARWIN + pthread_setname_np("sim_rcv"); +#else + pthread_setname_np(pthread_self(), "sim_rcv"); +#endif + // wait for new mavlink messages to arrive while (true) { - pret = ::poll(&fds[0], fd_count, 100); + pret = ::poll(&fds[0], fd_count, 10); //timed out if (pret == 0) { + PX4_WARN("mavlink sim timeout for 10 ms"); continue; } @@ -531,7 +558,7 @@ void Simulator::pollForMAVLinkMessages(bool publish) mavlink_message_t msg; mavlink_status_t status; - for (int i = 0; i < len; ++i) { + for (int i = 0; i < len; i++) { if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &status)) { // have a message, handle it handle_message(&msg, publish); @@ -541,7 +568,7 @@ void Simulator::pollForMAVLinkMessages(bool publish) } // got data from PIXHAWK - if (fds[1].revents & POLLIN) { + if (fd_count > 1 && fds[1].revents & POLLIN) { len = ::read(serial_fd, serial_buf, sizeof(serial_buf)); if (len > 0) { @@ -664,8 +691,6 @@ int openUart(const char *uart_name, int baud) int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu) { - - //uint64_t timestamp = imu->time_usec; uint64_t timestamp = hrt_absolute_time(); if ((imu->fields_updated & 0x1FFF) != 0x1FFF) { diff --git a/src/modules/systemlib/circuit_breaker.cpp b/src/modules/systemlib/circuit_breaker.cpp index ba9e9f6219..c980ee757c 100644 --- a/src/modules/systemlib/circuit_breaker.cpp +++ b/src/modules/systemlib/circuit_breaker.cpp @@ -48,9 +48,12 @@ bool circuit_breaker_enabled(const char *breaker, int32_t magic) { - int32_t val; - (void)PX4_PARAM_GET_BYNAME(breaker, &val); + int32_t val = -1; - return (val == magic); + if (PX4_PARAM_GET_BYNAME(breaker, &val) == 0 && val == magic) { + return true; + } + + return false; } diff --git a/src/modules/systemlib/mavlink_log.c b/src/modules/systemlib/mavlink_log.c index 56201588a2..4216546c1a 100644 --- a/src/modules/systemlib/mavlink_log.c +++ b/src/modules/systemlib/mavlink_log.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Copyright (c) 2012, 2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,7 +35,7 @@ * @file mavlink_log.c * MAVLink text logging. * - * @author Lorenz Meier + * @author Lorenz Meier */ #include @@ -102,11 +101,15 @@ __EXPORT int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink __EXPORT void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...) { + if (!fmt) { + return; + } + va_list ap; va_start(ap, fmt); int end = (lb->start + lb->count) % lb->size; lb->elems[end].severity = severity; - vsnprintf(lb->elems[end].text, sizeof(lb->elems[0].text), fmt, ap); + vsnprintf(lb->elems[end].text, sizeof(lb->elems[end].text), fmt, ap); va_end(ap); /* increase count */ @@ -120,6 +123,10 @@ __EXPORT void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int seve __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...) { + if (!fmt) { + return; + } + va_list ap; va_start(ap, fmt); char text[MAVLINK_LOG_MAXLEN + 1]; diff --git a/src/modules/systemlib/mixer/CMakeLists.txt b/src/modules/systemlib/mixer/CMakeLists.txt index 20187eb97f..e9abd7a2a0 100644 --- a/src/modules/systemlib/mixer/CMakeLists.txt +++ b/src/modules/systemlib/mixer/CMakeLists.txt @@ -38,7 +38,7 @@ add_custom_command(OUTPUT mixer_multirotor.generated.h > mixer_multirotor.generated.h) add_custom_target(mixer_gen - DEPENDS mixer_multirotor.generated.h) + DEPENDS mixer_multirotor.generated.h multi_tables.py) px4_add_module( MODULE modules__systemlib__mixer @@ -51,5 +51,6 @@ px4_add_module( mixer_multirotor.generated.h DEPENDS platforms__common + mixer_gen ) # vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 38074f7ae5..d46906385b 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -150,6 +150,9 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl } else if (!strcmp(geomname, "4x")) { geometry = MultirotorGeometry::QUAD_X; + } else if (!strcmp(geomname, "4h")) { + geometry = MultirotorGeometry::QUAD_H; + } else if (!strcmp(geomname, "4v")) { geometry = MultirotorGeometry::QUAD_V; diff --git a/src/modules/systemlib/mixer/multi_tables.py b/src/modules/systemlib/mixer/multi_tables.py index e01fcaf8f2..4ca40a2591 100755 --- a/src/modules/systemlib/mixer/multi_tables.py +++ b/src/modules/systemlib/mixer/multi_tables.py @@ -62,6 +62,13 @@ quad_x = [ [135, CW], ] +quad_h = [ + [ 45, CW], + [-135, CW], + [-45, CCW], + [135, CCW], +] + quad_plus = [ [ 90, CCW], [ -90, CCW], @@ -162,7 +169,7 @@ tri_y = [ ] -tables = [quad_x, quad_plus, quad_v, quad_wide, quad_deadcat, hex_x, hex_plus, hex_cox, octa_x, octa_plus, octa_cox, twin_engine, tri_y] +tables = [quad_x, quad_h, quad_plus, quad_v, quad_wide, quad_deadcat, hex_x, hex_plus, hex_cox, octa_x, octa_plus, octa_cox, twin_engine, tri_y] def variableName(variable): for variableName, value in list(globals().items()): diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index a85e8da02d..0c9733712c 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -84,7 +84,9 @@ SubscriptionBase::SubscriptionBase(const struct orb_metadata *meta, if (_handle < 0) { warnx("sub failed"); } - orb_set_interval(getHandle(), interval); + if (interval > 0) { + orb_set_interval(getHandle(), interval); + } } bool SubscriptionBase::updated() diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 8fe70d7604..1b8fb18735 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -164,8 +164,12 @@ ORB_DEFINE(fence_vertex, struct fence_vertex_s); #include "topics/vehicle_attitude_setpoint.h" ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s); -ORB_DEFINE(mc_virtual_attitude_setpoint, struct vehicle_attitude_setpoint_s); -ORB_DEFINE(fw_virtual_attitude_setpoint, struct vehicle_attitude_setpoint_s); + +#include "topics/mc_virtual_attitude_setpoint.h" +ORB_DEFINE(mc_virtual_attitude_setpoint, struct mc_virtual_attitude_setpoint_s); + +#include "topics/fw_virtual_attitude_setpoint.h" +ORB_DEFINE(fw_virtual_attitude_setpoint, struct fw_virtual_attitude_setpoint_s); #include "topics/manual_control_setpoint.h" ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s); diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index 6ead26186b..cfcf39b408 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -366,7 +366,6 @@ __END_DECLS /* Diverse uORB header defines */ //XXX: move to better location #define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) -typedef struct vehicle_attitude_setpoint_s fw_virtual_attitude_setpoint_s; typedef uint8_t arming_state_t; typedef uint8_t main_state_t; typedef uint8_t hil_state_t; diff --git a/src/modules/uORB/uORBManager_posix.cpp b/src/modules/uORB/uORBManager_posix.cpp index 7fe3f6a687..3a01038b4a 100644 --- a/src/modules/uORB/uORBManager_posix.cpp +++ b/src/modules/uORB/uORBManager_posix.cpp @@ -164,6 +164,8 @@ int uORB::Manager::orb_copy(const struct orb_metadata *meta, int handle, void *b int uORB::Manager::orb_check(int handle, bool *updated) { + /* Set to false here so that if `px4_ioctl` fails to false. */ + *updated = false; return px4_ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated); } diff --git a/src/modules/uavcan/CMakeLists.txt b/src/modules/uavcan/CMakeLists.txt index ce348ec910..18ddc0f261 100644 --- a/src/modules/uavcan/CMakeLists.txt +++ b/src/modules/uavcan/CMakeLists.txt @@ -41,7 +41,7 @@ add_definitions( -DUAVCAN_NO_ASSERTIONS -DUAVCAN_PLATFORM=stm32 -DUAVCAN_STM32_${OS_UPPER}=1 - -DUAVCAN_STM32_NUM_IFACES=2 + -DUAVCAN_STM32_NUM_IFACES=${config_uavcan_num_ifaces} -DUAVCAN_STM32_TIMER_NUMBER=5 ) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 00671decd2..ee79e0573b 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -533,12 +533,17 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) * If no transceiver is connected, the RX pin will float, occasionally causing CAN controller to * fail during initialization. */ - #ifdef GPIO_CAN1_RX +#if defined(GPIO_CAN1_RX) stm32_configgpio(GPIO_CAN1_RX); stm32_configgpio(GPIO_CAN1_TX); - #endif +#endif +#if defined(GPIO_CAN2_RX) stm32_configgpio(GPIO_CAN2_RX | GPIO_PULLUP); stm32_configgpio(GPIO_CAN2_TX); +#endif +#if !defined(GPIO_CAN1_RX) && !defined(GPIO_CAN2_RX) +# error "Need to define GPIO_CAN1_RX and/or GPIO_CAN2_RX" +#endif /* * CAN driver init diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp index ca5bcede66..5e51317737 100644 --- a/src/modules/uavcan/uavcan_servers.cpp +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -188,9 +188,12 @@ int UavcanServers::start(uavcan::INode &main_node) struct sched_param param; pthread_attr_init(&tattr); + (void)pthread_attr_getschedparam(&tattr, ¶m); tattr.stacksize = StackSize; param.sched_priority = Priority; - pthread_attr_setschedparam(&tattr, ¶m); + if (pthread_attr_setschedparam(&tattr, ¶m)) { + warnx("setting sched params failed"); + } static auto run_trampoline = [](void *) {return UavcanServers::_instance->run(_instance);}; @@ -523,8 +526,12 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) switch (command_id) { case 1: { // Param save request - _param_save_opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_SAVE; - param_opcode(get_next_dirty_node_id(1)); + int node_id; + node_id = get_next_dirty_node_id(1); + if (node_id < 128) { + _param_save_opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_SAVE; + param_opcode(node_id); + } break; } case 2: { @@ -575,7 +582,8 @@ void UavcanServers::cb_getset(const uavcan::ServiceCallResultd_name[sizeof(UAVCAN_ROMFS_FW_PREFIX) - 1], src_fw_dirent->d_name, fw_len)) { + if (!memcmp(&fw_dirent->d_name, src_fw_dirent->d_name, fw_len)) { /* * Exact match between SD card filename and ROMFS filename; must be the same version * so don't bother deleting and rewriting it. @@ -1037,17 +1051,16 @@ void UavcanServers::unpackFwFromROMFS(const char* sd_path, const char* romfs_pat copy_fw = false; } else if (!memcmp(fw_dirent->d_name, UAVCAN_ROMFS_FW_PREFIX, sizeof(UAVCAN_ROMFS_FW_PREFIX) - 1)) { size_t fw_len = strlen(fw_dirent->d_name); - size_t dstpath_fw_len = dstpath_ver_len + sizeof(UAVCAN_ROMFS_FW_PREFIX) + fw_len; + size_t dstpath_fw_len = dstpath_ver_len + fw_len; if (dstpath_fw_len > maxlen) { // sizeof(prefix) includes trailing NUL, cancelling out the +1 for the path separator - warnx("unlink: path '%s/%s%s' too long", dstpath, UAVCAN_ROMFS_FW_PREFIX, fw_dirent->d_name); + warnx("unlink: path '%s/%s' too long", dstpath, fw_dirent->d_name); } else { - // File name starts with "romfs_", delete it. + // File name starts with "_", delete it. dstpath[dstpath_ver_len] = '/'; memcpy(&dstpath[dstpath_ver_len + 1], fw_dirent->d_name, fw_len + 1); unlink(dstpath); - - warnx("unlink: removed '%s/%s%s'", dstpath, UAVCAN_ROMFS_FW_PREFIX, fw_dirent->d_name); + warnx("unlink: removed '%s'", dstpath); } } else { // User file, don't copy firmware @@ -1060,20 +1073,19 @@ void UavcanServers::unpackFwFromROMFS(const char* sd_path, const char* romfs_pat // If we need to, copy the file from ROMFS to the SD card if (copy_fw) { size_t srcpath_fw_len = srcpath_ver_len + 1 + fw_len; - size_t dstpath_fw_len = dstpath_ver_len + sizeof(UAVCAN_ROMFS_FW_PREFIX) + fw_len; + size_t dstpath_fw_len = dstpath_ver_len + fw_len; if (srcpath_fw_len > maxlen) { warnx("copy: srcpath '%s/%s' too long", srcpath, src_fw_dirent->d_name); } else if (dstpath_fw_len > maxlen) { - warnx("copy: dstpath '%s/%s%s' too long", dstpath, UAVCAN_ROMFS_FW_PREFIX, src_fw_dirent->d_name); + warnx("copy: dstpath '%s/%s' too long", dstpath, src_fw_dirent->d_name); } else { // All OK, make the paths and copy the file srcpath[srcpath_ver_len] = '/'; memcpy(&srcpath[srcpath_ver_len + 1], src_fw_dirent->d_name, fw_len + 1); dstpath[dstpath_ver_len] = '/'; - memcpy(&dstpath[dstpath_ver_len + 1], UAVCAN_ROMFS_FW_PREFIX, sizeof(UAVCAN_ROMFS_FW_PREFIX)); - memcpy(&dstpath[dstpath_ver_len + sizeof(UAVCAN_ROMFS_FW_PREFIX)], src_fw_dirent->d_name, fw_len + 1); + memcpy(&dstpath[dstpath_ver_len +1], src_fw_dirent->d_name, fw_len + 1); rv = copyFw(dstpath, srcpath); if (rv != 0) { diff --git a/src/modules/uavcan/uavcan_servers.hpp b/src/modules/uavcan/uavcan_servers.hpp index 3cf4cbb4da..30fb3f5255 100644 --- a/src/modules/uavcan/uavcan_servers.hpp +++ b/src/modules/uavcan/uavcan_servers.hpp @@ -70,7 +70,7 @@ #define UAVCAN_NODE_DB_PATH "/fs/microsd/uavcan.db" #define UAVCAN_FIRMWARE_PATH "/fs/microsd/fw" #define UAVCAN_ROMFS_FW_PATH "/etc/uavcan/fw" -#define UAVCAN_ROMFS_FW_PREFIX "romfs_" +#define UAVCAN_ROMFS_FW_PREFIX "_" #define UAVCAN_MAX_PATH_LENGTH (128 + 40) #define UAVCAN_LOG_FILE UAVCAN_NODE_DB_PATH"/trace.log" diff --git a/src/modules/uavcan/uavcan_virtual_can_driver.hpp b/src/modules/uavcan/uavcan_virtual_can_driver.hpp index 956caf11ee..d395f2b76c 100644 --- a/src/modules/uavcan/uavcan_virtual_can_driver.hpp +++ b/src/modules/uavcan/uavcan_virtual_can_driver.hpp @@ -494,9 +494,8 @@ public: const unsigned quota_per_queue = virtual_iface_block_allocation_quota; // 2x overcommit for (unsigned i = 0; i < num_ifaces_; i++) { - ifaces_[i].template - construct - (allocator, clock_, driver_mutex_, quota_per_queue); + ifaces_[i].construct(allocator, clock_, driver_mutex_, quota_per_queue); } } diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 05c1a525b1..8a5896999e 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -186,6 +186,9 @@ void Standard::update_vtol_state() void Standard::update_transition_state() { + // copy virtual attitude setpoint to real attitude setpoint + memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); + if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { if (_params_standard.front_trans_dur <= 0.0f) { // just set the final target throttle value @@ -242,11 +245,15 @@ void Standard::update_transition_state() void Standard::update_mc_state() { - // do nothing + // copy virtual attitude setpoint to real attitude setpoint + memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); } void Standard::update_fw_state() { + // copy virtual attitude setpoint to real attitude setpoint + memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); + // in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again if (!_flag_enable_mc_motors) { set_max_mc(950); diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 7368870847..106f91a8e4 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -35,18 +35,42 @@ * @file tailsitter.cpp * * @author Roman Bapst +* @author David Vorsin * */ #include "tailsitter.h" #include "vtol_att_control_main.h" -Tailsitter::Tailsitter(VtolAttitudeControl *att_controller) : - VtolType(att_controller), - _airspeed_tot(0), +#define ARSP_YAW_CTRL_DISABLE 7.0f // airspeed at which we stop controlling yaw during a front transition +#define THROTTLE_TRANSITION_MAX 0.25f // maximum added thrust above last value in transition +#define PITCH_TRANSITION_FRONT_P1 -1.1f // pitch angle to switch to TRANSITION_P2 +#define PITCH_TRANSITION_FRONT_P2 -1.2f // pitch angle to switch to FW +#define PITCH_TRANSITION_BACK -0.25f // pitch angle to switch to MC + +Tailsitter::Tailsitter(VtolAttitudeControl *attc) : + VtolType(attc), + _airspeed_tot(0.0f), + _min_front_trans_dur(0.5f), + _loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")), _nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input")) { + _vtol_schedule.flight_mode = MC_MODE; + _vtol_schedule.transition_start = 0; + + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + _mc_yaw_weight = 1.0f; + + _flag_was_in_trans_mode = false; + + _params_handles_tailsitter.front_trans_dur = param_find("VT_F_TRANS_DUR"); + _params_handles_tailsitter.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR"); + _params_handles_tailsitter.back_trans_dur = param_find("VT_B_TRANS_DUR"); + _params_handles_tailsitter.airspeed_trans = param_find("VT_ARSP_TRANS"); + _params_handles_tailsitter.airspeed_blend_start = param_find("VT_ARSP_BLEND"); + _params_handles_tailsitter.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK"); } @@ -55,23 +79,170 @@ Tailsitter::~Tailsitter() } +int +Tailsitter::parameters_update() +{ + float v; + int l; + + /* vtol duration of a front transition */ + param_get(_params_handles_tailsitter.front_trans_dur, &v); + _params_tailsitter.front_trans_dur = math::constrain(v, 1.0f, 5.0f); + + /* vtol front transition phase 2 duration */ + param_get(_params_handles_tailsitter.front_trans_dur_p2, &v); + _params_tailsitter.front_trans_dur_p2 = v; + + /* vtol duration of a back transition */ + param_get(_params_handles_tailsitter.back_trans_dur, &v); + _params_tailsitter.back_trans_dur = math::constrain(v, 0.0f, 5.0f); + + /* vtol airspeed at which it is ok to switch to fw mode */ + param_get(_params_handles_tailsitter.airspeed_trans, &v); + _params_tailsitter.airspeed_trans = v; + + /* vtol airspeed at which we start blending mc/fw controls */ + param_get(_params_handles_tailsitter.airspeed_blend_start, &v); + _params_tailsitter.airspeed_blend_start = v; + + /* vtol lock elevons in multicopter */ + param_get(_params_handles_tailsitter.elevons_mc_lock, &l); + _params_tailsitter.elevons_mc_lock = l; + + /* avoid parameters which will lead to zero division in the transition code */ + _params_tailsitter.front_trans_dur = math::max(_params_tailsitter.front_trans_dur, _min_front_trans_dur); + + if (_params_tailsitter.airspeed_trans < _params_tailsitter.airspeed_blend_start + 1.0f) { + _params_tailsitter.airspeed_trans = _params_tailsitter.airspeed_blend_start + 1.0f; + } + + return OK; +} + void Tailsitter::update_vtol_state() { - // simply switch between the two modes - if (!_attc->is_fixed_wing_requested()) { - _vtol_mode = ROTARY_WING; + parameters_update(); - } else { + /* simple logic using a two way switch to perform transitions. + * after flipping the switch the vehicle will start tilting in MC control mode, picking up + * forward speed. After the vehicle has picked up enough and sufficient pitch angle the uav will go into FW mode. + * For the backtransition the pitch is controlled in MC mode again and switches to full MC control reaching the sufficient pitch angle. + */ + + if (_manual_control_sp->aux1 < 0.0f) { + + + switch (_vtol_schedule.flight_mode) { // user switchig to MC mode + case MC_MODE: + break; + + case FW_MODE: + _vtol_schedule.flight_mode = TRANSITION_BACK; + _vtol_schedule.transition_start = hrt_absolute_time(); + break; + + case TRANSITION_FRONT_P1: + // failsafe into multicopter mode + _vtol_schedule.flight_mode = MC_MODE; + break; + + case TRANSITION_FRONT_P2: + // NOT USED + // failsafe into multicopter mode + //_vtol_schedule.flight_mode = MC_MODE; + break; + + case TRANSITION_BACK: + + // check if we have reached pitch angle to switch to MC mode + if (_v_att->pitch >= PITCH_TRANSITION_BACK) { + _vtol_schedule.flight_mode = MC_MODE; + } + + break; + } + + } else { // user switchig to FW mode + + switch (_vtol_schedule.flight_mode) { + case MC_MODE: + // initialise a front transition + _vtol_schedule.flight_mode = TRANSITION_FRONT_P1; + _vtol_schedule.transition_start = hrt_absolute_time(); + break; + + case FW_MODE: + break; + + case TRANSITION_FRONT_P1: + + // check if we have reached airspeed and pitch angle to switch to TRANSITION P2 mode + if ((_airspeed->true_airspeed_m_s >= _params_tailsitter.airspeed_trans + && _v_att->pitch <= PITCH_TRANSITION_FRONT_P1) || !_armed->armed) { + _vtol_schedule.flight_mode = FW_MODE; + //_vtol_schedule.transition_start = hrt_absolute_time(); + } + + break; + + case TRANSITION_FRONT_P2: + + case TRANSITION_BACK: + // failsafe into fixed wing mode + _vtol_schedule.flight_mode = FW_MODE; + + /* **LATER*** if pitch is closer to mc (-45>) + * go to transition P1 + */ + break; + } + } + + // map tailsitter specific control phases to simple control modes + switch (_vtol_schedule.flight_mode) { + case MC_MODE: + _vtol_mode = ROTARY_WING; + _vtol_vehicle_status->vtol_in_trans_mode = false; + _flag_was_in_trans_mode = false; + break; + + case FW_MODE: _vtol_mode = FIXED_WING; + _vtol_vehicle_status->vtol_in_trans_mode = false; + _flag_was_in_trans_mode = false; + break; + + case TRANSITION_FRONT_P1: + _vtol_mode = TRANSITION; + _vtol_vehicle_status->vtol_in_trans_mode = true; + + case TRANSITION_FRONT_P2: + _vtol_mode = TRANSITION; + _vtol_vehicle_status->vtol_in_trans_mode = true; + + case TRANSITION_BACK: + _vtol_mode = TRANSITION; + _vtol_vehicle_status->vtol_in_trans_mode = true; + break; } } + + void Tailsitter::update_mc_state() { + // set idle speed for rotary wing mode if (!flag_idle_mc) { set_idle_mc(); flag_idle_mc = true; } + + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + _mc_yaw_weight = 1.0f; + + // copy virtual attitude setpoint to real attitude setpoint + memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); } void Tailsitter::update_fw_state() @@ -80,13 +251,137 @@ void Tailsitter::update_fw_state() set_idle_fw(); flag_idle_mc = false; } + + _mc_roll_weight = 0.0f; + _mc_pitch_weight = 0.0f; + _mc_yaw_weight = 0.0f; + + // copy virtual attitude setpoint to real attitude setpoint + memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); } void Tailsitter::update_transition_state() { + if (!_flag_was_in_trans_mode) { + // save desired heading for transition and last thrust value + _yaw_transition = _v_att_sp->yaw_body; + _pitch_transition_start = _v_att_sp->pitch_body; + _throttle_transition = _v_att_sp->thrust; + _flag_was_in_trans_mode = true; + } + if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) { + + /** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ + _v_att_sp->pitch_body = _pitch_transition_start - (fabsf(PITCH_TRANSITION_FRONT_P1 - _pitch_transition_start) * + (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur * 1000000.0f)); + _v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body , PITCH_TRANSITION_FRONT_P1 - 0.2f , + _pitch_transition_start); + + /** create time dependant throttle signal higher than in MC and growing untill P2 switch speed reached */ + if (_airspeed->true_airspeed_m_s <= _params_tailsitter.airspeed_trans) { + _v_att_sp->thrust = _throttle_transition + (fabsf(THROTTLE_TRANSITION_MAX * _throttle_transition) * + (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur * 1000000.0f)); + _v_att_sp->thrust = math::constrain(_v_att_sp->thrust , _throttle_transition , + (1.0f + THROTTLE_TRANSITION_MAX) * _throttle_transition); + } + + // disable mc yaw control once the plane has picked up speed + if (_airspeed->true_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) { + _mc_yaw_weight = 0.0f; + + } else { + _mc_yaw_weight = 1.0f; + } + + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + + } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) { + // the plane is ready to go into fixed wing mode, smoothly switch the actuator controls, keep pitching down + + /** no motor switching */ + + if (flag_idle_mc) { + set_idle_fw(); + flag_idle_mc = false; + } + + /** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ + if (_v_att_sp->pitch_body >= (PITCH_TRANSITION_FRONT_P2 - 0.2f)) { + _v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P1 - + (fabsf(PITCH_TRANSITION_FRONT_P2 - PITCH_TRANSITION_FRONT_P1) * (float)hrt_elapsed_time( + &_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f)); + + if (_v_att_sp->pitch_body <= (PITCH_TRANSITION_FRONT_P2 - 0.2f)) { + _v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P2 - 0.2f; + } + + } + + /** start blending MC and FW controls from pitch -45 to pitch -70 for smooth control takeover*/ + + //_mc_roll_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f)); + //_mc_pitch_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f)); + + + _mc_roll_weight = 0.0f; + _mc_pitch_weight = 0.0f; + + /** keep yaw disabled */ + _mc_yaw_weight = 0.0f; + + + } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) { + + if (!flag_idle_mc) { + set_idle_mc(); + flag_idle_mc = true; + } + + /** create time dependant pitch angle set point stating at -pi/2 + 0.2 rad overlap over the switch value*/ + _v_att_sp->pitch_body = M_PI_2_F + _pitch_transition_start + fabsf(PITCH_TRANSITION_BACK + 1.57f) * + (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.back_trans_dur * 1000000.0f); + _v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body , -2.0f , PITCH_TRANSITION_BACK + 0.2f); + + // throttle value is decreesed + _v_att_sp->thrust = _throttle_transition * 0.9f; + + /** keep yaw disabled */ + _mc_yaw_weight = 0.0f; + + /** smoothly move control weight to MC */ + _mc_roll_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / + (_params_tailsitter.back_trans_dur * 1000000.0f); + _mc_pitch_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / + (_params_tailsitter.back_trans_dur * 1000000.0f); + + } + + + + + _mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f); + _mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f); + _mc_pitch_weight = math::constrain(_mc_pitch_weight, 0.0f, 1.0f); + + // compute desired attitude and thrust setpoint for the transition + + _v_att_sp->timestamp = hrt_absolute_time(); + _v_att_sp->roll_body = 0.0f; + _v_att_sp->yaw_body = _yaw_transition; + _v_att_sp->R_valid = true; + + math::Matrix<3, 3> R_sp; + R_sp.from_euler(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body); + memcpy(&_v_att_sp->R_body[0], R_sp.data, sizeof(_v_att_sp->R_body)); + + math::Quaternion q_sp; + q_sp.from_dcm(R_sp); + memcpy(&_v_att_sp->q_d[0], &q_sp.data[0], sizeof(_v_att_sp->q_d)); } + void Tailsitter::update_external_state() { @@ -110,8 +405,7 @@ void Tailsitter::calc_tot_airspeed() _airspeed_tot = _params->arsp_lp_gain * (_airspeed_tot - airspeed_raw) + airspeed_raw; } -void -Tailsitter::scale_mc_output() +void Tailsitter::scale_mc_output() { // scale around tuning airspeed float airspeed; @@ -167,8 +461,7 @@ void Tailsitter::fill_actuator_outputs() // NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation! _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; //roll elevon - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = - _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; //pitch elevon + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; //pitch elevon } break; @@ -192,6 +485,26 @@ void Tailsitter::fill_actuator_outputs() break; case TRANSITION: + // in transition engines are mixed by weight (BACK TRANSITION ONLY) + _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] + * _mc_roll_weight; + _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = + _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; + _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * + _mc_yaw_weight; + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = + _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; + + // NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation! + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] + * (1 - _mc_roll_weight); + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = + _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; + // **LATER** + (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight); + _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = + _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; + break; + case EXTERNAL: // not yet implemented, we are switching brute force at the moment break; diff --git a/src/modules/vtol_att_control/tailsitter.h b/src/modules/vtol_att_control/tailsitter.h index ec8d007567..f68fbe9106 100644 --- a/src/modules/vtol_att_control/tailsitter.h +++ b/src/modules/vtol_att_control/tailsitter.h @@ -35,6 +35,7 @@ * @file tiltrotor.h * * @author Roman Bapst +* @author David Vorsin * */ @@ -42,7 +43,9 @@ #define TAILSITTER_H #include "vtol_type.h" -#include +#include /** is it necsacery? **/ +#include +#include class Tailsitter : public VtolType { @@ -51,21 +54,96 @@ public: Tailsitter(VtolAttitudeControl *_att_controller); ~Tailsitter(); + /** + * Update vtol state. + */ void update_vtol_state(); + + /** + * Update multicopter state. + */ void update_mc_state(); + + /** + * Update fixed wing state. + */ void update_fw_state(); + + /** + * Update transition state. + */ void update_transition_state(); + + /** + * Update external state. + */ void update_external_state(); private: - void fill_actuator_outputs(); - void calc_tot_airspeed(); - void scale_mc_output(); - float _airspeed_tot; + + struct { + float front_trans_dur; /**< duration of first part of front transition */ + float front_trans_dur_p2; + float back_trans_dur; /**< duration of back transition */ + float airspeed_trans; /**< airspeed at which we switch to fw mode after transition */ + float airspeed_blend_start; /**< airspeed at which we start blending mc/fw controls */ + int elevons_mc_lock; /**< lock elevons in multicopter mode */ + + } _params_tailsitter; + + struct { + param_t front_trans_dur; + param_t front_trans_dur_p2; + param_t back_trans_dur; + param_t airspeed_trans; + param_t airspeed_blend_start; + param_t elevons_mc_lock; + + } _params_handles_tailsitter; + + enum vtol_mode { + MC_MODE = 0, /**< vtol is in multicopter mode */ + TRANSITION_FRONT_P1, /**< vtol is in front transition part 1 mode */ + TRANSITION_FRONT_P2, /**< vtol is in front transition part 2 mode */ + TRANSITION_BACK, /**< vtol is in back transition mode */ + FW_MODE /**< vtol is in fixed wing mode */ + }; + + struct { + vtol_mode flight_mode; /**< vtol flight mode, defined by enum vtol_mode */ + hrt_abstime transition_start; /**< absoulte time at which front transition started */ + } _vtol_schedule; + + float _airspeed_tot; /** speed estimation for propwash controlled surfaces */ + + /** not sure about it yet ?! **/ + float _min_front_trans_dur; /**< min possible time in which rotors are rotated into the first position */ + + + /** should this anouncement stay? **/ perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ + /** + * Speed estimation for propwash controlled surfaces. + */ + void calc_tot_airspeed(); + + + /** is this one still needed? */ + void scale_mc_output(); + + /** + * Write control values to actuator output topics. + */ + void fill_actuator_outputs(); + + /** + * Update parameters. + */ + int parameters_update(); + }; #endif diff --git a/src/modules/vtol_att_control/tailsitter_params.c b/src/modules/vtol_att_control/tailsitter_params.c new file mode 100644 index 0000000000..af38b55693 --- /dev/null +++ b/src/modules/vtol_att_control/tailsitter_params.c @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * 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 tailsitter_params.c + * Parameters for vtol attitude controller. + * + * @author Roman Bapst + * @author David Vorsin + */ + +#include + +/** + * Duration of front transition phase 2 + * + * Time in seconds it should take for the rotors to rotate forward completely from the point + * when the plane has picked up enough airspeed and is ready to go into fixed wind mode. + * + * @min 0.1 + * @max 2 + * @group VTOL Attitude Control + +PARAM_DEFINE_FLOAT(VT_TRANS_P2_DUR, 0.5f);*/ + + diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 270aa065a9..3890a71843 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -56,6 +56,8 @@ Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) : _mc_pitch_weight = 1.0f; _mc_yaw_weight = 1.0f; + _flag_was_in_trans_mode = false; + _params_handles_tiltrotor.front_trans_dur = param_find("VT_F_TRANS_DUR"); _params_handles_tiltrotor.back_trans_dur = param_find("VT_B_TRANS_DUR"); _params_handles_tiltrotor.tilt_mc = param_find("VT_TILT_MC"); @@ -250,6 +252,9 @@ void Tiltrotor::update_vtol_state() void Tiltrotor::update_mc_state() { + // copy virtual attitude setpoint to real attitude setpoint + memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); + // make sure motors are not tilted _tilt_control = _params_tiltrotor.tilt_mc; @@ -271,6 +276,9 @@ void Tiltrotor::update_mc_state() void Tiltrotor::update_fw_state() { + // copy virtual attitude setpoint to real attitude setpoint + memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); + // make sure motors are tilted forward _tilt_control = _params_tiltrotor.tilt_fw; @@ -292,6 +300,13 @@ void Tiltrotor::update_fw_state() void Tiltrotor::update_transition_state() { + if (!_flag_was_in_trans_mode) { + // save desired heading for transition and last thrust value + _yaw_transition = _v_att->yaw; + _throttle_transition = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; + _flag_was_in_trans_mode = true; + } + if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) { // for the first part of the transition the rear rotors are enabled if (_rear_motors != ENABLED) { @@ -354,6 +369,9 @@ void Tiltrotor::update_transition_state() _mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f); _mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f); + + // copy virtual attitude setpoint to real attitude setpoint (we use multicopter att sp) + memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); } void Tiltrotor::update_external_state() diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 0248a20727..f21ed3fe52 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -41,6 +41,7 @@ * @author Roman Bapst * @author Lorenz Meier * @author Thomas Gubler + * @author David Vorsin * */ #include "vtol_att_control_main.h" @@ -60,6 +61,8 @@ VtolAttitudeControl::VtolAttitudeControl() : //init subscription handlers _v_att_sub(-1), _v_att_sp_sub(-1), + _mc_virtual_att_sp_sub(-1), + _fw_virtual_att_sp_sub(-1), _mc_virtual_v_rates_sp_sub(-1), _fw_virtual_v_rates_sp_sub(-1), _v_control_mode_sub(-1), @@ -75,13 +78,16 @@ VtolAttitudeControl::VtolAttitudeControl() : _actuators_0_pub(nullptr), _actuators_1_pub(nullptr), _vtol_vehicle_status_pub(nullptr), - _v_rates_sp_pub(nullptr) + _v_rates_sp_pub(nullptr), + _v_att_sp_pub(nullptr) { memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status)); _vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/ memset(&_v_att, 0, sizeof(_v_att)); memset(&_v_att_sp, 0, sizeof(_v_att_sp)); + memset(&_mc_virtual_att_sp, 0, sizeof(_mc_virtual_att_sp)); + memset(&_fw_virtual_att_sp, 0, sizeof(_fw_virtual_att_sp)); memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); memset(&_mc_virtual_v_rates_sp, 0, sizeof(_mc_virtual_v_rates_sp)); memset(&_fw_virtual_v_rates_sp, 0, sizeof(_fw_virtual_v_rates_sp)); @@ -98,7 +104,7 @@ VtolAttitudeControl::VtolAttitudeControl() : memset(&_batt_status, 0, sizeof(_batt_status)); memset(&_vehicle_cmd, 0, sizeof(_vehicle_cmd)); - _params.idle_pwm_mc = PWM_LOWEST_MIN; + _params.idle_pwm_mc = PWM_DEFAULT_MIN; _params.vtol_motor_count = 0; _params.vtol_fw_permanent_stab = 0; @@ -271,6 +277,36 @@ VtolAttitudeControl::vehicle_airspeed_poll() } } +/** +* Check for attitude set points update. +*/ +void +VtolAttitudeControl::vehicle_attitude_setpoint_poll() +{ + /* check if there is a new setpoint */ + bool updated; + orb_check(_v_att_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp); + } +} + +/** +* Check for attitude update. +*/ +void +VtolAttitudeControl::vehicle_attitude_poll() +{ + /* check if there is a new setpoint */ + bool updated; + orb_check(_v_att_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att); + } +} + /** * Check for battery updates. */ @@ -319,6 +355,38 @@ VtolAttitudeControl::vehicle_local_pos_poll() } +/** +* Check for mc virtual attitude setpoint updates. +*/ +void +VtolAttitudeControl::mc_virtual_att_sp_poll() +{ + bool updated; + + orb_check(_mc_virtual_att_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(mc_virtual_attitude_setpoint), _mc_virtual_att_sp_sub , &_mc_virtual_att_sp); + } + +} + +/** +* Check for fw virtual attitude setpoint updates. +*/ +void +VtolAttitudeControl::fw_virtual_att_sp_poll() +{ + bool updated; + + orb_check(_fw_virtual_att_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(fw_virtual_attitude_setpoint), _fw_virtual_att_sp_sub , &_fw_virtual_att_sp); + } + +} + /** * Check for command updates. */ @@ -439,6 +507,18 @@ void VtolAttitudeControl::fill_fw_att_rates_sp() _v_rates_sp.thrust = _fw_virtual_v_rates_sp.thrust; } +void VtolAttitudeControl::publish_att_sp() +{ + if (_v_att_sp_pub != nullptr) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_pub, &_v_att_sp); + + } else { + /* advertise and publish */ + _v_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp); + } +} + void VtolAttitudeControl::task_main_trampoline(int argc, char *argv[]) { @@ -452,9 +532,12 @@ void VtolAttitudeControl::task_main() /* do subscriptions */ _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + _mc_virtual_att_sp_sub = orb_subscribe(ORB_ID(mc_virtual_attitude_setpoint)); + _fw_virtual_att_sp_sub = orb_subscribe(ORB_ID(fw_virtual_attitude_setpoint)); _mc_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(mc_virtual_rates_setpoint)); _fw_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(fw_virtual_rates_setpoint)); _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); @@ -523,9 +606,13 @@ void VtolAttitudeControl::task_main() _vtol_vehicle_status.fw_permanent_stab = _params.vtol_fw_permanent_stab == 1 ? true : false; + mc_virtual_att_sp_poll(); + fw_virtual_att_sp_poll(); vehicle_control_mode_poll(); //Check for changes in vehicle control mode. vehicle_manual_poll(); //Check for changes in manual inputs. arming_status_poll(); //Check for arming status updates. + vehicle_attitude_setpoint_poll();//Check for changes in attitude set points + vehicle_attitude_poll(); //Check for changes in attitude actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output vehicle_rates_sp_mc_poll(); @@ -582,6 +669,7 @@ void VtolAttitudeControl::task_main() } else if (_vtol_type->get_mode() == TRANSITION) { // vehicle is doing a transition _vtol_vehicle_status.vtol_in_trans_mode = true; + _vtol_vehicle_status.vtol_in_rw_mode = true; //making mc attitude controller work during transition bool got_new_data = false; @@ -599,6 +687,7 @@ void VtolAttitudeControl::task_main() if (got_new_data) { _vtol_type->update_transition_state(); fill_mc_att_rates_sp(); + publish_att_sp(); } } else if (_vtol_type->get_mode() == EXTERNAL) { @@ -606,6 +695,7 @@ void VtolAttitudeControl::task_main() _vtol_type->update_external_state(); } + publish_att_sp(); _vtol_type->fill_actuator_outputs(); /* Only publish if the proper mode(s) are enabled */ diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index c10f7b0dd7..0e99d58aab 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -41,6 +41,7 @@ * @author Roman Bapst * @author Lorenz Meier * @author Thomas Gubler + * @author David Vorsin * */ #ifndef VTOL_ATT_CONTROL_MAIN_H @@ -62,6 +63,8 @@ #include #include #include +#include +#include #include #include #include @@ -104,22 +107,24 @@ public: int start(); /* start the task and return OK on success */ bool is_fixed_wing_requested(); - struct vehicle_attitude_s *get_att() {return &_v_att;} - struct vehicle_attitude_setpoint_s *get_att_sp() {return &_v_att_sp;} - struct vehicle_rates_setpoint_s *get_rates_sp() {return &_v_rates_sp;} - struct vehicle_rates_setpoint_s *get_mc_virtual_rates_sp() {return &_mc_virtual_v_rates_sp;} - struct vehicle_rates_setpoint_s *get_fw_virtual_rates_sp() {return &_fw_virtual_v_rates_sp;} - struct manual_control_setpoint_s *get_manual_control_sp() {return &_manual_control_sp;} - struct vehicle_control_mode_s *get_control_mode() {return &_v_control_mode;} - struct vtol_vehicle_status_s *get_vehicle_status() {return &_vtol_vehicle_status;} - struct actuator_controls_s *get_actuators_out0() {return &_actuators_out_0;} - struct actuator_controls_s *get_actuators_out1() {return &_actuators_out_1;} - struct actuator_controls_s *get_actuators_mc_in() {return &_actuators_mc_in;} - struct actuator_controls_s *get_actuators_fw_in() {return &_actuators_fw_in;} - struct actuator_armed_s *get_armed() {return &_armed;} - struct vehicle_local_position_s *get_local_pos() {return &_local_pos;} - struct airspeed_s *get_airspeed() {return &_airspeed;} - struct battery_status_s *get_batt_status() {return &_batt_status;} + struct vehicle_attitude_s *get_att() {return &_v_att;} + struct vehicle_attitude_setpoint_s *get_att_sp() {return &_v_att_sp;} + struct mc_virtual_attitude_setpoint_s *get_mc_virtual_att_sp() {return &_mc_virtual_att_sp;} + struct fw_virtual_attitude_setpoint_s *get_fw_virtual_att_sp() {return &_fw_virtual_att_sp;} + struct vehicle_rates_setpoint_s *get_rates_sp() {return &_v_rates_sp;} + struct vehicle_rates_setpoint_s *get_mc_virtual_rates_sp() {return &_mc_virtual_v_rates_sp;} + struct vehicle_rates_setpoint_s *get_fw_virtual_rates_sp() {return &_fw_virtual_v_rates_sp;} + struct manual_control_setpoint_s *get_manual_control_sp() {return &_manual_control_sp;} + struct vehicle_control_mode_s *get_control_mode() {return &_v_control_mode;} + struct vtol_vehicle_status_s *get_vehicle_status() {return &_vtol_vehicle_status;} + struct actuator_controls_s *get_actuators_out0() {return &_actuators_out_0;} + struct actuator_controls_s *get_actuators_out1() {return &_actuators_out_1;} + struct actuator_controls_s *get_actuators_mc_in() {return &_actuators_mc_in;} + struct actuator_controls_s *get_actuators_fw_in() {return &_actuators_fw_in;} + struct actuator_armed_s *get_armed() {return &_armed;} + struct vehicle_local_position_s *get_local_pos() {return &_local_pos;} + struct airspeed_s *get_airspeed() {return &_airspeed;} + struct battery_status_s *get_batt_status() {return &_batt_status;} struct Params *get_params() {return &_params;} @@ -132,6 +137,8 @@ private: /* handlers for subscriptions */ int _v_att_sub; //vehicle attitude subscription int _v_att_sp_sub; //vehicle attitude setpoint subscription + int _mc_virtual_att_sp_sub; + int _fw_virtual_att_sp_sub; int _mc_virtual_v_rates_sp_sub; //vehicle rates setpoint subscription int _fw_virtual_v_rates_sp_sub; //vehicle rates setpoint subscription int _v_control_mode_sub; //vehicle control mode subscription @@ -151,9 +158,12 @@ private: orb_advert_t _actuators_1_pub; orb_advert_t _vtol_vehicle_status_pub; orb_advert_t _v_rates_sp_pub; + orb_advert_t _v_att_sp_pub; //*******************data containers*********************************************************** struct vehicle_attitude_s _v_att; //vehicle attitude struct vehicle_attitude_setpoint_s _v_att_sp; //vehicle attitude setpoint + struct mc_virtual_attitude_setpoint_s _mc_virtual_att_sp; // virtual mc attitude setpoint + struct fw_virtual_attitude_setpoint_s _fw_virtual_att_sp; // virtual fw attitude setpoint struct vehicle_rates_setpoint_s _v_rates_sp; //vehicle rates setpoint struct vehicle_rates_setpoint_s _mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint struct vehicle_rates_setpoint_s _fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint @@ -187,6 +197,9 @@ private: param_t elevons_mc_lock; } _params_handles; + /* for multicopters it is usual to have a non-zero idle speed of the engines + * for fixed wings we want to have an idle speed of zero since we do not want + * to waste energy when gliding. */ int _transition_command; VtolType *_vtol_type; // base class for different vtol types @@ -202,12 +215,16 @@ private: void vehicle_control_mode_poll(); //Check for changes in vehicle control mode. void vehicle_manual_poll(); //Check for changes in manual inputs. void arming_status_poll(); //Check for arming status updates. + void mc_virtual_att_sp_poll(); + void fw_virtual_att_sp_poll(); void actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output void actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output void vehicle_rates_sp_mc_poll(); void vehicle_rates_sp_fw_poll(); void vehicle_local_pos_poll(); // Check for changes in sensor values void vehicle_airspeed_poll(); // Check for changes in airspeed + void vehicle_attitude_setpoint_poll(); //Check for attitude setpoint updates. + void vehicle_attitude_poll(); //Check for attitude updates. void vehicle_battery_poll(); // Check for battery updates void vehicle_cmd_poll(); void parameters_update_poll(); //Check if parameters have changed @@ -215,6 +232,7 @@ private: void fill_mc_att_rates_sp(); void fill_fw_att_rates_sp(); void handle_command(); + void publish_att_sp(); }; #endif diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index f57080a2a7..2f361eb8f4 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -49,6 +49,8 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) : { _v_att = _attc->get_att(); _v_att_sp = _attc->get_att_sp(); + _mc_virtual_att_sp = _attc->get_mc_virtual_att_sp(); + _fw_virtual_att_sp = _attc->get_fw_virtual_att_sp(); _v_rates_sp = _attc->get_rates_sp(); _mc_virtual_v_rates_sp = _attc->get_mc_virtual_rates_sp(); _fw_virtual_v_rates_sp = _attc->get_fw_virtual_rates_sp(); @@ -115,13 +117,12 @@ void VtolType::set_idle_fw() if (fd < 0) {PX4_WARN("can't open %s", dev);} - unsigned pwm_value = PWM_LOWEST_MIN; struct pwm_output_values pwm_values; memset(&pwm_values, 0, sizeof(pwm_values)); for (int i = 0; i < _params->vtol_motor_count; i++) { - pwm_values.values[i] = pwm_value; + pwm_values.values[i] = PWM_MOTOR_OFF; pwm_values.channel_count++; } diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index d8557110c0..196751016c 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -93,20 +93,22 @@ protected: struct vehicle_attitude_s *_v_att; //vehicle attitude struct vehicle_attitude_setpoint_s *_v_att_sp; //vehicle attitude setpoint + struct mc_virtual_attitude_setpoint_s *_mc_virtual_att_sp; // virtual mc attitude setpoint + struct fw_virtual_attitude_setpoint_s *_fw_virtual_att_sp; // virtual fw attitude setpoint struct vehicle_rates_setpoint_s *_v_rates_sp; //vehicle rates setpoint struct vehicle_rates_setpoint_s *_mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint struct vehicle_rates_setpoint_s *_fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint struct manual_control_setpoint_s *_manual_control_sp; //manual control setpoint struct vehicle_control_mode_s *_v_control_mode; //vehicle control mode struct vtol_vehicle_status_s *_vtol_vehicle_status; - struct actuator_controls_s *_actuators_out_0; //actuator controls going to the mc mixer - struct actuator_controls_s *_actuators_out_1; //actuator controls going to the fw mixer (used for elevons) - struct actuator_controls_s *_actuators_mc_in; //actuator controls from mc_att_control - struct actuator_controls_s *_actuators_fw_in; //actuator controls from fw_att_control - struct actuator_armed_s *_armed; //actuator arming status + struct actuator_controls_s *_actuators_out_0; //actuator controls going to the mc mixer + struct actuator_controls_s *_actuators_out_1; //actuator controls going to the fw mixer (used for elevons) + struct actuator_controls_s *_actuators_mc_in; //actuator controls from mc_att_control + struct actuator_controls_s *_actuators_fw_in; //actuator controls from fw_att_control + struct actuator_armed_s *_armed; //actuator arming status struct vehicle_local_position_s *_local_pos; - struct airspeed_s *_airspeed; // airspeed - struct battery_status_s *_batt_status; // battery status + struct airspeed_s *_airspeed; // airspeed + struct battery_status_s *_batt_status; // battery status struct Params *_params; @@ -116,6 +118,11 @@ protected: float _mc_pitch_weight; // weight for multicopter attitude controller pitch output float _mc_yaw_weight; // weight for multicopter attitude controller yaw output + float _yaw_transition; // yaw angle in which transition will take place + float _pitch_transition_start; // pitch angle at the start of transition (tailsitter) + float _throttle_transition; // throttle value used for the transition phase + bool _flag_was_in_trans_mode; // true if mode has just switched to transition + }; #endif diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index 5017bb111c..63e3785118 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -44,17 +44,16 @@ #include #include #include -#include #include #include #include +#include #include #include #include -#include #include #include #include @@ -63,15 +62,9 @@ #include #include #include - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; +#include #define ACCELSIM_DEVICE_PATH_ACCEL "/dev/sim_accel" -#define ACCELSIM_DEVICE_PATH_ACCEL_EXT "/dev/sim_accel_ext" #define ACCELSIM_DEVICE_PATH_MAG "/dev/sim_mag" #define ADDR_WHO_AM_I 0x0F @@ -87,10 +80,11 @@ static const int ERROR = -1; extern "C" { __EXPORT int accelsim_main(int argc, char *argv[]); } +using namespace DriverFramework; class ACCELSIM_mag; -class ACCELSIM : public device::VDev +class ACCELSIM : public VirtDevObj { public: ACCELSIM(const char *path, enum Rotation rotation); @@ -98,8 +92,8 @@ public: virtual int init(); - virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); + virtual ssize_t devRead(void *buffer, size_t buflen); + virtual int devIOCTL(unsigned long cmd, unsigned long arg); /** * dump register values @@ -109,22 +103,16 @@ public: protected: friend class ACCELSIM_mag; - ssize_t mag_read(device::file_t *filp, char *buffer, size_t buflen); - int mag_ioctl(device::file_t *filp, int cmd, unsigned long arg); + ssize_t mag_read(void *buffer, size_t buflen); + int mag_ioctl(unsigned long cmd, unsigned long arg); int transfer(uint8_t *send, uint8_t *recv, unsigned len); private: ACCELSIM_mag *_mag; - struct hrt_call _accel_call; - struct hrt_call _mag_call; - - unsigned _call_accel_interval; - unsigned _call_mag_interval; - - ringbuffer::RingBuffer *_accel_reports; - ringbuffer::RingBuffer *_mag_reports; + ringbuffer::RingBuffer *_accel_reports; + ringbuffer::RingBuffer *_mag_reports; struct accel_scale _accel_scale; unsigned _accel_range_m_s2; @@ -163,49 +151,20 @@ private: // last temperature value float _last_temperature; - // this is used to support runtime checking of key - // configuration registers to detect SPI bus errors and sensor - // reset + /** + * Override Start automatic measurement. + */ + virtual int start(); /** - * Start automatic measurement. + * Override Stop automatic measurement. */ - void start(); - - /** - * Stop automatic measurement. - */ - void stop(); - - /** - * Reset chip. - * - * Resets the chip and measurements ranges, but not scale and offset. - */ - void reset(); - - /** - * Static trampoline from the hrt_call context; because we don't have a - * generic hrt wrapper yet. - * - * Called by the HRT in interrupt context at the specified rate if - * automatic polling is enabled. - * - * @param arg Instance pointer for the driver that is polling. - */ - static void measure_trampoline(void *arg); - - /** - * Static trampoline for the mag because it runs at a lower rate - * - * @param arg Instance pointer for the driver that is polling. - */ - static void mag_measure_trampoline(void *arg); + virtual int stop(); /** * Fetch accel measurements from the sensor and update the report ring. */ - void measure(); + virtual void _measure(); /** * Fetch mag measurements from the sensor and update the report ring. @@ -239,26 +198,6 @@ private: */ int accel_set_driver_lowpass_filter(float samplerate, float bandwidth); - /** - * Set the ACCELSIM internal accel sampling frequency. - * - * @param frequency The internal accel sampling frequency is set to not less than - * this value. - * Zero selects the maximum rate supported. - * @return OK if the value can be supported. - */ - int accel_set_samplerate(unsigned frequency); - - /** - * Set the ACCELSIM internal mag sampling frequency. - * - * @param frequency The internal mag sampling frequency is set to not less than - * this value. - * Zero selects the maximum rate supported. - * @return OK if the value can be supported. - */ - int mag_set_samplerate(unsigned frequency); - /* this class cannot be copied */ ACCELSIM(const ACCELSIM &); ACCELSIM operator=(const ACCELSIM &); @@ -267,16 +206,17 @@ private: /** * Helper class implementing the mag driver node. */ -class ACCELSIM_mag : public device::VDev +class ACCELSIM_mag : public VirtDevObj { public: ACCELSIM_mag(ACCELSIM *parent); ~ACCELSIM_mag(); - virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); + virtual ssize_t devRead(void *buffer, size_t buflen); + virtual int devIOCTL(unsigned long cmd, unsigned long arg); - virtual int init(); + virtual int start(); + virtual int stop(); protected: friend class ACCELSIM; @@ -288,9 +228,7 @@ private: int _mag_orb_class_instance; int _mag_class_instance; - void measure(); - - void measure_trampoline(void *arg); + virtual void _measure(); /* this class does not allow copying due to ptr data members */ ACCELSIM_mag(const ACCELSIM_mag &); @@ -299,12 +237,8 @@ private: ACCELSIM::ACCELSIM(const char *path, enum Rotation rotation) : - VDev("ACCELSIM", path), + VirtDevObj("ACCELSIM", path, ACCEL_BASE_DEVICE_PATH, 1e6 / 400), _mag(new ACCELSIM_mag(this)), - _accel_call{}, - _mag_call{}, - _call_accel_interval(0), - _call_mag_interval(0), _accel_reports(nullptr), _mag_reports(nullptr), _accel_scale{}, @@ -333,15 +267,12 @@ ACCELSIM::ACCELSIM(const char *path, enum Rotation rotation) : _constant_accel_count(0), _last_temperature(0) { - // enable debug() calls - _debug_enabled = false; - - _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_ACCELSIM; + m_id.dev_id_s.bus = 1; + m_id.dev_id_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; - + _mag->m_id.dev_id = m_id.dev_id; + _mag->m_id.dev_id_s.devtype = DRV_MAG_DEVTYPE_ACCELSIM; // default scale factors _accel_scale.x_offset = 0.0f; @@ -362,6 +293,7 @@ ACCELSIM::ACCELSIM(const char *path, enum Rotation rotation) : ACCELSIM::~ACCELSIM() { /* make sure we are truly inactive */ + _mag->stop(); stop(); /* free any existing reports */ @@ -373,10 +305,6 @@ ACCELSIM::~ACCELSIM() delete _mag_reports; } - if (_accel_class_instance != -1) { - unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance); - } - delete _mag; /* delete the perf counter */ @@ -390,13 +318,13 @@ ACCELSIM::~ACCELSIM() int ACCELSIM::init() { - int ret = ERROR; + int ret = -1; struct mag_report mrp = {}; struct accel_report arp = {}; /* do SIM init first */ - if (VDev::init() != OK) { + if (VirtDevObj::init() != 0) { PX4_WARN("SIM init failed"); goto out; } @@ -414,9 +342,7 @@ ACCELSIM::init() goto out; } - reset(); - - /* do VDev init for the mag device node */ + /* do init for the mag device node */ ret = _mag->init(); if (ret != OK) { @@ -425,7 +351,7 @@ ACCELSIM::init() } /* fill report structures */ - measure(); + _measure(); /* advertise sensor topic, measure manually to initialize valid report */ _mag_reports->get(&mrp); @@ -438,9 +364,6 @@ ACCELSIM::init() PX4_WARN("ADVERT ERR"); } - - _accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH); - /* advertise sensor topic, measure manually to initialize valid report */ _accel_reports->get(&arp); @@ -456,11 +379,6 @@ out: return ret; } -void -ACCELSIM::reset() -{ -} - int ACCELSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) { @@ -490,7 +408,7 @@ ACCELSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) } ssize_t -ACCELSIM::read(device::file_t *filp, char *buffer, size_t buflen) +ACCELSIM::devRead(void *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct accel_report); accel_report *arb = reinterpret_cast(buffer); @@ -502,7 +420,7 @@ ACCELSIM::read(device::file_t *filp, char *buffer, size_t buflen) } /* if automatic measurement is enabled */ - if (_call_accel_interval > 0) { + if (m_sample_interval_usecs > 0) { /* * While there is space in the caller's buffer, and reports, copy them. */ @@ -518,7 +436,7 @@ ACCELSIM::read(device::file_t *filp, char *buffer, size_t buflen) } /* manual measurement */ - measure(); + _measure(); /* measurement will have generated a report, copy it out */ if (_accel_reports->get(arb)) { @@ -529,7 +447,7 @@ ACCELSIM::read(device::file_t *filp, char *buffer, size_t buflen) } ssize_t -ACCELSIM::mag_read(device::file_t *filp, char *buffer, size_t buflen) +ACCELSIM::mag_read(void *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct mag_report); mag_report *mrb = reinterpret_cast(buffer); @@ -541,7 +459,7 @@ ACCELSIM::mag_read(device::file_t *filp, char *buffer, size_t buflen) } /* if automatic measurement is enabled */ - if (_call_mag_interval > 0) { + if (_mag->m_sample_interval_usecs > 0) { /* * While there is space in the caller's buffer, and reports, copy them. @@ -559,7 +477,7 @@ ACCELSIM::mag_read(device::file_t *filp, char *buffer, size_t buflen) /* manual measurement */ _mag_reports->flush(); - _mag->measure(); + _mag->_measure(); /* measurement will have generated a report, copy it out */ if (_mag_reports->get(mrb)) { @@ -570,17 +488,19 @@ ACCELSIM::mag_read(device::file_t *filp, char *buffer, size_t buflen) } int -ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) +ACCELSIM::devIOCTL(unsigned long cmd, unsigned long arg) { + unsigned long ul_arg = (unsigned long)arg; + switch (cmd) { case SENSORIOCSPOLLRATE: { - switch (arg) { + switch (ul_arg) { /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); - _call_accel_interval = 0; + m_sample_interval_usecs = 0; return OK; /* external signalling not supported */ @@ -592,32 +512,29 @@ ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: - return ioctl(filp, SENSORIOCSPOLLRATE, 1600); + return devIOCTL(SENSORIOCSPOLLRATE, 1600); case SENSOR_POLLRATE_DEFAULT: - return ioctl(filp, SENSORIOCSPOLLRATE, ACCELSIM_ACCEL_DEFAULT_RATE); + return devIOCTL(SENSORIOCSPOLLRATE, ACCELSIM_ACCEL_DEFAULT_RATE); /* adjust to a legal polling interval in Hz */ default: { - /* 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; + unsigned interval = 1000000 / ul_arg; /* check against maximum sane rate */ - if (period < 500) { + if (interval < 500) { return -EINVAL; } /* adjust filters */ - accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq()); + accel_set_driver_lowpass_filter((float)ul_arg, _accel_filter_x.get_cutoff_freq()); + + bool want_start = (m_sample_interval_usecs == 0); /* update interval for next measurement */ - /* XXX this is a bit shady, but no other way to adjust... */ - _accel_call.period = _call_accel_interval = period; + setSampleInterval(interval); - /* if we need to start the poll state machine, do it */ if (want_start) { start(); } @@ -628,19 +545,19 @@ ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_call_accel_interval == 0) { + if (m_sample_interval_usecs == 0) { return SENSOR_POLLRATE_MANUAL; } - return 1000000 / _call_accel_interval; + return 1000000 / m_sample_interval_usecs; case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) { + if ((ul_arg < 1) || (ul_arg > 100)) { return -EINVAL; } - if (!_accel_reports->resize(arg)) { + if (!_accel_reports->resize(ul_arg)) { return -ENOMEM; } @@ -651,17 +568,18 @@ ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) return _accel_reports->size(); case SENSORIOCRESET: - reset(); + // Nothing to do for simulator return OK; case ACCELIOCSSAMPLERATE: - return accel_set_samplerate(arg); + // No need to set internal sampling rate for simulator + return OK; case ACCELIOCGSAMPLERATE: 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)ul_arg); } case ACCELIOCSSCALE: { @@ -680,7 +598,7 @@ ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) case ACCELIOCSRANGE: /* arg needs to be in G */ - return accel_set_range(arg); + return accel_set_range(ul_arg); case ACCELIOCGRANGE: /* convert to m/s^2 and return rounded in G */ @@ -688,7 +606,7 @@ ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) case ACCELIOCGSCALE: /* copy scale out */ - memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale)); + memcpy((struct accel_scale *) arg, &(_accel_scale), sizeof(_accel_scale)); return OK; case ACCELIOCSELFTEST: @@ -696,13 +614,15 @@ ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) default: /* give it to the superclass */ - return VDev::ioctl(filp, cmd, arg); + return VirtDevObj::devIOCTL(cmd, arg); } } int -ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) +ACCELSIM::mag_ioctl(unsigned long cmd, unsigned long arg) { + unsigned long ul_arg = (unsigned long)arg; + switch (cmd) { case SENSORIOCSPOLLRATE: { @@ -710,8 +630,8 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: - stop(); - _call_mag_interval = 0; + _mag->stop(); + _mag->m_sample_interval_usecs = 0; return OK; /* external signalling not supported */ @@ -725,30 +645,25 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: /* 100 Hz is max for mag */ - return mag_ioctl(filp, SENSORIOCSPOLLRATE, 100); + return mag_ioctl(SENSORIOCSPOLLRATE, 100); /* adjust to a legal polling interval in Hz */ default: { - /* do we need to start internal polling? */ - bool want_start = (_call_mag_interval == 0); - /* convert hz to hrt interval via microseconds */ - unsigned period = 1000000 / arg; + unsigned interval = 1000000 / ul_arg; /* check against maximum sane rate (1ms) */ - if (period < 10000) { + if (interval < 10000) { return -EINVAL; } + bool want_start = (_mag->m_sample_interval_usecs == 0); + /* update interval for next measurement */ - /* XXX this is a bit shady, but no other way to adjust... */ - _mag_call.period = _call_mag_interval = period; + _mag->setSampleInterval(interval); - //PX4_INFO("SET _call_mag_interval=%u", _call_mag_interval); - - /* if we need to start the poll state machine, do it */ if (want_start) { - start(); + _mag->start(); } return OK; @@ -757,11 +672,11 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_call_mag_interval == 0) { + if (_mag->m_sample_interval_usecs == 0) { return SENSOR_POLLRATE_MANUAL; } - return 1000000 / _call_mag_interval; + return 1000000 / _mag->m_sample_interval_usecs; case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ @@ -780,11 +695,12 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) return _mag_reports->size(); case SENSORIOCRESET: - reset(); + // Nothing to do for simulator return OK; case MAGIOCSSAMPLERATE: - return mag_set_samplerate(arg); + // No need to set internal sampling rate for simulator + return OK; case MAGIOCGSAMPLERATE: return _mag_samplerate; @@ -822,7 +738,7 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) default: /* give it to the superclass */ - return VDev::ioctl(filp, cmd, arg); + return VirtDevObj::devIOCTL(cmd, arg); } } @@ -857,20 +773,9 @@ ACCELSIM::accel_set_driver_lowpass_filter(float samplerate, float bandwidth) } int -ACCELSIM::accel_set_samplerate(unsigned frequency) -{ - return OK; -} - -int -ACCELSIM::mag_set_samplerate(unsigned frequency) -{ - return OK; -} - -void ACCELSIM::start() { + //PX4_INFO("ACCELSIM::start"); /* make sure we are stopped first */ stop(); @@ -878,57 +783,33 @@ ACCELSIM::start() _accel_reports->flush(); _mag_reports->flush(); - /* start polling at the specified rate */ - //PX4_INFO("ACCELSIM::start accel %u", _call_accel_interval); - hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&ACCELSIM::measure_trampoline, this); + int ret2 = VirtDevObj::start(); - // There is a race here where SENSORIOCSPOLLRATE on the accel starts polling of mag but _call_mag_interval is 0 - if (_call_mag_interval == 0) { - //PX4_INFO("_call_mag_interval uninitilized - would have set period delay of 0"); - _call_mag_interval = 10000; // Max 100Hz + if (ret2 != 0) { + PX4_ERR("ACCELSIM::start base class start failed"); } - //PX4_INFO("ACCELSIM::start mag %u", _call_mag_interval); - hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&ACCELSIM::mag_measure_trampoline, this); + return (ret2 != 0) ? -1 : 0; } -void +int ACCELSIM::stop() { - hrt_cancel(&_accel_call); - hrt_cancel(&_mag_call); + //PX4_INFO("ACCELSIM::stop"); + return VirtDevObj::stop(); } void -ACCELSIM::measure_trampoline(void *arg) -{ - //PX4_INFO("ACCELSIM::measure_trampoline"); - ACCELSIM *dev = (ACCELSIM *)arg; - - /* make another measurement */ - dev->measure(); -} - -void -ACCELSIM::mag_measure_trampoline(void *arg) -{ - //PX4_INFO("ACCELSIM::mag_measure_trampoline"); - ACCELSIM *dev = (ACCELSIM *)arg; - - /* make another measurement */ - dev->mag_measure(); -} - -void -ACCELSIM::measure() +ACCELSIM::_measure() { + //PX4_INFO("ACCELSIM::_measure"); /* status register and data as read back from the device */ #pragma pack(push, 1) struct { uint8_t cmd; uint8_t status; - float temperature; + float temperature; float x; float y; float z; @@ -989,9 +870,9 @@ ACCELSIM::measure() _accel_reports->force(&accel_report); /* notify anyone waiting for data */ - poll_notify(POLLIN); + DevMgr::updateNotify(*this); - if (!(_pub_blocked)) { + if (!(m_pub_blocked)) { /* publish it */ // The first call to measure() is from init() and _accel_topic is not @@ -1078,9 +959,9 @@ ACCELSIM::mag_measure() _mag_reports->force(&mag_report); /* notify anyone waiting for data */ - poll_notify(POLLIN); + DevMgr::updateNotify(*this); - if (!(_pub_blocked)) { + if (!(m_pub_blocked)) { /* publish it */ orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report); } @@ -1092,67 +973,59 @@ ACCELSIM::mag_measure() } ACCELSIM_mag::ACCELSIM_mag(ACCELSIM *parent) : - VDev("ACCELSIM_mag", ACCELSIM_DEVICE_PATH_MAG), + VirtDevObj("ACCELSIM_mag", ACCELSIM_DEVICE_PATH_MAG, MAG_BASE_DEVICE_PATH, 10000), _parent(parent), _mag_topic(nullptr), _mag_orb_class_instance(-1), _mag_class_instance(-1) { + m_id.dev_id_s.bus = 1; + m_id.dev_id_s.devtype = DRV_ACC_DEVTYPE_ACCELSIM; } ACCELSIM_mag::~ACCELSIM_mag() { - if (_mag_class_instance != -1) { - unregister_class_devname(MAG_BASE_DEVICE_PATH, _mag_class_instance); - } -} - -int -ACCELSIM_mag::init() -{ - int ret; - - ret = VDev::init(); - - if (ret != OK) { - goto out; - } - - _mag_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH); - -out: - return ret; } ssize_t -ACCELSIM_mag::read(device::file_t *filp, char *buffer, size_t buflen) +ACCELSIM_mag::devRead(void *buffer, size_t buflen) { - return _parent->mag_read(filp, buffer, buflen); + return _parent->mag_read(buffer, buflen); } int -ACCELSIM_mag::ioctl(device::file_t *filp, int cmd, unsigned long arg) +ACCELSIM_mag::devIOCTL(unsigned long cmd, unsigned long arg) { + int ret; + switch (cmd) { case DEVIOCGDEVICEID: - return (int)VDev::ioctl(filp, cmd, arg); + ret = (int)VirtDevObj::devIOCTL(cmd, arg); + //PX4_WARN("DEVICE ID: %d", ret); + return ret; break; default: - return _parent->mag_ioctl(filp, cmd, arg); + return _parent->mag_ioctl(cmd, arg); } } -void -ACCELSIM_mag::measure() +int ACCELSIM_mag::start() { - _parent->mag_measure(); + //PX4_INFO("ACCELSIM_mag::start"); + return VirtDevObj::start(); } -void -ACCELSIM_mag::measure_trampoline(void *arg) +int ACCELSIM_mag::stop() { - _parent->mag_measure_trampoline(arg); + //PX4_INFO("ACCELSIM_mag::stop"); + return VirtDevObj::stop(); +} + +void ACCELSIM_mag::_measure() +{ + //PX4_INFO("ACCELSIM_mag::_measure"); + _parent->mag_measure(); } /** @@ -1176,13 +1049,14 @@ void usage(); int start(enum Rotation rotation) { - int fd, fd_mag; - if (g_dev != nullptr) { PX4_WARN("already started"); return 0; } + DevHandle h; + DevHandle h_mag; + /* create the driver */ g_dev = new ACCELSIM(ACCELSIM_DEVICE_PATH_ACCEL, rotation); @@ -1197,33 +1071,33 @@ start(enum Rotation rotation) } /* set the poll rate to default, starts automatic data collection */ - fd = px4_open(ACCELSIM_DEVICE_PATH_ACCEL, O_RDONLY); + DevMgr::getHandle(ACCELSIM_DEVICE_PATH_ACCEL, h); - if (fd < 0) { + if (!h.isValid()) { PX4_WARN("open %s failed", ACCELSIM_DEVICE_PATH_ACCEL); goto fail; } - if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + if (h.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); - px4_close(fd); + DevMgr::releaseHandle(h); goto fail; } - fd_mag = px4_open(ACCELSIM_DEVICE_PATH_MAG, O_RDONLY); + DevMgr::getHandle(ACCELSIM_DEVICE_PATH_MAG, h_mag); /* don't fail if mag dev cannot be opened */ - if (0 <= fd_mag) { - if (px4_ioctl(fd_mag, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); + if (h_mag.isValid()) { + if (h_mag.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_MAG); } } else { - PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); + PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_MAG); } - px4_close(fd); - px4_close(fd_mag); + DevMgr::releaseHandle(h); + DevMgr::releaseHandle(h_mag); return 0; fail: @@ -1250,6 +1124,9 @@ info() PX4_DEBUG("state @ %p", g_dev); + unsigned dummy = 0; + PX4_WARN("device_id: %u", (unsigned int)g_dev->devIOCTL(DEVIOCGDEVICEID, dummy)); + return 0; } @@ -1285,6 +1162,11 @@ accelsim_main(int argc, char *argv[]) } } + if (argc <= 1) { + accelsim::usage(); + return 1; + } + const char *verb = argv[myoptind]; /* diff --git a/src/platforms/posix/drivers/accelsim/module.mk b/src/platforms/posix/drivers/accelsim/module.mk deleted file mode 100644 index dc980c5cc8..0000000000 --- a/src/platforms/posix/drivers/accelsim/module.mk +++ /dev/null @@ -1,7 +0,0 @@ -# -# Simulated accel/mag driver -# - -MODULE_COMMAND = accelsim -SRCS = accelsim.cpp - diff --git a/src/platforms/posix/drivers/adcsim/CMakeLists.txt b/src/platforms/posix/drivers/adcsim/CMakeLists.txt index 93cdd95a0f..c1c925c15c 100644 --- a/src/platforms/posix/drivers/adcsim/CMakeLists.txt +++ b/src/platforms/posix/drivers/adcsim/CMakeLists.txt @@ -39,5 +39,12 @@ px4_add_module( adcsim.cpp DEPENDS platforms__common + git_driverframework + df_driver_framework ) + +target_link_libraries(platforms__posix__drivers__adcsim + df_driver_framework +) + # vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp index c72cc1a494..5cd9fff61f 100644 --- a/src/platforms/posix/drivers/adcsim/adcsim.cpp +++ b/src/platforms/posix/drivers/adcsim/adcsim.cpp @@ -65,36 +65,29 @@ #include +#include "SyncObj.hpp" +#include "VirtDevObj.hpp" -class ADCSIM : public device::VDev +using namespace DriverFramework; + +#define ADC_BASE_DEV_PATH "/dev/adc" + +class ADCSIM : public VirtDevObj { public: ADCSIM(uint32_t channels); - ~ADCSIM(); + virtual ~ADCSIM(); - virtual int init(); - - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); - virtual ssize_t read(device::file_t *filp, char *buffer, size_t len); - -protected: - virtual int open_first(device::file_t *filp); - virtual int close_last(device::file_t *filp); + virtual ssize_t devRead(void *buffer, size_t len); private: - static const hrt_abstime _tickrate = 10000; /**< 100Hz base rate */ - - hrt_call _call; perf_counter_t _sample_perf; unsigned _channel_count; adc_msg_s *_samples; /**< sample buffer */ - /** work trampoline */ - static void _tick_trampoline(void *arg); - /** worker function */ - void _tick(); + virtual void _measure(); /** * Sample a single channel and return the measured value. @@ -105,13 +98,11 @@ private: */ uint16_t _sample(unsigned channel); - // update system_power ORB topic, only on FMUv2 - void update_system_power(void); + SyncObj m_lock; }; ADCSIM::ADCSIM(uint32_t channels) : - VDev("adcsim", ADCSIM0_DEVICE_PATH), - _call(), + VirtDevObj("adcsim", "/dev/adcsim", ADC_BASE_DEV_PATH, 10000), _sample_perf(perf_alloc(PC_ELAPSED, "adc_samples")), _channel_count(0), _samples(nullptr) @@ -151,23 +142,8 @@ ADCSIM::~ADCSIM() } } -int -ADCSIM::init() -{ - DEVICE_DEBUG("init done"); - - /* create the device node */ - return VDev::init(); -} - -int -ADCSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) -{ - return -ENOTTY; -} - ssize_t -ADCSIM::read(device::file_t *filp, char *buffer, size_t len) +ADCSIM::devRead(void *buffer, size_t len) { const size_t maxsize = sizeof(adc_msg_s) * _channel_count; @@ -176,50 +152,24 @@ ADCSIM::read(device::file_t *filp, char *buffer, size_t len) } /* block interrupts while copying samples to avoid racing with an update */ + m_lock.lock(); memcpy(buffer, _samples, len); + m_lock.unlock(); return len; } -int -ADCSIM::open_first(device::file_t *filp) -{ - /* get fresh data */ - _tick(); - - /* and schedule regular updates */ - hrt_call_every(&_call, _tickrate, _tickrate, _tick_trampoline, this); - - return 0; -} - -int -ADCSIM::close_last(device::file_t *filp) -{ - hrt_cancel(&_call); - return 0; -} - void -ADCSIM::_tick_trampoline(void *arg) +ADCSIM::_measure() { - (reinterpret_cast(arg))->_tick(); -} + m_lock.lock(); -void -ADCSIM::_tick() -{ /* scan the channel set and sample each */ for (unsigned i = 0; i < _channel_count; i++) { _samples[i].am_data = _sample(_samples[i].am_channel); } - update_system_power(); -} - -void -ADCSIM::update_system_power(void) -{ + m_lock.unlock(); } uint16_t @@ -245,20 +195,20 @@ ADCSIM *g_adc; int test(void) { + DevHandle h; + DevMgr::getHandle(ADCSIM0_DEVICE_PATH, h); - int fd = px4_open(ADCSIM0_DEVICE_PATH, O_RDONLY); - - if (fd < 0) { - PX4_ERR("can't open ADCSIM device"); + if (!h.isValid()) { + PX4_ERR("can't open ADCSIM device (%d)", h.getError()); return 1; } for (unsigned i = 0; i < 50; i++) { adc_msg_s data[12]; - ssize_t count = px4_read(fd, data, sizeof(data)); + ssize_t count = h.read(data, sizeof(data)); if (count < 0) { - PX4_ERR("read error"); + PX4_ERR("read error (%d)", h.getError()); return 1; } @@ -271,7 +221,7 @@ test(void) usleep(500000); } - px4_close(fd); + DevMgr::releaseHandle(h); return 0; } } @@ -290,14 +240,15 @@ adcsim_main(int argc, char *argv[]) return 1; } - if (g_adc->init() != OK) { - delete g_adc; - PX4_ERR("ADCSIM init failed"); + ret = g_adc->init(); + + if (ret != 0) { + PX4_ERR("ADCSIM init failed (%d)", ret); return 1; } } - if (argc > 1) { + if (argc > 1 && g_adc) { if (!strcmp(argv[1], "test")) { ret = test(); } diff --git a/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp b/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp index 89691f7483..ae43a2cd5d 100644 --- a/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp +++ b/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp @@ -291,7 +291,7 @@ MEASAirspeedSim::cycle() void MEASAirspeedSim::voltage_correction(float &diff_press_pa, float &temperature) { -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) if (_t_system_power == -1) { _t_system_power = orb_subscribe(ORB_ID(system_power)); diff --git a/src/platforms/posix/drivers/barosim/CMakeLists.txt b/src/platforms/posix/drivers/barosim/CMakeLists.txt index 15963f87eb..6e3a105204 100644 --- a/src/platforms/posix/drivers/barosim/CMakeLists.txt +++ b/src/platforms/posix/drivers/barosim/CMakeLists.txt @@ -37,7 +37,6 @@ px4_add_module( -Os SRCS baro.cpp - baro_sim.cpp DEPENDS platforms__common ) diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index 975fa3687c..2485189487 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -48,13 +48,11 @@ #include #include #include -#include #include #include #include #include -#include #include #include @@ -63,14 +61,13 @@ #include #include +#include + #include #include #include "barosim.h" - -enum BAROSIM_BUS { - BAROSIM_BUS_SIM_EXTERNAL -}; +#include "VirtDevObj.hpp" /* helper macro for handling report buffer indices */ #define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) @@ -78,6 +75,7 @@ enum BAROSIM_BUS { /* helper macro for arithmetic - returns the square of the argument */ #define POW2(_x) ((_x) * (_x)) +#define BAROSIM_DEV_PATH "/dev/barosim" /* * BAROSIM internal constants and data structures. */ @@ -86,16 +84,18 @@ enum BAROSIM_BUS { #define BAROSIM_CONVERSION_INTERVAL 10000 /* microseconds */ #define BAROSIM_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ -class BAROSIM : public device::VDev +using namespace DriverFramework; + +class BAROSIM : public VirtDevObj { public: - BAROSIM(device::Device *interface, barosim::prom_u &prom_buf, const char *path); + BAROSIM(const char *path); ~BAROSIM(); virtual int init(); - virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); + virtual ssize_t devRead(void *buffer, size_t buflen); + virtual int devIOCTL(unsigned long cmd, unsigned long arg); /** * Diagnostics - print some basic information about the driver. @@ -103,12 +103,6 @@ public: void print_info(); protected: - Device *_interface; - - barosim::prom_s _prom; - - struct work_s _work; - unsigned _measure_ticks; ringbuffer::RingBuffer *_reports; @@ -127,7 +121,6 @@ protected: orb_advert_t _baro_topic; int _orb_class_instance; - int _class_instance; perf_counter_t _sample_perf; perf_counter_t _measure_perf; @@ -162,6 +155,8 @@ protected: */ void cycle(); + int transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len); + /** * Get the internal / external state * @@ -182,7 +177,10 @@ protected: * * @return OK if the measurement command was successful. */ - virtual int measure(); + int measure(); + + // Unused + virtual void _measure(); /** * Collect the result of the most recent measurement. @@ -190,33 +188,32 @@ protected: virtual int collect(); }; +static BAROSIM *g_barosim = nullptr; + /* * Driver 'main' command. */ extern "C" __EXPORT int barosim_main(int argc, char *argv[]); -BAROSIM::BAROSIM(device::Device *interface, barosim::prom_u &prom_buf, const char *path) : - VDev("BAROSIM", path), - _interface(interface), - _prom(prom_buf.s), - _measure_ticks(0), +BAROSIM::BAROSIM(const char *path) : + VirtDevObj("BAROSIM", path, BARO_BASE_DEVICE_PATH, 1e6 / 100), _reports(nullptr), _collect_phase(false), _measure_phase(0), _TEMP(0), _OFF(0), _SENS(0), + _P(0.0), + _T(0.0), _msl_pressure(101325), _baro_topic(nullptr), _orb_class_instance(-1), - _class_instance(-1), _sample_perf(perf_alloc(PC_ELAPSED, "barosim_read")), _measure_perf(perf_alloc(PC_ELAPSED, "barosim_measure")), _comms_errors(perf_alloc(PC_COUNT, "barosim_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "barosim_buffer_overflows")) { - // work_cancel in stop_cycle called from the dtor will explode if we don't do this... - memset(&_work, 0, sizeof(_work)); + } BAROSIM::~BAROSIM() @@ -224,10 +221,6 @@ BAROSIM::~BAROSIM() /* make sure we are truly inactive */ stop_cycle(); - if (_class_instance != -1) { - unregister_class_devname(get_devname(), _class_instance); - } - /* free any existing reports */ if (_reports != nullptr) { delete _reports; @@ -239,19 +232,19 @@ BAROSIM::~BAROSIM() perf_free(_comms_errors); perf_free(_buffer_overflows); - delete _interface; } int BAROSIM::init() { int ret; - DEVICE_DEBUG("BAROSIM::init"); + struct baro_report brp = {}; + //PX4_DEBUG("BAROSIM::init"); - ret = VDev::init(); + ret = VirtDevObj::init(); if (ret != OK) { - DEVICE_DEBUG("VDev init failed"); + PX4_ERR("VirtDevObj init failed"); goto out; } @@ -259,17 +252,14 @@ BAROSIM::init() _reports = new ringbuffer::RingBuffer(2, sizeof(baro_report)); if (_reports == nullptr) { - DEVICE_DEBUG("can't get memory for reports"); + PX4_ERR("can't get memory for reports"); ret = -ENOMEM; goto out; } - /* register alternate interfaces if we have to */ - _class_instance = register_class_devname(BARO_BASE_DEVICE_PATH); - - struct baro_report brp; /* do a first measurement cycle to populate reports with valid data */ _measure_phase = 0; + _reports->flush(); _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp, @@ -325,7 +315,7 @@ out: } ssize_t -BAROSIM::read(device::file_t *filp, char *buffer, size_t buflen) +BAROSIM::devRead(void *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct baro_report); struct baro_report *brp = reinterpret_cast(buffer); @@ -337,7 +327,7 @@ BAROSIM::read(device::file_t *filp, char *buffer, size_t buflen) } /* if automatic measurement is enabled */ - if (_measure_ticks > 0) { + if (m_sample_interval_usecs > 0) { /* * While there is space in the caller's buffer, and reports, copy them. @@ -397,8 +387,10 @@ BAROSIM::read(device::file_t *filp, char *buffer, size_t buflen) } int -BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) +BAROSIM::devIOCTL(unsigned long cmd, unsigned long arg) { + PX4_WARN("baro IOCTL %llu", hrt_absolute_time()); + switch (cmd) { case SENSORIOCSPOLLRATE: @@ -407,7 +399,8 @@ BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop_cycle(); - _measure_ticks = 0; + setSampleInterval(0); + m_sample_interval_usecs = 0; return OK; /* external signalling not supported */ @@ -420,39 +413,27 @@ BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) /* 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(BAROSIM_CONVERSION_INTERVAL); - - /* if we need to start the poll state machine, do it */ - if (want_start) { - start_cycle(); - } - + setSampleInterval(BAROSIM_CONVERSION_INTERVAL); return OK; } /* adjust to a legal polling interval in Hz */ default: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); - /* convert hz to tick interval via microseconds */ - unsigned long ticks = USEC2TICK(1000000 / arg); + unsigned long interval = 1000000 / arg; /* check against maximum rate */ - if (ticks < USEC2TICK(BAROSIM_CONVERSION_INTERVAL)) { + if (interval < BAROSIM_CONVERSION_INTERVAL) { return -EINVAL; } - /* update interval for next measurement */ - _measure_ticks = ticks; + bool want_start = (m_sample_interval_usecs == 0); + + /* update interval for next measurement */ + setSampleInterval(interval); - /* if we need to start the poll state machine, do it */ if (want_start) { - start_cycle(); + start(); } return OK; @@ -460,11 +441,11 @@ BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) { + if (m_sample_interval_usecs == 0) { return SENSOR_POLLRATE_MANUAL; } - return (1000 / _measure_ticks); + return (1000000 / m_sample_interval_usecs); case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ @@ -508,7 +489,7 @@ BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) /* give it to the bus-specific superclass */ // return bus_ioctl(filp, cmd, arg); - return VDev::ioctl(filp, cmd, arg); + return VirtDevObj::devIOCTL(cmd, arg); } void @@ -521,28 +502,29 @@ BAROSIM::start_cycle() _reports->flush(); /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&BAROSIM::cycle_trampoline, this, 1); + setSampleInterval(1000); + start(); } void BAROSIM::stop_cycle() { - work_cancel(HPWORK, &_work); + stop(); + setSampleInterval(0); } void -BAROSIM::cycle_trampoline(void *arg) +BAROSIM::_measure() { - BAROSIM *dev = reinterpret_cast(arg); - - dev->cycle(); + cycle(); } void BAROSIM::cycle() { int ret; - unsigned dummy; + + //PX4_WARN("baro cycle %llu", hrt_absolute_time()); /* collection phase? */ if (_collect_phase) { @@ -551,10 +533,14 @@ BAROSIM::cycle() ret = collect(); if (ret != OK) { - /* issue a reset command to the sensor */ - _interface->dev_ioctl(IOCTL_RESET, dummy); + uint8_t cmd = ADDR_RESET_CMD; + + /* bump the retry count */ + (void)transfer(&cmd, 1, nullptr, 0); + /* reset the collection state machine and try again */ - start_cycle(); + //start_cycle(); + return; } @@ -566,15 +552,9 @@ BAROSIM::cycle() * Don't inject one after temperature measurements, so we can keep * doing pressure measurements at something close to the desired rate. */ - if ((_measure_phase != 0) && - (_measure_ticks > USEC2TICK(BAROSIM_CONVERSION_INTERVAL))) { + if (_measure_phase != 0) { - /* schedule a fresh cycle call when we are ready to measure again */ - work_queue(HPWORK, - &_work, - (worker_t)&BAROSIM::cycle_trampoline, - this, - _measure_ticks - USEC2TICK(BAROSIM_CONVERSION_INTERVAL)); + //setSampleInterval(BAROSIM_CONVERSION_INTERVAL); return; } @@ -586,21 +566,17 @@ BAROSIM::cycle() if (ret != OK) { //DEVICE_LOG("measure error %d", ret); /* issue a reset command to the sensor */ - _interface->dev_ioctl(IOCTL_RESET, dummy); + //_interface->devIOCTL(IOCTL_RESET, dummy); + /* reset the collection state machine and try again */ - start_cycle(); + //start_cycle(); return; } /* next phase is collection */ _collect_phase = true; - /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&BAROSIM::cycle_trampoline, - this, - USEC2TICK(BAROSIM_CONVERSION_INTERVAL)); + //setSampleInterval(BAROSIM_CONVERSION_INTERVAL); } int @@ -608,17 +584,20 @@ BAROSIM::measure() { int ret; + //PX4_WARN("baro measure %llu", hrt_absolute_time()); + perf_begin(_measure_perf); /* * In phase zero, request temperature; in other phases, request pressure. */ - unsigned addr = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1; + unsigned long addr = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1; /* * Send the command to begin measuring. */ - ret = _interface->dev_ioctl(IOCTL_MEASURE, addr); + uint8_t cmd = addr; + ret = transfer(&cmd, 1, nullptr, 0); if (OK != ret) { perf_count(_comms_errors); @@ -629,28 +608,68 @@ BAROSIM::measure() return ret; } +int +BAROSIM::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) +{ + if (send_len == 1 && send[0] == ADDR_RESET_CMD) { + /* reset command */ + return 0; + + } else if (send_len == 1 && (send[0] == ADDR_CMD_CONVERT_D2 || send[0] == ADDR_CMD_CONVERT_D1)) { + /* measure command */ + if (send[0] == ADDR_CMD_CONVERT_D2) { + } else { + } + + return 0; + + } else if (send[0] == 0 && send_len == 1) { + /* read requested */ + Simulator *sim = Simulator::getInstance(); + + if (sim == NULL) { + PX4_ERR("Error BAROSIM_DEV::transfer no simulator"); + return -ENODEV; + } + + PX4_DEBUG("BAROSIM_DEV::transfer getting sample"); + sim->getBaroSample(recv, recv_len); + return recv_len; + + } else { + PX4_WARN("BAROSIM_DEV::transfer invalid param %u %u %u", send_len, send[0], recv_len); + return 1; + } + + + return 0; +} + int BAROSIM::collect() { + //PX4_WARN("baro collect %llu", hrt_absolute_time()); + int ret; #pragma pack(push, 1) - struct { + struct raw_baro_s { float pressure; float altitude; float temperature; - } baro_report; + } raw_baro; #pragma pack(pop) perf_begin(_sample_perf); - struct baro_report report; + struct baro_report report = {}; /* this should be fairly close to the end of the conversion, so the best approximation of the time */ report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); /* read the most recent measurement - read offset/size are hardcoded in the interface */ - ret = _interface->dev_read(0, (void *)&baro_report, sizeof(baro_report)); + uint8_t cmd = 0; + ret = transfer(&cmd, 1, (uint8_t *)(&raw_baro), sizeof(raw_baro)); if (ret < 0) { perf_count(_comms_errors); @@ -660,19 +679,17 @@ BAROSIM::collect() /* handle a measurement */ if (_measure_phase == 0) { - report.pressure = baro_report.pressure; - report.altitude = baro_report.altitude; - report.temperature = baro_report.temperature; - report.timestamp = hrt_absolute_time(); + report.pressure = raw_baro.pressure; + report.altitude = raw_baro.altitude; + report.temperature = raw_baro.temperature; } else { - report.pressure = baro_report.pressure; - report.altitude = baro_report.altitude; - report.temperature = baro_report.temperature; - report.timestamp = hrt_absolute_time(); + report.pressure = raw_baro.pressure; + report.altitude = raw_baro.altitude; + report.temperature = raw_baro.temperature; /* publish it */ - if (!(_pub_blocked)) { + if (!(m_pub_blocked)) { if (_baro_topic != nullptr) { /* publish it */ orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); @@ -687,7 +704,8 @@ BAROSIM::collect() } /* notify anyone waiting for data */ - poll_notify(POLLIN); + //DevMgr::updateNotify(*this); + updateNotify(); } /* update the measurement state machine */ @@ -704,7 +722,7 @@ BAROSIM::print_info() perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); - PX4_INFO("poll interval: %u ticks", _measure_ticks); + PX4_INFO("poll interval: %u usec", m_sample_interval_usecs); _reports->print_info("report queue"); PX4_INFO("TEMP: %ld", (long)_TEMP); PX4_INFO("SENS: %lld", (long long)_SENS); @@ -712,45 +730,11 @@ BAROSIM::print_info() PX4_INFO("P: %.3f", (double)_P); PX4_INFO("T: %.3f", (double)_T); PX4_INFO("MSL pressure: %10.4f", (double)(_msl_pressure / 100.f)); - - PX4_INFO("factory_setup %u", _prom.factory_setup); - PX4_INFO("c1_pressure_sens %u", _prom.c1_pressure_sens); - PX4_INFO("c2_pressure_offset %u", _prom.c2_pressure_offset); - PX4_INFO("c3_temp_coeff_pres_sens %u", _prom.c3_temp_coeff_pres_sens); - PX4_INFO("c4_temp_coeff_pres_offset %u", _prom.c4_temp_coeff_pres_offset); - PX4_INFO("c5_reference_temp %u", _prom.c5_reference_temp); - PX4_INFO("c6_temp_coeff_temp %u", _prom.c6_temp_coeff_temp); - PX4_INFO("serial_and_crc %u", _prom.serial_and_crc); } -/** - * Local functions in support of the shell command. - */ namespace barosim { -/* - list of supported bus configurations - */ -struct barosim_bus_option { - enum BAROSIM_BUS busid; - const char *devpath; - BAROSIM_constructor interface_constructor; - uint8_t busnum; - BAROSIM *dev; -} bus_options[] = { - { BAROSIM_BUS_SIM_EXTERNAL, "/dev/baro_sim", &BAROSIM_sim_interface, PX4_SIM_BUS_TEST, NULL }, -}; -#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) - -bool start_bus(struct barosim_bus_option &bus); -int start(); -int test(); -int reset(); -int info(); -int calibrate(unsigned altitude); -void usage(); - /** * BAROSIM crc4 cribbed from the datasheet */ @@ -797,75 +781,41 @@ crc4(uint16_t *n_prom) return (0x000F & crc_read) == (n_rem ^ 0x00); } - -/** - * Start the driver. - */ -bool -start_bus(struct barosim_bus_option &bus) -{ - if (bus.dev != nullptr) { - PX4_ERR("bus option already started"); - return false; - } - - prom_u prom_buf; - device::Device *interface = bus.interface_constructor(prom_buf, bus.busnum); - - if (interface->init() != OK) { - delete interface; - PX4_ERR("no device on bus %u", (unsigned)bus.busid); - return false; - } - - 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 */ - if (fd == -1) { - 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"); - return false; - } - - px4_close(fd); - return true; -} - - /** * Start the driver. * * This function call only returns once the driver * is either successfully up and running or failed to start. */ -int +static int start() { - bool started = false; + g_barosim = new BAROSIM(BAROSIM_DEV_PATH); - started |= start_bus(bus_options[0]); - - if (!started) { - PX4_ERR("driver start failed"); - return 1; + if (g_barosim != nullptr && OK != g_barosim->init()) { + delete g_barosim; + g_barosim = NULL; + PX4_ERR("bus init failed"); + return false; } - // driver started OK - return 0; + DevHandle h; + DevMgr::getHandle(BAROSIM_DEV_PATH, h); + + /* set the poll rate to default, starts automatic data collection */ + if (!h.isValid()) { + PX4_ERR("can't open baro device"); + return false; + } + + if (h.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + DevMgr::releaseHandle(h); + PX4_ERR("failed setting default poll rate"); + return false; + } + + DevMgr::releaseHandle(h); + return true; } @@ -874,25 +824,23 @@ start() * make sure we can collect data from the sensor in polled * and automatic modes. */ -int +static int test() { - struct barosim_bus_option &bus = bus_options[0]; struct baro_report report; ssize_t sz; int ret; - int fd; + DevHandle h; + DevMgr::getHandle(BAROSIM_DEV_PATH, h); - fd = px4_open(bus.devpath, O_RDONLY); - - if (fd < 0) { - PX4_ERR("open failed (try 'barosim start' if the driver is not running)"); + if (!h.isValid()) { + PX4_ERR("getHandle failed (try 'barosim start' if the driver is not running)"); return 1; } /* do a simple demand read */ - sz = px4_read(fd, &report, sizeof(report)); + sz = h.read(&report, sizeof(report)); if (sz != sizeof(report)) { PX4_ERR("immediate read failed"); @@ -906,38 +854,37 @@ test() PX4_INFO("time: %lld", (long long)report.timestamp); /* set the queue depth to 10 */ - if (OK != px4_ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) { + if (OK != h.ioctl(SENSORIOCSQUEUEDEPTH, 10UL)) { PX4_ERR("failed to set queue depth"); - px4_close(fd); + DevMgr::releaseHandle(h); return 1; } /* start the sensor polling at 2Hz */ - if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) { + if (OK != h.ioctl(SENSORIOCSPOLLRATE, 2UL)) { PX4_ERR("failed to set 2Hz poll rate"); - px4_close(fd); + DevMgr::releaseHandle(h); return 1; } /* read the sensor 5x and report each value */ for (unsigned i = 0; i < 5; i++) { - px4_pollfd_struct_t fds; + UpdateList in_set, out_set; + in_set.pushBack(&h); /* wait for data to be ready */ - fds.fd = fd; - fds.events = POLLIN; - ret = px4_poll(&fds, 1, 2000); + ret = DevMgr::waitForUpdate(in_set, out_set, 1000); if (ret != 1) { PX4_WARN("timed out waiting for sensor data"); } /* now go get it */ - sz = px4_read(fd, &report, sizeof(report)); + sz = h.read(&report, sizeof(report)); if (sz != sizeof(report)) { PX4_ERR("periodic read failed"); - px4_close(fd); + DevMgr::releaseHandle(h); return 1; } @@ -948,7 +895,7 @@ test() PX4_INFO("time: %lld", (long long)report.timestamp); } - px4_close(fd); + DevMgr::releaseHandle(h); PX4_INFO("PASS"); return 0; } @@ -956,25 +903,23 @@ test() /** * Reset the driver. */ -int +static int reset() { - struct barosim_bus_option &bus = bus_options[0]; - int fd; + DevHandle h; + DevMgr::getHandle(BAROSIM_DEV_PATH, h); - fd = px4_open(bus.devpath, O_RDONLY); - - if (fd < 0) { + if (!h.isValid()) { PX4_ERR("failed "); return 1; } - if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) { + if (h.ioctl(SENSORIOCRESET, 0) < 0) { PX4_ERR("driver reset failed"); return 1; } - if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + if (h.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { PX4_ERR("driver poll restart failed"); return 1; } @@ -985,16 +930,12 @@ reset() /** * Print a little info about the driver. */ -int +static int info() { - for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) { - struct barosim_bus_option &bus = bus_options[i]; - - if (bus.dev != nullptr) { - PX4_INFO("%s", bus.devpath); - bus.dev->print_info(); - } + if (g_barosim != nullptr) { + PX4_INFO("%s", BAROSIM_DEV_PATH); + g_barosim->print_info(); } return 0; @@ -1003,25 +944,23 @@ info() /** * Calculate actual MSL pressure given current altitude */ -int +static int calibrate(unsigned altitude) { - struct barosim_bus_option &bus = bus_options[0]; struct baro_report report; - float pressure; - float p1; + float pressure; + float p1; - int fd; + DevHandle h; + DevMgr::getHandle(BAROSIM_DEV_PATH, h); - fd = px4_open(bus.devpath, O_RDONLY); - - if (fd < 0) { + if (!h.isValid()) { PX4_ERR("open failed (try 'barosim start' if the driver is not running)"); return 1; } /* start the sensor polling at max */ - if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX)) { + if (OK != h.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX)) { PX4_ERR("failed to set poll rate"); return 1; } @@ -1030,14 +969,13 @@ calibrate(unsigned altitude) pressure = 0.0f; for (unsigned i = 0; i < 20; i++) { - px4_pollfd_struct_t fds; int ret; ssize_t sz; + UpdateList in_set, out_set; + in_set.pushBack(&h); /* wait for data to be ready */ - fds.fd = fd; - fds.events = POLLIN; - ret = px4_poll(&fds, 1, 1000); + ret = DevMgr::waitForUpdate(in_set, out_set, 1000); if (ret != 1) { PX4_ERR("timed out waiting for sensor data"); @@ -1045,7 +983,7 @@ calibrate(unsigned altitude) } /* now go get it */ - sz = px4_read(fd, &report, sizeof(report)); + sz = h.read(&report, sizeof(report)); if (sz != sizeof(report)) { PX4_ERR("sensor read failed"); @@ -1073,22 +1011,22 @@ calibrate(unsigned altitude) /* save as integer Pa */ p1 *= 1000.0f; - if (px4_ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK) { + if (h.ioctl(BAROIOCSMSLPRESSURE, (unsigned long)(p1)) != OK) { PX4_WARN("BAROIOCSMSLPRESSURE"); return 1; } - px4_close(fd); + DevMgr::releaseHandle(h); return 0; } -void +static void usage() { PX4_WARN("missing command: try 'start', 'info', 'test', 'test2', 'reset', 'calibrate '"); } -} // namespace +}; // namespace barosim int barosim_main(int argc, char *argv[]) diff --git a/src/platforms/posix/drivers/barosim/baro_sim.cpp b/src/platforms/posix/drivers/barosim/baro_sim.cpp deleted file mode 100644 index 252d1428c9..0000000000 --- a/src/platforms/posix/drivers/barosim/baro_sim.cpp +++ /dev/null @@ -1,228 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file baro_sim.cpp - * - * Simulation interface for barometer - */ - -/* XXX trim includes */ -#include -#include - -#include -#include -#include -#include -//#include -#include -#include - -#include -#include -#include "barosim.h" -#include "board_config.h" - -device::Device *BAROSIM_sim_interface(barosim::prom_u &prom_buf); - -class BARO_SIM : public device::SIM -{ -public: - BARO_SIM(uint8_t bus, barosim::prom_u &prom_buf); - virtual ~BARO_SIM(); - - virtual int init(); - virtual int dev_read(unsigned offset, void *data, unsigned count); - virtual int dev_ioctl(unsigned operation, unsigned &arg); - - virtual int transfer(const uint8_t *send, unsigned send_len, - uint8_t *recv, unsigned recv_len); -private: - //barosim::prom_u &_prom; - - /** - * Send a reset command to the barometer simulator. - * - * This is required after any bus reset. - */ - int _reset(); - - /** - * Send a measure command to the barometer simulator. - * - * @param addr Which address to use for the measure operation. - */ - int _measure(unsigned addr); - - /** - * Read the MS5611 PROM - * - * @return PX4_OK if the PROM reads successfully. - */ - int _read_prom(); - -}; - -device::Device * -BAROSIM_sim_interface(barosim::prom_u &prom_buf, uint8_t busnum) -{ - return new BARO_SIM(busnum, prom_buf); -} - -BARO_SIM::BARO_SIM(uint8_t bus, barosim::prom_u &prom) : - SIM("BARO_SIM", "/dev/BARO_SIM", bus, 0) -{ -} - -BARO_SIM::~BARO_SIM() -{ -} - -int -BARO_SIM::init() -{ - return SIM::init(); -} - -int -BARO_SIM::dev_read(unsigned offset, void *data, unsigned count) -{ - /* - union _cvt { - uint8_t b[4]; - uint32_t w; - } *cvt = (_cvt *)data; - */ - - /* read the most recent measurement */ - uint8_t cmd = 0; - int ret = transfer(&cmd, 1, static_cast(data), count); - - /* - if (ret == PX4_OK) { - // fetch the raw value - cvt->b[0] = buf[2]; - cvt->b[1] = buf[1]; - cvt->b[2] = buf[0]; - cvt->b[3] = 0; - } - */ - - return ret; -} - -int -BARO_SIM::dev_ioctl(unsigned operation, unsigned &arg) -{ - int ret; - - switch (operation) { - case IOCTL_RESET: - ret = _reset(); - break; - - case IOCTL_MEASURE: - ret = _measure(arg); - break; - - default: - ret = EINVAL; - } - - return ret; -} - -int -BARO_SIM::_reset() -{ - unsigned old_retrycount = _retries; - uint8_t cmd = ADDR_RESET_CMD; - int result; - - /* bump the retry count */ - _retries = 10; - result = transfer(&cmd, 1, nullptr, 0); - _retries = old_retrycount; - - return result; -} - -int -BARO_SIM::_measure(unsigned addr) -{ - /* - * Disable retries on this command; we can't know whether failure - * means the device did or did not see the command. - */ - _retries = 0; - - uint8_t cmd = addr; - return transfer(&cmd, 1, nullptr, 0); -} - -int -BARO_SIM::_read_prom() -{ - int ret = 1; - // TODO input simlation data - return ret; -} - -int -BARO_SIM::transfer(const uint8_t *send, unsigned send_len, - uint8_t *recv, unsigned recv_len) -{ - // TODO add Simulation data connection so calls retrieve - // data from the simulator - if (recv_len == 0) { - PX4_DEBUG("BARO_SIM measurement requested"); - - } 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 { - 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/barosim/barosim.h b/src/platforms/posix/drivers/barosim/barosim.h index bac176d400..b029dd9d09 100644 --- a/src/platforms/posix/drivers/barosim/barosim.h +++ b/src/platforms/posix/drivers/barosim/barosim.h @@ -36,13 +36,14 @@ * * Shared defines for the ms5611 driver. */ +#include "VirtDevObj.hpp" -#define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */ +#define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */ #define ADDR_CMD_CONVERT_D1 0x48 /* write to this address to start pressure conversion */ #define ADDR_CMD_CONVERT_D2 0x58 /* write to this address to start temperature conversion */ -#define ADDR_DATA 0x00 /* address of 3 bytes / 32bit pressure data */ -#define ADDR_PROM_SETUP 0xA0 /* address of 8x 2 bytes factory and calibration data */ -#define ADDR_PROM_C1 0xA2 /* address of 6x 2 bytes calibration data */ +#define ADDR_DATA 0x00 /* address of 3 bytes / 32bit pressure data */ +#define ADDR_PROM_SETUP 0xA0 /* address of 8x 2 bytes factory and calibration data */ +#define ADDR_PROM_C1 0xA2 /* address of 6x 2 bytes calibration data */ /* interface ioctls */ #define IOCTL_RESET 2 @@ -78,7 +79,3 @@ union prom_u { extern bool crc4(uint16_t *n_prom); } /* namespace */ - -/* interface factories */ -extern device::Device *BAROSIM_sim_interface(barosim::prom_u &prom_buf, uint8_t busnum); -typedef device::Device *(*BAROSIM_constructor)(barosim::prom_u &prom_buf, uint8_t busnum); diff --git a/src/platforms/posix/drivers/gpssim/gpssim.cpp b/src/platforms/posix/drivers/gpssim/gpssim.cpp index 889d793486..afc9d94fb4 100644 --- a/src/platforms/posix/drivers/gpssim/gpssim.cpp +++ b/src/platforms/posix/drivers/gpssim/gpssim.cpp @@ -62,6 +62,11 @@ #include +#include "DevMgr.hpp" +#include "VirtDevObj.hpp" + +using namespace DriverFramework; + #define GPS_DRIVER_MODE_UBX_SIM #define GPSSIM_DEVICE_PATH "/dev/gpssim" @@ -82,7 +87,7 @@ public: }; -class GPSSIM : public device::VDev +class GPSSIM : public VirtDevObj { public: GPSSIM(const char *uart_path, bool fake_gps, bool enable_sat_info); @@ -90,13 +95,16 @@ public: virtual int init(); - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); + virtual int devIOCTL(unsigned long cmd, unsigned long arg); /** * Diagnostics - print some basic information about the driver. */ void print_info(); +protected: + virtual void _measure() {} + private: bool _task_should_exit; ///< flag to make the main worker task exit @@ -115,6 +123,7 @@ private: orb_advert_t _report_sat_info_pub; ///< uORB pub for satellite info float _rate; ///< position update rate bool _fake_gps; ///< fake gps output + SyncObj _sync; /** * Try to configure the GPS, handle outgoing communication to the GPS @@ -160,7 +169,7 @@ GPSSIM *g_dev = nullptr; GPSSIM::GPSSIM(const char *uart_path, bool fake_gps, bool enable_sat_info) : - VDev("gps", GPSSIM_DEVICE_PATH), + VirtDevObj("gps", GPSSIM_DEVICE_PATH, nullptr, 1e6 / 10), _task_should_exit(false), //_healthy(false), //_mode_changed(false), @@ -188,8 +197,6 @@ GPSSIM::GPSSIM(const char *uart_path, bool fake_gps, bool enable_sat_info) : _p_report_sat_info = &_Sat_Info->_data; memset(_p_report_sat_info, 0, sizeof(*_p_report_sat_info)); } - - _debug_enabled = true; } GPSSIM::~GPSSIM() @@ -217,7 +224,7 @@ GPSSIM::init() int ret = ERROR; /* do regular cdev init */ - if (VDev::init() != OK) { + if (VirtDevObj::init() != OK) { goto out; } @@ -236,9 +243,9 @@ out: } int -GPSSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) +GPSSIM::devIOCTL(unsigned long cmd, unsigned long arg) { - lock(); + _sync.lock(); int ret = OK; @@ -249,11 +256,11 @@ GPSSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) default: /* give it to parent if no one wants it */ - ret = VDev::ioctl(filp, cmd, arg); + ret = VirtDevObj::devIOCTL(cmd, arg); break; } - unlock(); + _sync.unlock(); return ret; } @@ -319,7 +326,7 @@ GPSSIM::task_main() //no time and satellite information simulated - if (!(_pub_blocked)) { + if (!(m_pub_blocked)) { if (_report_gps_pos_pub != nullptr) { orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); @@ -339,7 +346,7 @@ GPSSIM::task_main() _report_gps_pos.timestamp_velocity = hrt_absolute_time(); _report_gps_pos.timestamp_time = hrt_absolute_time(); - if (!(_pub_blocked)) { + if (!(m_pub_blocked)) { if (_report_gps_pos_pub != nullptr) { orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); @@ -354,7 +361,7 @@ GPSSIM::task_main() while ((receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) { /* opportunistic publishing - else invalid data would end up on the bus */ - if (!(_pub_blocked)) { + if (!(m_pub_blocked)) { orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); if (_p_report_sat_info) { @@ -368,7 +375,8 @@ GPSSIM::task_main() } } - lock(); + // FIXME - if ioctl is called then it will deadlock + _sync.lock(); } } @@ -440,7 +448,7 @@ void info(); void start(const char *uart_path, bool fake_gps, bool enable_sat_info) { - int fd; + DevHandle h; /* create the driver */ g_dev = new GPSSIM(uart_path, fake_gps, enable_sat_info); @@ -454,10 +462,10 @@ start(const char *uart_path, bool fake_gps, bool enable_sat_info) } /* set the poll rate to default, starts automatic data collection */ - fd = px4_open(GPSSIM_DEVICE_PATH, O_RDONLY); + DevMgr::getHandle(GPSSIM_DEVICE_PATH, h); - if (fd < 0) { - PX4_ERR("open: %s\n", GPS0_DEVICE_PATH); + if (!h.isValid()) { + PX4_ERR("getHandle failed: %s", GPS0_DEVICE_PATH); goto fail; } @@ -500,13 +508,14 @@ test() void reset() { - int fd = px4_open(GPSSIM_DEVICE_PATH, O_RDONLY); + DevHandle h; + DevMgr::getHandle(GPSSIM_DEVICE_PATH, h); - if (fd < 0) { + if (!h.isValid()) { PX4_ERR("failed "); } - if (ioctl(fd, SENSORIOCRESET, 0) < 0) { + if (h.ioctl(SENSORIOCRESET, 0) < 0) { PX4_ERR("reset failed"); } } diff --git a/src/platforms/posix/drivers/gyrosim/CMakeLists.txt b/src/platforms/posix/drivers/gyrosim/CMakeLists.txt index 335bee65d6..a1b42bd11c 100644 --- a/src/platforms/posix/drivers/gyrosim/CMakeLists.txt +++ b/src/platforms/posix/drivers/gyrosim/CMakeLists.txt @@ -40,5 +40,12 @@ px4_add_module( gyrosim.cpp DEPENDS platforms__common + git_driverframework + df_driver_framework ) + +target_link_libraries(platforms__posix__drivers__gyrosim + df_driver_framework +) + # vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 0ac023285c..3b42951cc9 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -71,11 +71,16 @@ #include #include +#include #include #include #include #include +#include "VirtDevObj.hpp" + +using namespace DriverFramework; + #define DIR_READ 0x80 #define DIR_WRITE 0x00 @@ -95,9 +100,9 @@ // Product Name Product Revision #define GYROSIMES_REV_C4 0x14 -#define GYROSIM_ACCEL_DEFAULT_RATE 1000 +#define GYROSIM_ACCEL_DEFAULT_RATE 400 -#define GYROSIM_GYRO_DEFAULT_RATE 1000 +#define GYROSIM_GYRO_DEFAULT_RATE 400 #define GYROSIM_ONE_G 9.80665f @@ -114,16 +119,17 @@ */ class GYROSIM_gyro; -class GYROSIM : public device::VDev +class GYROSIM : public VirtDevObj { public: GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation rotation); virtual ~GYROSIM(); - virtual int init(); + int init(); + virtual int start(); - virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); + virtual ssize_t devRead(void *buffer, size_t buflen); + virtual int devIOCTL(unsigned long cmd, unsigned long arg); int transfer(uint8_t *send, uint8_t *recv, unsigned len); /** @@ -136,14 +142,14 @@ public: protected: friend class GYROSIM_gyro; - virtual ssize_t gyro_read(device::file_t *filp, char *buffer, size_t buflen); - virtual int gyro_ioctl(device::file_t *filp, int cmd, unsigned long arg); + virtual ssize_t gyro_read(void *buffer, size_t buflen); + virtual int gyro_ioctl(unsigned long cmd, unsigned long arg); private: GYROSIM_gyro *_gyro; uint8_t _product; /** product code */ - struct hrt_call _call; + WorkHandle _call; unsigned _call_interval; ringbuffer::RingBuffer *_accel_reports; @@ -161,30 +167,21 @@ private: float _gyro_range_scale; float _gyro_range_rad_s; - unsigned _sample_rate; perf_counter_t _accel_reads; perf_counter_t _gyro_reads; perf_counter_t _sample_perf; perf_counter_t _good_transfers; perf_counter_t _reset_retries; - perf_counter_t _system_latency_perf; perf_counter_t _controller_latency_perf; + Integrator _accel_int; + Integrator _gyro_int; + enum Rotation _rotation; // last temperature reading for print_info() float _last_temperature; - /** - * Start automatic measurement. - */ - void start(); - - /** - * Stop automatic measurement. - */ - void stop(); - /** * Reset chip. * @@ -192,21 +189,10 @@ private: */ int reset(); - /** - * Static trampoline from the hrt_call context; because we don't have a - * generic hrt wrapper yet. - * - * Called by the HRT in interrupt context at the specified rate if - * automatic polling is enabled. - * - * @param arg Instance pointer for the driver that is polling. - */ - static void measure_trampoline(void *arg); - /** * Fetch measurements from the sensor and update the report buffers. */ - void measure(); + virtual void _measure(); /** * Read a register from the GYROSIM @@ -286,19 +272,21 @@ private: #pragma pack(pop) uint8_t _regdata[108]; + + bool _pub_blocked = true; // Don't publish until initialized }; /** * Helper class implementing the gyro driver node. */ -class GYROSIM_gyro : public device::VDev +class GYROSIM_gyro : public VirtDevObj { public: GYROSIM_gyro(GYROSIM *parent, const char *path); - ~GYROSIM_gyro(); + virtual ~GYROSIM_gyro() {} - virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); + virtual ssize_t devRead(void *buffer, size_t buflen); + virtual int devIOCTL(unsigned long cmd, unsigned long arg); virtual int init(); @@ -307,11 +295,11 @@ protected: void parent_poll_notify(); + virtual void _measure() {}; private: GYROSIM *_parent; orb_advert_t _gyro_topic; int _gyro_orb_class_instance; - int _gyro_class_instance; /* do not allow to copy this class due to pointer data members */ GYROSIM_gyro(const GYROSIM_gyro &); @@ -322,11 +310,10 @@ private: extern "C" { __EXPORT int gyrosim_main(int argc, char *argv[]); } GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation rotation) : - VDev("GYROSIM", path_accel), + VirtDevObj("GYROSIM", path_accel, ACCEL_BASE_DEVICE_PATH, 1e6 / 400), _gyro(new GYROSIM_gyro(this, path_gyro)), _product(GYROSIMES_REV_C4), _call{}, - _call_interval(0), _accel_reports(nullptr), _accel_scale{}, _accel_range_scale(0.0f), @@ -338,28 +325,24 @@ GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation ro _gyro_scale{}, _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), - _sample_rate(1000), _accel_reads(perf_alloc(PC_COUNT, "gyrosim_accel_read")), _gyro_reads(perf_alloc(PC_COUNT, "gyrosim_gyro_read")), _sample_perf(perf_alloc(PC_ELAPSED, "gyrosim_read")), _good_transfers(perf_alloc(PC_COUNT, "gyrosim_good_transfers")), _reset_retries(perf_alloc(PC_COUNT, "gyrosim_reset_retries")), - _system_latency_perf(perf_alloc_once(PC_ELAPSED, "sys_latency")), _controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")), + _accel_int(1000000 / GYROSIM_ACCEL_DEFAULT_RATE, true), + _gyro_int(1000000 / GYROSIM_GYRO_DEFAULT_RATE, true), _rotation(rotation), _last_temperature(0) { - // disable debug() calls - _debug_enabled = false; - // Don't publish until initialized - _pub_blocked = true; - - _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_GYROSIM; + m_id.dev_id_s.bus = 1; + m_id.dev_id_s.devtype = DRV_ACC_DEVTYPE_GYROSIM; /* Prime _gyro with parents devid. */ - _gyro->_device_id.devid = _device_id.devid; - _gyro->_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_GYROSIM; + _gyro->m_id.dev_id = m_id.dev_id; + _gyro->m_id.dev_id_s.devtype = DRV_GYR_DEVTYPE_GYROSIM; // default accel scale factors _accel_scale.x_offset = 0; @@ -376,8 +359,6 @@ GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation ro _gyro_scale.y_scale = 1.0f; _gyro_scale.z_offset = 0; _gyro_scale.z_scale = 1.0f; - - memset(&_call, 0, sizeof(_call)); } GYROSIM::~GYROSIM() @@ -397,10 +378,6 @@ GYROSIM::~GYROSIM() 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); perf_free(_accel_reads); @@ -411,21 +388,21 @@ GYROSIM::~GYROSIM() int GYROSIM::init() { - int ret; - - /* do VDevinit first */ - ret = VDev::init(); - - /* if probe/setup failed, bail now */ - if (ret != OK) { - PX4_WARN("VDev setup failed"); - return ret; - } + PX4_INFO("GYROSIM::init"); + int ret = 1; struct accel_report arp = {}; struct gyro_report grp = {}; + ret = VirtDevObj::init(); + + if (ret != 0) { + PX4_WARN("Base class init failed"); + ret = 1; + goto out; + } + /* allocate basic report buffers */ _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); @@ -462,18 +439,25 @@ GYROSIM::init() _gyro_scale.z_scale = 1.0f; - /* do VDev init for the gyro device node, keep it optional */ + /* do 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"); + PX4_ERR("gyro init failed"); return ret; } - _accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH); + ret = start(); - measure(); + if (ret != OK) { + PX4_ERR("gyro accel start failed (%d)", ret); + return ret; + } + + // Do not call _gyro->start() because polling is done in accel + + _measure(); /* advertise sensor topic, measure manually to initialize valid report */ _accel_reports->get(&arp); @@ -520,7 +504,7 @@ GYROSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) // Get data from the simulator Simulator *sim = Simulator::getInstance(); - if (sim == NULL) { + if (sim == nullptr) { PX4_WARN("failed accessing simulator"); return ENODEV; } @@ -551,7 +535,7 @@ GYROSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) void GYROSIM::_set_sample_rate(unsigned desired_sample_rate_hz) { - PX4_INFO("GYROSIM::_set_sample_rate %uHz", desired_sample_rate_hz); + PX4_INFO("GYROSIM::_set_sample_rate %u Hz", desired_sample_rate_hz); if (desired_sample_rate_hz == 0 || desired_sample_rate_hz == GYRO_SAMPLERATE_DEFAULT || @@ -569,15 +553,14 @@ GYROSIM::_set_sample_rate(unsigned desired_sample_rate_hz) // register dumps look correct 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; - hrt_cancel(&_call); - hrt_call_every(&_call, _call_interval, _call_interval, (hrt_callout)&GYROSIM::measure_trampoline, this); + unsigned sample_rate = 1000 / div; + PX4_INFO("GYROSIM: Changed sample rate to %uHz", sample_rate); + setSampleInterval(1000000 / sample_rate); + _gyro->setSampleInterval(1000000 / sample_rate); } ssize_t -GYROSIM::read(device::file_t *filp, char *buffer, size_t buflen) +GYROSIM::devRead(void *buffer, size_t buflen) { unsigned count = buflen / sizeof(accel_report); @@ -589,7 +572,7 @@ GYROSIM::read(device::file_t *filp, char *buffer, size_t buflen) /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { _accel_reports->flush(); - measure(); + _measure(); } /* if no data, error (we could block here) */ @@ -620,7 +603,7 @@ int GYROSIM::self_test() { if (perf_event_count(_sample_perf) == 0) { - measure(); + _measure(); } /* return 0 on success, 1 else */ @@ -724,7 +707,7 @@ GYROSIM::gyro_self_test() } ssize_t -GYROSIM::gyro_read(device::file_t *filp, char *buffer, size_t buflen) +GYROSIM::gyro_read(void *buffer, size_t buflen) { unsigned count = buflen / sizeof(gyro_report); @@ -736,7 +719,7 @@ GYROSIM::gyro_read(device::file_t *filp, char *buffer, size_t buflen) /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { _gyro_reports->flush(); - measure(); + _measure(); } /* if no data, error (we could block here) */ @@ -764,8 +747,9 @@ GYROSIM::gyro_read(device::file_t *filp, char *buffer, size_t buflen) } int -GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) +GYROSIM::devIOCTL(unsigned long cmd, unsigned long arg) { + switch (cmd) { case SENSORIOCRESET: @@ -789,10 +773,10 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: - return ioctl(filp, SENSORIOCSPOLLRATE, 1000); + return devIOCTL(SENSORIOCSPOLLRATE, 1000); case SENSOR_POLLRATE_DEFAULT: - return ioctl(filp, SENSORIOCSPOLLRATE, GYROSIM_ACCEL_DEFAULT_RATE); + return devIOCTL(SENSORIOCSPOLLRATE, GYROSIM_ACCEL_DEFAULT_RATE); /* adjust to a legal polling interval in Hz */ default: { @@ -844,7 +828,7 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) return _accel_reports->size(); case ACCELIOCGSAMPLERATE: - return _sample_rate; + return 1e6 / m_sample_interval_usecs; case ACCELIOCSSAMPLERATE: _set_sample_rate(arg); @@ -883,12 +867,12 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) default: /* give it to the superclass */ - return VDev::ioctl(filp, cmd, arg); + return VirtDevObj::devIOCTL(cmd, arg); } } int -GYROSIM::gyro_ioctl(device::file_t *filp, int cmd, unsigned long arg) +GYROSIM::gyro_ioctl(unsigned long cmd, unsigned long arg) { switch (cmd) { @@ -896,7 +880,7 @@ GYROSIM::gyro_ioctl(device::file_t *filp, int cmd, unsigned long arg) case SENSORIOCSPOLLRATE: case SENSORIOCGPOLLRATE: case SENSORIOCRESET: - return ioctl(filp, cmd, arg); + return devIOCTL(cmd, arg); case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ @@ -915,7 +899,7 @@ GYROSIM::gyro_ioctl(device::file_t *filp, int cmd, unsigned long arg) return _gyro_reports->size(); case GYROIOCGSAMPLERATE: - return _sample_rate; + return 1e6 / m_sample_interval_usecs; case GYROIOCSSAMPLERATE: _set_sample_rate(arg); @@ -949,7 +933,7 @@ GYROSIM::gyro_ioctl(device::file_t *filp, int cmd, unsigned long arg) default: /* give it to the superclass */ - return VDev::ioctl(filp, cmd, arg); + return VirtDevObj::devIOCTL(cmd, arg); } } @@ -1020,7 +1004,7 @@ GYROSIM::set_accel_range(unsigned max_g_in) return OK; } -void +int GYROSIM::start() { /* make sure we are stopped first */ @@ -1031,28 +1015,11 @@ GYROSIM::start() _gyro_reports->flush(); /* start polling at the specified rate */ - if (_call_interval > 0) { - hrt_call_every(&_call, _call_interval, _call_interval, (hrt_callout)&GYROSIM::measure_trampoline, this); - } + return DevObj::start(); } void -GYROSIM::stop() -{ - hrt_cancel(&_call); -} - -void -GYROSIM::measure_trampoline(void *arg) -{ - GYROSIM *dev = reinterpret_cast(arg); - - /* make another measurement */ - dev->measure(); -} - -void -GYROSIM::measure() +GYROSIM::_measure() { #if 0 @@ -1133,6 +1100,14 @@ GYROSIM::measure() arb.y = mpu_report.accel_y; arb.z = mpu_report.accel_z; + math::Vector<3> aval(mpu_report.accel_x, mpu_report.accel_y, mpu_report.accel_z); + math::Vector<3> aval_integrated; + + bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt); + arb.x_integral = aval_integrated(0); + arb.y_integral = aval_integrated(1); + arb.z_integral = aval_integrated(2); + 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); @@ -1147,25 +1122,39 @@ GYROSIM::measure() grb.y = mpu_report.gyro_y; grb.z = mpu_report.gyro_z; + math::Vector<3> gval(mpu_report.gyro_x, mpu_report.gyro_y, mpu_report.gyro_z); + math::Vector<3> gval_integrated; + + bool gyro_notify = _gyro_int.put(grb.timestamp, gval, gval_integrated, grb.integral_dt); + grb.x_integral = gval_integrated(0); + grb.y_integral = gval_integrated(1); + grb.z_integral = gval_integrated(2); + _accel_reports->force(&arb); _gyro_reports->force(&grb); - /* notify anyone waiting for data */ - poll_notify(POLLIN); - _gyro->parent_poll_notify(); + if (accel_notify) { + /* notify anyone waiting for data */ + updateNotify(); + _gyro->parent_poll_notify(); - if (!(_pub_blocked)) { - /* log the time of this report */ - perf_begin(_controller_latency_perf); - perf_begin(_system_latency_perf); - /* publish it */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); + if (!(_pub_blocked)) { + /* log the time of this report */ + perf_begin(_controller_latency_perf); + /* publish it */ + orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); + } } - if (!(_pub_blocked)) { - /* publish it */ - orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); + if (gyro_notify) { + updateNotify(); + _gyro->parent_poll_notify(); + + if (!(_pub_blocked)) { + /* publish it */ + orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); + } } /* stop measuring */ @@ -1211,63 +1200,46 @@ GYROSIM::print_registers() GYROSIM_gyro::GYROSIM_gyro(GYROSIM *parent, const char *path) : - VDev("GYROSIM_gyro", path), + // Set sample interval to 0 since device is read by parent + VirtDevObj("GYROSIM_gyro", path, GYRO_BASE_DEVICE_PATH, 0), _parent(parent), _gyro_topic(nullptr), - _gyro_orb_class_instance(-1), - _gyro_class_instance(-1) + _gyro_orb_class_instance(-1) { } -GYROSIM_gyro::~GYROSIM_gyro() -{ - if (_gyro_class_instance != -1) { - unregister_class_devname(GYRO_BASE_DEVICE_PATH, _gyro_class_instance); - } -} int GYROSIM_gyro::init() { - int ret; - - // do base class init - ret = VDev::init(); - - /* if probe/setup failed, bail now */ - if (ret != OK) { - DEVICE_DEBUG("gyro init failed"); - return ret; - } - - _gyro_class_instance = register_class_devname(GYRO_BASE_DEVICE_PATH); - + int ret = VirtDevObj::init(); + PX4_INFO("GYROSIM_gyro::init base class ret: %d", ret); return ret; } void GYROSIM_gyro::parent_poll_notify() { - poll_notify(POLLIN); + updateNotify(); } ssize_t -GYROSIM_gyro::read(device::file_t *filp, char *buffer, size_t buflen) +GYROSIM_gyro::devRead(void *buffer, size_t buflen) { - return _parent->gyro_read(filp, buffer, buflen); + return _parent->gyro_read(buffer, buflen); } int -GYROSIM_gyro::ioctl(device::file_t *filp, int cmd, unsigned long arg) +GYROSIM_gyro::devIOCTL(unsigned long cmd, unsigned long arg) { switch (cmd) { case DEVIOCGDEVICEID: - return (int)VDev::ioctl(filp, cmd, arg); + return (int)VirtDevObj::devIOCTL(cmd, arg); break; default: - return _parent->gyro_ioctl(filp, cmd, arg); + return _parent->gyro_ioctl(cmd, arg); } } @@ -1296,10 +1268,10 @@ void usage(); int start(enum Rotation rotation) { - int fd; GYROSIM **g_dev_ptr = &g_dev_sim; const char *path_accel = MPU_DEVICE_PATH_ACCEL; const char *path_gyro = MPU_DEVICE_PATH_GYRO; + DevHandle h; if (*g_dev_ptr != nullptr) { /* if already started, the still command succeeded */ @@ -1319,18 +1291,18 @@ start(enum Rotation rotation) } /* set the poll rate to default, starts automatic data collection */ - fd = px4_open(path_accel, O_RDONLY); + DevMgr::getHandle(path_accel, h); - if (fd < 0) { + if (!h.isValid()) { goto fail; } - if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - px4_close(fd); + if (h.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + DevMgr::releaseHandle(h); goto fail; } - px4_close(fd); + DevMgr::releaseHandle(h); return 0; fail: @@ -1375,29 +1347,31 @@ test() ssize_t sz; /* get the driver */ - int fd = px4_open(path_accel, O_RDONLY); + DevHandle h_accel; + DevMgr::getHandle(path_accel, h_accel); - if (fd < 0) { + if (!h_accel.isValid()) { PX4_ERR("%s open failed (try 'gyrosim start')", path_accel); return 1; } /* get the driver */ - int fd_gyro = px4_open(path_gyro, O_RDONLY); + DevHandle h_gyro; + DevMgr::getHandle(path_gyro, h_gyro); - if (fd_gyro < 0) { + if (!h_gyro.isValid()) { PX4_ERR("%s open failed", path_gyro); return 1; } /* reset to manual polling */ - if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) { + if (h_accel.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) { PX4_ERR("reset to manual polling"); return 1; } /* do a simple demand read */ - sz = px4_read(fd, &a_report, sizeof(a_report)); + sz = h_accel.read(&a_report, sizeof(a_report)); if (sz != sizeof(a_report)) { PX4_WARN("ret: %zd, expected: %zd", sz, sizeof(a_report)); @@ -1417,7 +1391,7 @@ test() (double)(a_report.range_m_s2 / GYROSIM_ONE_G)); /* do a simple demand read */ - sz = px4_read(fd_gyro, &g_report, sizeof(g_report)); + sz = h_gyro.read(&g_report, sizeof(g_report)); if (sz != sizeof(g_report)) { PX4_WARN("ret: %zd, expected: %zd", sz, sizeof(g_report)); @@ -1440,11 +1414,12 @@ test() /* XXX add poll-rate tests here too */ - px4_close(fd); + // Destructor would clean the up too + DevMgr::releaseHandle(h_accel); + DevMgr::releaseHandle(h_gyro); reset(); PX4_INFO("PASS"); - return 0; } @@ -1454,30 +1429,28 @@ test() int reset() { - const char *path_accel = MPU_DEVICE_PATH_ACCEL; - int fd = px4_open(path_accel, O_RDONLY); + DevHandle h; + DevMgr::getHandle(MPU_DEVICE_PATH_ACCEL, h); - if (fd < 0) { + if (!h.isValid()) { PX4_ERR("reset failed"); return 1; } - if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) { + if (h.ioctl(SENSORIOCRESET, 0) < 0) { PX4_ERR("driver reset failed"); goto reset_fail; } - if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + if (h.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { PX4_ERR("driver poll restart failed"); goto reset_fail; } - px4_close(fd); return 0; reset_fail: - px4_close(fd); return 1; } @@ -1496,6 +1469,8 @@ info() PX4_INFO("state @ %p", *g_dev_ptr); (*g_dev_ptr)->print_info(); + unsigned dummy = 0; + PX4_INFO("device_id: %u", (unsigned int)(*g_dev_ptr)->devIOCTL(DEVIOCGDEVICEID, dummy)); return 0; } @@ -1538,7 +1513,7 @@ gyrosim_main(int argc, char *argv[]) /* jump over start/off/etc and look at options first */ int myoptind = 1; - const char *myoptarg = NULL; + const char *myoptarg = nullptr; while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { switch (ch) { diff --git a/src/platforms/posix/drivers/gpssim/module.mk b/src/platforms/posix/drivers/ledsim/CMakeLists.txt similarity index 87% rename from src/platforms/posix/drivers/gpssim/module.mk rename to src/platforms/posix/drivers/ledsim/CMakeLists.txt index 8db69770fe..0f20b87988 100644 --- a/src/platforms/posix/drivers/gpssim/module.mk +++ b/src/platforms/posix/drivers/ledsim/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# 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 @@ -30,13 +30,13 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ - -# -# Simulated GPS driver -# - -MODULE_COMMAND = gpssim - -SRCS = gpssim.cpp - -MAXOPTIMIZATION = -Os +px4_add_module( + MODULE platforms__posix__drivers__ledsim + COMPILE_FLAGS + -Os + SRCS + led.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/posix/drivers/ledsim/led.cpp b/src/platforms/posix/drivers/ledsim/led.cpp new file mode 100644 index 0000000000..161514cd93 --- /dev/null +++ b/src/platforms/posix/drivers/ledsim/led.cpp @@ -0,0 +1,142 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file led.cpp + * + * LED driver. + */ + +#include +#include +#include + +#include "VirtDevObj.hpp" + +using namespace DriverFramework; + +#define DEVICE_DEBUG PX4_INFO + +/* + * Ideally we'd be able to get these from up_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(); +extern void led_on(int led); +extern void led_off(int led); +extern void led_toggle(int led); +__END_DECLS + +class LED : public VirtDevObj +{ +public: + LED(); + virtual ~LED(); + + virtual int init(); + virtual int devIOCTL(unsigned long cmd, unsigned long arg); + +protected: + virtual void _measure() {} +}; + +LED::LED() : + VirtDevObj("led", "/dev/ledsim", LED_BASE_DEVICE_PATH, 0) +{ + // force immediate init/device registration + init(); +} + +LED::~LED() +{ +} + +int +LED::init() +{ + DEVICE_DEBUG("LED::init"); + int ret = VirtDevObj::init(); + + if (ret == 0) { + led_init(); + } + + return ret; +} + +int +LED::devIOCTL(unsigned long cmd, unsigned long arg) +{ + int result = OK; + + switch (cmd) { + case LED_ON: + led_on(arg); + break; + + case LED_OFF: + led_off(arg); + break; + + case LED_TOGGLE: + led_toggle(arg); + break; + + + default: + result = VirtDevObj::devIOCTL(cmd, arg); + } + + return result; +} + +namespace +{ +LED *gLED; +} + +void +drv_led_start(void) +{ + if (gLED == nullptr) { + gLED = new LED; + + if (gLED != nullptr) { + gLED->init(); + } + } +} diff --git a/src/platforms/posix/drivers/rgbledsim/CMakeLists.txt b/src/platforms/posix/drivers/rgbledsim/CMakeLists.txt new file mode 100644 index 0000000000..edb2392a3a --- /dev/null +++ b/src/platforms/posix/drivers/rgbledsim/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 platforms__posix__drivers__rgbledsim + MAIN rgbledsim + COMPILE_FLAGS + -Os + SRCS + rgbled.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/posix/drivers/rgbledsim/rgbled.cpp b/src/platforms/posix/drivers/rgbledsim/rgbled.cpp new file mode 100644 index 0000000000..64d8580250 --- /dev/null +++ b/src/platforms/posix/drivers/rgbledsim/rgbled.cpp @@ -0,0 +1,802 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file rgbled.cpp + * + * Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C. + * + * @author Julian Oes + * @author Anton Babushkin + */ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include + +#include + +#include "VirtDevObj.hpp" + +using namespace DriverFramework; + +#define DEVICE_LOG PX4_INFO +#define RGBLED_ONTIME 120 +#define RGBLED_OFFTIME 120 + +#define ADDR PX4_I2C_OBDEV_LED /**< I2C adress of TCA62724FMG */ +#define SUB_ADDR_START 0x01 /**< write everything (with auto-increment) */ +#define SUB_ADDR_PWM0 0x81 /**< blue (without auto-increment) */ +#define SUB_ADDR_PWM1 0x82 /**< green (without auto-increment) */ +#define SUB_ADDR_PWM2 0x83 /**< red (without auto-increment) */ +#define SUB_ADDR_SETTINGS 0x84 /**< settings (without auto-increment)*/ + +#define SETTING_NOT_POWERSAVE 0x01 /**< power-save mode not off */ +#define SETTING_ENABLE 0x02 /**< on */ + + +class RGBLEDSIM : public VirtDevObj +{ +public: + RGBLEDSIM(int bus, int rgbled); + virtual ~RGBLEDSIM(); + + + virtual int init(); + virtual int probe(); + virtual int info(); + virtual int devIOCTL(unsigned long cmd, unsigned long arg); + + // Don't use the default periodic callback + virtual void _measure() {} + +private: + work_s _work; + + rgbled_mode_t _mode; + rgbled_pattern_t _pattern; + + uint8_t _r; + uint8_t _g; + uint8_t _b; + float _brightness; + float _max_brightness; + + bool _running; + int _led_interval; + bool _should_run; + int _counter; + int _param_sub; + + void set_color(rgbled_color_t ledcolor); + void set_mode(rgbled_mode_t mode); + void set_pattern(rgbled_pattern_t *pattern); + + static void led_trampoline(void *arg); + void led(); + + int send_led_enable(bool enable); + int send_led_rgb(); + int get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b); + void update_params(); + + /** + * Perform an I2C transaction to the device. + * + * At least one of send_len and recv_len must be non-zero. + * + * @param send Pointer to bytes to send. + * @param send_len Number of bytes to send. + * @param recv Pointer to buffer for bytes received. + * @param recv_len Number of bytes to receive. + * @return OK if the transfer was successful, -errno + * otherwise. + */ + int transfer(const uint8_t *send, unsigned send_len, + uint8_t *recv, unsigned recv_len); + +}; + +/* for now, we only support one RGBLEDSIM */ +namespace +{ +RGBLEDSIM *g_rgbled = nullptr; +} + +void rgbled_usage(); + +extern "C" __EXPORT int rgbledsim_main(int argc, char *argv[]); + +RGBLEDSIM::RGBLEDSIM(int bus, int rgbled) : + VirtDevObj("rgbled", "/dev/rgbledsim", RGBLED_BASE_DEVICE_PATH, 0), + _mode(RGBLED_MODE_OFF), + _r(0), + _g(0), + _b(0), + _brightness(1.0f), + _max_brightness(1.0f), + _running(false), + _led_interval(0), + _should_run(false), + _counter(0), + _param_sub(-1) +{ + memset(&_work, 0, sizeof(_work)); + memset(&_pattern, 0, sizeof(_pattern)); +} + +RGBLEDSIM::~RGBLEDSIM() +{ +} + +int +RGBLEDSIM::init() +{ + int ret; + ret = VirtDevObj::init(); + + if (ret != OK) { + return ret; + } + + /* switch off LED on start */ + send_led_enable(false); + send_led_rgb(); + + return OK; +} + +int +RGBLEDSIM::probe() +{ + int ret; + bool on, powersave; + uint8_t r, g, b; + + /** + this may look strange, but is needed. There is a serial + EEPROM (Microchip-24aa01) on the PX4FMU-v1 that responds to + a bunch of I2C addresses, including the 0x55 used by this + LED device. So we need to do enough operations to be sure + we are talking to the right device. These 3 operations seem + to be enough, as the 3rd one consistently fails if no + RGBLEDSIM is on the bus. + */ + + if ((ret = get(on, powersave, r, g, b)) != OK || + (ret = send_led_enable(false) != OK) || + (ret = send_led_enable(false) != OK)) { + return ret; + } + + return ret; +} + +int +RGBLEDSIM::info() +{ + int ret; + bool on, powersave; + uint8_t r, g, b; + + ret = get(on, powersave, r, g, b); + + if (ret == OK) { + /* we don't care about power-save mode */ + DEVICE_LOG("state: %s", on ? "ON" : "OFF"); + DEVICE_LOG("red: %u, green: %u, blue: %u", (unsigned)r, (unsigned)g, (unsigned)b); + + } else { + PX4_WARN("failed to read led"); + } + + return ret; +} + +int +RGBLEDSIM::devIOCTL(unsigned long cmd, unsigned long arg) +{ + int ret = ENOTTY; + + switch (cmd) { + case RGBLED_SET_RGB: + /* set the specified color */ + _r = ((rgbled_rgbset_t *) arg)->red; + _g = ((rgbled_rgbset_t *) arg)->green; + _b = ((rgbled_rgbset_t *) arg)->blue; + send_led_rgb(); + return OK; + + case RGBLED_SET_COLOR: + /* set the specified color name */ + set_color((rgbled_color_t)arg); + send_led_rgb(); + return OK; + + case RGBLED_SET_MODE: + /* set the specified mode */ + set_mode((rgbled_mode_t)arg); + return OK; + + case RGBLED_SET_PATTERN: + /* set a special pattern */ + set_pattern((rgbled_pattern_t *)arg); + return OK; + + default: + /* see if the parent class can make any use of it */ + ret = VirtDevObj::devIOCTL(cmd, arg); + break; + } + + return ret; +} + + +void +RGBLEDSIM::led_trampoline(void *arg) +{ + RGBLEDSIM *rgbl = reinterpret_cast(arg); + + rgbl->led(); +} + +/** + * Main loop function + */ +void +RGBLEDSIM::led() +{ + if (!_should_run) { + _running = false; + return; + } + + if (_param_sub < 0) { + _param_sub = orb_subscribe(ORB_ID(parameter_update)); + } + + 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); + update_params(); + // Immediately update to change brightness + send_led_rgb(); + } + } + + switch (_mode) { + case RGBLED_MODE_BLINK_SLOW: + case RGBLED_MODE_BLINK_NORMAL: + case RGBLED_MODE_BLINK_FAST: + if (_counter >= 2) { + _counter = 0; + } + + send_led_enable(_counter == 0); + + break; + + case RGBLED_MODE_BREATHE: + + if (_counter >= 62) { + _counter = 0; + } + + int n; + + if (_counter < 32) { + n = _counter; + + } else { + n = 62 - _counter; + } + + _brightness = n * n / (31.0f * 31.0f); + send_led_rgb(); + break; + + 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) { + _counter = 0; + } + + set_color(_pattern.color[_counter]); + send_led_rgb(); + _led_interval = _pattern.duration[_counter]; + break; + + default: + break; + } + + _counter++; +} + +/** + * Parse color constant and set _r _g _b values + */ +void +RGBLEDSIM::set_color(rgbled_color_t color) +{ + switch (color) { + case RGBLED_COLOR_OFF: + _r = 0; + _g = 0; + _b = 0; + break; + + case RGBLED_COLOR_RED: + _r = 255; + _g = 0; + _b = 0; + break; + + case RGBLED_COLOR_YELLOW: + _r = 255; + _g = 200; + _b = 0; + break; + + case RGBLED_COLOR_PURPLE: + _r = 255; + _g = 0; + _b = 255; + break; + + case RGBLED_COLOR_GREEN: + _r = 0; + _g = 255; + _b = 0; + break; + + case RGBLED_COLOR_BLUE: + _r = 0; + _g = 0; + _b = 255; + break; + + case RGBLED_COLOR_WHITE: + _r = 255; + _g = 255; + _b = 255; + break; + + case RGBLED_COLOR_AMBER: + _r = 255; + _g = 80; + _b = 0; + break; + + case RGBLED_COLOR_DIM_RED: + _r = 90; + _g = 0; + _b = 0; + break; + + case RGBLED_COLOR_DIM_YELLOW: + _r = 80; + _g = 30; + _b = 0; + break; + + case RGBLED_COLOR_DIM_PURPLE: + _r = 45; + _g = 0; + _b = 45; + break; + + case RGBLED_COLOR_DIM_GREEN: + _r = 0; + _g = 90; + _b = 0; + break; + + case RGBLED_COLOR_DIM_BLUE: + _r = 0; + _g = 0; + _b = 90; + break; + + case RGBLED_COLOR_DIM_WHITE: + _r = 30; + _g = 30; + _b = 30; + break; + + case RGBLED_COLOR_DIM_AMBER: + _r = 80; + _g = 20; + _b = 0; + break; + + default: + PX4_WARN("color unknown"); + break; + } +} + +/** + * Set mode, if mode not changed has no any effect (doesn't reset blinks phase) + */ +void +RGBLEDSIM::set_mode(rgbled_mode_t mode) +{ + if (mode != _mode) { + _mode = mode; + + switch (mode) { + case RGBLED_MODE_OFF: + _should_run = false; + send_led_enable(false); + break; + + case RGBLED_MODE_ON: + _brightness = 1.0f; + send_led_rgb(); + send_led_enable(true); + break; + + case RGBLED_MODE_BLINK_SLOW: + _should_run = true; + _counter = 0; + _led_interval = 2000; + _brightness = 1.0f; + send_led_rgb(); + break; + + case RGBLED_MODE_BLINK_NORMAL: + _should_run = true; + _counter = 0; + _led_interval = 500; + _brightness = 1.0f; + send_led_rgb(); + break; + + case RGBLED_MODE_BLINK_FAST: + _should_run = true; + _counter = 0; + _led_interval = 100; + _brightness = 1.0f; + send_led_rgb(); + break; + + case RGBLED_MODE_BREATHE: + _should_run = true; + _counter = 0; + _led_interval = 25; + send_led_enable(true); + break; + + case RGBLED_MODE_PATTERN: + _should_run = true; + _counter = 0; + _brightness = 1.0f; + send_led_enable(true); + break; + + default: + PX4_WARN("mode unknown"); + break; + } + + /* if it should run now, start the workq */ + if (_should_run && !_running) { + _running = true; + work_queue(LPWORK, &_work, (worker_t)&RGBLEDSIM::led_trampoline, this, 1); + } + + } +} + +/** + * Set pattern for PATTERN mode, but don't change current mode + */ +void +RGBLEDSIM::set_pattern(rgbled_pattern_t *pattern) +{ + memcpy(&_pattern, pattern, sizeof(rgbled_pattern_t)); +} + +/** + * Sent ENABLE flag to LED driver + */ +int +RGBLEDSIM::send_led_enable(bool enable) +{ + uint8_t settings_byte = 0; + + if (enable) { + settings_byte |= SETTING_ENABLE; + } + + settings_byte |= SETTING_NOT_POWERSAVE; + + const uint8_t msg[2] = { SUB_ADDR_SETTINGS, settings_byte}; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +/** + * Send RGB PWM settings to LED driver according to current color and brightness + */ +int +RGBLEDSIM::send_led_rgb() +{ + /* To scale from 0..255 -> 0..15 shift right by 4 bits */ + const uint8_t msg[6] = { + SUB_ADDR_PWM0, static_cast((_b >> 4) * _brightness * _max_brightness + 0.5f), + SUB_ADDR_PWM1, static_cast((_g >> 4) * _brightness * _max_brightness + 0.5f), + SUB_ADDR_PWM2, static_cast((_r >> 4) * _brightness * _max_brightness + 0.5f) + }; + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +RGBLEDSIM::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b) +{ + uint8_t result[2] = {0, 0}; + int ret; + + ret = transfer(nullptr, 0, &result[0], 2); + + if (ret == OK) { + on = result[0] & SETTING_ENABLE; + powersave = !(result[0] & SETTING_NOT_POWERSAVE); + /* XXX check, looks wrong */ + r = (result[0] & 0x0f) << 4; + g = (result[1] & 0xf0); + b = (result[1] & 0x0f) << 4; + } + + return ret; +} + +void +RGBLEDSIM::update_params() +{ + int32_t maxbrt = 15; + param_get(param_find("LED_RGB_MAXBRT"), &maxbrt); + maxbrt = maxbrt > 15 ? 15 : maxbrt; + maxbrt = maxbrt < 0 ? 0 : maxbrt; + + // A minimum of 2 "on" steps is required for breathe effect + if (maxbrt == 1) { + maxbrt = 2; + } + + _max_brightness = maxbrt / 15.0f; +} + +void +rgbled_usage() +{ + PX4_WARN("missing command: try 'start', 'test', 'info', 'off', 'stop', 'rgb 30 40 50'"); + PX4_WARN("options:"); + PX4_WARN(" -b i2cbus (%d)", PX4_I2C_BUS_LED); + PX4_WARN(" -a addr (0x%x)", ADDR); +} + +int +rgbledsim_main(int argc, char *argv[]) +{ + int i2cdevice = -1; + int rgbledadr = ADDR; /* 7bit */ + + int ch; + + /* 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': + rgbledadr = strtol(myoptarg, NULL, 0); + break; + + case 'b': + i2cdevice = strtol(myoptarg, NULL, 0); + break; + + default: + rgbled_usage(); + return 1; + } + } + + if (myoptind >= argc) { + rgbled_usage(); + return 1; + } + + const char *verb = argv[myoptind]; + + int ret; + + if (!strcmp(verb, "start")) { + if (g_rgbled != nullptr) { + PX4_WARN("already started"); + return 1; + } + + if (i2cdevice == -1) { + // try the external bus first + i2cdevice = PX4_I2C_BUS_EXPANSION; + g_rgbled = new RGBLEDSIM(PX4_I2C_BUS_EXPANSION, rgbledadr); + + if (g_rgbled != nullptr && OK != g_rgbled->init()) { + delete g_rgbled; + g_rgbled = nullptr; + } + + if (g_rgbled == nullptr) { + // fall back to default bus + if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) { + PX4_WARN("no RGB led on bus #%d", i2cdevice); + return 1; + } + + i2cdevice = PX4_I2C_BUS_LED; + } + } + + if (g_rgbled == nullptr) { + g_rgbled = new RGBLEDSIM(i2cdevice, rgbledadr); + + if (g_rgbled == nullptr) { + PX4_WARN("new failed"); + return 1; + } + + if (OK != g_rgbled->init()) { + delete g_rgbled; + g_rgbled = nullptr; + PX4_WARN("no RGB led on bus #%d", i2cdevice); + return 1; + } + } + + return 0; + } + + /* need the driver past this point */ + if (g_rgbled == nullptr) { + PX4_WARN("not started"); + rgbled_usage(); + return 1; + } + + if (!strcmp(verb, "test")) { + DevHandle h; + DevMgr::getHandle(RGBLED0_DEVICE_PATH, h); + + if (!h.isValid()) { + PX4_WARN("Unable to open " RGBLED0_DEVICE_PATH); + return 1; + } + + rgbled_pattern_t pattern = { {RGBLED_COLOR_RED, RGBLED_COLOR_GREEN, RGBLED_COLOR_BLUE, RGBLED_COLOR_WHITE, RGBLED_COLOR_OFF, RGBLED_COLOR_OFF}, + {500, 500, 500, 500, 1000, 0 } // "0" indicates end of pattern + }; + + ret = h.ioctl(RGBLED_SET_PATTERN, (unsigned long)&pattern); + ret = h.ioctl(RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_PATTERN); + + DevMgr::releaseHandle(h); + return ret; + } + + if (!strcmp(verb, "info")) { + g_rgbled->info(); + return 0; + } + + if (!strcmp(verb, "off") || !strcmp(verb, "stop")) { + DevHandle h; + DevMgr::getHandle(RGBLED0_DEVICE_PATH, h); + + if (!h.isValid()) { + PX4_WARN("Unable to open " RGBLED0_DEVICE_PATH); + return 1; + } + + ret = h.ioctl(RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF); + DevMgr::releaseHandle(h); + + /* 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; + } + + if (!strcmp(verb, "rgb")) { + if (argc < 5) { + PX4_WARN("Usage: rgbled rgb "); + return 1; + } + + DevHandle h; + DevMgr::getHandle(RGBLED0_DEVICE_PATH, h); + + if (!h.isValid()) { + PX4_WARN("Unable to open " RGBLED0_DEVICE_PATH); + return 1; + } + + rgbled_rgbset_t v; + v.red = strtol(argv[2], NULL, 0); + v.green = strtol(argv[3], NULL, 0); + v.blue = strtol(argv[4], NULL, 0); + ret = h.ioctl(RGBLED_SET_RGB, (unsigned long)&v); + ret = h.ioctl(RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON); + DevMgr::releaseHandle(h); + return ret; + } + + rgbled_usage(); + return 1; +} + +int RGBLEDSIM::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) +{ + return 0; +} diff --git a/src/platforms/posix/drivers/tonealrmsim/module.mk b/src/platforms/posix/drivers/tonealrmsim/module.mk deleted file mode 100644 index 948b3912a5..0000000000 --- a/src/platforms/posix/drivers/tonealrmsim/module.mk +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Tone alarm driver -# - -MODULE_COMMAND = tone_alarm - -SRCS = tone_alarm.cpp - -MAXOPTIMIZATION = -Os diff --git a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp index b58cccb5bd..4946b00613 100644 --- a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp +++ b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp @@ -109,17 +109,18 @@ #include +#include "VirtDevObj.hpp" -class ToneAlarm : public device::VDev +using namespace DriverFramework; + +class ToneAlarm : public VirtDevObj { public: ToneAlarm(); ~ToneAlarm(); - virtual int init(); - - 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); + virtual int devIOCTL(unsigned long cmd, unsigned long arg); + virtual ssize_t devWrite(const void *buffer, size_t len); inline const char *name(int tune) { return _tune_names[tune]; @@ -196,6 +197,8 @@ private: // static void next_trampoline(void *arg); + // Unused + virtual void _measure() {} }; // semitone offsets from C for the characters 'A'-'G' @@ -208,11 +211,12 @@ extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]); ToneAlarm::ToneAlarm() : - VDev("tone_alarm", TONEALARM0_DEVICE_PATH), + VirtDevObj("tone_alarm", TONEALARM0_DEVICE_PATH, nullptr, 0), _default_tune_number(0), _user_tune(nullptr), _tune(nullptr), - _next(nullptr) + _next(nullptr), + _note_call{} { // enable debug() calls //_debug_enabled = true; @@ -251,21 +255,6 @@ ToneAlarm::~ToneAlarm() { } -int -ToneAlarm::init() -{ - int ret; - - ret = VDev::init(); - - if (ret != OK) { - return ret; - } - - DEVICE_DEBUG("ready"); - return OK; -} - unsigned ToneAlarm::note_to_divisor(unsigned note) { @@ -388,6 +377,7 @@ ToneAlarm::start_tune(const char *tune) _repeat = false; // otherwise command-line tunes repeat forever... // schedule a callback to start playing + _note_call = {}; hrt_call_after(&_note_call, 0, (hrt_callout)next_trampoline, this); } @@ -397,6 +387,7 @@ ToneAlarm::next_note() // do we have an inter-note gap to wait for? if (_silence_length > 0) { stop_note(); + _note_call = {}; hrt_call_after(&_note_call, (hrt_abstime)_silence_length, (hrt_callout)next_trampoline, this); _silence_length = 0; return; @@ -494,6 +485,7 @@ ToneAlarm::next_note() case 'P': // pause for a note length stop_note(); + _note_call = {}; hrt_call_after(&_note_call, (hrt_abstime)rest_duration(next_number(), next_dots()), (hrt_callout)next_trampoline, @@ -522,6 +514,7 @@ ToneAlarm::next_note() if (note == 0) { // this is a rest - pause for the current note length + _note_call = {}; hrt_call_after(&_note_call, (hrt_abstime)rest_duration(_note_length, next_dots()), (hrt_callout)next_trampoline, @@ -579,6 +572,7 @@ ToneAlarm::next_note() start_note(note); // and arrange a callback when the note should stop + _note_call = {}; hrt_call_after(&_note_call, (hrt_abstime)duration, (hrt_callout)next_trampoline, this); return; @@ -653,13 +647,11 @@ ToneAlarm::next_trampoline(void *arg) int -ToneAlarm::ioctl(device::file_t *filp, int cmd, unsigned long arg) +ToneAlarm::devIOCTL(unsigned long cmd, unsigned long arg) { int result = OK; - DEVICE_DEBUG("ioctl %i %lu", cmd, arg); - -// irqstate_t flags = irqsave(); + PX4_DEBUG("ToneAlarm::devIOCTL %i %lu", cmd, arg); /* decide whether to increase the alarm level to cmd or leave it alone */ switch (cmd) { @@ -694,18 +686,16 @@ ToneAlarm::ioctl(device::file_t *filp, int cmd, unsigned long arg) break; } -// irqrestore(flags); - /* give it to the superclass if we didn't like it */ if (result == -ENOTTY) { - result = VDev::ioctl(filp, cmd, arg); + result = VirtDevObj::devIOCTL(cmd, arg); } return result; } ssize_t -ToneAlarm::write(device::file_t *filp, const char *buffer, size_t len) +ToneAlarm::devWrite(const void *buffer, size_t len) { // sanity-check the buffer for length and nul-termination if (len > _tune_max) { @@ -726,13 +716,15 @@ ToneAlarm::write(device::file_t *filp, const char *buffer, size_t len) _user_tune = nullptr; } + const char *buf = reinterpret_cast(buffer); + // if the new tune is empty, we're done - if (buffer[0] == '\0') { + if (buf[0] == '\0') { return OK; } // allocate a copy of the new tune - _user_tune = strndup(buffer, len); + _user_tune = strndup(buf, len); if (_user_tune == nullptr) { return -ENOMEM; @@ -755,17 +747,17 @@ ToneAlarm *g_dev; int play_tune(unsigned tune) { - int fd, ret; + int ret; - fd = px4_open(TONEALARM0_DEVICE_PATH, 0); + DevHandle h; + DevMgr::getHandle(TONEALARM0_DEVICE_PATH, h); - if (fd < 0) { - PX4_WARN("Error: failed to open %s", TONEALARM0_DEVICE_PATH); + if (!h.isValid()) { + PX4_WARN("Error: failed to open %s (%d)", TONEALARM0_DEVICE_PATH, h.getError()); return 1; } - ret = px4_ioctl(fd, TONE_SET_ALARM, tune); - px4_close(fd); + ret = h.ioctl(TONE_SET_ALARM, tune); if (ret != 0) { PX4_WARN("TONE_SET_ALARM"); @@ -778,17 +770,18 @@ play_tune(unsigned tune) int play_string(const char *str, bool free_buffer) { - int fd, ret; + int ret; - fd = px4_open(TONEALARM0_DEVICE_PATH, O_WRONLY); + DevHandle h; + DevMgr::getHandle(TONEALARM0_DEVICE_PATH, h); - if (fd < 0) { - PX4_WARN("Error: failed to open %s", TONEALARM0_DEVICE_PATH); + if (!h.isValid()) { + PX4_WARN("Error: failed to get handle to %s", TONEALARM0_DEVICE_PATH); return 1; } - ret = px4_write(fd, str, strlen(str) + 1); - px4_close(fd); + ret = h.write(str, strlen(str) + 1); + DevMgr::releaseHandle(h); if (free_buffer) { free((void *)str); @@ -871,18 +864,19 @@ tone_alarm_main(int argc, char *argv[]) } /* if it looks like a PLAY string... */ - else if (strlen(argv1) > 2) { + else if (argv1 && (strlen(argv1) > 2)) { if (*argv1 == 'M') { ret = play_string(argv1, false); } } else { /* It might be a tune name */ - for (tune = 1; tune < TONE_NUMBER_OF_TUNES; tune++) + 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/main.cpp b/src/platforms/posix/main.cpp index 1b22627721..7bf431b4f4 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -35,6 +35,7 @@ * Basic shell to execute builtin "apps" * * @author Mark Charlebois + * @auther Roman Bapst */ #include @@ -43,6 +44,10 @@ #include #include #include +#include "apps.h" +#include "px4_middleware.h" +#include "DriverFramework.hpp" +#include namespace px4 { @@ -53,8 +58,7 @@ using namespace std; typedef int (*px4_main_t)(int argc, char *argv[]); -#include "apps.h" -#include "px4_middleware.h" +#define CMD_BUFF_SIZE 100 static bool _ExitFlag = false; extern "C" { @@ -71,6 +75,7 @@ extern "C" { { cout.flush(); cout << endl << "floating point exception" << endl; + PX4_BACKTRACE(); cout.flush(); } } @@ -98,14 +103,17 @@ static void run_cmd(const vector &appargs, bool exit_on_fail) } arg[i] = (char *)0; + + if (exit_on_fail) { + cout << endl; + } + int retval = apps[command](i, (char **)arg); if (exit_on_fail && retval) { exit(retval); } - usleep(65000); - } else if (command.compare("help") == 0) { list_builtins(); @@ -113,11 +121,13 @@ static void run_cmd(const vector &appargs, bool exit_on_fail) // Do nothing } else { - cout << "Invalid command: " << command << "\ntype 'help' for a list of commands" << endl; + cout << endl << "Invalid command: " << command << "\ntype 'help' for a list of commands" << endl; } - print_prompt(); + if (exit_on_fail) { + print_prompt(); + } } static void usage() @@ -133,10 +143,14 @@ static void usage() static void process_line(string &line, bool exit_on_fail) { - vector appargs(8); + if (line.length() == 0) { + printf("\n"); + } + + vector appargs(10); stringstream(line) >> appargs[0] >> appargs[1] >> appargs[2] >> appargs[3] >> appargs[4] >> appargs[5] >> appargs[6] >> - appargs[7]; + appargs[7] >> appargs[8] >> appargs[9]; run_cmd(appargs, exit_on_fail); } @@ -184,6 +198,7 @@ int main(int argc, char **argv) } if (!error_detected) { + DriverFramework::Framework::initialize(); px4::init_once(); px4::init(argc, argv, "mainapp"); @@ -203,22 +218,95 @@ int main(int argc, char **argv) } if (!daemon_mode) { - string mystr; + string mystr = ""; + string string_buffer[CMD_BUFF_SIZE]; + int buf_ptr_write = 0; + int buf_ptr_read = 0; print_prompt(); + // change input mode so that we can manage shell + struct termios term; + tcgetattr(0, &term); + term.c_lflag &= ~ICANON; + term.c_lflag &= ~ECHO; + tcsetattr(0, TCSANOW, &term); + setbuf(stdin, NULL); + while (!_ExitFlag) { - struct pollfd fds; - int ret; - fds.fd = 0; /* stdin */ - fds.events = POLLIN; - ret = poll(&fds, 1, 100); + char c = getchar(); + + switch (c) { + case 127: // backslash + if (mystr.length() > 0) { + mystr.pop_back(); + printf("%c[2K", 27); // clear line + cout << (char)13; + print_prompt(); + cout << mystr; + } + + break; + + case'\n': // user hit enter + if (buf_ptr_write == CMD_BUFF_SIZE) { + buf_ptr_write = 0; + } + + if (buf_ptr_write > 0) { + if (mystr != string_buffer[buf_ptr_write - 1]) { + string_buffer[buf_ptr_write] = mystr; + buf_ptr_write++; + } + + } else { + if (mystr != string_buffer[CMD_BUFF_SIZE - 1]) { + string_buffer[buf_ptr_write] = mystr; + buf_ptr_write++; + } + } - if (ret > 0) { - getline(cin, mystr); process_line(mystr, !daemon_mode); mystr = ""; + buf_ptr_read = buf_ptr_write; + break; + + case '\033': { // arrow keys + c = getchar(); // skip first one, does not have the info + c = getchar(); + + // arrow up + if (c == 'A') { + buf_ptr_read--; + // arrow down + + } else if (c == 'B') { + if (buf_ptr_read < buf_ptr_write) { + buf_ptr_read++; + } + + } else { + // TODO: Support editing current line + } + + if (buf_ptr_read < 0) { + buf_ptr_read = 0; + } + + string saved_cmd = string_buffer[buf_ptr_read]; + printf("%c[2K", 27); + cout << (char)13; + mystr = saved_cmd; + print_prompt(); + cout << mystr; + break; + } + + default: // any other input + cout << c; + mystr += c; + break; } } @@ -236,5 +324,6 @@ int main(int argc, char **argv) vector shutdown_cmd = { "shutdown" }; run_cmd(shutdown_cmd, true); + DriverFramework::Framework::shutdown(); } } diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c index e4839f0ac7..3bb8fa129f 100644 --- a/src/platforms/posix/px4_layer/drv_hrt.c +++ b/src/platforms/posix/px4_layer/drv_hrt.c @@ -86,6 +86,10 @@ static void hrt_unlock(void) #include #define CLOCK_REALTIME 0 +#ifndef CLOCK_MONOTONIC +#define CLOCK_MONOTONIC 1 +#endif + int px4_clock_gettime(clockid_t clk_id, struct timespec *tp) { struct timeval now; diff --git a/src/platforms/posix/px4_layer/px4_posix_impl.cpp b/src/platforms/posix/px4_layer/px4_posix_impl.cpp index 19747556a9..c77fe760b8 100644 --- a/src/platforms/posix/px4_layer/px4_posix_impl.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_impl.cpp @@ -40,6 +40,7 @@ #include #include #include +#include #include #include #include @@ -49,6 +50,7 @@ #include "hrt_work.h" #include #include "px4_time.h" +#include extern pthread_t _shell_task_id; @@ -86,6 +88,13 @@ void init(int argc, char *argv[], const char *app_name) printf("Ready to fly.\n"); printf("\n"); printf("\n"); + + // set the threads name +#ifdef __PX4_DARWIN + (void)pthread_setname_np(app_name); +#else + (void)pthread_setname_np(pthread_self(), app_name); +#endif } uint64_t get_time_micros() diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp index 41cade1d5d..a220b88c67 100644 --- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp @@ -38,6 +38,7 @@ */ #include +#include #include #include #include @@ -48,6 +49,7 @@ #include #include #include +#include #include #include @@ -62,6 +64,7 @@ #define SHELL_TASK_ID (PX4_MAX_TASKS+1) pthread_t _shell_task_id = 0; +pthread_mutex_t task_mutex = PTHREAD_MUTEX_INITIALIZER; struct task_entry { pthread_t pid; @@ -70,10 +73,11 @@ struct task_entry { task_entry() : isused(false) {} }; -static task_entry taskmap[PX4_MAX_TASKS]; +static task_entry taskmap[PX4_MAX_TASKS] = {}; typedef struct { px4_main_t entry; + const char *name; int argc; char *argv[]; // strings are allocated after the @@ -82,6 +86,19 @@ typedef struct { static void *entry_adapter(void *ptr) { pthdata_t *data = (pthdata_t *) ptr; + + int rv; + // set the threads name +#ifdef __PX4_DARWIN + rv = pthread_setname_np(data->name); +#else + rv = pthread_setname_np(pthread_self(), data->name); +#endif + + if (rv) { + PX4_ERR("px4_task_spawn_cmd: failed to set name of thread %d %d\n", rv, errno); + } + data->entry(data->argc, data->argv); free(ptr); PX4_DEBUG("Before px4_task_exit"); @@ -101,6 +118,7 @@ px4_systemreset(bool to_bootloader) 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; int i; @@ -109,9 +127,8 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int unsigned long structsize; char *p = (char *)argv; - pthread_t task; pthread_attr_t attr; - struct sched_param param; + struct sched_param param = {}; // Calculate argc while (p != (char *)0) { @@ -126,12 +143,13 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int } 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); + pthdata_t *taskdata = (pthdata_t *)malloc(structsize + len); + memset(taskdata, 0, structsize + len); offset = ((unsigned long)taskdata) + structsize; + taskdata->name = name; taskdata->entry = entry; taskdata->argc = argc; @@ -177,16 +195,30 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int return (rv < 0) ? rv : -rv; } - rv = pthread_create(&task, &attr, &entry_adapter, (void *) taskdata); + pthread_mutex_lock(&task_mutex); + + int taskid = 0; + + for (i = 0; i < PX4_MAX_TASKS; ++i) { + if (taskmap[i].isused == false) { + taskmap[i].name = name; + taskmap[i].isused = true; + taskid = i; + break; + } + } + + rv = pthread_create(&taskmap[taskid].pid, &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(&taskmap[taskid].pid, NULL, &entry_adapter, (void *) taskdata); if (rv != 0) { PX4_ERR("px4_task_spawn_cmd: failed to create thread %d %d\n", rv, errno); + taskmap[taskid].isused = false; return (rv < 0) ? rv : -rv; } @@ -195,14 +227,7 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int } } - for (i = 0; i < PX4_MAX_TASKS; ++i) { - if (taskmap[i].isused == false) { - taskmap[i].pid = task; - taskmap[i].name = name; - taskmap[i].isused = true; - break; - } - } + pthread_mutex_unlock(&task_mutex); if (i >= PX4_MAX_TASKS) { return -ENOSPC; @@ -224,9 +249,12 @@ int px4_task_delete(px4_task_t id) return -EINVAL; } + pthread_mutex_lock(&task_mutex); + // If current thread then exit, otherwise cancel if (pthread_self() == pid) { taskmap[id].isused = false; + pthread_mutex_unlock(&task_mutex); pthread_exit(0); } else { @@ -234,6 +262,7 @@ int px4_task_delete(px4_task_t id) } taskmap[id].isused = false; + pthread_mutex_unlock(&task_mutex); return rv; } @@ -246,6 +275,7 @@ void px4_task_exit(int ret) // Get pthread ID from the opaque ID for (i = 0; i < PX4_MAX_TASKS; ++i) { if (taskmap[i].pid == pid) { + pthread_mutex_lock(&task_mutex); taskmap[i].isused = false; break; } @@ -258,6 +288,8 @@ void px4_task_exit(int ret) PX4_DEBUG("px4_task_exit: %s", taskmap[i].name.c_str()); } + pthread_mutex_unlock(&task_mutex); + pthread_exit((void *)(unsigned long)ret); } @@ -268,7 +300,9 @@ int px4_task_kill(px4_task_t id, int sig) PX4_DEBUG("Called px4_task_kill %d", sig); if (id < PX4_MAX_TASKS && taskmap[id].isused && taskmap[id].pid != 0) { + pthread_mutex_lock(&task_mutex); pid = taskmap[id].pid; + pthread_mutex_unlock(&task_mutex); } else { return -EINVAL; @@ -326,10 +360,36 @@ const char *getprogname() for (int i = 0; i < PX4_MAX_TASKS; i++) { if (taskmap[i].isused && taskmap[i].pid == pid) { + pthread_mutex_lock(&task_mutex); return taskmap[i].name.c_str(); + pthread_mutex_unlock(&task_mutex); } } return "Unknown App"; } + +int px4_prctl(int option, const char *arg2, unsigned pid) +{ + int rv; + + switch (option) { + case PR_SET_NAME: + // set the threads name +#ifdef __PX4_DARWIN + rv = pthread_setname_np(arg2); +#else + rv = pthread_setname_np(pthread_self(), arg2); +#endif + break; + + default: + rv = -1; + PX4_WARN("FAILED SETTING TASK NAME"); + break; + } + + return rv; +} + __END_DECLS diff --git a/src/platforms/posix/px4_layer/px4_sem.cpp b/src/platforms/posix/px4_layer/px4_sem.cpp index 0aa56a1f3e..67e59bc8fd 100644 --- a/src/platforms/posix/px4_layer/px4_sem.cpp +++ b/src/platforms/posix/px4_layer/px4_sem.cpp @@ -43,13 +43,12 @@ #include #include #include +#include #ifdef __PX4_DARWIN #include -#include - int px4_sem_init(px4_sem_t *s, int pshared, unsigned value) { // We do not used the process shared arg @@ -63,39 +62,108 @@ int px4_sem_init(px4_sem_t *s, int pshared, unsigned value) int px4_sem_wait(px4_sem_t *s) { - pthread_mutex_lock(&(s->lock)); + int ret = pthread_mutex_lock(&(s->lock)); + + if (ret) { + return ret; + } + s->value--; if (s->value < 0) { - pthread_cond_wait(&(s->wait), &(s->lock)); + ret = pthread_cond_wait(&(s->wait), &(s->lock)); + + } else { + ret = 0; } - pthread_mutex_unlock(&(s->lock)); + if (ret) { + PX4_WARN("px4_sem_wait failure"); + } - return 0; + int mret = pthread_mutex_unlock(&(s->lock)); + + return (ret) ? ret : mret; +} + +int px4_sem_timedwait(px4_sem_t *s, const struct timespec *abstime) +{ + int ret = pthread_mutex_lock(&(s->lock)); + + if (ret) { + return ret; + } + + s->value--; + errno = 0; + + if (s->value < 0) { + ret = pthread_cond_timedwait(&(s->wait), &(s->lock), abstime); + + } else { + ret = 0; + } + + int err = ret; + + if (err != 0 && err != ETIMEDOUT) { + setbuf(stdout, NULL); + setbuf(stderr, NULL); + const unsigned NAMELEN = 32; + char thread_name[NAMELEN] = {}; + (void)pthread_getname_np(pthread_self(), thread_name, NAMELEN); + PX4_WARN("%s: px4_sem_timedwait failure: ret: %d, %s", thread_name, ret, strerror(err)); + } + + int mret = pthread_mutex_unlock(&(s->lock)); + + return (err) ? err : mret; } int px4_sem_post(px4_sem_t *s) { - pthread_mutex_lock(&(s->lock)); + int ret = pthread_mutex_lock(&(s->lock)); + + if (ret) { + return ret; + } + s->value++; if (s->value <= 0) { - pthread_cond_signal(&(s->wait)); + ret = pthread_cond_signal(&(s->wait)); + + } else { + ret = 0; } - pthread_mutex_unlock(&(s->lock)); + if (ret) { + PX4_WARN("px4_sem_post failure"); + } - return 0; + int mret = pthread_mutex_unlock(&(s->lock)); + + // return the cond signal failure if present, + // else return the mutex status + return (ret) ? ret : mret; } int px4_sem_getvalue(px4_sem_t *s, int *sval) { - pthread_mutex_lock(&(s->lock)); - *sval = s->value; - pthread_mutex_unlock(&(s->lock)); + int ret = pthread_mutex_lock(&(s->lock)); - return 0; + if (ret) { + PX4_WARN("px4_sem_getvalue failure"); + } + + if (ret) { + return ret; + } + + *sval = s->value; + ret = pthread_mutex_unlock(&(s->lock)); + + return ret; } int px4_sem_destroy(px4_sem_t *s) diff --git a/src/platforms/posix/work_queue/hrt_thread.c b/src/platforms/posix/work_queue/hrt_thread.c index 98331ca798..5199785d78 100644 --- a/src/platforms/posix/work_queue/hrt_thread.c +++ b/src/platforms/posix/work_queue/hrt_thread.c @@ -119,6 +119,14 @@ static void hrt_work_process() uint32_t remaining; uint32_t next; + // set the threads name +#ifdef __PX4_DARWIN + pthread_setname_np("HRT"); +#else + // The Linux headers do not actually contain this + //rv = pthread_setname_np(pthread_self(), "HRT"); +#endif + /* Then process queued work. We need to keep interrupts disabled while * we process items in the work list. */ @@ -143,7 +151,7 @@ static void hrt_work_process() if (elapsed >= work->delay) { /* Remove the ready-to-execute work from the list */ - (void)dq_rem((struct dq_entry_s *)work, &wqueue->q); + (void)dq_rem((struct dq_entry_s *) & (work->dq), &(wqueue->q)); //PX4_INFO("Dequeued work=%p", work); /* Extract the work description from the entry (in case the work @@ -265,6 +273,7 @@ static int work_hrtthread(int argc, char *argv[]) void hrt_work_queue_init(void) { px4_sem_init(&_hrt_work_lock, 0, 1); + memset(&g_hrt_work, 0, sizeof(g_hrt_work)); // Create high priority worker thread g_hrt_work.pid = px4_task_spawn_cmd("wkr_hrt", diff --git a/src/platforms/posix/work_queue/work_thread.c b/src/platforms/posix/work_queue/work_thread.c index ec565bcfcc..2050ad51e6 100644 --- a/src/platforms/posix/work_queue/work_thread.c +++ b/src/platforms/posix/work_queue/work_thread.c @@ -45,6 +45,7 @@ #include #include #include +#include #include #include #include "work_lock.h" @@ -195,7 +196,7 @@ void work_queues_init(void) #endif // Create high priority worker thread - g_work[HPWORK].pid = px4_task_spawn_cmd("wkr_high", + g_work[HPWORK].pid = px4_task_spawn_cmd("hpwork", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 1, 2000, @@ -203,7 +204,7 @@ void work_queues_init(void) (char *const *)NULL); // Create low priority worker thread - g_work[LPWORK].pid = px4_task_spawn_cmd("wkr_low", + g_work[LPWORK].pid = px4_task_spawn_cmd("lpwork", SCHED_DEFAULT, SCHED_PRIORITY_MIN, 2000, @@ -306,6 +307,14 @@ int work_usrthread(int argc, char *argv[]) { /* Loop forever */ + int rv; + // set the threads name +#ifdef __PX4_DARWIN + rv = pthread_setname_np("USR"); +#else + rv = pthread_setname_np(pthread_self(), "USR"); +#endif + for (;;) { /* Then process queued work. We need to keep interrupts disabled while * we process items in the work list. diff --git a/src/platforms/px4_config.h b/src/platforms/px4_config.h index 4417c5b894..e44fd4afce 100644 --- a/src/platforms/px4_config.h +++ b/src/platforms/px4_config.h @@ -42,6 +42,7 @@ #if defined(__PX4_NUTTX) #include +#include #elif defined (__PX4_POSIX) diff --git a/src/platforms/px4_posix.h b/src/platforms/px4_posix.h index ab53d03900..fa1c60adee 100644 --- a/src/platforms/px4_posix.h +++ b/src/platforms/px4_posix.h @@ -62,6 +62,7 @@ typedef struct { __EXPORT int px4_sem_init(px4_sem_t *s, int pshared, unsigned value); __EXPORT int px4_sem_wait(px4_sem_t *s); +__EXPORT int px4_sem_timedwait(px4_sem_t *sem, const struct timespec *abstime); __EXPORT int px4_sem_post(px4_sem_t *s); __EXPORT int px4_sem_getvalue(px4_sem_t *s, int *sval); __EXPORT int px4_sem_destroy(px4_sem_t *s); @@ -76,6 +77,7 @@ typedef sem_t px4_sem_t; #define px4_sem_init sem_init #define px4_sem_wait sem_wait +#define px4_sem_timedwait sem_timedwait #define px4_sem_post sem_post #define px4_sem_getvalue sem_getvalue #define px4_sem_destroy sem_destroy diff --git a/src/platforms/px4_tasks.h b/src/platforms/px4_tasks.h index 04585aaced..33c2fb4705 100644 --- a/src/platforms/px4_tasks.h +++ b/src/platforms/px4_tasks.h @@ -49,6 +49,9 @@ #elif defined(__PX4_NUTTX) typedef int px4_task_t; +#include +#define px4_prctl prctl + /** Default scheduler type */ #if CONFIG_RR_INTERVAL > 0 # define SCHED_DEFAULT SCHED_RR @@ -80,10 +83,10 @@ typedef int px4_task_t; #error "No target OS defined" #endif -#if defined (__PX4_LINUX) || defined(__PX4_NUTTX) +#if defined (__PX4_LINUX) #include #else -#define prctl(_action, _string, _pid) +#define PR_SET_NAME 1 #endif typedef int px4_task_t; @@ -126,5 +129,10 @@ __EXPORT void px4_show_tasks(void); /** See if a task is running **/ __EXPORT bool px4_task_is_running(const char *taskname); +#ifdef __PX4_POSIX +/** set process (and thread) options */ +__EXPORT int px4_prctl(int option, const char *arg2, unsigned pid); +#endif + __END_DECLS diff --git a/src/platforms/qurt/include/sched.h b/src/platforms/qurt/include/sched.h deleted file mode 100644 index b67db23b1d..0000000000 --- a/src/platforms/qurt/include/sched.h +++ /dev/null @@ -1,12 +0,0 @@ -#pragma once - -#define SCHED_FIFO 1 -#define SCHED_RR 2 - -struct sched_param { - int sched_priority; -}; - -int sched_get_priority_max(int policy); -int sched_get_priority_min(int policy); - diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index 4216bbf599..19319d1736 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -161,6 +161,13 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int return (rv < 0) ? rv : -rv; } + rv = pthread_attr_getschedparam(&attr, ¶m); + + if (rv != 0) { + PX4_WARN("px4_task_spawn_cmd: failed to get thread sched param"); + return (rv < 0) ? rv : -rv; + } + #if 0 rv = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED); diff --git a/src/platforms/qurt/stubs/stubs_posix.c b/src/platforms/qurt/stubs/stubs_posix.c index 0b9faab882..3dd4720cb3 100644 --- a/src/platforms/qurt/stubs/stubs_posix.c +++ b/src/platforms/qurt/stubs/stubs_posix.c @@ -97,3 +97,41 @@ int clock_gettime(clockid_t clk_id, struct timespec *tp) { return 1; } + +int pthread_mutex_lock(pthread_mutex_t *mutex) +{ + return 1; +} + +int pthread_mutex_unlock(pthread_mutex_t *mutex) +{ + return 1; +} + +int pthread_cond_signal(pthread_cond_t *cond) +{ + return 1; +} +int pthread_mutex_destroy(pthread_mutex_t *mutex) +{ + return 1; +} +int pthread_mutex_init(pthread_mutex_t *mutex, const pthread_mutexattr_t *attr) +{ + return 1; +} + +int pthread_cond_wait(pthread_cond_t *cond, pthread_mutex_t *mutex) +{ + return 1; +} + +int pthread_cond_timedwait(pthread_cond_t *cond, pthread_mutex_t *mutex, const struct timespec *abstime) +{ + return 1; +} + +int pthread_cond_init(pthread_cond_t *cond, const pthread_condattr_t *attr) +{ + return 1; +} diff --git a/src/systemcmds/mixer/CMakeLists.txt b/src/systemcmds/mixer/CMakeLists.txt index 703d8210fb..117f96ff2a 100644 --- a/src/systemcmds/mixer/CMakeLists.txt +++ b/src/systemcmds/mixer/CMakeLists.txt @@ -31,9 +31,7 @@ # ############################################################################ set(MIXER_CFLAGS -Os) -if(${OS} STREQUAL "qurt") - list(APPEND MIXER_CFLAGS -Wframe-larger-than=2176) -else() +if(${OS} STREQUAL "nuttx") list(APPEND MIXER_CFLAGS -Wframe-larger-than=2100) endif() diff --git a/src/systemcmds/tests/CMakeLists.txt b/src/systemcmds/tests/CMakeLists.txt index 25cad8505c..e9cb903477 100644 --- a/src/systemcmds/tests/CMakeLists.txt +++ b/src/systemcmds/tests/CMakeLists.txt @@ -58,7 +58,6 @@ set(srcs test_rc.c test_conv.cpp test_mount.c - test_eigen.cpp ) if(${OS} STREQUAL "nuttx") @@ -73,6 +72,7 @@ px4_add_module( STACK 60000 COMPILE_FLAGS -Wframe-larger-than=6000 + -Wno-float-equal -O0 SRCS ${srcs} DEPENDS diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk deleted file mode 100644 index 7e31442525..0000000000 --- a/src/systemcmds/tests/module.mk +++ /dev/null @@ -1,51 +0,0 @@ -# -# Assorted tests and the like -# - -MODULE_COMMAND = tests -MODULE_STACKSIZE = 60000 -MAXOPTIMIZATION = -O0 - -SRCS = test_adc.c \ - test_bson.c \ - test_float.c \ - test_gpio.c \ - test_hott_telemetry.c \ - test_hrt.c \ - test_int.c \ - test_jig_voltages.c \ - test_led.c \ - test_sensors.c \ - test_servo.c \ - test_sleep.c \ - test_uart_baudchange.c \ - test_uart_console.c \ - test_uart_loopback.c \ - test_uart_send.c \ - test_mixer.cpp \ - test_mathlib.cpp \ - test_file.c \ - test_file2.c \ - tests_main.c \ - test_param.c \ - test_ppm_loopback.c \ - test_rc.c \ - test_conv.cpp \ - test_mount.c \ - test_eigen.cpp - -ifeq ($(PX4_TARGET_OS), nuttx) -SRCS += test_time.c - -EXTRACXXFLAGS = -Wframe-larger-than=6000 -else -EXTRACXXFLAGS = -endif - -EXTRACXXFLAGS += -Wno-float-equal - -# Flag is only valid for GCC, not clang -ifneq ($(USE_GCC), 0) -EXTRACXXFLAGS += -Wno-double-promotion -Wno-error=logical-op -endif - diff --git a/src/systemcmds/tests/test_hrt.c b/src/systemcmds/tests/test_hrt.c index 8bc0888122..c88603e745 100644 --- a/src/systemcmds/tests/test_hrt.c +++ b/src/systemcmds/tests/test_hrt.c @@ -37,7 +37,7 @@ ****************************************************************************/ #include - +#include #include #include @@ -88,8 +88,8 @@ extern uint16_t ppm_buffer[]; extern unsigned ppm_decoded_channels; -extern uint16_t ppm_edge_history[]; -extern uint16_t ppm_pulse_history[]; +// extern uint16_t ppm_edge_history[]; +// extern uint16_t ppm_pulse_history[]; int test_ppm(int argc, char *argv[]) { @@ -102,17 +102,17 @@ int test_ppm(int argc, char *argv[]) printf(" %u\n", ppm_buffer[i]); } - printf("edges\n"); + // printf("edges\n"); - for (i = 0; i < 32; i++) { - printf(" %u\n", ppm_edge_history[i]); - } + // for (i = 0; i < 32; i++) { + // printf(" %u\n", ppm_edge_history[i]); + // } - printf("pulses\n"); + // printf("pulses\n"); - for (i = 0; i < 32; i++) { - printf(" %u\n", ppm_pulse_history[i]); - } + // for (i = 0; i < 32; i++) { + // printf(" %u\n", ppm_pulse_history[i]); + // } fflush(stdout); #else diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index 98b65f10d1..4da4b77c1e 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -193,7 +193,7 @@ int test_mixer(int argc, char *argv[]) /* run through arming phase */ for (unsigned i = 0; i < output_max; i++) { actuator_controls[i] = 0.1f; - r_page_servo_disarmed[i] = PWM_LOWEST_MIN; + r_page_servo_disarmed[i] = PWM_MOTOR_OFF; r_page_servo_control_min[i] = PWM_DEFAULT_MIN; r_page_servo_control_max[i] = PWM_DEFAULT_MAX; } diff --git a/src/systemcmds/tests/test_time.c b/src/systemcmds/tests/test_time.c index d89f90c683..c67f11f467 100644 --- a/src/systemcmds/tests/test_time.c +++ b/src/systemcmds/tests/test_time.c @@ -37,7 +37,8 @@ ****************************************************************************/ #include - +#include +#include #include #include diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index e16e0adb56..7c7cbeb28f 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -50,11 +50,11 @@ #include #include - -//#include - #include +// Not using Eigen at the moment +#define TESTS_EIGEN_DISABLE + #include "tests.h" /**************************************************************************** @@ -113,7 +113,9 @@ const struct { #ifndef TESTS_MATHLIB_DISABLE {"mathlib", test_mathlib, 0}, #endif +#ifndef TESTS_EIGEN_DISABLE {"eigen", test_eigen, OPT_NOJIGTEST}, +#endif {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, {NULL, NULL, 0} }; diff --git a/unittests/.gitignore b/unittests/.gitignore index aa316941f4..d5bf4e2412 100644 --- a/unittests/.gitignore +++ b/unittests/.gitignore @@ -4,5 +4,13 @@ mixer_test sf0x_test sbus2_test autodeclination_test -st24_test +rc_input_test +conversion_test +param_test +*.a sample_unittest +CMakeCache.txt +CMakeFiles +CTestTestfile.cmake +cmake_install.cmake +Makefile diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index 4285520b0c..fcfcee3702 100644 --- a/unittests/CMakeLists.txt +++ b/unittests/CMakeLists.txt @@ -26,9 +26,9 @@ include(CheckCXXCompilerFlag) CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) if(COMPILER_SUPPORTS_CXX11) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -fno-exceptions -fno-rtti -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ -g -Wall") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -fno-exceptions -fno-rtti -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ -D__PX4_UNIT_TESTS -g -Wall") elseif(COMPILER_SUPPORTS_CXX0X) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x -fno-exceptions -fno-rtti -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ -g -Wall") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x -fno-exceptions -fno-rtti -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ -D__PX4_UNIT_TESTS -g -Wall") else() message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") endif() @@ -37,7 +37,7 @@ set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=gnu99 -g") #set(CMAKE_INCLUDE_SYSTEM_FLAG_C "-isystem" ) #set(CMAKE_INCLUDE_SYSTEM_FLAG_CXX "-isystem" ) -set(GTEST_DIR gtest) +set(GTEST_DIR googletest) add_subdirectory(${GTEST_DIR}) include_directories(${GTEST_DIR}/include) @@ -48,10 +48,15 @@ include_directories(${PX_SRC}/modules) include_directories(${PX_SRC}/modules/uORB) include_directories(${PX_SRC}/lib) include_directories(${PX_SRC}/drivers) +include_directories(${PX_SRC}/lib/DriverFramework/framework/include) +include_directories(${PX_SRC}/../build_posix_sitl_default/src/modules) +include_directories(${PX_SRC}/../build_posix_sitl_default/src/modules/uORB) +include_directories(${PX_SRC}/../build_posix_sitl_default/src) +include_directories(${PX_SRC}/../build_posix_sitl_default/src/modules/param) include_directories(${PX_SRC}/platforms) include_directories(${PX_SRC}/platforms/posix/include) include_directories(${PX_SRC}/platforms/posix/px4_layer) -include_directories(${PX_SRC}/drivers/device ) +include_directories(${PX_SRC}/drivers/device) add_definitions(-D__EXPORT=) @@ -101,13 +106,15 @@ add_library( px4_platform ${PX_SRC}/platforms/posix/work_queue/dq_addlast.c ${PX_SRC}/platforms/posix/px4_layer/lib_crc32.c ${PX_SRC}/platforms/posix/px4_layer/drv_hrt.c + ${PX_SRC}/platforms/posix/px4_layer/px4_sem.cpp ${PX_SRC}/drivers/device/device_posix.cpp ${PX_SRC}/drivers/device/vdev.cpp ${PX_SRC}/drivers/device/vfile.cpp ${PX_SRC}/drivers/device/vdev_posix.cpp ${PX_SRC}/drivers/device/i2c_posix.cpp ${PX_SRC}/drivers/device/sim.cpp - ${PX_SRC}/drivers/device/ringbuffer.cpp + ${PX_SRC}/drivers/device/ringbuffer.cpp + ${PX_SRC}/../build_posix_sitl_default/src/modules/param/px4_parameters.c ) #target_include_directories( px4_platform PUBLIC ${PX_SRC}/platforms ) @@ -141,19 +148,15 @@ target_link_libraries( conversion_test px4_platform ) add_gtest(conversion_test) # sbus2_test -add_executable(sbus2_test sbus2_test.cpp hrt.cpp) +add_executable(sbus2_test sbus2_test.cpp hrt.cpp + ${PX_SRC}/lib/rc/sbus.c) target_link_libraries( sbus2_test px4_platform ) add_gtest(sbus2_test) # st24_test -add_executable(st24_test st24_test.cpp hrt.cpp ${PX_SRC}/lib/rc/st24.c) -target_link_libraries( st24_test px4_platform ) -add_gtest(st24_test) - -# sumd_test -add_executable(sumd_test sumd_test.cpp hrt.cpp ${PX_SRC}/lib/rc/sumd.c) -target_link_libraries( sumd_test px4_platform ) -add_gtest(sumd_test) +add_executable(rc_input_test st24_test.cpp hrt.cpp ${PX_SRC}/lib/rc/st24.c sumd_test.cpp ${PX_SRC}/lib/rc/sumd.c) +target_link_libraries(rc_input_test px4_platform) +add_gtest(rc_input_test) # sf0x_test add_executable(sf0x_test sf0x_test.cpp ${PX_SRC}/drivers/sf0x/sf0x_parser.cpp) @@ -172,15 +175,15 @@ target_link_libraries( param_test px4_platform ) add_gtest(param_test) # uorb test -add_executable(uorb_tests uorb_unittests/uORBCommunicator_gtests.cpp - uorb_unittests/uORBCommunicatorMock.cpp - uorb_unittests/uORBCommunicatorMockLoopback.cpp - ${PX_SRC}/modules/uORB/uORBDevices_posix.cpp - ${PX_SRC}/modules/uORB/uORBManager_posix.cpp - ${PX_SRC}/modules/uORB/objects_common.cpp - ${PX_SRC}/modules/uORB/uORBUtils.cpp - ${PX_SRC}/modules/uORB/uORB.cpp - ) -target_link_libraries( uorb_tests px4_platform ) +#add_executable(uorb_tests uorb_unittests/uORBCommunicator_gtests.cpp +# uorb_unittests/uORBCommunicatorMock.cpp +# uorb_unittests/uORBCommunicatorMockLoopback.cpp +# ${PX_SRC}/modules/uORB/uORBDevices_posix.cpp +# ${PX_SRC}/modules/uORB/uORBManager_posix.cpp +# ${PX_SRC}/modules/uORB/objects_common.cpp +# ${PX_SRC}/modules/uORB/uORBUtils.cpp +# ${PX_SRC}/modules/uORB/uORB.cpp +# ) +#target_link_libraries( uorb_tests px4_platform ) -add_gtest(uorb_tests) +#add_gtest(uorb_tests) diff --git a/unittests/mixer_test.cpp b/unittests/mixer_test.cpp index 5be4cb032b..171bad252f 100644 --- a/unittests/mixer_test.cpp +++ b/unittests/mixer_test.cpp @@ -7,6 +7,6 @@ TEST(MixerTest, Mixer) { - char *args[] = {"empty", "../ROMFS/px4fmu_common/mixers/IO_pass.mix", "../ROMFS/px4fmu_common/mixers/quad_w.main.mix"}; - ASSERT_EQ(test_mixer(3, args), 0) << "IO_pass.mix failed"; + const char *args[] = {"empty", "../ROMFS/px4fmu_common/mixers/IO_pass.mix", "../ROMFS/px4fmu_common/mixers/quad_w.main.mix"}; + ASSERT_EQ(test_mixer(3, (char **)args), 0) << "IO_pass.mix failed"; } diff --git a/unittests/run_tests.sh b/unittests/run_tests.sh index ff5ee509ac..85aba9bd88 100755 --- a/unittests/run_tests.sh +++ b/unittests/run_tests.sh @@ -1,6 +1,14 @@ #!/bin/sh +cmake . make clean -make all -./mixer_test -./sbus2_test ../../../../data/sbus2/sbus2_r7008SB_gps_baro_tx_off.txt \ No newline at end of file +make all -j4 + +set -e + +#./param_test +./conversion_test +./autodeclination_test +./mixer_test > /dev/null +./sbus2_test +./rc_input_test diff --git a/unittests/sbus2_test.cpp b/unittests/sbus2_test.cpp index 54c7fc412b..828f2a6160 100644 --- a/unittests/sbus2_test.cpp +++ b/unittests/sbus2_test.cpp @@ -4,7 +4,9 @@ #include "../../src/systemcmds/tests/tests.h" #include -#include +// Enable S.BUS parser output +#define SBUS_DEBUG +#include #include #include @@ -20,6 +22,18 @@ TEST(SBUS2Test, SBUS2) ASSERT_TRUE(fp); warnx("loading data from: %s", filepath); + // if (argc < 2) + // errx(1, "Need a filename for the input file"); + + int byte_offset = 7; + + // if (argc > 2) { + // char* end; + // byte_offset = strtol(argv[2],&end,10); + // } + + warnx("RUNNING TEST WITH BYTE OFFSET OF: %d", byte_offset); + float f; unsigned x; int ret; @@ -31,38 +45,44 @@ TEST(SBUS2Test, SBUS2) } // Init the parser - uint8_t frame[30]; - unsigned partial_frame_count = 0; - //uint16_t rc_values[18]; - //uint16_t num_values; - //bool sbus_failsafe; - //bool sbus_frame_drop; - //uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]); + uint8_t frame[SBUS_BUFFER_SIZE]; + uint16_t rc_values[18]; + uint16_t num_values; + unsigned sbus_frame_drops = 0; + unsigned sbus_frame_resets = 0; + bool sbus_failsafe; + bool sbus_frame_drop; + uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]); - float last_time = 0; + int rate_limiter = 0; + unsigned last_drop = 0; while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) { - if (((f - last_time) * 1000 * 1000) > 3000) { - partial_frame_count = 0; - //warnx("FRAME RESET\n\n"); - } - frame[partial_frame_count] = x; - partial_frame_count++; + ASSERT_GT(ret, 0); - //warnx("%f: 0x%02x, first: 0x%02x, last: 0x%02x, pcount: %u", (double)f, x, frame[0], frame[24], partial_frame_count); - - if (partial_frame_count == sizeof(frame)) { - partial_frame_count = 0; - } - - last_time = f; + frame[0] = x; + unsigned len = 1; // Pipe the data into the parser - //hrt_abstime now = hrt_absolute_time(); + hrt_abstime now = hrt_absolute_time(); - //if (partial_frame_count % 25 == 0) - //sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, &sbus_failsafe, &sbus_frame_drop, max_channels); + // if (rate_limiter % byte_offset == 0) { + bool result = sbus_parse(now, &frame[0], len, rc_values, &num_values, + &sbus_failsafe, &sbus_frame_drop, &sbus_frame_drops, max_channels); + + if (result) { + //warnx("decoded packet"); + } + + // } + + if (last_drop != (sbus_frame_drops + sbus_frame_resets)) { + warnx("frame dropped, now #%d", (sbus_frame_drops + sbus_frame_resets)); + last_drop = sbus_frame_drops + sbus_frame_resets; + } + + rate_limiter++; } ASSERT_EQ(ret, EOF); diff --git a/unittests/st24_test.cpp b/unittests/st24_test.cpp index c7fded68d4..5b6a22fd27 100644 --- a/unittests/st24_test.cpp +++ b/unittests/st24_test.cpp @@ -12,12 +12,12 @@ TEST(ST24Test, ST24) { const char *filepath = "testdata/st24_data.txt"; - warnx("loading data from: %s", filepath); + //warnx("loading data from: %s", filepath); FILE *fp; fp = fopen(filepath, "rt"); - //ASSERT_TRUE(fp); + ASSERT_TRUE(fp); float f; unsigned x; diff --git a/unittests/sumd_test.cpp b/unittests/sumd_test.cpp index 65c944676c..40be7a9d8f 100644 --- a/unittests/sumd_test.cpp +++ b/unittests/sumd_test.cpp @@ -12,7 +12,7 @@ TEST(SUMDTest, SUMD) { const char *filepath = "testdata/sumd_data.txt"; - warnx("loading data from: %s", filepath); + //warnx("loading data from: %s", filepath); FILE *fp; diff --git a/unittests/testdata/sbus2_r7008SB.txt b/unittests/testdata/sbus2_r7008SB.txt index d504ba7056..3de5544284 100644 --- a/unittests/testdata/sbus2_r7008SB.txt +++ b/unittests/testdata/sbus2_r7008SB.txt @@ -1,5 +1,4 @@ Time [s],Value,Parity Error,Framing Error -0,0x0F,, 0.00012,0x11,, 0.00024,0x04,, 0.00036,0x20,, @@ -13,20 +12,6 @@ Time [s],Value,Parity Error,Framing Error 0.001320125,0x80,, 0.001440125,0x00,, 0.001560125,0x04,, -0.001680125,0x20,, -0.001800125,0x00,, -0.001920125,0x01,, -0.002040125,0x08,, -0.002160125,0x40,, -0.002280125,0x00,, -0.002400125,0x02,, -0.002520125,0x10,, -0.002640125,0x80,, -0.002760125,0x00,, -0.002880125,0x04,, -0.00504025,0x03,, -0.00516025,0xC4,, -0.00528025,0x00,, 0.015000625,0x0F,, 0.015120625,0x11,, 0.015240625,0x04,, @@ -114,7 +99,6 @@ Time [s],Value,Parity Error,Framing Error 0.061082625,0x03,, 0.061202625,0x10,, 0.061322625,0x80,, -0.061442625,0x00,, 0.061562625,0x04,, 0.061682625,0x20,, 0.061802625,0x00,, @@ -2118,16 +2102,6 @@ Time [s],Value,Parity Error,Framing Error 1.230450875,0x20,, 1.230570875,0xA8,, 1.230690875,0x01,, -1.230810875,0x08,, -1.230930875,0x16,, -1.231050875,0x50,, -1.231170875,0x03,, -1.231290875,0x10,, -1.231410875,0x80,, -1.231530875,0x00,, -1.231650875,0x04,, -1.231770875,0x20,, -1.231890875,0x00,, 1.232010875,0x01,, 1.232130875,0x08,, 1.232250875,0x40,,