diff --git a/.gitignore b/.gitignore index 07848d7a90..f936568899 100644 --- a/.gitignore +++ b/.gitignore @@ -23,6 +23,7 @@ build_*/ core cscope.out Firmware.sublime-workspace +user.sublime* Images/*.bin Images/*.px4 mavlink/include/mavlink/v0.9/ diff --git a/.gitmodules b/.gitmodules index b84b78548e..98a23841b5 100644 --- a/.gitmodules +++ b/.gitmodules @@ -31,3 +31,6 @@ [submodule "src/lib/DriverFramework"] path = src/lib/DriverFramework url = https://github.com/PX4/DriverFramework.git +[submodule "src/lib/ecl"] + path = src/lib/ecl + url = https://github.com/PX4/ecl.git diff --git a/.travis.yml b/.travis.yml index 209bcc8795..92ac917fa0 100644 --- a/.travis.yml +++ b/.travis.yml @@ -27,8 +27,8 @@ addons: - ccache - clang-3.5 - cmake - - g++-4.8 - - gcc-4.8 + - g++-4.9 + - gcc-4.9 - genromfs - libc6-i386 - libncurses5-dev @@ -55,7 +55,7 @@ before_install: && mkdir -p ~/bin && wget -O ~/bin/astyle https://github.com/PX4/astyle/releases/download/2.05.1/astyle-linux && chmod +x ~/bin/astyle && astyle --version - && if [ "$CXX" = "g++" ]; then export CXX="g++-4.8" CC="gcc-4.8"; fi + && if [ "$CXX" = "g++" ]; then export CXX="g++-4.9" CC="gcc-4.9"; fi ; elif [ "${TRAVIS_OS_NAME}" = "osx" ]; then brew tap PX4/homebrew-px4 @@ -81,8 +81,6 @@ before_script: - ln -s /usr/bin/ccache ~/bin/clang - ln -s /usr/bin/ccache ~/bin/clang-3.4 - ln -s /usr/bin/ccache ~/bin/clang-3.5 - - ln -s /usr/bin/ccache ~/bin/g++-4.8 - - ln -s /usr/bin/ccache ~/bin/gcc-4.8 - export PATH=~/bin:$PATH env: @@ -101,14 +99,16 @@ script: - echo 'Running Tests..' && make posix_sitl_default test - echo 'Running Unittests..' && cd unittests && ./run_tests.sh - cd .. + - echo 'Building NuttX px4fmu-v1 Firmware..' && make px4fmu-v1_default + - echo 'Building NuttX px4fmu-v2 Firmware..' && make px4fmu-v2_default +# Only provide UAVCAN firmware binaries for Pixracer and Pixhawk 3 - 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 .. - - 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 'Building NuttX px4-stm32f4discovery Firmware..' && make px4-stm32f4discovery_default - echo 'Running Tests..' && make px4fmu-v2_default test after_success: @@ -116,8 +116,9 @@ after_success: 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 && 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/ + && cp build_px4-stm32f4discovery_default/src/firmware/nuttx/nuttx-px4-stm32f4discovery-default.px4 px4-stm32f4discovery-default.px4 + && zip Firmware.zip px4fmu-v1_default.px4 px4fmu-v2_default.px4 px4fmu-v4_default.px4 px4-stm32f4discovery-default.px4 + && ./CI-Tools/s3cmd-put px4fmu-v1_default.px4 px4fmu-v2_default.px4 px4fmu-v4_default.px4 px4-stm32f4discovery-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 607fafb9ee..9bd26e839c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -245,6 +245,7 @@ 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_ecl PATH "src/lib/ecl") 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") diff --git a/Makefile b/Makefile index 1c269a8b56..2a5ab238f2 100644 --- a/Makefile +++ b/Makefile @@ -133,21 +133,24 @@ px4fmu-v2_default: px4fmu-v4_default: $(call cmake-build,nuttx_px4fmu-v4_default) -px4fmu-v2_simple: - $(call cmake-build,nuttx_px4fmu-v2_simple) +px4-stm32f4discovery_default: + $(call cmake-build,nuttx_px4-stm32f4discovery_default) + +px4fmu-v2_ekf2: + $(call cmake-build,nuttx_px4fmu-v2_ekf2) px4fmu-v2_lpe: $(call cmake-build,nuttx_px4fmu-v2_lpe) -nuttx_sim_simple: - $(call cmake-build,$@) - posix_sitl_default: $(call cmake-build,$@) posix_sitl_lpe: $(call cmake-build,$@) +posix_sitl_ekf2: + $(call cmake-build,$@) + ros_sitl_default: $(call cmake-build,$@) @@ -160,6 +163,12 @@ qurt_eagle_release: posix_eagle_release: $(call cmake-build,$@) +qurt_eagle_default: + $(call cmake-build,$@) + +posix_eagle_default: + $(call cmake-build,$@) + posix: posix_sitl_default sitl_deprecation: diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil index 7938c47bae..e37fc4389a 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -39,5 +39,7 @@ then param set FW_RR_P 0.3 fi +param set RWTO_TKOFF 1 + set HIL yes set MIXER AERT diff --git a/ROMFS/px4fmu_common/init.d/10020_3dr_quad b/ROMFS/px4fmu_common/init.d/10020_3dr_quad index 26509d128d..a1b2e90270 100644 --- a/ROMFS/px4fmu_common/init.d/10020_3dr_quad +++ b/ROMFS/px4fmu_common/init.d/10020_3dr_quad @@ -12,11 +12,11 @@ sh /etc/init.d/rc.mc_defaults if [ $AUTOCNF == yes ] then param set MC_ROLL_P 6.5 - param set MC_ROLLRATE_P 0.08 + param set MC_ROLLRATE_P 0.14 param set MC_ROLLRATE_I 0.1 param set MC_ROLLRATE_D 0.004 param set MC_PITCH_P 6.0 - param set MC_PITCHRATE_P 0.16 + param set MC_PITCHRATE_P 0.14 param set MC_PITCHRATE_I 0.09 param set MC_PITCHRATE_D 0.004 param set MC_YAW_P 4 diff --git a/ROMFS/px4fmu_common/init.d/13002_firefly6 b/ROMFS/px4fmu_common/init.d/13002_firefly6 index d840f6c7e0..96569ddc94 100644 --- a/ROMFS/px4fmu_common/init.d/13002_firefly6 +++ b/ROMFS/px4fmu_common/init.d/13002_firefly6 @@ -46,6 +46,6 @@ set PWM_AUX_MAX 2000 set MAV_TYPE 21 param set VT_MOT_COUNT 6 -param set VT_FW_MOT_OFF 23 +param set VT_FW_MOT_OFFID 34 param set VT_IDLE_PWM_MC 1080 param set VT_TYPE 1 diff --git a/ROMFS/px4fmu_common/init.d/4030_solo b/ROMFS/px4fmu_common/init.d/4030_solo new file mode 100644 index 0000000000..5fbdcc8e48 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/4030_solo @@ -0,0 +1,88 @@ +#!nsh +# +# @name Generic Quadrotor X config +# +# @type Quadrotor x +# +# @maintainer Andreas Antener +# + +sh /etc/init.d/rc.mc_defaults + +if [ $AUTOCNF == yes ] +then + # tuning + param set MC_PITCHRATE_P 0.11 + param set MC_ROLLRATE_P 0.11 + param set MPC_MANTHR_MIN 0.08 + param set MPC_XY_VEL_MAX 3.0 + param set MPC_Z_VEL_MAX 2.0 + + # INAV: higher GPS weights for better altitude control + param set INAV_W_Z_BARO 0.3 + param set INAV_W_Z_GPS_P 0.8 + param set INAV_W_Z_GPS_V 0.8 + + # takeoff, land and RTL settings + param set MIS_TAKEOFF_ALT 4.0 + param set COM_DISARM_LAND 1 + param set RTL_LAND_DELAY 1 + param set RTL_DESCEND_ALT 5.0 + param set RTL_RETURN_ALT 15.0 + param set MPC_TILTMAX_LND 8.0 + param set MPC_LAND_SPEED 0.4 + param set MPC_HOLD_MAX_Z 1.5 + param set MPC_TKO_JMPSPD 2.0 + param set MPC_TKO_SPEED 1.5 + + # setup + # main board rotation: pitch 180 + param set SENS_BOARD_ROT 12 + # solo external mag rotation + param set CAL_MAG0_ROT 30 + # no safety switch + param set CBRK_IO_SAFETY 22027 + # battery voltage not available yet + param set CBRK_SUPPLY_CHK 894281 + + # RC configuration + param set RC_MAP_MODE_SW 5 + param set RC_MAP_PITCH 2 + param set RC_MAP_ROLL 1 + param set RC_MAP_THROTTLE 3 + param set RC_MAP_YAW 4 + + param set RC1_DZ 10 + param set RC1_MAX 1988 + param set RC1_MIN 1003 + param set RC1_REV 1 + param set RC1_TRIM 1499 + param set RC2_DZ 10 + param set RC2_MAX 1987 + param set RC2_MIN 1023 + param set RC2_REV -1 + param set RC2_TRIM 1499 + param set RC3_DZ 10 + param set RC3_MAX 1877 + param set RC3_MIN 1023 + param set RC3_REV 1 + param set RC3_TRIM 1023 + param set RC4_DZ 10 + param set RC4_MAX 1998 + param set RC4_MIN 1012 + param set RC4_REV 1 + param set RC4_TRIM 1500 + param set RC5_DZ 10 + param set RC5_MAX 2000 + param set RC5_MIN 1000 + param set RC5_REV 1 + param set RC5_TRIM 1500 +fi + +set MIXER solo + +set PWM_OUT 1234 +set MIXER_AUX none + +# enable high-speed link on telem 1 +set MAVLINK_F "-d /dev/ttyS1 -b 921600 -r 80000 -m onboard -x" diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 4c6cdbd7cd..9c782d7bb0 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -17,7 +17,7 @@ else attitude_estimator_q start local_position_estimator start else - ekf_att_pos_estimator start + ekf2 start fi fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index a2a726bd7c..5e859ae222 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -60,9 +60,6 @@ unset FRC if [ $MODE == autostart ] then - # Try to get an USB console - # REBOOTWORK this needs to start after the flight control loop - nshterm /dev/ttyACM0 & # # Start the ORB (first app to start) @@ -844,6 +841,15 @@ then px4flow start & fi +# Start USB shell if no microSD present, MAVLink else +if [ $LOG_FILE == /dev/null ] +then + # Try to get an USB console + nshterm /dev/ttyACM0 & +else + mavlink start -r 800000 -d /dev/ttyACM0 -m config -x +fi + if [ $EXIT_ON_END == yes ] then echo "Exit from nsh" diff --git a/ROMFS/px4fmu_common/mixers/solo.main.mix b/ROMFS/px4fmu_common/mixers/solo.main.mix new file mode 100644 index 0000000000..12a3bee20c --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/solo.main.mix @@ -0,0 +1,7 @@ +Multirotor mixer for PX4FMU +=========================== + +This file defines a single mixer for a quadrotor in the X configuration. All controls +are mixed 100%. + +R: 4x 10000 10000 10000 0 diff --git a/Tools/jMAVSim b/Tools/jMAVSim index 4267771753..0876a46d53 160000 --- a/Tools/jMAVSim +++ b/Tools/jMAVSim @@ -1 +1 @@ -Subproject commit 4267771753c4bb5d0696a678856620229523162e +Subproject commit 0876a46d53954db34775151f73db024bcfc656a0 diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index 03a749a3b3..6af47249ca 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit 03a749a3b3238c014329e9e8da57d102ede6f4f0 +Subproject commit 6af47249caefa3ddb6de2c31ae061aff1858fd0e diff --git a/Vagrantfile b/Vagrantfile index dfa4da4d8b..685d346e59 100644 --- a/Vagrantfile +++ b/Vagrantfile @@ -37,17 +37,42 @@ Vagrant.configure(2) do |config| # the path on the host to the actual folder. The second argument is # the path on the guest to mount the folder. And the optional third # argument is a set of non-required options. + # NFS should be faster: https://stefanwrobel.com/how-to-make-vagrant-performance-not-suck config.vm.synced_folder ".", "/Firmware", type: "nfs" # Provider-specific configuration so you can fine-tune various # backing providers for Vagrant. These expose provider-specific options. # Example for VirtualBox: # + + # This is to configure the machine to be as fast as possible + # Alternative: https://github.com/rdsubhas/vagrant-faster config.vm.provider "virtualbox" do |vb| # Display the VirtualBox GUI when booting the machine vb.gui = false vb.customize ["modifyvm", :id, "--ioapic", "on"] - vb.customize ["modifyvm", :id, "--cpus", "2"] + #vb.customize ["modifyvm", :id, "--cpus", "2"] + + config.vm.provider "virtualbox" do |v| + host = RbConfig::CONFIG['host_os'] + + # Give VM 1/4 system memory & access to all cpu cores on the host + if host =~ /darwin/ + cpus = `sysctl -n hw.ncpu`.to_i + # sysctl returns Bytes and we need to convert to MB + mem = `sysctl -n hw.memsize`.to_i / 1024 / 1024 / 4 + elsif host =~ /linux/ + cpus = `nproc`.to_i + # meminfo shows KB and we need to convert to MB + mem = `grep 'MemTotal' /proc/meminfo | sed -e 's/MemTotal://' -e 's/ kB//'`.to_i / 1024 / 4 + else # sorry Windows folks, I can't help you + cpus = 2 + mem = 1024 + end + + v.customize ["modifyvm", :id, "--memory", mem] + v.customize ["modifyvm", :id, "--cpus", cpus] + end # Since make and other tools freak out if they see timestamps # from the future and we share directories, tightly lock the host and guest clocks together (clock sync if more than 2 seconds off) @@ -57,7 +82,7 @@ Vagrant.configure(2) do |config| vb.customize ["guestproperty", "set", :id, "/VirtualBox/GuestAdd/VBoxService/--timesync-set-on-restore", "1"] # Customize the amount of memory on the VM: - vb.memory = "2048" + #vb.memory = "2048" end # # View the documentation for the provider you are using for more diff --git a/cmake/configs/nuttx_px4-stm32f4discovery_default.cmake b/cmake/configs/nuttx_px4-stm32f4discovery_default.cmake new file mode 100644 index 0000000000..8815992e40 --- /dev/null +++ b/cmake/configs/nuttx_px4-stm32f4discovery_default.cmake @@ -0,0 +1,87 @@ +include(nuttx/px4_impl_nuttx) + +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake) + +set(config_module_list + # + # Board support modules + # + drivers/device + drivers/stm32 + drivers/led + drivers/boards/px4-stm32f4discovery + + # + # System commands + # + systemcmds/bl_update + systemcmds/mixer + systemcmds/param + systemcmds/perf + systemcmds/reboot + systemcmds/top + systemcmds/config + systemcmds/nshterm + systemcmds/ver + + # + # Library modules + # + modules/param + modules/systemlib + modules/systemlib/mixer + modules/controllib + modules/uORB + + # + # Libraries + # + #lib/mathlib/CMSIS + lib/mathlib + lib/mathlib/math/filter + lib/ecl + lib/external_lgpl + lib/geo + lib/conversion + platforms/nuttx + + # had to add for cmake, not sure why wasn't in original config + platforms/common + platforms/nuttx/px4_layer + + # + # 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 + ) + +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/nuttx_px4fmu-v1_default.cmake b/cmake/configs/nuttx_px4fmu-v1_default.cmake index 902447c282..de8753341c 100644 --- a/cmake/configs/nuttx_px4fmu-v1_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v1_default.cmake @@ -27,16 +27,16 @@ set(config_module_list drivers/trone drivers/gps drivers/pwm_out_sim - drivers/hott - drivers/hott/hott_telemetry - drivers/hott/hott_sensors + #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/mkblctrl drivers/px4flow # @@ -73,6 +73,7 @@ set(config_module_list modules/attitude_estimator_q modules/ekf_att_pos_estimator modules/position_estimator_inav + modules/ekf2 # # Vehicle Control @@ -113,6 +114,7 @@ set(config_module_list lib/launchdetection lib/terrain_estimation lib/runway_takeoff + lib/tailsitter_recovery 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 9fafe851be..9b2c674167 100644 --- a/cmake/configs/nuttx_px4fmu-v2_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake @@ -23,16 +23,16 @@ set(config_module_list drivers/l3gd20 drivers/hmc5883 drivers/ms5611 - drivers/mb12xx + #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/hott + #drivers/hott/hott_telemetry + #drivers/hott/hott_sensors drivers/blinkm drivers/airspeed drivers/ets_airspeed @@ -45,7 +45,7 @@ set(config_module_list drivers/gimbal drivers/pwm_input drivers/camera_trigger - drivers/bst + drivers/bst # # System commands @@ -123,6 +123,7 @@ set(config_module_list lib/launchdetection lib/terrain_estimation lib/runway_takeoff + lib/tailsitter_recovery platforms/nuttx # had to add for cmake, not sure why wasn't in original config @@ -132,7 +133,7 @@ set(config_module_list # # OBC challenge # - modules/bottle_drop + #modules/bottle_drop # # Rover apps diff --git a/cmake/configs/nuttx_px4fmu-v2_simple.cmake b/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake similarity index 65% rename from cmake/configs/nuttx_px4fmu-v2_simple.cmake rename to cmake/configs/nuttx_px4fmu-v2_ekf2.cmake index 070fbd703f..77526b4afd 100644 --- a/cmake/configs/nuttx_px4fmu-v2_simple.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_ekf2.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 @@ -21,27 +23,29 @@ set(config_module_list drivers/l3gd20 drivers/hmc5883 drivers/ms5611 - drivers/mb12xx + #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/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/mkblctrl drivers/px4flow drivers/oreoled drivers/gimbal drivers/pwm_input drivers/camera_trigger + drivers/bst # # System commands @@ -53,6 +57,7 @@ set(config_module_list systemcmds/pwm systemcmds/esc_calib systemcmds/reboot + #systemcmds/topic_listener systemcmds/top systemcmds/config systemcmds/nshterm @@ -60,6 +65,35 @@ set(config_module_list systemcmds/dumpfile systemcmds/ver + # + # 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/ekf2 + 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 # @@ -87,34 +121,47 @@ set(config_module_list lib/geo_lookup lib/conversion lib/launchdetection + lib/terrain_estimation + lib/runway_takeoff + lib/tailsitter_recovery 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 + #examples/px4_simple_app # Tutorial code from # https://px4.io/dev/daemon - examples/px4_daemon_app + #examples/px4_daemon_app # Tutorial code from # https://px4.io/dev/debug_values - examples/px4_mavlink_debug + #examples/px4_mavlink_debug # Tutorial code from # https://px4.io/dev/example_fixedwing_control - examples/fixedwing_control + #examples/fixedwing_control # Hardware test - examples/hwtest + #examples/hwtest ) set(config_extra_builtin_cmds @@ -128,10 +175,12 @@ set(config_io_board 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 + #${CMAKE_SOURCE_DIR}/src/lib/mathlib/CMSIS/libarm_cortexM3l_math.a ) add_custom_target(sercon) diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake index 5c9de8fd56..8bb4005353 100644 --- a/cmake/configs/nuttx_px4fmu-v4_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake @@ -42,6 +42,7 @@ set(config_module_list drivers/gimbal drivers/pwm_input drivers/camera_trigger + drivers/bst # # System commands @@ -80,6 +81,7 @@ set(config_module_list modules/attitude_estimator_q modules/ekf_att_pos_estimator modules/position_estimator_inav + modules/ekf2 # # Vehicle Control @@ -121,6 +123,7 @@ set(config_module_list lib/launchdetection lib/terrain_estimation lib/runway_takeoff + lib/tailsitter_recovery platforms/nuttx # had to add for cmake, not sure why wasn't in original config diff --git a/cmake/configs/nuttx_sim_simple.cmake b/cmake/configs/nuttx_sim_simple.cmake deleted file mode 100644 index 068c6409f2..0000000000 --- a/cmake/configs/nuttx_sim_simple.cmake +++ /dev/null @@ -1,21 +0,0 @@ -include(nuttx/px4_impl_nuttx) - -message(WARNING "this is a work in progress and doesn't build yet") - -set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake) - -set(config_module_list - #platforms/nuttx - #platforms/nuttx/px4_layer - platforms/common - #drivers/led - drivers/device - #modules/systemlib - #modules/uORB - #examples/px4_simple_app - #lib/mathlib/math/filter - #lib/conversion - ) - -set(config_extra_builtin_cmds - ) diff --git a/cmake/configs/posix_eagle_default.cmake b/cmake/configs/posix_eagle_default.cmake index 76d9711187..9de00fb694 100644 --- a/cmake/configs/posix_eagle_default.cmake +++ b/cmake/configs/posix_eagle_default.cmake @@ -35,11 +35,13 @@ set(config_module_list lib/mathlib lib/mathlib/math/filter + lib/conversion + lib/ecl lib/geo lib/geo_lookup - lib/conversion lib/terrain_estimation lib/runway_takeoff + lib/tailsitter_recovery platforms/common platforms/posix/px4_layer diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake index 4f4be42ab2..66eca5205d 100644 --- a/cmake/configs/posix_sitl_default.cmake +++ b/cmake/configs/posix_sitl_default.cmake @@ -59,6 +59,7 @@ set(config_module_list lib/launchdetection lib/terrain_estimation lib/runway_takeoff + lib/tailsitter_recovery examples/px4_simple_app ) diff --git a/cmake/configs/posix_sitl_ekf2.cmake b/cmake/configs/posix_sitl_ekf2.cmake new file mode 100644 index 0000000000..20c2d82ff9 --- /dev/null +++ b/cmake/configs/posix_sitl_ekf2.cmake @@ -0,0 +1,99 @@ +include(posix/px4_impl_posix) + +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake) + +set(config_module_list + drivers/device + drivers/boards/sitl + drivers/pwm_out_sim + platforms/common + platforms/posix/px4_layer + platforms/posix/work_queue + platforms/posix/drivers/adcsim + platforms/posix/drivers/gpssim + platforms/posix/drivers/tonealrmsim + platforms/posix/drivers/accelsim + platforms/posix/drivers/airspeedsim + platforms/posix/drivers/barosim + platforms/posix/drivers/gyrosim + platforms/posix/drivers/rgbledsim + platforms/posix/drivers/ledsim + systemcmds/param + systemcmds/mixer + systemcmds/ver + systemcmds/esc_calib + systemcmds/reboot + systemcmds/topic_listener + systemcmds/perf + modules/uORB + modules/param + modules/systemlib + modules/systemlib/mixer + modules/sensors + modules/simulator + modules/mavlink + modules/attitude_estimator_ekf + modules/attitude_estimator_q + modules/ekf_att_pos_estimator + modules/ekf2 + modules/position_estimator_inav + modules/navigator + modules/vtol_att_control + modules/mc_pos_control + modules/mc_att_control + modules/mc_pos_control_multiplatform + modules/mc_att_control_multiplatform + modules/land_detector + modules/fw_att_control + modules/fw_pos_control_l1 + modules/dataman + modules/sdlog2 + modules/commander + modules/controllib + lib/mathlib + lib/mathlib/math/filter + lib/conversion + lib/ecl + lib/external_lgpl + lib/geo + lib/geo_lookup + lib/launchdetection + lib/terrain_estimation + lib/runway_takeoff + lib/tailsitter_recovery + examples/px4_simple_app + ) + +set(config_extra_builtin_cmds + serdis + sercon + ) + +set(config_sitl_rcS + posix-configs/SITL/init/rcS_ekf2 + CACHE FILEPATH "init script for sitl" + ) + +set(config_sitl_viewer + jmavsim + CACHE STRING "viewer for sitl" + ) +set_property(CACHE config_sitl_viewer + PROPERTY STRINGS "jmavsim;none") + +set(config_sitl_debugger + disable + CACHE STRING "debugger for sitl" + ) +set_property(CACHE config_sitl_debugger + PROPERTY STRINGS "disable;gdb;lldb") + + + +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/qurt_eagle_default.cmake b/cmake/configs/qurt_eagle_default.cmake new file mode 100644 index 0000000000..e39a96fe3e --- /dev/null +++ b/cmake/configs/qurt_eagle_default.cmake @@ -0,0 +1,90 @@ +include(qurt/px4_impl_qurt) + +#if ("${HEXAGON_DRIVERS_ROOT}" #STREQUAL "") +# message(FATAL_ERROR "HEXAGON_DRIVERS_ROOT is not set") +#endif() + +#if ("${EAGLE_DRIVERS_SRC}" STREQUAL "") +# message(FATAL_ERROR "EAGLE_DRIVERS_SRC is not set") +#endif() + +#include_directories(${HEXAGON_DRIVERS_ROOT}/inc) + +# For Actual flight we need to link against the driver dynamic libraries +set(target_libraries + -L${HEXAGON_DRIVERS_ROOT}/libs +# The plan is to replace these with our drivers +# mpu9x50 +# uart_esc +# csr_gps +# rc_receiver + ) + + +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake) + +set(config_module_list + # + # Board support modules + # + drivers/device + modules/sensors +# The plan is to replace these with our drivers +# $(EAGLE_DRIVERS_SRC)/mpu9x50 +# $(EAGLE_DRIVERS_SRC)/uart_esc +# $(EAGLE_DRIVERS_SRC)/rc_receiver +# $(EAGLE_DRIVERS_SRC)/csr_gps + + # + # System commands + # + systemcmds/param + + # + # Estimation modules (EKF/ SO3 / other filters) + # + #modules/attitude_estimator_ekf + modules/ekf_att_pos_estimator + modules/attitude_estimator_q + modules/position_estimator_inav + + # + # Vehicle Control + # + modules/mc_att_control + modules/mc_pos_control + + # + # Library modules + # + modules/param + modules/systemlib + modules/systemlib/mixer + modules/uORB + modules/commander + modules/controllib + + # + # Libraries + # + lib/mathlib + lib/mathlib/math/filter + lib/geo + lib/geo_lookup + lib/conversion + lib/terrain_estimation + lib/runway_takeoff + lib/tailsitter_recovery + + # + # QuRT port + # + platforms/common + platforms/qurt/px4_layer + platforms/posix/work_queue + + # + # sources for muorb over fastrpc + # + modules/muorb/adsp + ) diff --git a/cmake/configs/qurt_eagle_hil.cmake b/cmake/configs/qurt_eagle_hil.cmake index 87dd29152e..0f2e92b0c0 100644 --- a/cmake/configs/qurt_eagle_hil.cmake +++ b/cmake/configs/qurt_eagle_hil.cmake @@ -49,6 +49,7 @@ set(config_module_list lib/conversion lib/terrain_estimation lib/runway_takeoff + lib/tailsitter_recovery modules/controllib # diff --git a/cmake/configs/qurt_eagle_release.cmake b/cmake/configs/qurt_eagle_release.cmake index 62a20263e5..67d84c8fa7 100644 --- a/cmake/configs/qurt_eagle_release.cmake +++ b/cmake/configs/qurt_eagle_release.cmake @@ -72,6 +72,7 @@ set(config_module_list lib/conversion lib/terrain_estimation lib/runway_takeoff + lib/tailsitter_recovery # # QuRT port diff --git a/cmake/configs/qurt_eagle_travis.cmake b/cmake/configs/qurt_eagle_travis.cmake index 75cb5f2394..85fae3373b 100644 --- a/cmake/configs/qurt_eagle_travis.cmake +++ b/cmake/configs/qurt_eagle_travis.cmake @@ -54,6 +54,7 @@ set(config_module_list lib/ecl lib/terrain_estimation lib/runway_takeoff + lib/tailsitter_recovery # # QuRT port diff --git a/cmake/nuttx/px4_impl_nuttx.cmake b/cmake/nuttx/px4_impl_nuttx.cmake index ae3d95eaf5..783886e373 100644 --- a/cmake/nuttx/px4_impl_nuttx.cmake +++ b/cmake/nuttx/px4_impl_nuttx.cmake @@ -467,6 +467,14 @@ function(px4_os_add_flags) -mfpu=fpv4-sp-d16 -mfloat-abi=hard ) + elseif (${BOARD} STREQUAL "px4-stm32f4discovery") + set(cpu_flags + -mcpu=cortex-m4 + -mthumb + -march=armv7e-m + -mfpu=fpv4-sp-d16 + -mfloat-abi=hard + ) elseif (${BOARD} STREQUAL "aerocore") set(cpu_flags -mcpu=cortex-m4 diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index db5cf66d1a..21d36c9e23 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit db5cf66d1a7f549ccbaa92b5476e78754e008297 +Subproject commit 21d36c9e23867b09145dc61b11941fb606726cc2 diff --git a/msg/position_setpoint.msg b/msg/position_setpoint.msg index ae6756aa8b..e46e2990c1 100644 --- a/msg/position_setpoint.msg +++ b/msg/position_setpoint.msg @@ -33,3 +33,4 @@ float32 a_y # acceleration y setpoint float32 a_z # acceleration z setpoint bool acceleration_valid # true if acceleration setpoint is valid/should be used bool acceleration_is_force # interprete acceleration as force +float32 acceptance_radius # navigation acceptance_radius if we're doint waypoint navigation diff --git a/msg/vehicle_command_ack.msg b/msg/vehicle_command_ack.msg new file mode 100644 index 0000000000..2c90469c0e --- /dev/null +++ b/msg/vehicle_command_ack.msg @@ -0,0 +1,8 @@ +uint8 VEHICLE_RESULT_ACCEPTED = 0 +uint8 VEHICLE_RESULT_TEMPORARILY_REJECTED = 1 +uint8 VEHICLE_RESULT_DENIED = 2 +uint8 VEHICLE_RESULT_UNSUPPORTED = 3 +uint8 VEHICLE_RESULT_FAILED = 4 + +uint16 command +uint8 result diff --git a/msg/vehicle_global_position.msg b/msg/vehicle_global_position.msg index cee381e871..8aa7917df0 100644 --- a/msg/vehicle_global_position.msg +++ b/msg/vehicle_global_position.msg @@ -4,18 +4,18 @@ # estimator, which will take more sources of information into account than just GPS, # e.g. control inputs of the vehicle in a Kalman-filter implementation. # -uint64 timestamp # Time of this estimate, in microseconds since system start -uint64 time_utc_usec # GPS UTC timestamp in microseconds -float64 lat # Latitude in degrees -float64 lon # Longitude in degrees -float32 alt # Altitude AMSL in meters -float32 vel_n # Ground north velocity, m/s -float32 vel_e # Ground east velocity, m/s -float32 vel_d # Ground downside velocity, m/s -float32 yaw # Yaw in radians -PI..+PI. -float32 eph # Standard deviation of position estimate horizontally -float32 epv # Standard deviation of position vertically -float32 terrain_alt # Terrain altitude in m, WGS84 +uint64 timestamp # Time of this estimate since system start, (microseconds) +uint64 time_utc_usec # GPS UTC timestamp, (microseconds) +float64 lat # Latitude, (degrees) +float64 lon # Longitude, (degrees) +float32 alt # Altitude AMSL, (meters) +float32 vel_n # North velocity in NED earth-fixed frame, (metres/sec) +float32 vel_e # East velocity in NED earth-fixed frame, (metres/sec) +float32 vel_d # Down velocity in NED earth-fixed frame, (metres/sec) +float32 yaw # Euler yaw angle relative to NED earth-fixed frame, -PI..+PI, (radians) +float32 eph # Standard deviation of horizontal position error, (metres) +float32 epv # Standard deviation of vertical position error, (metres) +float32 terrain_alt # Terrain altitude WGS84, (metres) 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 +float32 pressure_alt # Pressure altitude AMSL, (metres) diff --git a/msg/vehicle_gps_position.msg b/msg/vehicle_gps_position.msg index 718144f327..697bfb51d5 100644 --- a/msg/vehicle_gps_position.msg +++ b/msg/vehicle_gps_position.msg @@ -1,30 +1,30 @@ # GPS position in WGS84 coordinates. -uint64 timestamp_position # Timestamp for position information -int32 lat # Latitude in 1E-7 degrees +uint64 timestamp_position # Time of the position estimates since system start, (microseconds) +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 +int32 alt # Altitude in 1E-3 meters above MSL, (millimetres) +int32 alt_ellipsoid # Altitude in 1E-3 meters bove Ellipsoid, (millimetres) -uint64 timestamp_variance -float32 s_variance_m_s # speed accuracy estimate m/s -float32 c_variance_rad # course accuracy estimate rad +uint64 timestamp_variance # Time of the accuracy estimates since system start, (microseconds) +float32 s_variance_m_s # GPS speed accuracy estimate, (metres/sec) +float32 c_variance_rad # GPS course accuracy estimate, (radians) uint8 fix_type # 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic, float, 6: Real-Time Kinematic, fixed, 8: Extrapolated. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. -float32 eph # GPS HDOP horizontal dilution of position in m -float32 epv # GPS VDOP horizontal dilution of position in m +float32 eph # GPS horizontal position accuracy (metres) +float32 epv # GPS vertical position accuracy (metres) int32 noise_per_ms # GPS noise per millisecond int32 jamming_indicator # indicates jamming is occurring -uint64 timestamp_velocity # Timestamp for velocity informations -float32 vel_m_s # GPS ground speed (m/s) -float32 vel_n_m_s # GPS ground speed in m/s -float32 vel_e_m_s # GPS ground speed in m/s -float32 vel_d_m_s # GPS ground speed in m/s -float32 cog_rad # Course over ground (NOT heading, but direction of movement) in rad, -PI..PI -bool vel_ned_valid # Flag to indicate if NED speed is valid +uint64 timestamp_velocity # Time of the velocity estimates since system start, (microseconds) +float32 vel_m_s # GPS ground speed, (metres/sec) +float32 vel_n_m_s # GPS North velocity, (metres/sec) +float32 vel_e_m_s # GPS East velocity, (metres/sec) +float32 vel_d_m_s # GPS Down velocity, (metres/sec) +float32 cog_rad # Course over ground (NOT heading, but direction of movement), -PI..PI, (radians) +bool vel_ned_valid # True if NED velocity is valid -uint64 timestamp_time # Timestamp for time information +uint64 timestamp_time # Time of the UTC timestamp since system start, (microseconds) uint64 time_utc_usec # Timestamp (microseconds, UTC), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0 uint8 satellites_used # Number of satellites used diff --git a/msg/vehicle_local_position.msg b/msg/vehicle_local_position.msg index 4da027ae77..89c141c417 100644 --- a/msg/vehicle_local_position.msg +++ b/msg/vehicle_local_position.msg @@ -1,36 +1,36 @@ # Fused local position in NED. -uint64 timestamp # Time of this estimate, in microseconds since system start -bool xy_valid # true if x and y are valid -bool z_valid # true if z is valid -bool v_xy_valid # true if vy and vy are valid -bool v_z_valid # true if vz is valid +uint64 timestamp # Time of this estimate since system start, (microseconds) +bool xy_valid # true if x and y are valid +bool z_valid # true if z is valid +bool v_xy_valid # true if vy and vy are valid +bool v_z_valid # true if vz is valid # Position in local NED frame -float32 x # X position in meters in NED earth-fixed frame -float32 y # X position in meters in NED earth-fixed frame -float32 z # Z position in meters in NED earth-fixed frame (negative altitude) +float32 x # North position in NED earth-fixed frame, (metres) +float32 y # East position in NED earth-fixed frame, (metres) +float32 z # Down position (negative altitude) in NED earth-fixed frame, (metres) # Velocity in NED frame -float32 vx # Ground X Speed (Latitude), m/s in NED -float32 vy # Ground Y Speed (Longitude), m/s in NED -float32 vz # Ground Z Speed (Altitude), m/s in NED +float32 vx # North velocity in NED earth-fixed frame, (metres/sec) +float32 vy # East velocity in NED earth-fixed frame, (metres/sec) +float32 vz # Down velocity in NED earth-fixed frame, (metres/sec) # Heading -float32 yaw +float32 yaw # Euler yaw angle transforming the tangent plane relative to NED earth-fixed frame, -PI..+PI, (radians) -# Reference position in GPS / WGS84 frame -bool xy_global # true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) -bool z_global # true if z is valid and has valid global reference (ref_alt) -uint64 ref_timestamp # Time when reference position was set -float64 ref_lat # Reference point latitude in degrees -float64 ref_lon # Reference point longitude in degrees -float32 ref_alt # Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level +# Position of reference point (local NED frame origin) in global (GPS / WGS84) frame +bool xy_global # true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) +bool z_global # true if z is valid and has valid global reference (ref_alt) +uint64 ref_timestamp # Time when reference position was set since system start, (microseconds) +float64 ref_lat # Reference point latitude, (degrees) +float64 ref_lon # Reference point longitude, (degrees) +float32 ref_alt # Reference altitude AMSL, MUST be set to current (not at reference point!) ground level, (metres) # Distance to surface -float32 dist_bottom # Distance to bottom surface (ground) -float32 dist_bottom_rate # Distance to bottom surface (ground) change rate -uint64 surface_bottom_timestamp # Time when new bottom surface found -bool dist_bottom_valid # true if distance to bottom surface is valid -float32 eph -float32 epv +float32 dist_bottom # Distance from from bottom surface to ground, (metres) +float32 dist_bottom_rate # Rate of change of distance from bottom surface to ground, (metres/sec) +uint64 surface_bottom_timestamp # Time when new bottom surface found since system start, (microseconds) +bool dist_bottom_valid # true if distance to bottom surface is valid +float32 eph # Standard deviation of horizontal position error, (metres) +float32 epv # Standard deviation of vertical position error, (metres) \ No newline at end of file diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index f3a1767dde..8a67f928e5 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -10,7 +10,8 @@ uint8 MAIN_STATE_OFFBOARD = 7 uint8 MAIN_STATE_STAB = 8 uint8 MAIN_STATE_RATTITUDE = 9 uint8 MAIN_STATE_AUTO_TAKEOFF = 10 -uint8 MAIN_STATE_MAX = 11 +uint8 MAIN_STATE_AUTO_LAND = 11 +uint8 MAIN_STATE_MAX = 12 # 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. @@ -104,6 +105,7 @@ uint16 counter # incremented by the writing thread everytime new data is store uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data uint8 main_state # main state machine +uint8 main_state_prev # previous main state uint8 nav_state # set navigation state machine to specified value uint8 arming_state # current arming state uint8 hil_state # current hil state diff --git a/nuttx-configs/px4fmu-v4/nsh/defconfig b/nuttx-configs/px4fmu-v4/nsh/defconfig index 5e9c276494..f4533279d4 100644 --- a/nuttx-configs/px4fmu-v4/nsh/defconfig +++ b/nuttx-configs/px4fmu-v4/nsh/defconfig @@ -552,7 +552,7 @@ CONFIG_UART7_SERIAL_CONSOLE=y # USART1 Configuration # CONFIG_USART1_RXBUFSIZE=600 -CONFIG_USART1_TXBUFSIZE=1100 +CONFIG_USART1_TXBUFSIZE=2000 CONFIG_USART1_BAUD=115200 CONFIG_USART1_BITS=8 CONFIG_USART1_PARITY=0 @@ -669,7 +669,7 @@ CONFIG_CDCACM_BULKIN_REQLEN=96 CONFIG_CDCACM_RXBUFSIZE=600 CONFIG_CDCACM_TXBUFSIZE=4000 CONFIG_CDCACM_VENDORID=0x26ac -CONFIG_CDCACM_PRODUCTID=0x0011 +CONFIG_CDCACM_PRODUCTID=0x0012 CONFIG_CDCACM_VENDORSTR="3D Robotics" CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v4.x" # CONFIG_USBMSC is not set diff --git a/posix-configs/SITL/init/rcS_ekf2_jmavsim_iris b/posix-configs/SITL/init/rcS_ekf2_jmavsim_iris new file mode 100644 index 0000000000..58237da408 --- /dev/null +++ b/posix-configs/SITL/init/rcS_ekf2_jmavsim_iris @@ -0,0 +1,62 @@ +uorb start +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.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 2293768 +param set CAL_ACC0_ID 1376264 +param set CAL_ACC1_ID 1310728 +param set CAL_MAG0_ID 196616 +param set CAL_GYRO0_XOFF 0.01 +param set CAL_ACC0_XOFF 0.01 +param set CAL_ACC0_YOFF -0.01 +param set CAL_ACC0_ZOFF 0.01 +param set CAL_ACC0_XSCALE 1.01 +param set CAL_ACC0_YSCALE 1.01 +param set CAL_ACC0_ZSCALE 1.01 +param set CAL_ACC1_XOFF 0.01 +param set CAL_MAG0_XOFF 0.01 +param set MPC_XY_P 0.4 +param set MPC_XY_VEL_P 0.2 +param set MPC_XY_VEL_D 0.005 +param set SENS_BOARD_ROT 0 +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 +barosim start +adcsim start +gpssim start +pwm_out_sim mode_pwm +sleep 1 +sensors start +commander start +land_detector start multicopter +navigator start +mc_pos_control start +mc_att_control start +mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix +mavlink start -u 14556 -r 2000000 +mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556 +mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556 +mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556 +mavlink stream -r 80 -s ATTITUDE -u 14556 +mavlink stream -r 80 -s ATTITUDE_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 +ekf2 start diff --git a/posix-configs/SITL/init/rcS_gazebo_iris b/posix-configs/SITL/init/rcS_gazebo_iris index 95be984fb4..a85419a9e4 100644 --- a/posix-configs/SITL/init/rcS_gazebo_iris +++ b/posix-configs/SITL/init/rcS_gazebo_iris @@ -50,6 +50,7 @@ mc_pos_control start mc_att_control start mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_w.main.mix mavlink start -u 14556 -r 2000000 +mavlink start -u 14557 -r 2000000 -m onboard 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 diff --git a/posix-configs/SITL/init/rcS_gazebo_tailsitter b/posix-configs/SITL/init/rcS_gazebo_tailsitter index c88948cf55..1f4fa0ad60 100644 --- a/posix-configs/SITL/init/rcS_gazebo_tailsitter +++ b/posix-configs/SITL/init/rcS_gazebo_tailsitter @@ -52,6 +52,7 @@ 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 start -u 14557 -r 2000000 -m onboard 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 @@ -61,4 +62,4 @@ 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 +sdlog2 start -r 200 -e -t -a diff --git a/posix-configs/SITL/init/rcS_jmavsim_iris b/posix-configs/SITL/init/rcS_jmavsim_iris index 88a3f3978e..6fd1d59db1 100644 --- a/posix-configs/SITL/init/rcS_jmavsim_iris +++ b/posix-configs/SITL/init/rcS_jmavsim_iris @@ -32,6 +32,12 @@ param set COM_RC_IN_MODE 1 param set NAV_ACC_RAD 2.0 param set RTL_RETURN_ALT 30.0 param set RTL_DESCEND_ALT 10.0 +param set MIS_TAKEOFF_ALT 5.0 +param set MPC_HOLD_MAX_Z 2.0 +param set MPC_HOLD_XY_DZ 0.1 +param set MPC_HOLD_Z_DZ 0.1 +param set MPC_Z_VEL_MAX 2.0 +param set MPC_Z_VEL_P 0.4 rgbledsim start tone_alarm start gyrosim start @@ -51,6 +57,7 @@ mc_pos_control start mc_att_control start mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix mavlink start -u 14556 -r 2000000 +mavlink start -u 14557 -r 2000000 -m onboard 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 @@ -59,5 +66,6 @@ mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556 mavlink stream -r 20 -s RC_CHANNELS -u 14556 mavlink stream -r 250 -s HIGHRES_IMU -u 14556 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 +mavlink stream -r 20 -s MANUAL_CONTROL -u 14556 mavlink boot_complete sdlog2 start -r 100 -e -t -a diff --git a/src/drivers/boards/aerocore/board_config.h b/src/drivers/boards/aerocore/board_config.h index 155173db5f..d6932c713d 100644 --- a/src/drivers/boards/aerocore/board_config.h +++ b/src/drivers/boards/aerocore/board_config.h @@ -150,6 +150,7 @@ __BEGIN_DECLS #define GPIO_TIM3_CH2OUT GPIO_TIM3_CH2OUT_3 #define GPIO_TIM3_CH3OUT GPIO_TIM3_CH3OUT_2 #define GPIO_TIM3_CH4OUT GPIO_TIM3_CH4OUT_2 +#define DIRECT_PWM_OUTPUT_CHANNELS 8 /* High-resolution timer */ #define HRT_TIMER 8 /* use timer 8 for the HRT */ diff --git a/src/drivers/boards/px4-stm32f4discovery/px4discovery_init.c b/src/drivers/boards/px4-stm32f4discovery/px4discovery_init.c index fb1b282364..1c74be9058 100644 --- a/src/drivers/boards/px4-stm32f4discovery/px4discovery_init.c +++ b/src/drivers/boards/px4-stm32f4discovery/px4discovery_init.c @@ -129,18 +129,6 @@ stm32_boardinitialize(void) #include -#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-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h index 5957ce6fc5..2c14788ee6 100644 --- a/src/drivers/boards/px4fmu-v1/board_config.h +++ b/src/drivers/boards/px4fmu-v1/board_config.h @@ -182,6 +182,7 @@ __BEGIN_DECLS #define GPIO_TIM2_CH2OUT GPIO_TIM2_CH2OUT_1 #define GPIO_TIM2_CH3OUT GPIO_TIM2_CH3OUT_1 #define GPIO_TIM2_CH4OUT GPIO_TIM2_CH4OUT_1 +#define DIRECT_PWM_OUTPUT_CHANNELS 4 /* USB OTG FS * diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index ea60df9100..12593937f7 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -210,6 +210,7 @@ __BEGIN_DECLS #define GPIO_TIM1_CH4OUT GPIO_TIM1_CH4OUT_2 #define GPIO_TIM4_CH2OUT GPIO_TIM4_CH2OUT_2 #define GPIO_TIM4_CH3OUT GPIO_TIM4_CH3OUT_2 +#define DIRECT_PWM_OUTPUT_CHANNELS 6 /* USB OTG FS * diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index a29c961f5a..d57396a8fd 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -65,7 +65,7 @@ __BEGIN_DECLS //{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_SPEKTRUM_PWR_EN, 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 @@ -200,6 +200,7 @@ __BEGIN_DECLS #define GPIO_TIM1_CH4OUT GPIO_TIM1_CH4OUT_2 #define GPIO_TIM4_CH2OUT GPIO_TIM4_CH2OUT_2 #define GPIO_TIM4_CH3OUT GPIO_TIM4_CH3OUT_2 +#define DIRECT_PWM_OUTPUT_CHANNELS 6 /* USB OTG FS * @@ -223,15 +224,23 @@ __BEGIN_DECLS #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_BTN_SAFETY (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_SPEKTRUM_PWR_EN (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) +/* Power switch controls ******************************************************/ + +#define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s)) +#define GPIO_USART1_RX_SPEKTRUM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN7) +#define SPEKTRUM_RX_HIGH(_s) stm32_gpiowrite(GPIO_USART1_RX_SPEKTRUM, (_s)) +#define SPEKTRUM_RX_AS_UART() stm32_configgpio(GPIO_USART1_RX) +#define SPEKTRUM_RX_AS_GPIO() stm32_configgpio(GPIO_USART1_RX_SPEKTRUM) + /**************************************************************************************************** * Public Types ****************************************************************************************************/ diff --git a/src/drivers/boards/px4fmu-v4/px4fmu_init.c b/src/drivers/boards/px4fmu-v4/px4fmu_init.c index 7966f57165..d093e47bf4 100644 --- a/src/drivers/boards/px4fmu-v4/px4fmu_init.c +++ b/src/drivers/boards/px4fmu-v4/px4fmu_init.c @@ -230,7 +230,7 @@ __EXPORT int nsh_archinitialize(void) stm32_configgpio(GPIO_SBUS_INV); stm32_configgpio(GPIO_8266_GPIO0); - stm32_configgpio(GPIO_SPEKTRUM_POWER); + stm32_configgpio(GPIO_SPEKTRUM_PWR_EN); stm32_configgpio(GPIO_8266_PD); stm32_configgpio(GPIO_8266_RST); diff --git a/src/drivers/boards/px4io-v1/board_config.h b/src/drivers/boards/px4io-v1/board_config.h index f83f18e503..6475d717d8 100644 --- a/src/drivers/boards/px4io-v1/board_config.h +++ b/src/drivers/boards/px4io-v1/board_config.h @@ -83,6 +83,13 @@ #define GPIO_RELAY1_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN12) #define GPIO_RELAY2_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN11) +#define GPIO_SPEKTRUM_PWR_EN GPIO_RELAY1_EN +#define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s)) + +#define SPEKTRUM_RX_HIGH(_s) stm32_gpiowrite(GPIO_USART1_RX_SPEKTRUM, (_s)) +#define SPEKTRUM_RX_AS_UART() stm32_configgpio(GPIO_USART1_RX) +#define SPEKTRUM_RX_AS_GPIO() stm32_configgpio(GPIO_USART1_RX_SPEKTRUM) + /* Analog inputs ********************************************************************/ #define GPIO_ADC_VBATT (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4) diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h index d97f3b203c..c54cf7b6f6 100644 --- a/src/drivers/boards/px4io-v2/board_config.h +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -79,7 +79,7 @@ #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) +#define GPIO_USART1_RX_SPEKTRUM (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN10) /* Safety switch button *******************************************************/ @@ -88,6 +88,11 @@ /* Power switch controls ******************************************************/ #define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) +#define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s)) + +#define SPEKTRUM_RX_HIGH(_s) stm32_gpiowrite(GPIO_USART1_RX_SPEKTRUM, (_s)) +#define SPEKTRUM_RX_AS_UART() stm32_configgpio(GPIO_USART1_RX) +#define SPEKTRUM_RX_AS_GPIO() stm32_configgpio(GPIO_USART1_RX_SPEKTRUM) #define GPIO_SERVO_FAULT_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN15) diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index d99c7cfc74..6f2d749e9b 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -56,7 +56,7 @@ pthread_mutex_t filemutex = PTHREAD_MUTEX_INITIALIZER; extern "C" { -#define PX4_MAX_FD 200 +#define PX4_MAX_FD 300 static device::file_t *filemap[PX4_MAX_FD] = {}; int px4_errno; @@ -124,7 +124,22 @@ extern "C" { ret = dev->open(filemap[i]); } else { - PX4_WARN("exceeded maximum number of file descriptors!"); + + const unsigned NAMELEN = 32; + char thread_name[NAMELEN] = {}; + +#ifndef __PX4_QURT + int nret = pthread_getname_np(pthread_self(), thread_name, NAMELEN); + + if (nret || thread_name[0] == 0) { + PX4_WARN("failed getting thread name"); + } + + PX4_BACKTRACE(); +#endif + + PX4_WARN("%s: exceeded maximum number of file descriptors, accessing %s", + thread_name, path); ret = -ENOENT; } @@ -245,12 +260,16 @@ extern "C" { const unsigned NAMELEN = 32; char thread_name[NAMELEN] = {}; + +#ifndef __PX4_QURT int nret = pthread_getname_np(pthread_self(), thread_name, NAMELEN); if (nret || thread_name[0] == 0) { PX4_WARN("failed getting thread name"); } +#endif + PX4_DEBUG("Called px4_poll timeout = %d", timeout); px4_sem_init(&sem, 0, 0); diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 06d1f10d47..5d9c6901a1 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -81,7 +81,7 @@ __BEGIN_DECLS /** * Default value for a shutdown motor */ -#define PWM_MOTOR_OFF 800 +#define PWM_MOTOR_OFF 900 /** * Default minimum PWM in us diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 8892cfedeb..05d7047c97 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -90,12 +90,8 @@ # include #endif -/* - * This is the analog to FMU_INPUT_DROP_LIMIT_US on the IO side - */ - -#define CONTROL_INPUT_DROP_LIMIT_US 2000 -#define NAN_VALUE (0.0f/0.0f) +#define SCHEDULE_INTERVAL 2000 /**< The schedule interval in usec (500 Hz) */ +#define NAN_VALUE (0.0f/0.0f) /**< NaN value for throttle lock mode */ class PX4FMU : public device::CDev { @@ -123,15 +119,7 @@ public: int set_i2c_bus_clock(unsigned bus, unsigned clock_hz); private: -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) - static const unsigned _max_actuators = 4; -#endif -#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) - static const unsigned _max_actuators = 8; -#endif + static const unsigned _max_actuators = DIRECT_PWM_OUTPUT_CHANNELS; Mode _mode; unsigned _pwm_default_rate; @@ -150,7 +138,7 @@ private: int _dsm_fd; volatile bool _initialized; - bool _servo_armed; + bool _throttle_armed; bool _pwm_on; MixerGroup *_mixers; @@ -189,7 +177,7 @@ private: int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate); int pwm_ioctl(file *filp, int cmd, unsigned long arg); void update_pwm_rev_mask(); - void publish_pwm_outputs(uint16_t *values, size_t numvalues); + void publish_pwm_outputs(uint16_t *values, size_t numvalues); struct GPIOConfig { uint32_t input; @@ -295,7 +283,7 @@ PX4FMU::PX4FMU() : _sbus_fd(-1), _dsm_fd(-1), _initialized(false), - _servo_armed(false), + _throttle_armed(false), _pwm_on(false), _mixers(nullptr), _groups_required(0), @@ -321,11 +309,9 @@ PX4FMU::PX4FMU() : memset(_controls, 0, sizeof(_controls)); memset(_poll_fds, 0, sizeof(_poll_fds)); -#ifdef HRT_PPM_CHANNEL // rc input, published to ORB memset(&_rc_in, 0, sizeof(_rc_in)); _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 @@ -646,7 +632,7 @@ PX4FMU::cycle() update_pwm_rev_mask(); #ifdef SBUS_SERIAL_PORT - _sbus_fd = sbus_init(SBUS_SERIAL_PORT, true); + _sbus_fd = sbus_init(SBUS_SERIAL_PORT, false); #endif #ifdef DSM_SERIAL_PORT @@ -728,8 +714,8 @@ PX4FMU::cycle() //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 (abs(main_out_latency) > SCHEDULE_INTERVAL) { + main_out_latency = SCHEDULE_INTERVAL; } if (main_out_latency < 250) { @@ -783,7 +769,7 @@ PX4FMU::cycle() uint16_t pwm_limited[_max_actuators]; /* the PWM limit call takes care of out of band errors, NaN and constrains */ - pwm_limit_calc(_servo_armed, arm_nothrottle(), num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm, + pwm_limit_calc(_throttle_armed, arm_nothrottle(), num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm, outputs, pwm_limited, &_pwm_limit); /* output to the servos */ @@ -803,14 +789,10 @@ PX4FMU::cycle() orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); /* update the armed status and check that we're not locked down */ - bool set_armed = (_armed.armed || _armed.prearmed) && !_armed.lockdown; - - if (_servo_armed != set_armed) { - _servo_armed = set_armed; - } + _throttle_armed = _armed.armed && !_armed.lockdown; /* update PWM status if armed or if disarmed PWM values are set */ - bool pwm_on = (set_armed || _num_disarmed_set > 0); + bool pwm_on = (_armed.armed || _num_disarmed_set > 0); if (_pwm_on != pwm_on) { _pwm_on = pwm_on; @@ -905,7 +887,7 @@ PX4FMU::cycle() } work_queue(HPWORK, &_work, (worker_t)&PX4FMU::cycle_trampoline, this, - USEC2TICK(CONTROL_INPUT_DROP_LIMIT_US - main_out_latency)); + USEC2TICK(SCHEDULE_INTERVAL - main_out_latency)); } void PX4FMU::work_stop() @@ -1683,9 +1665,9 @@ PX4FMU::peripheral_reset(int ms) stm32_gpiowrite(GPIO_PERIPH_3V3_EN, 0); - bool last = stm32_gpioread(GPIO_SPEKTRUM_POWER); + bool last = stm32_gpioread(GPIO_SPEKTRUM_PWR_EN); /* Keep Spektum on to discharge rail*/ - stm32_gpiowrite(GPIO_SPEKTRUM_POWER, 1); + stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, 1); /* wait for the peripheral rail to reach GND */ usleep(ms * 1000); @@ -1694,7 +1676,7 @@ PX4FMU::peripheral_reset(int ms) /* re-enable power */ /* switch the peripheral rail back on */ - stm32_gpiowrite(GPIO_SPEKTRUM_POWER, last); + stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, last); stm32_gpiowrite(GPIO_PERIPH_3V3_EN, 1); #endif } diff --git a/src/examples/hwtest/CMakeLists.txt b/src/examples/hwtest/CMakeLists.txt index 31548df813..ff256f21be 100644 --- a/src/examples/hwtest/CMakeLists.txt +++ b/src/examples/hwtest/CMakeLists.txt @@ -33,6 +33,7 @@ px4_add_module( MODULE examples__hwtest MAIN ex_hwtest + STACK 2000 SRCS hwtest.c DEPENDS diff --git a/src/examples/matlab_csv_serial/CMakeLists.txt b/src/examples/matlab_csv_serial/CMakeLists.txt index 516acda092..648d03dbbc 100644 --- a/src/examples/matlab_csv_serial/CMakeLists.txt +++ b/src/examples/matlab_csv_serial/CMakeLists.txt @@ -33,6 +33,7 @@ px4_add_module( MODULE examples__matlab_csv_serial MAIN matlab_csv_serial + STACK 2000 SRCS matlab_csv_serial.c DEPENDS diff --git a/src/examples/px4_simple_app/CMakeLists.txt b/src/examples/px4_simple_app/CMakeLists.txt index 5ba6781c60..a41f6997b3 100644 --- a/src/examples/px4_simple_app/CMakeLists.txt +++ b/src/examples/px4_simple_app/CMakeLists.txt @@ -33,6 +33,7 @@ px4_add_module( MODULE examples__px4_simple_app MAIN px4_simple_app + STACK 2000 SRCS px4_simple_app.c DEPENDS diff --git a/src/firmware/nuttx/CMakeLists.txt b/src/firmware/nuttx/CMakeLists.txt index 895eec044a..2b24b477b5 100644 --- a/src/firmware/nuttx/CMakeLists.txt +++ b/src/firmware/nuttx/CMakeLists.txt @@ -51,7 +51,7 @@ if(NOT ${BOARD} STREQUAL "sim") ) if (config_io_board) add_dependencies(romfs fw_io) - endif() + endif() set(fw_file ${CMAKE_CURRENT_BINARY_DIR}/${OS}-${BOARD}-${LABEL}.px4) diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index f56513e209..872f22ec4a 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -233,5 +233,21 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z) y = -y; return; } + + case ROTATION_PITCH_90_ROLL_90: { + tmp = x; x = y; + y = -z; z = -tmp; + return; + } + + case ROTATION_YAW_293_PITCH_68_ROLL_90: { + float tmpx = x; + float tmpy = y; + float tmpz = z; + x = 0.143039f * tmpx + 0.368776f * tmpy + -0.918446f * tmpz; + y = -0.332133f * tmpx + -0.856289f * tmpy + -0.395546f * tmpz; + z = -0.932324f * tmpx + 0.361625f * tmpy + 0.000000f * tmpz; + return; + } } } diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index 280b9cad0c..09336c15e0 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -77,6 +77,8 @@ enum Rotation { ROTATION_ROLL_270_YAW_270 = 26, ROTATION_ROLL_180_PITCH_270 = 27, ROTATION_PITCH_90_YAW_180 = 28, + ROTATION_PITCH_90_ROLL_90 = 29, + ROTATION_YAW_293_PITCH_68_ROLL_90 = 30, ROTATION_MAX }; @@ -115,7 +117,9 @@ const rot_lookup_t rot_lookup[] = { { 0, 270, 0 }, {270, 0, 270 }, {180, 270, 0 }, - { 0, 90, 180 } + { 0, 90, 180 }, + { 90, 90, 0 }, + { 90, 68, 293 }, }; /** diff --git a/src/lib/dspal b/src/lib/dspal index 94188fb83a..135d402c42 160000 --- a/src/lib/dspal +++ b/src/lib/dspal @@ -1 +1 @@ -Subproject commit 94188fb83ab6b2da59eb9a8c8596ab49b67c42f6 +Subproject commit 135d402c428ade4045cb3d0c404fbabcbad432fd diff --git a/src/lib/ecl b/src/lib/ecl new file mode 160000 index 0000000000..2af5856361 --- /dev/null +++ b/src/lib/ecl @@ -0,0 +1 @@ +Subproject commit 2af5856361adf7ede444cc7f3fd6cf725dfa0213 diff --git a/src/lib/ecl/attitude_fw/ecl_controller.cpp b/src/lib/ecl/attitude_fw/ecl_controller.cpp deleted file mode 100644 index 7bf4129e48..0000000000 --- a/src/lib/ecl/attitude_fw/ecl_controller.cpp +++ /dev/null @@ -1,140 +0,0 @@ -/**************************************************************************** - * - * 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_controller.cpp - * Definition of base class for other controllers - * - * @author Lorenz Meier - * @author Thomas Gubler - * - * 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. - */ - -#include "ecl_controller.h" - -#include -#include - -ECL_Controller::ECL_Controller(const char *name) : - _last_run(0), - _tc(0.1f), - _k_p(0.0f), - _k_i(0.0f), - _k_ff(0.0f), - _integrator_max(0.0f), - _max_rate(0.0f), - _last_output(0.0f), - _integrator(0.0f), - _rate_error(0.0f), - _rate_setpoint(0.0f), - _bodyrate_setpoint(0.0f), - _perf_name() -{ - /* Init performance counter */ - snprintf(_perf_name, sizeof(_perf_name), "fw att control %s nonfinite input", name); - _nonfinite_input_perf = perf_alloc(PC_COUNT, _perf_name); -} - -ECL_Controller::~ECL_Controller() -{ - perf_free(_nonfinite_input_perf); -} - -void ECL_Controller::reset_integrator() -{ - _integrator = 0.0f; -} - -void ECL_Controller::set_time_constant(float time_constant) -{ - if (time_constant > 0.1f && time_constant < 3.0f) { - _tc = time_constant; - } -} - -void ECL_Controller::set_k_p(float k_p) -{ - _k_p = k_p; -} - -void ECL_Controller::set_k_i(float k_i) -{ - _k_i = k_i; -} - -void ECL_Controller::set_k_ff(float k_ff) -{ - _k_ff = k_ff; -} - -void ECL_Controller::set_integrator_max(float max) -{ - _integrator_max = max; -} - -void ECL_Controller::set_max_rate(float max_rate) -{ - _max_rate = max_rate; -} - -float ECL_Controller::get_rate_error() -{ - return _rate_error; -} - -float ECL_Controller::get_desired_rate() -{ - return _rate_setpoint; -} - -float ECL_Controller::get_desired_bodyrate() -{ - return _bodyrate_setpoint; -} - -float ECL_Controller::constrain_airspeed(float airspeed, float minspeed, float maxspeed) { - float airspeed_result = airspeed; - if (!PX4_ISFINITE(airspeed)) { - /* airspeed is NaN, +- INF or not available, pick center of band */ - airspeed_result = 0.5f * (minspeed + maxspeed); - } else if (airspeed < minspeed) { - airspeed_result = minspeed; - } - return airspeed_result; -} diff --git a/src/lib/ecl/attitude_fw/ecl_controller.h b/src/lib/ecl/attitude_fw/ecl_controller.h deleted file mode 100644 index fed7d1d8fb..0000000000 --- a/src/lib/ecl/attitude_fw/ecl_controller.h +++ /dev/null @@ -1,125 +0,0 @@ -/**************************************************************************** - * - * 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_controller.h - * Definition of base class for other controllers - * - * @author Lorenz Meier - * @author Thomas Gubler - * - * 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. - */ - -#pragma once - -#include -#include -#include - -struct ECL_ControlData { - float roll; - float pitch; - float yaw; - float roll_rate; - float pitch_rate; - float yaw_rate; - float speed_body_u; - float speed_body_v; - float speed_body_w; - float acc_body_x; - float acc_body_y; - float acc_body_z; - float roll_setpoint; - float pitch_setpoint; - float yaw_setpoint; - float roll_rate_setpoint; - float pitch_rate_setpoint; - float yaw_rate_setpoint; - float airspeed_min; - float airspeed_max; - float airspeed; - float scaler; - float groundspeed; - float groundspeed_scaler; - bool lock_integrator; -}; - -class __EXPORT ECL_Controller -{ -public: - ECL_Controller(const char *name); - - ~ECL_Controller(); - - virtual float control_attitude(const struct ECL_ControlData &ctl_data) = 0; - virtual float control_bodyrate(const struct ECL_ControlData &ctl_data) = 0; - - /* Setters */ - void set_time_constant(float time_constant); - void set_k_p(float k_p); - void set_k_i(float k_i); - void set_k_ff(float k_ff); - void set_integrator_max(float max); - void set_max_rate(float max_rate); - - /* Getters */ - float get_rate_error(); - float get_desired_rate(); - float get_desired_bodyrate(); - - void reset_integrator(); - -protected: - uint64_t _last_run; - float _tc; - float _k_p; - float _k_i; - float _k_ff; - float _integrator_max; - float _max_rate; - float _last_output; - float _integrator; - float _rate_error; - float _rate_setpoint; - float _bodyrate_setpoint; - perf_counter_t _nonfinite_input_perf; - static const uint8_t _perf_name_max = 40; - char _perf_name[_perf_name_max]; - float constrain_airspeed(float airspeed, float minspeed, float maxspeed); -}; diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp deleted file mode 100644 index d04fe38ac3..0000000000 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ /dev/null @@ -1,198 +0,0 @@ -/**************************************************************************** - * - * 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_pitch_controller.cpp - * Implementation of a simple orthogonal pitch PID controller. - * - * Authors and acknowledgements in header. - */ - -#include "ecl_pitch_controller.h" -#include -#include -#include -#include -#include -#include -#include - -ECL_PitchController::ECL_PitchController() : - ECL_Controller("pitch"), - _max_rate_neg(0.0f), - _roll_ff(0.0f) -{ -} - -ECL_PitchController::~ECL_PitchController() -{ -} - -float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_data) -{ - - /* Do not calculate control signal with bad inputs */ - if (!(PX4_ISFINITE(ctl_data.pitch_setpoint) && - PX4_ISFINITE(ctl_data.roll) && - PX4_ISFINITE(ctl_data.pitch) && - PX4_ISFINITE(ctl_data.airspeed))) { - perf_count(_nonfinite_input_perf); - warnx("not controlling pitch"); - return _rate_setpoint; - } - - /* Calculate the error */ - float pitch_error = ctl_data.pitch_setpoint - ctl_data.pitch; - - /* Apply P controller: rate setpoint from current error and time constant */ - _rate_setpoint = pitch_error / _tc; - - /* limit the rate */ - if (_max_rate > 0.01f && _max_rate_neg > 0.01f) { - if (_rate_setpoint > 0.0f) { - _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; - - } else { - _rate_setpoint = (_rate_setpoint < -_max_rate_neg) ? -_max_rate_neg : _rate_setpoint; - } - - } - - return _rate_setpoint; -} - -float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_data) -{ - /* Do not calculate control signal with bad inputs */ - if (!(PX4_ISFINITE(ctl_data.roll) && - PX4_ISFINITE(ctl_data.pitch) && - PX4_ISFINITE(ctl_data.pitch_rate) && - PX4_ISFINITE(ctl_data.yaw_rate) && - PX4_ISFINITE(ctl_data.yaw_rate_setpoint) && - PX4_ISFINITE(ctl_data.airspeed_min) && - PX4_ISFINITE(ctl_data.airspeed_max) && - PX4_ISFINITE(ctl_data.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; - } - - /* Transform setpoint to body angular rates (jacobian) */ - _bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint + - cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint; - - /* apply turning offset to desired bodyrate setpoint*/ - /* flying inverted (wings upside down)*/ - bool inverted = false; - float constrained_roll; - /* roll is used as feedforward term and inverted flight needs to be considered */ - if (fabsf(ctl_data.roll) < math::radians(90.0f)) { - /* not inverted, but numerically still potentially close to infinity */ - constrained_roll = math::constrain(ctl_data.roll, math::radians(-80.0f), math::radians(80.0f)); - - } else { - /* inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity */ - inverted = true; - /* note: the ranges are extended by 10 deg here to avoid numeric resolution effects */ - if (ctl_data.roll > 0.0f) { - /* right hemisphere */ - constrained_roll = math::constrain(ctl_data.roll, math::radians(100.0f), math::radians(180.0f)); - - } else { - /* left hemisphere */ - constrained_roll = math::constrain(ctl_data.roll, math::radians(-100.0f), math::radians(-180.0f)); - } - } - - /* input conditioning */ - float airspeed = constrain_airspeed(ctl_data.airspeed, ctl_data.airspeed_min, ctl_data.airspeed_max); - - /* Calculate desired body fixed y-axis angular rate needed to compensate for roll angle. - For reference see Automatic Control of Aircraft and Missiles by John H. Blakelock, pg. 175 - Availible on google books 8/11/2015: - https://books.google.com/books?id=ubcczZUDCsMC&pg=PA175#v=onepage&q&f=false*/ - float body_fixed_turn_offset = (fabsf((CONSTANTS_ONE_G / airspeed) * - tanf(constrained_roll) * sinf(constrained_roll))); - - if (inverted) { - body_fixed_turn_offset = -body_fixed_turn_offset; - } - - /* Finally add the turn offset to your bodyrate setpoint*/ - _bodyrate_setpoint += body_fixed_turn_offset; - - - _rate_error = _bodyrate_setpoint - ctl_data.pitch_rate; - - if (!lock_integrator && _k_i > 0.0f) { - - float id = _rate_error * dt * ctl_data.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 = _bodyrate_setpoint * _k_ff * ctl_data.scaler + - _rate_error * _k_p * ctl_data.scaler * ctl_data.scaler - + integrator_constrained; //scaler is proportional to 1/airspeed -// warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p); -// warnx("roll: _last_output %.4f", (double)_last_output); - return math::constrain(_last_output, -1.0f, 1.0f); -} diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h deleted file mode 100644 index fa7612cb99..0000000000 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h +++ /dev/null @@ -1,89 +0,0 @@ -/**************************************************************************** - * - * 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_pitch_controller.h - * Definition of a simple orthogonal pitch PID controller. - * - * @author Lorenz Meier - * @author Thomas Gubler - * - * 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_PITCH_CONTROLLER_H -#define ECL_PITCH_CONTROLLER_H - -#include -#include - -#include "ecl_controller.h" - -class __EXPORT ECL_PitchController : - public ECL_Controller -{ -public: - ECL_PitchController(); - - ~ECL_PitchController(); - - float control_attitude(const struct ECL_ControlData &ctl_data); - float control_bodyrate(const struct ECL_ControlData &ctl_data); - - /* Additional Setters */ - void set_max_rate_pos(float max_rate_pos) - { - _max_rate = max_rate_pos; - } - - void set_max_rate_neg(float max_rate_neg) - { - _max_rate_neg = max_rate_neg; - } - - void set_roll_ff(float roll_ff) - { - _roll_ff = roll_ff; - } - -protected: - float _max_rate_neg; - float _roll_ff; -}; - -#endif // ECL_PITCH_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp deleted file mode 100644 index 9d40c73f3b..0000000000 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ /dev/null @@ -1,146 +0,0 @@ -/**************************************************************************** - * - * 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_roll_controller.cpp - * Implementation of a simple orthogonal roll PID controller. - * - * Authors and acknowledgements in header. - */ - -#include -#include "ecl_roll_controller.h" -#include -#include -#include -#include -#include -#include - -ECL_RollController::ECL_RollController() : - ECL_Controller("roll") -{ -} - -ECL_RollController::~ECL_RollController() -{ -} - -float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_data) -{ - /* Do not calculate control signal with bad inputs */ - if (!(PX4_ISFINITE(ctl_data.roll_setpoint) && PX4_ISFINITE(ctl_data.roll))) { - perf_count(_nonfinite_input_perf); - return _rate_setpoint; - } - - /* Calculate error */ - float roll_error = ctl_data.roll_setpoint - ctl_data.roll; - - /* Apply P controller */ - _rate_setpoint = roll_error / _tc; - - /* limit the rate */ //XXX: move to body angluar rates - - if (_max_rate > 0.01f) { - _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; - _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; - } - - return _rate_setpoint; -} - -float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_data) -{ - /* Do not calculate control signal with bad inputs */ - if (!(PX4_ISFINITE(ctl_data.pitch) && - PX4_ISFINITE(ctl_data.roll_rate) && - PX4_ISFINITE(ctl_data.yaw_rate) && - PX4_ISFINITE(ctl_data.yaw_rate_setpoint) && - PX4_ISFINITE(ctl_data.airspeed_min) && - PX4_ISFINITE(ctl_data.airspeed_max) && - PX4_ISFINITE(ctl_data.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; - } - - /* Transform setpoint to body angular rates (jacobian) */ - _bodyrate_setpoint = _rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint; - - /* Calculate body angular rate error */ - _rate_error = _bodyrate_setpoint - ctl_data.roll_rate; //body angular rate error - - if (!lock_integrator && _k_i > 0.0f) { - - float id = _rate_error * dt * ctl_data.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); - //warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max); - - /* Apply PI rate controller and store non-limited output */ - _last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler + - _rate_error * _k_p * ctl_data.scaler * ctl_data.scaler - + integrator_constrained; //scaler is proportional to 1/airspeed - - return math::constrain(_last_output, -1.0f, 1.0f); -} - diff --git a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp deleted file mode 100644 index 7e34ff2a61..0000000000 --- a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp +++ /dev/null @@ -1,153 +0,0 @@ -/**************************************************************************** - * - * 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_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp deleted file mode 100644 index 4849bdad79..0000000000 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ /dev/null @@ -1,240 +0,0 @@ -/**************************************************************************** - * - * 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_yaw_controller.cpp - * Implementation of a simple orthogonal coordinated turn yaw PID controller. - * - * Authors and acknowledgements in header. - */ - -#include "ecl_yaw_controller.h" -#include -#include -#include -#include -#include -#include -#include - -ECL_YawController::ECL_YawController() : - ECL_Controller("yaw"), - _coordinated_min_speed(1.0f), - _coordinated_method(0) -{ -} - -ECL_YawController::~ECL_YawController() -{ -} - -float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data) -{ - switch (_coordinated_method) { - case COORD_METHOD_OPEN: - return control_attitude_impl_openloop(ctl_data); - - case COORD_METHOD_CLOSEACC: - return control_attitude_impl_accclosedloop(ctl_data); - - default: - static hrt_abstime last_print = 0; - - if (ecl_elapsed_time(&last_print) > 5e6) { - warnx("invalid param setting FW_YCO_METHOD"); - last_print = ecl_absolute_time(); - } - } - - return _rate_setpoint; -} - -float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data) -{ - switch (_coordinated_method) { - case COORD_METHOD_OPEN: - case COORD_METHOD_CLOSEACC: - return control_bodyrate_impl(ctl_data); - - default: - static hrt_abstime last_print = 0; - - if (ecl_elapsed_time(&last_print) > 5e6) { - warnx("invalid param setting FW_YCO_METHOD"); - last_print = ecl_absolute_time(); - } - } - - return math::constrain(_last_output, -1.0f, 1.0f); -} - -float ECL_YawController::control_attitude_impl_openloop(const struct ECL_ControlData &ctl_data) -{ - /* Do not calculate control signal with bad inputs */ - if (!(PX4_ISFINITE(ctl_data.roll) && - PX4_ISFINITE(ctl_data.pitch) && - PX4_ISFINITE(ctl_data.speed_body_u) && - PX4_ISFINITE(ctl_data.speed_body_v) && - PX4_ISFINITE(ctl_data.speed_body_w) && - PX4_ISFINITE(ctl_data.roll_rate_setpoint) && - PX4_ISFINITE(ctl_data.pitch_rate_setpoint))) { - perf_count(_nonfinite_input_perf); - return _rate_setpoint; - } - -// static int counter = 0; - /* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */ - _rate_setpoint = 0.0f; - - if (sqrtf(ctl_data.speed_body_u * ctl_data.speed_body_u + ctl_data.speed_body_v * ctl_data.speed_body_v + - ctl_data.speed_body_w * ctl_data.speed_body_w) > _coordinated_min_speed) { - float denumerator = (ctl_data.speed_body_u * cosf(ctl_data.roll) * cosf(ctl_data.pitch) + - ctl_data.speed_body_w * sinf(ctl_data.pitch)); - - if (fabsf(denumerator) > FLT_EPSILON) { - _rate_setpoint = (ctl_data.speed_body_w * ctl_data.roll_rate_setpoint + - 9.81f * sinf(ctl_data.roll) * cosf(ctl_data.pitch) + - ctl_data.speed_body_u * ctl_data.pitch_rate_setpoint * sinf(ctl_data.roll)) / - denumerator; - -// warnx("yaw: speed_body_u %.f speed_body_w %1.f roll %.1f pitch %.1f denumerator %.1f _rate_setpoint %.1f", speed_body_u, speed_body_w, denumerator, _rate_setpoint); - } - -// if(counter % 20 == 0) { -// warnx("denumerator: %.4f, speed_body_u: %.4f, speed_body_w: %.4f, cosf(roll): %.4f, cosf(pitch): %.4f, sinf(pitch): %.4f", (double)denumerator, (double)speed_body_u, (double)speed_body_w, (double)cosf(roll), (double)cosf(pitch), (double)sinf(pitch)); -// } - } - - /* limit the rate */ //XXX: move to body angluar rates - - if (_max_rate > 0.01f) { - _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; - _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; - } - - -// counter++; - - if (!PX4_ISFINITE(_rate_setpoint)) { - warnx("yaw rate sepoint not finite"); - _rate_setpoint = 0.0f; - } - - return _rate_setpoint; -} - -float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl_data) -{ - /* Do not calculate control signal with bad inputs */ - if (!(PX4_ISFINITE(ctl_data.roll) && PX4_ISFINITE(ctl_data.pitch) && PX4_ISFINITE(ctl_data.pitch_rate) && - PX4_ISFINITE(ctl_data.yaw_rate) && PX4_ISFINITE(ctl_data.pitch_rate_setpoint) && - PX4_ISFINITE(ctl_data.airspeed_min) && PX4_ISFINITE(ctl_data.airspeed_max) && - PX4_ISFINITE(ctl_data.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 airspeed = ctl_data.airspeed; - - if (!PX4_ISFINITE(airspeed)) { - /* airspeed is NaN, +- INF or not available, pick center of band */ - airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max); - - } else if (airspeed < ctl_data.airspeed_min) { - airspeed = ctl_data.airspeed_min; - } - - - /* Transform setpoint to body angular rates (jacobian) */ - _bodyrate_setpoint = -sinf(ctl_data.roll) * ctl_data.pitch_rate_setpoint + - cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _rate_setpoint; - - /* Close the acceleration loop if _coordinated_method wants this: change body_rate setpoint */ - if (_coordinated_method == COORD_METHOD_CLOSEACC) { - //XXX: filtering of acceleration? - _bodyrate_setpoint -= (ctl_data.acc_body_y / (airspeed * cosf(ctl_data.pitch))); - } - - /* Calculate body angular rate error */ - _rate_error = _bodyrate_setpoint - ctl_data.yaw_rate; //body angular rate error - - if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) { - - float id = _rate_error * dt; - - /* - * 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 = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * ctl_data.scaler * - ctl_data.scaler; //scaler is proportional to 1/airspeed - //warnx("yaw:_last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_last_output, (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p); - - - return math::constrain(_last_output, -1.0f, 1.0f); -} - -float ECL_YawController::control_attitude_impl_accclosedloop(const struct ECL_ControlData &ctl_data) -{ - /* dont set a rate setpoint */ - return 0.0f; -} diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h deleted file mode 100644 index 511b5fd36f..0000000000 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ /dev/null @@ -1,98 +0,0 @@ -/**************************************************************************** - * - * 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_yaw_controller.h - * Definition of a simple orthogonal coordinated turn yaw PID controller. - * - * @author Lorenz Meier - * @author Thomas Gubler - * - * 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_YAW_CONTROLLER_H -#define ECL_YAW_CONTROLLER_H - -#include -#include - -#include "ecl_controller.h" - -class __EXPORT ECL_YawController : - public ECL_Controller -{ -public: - ECL_YawController(); - - ~ECL_YawController(); - - float control_attitude(const struct ECL_ControlData &ctl_data); - - float control_bodyrate(const struct ECL_ControlData &ctl_data); - - /* Additional setters */ - void set_coordinated_min_speed(float coordinated_min_speed) - { - _coordinated_min_speed = coordinated_min_speed; - } - - void set_coordinated_method(int32_t coordinated_method) - { - _coordinated_method = coordinated_method; - } - - enum { - COORD_METHOD_OPEN = 0, - 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); - - float control_attitude_impl_openloop(const struct ECL_ControlData &ctl_data); - - float control_attitude_impl_accclosedloop(const struct ECL_ControlData &ctl_data); - -}; - -#endif // ECL_YAW_CONTROLLER_H diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp deleted file mode 100644 index d1c864d782..0000000000 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp +++ /dev/null @@ -1,372 +0,0 @@ -/**************************************************************************** - * - * 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_l1_pos_controller.h - * Implementation of L1 position control. - * Authors and acknowledgements in header. - * - */ - -#include - -#include "ecl_l1_pos_controller.h" - -float ECL_L1_Pos_Controller::nav_roll() -{ - float ret = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G); - ret = math::constrain(ret, -_roll_lim_rad, _roll_lim_rad); - return ret; -} - -float ECL_L1_Pos_Controller::nav_lateral_acceleration_demand() -{ - return _lateral_accel; -} - -float ECL_L1_Pos_Controller::nav_bearing() -{ - return _wrap_pi(_nav_bearing); -} - -float ECL_L1_Pos_Controller::bearing_error() -{ - return _bearing_error; -} - -float ECL_L1_Pos_Controller::target_bearing() -{ - return _target_bearing; -} - -float ECL_L1_Pos_Controller::switch_distance(float wp_radius) -{ - /* following [2], switching on L1 distance */ - return math::min(wp_radius, _L1_distance); -} - -bool ECL_L1_Pos_Controller::reached_loiter_target(void) -{ - return _circle_mode; -} - -float ECL_L1_Pos_Controller::crosstrack_error(void) -{ - return _crosstrack_error; -} - -void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector<2> &vector_A, const math::Vector<2> &vector_B, const math::Vector<2> &vector_curr_position, - const math::Vector<2> &ground_speed_vector) -{ - - /* this follows the logic presented in [1] */ - - float eta; - float xtrack_vel; - float ltrack_vel; - - /* get the direction between the last (visited) and next waypoint */ - _target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_B(0), vector_B(1)); - - /* enforce a minimum ground speed of 0.1 m/s to avoid singularities */ - float ground_speed = math::max(ground_speed_vector.length(), 0.1f); - - /* calculate the L1 length required for the desired period */ - _L1_distance = _L1_ratio * ground_speed; - - /* calculate vector from A to B */ - math::Vector<2> vector_AB = get_local_planar_vector(vector_A, vector_B); - - /* - * check if waypoints are on top of each other. If yes, - * skip A and directly continue to B - */ - if (vector_AB.length() < 1.0e-6f) { - vector_AB = get_local_planar_vector(vector_curr_position, vector_B); - } - - vector_AB.normalize(); - - /* calculate the vector from waypoint A to the aircraft */ - math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); - - /* calculate crosstrack error (output only) */ - _crosstrack_error = vector_AB % vector_A_to_airplane; - - /* - * If the current position is in a +-135 degree angle behind waypoint A - * and further away from A than the L1 distance, then A becomes the L1 point. - * If the aircraft is already between A and B normal L1 logic is applied. - */ - float distance_A_to_airplane = vector_A_to_airplane.length(); - float alongTrackDist = vector_A_to_airplane * vector_AB; - - /* estimate airplane position WRT to B */ - math::Vector<2> vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized(); - - /* calculate angle of airplane position vector relative to line) */ - - // XXX this could probably also be based solely on the dot product - float AB_to_BP_bearing = atan2f(vector_B_to_P_unit % vector_AB, vector_B_to_P_unit * vector_AB); - - /* extension from [2], fly directly to A */ - if (distance_A_to_airplane > _L1_distance && alongTrackDist / math::max(distance_A_to_airplane , 1.0f) < -0.7071f) { - - /* calculate eta to fly to waypoint A */ - - /* unit vector from waypoint A to current position */ - math::Vector<2> vector_A_to_airplane_unit = vector_A_to_airplane.normalized(); - /* velocity across / orthogonal to line */ - xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit); - /* velocity along line */ - ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit); - eta = atan2f(xtrack_vel, ltrack_vel); - /* bearing from current position to L1 point */ - _nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0)); - - /* - * If the AB vector and the vector from B to airplane point in the same - * direction, we have missed the waypoint. At +- 90 degrees we are just passing it. - */ - } else if (fabsf(AB_to_BP_bearing) < math::radians(100.0f)) { - /* - * Extension, fly back to waypoint. - * - * This corner case is possible if the system was following - * the AB line from waypoint A to waypoint B, then is - * switched to manual mode (or otherwise misses the waypoint) - * and behind the waypoint continues to follow the AB line. - */ - - /* calculate eta to fly to waypoint B */ - - /* velocity across / orthogonal to line */ - xtrack_vel = ground_speed_vector % (-vector_B_to_P_unit); - /* velocity along line */ - ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit); - eta = atan2f(xtrack_vel, ltrack_vel); - /* bearing from current position to L1 point */ - _nav_bearing = atan2f(-vector_B_to_P_unit(1) , -vector_B_to_P_unit(0)); - - } else { - - /* calculate eta to fly along the line between A and B */ - - /* velocity across / orthogonal to line */ - xtrack_vel = ground_speed_vector % vector_AB; - /* velocity along line */ - ltrack_vel = ground_speed_vector * vector_AB; - /* calculate eta2 (angle of velocity vector relative to line) */ - float eta2 = atan2f(xtrack_vel, ltrack_vel); - /* calculate eta1 (angle to L1 point) */ - float xtrackErr = vector_A_to_airplane % vector_AB; - float sine_eta1 = xtrackErr / math::max(_L1_distance , 0.1f); - /* limit output to 45 degrees */ - sine_eta1 = math::constrain(sine_eta1, -0.7071f, 0.7071f); //sin(pi/4) = 0.7071 - float eta1 = asinf(sine_eta1); - eta = eta1 + eta2; - /* bearing from current position to L1 point */ - _nav_bearing = atan2f(vector_AB(1), vector_AB(0)) + eta1; - - } - - /* limit angle to +-90 degrees */ - eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f); - _lateral_accel = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta); - - /* flying to waypoints, not circling them */ - _circle_mode = false; - - /* the bearing angle, in NED frame */ - _bearing_error = eta; -} - -void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, const math::Vector<2> &vector_curr_position, float radius, int8_t loiter_direction, - const math::Vector<2> &ground_speed_vector) -{ - /* the complete guidance logic in this section was proposed by [2] */ - - /* calculate the gains for the PD loop (circle tracking) */ - float omega = (2.0f * M_PI_F / _L1_period); - float K_crosstrack = omega * omega; - float K_velocity = 2.0f * _L1_damping * omega; - - /* update bearing to next waypoint */ - _target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_A(0), vector_A(1)); - - /* ground speed, enforce minimum of 0.1 m/s to avoid singularities */ - float ground_speed = math::max(ground_speed_vector.length() , 0.1f); - - /* calculate the L1 length required for the desired period */ - _L1_distance = _L1_ratio * ground_speed; - - /* calculate the vector from waypoint A to current position */ - math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); - - math::Vector<2> vector_A_to_airplane_unit; - - /* prevent NaN when normalizing */ - if (vector_A_to_airplane.length() > FLT_EPSILON) { - /* store the normalized vector from waypoint A to current position */ - vector_A_to_airplane_unit = vector_A_to_airplane.normalized(); - } else { - vector_A_to_airplane_unit = vector_A_to_airplane; - } - - /* calculate eta angle towards the loiter center */ - - /* velocity across / orthogonal to line from waypoint to current position */ - float xtrack_vel_center = vector_A_to_airplane_unit % ground_speed_vector; - /* velocity along line from waypoint to current position */ - float ltrack_vel_center = - (ground_speed_vector * vector_A_to_airplane_unit); - float eta = atan2f(xtrack_vel_center, ltrack_vel_center); - /* limit eta to 90 degrees */ - eta = math::constrain(eta, -M_PI_F / 2.0f, +M_PI_F / 2.0f); - - /* calculate the lateral acceleration to capture the center point */ - float lateral_accel_sp_center = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta); - - /* for PD control: Calculate radial position and velocity errors */ - - /* radial velocity error */ - float xtrack_vel_circle = -ltrack_vel_center; - /* radial distance from the loiter circle (not center) */ - float xtrack_err_circle = vector_A_to_airplane.length() - radius; - - /* cross track error for feedback */ - _crosstrack_error = xtrack_err_circle; - - /* calculate PD update to circle waypoint */ - float lateral_accel_sp_circle_pd = (xtrack_err_circle * K_crosstrack + xtrack_vel_circle * K_velocity); - - /* calculate velocity on circle / along tangent */ - float tangent_vel = xtrack_vel_center * loiter_direction; - - /* prevent PD output from turning the wrong way */ - if (tangent_vel < 0.0f) { - lateral_accel_sp_circle_pd = math::max(lateral_accel_sp_circle_pd , 0.0f); - } - - /* calculate centripetal acceleration setpoint */ - float lateral_accel_sp_circle_centripetal = tangent_vel * tangent_vel / math::max((0.5f * radius) , (radius + xtrack_err_circle)); - - /* add PD control on circle and centripetal acceleration for total circle command */ - float lateral_accel_sp_circle = loiter_direction * (lateral_accel_sp_circle_pd + lateral_accel_sp_circle_centripetal); - - /* - * Switch between circle (loiter) and capture (towards waypoint center) mode when - * the commands switch over. Only fly towards waypoint if outside the circle. - */ - - // XXX check switch over - if ((lateral_accel_sp_center < lateral_accel_sp_circle && loiter_direction > 0 && xtrack_err_circle > 0.0f) || - (lateral_accel_sp_center > lateral_accel_sp_circle && loiter_direction < 0 && xtrack_err_circle > 0.0f)) { - _lateral_accel = lateral_accel_sp_center; - _circle_mode = false; - /* angle between requested and current velocity vector */ - _bearing_error = eta; - /* bearing from current position to L1 point */ - _nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0)); - - } else { - _lateral_accel = lateral_accel_sp_circle; - _circle_mode = true; - _bearing_error = 0.0f; - /* bearing from current position to L1 point */ - _nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0)); - } -} - - -void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector<2> &ground_speed_vector) -{ - /* the complete guidance logic in this section was proposed by [2] */ - - float eta; - - /* - * As the commanded heading is the only reference - * (and no crosstrack correction occurs), - * target and navigation bearing become the same - */ - _target_bearing = _nav_bearing = _wrap_pi(navigation_heading); - eta = _target_bearing - _wrap_pi(current_heading); - eta = _wrap_pi(eta); - - /* consequently the bearing error is exactly eta: */ - _bearing_error = eta; - - /* ground speed is the length of the ground speed vector */ - float ground_speed = ground_speed_vector.length(); - - /* adjust L1 distance to keep constant frequency */ - _L1_distance = ground_speed / _heading_omega; - float omega_vel = ground_speed * _heading_omega; - - /* not circling a waypoint */ - _circle_mode = false; - - /* navigating heading means by definition no crosstrack error */ - _crosstrack_error = 0; - - /* limit eta to 90 degrees */ - eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f); - _lateral_accel = 2.0f * sinf(eta) * omega_vel; -} - -void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading) -{ - /* the logic in this section is trivial, but originally proposed by [2] */ - - /* reset all heading / error measures resulting in zero roll */ - _target_bearing = current_heading; - _nav_bearing = current_heading; - _bearing_error = 0; - _crosstrack_error = 0; - _lateral_accel = 0; - - /* not circling a waypoint when flying level */ - _circle_mode = false; - -} - - -math::Vector<2> ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const -{ - /* this is an approximation for small angles, proposed by [2] */ - - math::Vector<2> out(math::radians((target(0) - origin(0))), math::radians((target(1) - origin(1))*cosf(math::radians(origin(0))))); - - return out * static_cast(CONSTANTS_RADIUS_OF_EARTH); -} - diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.h b/src/lib/ecl/l1/ecl_l1_pos_controller.h deleted file mode 100644 index 5c0804a392..0000000000 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.h +++ /dev/null @@ -1,268 +0,0 @@ -/**************************************************************************** - * - * 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_l1_pos_control.h - * Implementation of L1 position control. - * - * - * Acknowledgements and References: - * - * This implementation has been built for PX4 based on the original - * publication from [1] and does include a lot of the ideas (not code) - * from [2]. - * - * - * [1] S. Park, J. Deyst, and J. P. How, "A New Nonlinear Guidance Logic for Trajectory Tracking," - * Proceedings of the AIAA Guidance, Navigation and Control - * Conference, Aug 2004. AIAA-2004-4900. - * - * [2] Paul Riseborough, Brandon Jones and Andrew Tridgell, L1 control for APM. Aug 2013. - * - Explicit control over frequency and damping - * - Explicit control over track capture angle - * - Ability to use loiter radius smaller than L1 length - * - Modified to use PD control for circle tracking to enable loiter radius less than L1 length - * - Modified to enable period and damping of guidance loop to be set explicitly - * - Modified to provide explicit control over capture angle - * - */ - -#ifndef ECL_L1_POS_CONTROLLER_H -#define ECL_L1_POS_CONTROLLER_H - -#include -#include -#include - -/** - * L1 Nonlinear Guidance Logic - */ -class __EXPORT ECL_L1_Pos_Controller -{ -public: - ECL_L1_Pos_Controller() { - _L1_period = 25; - _L1_damping = 0.75f; - } - - /** - * The current target bearing - * - * @return bearing angle (-pi..pi, in NED frame) - */ - float nav_bearing(); - - - /** - * Get lateral acceleration demand. - * - * @return Lateral acceleration in m/s^2 - */ - float nav_lateral_acceleration_demand(); - - - /** - * Heading error. - * - * The heading error is either compared to the current track - * or to the tangent of the current loiter radius. - */ - float bearing_error(); - - - /** - * Bearing from aircraft to current target. - * - * @return bearing angle (-pi..pi, in NED frame) - */ - float target_bearing(); - - - /** - * Get roll angle setpoint for fixed wing. - * - * @return Roll angle (in NED frame) - */ - float nav_roll(); - - - /** - * Get the current crosstrack error. - * - * @return Crosstrack error in meters. - */ - float crosstrack_error(); - - - /** - * Returns true if the loiter waypoint has been reached - */ - bool reached_loiter_target(); - - - /** - * Returns true if following a circle (loiter) - */ - bool circle_mode() { - return _circle_mode; - } - - - /** - * Get the switch distance - * - * This is the distance at which the system will - * switch to the next waypoint. This depends on the - * period and damping - * - * @param waypoint_switch_radius The switching radius the waypoint has set. - */ - float switch_distance(float waypoint_switch_radius); - - - /** - * Navigate between two waypoints - * - * Calling this function with two waypoints results in the - * control outputs to fly to the line segment defined by - * the points and once captured following the line segment. - * This follows the logic in [1]. - * - * @return sets _lateral_accel setpoint - */ - void navigate_waypoints(const math::Vector<2> &vector_A, const math::Vector<2> &vector_B, const math::Vector<2> &vector_curr_position, - const math::Vector<2> &ground_speed); - - - /** - * Navigate on an orbit around a loiter waypoint. - * - * This allow orbits smaller than the L1 length, - * this modification was introduced in [2]. - * - * @return sets _lateral_accel setpoint - */ - void navigate_loiter(const math::Vector<2> &vector_A, const math::Vector<2> &vector_curr_position, float radius, int8_t loiter_direction, - const math::Vector<2> &ground_speed_vector); - - - /** - * Navigate on a fixed bearing. - * - * This only holds a certain direction and does not perform cross - * track correction. Helpful for semi-autonomous modes. Introduced - * by [2]. - * - * @return sets _lateral_accel setpoint - */ - void navigate_heading(float navigation_heading, float current_heading, const math::Vector<2> &ground_speed); - - - /** - * Keep the wings level. - * - * This is typically needed for maximum-lift-demand situations, - * such as takeoff or near stall. Introduced in [2]. - */ - void navigate_level_flight(float current_heading); - - - /** - * Set the L1 period. - */ - void set_l1_period(float period) { - _L1_period = period; - /* calculate the ratio introduced in [2] */ - _L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period; - /* calculate normalized frequency for heading tracking */ - _heading_omega = sqrtf(2.0f) * M_PI_F / _L1_period; - } - - - /** - * Set the L1 damping factor. - * - * The original publication recommends a default of sqrt(2) / 2 = 0.707 - */ - void set_l1_damping(float damping) { - _L1_damping = damping; - /* calculate the ratio introduced in [2] */ - _L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period; - /* calculate the L1 gain (following [2]) */ - _K_L1 = 4.0f * _L1_damping * _L1_damping; - } - - - /** - * Set the maximum roll angle output in radians - * - */ - void set_l1_roll_limit(float roll_lim_rad) { - _roll_lim_rad = roll_lim_rad; - } - -private: - - float _lateral_accel; ///< Lateral acceleration setpoint in m/s^2 - float _L1_distance; ///< L1 lead distance, defined by period and damping - bool _circle_mode; ///< flag for loiter mode - float _nav_bearing; ///< bearing to L1 reference point - float _bearing_error; ///< bearing error - float _crosstrack_error; ///< crosstrack error in meters - float _target_bearing; ///< the heading setpoint - - float _L1_period; ///< L1 tracking period in seconds - float _L1_damping; ///< L1 damping ratio - float _L1_ratio; ///< L1 ratio for navigation - float _K_L1; ///< L1 control gain for _L1_damping - float _heading_omega; ///< Normalized frequency - - float _roll_lim_rad; /// get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const; - -}; - - -#endif /* ECL_L1_POS_CONTROLLER_H */ diff --git a/src/lib/ecl/validation/data_validator.cpp b/src/lib/ecl/validation/data_validator.cpp deleted file mode 100644 index 4e55849df0..0000000000 --- a/src/lib/ecl/validation/data_validator.cpp +++ /dev/null @@ -1,169 +0,0 @@ -/**************************************************************************** - * - * 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 data_validator.c - * - * A data validation class to identify anomalies in data streams - * - * @author Lorenz Meier - */ - -#include "data_validator.h" -#include - -DataValidator::DataValidator(DataValidator *prev_sibling) : - _error_mask(ERROR_FLAG_NO_ERROR), - _time_last(0), - _timeout_interval(20000), - _event_count(0), - _error_count(0), - _error_density(0), - _priority(0), - _mean{0.0f}, - _lp{0.0f}, - _M2{0.0f}, - _rms{0.0f}, - _value{0.0f}, - _value_equal_count(0), - _sibling(prev_sibling) -{ - -} - -DataValidator::~DataValidator() -{ - -} - -void -DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in, int priority_in) -{ - _event_count++; - - if (error_count_in > _error_count) { - _error_density += (error_count_in - _error_count); - } else if (_error_density > 0) { - _error_density--; - } - - _error_count = error_count_in; - _priority = priority_in; - - for (unsigned i = 0; i < _dimensions; i++) { - if (_time_last == 0) { - _mean[i] = 0; - _lp[i] = val[i]; - _M2[i] = 0; - } else { - float lp_val = val[i] - _lp[i]; - - float delta_val = lp_val - _mean[i]; - _mean[i] += delta_val / _event_count; - _M2[i] += delta_val * (lp_val - _mean[i]); - _rms[i] = sqrtf(_M2[i] / (_event_count - 1)); - - if (fabsf(_value[i] - val[i]) < 0.000001f) { - _value_equal_count++; - } else { - _value_equal_count = 0; - } - } - - // XXX replace with better filter, make it auto-tune to update rate - _lp[i] = _lp[i] * 0.5f + val[i] * 0.5f; - - _value[i] = val[i]; - } - - _time_last = timestamp; -} - -float -DataValidator::confidence(uint64_t timestamp) -{ - float ret = 1.0f; - - /* check if we have any data */ - if (_time_last == 0) { - _error_mask |= ERROR_FLAG_NO_DATA; - ret = 0.0f; - - /* timed out - that's it */ - } 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 */ - } else if (_value_equal_count > VALUE_EQUAL_COUNT_MAX) { - _error_mask |= ERROR_FLAG_STALE_DATA; - ret = 0.0f; - - /* check error count limit */ - } else if (_error_count > NORETURN_ERRCOUNT) { - _error_mask |= ERROR_FLAG_HIGH_ERRCOUNT; - ret = 0.0f; - - /* cap error density counter at window size */ - } 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) { - /* return local error density for last N measurements */ - ret = 1.0f - (_error_density / ERROR_DENSITY_WINDOW); - } - - return ret; -} - -void -DataValidator::print() -{ - if (_time_last == 0) { - ECL_INFO("\tno data"); - return; - } - - for (unsigned i = 0; i < _dimensions; i++) { - ECL_INFO("\tval: %8.4f, lp: %8.4f mean dev: %8.4f RMS: %8.4f conf: %8.4f", - (double) _value[i], (double)_lp[i], (double)_mean[i], - (double)_rms[i], (double)confidence(hrt_absolute_time())); - } -} diff --git a/src/lib/ecl/validation/data_validator.h b/src/lib/ecl/validation/data_validator.h deleted file mode 100644 index c313b1cb04..0000000000 --- a/src/lib/ecl/validation/data_validator.h +++ /dev/null @@ -1,159 +0,0 @@ -/**************************************************************************** - * - * 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 data_validator.h - * - * A data validation class to identify anomalies in data streams - * - * @author Lorenz Meier - */ - -#pragma once - -#include -#include - -class __EXPORT DataValidator { -public: - DataValidator(DataValidator *prev_sibling = nullptr); - virtual ~DataValidator(); - - /** - * Put an item into the validator. - * - * @param val Item to put - */ - void put(uint64_t timestamp, float val[3], uint64_t error_count, int priority); - - /** - * Get the next sibling in the group - * - * @return the next sibling - */ - DataValidator* sibling() { return _sibling; } - - /** - * Get the confidence of this validator - * @return the confidence between 0 and 1 - */ - float confidence(uint64_t timestamp); - - /** - * Get the error count of this validator - * @return the error count - */ - uint64_t error_count() { return _error_count; } - - /** - * Get the values of this validator - * @return the stored value - */ - float* value() { return _value; } - - /** - * Get the used status of this validator - * @return true if this validator ever saw data - */ - bool used() { return (_time_last > 0); } - - /** - * Get the priority of this validator - * @return the stored priority - */ - int priority() { return (_priority); } - - /** - * Get the error state of this validator - * @return the bitmask with the error status - */ - uint32_t state() { return _error_mask; } - - /** - * Reset the error state of this validator - */ - void reset_state() { _error_mask = ERROR_FLAG_NO_ERROR; } - - /** - * Get the RMS values of this validator - * @return the stored RMS - */ - float* rms() { return _rms; } - - /** - * Print the validator value - * - */ - void print(); - - /** - * Set the timeout value - * - * @param timeout_interval_us The timeout interval in microseconds - */ - void set_timeout(uint64_t timeout_interval_us) { _timeout_interval = timeout_interval_us; } - - /** - * Data validator error states - */ - static constexpr uint32_t ERROR_FLAG_NO_ERROR = (0x00000000U); - static constexpr uint32_t ERROR_FLAG_NO_DATA = (0x00000001U); - static constexpr uint32_t ERROR_FLAG_STALE_DATA = (0x00000001U << 1); - static constexpr uint32_t ERROR_FLAG_TIMEOUT = (0x00000001U << 2); - static constexpr uint32_t ERROR_FLAG_HIGH_ERRCOUNT = (0x00000001U << 3); - static constexpr uint32_t ERROR_FLAG_HIGH_ERRDENSITY = (0x00000001U << 4); - -private: - static const unsigned _dimensions = 3; - uint32_t _error_mask; /**< sensor error state */ - uint64_t _time_last; /**< last timestamp */ - uint64_t _timeout_interval; /**< interval in which the datastream times out in us */ - uint64_t _event_count; /**< total data counter */ - uint64_t _error_count; /**< error count */ - int _error_density; /**< ratio between successful reads and errors */ - int _priority; /**< sensor nominal priority */ - float _mean[_dimensions]; /**< mean of value */ - float _lp[3]; /**< low pass value */ - float _M2[3]; /**< RMS component value */ - float _rms[3]; /**< root mean square error */ - float _value[3]; /**< last value */ - float _value_equal_count; /**< equal values in a row */ - DataValidator *_sibling; /**< sibling in the group */ - const unsigned NORETURN_ERRCOUNT = 10000; /**< if the error count reaches this value, return sensor as invalid */ - const float ERROR_DENSITY_WINDOW = 100.0f; /**< window in measurement counts for errors */ - const unsigned VALUE_EQUAL_COUNT_MAX = 100; /**< if the sensor value is the same (accumulated also between axes) this many times, flag it */ - - /* we don't want this class to be copied */ - DataValidator(const DataValidator&); - DataValidator operator=(const DataValidator&); -}; diff --git a/src/lib/ecl/validation/data_validator_group.cpp b/src/lib/ecl/validation/data_validator_group.cpp deleted file mode 100644 index 8bf060cf65..0000000000 --- a/src/lib/ecl/validation/data_validator_group.cpp +++ /dev/null @@ -1,266 +0,0 @@ -/**************************************************************************** - * - * 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 data_validator_group.cpp - * - * A data validation group to identify anomalies in data streams - * - * @author Lorenz Meier - */ - -#include "data_validator_group.h" -#include - -DataValidatorGroup::DataValidatorGroup(unsigned siblings) : - _first(nullptr), - _curr_best(-1), - _prev_best(-1), - _first_failover_time(0), - _toggle_count(0) -{ - DataValidator *next = _first; - - for (unsigned i = 0; i < siblings; i++) { - next = new DataValidator(next); - } - - _first = next; -} - -DataValidatorGroup::~DataValidatorGroup() -{ - -} - -void -DataValidatorGroup::set_timeout(uint64_t timeout_interval_us) -{ - DataValidator *next = _first; - - while (next != nullptr) { - next->set_timeout(timeout_interval_us); - next = next->sibling(); - } -} - -void -DataValidatorGroup::put(unsigned index, uint64_t timestamp, float val[3], uint64_t error_count, int priority) -{ - DataValidator *next = _first; - unsigned i = 0; - - while (next != nullptr) { - if (i == index) { - next->put(timestamp, val, error_count, priority); - break; - } - next = next->sibling(); - i++; - } -} - -float* -DataValidatorGroup::get_best(uint64_t timestamp, int *index) -{ - DataValidator *next = _first; - - // XXX This should eventually also include voting - int pre_check_best = _curr_best; - float pre_check_confidence = 1.0f; - int pre_check_prio = -1; - float max_confidence = -1.0f; - int max_priority = -1000; - int max_index = -1; - DataValidator *best = nullptr; - - unsigned i = 0; - - while (next != nullptr) { - float confidence = next->confidence(timestamp); - - if (static_cast(i) == pre_check_best) { - pre_check_prio = next->priority(); - pre_check_confidence = confidence; - } - - /* - * Switch if: - * 1) the confidence is higher and priority is equal or higher - * 2) the confidence is no less than 1% different and the priority is higher - */ - if (((max_confidence < MIN_REGULAR_CONFIDENCE) && (confidence >= MIN_REGULAR_CONFIDENCE)) || - (confidence > max_confidence && (next->priority() >= max_priority)) || - (fabsf(confidence - max_confidence) < 0.01f && (next->priority() > max_priority)) - ) { - max_index = i; - max_confidence = confidence; - max_priority = next->priority(); - best = next; - } - - next = next->sibling(); - i++; - } - - /* the current best sensor is not matching the previous best sensor */ - if (max_index != _curr_best) { - - bool true_failsafe = true; - - /* check whether the switch was a failsafe or preferring a higher priority sensor */ - if (pre_check_prio != -1 && pre_check_prio < max_priority && - fabsf(pre_check_confidence - max_confidence) < 0.1f) { - /* this is not a failover */ - true_failsafe = false; - /* reset error flags, this is likely a hotplug sensor coming online late */ - best->reset_state(); - } - - /* if we're no initialized, initialize the bookkeeping but do not count a failsafe */ - if (_curr_best < 0) { - _prev_best = max_index; - } else { - /* we were initialized before, this is a real failsafe */ - _prev_best = pre_check_best; - - if (true_failsafe) { - _toggle_count++; - - /* if this is the first time, log when we failed */ - if (_first_failover_time == 0) { - _first_failover_time = timestamp; - } - } - } - - /* for all cases we want to keep a record of the best index */ - _curr_best = max_index; - } - *index = max_index; - return (best) ? best->value() : nullptr; -} - -float -DataValidatorGroup::get_vibration_factor(uint64_t timestamp) -{ - DataValidator *next = _first; - - float vibe = 0.0f; - - /* find the best RMS value of a non-timed out sensor */ - while (next != nullptr) { - - if (next->confidence(timestamp) > 0.5f) { - float* rms = next->rms(); - - for (unsigned j = 0; j < 3; j++) { - if (rms[j] > vibe) { - vibe = rms[j]; - } - } - } - - next = next->sibling(); - } - - return vibe; -} - -void -DataValidatorGroup::print() -{ - /* print the group's state */ - ECL_INFO("validator: best: %d, prev best: %d, failsafe: %s (%u events)", - _curr_best, _prev_best, (_toggle_count > 0) ? "YES" : "NO", - _toggle_count); - - DataValidator *next = _first; - unsigned i = 0; - - while (next != nullptr) { - if (next->used()) { - uint32_t flags = next->state(); - - ECL_INFO("sensor #%u, prio: %d, state:%s%s%s%s%s%s", i, next->priority(), - ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " NO_DATA" : ""), - ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE_DATA" : ""), - ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " DATA_TIMEOUT" : ""), - ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " HIGH_ERRCOUNT" : ""), - ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " HIGH_ERRDENSITY" : ""), - ((flags == DataValidator::ERROR_FLAG_NO_ERROR) ? " OK" : "")); - - next->print(); - } - next = next->sibling(); - i++; - } -} - -unsigned -DataValidatorGroup::failover_count() -{ - return _toggle_count; -} - -int -DataValidatorGroup::failover_index() -{ - DataValidator *next = _first; - unsigned i = 0; - - while (next != nullptr) { - if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == (unsigned)_prev_best)) { - return i; - } - next = next->sibling(); - i++; - } - return -1; -} - -uint32_t -DataValidatorGroup::failover_state() -{ - DataValidator *next = _first; - unsigned i = 0; - - while (next != nullptr) { - if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == (unsigned)_prev_best)) { - return next->state(); - } - next = next->sibling(); - i++; - } - return DataValidator::ERROR_FLAG_NO_ERROR; -} diff --git a/src/lib/ecl/validation/data_validator_group.h b/src/lib/ecl/validation/data_validator_group.h deleted file mode 100644 index 855c3ed4ae..0000000000 --- a/src/lib/ecl/validation/data_validator_group.h +++ /dev/null @@ -1,122 +0,0 @@ -/**************************************************************************** - * - * 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 data_validator_group.h - * - * A data validation group to identify anomalies in data streams - * - * @author Lorenz Meier - */ - -#pragma once - -#include "data_validator.h" - -class __EXPORT DataValidatorGroup { -public: - DataValidatorGroup(unsigned siblings); - virtual ~DataValidatorGroup(); - - /** - * Put an item into the validator group. - * - * @param index Sensor index - * @param timestamp The timestamp of the measurement - * @param val The 3D vector - * @param error_count The current error count of the sensor - * @param priority The priority of the sensor - */ - void put(unsigned index, uint64_t timestamp, - float val[3], uint64_t error_count, int priority); - - /** - * Get the best data triplet of the group - * - * @return pointer to the array of best values - */ - float* get_best(uint64_t timestamp, int *index); - - /** - * Get the RMS / vibration factor - * - * @return float value representing the RMS, which a valid indicator for vibration - */ - float get_vibration_factor(uint64_t timestamp); - - /** - * Get the number of failover events - * - * @return the number of failovers - */ - unsigned failover_count(); - - /** - * Get the index of the failed sensor in the group - * - * @return index of the failed sensor - */ - int failover_index(); - - /** - * Get the error state of the failed sensor in the group - * - * @return bitmask with erro states of the failed sensor - */ - uint32_t failover_state(); - - /** - * Print the validator value - * - */ - void print(); - - /** - * Set the timeout value - * - * @param timeout_interval_us The timeout interval in microseconds - */ - void set_timeout(uint64_t timeout_interval_us); - -private: - DataValidator *_first; /**< sibling in the group */ - int _curr_best; /**< currently best index */ - int _prev_best; /**< the previous best index */ - uint64_t _first_failover_time; /**< timestamp where the first failover occured or zero if none occured */ - unsigned _toggle_count; /**< number of back and forth switches between two sensors */ - static constexpr float MIN_REGULAR_CONFIDENCE = 0.9f; - - /* we don't want this class to be copied */ - DataValidatorGroup(const DataValidatorGroup&); - DataValidatorGroup operator=(const DataValidatorGroup&); -}; diff --git a/src/lib/matrix b/src/lib/matrix index 9cd6ac3dd9..b9d6cac199 160000 --- a/src/lib/matrix +++ b/src/lib/matrix @@ -1 +1 @@ -Subproject commit 9cd6ac3dd9a255a1a2ef299da413c38a9040b8c7 +Subproject commit b9d6cac199bd4476c6268f7eba76c01bf29f0295 diff --git a/src/lib/rc/dsm.c b/src/lib/rc/dsm.c index 5d7186ec1b..22aeeddb63 100644 --- a/src/lib/rc/dsm.c +++ b/src/lib/rc/dsm.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -40,7 +40,8 @@ */ #include -#include +#include +#include #include #include @@ -49,27 +50,32 @@ #include "dsm.h" #include -#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 -#include +#if defined (__PX4_LINUX) || defined (__PX4_DARWIN) +#define dsm_udelay(arg) usleep(arg) +#else +#include +#define dsm_udelay(arg) up_udelay(arg) #endif -#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 -#include -#endif +//#define DSM_DEBUG -#define DSM_FRAME_SIZE 16 /**= 0) { @@ -247,17 +257,43 @@ dsm_init(const char *device) /* reset the format detector */ dsm_guess_format(true); - //debug("DSM: ready"); - - } else { - - //debug("DSM: open failed"); - + ret = 0; } - return dsm_fd; + return ret; } +/** + * Initialize the DSM receive functionality + * + * Open the UART for receiving DSM frames and configure it appropriately + * + * @param[in] device Device name of DSM UART + */ +int +dsm_init(const char *device) +{ + + if (dsm_fd < 0) { + dsm_fd = open(device, O_RDONLY | O_NONBLOCK); + } + + dsm_channel_shift = 0; + dsm_frame_drops = 0; + dsm_chan_count = 0; + dsm_decode_state = DSM_DECODE_STATE_DESYNC; + + int ret = dsm_config(dsm_fd); + + if (!ret) { + return dsm_fd; + + } else { + return -1; + } +} + +#ifdef GPIO_SPEKTRUM_PWR_EN /** * Handle DSM satellite receiver bind mode handler * @@ -267,64 +303,52 @@ dsm_init(const char *device) void dsm_bind(uint16_t cmd, int pulses) { -#if !defined(GPIO_USART1_RX_SPEKTRUM) -#else - if (dsm_fd < 0) { return; } switch (cmd) { - case dsm_bind_power_down: + case DSM_CMD_BIND_POWER_DOWN: /*power down DSM satellite*/ -#if defined(CONFIG_ARCH_BOARD_PX4IO_V1) - POWER_RELAY1(0); -#elif defined(CONFIG_ARCH_BOARD_PX4IO_V2) POWER_SPEKTRUM(0); -#endif break; - case dsm_bind_power_up: + case DSM_CMD_BIND_POWER_UP: /*power up DSM satellite*/ -#if defined(CONFIG_ARCH_BOARD_PX4IO_V1) - POWER_RELAY1(1); -#elif defined(CONFIG_ARCH_BOARD_PX4IO_V2) POWER_SPEKTRUM(1); -#endif dsm_guess_format(true); break; - case dsm_bind_set_rx_out: + case DSM_CMD_BIND_SET_RX_OUT: /*Set UART RX pin to active output mode*/ - stm32_configgpio(GPIO_USART1_RX_SPEKTRUM); + SPEKTRUM_RX_AS_GPIO(); break; - case dsm_bind_send_pulses: + case DSM_CMD_BIND_SEND_PULSES: /*Pulse RX pin a number of times*/ for (int i = 0; i < pulses; i++) { - up_udelay(120); - stm32_gpiowrite(GPIO_USART1_RX_SPEKTRUM, false); - up_udelay(120); - stm32_gpiowrite(GPIO_USART1_RX_SPEKTRUM, true); + dsm_udelay(120); + SPEKTRUM_RX_HIGH(false); + dsm_udelay(120); + SPEKTRUM_RX_HIGH(true); } break; - case dsm_bind_reinit_uart: + case DSM_CMD_BIND_REINIT_UART: /*Restore USART RX pin to RS232 receive mode*/ - stm32_configgpio(GPIO_USART1_RX); + SPEKTRUM_RX_AS_UART(); break; } - -#endif } +#endif /** * Decode the entire dsm frame (all contained channels) @@ -335,7 +359,7 @@ dsm_bind(uint16_t cmd, int pulses) * @return true=DSM frame successfully decoded, false=no update */ bool -dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, unsigned max_values) +dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, unsigned max_values) { /* debug("DSM dsm_frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x", @@ -350,13 +374,11 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, unsig dsm_guess_format(true); } - /* we have received something we think is a dsm_frame */ - dsm_last_frame_time = frame_time; - /* if we don't know the dsm_frame format, update the guessing state machine */ if (dsm_channel_shift == 0) { - dsm_guess_format(false); - return false; + if (!dsm_guess_format(false)) { + return false; + } } /* @@ -380,6 +402,12 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, unsig continue; } + /* reset bit guessing state machine if the channel index is out of bounds */ + if (channel > DSM_MAX_CHANNEL_COUNT) { + dsm_guess_format(true); + return false; + } + /* ignore channels out of range */ if (channel >= max_values) { continue; @@ -447,14 +475,32 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, unsig *num_values = 12; } - if (dsm_channel_shift == 11) { - /* Set the 11-bit data indicator */ - *num_values |= 0x8000; - } + /* Set the 11-bit data indicator */ + *dsm_11_bit = (dsm_channel_shift == 11); + + /* we have received something we think is a dsm_frame */ + dsm_last_frame_time = frame_time; /* * XXX Note that we may be in failsafe here; we need to work out how to detect that. */ + +#ifdef DSM_DEBUG + printf("PARSED PACKET\n"); +#endif + + /* check all values */ + for (unsigned i = 0; i < *num_values; i++) { + /* if the value is unrealistic, fail the parsing entirely */ + if (values[i] < 600 || values[i] > 2400) { +#ifdef DSM_DEBUG + printf("DSM: VALUE RANGE FAIL\n"); +#endif + dsm_chan_count = 0; + return false; + } + } + return true; } @@ -480,27 +526,35 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, unsig * @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, unsigned max_values) +dsm_input(int dsm_fd, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, uint8_t *n_bytes, uint8_t **bytes, + unsigned max_values) { - ssize_t ret; + int ret = 1; hrt_abstime now; /* + * 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. + * + * We expect to only be called when bytes arrive for processing, + * and if an interval of more than 3ms 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... */ now = hrt_absolute_time(); - if ((now - dsm_last_rx_time) > 5000) { - if (dsm_partial_frame_count > 0) { - dsm_frame_drops++; - dsm_partial_frame_count = 0; - } - } - /* * Fetch bytes, but no more than we would need to complete - * the current dsm frame. + * a complete frame. */ - ret = read(dsm_fd, &dsm_frame[dsm_partial_frame_count], DSM_FRAME_SIZE - dsm_partial_frame_count); + + ret = read(dsm_fd, &dsm_buf[0], sizeof(dsm_buf) / sizeof(dsm_buf[0])); /* if the read failed for any reason, just give up here */ if (ret < 1) { @@ -508,27 +562,113 @@ dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **by } else { *n_bytes = ret; - *bytes = &dsm_frame[dsm_partial_frame_count]; + *bytes = &dsm_buf[0]; + } + + /* + * Try to decode something with what we got + */ + return dsm_parse(now, &dsm_buf[0], ret, values, num_values, dsm_11_bit, &dsm_frame_drops, max_values); +} + +bool +dsm_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values, + uint16_t *num_values, bool *dsm_11_bit, unsigned *frame_drops, uint16_t max_channels) +{ + + /* this is set by the decoding state machine and will default to false + * once everything that was decodable has been decoded. + */ + bool decode_ret = false; + + /* keep decoding until we have consumed the buffer */ + for (unsigned d = 0; d < len; d++) { + + /* overflow check */ + if (dsm_partial_frame_count == sizeof(dsm_frame) / sizeof(dsm_frame[0])) { + dsm_partial_frame_count = 0; + dsm_decode_state = DSM_DECODE_STATE_DESYNC; +#ifdef DSM_DEBUG + printf("DSM: RESET (BUF LIM)\n"); +#endif + } + + if (dsm_partial_frame_count == DSM_FRAME_SIZE) { + dsm_partial_frame_count = 0; + dsm_decode_state = DSM_DECODE_STATE_DESYNC; +#ifdef DSM_DEBUG + printf("DSM: RESET (PACKET LIM)\n"); +#endif + } + +#ifdef DSM_DEBUG +#if 1 + printf("dsm state: %s%s, count: %d, val: %02x\n", + (dsm_decode_state == DSM_DECODE_STATE_DESYNC) ? "DSM_DECODE_STATE_DESYNC" : "", + (dsm_decode_state == DSM_DECODE_STATE_SYNC) ? "DSM_DECODE_STATE_SYNC" : "", + dsm_partial_frame_count, + (unsigned)frame[d]); +#endif +#endif + + switch (dsm_decode_state) { + case DSM_DECODE_STATE_DESYNC: + + /* we are de-synced and only interested in the frame marker */ + if ((now - dsm_last_rx_time) > 5000) { + dsm_decode_state = DSM_DECODE_STATE_SYNC; + dsm_partial_frame_count = 0; + dsm_chan_count = 0; + dsm_frame[dsm_partial_frame_count++] = frame[d]; + } + + break; + + case DSM_DECODE_STATE_SYNC: { + dsm_frame[dsm_partial_frame_count++] = frame[d]; + + /* decode whatever we got and expect */ + if (dsm_partial_frame_count < DSM_FRAME_SIZE) { + break; + } + + /* + * Great, it looks like we might have a frame. Go ahead and + * decode it. + */ + decode_ret = dsm_decode(now, values, &dsm_chan_count, dsm_11_bit, max_channels); + + /* we consumed the partial frame, reset */ + dsm_partial_frame_count = 0; + + /* if decoding failed, set proto to desync */ + if (decode_ret == false) { + dsm_decode_state = DSM_DECODE_STATE_DESYNC; + dsm_frame_drops++; + } + } + break; + + default: +#ifdef DSM_DEBUG + printf("UNKNOWN PROTO STATE"); +#endif + decode_ret = false; + } + + + } + + if (frame_drops) { + *frame_drops = dsm_frame_drops; + } + + if (decode_ret) { + *num_values = dsm_chan_count; } dsm_last_rx_time = now; - /* - * Add bytes to the current dsm frame - */ - dsm_partial_frame_count += ret; - - /* - * If we don't have a full dsm frame, return - */ - if (dsm_partial_frame_count < DSM_FRAME_SIZE) { - return false; - } - - /* - * Great, it looks like we might have a dsm frame. Go ahead and - * decode it. - */ - dsm_partial_frame_count = 0; - return dsm_decode(now, values, num_values, max_values); + /* return false as default */ + return decode_ret; } diff --git a/src/lib/rc/dsm.h b/src/lib/rc/dsm.h index 80614d066e..669b319017 100644 --- a/src/lib/rc/dsm.h +++ b/src/lib/rc/dsm.h @@ -42,11 +42,35 @@ #pragma once #include +#include +#include +#include __BEGIN_DECLS +#define DSM_FRAME_SIZE 16 /**< DSM frame size in bytes */ +#define DSM_FRAME_CHANNELS 7 /**< Max supported DSM channels per frame */ +#define DSM_MAX_CHANNEL_COUNT 18 /**< Max channel count of any DSM RC */ +#define DSM_BUFFER_SIZE (DSM_FRAME_SIZE + DSM_FRAME_SIZE / 2) + __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 int dsm_config(int dsm_fd); +__EXPORT bool dsm_input(int dsm_fd, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, uint8_t *n_bytes, + uint8_t **bytes, unsigned max_values); + +__EXPORT bool dsm_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values, + uint16_t *num_values, bool *dsm_11_bit, unsigned *frame_drops, uint16_t max_channels); + +#ifdef GPIO_SPEKTRUM_PWR_EN __EXPORT void dsm_bind(uint16_t cmd, int pulses); +#endif + +enum DSM_CMD { /* DSM bind states */ + DSM_CMD_BIND_POWER_DOWN = 0, + DSM_CMD_BIND_POWER_UP, + DSM_CMD_BIND_SET_RX_OUT, + DSM_CMD_BIND_SEND_PULSES, + DSM_CMD_BIND_REINIT_UART +}; __END_DECLS diff --git a/src/lib/rc/sbus.c b/src/lib/rc/sbus.c index f911648416..fe72bfb959 100644 --- a/src/lib/rc/sbus.c +++ b/src/lib/rc/sbus.c @@ -44,6 +44,10 @@ #include #include +#ifdef TIOCSSINGLEWIRE +#include +#endif + #include "sbus.h" #include @@ -122,6 +126,21 @@ sbus_init(const char *device, bool singlewire) { int sbus_fd = open(device, O_RDWR | O_NONBLOCK); + int ret = sbus_config(sbus_fd, singlewire); + + if (!ret) { + return sbus_fd; + + } else { + return -1; + } +} + +int +sbus_config(int sbus_fd, bool singlewire) +{ + int ret = -1; + if (sbus_fd >= 0) { struct termios t; @@ -133,8 +152,8 @@ sbus_init(const char *device, bool singlewire) if (singlewire) { /* only defined in configs capable of IOCTL */ -#ifdef SBUS_SERIAL_PORT - //ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); +#ifdef TIOCSSINGLEWIRE + ioctl(sbus_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); #endif } @@ -143,9 +162,11 @@ sbus_init(const char *device, bool singlewire) last_rx_time = hrt_absolute_time(); last_frame_time = last_rx_time; sbus_frame_drops = 0; + + ret = 0; } - return sbus_fd; + return ret; } void diff --git a/src/lib/rc/sbus.h b/src/lib/rc/sbus.h index 0da5ea2935..2c8f389c86 100644 --- a/src/lib/rc/sbus.h +++ b/src/lib/rc/sbus.h @@ -68,6 +68,7 @@ __EXPORT int sbus_init(const char *device, bool singlewire); * provides a degree of protection. Of course, it would be better * if we didn't drop bytes... */ +__EXPORT int sbus_config(int sbus_fd, bool singlewire); __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); diff --git a/src/lib/ecl/CMakeLists.txt b/src/lib/tailsitter_recovery/CMakeLists.txt similarity index 85% rename from src/lib/ecl/CMakeLists.txt rename to src/lib/tailsitter_recovery/CMakeLists.txt index 9f8e02ba2e..c7bd713592 100644 --- a/src/lib/ecl/CMakeLists.txt +++ b/src/lib/tailsitter_recovery/CMakeLists.txt @@ -31,18 +31,11 @@ # ############################################################################ px4_add_module( - MODULE lib__ecl + MODULE lib__tailsitter_recovery COMPILE_FLAGS -Os SRCS - attitude_fw/ecl_controller.cpp - 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 + tailsitter_recovery.cpp DEPENDS platforms__common ) diff --git a/src/lib/tailsitter_recovery/tailsitter_recovery.cpp b/src/lib/tailsitter_recovery/tailsitter_recovery.cpp new file mode 100644 index 0000000000..f18081b6f5 --- /dev/null +++ b/src/lib/tailsitter_recovery/tailsitter_recovery.cpp @@ -0,0 +1,182 @@ +/**************************************************************************** + * + * 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_recovery.cpp +*/ + +#include "tailsitter_recovery.h" +#include + +TailsitterRecovery::TailsitterRecovery(): + _in_recovery_mode(false) +{ + _att_p(0) = _att_p(1) = _att_p(2) = 4.0f; +} + +TailsitterRecovery::~TailsitterRecovery() +{ + +} + +void TailsitterRecovery::setAttGains(math::Vector<3> &att_p, float yaw_ff) +{ + _att_p = att_p; + _yaw_ff = yaw_ff; +} + +void TailsitterRecovery::calcOptimalRates(math::Quaternion &q, math::Quaternion &q_sp, float yaw_move_rate, + math::Vector<3> &rates_opt) +{ + math::Matrix<3, 3> R = q.to_dcm(); + + // compute error quaternion + math::Quaternion q_sp_inv = {q_sp(0), -q_sp(1), -q_sp(2), -q_sp(3)}; + math::Quaternion q_error = q * q_sp_inv; + + // compute tilt angle and corresponding tilt axis + math::Vector<3> zB = {0, 0, -1.0f}; + + math::Vector<3> zI = q_error.conjugate(zB); + + float tilt_angle; + float inner_prod = zI * zB; + + if (inner_prod >= 1.0f) { + tilt_angle = 0.0f; + + } else if (inner_prod <= -1.0f) { + tilt_angle = M_PI_F; + + } else { + tilt_angle = acosf(inner_prod); + } + + math::Vector<3> tilt_axis = {0, 0, -1}; + + if (math::min(fabsf(tilt_angle), fabsf(tilt_angle - M_PI_F)) > 0.00001f) { + tilt_axis = zI % zB; + } + + tilt_axis = R.transposed() * tilt_axis; + + // compute desired rates based on tilt angle and tilt axis + float tilt_dir = atan2f(tilt_axis(0), tilt_axis(1)); + float sign_x; + float sign_y; + float sign_z; + + if (tilt_dir < -M_PI_2_F) { + tilt_dir += M_PI_F; + sign_x = -1; + sign_y = -1; + sign_z = 1; + + } else if (tilt_dir < 0.0f) { + tilt_dir = 0.0f - tilt_dir; + sign_x = -1; + sign_y = 1; + sign_z = -1; + + } else if (tilt_dir < M_PI_2_F) { + tilt_dir += 0; + sign_x = 1; + sign_y = 1; + sign_z = 1; + + } else { + tilt_dir = M_PI_F - tilt_dir; + sign_x = 1; + sign_y = -1; + sign_z = -1; + } + + // optimal coefficients + float pwx_kx1 = -1.676f; + float pwx_ky1 = 1.38f; + float pwx_ky2 = 0.8725f; + float pwx_sx0 = 0.3586f; + float pwx_sx1 = 2.642f; + + float pwy_kx1 = -3.997f; + float pwy_sx0 = 2.133f; + float pwy_sx1 = 4.013f; + + float pwz_kx1 = 2.726f; + float pwz_ky1 = 3.168f; + float pwz_ky2 = -0.3913f; + float pwz_sx0 = 1.75f; + float pwz_sx1 = 2.298f; + + float x = tilt_angle; + float y = tilt_dir; + + rates_opt(1) = (pwx_ky1 * (M_PI_2_F - y) + pwx_ky2 * powf(M_PI_2_F - y, 2)) * + (pwx_kx1 * x * SigmoidFunction(pwx_sx1 * (pwx_sx0 - x)) - pwx_kx1 * x * expf(pwx_sx1 * + (pwx_sx0 - M_PI_F)) * SigmoidFunction(pwx_sx1 * (M_PI_F - pwx_sx0))); + rates_opt(0) = pwy_kx1 * x + powf((M_PI_2_F - y) / (M_PI_2_F), + 2) * (-pwy_kx1 * x * SigmoidFunction(pwy_sx1 * (pwy_sx0 - x))); + rates_opt(2) = (pwz_ky1 * (M_PI_2_F - y) + pwz_ky2 * powf(M_PI_2_F - y, 2)) * + (pwz_kx1 * (x - M_PI_F) * SigmoidFunction(pwz_sx1 * (x - pwz_sx0)) - pwz_kx1 * (x - M_PI_F) * SigmoidFunction( + -pwz_sx0 * pwz_sx1)); + rates_opt(0) *= -sign_x; + rates_opt(1) *= -sign_y * 1.5f; + rates_opt(2) *= -sign_z; + + float yaw_w = R(2, 2) > 0.0f ? R(2, 2) : 0.0f; + yaw_w = math::constrain(yaw_w, 0.0f, 1.0f); + + // we activate recovery mode if our tilt error is more that 30 degrees or + if (!_in_recovery_mode) { + if (fabsf(tilt_angle) > math::radians(30.0f)) { + _in_recovery_mode = true; + } + + } else { + if (fabsf(tilt_angle) < math::radians(5.0f)) { + _in_recovery_mode = false; + } + } + + // do normal attitude control, the other case has already been handled + if (!_in_recovery_mode) { + q_error = q_sp_inv * q; + rates_opt = q_error(0) > 0.0f ? _att_p.emult(q_error.imag()) * (-2.0f) : _att_p.emult(q_error.imag()) * (2.0f); + // don't want too strong yaw control. after recovery vehicle might have + // very large yaw error and shouldn't turn too fast + rates_opt(2) = math::constrain(rates_opt(2), -1.0f, 1.0f); + } + + rates_opt(2) += yaw_move_rate * yaw_w * _yaw_ff; + +} diff --git a/src/lib/tailsitter_recovery/tailsitter_recovery.h b/src/lib/tailsitter_recovery/tailsitter_recovery.h new file mode 100644 index 0000000000..5de16d25e8 --- /dev/null +++ b/src/lib/tailsitter_recovery/tailsitter_recovery.h @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * 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_recovery.h + * + * Tailsitter optimal rate controller (underactuated pitch axis) + * + * This function can be used to compute desired body rates of a tailsitter + * suffering from an underactuated pitch axis. Tailsitters which produce a pitching moment + * from airflow over control surfaces mostly suffer from an underactuated pitch axis. + * This functions captures the solution of an optimal control problem which minimises + * the vehicle's tilt error and which penalises the desired rates of the underactuated + * pitch axis. + * Publication: + * Robin Ritz and Raffaello D'Andrea. A Global Strategy for Tailsitter Hover Control. + * + * @author Roman Bapst +*/ +#include + +#define SigmoidFunction(val) 1/(1 + expf(-val)) + +class TailsitterRecovery +{ +public: + TailsitterRecovery(); + ~TailsitterRecovery(); + + // Calculate the optimal rates: + // If the vehicle is not in need of a recovery, this function will do normal + // attitude control based on attitude error. If a recovery situation is detected + // then the rates are computed in an optimal way as described above. + void calcOptimalRates(math::Quaternion &q, math::Quaternion &q_sp, float yaw_move_rate, math::Vector<3> &rates_opt); + + // Set the gains of the controller attitude loop. + void setAttGains(math::Vector<3> &att_p, float yaw_ff); + +private: + bool _in_recovery_mode; // indicates that the tailsitter is performing a recovery to hover + + math::Vector<3> _att_p; // gains for attitude loop + float _yaw_ff; // yaw feed forward gain +}; 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 3e44579c66..fb0dce7ca5 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -552,7 +552,7 @@ void AttitudeEstimatorQ::task_main() _vel_prev_t = 0; } - // Time from previous iteration + /* time from previous iteration */ hrt_abstime now = hrt_absolute_time(); float dt = (last_time > 0) ? ((now - last_time) / 1000000.0f) : 0.00001f; last_time = now; @@ -590,32 +590,28 @@ void AttitudeEstimatorQ::task_main() /* copy rotation matrix */ memcpy(&att.R[0], R.data, sizeof(att.R)); att.R_valid = true; + memcpy(&att.q[0], _q.data, sizeof(att.q)); + att.q_valid = true; att.rate_vibration = _voter_gyro.get_vibration_factor(hrt_absolute_time()); att.accel_vibration = _voter_accel.get_vibration_factor(hrt_absolute_time()); att.mag_vibration = _voter_mag.get_vibration_factor(hrt_absolute_time()); - if (_att_pub == nullptr) { - _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); - - } else { - orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att); - } + /* the instance count is not used here */ + int att_inst; + orb_publish_auto(ORB_ID(vehicle_attitude), &_att_pub, &att, &att_inst, ORB_PRIO_HIGH); struct control_state_s ctrl_state = {}; ctrl_state.timestamp = sensors.timestamp; - /* Attitude quaternions for control state */ + /* attitude quaternions for control state */ ctrl_state.q[0] = _q(0); - ctrl_state.q[1] = _q(1); - ctrl_state.q[2] = _q(2); - ctrl_state.q[3] = _q(3); - /* Attitude rates for control state */ + /* attitude rates for control state */ ctrl_state.roll_rate = _lp_roll_rate.apply(_rates(0)); ctrl_state.pitch_rate = _lp_pitch_rate.apply(_rates(1)); @@ -632,13 +628,10 @@ void AttitudeEstimatorQ::task_main() 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); - - } else { - orb_publish(ORB_ID(control_state), _ctrl_state_pub, &ctrl_state); - } + /* the instance count is not used here */ + int ctrl_inst; + /* publish to control state topic */ + orb_publish_auto(ORB_ID(control_state), &_ctrl_state_pub, &ctrl_state, &ctrl_inst, ORB_PRIO_HIGH); } } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index cccd2bec0c..03684847e6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -94,6 +94,7 @@ #include #include #include +#include #include #include @@ -227,7 +228,8 @@ void usage(const char *reason); */ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, - struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub); + struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub, + orb_advert_t *command_ack_pub, struct vehicle_command_ack_s *command_ack); /** * Mainloop of commander. @@ -270,7 +272,8 @@ static void commander_set_home_position(orb_advert_t &homePub, home_position_s & */ void *commander_low_prio_loop(void *arg); -void answer_command(struct vehicle_command_s &cmd, unsigned result); +void answer_command(struct vehicle_command_s &cmd, unsigned result, + orb_advert_t &command_ack_pub, vehicle_command_ack_s &command_ack); /** * check whether autostart ID is in the reserved range for HIL setups @@ -443,6 +446,28 @@ int commander_main(int argc, char *argv[]) return 0; } + if (!strcmp(argv[1], "land")) { + int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0); + + vehicle_command_s cmd = {}; + cmd.target_system = status.system_id; + cmd.target_component = status.component_id; + + cmd.command = vehicle_command_s::VEHICLE_CMD_NAV_LAND; + // cmd.param1 = 0.25f; /* minimum pitch */ + // /* param 2-3 unused */ + // cmd.param4 = home_position.yaw; + // cmd.param5 = home_position.lat; + // cmd.param6 = home_position.lon; + // cmd.param7 = home_position.alt; + + // XXX inspect use of publication handle + (void)orb_advertise(ORB_ID(vehicle_command), &cmd); + + px4_close(mavlink_fd_local); + return 0; + } + usage("unrecognized command"); return 1; } @@ -462,7 +487,7 @@ void print_status() warnx("power: USB: %s, BRICK: %s", (status.usb_connected) ? "[OK]" : "[NO]", (status.condition_power_input_valid) ? " [OK]" : "[NO]"); warnx("avionics rail: %6.2f V", (double)status.avionics_power_rail_voltage); - warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", _home.lat, _home.lon, (double)_home.alt); + warnx("home: lat = %.7f, lon = %.7f, alt = %.2f, yaw: %.2f", _home.lat, _home.lon, (double)_home.alt, (double)_home.yaw); warnx("home: x = %.7f, y = %.7f, z = %.2f ", (double)_home.x, (double)_home.y, (double)_home.z); /* read all relevant states */ @@ -543,7 +568,8 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char bool handle_command(struct vehicle_status_s *status_local, const struct safety_s *safety_local, struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local, struct home_position_s *home, struct vehicle_global_position_s *global_pos, - struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub) + struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub, + orb_advert_t *command_ack_pub, struct vehicle_command_ack_s *command_ack) { /* only handle commands that are meant to be handled by this system and component */ if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id) @@ -611,6 +637,9 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s case PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF: main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF); break; + case PX4_CUSTOM_SUB_MODE_AUTO_LAND: + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_LAND); + break; default: main_ret = TRANSITION_DENIED; @@ -883,6 +912,17 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } break; + case vehicle_command_s::VEHICLE_CMD_NAV_LAND: { + /* ok, home set, use it to take off */ + if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_LAND)) { + warnx("landing!"); + } else { + warnx("landing 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: @@ -904,19 +944,13 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s default: /* Warn about unsupported commands, this makes sense because only commands * to this component ID (or all) are passed by mavlink. */ - answer_command(*cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED); + answer_command(*cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED, *command_ack_pub, *command_ack); break; } if (cmd_result != vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED) { /* already warned about unsupported commands in "default" case */ - answer_command(*cmd, cmd_result); - } - - /* send any requested ACKs */ - if (cmd->confirmation > 0 && cmd_result != vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED) { - /* send acknowledge command */ - // XXX TODO + answer_command(*cmd, cmd_result, *command_ack_pub, *command_ack); } return true; @@ -1085,6 +1119,7 @@ int commander_thread_main(int argc, char *argv[]) status.rc_input_blocked = false; status.rc_input_mode = vehicle_status_s::RC_IN_MODE_DEFAULT; status.main_state =vehicle_status_s::MAIN_STATE_MANUAL; + status.main_state_prev = vehicle_status_s::MAIN_STATE_MAX; status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; status.arming_state = vehicle_status_s::ARMING_STATE_INIT; @@ -1150,6 +1185,11 @@ int commander_thread_main(int argc, char *argv[]) orb_advert_t home_pub = nullptr; memset(&_home, 0, sizeof(_home)); + /* command ack */ + orb_advert_t command_ack_pub = nullptr; + struct vehicle_command_ack_s command_ack; + memset(&command_ack, 0, sizeof(command_ack)); + /* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */ orb_advert_t mission_pub = nullptr; mission_s mission; @@ -2345,6 +2385,19 @@ int commander_thread_main(int argc, char *argv[]) } } + /* reset main state after takeoff or land has been completed */ + /* only switch back to at least altitude controlled modes */ + if (status.main_state_prev == vehicle_status_s::MAIN_STATE_POSCTL || + status.main_state_prev == vehicle_status_s::MAIN_STATE_ALTCTL) { + + if ((status.main_state == vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF + && mission_result.finished) || + (status.main_state == vehicle_status_s::MAIN_STATE_AUTO_LAND + && status.condition_landed)) { + + main_state_transition(&status, status.main_state_prev); + } + } /* handle commands last, as the system needs to be updated to handle them */ orb_check(cmd_sub, &updated); @@ -2354,7 +2407,8 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &local_position, &attitude, &home_pub)) { + if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &local_position, + &attitude, &home_pub, &command_ack_pub, &command_ack)) { status_changed = true; } } @@ -3107,7 +3161,8 @@ print_reject_arm(const char *msg) } } -void answer_command(struct vehicle_command_s &cmd, unsigned result) +void answer_command(struct vehicle_command_s &cmd, unsigned result, + orb_advert_t &command_ack_pub, vehicle_command_ack_s &command_ack) { switch (result) { case vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED: @@ -3138,18 +3193,34 @@ void answer_command(struct vehicle_command_s &cmd, unsigned result) default: break; } + + /* publish ACK */ + command_ack.command = cmd.command; + command_ack.result = result; + + if (command_ack_pub != nullptr) { + orb_publish(ORB_ID(vehicle_command_ack), command_ack_pub, &command_ack); + + } else { + command_ack_pub = orb_advertise(ORB_ID(vehicle_command_ack), &command_ack); + } } void *commander_low_prio_loop(void *arg) { /* Set thread name */ - px4_prctl(PR_SET_NAME, "commander_low_prio", getpid()); + px4_prctl(PR_SET_NAME, "commander_low_prio", px4_getpid()); /* Subscribe to command topic */ int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); struct vehicle_command_s cmd; memset(&cmd, 0, sizeof(cmd)); + /* command ack */ + orb_advert_t command_ack_pub = nullptr; + struct vehicle_command_ack_s command_ack; + memset(&command_ack, 0, sizeof(command_ack)); + /* timeout for param autosave */ hrt_abstime need_param_autosave_timeout = 0; @@ -3207,23 +3278,23 @@ void *commander_low_prio_loop(void *arg) if (is_safe(&status, &safety, &armed)) { if (((int)(cmd.param1)) == 1) { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); usleep(100000); /* reboot */ px4_systemreset(false); } else if (((int)(cmd.param1)) == 3) { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); usleep(100000); /* reboot to bootloader */ px4_systemreset(true); } else { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub, command_ack); } } else { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub, command_ack); } break; @@ -3235,7 +3306,7 @@ void *commander_low_prio_loop(void *arg) /* try to go to INIT/PREFLIGHT arming state */ if (TRANSITION_DENIED == arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_INIT, &armed, false /* fRunPreArmChecks */, mavlink_fd)) { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub, command_ack); break; } else { status.calibration_enabled = true; @@ -3243,21 +3314,21 @@ void *commander_low_prio_loop(void *arg) if ((int)(cmd.param1) == 1) { /* gyro calibration */ - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); calib_ret = do_gyro_calibration(mavlink_fd); } else if ((int)(cmd.param2) == 1) { /* magnetometer calibration */ - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); calib_ret = do_mag_calibration(mavlink_fd); } else if ((int)(cmd.param3) == 1) { /* zero-altitude pressure calibration */ - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub, command_ack); } else if ((int)(cmd.param4) == 1) { /* RC calibration */ - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); /* disable RC control input completely */ status.rc_input_blocked = true; calib_ret = OK; @@ -3265,31 +3336,31 @@ void *commander_low_prio_loop(void *arg) } else if ((int)(cmd.param4) == 2) { /* RC trim calibration */ - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); calib_ret = do_trim_calibration(mavlink_fd); } else if ((int)(cmd.param5) == 1) { /* accelerometer calibration */ - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); calib_ret = do_accel_calibration(mavlink_fd); } else if ((int)(cmd.param5) == 2) { // board offset calibration - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); calib_ret = do_level_calibration(mavlink_fd); } else if ((int)(cmd.param6) == 1) { /* airspeed calibration */ - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); calib_ret = do_airspeed_calibration(mavlink_fd); } else if ((int)(cmd.param7) == 1) { /* do esc calibration */ - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); calib_ret = do_esc_calibration(mavlink_fd, &armed); } else if ((int)(cmd.param4) == 0) { /* RC calibration ended - have we been in one worth confirming? */ if (status.rc_input_blocked) { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); /* enable RC control input */ status.rc_input_blocked = false; mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN"); @@ -3336,7 +3407,7 @@ void *commander_low_prio_loop(void *arg) if (ret == OK) { mavlink_log_info(mavlink_fd, "settings loaded"); - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); } else { mavlink_log_critical(mavlink_fd, "settings load ERROR"); @@ -3350,7 +3421,7 @@ void *commander_low_prio_loop(void *arg) mavlink_log_critical(mavlink_fd, "ERROR: %s", strerror(ret)); } - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED, command_ack_pub, command_ack); } } else if (((int)(cmd.param1)) == 1) { @@ -3364,7 +3435,7 @@ void *commander_low_prio_loop(void *arg) } /* do not spam MAVLink, but provide the answer / green led mechanism */ - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); } else { mavlink_log_critical(mavlink_fd, "settings save error"); @@ -3378,7 +3449,7 @@ void *commander_low_prio_loop(void *arg) mavlink_log_critical(mavlink_fd, "ERROR: %s", strerror(ret)); } - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED, command_ack_pub, command_ack); } } else if (((int)(cmd.param1)) == 2) { @@ -3389,11 +3460,11 @@ void *commander_low_prio_loop(void *arg) if (ret == OK) { /* do not spam MAVLink, but provide the answer / green led mechanism */ mavlink_log_critical(mavlink_fd, "onboard parameters reset"); - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); } else { mavlink_log_critical(mavlink_fd, "param reset error"); - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED, command_ack_pub, command_ack); } } @@ -3408,13 +3479,6 @@ void *commander_low_prio_loop(void *arg) /* don't answer on unsupported commands, it will be done in main loop */ break; } - - /* send any requested ACKs */ - if (cmd.confirmation > 0 && cmd.command != vehicle_command_s::VEHICLE_CMD_DO_SET_MODE - && cmd.command != vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM) { - /* send acknowledge command */ - // XXX TODO - } } } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index eba8feec99..ef0689bec2 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -73,6 +73,8 @@ static constexpr unsigned int calibration_sides = 6; ///< The total number of static constexpr unsigned int calibration_total_points = 240; ///< The total points per magnetometer static constexpr unsigned int calibraton_duration_seconds = 42; ///< The total duration the routine is allowed to take +static constexpr float MAG_MAX_OFFSET_LEN = 0.9f; ///< The maximum measurement range is ~1.4 Ga, the earth field is ~0.6 Ga, so an offset larger than ~0.8-0.6 Ga means the mag will saturate in some directions. + int32_t device_ids[max_mags]; int device_prio_max = 0; int32_t device_id_primary = 0; @@ -507,6 +509,13 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: NaN in sphere fit for mag #%u", cur_mag); result = calibrate_return_error; } + + if (sqrtf(sphere_x[cur_mag] * sphere_x[cur_mag] + + sphere_y[cur_mag] * sphere_y[cur_mag] + sphere_z[cur_mag] * sphere_z[cur_mag]) + > MAG_MAX_OFFSET_LEN) { + mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: Excessive offset for mag #%u", cur_mag); + result = calibrate_return_error; + } } } } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 1bde387c63..5163087830 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -352,6 +352,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: + case vehicle_status_s::MAIN_STATE_AUTO_LAND: /* need global position and home position */ if (status->condition_global_position_valid && status->condition_home_position_valid) { ret = TRANSITION_CHANGED; @@ -373,6 +374,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta } if (ret == TRANSITION_CHANGED) { if (status->main_state != new_main_state) { + status->main_state_prev = status->main_state; status->main_state = new_main_state; } else { ret = TRANSITION_NOT_CHANGED; @@ -766,6 +768,25 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } break; + case vehicle_status_s::MAIN_STATE_AUTO_LAND: + /* require global position and home */ + + if (status->engine_failure) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; + } else if ((!status->condition_global_position_valid || + !status->condition_home_position_valid)) { + status->failsafe = true; + + 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_LAND; + } + 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) { diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt new file mode 100644 index 0000000000..37e4f78d0b --- /dev/null +++ b/src/modules/ekf2/CMakeLists.txt @@ -0,0 +1,48 @@ +############################################################################ +# +# 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. +# +############################################################################# +set(MODULE_CFLAGS) +if (${OS} STREQUAL "nuttx") + list(APPEND MODULE_CFLAGS -Wframe-larger-than=4000) +endif() +px4_add_module( + MODULE modules__ekf2 + MAIN ekf2 + COMPILE_FLAGS ${MODULE_CFLAGS} + STACK 1000 + SRCS + ekf2_main.cpp + DEPENDS + platforms__common + git_ecl + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp new file mode 100644 index 0000000000..2bd5d25fa0 --- /dev/null +++ b/src/modules/ekf2/ekf2_main.cpp @@ -0,0 +1,491 @@ +/**************************************************************************** + * + * 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 ekf2_main.cpp + * Implementation of the attitude and position estimator. + * + * @author Roman Bapst + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + + +extern "C" __EXPORT int ekf2_main(int argc, char *argv[]); + + +class Ekf2; + +namespace ekf2 +{ +Ekf2 *instance; +} + + +class Ekf2 +{ +public: + /** + * Constructor + */ + Ekf2(); + + /** + * Destructor, also kills task. + */ + ~Ekf2(); + + /** + * Start task. + * + * @return OK on success. + */ + int start(); + + static void task_main_trampoline(int argc, char *argv[]); + + void task_main(); + + void print(); + + void print_status(); + +private: + static constexpr float _dt_max = 0.02; + bool _task_should_exit = false; /**< if true, task should exit */ + int _control_task = -1; /**< task handle for task */ + + int _sensors_sub = -1; + int _gps_sub = -1; + int _airspeed_sub = -1; + + orb_advert_t _att_pub; + orb_advert_t _lpos_pub; + orb_advert_t _control_state_pub; + orb_advert_t _vehicle_global_position_pub; + + /* Low pass filter for attitude rates */ + math::LowPassFilter2p _lp_roll_rate; + math::LowPassFilter2p _lp_pitch_rate; + math::LowPassFilter2p _lp_yaw_rate; + + EstimatorBase *_ekf; + + + void update_parameters(bool force); + + int update_subscriptions(); + +}; + +Ekf2::Ekf2(): +_lp_roll_rate(250.0f, 30.0f), +_lp_pitch_rate(250.0f, 30.0f), +_lp_yaw_rate(250.0f, 20.0f) +{ + _ekf = new Ekf(); + _att_pub = nullptr; + _lpos_pub = nullptr; + _control_state_pub = nullptr; +} + +Ekf2::~Ekf2() +{ + +} + +void Ekf2::print() +{ + _ekf->printStoredGps(); + _ekf->printStoredBaro(); + _ekf->printStoredMag(); + _ekf->printStoredIMU(); +} + +void Ekf2::print_status() +{ + warnx("position OK %s", (_ekf->position_is_valid()) ? "[YES]" : "[NO]"); +} + +void Ekf2::task_main() +{ + // subscribe to relevant topics + _sensors_sub = orb_subscribe(ORB_ID(sensor_combined)); + _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + + px4_pollfd_struct_t fds[1]; + fds[0].fd = _sensors_sub; + fds[0].events = POLLIN; + + while (!_task_should_exit) { + int ret = px4_poll(fds, 1, 1000); + + if (ret < 0) { + // Poll error, sleep and try again + usleep(10000); + continue; + + } else if (ret == 0 || fds[0].revents != POLLIN) { + // Poll timeout or no new data, do nothing + continue; + } + + bool gps_updated = false; + bool airspeed_updated = false; + + sensor_combined_s sensors = {}; + vehicle_gps_position_s gps = {}; + airspeed_s airspeed = {}; + + orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors); + + // update all other topics if they have new data + orb_check(_gps_sub, &gps_updated); + + if (gps_updated) { + orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &gps); + } + + orb_check(_airspeed_sub, &airspeed_updated); + + if (airspeed_updated) { + orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed); + } + + hrt_abstime now = hrt_absolute_time(); + // push imu data into estimator + _ekf->setIMUData(now, sensors.gyro_integral_dt[0], sensors.accelerometer_integral_dt[0], + &sensors.gyro_integral_rad[0], &sensors.accelerometer_integral_m_s[0]); + + // read mag data + _ekf->setMagData(sensors.magnetometer_timestamp[0], &sensors.magnetometer_ga[0]); + + // read baro data + _ekf->setBaroData(sensors.baro_timestamp[0], &sensors.baro_alt_meter[0]); + + // read gps data if available + if (gps_updated) { + struct gps_message gps_msg = {}; + gps_msg.time_usec = gps.timestamp_position; + gps_msg.lat = gps.lat; + gps_msg.lon = gps.lon; + gps_msg.alt = gps.alt; + gps_msg.fix_type = gps.fix_type; + gps_msg.eph = gps.eph; + gps_msg.epv = gps.epv; + gps_msg.time_usec_vel = gps.timestamp_velocity; + gps_msg.vel_m_s = gps.vel_m_s; + gps_msg.vel_ned[0] = gps.vel_n_m_s; + gps_msg.vel_ned[1] = gps.vel_e_m_s; + gps_msg.vel_ned[2] = gps.vel_d_m_s; + gps_msg.vel_ned_valid = gps.vel_ned_valid; + + _ekf->setGpsData(gps.timestamp_position, &gps_msg); + } + + // read airspeed data if available + if (airspeed_updated) { + _ekf->setAirspeedData(airspeed.timestamp, &airspeed.indicated_airspeed_m_s); + } + + // run the EKF update + _ekf->update(); + + // generate vehicle attitude data + struct vehicle_attitude_s att; + att.timestamp = hrt_absolute_time(); + + _ekf->copy_quaternion(att.q); + matrix::Quaternion q(att.q[0], att.q[1], att.q[2], att.q[3]); + matrix::Euler euler(q); + att.roll = euler(0); + att.pitch = euler(1); + att.yaw = euler(2); + + // generate vehicle local position data + struct vehicle_local_position_s lpos; + float pos[3] = {}; + float vel[3] = {}; + + lpos.timestamp = hrt_absolute_time(); + + // Position in local NED frame + _ekf->copy_position(pos); + lpos.x = pos[0]; + lpos.y = pos[1]; + lpos.z = pos[2]; + + // Velocity in NED frame (m/s) + _ekf->copy_velocity(vel); + lpos.vx = vel[0]; + lpos.vy = vel[1]; + lpos.vz = vel[2]; + + // TODO: better status reporting + lpos.xy_valid = _ekf->position_is_valid(); + lpos.z_valid = true; + lpos.v_xy_valid = _ekf->position_is_valid(); + lpos.v_z_valid = true; + + // Position of local NED origin in GPS / WGS84 frame + lpos.ref_timestamp = _ekf->_last_gps_origin_time_us; // Time when reference position was set + lpos.xy_global = _ekf->position_is_valid();// true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) + lpos.z_global = true;// true if z is valid and has valid global reference (ref_alt) + lpos.ref_lat = _ekf->_posRef.lat_rad * (double)180.0 * M_PI; // Reference point latitude in degrees + lpos.ref_lon = _ekf->_posRef.lon_rad * (double)180.0 * M_PI; // Reference point longitude in degrees + lpos.ref_alt = _ekf->_gps_alt_ref; // Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level + + // The rotation of the tangent plane vs. geographical north + lpos.yaw = 0.0f; + + lpos.dist_bottom = 0.0f; // Distance to bottom surface (ground) in meters + lpos.dist_bottom_rate = 0.0f; // Distance to bottom surface (ground) change rate + lpos.surface_bottom_timestamp = 0; // Time when new bottom surface found + lpos.dist_bottom_valid = false; // true if distance to bottom surface is valid + + // TODO: uORB definition does not define what thes variables are. We have assumed them to be horizontal and vertical 1-std dev accuracy in metres + // TODO: Should use sqrt of filter position variances + lpos.eph = gps.eph; + lpos.epv = gps.epv; + + // publish vehicle local position data + if (_lpos_pub == nullptr) { + _lpos_pub = orb_advertise(ORB_ID(vehicle_local_position), &lpos); + } else { + orb_publish(ORB_ID(vehicle_local_position), _lpos_pub, &lpos); + } + + // generate control state data + control_state_s ctrl_state = {}; + ctrl_state.timestamp = hrt_absolute_time(); + ctrl_state.roll_rate = _lp_roll_rate.apply(sensors.gyro_rad_s[0]); + ctrl_state.pitch_rate = _lp_pitch_rate.apply(sensors.gyro_rad_s[1]); + ctrl_state.yaw_rate = _lp_yaw_rate.apply(sensors.gyro_rad_s[2]); + + ctrl_state.q[0] = q(0); + ctrl_state.q[1] = q(1); + ctrl_state.q[2] = q(2); + ctrl_state.q[3] = q(3); + + // publish control state data + if (_control_state_pub == nullptr) { + _control_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state); + } else { + orb_publish(ORB_ID(control_state), _control_state_pub, &ctrl_state); + } + + // generate vehicle attitude data + att.q[0] = q(0); + att.q[1] = q(1); + att.q[2] = q(2); + att.q[3] = q(3); + att.q_valid = true; + + att.rollspeed = sensors.gyro_rad_s[0]; + att.pitchspeed = sensors.gyro_rad_s[1]; + att.yawspeed = sensors.gyro_rad_s[2]; + + // publish vehicle attitude data + if (_att_pub == nullptr) { + _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); + } else { + orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att); + } + + // generate and publish global position data + struct vehicle_global_position_s global_pos; + if (_ekf->position_is_valid()) { + // TODO: local origin is currenlty at GPS height origin - this is different to ekf_att_pos_estimator + + global_pos.timestamp = hrt_absolute_time(); // Time of this estimate, in microseconds since system start + global_pos.time_utc_usec = gps.time_utc_usec; // GPS UTC timestamp in microseconds + + double est_lat, est_lon; + map_projection_reproject(&_ekf->_posRef, lpos.x, lpos.y, &est_lat, &est_lon); + global_pos.lat = est_lat; // Latitude in degrees + global_pos.lon = est_lon; // Longitude in degrees + + global_pos.alt = -pos[2]; // Altitude AMSL in meters + + global_pos.vel_n = vel[0]; // Ground north velocity, m/s + global_pos.vel_e = vel[1]; // Ground east velocity, m/s + global_pos.vel_d = vel[2]; // Ground downside velocity, m/s + + global_pos.yaw = euler(2); // Yaw in radians -PI..+PI. + + global_pos.eph = gps.eph; // Standard deviation of position estimate horizontally + global_pos.epv = gps.epv; // Standard deviation of position vertically + + // TODO: implement terrain estimator + global_pos.terrain_alt = 0.0f; // Terrain altitude in m, WGS84 + global_pos.terrain_alt_valid = false; // Terrain altitude estimate is valid + // TODO use innovatun consistency check timouts to set this + global_pos.dead_reckoning = false; // True if this position is estimated through dead-reckoning + + global_pos.pressure_alt = sensors.baro_alt_meter[0]; // Pressure altitude AMSL (m) + + if (_vehicle_global_position_pub == nullptr) { + _vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); + } else { + orb_publish(ORB_ID(vehicle_global_position), _vehicle_global_position_pub, &global_pos); + } + } + } +} + +void Ekf2::task_main_trampoline(int argc, char *argv[]) +{ + ekf2::instance->task_main(); +} + +int Ekf2::start() +{ + ASSERT(_control_task == -1); + + /* start the task */ + _control_task = px4_task_spawn_cmd("ekf2", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 9000, + (px4_main_t)&Ekf2::task_main_trampoline, + nullptr); + + if (_control_task < 0) { + PX4_WARN("task start failed"); + return -errno; + } + + return OK; +} + +int ekf2_main(int argc, char *argv[]) +{ + if (argc < 1) { + PX4_WARN("usage: ekf2 {start|stop|status}"); + return 1; + } + + if (!strcmp(argv[1], "start")) { + + if (ekf2::instance != nullptr) { + PX4_WARN("already running"); + return 1; + } + + ekf2::instance = new Ekf2(); + + if (ekf2::instance == nullptr) { + PX4_WARN("alloc failed"); + return 1; + } + + if (OK != ekf2::instance->start()) { + delete ekf2::instance; + ekf2::instance = nullptr; + PX4_WARN("start failed"); + return 1; + } + + return 0; + } + + if (!strcmp(argv[1], "stop")) { + if (ekf2::instance == nullptr) { + PX4_WARN("not running"); + return 1; + } + + delete ekf2::instance; + ekf2::instance = nullptr; + return 0; + } + + if (!strcmp(argv[1], "print")) { + if (ekf2::instance != nullptr) { + + return 0; + } + + return 1; + } + + if (!strcmp(argv[1], "status")) { + if (ekf2::instance) { + PX4_WARN("running"); + ekf2::instance->print_status(); + return 0; + + } else { + PX4_WARN("not running"); + return 1; + } + } + + PX4_WARN("unrecognized command"); + return 1; +} \ No newline at end of file diff --git a/src/modules/fw_att_control/CMakeLists.txt b/src/modules/fw_att_control/CMakeLists.txt index b093c71cee..5d0e6648c2 100644 --- a/src/modules/fw_att_control/CMakeLists.txt +++ b/src/modules/fw_att_control/CMakeLists.txt @@ -40,5 +40,7 @@ px4_add_module( fw_att_control_main.cpp DEPENDS platforms__common + git_ecl + lib__ecl ) # vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/fw_pos_control_l1/CMakeLists.txt b/src/modules/fw_pos_control_l1/CMakeLists.txt index e74b6e4b37..720d97fac7 100644 --- a/src/modules/fw_pos_control_l1/CMakeLists.txt +++ b/src/modules/fw_pos_control_l1/CMakeLists.txt @@ -44,6 +44,7 @@ px4_add_module( mtecs/limitoverride.cpp DEPENDS platforms__common + git_ecl lib__external_lgpl lib__ecl ) 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 14b73038a0..d52722c1bf 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 @@ -51,8 +51,8 @@ * A value of 25 meters works for most aircraft. Shorten * slowly during tuning until response is sharp without oscillation. * - * @min 1.0 - * @max 100.0 + * @min 12.0 + * @max 50.0 * @group L1 Control */ PARAM_DEFINE_FLOAT(FW_L1_PERIOD, 20.0f); diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index b86e431150..4b418f6523 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -100,7 +100,7 @@ protected: static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold before triggering a land */ static constexpr uint64_t LAND_DETECTOR_ARM_PHASE_TIME = - 1000000; /**< time interval in which wider acceptance thresholds are used after arming */ + 2000000; /**< time interval in which wider acceptance thresholds are used after arming */ protected: orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt index c8a0e7d870..3ae2e570ec 100644 --- a/src/modules/mavlink/CMakeLists.txt +++ b/src/modules/mavlink/CMakeLists.txt @@ -56,6 +56,7 @@ px4_add_module( mavlink_rate_limiter.cpp mavlink_receiver.cpp mavlink_ftp.cpp + mavlink_log_handler.cpp DEPENDS platforms__common ) diff --git a/src/modules/mavlink/mavlink_log_handler.cpp b/src/modules/mavlink/mavlink_log_handler.cpp new file mode 100644 index 0000000000..08197f51fe --- /dev/null +++ b/src/modules/mavlink/mavlink_log_handler.cpp @@ -0,0 +1,547 @@ +/**************************************************************************** + * + * Copyright (c) 2014, 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 mavlink_log_handler.h +/// @author px4dev, Gus Grubba + +#include "mavlink_log_handler.h" +#include "mavlink_main.h" +#include +#include + +#define MOUNTPOINT PX4_ROOTFSDIR "/fs/microsd" + +static const char *kLogRoot = MOUNTPOINT "/log"; +static const char *kLogData = MOUNTPOINT "/logdata.txt"; +static const char *kTmpData = MOUNTPOINT "/$log$.txt"; + +#ifdef __PX4_NUTTX + #define PX4LOG_REGULAR_FILE DTYPE_FILE + #define PX4LOG_DIRECTORY DTYPE_DIRECTORY +#else + #define PX4LOG_REGULAR_FILE DT_REG + #define PX4LOG_DIRECTORY DT_DIR +#endif + +//#define MAVLINK_LOG_HANDLER_VERBOSE + +#ifdef MAVLINK_LOG_HANDLER_VERBOSE +#define PX4LOG_WARN(fmt, ...) warnx(fmt, ##__VA_ARGS__) +#else +#define PX4LOG_WARN(fmt, ...) +#endif + +//------------------------------------------------------------------- +static bool +stat_file(const char* file, time_t* date = 0, uint32_t* size = 0) { + struct stat st; + if (stat(file, &st) == 0) { + if (date) *date = st.st_mtime; + if (size) *size = st.st_size; + return true; + } + return false; +} + +//------------------------------------------------------------------- +MavlinkLogHandler * +MavlinkLogHandler::new_instance(Mavlink *mavlink) +{ + return new MavlinkLogHandler(mavlink); +} + +//------------------------------------------------------------------- +MavlinkLogHandler::MavlinkLogHandler(Mavlink *mavlink) + : MavlinkStream(mavlink) + , _pLogHandlerHelper(0) +{ + +} + +//------------------------------------------------------------------- +void +MavlinkLogHandler::handle_message(const mavlink_message_t *msg) +{ + switch (msg->msgid) { + case MAVLINK_MSG_ID_LOG_REQUEST_LIST: + _log_request_list(msg); + break; + case MAVLINK_MSG_ID_LOG_REQUEST_DATA: + _log_request_data(msg); + break; + case MAVLINK_MSG_ID_LOG_ERASE: + _log_request_erase(msg); + break; + case MAVLINK_MSG_ID_LOG_REQUEST_END: + _log_request_end(msg); + break; + } +} + +//------------------------------------------------------------------- +const char* +MavlinkLogHandler::get_name(void) const +{ + return "MAVLINK_LOG_HANDLER"; +} + +//------------------------------------------------------------------- +uint8_t +MavlinkLogHandler::get_id(void) +{ + return MAVLINK_MSG_ID_LOG_ENTRY; +} + +//------------------------------------------------------------------- +unsigned +MavlinkLogHandler::get_size(void) +{ + //-- Sending Log Entries + if (_pLogHandlerHelper && _pLogHandlerHelper->current_status == LogListHelper::LOG_HANDLER_LISTING) { + return MAVLINK_MSG_ID_LOG_ENTRY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + //-- Sending Log Data (contents of one of the log files) + } else if (_pLogHandlerHelper && _pLogHandlerHelper->current_status == LogListHelper::LOG_HANDLER_SENDING_DATA) { + return MAVLINK_MSG_ID_LOG_DATA_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + //-- Idle + } else { + return 0; + } +} + +//------------------------------------------------------------------- +void +MavlinkLogHandler::send(const hrt_abstime /*t*/) +{ + //-- Send log entry stream packets until buffer is full + while (_pLogHandlerHelper && _pLogHandlerHelper->current_status == LogListHelper::LOG_HANDLER_LISTING && _mavlink->get_free_tx_buf() > get_size()) { + _log_send_listing(); + }; + //-- An arbitrary count of max log data packets in one go + int count = 100; + while (_pLogHandlerHelper && _pLogHandlerHelper->current_status == LogListHelper::LOG_HANDLER_SENDING_DATA && _mavlink->get_free_tx_buf() > get_size() && --count) { + _log_send_data(); + }; +} + +//------------------------------------------------------------------- +void +MavlinkLogHandler::_log_request_list(const mavlink_message_t *msg) +{ + mavlink_log_request_list_t request; + mavlink_msg_log_request_list_decode(msg, &request); + //-- Check for re-requests (data loss) or new request + if(_pLogHandlerHelper) { + _pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE; + //-- Is this a new request? + if((request.end - request.start) > _pLogHandlerHelper->log_count) { + delete _pLogHandlerHelper; + _pLogHandlerHelper = NULL; + } + } + if(!_pLogHandlerHelper) { + //-- Prepare new request + _pLogHandlerHelper = new LogListHelper; + } + if (_pLogHandlerHelper->log_count) + { + //-- Define (and clamp) range + _pLogHandlerHelper->next_entry = request.start < _pLogHandlerHelper->log_count ? request.start : _pLogHandlerHelper->log_count - 1; + _pLogHandlerHelper->last_entry = request.end < _pLogHandlerHelper->log_count ? request.end : _pLogHandlerHelper->log_count - 1; + } + PX4LOG_WARN("\nMavlinkLogHandler::_log_request_list: start: %u last: %u count: %u\n", + _pLogHandlerHelper->next_entry, + _pLogHandlerHelper->last_entry, + _pLogHandlerHelper->log_count); + //-- Enable streaming + _pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_LISTING; +} + +//------------------------------------------------------------------- +void +MavlinkLogHandler::_log_request_data(const mavlink_message_t *msg) +{ + //-- If we haven't listed, we can't do much + if(!_pLogHandlerHelper) { + PX4LOG_WARN("MavlinkLogHandler::_log_request_data Log request with no list requested.\n"); + return; + } + mavlink_log_request_data_t request; + mavlink_msg_log_request_data_decode(msg, &request); + //-- Does the requested log exist? + if (request.id >= _pLogHandlerHelper->log_count) { + PX4LOG_WARN("MavlinkLogHandler::_log_request_data Requested log %u but we only have %u.\n", request.id, _pLogHandlerHelper->log_count); + return; + } + //-- If we were sending log entries, stop it + _pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE; + //-- Init send log dataset + _pLogHandlerHelper->current_log_filename[0] = 0; + _pLogHandlerHelper->current_log_index = request.id; + uint32_t time_utc = 0; + _pLogHandlerHelper->get_entry(_pLogHandlerHelper->current_log_index, _pLogHandlerHelper->current_log_size, time_utc, _pLogHandlerHelper->current_log_filename); + _pLogHandlerHelper->current_log_data_offset = request.ofs; + if (_pLogHandlerHelper->current_log_data_offset >= _pLogHandlerHelper->current_log_size) { + _pLogHandlerHelper->current_log_data_remaining = 0; + } else { + _pLogHandlerHelper->current_log_data_remaining = _pLogHandlerHelper->current_log_size - request.ofs; + } + if (_pLogHandlerHelper->current_log_data_remaining > request.count) { + _pLogHandlerHelper->current_log_data_remaining = request.count; + } + //-- Enable streaming + _pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_SENDING_DATA; +} + +//------------------------------------------------------------------- +void +MavlinkLogHandler::_log_request_erase(const mavlink_message_t* /*msg*/) +{ + /* + mavlink_log_erase_t request; + mavlink_msg_log_erase_decode(msg, &request); + */ + if (_pLogHandlerHelper) { + delete _pLogHandlerHelper; + _pLogHandlerHelper = 0; + } + LogListHelper::delete_all(kLogRoot); +} + +//------------------------------------------------------------------- +void +MavlinkLogHandler::_log_request_end(const mavlink_message_t* /*msg*/) +{ + PX4LOG_WARN("MavlinkLogHandler::_log_request_end\n"); + if (_pLogHandlerHelper) { + delete _pLogHandlerHelper; + _pLogHandlerHelper = 0; + } +} + +//------------------------------------------------------------------- +void +MavlinkLogHandler::_log_send_listing() +{ + mavlink_log_entry_t response; + _pLogHandlerHelper->get_entry(_pLogHandlerHelper->next_entry, response.size, response.time_utc); + response.id = _pLogHandlerHelper->next_entry; + response.num_logs = _pLogHandlerHelper->log_count; + response.last_log_num = _pLogHandlerHelper->last_entry; + _mavlink->send_message(MAVLINK_MSG_ID_LOG_ENTRY, &response); + //-- If we're done listing, flag it. + if (_pLogHandlerHelper->next_entry == _pLogHandlerHelper->last_entry) { + _pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE; + } else { + _pLogHandlerHelper->next_entry++; + } + PX4LOG_WARN("MavlinkLogHandler::_log_send_listing id: %u count: %u last: %u size: %u date: %u status: %d\n", + response.id, + response.num_logs, + response.last_log_num, + response.size, + response.time_utc, + _pLogHandlerHelper->current_status); +} + +//------------------------------------------------------------------- +void +MavlinkLogHandler::_log_send_data() +{ + mavlink_log_data_t response; + uint32_t len = _pLogHandlerHelper->current_log_data_remaining; + if (len > sizeof(response.data)) { + len = sizeof(response.data); + } + size_t read_size = _pLogHandlerHelper->get_log_data(len, response.data); + if (read_size < sizeof(response.data)) { + memset(&response.data[read_size], 0, sizeof(response.data) - read_size); + } + response.ofs = _pLogHandlerHelper->current_log_data_offset; + response.id = _pLogHandlerHelper->current_log_index; + response.count = read_size; + _mavlink->send_message(MAVLINK_MSG_ID_LOG_DATA, &response); + _pLogHandlerHelper->current_log_data_offset += read_size; + _pLogHandlerHelper->current_log_data_remaining -= read_size; + if (read_size < sizeof(response.data) || _pLogHandlerHelper->current_log_data_remaining == 0) { + _pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE; + } +} + +//------------------------------------------------------------------- +LogListHelper::LogListHelper() + : next_entry(0) + , last_entry(0) + , log_count(0) + , current_status(LOG_HANDLER_IDLE) + , current_log_index(0) + , current_log_size(0) + , current_log_data_offset(0) + , current_log_data_remaining(0) +{ + _init(); +} + +//------------------------------------------------------------------- +LogListHelper::~LogListHelper() +{ + // Remove log data files (if any) + unlink(kLogData); + unlink(kTmpData); +} + +//------------------------------------------------------------------- +bool +LogListHelper::get_entry(int idx, uint32_t& size, uint32_t& date, char* filename) +{ + //-- Find log file in log list file created during init() + size = 0; + date = 0; + bool result = false; + //-- Open list of log files + FILE* f = ::fopen(kLogData, "r"); + if (f) { + //--- Find requested entry + char line[160]; + int count = 0; + while (fgets(line, sizeof(line), f)) { + //-- Found our "index" + if(count++ == idx) { + char file[128]; + if(sscanf(line, "%u %u %s", &date, &size, file) == 3) { + if(filename) { + strcpy(filename, file); + } + result = true; + break; + } + } + } + fclose(f); + } + return result; +} + +//------------------------------------------------------------------- +size_t +LogListHelper::get_log_data(uint8_t len, uint8_t* buffer) +{ + if(!current_log_filename[0]) + return 0; + FILE* f = fopen(current_log_filename, "r"); + if (!f) { + PX4LOG_WARN("MavlinkLogHandler::get_log_data Could not open %s\n", current_log_filename); + return 0; + } + if(fseek(f, current_log_data_offset, SEEK_SET)) { + fclose(f); + PX4LOG_WARN("MavlinkLogHandler::get_log_data Seek error in %s\n", current_log_filename); + return 0; + } + size_t result = fread(buffer, 1, len, f); + fclose(f); + return result; +} + +//------------------------------------------------------------------- +void +LogListHelper::_init() +{ + /* + + When this helper is created, it scans the log directory + and collects all log files found into one file for easy, + subsequent access. + */ + + current_log_filename[0] = 0; + // Remove old log data file (if any) + unlink(kLogData); + // Open log directory + DIR *dp = opendir(kLogRoot); + if (dp == nullptr) { + // No log directory. Nothing to do. + return; + } + // Create work file + FILE* f = ::fopen(kTmpData, "w"); + if (!f) { + PX4LOG_WARN("MavlinkLogHandler::init Error creating %s\n", kTmpData); + closedir(dp); + return; + } + // Scan directory and collect log files + struct dirent entry, *result = nullptr; + while (readdir_r(dp, &entry, &result) == 0) { + // no more entries? + if (result == nullptr) { + break; + } + if (entry.d_type == PX4LOG_DIRECTORY) + { + time_t tt; + char log_path[128]; + snprintf(log_path, sizeof(log_path), "%s/%s", kLogRoot, entry.d_name); + if (_get_session_date(log_path, entry.d_name, tt)) { + _scan_logs(f, log_path, tt); + } + } + } + closedir(dp); + fclose(f); + // Rename temp file to data file + if (rename(kTmpData, kLogData)) { + PX4LOG_WARN("MavlinkLogHandler::init Error renaming %s\n", kTmpData); + log_count = 0; + } +} + +//------------------------------------------------------------------- +bool +LogListHelper::_get_session_date(const char* path, const char* dir, time_t& date) +{ + if(strlen(dir) > 4) { + // Convert "sess000" to 00:00 Jan 1 1970 (day per session) + if (strncmp(dir, "sess", 4) == 0) { + unsigned u; + if(sscanf(&dir[4], "%u", &u) == 1) { + date = u * 60 * 60 * 24; + return true; + } + } else { + if (stat_file(path, &date)) { + return true; + } + /* strptime not available for some reason + // Get date from directory name + struct tm tt; + if(strptime(dir, "%Y-%m-%d", &tt)) { + date = mktime(&tt); + return true; + } + */ + } + } + return false; +} + +//------------------------------------------------------------------- +void +LogListHelper::_scan_logs(FILE* f, const char* dir, time_t& date) +{ + DIR *dp = opendir(dir); + if (dp) { + struct dirent entry, *result = nullptr; + while (readdir_r(dp, &entry, &result) == 0) { + // no more entries? + if (result == nullptr) { + break; + } + if (entry.d_type == PX4LOG_REGULAR_FILE) { + time_t ldate = date; + uint32_t size = 0; + char log_file_path[128]; + snprintf(log_file_path, sizeof(log_file_path), "%s/%s", dir, entry.d_name); + if(_get_log_time_size(log_file_path, entry.d_name, ldate, size)) { + //-- Write entry out to list file + fprintf(f, "%u %u %s\n", (unsigned)ldate, (unsigned)size, log_file_path); + log_count++; + } + } + } + } +} + +//------------------------------------------------------------------- +bool +LogListHelper::_get_log_time_size(const char* path, const char* file, time_t& date, uint32_t& size) +{ + if(file && file[0] && strstr(file, ".px4log")) { + // Convert "log000" to 00:00 (minute per flight in session) + if (strncmp(file, "log", 3) == 0) { + unsigned u; + if(sscanf(&file[3], "%u", &u) == 1) { + date += (u * 60); + if (stat_file(path, 0, &size)) { + return true; + } + } + } else { + if (stat_file(path, &date, &size)) { + return true; + } + /* strptime not available for some reason + // Get time from file name + struct tm tt; + if(strptime(file, "%H_%M_%S", &tt)) { + date += mktime(&tt); + return true; + } + */ + } + } + return false; +} + +//------------------------------------------------------------------- +void +LogListHelper::delete_all(const char* dir) +{ + //-- Open log directory + DIR *dp = opendir(dir); + if (dp == nullptr) { + return; + } + struct dirent entry, *result = nullptr; + while (readdir_r(dp, &entry, &result) == 0) { + // no more entries? + if (result == nullptr) { + break; + } + if (entry.d_type == PX4LOG_DIRECTORY && entry.d_name[0] != '.') { + char log_path[128]; + snprintf(log_path, sizeof(log_path), "%s/%s", dir, entry.d_name); + LogListHelper::delete_all(log_path); + if(rmdir(log_path)) { + PX4LOG_WARN("MavlinkLogHandler::delete_all Error removing %s\n", log_path); + } + } + if (entry.d_type == PX4LOG_REGULAR_FILE) { + char log_path[128]; + snprintf(log_path, sizeof(log_path), "%s/%s", dir, entry.d_name); + if(unlink(log_path)) { + PX4LOG_WARN("MavlinkLogHandler::delete_all Error deleting %s\n", log_path); + } + } + } + closedir(dp); +} diff --git a/src/modules/mavlink/mavlink_log_handler.h b/src/modules/mavlink/mavlink_log_handler.h new file mode 100644 index 0000000000..ebbbe116a8 --- /dev/null +++ b/src/modules/mavlink/mavlink_log_handler.h @@ -0,0 +1,116 @@ +/**************************************************************************** + * + * Copyright (c) 2014, 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. + * + ****************************************************************************/ + +#pragma once + +/// @file mavlink_log_handler.h +/// @author px4dev, Gus Grubba + +#include +#include +#include +#include +#include +#include "mavlink_stream.h" + +class Mavlink; + +// Log Listing Helper +class LogListHelper +{ +public: + LogListHelper(); + ~LogListHelper(); + +public: + static void delete_all(const char* dir); + +public: + + bool get_entry (int idx, uint32_t& size, uint32_t& date, char* filename = 0); + size_t get_log_data (uint8_t len, uint8_t* buffer); + + enum { + LOG_HANDLER_IDLE, + LOG_HANDLER_LISTING, + LOG_HANDLER_SENDING_DATA + }; + + int next_entry; + int last_entry; + int log_count; + + int current_status; + uint16_t current_log_index; + uint32_t current_log_size; + uint32_t current_log_data_offset; + uint32_t current_log_data_remaining; + char current_log_filename[128]; + +private: + void _init (); + bool _get_session_date (const char* path, const char* dir, time_t& date); + void _scan_logs (FILE* f, const char* dir, time_t& date); + bool _get_log_time_size (const char* path, const char* file, time_t& date, uint32_t& size); +}; + +// MAVLink LOG_* Message Handler +class MavlinkLogHandler : public MavlinkStream +{ +public: + MavlinkLogHandler(Mavlink *mavlink); + + static MavlinkLogHandler *new_instance(Mavlink *mavlink); + + // Handle possible LOG message + void handle_message (const mavlink_message_t *msg); + + // Overrides from MavlinkStream + const char* get_name (void) const; + uint8_t get_id (void); + unsigned get_size (void); + void send (const hrt_abstime t); + +private: + void _log_message (const mavlink_message_t *msg); + void _log_request_list (const mavlink_message_t *msg); + void _log_request_data (const mavlink_message_t *msg); + void _log_request_erase (const mavlink_message_t *msg); + void _log_request_end (const mavlink_message_t *msg); + void _log_send_listing (); + void _log_send_data (); + +private: + LogListHelper *_pLogHandlerHelper; + +}; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 07b768354f..d58fd0c93e 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -51,9 +51,7 @@ #include #include #include -#ifndef __PX4_POSIX #include -#endif #include #include /* isinf / isnan checks */ @@ -77,6 +75,7 @@ #include #include +#include #include "mavlink_bridge_header.h" #include "mavlink_main.h" @@ -146,6 +145,7 @@ Mavlink::Mavlink() : _mission_manager(nullptr), _parameters_manager(nullptr), _mavlink_ftp(nullptr), + _mavlink_log_handler(nullptr), _mode(MAVLINK_MODE_NORMAL), _channel(MAVLINK_COMM_0), _radio_id(0), @@ -155,9 +155,7 @@ Mavlink::Mavlink() : _verbose(false), _forwarding_on(false), _ftp_on(false), -#ifndef __PX4_POSIX _uart_fd(-1), -#endif _baudrate(57600), _datarate(1000), _datarate_events(500), @@ -605,9 +603,16 @@ int Mavlink::get_component_id() return mavlink_system.compid; } -#ifndef __PX4_POSIX int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original) { +#ifndef B460800 + #define B460800 460800 +#endif + +#ifndef B921600 + #define B921600 921600 +#endif + /* process baud rate */ int speed; @@ -660,9 +665,48 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * return -EINVAL; } + /* back off 1800 ms to avoid running into the USB setup timing */ + while (_mode == MAVLINK_MODE_CONFIG && + hrt_absolute_time() < 1800U * 1000U) { + usleep(50000); + } + /* open uart */ _uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY); + /* if this is a config link, stay here and wait for it to open */ + if (_uart_fd < 0 && _mode == MAVLINK_MODE_CONFIG) { + + int armed_fd = orb_subscribe(ORB_ID(actuator_armed)); + struct actuator_armed_s armed; + + /* get the system arming state and abort on arming */ + while (_uart_fd < 0) { + + /* abort if an arming topic is published and system is armed */ + bool updated = false; + orb_check(armed_fd, &updated); + + if (updated) { + /* the system is now providing arming status feedback. + * instead of timing out, we resort to abort bringing + * up the terminal. + */ + orb_copy(ORB_ID(actuator_armed), armed_fd, &armed); + + if (armed.armed) { + /* this is not an error, but we are done */ + return -1; + } + } + + usleep(100000); + _uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY); + }; + + ::close(armed_fd); + } + if (_uart_fd < 0) { return _uart_fd; } @@ -767,8 +811,6 @@ Mavlink::enable_flow_control(bool enabled) return ret; } -#endif - int Mavlink::set_hil_enabled(bool hil_enabled) { @@ -801,31 +843,27 @@ Mavlink::get_free_tx_buf() */ int buf_free = 0; -#ifndef __PX4_POSIX - -// No FIONWRITE on Linux -#if !defined(__PX4_LINUX) && !defined(__PX4_DARWIN) - (void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free); -#endif - - if (get_flow_control_enabled() && buf_free < FLOW_CONTROL_DISABLE_THRESHOLD) { - /* Disable hardware flow control: - * if no successful write since a defined time - * and if the last try was not the last successful write - */ - if (_last_write_try_time != 0 && - hrt_elapsed_time(&_last_write_success_time) > 500 * 1000UL && - _last_write_success_time != _last_write_try_time) { - warnx("Disabling hardware flow control"); - enable_flow_control(false); - } - } - -#endif - // if we are using network sockets, return max lenght of one packet if (get_protocol() == UDP || get_protocol() == TCP ) { return 1500; + } else { + // No FIONWRITE on Linux +#if !defined(__PX4_LINUX) && !defined(__PX4_DARWIN) + (void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free); +#endif + + if (get_flow_control_enabled() && buf_free < FLOW_CONTROL_DISABLE_THRESHOLD) { + /* Disable hardware flow control: + * if no successful write since a defined time + * and if the last try was not the last successful write + */ + if (_last_write_try_time != 0 && + hrt_elapsed_time(&_last_write_success_time) > 500 * 1000UL && + _last_write_success_time != _last_write_try_time) { + warnx("Disabling hardware flow control"); + enable_flow_control(false); + } + } } return buf_free; @@ -883,21 +921,25 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8); size_t ret = -1; -#ifndef __PX4_POSIX + /* send message to UART */ if (get_protocol() == SERIAL) { ret = ::write(_uart_fd, buf, packet_len); } -#else + +#ifdef __PX4_POSIX if (get_protocol() == UDP) { - ret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr)); + if (get_client_source_initialized()) { + ret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr)); + } struct telemetry_status_s &tstatus = get_rx_status(); /* resend heartbeat via broadcast */ - if (((hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000) || + if ((_mode != MAVLINK_MODE_ONBOARD) + && (((hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000) || (tstatus.heartbeat_time == 0)) && - msgid == MAVLINK_MSG_ID_HEARTBEAT) { + msgid == MAVLINK_MSG_ID_HEARTBEAT)) { int bret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_bcast_addr, sizeof(_bcast_addr)); @@ -959,21 +1001,20 @@ Mavlink::resend_message(mavlink_message_t *msg) buf[MAVLINK_NUM_HEADER_BYTES + msg->len] = (uint8_t)(msg->checksum & 0xFF); buf[MAVLINK_NUM_HEADER_BYTES + msg->len + 1] = (uint8_t)(msg->checksum >> 8); -#ifndef __PX4_POSIX - /* send message to UART */ - ssize_t ret = ::write(_uart_fd, buf, packet_len); + if (_uart_fd >= 0) { + /* send message to UART */ + ssize_t ret = ::write(_uart_fd, buf, packet_len); - if (ret != (int) packet_len) { - count_txerr(); - count_txerrbytes(packet_len); + if (ret != (int) packet_len) { + count_txerr(); + count_txerrbytes(packet_len); - } else { - _last_write_success_time = _last_write_try_time; - count_txbytes(packet_len); + } else { + _last_write_success_time = _last_write_try_time; + count_txbytes(packet_len); + } } -#endif - pthread_mutex_unlock(&_send_mutex); } @@ -1004,11 +1045,14 @@ Mavlink::init_udp() return; } - /* set default target address */ + /* set default target address, but not for onboard mode (will be set on first recieved packet) */ memset((char *)&_src_addr, 0, sizeof(_src_addr)); - _src_addr.sin_family = AF_INET; - inet_aton("127.0.0.1", &_src_addr.sin_addr); - _src_addr.sin_port = htons(DEFAULT_REMOTE_PORT_UDP); + if (_mode != MAVLINK_MODE_ONBOARD) { + set_client_source_initialized(); + _src_addr.sin_family = AF_INET; + inet_aton("127.0.0.1", &_src_addr.sin_addr); + _src_addr.sin_port = htons(DEFAULT_REMOTE_PORT_UDP); + } /* default broadcast address */ memset((char *)&_bcast_addr, 0, sizeof(_bcast_addr)); @@ -1031,6 +1075,9 @@ Mavlink::handle_message(const mavlink_message_t *msg) /* handle packet with ftp component */ _mavlink_ftp->handle_message(msg); + /* handle packet with log component */ + _mavlink_log_handler->handle_message(msg); + if (get_forwarding_on()) { /* forward any messages to other mavlink instances */ Mavlink::forward_message(msg, this); @@ -1568,31 +1615,38 @@ Mavlink::task_main(int argc, char *argv[]) _datarate = MAX_DATA_RATE; } - if (Mavlink::instance_exists(_device_name, this)) { - warnx("%s already running", _device_name); - return ERROR; - } + struct termios uart_config_original = {}; if (get_protocol() == SERIAL) { - warnx("mode: %u, data rate: %d B/s on %s @ %dB", _mode, _datarate, _device_name, _baudrate); + if (Mavlink::instance_exists(_device_name, this)) { + warnx("%s already running", _device_name); + return ERROR; + } + + warnx("mode: %u, data rate: %d B/s on %s @ %dB", _mode, _datarate, _device_name, _baudrate); + + /* flush stdout in case MAVLink is about to take it over */ + fflush(stdout); + + /* default values for arguments */ + _uart_fd = mavlink_open_uart(_baudrate, _device_name, &uart_config_original); + + if (_uart_fd < 0 && _mode != MAVLINK_MODE_CONFIG) { + warn("could not open %s", _device_name); + return ERROR; + } else if (_uart_fd < 0 && _mode == MAVLINK_MODE_CONFIG) { + /* the config link is optional */ + return OK; + } + } else if (get_protocol() == UDP) { + if (Mavlink::get_instance_for_network_port(_network_port) != nullptr) { + warnx("port %d already occupied", _network_port); + return ERROR; + } + warnx("mode: %u, data rate: %d B/s on udp port %hu", _mode, _datarate, _network_port); } - /* flush stdout in case MAVLink is about to take it over */ - fflush(stdout); - -#ifndef __PX4_POSIX - struct termios uart_config_original; - - /* default values for arguments */ - _uart_fd = mavlink_open_uart(_baudrate, _device_name, &uart_config_original); - - if (_uart_fd < 0) { - warn("could not open %s", _device_name); - return ERROR; - } - -#endif /* initialize send mutex */ pthread_mutex_init(&_send_mutex, NULL); @@ -1639,9 +1693,13 @@ Mavlink::task_main(int argc, char *argv[]) uint64_t param_time = 0; MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status)); uint64_t status_time = 0; + MavlinkOrbSubscription *ack_sub = add_orb_subscription(ORB_ID(vehicle_command_ack)); + uint64_t ack_time = 0; struct vehicle_status_s status; status_sub->update(&status_time, &status); + struct vehicle_command_ack_s command_ack; + ack_sub->update(&ack_time, &command_ack); /* add default streams depending on mode */ @@ -1664,6 +1722,11 @@ Mavlink::task_main(int argc, char *argv[]) _mavlink_ftp->set_interval(interval_from_rate(80.0f)); LL_APPEND(_streams, _mavlink_ftp); + /* MAVLINK_Log_Handler */ + _mavlink_log_handler = (MavlinkLogHandler *) MavlinkLogHandler::new_instance(this); + _mavlink_log_handler->set_interval(interval_from_rate(80.0f)); + LL_APPEND(_streams, _mavlink_log_handler); + /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on * remote requests rate. Rate specified here controls how much bandwidth we will reserve for * mission messages. */ @@ -1806,7 +1869,7 @@ Mavlink::task_main(int argc, char *argv[]) } /* radio config check */ - if (_radio_id != 0 && _rstatus.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) { + if (_uart_fd >= 0 && _radio_id != 0 && _rstatus.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) { /* request to configure radio and radio is present */ FILE *fs = fdopen(_uart_fd, "w"); @@ -1856,6 +1919,15 @@ Mavlink::task_main(int argc, char *argv[]) set_manual_input_mode_generation(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_GENERATED); } + /* send command ACK */ + if (ack_sub->update(&ack_time, &command_ack)) { + mavlink_command_ack_t msg; + msg.result = command_ack.result; + msg.command = command_ack.command; + + send_message(MAVLINK_MSG_ID_COMMAND_ACK, &msg); + } + /* check for requested subscriptions */ if (_subscribe_to_stream != nullptr) { if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) { @@ -1992,13 +2064,13 @@ Mavlink::task_main(int argc, char *argv[]) /* wait for threads to complete */ pthread_join(_receive_thread, NULL); -#ifndef __PX4_POSIX - /* reset the UART flags to original state */ - tcsetattr(_uart_fd, TCSANOW, &uart_config_original); + if (_uart_fd >= 0) { + /* reset the UART flags to original state */ + tcsetattr(_uart_fd, TCSANOW, &uart_config_original); - /* close UART */ - ::close(_uart_fd); -#endif + /* close UART */ + ::close(_uart_fd); + } /* close mavlink logging device */ px4_close(_mavlink_fd); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 37c66b5963..530eb760b2 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -67,6 +67,7 @@ #include "mavlink_mission.h" #include "mavlink_parameters.h" #include "mavlink_ftp.h" +#include "mavlink_log_handler.h" enum Protocol { SERIAL = 0, @@ -339,9 +340,15 @@ public: int get_socket_fd () { return _socket_fd; }; #ifdef __PX4_POSIX struct sockaddr_in * get_client_source_address() {return &_src_addr;}; + + void set_client_source_initialized() { _src_addr_initialized = true; }; + + bool get_client_source_initialized() { return _src_addr_initialized; }; #endif static bool boot_complete() { return _boot_complete; } + bool is_usb_uart() { return _is_usb_uart; } + protected: Mavlink *next; @@ -369,6 +376,7 @@ private: MavlinkMissionManager *_mission_manager; MavlinkParametersManager *_parameters_manager; MavlinkFTP *_mavlink_ftp; + MavlinkLogHandler *_mavlink_log_handler; MAVLINK_MODE _mode; @@ -421,6 +429,7 @@ private: struct sockaddr_in _myaddr; struct sockaddr_in _src_addr; struct sockaddr_in _bcast_addr; + bool _src_addr_initialized; #endif int _socket_fd; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index ea8d2fae84..a196b6eb8e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -2170,6 +2170,22 @@ protected: msg.rssi = rc.rssi; _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS, &msg); + + /* send override message - harmless if connected to GCS, allows to connect a board to a Linux system */ + /* http://mavlink.org/messages/common#RC_CHANNELS_OVERRIDE */ + mavlink_rc_channels_override_t over; + over.target_system = mavlink_system.sysid; + over.target_component = 0; + over.chan1_raw = msg.chan1_raw; + over.chan2_raw = msg.chan2_raw; + over.chan3_raw = msg.chan3_raw; + over.chan4_raw = msg.chan4_raw; + over.chan5_raw = msg.chan5_raw; + over.chan6_raw = msg.chan6_raw; + over.chan7_raw = msg.chan7_raw; + over.chan8_raw = msg.chan8_raw; + + _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, &over); } } }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 51f08bb01c..231ea21c1d 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1814,9 +1814,12 @@ MavlinkReceiver::receive_thread(void *arg) struct sockaddr_in * srcaddr_last = _mavlink->get_client_source_address(); int localhost = (127 << 24) + 1; - if (srcaddr_last->sin_addr.s_addr == htonl(localhost) && srcaddr.sin_addr.s_addr != htonl(localhost)) { + if ((srcaddr_last->sin_addr.s_addr == htonl(localhost) && srcaddr.sin_addr.s_addr != htonl(localhost)) + || (_mavlink->get_mode() == Mavlink::MAVLINK_MODE_ONBOARD && !_mavlink->get_client_source_initialized())) { // if we were sending to localhost before but have a new host then accept him memcpy(srcaddr_last, &srcaddr, sizeof(srcaddr)); + PX4_WARN("UDP source addr changed: %s", inet_ntoa(srcaddr.sin_addr)); + _mavlink->set_client_source_initialized(); } #endif /* if read failed, this loop won't execute */ diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 477e364a5b..067fa909ae 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -88,6 +88,7 @@ #include #include #include +#include /** * Multicopter attitude control app start / stop handling function @@ -100,6 +101,7 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); #define MIN_TAKEOFF_THRUST 0.2f #define RATES_I_LIMIT 0.3f #define MANUAL_THROTTLE_MAX_MULTICOPTER 0.9f +#define ATTITUDE_TC_DEFAULT 0.2f class MulticopterAttitudeControl { @@ -194,6 +196,10 @@ private: param_t acro_yaw_max; param_t rattitude_thres; + param_t vtol_type; + param_t roll_tc; + param_t pitch_tc; + } _params_handles; /**< handles for interesting parameters */ struct { @@ -211,8 +217,11 @@ private: math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ float rattitude_thres; + int vtol_type; /**< 0 = Tailsitter, 1 = Tiltrotor, 2 = Standard airframe */ } _params; + TailsitterRecovery *_ts_opt_recovery; /**< Computes optimal rates for tailsitter recovery */ + /** * Update our local parameter cache. */ @@ -296,7 +305,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _task_should_exit(false), _control_task(-1), -/* subscriptions */ + /* subscriptions */ _ctrl_state_sub(-1), _v_att_sp_sub(-1), _v_control_mode_sub(-1), @@ -305,7 +314,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _armed_sub(-1), _vehicle_status_sub(-1), -/* publications */ + /* publications */ _v_rates_sp_pub(nullptr), _actuators_0_pub(nullptr), _controller_status_pub(nullptr), @@ -314,9 +323,10 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _actuators_0_circuit_breaker_enabled(false), -/* performance counters */ + /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")), - _controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")) + _controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")), + _ts_opt_recovery(nullptr) { memset(&_ctrl_state, 0, sizeof(_ctrl_state)); @@ -328,7 +338,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : memset(&_armed, 0, sizeof(_armed)); memset(&_vehicle_status, 0, sizeof(_vehicle_status)); memset(&_motor_limits, 0, sizeof(_motor_limits)); - memset(&_controller_status,0,sizeof(_controller_status)); + memset(&_controller_status, 0, sizeof(_controller_status)); _vehicle_status.is_rotary_wing = true; _params.att_p.zero(); @@ -376,9 +386,19 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.acro_pitch_max = param_find("MC_ACRO_P_MAX"); _params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX"); _params_handles.rattitude_thres = param_find("MC_RATT_TH"); + _params_handles.vtol_type = param_find("VT_TYPE"); + _params_handles.roll_tc = param_find("MC_ROLL_TC"); + _params_handles.pitch_tc = param_find("MC_PITCH_TC"); /* fetch initial parameter values */ parameters_update(); + + if (_params.vtol_type == 0) { + // the vehicle is a tailsitter, use optimal recovery control strategy + _ts_opt_recovery = new TailsitterRecovery(); + } + + } MulticopterAttitudeControl::~MulticopterAttitudeControl() @@ -402,6 +422,7 @@ MulticopterAttitudeControl::~MulticopterAttitudeControl() } while (_control_task != -1); } + delete _ts_opt_recovery; mc_att_control::g_control = nullptr; } @@ -410,27 +431,32 @@ MulticopterAttitudeControl::parameters_update() { float v; + float roll_tc, pitch_tc; + + param_get(_params_handles.roll_tc, &roll_tc); + param_get(_params_handles.pitch_tc, &pitch_tc); + /* roll gains */ param_get(_params_handles.roll_p, &v); - _params.att_p(0) = v; + _params.att_p(0) = v * (ATTITUDE_TC_DEFAULT / roll_tc); param_get(_params_handles.roll_rate_p, &v); - _params.rate_p(0) = v; + _params.rate_p(0) = v * (ATTITUDE_TC_DEFAULT / roll_tc); param_get(_params_handles.roll_rate_i, &v); _params.rate_i(0) = v; param_get(_params_handles.roll_rate_d, &v); - _params.rate_d(0) = v; + _params.rate_d(0) = v * (ATTITUDE_TC_DEFAULT / roll_tc); param_get(_params_handles.roll_rate_ff, &v); _params.rate_ff(0) = v; /* pitch gains */ param_get(_params_handles.pitch_p, &v); - _params.att_p(1) = v; + _params.att_p(1) = v * (ATTITUDE_TC_DEFAULT / pitch_tc); param_get(_params_handles.pitch_rate_p, &v); - _params.rate_p(1) = v; + _params.rate_p(1) = v * (ATTITUDE_TC_DEFAULT / pitch_tc); param_get(_params_handles.pitch_rate_i, &v); _params.rate_i(1) = v; param_get(_params_handles.pitch_rate_d, &v); - _params.rate_d(1) = v; + _params.rate_d(1) = v * (ATTITUDE_TC_DEFAULT / pitch_tc); param_get(_params_handles.pitch_rate_ff, &v); _params.rate_ff(1) = v; @@ -467,6 +493,8 @@ MulticopterAttitudeControl::parameters_update() /* stick deflection needed in rattitude mode to control rates not angles */ param_get(_params_handles.rattitude_thres, &_params.rattitude_thres); + param_get(_params_handles.vtol_type, &_params.vtol_type); + _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY); return OK; @@ -558,11 +586,13 @@ MulticopterAttitudeControl::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(mc_virtual_rates_setpoint); _actuators_id = ORB_ID(actuator_controls_virtual_mc); + } else { _rates_sp_id = ORB_ID(vehicle_rates_setpoint); _actuators_id = ORB_ID(actuator_controls_0); @@ -699,12 +729,13 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) /* angular rates error */ math::Vector<3> rates_err = _rates_sp - rates; - _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int + _params.rate_ff.emult(_rates_sp - _rates_sp_prev) / dt; + _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int + + _params.rate_ff.emult(_rates_sp - _rates_sp_prev) / dt; _rates_sp_prev = _rates_sp; _rates_prev = rates; /* update integral only if not saturated on low limit and if motor commands are not saturated */ - if (_thrust_sp > MIN_TAKEOFF_THRUST && !_motor_limits.lower_limit && !_motor_limits.upper_limit ) { + if (_thrust_sp > MIN_TAKEOFF_THRUST && !_motor_limits.lower_limit && !_motor_limits.upper_limit) { for (int i = 0; i < 3; i++) { if (fabsf(_att_control(i)) < _thrust_sp) { float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; @@ -756,8 +787,9 @@ MulticopterAttitudeControl::task_main() int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit */ - if (pret == 0) + if (pret == 0) { continue; + } /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { @@ -795,38 +827,58 @@ MulticopterAttitudeControl::task_main() vehicle_motor_limits_poll(); /* Check if we are in rattitude mode and the pilot is above the threshold on pitch - * or roll (yaw can rotate 360 in normal att control). If both are true don't + * or roll (yaw can rotate 360 in normal att control). If both are true don't * even bother running the attitude controllers */ - if(_vehicle_status.main_state == vehicle_status_s::MAIN_STATE_RATTITUDE){ + if (_vehicle_status.main_state == vehicle_status_s::MAIN_STATE_RATTITUDE) { if (fabsf(_manual_control_sp.y) > _params.rattitude_thres || - fabsf(_manual_control_sp.x) > _params.rattitude_thres){ - _v_control_mode.flag_control_attitude_enabled = false; - } + fabsf(_manual_control_sp.x) > _params.rattitude_thres) { + _v_control_mode.flag_control_attitude_enabled = false; + } } if (_v_control_mode.flag_control_attitude_enabled) { - control_attitude(dt); + if (_ts_opt_recovery == nullptr) { + // the tailsitter recovery instance has not been created, thus, the vehicle + // is not a tailsitter, do normal attitude control + control_attitude(dt); - /* publish attitude rates setpoint */ - _v_rates_sp.roll = _rates_sp(0); - _v_rates_sp.pitch = _rates_sp(1); - _v_rates_sp.yaw = _rates_sp(2); - _v_rates_sp.thrust = _thrust_sp; - _v_rates_sp.timestamp = hrt_absolute_time(); + } else { + vehicle_attitude_setpoint_poll(); + _thrust_sp = _v_att_sp.thrust; + math::Quaternion q(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]); + math::Quaternion q_sp(&_v_att_sp.q_d[0]); + _ts_opt_recovery->setAttGains(_params.att_p, _params.yaw_ff); + _ts_opt_recovery->calcOptimalRates(q, q_sp, _v_att_sp.yaw_sp_move_rate, _rates_sp); - if (_v_rates_sp_pub != nullptr) { - orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); - - } else if (_rates_sp_id) { - _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); + /* limit rates */ + for (int i = 0; i < 3; i++) { + _rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i)); } + } + + /* publish attitude rates setpoint */ + _v_rates_sp.roll = _rates_sp(0); + _v_rates_sp.pitch = _rates_sp(1); + _v_rates_sp.yaw = _rates_sp(2); + _v_rates_sp.thrust = _thrust_sp; + _v_rates_sp.timestamp = hrt_absolute_time(); + + if (_v_rates_sp_pub != nullptr) { + orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); + + } else if (_rates_sp_id) { + _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); + } + //} + } else { /* attitude controller disabled, poll rates setpoint topic */ if (_v_control_mode.flag_control_manual_enabled) { /* manual rates control - ACRO mode */ - _rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max); + _rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, + _manual_control_sp.r).emult(_params.acro_rate_max); _thrust_sp = math::min(_manual_control_sp.z, MANUAL_THROTTLE_MAX_MULTICOPTER); /* publish attitude rates setpoint */ @@ -881,8 +933,9 @@ MulticopterAttitudeControl::task_main() } /* publish controller status */ - if(_controller_status_pub != nullptr) { - orb_publish(ORB_ID(mc_att_ctrl_status),_controller_status_pub, &_controller_status); + if (_controller_status_pub != nullptr) { + orb_publish(ORB_ID(mc_att_ctrl_status), _controller_status_pub, &_controller_status); + } else { _controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status); } @@ -903,11 +956,11 @@ MulticopterAttitudeControl::start() /* start the task */ _control_task = px4_task_spawn_cmd("mc_att_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 1500, - (px4_main_t)&MulticopterAttitudeControl::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 1500, + (px4_main_t)&MulticopterAttitudeControl::task_main_trampoline, + nullptr); if (_control_task < 0) { warn("task start failed"); diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 0c7e4b0134..011a6755ad 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -39,6 +39,28 @@ * @author Anton Babushkin */ +/** + * Roll time constant + * + * Reduce if the system is too twitchy, increase if the response is too slow and sluggish. + * + * @min 0.15 + * @max 0.25 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_ROLL_TC, 0.2f); + +/** + * Pitch time constant + * + * Reduce if the system is too twitchy, increase if the response is too slow and sluggish. + * + * @min 0.15 + * @max 0.25 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_PITCH_TC, 0.2f); + /** * Roll P gain * @@ -271,13 +293,13 @@ PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 360.0f); /** * Threshold for Rattitude mode - * + * * Manual input needed in order to override attitude control rate setpoints * and instead pass manual stick inputs as rate setpoints - * - * @unit + * + * @unit * @min 0.0 * @max 1.0 * @group Multicopter Attitude Control */ - PARAM_DEFINE_FLOAT(MC_RATT_TH, 1.0f); +PARAM_DEFINE_FLOAT(MC_RATT_TH, 1.0f); 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 a49714ff52..dad5173cd5 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -180,6 +180,8 @@ private: param_t xy_ff; param_t tilt_max_air; param_t land_speed; + param_t tko_jmpspd; + param_t tko_speed; param_t tilt_max_land; param_t man_roll_max; param_t man_pitch_max; @@ -197,6 +199,8 @@ private: float thr_max; float tilt_max_air; float land_speed; + float tko_jmpspd; + float tko_speed; float tilt_max_land; float man_roll_max; float man_pitch_max; @@ -241,6 +245,11 @@ private: math::Matrix<3, 3> _R; /**< rotation matrix from attitude quaternions */ float _yaw; /**< yaw angle (euler) */ + float _landing_thrust; + hrt_abstime _landing_started; + bool _takeoff_jumped; + float _vel_z_lp; + float _acc_z_lp; /** * Update our local parameter cache. @@ -370,10 +379,16 @@ MulticopterPositionControl::MulticopterPositionControl() : _alt_hold_engaged(false), _run_pos_control(true), _run_alt_control(true), - _yaw(0.0f) + _yaw(0.0f), + _landing_thrust(1.0f), + _landing_started(0), + _takeoff_jumped(false), + _vel_z_lp(0), + _acc_z_lp(0) { memset(&_vehicle_status, 0, sizeof(_vehicle_status)); memset(&_ctrl_state, 0, sizeof(_ctrl_state)); + _ctrl_state.q[0] = 1.0f; memset(&_att_sp, 0, sizeof(_att_sp)); memset(&_manual, 0, sizeof(_manual)); memset(&_control_mode, 0, sizeof(_control_mode)); @@ -420,6 +435,8 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_handles.xy_ff = param_find("MPC_XY_FF"); _params_handles.tilt_max_air = param_find("MPC_TILTMAX_AIR"); _params_handles.land_speed = param_find("MPC_LAND_SPEED"); + _params_handles.tko_jmpspd = param_find("MPC_TKO_JMPSPD"); + _params_handles.tko_speed = param_find("MPC_TKO_SPEED"); _params_handles.tilt_max_land = param_find("MPC_TILTMAX_LND"); _params_handles.man_roll_max = param_find("MPC_MAN_R_MAX"); _params_handles.man_pitch_max = param_find("MPC_MAN_P_MAX"); @@ -482,6 +499,8 @@ MulticopterPositionControl::parameters_update(bool force) param_get(_params_handles.tilt_max_air, &_params.tilt_max_air); _params.tilt_max_air = math::radians(_params.tilt_max_air); param_get(_params_handles.land_speed, &_params.land_speed); + param_get(_params_handles.tko_jmpspd, &_params.tko_jmpspd); + param_get(_params_handles.tko_speed, &_params.tko_speed); param_get(_params_handles.tilt_max_land, &_params.tilt_max_land); _params.tilt_max_land = math::radians(_params.tilt_max_land); @@ -573,6 +592,13 @@ MulticopterPositionControl::poll_subscriptions() if (updated) { orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); + + /* get current rotation matrix and euler angles from control state quaternions */ + math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]); + _R = q_att.to_dcm(); + math::Vector<3> euler_angles; + euler_angles = _R.to_euler(); + _yaw = euler_angles(2); } orb_check(_att_sp_sub, &updated); @@ -941,10 +967,6 @@ void MulticopterPositionControl::control_auto(float dt) } if (current_setpoint_valid) { - /* in case of interrupted mission don't go to waypoint but stay at current position */ - _reset_pos_sp = true; - _reset_alt_sp = true; - /* scaled space: 1 == position error resulting max allowed speed */ math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max); // TODO add mult param here @@ -1049,6 +1071,24 @@ void MulticopterPositionControl::control_auto(float dt) _att_sp.yaw_body = _pos_sp_triplet.current.yaw; } + /* + * if we're already near the current takeoff setpoint don't reset in case we switch back to posctl. + * this makes the takeoff finish smoothly. + */ + if ((_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF + || _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) + && _pos_sp_triplet.current.acceptance_radius > 0.0f + /* need to detect we're close a bit before the navigator switches from takeoff to next waypoint */ + && (_pos - _pos_sp).length() < _pos_sp_triplet.current.acceptance_radius * 1.2f) { + _reset_pos_sp = false; + _reset_alt_sp = false; + + /* otherwise: in case of interrupted mission don't go to waypoint but stay at current position */ + } else { + _reset_pos_sp = true; + _reset_alt_sp = true; + } + } else { /* no waypoint, do nothing, setpoint was already reset */ } @@ -1120,13 +1160,6 @@ MulticopterPositionControl::task_main() poll_subscriptions(); - /* get current rotation matrix and euler angles from control state quaternions */ - math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]); - _R = q_att.to_dcm(); - math::Vector<3> euler_angles; - euler_angles = _R.to_euler(); - _yaw = euler_angles(2); - parameters_update(false); hrt_abstime t = hrt_absolute_time(); @@ -1247,6 +1280,33 @@ MulticopterPositionControl::task_main() _att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp); } + } else if (_control_mode.flag_control_manual_enabled + && _vehicle_status.condition_landed) { + /* don't run controller when landed */ + _reset_pos_sp = true; + _reset_alt_sp = true; + _mode_auto = false; + reset_int_z = true; + reset_int_xy = true; + + R.identity(); + memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); + _att_sp.R_valid = true; + + _att_sp.roll_body = 0.0f; + _att_sp.pitch_body = 0.0f; + _att_sp.yaw_body = _yaw; + _att_sp.thrust = 0.0f; + + _att_sp.timestamp = hrt_absolute_time(); + + /* publish attitude setpoint */ + 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); + } + } else { /* run position & altitude controllers, if enabled (otherwise use already computed velocity setpoints) */ if (_run_pos_control) { @@ -1300,6 +1360,31 @@ MulticopterPositionControl::task_main() _vel_sp(2) = _params.land_speed; } + /* velocity handling during takeoff */ + if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid + && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { + + if (!_takeoff_jumped) { + /* do a quick jump (ramp vel sp up in 1/2sec) until we shoot over takeoff speed */ + float accel = _vel_sp_prev(2) - _params.tko_jmpspd; + float tmp = _vel_sp_prev(2) + accel * dt * 2.0f; + _vel_sp(2) = math::max(tmp, -_params.tko_jmpspd); + + if (_vel(2) < -_params.tko_speed) { + _takeoff_jumped = true; + } + + } else { + /* slowly reduce forced takeoff speed to set climb rate (in 3 sec) */ + float decel = _params.tko_jmpspd - _params.tko_speed; + float tmp = decel / 3.0f * dt + _vel_sp_prev(2); + _vel_sp(2) = math::min(tmp, -_params.tko_speed); + } + + } else { + _takeoff_jumped = false; + } + // limit total horizontal acceleration math::Vector<2> acc_hor; acc_hor(0) = (_vel_sp(0) - _vel_sp_prev(0)) / dt; @@ -1392,7 +1477,14 @@ MulticopterPositionControl::task_main() thr_min = 0.0f; } + float thrust_abs = thrust_sp.length(); float tilt_max = _params.tilt_max_air; + float thr_max = _params.thr_max; + /* filter vel_z over 1/8sec */ + _vel_z_lp = _vel_z_lp * (1.0f - dt * 8.0f) + dt * 8.0f * _vel(2); + /* filter vel_z change over 1/8sec */ + float vel_z_change = (_vel(2) - _vel_prev(2)) / dt; + _acc_z_lp = _acc_z_lp * (1.0f - dt * 8.0f) + dt * 8.0f * vel_z_change; /* adjust limits for landing mode */ if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && @@ -1403,6 +1495,45 @@ MulticopterPositionControl::task_main() if (thr_min < 0.0f) { thr_min = 0.0f; } + + if (_landing_started == 0) { + _landing_started = hrt_absolute_time(); + } + + /* don't let it throttle up again during landing */ + if (thrust_sp(2) < 0.0f && thrust_abs < _landing_thrust + /* fix landing thrust after a certain time when velocity change is minimal */ + && (float)fabs(_acc_z_lp) < 0.1f + && _vel_z_lp > 0.5f * _params.land_speed + && hrt_elapsed_time(&_landing_started) > 15e5) { + _landing_thrust = thrust_abs; + } + + /* assume ground, reduce thrust */ + if (hrt_elapsed_time(&_landing_started) > 15e5 + && _landing_thrust > FLT_EPSILON + && _vel_z_lp < 0.1f + ) { + _landing_thrust = 0.0f; + } + + /* if we suddenly fall, reset landing logic and remove thrust limit */ + if (hrt_elapsed_time(&_landing_started) > 15e5 + && _landing_thrust < FLT_EPSILON + /* XXX: magic value, assuming free fall above 4m/s2 acceleration */ + && (_acc_z_lp > 4.0f + || _vel_z_lp > 2.0f * _params.land_speed) + ) { + _landing_thrust = _params.thr_max; + _landing_started = 0; + } + + /* add 5% to give it some margin to compensate external influences */ + thr_max = _landing_thrust + 0.05f * _landing_thrust; + + } else { + _landing_started = 0; + _landing_thrust = _params.thr_max; } /* limit min lift */ @@ -1452,21 +1583,21 @@ MulticopterPositionControl::task_main() } /* limit max thrust */ - float thrust_abs = thrust_sp.length(); + thrust_abs = thrust_sp.length(); /* recalculate because it might have changed */ - if (thrust_abs > _params.thr_max) { + if (thrust_abs > thr_max) { if (thrust_sp(2) < 0.0f) { - if (-thrust_sp(2) > _params.thr_max) { + if (-thrust_sp(2) > thr_max) { /* thrust Z component is too large, limit it */ thrust_sp(0) = 0.0f; thrust_sp(1) = 0.0f; - thrust_sp(2) = -_params.thr_max; + thrust_sp(2) = -thr_max; saturation_xy = true; saturation_z = true; } else { /* preserve thrust Z component and lower XY, keeping altitude is more important than position */ - float thrust_xy_max = sqrtf(_params.thr_max * _params.thr_max - thrust_sp(2) * thrust_sp(2)); + float thrust_xy_max = sqrtf(thr_max * thr_max - thrust_sp(2) * thrust_sp(2)); float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); float k = thrust_xy_max / thrust_xy_abs; thrust_sp(0) *= k; @@ -1476,13 +1607,13 @@ MulticopterPositionControl::task_main() } else { /* Z component is negative, going down, simply limit thrust vector */ - float k = _params.thr_max / thrust_abs; + float k = thr_max / thrust_abs; thrust_sp *= k; saturation_xy = true; saturation_z = true; } - thrust_abs = _params.thr_max; + thrust_abs = thr_max; } /* update integrals */ @@ -1634,8 +1765,8 @@ MulticopterPositionControl::task_main() _att_sp.yaw_body = _yaw; } - /* do not move yaw while arming */ - else if (_manual.z > 0.1f) { + /* do not move yaw while sitting on the ground */ + else if (!_vehicle_status.condition_landed) { 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; 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 65673a8284..3e00c5f58c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -223,7 +223,7 @@ PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR, 45.0f); * @max 90.0 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 15.0f); +PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 12.0f); /** * Landing descend rate @@ -232,7 +232,25 @@ PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 15.0f); * @min 0.0 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 1.0f); +PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 0.5f); + +/** + * Takeoff initial speed to jump off the ground + * + * @unit m/s + * @min 0.0 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_TKO_JMPSPD, 2.0f); + +/** + * Takeoff climb rate + * + * @unit m/s + * @min 0.0 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_TKO_SPEED, 1.5f); /** * Max manual roll diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index 6a5c4c15d9..0ce9399a27 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -45,6 +45,7 @@ px4_add_module( loiter.cpp rtl.cpp takeoff.cpp + land.cpp mission_feasibility_checker.cpp geofence.cpp datalinkloss.cpp diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp new file mode 100644 index 0000000000..2f9ae7e57c --- /dev/null +++ b/src/modules/navigator/land.cpp @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * 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 land.cpp + * + * Helper class to land at the current position + * + * @author Andreas Antener + */ + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include "land.h" +#include "navigator.h" + +Land::Land(Navigator *navigator, const char *name) : + MissionBlock(navigator, name) +{ + /* load initial params */ + updateParams(); +} + +Land::~Land() +{ +} + +void +Land::on_inactive() +{ +} + +void +Land::on_activation() +{ + /* set current mission item to Land */ + set_land_item(&_mission_item, true); + _navigator->get_mission_result()->reached = false; + _navigator->get_mission_result()->finished = false; + _navigator->set_mission_result_updated(); + reset_mission_item_reached(); + + /* 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->next.valid = false; + + _navigator->set_can_loiter_at_sp(false); + + _navigator->set_position_setpoint_triplet_updated(); +} + +void +Land::on_active() +{ + if (is_mission_item_reached() && !_navigator->get_mission_result()->finished) { + _navigator->get_mission_result()->finished = true; + _navigator->set_mission_result_updated(); + set_idle_item(&_mission_item); + + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + _navigator->set_position_setpoint_triplet_updated(); + } +} diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/modules/navigator/land.h similarity index 63% rename from src/lib/ecl/attitude_fw/ecl_roll_controller.h rename to src/modules/navigator/land.h index 6d07bab8ab..c97feb7046 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h +++ b/src/modules/navigator/land.h @@ -1,6 +1,6 @@ -/**************************************************************************** +/*************************************************************************** * - * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * @@ -30,40 +30,36 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ - /** - * @file ecl_roll_controller.h - * Definition of a simple orthogonal roll PID controller. + * @file land.h * - * @author Lorenz Meier - * @author Thomas Gubler + * Helper class to land at the current position * - * 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. + * @author Andreas Antener */ -#ifndef ECL_ROLL_CONTROLLER_H -#define ECL_ROLL_CONTROLLER_H +#ifndef NAVIGATOR_LAND_H +#define NAVIGATOR_LAND_H -#include -#include +#include +#include -#include "ecl_controller.h" +#include "navigator_mode.h" +#include "mission_block.h" -class __EXPORT ECL_RollController : - public ECL_Controller +class Land : public MissionBlock { public: - ECL_RollController(); + Land(Navigator *navigator, const char *name); - ~ECL_RollController(); + ~Land(); + + virtual void on_inactive(); + + virtual void on_activation(); + + virtual void on_active(); - float control_attitude(const struct ECL_ControlData &ctl_data); - float control_bodyrate(const struct ECL_ControlData &ctl_data); }; -#endif // ECL_ROLL_CONTROLLER_H +#endif diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 299b69b3d0..992a426bfe 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -189,8 +189,9 @@ Mission::on_active() } /* see if we need to update the current yaw heading for rotary wing types */ - if (_navigator->get_vstatus()->is_rotary_wing + if (_navigator->get_vstatus()->is_rotary_wing && _param_yawmode.get() != MISSION_YAWMODE_NONE + && _param_yawmode.get() < MISSION_YAWMODE_MAX && _mission_type != MISSION_TYPE_NONE) { heading_sp_update(); } @@ -285,6 +286,7 @@ Mission::advance_mission() { if (_takeoff) { _takeoff = false; + _takeoff_finished = true; } else { switch (_mission_type) { @@ -429,6 +431,8 @@ Mission::set_mission_items() /* do takeoff before going to setpoint */ /* set mission item as next position setpoint */ mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->next); + /* next SP is not takeoff anymore */ + pos_sp_triplet->next.type = position_setpoint_s::SETPOINT_TYPE_POSITION; /* calculate takeoff altitude */ float takeoff_alt = get_absolute_altitude_for_item(_mission_item); @@ -465,6 +469,13 @@ Mission::set_mission_items() } } + if (_takeoff_finished) { + /* we just finished takeoff */ + /* in case we still have to move to the takeoff waypoint we need a waypoint mission item */ + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _takeoff_finished = false; + } + /* set current position setpoint from mission item */ mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); @@ -525,36 +536,52 @@ Mission::heading_sp_update() return; } - /* Don't do FOH for landing and takeoff waypoints, the ground may be near - * and the FW controller has a custom landing logic */ - if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) { + /* Don't change heading for takeoff waypoints, the ground may be near */ + if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { return; } /* set yaw angle for the waypoint iff a loiter time has been specified */ if (_waypoint_position_reached && _mission_item.time_inside > 0.0f) { _mission_item.yaw = _on_arrival_yaw; - /* always keep the front of the rotary wing pointing to the next waypoint */ - } else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_WAYPOINT) { - _mission_item.yaw = get_bearing_to_next_waypoint( - _navigator->get_global_position()->lat, - _navigator->get_global_position()->lon, - _mission_item.lat, - _mission_item.lon); - /* always keep the back of the rotary wing pointing towards home */ - } else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_HOME) { - _mission_item.yaw = get_bearing_to_next_waypoint( - _navigator->get_global_position()->lat, - _navigator->get_global_position()->lon, - _navigator->get_home_position()->lat, - _navigator->get_home_position()->lon); - /* always keep the back of the rotary wing pointing towards home */ - } else if (_param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) { - _mission_item.yaw = _wrap_pi(get_bearing_to_next_waypoint( - _navigator->get_global_position()->lat, - _navigator->get_global_position()->lon, - _navigator->get_home_position()->lat, - _navigator->get_home_position()->lon) + M_PI_F); + } else { + + /* calculate direction the vehicle should point to: + * normal waypoint: current position to {waypoint or home or home + 180deg} + * landing waypoint: last waypoint to {waypoint or home or home + 180deg} + * For landing the last waypoint (= constant) is used to avoid excessive yawing near the ground + */ + double point_from_latlon[2]; + if (_mission_item.nav_cmd == NAV_CMD_LAND) { + point_from_latlon[0] = pos_sp_triplet->previous.lat; + point_from_latlon[1] = pos_sp_triplet->previous.lon; + } else { + point_from_latlon[0] = _navigator->get_global_position()->lat; + point_from_latlon[1] = _navigator->get_global_position()->lon; + } + + /* always keep the front of the rotary wing pointing to the next waypoint */ + if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_WAYPOINT) { + _mission_item.yaw = get_bearing_to_next_waypoint( + point_from_latlon[0], + point_from_latlon[1], + _mission_item.lat, + _mission_item.lon); + /* always keep the back of the rotary wing pointing towards home */ + } else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_HOME) { + _mission_item.yaw = get_bearing_to_next_waypoint( + point_from_latlon[0], + point_from_latlon[1], + _navigator->get_home_position()->lat, + _navigator->get_home_position()->lon); + /* always keep the back of the rotary wing pointing towards home */ + } else if (_param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) { + _mission_item.yaw = _wrap_pi(get_bearing_to_next_waypoint( + point_from_latlon[0], + point_from_latlon[1], + _navigator->get_home_position()->lat, + _navigator->get_home_position()->lon) + M_PI_F); + } } mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index b0117cf1fe..d54d91244b 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -88,7 +88,8 @@ public: MISSION_YAWMODE_NONE = 0, MISSION_YAWMODE_FRONT_TO_WAYPOINT = 1, MISSION_YAWMODE_FRONT_TO_HOME = 2, - MISSION_YAWMODE_BACK_TO_HOME = 3 + MISSION_YAWMODE_BACK_TO_HOME = 3, + MISSION_YAWMODE_MAX = 4 }; private: @@ -184,6 +185,7 @@ private: int _current_offboard_mission_index; bool _need_takeoff; /**< if true, then takeoff must be performed before going to the first waypoint (if needed) */ bool _takeoff; /**< takeoff state flag */ + bool _takeoff_finished; /**< set if takeoff was requested before and is now done */ enum { MISSION_TYPE_NONE, diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index e2f454d4cf..897173d0d9 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -119,8 +119,16 @@ MissionBlock::is_mission_item_reached() if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator->get_vstatus()->is_rotary_wing) { /* require only altitude for takeoff for multicopter */ + + /* _mission_item.acceptance_radius is not always set */ + float mission_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 (_navigator->get_global_position()->alt > - altitude_amsl - _navigator->get_acceptance_radius()) { + altitude_amsl - mission_acceptance_radius) { _waypoint_position_reached = true; } } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { @@ -214,6 +222,7 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite _navigator->get_loiter_radius(); sp->loiter_direction = item->loiter_direction; sp->pitch_min = item->pitch_min; + sp->acceptance_radius = item->acceptance_radius; switch (item->nav_cmd) { case NAV_CMD_DO_SET_SERVO: @@ -301,7 +310,7 @@ MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance) void MissionBlock::set_takeoff_item(struct mission_item_s *item, float min_clearance, float min_pitch) { - item->nav_cmd = NAV_CMD_LOITER_UNLIMITED; + item->nav_cmd = NAV_CMD_TAKEOFF; /* use current position and use return altitude as clearance */ item->lat = _navigator->get_global_position()->lat; @@ -323,3 +332,51 @@ MissionBlock::set_takeoff_item(struct mission_item_s *item, float min_clearance, item->autocontinue = false; item->origin = ORIGIN_ONBOARD; } + +void +MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_location) +{ + item->nav_cmd = NAV_CMD_LAND; + + /* use current position */ + if (at_current_location) { + item->lat = _navigator->get_global_position()->lat; + item->lon = _navigator->get_global_position()->lon; + + /* use home position */ + } else { + item->lat = _navigator->get_home_position()->lat; + item->lon = _navigator->get_home_position()->lon; + } + + item->altitude = 0; + 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(); + item->time_inside = 0.0f; + item->pitch_min = 0.0f; + item->autocontinue = true; + item->origin = ORIGIN_ONBOARD; +} + +void +MissionBlock::set_idle_item(struct mission_item_s *item) +{ + item->nav_cmd = NAV_CMD_IDLE; + item->lat = _navigator->get_home_position()->lat; + item->lon = _navigator->get_home_position()->lon; + item->altitude_is_relative = false; + item->altitude = _navigator->get_home_position()->alt; + item->yaw = NAN; + item->loiter_radius = _navigator->get_loiter_radius(); + item->loiter_direction = 1; + item->acceptance_radius = _navigator->get_acceptance_radius(); + item->time_inside = 0.0f; + item->pitch_min = 0.0f; + item->autocontinue = true; + item->origin = ORIGIN_ONBOARD; +} + + diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 5490b4c6b8..36a1ecaba1 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -101,6 +101,16 @@ protected: */ void set_takeoff_item(struct mission_item_s *item, float min_clearance = -1.0f, float min_pitch = 0.0f); + /** + * Set a land mission item + */ + void set_land_item(struct mission_item_s *item, bool at_current_location); + + /** + * Set idle mission item + */ + void set_idle_item(struct mission_item_s *item); + mission_item_s _mission_item; bool _waypoint_position_reached; bool _waypoint_yaw_reached; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index b154d21769..514705a68a 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -62,6 +62,7 @@ #include "mission.h" #include "loiter.h" #include "takeoff.h" +#include "land.h" #include "rtl.h" #include "datalinkloss.h" #include "enginefailure.h" @@ -72,7 +73,7 @@ /** * Number of navigation modes that need on_active/on_inactive calls */ -#define NAVIGATOR_MODE_ARRAY_SIZE 8 +#define NAVIGATOR_MODE_ARRAY_SIZE 9 class Navigator : public control::SuperBlock { @@ -226,6 +227,7 @@ private: Mission _mission; /**< class that handles the missions */ Loiter _loiter; /**< class that handles loiter */ Takeoff _takeoff; /**< class for handling takeoff commands */ + Land _land; /**< class for handling land commands */ RTL _rtl; /**< class that handles RTL */ RCLoss _rcLoss; /**< class that handles RTL according to OBC rules (rc loss mode) */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index cd6021a971..b16f9b9b91 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -142,6 +142,7 @@ Navigator::Navigator() : _mission(this, "MIS"), _loiter(this, "LOI"), _takeoff(this, "TKF"), + _land(this, "LND"), _rtl(this, "RTL"), _rcLoss(this, "RCL"), _dataLinkLoss(this, "DLL"), @@ -161,6 +162,7 @@ Navigator::Navigator() : _navigation_mode_array[5] = &_gpsFailure; _navigation_mode_array[6] = &_rcLoss; _navigation_mode_array[7] = &_takeoff; + _navigation_mode_array[8] = &_land; updateParams(); } @@ -275,9 +277,8 @@ Navigator::task_main() _geofence.loadFromFile(GEOFENCE_FILENAME); } else { - mavlink_log_info(_mavlink_fd, "No geofence set"); if (_geofence.clearDm() != OK) { - warnx("Could not clear geofence"); + mavlink_log_critical(_mavlink_fd, "failed clearing geofence"); } } @@ -445,7 +446,6 @@ Navigator::task_main() case vehicle_status_s::NAVIGATION_STATE_ACRO: case vehicle_status_s::NAVIGATION_STATE_ALTCTL: case vehicle_status_s::NAVIGATION_STATE_POSCTL: - case vehicle_status_s::NAVIGATION_STATE_LAND: case vehicle_status_s::NAVIGATION_STATE_TERMINATION: case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: _navigation_mode = nullptr; @@ -482,6 +482,10 @@ Navigator::task_main() _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_takeoff; break; + case vehicle_status_s::NAVIGATION_STATE_LAND: + _pos_sp_triplet_published_invalid_once = false; + _navigation_mode = &_land; + break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: /* Use complex data link loss mode only when enabled via param * otherwise use rtl */ diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index a77f5895e1..0fc377874d 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -243,39 +243,14 @@ RTL::set_rtl_item() } case RTL_STATE_LAND: { - _mission_item.lat = _navigator->get_home_position()->lat; - _mission_item.lon = _navigator->get_home_position()->lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _navigator->get_home_position()->alt; - _mission_item.yaw = _navigator->get_home_position()->yaw; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_LAND; - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; + set_land_item(&_mission_item, false); mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: land at home"); break; } case RTL_STATE_LANDED: { - _mission_item.lat = _navigator->get_home_position()->lat; - _mission_item.lon = _navigator->get_home_position()->lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _navigator->get_home_position()->alt; - // Do not change / control yaw in landed - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_IDLE; - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; + set_idle_item(&_mission_item); mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: completed, landed"); break; diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp index f669429449..2effa1b042 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -75,15 +75,20 @@ Takeoff::on_activation() { /* set current mission item to Takeoff */ set_takeoff_item(&_mission_item, _param_min_alt.get()); + _navigator->get_mission_result()->reached = false; + _navigator->get_mission_result()->finished = false; + _navigator->set_mission_result_updated(); + reset_mission_item_reached(); /* 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->current.yaw = _navigator->get_home_position()->yaw; + pos_sp_triplet->current.yaw_valid = true; 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_can_loiter_at_sp(true); _navigator->set_position_setpoint_triplet_updated(); } @@ -91,4 +96,14 @@ Takeoff::on_activation() void Takeoff::on_active() { + if (is_mission_item_reached() && !_navigator->get_mission_result()->finished) { + _navigator->get_mission_result()->finished = true; + _navigator->set_mission_result_updated(); + + /* set loiter item so position controllers stop doing takeoff logic */ + set_loiter_item(&_mission_item); + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + _navigator->set_position_setpoint_triplet_updated(); + } } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp index 1da3ff305c..3244392829 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -63,7 +63,6 @@ #include #include #include -#include #include #include #include @@ -273,7 +272,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) const hrt_abstime ref_init_delay = 1000000; // wait for 1s after 3D fix struct map_projection_reference_s ref; memset(&ref, 0, sizeof(ref)); - hrt_abstime home_timestamp = 0; uint16_t accel_updates = 0; uint16_t baro_updates = 0; @@ -346,8 +344,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memset(&sensor, 0, sizeof(sensor)); struct vehicle_gps_position_s gps; memset(&gps, 0, sizeof(gps)); - struct home_position_s home; - memset(&home, 0, sizeof(home)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_local_position_s local_pos; @@ -373,7 +369,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); int vision_position_estimate_sub = orb_subscribe(ORB_ID(vision_position_estimate)); int att_pos_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap)); - int home_position_sub = orb_subscribe(ORB_ID(home_position)); int distance_sensor_sub = orb_subscribe(ORB_ID(distance_sensor)); /* advertise */ @@ -677,46 +672,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) flow_updates++; } - /* home position */ - orb_check(home_position_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(home_position), home_position_sub, &home); - - if (home.timestamp != home_timestamp) { - home_timestamp = home.timestamp; - - double est_lat, est_lon; - float est_alt; - - if (ref_inited) { - /* calculate current estimated position in global frame */ - est_alt = local_pos.ref_alt - local_pos.z; - map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon); - } - - /* update reference */ - map_projection_init(&ref, home.lat, home.lon); - - /* update baro offset */ - baro_offset += home.alt - local_pos.ref_alt; - - local_pos.ref_lat = home.lat; - local_pos.ref_lon = home.lon; - local_pos.ref_alt = home.alt; - local_pos.ref_timestamp = home.timestamp; - - if (ref_inited) { - /* reproject position estimate with new reference */ - map_projection_project(&ref, est_lat, est_lon, &x_est[0], &y_est[0]); - z_est[0] = -(est_alt - local_pos.ref_alt); - } - - ref_inited = true; - } - } - - /* check no vision circuit breaker is set */ if (params.no_vision != CBRK_NO_VISION_KEY) { /* vehicle vision position */ diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index f7321a966d..b857aa41fa 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -74,15 +74,15 @@ static unsigned _rssi_adc_counts = 0; bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated) { perf_begin(c_gather_dsm); - 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, PX4IO_RC_INPUT_CHANNELS); + bool dsm_11_bit; + *dsm_updated = dsm_input(_dsm_fd, r_raw_rc_values, &r_raw_rc_count, &dsm_11_bit, &n_bytes, &bytes, + PX4IO_RC_INPUT_CHANNELS); if (*dsm_updated) { - r_raw_rc_count = temp_count & 0x7fff; - if (temp_count & 0x8000) { + if (dsm_11_bit) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11; } else { diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 2197b7b776..3f35c471a5 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -170,7 +170,6 @@ extern pwm_limit_t pwm_limit; #ifdef CONFIG_ARCH_BOARD_PX4IO_V2 # define PX4IO_RELAY_CHANNELS 0 -# define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s)) # define ENABLE_SBUS_OUT(_s) stm32_gpiowrite(GPIO_SBUS_OENABLE, !(_s)) # define VDD_SERVO_FAULT (!stm32_gpioread(GPIO_SERVO_FAULT_DETECT)) diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 182be7916b..5da68d002a 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -230,6 +230,7 @@ private: _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")), + _perf_sim_interval(perf_alloc(PC_INTERVAL, "sim_network_interval")), _accel_pub(nullptr), _baro_pub(nullptr), _gyro_pub(nullptr), @@ -270,6 +271,7 @@ private: perf_counter_t _perf_gps; perf_counter_t _perf_airspeed; perf_counter_t _perf_sim_delay; + perf_counter_t _perf_sim_interval; // uORB publisher handlers orb_advert_t _accel_pub; diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 507f220a43..2c807fca05 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -248,6 +248,7 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish) uint64_t timestamp = ts.tv_sec * 1000 * 1000 + ts.tv_nsec / 1000; perf_set(_perf_sim_delay, timestamp - sim_timestamp); + perf_count(_perf_sim_interval); if (publish) { publish_sensor_topics(&imu); @@ -426,6 +427,13 @@ void Simulator::initializeSensorData() void Simulator::pollForMAVLinkMessages(bool publish) { + // set the threads name +#ifdef __PX4_DARWIN + pthread_setname_np("sim_rcv"); +#else + pthread_setname_np(pthread_self(), "sim_rcv"); +#endif + // udp socket data struct sockaddr_in _myaddr; const int _port = UDP_PORT; @@ -504,14 +512,13 @@ void Simulator::pollForMAVLinkMessages(bool publish) pret = ::poll(&fds[0], fd_count, 100); } - PX4_INFO("Found initial message, pret = %d", pret); _initialized = true; // reset system time (void)hrt_reset(); if (fds[0].revents & POLLIN) { len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen); - PX4_INFO("Sending initial controls message to jMAVSim."); + PX4_INFO("Sending initial controls message to simulator"); send_controls(); } @@ -523,13 +530,8 @@ 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 + mavlink_status_t udp_status = {}; + mavlink_status_t serial_status = {}; // wait for new mavlink messages to arrive while (true) { @@ -556,10 +558,9 @@ void Simulator::pollForMAVLinkMessages(bool publish) if (len > 0) { mavlink_message_t msg; - mavlink_status_t status; for (int i = 0; i < len; i++) { - if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &status)) { + if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &udp_status)) { // have a message, handle it handle_message(&msg, publish); } @@ -573,10 +574,9 @@ void Simulator::pollForMAVLinkMessages(bool publish) if (len > 0) { mavlink_message_t msg; - mavlink_status_t status; for (int i = 0; i < len; ++i) { - if (mavlink_parse_char(MAVLINK_COMM_0, serial_buf[i], &msg, &status)) { + if (mavlink_parse_char(MAVLINK_COMM_1, serial_buf[i], &msg, &serial_status)) { // have a message, handle it handle_message(&msg, true); } diff --git a/src/modules/systemlib/rc_check.h b/src/modules/systemlib/rc_check.h index 239c8a6a13..bf869bda33 100644 --- a/src/modules/systemlib/rc_check.h +++ b/src/modules/systemlib/rc_check.h @@ -36,6 +36,7 @@ * * RC calibration check */ +#include #pragma once diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 1b8fb18735..15dab60df4 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -270,3 +270,6 @@ ORB_DEFINE(distance_sensor, struct distance_sensor_s); #include "topics/camera_trigger.h" ORB_DEFINE(camera_trigger, struct camera_trigger_s); + +#include "topics/vehicle_command_ack.h" +ORB_DEFINE(vehicle_command_ack, struct vehicle_command_ack_s); diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index 6d6f084d24..8d8572b3be 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -96,6 +96,46 @@ orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *da return uORB::Manager::get_instance()->orb_advertise_multi(meta, data, instance, priority); } +/** + * Advertise as the publisher of a topic. + * + * This performs the initial advertisement of a topic; it creates the topic + * node in /obj if required and publishes the initial data. + * + * Any number of advertisers may publish to a topic; publications are atomic + * but co-ordination between publishers is not provided by the ORB. + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @param data A pointer to the initial data to be published. + * For topics updated by interrupt handlers, the advertisement + * must be performed from non-interrupt context. + * @param instance Pointer to an integer which will yield the instance ID (0-based) + * of the publication. + * @param priority The priority of the instance. If a subscriber subscribes multiple + * instances, the priority allows the subscriber to prioritize the best + * data source as long as its available. + * @return ERROR on error, zero on success. + * If the topic in question is not known (due to an + * ORB_DEFINE with no corresponding ORB_DECLARE) + * this function will return -1 and set errno to ENOENT. + */ +int orb_publish_auto(const struct orb_metadata *meta, orb_advert_t *handle, const void *data, int *instance, + int priority) +{ + if (*handle == nullptr) { + *handle = orb_advertise_multi(meta, data, instance, priority); + + if (*handle != nullptr) { + return 0; + } + + } else { + return orb_publish(meta, *handle, data); + } + + return -1; +} /** * Publish new data to a topic. diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index cfcf39b408..afab3cd813 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -181,6 +181,29 @@ extern orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *d extern orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority) __EXPORT; +/** + * Advertise and publish as the publisher of a topic. + * + * This performs the initial advertisement of a topic; it creates the topic + * node in /obj if required and publishes the initial data. + * + * Any number of advertisers may publish to a topic; publications are atomic + * but co-ordination between publishers is not provided by the ORB. + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @param data A pointer to the initial data to be published. + * For topics updated by interrupt handlers, the advertisement + * must be performed from non-interrupt context. + * @param instance Pointer to an integer which will yield the instance ID (0-based, + * limited by ORB_MULTI_MAX_INSTANCES) of the publication. + * @param priority The priority of the instance. If a subscriber subscribes multiple + * instances, the priority allows the subscriber to prioritize the best + * data source as long as its available. + * @return zero on success, error number on failure + */ +extern int orb_publish_auto(const struct orb_metadata *meta, orb_advert_t *handle, const void *data, int *instance, + int priority); /** * Publish new data to a topic. diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index c5a015eb4d..c0d34c0384 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -282,6 +282,7 @@ void Standard::update_external_state() void Standard::fill_actuator_outputs() { /* multirotor controls */ + _actuators_out_0->timestamp = _actuators_mc_in->timestamp; _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight; // roll _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = @@ -292,6 +293,7 @@ void Standard::fill_actuator_outputs() _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight; // throttle /* fixed wing controls */ + _actuators_out_1->timestamp = _actuators_fw_in->timestamp; _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] * (1 - _mc_roll_weight); //roll _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = @@ -300,7 +302,7 @@ void Standard::fill_actuator_outputs() * (1 - _mc_yaw_weight); // yaw // set the fixed wing throttle control - if (_vtol_schedule.flight_mode == FW_MODE) { + if (_vtol_schedule.flight_mode == FW_MODE && _armed->armed) { // take the throttle value commanded by the fw controller _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index e7daa2c3a3..a1680cc09c 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -129,7 +129,7 @@ void Tailsitter::update_vtol_state() * 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) { + if (!_attc->is_fixed_wing_requested()) { switch (_vtol_schedule.flight_mode) { // user switchig to MC mode @@ -446,6 +446,7 @@ void Tailsitter::fill_actuator_outputs() { switch (_vtol_mode) { case ROTARY_WING: + _actuators_out_0->timestamp = _actuators_mc_in->timestamp; _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]; _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; @@ -453,6 +454,8 @@ void Tailsitter::fill_actuator_outputs() _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; + _actuators_out_1->timestamp = _actuators_mc_in->timestamp; + if (_params->elevons_mc_lock == 1) { _actuators_out_1->control[0] = 0; _actuators_out_1->control[1] = 0; @@ -469,6 +472,7 @@ void Tailsitter::fill_actuator_outputs() case FIXED_WING: // in fixed wing mode we use engines only for providing thrust, no moments are generated + _actuators_out_0->timestamp = _actuators_fw_in->timestamp; _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = 0; _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = 0; _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = 0; @@ -487,6 +491,8 @@ void Tailsitter::fill_actuator_outputs() case TRANSITION: // in transition engines are mixed by weight (BACK TRANSITION ONLY) + _actuators_out_0->timestamp = _actuators_mc_in->timestamp; + _actuators_out_1->timestamp = _actuators_mc_in->timestamp; _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] = diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 3890a71843..81cb1cb710 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -67,7 +67,7 @@ Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) : _params_handles_tiltrotor.airspeed_blend_start = param_find("VT_ARSP_BLEND"); _params_handles_tiltrotor.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK"); _params_handles_tiltrotor.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR"); - _params_handles_tiltrotor.fw_motors_off = param_find("VT_FW_MOT_OFF"); + _params_handles_tiltrotor.fw_motors_off = param_find("VT_FW_MOT_OFFID"); } Tiltrotor::~Tiltrotor() @@ -145,7 +145,7 @@ int Tiltrotor::get_motor_off_channels(int channels) break; } - channel_bitmap |= 1 << channel; + channel_bitmap |= 1 << (channel - 1); channels = channels / 10; } @@ -384,6 +384,7 @@ void Tiltrotor::update_external_state() */ void Tiltrotor::fill_actuator_outputs() { + _actuators_out_0->timestamp = _actuators_mc_in->timestamp; _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] = @@ -400,6 +401,7 @@ void Tiltrotor::fill_actuator_outputs() _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];; } + _actuators_out_1->timestamp = _actuators_fw_in->timestamp; _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] = @@ -426,7 +428,7 @@ void Tiltrotor::set_rear_motor_state(rear_motor_state state) break; case DISABLED: - pwm_value = PWM_LOWEST_MAX; + pwm_value = PWM_MOTOR_OFF; _rear_motors = DISABLED; break; @@ -444,21 +446,21 @@ void Tiltrotor::set_rear_motor_state(rear_motor_state state) if (fd < 0) {PX4_WARN("can't open %s", dev);} ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); - struct pwm_output_values pwm_values; - memset(&pwm_values, 0, sizeof(pwm_values)); + struct pwm_output_values pwm_max_values; + memset(&pwm_max_values, 0, sizeof(pwm_max_values)); for (int i = 0; i < _params->vtol_motor_count; i++) { if (is_motor_off_channel(i)) { - pwm_values.values[i] = pwm_value; + pwm_max_values.values[i] = pwm_value; } else { - pwm_values.values[i] = PWM_DEFAULT_MAX; + pwm_max_values.values[i] = PWM_DEFAULT_MAX; } - pwm_values.channel_count = _params->vtol_motor_count; + pwm_max_values.channel_count = _params->vtol_motor_count; } - ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); + ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_max_values); if (ret != OK) {PX4_WARN("failed setting max values");} diff --git a/src/modules/vtol_att_control/tiltrotor_params.c b/src/modules/vtol_att_control/tiltrotor_params.c index 58062c40b0..e37b520473 100644 --- a/src/modules/vtol_att_control/tiltrotor_params.c +++ b/src/modules/vtol_att_control/tiltrotor_params.c @@ -90,7 +90,7 @@ PARAM_DEFINE_FLOAT(VT_TRANS_P2_DUR, 0.5f); * * * @min 0 - * @max 123456 + * @max 12345678 * @group VTOL Attitude Control */ -PARAM_DEFINE_INT32(VT_FW_MOT_OFF, 0); +PARAM_DEFINE_INT32(VT_FW_MOT_OFFID, 0); 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 f21ed3fe52..65ab5f6233 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -559,7 +559,7 @@ void VtolAttitudeControl::task_main() _vtol_type->set_idle_mc(); /* wakeup source*/ - px4_pollfd_struct_t fds[3]; /*input_mc, input_fw, parameters*/ + px4_pollfd_struct_t fds[3] = {}; /*input_mc, input_fw, parameters*/ fds[0].fd = _actuator_inputs_mc; fds[0].events = POLLIN; diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c index 3bb8fa129f..69dbb90609 100644 --- a/src/platforms/posix/px4_layer/drv_hrt.c +++ b/src/platforms/posix/px4_layer/drv_hrt.c @@ -81,7 +81,7 @@ static void hrt_unlock(void) px4_sem_post(&_hrt_lock); } -#if defined(__APPLE__) && defined(__MACH__) +#if (defined(__APPLE__) && defined(__MACH__)) || defined(__PX4_QURT) #include #include #define CLOCK_REALTIME 0 diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 4d453e5983..38ea682943 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -40,6 +40,7 @@ #pragma once #include +#include /* Get the name of the default value fiven the param name */ #define PX4_PARAM_DEFAULT_VALUE_NAME(_name) PARAM_##_name##_DEFAULT @@ -176,8 +177,7 @@ __END_DECLS #define MAX_RAND 32767 #if defined(__PX4_QURT) -#define M_PI 3.14159265358979323846 -#define M_PI_2 1.57079632679489661923 +#include "dspal_math.h" __BEGIN_DECLS #include __END_DECLS diff --git a/src/platforms/px4_posix.h b/src/platforms/px4_posix.h index fa1c60adee..9f48524b45 100644 --- a/src/platforms/px4_posix.h +++ b/src/platforms/px4_posix.h @@ -77,11 +77,16 @@ 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 +#ifdef __PX4_QURT +__EXPORT int px4_sem_timedwait(px4_sem_t *sem, const struct timespec *abstime); +#else +#define px4_sem_timedwait sem_timedwait +#endif + __END_DECLS #endif diff --git a/src/platforms/qurt/include/qurt_log.h b/src/platforms/qurt/include/qurt_log.h index 29ef6302e1..7a9a0eba20 100644 --- a/src/platforms/qurt/include/qurt_log.h +++ b/src/platforms/qurt/include/qurt_log.h @@ -35,11 +35,14 @@ #include #include +#include #ifdef __cplusplus extern "C" { #endif +__EXPORT extern uint64_t hrt_absolute_time(void); + //void qurt_log(int level, const char *file, int line, const char *format, ...); // declaration to make the compiler happy. This symbol is part of the adsp static image. diff --git a/src/lib/ecl/attitude_fw/ecl_wheel_controller.h b/src/platforms/qurt/px4_layer/commands_default.c similarity index 61% rename from src/lib/ecl/attitude_fw/ecl_wheel_controller.h rename to src/platforms/qurt/px4_layer/commands_default.c index f153a8f8f1..ce7ac90768 100644 --- a/src/lib/ecl/attitude_fw/ecl_wheel_controller.h +++ b/src/platforms/qurt/px4_layer/commands_default.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * Copyright (C) 2015 Mark Charlebois. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * @@ -30,41 +30,28 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ - /** - * @file ecl_wheel_controller.h - * Definition of a simple orthogonal coordinated turn yaw PID controller. + * @file commands_default.c + * Commands to run for the "qurt_eagle_default" config * - * @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. + * @author Mark Charlebois */ -#ifndef ECL_HEADING_CONTROLLER_H -#define ECL_HEADING_CONTROLLER_H -#include -#include +#include "get_commands.h" -#include "ecl_controller.h" - -class __EXPORT ECL_WheelController : - public ECL_Controller +const char *get_commands() { -public: - ECL_WheelController(); - ~ECL_WheelController(); + static const char *commands = + "uorb start\n" + "param set CAL_GYRO0_ID 2293760\n" + "param set CAL_ACC0_ID 1310720\n" + "param set CAL_ACC1_ID 1376256\n" + "param set CAL_MAG0_ID 196608\n" + "commander start\n" - float control_attitude(const struct ECL_ControlData &ctl_data); + ; - float control_bodyrate(const struct ECL_ControlData &ctl_data); -}; + return commands; -#endif // ECL_HEADING_CONTROLLER_H +} diff --git a/src/platforms/qurt/px4_layer/drv_hrt.c b/src/platforms/qurt/px4_layer/drv_hrt.c index c15c818172..bdc49da74a 100644 --- a/src/platforms/qurt/px4_layer/drv_hrt.c +++ b/src/platforms/qurt/px4_layer/drv_hrt.c @@ -63,8 +63,36 @@ static void hrt_call_reschedule(void); static sem_t _hrt_lock; static struct work_s _hrt_work; -static void -hrt_call_invoke(void); +#include +#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; + int rv = gettimeofday(&now, NULL); + + if (rv) { + return rv; + } + + tp->tv_sec = now.tv_sec; + tp->tv_nsec = now.tv_usec * 1000; + + return 0; +} + +int px4_clock_settime(clockid_t clk_id, struct timespec *tp) +{ + /* do nothing right now */ + return 0; +} + +static void hrt_call_invoke(void); static void hrt_lock(void) { diff --git a/src/lib/ecl/ecl.h b/src/platforms/qurt/px4_layer/get_commands.h similarity index 77% rename from src/lib/ecl/ecl.h rename to src/platforms/qurt/px4_layer/get_commands.h index 39e1f50809..df0fbd50c6 100644 --- a/src/lib/ecl/ecl.h +++ b/src/platforms/qurt/px4_layer/get_commands.h @@ -1,6 +1,5 @@ /**************************************************************************** - * - * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * Copyright (C) 2015 Mark Charlebois. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -12,7 +11,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 APL nor the names of its contributors may be + * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * @@ -30,17 +29,18 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ - /** - * @file ecl.h - * Adapter / shim layer for system calls needed by ECL + * @file get_commands.cpp + * functions to call to run the set of startup commands * + * @author Mark Charlebois */ -#include -#include +__BEGIN_DECLS +// The commands to run are specified in a target file: commands_.c +extern const char *get_commands(void); + +// Enable external library hook +void qurt_external_hook(void) __attribute__((weak)); +__END_DECLS -#define ecl_absolute_time hrt_absolute_time -#define ecl_elapsed_time hrt_elapsed_time -#define ECL_WARN PX4_WARN -#define ECL_INFO PX4_INFO diff --git a/src/platforms/qurt/px4_layer/main.cpp b/src/platforms/qurt/px4_layer/main.cpp index fc8801b60e..d8caf4659b 100644 --- a/src/platforms/qurt/px4_layer/main.cpp +++ b/src/platforms/qurt/px4_layer/main.cpp @@ -47,6 +47,7 @@ #include #include #include +#include "get_commands.h" using namespace std; diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index 19319d1736..b73e296b0c 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -37,7 +37,10 @@ * Implementation of existing task API for Linux */ -#include +#include "px4_log.h" +#include "px4_posix.h" +#include "px4_workqueue.h" +#include "hrt_work.h" #include #include #include @@ -318,7 +321,7 @@ void px4_show_tasks() __BEGIN_DECLS -int px4_getpid() +unsigned long px4_getpid() { pthread_t pid = pthread_self(); // @@ -333,7 +336,7 @@ int px4_getpid() } PX4_ERR("px4_getpid() called from unknown thread context!"); - return -EINVAL; + return ~0; } @@ -350,5 +353,44 @@ const char *getprogname() return "Unknown App"; } -__END_DECLS +static void timer_cb(void *data) +{ + px4_sem_t *sem = reinterpret_cast(data); + + sem_post(sem); +} + +int px4_sem_timedwait(px4_sem_t *sem, const struct timespec *ts) +{ + void *result; + work_s _hpwork = {}; + + // Create a timer to unblock + uint32_t timeout = ts->tv_sec * 1000000 + (ts->tv_nsec / 1000); + hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)sem, timeout); + sem_wait(sem); + hrt_work_cancel(&_hpwork); + return 0; +} + +int px4_prctl(int option, const char *arg2, unsigned pid) +{ + int rv; + + switch (option) { + case PR_SET_NAME: + // set the threads name - Not supported + // rv = pthread_setname_np(pthread_self(), arg2); + rv = -1; + break; + + default: + rv = -1; + PX4_WARN("FAILED SETTING TASK NAME"); + break; + } + + return rv; +} +__END_DECLS diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index fcfcee3702..09c9b767f5 100644 --- a/unittests/CMakeLists.txt +++ b/unittests/CMakeLists.txt @@ -153,6 +153,12 @@ add_executable(sbus2_test sbus2_test.cpp hrt.cpp target_link_libraries( sbus2_test px4_platform ) add_gtest(sbus2_test) +# DSM test +add_executable(dsm_test dsm_test.cpp hrt.cpp + ${PX_SRC}/lib/rc/dsm.c) +target_link_libraries( dsm_test px4_platform ) +add_gtest(dsm_test) + # st24_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) diff --git a/unittests/dsm_test.cpp b/unittests/dsm_test.cpp new file mode 100644 index 0000000000..7bd6558139 --- /dev/null +++ b/unittests/dsm_test.cpp @@ -0,0 +1,74 @@ +#include +#include +#include + +#include "../../src/systemcmds/tests/tests.h" +#include +// Enable DSM parser output +#define DSM_DEBUG +#include +#include +#include + +#include "gtest/gtest.h" + +TEST(DSMTest, DSM) +{ + const char *filepath = "testdata/dsm_x_data.txt"; + + FILE *fp; + fp = fopen(filepath, "rt"); + + ASSERT_TRUE(fp); + warnx("loading data from: %s", filepath); + + float f; + unsigned x; + int ret; + + // Trash the first 20 lines + for (unsigned i = 0; i < 20; i++) { + char buf[200]; + (void)fgets(buf, sizeof(buf), fp); + } + + // Init the parser + uint8_t frame[20]; + uint16_t rc_values[18]; + uint16_t num_values; + bool dsm_11_bit; + unsigned dsm_frame_drops = 0; + uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]); + + int rate_limiter = 0; + unsigned last_drop = 0; + + while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) { + + ASSERT_GT(ret, 0); + + frame[0] = x; + unsigned len = 1; + + // Pipe the data into the parser + bool result = dsm_parse(f*1e6, &frame[0], len, rc_values, &num_values, + &dsm_11_bit, &dsm_frame_drops, max_channels); + + if (result) { + warnx("decoded packet with %d channels and %s encoding:", num_values, (dsm_11_bit) ? "11 bit" : "10 bit"); + + for (unsigned i = 0; i < num_values; i++) { + printf("chan #%u:\t%d\n", i, (int)rc_values[i]); + } + } + + if (last_drop != (dsm_frame_drops)) { + warnx("frame dropped, now #%d", (dsm_frame_drops)); + last_drop = dsm_frame_drops; + } + + rate_limiter++; + } + + ASSERT_EQ(ret, EOF); +} diff --git a/unittests/testdata/dsm_x_data.txt b/unittests/testdata/dsm_x_data.txt new file mode 100644 index 0000000000..2a4cf782bc --- /dev/null +++ b/unittests/testdata/dsm_x_data.txt @@ -0,0 +1,1106 @@ +Time [s],Value,Parity Error,Framing Error +9.260826000000000,0x00,, +9.260914000000000,0x00,, +9.260999999999999,0x8C,, +9.261088000000001,0x00,, +9.261174000000000,0x2E,, +9.261262000000000,0xAA,, +9.261350000000000,0x14,, +9.261436000000000,0x00,, +9.261524000000000,0x1C,, +9.261609999999999,0x00,, +9.261698000000001,0x01,, +9.261786000000001,0x60,, +9.261872000000000,0x3C,, +9.261960000000000,0x00,, +9.262046000000000,0x36,, +9.262134000000000,0xAA,, +9.271818000000000,0x00,, +9.271906000000000,0x00,, +9.271991999999999,0x0C,, +9.272080000000001,0x00,, +9.272168000000001,0x2E,, +9.272254000000000,0xAA,, +9.272342000000000,0x14,, +9.272428000000000,0x00,, +9.272516000000000,0x1C,, +9.272603999999999,0x00,, +9.272690000000001,0x26,, +9.272778000000001,0xAA,, +9.272864000000000,0x44,, +9.272952000000000,0x00,, +9.273040000000000,0x4C,, +9.273126000000000,0x00,, +9.282828000000000,0x00,, +9.282916000000000,0x00,, +9.283002000000000,0x8C,, +9.283090000000000,0x00,, +9.283177999999999,0x2E,, +9.283264000000001,0xAA,, +9.283352000000001,0x14,, +9.283438000000000,0x00,, +9.283526000000000,0x1C,, +9.283614000000000,0x00,, +9.283700000000000,0x01,, +9.283787999999999,0x61,, +9.283874000000001,0x3C,, +9.283962000000001,0x00,, +9.284050000000001,0x36,, +9.284136000000000,0xAA,, +9.293830000000000,0x00,, +9.293915999999999,0x00,, +9.294003999999999,0x0C,, +9.294090000000001,0x00,, +9.294178000000000,0x2E,, +9.294266000000000,0xAA,, +9.294352000000000,0x14,, +9.294440000000000,0x00,, +9.294525999999999,0x1C,, +9.294613999999999,0x00,, +9.294701999999999,0x26,, +9.294788000000000,0xAA,, +9.294876000000000,0x44,, +9.294962000000000,0x00,, +9.295050000000000,0x4C,, +9.295138000000000,0x00,, +9.304830000000001,0x00,, +9.304918000000001,0x00,, +9.305004000000000,0x8C,, +9.305092000000000,0x00,, +9.305178000000000,0x2E,, +9.305266000000000,0xAA,, +9.305353999999999,0x14,, +9.305440000000001,0x00,, +9.305528000000001,0x1C,, +9.305614000000000,0x00,, +9.305702000000000,0x01,, +9.305790000000000,0x60,, +9.305876000000000,0x3C,, +9.305963999999999,0x00,, +9.306050000000001,0x36,, +9.306138000000001,0xAA,, +9.315832000000000,0x00,, +9.315918000000000,0x00,, +9.316006000000000,0x0C,, +9.316091999999999,0x00,, +9.316179999999999,0x2E,, +9.316268000000001,0xAA,, +9.316354000000000,0x14,, +9.316442000000000,0x00,, +9.316528000000000,0x1C,, +9.316616000000000,0x00,, +9.316704000000000,0x26,, +9.316789999999999,0xAA,, +9.316878000000001,0x44,, +9.316964000000000,0x00,, +9.317052000000000,0x4C,, +9.317140000000000,0x00,, +9.326824000000000,0x00,, +9.326910000000000,0x00,, +9.326998000000000,0x8C,, +9.327083999999999,0x00,, +9.327171999999999,0x2E,, +9.327260000000001,0xAA,, +9.327346000000000,0x14,, +9.327434000000000,0x00,, +9.327520000000000,0x1C,, +9.327608000000000,0x00,, +9.327696000000000,0x01,, +9.327781999999999,0x61,, +9.327870000000001,0x3C,, +9.327956000000000,0x00,, +9.328044000000000,0x36,, +9.328132000000000,0xAA,, +9.337834000000001,0x00,, +9.337920000000000,0x00,, +9.338008000000000,0x0C,, +9.338094000000000,0x00,, +9.338182000000000,0x2E,, +9.338270000000000,0xAA,, +9.338355999999999,0x14,, +9.338444000000001,0x00,, +9.338530000000000,0x1C,, +9.338618000000000,0x00,, +9.338706000000000,0x26,, +9.338792000000000,0xAA,, +9.338880000000000,0x44,, +9.338965999999999,0x00,, +9.339054000000001,0x4C,, +9.339142000000001,0x00,, +9.348834000000000,0x00,, +9.348922000000000,0x00,, +9.349008000000000,0x8C,, +9.349095999999999,0x00,, +9.349183999999999,0x2E,, +9.349270000000001,0xAA,, +9.349358000000001,0x14,, +9.349444000000000,0x00,, +9.349532000000000,0x1C,, +9.349620000000000,0x00,, +9.349705999999999,0x01,, +9.349793999999999,0x61,, +9.349880000000001,0x3C,, +9.349968000000001,0x00,, +9.350056000000000,0x36,, +9.350142000000000,0xAA,, +9.359836000000000,0x00,, +9.359921999999999,0x00,, +9.360010000000001,0x0C,, +9.360098000000001,0x00,, +9.360184000000000,0x2E,, +9.360272000000000,0xAA,, +9.360358000000000,0x14,, +9.360446000000000,0x00,, +9.360533999999999,0x1C,, +9.360620000000001,0x00,, +9.360708000000001,0x26,, +9.360794000000000,0xAA,, +9.360882000000000,0x44,, +9.360970000000000,0x00,, +9.361056000000000,0x4C,, +9.361143999999999,0x00,, +9.370837999999999,0x00,, +9.370924000000000,0x00,, +9.371012000000000,0x8C,, +9.371098000000000,0x00,, +9.371186000000000,0x2E,, +9.371274000000000,0xAA,, +9.371359999999999,0x14,, +9.371447999999999,0x00,, +9.371534000000000,0x1C,, +9.371622000000000,0x00,, +9.371710000000000,0x01,, +9.371796000000000,0x61,, +9.371884000000000,0x3C,, +9.371969999999999,0x00,, +9.372058000000001,0x36,, +9.372146000000001,0xAA,, +9.381840000000000,0x00,, +9.381926000000000,0x00,, +9.382014000000000,0x0C,, +9.382099999999999,0x00,, +9.382187999999999,0x2E,, +9.382275999999999,0xAA,, +9.382362000000001,0x14,, +9.382450000000000,0x00,, +9.382536000000000,0x1C,, +9.382624000000000,0x00,, +9.382712000000000,0x26,, +9.382797999999999,0xAA,, +9.382885999999999,0x44,, +9.382972000000001,0x00,, +9.383060000000000,0x4C,, +9.383148000000000,0x00,, +9.392842000000000,0x00,, +9.392927999999999,0x00,, +9.393015999999999,0x8C,, +9.393102000000001,0x00,, +9.393190000000001,0x2E,, +9.393278000000000,0xAA,, +9.393364000000000,0x14,, +9.393452000000000,0x00,, +9.393537999999999,0x1C,, +9.393625999999999,0x00,, +9.393713999999999,0x01,, +9.393800000000001,0x61,, +9.393888000000000,0x3C,, +9.393974000000000,0x00,, +9.394062000000000,0x36,, +9.394150000000000,0xAA,, +9.403834000000000,0x00,, +9.403922000000000,0x00,, +9.404007999999999,0x0C,, +9.404095999999999,0x00,, +9.404184000000001,0x2E,, +9.404270000000000,0xAA,, +9.404358000000000,0x14,, +9.404444000000000,0x00,, +9.404532000000000,0x1C,, +9.404620000000000,0x00,, +9.404705999999999,0x26,, +9.404794000000001,0xAA,, +9.404880000000000,0x44,, +9.404968000000000,0x00,, +9.405056000000000,0x4C,, +9.405142000000000,0x00,, +9.414835999999999,0x00,, +9.414923999999999,0x00,, +9.415010000000001,0x8C,, +9.415098000000000,0x00,, +9.415184000000000,0x2E,, +9.415272000000000,0xAA,, +9.415360000000000,0x14,, +9.415445999999999,0x00,, +9.415533999999999,0x1C,, +9.415620000000001,0x00,, +9.415708000000000,0x01,, +9.415796000000000,0x60,, +9.415882000000000,0x3C,, +9.415970000000000,0x00,, +9.416055999999999,0x36,, +9.416143999999999,0xAA,, +9.425838000000001,0x00,, +9.425926000000000,0x00,, +9.426012000000000,0x0C,, +9.426100000000000,0x00,, +9.426186000000000,0x2E,, +9.426273999999999,0xAA,, +9.426361999999999,0x14,, +9.426448000000001,0x00,, +9.426536000000000,0x1C,, +9.426622000000000,0x00,, +9.426710000000000,0x26,, +9.426798000000000,0xAA,, +9.426883999999999,0x44,, +9.426971999999999,0x00,, +9.427058000000001,0x4C,, +9.427146000000000,0x00,, +9.436840000000000,0x00,, +9.436926000000000,0x00,, +9.437014000000000,0x8C,, +9.437101999999999,0x00,, +9.437188000000001,0x2E,, +9.437276000000001,0xAA,, +9.437362000000000,0x14,, +9.437450000000000,0x00,, +9.437538000000000,0x1C,, +9.437624000000000,0x00,, +9.437711999999999,0x01,, +9.437798000000001,0x61,, +9.437886000000001,0x3C,, +9.437974000000001,0x00,, +9.438060000000000,0x36,, +9.438148000000000,0xAA,, +9.447842000000000,0x00,, +9.447927999999999,0x00,, +9.448016000000001,0x0C,, +9.448102000000000,0x00,, +9.448190000000000,0x2E,, +9.448278000000000,0xAA,, +9.448364000000000,0x14,, +9.448452000000000,0x00,, +9.448537999999999,0x1C,, +9.448626000000001,0x00,, +9.448714000000001,0x26,, +9.448800000000000,0xAA,, +9.448888000000000,0x44,, +9.448974000000000,0x00,, +9.449062000000000,0x4C,, +9.449149999999999,0x00,, +9.458842000000001,0x00,, +9.458930000000001,0x00,, +9.459018000000000,0x8C,, +9.459104000000000,0x00,, +9.459192000000000,0x2E,, +9.459277999999999,0xAA,, +9.459365999999999,0x14,, +9.459453999999999,0x00,, +9.459540000000001,0x1C,, +9.459628000000000,0x00,, +9.459714000000000,0x01,, +9.459802000000000,0x60,, +9.459890000000000,0x3C,, +9.459975999999999,0x00,, +9.460063999999999,0x36,, +9.460150000000001,0xAA,, +9.469844000000000,0x00,, +9.469932000000000,0x00,, +9.470018000000000,0x0C,, +9.470105999999999,0x00,, +9.470193999999999,0x2E,, +9.470280000000001,0xAA,, +9.470368000000001,0x14,, +9.470454000000000,0x00,, +9.470542000000000,0x1C,, +9.470630000000000,0x00,, +9.470715999999999,0x26,, +9.470803999999999,0xAA,, +9.470890000000001,0x44,, +9.470978000000001,0x00,, +9.471066000000000,0x4C,, +9.471152000000000,0x00,, +9.480846000000000,0x00,, +9.480931999999999,0x00,, +9.481019999999999,0x8C,, +9.481108000000001,0x00,, +9.481194000000000,0x2E,, +9.481282000000000,0xAA,, +9.481368000000000,0x14,, +9.481456000000000,0x00,, +9.481544000000000,0x1C,, +9.481629999999999,0x00,, +9.481718000000001,0x01,, +9.481804000000000,0x60,, +9.481892000000000,0x3C,, +9.481980000000000,0x00,, +9.482066000000000,0x36,, +9.482154000000000,0xAA,, +9.491838000000000,0x00,, +9.491925999999999,0x00,, +9.492012000000001,0x0C,, +9.492100000000001,0x00,, +9.492186000000000,0x2E,, +9.492274000000000,0xAA,, +9.492362000000000,0x14,, +9.492448000000000,0x00,, +9.492535999999999,0x1C,, +9.492622000000001,0x00,, +9.492710000000001,0x26,, +9.492796000000000,0xAA,, +9.492884000000000,0x44,, +9.492972000000000,0x00,, +9.493058000000000,0x4C,, +9.493145999999999,0x00,, +9.502848000000000,0x00,, +9.502934000000000,0x00,, +9.503022000000000,0x8C,, +9.503110000000000,0x00,, +9.503196000000001,0x2E,, +9.503284000000001,0xAA,, +9.503370000000000,0x14,, +9.503458000000000,0x00,, +9.503546000000000,0x1C,, +9.503632000000000,0x00,, +9.503720000000000,0x01,, +9.503806000000001,0x61,, +9.503894000000001,0x3C,, +9.503982000000001,0x00,, +9.504068000000000,0x36,, +9.504156000000000,0xAA,, +9.513847999999999,0x00,, +9.513935999999999,0x00,, +9.514023999999999,0x0C,, +9.514110000000001,0x00,, +9.514198000000000,0x2E,, +9.514284000000000,0xAA,, +9.514372000000000,0x14,, +9.514460000000000,0x00,, +9.514545999999999,0x1C,, +9.514633999999999,0x00,, +9.514720000000001,0x26,, +9.514808000000000,0xAA,, +9.514896000000000,0x44,, +9.514982000000000,0x00,, +9.515070000000000,0x4C,, +9.515155999999999,0x00,, +9.524842000000000,0x00,, +9.524927999999999,0x00,, +9.525016000000001,0x8C,, +9.525102000000000,0x00,, +9.525190000000000,0x2E,, +9.525278000000000,0xAA,, +9.525364000000000,0x14,, +9.525452000000000,0x00,, +9.525537999999999,0x1C,, +9.525626000000001,0x00,, +9.525714000000001,0x01,, +9.525800000000000,0x60,, +9.525888000000000,0x3C,, +9.525974000000000,0x00,, +9.526062000000000,0x36,, +9.526149999999999,0xAA,, +9.535852000000000,0x00,, +9.535938000000000,0x00,, +9.536026000000000,0x0C,, +9.536111999999999,0x00,, +9.536199999999999,0x2E,, +9.536288000000001,0xAA,, +9.536374000000000,0x14,, +9.536462000000000,0x00,, +9.536548000000000,0x1C,, +9.536636000000000,0x00,, +9.536724000000000,0x26,, +9.536809999999999,0xAA,, +9.536898000000001,0x44,, +9.536984000000000,0x00,, +9.537072000000000,0x4C,, +9.537160000000000,0x00,, +9.546851999999999,0x00,, +9.546939999999999,0x00,, +9.547026000000001,0x8C,, +9.547114000000001,0x00,, +9.547202000000000,0x2E,, +9.547288000000000,0xAA,, +9.547376000000000,0x14,, +9.547461999999999,0x00,, +9.547549999999999,0x1C,, +9.547637999999999,0x00,, +9.547724000000001,0x01,, +9.547812000000000,0x60,, +9.547898000000000,0x3C,, +9.547986000000000,0x00,, +9.548074000000000,0x36,, +9.548159999999999,0xAA,, +9.557846000000000,0x00,, +9.557931999999999,0x00,, +9.558020000000001,0x0C,, +9.558106000000000,0x00,, +9.558194000000000,0x2E,, +9.558282000000000,0xAA,, +9.558368000000000,0x14,, +9.558456000000000,0x00,, +9.558541999999999,0x1C,, +9.558630000000001,0x00,, +9.558718000000001,0x26,, +9.558804000000000,0xAA,, +9.558892000000000,0x44,, +9.558978000000000,0x00,, +9.559066000000000,0x4C,, +9.559153999999999,0x00,, +9.568856000000000,0x00,, +9.568944000000000,0x00,, +9.569030000000000,0x8C,, +9.569118000000000,0x00,, +9.569203999999999,0x2E,, +9.569292000000001,0xAA,, +9.569380000000001,0x14,, +9.569466000000000,0x00,, +9.569554000000000,0x1C,, +9.569640000000000,0x00,, +9.569728000000000,0x01,, +9.569815999999999,0x60,, +9.569902000000001,0x3C,, +9.569990000000001,0x00,, +9.570076000000000,0x36,, +9.570164000000000,0xAA,, +9.579848000000000,0x00,, +9.579936000000000,0x00,, +9.580024000000000,0x0C,, +9.580109999999999,0x00,, +9.580197999999999,0x2E,, +9.580284000000001,0xAA,, +9.580372000000001,0x14,, +9.580460000000000,0x00,, +9.580546000000000,0x1C,, +9.580634000000000,0x00,, +9.580719999999999,0x26,, +9.580807999999999,0xAA,, +9.580895999999999,0x44,, +9.580982000000001,0x00,, +9.581070000000000,0x4C,, +9.581156000000000,0x00,, +9.590850000000000,0x00,, +9.590938000000000,0x00,, +9.591024000000001,0x8C,, +9.591112000000001,0x00,, +9.591200000000001,0x2E,, +9.591286000000000,0xAA,, +9.591374000000000,0x14,, +9.591460000000000,0x00,, +9.591548000000000,0x1C,, +9.591635999999999,0x00,, +9.591722000000001,0x01,, +9.591810000000001,0x60,, +9.591896000000000,0x3C,, +9.591984000000000,0x00,, +9.592072000000000,0x36,, +9.592158000000000,0xAA,, +9.601851999999999,0x00,, +9.601938000000001,0x00,, +9.602026000000000,0x0C,, +9.602114000000000,0x00,, +9.602200000000000,0x2E,, +9.602288000000000,0xAA,, +9.602373999999999,0x14,, +9.602461999999999,0x00,, +9.602550000000001,0x1C,, +9.602636000000000,0x00,, +9.602724000000000,0x26,, +9.602810000000000,0xAA,, +9.602898000000000,0x44,, +9.602986000000000,0x00,, +9.603071999999999,0x4C,, +9.603160000000001,0x00,, +9.612852000000000,0x00,, +9.612940000000000,0x00,, +9.613028000000000,0x8C,, +9.613113999999999,0x00,, +9.613201999999999,0x2E,, +9.613288000000001,0xAA,, +9.613376000000001,0x14,, +9.613464000000000,0x00,, +9.613550000000000,0x1C,, +9.613638000000000,0x00,, +9.613723999999999,0x01,, +9.613811999999999,0x61,, +9.613898000000001,0x3C,, +9.613986000000001,0x00,, +9.614074000000000,0x36,, +9.614160000000000,0xAA,, +9.623854000000000,0x00,, +9.623939999999999,0x00,, +9.624027999999999,0x0C,, +9.624116000000001,0x00,, +9.624202000000000,0x2E,, +9.624290000000000,0xAA,, +9.624376000000000,0x14,, +9.624464000000000,0x00,, +9.624549999999999,0x1C,, +9.624637999999999,0x00,, +9.624726000000001,0x26,, +9.624812000000000,0xAA,, +9.624900000000000,0x44,, +9.624986000000000,0x00,, +9.625074000000000,0x4C,, +9.625162000000000,0x00,, +9.634854000000001,0x00,, +9.634942000000001,0x00,, +9.635028000000000,0x8C,, +9.635116000000000,0x00,, +9.635204000000000,0x2E,, +9.635289999999999,0xAA,, +9.635377999999999,0x14,, +9.635464000000001,0x00,, +9.635552000000001,0x1C,, +9.635640000000000,0x00,, +9.635726000000000,0x01,, +9.635814000000000,0x61,, +9.635899999999999,0x3C,, +9.635987999999999,0x00,, +9.636075999999999,0x36,, +9.636162000000001,0xAA,, +9.645856000000000,0x00,, +9.645944000000000,0x00,, +9.646030000000000,0x0C,, +9.646118000000000,0x00,, +9.646203999999999,0x2E,, +9.646292000000001,0xAA,, +9.646380000000001,0x14,, +9.646466000000000,0x00,, +9.646554000000000,0x1C,, +9.646640000000000,0x00,, +9.646728000000000,0x26,, +9.646815999999999,0xAA,, +9.646902000000001,0x44,, +9.646990000000001,0x00,, +9.647076000000000,0x4C,, +9.647164000000000,0x00,, +9.656858000000000,0x00,, +9.656943999999999,0x00,, +9.657031999999999,0x8C,, +9.657118000000001,0x00,, +9.657206000000000,0x2E,, +9.657294000000000,0xAA,, +9.657380000000000,0x14,, +9.657468000000000,0x00,, +9.657553999999999,0x1C,, +9.657641999999999,0x00,, +9.657730000000001,0x01,, +9.657816000000000,0x61,, +9.657904000000000,0x3C,, +9.657990000000000,0x00,, +9.658078000000000,0x36,, +9.658166000000000,0xAA,, +9.667858000000001,0x00,, +9.667946000000001,0x00,, +9.668032000000000,0x0C,, +9.668120000000000,0x00,, +9.668208000000000,0x2E,, +9.668293999999999,0xAA,, +9.668381999999999,0x14,, +9.668468000000001,0x00,, +9.668556000000001,0x1C,, +9.668644000000000,0x00,, +9.668730000000000,0x26,, +9.668818000000000,0xAA,, +9.668903999999999,0x44,, +9.668991999999999,0x00,, +9.669079999999999,0x4C,, +9.669166000000001,0x00,, +9.678860000000000,0x00,, +9.678946000000000,0x00,, +9.679034000000000,0x8C,, +9.679122000000000,0x00,, +9.679207999999999,0x2E,, +9.679296000000001,0xAA,, +9.679382000000000,0x14,, +9.679470000000000,0x00,, +9.679556000000000,0x1C,, +9.679644000000000,0x00,, +9.679732000000000,0x01,, +9.679817999999999,0x60,, +9.679906000000001,0x3C,, +9.679992000000000,0x00,, +9.680080000000000,0x36,, +9.680168000000000,0xAA,, +9.689859999999999,0x00,, +9.689947999999999,0x00,, +9.690034000000001,0x0C,, +9.690122000000001,0x00,, +9.690210000000000,0x2E,, +9.690296000000000,0xAA,, +9.690384000000000,0x14,, +9.690469999999999,0x00,, +9.690557999999999,0x1C,, +9.690644000000001,0x00,, +9.690732000000001,0x26,, +9.690820000000000,0xAA,, +9.690906000000000,0x44,, +9.690994000000000,0x00,, +9.691079999999999,0x4C,, +9.691167999999999,0x00,, +9.700862000000001,0x00,, +9.700948000000000,0x00,, +9.701036000000000,0x8C,, +9.701122000000000,0x00,, +9.701210000000000,0x2E,, +9.701298000000000,0xAA,, +9.701383999999999,0x14,, +9.701472000000001,0x00,, +9.701558000000000,0x1C,, +9.701646000000000,0x00,, +9.701734000000000,0x01,, +9.701820000000000,0x61,, +9.701908000000000,0x3C,, +9.701993999999999,0x00,, +9.702082000000001,0x36,, +9.702170000000001,0xAA,, +9.711862000000000,0x00,, +9.711950000000000,0x00,, +9.712038000000000,0x0C,, +9.712123999999999,0x00,, +9.712211999999999,0x2E,, +9.712298000000001,0xAA,, +9.712386000000000,0x14,, +9.712474000000000,0x00,, +9.712560000000000,0x1C,, +9.712648000000000,0x00,, +9.712733999999999,0x26,, +9.712821999999999,0xAA,, +9.712910000000001,0x44,, +9.712996000000000,0x00,, +9.713084000000000,0x4C,, +9.713170000000000,0x00,, +9.722864000000000,0x00,, +9.722951999999999,0x00,, +9.723038000000001,0x8C,, +9.723126000000001,0x00,, +9.723212000000000,0x2E,, +9.723300000000000,0xAA,, +9.723388000000000,0x14,, +9.723474000000000,0x00,, +9.723561999999999,0x1C,, +9.723648000000001,0x00,, +9.723736000000001,0x01,, +9.723824000000000,0x60,, +9.723910000000000,0x3C,, +9.723998000000000,0x00,, +9.724084000000000,0x36,, +9.724171999999999,0xAA,, +9.733866000000001,0x00,, +9.733952000000000,0x00,, +9.734040000000000,0x0C,, +9.734126000000000,0x00,, +9.734214000000000,0x2E,, +9.734302000000000,0xAA,, +9.734387999999999,0x14,, +9.734476000000001,0x00,, +9.734562000000000,0x1C,, +9.734650000000000,0x00,, +9.734738000000000,0x26,, +9.734824000000000,0xAA,, +9.734912000000000,0x44,, +9.734997999999999,0x00,, +9.735086000000001,0x4C,, +9.735174000000001,0x00,, +9.744866000000000,0x00,, +9.744954000000000,0x00,, +9.745039999999999,0x8C,, +9.745127999999999,0x00,, +9.745215999999999,0x2E,, +9.745302000000001,0xAA,, +9.745390000000000,0x14,, +9.745476000000000,0x00,, +9.745564000000000,0x1C,, +9.745652000000000,0x00,, +9.745737999999999,0x01,, +9.745825999999999,0x60,, +9.745912000000001,0x3C,, +9.746000000000000,0x00,, +9.746088000000000,0x36,, +9.746174000000000,0xAA,, +9.755858000000000,0x00,, +9.755946000000000,0x00,, +9.756034000000000,0x0C,, +9.756119999999999,0x00,, +9.756208000000001,0x2E,, +9.756294000000000,0xAA,, +9.756382000000000,0x14,, +9.756470000000000,0x00,, +9.756556000000000,0x1C,, +9.756644000000000,0x00,, +9.756729999999999,0x26,, +9.756818000000001,0xAA,, +9.756906000000001,0x44,, +9.756992000000000,0x00,, +9.757080000000000,0x4C,, +9.757166000000000,0x00,, +9.766868000000001,0x00,, +9.766956000000000,0x00,, +9.767042000000000,0x8C,, +9.767130000000000,0x00,, +9.767218000000000,0x2E,, +9.767303999999999,0xAA,, +9.767391999999999,0x14,, +9.767478000000001,0x00,, +9.767566000000000,0x1C,, +9.767654000000000,0x00,, +9.767740000000000,0x01,, +9.767828000000000,0x61,, +9.767913999999999,0x3C,, +9.768001999999999,0x00,, +9.768090000000001,0x36,, +9.768176000000000,0xAA,, +9.777870000000000,0x00,, +9.777956000000000,0x00,, +9.778044000000000,0x0C,, +9.778131999999999,0x00,, +9.778218000000001,0x2E,, +9.778306000000001,0xAA,, +9.778392000000000,0x14,, +9.778480000000000,0x00,, +9.778568000000000,0x1C,, +9.778654000000000,0x00,, +9.778741999999999,0x26,, +9.778828000000001,0xAA,, +9.778916000000001,0x44,, +9.779004000000000,0x00,, +9.779090000000000,0x4C,, +9.779178000000000,0x00,, +9.788869999999999,0x00,, +9.788957999999999,0x00,, +9.789046000000001,0x8C,, +9.789132000000000,0x00,, +9.789220000000000,0x2E,, +9.789306000000000,0xAA,, +9.789394000000000,0x14,, +9.789482000000000,0x00,, +9.789567999999999,0x1C,, +9.789656000000001,0x00,, +9.789742000000000,0x01,, +9.789830000000000,0x61,, +9.789918000000000,0x3C,, +9.790004000000000,0x00,, +9.790092000000000,0x36,, +9.790177999999999,0xAA,, +9.799863999999999,0x00,, +9.799950000000001,0x00,, +9.800038000000001,0x0C,, +9.800126000000001,0x00,, +9.800212000000000,0x2E,, +9.800300000000000,0xAA,, +9.800386000000000,0x14,, +9.800473999999999,0x00,, +9.800561999999999,0x1C,, +9.800648000000001,0x00,, +9.800736000000001,0x26,, +9.800822000000000,0xAA,, +9.800910000000000,0x44,, +9.800998000000000,0x00,, +9.801083999999999,0x4C,, +9.801171999999999,0x00,, +9.810874000000000,0x00,, +9.810962000000000,0x00,, +9.811048000000000,0x8C,, +9.811135999999999,0x00,, +9.811223999999999,0x2E,, +9.811310000000001,0xAA,, +9.811398000000001,0x14,, +9.811484000000000,0x00,, +9.811572000000000,0x1C,, +9.811660000000000,0x00,, +9.811745999999999,0x01,, +9.811833999999999,0x61,, +9.811920000000001,0x3C,, +9.812008000000001,0x00,, +9.812096000000000,0x36,, +9.812182000000000,0xAA,, +9.821876000000000,0x00,, +9.821963999999999,0x00,, +9.822050000000001,0x0C,, +9.822138000000001,0x00,, +9.822224000000000,0x2E,, +9.822312000000000,0xAA,, +9.822400000000000,0x14,, +9.822486000000000,0x00,, +9.822573999999999,0x1C,, +9.822660000000001,0x00,, +9.822748000000001,0x26,, +9.822836000000001,0xAA,, +9.822922000000000,0x44,, +9.823010000000000,0x00,, +9.823096000000000,0x4C,, +9.823183999999999,0x00,, +9.832870000000000,0x00,, +9.832955999999999,0x00,, +9.833043999999999,0x8C,, +9.833130000000001,0x00,, +9.833218000000000,0x2E,, +9.833306000000000,0xAA,, +9.833392000000000,0x14,, +9.833480000000000,0x00,, +9.833565999999999,0x1C,, +9.833653999999999,0x00,, +9.833742000000001,0x01,, +9.833828000000000,0x61,, +9.833916000000000,0x3C,, +9.834002000000000,0x00,, +9.834090000000000,0x36,, +9.834178000000000,0xAA,, +9.843870000000001,0x00,, +9.843958000000001,0x00,, +9.844044000000000,0x0C,, +9.844132000000000,0x00,, +9.844220000000000,0x2E,, +9.844306000000000,0xAA,, +9.844393999999999,0x14,, +9.844480000000001,0x00,, +9.844568000000001,0x1C,, +9.844656000000001,0x00,, +9.844742000000000,0x26,, +9.844830000000000,0xAA,, +9.844916000000000,0x44,, +9.845003999999999,0x00,, +9.845091999999999,0x4C,, +9.845178000000001,0x00,, +9.854872000000000,0x00,, +9.854958000000000,0x00,, +9.855046000000000,0x8C,, +9.855134000000000,0x00,, +9.855219999999999,0x2E,, +9.855308000000001,0xAA,, +9.855394000000000,0x14,, +9.855482000000000,0x00,, +9.855570000000000,0x1C,, +9.855656000000000,0x00,, +9.855744000000000,0x01,, +9.855829999999999,0x61,, +9.855918000000001,0x3C,, +9.856006000000001,0x00,, +9.856092000000000,0x36,, +9.856180000000000,0xAA,, +9.865881999999999,0x00,, +9.865968000000001,0x00,, +9.866056000000000,0x0C,, +9.866142000000000,0x00,, +9.866230000000000,0x2E,, +9.866318000000000,0xAA,, +9.866403999999999,0x14,, +9.866491999999999,0x00,, +9.866578000000001,0x1C,, +9.866666000000000,0x00,, +9.866754000000000,0x26,, +9.866840000000000,0xAA,, +9.866928000000000,0x44,, +9.867013999999999,0x00,, +9.867101999999999,0x4C,, +9.867190000000001,0x00,, +9.876882000000000,0x00,, +9.876970000000000,0x00,, +9.877056000000000,0x8C,, +9.877143999999999,0x00,, +9.877231999999999,0x2E,, +9.877318000000001,0xAA,, +9.877406000000001,0x14,, +9.877492000000000,0x00,, +9.877580000000000,0x1C,, +9.877668000000000,0x00,, +9.877753999999999,0x01,, +9.877841999999999,0x61,, +9.877928000000001,0x3C,, +9.878016000000001,0x00,, +9.878104000000000,0x36,, +9.878190000000000,0xAA,, +9.887874000000000,0x00,, +9.887962000000000,0x00,, +9.888050000000000,0x0C,, +9.888135999999999,0x00,, +9.888223999999999,0x2E,, +9.888310000000001,0xAA,, +9.888398000000000,0x14,, +9.888486000000000,0x00,, +9.888572000000000,0x1C,, +9.888660000000000,0x00,, +9.888745999999999,0x26,, +9.888833999999999,0xAA,, +9.888922000000001,0x44,, +9.889008000000000,0x00,, +9.889096000000000,0x4C,, +9.889182000000000,0x00,, +9.898876000000000,0x00,, +9.898963999999999,0x00,, +9.899050000000001,0x8C,, +9.899138000000001,0x00,, +9.899224000000000,0x2E,, +9.899312000000000,0xAA,, +9.899400000000000,0x14,, +9.899486000000000,0x00,, +9.899573999999999,0x1C,, +9.899660000000001,0x00,, +9.899748000000001,0x01,, +9.899836000000001,0x61,, +9.899922000000000,0x3C,, +9.900010000000000,0x00,, +9.900096000000000,0x36,, +9.900183999999999,0xAA,, +9.909878000000001,0x00,, +9.909964000000000,0x00,, +9.910052000000000,0x0C,, +9.910226000000000,0x2E,, +9.910314000000000,0xAA,, +9.910399999999999,0x14,, +9.910488000000001,0x00,, +9.910574000000000,0x1C,, +9.910662000000000,0x00,, +9.910836000000000,0xAA,, +9.910924000000000,0x44,, +9.911009999999999,0x00,, +9.911098000000001,0x4C,, +9.911186000000001,0x00,, +9.920878000000000,0x00,, +9.920966000000000,0x00,, +9.921052000000000,0x8C,, +9.921139999999999,0x00,, +9.921227999999999,0x2E,, +9.921314000000001,0xAA,, +9.921402000000000,0x14,, +9.921488000000000,0x00,, +9.921576000000000,0x1C,, +9.921664000000000,0x00,, +9.921749999999999,0x01,, +9.921837999999999,0x60,, +9.921924000000001,0x3C,, +9.922012000000000,0x00,, +9.922100000000000,0x36,, +9.922186000000000,0xAA,, +9.932054000000001,0x0C,, +9.932140000000000,0x00,, +9.932228000000000,0x2E,, +9.932316000000000,0xAA,, +9.932402000000000,0x14,, +9.932490000000000,0x00,, +9.932575999999999,0x1C,, +9.932664000000001,0x00,, +9.932752000000001,0x26,, +9.932838000000000,0xAA,, +9.932926000000000,0x44,, +9.933012000000000,0x00,, +9.933100000000000,0x4C,, +9.933187999999999,0x00,, +9.942888000000000,0x00,, +9.942976000000000,0x00,, +9.943064000000000,0x8C,, +9.943149999999999,0x00,, +9.943237999999999,0x2E,, +9.943324000000000,0xAA,, +9.943412000000000,0x14,, +9.943500000000000,0x00,, +9.943586000000000,0x1C,, +9.943674000000000,0x00,, +9.943759999999999,0x01,, +9.943847999999999,0x61,, +9.943936000000001,0x3C,, +9.944022000000000,0x00,, +9.944110000000000,0x36,, +9.944196000000000,0xAA,, +9.953882000000000,0x00,, +9.954751999999999,0x26,, +9.954840000000001,0xAA,, +9.954928000000001,0x44,, +9.955014000000000,0x00,, +9.955102000000000,0x4C,, +9.955188000000000,0x00,, +9.964890000000000,0x00,, +9.964978000000000,0x00,, +9.965066000000000,0x8C,, +9.965152000000000,0x00,, +9.965240000000000,0x2E,, +9.965325999999999,0xAA,, +9.965414000000001,0x14,, +9.965502000000001,0x00,, +9.965588000000000,0x1C,, +9.965676000000000,0x00,, +9.965762000000000,0x01,, +9.965850000000000,0x60,, +9.965938000000000,0x3C,, +9.966024000000001,0x00,, +9.966112000000001,0x36,, +9.966198000000000,0xAA,, +9.975882000000000,0x00,, +9.975970000000000,0x00,, +9.976058000000000,0x0C,, +9.976144000000000,0x00,, +9.976232000000000,0x2E,, +9.976317999999999,0xAA,, +9.976406000000001,0x14,, +9.976494000000001,0x00,, +9.976580000000000,0x1C,, +9.976668000000000,0x00,, +9.976754000000000,0x26,, +9.976842000000000,0xAA,, +9.976929999999999,0x44,, +9.977016000000001,0x00,, +9.977104000000001,0x4C,, +9.977190000000000,0x00,, +9.986884000000000,0x00,, +9.986969999999999,0x00,, +9.987057999999999,0x8C,, +9.987144000000001,0x00,, +9.987232000000001,0x2E,, +9.987320000000000,0xAA,, +9.987406000000000,0x14,, +9.987494000000000,0x00,, +9.987579999999999,0x1C,, +9.987667999999999,0x00,, +9.987755999999999,0x01,, +9.987842000000001,0x61,, +9.987930000000000,0x3C,, +9.988016000000000,0x00,, +9.988104000000000,0x36,, +9.988192000000000,0xAA,, +9.997884000000001,0x00,, +9.997972000000001,0x00,, +9.998058000000000,0x0C,, +9.998146000000000,0x00,, +9.998232000000000,0x2E,, +9.998320000000000,0xAA,, +9.998408000000000,0x14,, +9.998494000000001,0x00,, +9.998582000000001,0x1C,, +9.998668000000000,0x00,, +9.998756000000000,0x26,, +9.998844000000000,0xAA,, +9.998930000000000,0x44,, +9.999018000000000,0x00,, +9.999104000000001,0x4C,, +9.999192000000001,0x00,, +10.008894000000000,0x00,, +10.008979999999999,0x00,, +10.009067999999999,0x8C,, +10.009156000000001,0x00,, +10.009242000000000,0x2E,, +10.009330000000000,0xAA,, +10.009416000000000,0x14,, +10.009504000000000,0x00,, +10.009592000000000,0x1C,, +10.009677999999999,0x00,, +10.009766000000001,0x01,, +10.009852000000000,0x60,, +10.009940000000000,0x3C,, +10.010028000000000,0x00,, +10.010114000000000,0x36,, +10.010202000000000,0xAA,, +10.019886000000000,0x00,, +10.019971999999999,0x00,, +10.020060000000001,0x0C,, +10.020148000000001,0x00,, +10.020234000000000,0x2E,, +10.020322000000000,0xAA,, +10.020408000000000,0x14,, +10.020496000000000,0x00,, +10.020583999999999,0x1C,, +10.020670000000001,0x00,, +10.020758000000001,0x26,, +10.020844000000000,0xAA,, +10.020932000000000,0x44,, +10.021020000000000,0x00,,