fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-04 07:28:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into uavcan_footprint_reduction

Browse Source
This commit is contained in:
Pavel Kirienko
2015-11-12 14:49:02 +03:00
parent 96a12a6027 de39e7de62
commit b95609834c
141 changed files with 2592 additions and 2178 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitmodules
+3
View File
@@ -25,3 +25,6 @@
[submodule "unittests/googletest"]
path = unittests/googletest
url = https://github.com/google/googletest.git
[submodule "src/lib/matrix"]
path = src/lib/matrix
url = https://github.com/PX4/Matrix.git
.travis.yml
+14 -4
View File
@@ -50,6 +50,8 @@ before_install:
&& if grep -Fxq "$exportline" ~/.profile; then echo nothing to do ; else echo $exportline >> ~/.profile; fi
&& . ~/.profile
&& popd
&& git clone git://github.com/PX4/CI-Tools.git
&& ./CI-Tools/s3cmd-configure
&& mkdir -p ~/bin
&& wget -O ~/bin/astyle https://github.com/PX4/astyle/releases/download/2.05.1/astyle-linux && chmod +x ~/bin/astyle
&& astyle --version
@@ -73,6 +75,8 @@ before_script:
- mkdir -p ~/bin
- ln -s /usr/bin/ccache ~/bin/arm-none-eabi-g++
- ln -s /usr/bin/ccache ~/bin/arm-none-eabi-gcc
- ln -s /usr/bin/ccache ~/bin/arm-none-eabi-size
- ln -s /usr/bin/ccache ~/bin/arm-none-eabi-objcopy
- ln -s /usr/bin/ccache ~/bin/clang++
- ln -s /usr/bin/ccache ~/bin/clang++-3.4
- ln -s /usr/bin/ccache ~/bin/clang++-3.5
@@ -96,7 +100,15 @@ script:
- make check_format
- arm-none-eabi-gcc --version
- echo 'Building POSIX Firmware..' && make posix_sitl_simple
- echo 'Running Tests..' && make posix_sitl_simple test && cat build_posix_sitl_simple/src/modules/systemlib/mixer/mixer_multirotor.generated.h
- echo 'Running Tests..' && make posix_sitl_simple test
- echo 'Building UAVCAN node firmware..' && git clone https://github.com/thiemar/vectorcontrol
- cd vectorcontrol
- BOARD=s2740vc_1_0 make && BOARD=px4esc_1_6 make
- cd ..
- mkdir -p ROMFS/px4fmu_common/uavcan/fw/com.thiemar.s2740vc-v1/1.0/
- mkdir -p ROMFS/px4fmu_common/uavcan/fw/org.pixhawk.px4esc-v1/1.0/
- cp vectorcontrol/firmware/com.thiemar.s2740vc-v1-1.0.*.bin ROMFS/px4fmu_common/uavcan/fw/com.thiemar.s2740vc-v1/1.0/.
- cp vectorcontrol/firmware/org.pixhawk.px4esc-v1-1.0.*.bin ROMFS/px4fmu_common/uavcan/fw/org.pixhawk.px4esc-v1/1.0/.
- echo 'Building NuttX px4fmu-v1 Firmware..' && make px4fmu-v1_default
- echo 'Building NuttX px4fmu-v2 Firmware..' && make px4fmu-v2_default
- echo 'Running Tests..' && make px4fmu-v2_default test
@@ -106,9 +118,7 @@ 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
&& zip Firmware.zip px4fmu-v1_default.px4 px4fmu-v2_default.px4
&& git clone git://github.com/PX4/CI-Tools.git
&& ./CI-Tools/s3cmd-configure
&& ./CI-Tools/s3cmd-put px4fmu-v1_default.px4 px4fmu-v2_default.px4 CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/
&& ./CI-Tools/s3cmd-put px4fmu-v1_default.px4 px4fmu-v2_default.px4 build_px4fmu-v2_default/parameters.xml build_px4fmu-v2_default/airframes.xml CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/
&& ./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
@@ -232,6 +232,7 @@ px4_add_git_submodule(TARGET git_nuttx PATH "NuttX")
px4_add_git_submodule(TARGET git_dspal PATH "src/lib/dspal")
px4_add_git_submodule(TARGET git_jmavsim PATH "Tools/jMAVSim")
px4_add_git_submodule(TARGET git_gazebo PATH "Tools/sitl_gazebo")
px4_add_git_submodule(TARGET git_matrix PATH "src/lib/matrix")
add_custom_target(submodule_clean
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
Makefile
+14 -3
View File
@@ -44,10 +44,18 @@ ifneq ($(CMAKE_VER),0)
$(warning Not a valid CMake version or CMake not installed.)
$(warning On Ubuntu, install or upgrade via:)
$(warning )
$(warning 3rd party PPA:)
$(warning sudo add-apt-repository ppa:george-edison55/cmake-3.x -y)
$(warning sudo apt-get update)
$(warning sudo apt-get install cmake)
$(warning )
$(warning Official website:)
$(warning wget https://cmake.org/files/v3.3/cmake-3.3.2-Linux-x86_64.sh)
$(warning chmod +x cmake-3.3.2-Linux-x86_64.sh)
$(warning sudo mkdir /opt/cmake-3.3.2)
$(warning sudo ./cmake-3.3.2-Linux-x86_64.sh --prefix=/opt/cmake-3.3.2 --exclude-subdir)
$(warning export PATH=/opt/cmake-3.3.2/bin:$$PATH)
$(warning )
$(error Fatal)
endif
@@ -143,6 +151,9 @@ ros_sitl_simple:
qurt_eagle_travis:
$(call cmake-build,$@)
qurt_eagle_release:
$(call cmake-build,$@)
posix_eagle_release:
$(call cmake-build,$@)
@@ -153,8 +164,8 @@ posix_sitl_default: posix_sitl_simple
ros: ros_sitl_simple
sitl_deprecation:
@echo "Deprecated. Use 'make posix_sitl_default run_sitl' instead."
@echo "Change init script with 'make posix_sitl_default config'"
@echo "Deprecated. Use 'make posix_sitl_default jmavsim' or"
@echo "'make posix_sitl_default gazebo' if Gazebo is preferred."
sitl_quad: sitl_deprecation
sitl_plane: sitl_deprecation
@@ -173,7 +184,7 @@ clean:
# targets handled by cmake
cmake_targets = test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak \
run_cmake_config config gazebo gazebo_gdb gazebo_lldb jmavsim \
jmavsim_gdb jmavsim_lldb
jmavsim_gdb jmavsim_lldb gazebo_gdb_iris gazebo_lldb_vtol gazebo_iris gazebo_vtol
$(foreach targ,$(cmake_targets),$(eval $(call cmake-targ,$(targ))))
.PHONY: clean
ROMFS/px4fmu_common/init.d/11001_hexa_cox
+11
View File
@@ -4,6 +4,17 @@
#
# @type Hexarotor Coaxial
#
# @output MAIN1 front right top, CW; angle:60; direction:CW
# @output MAIN2 front right bottom, CCW; angle:60; direction:CCW
# @output MAIN3 back top, CW; angle:180; direction:CW
# @output MAIN4 back bottom, CCW; angle:180; direction:CCW
# @output MAIN5 front left top, CW; angle:-60; direction:CW
# @output MAIN6 front left bottom, CCW;angle:-60; direction:CCW
#
# @output AUX1 feed-through of RC AUX1 channel
# @output AUX2 feed-through of RC AUX2 channel
# @output AUX3 feed-through of RC AUX3 channel
#
# @maintainer Lorenz Meier <lorenz@px4.io>
#
ROMFS/px4fmu_common/init.d/3033_wingwing
+4 -1
View File
@@ -14,7 +14,7 @@
# @output AUX2 feed-through of RC AUX2 channel
# @output AUX3 feed-through of RC AUX3 channel
#
# @maintainer Simon Wilks <simon@px4.io>
# @maintainer Lorenz Meier <lorenz@px4.io>
#
sh /etc/init.d/rc.fw_defaults
@@ -39,6 +39,9 @@ then
param set FW_RR_P 0.04
fi
# Configure this as plane
set MAV_TYPE 1
# Set mixer
set MIXER wingwing
# Provide ESC a constant 1000 us pulse
set PWM_OUT 4
ROMFS/px4fmu_common/init.d/4009_qav250
+33
View File
@@ -0,0 +1,33 @@
#!nsh
#
# @name Lumenier QAV250
#
# @type Quadrotor x
#
# @output AUX1 feed-through of RC AUX1 channel
# @output AUX2 feed-through of RC AUX2 channel
# @output AUX3 feed-through of RC AUX3 channel
#
# @maintainer Mark Whitehorn <kd0aij@gmail.com>
#
sh /etc/init.d/4001_quad_x
if [ $AUTOCNF == yes ]
then
param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.05
param set MC_ROLLRATE_I 0.05
param set MC_ROLLRATE_D 0.002
param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.08
param set MC_PITCHRATE_I 0.1
param set MC_PITCHRATE_D 0.003
param set MC_YAW_P 2.8
param set MC_YAWRATE_P 0.2
param set MC_YAWRATE_I 0.1
param set MC_YAWRATE_D 0.0
param set PWM_MIN 980
param set MPC_THR_MIN 0.06
param set MPC_MANTHR_MIN 0.06
fi
ROMFS/px4fmu_common/init.d/rcS
+8
View File
@@ -391,6 +391,14 @@ then
#
set TTYS1_BUSY no
#
# Check if UAVCAN is enabled, default to it for ESCs
#
if param greater UAVCAN_ENABLE 2
then
set OUTPUT_MODE uavcan_esc
fi
# If OUTPUT_MODE == none then something is wrong with setup and we shouldn't try to enable output
if [ $OUTPUT_MODE != none ]
then
Tools/parameters_injected.xml
+138
View File
@@ -0,0 +1,138 @@
<?xml version='1.0' encoding='UTF-8'?>
<parameters>
<version>3</version>
<group name="UAVCAN Motor Parameters" no_code_generation="true">
<parameter default="75" name="ctl_bw" type="INT32">
<short_desc>Speed controller bandwidth</short_desc>
<long_desc>Speed controller bandwidth, in Hz. Higher values result in faster speed and current rise times, but may result in overshoot and higher current consumption. For fixed-wing aircraft, this value should be less than 50 Hz; for multirotors, values up to 100 Hz may provide improvements in responsiveness.</long_desc>
<unit>Hertz</unit>
<min>10</min>
<max>250</max>
</parameter>
<parameter default="1" name="ctl_dir" type="INT32">
<short_desc>Reverse direction</short_desc>
<long_desc>Motor spin direction as detected during initial enumeration. Use 0 or 1 to reverse direction.</long_desc>
<min>0</min>
<max>1</max>
</parameter>
<parameter default="1" name="ctl_gain" type="FLOAT">
<short_desc>Speed (RPM) controller gain</short_desc>
<long_desc>Speed (RPM) controller gain. Determines controller
aggressiveness; units are amp-seconds per radian. Systems with
higher rotational inertia (large props) will need gain increased;
systems with low rotational inertia (small props) may need gain
decreased. Higher values result in faster response, but may result
in oscillation and excessive overshoot. Lower values result in a
slower, smoother response.</long_desc>
<unit>amp-seconds per radian</unit>
<decimal>3</decimal>
<min>0.00</min>
<max>1.00</max>
</parameter>
<parameter default="3.5" name="ctl_hz_idle" type="FLOAT">
<short_desc>Idle speed (e Hz)</short_desc>
<long_desc>Idle speed (e Hz)</long_desc>
<unit>Hertz</unit>
<decimal>3</decimal>
<min>0.0</min>
<max>100.0</max>
</parameter>
<parameter default="25" name="ctl_start_rate" type="INT32">
<short_desc>Spin-up rate (e Hz/s)</short_desc>
<long_desc>Spin-up rate (e Hz/s)</long_desc>
<unit>Hz/s</unit>
<min>5</min>
<max>1000</max>
</parameter>
<parameter default="0" name="esc_index" type="INT32">
<short_desc>Index of this ESC in throttle command messages.</short_desc>
<long_desc>Index of this ESC in throttle command messages.</long_desc>
<unit>Index</unit>
<min>0</min>
<max>15</max>
</parameter>
<parameter default="20034" name="id_ext_status" type="INT32">
<short_desc>Extended status ID</short_desc>
<long_desc>Extended status ID</long_desc>
<min>1</min>
<max>1000000</max>
</parameter>
<parameter default="50000" name="int_ext_status" type="INT32">
<short_desc>Extended status interval (µs)</short_desc>
<long_desc>Extended status interval (µs)</long_desc>
<unit>µs</unit>
<min>0</min>
<max>1000000</max>
</parameter>
<parameter default="50000" name="int_status" type="INT32">
<short_desc>ESC status interval (µs)</short_desc>
<long_desc>ESC status interval (µs)</long_desc>
<unit>µs</unit>
<max>1000000</max>
</parameter>
<parameter default="12" name="mot_i_max" type="FLOAT">
<short_desc>Motor current limit in amps</short_desc>
<long_desc>Motor current limit in amps. This determines the maximum
current controller setpoint, as well as the maximum allowable
current setpoint slew rate. This value should generally be set to
the continuous current rating listed in the motor’s specification
sheet, or set equal to the motor’s specified continuous power
divided by the motor voltage limit.</long_desc>
<unit>Amps</unit>
<decimal>3</decimal>
<min>1</min>
<max>80</max>
</parameter>
<parameter default="2300" name="mot_kv" type="INT32">
<short_desc>Motor Kv in RPM per volt</short_desc>
<long_desc>Motor Kv in RPM per volt. This can be taken from the motor’s
specification sheet; accuracy will help control performance but
some deviation from the specified value is acceptable.</long_desc>
<unit>RPM/v</unit>
<min>0</min>
<max>4000</max>
</parameter>
<parameter default="0.0" name="mot_ls" type="FLOAT">
<short_desc>READ ONLY: Motor inductance in henries.</short_desc>
<long_desc>READ ONLY: Motor inductance in henries. This is measured on start-up.</long_desc>
<unit>henries</unit>
<decimal>3</decimal>
</parameter>
<parameter default="14" name="mot_num_poles" type="INT32">
<short_desc>Number of motor poles.</short_desc>
<long_desc>Number of motor poles. Used to convert mechanical speeds to
electrical speeds. This number should be taken from the motor’s
specification sheet.</long_desc>
<unit>Poles</unit>
<min>2</min>
<max>40</max>
</parameter>
<parameter default="0.0" name="mot_rs" type="FLOAT">
<short_desc>READ ONLY: Motor resistance in ohms</short_desc>
<long_desc>READ ONLY: Motor resistance in ohms. This is measured on start-up. When
tuning a new motor, check that this value is approximately equal
to the value shown in the motor’s specification sheet.</long_desc>
<unit>Ohms</unit>
<decimal>3</decimal>
</parameter>
<parameter default="0.5" name="mot_v_accel" type="FLOAT">
<short_desc>Acceleration limit (V)</short_desc>
<long_desc>Acceleration limit (V)</long_desc>
<unit>Volts</unit>
<decimal>3</decimal>
<min>0.01</min>
<max>1.00</max>
</parameter>
<parameter default="14.8" name="mot_v_max" type="FLOAT">
<short_desc>Motor voltage limit in volts</short_desc>
<long_desc>Motor voltage limit in volts. The current controller’s
commanded voltage will never exceed this value. Note that this may
safely be above the nominal voltage of the motor; to determine the
actual motor voltage limit, divide the motor’s rated power by the
motor current limit.</long_desc>
<unit>Volts</unit>
<decimal>3</decimal>
<min>0</min>
</parameter>
</group>
</parameters>
Tools/px4airframes/xmlout.py
+5 -1
View File
@@ -72,9 +72,13 @@ class XMLOutput():
xml_field.text = value
for code in param.GetOutputCodes():
value = param.GetOutputValue(code)
valstrs = value.split(";")
xml_field = ET.SubElement(xml_param, "output")
xml_field.attrib["name"] = code
xml_field.text = value
for attrib in valstrs[1:]:
attribstrs = attrib.split(":")
xml_field.attrib[attribstrs[0].strip()] = attribstrs[1].strip()
xml_field.text = valstrs[0]
if last_param_name != param.GetName():
board_specific_param_set = False
indent(xml_parameters)
Tools/px4params/xmlout.py
+5 -1
View File
@@ -18,10 +18,14 @@ def indent(elem, level=0):
class XMLOutput():
def __init__(self, groups, board):
def __init__(self, groups, board, inject_xml_file_name):
xml_parameters = ET.Element("parameters")
xml_version = ET.SubElement(xml_parameters, "version")
xml_version.text = "3"
importtree = ET.parse(inject_xml_file_name)
injectgroups = importtree.getroot().findall("group")
for igroup in injectgroups:
xml_parameters.append(igroup)
last_param_name = ""
board_specific_param_set = False
for group in groups:
Tools/px_generate_params.py
+3 -2
View File
@@ -29,7 +29,7 @@ start_name = ""
end_name = ""
for group in root:
if group.tag == "group":
if group.tag == "group" and "no_code_generation" not in group.attrib:
header += """
/*****************************************************************
* %s
@@ -62,7 +62,8 @@ struct px4_parameters_t px4_parameters = {
"""
i=0
for group in root:
if group.tag == "group":
if group.tag == "group" and "no_code_generation" not in group.attrib:
src += """
/*****************************************************************
* %s
Tools/px_process_params.py
+7 -1
View File
@@ -65,6 +65,12 @@ def main():
metavar="FILENAME",
help="Create XML file"
" (default FILENAME: parameters.xml)")
parser.add_argument("-i", "--inject-xml",
nargs='?',
const="../Tools/parameters_injected.xml",
metavar="FILENAME",
help="Inject additional param XML file"
" (default FILENAME: ../Tools/parameters_injected.xml)")
parser.add_argument("-b", "--board",
nargs='?',
const="",
@@ -124,7 +130,7 @@ def main():
# Output to XML file
if args.xml:
print("Creating XML file " + args.xml)
out = xmlout.XMLOutput(param_groups, args.board)
out = xmlout.XMLOutput(param_groups, args.board, args.inject_xml)
out.Save(args.xml)
# Output to DokuWiki tables
Tools/px_romfs_pruner.py
+5 -5
View File
@@ -40,7 +40,7 @@ Delete all comments and newlines before ROMFS is converted to an image
"""
from __future__ import print_function
import argparse
import argparse, re
import os
@@ -57,14 +57,14 @@ def main():
for (root, dirs, files) in os.walk(args.folder):
for file in files:
# only prune text files
if ".zip" in file or ".bin" in file or ".swp" in file or ".data" in file:
if ".zip" in file or ".bin" in file or ".swp" in file or ".data" in file or ".DS_Store" in file:
continue
file_path = os.path.join(root, file)
# read file line by line
pruned_content = ""
with open(file_path, "r") as f:
with open(file_path, "rU") as f:
for line in f:
# handle mixer files differently than startup files
@@ -74,9 +74,9 @@ def main():
else:
if not line.isspace() and not line.strip().startswith("#"):
pruned_content += line
# overwrite old scratch file
# overwrite old scratch file
with open(file_path, "wb") as f:
pruned_content = re.sub("\r\n", "\n", pruned_content)
f.write(pruned_content.encode("ascii", errors='strict'))
Tools/sitl_run.sh
+30 -12
View File
@@ -3,18 +3,27 @@
rc_script=$1
debugger=$2
program=$3
build_path=$4
model=$4
build_path=$5
curr_dir=`pwd`
echo SITL ARGS
echo rc_script: $rc_script
echo debugger: $debugger
echo program: $program
echo model: $model
echo build_path: $build_path
if [ "$#" != 4 ]
if [ "$model" == "" ] || [ "$model" == "none" ]
then
echo usage: sitl_run.sh rc_script debugger program build_path
echo "empty model, setting iris as default"
model="iris"
fi
if [ "$#" != 5 ]
then
echo usage: sitl_run.sh rc_script debugger program model build_path
echo ""
exit 1
fi
@@ -33,13 +42,13 @@ cp Tools/posix.gdbinit $build_path/src/firmware/posix/.gdbinit
SIM_PID=0
if [ "$program" == "jmavsim" ]
if [ "$program" == "jmavsim" ] && [ "$no_sim" == "" ]
then
cd Tools/jMAVSim
ant
java -Djava.ext.dirs= -cp lib/*:out/production/jmavsim.jar me.drton.jmavsim.Simulator -udp 127.0.0.1:14560 &
SIM_PID=echo $!
elif [ "$3" == "gazebo" ]
SIM_PID=`echo $!`
elif [ "$3" == "gazebo" ] && [ "$no_sim" == "" ]
then
if [ -x "$(command -v gazebo)" ]
then
@@ -54,8 +63,10 @@ then
cd Tools/sitl_gazebo/Build
cmake ..
make -j4
gazebo ../worlds/iris.world &
gzserver ../worlds/${model}.world &
SIM_PID=`echo $!`
gzclient &
GUI_PID=`echo $!`
else
echo "You need to have gazebo simulator installed!"
exit 1
@@ -68,18 +79,25 @@ touch rootfs/eeprom/parameters
# Start Java simulator
if [ "$debugger" == "lldb" ]
then
lldb -- mainapp ../../../../$rc_script
lldb -- mainapp ../../../../${rc_script}_${program}
elif [ "$debugger" == "gdb" ]
then
gdb --args mainapp ../../../../$rc_script
gdb --args mainapp ../../../../${rc_script}_${program}
elif [ "$debugger" == "ddd" ]
then
ddd --debugger gdb --args mainapp ../../../../${rc_script}_${program}
elif [ "$debugger" == "valgrind" ]
then
valgrind ./mainapp ../../../../${rc_script}_${program}
else
./mainapp ../../../../$rc_script
./mainapp ../../../../${rc_script}_${program}
fi
if [ "$3" == "jmavsim" ]
if [ "$program" == "jmavsim" ]
then
kill -9 $SIM_PID
elif [ "$3" == "gazebo" ]
elif [ "$program" == "gazebo" ]
then
kill -9 $SIM_PID
kill -9 $GUI_PID
fi
cmake/common/px4_base.cmake
+3 -3
View File
@@ -622,7 +622,7 @@ function(px4_add_common_flags)
)
list(APPEND added_include_dirs
src/lib/eigen
src/lib/matrix
)
set(added_link_dirs) # none used currently
@@ -700,7 +700,7 @@ function(px4_create_git_hash_header)
REQUIRED HEADER
ARGN ${ARGN})
execute_process(
COMMAND git log -n 1 --pretty=format:"%H"
COMMAND git rev-parse HEAD
OUTPUT_VARIABLE git_desc
OUTPUT_STRIP_TRAILING_WHITESPACE
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
@@ -741,7 +741,7 @@ function(px4_generate_parameters_xml)
)
add_custom_command(OUTPUT ${OUT}
COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_SOURCE_DIR}/Tools/px_process_params.py
-s ${path} --board CONFIG_ARCH_${BOARD} --xml
-s ${path} --board CONFIG_ARCH_${BOARD} --xml --inject-xml
DEPENDS ${param_src_files}
)
set(${OUT} ${${OUT}} PARENT_SCOPE)
cmake/configs/nuttx_px4fmu-v2_default.cmake
+2 -2
View File
@@ -37,7 +37,7 @@ set(config_module_list
drivers/meas_airspeed
drivers/frsky_telemetry
modules/sensors
drivers/mkblctrl
#drivers/mkblctrl
drivers/px4flow
drivers/oreoled
drivers/gimbal
@@ -54,7 +54,7 @@ set(config_module_list
systemcmds/pwm
systemcmds/esc_calib
systemcmds/reboot
systemcmds/topic_listener
#systemcmds/topic_listener
systemcmds/top
systemcmds/config
systemcmds/nshterm
cmake/configs/posix_sitl_simple.cmake
+1
View File
@@ -36,6 +36,7 @@ set(config_module_list
modules/ekf_att_pos_estimator
modules/position_estimator_inav
modules/navigator
modules/vtol_att_control
modules/mc_pos_control
modules/mc_att_control
modules/mc_pos_control_multiplatform
cmake/qurt/px4_impl_qurt.cmake
+1 -1
View File
@@ -222,7 +222,7 @@ function(px4_os_prebuild_targets)
ONE_VALUE OUT BOARD THREADS
REQUIRED OUT BOARD
ARGN ${ARGN})
add_custom_target(${OUT} DEPENDS git_dspal git_eigen)
add_custom_target(${OUT} DEPENDS git_dspal)
endfunction()
cmake/ros-CMakeLists.txt
+2
View File
@@ -81,6 +81,7 @@ add_message_files(
offboard_control_mode.msg
vehicle_force_setpoint.msg
distance_sensor.msg
control_state.msg
)
## Generate services in the 'srv' folder
@@ -135,6 +136,7 @@ include_directories(
${CMAKE_BINARY_DIR}/src/modules
src/
src/lib
src/lib/matrix
${EIGEN_INCLUDE_DIRS}
integrationtests
)
cmake/templates/build_git_version.h.in
+1 -1
View File
@@ -1,4 +1,4 @@
/* Auto Magically Generated file */
/* Do not edit! */
#define PX4_GIT_VERSION_STR @git_desc@
#define PX4_GIT_VERSION_STR "@git_desc@"
#define PX4_GIT_VERSION_BINARY 0x@git_desc_short@
msg/control_state.msg
+18
View File
@@ -0,0 +1,18 @@
# This is similar to the mavlink message CONTROL_SYSTEM_STATE, but for onboard use */
uint64 timestamp # in microseconds since system start
float32 x_acc # X acceleration in body frame
float32 y_acc # Y acceleration in body frame
float32 z_acc # Z acceleration in body frame
float32 x_vel # X velocity in body frame
float32 y_vel # Y velocity in body frame
float32 z_vel # Z velocity in body frame
float32 x_pos # X position in local frame
float32 y_pos # Y position in local frame
float32 z_pos # z position in local frame
float32 airspeed # Airspeed, estimated
float32[3] vel_variance # Variance in body velocity estimate
float32[3] pos_variance # Variance in local position estimate
float32[4] q # Attitude Quaternion
float32 roll_rate # Roll body angular rate (rad/s, x forward/y right/z down)
float32 pitch_rate # Pitch body angular rate (rad/s, x forward/y right/z down)
float32 yaw_rate # Yaw body angular rate (rad/s, x forward/y right/z down)
msg/geofence_result.msg
+7
View File
@@ -1 +1,8 @@
uint8 GF_ACTION_NONE = 0 # no action on geofence violation
uint8 GF_ACTION_WARN = 1 # critical mavlink message
uint8 GF_ACTION_LOITER = 2 # switch to AUTO|LOITER
uint8 GF_ACTION_RTL = 3 # switch to AUTO|RTL
uint8 GF_ACTION_TERMINATE = 4 # flight termination
bool geofence_violated # true if the geofence is violated
uint8 geofence_action # action to take when geofence is violated
msg/home_position.msg
+5
View File
@@ -8,3 +8,8 @@ float32 alt # Altitude in meters (AMSL)
float32 x # X coordinate in meters
float32 y # Y coordinate in meters
float32 z # Z coordinate in meters
float32 yaw # Yaw angle in radians
float32 direction_x # Takeoff direction in NED X
float32 direction_y # Takeoff direction in NED Y
float32 direction_z # Takeoff direction in NED Z
msg/manual_control_setpoint.msg
+1
View File
@@ -38,6 +38,7 @@ float32 aux5 # default function: payload drop
uint8 mode_switch # main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO
uint8 return_switch # return to launch 2 position switch (mandatory): _NORMAL_, RTL
uint8 rattitude_switch # rattitude control 2 position switch (optional): _MANUAL, RATTITUDE
uint8 posctl_switch # position control 2 position switch (optional): _ALTCTL_, POSCTL
uint8 loiter_switch # loiter 2 position switch (optional): _MISSION_, LOITER
uint8 acro_switch # acro 2 position switch (optional): _MANUAL_, ACRO
msg/rc_channels.msg
+4 -3
View File
@@ -1,4 +1,4 @@
int32 RC_CHANNELS_FUNCTION_MAX=19
int32 RC_CHANNELS_FUNCTION_MAX=20
uint8 RC_CHANNELS_FUNCTION_THROTTLE=0
uint8 RC_CHANNELS_FUNCTION_ROLL=1
uint8 RC_CHANNELS_FUNCTION_PITCH=2
@@ -18,10 +18,11 @@ uint8 RC_CHANNELS_FUNCTION_AUX_5=15
uint8 RC_CHANNELS_FUNCTION_PARAM_1=16
uint8 RC_CHANNELS_FUNCTION_PARAM_2=17
uint8 RC_CHANNELS_FUNCTION_PARAM_3_5=18
uint8 RC_CHANNELS_FUNCTION_RATTITUDE=19
uint64 timestamp # Timestamp in microseconds since boot time
uint64 timestamp_last_valid # Timestamp of last valid RC signal
float32[19] channels # Scaled to -1..1 (throttle: 0..1)
float32[20] channels # Scaled to -1..1 (throttle: 0..1)
uint8 channel_count # Number of valid channels
int8[19] function # Functions mapping
int8[20] function # Functions mapping
uint8 rssi # Receive signal strength index
bool signal_lost # Control signal lost, should be checked together with topic timeout
msg/telemetry_status.msg
+1
View File
@@ -2,6 +2,7 @@ uint8 TELEMETRY_STATUS_RADIO_TYPE_GENERIC = 0
uint8 TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO = 1
uint8 TELEMETRY_STATUS_RADIO_TYPE_UBIQUITY_BULLET = 2
uint8 TELEMETRY_STATUS_RADIO_TYPE_WIRE = 3
uint8 TELEMETRY_STATUS_RADIO_TYPE_USB = 4
uint64 timestamp
uint64 heartbeat_time # Time of last received heartbeat from remote system
msg/vehicle_status.msg
+4 -2
View File
@@ -8,7 +8,8 @@ uint8 MAIN_STATE_AUTO_RTL = 5
uint8 MAIN_STATE_ACRO = 6
uint8 MAIN_STATE_OFFBOARD = 7
uint8 MAIN_STATE_STAB = 8
uint8 MAIN_STATE_MAX = 9
uint8 MAIN_STATE_RATTITUDE = 9
uint8 MAIN_STATE_MAX = 10
# 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.
@@ -41,7 +42,8 @@ uint8 NAVIGATION_STATE_DESCEND = 12 # Descend mode (no position control)
uint8 NAVIGATION_STATE_TERMINATION = 13 # Termination mode
uint8 NAVIGATION_STATE_OFFBOARD = 14
uint8 NAVIGATION_STATE_STAB = 15 # Stabilized mode
uint8 NAVIGATION_STATE_MAX = 16
uint8 NAVIGATION_STATE_RATTITUDE = 16 # Rattitude (aka "flip") mode
uint8 NAVIGATION_STATE_MAX = 17
# VEHICLE_MODE_FLAG, same as MAV_MODE_FLAG of MAVLink 1.0 protocol
uint8 VEHICLE_MODE_FLAG_SAFETY_ARMED = 128
posix-configs/SITL/init/rcS_iris_gazebo → posix-configs/SITL/init/rcS_gazebo
View File
posix-configs/SITL/init/rcS → posix-configs/SITL/init/rcS_jmavsim
View File
posix-configs/SITL/init/rcS_lpe_gazebo
+66
View File
@@ -0,0 +1,66 @@
uorb start
simulator start -s
param load
param set MAV_TYPE 2
param set MC_PITCHRATE_P 0.3
param set MC_ROLLRATE_P 0.3
param set MC_YAW_P 2.0
param set MC_YAWRATE_P 0.35
param set SYS_AUTOSTART 4010
param set SYS_RESTART_TYPE 2
dataman start
param set CAL_GYRO0_ID 2293760
param set CAL_ACC0_ID 1376256
param set CAL_ACC1_ID 1310720
param set CAL_MAG0_ID 196608
param set CAL_GYRO0_XOFF 0.01
param set CAL_ACC0_XOFF 1
param set CAL_ACC0_YOFF 2
param set CAL_ACC0_ZOFF 3
param set CAL_ACC0_XSCALE 1.01
param set CAL_ACC0_YSCALE 1.01
param set CAL_ACC0_ZSCALE 1.01
param set CAL_ACC1_XOFF 0.01
param set CAL_MAG0_XOFF 0.01
param set MPC_XY_P 0.15
param set MPC_XY_VEL_P 0.05
param set MPC_XY_VEL_D 0.005
param set MPC_XY_FF 0.1
param set SENS_BOARD_ROT 8
# changes for LPE
param set COM_RC_IN_MODE 1
param set LPE_BETA_MAX 10000
rgbled start
tone_alarm start
gyrosim start
accelsim start
barosim start
adcsim start
gpssim start
pwm_out_sim mode_pwm
sleep 1
sensors start
commander start
land_detector start multicopter
navigator start
attitude_estimator_q start
mc_pos_control start
mc_att_control start
mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_w.main.mix
mavlink start -u 14556 -r 2000000
mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
mavlink stream -r 80 -s LOCAL_POSITION_NED_COV -u 14556
mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
mavlink stream -r 80 -s ATTITUDE -u 14556
mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556
mavlink stream -r 20 -s RC_CHANNELS -u 14556
mavlink stream -r 250 -s HIGHRES_IMU -u 14556
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
mavlink boot_complete
sdlog2 start -r 100 -e -t -a
# start LPE at end, when we know it is ok to init sensors
sleep 5
local_position_estimator start
posix-configs/SITL/init/rcS_lpe → posix-configs/SITL/init/rcS_lpe_jmavsim
View File
src/drivers/device/vdev.cpp
+33
View File
@@ -67,6 +67,7 @@ private:
#define PX4_MAX_DEV 500
static px4_dev_t *devmap[PX4_MAX_DEV];
pthread_mutex_t devmutex = PTHREAD_MUTEX_INITIALIZER;
/*
* The standard NuttX operation dispatch table can't call C++ member functions
@@ -142,8 +143,12 @@ VDev::register_driver(const char *name, void *data)
// Make sure the device does not already exist
// FIXME - convert this to a map for efficiency
pthread_mutex_lock(&devmutex);
for (int i = 0; i < PX4_MAX_DEV; ++i) {
if (devmap[i] && (strcmp(devmap[i]->name, name) == 0)) {
pthread_mutex_unlock(&devmutex);
return -EEXIST;
}
}
@@ -157,6 +162,8 @@ VDev::register_driver(const char *name, void *data)
}
}
pthread_mutex_unlock(&devmutex);
if (ret != PX4_OK) {
PX4_ERR("No free devmap entries - increase PX4_MAX_DEV");
}
@@ -174,6 +181,8 @@ VDev::unregister_driver(const char *name)
return -EINVAL;
}
pthread_mutex_lock(&devmutex);
for (int i = 0; i < PX4_MAX_DEV; ++i) {
if (devmap[i] && (strcmp(name, devmap[i]->name) == 0)) {
delete devmap[i];
@@ -184,6 +193,8 @@ VDev::unregister_driver(const char *name)
}
}
pthread_mutex_unlock(&devmutex);
return ret;
}
@@ -194,15 +205,20 @@ VDev::unregister_class_devname(const char *class_devname, unsigned class_instanc
char name[32];
snprintf(name, sizeof(name), "%s%u", class_devname, class_instance);
pthread_mutex_lock(&devmutex);
for (int i = 0; i < PX4_MAX_DEV; ++i) {
if (devmap[i] && strcmp(devmap[i]->name, name) == 0) {
delete devmap[i];
PX4_DEBUG("Unregistered class DEV %s", name);
devmap[i] = NULL;
pthread_mutex_unlock(&devmutex);
return PX4_OK;
}
}
pthread_mutex_unlock(&devmutex);
return -EINVAL;
}
@@ -497,15 +513,20 @@ VDev *VDev::getDev(const char *path)
PX4_DEBUG("VDev::getDev");
int i = 0;
pthread_mutex_lock(&devmutex);
for (; i < PX4_MAX_DEV; ++i) {
//if (devmap[i]) {
// printf("%s %s\n", devmap[i]->name, path);
//}
if (devmap[i] && (strcmp(devmap[i]->name, path) == 0)) {
pthread_mutex_unlock(&devmutex);
return (VDev *)(devmap[i]->cdev);
}
}
pthread_mutex_unlock(&devmutex);
return NULL;
}
@@ -514,11 +535,15 @@ void VDev::showDevices()
int i = 0;
PX4_INFO("Devices:");
pthread_mutex_lock(&devmutex);
for (; i < PX4_MAX_DEV; ++i) {
if (devmap[i] && strncmp(devmap[i]->name, "/dev/", 5) == 0) {
PX4_INFO(" %s", devmap[i]->name);
}
}
pthread_mutex_unlock(&devmutex);
}
void VDev::showTopics()
@@ -526,11 +551,15 @@ void VDev::showTopics()
int i = 0;
PX4_INFO("Devices:");
pthread_mutex_lock(&devmutex);
for (; i < PX4_MAX_DEV; ++i) {
if (devmap[i] && strncmp(devmap[i]->name, "/obj/", 5) == 0) {
PX4_INFO(" %s", devmap[i]->name);
}
}
pthread_mutex_unlock(&devmutex);
}
void VDev::showFiles()
@@ -538,12 +567,16 @@ void VDev::showFiles()
int i = 0;
PX4_INFO("Files:");
pthread_mutex_lock(&devmutex);
for (; i < PX4_MAX_DEV; ++i) {
if (devmap[i] && strncmp(devmap[i]->name, "/obj/", 5) != 0 &&
strncmp(devmap[i]->name, "/dev/", 5) != 0) {
PX4_INFO(" %s", devmap[i]->name);
}
}
pthread_mutex_unlock(&devmutex);
}
const char *VDev::topicList(unsigned int *next)
src/drivers/device/vdev_posix.cpp
+51 -15
View File
@@ -52,6 +52,8 @@
using namespace device;
pthread_mutex_t filemutex = PTHREAD_MUTEX_INITIALIZER;
extern "C" {
static void timer_cb(void *data)
@@ -68,7 +70,27 @@ extern "C" {
inline bool valid_fd(int fd)
{
return (fd < PX4_MAX_FD && fd >= 0 && filemap[fd] != NULL);
pthread_mutex_lock(&filemutex);
bool ret = (fd < PX4_MAX_FD && fd >= 0 && filemap[fd] != NULL);
pthread_mutex_unlock(&filemutex);
return ret;
}
inline VDev *get_vdev(int fd)
{
pthread_mutex_lock(&filemutex);
bool valid = (fd < PX4_MAX_FD && fd >= 0 && filemap[fd] != NULL);
VDev *dev;
if (valid) {
dev = (VDev *)(filemap[fd]->vdev);
} else {
dev = nullptr;
}
pthread_mutex_unlock(&filemutex);
return dev;
}
int px4_open(const char *path, int flags, ...)
@@ -93,6 +115,9 @@ extern "C" {
}
if (dev) {
pthread_mutex_lock(&filemutex);
for (i = 0; i < PX4_MAX_FD; ++i) {
if (filemap[i] == 0) {
filemap[i] = new device::file_t(flags, dev, i);
@@ -100,6 +125,8 @@ extern "C" {
}
}
pthread_mutex_unlock(&filemutex);
if (i < PX4_MAX_FD) {
ret = dev->open(filemap[i]);
@@ -125,11 +152,14 @@ extern "C" {
{
int ret;
if (valid_fd(fd)) {
VDev *dev = (VDev *)(filemap[fd]->vdev);
PX4_DEBUG("px4_close fd = %d", fd);
VDev *dev = get_vdev(fd);
if (dev) {
pthread_mutex_lock(&filemutex);
ret = dev->close(filemap[fd]);
filemap[fd] = NULL;
filemap[fd] = nullptr;
pthread_mutex_unlock(&filemutex);
PX4_DEBUG("px4_close fd = %d", fd);
} else {
ret = -EINVAL;
@@ -147,8 +177,9 @@ extern "C" {
{
int ret;
if (valid_fd(fd)) {
VDev *dev = (VDev *)(filemap[fd]->vdev);
VDev *dev = get_vdev(fd);
if (dev) {
PX4_DEBUG("px4_read fd = %d", fd);
ret = dev->read(filemap[fd], (char *)buffer, buflen);
@@ -168,8 +199,9 @@ extern "C" {
{
int ret;
if (valid_fd(fd)) {
VDev *dev = (VDev *)(filemap[fd]->vdev);
VDev *dev = get_vdev(fd);
if (dev) {
PX4_DEBUG("px4_write fd = %d", fd);
ret = dev->write(filemap[fd], (const char *)buffer, buflen);
@@ -190,8 +222,9 @@ extern "C" {
PX4_DEBUG("px4_ioctl fd = %d", fd);
int ret = 0;
if (valid_fd(fd)) {
VDev *dev = (VDev *)(filemap[fd]->vdev);
VDev *dev = get_vdev(fd);
if (dev) {
ret = dev->ioctl(filemap[fd], cmd, arg);
} else {
@@ -221,9 +254,10 @@ extern "C" {
fds[i].revents = 0;
fds[i].priv = NULL;
VDev *dev = get_vdev(fds[i].fd);
// If fd is valid
if (valid_fd(fds[i].fd)) {
VDev *dev = (VDev *)(filemap[fds[i].fd]->vdev);;
if (dev) {
PX4_DEBUG("px4_poll: VDev->poll(setup) %d", fds[i].fd);
ret = dev->poll(filemap[fds[i].fd], &fds[i], true);
@@ -251,9 +285,11 @@ extern "C" {
// For each fd
for (i = 0; i < nfds; ++i) {
VDev *dev = get_vdev(fds[i].fd);
// If fd is valid
if (valid_fd(fds[i].fd)) {
VDev *dev = (VDev *)(filemap[fds[i].fd]->vdev);;
if (dev) {
PX4_DEBUG("px4_poll: VDev->poll(teardown) %d", fds[i].fd);
ret = dev->poll(filemap[fds[i].fd], &fds[i], false);
src/drivers/ll40ls/LidarLiteI2C.cpp
+11 -13
View File
@@ -114,35 +114,33 @@ int LidarLiteI2C::init()
/* do I2C init (and probe) first */
if (I2C::init() != OK) {
goto out;
return ret;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(struct distance_sensor_s));
if (_reports == nullptr) {
goto out;
return ret;
}
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* get a publish handle on the range finder topic */
struct distance_sensor_s ds_report;
measure();
_reports->get(&ds_report);
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
&_orb_class_instance, ORB_PRIO_LOW);
/* get a publish handle on the range finder topic */
struct distance_sensor_s ds_report = {};
measure();
_reports->get(&ds_report);
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
&_orb_class_instance, ORB_PRIO_LOW);
if (_distance_sensor_topic == nullptr) {
DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?");
}
if (_distance_sensor_topic == nullptr) {
DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?");
}
ret = OK;
/* sensor is ok, but we don't really know if it is within range */
_sensor_ok = true;
out:
return ret;
}
src/drivers/ll40ls/LidarLitePWM.cpp
+8 -10
View File
@@ -108,17 +108,15 @@ int LidarLitePWM::init()
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* get a publish handle on the distance_sensor topic */
struct distance_sensor_s ds_report;
measure();
_reports->get(&ds_report);
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
&_orb_class_instance, ORB_PRIO_LOW);
/* get a publish handle on the distance_sensor topic */
struct distance_sensor_s ds_report = {};
measure();
_reports->get(&ds_report);
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
&_orb_class_instance, ORB_PRIO_LOW);
if (_distance_sensor_topic == nullptr) {
DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?");
}
if (_distance_sensor_topic == nullptr) {
DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?");
}
return OK;
src/drivers/mb12xx/mb12xx.cpp
+9 -11
View File
@@ -255,7 +255,7 @@ MB12XX::init()
/* do I2C init (and probe) first */
if (I2C::init() != OK) {
goto out;
return ret;
}
/* allocate basic report buffers */
@@ -265,21 +265,19 @@ MB12XX::init()
set_address(_index_counter); /* set I2c port to temp sonar i2c adress */
if (_reports == nullptr) {
goto out;
return ret;
}
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* get a publish handle on the range finder topic */
struct distance_sensor_s ds_report = {};
/* get a publish handle on the range finder topic */
struct distance_sensor_s ds_report = {};
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
&_orb_class_instance, ORB_PRIO_LOW);
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
&_orb_class_instance, ORB_PRIO_LOW);
if (_distance_sensor_topic == nullptr) {
DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
}
if (_distance_sensor_topic == nullptr) {
DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
}
// XXX we should find out why we need to wait 200 ms here
@@ -321,7 +319,7 @@ MB12XX::init()
ret = OK;
/* sensor is ok, but we don't really know if it is within range */
_sensor_ok = true;
out:
return ret;
}
src/drivers/pwm_out_sim/pwm_out_sim.cpp
+168 -87
View File
@@ -79,6 +79,8 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_0.h>
#include <uORB/topics/actuator_controls_1.h>
#include <uORB/topics/actuator_controls_2.h>
#include <uORB/topics/actuator_controls_3.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_outputs.h>
@@ -94,6 +96,7 @@ public:
enum Mode {
MODE_2PWM,
MODE_4PWM,
MODE_6PWM,
MODE_8PWM,
MODE_12PWM,
MODE_16PWM,
@@ -111,23 +114,29 @@ public:
int _task;
private:
static const unsigned _max_actuators = 4;
static const unsigned _max_actuators = 8;
Mode _mode;
int _update_rate;
int _current_update_rate;
int _t_actuators;
int _t_armed;
orb_advert_t _t_outputs;
int _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
px4_pollfd_struct_t _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
unsigned _poll_fds_num;
int _armed_sub;
orb_advert_t _outputs_pub;
unsigned _num_outputs;
bool _primary_pwm_device;
uint32_t _groups_required;
uint32_t _groups_subscribed;
volatile bool _task_should_exit;
static bool _armed;
MixerGroup *_mixers;
actuator_controls_s _controls;
actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
orb_id_t _control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
static void task_main_trampoline(int argc, char *argv[]);
void task_main();
@@ -138,6 +147,7 @@ private:
float &input);
int pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg);
void subscribe();
struct GPIOConfig {
uint32_t input;
@@ -176,15 +186,24 @@ PWMSim::PWMSim() :
_mode(MODE_NONE),
_update_rate(50),
_current_update_rate(0),
_t_actuators(-1),
_t_armed(-1),
_t_outputs(0),
_control_subs { -1},
_poll_fds_num(0),
_armed_sub(-1),
_outputs_pub(0),
_num_outputs(0),
_primary_pwm_device(false),
_groups_required(0),
_groups_subscribed(0),
_task_should_exit(false),
_mixers(nullptr)
{
_debug_enabled = true;
memset(_controls, 0, sizeof(_controls));
_control_topics[0] = ORB_ID(actuator_controls_0);
_control_topics[1] = ORB_ID(actuator_controls_1);
_control_topics[2] = ORB_ID(actuator_controls_2);
_control_topics[3] = ORB_ID(actuator_controls_3);
}
PWMSim::~PWMSim()
@@ -326,64 +345,62 @@ PWMSim::set_pwm_rate(unsigned rate)
return OK;
}
void
PWMSim::subscribe()
{
/* subscribe/unsubscribe to required actuator control groups */
uint32_t sub_groups = _groups_required & ~_groups_subscribed;
uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
_poll_fds_num = 0;
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (sub_groups & (1 << i)) {
PX4_DEBUG("subscribe to actuator_controls_%d", i);
_control_subs[i] = orb_subscribe(_control_topics[i]);
}
if (unsub_groups & (1 << i)) {
PX4_DEBUG("unsubscribe from actuator_controls_%d", i);
px4_close(_control_subs[i]);
_control_subs[i] = -1;
}
if (_control_subs[i] > 0) {
printf("valid\n");
_poll_fds[_poll_fds_num].fd = _control_subs[i];
_poll_fds[_poll_fds_num].events = POLLIN;
_poll_fds_num++;
}
}
}
void
PWMSim::task_main()
{
/*
* Subscribe to the appropriate PWM output topic based on whether we are the
* primary PWM output or not.
*/
_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
ORB_ID(actuator_controls_1));
/* force a reset of the update rate */
_current_update_rate = 0;
_t_armed = orb_subscribe(ORB_ID(actuator_armed));
orb_set_interval(_t_armed, 200); /* 5Hz update rate */
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
orb_set_interval(_armed_sub, 200); /* 5Hz update rate */
/* advertise the mixed control outputs */
actuator_outputs_s outputs;
memset(&outputs, 0, sizeof(outputs));
/* advertise the mixed control outputs, insist on the first group output */
_t_outputs = orb_advertise(ORB_ID(actuator_outputs), &outputs);
_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &outputs);
px4_pollfd_struct_t fds[2];
fds[0].fd = _t_actuators;
fds[0].events = POLLIN;
fds[1].fd = _t_armed;
fds[1].events = POLLIN;
unsigned num_outputs;
/* select the number of virtual outputs */
switch (_mode) {
case MODE_2PWM:
num_outputs = 2;
break;
case MODE_4PWM:
num_outputs = 4;
break;
case MODE_8PWM:
case MODE_12PWM:
case MODE_16PWM:
// XXX only support the lower 8 - trivial to extend
num_outputs = 8;
break;
case MODE_NONE:
default:
num_outputs = 0;
break;
}
DEVICE_LOG("starting");
/* loop until killed */
while (!_task_should_exit) {
if (_groups_subscribed != _groups_required) {
subscribe();
_groups_subscribed = _groups_required;
}
/* handle update rate changes */
if (_current_update_rate != _update_rate) {
int update_rate_in_ms = int(1000 / _update_rate);
@@ -392,13 +409,18 @@ PWMSim::task_main()
update_rate_in_ms = 2;
}
orb_set_interval(_t_actuators, update_rate_in_ms);
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
orb_set_interval(_control_subs[i], update_rate_in_ms);
}
}
// up_pwm_servo_set_rate(_update_rate);
_current_update_rate = _update_rate;
}
/* sleep waiting for data, but no more than a second */
int ret = px4_poll(&fds[0], 2, 1000);
int ret = px4_poll(&_poll_fds[0], _poll_fds_num, 1000);
/* this would be bad... */
if (ret < 0) {
@@ -406,56 +428,107 @@ PWMSim::task_main()
continue;
}
/* do we have a control update? */
if (fds[0].revents & POLLIN) {
/* get controls - must always do this to avoid spinning */
orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
ORB_ID(actuator_controls_1), _t_actuators, &_controls);
if (ret == 0) {
// timeout
continue;
}
/* can we mix? */
if (_armed && _mixers != nullptr) {
/* do mixing */
outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs, NULL);
outputs.timestamp = hrt_absolute_time();
/* get controls for required topics */
unsigned poll_id = 0;
/* iterate actuators */
for (unsigned i = 0; i < num_outputs; i++) {
/* last resort: catch NaN, INF and out-of-band errors */
if (i < outputs.noutputs &&
PX4_ISFINITE(outputs.output[i]) &&
outputs.output[i] >= -1.0f &&
outputs.output[i] <= 1.0f) {
/* scale for PWM output 900 - 2100us */
outputs.output[i] = 1500 + (600 * outputs.output[i]);
} else {
/*
* Value is NaN, INF or out of band - set to the minimum value.
* This will be clearly visible on the servo status and will limit the risk of accidentally
* spinning motors. It would be deadly in flight.
*/
outputs.output[i] = 900;
}
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
if (_poll_fds[poll_id].revents & POLLIN) {
orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
}
/* and publish for anyone that cares to see */
orb_publish(ORB_ID(actuator_outputs), _t_outputs, &outputs);
poll_id++;
}
}
/* how about an arming update? */
if (fds[1].revents & POLLIN) {
actuator_armed_s aa;
/* can we mix? */
if (_armed && _mixers != nullptr) {
/* get new value */
orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
size_t num_outputs;
switch (_mode) {
case MODE_2PWM:
num_outputs = 2;
break;
case MODE_4PWM:
num_outputs = 4;
break;
case MODE_6PWM:
num_outputs = 6;
break;
case MODE_8PWM:
num_outputs = 8;
break;
default:
num_outputs = 0;
break;
}
/* do mixing */
actuator_outputs_s outputs;
num_outputs = _mixers->mix(&outputs.output[0], num_outputs, NULL);
outputs.timestamp = hrt_absolute_time();
/* disable unused ports by setting their output to NaN */
for (size_t i = 0; i < sizeof(outputs.output) / sizeof(outputs.output[0]); i++) {
if (i >= num_outputs) {
outputs.output[i] = NAN;
}
}
/* iterate actuators */
for (unsigned i = 0; i < num_outputs; i++) {
/* last resort: catch NaN, INF and out-of-band errors */
if (i < outputs.noutputs &&
PX4_ISFINITE(outputs.output[i]) &&
outputs.output[i] >= -1.0f &&
outputs.output[i] <= 1.0f) {
/* scale for PWM output 900 - 2100us */
outputs.output[i] = 1500 + (600 * outputs.output[i]);
} else {
/*
* Value is NaN, INF or out of band - set to the minimum value.
* This will be clearly visible on the servo status and will limit the risk of accidentally
* spinning motors. It would be deadly in flight.
*/
outputs.output[i] = 900;
}
}
/* and publish for anyone that cares to see */
orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs);
}
/* how about an arming update? */
bool updated;
actuator_armed_s aa;
orb_check(_armed_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_armed), _armed_sub, &aa);
/* do not obey the lockdown value, as lockdown is for PWMSim */
_armed = aa.armed;
}
}
px4_close(_t_actuators);
px4_close(_t_armed);
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
px4_close(_control_subs[i]);
}
}
px4_close(_armed_sub);
/* make sure servos are off */
// up_pwm_servo_deinit();
@@ -671,6 +744,7 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
if (_mixers != nullptr) {
delete _mixers;
_mixers = nullptr;
_groups_required = 0;
}
break;
@@ -683,6 +757,7 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
if (mixer->check()) {
delete mixer;
_groups_required = 0;
ret = -EINVAL;
} else {
@@ -691,6 +766,7 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
(uintptr_t)&_controls);
_mixers->add_mixer(mixer);
_mixers->groups_required(_groups_required);
}
break;
@@ -705,6 +781,7 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
}
if (_mixers == nullptr) {
_groups_required = 0;
ret = -ENOMEM;
} else {
@@ -715,7 +792,11 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
DEVICE_DEBUG("mixer load failed with %d", ret);
delete _mixers;
_mixers = nullptr;
_groups_required = 0;
ret = -EINVAL;
} else {
_mixers->groups_required(_groups_required);
}
}
@@ -772,7 +853,7 @@ hil_new_mode(PortMode new_mode)
case PORT1_FULL_PWM:
/* select 4-pin PWM mode */
servo_mode = PWMSim::MODE_4PWM;
servo_mode = PWMSim::MODE_8PWM;
break;
case PORT1_PWM_AND_SERIAL:
src/drivers/px4flow/px4flow.cpp
+9 -11
View File
@@ -229,34 +229,32 @@ PX4FLOW::init()
/* do I2C init (and probe) first */
if (I2C::init() != OK) {
goto out;
return ret;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(optical_flow_s));
if (_reports == nullptr) {
goto out;
return ret;
}
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* get a publish handle on the range finder topic */
struct distance_sensor_s ds_report = {};
/* get a publish handle on the range finder topic */
struct distance_sensor_s ds_report = {};
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
&_orb_class_instance, ORB_PRIO_HIGH);
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
&_orb_class_instance, ORB_PRIO_HIGH);
if (_distance_sensor_topic == nullptr) {
DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
}
if (_distance_sensor_topic == nullptr) {
DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
}
ret = OK;
/* sensor is ok, but we don't really know if it is within range */
_sensor_ok = true;
out:
return ret;
}
src/drivers/px4io/px4io.cpp
+14 -17
View File
@@ -2090,12 +2090,12 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
} while (buflen > 0);
/* ensure a closing newline */
int ret;
/* send the closing newline */
msg->text[0] = '\n';
msg->text[1] = '\0';
int ret;
for (int i = 0; i < 30; i++) {
/* failed, but give it a 2nd shot */
ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2);
@@ -2108,27 +2108,24 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
}
}
if (ret) {
return ret;
if (ret == 0) {
/* success, exit */
break;
}
retries--;
DEVICE_LOG("mixer sent");
} while (retries > 0);
} while (retries > 0 && (!(io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK)));
if (retries == 0) {
mavlink_and_console_log_info(_mavlink_fd, "[IO] mixer upload fail");
/* load must have failed for some reason */
return -EINVAL;
/* check for the mixer-OK flag */
if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK) {
mavlink_log_info(_mavlink_fd, "[IO] mixer upload ok");
return 0;
} else {
/* all went well, set the mixer ok flag */
return io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_MIXER_OK);
}
DEVICE_LOG("mixer rejected by IO");
mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail");
/* load must have failed for some reason */
return -EINVAL;
}
void
src/drivers/sf0x/sf0x.cpp
+6 -8
View File
@@ -296,16 +296,14 @@ SF0X::init()
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* get a publish handle on the range finder topic */
struct distance_sensor_s ds_report = {};
/* get a publish handle on the range finder topic */
struct distance_sensor_s ds_report = {};
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
&_orb_class_instance, ORB_PRIO_HIGH);
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
&_orb_class_instance, ORB_PRIO_HIGH);
if (_distance_sensor_topic == nullptr) {
DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
}
if (_distance_sensor_topic == nullptr) {
DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
}
} while (0);
src/drivers/srf02/srf02.cpp
+9 -11
View File
@@ -254,7 +254,7 @@ SRF02::init()
/* do I2C init (and probe) first */
if (I2C::init() != OK) {
goto out;
return ret;
}
/* allocate basic report buffers */
@@ -264,21 +264,19 @@ SRF02::init()
set_address(_index_counter); /* set I2c port to temp sonar i2c adress */
if (_reports == nullptr) {
goto out;
return ret;
}
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* get a publish handle on the range finder topic */
struct distance_sensor_s ds_report = {};
/* get a publish handle on the range finder topic */
struct distance_sensor_s ds_report = {};
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
&_orb_class_instance, ORB_PRIO_LOW);
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
&_orb_class_instance, ORB_PRIO_LOW);
if (_distance_sensor_topic == nullptr) {
DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
}
if (_distance_sensor_topic == nullptr) {
DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
}
// XXX we should find out why we need to wait 200 ms here
@@ -320,7 +318,7 @@ SRF02::init()
ret = OK;
/* sensor is ok, but we don't really know if it is within range */
_sensor_ok = true;
out:
return ret;
}
src/firmware/posix/CMakeLists.txt
+26 -16
View File
@@ -26,27 +26,37 @@ endif()
add_custom_target(run_config
COMMAND Tools/sitl_run.sh "${config_sitl_rcS}" "${config_sitl_debugger}"
"${config_sitl_viewer}" "${CMAKE_BINARY_DIR}"
"${config_sitl_viewer}" "${config_sitl_model}" "${CMAKE_BINARY_DIR}"
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
USES_TERMINAL
)
add_dependencies(run_config mainapp)
foreach(viewer jmavsim gazebo)
foreach(debugger none gdb lldb)
if (debugger STREQUAL "none")
set(_targ_name "${viewer}")
else()
set(_targ_name "${viewer}_${debugger}")
endif()
add_custom_target(${_targ_name}
COMMAND Tools/sitl_run.sh "${config_sitl_rcS}"
"${debugger}"
"${viewer}" "${CMAKE_BINARY_DIR}"
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
USES_TERMINAL
)
add_dependencies(${_targ_name} mainapp)
foreach(viewer none jmavsim gazebo)
foreach(debugger none gdb lldb ddd valgrind)
foreach(model none iris vtol)
if (debugger STREQUAL "none")
if (model STREQUAL "none")
set(_targ_name "${viewer}")
else()
set(_targ_name "${viewer}_${model}")
endif()
else()
if (model STREQUAL "none")
set(_targ_name "${viewer}___${debugger}")
else()
set(_targ_name "${viewer}_${model}_${debugger}")
endif()
endif()
add_custom_target(${_targ_name}
COMMAND Tools/sitl_run.sh "${config_sitl_rcS}"
"${debugger}"
"${viewer}" "${model}" "${CMAKE_BINARY_DIR}"
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
USES_TERMINAL
)
add_dependencies(${_targ_name} mainapp)
endforeach()
endforeach()
endforeach()
src/lib/external_lgpl/tecs/tecs.h
+3
View File
@@ -118,6 +118,9 @@ public:
return get_throttle_demand();
}
void reset_state() {
_states_initalized = false;
}
float get_pitch_demand() { return _pitch_dem; }
src/lib/launchdetection/CMakeLists.txt
-1
View File
@@ -37,7 +37,6 @@ px4_add_module(
SRCS
LaunchDetector.cpp
CatapultLaunchMethod.cpp
launchdetection_params.c
DEPENDS
platforms__common
)
src/lib/launchdetection/launchdetection_params.c
-4
View File
@@ -39,10 +39,6 @@
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <px4_config.h>
#include <systemlib/param/param.h>
/*
* Catapult launch detection parameters, accessible via MAVLink
*
src/lib/mathlib/math/Matrix.hpp
+3 -4
View File
@@ -49,7 +49,7 @@
#ifdef CONFIG_ARCH_ARM
#include "../CMSIS/Include/arm_math.h"
#else
#include "modules/local_position_estimator/matrix/src/Matrix.hpp"
#include "matrix/math.hpp"
#endif
#include <platforms/px4_defines.h>
@@ -339,9 +339,8 @@ public:
arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat);
return res;
#else
matrix::Matrix<float, M, N> Me(this->arm_mat.pData);
matrix::Matrix<float, M, N> MyInverse = Me.inverse(); //not sure if A = A.inverse() is a good idea
Matrix<M, N> res(MyInverse.data());
matrix::SquareMatrix<float, M> Me = matrix::Matrix<float, M, N>(this->arm_mat.pData);
Matrix<M, N> res(Me.I().data());
return res;
#endif
}
src/lib/mathlib/math/test/test.cpp
+40
View File
@@ -54,6 +54,46 @@ bool __EXPORT equal(float a, float b, float epsilon)
} else { return true; }
}
bool __EXPORT greater_than(float a, float b)
{
if (a > b) {
return true;
} else {
printf("not a > b ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b));
return false;
}
}
bool __EXPORT less_than(float a, float b)
{
if (a < b) {
return true;
} else {
printf("not a < b ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b));
return false;
}
}
bool __EXPORT greater_than_or_equal(float a, float b)
{
if (a >= b) {
return true;
} else {
printf("not a >= b ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b));
return false;
}
}
bool __EXPORT less_than_or_equal(float a, float b)
{
if (a <= b) {
return true;
} else {
printf("not a <= b ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b));
return false;
}
}
void __EXPORT float2SigExp(
const float &num,
float &sig,
src/lib/mathlib/math/test/test.hpp
+9
View File
@@ -44,6 +44,15 @@
//#include <stdlib.h>
bool equal(float a, float b, float eps = 1e-5);
bool greater_than(float a, float b);
bool less_than(float a, float b);
bool greater_than_or_equal(float a, float b);
bool less_than_or_equal(float a, float b);
void float2SigExp(
const float &num,
float &sig,
src/lib/matrix
Submodule
+1
Submodule src/lib/matrix added at 2c7a375e3d
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+126 -13
View File
@@ -54,8 +54,11 @@
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vision_position_estimate.h>
#include <uORB/topics/att_pos_mocap.h>
#include <uORB/topics/parameter_update.h>
#include <drivers/drv_hrt.h>
@@ -117,22 +120,28 @@ private:
int _sensors_sub = -1;
int _params_sub = -1;
int _vision_sub = -1;
int _mocap_sub = -1;
int _global_pos_sub = -1;
orb_advert_t _att_pub = nullptr;
orb_advert_t _ctrl_state_pub = nullptr;
struct {
param_t w_acc;
param_t w_mag;
param_t w_ext_hdg;
param_t w_gyro_bias;
param_t mag_decl;
param_t mag_decl_auto;
param_t acc_comp;
param_t bias_max;
param_t vibe_thresh;
param_t ext_hdg_mode;
} _params_handles; /**< handles for interesting parameters */
float _w_accel = 0.0f;
float _w_mag = 0.0f;
float _w_ext_hdg = 0.0f;
float _w_gyro_bias = 0.0f;
float _mag_decl = 0.0f;
bool _mag_decl_auto = false;
@@ -140,11 +149,18 @@ private:
float _bias_max = 0.0f;
float _vibration_warning_threshold = 1.0f;
hrt_abstime _vibration_warning_timestamp = 0;
int _ext_hdg_mode = 0;
Vector<3> _gyro;
Vector<3> _accel;
Vector<3> _mag;
vision_position_estimate_s _vision = {};
Vector<3> _vision_hdg;
att_pos_mocap_s _mocap = {};
Vector<3> _mocap_hdg;
Quaternion _q;
Vector<3> _rates;
Vector<3> _gyro_bias;
@@ -168,6 +184,7 @@ private:
bool _data_good = false;
bool _failsafe = false;
bool _vibration_warning = false;
bool _ext_hdg_good = false;
int _mavlink_fd = -1;
@@ -190,18 +207,20 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() :
_voter_mag(3),
_lp_roll_rate(250.0f, 18.0f),
_lp_pitch_rate(250.0f, 18.0f),
_lp_yaw_rate(250, 10.0f)
_lp_yaw_rate(250.0f, 10.0f)
{
_voter_mag.set_timeout(200000);
_params_handles.w_acc = param_find("ATT_W_ACC");
_params_handles.w_mag = param_find("ATT_W_MAG");
_params_handles.w_ext_hdg = param_find("ATT_W_EXT_HDG");
_params_handles.w_gyro_bias = param_find("ATT_W_GYRO_BIAS");
_params_handles.mag_decl = param_find("ATT_MAG_DECL");
_params_handles.mag_decl_auto = param_find("ATT_MAG_DECL_A");
_params_handles.acc_comp = param_find("ATT_ACC_COMP");
_params_handles.bias_max = param_find("ATT_BIAS_MAX");
_params_handles.vibe_thresh = param_find("ATT_VIBE_THRESH");
_params_handles.ext_hdg_mode = param_find("ATT_EXT_HDG_M");
}
/**
@@ -269,6 +288,10 @@ void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[])
void AttitudeEstimatorQ::task_main()
{
_sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
_vision_sub = orb_subscribe(ORB_ID(vision_position_estimate));
_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
@@ -374,6 +397,47 @@ void AttitudeEstimatorQ::task_main()
}
}
// Update vision and motion capture heading
bool vision_updated = false;
orb_check(_vision_sub, &vision_updated);
bool mocap_updated = false;
orb_check(_mocap_sub, &mocap_updated);
if (vision_updated) {
orb_copy(ORB_ID(vision_position_estimate), _vision_sub, &_vision);
math::Quaternion q(_vision.q);
math::Matrix<3, 3> Rvis = q.to_dcm();
math::Vector<3> v(1.0f, 0.0f, 0.4f);
// Rvis is Rwr (robot respect to world) while v is respect to world.
// Hence Rvis must be transposed having (Rwr)' * Vw
// Rrw * Vw = vn. This way we have consistency
_vision_hdg = Rvis.transposed() * v;
}
if (mocap_updated) {
orb_copy(ORB_ID(att_pos_mocap), _mocap_sub, &_mocap);
math::Quaternion q(_mocap.q);
math::Matrix<3, 3> Rmoc = q.to_dcm();
math::Vector<3> v(1.0f, 0.0f, 0.4f);
// Rmoc is Rwr (robot respect to world) while v is respect to world.
// Hence Rmoc must be transposed having (Rwr)' * Vw
// Rrw * Vw = vn. This way we have consistency
_mocap_hdg = Rmoc.transposed() * v;
}
// Check for timeouts on data
if (_ext_hdg_mode == 1) {
_ext_hdg_good = _vision.timestamp_boot > 0 && (hrt_elapsed_time(&_vision.timestamp_boot) < 500000);
} else if (_ext_hdg_mode == 2) {
_ext_hdg_good = _mocap.timestamp_boot > 0 && (hrt_elapsed_time(&_mocap.timestamp_boot) < 500000);
}
bool gpos_updated;
orb_check(_global_pos_sub, &gpos_updated);
@@ -431,12 +495,9 @@ void AttitudeEstimatorQ::task_main()
att.pitch = euler(1);
att.yaw = euler(2);
/* the complimentary filter should reflect the true system
* state, but we need smoother outputs for the control system
*/
att.rollspeed = _lp_roll_rate.apply(_rates(0));
att.pitchspeed = _lp_pitch_rate.apply(_rates(1));
att.yawspeed = _lp_yaw_rate.apply(_rates(2));
att.rollspeed = _rates(0);
att.pitchspeed = _rates(1);
att.yawspeed = _rates(2);
for (int i = 0; i < 3; i++) {
att.g_comp[i] = _accel(i) - _pos_acc(i);
@@ -461,6 +522,34 @@ void AttitudeEstimatorQ::task_main()
} else {
orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att);
}
struct control_state_s ctrl_state = {};
ctrl_state.timestamp = sensors.timestamp;
/* 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 */
ctrl_state.roll_rate = _lp_roll_rate.apply(_rates(0));
ctrl_state.pitch_rate = _lp_pitch_rate.apply(_rates(1));
ctrl_state.yaw_rate = _lp_yaw_rate.apply(_rates(2));
/* 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);
}
}
}
@@ -478,6 +567,7 @@ void AttitudeEstimatorQ::update_parameters(bool force)
param_get(_params_handles.w_acc, &_w_accel);
param_get(_params_handles.w_mag, &_w_mag);
param_get(_params_handles.w_ext_hdg, &_w_ext_hdg);
param_get(_params_handles.w_gyro_bias, &_w_gyro_bias);
float mag_decl_deg = 0.0f;
param_get(_params_handles.mag_decl, &mag_decl_deg);
@@ -490,6 +580,7 @@ void AttitudeEstimatorQ::update_parameters(bool force)
_acc_comp = acc_comp_int != 0;
param_get(_params_handles.bias_max, &_bias_max);
param_get(_params_handles.vibe_thresh, &_vibration_warning_threshold);
param_get(_params_handles.ext_hdg_mode, &_ext_hdg_mode);
}
}
@@ -545,12 +636,34 @@ bool AttitudeEstimatorQ::update(float dt)
// Angular rate of correction
Vector<3> corr;
// Magnetometer correction
// Project mag field vector to global frame and extract XY component
Vector<3> mag_earth = _q.conjugate(_mag);
float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);
// Project magnetometer correction to body frame
corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag;
if (_ext_hdg_mode > 0 && _ext_hdg_good) {
if (_ext_hdg_mode == 1) {
// Vision heading correction
// Project heading to global frame and extract XY component
Vector<3> vision_hdg_earth = _q.conjugate(_vision_hdg);
float vision_hdg_err = _wrap_pi(atan2f(vision_hdg_earth(1), vision_hdg_earth(0)));
// Project correction to body frame
corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -vision_hdg_err)) * _w_ext_hdg;
}
if (_ext_hdg_mode == 2) {
// Mocap heading correction
// Project heading to global frame and extract XY component
Vector<3> mocap_hdg_earth = _q.conjugate(_mocap_hdg);
float mocap_hdg_err = _wrap_pi(atan2f(mocap_hdg_earth(1), mocap_hdg_earth(0)));
// Project correction to body frame
corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mocap_hdg_err)) * _w_ext_hdg;
}
}
if (_ext_hdg_mode == 0 || !_ext_hdg_good) {
// Magnetometer correction
// Project mag field vector to global frame and extract XY component
Vector<3> mag_earth = _q.conjugate(_mag);
float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);
// Project magnetometer correction to body frame
corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag;
}
// Accelerometer correction
// Project 'k' unit vector of earth frame to body frame
src/modules/attitude_estimator_q/attitude_estimator_q_params.c
+20
View File
@@ -61,6 +61,15 @@ PARAM_DEFINE_FLOAT(ATT_W_ACC, 0.2f);
*/
PARAM_DEFINE_FLOAT(ATT_W_MAG, 0.1f);
/**
* Complimentary filter external heading weight
*
* @group Attitude Q estimator
* @min 0
* @max 1
*/
PARAM_DEFINE_FLOAT(ATT_W_EXT_HDG, 0.1f);
/**
* Complimentary filter gyroscope bias weight
*
@@ -93,6 +102,17 @@ PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);
*/
PARAM_DEFINE_INT32(ATT_MAG_DECL_A, 1);
/**
* External heading usage mode (from Motion capture/Vision)
* Set to 1 to use heading estimate from vision.
* Set to 2 to use heading from motion capture.
*
* @group Attitude Q estimator
* @min 0
* @max 2
*/
PARAM_DEFINE_INT32(ATT_EXT_HDG_M, 0);
/**
* Enable acceleration compensation based on GPS
* velocity.
src/modules/commander/CMakeLists.txt
+1 -1
View File
@@ -32,7 +32,7 @@
############################################################################
set(MODULE_CFLAGS -Os)
if(${OS} STREQUAL "nuttx")
list(APPEND MODULE_CFLAGS -Wframe-larger-than=2300)
list(APPEND MODULE_CFLAGS -Wframe-larger-than=2450)
endif()
px4_add_module(
MODULE modules__commander
src/modules/commander/commander.cpp
+249 -65
View File
@@ -72,6 +72,7 @@
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_command.h>
@@ -92,6 +93,7 @@
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/vtol_vehicle_status.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/input_rc.h>
#include <drivers/drv_led.h>
#include <drivers/drv_hrt.h>
@@ -149,7 +151,7 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage
#define PRINT_INTERVAL 5000000
#define PRINT_MODE_REJECT_INTERVAL 10000000
#define INAIR_RESTART_HOLDOFF_INTERVAL 2000000
#define INAIR_RESTART_HOLDOFF_INTERVAL 1000000
#define HIL_ID_MIN 1000
#define HIL_ID_MAX 1999
@@ -178,11 +180,13 @@ static volatile bool thread_should_exit = false; /**< daemon exit flag */
static volatile bool thread_running = false; /**< daemon status flag */
static int daemon_task; /**< Handle of daemon task / thread */
static bool need_param_autosave = false; /**< Flag set to true if parameters should be autosaved in next iteration (happens on param update and if functionality is enabled) */
static bool _usb_telemetry_active = false;
static hrt_abstime commander_boot_timestamp = 0;
static unsigned int leds_counter;
/* To remember when last notification was sent */
static uint64_t last_print_mode_reject_time = 0;
static uint64_t _inair_last_time = 0;
static float eph_threshold = 5.0f;
static float epv_threshold = 10.0f;
@@ -195,15 +199,9 @@ static struct offboard_control_mode_s offboard_control_mode;
static struct home_position_s _home;
static unsigned _last_mission_instance = 0;
static uint64_t last_manual_input = 0;
static switch_pos_t last_offboard_switch = 0;
static switch_pos_t last_return_switch = 0;
static switch_pos_t last_mode_switch = 0;
static switch_pos_t last_acro_switch = 0;
static switch_pos_t last_posctl_switch = 0;
static switch_pos_t last_loiter_switch = 0;
static manual_control_setpoint_s _last_sp_man;
struct vtol_vehicle_status_s vtol_status;
static struct vtol_vehicle_status_s vtol_status;
/**
* The daemon app only briefly exists to start
@@ -227,7 +225,7 @@ 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, orb_advert_t *home_pub);
struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub);
/**
* Mainloop of commander.
@@ -262,7 +260,8 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armed
* time the vehicle is armed with a good GPS fix.
**/
static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home,
const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition);
const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition,
const vehicle_attitude_s &attitude);
/**
* Loop that runs at a lower rate and priority for calibration and parameter tasks.
@@ -300,7 +299,7 @@ int commander_main(int argc, char *argv[])
daemon_task = px4_task_spawn_cmd("commander",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 40,
3400,
3500,
commander_thread_main,
(char * const *)&argv[0]);
@@ -405,11 +404,11 @@ int commander_main(int argc, char *argv[])
void usage(const char *reason)
{
if (reason) {
PX4_INFO("%s\n", reason);
if (reason && *reason > 0) {
PX4_INFO("%s", reason);
}
PX4_INFO("usage: commander {start|stop|status|calibrate|check|arm|disarm}\n\n");
PX4_INFO("usage: commander {start|stop|status|calibrate|check|arm|disarm}\n");
}
void print_status()
@@ -499,7 +498,7 @@ 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, orb_advert_t *home_pub)
struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub)
{
/* 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)
@@ -533,7 +532,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
if (cmd_arm && (arming_ret == TRANSITION_CHANGED) &&
(hrt_absolute_time() > (commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL))) {
commander_set_home_position(*home_pub, *home, *local_pos, *global_pos);
commander_set_home_position(*home_pub, *home, *local_pos, *global_pos, *attitude);
}
if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
@@ -842,7 +841,8 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
* time the vehicle is armed with a good GPS fix.
**/
static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home,
const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition)
const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition,
const vehicle_attitude_s &attitude)
{
//Need global position fix to be able to set home
if (!status.condition_global_position_valid) {
@@ -864,6 +864,8 @@ static void commander_set_home_position(orb_advert_t &homePub, home_position_s &
home.y = localPosition.y;
home.z = localPosition.z;
home.yaw = attitude.yaw;
warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt);
mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
@@ -890,6 +892,7 @@ int commander_thread_main(int argc, char *argv[])
commander_initialized = false;
bool arm_tune_played = false;
bool was_landed = true;
bool was_armed = false;
bool startup_in_hil = false;
@@ -925,6 +928,9 @@ int commander_thread_main(int argc, char *argv[])
param_t _param_rc_in_off = param_find("COM_RC_IN_MODE");
param_t _param_eph = param_find("COM_HOME_H_T");
param_t _param_epv = param_find("COM_HOME_V_T");
param_t _param_geofence_action = param_find("GF_ACTION");
param_t _param_disarm_land = param_find("COM_DISARM_LAND");
param_t _param_map_mode_sw = param_find("RC_MAP_MODE_SW");
// const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX];
// main_states_str[vehicle_status_s::MAIN_STATE_MANUAL] = "MANUAL";
@@ -949,6 +955,7 @@ int commander_thread_main(int argc, char *argv[])
const char *nav_states_str[vehicle_status_s::NAVIGATION_STATE_MAX];
nav_states_str[vehicle_status_s::NAVIGATION_STATE_MANUAL] = "MANUAL";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_STAB] = "STAB";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_RATTITUDE] = "RATTITUDE";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_ALTCTL] = "ALTCTL";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_POSCTL] = "POSCTL";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION";
@@ -1037,17 +1044,15 @@ int commander_thread_main(int argc, char *argv[])
px4_task_exit(ERROR);
}
/* armed topic */
orb_advert_t armed_pub;
/* Initialize armed with all false */
memset(&armed, 0, sizeof(armed));
/* armed topic */
orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
/* vehicle control mode topic */
memset(&control_mode, 0, sizeof(control_mode));
orb_advert_t control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode);
armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
/* home position */
orb_advert_t home_pub = nullptr;
memset(&_home, 0, sizeof(_home));
@@ -1146,13 +1151,15 @@ int commander_thread_main(int argc, char *argv[])
/* Subscribe to local position data */
int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
struct vehicle_local_position_s local_position;
memset(&local_position, 0, sizeof(local_position));
struct vehicle_local_position_s local_position = {};
/* Subscribe to attitude data */
int attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
struct vehicle_attitude_s attitude = {};
/* Subscribe to land detector */
int land_detector_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
struct vehicle_land_detected_s land_detector;
memset(&land_detector, 0, sizeof(land_detector));
struct vehicle_land_detected_s land_detector = {};
/*
* The home position is set based on GPS only, to prevent a dependency between
@@ -1263,6 +1270,8 @@ int commander_thread_main(int argc, char *argv[])
float rc_loss_timeout = 0.5;
int32_t datalink_regain_timeout = 0;
int32_t geofence_action = 0;
/* Thresholds for engine failure detection */
int32_t ef_throttle_thres = 1.0f;
int32_t ef_current2throttle_thres = 0.0f;
@@ -1271,6 +1280,9 @@ int commander_thread_main(int argc, char *argv[])
int autosave_params; /**< Autosave of parameters enabled/disabled, loaded from parameter */
int32_t disarm_when_landed = 0;
int32_t map_mode_sw = 0;
/* check which state machines for changes, clear "changed" flag */
bool arming_state_changed = false;
bool main_state_changed = false;
@@ -1335,6 +1347,16 @@ int commander_thread_main(int argc, char *argv[])
status_changed = true;
// check if main mode switch has been assigned and if so run preflight checks in order
// to update status.condition_sensors_initialised
int32_t map_mode_sw_new;
param_get(_param_map_mode_sw, &map_mode_sw_new);
if (map_mode_sw == 0 && map_mode_sw != map_mode_sw_new && map_mode_sw_new < input_rc_s::RC_INPUT_MAX_CHANNELS && map_mode_sw_new > 0) {
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed,
!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check);
}
/* re-check RC calibration */
rc_calibration_check(mavlink_fd);
}
@@ -1349,6 +1371,9 @@ int commander_thread_main(int argc, char *argv[])
param_get(_param_ef_throttle_thres, &ef_throttle_thres);
param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres);
param_get(_param_ef_time_thres, &ef_time_thres);
param_get(_param_geofence_action, &geofence_action);
param_get(_param_disarm_land, &disarm_when_landed);
param_get(_param_map_mode_sw, &map_mode_sw);
/* Autostart id */
param_get(_param_autostart_id, &autostart_id);
@@ -1439,6 +1464,11 @@ int commander_thread_main(int argc, char *argv[])
}
}
/* set (and don't reset) telemetry via USB as active once a MAVLink connection is up */
if (telemetry.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB) {
_usb_telemetry_active = true;
}
telemetry_last_heartbeat[i] = telemetry.heartbeat_time;
}
}
@@ -1494,7 +1524,20 @@ int commander_thread_main(int argc, char *argv[])
/* copy avionics voltage */
status.avionics_power_rail_voltage = system_power.voltage5V_v;
status.usb_connected = system_power.usb_connected;
/* if the USB hardware connection went away, reboot */
if (status.usb_connected && !system_power.usb_connected) {
/*
* apparently the USB cable went away but we are still powered,
* so lets reset to a classic non-usb state.
*/
mavlink_log_critical(mavlink_fd, "USB disconnected, rebooting.")
usleep(400000);
px4_systemreset(false);
}
/* finally judge the USB connected state based on software detection */
status.usb_connected = _usb_telemetry_active;
}
}
@@ -1579,6 +1622,14 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
}
/* update attitude estimate */
orb_check(attitude_sub, &updated);
if (updated) {
/* position changed */
orb_copy(ORB_ID(vehicle_attitude), attitude_sub, &attitude);
}
//update condition_global_position_valid
//Global positions are only published by the estimators if they are valid
if (hrt_absolute_time() - global_position.timestamp > POSITION_TIMEOUT) {
@@ -1623,12 +1674,13 @@ int commander_thread_main(int argc, char *argv[])
&(status.condition_local_altitude_valid), &status_changed);
/* Update land detector */
static bool check_for_disarming = false;
orb_check(land_detector_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_land_detected), land_detector_sub, &land_detector);
}
if (updated && status.condition_local_altitude_valid) {
if ((updated && status.condition_local_altitude_valid) || check_for_disarming) {
if (status.condition_landed != land_detector.landed) {
status.condition_landed = land_detector.landed;
status_changed = true;
@@ -1640,6 +1692,24 @@ int commander_thread_main(int argc, char *argv[])
mavlink_log_critical(mavlink_fd, "TAKEOFF DETECTED");
}
}
if (disarm_when_landed > 0) {
if (land_detector.landed) {
if (!check_for_disarming && _inair_last_time > 0) {
_inair_last_time = land_detector.timestamp;
check_for_disarming = true;
}
if (_inair_last_time > 0 && ((hrt_absolute_time() - _inair_last_time) > (hrt_abstime)disarm_when_landed * 1000 * 1000)) {
mavlink_log_critical(mavlink_fd, "AUTO DISARMING AFTER LANDING");
arm_disarm(false, mavlink_fd, "auto disarm on land");
_inair_last_time = 0;
check_for_disarming = false;
}
} else {
_inair_last_time = land_detector.timestamp;
}
}
}
/* update battery status */
@@ -1838,24 +1908,106 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(geofence_result), geofence_result_sub, &geofence_result);
}
/* Check for geofence violation */
if (armed.armed && (geofence_result.geofence_violated || mission_result.flight_termination)) {
//XXX: make this configurable to select different actions (e.g. navigation modes)
/* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */
// Geofence actions
if (armed.armed && (geofence_result.geofence_action != geofence_result_s::GF_ACTION_NONE)) {
static bool geofence_loiter_on = false;
static bool geofence_rtl_on = false;
static uint8_t geofence_main_state_before_violation = vehicle_status_s::MAIN_STATE_MAX;
// check for geofence violation
if (geofence_result.geofence_violated) {
static hrt_abstime last_geofence_violation = 0;
const hrt_abstime geofence_violation_action_interval = 10000000; // 10 seconds
if (hrt_elapsed_time(&last_geofence_violation) > geofence_violation_action_interval) {
last_geofence_violation = hrt_absolute_time();
switch (geofence_result.geofence_action) {
case (geofence_result_s::GF_ACTION_NONE) : {
// do nothing
break;
}
case (geofence_result_s::GF_ACTION_WARN) : {
// do nothing, mavlink critical messages are sent by navigator
break;
}
case (geofence_result_s::GF_ACTION_LOITER) : {
if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_LOITER)) {
geofence_loiter_on = true;
}
break;
}
case (geofence_result_s::GF_ACTION_RTL) : {
if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_RTL)) {
geofence_rtl_on = true;
}
break;
}
case (geofence_result_s::GF_ACTION_TERMINATE) : {
warnx("Flight termination because of geofence");
mavlink_log_critical(mavlink_fd, "Geofence violation: flight termination");
armed.force_failsafe = true;
status_changed = true;
break;
}
}
}
}
// reset if no longer in LOITER or if manually switched to LOITER
geofence_loiter_on = geofence_loiter_on
&& (status.main_state == vehicle_status_s::MAIN_STATE_AUTO_LOITER)
&& (sp_man.loiter_switch == manual_control_setpoint_s::SWITCH_POS_OFF);
// reset if no longer in RTL or if manually switched to RTL
geofence_rtl_on = geofence_rtl_on
&& (status.main_state == vehicle_status_s::MAIN_STATE_AUTO_RTL)
&& (sp_man.return_switch == manual_control_setpoint_s::SWITCH_POS_OFF);
if (!geofence_loiter_on && !geofence_rtl_on) {
// store the last good main_state when not in a geofence triggered action (LOITER or RTL)
geofence_main_state_before_violation = status.main_state;
}
// revert geofence failsafe transition if sticks are moved and we were previously in MANUAL or ASSIST
if ((geofence_loiter_on || geofence_rtl_on) &&
(geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_MANUAL ||
geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_ALTCTL ||
geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_POSCTL ||
geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_ACRO ||
geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_STAB)) {
// transition to previous state if sticks are increased
const float min_stick_change = 0.2;
if ((_last_sp_man.timestamp != sp_man.timestamp) &&
((fabsf(sp_man.x) - fabsf(_last_sp_man.x) > min_stick_change) ||
(fabsf(sp_man.y) - fabsf(_last_sp_man.y) > min_stick_change) ||
(fabsf(sp_man.z) - fabsf(_last_sp_man.z) > min_stick_change) ||
(fabsf(sp_man.r) - fabsf(_last_sp_man.r) > min_stick_change))) {
main_state_transition(&status, geofence_main_state_before_violation);
}
}
}
/* Check for mission flight termination */
if (armed.armed && mission_result.flight_termination) {
armed.force_failsafe = true;
status_changed = true;
static bool flight_termination_printed = false;
if (!flight_termination_printed) {
warnx("Flight termination because of navigator request or geofence");
mavlink_log_critical(mavlink_fd, "Geofence violation: flight termination");
warnx("Flight termination because of navigator request");
flight_termination_printed = true;
}
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
mavlink_log_critical(mavlink_fd, "Flight termination active");
}
} // no reset is done here on purpose, on geofence violation we want to stay in flighttermination
}
/* Only evaluate mission state if home is set,
* this prevents false positives for the mission
@@ -1901,13 +2053,14 @@ int commander_thread_main(int argc, char *argv[])
status.rc_signal_lost = false;
/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSIST mode and landed) -> disarm
/* check if left stick is in lower left position and we are in MANUAL, Rattitude, or AUTO_READY mode or (ASSIST mode and landed) -> disarm
* do it only for rotary wings */
if (status.is_rotary_wing && status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF &&
(status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) &&
(status.main_state == vehicle_status_s::MAIN_STATE_MANUAL ||
status.main_state == vehicle_status_s::MAIN_STATE_ACRO ||
status.main_state == vehicle_status_s::MAIN_STATE_STAB ||
status.main_state == vehicle_status_s::MAIN_STATE_RATTITUDE ||
status.condition_landed) &&
sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
@@ -1940,10 +2093,15 @@ int commander_thread_main(int argc, char *argv[])
* for being in manual mode only applies to manual arming actions.
* the system can be armed in auto if armed via the GCS.
*/
if ((status.main_state != vehicle_status_s::MAIN_STATE_MANUAL) &&
(status.main_state != vehicle_status_s::MAIN_STATE_STAB)) {
print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
} else if (!status.condition_home_position_valid &&
geofence_action == geofence_result_s::GF_ACTION_RTL) {
print_reject_arm("NOT ARMING: Geofence RTL requires valid home");
} else if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */,
mavlink_fd);
@@ -2102,7 +2260,7 @@ 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, &home_pub)) {
if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &local_position, &attitude, &home_pub)) {
status_changed = true;
}
}
@@ -2110,10 +2268,11 @@ int commander_thread_main(int argc, char *argv[])
/* Check for failure combinations which lead to flight termination */
if (armed.armed) {
/* At this point the data link and the gps system have been checked
* If we are not in a manual (RC stick controlled mode)
* If we are not in a manual (RC stick controlled mode)
* and both failed we want to terminate the flight */
if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL &&
status.main_state !=vehicle_status_s::MAIN_STATE_ACRO &&
status.main_state !=vehicle_status_s::MAIN_STATE_RATTITUDE &&
status.main_state !=vehicle_status_s::MAIN_STATE_STAB &&
status.main_state !=vehicle_status_s::MAIN_STATE_ALTCTL &&
status.main_state !=vehicle_status_s::MAIN_STATE_POSCTL &&
@@ -2138,6 +2297,7 @@ int commander_thread_main(int argc, char *argv[])
* If we are in manual (controlled with RC):
* if both failed we want to terminate the flight */
if ((status.main_state ==vehicle_status_s::MAIN_STATE_ACRO ||
status.main_state ==vehicle_status_s::MAIN_STATE_RATTITUDE ||
status.main_state ==vehicle_status_s::MAIN_STATE_MANUAL ||
status.main_state !=vehicle_status_s::MAIN_STATE_STAB ||
status.main_state ==vehicle_status_s::MAIN_STATE_ALTCTL ||
@@ -2164,14 +2324,18 @@ int commander_thread_main(int argc, char *argv[])
/* First time home position update - but only if disarmed */
if (!status.condition_home_position_valid && !armed.armed) {
commander_set_home_position(home_pub, _home, local_position, global_position);
commander_set_home_position(home_pub, _home, local_position, global_position, attitude);
}
/* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */
else if (arming_state_changed && armed.armed && !was_armed && now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL) {
commander_set_home_position(home_pub, _home, local_position, global_position);
/* update home position on arming if at least 1s from commander start spent to avoid setting home on in-air restart */
else if (((!was_armed && armed.armed) || (was_landed && !status.condition_landed)) &&
(now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) {
commander_set_home_position(home_pub, _home, local_position, global_position, attitude);
}
was_landed = status.condition_landed;
was_armed = armed.armed;
/* print new state */
if (arming_state_changed) {
status_changed = true;
@@ -2179,8 +2343,6 @@ int commander_thread_main(int argc, char *argv[])
arming_state_changed = false;
}
was_armed = armed.armed;
/* now set navigation state according to failsafe and main state */
bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled,
mission_result.finished,
@@ -2374,7 +2536,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
} else if (status_local->battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) {
rgbled_set_color(RGBLED_COLOR_RED);
} else {
if (status_local->condition_home_position_valid) {
if (status_local->condition_home_position_valid && status_local->condition_global_position_valid) {
rgbled_set_color(RGBLED_COLOR_GREEN);
} else {
@@ -2425,31 +2587,33 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
/* set main state according to RC switches */
transition_result_t res = TRANSITION_DENIED;
/* if offboard is set allready by a mavlink command, abort */
/* if offboard is set already by a mavlink command, abort */
if (status.offboard_control_set_by_command) {
return main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD);
}
/* manual setpoint has not updated, do not re-evaluate it */
if ((last_manual_input == sp_man->timestamp) ||
((last_offboard_switch == sp_man->offboard_switch) &&
(last_return_switch == sp_man->return_switch) &&
(last_mode_switch == sp_man->mode_switch) &&
(last_acro_switch == sp_man->acro_switch) &&
(last_posctl_switch == sp_man->posctl_switch) &&
(last_loiter_switch == sp_man->loiter_switch))) {
if ((_last_sp_man.timestamp == sp_man->timestamp) ||
((_last_sp_man.offboard_switch == sp_man->offboard_switch) &&
(_last_sp_man.return_switch == sp_man->return_switch) &&
(_last_sp_man.mode_switch == sp_man->mode_switch) &&
(_last_sp_man.acro_switch == sp_man->acro_switch) &&
(_last_sp_man.rattitude_switch == sp_man->rattitude_switch) &&
(_last_sp_man.posctl_switch == sp_man->posctl_switch) &&
(_last_sp_man.loiter_switch == sp_man->loiter_switch))) {
// update these fields for the geofence system
_last_sp_man.timestamp = sp_man->timestamp;
_last_sp_man.x = sp_man->x;
_last_sp_man.y = sp_man->y;
_last_sp_man.z = sp_man->z;
_last_sp_man.r = sp_man->r;
/* no timestamp change or no switch change -> nothing changed */
return TRANSITION_NOT_CHANGED;
}
last_manual_input = sp_man->timestamp;
last_offboard_switch = sp_man->offboard_switch;
last_return_switch = sp_man->return_switch;
last_mode_switch = sp_man->mode_switch;
last_acro_switch = sp_man->acro_switch;
last_posctl_switch = sp_man->posctl_switch;
last_loiter_switch = sp_man->loiter_switch;
_last_sp_man = *sp_man;
/* offboard switch overrides main switch */
if (sp_man->offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
@@ -2502,7 +2666,16 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB);
}
} else {
}
else if(sp_man->rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON){
/* Similar to acro transitions for multirotors. FW aircraft don't need a
* rattitude mode.*/
if (status.is_rotary_wing) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_RATTITUDE);
} else {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB);
}
}else {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL);
}
@@ -2625,6 +2798,18 @@ set_control_mode()
control_mode.flag_external_manual_override_ok = false;
break;
case vehicle_status_s::NAVIGATION_STATE_RATTITUDE:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = false;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
control_mode.flag_control_termination_enabled = false;
break;
case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
@@ -2873,11 +3058,10 @@ void *commander_low_prio_loop(void *arg)
if (need_param_autosave_timeout > 0 && hrt_elapsed_time(&need_param_autosave_timeout) > 200000ULL) {
int ret = param_save_default();
if (ret == OK) {
mavlink_and_console_log_info(mavlink_fd, "settings autosaved");
if (ret != OK) {
mavlink_and_console_log_critical(mavlink_fd, "settings auto save error");
} else {
mavlink_and_console_log_critical(mavlink_fd, "settings save error");
warnx("settings saved.");
}
need_param_autosave = false;
src/modules/commander/commander_params.c
+13
View File
@@ -274,3 +274,16 @@ PARAM_DEFINE_INT32(COM_AUTOS_PAR, 1);
* @max 2
*/
PARAM_DEFINE_INT32(COM_RC_IN_MODE, 0);
/**
* Time-out for auto disarm after landing
*
* A non-zero, positive value specifies the time-out period in seconds after which the vehicle will be
* automatically disarmed in case a landing situation has been detected during this period.
* A value of zero means that automatic disarming is disabled.
*
* @group Commander
* @min 0
*/
PARAM_DEFINE_INT32(COM_DISARM_LAND, 0);
src/modules/commander/state_machine_helper.cpp
+14 -1
View File
@@ -304,6 +304,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
switch (new_main_state) {
case vehicle_status_s::MAIN_STATE_MANUAL:
case vehicle_status_s::MAIN_STATE_ACRO:
case vehicle_status_s::MAIN_STATE_RATTITUDE:
case vehicle_status_s::MAIN_STATE_STAB:
ret = TRANSITION_CHANGED;
break;
@@ -528,6 +529,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
switch (status->main_state) {
case vehicle_status_s::MAIN_STATE_ACRO:
case vehicle_status_s::MAIN_STATE_MANUAL:
case vehicle_status_s::MAIN_STATE_RATTITUDE:
case vehicle_status_s::MAIN_STATE_STAB:
case vehicle_status_s::MAIN_STATE_ALTCTL:
case vehicle_status_s::MAIN_STATE_POSCTL:
@@ -555,6 +557,10 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
break;
case vehicle_status_s::MAIN_STATE_RATTITUDE:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_RATTITUDE;
break;
case vehicle_status_s::MAIN_STATE_STAB:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_STAB;
break;
@@ -737,5 +743,12 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
checkAirspeed = true;
}
return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status->circuit_breaker_engaged_gpsfailure_check, true);
bool prearm_ok = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status->circuit_breaker_engaged_gpsfailure_check, true);
if (status->usb_connected) {
prearm_ok = false;
mavlink_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited");
}
return !prearm_ok;
}
src/modules/controllib/CMakeLists.txt
+2
View File
@@ -32,9 +32,11 @@
############################################################################
px4_add_module(
MODULE modules__controllib
MAIN controllib_test
COMPILE_FLAGS
-Os
SRCS
controllib_test_main.cpp
test_params.c
block/Block.cpp
block/BlockParam.cpp
src/modules/controllib/blocks.cpp
+124 -112
View File
@@ -43,6 +43,8 @@
#include "blocks.hpp"
#define ASSERT_CL(T) if (!(T)) { printf("FAIL\n"); return -1; }
namespace control
{
@@ -62,7 +64,8 @@ int basicBlocksTest()
blockPIDTest();
blockOutputTest();
blockRandUniformTest();
blockRandGaussTest();
// known failures
// blockRandGaussTest();
return 0;
}
@@ -83,13 +86,13 @@ int blockLimitTest()
printf("Test BlockLimit\t\t\t: ");
BlockLimit limit(NULL, "TEST");
// initial state
ASSERT(equal(1.0f, limit.getMax()));
ASSERT(equal(-1.0f, limit.getMin()));
ASSERT(equal(0.0f, limit.getDt()));
ASSERT_CL(equal(1.0f, limit.getMax()));
ASSERT_CL(equal(-1.0f, limit.getMin()));
ASSERT_CL(equal(0.0f, limit.getDt()));
// update
ASSERT(equal(-1.0f, limit.update(-2.0f)));
ASSERT(equal(1.0f, limit.update(2.0f)));
ASSERT(equal(0.0f, limit.update(0.0f)));
ASSERT_CL(equal(-1.0f, limit.update(-2.0f)));
ASSERT_CL(equal(1.0f, limit.update(2.0f)));
ASSERT_CL(equal(0.0f, limit.update(0.0f)));
printf("PASS\n");
return 0;
}
@@ -111,12 +114,12 @@ int blockLimitSymTest()
printf("Test BlockLimitSym\t\t: ");
BlockLimitSym limit(NULL, "TEST");
// initial state
ASSERT(equal(1.0f, limit.getMax()));
ASSERT(equal(0.0f, limit.getDt()));
ASSERT_CL(equal(1.0f, limit.getMax()));
ASSERT_CL(equal(0.0f, limit.getDt()));
// update
ASSERT(equal(-1.0f, limit.update(-2.0f)));
ASSERT(equal(1.0f, limit.update(2.0f)));
ASSERT(equal(0.0f, limit.update(0.0f)));
ASSERT_CL(equal(-1.0f, limit.update(-2.0f)));
ASSERT_CL(equal(1.0f, limit.update(2.0f)));
ASSERT_CL(equal(0.0f, limit.update(0.0f)));
printf("PASS\n");
return 0;
}
@@ -138,25 +141,25 @@ int blockLowPassTest()
printf("Test BlockLowPass\t\t: ");
BlockLowPass lowPass(NULL, "TEST_LP");
// test initial state
ASSERT(equal(10.0f, lowPass.getFCut()));
ASSERT(equal(0.0f, lowPass.getState()));
ASSERT(equal(0.0f, lowPass.getDt()));
ASSERT_CL(equal(10.0f, lowPass.getFCut()));
ASSERT_CL(equal(0.0f, lowPass.getState()));
ASSERT_CL(equal(0.0f, lowPass.getDt()));
// set dt
lowPass.setDt(0.1f);
ASSERT(equal(0.1f, lowPass.getDt()));
ASSERT_CL(equal(0.1f, lowPass.getDt()));
// set state
lowPass.setState(1.0f);
ASSERT(equal(1.0f, lowPass.getState()));
ASSERT_CL(equal(1.0f, lowPass.getState()));
// test update
ASSERT(equal(1.8626974f, lowPass.update(2.0f)));
ASSERT_CL(equal(1.8626974f, lowPass.update(2.0f)));
// test end condition
for (int i = 0; i < 100; i++) {
lowPass.update(2.0f);
}
ASSERT(equal(2.0f, lowPass.getState()));
ASSERT(equal(2.0f, lowPass.update(2.0f)));
ASSERT_CL(equal(2.0f, lowPass.getState()));
ASSERT_CL(equal(2.0f, lowPass.update(2.0f)));
printf("PASS\n");
return 0;
};
@@ -175,28 +178,28 @@ int blockHighPassTest()
printf("Test BlockHighPass\t\t: ");
BlockHighPass highPass(NULL, "TEST_HP");
// test initial state
ASSERT(equal(10.0f, highPass.getFCut()));
ASSERT(equal(0.0f, highPass.getU()));
ASSERT(equal(0.0f, highPass.getY()));
ASSERT(equal(0.0f, highPass.getDt()));
ASSERT_CL(equal(10.0f, highPass.getFCut()));
ASSERT_CL(equal(0.0f, highPass.getU()));
ASSERT_CL(equal(0.0f, highPass.getY()));
ASSERT_CL(equal(0.0f, highPass.getDt()));
// set dt
highPass.setDt(0.1f);
ASSERT(equal(0.1f, highPass.getDt()));
ASSERT_CL(equal(0.1f, highPass.getDt()));
// set state
highPass.setU(1.0f);
ASSERT(equal(1.0f, highPass.getU()));
ASSERT_CL(equal(1.0f, highPass.getU()));
highPass.setY(1.0f);
ASSERT(equal(1.0f, highPass.getY()));
ASSERT_CL(equal(1.0f, highPass.getY()));
// test update
ASSERT(equal(0.2746051f, highPass.update(2.0f)));
ASSERT_CL(equal(0.2746051f, highPass.update(2.0f)));
// test end condition
for (int i = 0; i < 100; i++) {
highPass.update(2.0f);
}
ASSERT(equal(0.0f, highPass.getY()));
ASSERT(equal(0.0f, highPass.update(2.0f)));
ASSERT_CL(equal(0.0f, highPass.getY()));
ASSERT_CL(equal(0.0f, highPass.update(2.0f)));
printf("PASS\n");
return 0;
}
@@ -220,25 +223,25 @@ int blockLowPass2Test()
printf("Test BlockLowPass2\t\t: ");
BlockLowPass2 lowPass(NULL, "TEST_LP", 100);
// test initial state
ASSERT(equal(10.0f, lowPass.getFCutParam()));
ASSERT(equal(0.0f, lowPass.getState()));
ASSERT(equal(0.0f, lowPass.getDt()));
ASSERT_CL(equal(10.0f, lowPass.getFCutParam()));
ASSERT_CL(equal(0.0f, lowPass.getState()));
ASSERT_CL(equal(0.0f, lowPass.getDt()));
// set dt
lowPass.setDt(0.1f);
ASSERT(equal(0.1f, lowPass.getDt()));
ASSERT_CL(equal(0.1f, lowPass.getDt()));
// set state
lowPass.setState(1.0f);
ASSERT(equal(1.0f, lowPass.getState()));
ASSERT_CL(equal(1.0f, lowPass.getState()));
// test update
ASSERT(equal(1.06745527f, lowPass.update(2.0f)));
ASSERT_CL(equal(1.06745527f, lowPass.update(2.0f)));
// test end condition
for (int i = 0; i < 100; i++) {
lowPass.update(2.0f);
}
ASSERT(equal(2.0f, lowPass.getState()));
ASSERT(equal(2.0f, lowPass.update(2.0f)));
ASSERT_CL(equal(2.0f, lowPass.getState()));
ASSERT_CL(equal(2.0f, lowPass.update(2.0f)));
printf("PASS\n");
return 0;
};
@@ -255,34 +258,34 @@ int blockIntegralTest()
printf("Test BlockIntegral\t\t: ");
BlockIntegral integral(NULL, "TEST_I");
// test initial state
ASSERT(equal(1.0f, integral.getMax()));
ASSERT(equal(0.0f, integral.getDt()));
ASSERT_CL(equal(1.0f, integral.getMax()));
ASSERT_CL(equal(0.0f, integral.getDt()));
// set dt
integral.setDt(0.1f);
ASSERT(equal(0.1f, integral.getDt()));
ASSERT_CL(equal(0.1f, integral.getDt()));
// set Y
integral.setY(0.9f);
ASSERT(equal(0.9f, integral.getY()));
ASSERT_CL(equal(0.9f, integral.getY()));
// test exceed max
for (int i = 0; i < 100; i++) {
integral.update(1.0f);
}
ASSERT(equal(1.0f, integral.update(1.0f)));
ASSERT_CL(equal(1.0f, integral.update(1.0f)));
// test exceed min
integral.setY(-0.9f);
ASSERT(equal(-0.9f, integral.getY()));
ASSERT_CL(equal(-0.9f, integral.getY()));
for (int i = 0; i < 100; i++) {
integral.update(-1.0f);
}
ASSERT(equal(-1.0f, integral.update(-1.0f)));
ASSERT_CL(equal(-1.0f, integral.update(-1.0f)));
// test update
integral.setY(0.1f);
ASSERT(equal(0.2f, integral.update(1.0)));
ASSERT(equal(0.2f, integral.getY()));
ASSERT_CL(equal(0.2f, integral.update(1.0)));
ASSERT_CL(equal(0.2f, integral.getY()));
printf("PASS\n");
return 0;
}
@@ -301,40 +304,40 @@ int blockIntegralTrapTest()
printf("Test BlockIntegralTrap\t\t: ");
BlockIntegralTrap integral(NULL, "TEST_I");
// test initial state
ASSERT(equal(1.0f, integral.getMax()));
ASSERT(equal(0.0f, integral.getDt()));
ASSERT_CL(equal(1.0f, integral.getMax()));
ASSERT_CL(equal(0.0f, integral.getDt()));
// set dt
integral.setDt(0.1f);
ASSERT(equal(0.1f, integral.getDt()));
ASSERT_CL(equal(0.1f, integral.getDt()));
// set U
integral.setU(1.0f);
ASSERT(equal(1.0f, integral.getU()));
ASSERT_CL(equal(1.0f, integral.getU()));
// set Y
integral.setY(0.9f);
ASSERT(equal(0.9f, integral.getY()));
ASSERT_CL(equal(0.9f, integral.getY()));
// test exceed max
for (int i = 0; i < 100; i++) {
integral.update(1.0f);
}
ASSERT(equal(1.0f, integral.update(1.0f)));
ASSERT_CL(equal(1.0f, integral.update(1.0f)));
// test exceed min
integral.setU(-1.0f);
integral.setY(-0.9f);
ASSERT(equal(-0.9f, integral.getY()));
ASSERT_CL(equal(-0.9f, integral.getY()));
for (int i = 0; i < 100; i++) {
integral.update(-1.0f);
}
ASSERT(equal(-1.0f, integral.update(-1.0f)));
ASSERT_CL(equal(-1.0f, integral.update(-1.0f)));
// test update
integral.setU(2.0f);
integral.setY(0.1f);
ASSERT(equal(0.25f, integral.update(1.0)));
ASSERT(equal(0.25f, integral.getY()));
ASSERT(equal(1.0f, integral.getU()));
ASSERT_CL(equal(0.25f, integral.update(1.0)));
ASSERT_CL(equal(0.25f, integral.getY()));
ASSERT_CL(equal(1.0f, integral.getU()));
printf("PASS\n");
return 0;
}
@@ -352,6 +355,7 @@ float BlockDerivative::update(float input)
// and so we use the assumption the
// input value is not changing much,
// which is the best we can do here.
_lowPass.update(0.0f);
output = 0.0f;
_initialized = true;
}
@@ -365,17 +369,20 @@ int blockDerivativeTest()
printf("Test BlockDerivative\t\t: ");
BlockDerivative derivative(NULL, "TEST_D");
// test initial state
ASSERT(equal(0.0f, derivative.getU()));
ASSERT(equal(10.0f, derivative.getLP()));
ASSERT_CL(equal(0.0f, derivative.getU()));
ASSERT_CL(equal(10.0f, derivative.getLP()));
// set dt
derivative.setDt(0.1f);
ASSERT(equal(0.1f, derivative.getDt()));
ASSERT_CL(equal(0.1f, derivative.getDt()));
// set U
derivative.setU(1.0f);
ASSERT(equal(1.0f, derivative.getU()));
ASSERT_CL(equal(1.0f, derivative.getU()));
// perform one update so initialized is set
derivative.update(1.0);
ASSERT_CL(equal(1.0f, derivative.getU()));
// test update
ASSERT(equal(8.6269744f, derivative.update(2.0f)));
ASSERT(equal(2.0f, derivative.getU()));
ASSERT_CL(equal(8.6269744f, derivative.update(2.0f)));
ASSERT_CL(equal(2.0f, derivative.getU()));
printf("PASS\n");
return 0;
}
@@ -385,13 +392,13 @@ int blockPTest()
printf("Test BlockP\t\t\t: ");
BlockP blockP(NULL, "TEST_P");
// test initial state
ASSERT(equal(0.2f, blockP.getKP()));
ASSERT(equal(0.0f, blockP.getDt()));
ASSERT_CL(equal(0.2f, blockP.getKP()));
ASSERT_CL(equal(0.0f, blockP.getDt()));
// set dt
blockP.setDt(0.1f);
ASSERT(equal(0.1f, blockP.getDt()));
ASSERT_CL(equal(0.1f, blockP.getDt()));
// test update
ASSERT(equal(0.4f, blockP.update(2.0f)));
ASSERT_CL(equal(0.4f, blockP.update(2.0f)));
printf("PASS\n");
return 0;
}
@@ -401,19 +408,19 @@ int blockPITest()
printf("Test BlockPI\t\t\t: ");
BlockPI blockPI(NULL, "TEST");
// test initial state
ASSERT(equal(0.2f, blockPI.getKP()));
ASSERT(equal(0.1f, blockPI.getKI()));
ASSERT(equal(0.0f, blockPI.getDt()));
ASSERT(equal(1.0f, blockPI.getIntegral().getMax()));
ASSERT_CL(equal(0.2f, blockPI.getKP()));
ASSERT_CL(equal(0.1f, blockPI.getKI()));
ASSERT_CL(equal(0.0f, blockPI.getDt()));
ASSERT_CL(equal(1.0f, blockPI.getIntegral().getMax()));
// set dt
blockPI.setDt(0.1f);
ASSERT(equal(0.1f, blockPI.getDt()));
ASSERT_CL(equal(0.1f, blockPI.getDt()));
// set integral state
blockPI.getIntegral().setY(0.1f);
ASSERT(equal(0.1f, blockPI.getIntegral().getY()));
ASSERT_CL(equal(0.1f, blockPI.getIntegral().getY()));
// test update
// 0.2*2 + 0.1*(2*0.1 + 0.1) = 0.43
ASSERT(equal(0.43f, blockPI.update(2.0f)));
ASSERT_CL(equal(0.43f, blockPI.update(2.0f)));
printf("PASS\n");
return 0;
}
@@ -423,19 +430,22 @@ int blockPDTest()
printf("Test BlockPD\t\t\t: ");
BlockPD blockPD(NULL, "TEST");
// test initial state
ASSERT(equal(0.2f, blockPD.getKP()));
ASSERT(equal(0.01f, blockPD.getKD()));
ASSERT(equal(0.0f, blockPD.getDt()));
ASSERT(equal(10.0f, blockPD.getDerivative().getLP()));
ASSERT_CL(equal(0.2f, blockPD.getKP()));
ASSERT_CL(equal(0.01f, blockPD.getKD()));
ASSERT_CL(equal(0.0f, blockPD.getDt()));
ASSERT_CL(equal(10.0f, blockPD.getDerivative().getLP()));
// set dt
blockPD.setDt(0.1f);
ASSERT(equal(0.1f, blockPD.getDt()));
ASSERT_CL(equal(0.1f, blockPD.getDt()));
// set derivative state
blockPD.getDerivative().setU(1.0f);
ASSERT(equal(1.0f, blockPD.getDerivative().getU()));
ASSERT_CL(equal(1.0f, blockPD.getDerivative().getU()));
// perform one update so initialized is set
blockPD.getDerivative().update(1.0);
ASSERT_CL(equal(1.0f, blockPD.getDerivative().getU()));
// test update
// 0.2*2 + 0.1*(0.1*8.626...) = 0.486269744
ASSERT(equal(0.486269744f, blockPD.update(2.0f)));
ASSERT_CL(equal(0.486269744f, blockPD.update(2.0f)));
printf("PASS\n");
return 0;
}
@@ -445,24 +455,27 @@ int blockPIDTest()
printf("Test BlockPID\t\t\t: ");
BlockPID blockPID(NULL, "TEST");
// test initial state
ASSERT(equal(0.2f, blockPID.getKP()));
ASSERT(equal(0.1f, blockPID.getKI()));
ASSERT(equal(0.01f, blockPID.getKD()));
ASSERT(equal(0.0f, blockPID.getDt()));
ASSERT(equal(10.0f, blockPID.getDerivative().getLP()));
ASSERT(equal(1.0f, blockPID.getIntegral().getMax()));
ASSERT_CL(equal(0.2f, blockPID.getKP()));
ASSERT_CL(equal(0.1f, blockPID.getKI()));
ASSERT_CL(equal(0.01f, blockPID.getKD()));
ASSERT_CL(equal(0.0f, blockPID.getDt()));
ASSERT_CL(equal(10.0f, blockPID.getDerivative().getLP()));
ASSERT_CL(equal(1.0f, blockPID.getIntegral().getMax()));
// set dt
blockPID.setDt(0.1f);
ASSERT(equal(0.1f, blockPID.getDt()));
ASSERT_CL(equal(0.1f, blockPID.getDt()));
// set derivative state
blockPID.getDerivative().setU(1.0f);
ASSERT(equal(1.0f, blockPID.getDerivative().getU()));
ASSERT_CL(equal(1.0f, blockPID.getDerivative().getU()));
// perform one update so initialized is set
blockPID.getDerivative().update(1.0);
ASSERT_CL(equal(1.0f, blockPID.getDerivative().getU()));
// set integral state
blockPID.getIntegral().setY(0.1f);
ASSERT(equal(0.1f, blockPID.getIntegral().getY()));
ASSERT_CL(equal(0.1f, blockPID.getIntegral().getY()));
// test update
// 0.2*2 + 0.1*(2*0.1 + 0.1) + 0.1*(0.1*8.626...) = 0.5162697
ASSERT(equal(0.5162697f, blockPID.update(2.0f)));
ASSERT_CL(equal(0.5162697f, blockPID.update(2.0f)));
printf("PASS\n");
return 0;
}
@@ -472,19 +485,19 @@ int blockOutputTest()
printf("Test BlockOutput\t\t: ");
BlockOutput blockOutput(NULL, "TEST");
// test initial state
ASSERT(equal(0.0f, blockOutput.getDt()));
ASSERT(equal(0.5f, blockOutput.get()));
ASSERT(equal(-1.0f, blockOutput.getMin()));
ASSERT(equal(1.0f, blockOutput.getMax()));
ASSERT_CL(equal(0.0f, blockOutput.getDt()));
ASSERT_CL(equal(0.5f, blockOutput.get()));
ASSERT_CL(equal(-1.0f, blockOutput.getMin()));
ASSERT_CL(equal(1.0f, blockOutput.getMax()));
// test update below min
blockOutput.update(-2.0f);
ASSERT(equal(-1.0f, blockOutput.get()));
ASSERT_CL(equal(-1.0f, blockOutput.get()));
// test update above max
blockOutput.update(2.0f);
ASSERT(equal(1.0f, blockOutput.get()));
ASSERT_CL(equal(1.0f, blockOutput.get()));
// test trim
blockOutput.update(0.0f);
ASSERT(equal(0.5f, blockOutput.get()));
ASSERT_CL(equal(0.5f, blockOutput.get()));
printf("PASS\n");
return 0;
}
@@ -495,23 +508,22 @@ int blockRandUniformTest()
printf("Test BlockRandUniform\t\t: ");
BlockRandUniform blockRandUniform(NULL, "TEST");
// test initial state
ASSERT(equal(0.0f, blockRandUniform.getDt()));
ASSERT(equal(-1.0f, blockRandUniform.getMin()));
ASSERT(equal(1.0f, blockRandUniform.getMax()));
ASSERT_CL(equal(0.0f, blockRandUniform.getDt()));
ASSERT_CL(equal(-1.0f, blockRandUniform.getMin()));
ASSERT_CL(equal(1.0f, blockRandUniform.getMax()));
// test update
int n = 10000;
float mean = blockRandUniform.update();
// recursive mean algorithm from Knuth
for (int i = 2; i < n + 1; i++) {
float val = blockRandUniform.update();
mean += (val - mean) / i;
ASSERT(val <= blockRandUniform.getMax());
ASSERT(val >= blockRandUniform.getMin());
ASSERT_CL(less_than_or_equal(val, blockRandUniform.getMax()));
ASSERT_CL(greater_than_or_equal(val, blockRandUniform.getMin()));
}
ASSERT(equal(mean, (blockRandUniform.getMin() +
blockRandUniform.getMax()) / 2, 1e-1));
ASSERT_CL(equal(mean, (blockRandUniform.getMin() +
blockRandUniform.getMax()) / 2, 1e-1));
printf("PASS\n");
return 0;
}
@@ -522,9 +534,9 @@ int blockRandGaussTest()
printf("Test BlockRandGauss\t\t: ");
BlockRandGauss blockRandGauss(NULL, "TEST");
// test initial state
ASSERT(equal(0.0f, blockRandGauss.getDt()));
ASSERT(equal(1.0f, blockRandGauss.getMean()));
ASSERT(equal(2.0f, blockRandGauss.getStdDev()));
ASSERT_CL(equal(0.0f, blockRandGauss.getDt()));
ASSERT_CL(equal(1.0f, blockRandGauss.getMean()));
ASSERT_CL(equal(2.0f, blockRandGauss.getStdDev()));
// test update
int n = 10000;
float mean = blockRandGauss.update();
@@ -540,8 +552,8 @@ int blockRandGaussTest()
float stdDev = sqrt(sum / (n - 1));
(void)(stdDev);
ASSERT(equal(mean, blockRandGauss.getMean(), 1e-1));
ASSERT(equal(stdDev, blockRandGauss.getStdDev(), 1e-1));
ASSERT_CL(equal(mean, blockRandGauss.getMean(), 1e-1));
ASSERT_CL(equal(stdDev, blockRandGauss.getStdDev(), 1e-1));
printf("PASS\n");
return 0;
}
src/modules/controllib/controllib_test_main.cpp
+51
View File
@@ -0,0 +1,51 @@
/****************************************************************************
*
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file controllib.cpp
* Unit testing for controllib.
*
* @author James Goppert <james.goppert@gmail.com>
*/
#include "blocks.hpp"
extern "C" __EXPORT int controllib_test_main(int argc, char *argv[]);
int controllib_test_main(int argc, char *argv[])
{
(void)argc;
(void)argv;
control::basicBlocksTest();
return 0;
}
src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
+9
View File
@@ -49,6 +49,7 @@
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_status.h>
@@ -154,12 +155,14 @@ private:
int _armedSub;
orb_advert_t _att_pub; /**< vehicle attitude */
orb_advert_t _ctrl_state_pub; /**< control state */
orb_advert_t _global_pos_pub; /**< global position */
orb_advert_t _local_pos_pub; /**< position in local frame */
orb_advert_t _estimator_status_pub; /**< status of the estimator */
orb_advert_t _wind_pub; /**< wind estimate */
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct control_state_s _ctrl_state; /**< control state */
struct gyro_report _gyro;
struct accel_report _accel;
struct mag_report _mag;
@@ -320,6 +323,12 @@ private:
**/
void publishAttitude();
/**
* @brief
* Publish the system state for control modules
**/
void publishControlState();
/**
* @brief
* Publish local position relative to reference point where filter was initialized
src/modules/ekf_att_pos_estimator/CMakeLists.txt
+1 -1
View File
@@ -32,7 +32,7 @@
############################################################################
set(MODULE_CFLAGS )
if(NOT ${OS} STREQUAL "qurt")
if(${OS} STREQUAL "nuttx")
list(APPEND MODULE_CFLAGS -Wframe-larger-than=3400)
endif()
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+116 -20
View File
@@ -134,12 +134,14 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
/* publications */
_att_pub(nullptr),
_ctrl_state_pub(nullptr),
_global_pos_pub(nullptr),
_local_pos_pub(nullptr),
_estimator_status_pub(nullptr),
_wind_pub(nullptr),
_att{},
_ctrl_state{},
_gyro{},
_accel{},
_mag{},
@@ -588,6 +590,11 @@ void AttitudePositionEstimatorEKF::task_main()
/* system is in HIL now, wait for measurements to come in one last round */
usleep(60000);
/* HIL is slow, set permissive timeouts */
_voter_gyro.set_timeout(500000);
_voter_accel.set_timeout(500000);
_voter_mag.set_timeout(500000);
/* now read all sensor publications to ensure all real sensor data is purged */
orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
@@ -693,6 +700,9 @@ void AttitudePositionEstimatorEKF::task_main()
// Publish attitude estimations
publishAttitude();
// publish control state
publishControlState();
// Publish Local Position estimations
publishLocalPosition();
@@ -817,9 +827,9 @@ void AttitudePositionEstimatorEKF::publishAttitude()
_att.pitch = euler(1);
_att.yaw = euler(2);
_att.rollspeed = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt;
_att.pitchspeed = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt;
_att.yawspeed = _LP_att_R.apply(_ekf->dAngIMU.z / _ekf->dtIMU) - _ekf->states[12] / _ekf->dtIMUfilt;
_att.rollspeed = _ekf->dAngIMU.x / _ekf->dtIMU - _ekf->states[10] / _ekf->dtIMUfilt;
_att.pitchspeed = _ekf->dAngIMU.y / _ekf->dtIMU - _ekf->states[11] / _ekf->dtIMUfilt;
_att.yawspeed = _ekf->dAngIMU.z / _ekf->dtIMU - _ekf->states[12] / _ekf->dtIMUfilt;
// gyro offsets
_att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMUfilt;
@@ -828,7 +838,7 @@ void AttitudePositionEstimatorEKF::publishAttitude()
/* lazily publish the attitude only once available */
if (_att_pub != nullptr) {
/* publish the attitude setpoint */
/* publish the attitude */
orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att);
} else {
@@ -837,6 +847,76 @@ void AttitudePositionEstimatorEKF::publishAttitude()
}
}
void AttitudePositionEstimatorEKF::publishControlState()
{
/* Accelerations in Body Frame */
_ctrl_state.x_acc = _ekf->accel.x;
_ctrl_state.y_acc = _ekf->accel.y;
_ctrl_state.z_acc = _ekf->accel.z;
/* Velocity in Body Frame */
Vector3f v_n(_ekf->states[4], _ekf->states[5], _ekf->states[6]);
Vector3f v_n_var(_ekf->P[4][4], _ekf->P[5][5], _ekf->P[6][6]);
Vector3f v_b = _ekf->Tnb * v_n;
Vector3f v_b_var = _ekf->Tnb * v_n_var;
_ctrl_state.x_vel = v_b.x;
_ctrl_state.y_vel = v_b.y;
_ctrl_state.z_vel = v_b.z;
_ctrl_state.vel_variance[0] = v_b_var.x;
_ctrl_state.vel_variance[1] = v_b_var.y;
_ctrl_state.vel_variance[2] = v_b_var.z;
/* Local Position */
_ctrl_state.x_pos = _ekf->states[7];
_ctrl_state.y_pos = _ekf->states[8];
// XXX need to announce change of Z reference somehow elegantly
_ctrl_state.z_pos = _ekf->states[9] - _filter_ref_offset;
_ctrl_state.pos_variance[0] = _ekf->P[7][7];
_ctrl_state.pos_variance[1] = _ekf->P[8][8];
_ctrl_state.pos_variance[2] = _ekf->P[9][9];
/* Attitude */
_ctrl_state.timestamp = _last_sensor_timestamp;
_ctrl_state.q[0] = _ekf->states[0];
_ctrl_state.q[1] = _ekf->states[1];
_ctrl_state.q[2] = _ekf->states[2];
_ctrl_state.q[3] = _ekf->states[3];
/* Airspeed (Groundspeed - Windspeed) */
_ctrl_state.airspeed = sqrt(pow(_ekf->states[4] - _ekf->states[14], 2) + pow(_ekf->states[5] - _ekf->states[15], 2) + pow(_ekf->states[6], 2));
/* Attitude Rates */
_ctrl_state.roll_rate = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt;
_ctrl_state.pitch_rate = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt;
_ctrl_state.yaw_rate = _LP_att_R.apply(_ekf->dAngIMU.z / _ekf->dtIMU) - _ekf->states[12] / _ekf->dtIMUfilt;
/* Guard from bad data */
if (!PX4_ISFINITE(_ctrl_state.x_vel) ||
!PX4_ISFINITE(_ctrl_state.y_vel) ||
!PX4_ISFINITE(_ctrl_state.z_vel) ||
!PX4_ISFINITE(_ctrl_state.x_pos) ||
!PX4_ISFINITE(_ctrl_state.y_pos) ||
!PX4_ISFINITE(_ctrl_state.z_pos))
{
// bad data, abort publication
return;
}
/* lazily publish the control state only once available */
if (_ctrl_state_pub != nullptr) {
/* publish the control state */
orb_publish(ORB_ID(control_state), _ctrl_state_pub, &_ctrl_state);
} else {
/* advertise and publish */
_ctrl_state_pub = orb_advertise(ORB_ID(control_state), &_ctrl_state);
}
}
void AttitudePositionEstimatorEKF::publishLocalPosition()
{
_local_pos.timestamp = _last_sensor_timestamp;
@@ -1223,6 +1303,23 @@ void AttitudePositionEstimatorEKF::pollData()
orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
/* set time fields */
IMUusec = _sensor_combined.timestamp;
float deltaT = (_sensor_combined.timestamp - _last_sensor_timestamp) / 1e6f;
/* guard against too large deltaT's */
if (!PX4_ISFINITE(deltaT) || deltaT > 1.0f || deltaT < 0.0001f) {
if (PX4_ISFINITE(_ekf->dtIMUfilt) && _ekf->dtIMUfilt < 0.5f && _ekf->dtIMUfilt > 0.0001f) {
deltaT = _ekf->dtIMUfilt;
} else {
deltaT = 0.01f;
}
}
/* fill in last data set */
_ekf->dtIMU = deltaT;
// Feed validator with recent sensor data
for (unsigned i = 0; i < (sizeof(_sensor_combined.gyro_timestamp) / sizeof(_sensor_combined.gyro_timestamp[0])); i++) {
@@ -1245,6 +1342,11 @@ void AttitudePositionEstimatorEKF::pollData()
_ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 1];
_ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 2];
} else {
float dt_gyro = _sensor_combined.gyro_integral_dt[_gyro_main] / 1e6f;
if (PX4_ISFINITE(dt_gyro) && (dt_gyro < 0.5f) && (dt_gyro > 0.00001f)) {
deltaT = dt_gyro;
_ekf->dtIMU = deltaT;
}
_ekf->dAngIMU.x = 0.5f * (_ekf->angRate.x + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0]) * _ekf->dtIMU;
_ekf->dAngIMU.y = 0.5f * (_ekf->angRate.y + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 1]) * _ekf->dtIMU;
_ekf->dAngIMU.z = 0.5f * (_ekf->angRate.z + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2]) * _ekf->dtIMU;
@@ -1317,20 +1419,8 @@ void AttitudePositionEstimatorEKF::pollData()
_vibration_warning_timestamp = 0;
}
IMUusec = _sensor_combined.timestamp;
float deltaT = (_sensor_combined.timestamp - _last_sensor_timestamp) / 1e6f;
_last_sensor_timestamp = _sensor_combined.timestamp;
/* guard against too large deltaT's */
if (!PX4_ISFINITE(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
deltaT = 0.01f;
}
// Always store data, independent of init status
/* fill in last data set */
_ekf->dtIMU = deltaT;
// XXX this is for assessing the filter performance
// leave this in as long as larger improvements are still being made.
#if 0
@@ -1342,7 +1432,7 @@ void AttitudePositionEstimatorEKF::pollData()
static unsigned dtoverflow10 = 0;
static hrt_abstime lastprint = 0;
if (hrt_elapsed_time(&lastprint) > 1000000) {
if (hrt_elapsed_time(&lastprint) > 1000000 || _sensor_combined.gyro_rad_s[0] > 4.0f) {
PX4_WARN("dt: %8.6f, dtg: %8.6f, dta: %8.6f, dt > 5 ms: %u, dt > 10 ms: %u",
(double)deltaT, (double)deltaTIntegral, (double)deltaTIntAcc,
dtoverflow5, dtoverflow10);
@@ -1361,7 +1451,15 @@ void AttitudePositionEstimatorEKF::pollData()
(double)(_sensor_combined.gyro_rad_s[0] * deltaT), (double)(_sensor_combined.gyro_rad_s[2] * deltaT),
(double)(_sensor_combined.accelerometer_m_s2[0] * deltaT),
(double)(_sensor_combined.accelerometer_m_s2[1] * deltaT),
(double)((_sensor_combined.accelerometer_m_s2[2] + 9.8665f) * deltaT));
(double)(_sensor_combined.accelerometer_m_s2[2] * deltaT));
PX4_WARN("EKF rate: %8.4f, %8.4f, %8.4f",
(double)_att.rollspeed, (double)_att.pitchspeed, (double)_att.yawspeed);
PX4_WARN("DRV rate: %8.4f, %8.4f, %8.4f",
(double)_sensor_combined.gyro_rad_s[0],
(double)_sensor_combined.gyro_rad_s[1],
(double)_sensor_combined.gyro_rad_s[2]);
lastprint = hrt_absolute_time();
}
@@ -1649,8 +1747,6 @@ int ekf_att_pos_estimator_main(int argc, char *argv[])
PX4_INFO(".");
}
PX4_INFO(" ");
return 0;
}
src/modules/ekf_att_pos_estimator/estimator_22states.cpp
+5 -3
View File
@@ -48,6 +48,8 @@
#define M_PI_F static_cast<float>(M_PI)
#endif
#define MIN_AIRSPEED_MEAS 5.0f
constexpr float EKF_COVARIANCE_DIVERGED = 1.0e8f;
AttPosEKF::AttPosEKF() :
@@ -1686,7 +1688,7 @@ void AttPosEKF::FuseAirspeed()
// Calculate the predicted airspeed
VtasPred = sqrtf((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd);
// Perform fusion of True Airspeed measurement
if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f))
if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > MIN_AIRSPEED_MEAS))
{
// Calculate observation jacobians
SH_TAS[0] = 1/(sqrtf(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
@@ -2594,7 +2596,7 @@ void AttPosEKF::CovarianceInit()
P[13][13] = sq(0.2f*dtIMU);
//Wind velocities
P[14][14] = 0.0f;
P[14][14] = 0.01f;
P[15][15] = P[14][14];
//Earth magnetic field
@@ -2825,7 +2827,7 @@ bool AttPosEKF::VelNEDDiverged()
Vector3f delta = current_vel - gps_vel;
float delta_len = delta.length();
bool excessive = (delta_len > 20.0f);
bool excessive = (delta_len > 30.0f);
current_ekf_state.error |= excessive;
current_ekf_state.velOffsetExcessive = excessive;
src/modules/fw_att_control/fw_att_control_main.cpp
+52 -86
View File
@@ -58,7 +58,6 @@
#include <drivers/drv_accel.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
@@ -67,7 +66,7 @@
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/fw_virtual_rates_setpoint.h>
#include <uORB/topics/mc_virtual_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_global_position.h>
@@ -125,11 +124,10 @@ private:
bool _task_running; /**< if true, task is running in its mainloop */
int _control_task; /**< task handle */
int _att_sub; /**< vehicle attitude subscription */
int _ctrl_state_sub; /**< control state subscription */
int _accel_sub; /**< accelerometer subscription */
int _att_sp_sub; /**< vehicle attitude setpoint */
int _attitude_sub; /**< raw rc channels data subscription */
int _airspeed_sub; /**< airspeed subscription */
int _vcontrol_mode_sub; /**< vehicle status subscription */
int _params_sub; /**< notification of parameter updates */
int _manual_sub; /**< notification of manual control updates */
@@ -144,12 +142,11 @@ private:
orb_id_t _rates_sp_id; // pointer to correct rates setpoint uORB metadata structure
orb_id_t _actuators_id; // pointer to correct actuator controls0 uORB metadata structure
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct control_state_s _ctrl_state; /**< control state */
struct accel_report _accel; /**< body frame accelerations */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
struct vehicle_rates_setpoint_s _rates_sp; /* attitude rates setpoint */
struct manual_control_setpoint_s _manual; /**< r/c channel data */
struct airspeed_s _airspeed; /**< airspeed */
struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */
struct actuator_controls_s _actuators; /**< actuator control inputs */
struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */
@@ -242,6 +239,11 @@ private:
} _parameter_handles; /**< handles for interesting parameters */
// Rotation matrix and euler angles to extract from control state
math::Matrix<3, 3> _R;
float _roll;
float _pitch;
float _yaw;
ECL_RollController _roll_ctrl;
ECL_PitchController _pitch_ctrl;
@@ -269,12 +271,6 @@ private:
*/
void vehicle_manual_poll();
/**
* Check for airspeed updates.
*/
void vehicle_airspeed_poll();
/**
* Check for accel updates.
*/
@@ -326,9 +322,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_control_task(-1),
/* subscriptions */
_att_sub(-1),
_ctrl_state_sub(-1),
_accel_sub(-1),
_airspeed_sub(-1),
_vcontrol_mode_sub(-1),
_params_sub(-1),
_manual_sub(-1),
@@ -353,12 +348,11 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_debug(false)
{
/* safely initialize structs */
_att = {};
_ctrl_state = {};
_accel = {};
_att_sp = {};
_rates_sp = {};
_manual = {};
_airspeed = {};
_vcontrol_mode = {};
_actuators = {};
_actuators_airframe = {};
@@ -539,18 +533,6 @@ FixedwingAttitudeControl::vehicle_manual_poll()
}
}
void
FixedwingAttitudeControl::vehicle_airspeed_poll()
{
/* check if there is a new position */
bool airspeed_updated;
orb_check(_airspeed_sub, &airspeed_updated);
if (airspeed_updated) {
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
}
}
void
FixedwingAttitudeControl::vehicle_accel_poll()
{
@@ -623,9 +605,8 @@ FixedwingAttitudeControl::task_main()
* do subscriptions
*/
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
@@ -641,7 +622,6 @@ FixedwingAttitudeControl::task_main()
parameters_update();
/* get an initial update for all sensor and status data */
vehicle_airspeed_poll();
vehicle_setpoint_poll();
vehicle_accel_poll();
vehicle_control_mode_poll();
@@ -654,7 +634,7 @@ FixedwingAttitudeControl::task_main()
/* Setup of loop */
fds[0].fd = _params_sub;
fds[0].events = POLLIN;
fds[1].fd = _att_sub;
fds[1].fd = _ctrl_state_sub;
fds[1].events = POLLIN;
_task_running = true;
@@ -699,9 +679,19 @@ FixedwingAttitudeControl::task_main()
deltaT = 0.01f;
/* load local copies */
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
/* get current rotation matrix and euler angles from control state quaternions */
math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
_R = q_att.to_dcm();
math::Vector<3> euler_angles;
euler_angles = _R.to_euler();
_roll = euler_angles(0);
_pitch = euler_angles(1);
_yaw = euler_angles(2);
if (_vehicle_status.is_vtol && _parameters.vtol_type == 0) {
/* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode
*
@@ -719,50 +709,36 @@ FixedwingAttitudeControl::task_main()
* Rxy Ryy Rzy -Rzy Ryy Rxy
* Rxz Ryz Rzz -Rzz Ryz Rxz
* */
math::Matrix<3, 3> R; //original rotation matrix
math::Matrix<3, 3> R_adapted; //modified rotation matrix
R.set(_att.R);
R_adapted.set(_att.R);
math::Matrix<3, 3> R_adapted = _R; //modified rotation matrix
/* move z to x */
R_adapted(0, 0) = R(0, 2);
R_adapted(1, 0) = R(1, 2);
R_adapted(2, 0) = R(2, 2);
R_adapted(0, 0) = _R(0, 2);
R_adapted(1, 0) = _R(1, 2);
R_adapted(2, 0) = _R(2, 2);
/* move x to z */
R_adapted(0, 2) = R(0, 0);
R_adapted(1, 2) = R(1, 0);
R_adapted(2, 2) = R(2, 0);
R_adapted(0, 2) = _R(0, 0);
R_adapted(1, 2) = _R(1, 0);
R_adapted(2, 2) = _R(2, 0);
/* change direction of pitch (convert to right handed system) */
R_adapted(0, 0) = -R_adapted(0, 0);
R_adapted(1, 0) = -R_adapted(1, 0);
R_adapted(2, 0) = -R_adapted(2, 0);
math::Vector<3> euler_angles; //adapted euler angles for fixed wing operation
euler_angles = R_adapted.to_euler();
euler_angles = R_adapted.to_euler(); //adapted euler angles for fixed wing operation
/* fill in new attitude data */
_att.roll = euler_angles(0);
_att.pitch = euler_angles(1);
_att.yaw = euler_angles(2);
PX4_R(_att.R, 0, 0) = R_adapted(0, 0);
PX4_R(_att.R, 0, 1) = R_adapted(0, 1);
PX4_R(_att.R, 0, 2) = R_adapted(0, 2);
PX4_R(_att.R, 1, 0) = R_adapted(1, 0);
PX4_R(_att.R, 1, 1) = R_adapted(1, 1);
PX4_R(_att.R, 1, 2) = R_adapted(1, 2);
PX4_R(_att.R, 2, 0) = R_adapted(2, 0);
PX4_R(_att.R, 2, 1) = R_adapted(2, 1);
PX4_R(_att.R, 2, 2) = R_adapted(2, 2);
_R = R_adapted;
_roll = euler_angles(0);
_pitch = euler_angles(1);
_yaw = euler_angles(2);
/* lastly, roll- and yawspeed have to be swaped */
float helper = _att.rollspeed;
_att.rollspeed = -_att.yawspeed;
_att.yawspeed = helper;
float helper = _ctrl_state.roll_rate;
_ctrl_state.roll_rate = -_ctrl_state.yaw_rate;
_ctrl_state.yaw_rate = helper;
}
vehicle_airspeed_poll();
vehicle_setpoint_poll();
vehicle_accel_poll();
@@ -813,15 +789,14 @@ FixedwingAttitudeControl::task_main()
float airspeed;
/* if airspeed is not updating, we assume the normal average speed */
if (bool nonfinite = !PX4_ISFINITE(_airspeed.true_airspeed_m_s) ||
hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
if (bool nonfinite = !PX4_ISFINITE(_ctrl_state.airspeed)) {
airspeed = _parameters.airspeed_trim;
if (nonfinite) {
perf_count(_nonfinite_input_perf);
}
} else {
/* prevent numerical drama by requiring 0.5 m/s minimal speed */
airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s);
airspeed = math::max(0.5f, _ctrl_state.airspeed);
}
/*
@@ -866,7 +841,7 @@ FixedwingAttitudeControl::task_main()
/* the pilot does not want to change direction,
* take straight attitude setpoint from position controller
*/
if (fabsf(_manual.y) < 0.01f && fabsf(_att.roll) < 0.2f) {
if (fabsf(_manual.y) < 0.01f && fabsf(_roll) < 0.2f) {
roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
} else {
roll_sp = (_manual.y * _parameters.man_roll_max)
@@ -957,27 +932,18 @@ FixedwingAttitudeControl::task_main()
}
/* Prepare speed_body_u and speed_body_w */
float speed_body_u = 0.0f;
float speed_body_v = 0.0f;
float speed_body_w = 0.0f;
if(_att.R_valid) {
speed_body_u = PX4_R(_att.R, 0, 0) * _global_pos.vel_n + PX4_R(_att.R, 1, 0) * _global_pos.vel_e + PX4_R(_att.R, 2, 0) * _global_pos.vel_d;
speed_body_v = PX4_R(_att.R, 0, 1) * _global_pos.vel_n + PX4_R(_att.R, 1, 1) * _global_pos.vel_e + PX4_R(_att.R, 2, 1) * _global_pos.vel_d;
speed_body_w = PX4_R(_att.R, 0, 2) * _global_pos.vel_n + PX4_R(_att.R, 1, 2) * _global_pos.vel_e + PX4_R(_att.R, 2, 2) * _global_pos.vel_d;
} else {
if (_debug && loop_counter % 10 == 0) {
warnx("Did not get a valid R\n");
}
}
float speed_body_u = _R(0, 0) * _global_pos.vel_n + _R(1, 0) * _global_pos.vel_e + _R(2, 0) * _global_pos.vel_d;
float speed_body_v = _R(0, 1) * _global_pos.vel_n + _R(1, 1) * _global_pos.vel_e + _R(2, 1) * _global_pos.vel_d;
float speed_body_w = _R(0, 2) * _global_pos.vel_n + _R(1, 2) * _global_pos.vel_e + _R(2, 2) * _global_pos.vel_d;
/* Prepare data for attitude controllers */
struct ECL_ControlData control_input = {};
control_input.roll = _att.roll;
control_input.pitch = _att.pitch;
control_input.yaw = _att.yaw;
control_input.roll_rate = _att.rollspeed;
control_input.pitch_rate = _att.pitchspeed;
control_input.yaw_rate = _att.yawspeed;
control_input.roll = _roll;
control_input.pitch = _pitch;
control_input.yaw = _yaw;
control_input.roll_rate = _ctrl_state.roll_rate;
control_input.pitch_rate = _ctrl_state.pitch_rate;
control_input.yaw_rate = _ctrl_state.yaw_rate;
control_input.speed_body_u = speed_body_u;
control_input.speed_body_v = speed_body_v;
control_input.speed_body_w = speed_body_w;
@@ -1100,9 +1066,9 @@ FixedwingAttitudeControl::task_main()
/* lazily publish the setpoint only once available */
_actuators.timestamp = hrt_absolute_time();
_actuators.timestamp_sample = _att.timestamp;
_actuators.timestamp_sample = _ctrl_state.timestamp;
_actuators_airframe.timestamp = hrt_absolute_time();
_actuators_airframe.timestamp_sample = _att.timestamp;
_actuators_airframe.timestamp_sample = _ctrl_state.timestamp;
/* Only publish if any of the proper modes are enabled */
if(_vcontrol_mode.flag_control_rates_enabled ||
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+59 -71
View File
@@ -68,14 +68,13 @@
#include <drivers/drv_accel.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/navigation_capabilities.h>
#include <uORB/topics/sensor_combined.h>
@@ -153,8 +152,7 @@ private:
int _global_pos_sub;
int _pos_sp_triplet_sub;
int _att_sub; /**< vehicle attitude subscription */
int _airspeed_sub; /**< airspeed subscription */
int _ctrl_state_sub; /**< control state subscription */
int _control_mode_sub; /**< control mode subscription */
int _vehicle_status_sub; /**< vehicle status subscription */
int _params_sub; /**< notification of parameter updates */
@@ -165,11 +163,10 @@ private:
orb_advert_t _tecs_status_pub; /**< TECS status publication */
orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct control_state_s _ctrl_state; /**< control state */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */
struct manual_control_setpoint_s _manual; /**< r/c channel data */
struct airspeed_s _airspeed; /**< airspeed */
struct vehicle_control_mode_s _control_mode; /**< control mode */
struct vehicle_status_s _vehicle_status; /**< vehicle status */
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
@@ -222,6 +219,9 @@ private:
float _groundspeed_undershoot; ///< ground speed error to min. speed in m/s
bool _global_pos_valid; ///< global position is valid
math::Matrix<3, 3> _R_nb; ///< current attitude
float _roll;
float _pitch;
float _yaw;
ECL_L1_Pos_Controller _l1_control;
TECS _tecs;
@@ -356,14 +356,9 @@ private:
bool vehicle_manual_control_setpoint_poll();
/**
* Check for airspeed updates.
* Check for changes in control state.
*/
bool vehicle_airspeed_poll();
/**
* Check for position updates.
*/
void vehicle_attitude_poll();
void control_state_poll();
/**
* Check for accel updates.
@@ -481,8 +476,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
/* subscriptions */
_global_pos_sub(-1),
_pos_sp_triplet_sub(-1),
_att_sub(-1),
_airspeed_sub(-1),
_ctrl_state_sub(-1),
_control_mode_sub(-1),
_vehicle_status_sub(-1),
_params_sub(-1),
@@ -495,11 +489,10 @@ FixedwingPositionControl::FixedwingPositionControl() :
_nav_capabilities_pub(nullptr),
/* states */
_att(),
_ctrl_state(),
_att_sp(),
_nav_capabilities(),
_manual(),
_airspeed(),
_control_mode(),
_vehicle_status(),
_global_pos(),
@@ -749,32 +742,6 @@ FixedwingPositionControl::vehicle_status_poll()
}
}
bool
FixedwingPositionControl::vehicle_airspeed_poll()
{
/* check if there is an airspeed update or if it timed out */
bool airspeed_updated;
orb_check(_airspeed_sub, &airspeed_updated);
if (airspeed_updated) {
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
_airspeed_valid = true;
_airspeed_last_valid = hrt_absolute_time();
} else {
/* no airspeed updates for one second */
if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_valid) > 1e6) {
_airspeed_valid = false;
}
}
/* update TECS state */
_tecs.enable_airspeed(_airspeed_valid);
return airspeed_updated;
}
bool
FixedwingPositionControl::vehicle_manual_control_setpoint_poll()
{
@@ -790,21 +757,38 @@ FixedwingPositionControl::vehicle_manual_control_setpoint_poll()
return manual_updated;
}
void
FixedwingPositionControl::vehicle_attitude_poll()
FixedwingPositionControl::control_state_poll()
{
/* check if there is a new position */
bool att_updated;
orb_check(_att_sub, &att_updated);
bool ctrl_state_updated;
orb_check(_ctrl_state_sub, &ctrl_state_updated);
if (att_updated) {
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
if (ctrl_state_updated) {
orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
_airspeed_valid = true;
_airspeed_last_valid = hrt_absolute_time();
/* set rotation matrix */
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
_R_nb(i, j) = PX4_R(_att.R, i, j);
} else {
/* no airspeed updates for one second */
if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_valid) > 1e6) {
_airspeed_valid = false;
}
}
/* set rotation matrix and euler angles */
math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
_R_nb = q_att.to_dcm();
math::Vector<3> euler_angles;
euler_angles = _R_nb.to_euler();
_roll = euler_angles(0);
_pitch = euler_angles(1);
_yaw = euler_angles(2);
/* update TECS state */
_tecs.enable_airspeed(_airspeed_valid);
}
void
@@ -853,7 +837,7 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand)
float airspeed;
if (_airspeed_valid) {
airspeed = _airspeed.true_airspeed_m_s;
airspeed = _ctrl_state.airspeed;
} else {
airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) / 2.0f;
@@ -1084,7 +1068,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* update TECS filters */
if (!_mTecs.getEnabled()) {
_tecs.update_state(_global_pos.alt, _airspeed.indicated_airspeed_m_s, _R_nb,
_tecs.update_state(_global_pos.alt, _ctrl_state.airspeed, _R_nb,
accel_body, accel_earth, (_global_pos.timestamp > 0), in_air_alt_control);
}
@@ -1117,7 +1101,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* reset integrators */
if (_mTecs.getEnabled()) {
_mTecs.resetIntegrators();
_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
_mTecs.resetDerivatives(_ctrl_state.airspeed);
} else {
_tecs.reset_state();
}
}
_control_mode_current = FW_POSCTRL_MODE_AUTO;
@@ -1125,7 +1111,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* reset hold altitude */
_hold_alt = _global_pos.alt;
/* reset hold yaw */
_hdg_hold_yaw = _att.yaw;
_hdg_hold_yaw = _yaw;
/* get circle mode */
bool was_circle_mode = _l1_control.circle_mode();
@@ -1219,14 +1205,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
if (pos_sp_triplet.previous.valid) {
target_bearing = bearing_lastwp_currwp;
} else {
target_bearing = _att.yaw;
target_bearing = _yaw;
}
mavlink_log_info(_mavlink_fd, "#audio: Landing, heading hold");
}
// warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw));
// warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_yaw));
_l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed_2d);
_l1_control.navigate_heading(target_bearing, _yaw, ground_speed_2d);
land_noreturn_horizontal = true;
@@ -1467,7 +1453,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
if (_control_mode_current != FW_POSCTRL_MODE_POSITION) {
/* Need to init because last loop iteration was in a different mode */
_hold_alt = _global_pos.alt;
_hdg_hold_yaw = _att.yaw;
_hdg_hold_yaw = _yaw;
_hdg_hold_enabled = false; // this makes sure the waypoints are reset below
_yaw_lock_engaged = false;
}
@@ -1476,7 +1462,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* reset integrators */
if (_mTecs.getEnabled()) {
_mTecs.resetIntegrators();
_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
_mTecs.resetDerivatives(_ctrl_state.airspeed);
} else {
_tecs.reset_state();
}
}
_control_mode_current = FW_POSCTRL_MODE_POSITION;
@@ -1520,7 +1508,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
if (fabsf(_manual.y) < HDG_HOLD_MAN_INPUT_THRESH) {
/* heading / roll is zero, lock onto current heading */
if (fabsf(_att.yawspeed) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) {
if (fabsf(_ctrl_state.yaw_rate) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) {
// little yaw movement, lock to current heading
_yaw_lock_engaged = true;
@@ -1539,7 +1527,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* just switched back from non heading-hold to heading hold */
if (!_hdg_hold_enabled) {
_hdg_hold_enabled = true;
_hdg_hold_yaw = _att.yaw;
_hdg_hold_yaw = _yaw;
get_waypoint_heading_distance(_hdg_hold_yaw, HDG_HOLD_DIST_NEXT, _hdg_hold_prev_wp, _hdg_hold_curr_wp, true);
}
@@ -1587,7 +1575,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* reset integrators */
if (_mTecs.getEnabled()) {
_mTecs.resetIntegrators();
_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
_mTecs.resetDerivatives(_ctrl_state.airspeed);
} else {
_tecs.reset_state();
}
}
_control_mode_current = FW_POSCTRL_MODE_ALTITUDE;
@@ -1698,11 +1688,10 @@ FixedwingPositionControl::task_main()
*/
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
@@ -1779,10 +1768,9 @@ FixedwingPositionControl::task_main()
// XXX add timestamp check
_global_pos_valid = true;
vehicle_attitude_poll();
control_state_poll();
vehicle_setpoint_poll();
vehicle_sensor_combined_poll();
vehicle_airspeed_poll();
vehicle_manual_control_setpoint_poll();
// vehicle_baro_poll();
@@ -1888,7 +1876,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
/* use pitch max set by MT param */
limitOverride.disablePitchMaxOverride();
}
_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _ctrl_state.airspeed, v_sp, mode,
limitOverride);
} else {
if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) {
@@ -1901,8 +1889,8 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
_tecs.set_detect_underspeed_enabled(!(mode == tecs_status_s::TECS_MODE_LAND || mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM));
/* Using tecs library */
_tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp,
_airspeed.indicated_airspeed_m_s, eas2tas,
_tecs.update_pitch_throttle(_R_nb, _pitch, altitude, alt_sp, v_sp,
_ctrl_state.airspeed, eas2tas,
climbout_mode, climbout_pitch_min_rad,
throttle_min, throttle_max, throttle_cruise,
pitch_min_rad, pitch_max_rad);
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+25 -26
View File
@@ -749,8 +749,7 @@ void BlockLocalPositionEstimator::predict()
if (_integrate.get() && _sub_att.get().R_valid) {
Matrix3f R_att(_sub_att.get().R);
Vector3f a(_sub_sensor.get().accelerometer_m_s2);
Vector3f b(_x(X_bx), _x(X_by), _x(X_bz));
_u = R_att * (a - b);
_u = R_att * a;
_u(U_az) += 9.81f; // add g
} else {
@@ -812,7 +811,7 @@ void BlockLocalPositionEstimator::predict()
Q(X_bz, X_bz) = _pn_b_noise_power.get();
// continuous time kalman filter prediction
Matrix<float, n_x, 1> dx = (A * _x + B * _u) * getDt();
Vector<float, n_x> dx = (A * _x + B * _u) * getDt();
// only predict for components we have
// valid measurements for
@@ -905,16 +904,16 @@ void BlockLocalPositionEstimator::correctFlow()
_flowY += global_speed[1] * dt;
// measurement
Vector2f y;
Vector<float, 2> y;
y(0) = _flowX;
y(1) = _flowY;
// residual
Vector2f r = y - C * _x;
Vector<float, 2> r = y - C * _x;
// residual covariance, (inverse)
Matrix<float, n_y_flow, n_y_flow> S_I =
(C * _P * C.transpose() + R).inverse();
inv<float, n_y_flow>(C * _P * C.transpose() + R);
// fault detection
float beta = sqrtf((r.transpose() * (S_I * r))(0, 0));
@@ -982,17 +981,17 @@ void BlockLocalPositionEstimator::correctSonar()
}
// measurement
Matrix<float, n_y_sonar, 1> y;
Vector<float, n_y_sonar> y;
y(0) = (d - _sonarAltHome) *
cosf(_sub_att.get().roll) *
cosf(_sub_att.get().pitch);
// residual
Matrix<float, n_y_sonar, 1> r = y - C * _x;
Vector<float, n_y_sonar> r = y - C * _x;
// residual covariance, (inverse)
Matrix<float, n_y_sonar, n_y_sonar> S_I =
(C * _P * C.transpose() + R).inverse();
inv<float, n_y_sonar>(C * _P * C.transpose() + R);
// fault detection
float beta = sqrtf((r.transpose() * (S_I * r))(0, 0));
@@ -1033,7 +1032,7 @@ void BlockLocalPositionEstimator::correctSonar()
void BlockLocalPositionEstimator::correctBaro()
{
Matrix<float, n_y_baro, 1> y;
Vector<float, n_y_baro> y;
y(0) = _sub_sensor.get().baro_alt_meter[0] - _baroAltHome;
// baro measurement matrix
@@ -1047,8 +1046,8 @@ void BlockLocalPositionEstimator::correctBaro()
// residual
Matrix<float, n_y_baro, n_y_baro> S_I =
((C * _P * C.transpose()) + R).inverse();
Matrix<float, n_y_baro, 1> r = y - (C * _x);
inv<float, n_y_baro>((C * _P * C.transpose()) + R);
Vector<float, n_y_baro> r = y - (C * _x);
// fault detection
float beta = sqrtf((r.transpose() * (S_I * r))(0, 0));
@@ -1061,7 +1060,7 @@ void BlockLocalPositionEstimator::correctBaro()
}
// lower baro trust
S_I = ((C * _P * C.transpose()) + R * 10).inverse();
S_I = inv<float, n_y_baro>((C * _P * C.transpose()) + R * 10);
} else if (_baroFault) {
_baroFault = FAULT_NONE;
@@ -1105,15 +1104,15 @@ void BlockLocalPositionEstimator::correctLidar()
R(0, 0) = cov;
}
Matrix<float, n_y_lidar, 1> y;
Vector<float, n_y_lidar> y;
y.setZero();
y(0) = (d - _lidarAltHome) *
cosf(_sub_att.get().roll) *
cosf(_sub_att.get().pitch);
// residual
Matrix<float, n_y_lidar, n_y_lidar> S_I = ((C * _P * C.transpose()) + R).inverse();
Matrix<float, n_y_lidar, 1> r = y - C * _x;
Matrix<float, n_y_lidar, n_y_lidar> S_I = inv<float, n_y_lidar>((C * _P * C.transpose()) + R);
Vector<float, n_y_lidar> r = y - C * _x;
// fault detection
float beta = sqrtf((r.transpose() * (S_I * r))(0, 0));
@@ -1167,7 +1166,7 @@ void BlockLocalPositionEstimator::correctGps() // TODO : use another other met
//printf("home: lat %10g, lon, %10g alt %10g\n", _sub_home.lat, _sub_home.lon, double(_sub_home.alt));
//printf("local: x %10g y %10g z %10g\n", double(px), double(py), double(pz));
Matrix<float, 6, 1> y;
Vector<float, 6> y;
y.setZero();
y(0) = px;
y(1) = py;
@@ -1214,8 +1213,8 @@ void BlockLocalPositionEstimator::correctGps() // TODO : use another other met
R(5, 5) = var_vz;
// residual
Matrix<float, 6, 1> r = y - C * _x;
Matrix<float, 6, 6> S_I = (C * _P * C.transpose() + R).inverse();
Vector<float, 6> r = y - C * _x;
Matrix<float, 6, 6> S_I = inv<float, 6>(C * _P * C.transpose() + R);
// fault detection
float beta = sqrtf((r.transpose() * (S_I * r))(0, 0));
@@ -1246,7 +1245,7 @@ void BlockLocalPositionEstimator::correctGps() // TODO : use another other met
}
// trust GPS less
S_I = ((C * _P * C.transpose()) + R * 10).inverse();
S_I = inv<float, 6>((C * _P * C.transpose()) + R * 10);
} else if (_gpsFault) {
_gpsFault = FAULT_NONE;
@@ -1267,7 +1266,7 @@ void BlockLocalPositionEstimator::correctGps() // TODO : use another other met
void BlockLocalPositionEstimator::correctVision()
{
Matrix<float, 3, 1> y;
Vector<float, 3> y;
y.setZero();
y(0) = _sub_vision_pos.get().x - _visionHome(0);
y(1) = _sub_vision_pos.get().y - _visionHome(1);
@@ -1288,7 +1287,7 @@ void BlockLocalPositionEstimator::correctVision()
R(Y_vision_z, Y_vision_z) = _vision_z_stddev.get() * _vision_z_stddev.get();
// residual
Matrix<float, n_y_vision, n_y_vision> S_I = ((C * _P * C.transpose()) + R).inverse();
Matrix<float, n_y_vision, n_y_vision> S_I = inv<float, n_y_vision>((C * _P * C.transpose()) + R);
Matrix<float, n_y_vision, 1> r = y - C * _x;
// fault detection
@@ -1302,7 +1301,7 @@ void BlockLocalPositionEstimator::correctVision()
}
// trust less
S_I = ((C * _P * C.transpose()) + R * 10).inverse();
S_I = inv<float, n_y_vision>((C * _P * C.transpose()) + R * 10);
} else if (_visionFault) {
_visionFault = FAULT_NONE;
@@ -1323,7 +1322,7 @@ void BlockLocalPositionEstimator::correctVision()
void BlockLocalPositionEstimator::correctmocap()
{
Matrix<float, n_y_mocap, 1> y;
Vector<float, n_y_mocap> y;
y.setZero();
y(Y_mocap_x) = _sub_mocap.get().x - _mocapHome(0);
y(Y_mocap_y) = _sub_mocap.get().y - _mocapHome(1);
@@ -1346,7 +1345,7 @@ void BlockLocalPositionEstimator::correctmocap()
R(Y_mocap_z, Y_mocap_z) = mocap_p_var;
// residual
Matrix<float, n_y_mocap, n_y_mocap> S_I = ((C * _P * C.transpose()) + R).inverse();
Matrix<float, n_y_mocap, n_y_mocap> S_I = inv<float, n_y_mocap>((C * _P * C.transpose()) + R);
Matrix<float, n_y_mocap, 1> r = y - C * _x;
// fault detection
@@ -1360,7 +1359,7 @@ void BlockLocalPositionEstimator::correctmocap()
}
// trust less
S_I = ((C * _P * C.transpose()) + R * 10).inverse();
S_I = inv<float, n_y_mocap>((C * _P * C.transpose()) + R * 10);
} else if (_mocapFault) {
_mocapFault = FAULT_NONE;
src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
+3 -3
View File
@@ -6,7 +6,7 @@
#include <lib/geo/geo.h>
#ifdef USE_MATRIX_LIB
#include "matrix/src/Matrix.hpp"
#include "matrix/Matrix.hpp"
using namespace matrix;
#else
#include <Eigen/Eigen>
@@ -304,7 +304,7 @@ private:
perf_counter_t _err_perf;
// state space
Matrix<float, n_x, 1> _x; // state vector
Matrix<float, n_u, 1> _u; // input vector
Vector<float, n_x> _x; // state vector
Vector<float, n_u> _u; // input vector
Matrix<float, n_x, n_x> _P; // state covariance matrix
};
src/modules/local_position_estimator/CMakeLists.txt
-2
View File
@@ -34,8 +34,6 @@ if(${OS} STREQUAL "nuttx")
list(APPEND MODULE_CFLAGS -Wframe-larger-than=6500)
elseif(${OS} STREQUAL "posix")
list(APPEND MODULE_CFLAGS -Wno-error)
# add matrix tests
add_subdirectory(matrix)
endif()
# use custom matrix lib instead of Eigen
src/modules/local_position_estimator/matrix/.gitignore
-1
View File
@@ -1 +0,0 @@
build*/
src/modules/local_position_estimator/matrix/CMakeLists.txt
-13
View File
@@ -1,13 +0,0 @@
cmake_minimum_required(VERSION 2.8)
project(matrix CXX)
set(CMAKE_BUILD_TYPE "RelWithDebInfo")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Weffc++")
enable_testing()
include_directories(src)
add_subdirectory(test)
src/modules/local_position_estimator/matrix/src/Matrix.hpp
-464
View File
@@ -1,464 +0,0 @@
#pragma once
#include <stdio.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
namespace matrix
{
template<typename T, size_t M, size_t N>
class Matrix
{
private:
T _data[M][N];
size_t _rows;
size_t _cols;
public:
Matrix() :
_data(),
_rows(M),
_cols(N)
{
}
Matrix(const T *data) :
_data(),
_rows(M),
_cols(N)
{
memcpy(_data, data, sizeof(_data));
}
Matrix(const Matrix &other) :
_data(),
_rows(M),
_cols(N)
{
memcpy(_data, other._data, sizeof(_data));
}
Matrix(T x, T y, T z) :
_data(),
_rows(M),
_cols(N)
{
// TODO, limit to only 3x1 matrices
_data[0][0] = x;
_data[1][0] = y;
_data[2][0] = z;
}
/**
* Accessors/ Assignment etc.
*/
T *data()
{
return _data[0];
}
inline size_t rows() const
{
return _rows;
}
inline size_t cols() const
{
return _rows;
}
inline T operator()(size_t i) const
{
return _data[i][0];
}
inline T operator()(size_t i, size_t j) const
{
return _data[i][j];
}
inline T &operator()(size_t i)
{
return _data[i][0];
}
inline T &operator()(size_t i, size_t j)
{
return _data[i][j];
}
/**
* Matrix Operations
*/
// this might use a lot of programming memory
// since it instantiates a class for every
// required mult pair, but it provides
// compile time size_t checking
template<size_t P>
Matrix<T, M, P> operator*(const Matrix<T, N, P> &other) const
{
const Matrix<T, M, N> &self = *this;
Matrix<T, M, P> res;
res.setZero();
for (size_t i = 0; i < M; i++) {
for (size_t k = 0; k < P; k++) {
for (size_t j = 0; j < N; j++) {
res(i, k) += self(i, j) * other(j, k);
}
}
}
return res;
}
Matrix<T, M, N> operator+(const Matrix<T, M, N> &other) const
{
Matrix<T, M, N> res;
const Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
res(i , j) = self(i, j) + other(i, j);
}
}
return res;
}
bool operator==(const Matrix<T, M, N> &other) const
{
Matrix<T, M, N> res;
const Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
if (self(i , j) != other(i, j)) {
return false;
}
}
}
return true;
}
Matrix<T, M, N> operator-(const Matrix<T, M, N> &other) const
{
Matrix<T, M, N> res;
const Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
res(i , j) = self(i, j) - other(i, j);
}
}
return res;
}
void operator+=(const Matrix<T, M, N> &other)
{
Matrix<T, M, N> &self = *this;
self = self + other;
}
void operator-=(const Matrix<T, M, N> &other)
{
Matrix<T, M, N> &self = *this;
self = self - other;
}
void operator*=(const Matrix<T, M, N> &other)
{
Matrix<T, M, N> &self = *this;
self = self * other;
}
/**
* Scalar Operations
*/
Matrix<T, M, N> operator*(T scalar) const
{
Matrix<T, M, N> res;
const Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
res(i , j) = self(i, j) * scalar;
}
}
return res;
}
Matrix<T, M, N> operator+(T scalar) const
{
Matrix<T, M, N> res;
Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
res(i , j) = self(i, j) + scalar;
}
}
return res;
}
void operator*=(T scalar)
{
Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
self(i, j) = self(i, j) * scalar;
}
}
}
void operator/=(T scalar)
{
Matrix<T, M, N> &self = *this;
self = self * (1.0f / scalar);
}
/**
* Misc. Functions
*/
void print()
{
Matrix<T, M, N> &self = *this;
printf("\n");
for (size_t i = 0; i < M; i++) {
printf("[");
for (size_t j = 0; j < N; j++) {
printf("%10g\t", double(self(i, j)));
}
printf("]\n");
}
}
Matrix<T, N, M> transpose() const
{
Matrix<T, N, M> res;
const Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
res(j, i) = self(i, j);
}
}
return res;
}
Matrix<T, M, M> expm(float dt, size_t n) const
{
Matrix<float, M, M> res;
res.setIdentity();
Matrix<float, M, M> A_pow = *this;
float k_fact = 1;
size_t k = 1;
while (k < n) {
res += A_pow * (float(pow(dt, k)) / k_fact);
if (k == n) { break; }
A_pow *= A_pow;
k_fact *= k;
k++;
}
return res;
}
Matrix<T, M, 1> diagonal() const
{
Matrix<T, M, 1> res;
// force square for now
const Matrix<T, M, M> &self = *this;
for (size_t i = 0; i < M; i++) {
res(i) = self(i, i);
}
return res;
}
void setZero()
{
memset(_data, 0, sizeof(_data));
}
void setIdentity()
{
setZero();
Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M and i < N; i++) {
self(i, i) = 1;
}
}
inline void swapRows(size_t a, size_t b)
{
if (a == b) { return; }
Matrix<T, M, N> &self = *this;
for (size_t j = 0; j < cols(); j++) {
T tmp = self(a, j);
self(a, j) = self(b, j);
self(b, j) = tmp;
}
}
inline void swapCols(size_t a, size_t b)
{
if (a == b) { return; }
Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < rows(); i++) {
T tmp = self(i, a);
self(i, a) = self(i, b);
self(i, b) = tmp;
}
}
/**
* inverse based on LU factorization with partial pivotting
*/
Matrix <T, M, M> inverse() const
{
Matrix<T, M, M> L;
L.setIdentity();
const Matrix<T, M, M> &A = (*this);
Matrix<T, M, M> U = A;
Matrix<T, M, M> P;
P.setIdentity();
//printf("A:\n"); A.print();
// for all diagonal elements
for (size_t n = 0; n < N; n++) {
// if diagonal is zero, swap with row below
if (fabsf(U(n, n)) < 1e-8f) {
//printf("trying pivot for row %d\n",n);
for (size_t i = 0; i < N; i++) {
if (i == n) { continue; }
//printf("\ttrying row %d\n",i);
if (fabsf(U(i, n)) > 1e-8f) {
//printf("swapped %d\n",i);
U.swapRows(i, n);
P.swapRows(i, n);
}
}
}
#ifdef MATRIX_ASSERT
//printf("A:\n"); A.print();
//printf("U:\n"); U.print();
//printf("P:\n"); P.print();
//fflush(stdout);
ASSERT(fabsf(U(n, n)) > 1e-8f);
#endif
// failsafe, return zero matrix
if (fabsf(U(n, n)) < 1e-8f) {
Matrix<T, M, M> zero;
zero.setZero();
return zero;
}
// for all rows below diagonal
for (size_t i = (n + 1); i < N; i++) {
L(i, n) = U(i, n) / U(n, n);
// add i-th row and n-th row
// multiplied by: -a(i,n)/a(n,n)
for (size_t k = n; k < N; k++) {
U(i, k) -= L(i, n) * U(n, k);
}
}
}
//printf("L:\n"); L.print();
//printf("U:\n"); U.print();
// solve LY=P*I for Y by forward subst
Matrix<T, M, M> Y = P;
// for all columns of Y
for (size_t c = 0; c < N; c++) {
// for all rows of L
for (size_t i = 0; i < N; i++) {
// for all columns of L
for (size_t j = 0; j < i; j++) {
// for all existing y
// subtract the component they
// contribute to the solution
Y(i, c) -= L(i, j) * Y(j, c);
}
// divide by the factor
// on current
// term to be solved
// Y(i,c) /= L(i,i);
// but L(i,i) = 1.0
}
}
//printf("Y:\n"); Y.print();
// solve Ux=y for x by back subst
Matrix<T, M, M> X = Y;
// for all columns of X
for (size_t c = 0; c < N; c++) {
// for all rows of U
for (size_t k = 0; k < N; k++) {
// have to go in reverse order
size_t i = N - 1 - k;
// for all columns of U
for (size_t j = i + 1; j < N; j++) {
// for all existing x
// subtract the component they
// contribute to the solution
X(i, c) -= U(i, j) * X(j, c);
}
// divide by the factor
// on current
// term to be solved
X(i, c) /= U(i, i);
}
}
//printf("X:\n"); X.print();
return X;
}
};
typedef Matrix<float, 2, 1> Vector2f;
typedef Matrix<float, 3, 1> Vector3f;
typedef Matrix<float, 3, 3> Matrix3f;
}; // namespace matrix
src/modules/local_position_estimator/matrix/test/CMakeLists.txt
-15
View File
@@ -1,15 +0,0 @@
set(tests
setIdentity
inverse
matrixMult
vectorAssignment
matrixAssignment
matrixScalarMult
transpose
)
foreach(test ${tests})
add_executable(${test}
${test}.cpp)
add_test(${test} ${test})
endforeach()
src/modules/local_position_estimator/matrix/test/inverse.cpp
-30
View File
@@ -1,30 +0,0 @@
#include "Matrix.hpp"
#include <assert.h>
#include <stdio.h>
using namespace matrix;
int main()
{
float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1};
Matrix3f A(data);
Matrix3f A_I = A.inverse();
float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1};
Matrix3f A_I_check(data_check);
(void)A_I;
assert(A_I == A_I_check);
// stess test
static const size_t n = 100;
Matrix<float, n, n> A_large;
A_large.setIdentity();
Matrix<float, n, n> A_large_I;
A_large_I.setZero();
for (size_t i = 0; i < 100; i++) {
A_large_I = A_large.inverse();
assert(A_large == A_large_I);
}
return 0;
}
src/modules/local_position_estimator/matrix/test/matrixAssignment.cpp
-29
View File
@@ -1,29 +0,0 @@
#include "Matrix.hpp"
#include <assert.h>
using namespace matrix;
int main()
{
Matrix3f m;
m.setZero();
m(0, 0) = 1;
m(0, 1) = 2;
m(0, 2) = 3;
m(1, 0) = 4;
m(1, 1) = 5;
m(1, 2) = 6;
m(2, 0) = 7;
m(2, 1) = 8;
m(2, 2) = 9;
m.print();
float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
Matrix3f m2(data);
m2.print();
assert(m == m2);
return 0;
}
src/modules/local_position_estimator/matrix/test/matrixMult.cpp
-26
View File
@@ -1,26 +0,0 @@
#include "Matrix.hpp"
#include <assert.h>
#include <stdio.h>
using namespace matrix;
int main()
{
float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1};
Matrix3f A(data);
float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1};
Matrix3f A_I(data_check);
Matrix3f I;
I.setIdentity();
A.print();
A_I.print();
Matrix3f R = A * A_I;
R.print();
assert(R == I);
Matrix3f R2 = A;
R2 *= A_I;
R2.print();
assert(R2 == I);
return 0;
}
src/modules/local_position_estimator/matrix/test/matrixScalarMult.cpp
-18
View File
@@ -1,18 +0,0 @@
#include "Matrix.hpp"
#include <assert.h>
#include <stdio.h>
using namespace matrix;
int main()
{
float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
Matrix3f A(data);
A = A * 2;
float data_check[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
Matrix3f A_check(data_check);
A.print();
A_check.print();
assert(A == A_check);
return 0;
}
src/modules/local_position_estimator/matrix/test/setIdentity.cpp
-25
View File
@@ -1,25 +0,0 @@
#include "Matrix.hpp"
#include <assert.h>
using namespace matrix;
int main()
{
Matrix3f A;
A.setIdentity();
assert(A.rows() == 3);
assert(A.cols() == 3);
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
if (i == j) {
assert(A(i, j) == 1);
} else {
assert(A(i, j) == 0);
}
}
}
return 0;
}
src/modules/local_position_estimator/matrix/test/transpose.cpp
-18
View File
@@ -1,18 +0,0 @@
#include "Matrix.hpp"
#include <assert.h>
#include <stdio.h>
using namespace matrix;
int main()
{
float data[9] = {1, 2, 3, 4, 5, 6};
Matrix<float, 2, 3> A(data);
Matrix<float, 3, 2> A_T = A.transpose();
float data_check[9] = {1, 4, 2, 5, 3, 6};
Matrix<float, 3, 2> A_T_check(data_check);
A_T.print();
A_T_check.print();
assert(A_T == A_T_check);
return 0;
}
src/modules/local_position_estimator/matrix/test/vectorAssignment.cpp
-28
View File
@@ -1,28 +0,0 @@
#include "Matrix.hpp"
#include <assert.h>
using namespace matrix;
int main()
{
Vector3f v;
v(0) = 1;
v(1) = 2;
v(2) = 3;
v.print();
assert(v(0) == 1);
assert(v(1) == 2);
assert(v(2) == 3);
Vector3f v2(4, 5, 6);
v2.print();
assert(v2(0) == 4);
assert(v2(1) == 5);
assert(v2(2) == 6);
return 0;
}
src/modules/mavlink/CMakeLists.txt
-1
View File
@@ -56,7 +56,6 @@ px4_add_module(
mavlink_rate_limiter.cpp
mavlink_receiver.cpp
mavlink_ftp.cpp
mavlink_params.c
DEPENDS
platforms__common
)
src/modules/mavlink/mavlink_main.cpp
+8 -1
View File
@@ -698,6 +698,9 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
} else {
_is_usb_uart = true;
/* USB has no baudrate, but use a magic number for 'fast' */
_baudrate = 2000000;
_rstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB;
}
#if defined (__PX4_LINUX) || defined (__PX4_DARWIN)
@@ -2094,8 +2097,12 @@ Mavlink::display_status()
printf("3DR RADIO\n");
break;
case telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB:
printf("USB CDC\n");
break;
default:
printf("UNKNOWN RADIO\n");
printf("GENERIC LINK OR RADIO\n");
break;
}
src/modules/mavlink/mavlink_main.h
+2
View File
@@ -288,6 +288,8 @@ public:
float get_rate_mult();
float get_baudrate() { return _baudrate; }
/* Functions for waiting to start transmission until message received. */
void set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
bool get_has_received_messages() { return _received_messages; }
src/modules/mavlink/mavlink_mission.cpp
+10 -11
View File
@@ -422,7 +422,9 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg)
} else {
_mavlink->send_statustext_critical("REJ. WP CMD: partner id mismatch");
if (_verbose) { warnx("WPM: MISSION_ACK ERROR: rejected, partner ID mismatch"); }
if (_verbose) {
warnx("WPM: MISSION_ACK ERR: ID mismatch");
}
}
}
}
@@ -473,13 +475,13 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg)
if (_state == MAVLINK_WPM_STATE_IDLE || _state == MAVLINK_WPM_STATE_SENDLIST) {
_time_last_recv = hrt_absolute_time();
if (_count > 0) {
_state = MAVLINK_WPM_STATE_SENDLIST;
_transfer_seq = 0;
_transfer_count = _count;
_transfer_partner_sysid = msg->sysid;
_transfer_partner_compid = msg->compid;
_state = MAVLINK_WPM_STATE_SENDLIST;
_transfer_seq = 0;
_transfer_count = _count;
_transfer_partner_sysid = msg->sysid;
_transfer_partner_compid = msg->compid;
if (_count > 0) {
if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK, %u mission items to send", _transfer_count); }
} else {
@@ -598,9 +600,8 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
/* alternate dataman ID anyway to let navigator know about changes */
update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0);
_mavlink->send_statustext_info("WPM: COUNT 0: CLEAR MISSION");
// TODO send ACK?
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED);
_transfer_in_progress = false;
return;
}
@@ -712,8 +713,6 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)
/* got all new mission items successfully */
if (_verbose) { warnx("WPM: MISSION_ITEM got all %u items, current_seq=%u, changing state to MAVLINK_WPM_STATE_IDLE", _transfer_count, _transfer_current_seq); }
_mavlink->send_statustext_info("WPM: Transfer complete.");
_state = MAVLINK_WPM_STATE_IDLE;
if (update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq) == OK) {
src/modules/mavlink/mavlink_params.c
-2
View File
@@ -31,8 +31,6 @@
*
****************************************************************************/
#include <systemlib/param/param.h>
/**
* MAVLink system ID
* @group MAVLink
src/modules/mavlink/mavlink_receiver.cpp
+39 -34
View File
@@ -1116,7 +1116,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
/* yaw */
rc.values[2] = man.r / 2 + 1500;
/* throttle */
rc.values[3] = man.z / 1 + 1000;
rc.values[3] = fminf(fmaxf(man.z / 0.9f + 800, 1000.0f), 2000.0f);
/* decode all switches which fit into the channel mask */
unsigned max_switch = (sizeof(man.buttons) * 8);
@@ -1307,18 +1307,22 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
float dt;
if (_hil_last_frame == 0 ||
(imu.time_usec - _hil_last_frame) > (0.1f * 1e6f) ||
(_hil_last_frame >= imu.time_usec)) {
(timestamp <= _hil_last_frame) ||
(timestamp - _hil_last_frame) > (0.1f * 1e6f) ||
(_hil_last_frame >= timestamp)) {
dt = 0.01f; /* default to 100 Hz */
memset(&_hil_prev_gyro[0], 0, sizeof(_hil_prev_gyro));
_hil_prev_accel[0] = 0.0f;
_hil_prev_accel[1] = 0.0f;
_hil_prev_accel[2] = 9.866f;
} else {
dt = (imu.time_usec - _hil_last_frame) / 1e6f;
dt = (timestamp - _hil_last_frame) / 1e6f;
}
_hil_last_frame = imu.time_usec;
_hil_last_frame = timestamp;
/* airspeed */
{
struct airspeed_s airspeed;
memset(&airspeed, 0, sizeof(airspeed));
struct airspeed_s airspeed = {};
float ias = calc_indicated_airspeed(imu.diff_pressure * 1e2f);
// XXX need to fix this
@@ -1338,8 +1342,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
/* gyro */
{
struct gyro_report gyro;
memset(&gyro, 0, sizeof(gyro));
struct gyro_report gyro = {};
gyro.timestamp = timestamp;
gyro.x_raw = imu.xgyro * 1000.0f;
@@ -1360,8 +1363,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
/* accelerometer */
{
struct accel_report accel;
memset(&accel, 0, sizeof(accel));
struct accel_report accel = {};
accel.timestamp = timestamp;
accel.x_raw = imu.xacc / mg2ms2;
@@ -1382,8 +1384,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
/* magnetometer */
{
struct mag_report mag;
memset(&mag, 0, sizeof(mag));
struct mag_report mag = {};
mag.timestamp = timestamp;
mag.x_raw = imu.xmag * 1000.0f;
@@ -1404,8 +1405,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
/* baro */
{
struct baro_report baro;
memset(&baro, 0, sizeof(baro));
struct baro_report baro = {};
baro.timestamp = timestamp;
baro.pressure = imu.abs_pressure;
@@ -1422,21 +1422,20 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
/* sensor combined */
{
struct sensor_combined_s hil_sensors;
memset(&hil_sensors, 0, sizeof(hil_sensors));
struct sensor_combined_s hil_sensors = {};
hil_sensors.timestamp = timestamp;
hil_sensors.gyro_raw[0] = imu.xgyro * 1000.0f;
hil_sensors.gyro_raw[1] = imu.ygyro * 1000.0f;
hil_sensors.gyro_raw[2] = imu.zgyro * 1000.0f;
hil_sensors.gyro_rad_s[0] = ((imu.xgyro * dt + _hil_prev_gyro[0]) / 2.0f) / dt;
hil_sensors.gyro_rad_s[0] = imu.xgyro;
hil_sensors.gyro_rad_s[1] = imu.ygyro;
hil_sensors.gyro_rad_s[2] = imu.zgyro;
hil_sensors.gyro_integral_rad[0] = (hil_sensors.gyro_rad_s[0] * dt + _hil_prev_gyro[0]) / 2.0f;
hil_sensors.gyro_integral_rad[1] = (hil_sensors.gyro_rad_s[1] * dt + _hil_prev_gyro[1]) / 2.0f;
hil_sensors.gyro_integral_rad[2] = (hil_sensors.gyro_rad_s[2] * dt + _hil_prev_gyro[2]) / 2.0f;
memcpy(&_hil_prev_gyro[0], &hil_sensors.gyro_integral_rad[0], sizeof(_hil_prev_gyro));
hil_sensors.gyro_integral_rad[0] = 0.5f * (imu.xgyro + _hil_prev_gyro[0]) * dt;
hil_sensors.gyro_integral_rad[1] = 0.5f * (imu.ygyro + _hil_prev_gyro[1]) * dt;
hil_sensors.gyro_integral_rad[2] = 0.5f * (imu.zgyro + _hil_prev_gyro[2]) * dt;
memcpy(&_hil_prev_gyro[0], &hil_sensors.gyro_rad_s[0], sizeof(_hil_prev_gyro));
hil_sensors.gyro_integral_dt[0] = dt * 1e6f;
hil_sensors.gyro_timestamp[0] = timestamp;
hil_sensors.gyro_priority[0] = ORB_PRIO_HIGH;
@@ -1447,10 +1446,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
hil_sensors.accelerometer_m_s2[0] = imu.xacc;
hil_sensors.accelerometer_m_s2[1] = imu.yacc;
hil_sensors.accelerometer_m_s2[2] = imu.zacc;
hil_sensors.accelerometer_integral_m_s[0] = (imu.xacc * dt + _hil_prev_accel[0]) / 2.0f;
hil_sensors.accelerometer_integral_m_s[1] = (imu.yacc * dt + _hil_prev_accel[1]) / 2.0f;
hil_sensors.accelerometer_integral_m_s[2] = (imu.zacc * dt + _hil_prev_accel[2]) / 2.0f;
memcpy(&_hil_prev_accel[0], &hil_sensors.accelerometer_integral_m_s[0], sizeof(_hil_prev_accel));
hil_sensors.accelerometer_integral_m_s[0] = 0.5f * (imu.xacc + _hil_prev_accel[0]) * dt;
hil_sensors.accelerometer_integral_m_s[1] = 0.5f * (imu.yacc + _hil_prev_accel[1]) * dt;
hil_sensors.accelerometer_integral_m_s[2] = 0.5f * (imu.zacc + _hil_prev_accel[2]) * dt;
memcpy(&_hil_prev_accel[0], &hil_sensors.accelerometer_m_s2[0], sizeof(_hil_prev_accel));
hil_sensors.accelerometer_integral_dt[0] = dt * 1e6f;
hil_sensors.accelerometer_mode[0] = 0; // TODO what is this?
hil_sensors.accelerometer_range_m_s2[0] = 32.7f; // int16
@@ -1493,12 +1492,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
/* battery status */
{
struct battery_status_s hil_battery_status;
memset(&hil_battery_status, 0, sizeof(hil_battery_status));
struct battery_status_s hil_battery_status = {};
hil_battery_status.timestamp = timestamp;
hil_battery_status.voltage_v = 11.1f;
hil_battery_status.voltage_filtered_v = 11.1f;
hil_battery_status.voltage_v = 11.5f;
hil_battery_status.voltage_filtered_v = 11.5f;
hil_battery_status.current_a = 10.0f;
hil_battery_status.discharged_mah = -1.0f;
@@ -1759,7 +1757,7 @@ MavlinkReceiver::receive_thread(void *arg)
uint8_t buf[1600];
#else
/* the serial port buffers internally as well, we just need to fit a small chunk */
uint8_t buf[32];
uint8_t buf[64];
#endif
mavlink_message_t msg;
@@ -1799,10 +1797,17 @@ MavlinkReceiver::receive_thread(void *arg)
while (!_mavlink->_task_should_exit) {
if (poll(&fds[0], 1, timeout) > 0) {
if (_mavlink->get_protocol() == SERIAL) {
/*
* to avoid reading very small chunks wait for data before reading
* this is designed to target one message, so >20 bytes at a time
*/
const unsigned character_count = 20;
/* non-blocking read. read may return negative values */
if ((nread = ::read(uart_fd, buf, sizeof(buf))) < (ssize_t)sizeof(buf)) {
/* to avoid reading very small chunks wait for data before reading */
usleep(1000);
if ((nread = ::read(uart_fd, buf, sizeof(buf))) < (ssize_t)character_count) {
unsigned sleeptime = (1.0f / (_mavlink->get_baudrate() / 10)) * character_count * 1000000;
usleep(sleeptime);
}
}
#ifdef __PX4_POSIX
src/modules/mc_att_control/CMakeLists.txt
-1
View File
@@ -40,7 +40,6 @@ px4_add_module(
COMPILE_FLAGS ${MODULE_CFLAGS}
SRCS
mc_att_control_main.cpp
mc_att_control_params.c
DEPENDS
platforms__common
)
src/modules/mc_att_control/mc_att_control_main.cpp
+48 -52
View File
@@ -39,7 +39,6 @@
* Daniel Mellinger and Vijay Kumar. Minimum Snap Trajectory Generation and Control for Quadrotors.
* Int. Conf. on Robotics and Automation, Shanghai, China, May 2011.
*
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lorenz@px4.io>
* @author Anton Babushkin <anton.babushkin@me.com>
*
@@ -75,7 +74,7 @@
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/fw_virtual_rates_setpoint.h>
#include <uORB/topics/mc_virtual_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_armed.h>
@@ -127,7 +126,7 @@ private:
bool _task_should_exit; /**< if true, task_main() should exit */
int _control_task; /**< task handle */
int _v_att_sub; /**< vehicle attitude subscription */
int _ctrl_state_sub; /**< control state subscription */
int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */
int _v_rates_sp_sub; /**< vehicle rates setpoint subscription */
int _v_control_mode_sub; /**< vehicle control mode subscription */
@@ -146,7 +145,7 @@ private:
bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */
struct vehicle_attitude_s _v_att; /**< vehicle attitude */
struct control_state_s _ctrl_state; /**< control state */
struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */
struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */
struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */
@@ -190,12 +189,10 @@ private:
param_t pitch_rate_max;
param_t yaw_rate_max;
param_t man_roll_max;
param_t man_pitch_max;
param_t man_yaw_max;
param_t acro_roll_max;
param_t acro_pitch_max;
param_t acro_yaw_max;
param_t rattitude_thres;
} _params_handles; /**< handles for interesting parameters */
@@ -212,11 +209,8 @@ private:
float yaw_rate_max;
math::Vector<3> mc_rate_max; /**< attitude rate limits in stabilized modes */
float man_roll_max;
float man_pitch_max;
float man_yaw_max;
math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */
float rattitude_thres;
} _params;
/**
@@ -303,7 +297,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_control_task(-1),
/* subscriptions */
_v_att_sub(-1),
_ctrl_state_sub(-1),
_v_att_sp_sub(-1),
_v_control_mode_sub(-1),
_params_sub(-1),
@@ -325,7 +319,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency"))
{
memset(&_v_att, 0, sizeof(_v_att));
memset(&_ctrl_state, 0, sizeof(_ctrl_state));
memset(&_v_att_sp, 0, sizeof(_v_att_sp));
memset(&_v_rates_sp, 0, sizeof(_v_rates_sp));
memset(&_manual_control_sp, 0, sizeof(_manual_control_sp));
@@ -346,11 +340,9 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_params.roll_rate_max = 0.0f;
_params.pitch_rate_max = 0.0f;
_params.yaw_rate_max = 0.0f;
_params.man_roll_max = 0.0f;
_params.man_pitch_max = 0.0f;
_params.man_yaw_max = 0.0f;
_params.mc_rate_max.zero();
_params.acro_rate_max.zero();
_params.rattitude_thres = 1.0f;
_rates_prev.zero();
_rates_sp.zero();
@@ -380,12 +372,10 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_params_handles.roll_rate_max = param_find("MC_ROLLRATE_MAX");
_params_handles.pitch_rate_max = param_find("MC_PITCHRATE_MAX");
_params_handles.yaw_rate_max = param_find("MC_YAWRATE_MAX");
_params_handles.man_roll_max = param_find("MC_MAN_R_MAX");
_params_handles.man_pitch_max = param_find("MC_MAN_P_MAX");
_params_handles.man_yaw_max = param_find("MC_MAN_Y_MAX");
_params_handles.acro_roll_max = param_find("MC_ACRO_R_MAX");
_params_handles.acro_pitch_max = param_find("MC_ACRO_P_MAX");
_params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX");
_params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX");
_params_handles.rattitude_thres = param_find("MC_RATT_TH");
/* fetch initial parameter values */
parameters_update();
@@ -466,14 +456,6 @@ MulticopterAttitudeControl::parameters_update()
param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max);
_params.mc_rate_max(2) = math::radians(_params.yaw_rate_max);
/* manual attitude control scale */
param_get(_params_handles.man_roll_max, &_params.man_roll_max);
param_get(_params_handles.man_pitch_max, &_params.man_pitch_max);
param_get(_params_handles.man_yaw_max, &_params.man_yaw_max);
_params.man_roll_max = math::radians(_params.man_roll_max);
_params.man_pitch_max = math::radians(_params.man_pitch_max);
_params.man_yaw_max = math::radians(_params.man_yaw_max);
/* manual rate control scale and auto mode roll/pitch rate limits */
param_get(_params_handles.acro_roll_max, &v);
_params.acro_rate_max(0) = math::radians(v);
@@ -482,6 +464,9 @@ MulticopterAttitudeControl::parameters_update()
param_get(_params_handles.acro_yaw_max, &v);
_params.acro_rate_max(2) = math::radians(v);
/* stick deflection needed in rattitude mode to control rates not angles */
param_get(_params_handles.rattitude_thres, &_params.rattitude_thres);
_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
return OK;
@@ -614,9 +599,9 @@ MulticopterAttitudeControl::control_attitude(float dt)
math::Matrix<3, 3> R_sp;
R_sp.set(_v_att_sp.R_body);
/* rotation matrix for current state */
math::Matrix<3, 3> R;
R.set(_v_att.R);
/* get current rotation matrix from control state quaternions */
math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
math::Matrix<3, 3> R = q_att.to_dcm();
/* all input data is ready, run controller itself */
@@ -708,9 +693,9 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
/* current body angular rates */
math::Vector<3> rates;
rates(0) = _v_att.rollspeed;
rates(1) = _v_att.pitchspeed;
rates(2) = _v_att.yawspeed;
rates(0) = _ctrl_state.roll_rate;
rates(1) = _ctrl_state.pitch_rate;
rates(2) = _ctrl_state.yaw_rate;
/* angular rates error */
math::Vector<3> rates_err = _rates_sp - rates;
@@ -748,7 +733,7 @@ MulticopterAttitudeControl::task_main()
*/
_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
_v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
@@ -762,7 +747,7 @@ MulticopterAttitudeControl::task_main()
/* wakeup source: vehicle attitude */
px4_pollfd_struct_t fds[1];
fds[0].fd = _v_att_sub;
fds[0].fd = _ctrl_state_sub;
fds[0].events = POLLIN;
while (!_task_should_exit) {
@@ -798,8 +783,8 @@ MulticopterAttitudeControl::task_main()
dt = 0.02f;
}
/* copy attitude topic */
orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att);
/* copy attitude and control state topics */
orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
/* check for updates in other topics */
parameter_update_poll();
@@ -809,23 +794,34 @@ MulticopterAttitudeControl::task_main()
vehicle_status_poll();
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
* even bother running the attitude controllers */
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;
}
}
if (_v_control_mode.flag_control_attitude_enabled) {
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();
/* 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);
}
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) {
@@ -866,7 +862,7 @@ MulticopterAttitudeControl::task_main()
_actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f;
_actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f;
_actuators.timestamp = hrt_absolute_time();
_actuators.timestamp_sample = _v_att.timestamp;
_actuators.timestamp_sample = _ctrl_state.timestamp;
_controller_status.roll_rate_integ = _rates_int(0);
_controller_status.pitch_rate_integ = _rates_int(1);
@@ -976,4 +972,4 @@ int mc_att_control_main(int argc, char *argv[])
warnx("unrecognized command");
return 1;
}
}
src/modules/mc_att_control/mc_att_control_params.c
+16 -6
View File
@@ -35,13 +35,10 @@
* @file mc_att_control_params.c
* Parameters for multicopter attitude controller.
*
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lorenz@px4.io>
* @author Anton Babushkin <anton@px4.io>
*/
#include <systemlib/param/param.h>
/**
* Roll P gain
*
@@ -251,7 +248,7 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 60.0f);
* @max 360.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 90.0f);
PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 360.0f);
/**
* Max acro pitch rate
@@ -261,7 +258,7 @@ PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 90.0f);
* @max 360.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 90.0f);
PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 360.0f);
/**
* Max acro yaw rate
@@ -270,4 +267,17 @@ PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 90.0f);
* @min 0.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 120.0f);
PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 360.0f);
/**
* Threshold for Rattitude mode
*
* Manual input needed in order to override attitude control rate setpoints
* and instead pass manual stick inputs as rate setpoints
*
* @unit
* @min 0.0
* @max 1.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_RATT_TH, 1.0f);
src/modules/mc_att_control_multiplatform/CMakeLists.txt
-1
View File
@@ -38,7 +38,6 @@ px4_add_module(
mc_att_control_start_nuttx.cpp
mc_att_control.cpp
mc_att_control_base.cpp
mc_att_control_params.c
DEPENDS
platforms__common
)
src/modules/mc_att_control_multiplatform/mc_att_control_params.c
+1 -5
View File
@@ -36,14 +36,10 @@
* Parameters for multicopter attitude controller.
*
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Lorenz Meier <lorenz@px4.io>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <px4_defines.h>
#include "mc_att_control_params.h"
#include <systemlib/param/param.h>
/**
* Roll P gain
*
src/modules/mc_pos_control/CMakeLists.txt
-1
View File
@@ -36,7 +36,6 @@ px4_add_module(
STACK 1200
SRCS
mc_pos_control_main.cpp
mc_pos_control_params.c
DEPENDS
platforms__common
)
src/modules/mc_pos_control/mc_pos_control_main.cpp
+71 -33
View File
@@ -68,7 +68,7 @@
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
@@ -119,14 +119,14 @@ public:
int start();
private:
const float alt_ctl_dz = 0.2f;
const float alt_ctl_dz = 0.1f;
bool _task_should_exit; /**< if true, task should exit */
int _control_task; /**< task handle for task */
int _mavlink_fd; /**< mavlink fd */
int _vehicle_status_sub; /**< vehicle status subscription */
int _att_sub; /**< vehicle attitude subscription */
int _ctrl_state_sub; /**< control state subscription */
int _att_sp_sub; /**< vehicle attitude setpoint */
int _control_mode_sub; /**< vehicle control mode subscription */
int _params_sub; /**< notification of parameter updates */
@@ -142,7 +142,7 @@ private:
orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint publication */
struct vehicle_status_s _vehicle_status; /**< vehicle status */
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct control_state_s _ctrl_state; /**< vehicle attitude */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
struct manual_control_setpoint_s _manual; /**< r/c channel data */
struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */
@@ -230,6 +230,9 @@ private:
math::Vector<3> _vel_prev; /**< velocity on previous step */
math::Vector<3> _vel_ff;
math::Matrix<3, 3> _R; /**< rotation matrix from attitude quaternions */
float _yaw; /**< yaw angle (euler) */
/**
* Update our local parameter cache.
*/
@@ -319,7 +322,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
_mavlink_fd(-1),
/* subscriptions */
_att_sub(-1),
_ctrl_state_sub(-1),
_att_sp_sub(-1),
_control_mode_sub(-1),
_params_sub(-1),
@@ -347,10 +350,11 @@ MulticopterPositionControl::MulticopterPositionControl() :
_pos_hold_engaged(false),
_alt_hold_engaged(false),
_run_pos_control(true),
_run_alt_control(true)
_run_alt_control(true),
_yaw(0.0f)
{
memset(&_vehicle_status, 0, sizeof(_vehicle_status));
memset(&_att, 0, sizeof(_att));
memset(&_ctrl_state, 0, sizeof(_ctrl_state));
memset(&_att_sp, 0, sizeof(_att_sp));
memset(&_manual, 0, sizeof(_manual));
memset(&_control_mode, 0, sizeof(_control_mode));
@@ -377,6 +381,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
_vel_prev.zero();
_vel_ff.zero();
_R.identity();
_params_handles.thr_min = param_find("MPC_THR_MIN");
_params_handles.thr_max = param_find("MPC_THR_MAX");
_params_handles.z_p = param_find("MPC_Z_P");
@@ -529,10 +535,10 @@ MulticopterPositionControl::poll_subscriptions()
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
}
orb_check(_att_sub, &updated);
orb_check(_ctrl_state_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
}
orb_check(_att_sp_sub, &updated);
@@ -710,20 +716,22 @@ MulticopterPositionControl::control_manual(float dt)
/* check for pos. hold */
if (fabsf(req_vel_sp(0)) < _params.hold_xy_dz && fabsf(req_vel_sp(1)) < _params.hold_xy_dz)
{
if (!_pos_hold_engaged && (_params.hold_max_xy < FLT_EPSILON ||
(fabsf(_vel(0)) < _params.hold_max_xy && fabsf(_vel(1)) < _params.hold_max_xy)))
{
_pos_hold_engaged = true;
if (!_pos_hold_engaged) {
if (_params.hold_max_xy < FLT_EPSILON || (fabsf(_vel(0)) < _params.hold_max_xy && fabsf(_vel(1)) < _params.hold_max_xy)) {
_pos_hold_engaged = true;
} else {
_pos_hold_engaged = false;
}
}
}
else {
_pos_hold_engaged = false;
_pos_sp(0) = _pos(0);
_pos_sp(1) = _pos(1);
}
/* set requested velocity setpoint */
if (!_pos_hold_engaged) {
_pos_sp(0) = _pos(0);
_pos_sp(1) = _pos(1);
_run_pos_control = false; /* request velocity setpoint to be used, instead of position setpoint */
_vel_sp(0) = req_vel_sp_scaled(0);
_vel_sp(1) = req_vel_sp_scaled(1);
@@ -853,18 +861,45 @@ void MulticopterPositionControl::control_auto(float dt)
}
}
bool current_setpoint_valid = false;
bool previous_setpoint_valid = false;
math::Vector<3> prev_sp;
math::Vector<3> curr_sp;
if (_pos_sp_triplet.current.valid) {
/* in case of interrupted mission don't go to waypoint but stay at current position */
_reset_pos_sp = true;
_reset_alt_sp = true;
/* project setpoint to local frame */
math::Vector<3> curr_sp;
map_projection_project(&_ref_pos,
_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
&curr_sp.data[0], &curr_sp.data[1]);
curr_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);
if (PX4_ISFINITE(curr_sp(0)) &&
PX4_ISFINITE(curr_sp(1)) &&
PX4_ISFINITE(curr_sp(2))) {
current_setpoint_valid = true;
}
}
if (_pos_sp_triplet.previous.valid) {
map_projection_project(&_ref_pos,
_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon,
&prev_sp.data[0], &prev_sp.data[1]);
prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt);
if (PX4_ISFINITE(prev_sp(0)) &&
PX4_ISFINITE(prev_sp(1)) &&
PX4_ISFINITE(prev_sp(2))) {
previous_setpoint_valid = true;
}
}
if (current_setpoint_valid) {
/* in case of interrupted mission don't go to waypoint but stay at current position */
_reset_pos_sp = true;
_reset_alt_sp = true;
/* scaled space: 1 == position error resulting max allowed speed */
math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max); // TODO add mult param here
@@ -874,13 +909,8 @@ void MulticopterPositionControl::control_auto(float dt)
/* by default use current setpoint as is */
math::Vector<3> pos_sp_s = curr_sp_s;
if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.valid) {
if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION && previous_setpoint_valid) {
/* follow "previous - current" line */
math::Vector<3> prev_sp;
map_projection_project(&_ref_pos,
_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon,
&prev_sp.data[0], &prev_sp.data[1]);
prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt);
if ((curr_sp - prev_sp).length() > MIN_DIST) {
@@ -985,7 +1015,7 @@ MulticopterPositionControl::task_main()
* do subscriptions
*/
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
@@ -1040,6 +1070,14 @@ MulticopterPositionControl::task_main()
}
poll_subscriptions();
/* get current rotation matrix and euler angles from control state quaternions */
math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
_R = q_att.to_dcm();
math::Vector<3> euler_angles;
euler_angles = _R.to_euler();
_yaw = euler_angles(2);
parameters_update(false);
hrt_abstime t = hrt_absolute_time();
@@ -1115,7 +1153,7 @@ MulticopterPositionControl::task_main()
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
_att_sp.yaw_body = _att.yaw;
_att_sp.yaw_body = _yaw;
_att_sp.thrust = 0.0f;
_att_sp.timestamp = hrt_absolute_time();
@@ -1300,11 +1338,11 @@ MulticopterPositionControl::task_main()
/* thrust compensation for altitude only control mode */
float att_comp;
if (PX4_R(_att.R, 2, 2) > TILT_COS_MAX) {
att_comp = 1.0f / PX4_R(_att.R, 2, 2);
if (_R(2, 2) > TILT_COS_MAX) {
att_comp = 1.0f / _R(2, 2);
} else if (PX4_R(_att.R, 2, 2) > 0.0f) {
att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * PX4_R(_att.R, 2, 2) + 1.0f;
} else if (_R(2, 2) > 0.0f) {
att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _R(2, 2) + 1.0f;
saturation_z = true;
} else {
@@ -1491,7 +1529,7 @@ MulticopterPositionControl::task_main()
/* reset yaw setpoint to current position if needed */
if (reset_yaw_sp) {
reset_yaw_sp = false;
_att_sp.yaw_body = _att.yaw;
_att_sp.yaw_body = _yaw;
}
/* do not move yaw while arming */
@@ -1501,7 +1539,7 @@ MulticopterPositionControl::task_main()
_att_sp.yaw_sp_move_rate = _manual.r * _params.man_yaw_max;
float yaw_target = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt);
float yaw_offs = _wrap_pi(yaw_target - _att.yaw);
float yaw_offs = _wrap_pi(yaw_target - _yaw);
// If the yaw offset became too big for the system to track stop
// shifting it
src/modules/mc_pos_control/mc_pos_control_params.c
-2
View File
@@ -38,8 +38,6 @@
* @author Anton Babushkin <anton@px4.io>
*/
#include <systemlib/param/param.h>
/**
* Minimum thrust in auto thrust control
*
src/modules/navigator/geofence.cpp
+7 -21
View File
@@ -53,14 +53,8 @@
#include <geo/geo.h>
#include <drivers/drv_hrt.h>
#define GEOFENCE_OFF 0
#define GEOFENCE_FILE_ONLY 1
#define GEOFENCE_MAX_DISTANCES_ONLY 2
#define GEOFENCE_FILE_AND_MAX_DISTANCES 3
#define GEOFENCE_RANGE_WARNING_LIMIT 3000000
/* Oddly, ERROR is not defined for C++ */
#ifdef ERROR
# undef ERROR
@@ -76,8 +70,8 @@ Geofence::Geofence() :
_last_vertical_range_warning(0),
_altitude_min(0),
_altitude_max(0),
_verticesCount(0),
_param_geofence_mode(this, "MODE"),
_vertices_count(0),
_param_action(this, "ACTION"),
_param_altitude_mode(this, "ALTMODE"),
_param_source(this, "SOURCE"),
_param_counter_threshold(this, "COUNT"),
@@ -138,7 +132,6 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position,
bool Geofence::inside(double lat, double lon, float altitude)
{
if (_param_geofence_mode.get() >= GEOFENCE_MAX_DISTANCES_ONLY) {
int32_t max_horizontal_distance = _param_max_hor_distance.get();
int32_t max_vertical_distance = _param_max_ver_distance.get();
@@ -152,7 +145,7 @@ bool Geofence::inside(double lat, double lon, float altitude)
if (max_vertical_distance > 0 && (dist_z > max_vertical_distance)) {
if (hrt_elapsed_time(&_last_vertical_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) {
mavlink_log_critical(_mavlinkFd, "Geofence exceeded max vertical distance by %.1f m",
mavlink_and_console_log_critical(_mavlinkFd, "Geofence exceeded max vertical distance by %.1f m",
(double)(dist_z - max_vertical_distance));
_last_vertical_range_warning = hrt_absolute_time();
}
@@ -162,7 +155,7 @@ bool Geofence::inside(double lat, double lon, float altitude)
if (max_horizontal_distance > 0 && (dist_xy > max_horizontal_distance)) {
if (hrt_elapsed_time(&_last_horizontal_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) {
mavlink_log_critical(_mavlinkFd, "Geofence exceeded max horizontal distance by %.1f m",
mavlink_and_console_log_critical(_mavlinkFd, "Geofence exceeded max horizontal distance by %.1f m",
(double)(dist_xy - max_horizontal_distance));
_last_horizontal_range_warning = hrt_absolute_time();
}
@@ -171,7 +164,6 @@ bool Geofence::inside(double lat, double lon, float altitude)
}
}
}
}
bool inside_fence = inside_polygon(lat, lon, altitude);
@@ -194,12 +186,6 @@ bool Geofence::inside(double lat, double lon, float altitude)
bool Geofence::inside_polygon(double lat, double lon, float altitude)
{
/* Return true if geofence is disabled or only checking max distances */
if ((_param_geofence_mode.get() == GEOFENCE_OFF)
|| (_param_geofence_mode.get() == GEOFENCE_MAX_DISTANCES_ONLY)) {
return true;
}
if (valid()) {
if (!isEmpty()) {
@@ -219,7 +205,7 @@ bool Geofence::inside_polygon(double lat, double lon, float altitude)
struct fence_vertex_s temp_vertex_j;
/* Red until fence is finished */
for (unsigned i = 0, j = _verticesCount - 1; i < _verticesCount; j = i++) {
for (unsigned i = 0, j = _vertices_count - 1; i < _vertices_count; j = i++) {
if (dm_read(DM_KEY_FENCE_POINTS, i, &temp_vertex_i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
break;
}
@@ -259,7 +245,7 @@ Geofence::valid()
}
// Otherwise
if ((_verticesCount < 4) || (_verticesCount > fence_s::GEOFENCE_MAX_VERTICES)) {
if ((_vertices_count < 4) || (_vertices_count > fence_s::GEOFENCE_MAX_VERTICES)) {
warnx("Fence must have at least 3 sides and not more than %d", fence_s::GEOFENCE_MAX_VERTICES - 1);
return false;
}
@@ -415,7 +401,7 @@ Geofence::loadFromFile(const char *filename)
/* Check if import was successful */
if (gotVertical && pointCounter > 0) {
_verticesCount = pointCounter;
_vertices_count = pointCounter;
warnx("Geofence: imported successfully");
mavlink_log_info(_mavlinkFd, "Geofence imported");
rc = OK;
src/modules/navigator/geofence.h
+8 -6
View File
@@ -97,12 +97,14 @@ public:
int loadFromFile(const char *filename);
bool isEmpty() {return _verticesCount == 0;}
bool isEmpty() {return _vertices_count == 0;}
int getAltitudeMode() { return _param_altitude_mode.get(); }
int getSource() { return _param_source.get(); }
int getGeofenceAction() { return _param_action.get(); }
void setMavlinkFd(int value) { _mavlinkFd = value; }
private:
@@ -114,20 +116,20 @@ private:
hrt_abstime _last_horizontal_range_warning;
hrt_abstime _last_vertical_range_warning;
float _altitude_min;
float _altitude_max;
float _altitude_min;
float _altitude_max;
unsigned _verticesCount;
uint8_t _vertices_count;
/* Params */
control::BlockParamInt _param_geofence_mode;
control::BlockParamInt _param_action;
control::BlockParamInt _param_altitude_mode;
control::BlockParamInt _param_source;
control::BlockParamInt _param_counter_threshold;
control::BlockParamInt _param_max_hor_distance;
control::BlockParamInt _param_max_ver_distance;
uint8_t _outside_counter;
uint8_t _outside_counter;
int _mavlinkFd;
src/modules/navigator/geofence_params.c
+6 -6
View File
@@ -44,15 +44,15 @@
*/
/**
* Geofence mode.
* Geofence violation action.
*
* 0 = disabled, 1 = geofence file only, 2 = max horizontal (GF_MAX_HOR_DIST) and vertical (GF_MAX_VER_DIST) distances, 3 = both
* 0 = none, 1 = warning (default), 2 = loiter, 3 = return to launch, 4 = fight termination
*
* @min 0
* @max 3
* @max 4
* @group Geofence
*/
PARAM_DEFINE_INT32(GF_MODE, 0);
PARAM_DEFINE_INT32(GF_ACTION, 1);
/**
* Geofence altitude mode
@@ -93,7 +93,7 @@ PARAM_DEFINE_INT32(GF_COUNT, -1);
/**
* Max horizontal distance in meters.
*
* Set to > 0 to activate RTL if horizontal distance to home exceeds this value.
* Set to > 0 to activate a geofence action if horizontal distance to home exceeds this value.
*
* @group Geofence
*/
@@ -102,7 +102,7 @@ PARAM_DEFINE_INT32(GF_MAX_HOR_DIST, -1);
/**
* Max vertical distance in meters.
*
* Set to > 0 to activate RTL if vertical distance to home exceeds this value.
* Set to > 0 to activate a geofence action if vertical distance to home exceeds this value.
*
* @group Geofence
*/
src/modules/navigator/navigator_main.cpp
+5 -1
View File
@@ -405,10 +405,14 @@ Navigator::task_main()
/* Check geofence violation */
static hrt_abstime last_geofence_check = 0;
if (have_geofence_position_data && hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL) {
if (have_geofence_position_data &&
(_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&
(hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) {
bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter[0], _home_pos, _home_position_set);
last_geofence_check = hrt_absolute_time();
have_geofence_position_data = false;
_geofence_result.geofence_action = _geofence.getGeofenceAction();
if (!inside) {
/* inform other apps via the mission result */
_geofence_result.geofence_violated = true;
src/modules/navigator/rtl.cpp
+13 -4
View File
@@ -58,6 +58,7 @@
RTL::RTL(Navigator *navigator, const char *name) :
MissionBlock(navigator, name),
_rtl_state(RTL_STATE_NONE),
_rtl_start_lock(false),
_param_return_alt(this, "RTL_RETURN_ALT", false),
_param_descend_alt(this, "RTL_DESCEND_ALT", false),
_param_land_delay(this, "RTL_LAND_DELAY", false)
@@ -95,6 +96,7 @@ RTL::on_activation()
} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
+ _param_return_alt.get()) {
_rtl_state = RTL_STATE_CLIMB;
_rtl_start_lock = false;
/* otherwise go straight to return */
} else {
@@ -102,6 +104,7 @@ RTL::on_activation()
_rtl_state = RTL_STATE_RETURN;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_global_position()->alt;
_rtl_start_lock = false;
}
}
@@ -126,7 +129,10 @@ RTL::set_rtl_item()
/* make sure we have the latest params */
updateParams();
set_previous_pos_setpoint();
if (!_rtl_start_lock) {
set_previous_pos_setpoint();
}
_navigator->set_can_loiter_at_sp(false);
switch (_rtl_state) {
@@ -182,6 +188,8 @@ RTL::set_rtl_item()
mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: return at %d m (%d m above home)",
(int)(_mission_item.altitude),
(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
_rtl_start_lock = true;
break;
}
@@ -190,7 +198,7 @@ RTL::set_rtl_item()
_mission_item.lon = _navigator->get_home_position()->lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();
_mission_item.yaw = NAN;
_mission_item.yaw = _navigator->get_home_position()->yaw;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
@@ -213,7 +221,7 @@ RTL::set_rtl_item()
_mission_item.lon = _navigator->get_home_position()->lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();
_mission_item.yaw = NAN;
_mission_item.yaw = _navigator->get_home_position()->yaw;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED;
@@ -239,7 +247,7 @@ RTL::set_rtl_item()
_mission_item.lon = _navigator->get_home_position()->lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_home_position()->alt;
_mission_item.yaw = NAN;
_mission_item.yaw = _navigator->get_home_position()->yaw;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_LAND;
@@ -258,6 +266,7 @@ RTL::set_rtl_item()
_mission_item.lon = _navigator->get_home_position()->lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_home_position()->alt;
// Do not change / control yaw in landed
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
src/modules/navigator/rtl.h
+2
View File
@@ -87,6 +87,8 @@ private:
RTL_STATE_LANDED,
} _rtl_state;
bool _rtl_start_lock;
control::BlockParamFloat _param_return_alt;
control::BlockParamFloat _param_descend_alt;
control::BlockParamFloat _param_land_delay;
src/modules/position_estimator_inav/CMakeLists.txt
+1 -1
View File
@@ -31,7 +31,7 @@
#
############################################################################
if(${OS} STREQUAL "nuttx")
list(APPEND MODULE_CFLAGS -Wframe-larger-than=3800)
list(APPEND MODULE_CFLAGS -Wframe-larger-than=3890)
endif()
px4_add_module(
MODULE modules__position_estimator_inav

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