fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 12:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into beta

Browse Source
This commit is contained in:
Lorenz Meier
2016-01-09 11:35:03 +01:00
parent f40243890e 501ad87dd2
commit b15757d6d5
140 changed files with 4852 additions and 3279 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+1
View File
@@ -23,6 +23,7 @@ build_*/
core
cscope.out
Firmware.sublime-workspace
user.sublime*
Images/*.bin
Images/*.px4
mavlink/include/mavlink/v0.9/
.gitmodules
+3
View File
@@ -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
.travis.yml
+10 -9
View File
@@ -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
CMakeLists.txt
+1
View File
@@ -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")
Makefile
+14 -5
View File
@@ -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:
ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil
+2
View File
@@ -39,5 +39,7 @@ then
param set FW_RR_P 0.3
fi
param set RWTO_TKOFF 1
set HIL yes
set MIXER AERT
ROMFS/px4fmu_common/init.d/10020_3dr_quad
+2 -2
View File
@@ -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
ROMFS/px4fmu_common/init.d/13002_firefly6
+1 -1
View File
@@ -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
ROMFS/px4fmu_common/init.d/4030_solo
+88
View File
@@ -0,0 +1,88 @@
#!nsh
#
# @name Generic Quadrotor X config
#
# @type Quadrotor x
#
# @maintainer Andreas Antener <andreas@uaventure.com>
#
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"
ROMFS/px4fmu_common/init.d/rc.mc_apps
+1 -1
View File
@@ -17,7 +17,7 @@ else
attitude_estimator_q start
local_position_estimator start
else
ekf_att_pos_estimator start
ekf2 start
fi
fi
ROMFS/px4fmu_common/init.d/rcS
+9 -3
View File
@@ -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"
ROMFS/px4fmu_common/mixers/solo.main.mix
+7
View File
@@ -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
Tools/jMAVSim
+1 -1
Submodule Tools/jMAVSim updated: 4267771753...0876a46d53
Tools/sitl_gazebo
+1 -1
Submodule Tools/sitl_gazebo updated: 03a749a3b3...6af47249ca
Vagrantfile
Vendored
+27 -2
View File
@@ -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
cmake/configs/nuttx_px4-stm32f4discovery_default.cmake
+87
View File
@@ -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")
cmake/configs/nuttx_px4fmu-v1_default.cmake
+6 -4
View File
@@ -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
cmake/configs/nuttx_px4fmu-v2_default.cmake
+7 -6
View File
@@ -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
cmake/configs/nuttx_px4fmu-v2_simple.cmake → cmake/configs/nuttx_px4fmu-v2_ekf2.cmake
+60 -11
View File
@@ -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)
cmake/configs/nuttx_px4fmu-v4_default.cmake
+3
View File
@@ -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
cmake/configs/nuttx_sim_simple.cmake
-21
View File
@@ -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
)
cmake/configs/posix_eagle_default.cmake
+3 -1
View File
@@ -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
cmake/configs/posix_sitl_default.cmake
+1
View File
@@ -59,6 +59,7 @@ set(config_module_list
lib/launchdetection
lib/terrain_estimation
lib/runway_takeoff
lib/tailsitter_recovery
examples/px4_simple_app
)
cmake/configs/posix_sitl_ekf2.cmake
+99
View File
@@ -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")
cmake/configs/qurt_eagle_default.cmake
+90
View File
@@ -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
)
cmake/configs/qurt_eagle_hil.cmake
+1
View File
@@ -49,6 +49,7 @@ set(config_module_list
lib/conversion
lib/terrain_estimation
lib/runway_takeoff
lib/tailsitter_recovery
modules/controllib
#
cmake/configs/qurt_eagle_release.cmake
+1
View File
@@ -72,6 +72,7 @@ set(config_module_list
lib/conversion
lib/terrain_estimation
lib/runway_takeoff
lib/tailsitter_recovery
#
# QuRT port
cmake/configs/qurt_eagle_travis.cmake
+1
View File
@@ -54,6 +54,7 @@ set(config_module_list
lib/ecl
lib/terrain_estimation
lib/runway_takeoff
lib/tailsitter_recovery
#
# QuRT port
cmake/nuttx/px4_impl_nuttx.cmake
+8
View File
@@ -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
mavlink/include/mavlink/v1.0
+1 -1
Submodule mavlink/include/mavlink/v1.0 updated: db5cf66d1a...21d36c9e23
msg/position_setpoint.msg
+1
View File
@@ -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
msg/vehicle_command_ack.msg
+8
View File
@@ -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
msg/vehicle_global_position.msg
+13 -13
View File
@@ -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)
msg/vehicle_gps_position.msg
+17 -17
View File
@@ -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
msg/vehicle_local_position.msg
+25 -25
View File
@@ -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)
msg/vehicle_status.msg
+3 -1
View File
@@ -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
nuttx-configs/px4fmu-v4/nsh/defconfig
+2 -2
View File
@@ -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
posix-configs/SITL/init/rcS_ekf2_jmavsim_iris
+62
View File
@@ -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
posix-configs/SITL/init/rcS_gazebo_iris
+1
View File
@@ -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
posix-configs/SITL/init/rcS_gazebo_tailsitter
+2 -1
View File
@@ -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
posix-configs/SITL/init/rcS_jmavsim_iris
+8
View File
@@ -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
src/drivers/boards/aerocore/board_config.h
+1
View File
@@ -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 */
src/drivers/boards/px4-stm32f4discovery/px4discovery_init.c
-12
View File
@@ -129,18 +129,6 @@ stm32_boardinitialize(void)
#include <math.h>
#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)
{
src/drivers/boards/px4fmu-v1/board_config.h
+1
View File
@@ -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
*
src/drivers/boards/px4fmu-v2/board_config.h
+1
View File
@@ -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
*
src/drivers/boards/px4fmu-v4/board_config.h
+12 -3
View File
@@ -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
****************************************************************************************************/
src/drivers/boards/px4fmu-v4/px4fmu_init.c
+1 -1
View File
@@ -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);
src/drivers/boards/px4io-v1/board_config.h
+7
View File
@@ -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)
src/drivers/boards/px4io-v2/board_config.h
+6 -1
View File
@@ -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)
src/drivers/device/vdev_posix.cpp
+21 -2
View File
@@ -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);
src/drivers/drv_pwm_output.h
+1 -1
View File
@@ -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
src/drivers/px4fmu/fmu.cpp
+16 -34
View File
@@ -90,12 +90,8 @@
# include <systemlib/ppm_decode.h>
#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
}
src/examples/hwtest/CMakeLists.txt
+1
View File
@@ -33,6 +33,7 @@
px4_add_module(
MODULE examples__hwtest
MAIN ex_hwtest
STACK 2000
SRCS
hwtest.c
DEPENDS
src/examples/matlab_csv_serial/CMakeLists.txt
+1
View File
@@ -33,6 +33,7 @@
px4_add_module(
MODULE examples__matlab_csv_serial
MAIN matlab_csv_serial
STACK 2000
SRCS
matlab_csv_serial.c
DEPENDS
src/examples/px4_simple_app/CMakeLists.txt
+1
View File
@@ -33,6 +33,7 @@
px4_add_module(
MODULE examples__px4_simple_app
MAIN px4_simple_app
STACK 2000
SRCS
px4_simple_app.c
DEPENDS
src/firmware/nuttx/CMakeLists.txt
+1 -1
View File
@@ -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)
src/lib/conversion/rotation.cpp
+16
View File
@@ -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;
}
}
}
src/lib/conversion/rotation.h
+5 -1
View File
@@ -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 },
};
/**
src/lib/dspal
+1 -1
Submodule src/lib/dspal updated: 94188fb83a...135d402c42
src/lib/ecl
Submodule
+1
Submodule src/lib/ecl added at 2af5856361
src/lib/ecl/attitude_fw/ecl_controller.cpp
-140
View File
@@ -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 <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
*
* 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 <stdio.h>
#include <mathlib/mathlib.h>
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;
}
src/lib/ecl/attitude_fw/ecl_controller.h
-125
View File
@@ -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 <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
*
* 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 <stdbool.h>
#include <stdint.h>
#include <systemlib/perf_counter.h>
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);
};
src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
-198
View File
@@ -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 <math.h>
#include <stdint.h>
#include <float.h>
#include <geo/geo.h>
#include <ecl/ecl.h>
#include <mathlib/mathlib.h>
#include <systemlib/err.h>
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);
}
src/lib/ecl/attitude_fw/ecl_pitch_controller.h
-89
View File
@@ -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 <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
*
* 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 <stdbool.h>
#include <stdint.h>
#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
src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
-146
View File
@@ -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 <ecl/ecl.h>
#include "ecl_roll_controller.h"
#include <stdint.h>
#include <float.h>
#include <geo/geo.h>
#include <ecl/ecl.h>
#include <mathlib/mathlib.h>
#include <systemlib/err.h>
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);
}
src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp
-153
View File
@@ -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 <stdint.h>
#include <float.h>
#include <geo/geo.h>
#include <ecl/ecl.h>
#include <mathlib/mathlib.h>
#include <systemlib/err.h>
#include <ecl/ecl.h>
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;
}
src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
-240
View File
@@ -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 <stdint.h>
#include <float.h>
#include <geo/geo.h>
#include <ecl/ecl.h>
#include <mathlib/mathlib.h>
#include <systemlib/err.h>
#include <ecl/ecl.h>
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;
}
src/lib/ecl/attitude_fw/ecl_yaw_controller.h
-98
View File
@@ -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 <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
*
* 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 <stdbool.h>
#include <stdint.h>
#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
src/lib/ecl/l1/ecl_l1_pos_controller.cpp
-372
View File
@@ -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 <float.h>
#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<float>(CONSTANTS_RADIUS_OF_EARTH);
}
src/lib/ecl/l1/ecl_l1_pos_controller.h
-268
View File
@@ -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 <mathlib/mathlib.h>
#include <geo/geo.h>
#include <ecl/ecl.h>
/**
* 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; ///<maximum roll angle
/**
* Convert a 2D vector from WGS84 to planar coordinates.
*
* This converts from latitude and longitude to planar
* coordinates with (0,0) being at the position of ref and
* returns a vector in meters towards wp.
*
* @param ref The reference position in WGS84 coordinates
* @param wp The point to convert to into the local coordinates, in WGS84 coordinates
* @return The vector in meters pointing from the reference position to the coordinates
*/
math::Vector<2> get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const;
};
#endif /* ECL_L1_POS_CONTROLLER_H */
src/lib/ecl/validation/data_validator.cpp
-169
View File
@@ -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 <lorenz@px4.io>
*/
#include "data_validator.h"
#include <ecl/ecl.h>
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()));
}
}
src/lib/ecl/validation/data_validator.h
-159
View File
@@ -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 <lorenz@px4.io>
*/
#pragma once
#include <cmath>
#include <stdint.h>
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&);
};
src/lib/ecl/validation/data_validator_group.cpp
-266
View File
@@ -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 <lorenz@px4.io>
*/
#include "data_validator_group.h"
#include <ecl/ecl.h>
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<int>(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;
}
src/lib/ecl/validation/data_validator_group.h
-122
View File
@@ -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 <lorenz@px4.io>
*/
#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&);
};
src/lib/matrix
+1 -1
Submodule src/lib/matrix updated: 9cd6ac3dd9...b9d6cac199
src/lib/rc/dsm.c
+251 -111
View File
@@ -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 <px4_config.h>
#include <nuttx/arch.h>
#include <board_config.h>
#include <px4_defines.h>
#include <fcntl.h>
#include <unistd.h>
@@ -49,27 +50,32 @@
#include "dsm.h"
#include <drivers/drv_hrt.h>
#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
#include <px4io.h>
#if defined (__PX4_LINUX) || defined (__PX4_DARWIN)
#define dsm_udelay(arg) usleep(arg)
#else
#include <nuttx/arch.h>
#define dsm_udelay(arg) up_udelay(arg)
#endif
#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
#include <px4io.h>
#endif
//#define DSM_DEBUG
#define DSM_FRAME_SIZE 16 /**<DSM frame size in bytes*/
#define DSM_FRAME_CHANNELS 7 /**<Max supported DSM channels*/
static enum DSM_DECODE_STATE {
DSM_DECODE_STATE_DESYNC = 0,
DSM_DECODE_STATE_SYNC
} dsm_decode_state = DSM_DECODE_STATE_DESYNC;
static int dsm_fd = -1; /**< File handle to the DSM UART */
static hrt_abstime dsm_last_rx_time; /**< Timestamp when we last received */
static hrt_abstime dsm_last_frame_time; /**< Timestamp for start of last dsm frame */
static uint8_t dsm_frame[DSM_FRAME_SIZE]; /**< DSM dsm frame receive buffer */
static hrt_abstime dsm_last_rx_time; /**< Timestamp when we last received data */
static hrt_abstime dsm_last_frame_time; /**< Timestamp for start of last valid dsm frame */
static uint8_t dsm_frame[DSM_BUFFER_SIZE]; /**< DSM dsm frame receive buffer */
static uint8_t dsm_buf[DSM_FRAME_SIZE * 2];
static unsigned dsm_partial_frame_count; /**< Count of bytes received for current dsm frame */
static unsigned dsm_channel_shift; /**< Channel resolution, 0=unknown, 1=10 bit, 2=11 bit */
static unsigned dsm_frame_drops; /**< Count of incomplete DSM frames */
static unsigned dsm_channel_shift = 0; /**< Channel resolution, 0=unknown, 1=10 bit, 2=11 bit */
static unsigned dsm_frame_drops = 0; /**< Count of incomplete DSM frames */
static uint16_t dsm_chan_count = 0; /**< DSM channel count */
static 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);
/**
* Attempt to decode a single channel raw channel datum
@@ -119,12 +125,12 @@ dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *va
*
* @param[in] reset true=reset the 10/11 bit state to unknown
*/
static void
static bool
dsm_guess_format(bool reset)
{
static uint32_t cs10;
static uint32_t cs11;
static unsigned samples;
static uint32_t cs10 = 0;
static uint32_t cs11 = 0;
static unsigned samples = 0;
/* reset the 10/11 bit sniffed channel masks */
if (reset) {
@@ -132,7 +138,7 @@ dsm_guess_format(bool reset)
cs11 = 0;
samples = 0;
dsm_channel_shift = 0;
return;
return false;
}
/* scan the channels in the current dsm_frame in both 10- and 11-bit mode */
@@ -154,9 +160,16 @@ dsm_guess_format(bool reset)
/* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-dsm_frame format */
}
samples++;
#ifdef DSM_DEBUG
printf("dsm guess format: samples: %d %s\n", samples,
(reset) ? "RESET" : "");
#endif
/* wait until we have seen plenty of frames - 5 should normally be enough */
if (samples++ < 5) {
return;
if (samples < 5) {
return false;
}
/*
@@ -195,40 +208,37 @@ dsm_guess_format(bool reset)
if ((votes11 == 1) && (votes10 == 0)) {
dsm_channel_shift = 11;
//debug("DSM: 11-bit format");
return;
#ifdef DSM_DEBUG
printf("DSM: 11-bit format\n");
#endif
return true;
}
if ((votes10 == 1) && (votes11 == 0)) {
dsm_channel_shift = 10;
//debug("DSM: 10-bit format");
return;
#ifdef DSM_DEBUG
printf("DSM: 10-bit format\n");
#endif
return true;
}
/* call ourselves to reset our state ... we have to try again */
//debug("DSM: format detect fail, 10: 0x%08x %d 11: 0x%08x %d", cs10, votes10, cs11, votes11);
#ifdef DSM_DEBUG
printf("DSM: format detect fail, 10: 0x%08x %d 11: 0x%08x %d\n", cs10, votes10, cs11, votes11);
#endif
dsm_guess_format(true);
return false;
}
/**
* 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)
dsm_config(int dsm_fd)
{
#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
#ifdef GPIO_SPEKTRUM_PWR_EN
// enable power on DSM connector
POWER_SPEKTRUM(true);
#endif
if (dsm_fd < 0) {
dsm_fd = open(device, O_RDONLY | O_NONBLOCK);
}
int ret = -1;
if (dsm_fd >= 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;
}
src/lib/rc/dsm.h
+25 -1
View File
@@ -42,11 +42,35 @@
#pragma once
#include <stdint.h>
#include <px4_config.h>
#include <board_config.h>
#include <px4_defines.h>
__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
src/lib/rc/sbus.c
+24 -3
View File
@@ -44,6 +44,10 @@
#include <termios.h>
#include <string.h>
#ifdef TIOCSSINGLEWIRE
#include <sys/ioctl.h>
#endif
#include "sbus.h"
#include <drivers/drv_hrt.h>
@@ -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
src/lib/rc/sbus.h
+1
View File
@@ -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);
src/lib/ecl/CMakeLists.txt → src/lib/tailsitter_recovery/CMakeLists.txt
+2 -9
View File
@@ -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
)
src/lib/tailsitter_recovery/tailsitter_recovery.cpp
+182
View File
@@ -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 <math.h>
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;
}
src/lib/tailsitter_recovery/tailsitter_recovery.h
+74
View File
@@ -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 <bapstroman@gmail.com>
*/
#include <lib/mathlib/mathlib.h>
#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
};
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+12 -19
View File
@@ -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);
}
}
src/modules/commander/commander.cpp
+107 -43
View File
@@ -94,6 +94,7 @@
#include <uORB/topics/vtol_vehicle_status.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/input_rc.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <drivers/drv_led.h>
#include <drivers/drv_hrt.h>
@@ -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
}
}
}
src/modules/commander/mag_calibration.cpp
+9
View File
@@ -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;
}
}
}
}
src/modules/commander/state_machine_helper.cpp
+21
View File
@@ -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) {
src/modules/ekf2/CMakeLists.txt
+48
View File
@@ -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 :
src/modules/ekf2/ekf2_main.cpp
+491
View File
@@ -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 <px4_config.h>
#include <px4_defines.h>
#include <px4_tasks.h>
#include <px4_posix.h>
#include <px4_time.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <time.h>
#include <float.h>
#include <arch/board/board.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <mathlib/mathlib.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <mavlink/mavlink_log.h>
#include <platforms/px4_defines.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_global_position.h>
#include <ecl/EKF/ekf.h>
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<float> q(att.q[0], att.q[1], att.q[2], att.q[3]);
matrix::Euler<float> 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;
}
src/modules/fw_att_control/CMakeLists.txt
+2
View File
@@ -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 :
src/modules/fw_pos_control_l1/CMakeLists.txt
+1
View File
@@ -44,6 +44,7 @@ px4_add_module(
mtecs/limitoverride.cpp
DEPENDS
platforms__common
git_ecl
lib__external_lgpl
lib__ecl
)
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+2 -2
View File
@@ -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);
src/modules/land_detector/LandDetector.h
+1 -1
View File
@@ -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 */
src/modules/mavlink/CMakeLists.txt
+1
View File
@@ -56,6 +56,7 @@ px4_add_module(
mavlink_rate_limiter.cpp
mavlink_receiver.cpp
mavlink_ftp.cpp
mavlink_log_handler.cpp
DEPENDS
platforms__common
)
src/modules/mavlink/mavlink_log_handler.cpp
+547
View File
@@ -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 <mavlink@grubba.com>
#include "mavlink_log_handler.h"
#include "mavlink_main.h"
#include <sys/stat.h>
#include <time.h>
#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);
}
src/modules/mavlink/mavlink_log_handler.h
+116
View File
@@ -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 <mavlink@grubba.com>
#include <dirent.h>
#include <queue.h>
#include <time.h>
#include <stdio.h>
#include <v1.0/mavlink_types.h>
#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;
};
src/modules/mavlink/mavlink_main.cpp
+148 -76
View File
@@ -51,9 +51,7 @@
#include <assert.h>
#include <math.h>
#include <poll.h>
#ifndef __PX4_POSIX
#include <termios.h>
#endif
#include <time.h>
#include <math.h> /* isinf / isnan checks */
@@ -77,6 +75,7 @@
#include <mavlink/mavlink_log.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_command_ack.h>
#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);
src/modules/mavlink/mavlink_main.h
+9
View File
@@ -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;
src/modules/mavlink/mavlink_messages.cpp
+16
View File
@@ -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);
}
}
};
src/modules/mavlink/mavlink_receiver.cpp
+4 -1
View File
@@ -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 */
src/modules/mc_att_control/mc_att_control_main.cpp
+92 -39
View File
@@ -88,6 +88,7 @@
#include <systemlib/circuit_breaker.h>
#include <lib/mathlib/mathlib.h>
#include <lib/geo/geo.h>
#include <lib/tailsitter_recovery/tailsitter_recovery.h>
/**
* 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");

Some files were not shown because too many files have changed in this diff Show More