Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 19:07:45 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

updated to master (solve merge conflicts)

Browse Source
This commit is contained in:
Youssef Demitri
2015-11-06 12:18:31 +01:00
parent 50c6d720f0 9b45daa8f7
commit 170f0032fd
72 changed files with 1418 additions and 830 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.travis.yml
+12 -2
View File
@@ -75,6 +75,8 @@ before_script:
- mkdir -p ~/bin - mkdir -p ~/bin
- ln -s /usr/bin/ccache ~/bin/arm-none-eabi-g++ - 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-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++
- ln -s /usr/bin/ccache ~/bin/clang++-3.4 - ln -s /usr/bin/ccache ~/bin/clang++-3.4
- ln -s /usr/bin/ccache ~/bin/clang++-3.5 - ln -s /usr/bin/ccache ~/bin/clang++-3.5
@@ -98,7 +100,15 @@ script:
- make check_format - make check_format
- arm-none-eabi-gcc --version - arm-none-eabi-gcc --version
- echo 'Building POSIX Firmware..' && make posix_sitl_simple - 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-v1 Firmware..' && make px4fmu-v1_default
- echo 'Building NuttX px4fmu-v2 Firmware..' && make px4fmu-v2_default - echo 'Building NuttX px4fmu-v2 Firmware..' && make px4fmu-v2_default
- echo 'Running Tests..' && make px4fmu-v2_default test - echo 'Running Tests..' && make px4fmu-v2_default test
@@ -108,7 +118,7 @@ after_success:
cp build_px4fmu-v1_default/src/firmware/nuttx/nuttx-px4fmu-v1-default.px4 px4fmu-v1_default.px4 cp build_px4fmu-v1_default/src/firmware/nuttx/nuttx-px4fmu-v1-default.px4 px4fmu-v1_default.px4
&& cp build_px4fmu-v2_default/src/firmware/nuttx/nuttx-px4fmu-v2-default.px4 px4fmu-v2_default.px4 && cp build_px4fmu-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 && zip Firmware.zip px4fmu-v1_default.px4 px4fmu-v2_default.px4
&& ./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 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/directory/index.html archives/Firmware/$TRAVIS_BRANCH/
&& ./CI-Tools/s3cmd-put CI-Tools/index.html index.html && ./CI-Tools/s3cmd-put CI-Tools/index.html index.html
Makefile
+11 -3
View File
@@ -44,10 +44,18 @@ ifneq ($(CMAKE_VER),0)
$(warning Not a valid CMake version or CMake not installed.) $(warning Not a valid CMake version or CMake not installed.)
$(warning On Ubuntu, install or upgrade via:) $(warning On Ubuntu, install or upgrade via:)
$(warning ) $(warning )
$(warning 3rd party PPA:)
$(warning sudo add-apt-repository ppa:george-edison55/cmake-3.x -y) $(warning sudo add-apt-repository ppa:george-edison55/cmake-3.x -y)
$(warning sudo apt-get update) $(warning sudo apt-get update)
$(warning sudo apt-get install cmake) $(warning sudo apt-get install cmake)
$(warning ) $(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) $(error Fatal)
endif endif
@@ -156,8 +164,8 @@ posix_sitl_default: posix_sitl_simple
ros: ros_sitl_simple ros: ros_sitl_simple
sitl_deprecation: sitl_deprecation:
@echo "Deprecated. Use 'make posix_sitl_default run_sitl' instead." @echo "Deprecated. Use 'make posix_sitl_default jmavsim' or"
@echo "Change init script with 'make posix_sitl_default config'" @echo "'make posix_sitl_default gazebo' if Gazebo is preferred."
sitl_quad: sitl_deprecation sitl_quad: sitl_deprecation
sitl_plane: sitl_deprecation sitl_plane: sitl_deprecation
@@ -176,7 +184,7 @@ clean:
# targets handled by cmake # 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 \ 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 \ 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)))) $(foreach targ,$(cmake_targets),$(eval $(call cmake-targ,$(targ))))
.PHONY: clean .PHONY: clean
ROMFS/px4fmu_common/init.d/11001_hexa_cox
+11
View File
@@ -4,6 +4,17 @@
# #
# @type Hexarotor Coaxial # @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> # @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 AUX2 feed-through of RC AUX2 channel
# @output AUX3 feed-through of RC AUX3 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 sh /etc/init.d/rc.fw_defaults
@@ -39,6 +39,9 @@ then
param set FW_RR_P 0.04 param set FW_RR_P 0.04
fi fi
# Configure this as plane
set MAV_TYPE 1
# Set mixer
set MIXER wingwing set MIXER wingwing
# Provide ESC a constant 1000 us pulse # Provide ESC a constant 1000 us pulse
set PWM_OUT 4 set PWM_OUT 4
ROMFS/px4fmu_common/init.d/4009_qav250
+34
View File
@@ -0,0 +1,34 @@
#!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.08
param set MC_ROLLRATE_I 0.05
param set MC_ROLLRATE_D 0.003
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 1075
param set PWM_THR_MIN 0.06
param set PWM_MANTHR_MIN 0.06
fi
ROMFS/px4fmu_common/init.d/rcS
+8
View File
@@ -391,6 +391,14 @@ then
# #
set TTYS1_BUSY no 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 something is wrong with setup and we shouldn't try to enable output
if [ $OUTPUT_MODE != none ] if [ $OUTPUT_MODE != none ]
then then
Tools/px4airframes/xmlout.py
+5 -1
View File
@@ -72,9 +72,13 @@ class XMLOutput():
xml_field.text = value xml_field.text = value
for code in param.GetOutputCodes(): for code in param.GetOutputCodes():
value = param.GetOutputValue(code) value = param.GetOutputValue(code)
valstrs = value.split(";")
xml_field = ET.SubElement(xml_param, "output") xml_field = ET.SubElement(xml_param, "output")
xml_field.attrib["name"] = code 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(): if last_param_name != param.GetName():
board_specific_param_set = False board_specific_param_set = False
indent(xml_parameters) indent(xml_parameters)
Tools/px_romfs_pruner.py
+1 -2
View File
@@ -57,7 +57,7 @@ def main():
for (root, dirs, files) in os.walk(args.folder): for (root, dirs, files) in os.walk(args.folder):
for file in files: for file in files:
# only prune text 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 continue
file_path = os.path.join(root, file) file_path = os.path.join(root, file)
@@ -74,7 +74,6 @@ def main():
else: else:
if not line.isspace() and not line.strip().startswith("#"): if not line.isspace() and not line.strip().startswith("#"):
pruned_content += line pruned_content += line
# overwrite old scratch file # overwrite old scratch file
with open(file_path, "wb") as f: with open(file_path, "wb") as f:
f.write(pruned_content.encode("ascii", errors='strict')) f.write(pruned_content.encode("ascii", errors='strict'))
Tools/sitl_run.sh
+20 -8
View File
@@ -3,18 +3,27 @@
rc_script=$1 rc_script=$1
debugger=$2 debugger=$2
program=$3 program=$3
build_path=$4 model=$4
build_path=$5
curr_dir=`pwd` curr_dir=`pwd`
echo SITL ARGS echo SITL ARGS
echo rc_script: $rc_script echo rc_script: $rc_script
echo debugger: $debugger echo debugger: $debugger
echo program: $program echo program: $program
echo model: $model
echo build_path: $build_path echo build_path: $build_path
if [ "$#" != 4 ] if [ "$model" == "" ] || [ "$model" == "none" ]
then 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 exit 1
fi fi
@@ -33,13 +42,13 @@ cp Tools/posix.gdbinit $build_path/src/firmware/posix/.gdbinit
SIM_PID=0 SIM_PID=0
if [ "$program" == "jmavsim" ] if [ "$program" == "jmavsim" ] && [ "$no_sim" == "" ]
then then
cd Tools/jMAVSim cd Tools/jMAVSim
ant ant
java -Djava.ext.dirs= -cp lib/*:out/production/jmavsim.jar me.drton.jmavsim.Simulator -udp 127.0.0.1:14560 & java -Djava.ext.dirs= -cp lib/*:out/production/jmavsim.jar me.drton.jmavsim.Simulator -udp 127.0.0.1:14560 &
SIM_PID=`echo $!` SIM_PID=`echo $!`
elif [ "$3" == "gazebo" ] elif [ "$3" == "gazebo" ] && [ "$no_sim" == "" ]
then then
if [ -x "$(command -v gazebo)" ] if [ -x "$(command -v gazebo)" ]
then then
@@ -54,8 +63,10 @@ then
cd Tools/sitl_gazebo/Build cd Tools/sitl_gazebo/Build
cmake .. cmake ..
make -j4 make -j4
gazebo ../worlds/iris.world & gzserver ../worlds/${model}.world &
SIM_PID=`echo $!` SIM_PID=`echo $!`
gzclient &
GUI_PID=`echo $!`
else else
echo "You need to have gazebo simulator installed!" echo "You need to have gazebo simulator installed!"
exit 1 exit 1
@@ -76,10 +87,11 @@ else
./mainapp ../../../../${rc_script}_${program} ./mainapp ../../../../${rc_script}_${program}
fi fi
if [ "$3" == "jmavsim" ] if [ "$program" == "jmavsim" ]
then then
kill -9 $SIM_PID kill -9 $SIM_PID
elif [ "$3" == "gazebo" ] elif [ "$program" == "gazebo" ]
then then
kill -9 $SIM_PID kill -9 $SIM_PID
kill -9 $GUI_PID
fi fi
cmake/common/px4_base.cmake
+1 -1
View File
@@ -700,7 +700,7 @@ function(px4_create_git_hash_header)
REQUIRED HEADER REQUIRED HEADER
ARGN ${ARGN}) ARGN ${ARGN})
execute_process( execute_process(
COMMAND git log -n 1 --pretty=format:"%H" COMMAND git rev-parse HEAD
OUTPUT_VARIABLE git_desc OUTPUT_VARIABLE git_desc
OUTPUT_STRIP_TRAILING_WHITESPACE OUTPUT_STRIP_TRAILING_WHITESPACE
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
cmake/configs/nuttx_px4fmu-v2_default.cmake
+2 -2
View File
@@ -37,7 +37,7 @@ set(config_module_list
drivers/meas_airspeed drivers/meas_airspeed
drivers/frsky_telemetry drivers/frsky_telemetry
modules/sensors modules/sensors
drivers/mkblctrl #drivers/mkblctrl
drivers/px4flow drivers/px4flow
drivers/oreoled drivers/oreoled
drivers/gimbal drivers/gimbal
@@ -54,7 +54,7 @@ set(config_module_list
systemcmds/pwm systemcmds/pwm
systemcmds/esc_calib systemcmds/esc_calib
systemcmds/reboot systemcmds/reboot
systemcmds/topic_listener #systemcmds/topic_listener
systemcmds/top systemcmds/top
systemcmds/config systemcmds/config
systemcmds/nshterm systemcmds/nshterm
cmake/templates/build_git_version.h.in
+1 -1
View File
@@ -1,4 +1,4 @@
/* Auto Magically Generated file */ /* Auto Magically Generated file */
/* Do not edit! */ /* 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@ #define PX4_GIT_VERSION_BINARY 0x@git_desc_short@
msg/home_position.msg
+5
View File
@@ -8,3 +8,8 @@ float32 alt # Altitude in meters (AMSL)
float32 x # X coordinate in meters float32 x # X coordinate in meters
float32 y # Y coordinate in meters float32 y # Y coordinate in meters
float32 z # Z 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 mode_switch # main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO
uint8 return_switch # return to launch 2 position switch (mandatory): _NORMAL_, RTL 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 posctl_switch # position control 2 position switch (optional): _ALTCTL_, POSCTL
uint8 loiter_switch # loiter 2 position switch (optional): _MISSION_, LOITER uint8 loiter_switch # loiter 2 position switch (optional): _MISSION_, LOITER
uint8 acro_switch # acro 2 position switch (optional): _MANUAL_, ACRO 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_THROTTLE=0
uint8 RC_CHANNELS_FUNCTION_ROLL=1 uint8 RC_CHANNELS_FUNCTION_ROLL=1
uint8 RC_CHANNELS_FUNCTION_PITCH=2 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_1=16
uint8 RC_CHANNELS_FUNCTION_PARAM_2=17 uint8 RC_CHANNELS_FUNCTION_PARAM_2=17
uint8 RC_CHANNELS_FUNCTION_PARAM_3_5=18 uint8 RC_CHANNELS_FUNCTION_PARAM_3_5=18
uint8 RC_CHANNELS_FUNCTION_RATTITUDE=19
uint64 timestamp # Timestamp in microseconds since boot time uint64 timestamp # Timestamp in microseconds since boot time
uint64 timestamp_last_valid # Timestamp of last valid RC signal 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 uint8 channel_count # Number of valid channels
int8[19] function # Functions mapping int8[20] function # Functions mapping
uint8 rssi # Receive signal strength index uint8 rssi # Receive signal strength index
bool signal_lost # Control signal lost, should be checked together with topic timeout 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_3DR_RADIO = 1
uint8 TELEMETRY_STATUS_RADIO_TYPE_UBIQUITY_BULLET = 2 uint8 TELEMETRY_STATUS_RADIO_TYPE_UBIQUITY_BULLET = 2
uint8 TELEMETRY_STATUS_RADIO_TYPE_WIRE = 3 uint8 TELEMETRY_STATUS_RADIO_TYPE_WIRE = 3
uint8 TELEMETRY_STATUS_RADIO_TYPE_USB = 4
uint64 timestamp uint64 timestamp
uint64 heartbeat_time # Time of last received heartbeat from remote system 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_ACRO = 6
uint8 MAIN_STATE_OFFBOARD = 7 uint8 MAIN_STATE_OFFBOARD = 7
uint8 MAIN_STATE_STAB = 8 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 # 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. # 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_TERMINATION = 13 # Termination mode
uint8 NAVIGATION_STATE_OFFBOARD = 14 uint8 NAVIGATION_STATE_OFFBOARD = 14
uint8 NAVIGATION_STATE_STAB = 15 # Stabilized mode 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 # VEHICLE_MODE_FLAG, same as MAV_MODE_FLAG of MAVLink 1.0 protocol
uint8 VEHICLE_MODE_FLAG_SAFETY_ARMED = 128 uint8 VEHICLE_MODE_FLAG_SAFETY_ARMED = 128
src/drivers/device/vdev.cpp
+33
View File
@@ -67,6 +67,7 @@ private:
#define PX4_MAX_DEV 500 #define PX4_MAX_DEV 500
static px4_dev_t *devmap[PX4_MAX_DEV]; 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 * 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 // Make sure the device does not already exist
// FIXME - convert this to a map for efficiency // FIXME - convert this to a map for efficiency
pthread_mutex_lock(&devmutex);
for (int i = 0; i < PX4_MAX_DEV; ++i) { for (int i = 0; i < PX4_MAX_DEV; ++i) {
if (devmap[i] && (strcmp(devmap[i]->name, name) == 0)) { if (devmap[i] && (strcmp(devmap[i]->name, name) == 0)) {
pthread_mutex_unlock(&devmutex);
return -EEXIST; return -EEXIST;
} }
} }
@@ -157,6 +162,8 @@ VDev::register_driver(const char *name, void *data)
} }
} }
pthread_mutex_unlock(&devmutex);
if (ret != PX4_OK) { if (ret != PX4_OK) {
PX4_ERR("No free devmap entries - increase PX4_MAX_DEV"); PX4_ERR("No free devmap entries - increase PX4_MAX_DEV");
} }
@@ -174,6 +181,8 @@ VDev::unregister_driver(const char *name)
return -EINVAL; return -EINVAL;
} }
pthread_mutex_lock(&devmutex);
for (int i = 0; i < PX4_MAX_DEV; ++i) { for (int i = 0; i < PX4_MAX_DEV; ++i) {
if (devmap[i] && (strcmp(name, devmap[i]->name) == 0)) { if (devmap[i] && (strcmp(name, devmap[i]->name) == 0)) {
delete devmap[i]; delete devmap[i];
@@ -184,6 +193,8 @@ VDev::unregister_driver(const char *name)
} }
} }
pthread_mutex_unlock(&devmutex);
return ret; return ret;
} }
@@ -194,15 +205,20 @@ VDev::unregister_class_devname(const char *class_devname, unsigned class_instanc
char name[32]; char name[32];
snprintf(name, sizeof(name), "%s%u", class_devname, class_instance); snprintf(name, sizeof(name), "%s%u", class_devname, class_instance);
pthread_mutex_lock(&devmutex);
for (int i = 0; i < PX4_MAX_DEV; ++i) { for (int i = 0; i < PX4_MAX_DEV; ++i) {
if (devmap[i] && strcmp(devmap[i]->name, name) == 0) { if (devmap[i] && strcmp(devmap[i]->name, name) == 0) {
delete devmap[i]; delete devmap[i];
PX4_DEBUG("Unregistered class DEV %s", name); PX4_DEBUG("Unregistered class DEV %s", name);
devmap[i] = NULL; devmap[i] = NULL;
pthread_mutex_unlock(&devmutex);
return PX4_OK; return PX4_OK;
} }
} }
pthread_mutex_unlock(&devmutex);
return -EINVAL; return -EINVAL;
} }
@@ -497,15 +513,20 @@ VDev *VDev::getDev(const char *path)
PX4_DEBUG("VDev::getDev"); PX4_DEBUG("VDev::getDev");
int i = 0; int i = 0;
pthread_mutex_lock(&devmutex);
for (; i < PX4_MAX_DEV; ++i) { for (; i < PX4_MAX_DEV; ++i) {
//if (devmap[i]) { //if (devmap[i]) {
// printf("%s %s\n", devmap[i]->name, path); // printf("%s %s\n", devmap[i]->name, path);
//} //}
if (devmap[i] && (strcmp(devmap[i]->name, path) == 0)) { if (devmap[i] && (strcmp(devmap[i]->name, path) == 0)) {
pthread_mutex_unlock(&devmutex);
return (VDev *)(devmap[i]->cdev); return (VDev *)(devmap[i]->cdev);
} }
} }
pthread_mutex_unlock(&devmutex);
return NULL; return NULL;
} }
@@ -514,11 +535,15 @@ void VDev::showDevices()
int i = 0; int i = 0;
PX4_INFO("Devices:"); PX4_INFO("Devices:");
pthread_mutex_lock(&devmutex);
for (; i < PX4_MAX_DEV; ++i) { for (; i < PX4_MAX_DEV; ++i) {
if (devmap[i] && strncmp(devmap[i]->name, "/dev/", 5) == 0) { if (devmap[i] && strncmp(devmap[i]->name, "/dev/", 5) == 0) {
PX4_INFO(" %s", devmap[i]->name); PX4_INFO(" %s", devmap[i]->name);
} }
} }
pthread_mutex_unlock(&devmutex);
} }
void VDev::showTopics() void VDev::showTopics()
@@ -526,11 +551,15 @@ void VDev::showTopics()
int i = 0; int i = 0;
PX4_INFO("Devices:"); PX4_INFO("Devices:");
pthread_mutex_lock(&devmutex);
for (; i < PX4_MAX_DEV; ++i) { for (; i < PX4_MAX_DEV; ++i) {
if (devmap[i] && strncmp(devmap[i]->name, "/obj/", 5) == 0) { if (devmap[i] && strncmp(devmap[i]->name, "/obj/", 5) == 0) {
PX4_INFO(" %s", devmap[i]->name); PX4_INFO(" %s", devmap[i]->name);
} }
} }
pthread_mutex_unlock(&devmutex);
} }
void VDev::showFiles() void VDev::showFiles()
@@ -538,12 +567,16 @@ void VDev::showFiles()
int i = 0; int i = 0;
PX4_INFO("Files:"); PX4_INFO("Files:");
pthread_mutex_lock(&devmutex);
for (; i < PX4_MAX_DEV; ++i) { for (; i < PX4_MAX_DEV; ++i) {
if (devmap[i] && strncmp(devmap[i]->name, "/obj/", 5) != 0 && if (devmap[i] && strncmp(devmap[i]->name, "/obj/", 5) != 0 &&
strncmp(devmap[i]->name, "/dev/", 5) != 0) { strncmp(devmap[i]->name, "/dev/", 5) != 0) {
PX4_INFO(" %s", devmap[i]->name); PX4_INFO(" %s", devmap[i]->name);
} }
} }
pthread_mutex_unlock(&devmutex);
} }
const char *VDev::topicList(unsigned int *next) const char *VDev::topicList(unsigned int *next)
src/drivers/device/vdev_posix.cpp
+51 -15
View File
@@ -52,6 +52,8 @@
using namespace device; using namespace device;
pthread_mutex_t filemutex = PTHREAD_MUTEX_INITIALIZER;
extern "C" { extern "C" {
static void timer_cb(void *data) static void timer_cb(void *data)
@@ -68,7 +70,27 @@ extern "C" {
inline bool valid_fd(int fd) 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, ...) int px4_open(const char *path, int flags, ...)
@@ -93,6 +115,9 @@ extern "C" {
} }
if (dev) { if (dev) {
pthread_mutex_lock(&filemutex);
for (i = 0; i < PX4_MAX_FD; ++i) { for (i = 0; i < PX4_MAX_FD; ++i) {
if (filemap[i] == 0) { if (filemap[i] == 0) {
filemap[i] = new device::file_t(flags, dev, i); filemap[i] = new device::file_t(flags, dev, i);
@@ -100,6 +125,8 @@ extern "C" {
} }
} }
pthread_mutex_unlock(&filemutex);
if (i < PX4_MAX_FD) { if (i < PX4_MAX_FD) {
ret = dev->open(filemap[i]); ret = dev->open(filemap[i]);
@@ -125,11 +152,14 @@ extern "C" {
{ {
int ret; int ret;
if (valid_fd(fd)) { VDev *dev = get_vdev(fd);
VDev *dev = (VDev *)(filemap[fd]->vdev);
PX4_DEBUG("px4_close fd = %d", fd); if (dev) {
pthread_mutex_lock(&filemutex);
ret = dev->close(filemap[fd]); ret = dev->close(filemap[fd]);
filemap[fd] = NULL; filemap[fd] = nullptr;
pthread_mutex_unlock(&filemutex);
PX4_DEBUG("px4_close fd = %d", fd);
} else { } else {
ret = -EINVAL; ret = -EINVAL;
@@ -147,8 +177,9 @@ extern "C" {
{ {
int ret; int ret;
if (valid_fd(fd)) { VDev *dev = get_vdev(fd);
VDev *dev = (VDev *)(filemap[fd]->vdev);
if (dev) {
PX4_DEBUG("px4_read fd = %d", fd); PX4_DEBUG("px4_read fd = %d", fd);
ret = dev->read(filemap[fd], (char *)buffer, buflen); ret = dev->read(filemap[fd], (char *)buffer, buflen);
@@ -168,8 +199,9 @@ extern "C" {
{ {
int ret; int ret;
if (valid_fd(fd)) { VDev *dev = get_vdev(fd);
VDev *dev = (VDev *)(filemap[fd]->vdev);
if (dev) {
PX4_DEBUG("px4_write fd = %d", fd); PX4_DEBUG("px4_write fd = %d", fd);
ret = dev->write(filemap[fd], (const char *)buffer, buflen); ret = dev->write(filemap[fd], (const char *)buffer, buflen);
@@ -190,8 +222,9 @@ extern "C" {
PX4_DEBUG("px4_ioctl fd = %d", fd); PX4_DEBUG("px4_ioctl fd = %d", fd);
int ret = 0; int ret = 0;
if (valid_fd(fd)) { VDev *dev = get_vdev(fd);
VDev *dev = (VDev *)(filemap[fd]->vdev);
if (dev) {
ret = dev->ioctl(filemap[fd], cmd, arg); ret = dev->ioctl(filemap[fd], cmd, arg);
} else { } else {
@@ -221,9 +254,10 @@ extern "C" {
fds[i].revents = 0; fds[i].revents = 0;
fds[i].priv = NULL; fds[i].priv = NULL;
VDev *dev = get_vdev(fds[i].fd);
// If fd is valid // If fd is valid
if (valid_fd(fds[i].fd)) { if (dev) {
VDev *dev = (VDev *)(filemap[fds[i].fd]->vdev);;
PX4_DEBUG("px4_poll: VDev->poll(setup) %d", fds[i].fd); PX4_DEBUG("px4_poll: VDev->poll(setup) %d", fds[i].fd);
ret = dev->poll(filemap[fds[i].fd], &fds[i], true); ret = dev->poll(filemap[fds[i].fd], &fds[i], true);
@@ -251,9 +285,11 @@ extern "C" {
// For each fd // For each fd
for (i = 0; i < nfds; ++i) { for (i = 0; i < nfds; ++i) {
VDev *dev = get_vdev(fds[i].fd);
// If fd is valid // If fd is valid
if (valid_fd(fds[i].fd)) { if (dev) {
VDev *dev = (VDev *)(filemap[fds[i].fd]->vdev);;
PX4_DEBUG("px4_poll: VDev->poll(teardown) %d", fds[i].fd); PX4_DEBUG("px4_poll: VDev->poll(teardown) %d", fds[i].fd);
ret = dev->poll(filemap[fds[i].fd], &fds[i], false); ret = dev->poll(filemap[fds[i].fd], &fds[i], false);
src/drivers/ll40ls/LidarLiteI2C.cpp
+4 -6
View File
@@ -114,21 +114,20 @@ int LidarLiteI2C::init()
/* do I2C init (and probe) first */ /* do I2C init (and probe) first */
if (I2C::init() != OK) { if (I2C::init() != OK) {
goto out; return ret;
} }
/* allocate basic report buffers */ /* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(struct distance_sensor_s)); _reports = new ringbuffer::RingBuffer(2, sizeof(struct distance_sensor_s));
if (_reports == nullptr) { if (_reports == nullptr) {
goto out; return ret;
} }
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); _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 */ /* get a publish handle on the range finder topic */
struct distance_sensor_s ds_report; struct distance_sensor_s ds_report = {};
measure(); measure();
_reports->get(&ds_report); _reports->get(&ds_report);
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
@@ -137,12 +136,11 @@ int LidarLiteI2C::init()
if (_distance_sensor_topic == nullptr) { if (_distance_sensor_topic == nullptr) {
DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?"); DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?");
} }
}
ret = OK; ret = OK;
/* sensor is ok, but we don't really know if it is within range */ /* sensor is ok, but we don't really know if it is within range */
_sensor_ok = true; _sensor_ok = true;
out:
return ret; return ret;
} }
src/drivers/ll40ls/LidarLitePWM.cpp
+1 -3
View File
@@ -108,9 +108,8 @@ int LidarLitePWM::init()
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); _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 */ /* get a publish handle on the distance_sensor topic */
struct distance_sensor_s ds_report; struct distance_sensor_s ds_report = {};
measure(); measure();
_reports->get(&ds_report); _reports->get(&ds_report);
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
@@ -119,7 +118,6 @@ int LidarLitePWM::init()
if (_distance_sensor_topic == nullptr) { if (_distance_sensor_topic == nullptr) {
DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?"); DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?");
} }
}
return OK; return OK;
} }
src/drivers/mb12xx/mb12xx.cpp
+3 -5
View File
@@ -255,7 +255,7 @@ MB12XX::init()
/* do I2C init (and probe) first */ /* do I2C init (and probe) first */
if (I2C::init() != OK) { if (I2C::init() != OK) {
goto out; return ret;
} }
/* allocate basic report buffers */ /* allocate basic report buffers */
@@ -265,12 +265,11 @@ MB12XX::init()
set_address(_index_counter); /* set I2c port to temp sonar i2c adress */ set_address(_index_counter); /* set I2c port to temp sonar i2c adress */
if (_reports == nullptr) { if (_reports == nullptr) {
goto out; return ret;
} }
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); _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 */ /* get a publish handle on the range finder topic */
struct distance_sensor_s ds_report = {}; struct distance_sensor_s ds_report = {};
@@ -280,7 +279,6 @@ MB12XX::init()
if (_distance_sensor_topic == nullptr) { if (_distance_sensor_topic == nullptr) {
DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); 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 // XXX we should find out why we need to wait 200 ms here
usleep(200000); usleep(200000);
@@ -321,7 +319,7 @@ MB12XX::init()
ret = OK; ret = OK;
/* sensor is ok, but we don't really know if it is within range */ /* sensor is ok, but we don't really know if it is within range */
_sensor_ok = true; _sensor_ok = true;
out:
return ret; return ret;
} }
src/drivers/pwm_out_sim/pwm_out_sim.cpp
+144 -63
View File
@@ -79,6 +79,8 @@
#include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_0.h> #include <uORB/topics/actuator_controls_0.h>
#include <uORB/topics/actuator_controls_1.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_armed.h>
#include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_outputs.h>
@@ -94,6 +96,7 @@ public:
enum Mode { enum Mode {
MODE_2PWM, MODE_2PWM,
MODE_4PWM, MODE_4PWM,
MODE_6PWM,
MODE_8PWM, MODE_8PWM,
MODE_12PWM, MODE_12PWM,
MODE_16PWM, MODE_16PWM,
@@ -111,23 +114,29 @@ public:
int _task; int _task;
private: private:
static const unsigned _max_actuators = 4; static const unsigned _max_actuators = 8;
Mode _mode; Mode _mode;
int _update_rate; int _update_rate;
int _current_update_rate; int _current_update_rate;
int _t_actuators; int _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
int _t_armed; px4_pollfd_struct_t _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
orb_advert_t _t_outputs; unsigned _poll_fds_num;
int _armed_sub;
orb_advert_t _outputs_pub;
unsigned _num_outputs; unsigned _num_outputs;
bool _primary_pwm_device; bool _primary_pwm_device;
uint32_t _groups_required;
uint32_t _groups_subscribed;
volatile bool _task_should_exit; volatile bool _task_should_exit;
static bool _armed; static bool _armed;
MixerGroup *_mixers; 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[]); static void task_main_trampoline(int argc, char *argv[]);
void task_main(); void task_main();
@@ -138,6 +147,7 @@ private:
float &input); float &input);
int pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg); int pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg);
void subscribe();
struct GPIOConfig { struct GPIOConfig {
uint32_t input; uint32_t input;
@@ -176,15 +186,24 @@ PWMSim::PWMSim() :
_mode(MODE_NONE), _mode(MODE_NONE),
_update_rate(50), _update_rate(50),
_current_update_rate(0), _current_update_rate(0),
_t_actuators(-1), _control_subs { -1},
_t_armed(-1), _poll_fds_num(0),
_t_outputs(0), _armed_sub(-1),
_outputs_pub(0),
_num_outputs(0), _num_outputs(0),
_primary_pwm_device(false), _primary_pwm_device(false),
_groups_required(0),
_groups_subscribed(0),
_task_should_exit(false), _task_should_exit(false),
_mixers(nullptr) _mixers(nullptr)
{ {
_debug_enabled = true; _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() PWMSim::~PWMSim()
@@ -326,64 +345,62 @@ PWMSim::set_pwm_rate(unsigned rate)
return OK; 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 void
PWMSim::task_main() 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 */ /* force a reset of the update rate */
_current_update_rate = 0; _current_update_rate = 0;
_t_armed = orb_subscribe(ORB_ID(actuator_armed)); _armed_sub = orb_subscribe(ORB_ID(actuator_armed));
orb_set_interval(_t_armed, 200); /* 5Hz update rate */ orb_set_interval(_armed_sub, 200); /* 5Hz update rate */
/* advertise the mixed control outputs */ /* advertise the mixed control outputs */
actuator_outputs_s outputs; actuator_outputs_s outputs;
memset(&outputs, 0, sizeof(outputs)); memset(&outputs, 0, sizeof(outputs));
/* advertise the mixed control outputs, insist on the first group output */ /* 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"); DEVICE_LOG("starting");
/* loop until killed */ /* loop until killed */
while (!_task_should_exit) { while (!_task_should_exit) {
if (_groups_subscribed != _groups_required) {
subscribe();
_groups_subscribed = _groups_required;
}
/* handle update rate changes */ /* handle update rate changes */
if (_current_update_rate != _update_rate) { if (_current_update_rate != _update_rate) {
int update_rate_in_ms = int(1000 / _update_rate); int update_rate_in_ms = int(1000 / _update_rate);
@@ -392,13 +409,18 @@ PWMSim::task_main()
update_rate_in_ms = 2; 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); // up_pwm_servo_set_rate(_update_rate);
_current_update_rate = _update_rate; _current_update_rate = _update_rate;
} }
/* sleep waiting for data, but no more than a second */ /* 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... */ /* this would be bad... */
if (ret < 0) { if (ret < 0) {
@@ -406,18 +428,63 @@ PWMSim::task_main()
continue; continue;
} }
/* do we have a control update? */ if (ret == 0) {
if (fds[0].revents & POLLIN) { // timeout
/* get controls - must always do this to avoid spinning */ continue;
orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : }
ORB_ID(actuator_controls_1), _t_actuators, &_controls);
/* get controls for required topics */
unsigned poll_id = 0;
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]);
}
poll_id++;
}
}
/* can we mix? */ /* can we mix? */
if (_armed && _mixers != nullptr) { if (_armed && _mixers != nullptr) {
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 */ /* do mixing */
outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs, NULL); actuator_outputs_s outputs;
num_outputs = _mixers->mix(&outputs.output[0], num_outputs, NULL);
outputs.timestamp = hrt_absolute_time(); 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 */ /* iterate actuators */
for (unsigned i = 0; i < num_outputs; i++) { for (unsigned i = 0; i < num_outputs; i++) {
/* last resort: catch NaN, INF and out-of-band errors */ /* last resort: catch NaN, INF and out-of-band errors */
@@ -438,24 +505,30 @@ PWMSim::task_main()
} }
} }
/* and publish for anyone that cares to see */ /* and publish for anyone that cares to see */
orb_publish(ORB_ID(actuator_outputs), _t_outputs, &outputs); orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs);
}
} }
/* how about an arming update? */ /* how about an arming update? */
if (fds[1].revents & POLLIN) { bool updated;
actuator_armed_s aa; actuator_armed_s aa;
orb_check(_armed_sub, &updated);
/* get new value */ if (updated) {
orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); orb_copy(ORB_ID(actuator_armed), _armed_sub, &aa);
/* do not obey the lockdown value, as lockdown is for PWMSim */ /* do not obey the lockdown value, as lockdown is for PWMSim */
_armed = aa.armed; _armed = aa.armed;
} }
} }
px4_close(_t_actuators); for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
px4_close(_t_armed); if (_control_subs[i] > 0) {
px4_close(_control_subs[i]);
}
}
px4_close(_armed_sub);
/* make sure servos are off */ /* make sure servos are off */
// up_pwm_servo_deinit(); // up_pwm_servo_deinit();
@@ -671,6 +744,7 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
if (_mixers != nullptr) { if (_mixers != nullptr) {
delete _mixers; delete _mixers;
_mixers = nullptr; _mixers = nullptr;
_groups_required = 0;
} }
break; break;
@@ -683,6 +757,7 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
if (mixer->check()) { if (mixer->check()) {
delete mixer; delete mixer;
_groups_required = 0;
ret = -EINVAL; ret = -EINVAL;
} else { } else {
@@ -691,6 +766,7 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
(uintptr_t)&_controls); (uintptr_t)&_controls);
_mixers->add_mixer(mixer); _mixers->add_mixer(mixer);
_mixers->groups_required(_groups_required);
} }
break; break;
@@ -705,6 +781,7 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
} }
if (_mixers == nullptr) { if (_mixers == nullptr) {
_groups_required = 0;
ret = -ENOMEM; ret = -ENOMEM;
} else { } 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); DEVICE_DEBUG("mixer load failed with %d", ret);
delete _mixers; delete _mixers;
_mixers = nullptr; _mixers = nullptr;
_groups_required = 0;
ret = -EINVAL; ret = -EINVAL;
} else {
_mixers->groups_required(_groups_required);
} }
} }
@@ -772,7 +853,7 @@ hil_new_mode(PortMode new_mode)
case PORT1_FULL_PWM: case PORT1_FULL_PWM:
/* select 4-pin PWM mode */ /* select 4-pin PWM mode */
servo_mode = PWMSim::MODE_4PWM; servo_mode = PWMSim::MODE_8PWM;
break; break;
case PORT1_PWM_AND_SERIAL: case PORT1_PWM_AND_SERIAL:
src/drivers/px4flow/px4flow.cpp
+3 -5
View File
@@ -229,19 +229,18 @@ PX4FLOW::init()
/* do I2C init (and probe) first */ /* do I2C init (and probe) first */
if (I2C::init() != OK) { if (I2C::init() != OK) {
goto out; return ret;
} }
/* allocate basic report buffers */ /* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(optical_flow_s)); _reports = new ringbuffer::RingBuffer(2, sizeof(optical_flow_s));
if (_reports == nullptr) { if (_reports == nullptr) {
goto out; return ret;
} }
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); _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 */ /* get a publish handle on the range finder topic */
struct distance_sensor_s ds_report = {}; struct distance_sensor_s ds_report = {};
@@ -251,12 +250,11 @@ PX4FLOW::init()
if (_distance_sensor_topic == nullptr) { if (_distance_sensor_topic == nullptr) {
DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
} }
}
ret = OK; ret = OK;
/* sensor is ok, but we don't really know if it is within range */ /* sensor is ok, but we don't really know if it is within range */
_sensor_ok = true; _sensor_ok = true;
out:
return ret; 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); } while (buflen > 0);
/* ensure a closing newline */ int ret;
/* send the closing newline */
msg->text[0] = '\n'; msg->text[0] = '\n';
msg->text[1] = '\0'; msg->text[1] = '\0';
int ret;
for (int i = 0; i < 30; i++) { for (int i = 0; i < 30; i++) {
/* failed, but give it a 2nd shot */ /* failed, but give it a 2nd shot */
ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2); 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) { if (ret == 0) {
return ret; /* success, exit */
break;
} }
retries--; 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)));
/* 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;
}
DEVICE_LOG("mixer rejected by IO");
mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail");
if (retries == 0) {
mavlink_and_console_log_info(_mavlink_fd, "[IO] mixer upload fail");
/* load must have failed for some reason */ /* load must have failed for some reason */
return -EINVAL; return -EINVAL;
} 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);
}
} }
void void
src/drivers/sf0x/sf0x.cpp
-2
View File
@@ -296,7 +296,6 @@ SF0X::init()
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); _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 */ /* get a publish handle on the range finder topic */
struct distance_sensor_s ds_report = {}; struct distance_sensor_s ds_report = {};
@@ -306,7 +305,6 @@ SF0X::init()
if (_distance_sensor_topic == nullptr) { if (_distance_sensor_topic == nullptr) {
DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
} }
}
} while (0); } while (0);
src/drivers/srf02/srf02.cpp
+3 -5
View File
@@ -254,7 +254,7 @@ SRF02::init()
/* do I2C init (and probe) first */ /* do I2C init (and probe) first */
if (I2C::init() != OK) { if (I2C::init() != OK) {
goto out; return ret;
} }
/* allocate basic report buffers */ /* allocate basic report buffers */
@@ -264,12 +264,11 @@ SRF02::init()
set_address(_index_counter); /* set I2c port to temp sonar i2c adress */ set_address(_index_counter); /* set I2c port to temp sonar i2c adress */
if (_reports == nullptr) { if (_reports == nullptr) {
goto out; return ret;
} }
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); _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 */ /* get a publish handle on the range finder topic */
struct distance_sensor_s ds_report = {}; struct distance_sensor_s ds_report = {};
@@ -279,7 +278,6 @@ SRF02::init()
if (_distance_sensor_topic == nullptr) { if (_distance_sensor_topic == nullptr) {
DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); 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 // XXX we should find out why we need to wait 200 ms here
usleep(200000); usleep(200000);
@@ -320,7 +318,7 @@ SRF02::init()
ret = OK; ret = OK;
/* sensor is ok, but we don't really know if it is within range */ /* sensor is ok, but we don't really know if it is within range */
_sensor_ok = true; _sensor_ok = true;
out:
return ret; return ret;
} }
src/firmware/posix/CMakeLists.txt
+14 -4
View File
@@ -26,28 +26,38 @@ endif()
add_custom_target(run_config add_custom_target(run_config
COMMAND Tools/sitl_run.sh "${config_sitl_rcS}" "${config_sitl_debugger}" 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} WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
USES_TERMINAL USES_TERMINAL
) )
add_dependencies(run_config mainapp) add_dependencies(run_config mainapp)
foreach(viewer jmavsim gazebo) foreach(viewer none jmavsim gazebo)
foreach(debugger none gdb lldb) foreach(debugger none gdb lldb)
foreach(model none iris vtol)
if (debugger STREQUAL "none") if (debugger STREQUAL "none")
if (model STREQUAL "none")
set(_targ_name "${viewer}") set(_targ_name "${viewer}")
else() else()
set(_targ_name "${viewer}_${debugger}") 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() endif()
add_custom_target(${_targ_name} add_custom_target(${_targ_name}
COMMAND Tools/sitl_run.sh "${config_sitl_rcS}" COMMAND Tools/sitl_run.sh "${config_sitl_rcS}"
"${debugger}" "${debugger}"
"${viewer}" "${CMAKE_BINARY_DIR}" "${viewer}" "${model}" "${CMAKE_BINARY_DIR}"
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
USES_TERMINAL USES_TERMINAL
) )
add_dependencies(${_targ_name} mainapp) add_dependencies(${_targ_name} mainapp)
endforeach() endforeach()
endforeach() endforeach()
endforeach()
# vim: set noet ft=cmake fenc=utf-8 ff=unix : # vim: set noet ft=cmake fenc=utf-8 ff=unix :
src/lib/external_lgpl/tecs/tecs.h
+3
View File
@@ -118,6 +118,9 @@ public:
return get_throttle_demand(); return get_throttle_demand();
} }
void reset_state() {
_states_initalized = false;
}
float get_pitch_demand() { return _pitch_dem; } float get_pitch_demand() { return _pitch_dem; }
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; } } 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( void __EXPORT float2SigExp(
const float &num, const float &num,
float &sig, float &sig,
src/lib/mathlib/math/test/test.hpp
+9
View File
@@ -44,6 +44,15 @@
//#include <stdlib.h> //#include <stdlib.h>
bool equal(float a, float b, float eps = 1e-5); 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( void float2SigExp(
const float &num, const float &num,
float &sig, float &sig,
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+86
View File
@@ -57,6 +57,8 @@
#include <uORB/topics/control_state.h> #include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_global_position.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 <uORB/topics/parameter_update.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
@@ -118,6 +120,8 @@ private:
int _sensors_sub = -1; int _sensors_sub = -1;
int _params_sub = -1; int _params_sub = -1;
int _vision_sub = -1;
int _mocap_sub = -1;
int _global_pos_sub = -1; int _global_pos_sub = -1;
orb_advert_t _att_pub = nullptr; orb_advert_t _att_pub = nullptr;
orb_advert_t _ctrl_state_pub = nullptr; orb_advert_t _ctrl_state_pub = nullptr;
@@ -125,16 +129,19 @@ private:
struct { struct {
param_t w_acc; param_t w_acc;
param_t w_mag; param_t w_mag;
param_t w_ext_hdg;
param_t w_gyro_bias; param_t w_gyro_bias;
param_t mag_decl; param_t mag_decl;
param_t mag_decl_auto; param_t mag_decl_auto;
param_t acc_comp; param_t acc_comp;
param_t bias_max; param_t bias_max;
param_t vibe_thresh; param_t vibe_thresh;
param_t ext_hdg_mode;
} _params_handles; /**< handles for interesting parameters */ } _params_handles; /**< handles for interesting parameters */
float _w_accel = 0.0f; float _w_accel = 0.0f;
float _w_mag = 0.0f; float _w_mag = 0.0f;
float _w_ext_hdg = 0.0f;
float _w_gyro_bias = 0.0f; float _w_gyro_bias = 0.0f;
float _mag_decl = 0.0f; float _mag_decl = 0.0f;
bool _mag_decl_auto = false; bool _mag_decl_auto = false;
@@ -142,11 +149,18 @@ private:
float _bias_max = 0.0f; float _bias_max = 0.0f;
float _vibration_warning_threshold = 1.0f; float _vibration_warning_threshold = 1.0f;
hrt_abstime _vibration_warning_timestamp = 0; hrt_abstime _vibration_warning_timestamp = 0;
int _ext_hdg_mode = 0;
Vector<3> _gyro; Vector<3> _gyro;
Vector<3> _accel; Vector<3> _accel;
Vector<3> _mag; Vector<3> _mag;
vision_position_estimate_s _vision = {};
Vector<3> _vision_hdg;
att_pos_mocap_s _mocap = {};
Vector<3> _mocap_hdg;
Quaternion _q; Quaternion _q;
Vector<3> _rates; Vector<3> _rates;
Vector<3> _gyro_bias; Vector<3> _gyro_bias;
@@ -170,6 +184,7 @@ private:
bool _data_good = false; bool _data_good = false;
bool _failsafe = false; bool _failsafe = false;
bool _vibration_warning = false; bool _vibration_warning = false;
bool _ext_hdg_good = false;
int _mavlink_fd = -1; int _mavlink_fd = -1;
@@ -198,12 +213,14 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() :
_params_handles.w_acc = param_find("ATT_W_ACC"); _params_handles.w_acc = param_find("ATT_W_ACC");
_params_handles.w_mag = param_find("ATT_W_MAG"); _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.w_gyro_bias = param_find("ATT_W_GYRO_BIAS");
_params_handles.mag_decl = param_find("ATT_MAG_DECL"); _params_handles.mag_decl = param_find("ATT_MAG_DECL");
_params_handles.mag_decl_auto = param_find("ATT_MAG_DECL_A"); _params_handles.mag_decl_auto = param_find("ATT_MAG_DECL_A");
_params_handles.acc_comp = param_find("ATT_ACC_COMP"); _params_handles.acc_comp = param_find("ATT_ACC_COMP");
_params_handles.bias_max = param_find("ATT_BIAS_MAX"); _params_handles.bias_max = param_find("ATT_BIAS_MAX");
_params_handles.vibe_thresh = param_find("ATT_VIBE_THRESH"); _params_handles.vibe_thresh = param_find("ATT_VIBE_THRESH");
_params_handles.ext_hdg_mode = param_find("ATT_EXT_HDG_M");
} }
/** /**
@@ -271,6 +288,10 @@ void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[])
void AttitudeEstimatorQ::task_main() void AttitudeEstimatorQ::task_main()
{ {
_sensors_sub = orb_subscribe(ORB_ID(sensor_combined)); _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)); _params_sub = orb_subscribe(ORB_ID(parameter_update));
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
@@ -376,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; bool gpos_updated;
orb_check(_global_pos_sub, &gpos_updated); orb_check(_global_pos_sub, &gpos_updated);
@@ -503,6 +565,7 @@ void AttitudeEstimatorQ::update_parameters(bool force)
param_get(_params_handles.w_acc, &_w_accel); param_get(_params_handles.w_acc, &_w_accel);
param_get(_params_handles.w_mag, &_w_mag); 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); param_get(_params_handles.w_gyro_bias, &_w_gyro_bias);
float mag_decl_deg = 0.0f; float mag_decl_deg = 0.0f;
param_get(_params_handles.mag_decl, &mag_decl_deg); param_get(_params_handles.mag_decl, &mag_decl_deg);
@@ -515,6 +578,7 @@ void AttitudeEstimatorQ::update_parameters(bool force)
_acc_comp = acc_comp_int != 0; _acc_comp = acc_comp_int != 0;
param_get(_params_handles.bias_max, &_bias_max); param_get(_params_handles.bias_max, &_bias_max);
param_get(_params_handles.vibe_thresh, &_vibration_warning_threshold); param_get(_params_handles.vibe_thresh, &_vibration_warning_threshold);
param_get(_params_handles.ext_hdg_mode, &_ext_hdg_mode);
} }
} }
@@ -570,12 +634,34 @@ bool AttitudeEstimatorQ::update(float dt)
// Angular rate of correction // Angular rate of correction
Vector<3> corr; Vector<3> corr;
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 // Magnetometer correction
// Project mag field vector to global frame and extract XY component // Project mag field vector to global frame and extract XY component
Vector<3> mag_earth = _q.conjugate(_mag); Vector<3> mag_earth = _q.conjugate(_mag);
float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl); float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);
// Project magnetometer correction to body frame // Project magnetometer correction to body frame
corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag; corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag;
}
// Accelerometer correction // Accelerometer correction
// Project 'k' unit vector of earth frame to body frame // 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); 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 * 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); 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 * Enable acceleration compensation based on GPS
* velocity. * velocity.
src/modules/commander/CMakeLists.txt
+1 -1
View File
@@ -32,7 +32,7 @@
############################################################################ ############################################################################
set(MODULE_CFLAGS -Os) set(MODULE_CFLAGS -Os)
if(${OS} STREQUAL "nuttx") if(${OS} STREQUAL "nuttx")
list(APPEND MODULE_CFLAGS -Wframe-larger-than=2300) list(APPEND MODULE_CFLAGS -Wframe-larger-than=2450)
endif() endif()
px4_add_module( px4_add_module(
MODULE modules__commander MODULE modules__commander
src/modules/commander/commander.cpp
+135 -34
View File
@@ -72,6 +72,7 @@
#include <uORB/topics/home_position.h> #include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_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/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_command.h> #include <uORB/topics/vehicle_command.h>
@@ -92,6 +93,7 @@
#include <uORB/topics/telemetry_status.h> #include <uORB/topics/telemetry_status.h>
#include <uORB/topics/vtol_vehicle_status.h> #include <uORB/topics/vtol_vehicle_status.h>
#include <uORB/topics/vehicle_land_detected.h> #include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/input_rc.h>
#include <drivers/drv_led.h> #include <drivers/drv_led.h>
#include <drivers/drv_hrt.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_INTERVAL 5000000
#define PRINT_MODE_REJECT_INTERVAL 10000000 #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_MIN 1000
#define HIL_ID_MAX 1999 #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 volatile bool thread_running = false; /**< daemon status flag */
static int daemon_task; /**< Handle of daemon task / thread */ 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 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 hrt_abstime commander_boot_timestamp = 0;
static unsigned int leds_counter; static unsigned int leds_counter;
/* To remember when last notification was sent */ /* To remember when last notification was sent */
static uint64_t last_print_mode_reject_time = 0; static uint64_t last_print_mode_reject_time = 0;
static uint64_t _inair_last_time = 0;
static float eph_threshold = 5.0f; static float eph_threshold = 5.0f;
static float epv_threshold = 10.0f; static float epv_threshold = 10.0f;
@@ -197,7 +201,7 @@ static struct home_position_s _home;
static unsigned _last_mission_instance = 0; static unsigned _last_mission_instance = 0;
static manual_control_setpoint_s _last_sp_man; 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 * The daemon app only briefly exists to start
@@ -221,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, 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 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. * Mainloop of commander.
@@ -246,6 +250,9 @@ void print_reject_arm(const char *msg);
void print_status(); void print_status();
transition_result_t check_navigation_state_machine(struct vehicle_status_s *status,
struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy); transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy);
/** /**
@@ -253,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. * time the vehicle is armed with a good GPS fix.
**/ **/
static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home, 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. * Loop that runs at a lower rate and priority for calibration and parameter tasks.
@@ -291,7 +299,7 @@ int commander_main(int argc, char *argv[])
daemon_task = px4_task_spawn_cmd("commander", daemon_task = px4_task_spawn_cmd("commander",
SCHED_DEFAULT, SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 40, SCHED_PRIORITY_MAX - 40,
3400, 3500,
commander_thread_main, commander_thread_main,
(char * const *)&argv[0]); (char * const *)&argv[0]);
@@ -396,11 +404,11 @@ int commander_main(int argc, char *argv[])
void usage(const char *reason) void usage(const char *reason)
{ {
if (reason) { if (reason && *reason > 0) {
PX4_INFO("%s\n", reason); 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() void print_status()
@@ -490,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, 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 vehicle_command_s *cmd, struct actuator_armed_s *armed_local,
struct home_position_s *home, struct vehicle_global_position_s *global_pos, 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 */ /* 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) if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id)
@@ -524,7 +532,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
if (cmd_arm && (arming_ret == TRANSITION_CHANGED) && if (cmd_arm && (arming_ret == TRANSITION_CHANGED) &&
(hrt_absolute_time() > (commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL))) { (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) { if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
@@ -833,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. * time the vehicle is armed with a good GPS fix.
**/ **/
static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home, 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 //Need global position fix to be able to set home
if (!status.condition_global_position_valid) { if (!status.condition_global_position_valid) {
@@ -855,6 +864,8 @@ static void commander_set_home_position(orb_advert_t &homePub, home_position_s &
home.y = localPosition.y; home.y = localPosition.y;
home.z = localPosition.z; home.z = localPosition.z;
home.yaw = attitude.yaw;
warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt);
mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
@@ -881,6 +892,7 @@ int commander_thread_main(int argc, char *argv[])
commander_initialized = false; commander_initialized = false;
bool arm_tune_played = false; bool arm_tune_played = false;
bool was_landed = true;
bool was_armed = false; bool was_armed = false;
bool startup_in_hil = false; bool startup_in_hil = false;
@@ -917,6 +929,8 @@ int commander_thread_main(int argc, char *argv[])
param_t _param_eph = param_find("COM_HOME_H_T"); param_t _param_eph = param_find("COM_HOME_H_T");
param_t _param_epv = param_find("COM_HOME_V_T"); param_t _param_epv = param_find("COM_HOME_V_T");
param_t _param_geofence_action = param_find("GF_ACTION"); 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]; // const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX];
// main_states_str[vehicle_status_s::MAIN_STATE_MANUAL] = "MANUAL"; // main_states_str[vehicle_status_s::MAIN_STATE_MANUAL] = "MANUAL";
@@ -941,6 +955,7 @@ int commander_thread_main(int argc, char *argv[])
const char *nav_states_str[vehicle_status_s::NAVIGATION_STATE_MAX]; 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_MANUAL] = "MANUAL";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_STAB] = "STAB"; 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_ALTCTL] = "ALTCTL";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_POSCTL] = "POSCTL"; nav_states_str[vehicle_status_s::NAVIGATION_STATE_POSCTL] = "POSCTL";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION";
@@ -1029,17 +1044,15 @@ int commander_thread_main(int argc, char *argv[])
px4_task_exit(ERROR); px4_task_exit(ERROR);
} }
/* armed topic */
orb_advert_t armed_pub;
/* Initialize armed with all false */ /* Initialize armed with all false */
memset(&armed, 0, sizeof(armed)); memset(&armed, 0, sizeof(armed));
/* armed topic */
orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
/* vehicle control mode topic */ /* vehicle control mode topic */
memset(&control_mode, 0, sizeof(control_mode)); memset(&control_mode, 0, sizeof(control_mode));
orb_advert_t control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &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 */ /* home position */
orb_advert_t home_pub = nullptr; orb_advert_t home_pub = nullptr;
memset(&_home, 0, sizeof(_home)); memset(&_home, 0, sizeof(_home));
@@ -1138,13 +1151,15 @@ int commander_thread_main(int argc, char *argv[])
/* Subscribe to local position data */ /* Subscribe to local position data */
int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
struct vehicle_local_position_s local_position; struct vehicle_local_position_s local_position = {};
memset(&local_position, 0, sizeof(local_position));
/* Subscribe to attitude data */
int attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
struct vehicle_attitude_s attitude = {};
/* Subscribe to land detector */ /* Subscribe to land detector */
int land_detector_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); int land_detector_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
struct vehicle_land_detected_s land_detector; struct vehicle_land_detected_s land_detector = {};
memset(&land_detector, 0, sizeof(land_detector));
/* /*
* The home position is set based on GPS only, to prevent a dependency between * The home position is set based on GPS only, to prevent a dependency between
@@ -1255,7 +1270,7 @@ int commander_thread_main(int argc, char *argv[])
float rc_loss_timeout = 0.5; float rc_loss_timeout = 0.5;
int32_t datalink_regain_timeout = 0; int32_t datalink_regain_timeout = 0;
uint8_t geofence_action = 0; int32_t geofence_action = 0;
/* Thresholds for engine failure detection */ /* Thresholds for engine failure detection */
int32_t ef_throttle_thres = 1.0f; int32_t ef_throttle_thres = 1.0f;
@@ -1265,6 +1280,9 @@ int commander_thread_main(int argc, char *argv[])
int autosave_params; /**< Autosave of parameters enabled/disabled, loaded from parameter */ 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 */ /* check which state machines for changes, clear "changed" flag */
bool arming_state_changed = false; bool arming_state_changed = false;
bool main_state_changed = false; bool main_state_changed = false;
@@ -1329,6 +1347,16 @@ int commander_thread_main(int argc, char *argv[])
status_changed = true; 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 */ /* re-check RC calibration */
rc_calibration_check(mavlink_fd); rc_calibration_check(mavlink_fd);
} }
@@ -1344,6 +1372,8 @@ int commander_thread_main(int argc, char *argv[])
param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres); param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres);
param_get(_param_ef_time_thres, &ef_time_thres); param_get(_param_ef_time_thres, &ef_time_thres);
param_get(_param_geofence_action, &geofence_action); 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 */ /* Autostart id */
param_get(_param_autostart_id, &autostart_id); param_get(_param_autostart_id, &autostart_id);
@@ -1434,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; telemetry_last_heartbeat[i] = telemetry.heartbeat_time;
} }
} }
@@ -1489,7 +1524,20 @@ int commander_thread_main(int argc, char *argv[])
/* copy avionics voltage */ /* copy avionics voltage */
status.avionics_power_rail_voltage = system_power.voltage5V_v; 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;
} }
} }
@@ -1574,6 +1622,14 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); 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 //update condition_global_position_valid
//Global positions are only published by the estimators if they are valid //Global positions are only published by the estimators if they are valid
if (hrt_absolute_time() - global_position.timestamp > POSITION_TIMEOUT) { if (hrt_absolute_time() - global_position.timestamp > POSITION_TIMEOUT) {
@@ -1618,12 +1674,13 @@ int commander_thread_main(int argc, char *argv[])
&(status.condition_local_altitude_valid), &status_changed); &(status.condition_local_altitude_valid), &status_changed);
/* Update land detector */ /* Update land detector */
static bool check_for_disarming = false;
orb_check(land_detector_sub, &updated); orb_check(land_detector_sub, &updated);
if (updated) { if (updated) {
orb_copy(ORB_ID(vehicle_land_detected), land_detector_sub, &land_detector); 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) { if (status.condition_landed != land_detector.landed) {
status.condition_landed = land_detector.landed; status.condition_landed = land_detector.landed;
status_changed = true; status_changed = true;
@@ -1635,6 +1692,24 @@ int commander_thread_main(int argc, char *argv[])
mavlink_log_critical(mavlink_fd, "TAKEOFF DETECTED"); 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 */ /* update battery status */
@@ -1978,13 +2053,14 @@ int commander_thread_main(int argc, char *argv[])
status.rc_signal_lost = false; 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 */ * do it only for rotary wings */
if (status.is_rotary_wing && status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF && 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.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_MANUAL ||
status.main_state == vehicle_status_s::MAIN_STATE_ACRO || 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_STAB ||
status.main_state == vehicle_status_s::MAIN_STATE_RATTITUDE ||
status.condition_landed) && status.condition_landed) &&
sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
@@ -2184,7 +2260,7 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
/* handle it */ /* 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; status_changed = true;
} }
} }
@@ -2196,6 +2272,7 @@ int commander_thread_main(int argc, char *argv[])
* and both failed we want to terminate the flight */ * and both failed we want to terminate the flight */
if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL && 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_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_STAB &&
status.main_state !=vehicle_status_s::MAIN_STATE_ALTCTL && status.main_state !=vehicle_status_s::MAIN_STATE_ALTCTL &&
status.main_state !=vehicle_status_s::MAIN_STATE_POSCTL && status.main_state !=vehicle_status_s::MAIN_STATE_POSCTL &&
@@ -2220,6 +2297,7 @@ int commander_thread_main(int argc, char *argv[])
* If we are in manual (controlled with RC): * If we are in manual (controlled with RC):
* if both failed we want to terminate the flight */ * if both failed we want to terminate the flight */
if ((status.main_state ==vehicle_status_s::MAIN_STATE_ACRO || 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_MANUAL ||
status.main_state !=vehicle_status_s::MAIN_STATE_STAB || status.main_state !=vehicle_status_s::MAIN_STATE_STAB ||
status.main_state ==vehicle_status_s::MAIN_STATE_ALTCTL || status.main_state ==vehicle_status_s::MAIN_STATE_ALTCTL ||
@@ -2246,14 +2324,18 @@ int commander_thread_main(int argc, char *argv[])
/* First time home position update - but only if disarmed */ /* First time home position update - but only if disarmed */
if (!status.condition_home_position_valid && !armed.armed) { 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 */ /* update home position on arming if at least 1s 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) { else if (((!was_armed && armed.armed) || (was_landed && !status.condition_landed)) &&
commander_set_home_position(home_pub, _home, local_position, global_position); (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 */ /* print new state */
if (arming_state_changed) { if (arming_state_changed) {
status_changed = true; status_changed = true;
@@ -2261,8 +2343,6 @@ int commander_thread_main(int argc, char *argv[])
arming_state_changed = false; arming_state_changed = false;
} }
was_armed = armed.armed;
/* now set navigation state according to failsafe and main state */ /* now set navigation state according to failsafe and main state */
bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled, bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled,
mission_result.finished, mission_result.finished,
@@ -2518,6 +2598,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
(_last_sp_man.return_switch == sp_man->return_switch) && (_last_sp_man.return_switch == sp_man->return_switch) &&
(_last_sp_man.mode_switch == sp_man->mode_switch) && (_last_sp_man.mode_switch == sp_man->mode_switch) &&
(_last_sp_man.acro_switch == sp_man->acro_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.posctl_switch == sp_man->posctl_switch) &&
(_last_sp_man.loiter_switch == sp_man->loiter_switch))) { (_last_sp_man.loiter_switch == sp_man->loiter_switch))) {
@@ -2585,6 +2666,15 @@ 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); res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB);
} }
}
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 { }else {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL); res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL);
} }
@@ -2708,6 +2798,18 @@ set_control_mode()
control_mode.flag_external_manual_override_ok = false; control_mode.flag_external_manual_override_ok = false;
break; 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: case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
control_mode.flag_control_manual_enabled = true; control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false; control_mode.flag_control_auto_enabled = false;
@@ -2956,11 +3058,10 @@ void *commander_low_prio_loop(void *arg)
if (need_param_autosave_timeout > 0 && hrt_elapsed_time(&need_param_autosave_timeout) > 200000ULL) { if (need_param_autosave_timeout > 0 && hrt_elapsed_time(&need_param_autosave_timeout) > 200000ULL) {
int ret = param_save_default(); int ret = param_save_default();
if (ret == OK) { if (ret != OK) {
mavlink_and_console_log_info(mavlink_fd, "settings autosaved"); mavlink_and_console_log_critical(mavlink_fd, "settings auto save error");
} else { } else {
mavlink_and_console_log_critical(mavlink_fd, "settings save error"); warnx("settings saved.");
} }
need_param_autosave = false; 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 * @max 2
*/ */
PARAM_DEFINE_INT32(COM_RC_IN_MODE, 0); 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) { switch (new_main_state) {
case vehicle_status_s::MAIN_STATE_MANUAL: case vehicle_status_s::MAIN_STATE_MANUAL:
case vehicle_status_s::MAIN_STATE_ACRO: case vehicle_status_s::MAIN_STATE_ACRO:
case vehicle_status_s::MAIN_STATE_RATTITUDE:
case vehicle_status_s::MAIN_STATE_STAB: case vehicle_status_s::MAIN_STATE_STAB:
ret = TRANSITION_CHANGED; ret = TRANSITION_CHANGED;
break; break;
@@ -528,6 +529,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
switch (status->main_state) { switch (status->main_state) {
case vehicle_status_s::MAIN_STATE_ACRO: case vehicle_status_s::MAIN_STATE_ACRO:
case vehicle_status_s::MAIN_STATE_MANUAL: 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_STAB:
case vehicle_status_s::MAIN_STATE_ALTCTL: case vehicle_status_s::MAIN_STATE_ALTCTL:
case vehicle_status_s::MAIN_STATE_POSCTL: 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; status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
break; 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: case vehicle_status_s::MAIN_STATE_STAB:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_STAB; status->nav_state = vehicle_status_s::NAVIGATION_STATE_STAB;
break; break;
@@ -737,5 +743,12 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
checkAirspeed = true; 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( px4_add_module(
MODULE modules__controllib MODULE modules__controllib
MAIN controllib_test
COMPILE_FLAGS COMPILE_FLAGS
-Os -Os
SRCS SRCS
controllib_test_main.cpp
test_params.c test_params.c
block/Block.cpp block/Block.cpp
block/BlockParam.cpp block/BlockParam.cpp
src/modules/controllib/blocks.cpp
+123 -111
View File
@@ -43,6 +43,8 @@
#include "blocks.hpp" #include "blocks.hpp"
#define ASSERT_CL(T) if (!(T)) { printf("FAIL\n"); return -1; }
namespace control namespace control
{ {
@@ -62,7 +64,8 @@ int basicBlocksTest()
blockPIDTest(); blockPIDTest();
blockOutputTest(); blockOutputTest();
blockRandUniformTest(); blockRandUniformTest();
blockRandGaussTest(); // known failures
// blockRandGaussTest();
return 0; return 0;
} }
@@ -83,13 +86,13 @@ int blockLimitTest()
printf("Test BlockLimit\t\t\t: "); printf("Test BlockLimit\t\t\t: ");
BlockLimit limit(NULL, "TEST"); BlockLimit limit(NULL, "TEST");
// initial state // initial state
ASSERT(equal(1.0f, limit.getMax())); ASSERT_CL(equal(1.0f, limit.getMax()));
ASSERT(equal(-1.0f, limit.getMin())); ASSERT_CL(equal(-1.0f, limit.getMin()));
ASSERT(equal(0.0f, limit.getDt())); ASSERT_CL(equal(0.0f, limit.getDt()));
// update // update
ASSERT(equal(-1.0f, limit.update(-2.0f))); ASSERT_CL(equal(-1.0f, limit.update(-2.0f)));
ASSERT(equal(1.0f, limit.update(2.0f))); ASSERT_CL(equal(1.0f, limit.update(2.0f)));
ASSERT(equal(0.0f, limit.update(0.0f))); ASSERT_CL(equal(0.0f, limit.update(0.0f)));
printf("PASS\n"); printf("PASS\n");
return 0; return 0;
} }
@@ -111,12 +114,12 @@ int blockLimitSymTest()
printf("Test BlockLimitSym\t\t: "); printf("Test BlockLimitSym\t\t: ");
BlockLimitSym limit(NULL, "TEST"); BlockLimitSym limit(NULL, "TEST");
// initial state // initial state
ASSERT(equal(1.0f, limit.getMax())); ASSERT_CL(equal(1.0f, limit.getMax()));
ASSERT(equal(0.0f, limit.getDt())); ASSERT_CL(equal(0.0f, limit.getDt()));
// update // update
ASSERT(equal(-1.0f, limit.update(-2.0f))); ASSERT_CL(equal(-1.0f, limit.update(-2.0f)));
ASSERT(equal(1.0f, limit.update(2.0f))); ASSERT_CL(equal(1.0f, limit.update(2.0f)));
ASSERT(equal(0.0f, limit.update(0.0f))); ASSERT_CL(equal(0.0f, limit.update(0.0f)));
printf("PASS\n"); printf("PASS\n");
return 0; return 0;
} }
@@ -138,25 +141,25 @@ int blockLowPassTest()
printf("Test BlockLowPass\t\t: "); printf("Test BlockLowPass\t\t: ");
BlockLowPass lowPass(NULL, "TEST_LP"); BlockLowPass lowPass(NULL, "TEST_LP");
// test initial state // test initial state
ASSERT(equal(10.0f, lowPass.getFCut())); ASSERT_CL(equal(10.0f, lowPass.getFCut()));
ASSERT(equal(0.0f, lowPass.getState())); ASSERT_CL(equal(0.0f, lowPass.getState()));
ASSERT(equal(0.0f, lowPass.getDt())); ASSERT_CL(equal(0.0f, lowPass.getDt()));
// set dt // set dt
lowPass.setDt(0.1f); lowPass.setDt(0.1f);
ASSERT(equal(0.1f, lowPass.getDt())); ASSERT_CL(equal(0.1f, lowPass.getDt()));
// set state // set state
lowPass.setState(1.0f); lowPass.setState(1.0f);
ASSERT(equal(1.0f, lowPass.getState())); ASSERT_CL(equal(1.0f, lowPass.getState()));
// test update // test update
ASSERT(equal(1.8626974f, lowPass.update(2.0f))); ASSERT_CL(equal(1.8626974f, lowPass.update(2.0f)));
// test end condition // test end condition
for (int i = 0; i < 100; i++) { for (int i = 0; i < 100; i++) {
lowPass.update(2.0f); lowPass.update(2.0f);
} }
ASSERT(equal(2.0f, lowPass.getState())); ASSERT_CL(equal(2.0f, lowPass.getState()));
ASSERT(equal(2.0f, lowPass.update(2.0f))); ASSERT_CL(equal(2.0f, lowPass.update(2.0f)));
printf("PASS\n"); printf("PASS\n");
return 0; return 0;
}; };
@@ -175,28 +178,28 @@ int blockHighPassTest()
printf("Test BlockHighPass\t\t: "); printf("Test BlockHighPass\t\t: ");
BlockHighPass highPass(NULL, "TEST_HP"); BlockHighPass highPass(NULL, "TEST_HP");
// test initial state // test initial state
ASSERT(equal(10.0f, highPass.getFCut())); ASSERT_CL(equal(10.0f, highPass.getFCut()));
ASSERT(equal(0.0f, highPass.getU())); ASSERT_CL(equal(0.0f, highPass.getU()));
ASSERT(equal(0.0f, highPass.getY())); ASSERT_CL(equal(0.0f, highPass.getY()));
ASSERT(equal(0.0f, highPass.getDt())); ASSERT_CL(equal(0.0f, highPass.getDt()));
// set dt // set dt
highPass.setDt(0.1f); highPass.setDt(0.1f);
ASSERT(equal(0.1f, highPass.getDt())); ASSERT_CL(equal(0.1f, highPass.getDt()));
// set state // set state
highPass.setU(1.0f); highPass.setU(1.0f);
ASSERT(equal(1.0f, highPass.getU())); ASSERT_CL(equal(1.0f, highPass.getU()));
highPass.setY(1.0f); highPass.setY(1.0f);
ASSERT(equal(1.0f, highPass.getY())); ASSERT_CL(equal(1.0f, highPass.getY()));
// test update // test update
ASSERT(equal(0.2746051f, highPass.update(2.0f))); ASSERT_CL(equal(0.2746051f, highPass.update(2.0f)));
// test end condition // test end condition
for (int i = 0; i < 100; i++) { for (int i = 0; i < 100; i++) {
highPass.update(2.0f); highPass.update(2.0f);
} }
ASSERT(equal(0.0f, highPass.getY())); ASSERT_CL(equal(0.0f, highPass.getY()));
ASSERT(equal(0.0f, highPass.update(2.0f))); ASSERT_CL(equal(0.0f, highPass.update(2.0f)));
printf("PASS\n"); printf("PASS\n");
return 0; return 0;
} }
@@ -220,25 +223,25 @@ int blockLowPass2Test()
printf("Test BlockLowPass2\t\t: "); printf("Test BlockLowPass2\t\t: ");
BlockLowPass2 lowPass(NULL, "TEST_LP", 100); BlockLowPass2 lowPass(NULL, "TEST_LP", 100);
// test initial state // test initial state
ASSERT(equal(10.0f, lowPass.getFCutParam())); ASSERT_CL(equal(10.0f, lowPass.getFCutParam()));
ASSERT(equal(0.0f, lowPass.getState())); ASSERT_CL(equal(0.0f, lowPass.getState()));
ASSERT(equal(0.0f, lowPass.getDt())); ASSERT_CL(equal(0.0f, lowPass.getDt()));
// set dt // set dt
lowPass.setDt(0.1f); lowPass.setDt(0.1f);
ASSERT(equal(0.1f, lowPass.getDt())); ASSERT_CL(equal(0.1f, lowPass.getDt()));
// set state // set state
lowPass.setState(1.0f); lowPass.setState(1.0f);
ASSERT(equal(1.0f, lowPass.getState())); ASSERT_CL(equal(1.0f, lowPass.getState()));
// test update // test update
ASSERT(equal(1.06745527f, lowPass.update(2.0f))); ASSERT_CL(equal(1.06745527f, lowPass.update(2.0f)));
// test end condition // test end condition
for (int i = 0; i < 100; i++) { for (int i = 0; i < 100; i++) {
lowPass.update(2.0f); lowPass.update(2.0f);
} }
ASSERT(equal(2.0f, lowPass.getState())); ASSERT_CL(equal(2.0f, lowPass.getState()));
ASSERT(equal(2.0f, lowPass.update(2.0f))); ASSERT_CL(equal(2.0f, lowPass.update(2.0f)));
printf("PASS\n"); printf("PASS\n");
return 0; return 0;
}; };
@@ -255,34 +258,34 @@ int blockIntegralTest()
printf("Test BlockIntegral\t\t: "); printf("Test BlockIntegral\t\t: ");
BlockIntegral integral(NULL, "TEST_I"); BlockIntegral integral(NULL, "TEST_I");
// test initial state // test initial state
ASSERT(equal(1.0f, integral.getMax())); ASSERT_CL(equal(1.0f, integral.getMax()));
ASSERT(equal(0.0f, integral.getDt())); ASSERT_CL(equal(0.0f, integral.getDt()));
// set dt // set dt
integral.setDt(0.1f); integral.setDt(0.1f);
ASSERT(equal(0.1f, integral.getDt())); ASSERT_CL(equal(0.1f, integral.getDt()));
// set Y // set Y
integral.setY(0.9f); integral.setY(0.9f);
ASSERT(equal(0.9f, integral.getY())); ASSERT_CL(equal(0.9f, integral.getY()));
// test exceed max // test exceed max
for (int i = 0; i < 100; i++) { for (int i = 0; i < 100; i++) {
integral.update(1.0f); integral.update(1.0f);
} }
ASSERT(equal(1.0f, integral.update(1.0f))); ASSERT_CL(equal(1.0f, integral.update(1.0f)));
// test exceed min // test exceed min
integral.setY(-0.9f); integral.setY(-0.9f);
ASSERT(equal(-0.9f, integral.getY())); ASSERT_CL(equal(-0.9f, integral.getY()));
for (int i = 0; i < 100; i++) { for (int i = 0; i < 100; i++) {
integral.update(-1.0f); integral.update(-1.0f);
} }
ASSERT(equal(-1.0f, integral.update(-1.0f))); ASSERT_CL(equal(-1.0f, integral.update(-1.0f)));
// test update // test update
integral.setY(0.1f); integral.setY(0.1f);
ASSERT(equal(0.2f, integral.update(1.0))); ASSERT_CL(equal(0.2f, integral.update(1.0)));
ASSERT(equal(0.2f, integral.getY())); ASSERT_CL(equal(0.2f, integral.getY()));
printf("PASS\n"); printf("PASS\n");
return 0; return 0;
} }
@@ -301,40 +304,40 @@ int blockIntegralTrapTest()
printf("Test BlockIntegralTrap\t\t: "); printf("Test BlockIntegralTrap\t\t: ");
BlockIntegralTrap integral(NULL, "TEST_I"); BlockIntegralTrap integral(NULL, "TEST_I");
// test initial state // test initial state
ASSERT(equal(1.0f, integral.getMax())); ASSERT_CL(equal(1.0f, integral.getMax()));
ASSERT(equal(0.0f, integral.getDt())); ASSERT_CL(equal(0.0f, integral.getDt()));
// set dt // set dt
integral.setDt(0.1f); integral.setDt(0.1f);
ASSERT(equal(0.1f, integral.getDt())); ASSERT_CL(equal(0.1f, integral.getDt()));
// set U // set U
integral.setU(1.0f); integral.setU(1.0f);
ASSERT(equal(1.0f, integral.getU())); ASSERT_CL(equal(1.0f, integral.getU()));
// set Y // set Y
integral.setY(0.9f); integral.setY(0.9f);
ASSERT(equal(0.9f, integral.getY())); ASSERT_CL(equal(0.9f, integral.getY()));
// test exceed max // test exceed max
for (int i = 0; i < 100; i++) { for (int i = 0; i < 100; i++) {
integral.update(1.0f); integral.update(1.0f);
} }
ASSERT(equal(1.0f, integral.update(1.0f))); ASSERT_CL(equal(1.0f, integral.update(1.0f)));
// test exceed min // test exceed min
integral.setU(-1.0f); integral.setU(-1.0f);
integral.setY(-0.9f); integral.setY(-0.9f);
ASSERT(equal(-0.9f, integral.getY())); ASSERT_CL(equal(-0.9f, integral.getY()));
for (int i = 0; i < 100; i++) { for (int i = 0; i < 100; i++) {
integral.update(-1.0f); integral.update(-1.0f);
} }
ASSERT(equal(-1.0f, integral.update(-1.0f))); ASSERT_CL(equal(-1.0f, integral.update(-1.0f)));
// test update // test update
integral.setU(2.0f); integral.setU(2.0f);
integral.setY(0.1f); integral.setY(0.1f);
ASSERT(equal(0.25f, integral.update(1.0))); ASSERT_CL(equal(0.25f, integral.update(1.0)));
ASSERT(equal(0.25f, integral.getY())); ASSERT_CL(equal(0.25f, integral.getY()));
ASSERT(equal(1.0f, integral.getU())); ASSERT_CL(equal(1.0f, integral.getU()));
printf("PASS\n"); printf("PASS\n");
return 0; return 0;
} }
@@ -352,6 +355,7 @@ float BlockDerivative::update(float input)
// and so we use the assumption the // and so we use the assumption the
// input value is not changing much, // input value is not changing much,
// which is the best we can do here. // which is the best we can do here.
_lowPass.update(0.0f);
output = 0.0f; output = 0.0f;
_initialized = true; _initialized = true;
} }
@@ -365,17 +369,20 @@ int blockDerivativeTest()
printf("Test BlockDerivative\t\t: "); printf("Test BlockDerivative\t\t: ");
BlockDerivative derivative(NULL, "TEST_D"); BlockDerivative derivative(NULL, "TEST_D");
// test initial state // test initial state
ASSERT(equal(0.0f, derivative.getU())); ASSERT_CL(equal(0.0f, derivative.getU()));
ASSERT(equal(10.0f, derivative.getLP())); ASSERT_CL(equal(10.0f, derivative.getLP()));
// set dt // set dt
derivative.setDt(0.1f); derivative.setDt(0.1f);
ASSERT(equal(0.1f, derivative.getDt())); ASSERT_CL(equal(0.1f, derivative.getDt()));
// set U // set U
derivative.setU(1.0f); 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 // test update
ASSERT(equal(8.6269744f, derivative.update(2.0f))); ASSERT_CL(equal(8.6269744f, derivative.update(2.0f)));
ASSERT(equal(2.0f, derivative.getU())); ASSERT_CL(equal(2.0f, derivative.getU()));
printf("PASS\n"); printf("PASS\n");
return 0; return 0;
} }
@@ -385,13 +392,13 @@ int blockPTest()
printf("Test BlockP\t\t\t: "); printf("Test BlockP\t\t\t: ");
BlockP blockP(NULL, "TEST_P"); BlockP blockP(NULL, "TEST_P");
// test initial state // test initial state
ASSERT(equal(0.2f, blockP.getKP())); ASSERT_CL(equal(0.2f, blockP.getKP()));
ASSERT(equal(0.0f, blockP.getDt())); ASSERT_CL(equal(0.0f, blockP.getDt()));
// set dt // set dt
blockP.setDt(0.1f); blockP.setDt(0.1f);
ASSERT(equal(0.1f, blockP.getDt())); ASSERT_CL(equal(0.1f, blockP.getDt()));
// test update // test update
ASSERT(equal(0.4f, blockP.update(2.0f))); ASSERT_CL(equal(0.4f, blockP.update(2.0f)));
printf("PASS\n"); printf("PASS\n");
return 0; return 0;
} }
@@ -401,19 +408,19 @@ int blockPITest()
printf("Test BlockPI\t\t\t: "); printf("Test BlockPI\t\t\t: ");
BlockPI blockPI(NULL, "TEST"); BlockPI blockPI(NULL, "TEST");
// test initial state // test initial state
ASSERT(equal(0.2f, blockPI.getKP())); ASSERT_CL(equal(0.2f, blockPI.getKP()));
ASSERT(equal(0.1f, blockPI.getKI())); ASSERT_CL(equal(0.1f, blockPI.getKI()));
ASSERT(equal(0.0f, blockPI.getDt())); ASSERT_CL(equal(0.0f, blockPI.getDt()));
ASSERT(equal(1.0f, blockPI.getIntegral().getMax())); ASSERT_CL(equal(1.0f, blockPI.getIntegral().getMax()));
// set dt // set dt
blockPI.setDt(0.1f); blockPI.setDt(0.1f);
ASSERT(equal(0.1f, blockPI.getDt())); ASSERT_CL(equal(0.1f, blockPI.getDt()));
// set integral state // set integral state
blockPI.getIntegral().setY(0.1f); blockPI.getIntegral().setY(0.1f);
ASSERT(equal(0.1f, blockPI.getIntegral().getY())); ASSERT_CL(equal(0.1f, blockPI.getIntegral().getY()));
// test update // test update
// 0.2*2 + 0.1*(2*0.1 + 0.1) = 0.43 // 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"); printf("PASS\n");
return 0; return 0;
} }
@@ -423,19 +430,22 @@ int blockPDTest()
printf("Test BlockPD\t\t\t: "); printf("Test BlockPD\t\t\t: ");
BlockPD blockPD(NULL, "TEST"); BlockPD blockPD(NULL, "TEST");
// test initial state // test initial state
ASSERT(equal(0.2f, blockPD.getKP())); ASSERT_CL(equal(0.2f, blockPD.getKP()));
ASSERT(equal(0.01f, blockPD.getKD())); ASSERT_CL(equal(0.01f, blockPD.getKD()));
ASSERT(equal(0.0f, blockPD.getDt())); ASSERT_CL(equal(0.0f, blockPD.getDt()));
ASSERT(equal(10.0f, blockPD.getDerivative().getLP())); ASSERT_CL(equal(10.0f, blockPD.getDerivative().getLP()));
// set dt // set dt
blockPD.setDt(0.1f); blockPD.setDt(0.1f);
ASSERT(equal(0.1f, blockPD.getDt())); ASSERT_CL(equal(0.1f, blockPD.getDt()));
// set derivative state // set derivative state
blockPD.getDerivative().setU(1.0f); 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 // test update
// 0.2*2 + 0.1*(0.1*8.626...) = 0.486269744 // 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"); printf("PASS\n");
return 0; return 0;
} }
@@ -445,24 +455,27 @@ int blockPIDTest()
printf("Test BlockPID\t\t\t: "); printf("Test BlockPID\t\t\t: ");
BlockPID blockPID(NULL, "TEST"); BlockPID blockPID(NULL, "TEST");
// test initial state // test initial state
ASSERT(equal(0.2f, blockPID.getKP())); ASSERT_CL(equal(0.2f, blockPID.getKP()));
ASSERT(equal(0.1f, blockPID.getKI())); ASSERT_CL(equal(0.1f, blockPID.getKI()));
ASSERT(equal(0.01f, blockPID.getKD())); ASSERT_CL(equal(0.01f, blockPID.getKD()));
ASSERT(equal(0.0f, blockPID.getDt())); ASSERT_CL(equal(0.0f, blockPID.getDt()));
ASSERT(equal(10.0f, blockPID.getDerivative().getLP())); ASSERT_CL(equal(10.0f, blockPID.getDerivative().getLP()));
ASSERT(equal(1.0f, blockPID.getIntegral().getMax())); ASSERT_CL(equal(1.0f, blockPID.getIntegral().getMax()));
// set dt // set dt
blockPID.setDt(0.1f); blockPID.setDt(0.1f);
ASSERT(equal(0.1f, blockPID.getDt())); ASSERT_CL(equal(0.1f, blockPID.getDt()));
// set derivative state // set derivative state
blockPID.getDerivative().setU(1.0f); 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 // set integral state
blockPID.getIntegral().setY(0.1f); blockPID.getIntegral().setY(0.1f);
ASSERT(equal(0.1f, blockPID.getIntegral().getY())); ASSERT_CL(equal(0.1f, blockPID.getIntegral().getY()));
// test update // test update
// 0.2*2 + 0.1*(2*0.1 + 0.1) + 0.1*(0.1*8.626...) = 0.5162697 // 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"); printf("PASS\n");
return 0; return 0;
} }
@@ -472,19 +485,19 @@ int blockOutputTest()
printf("Test BlockOutput\t\t: "); printf("Test BlockOutput\t\t: ");
BlockOutput blockOutput(NULL, "TEST"); BlockOutput blockOutput(NULL, "TEST");
// test initial state // test initial state
ASSERT(equal(0.0f, blockOutput.getDt())); ASSERT_CL(equal(0.0f, blockOutput.getDt()));
ASSERT(equal(0.5f, blockOutput.get())); ASSERT_CL(equal(0.5f, blockOutput.get()));
ASSERT(equal(-1.0f, blockOutput.getMin())); ASSERT_CL(equal(-1.0f, blockOutput.getMin()));
ASSERT(equal(1.0f, blockOutput.getMax())); ASSERT_CL(equal(1.0f, blockOutput.getMax()));
// test update below min // test update below min
blockOutput.update(-2.0f); blockOutput.update(-2.0f);
ASSERT(equal(-1.0f, blockOutput.get())); ASSERT_CL(equal(-1.0f, blockOutput.get()));
// test update above max // test update above max
blockOutput.update(2.0f); blockOutput.update(2.0f);
ASSERT(equal(1.0f, blockOutput.get())); ASSERT_CL(equal(1.0f, blockOutput.get()));
// test trim // test trim
blockOutput.update(0.0f); blockOutput.update(0.0f);
ASSERT(equal(0.5f, blockOutput.get())); ASSERT_CL(equal(0.5f, blockOutput.get()));
printf("PASS\n"); printf("PASS\n");
return 0; return 0;
} }
@@ -495,22 +508,21 @@ int blockRandUniformTest()
printf("Test BlockRandUniform\t\t: "); printf("Test BlockRandUniform\t\t: ");
BlockRandUniform blockRandUniform(NULL, "TEST"); BlockRandUniform blockRandUniform(NULL, "TEST");
// test initial state // test initial state
ASSERT(equal(0.0f, blockRandUniform.getDt())); ASSERT_CL(equal(0.0f, blockRandUniform.getDt()));
ASSERT(equal(-1.0f, blockRandUniform.getMin())); ASSERT_CL(equal(-1.0f, blockRandUniform.getMin()));
ASSERT(equal(1.0f, blockRandUniform.getMax())); ASSERT_CL(equal(1.0f, blockRandUniform.getMax()));
// test update // test update
int n = 10000; int n = 10000;
float mean = blockRandUniform.update(); float mean = blockRandUniform.update();
// recursive mean algorithm from Knuth
for (int i = 2; i < n + 1; i++) { for (int i = 2; i < n + 1; i++) {
float val = blockRandUniform.update(); float val = blockRandUniform.update();
mean += (val - mean) / i; mean += (val - mean) / i;
ASSERT(val <= blockRandUniform.getMax()); ASSERT_CL(less_than_or_equal(val, blockRandUniform.getMax()));
ASSERT(val >= blockRandUniform.getMin()); ASSERT_CL(greater_than_or_equal(val, blockRandUniform.getMin()));
} }
ASSERT(equal(mean, (blockRandUniform.getMin() + ASSERT_CL(equal(mean, (blockRandUniform.getMin() +
blockRandUniform.getMax()) / 2, 1e-1)); blockRandUniform.getMax()) / 2, 1e-1));
printf("PASS\n"); printf("PASS\n");
return 0; return 0;
@@ -522,9 +534,9 @@ int blockRandGaussTest()
printf("Test BlockRandGauss\t\t: "); printf("Test BlockRandGauss\t\t: ");
BlockRandGauss blockRandGauss(NULL, "TEST"); BlockRandGauss blockRandGauss(NULL, "TEST");
// test initial state // test initial state
ASSERT(equal(0.0f, blockRandGauss.getDt())); ASSERT_CL(equal(0.0f, blockRandGauss.getDt()));
ASSERT(equal(1.0f, blockRandGauss.getMean())); ASSERT_CL(equal(1.0f, blockRandGauss.getMean()));
ASSERT(equal(2.0f, blockRandGauss.getStdDev())); ASSERT_CL(equal(2.0f, blockRandGauss.getStdDev()));
// test update // test update
int n = 10000; int n = 10000;
float mean = blockRandGauss.update(); float mean = blockRandGauss.update();
@@ -540,8 +552,8 @@ int blockRandGaussTest()
float stdDev = sqrt(sum / (n - 1)); float stdDev = sqrt(sum / (n - 1));
(void)(stdDev); (void)(stdDev);
ASSERT(equal(mean, blockRandGauss.getMean(), 1e-1)); ASSERT_CL(equal(mean, blockRandGauss.getMean(), 1e-1));
ASSERT(equal(stdDev, blockRandGauss.getStdDev(), 1e-1)); ASSERT_CL(equal(stdDev, blockRandGauss.getStdDev(), 1e-1));
printf("PASS\n"); printf("PASS\n");
return 0; 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/ekf_att_pos_estimator_main.cpp
+37 -16
View File
@@ -590,6 +590,11 @@ void AttitudePositionEstimatorEKF::task_main()
/* system is in HIL now, wait for measurements to come in one last round */ /* system is in HIL now, wait for measurements to come in one last round */
usleep(60000); 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 */ /* now read all sensor publications to ensure all real sensor data is purged */
orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
@@ -1295,6 +1300,23 @@ void AttitudePositionEstimatorEKF::pollData()
orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); 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 // Feed validator with recent sensor data
for (unsigned i = 0; i < (sizeof(_sensor_combined.gyro_timestamp) / sizeof(_sensor_combined.gyro_timestamp[0])); i++) { for (unsigned i = 0; i < (sizeof(_sensor_combined.gyro_timestamp) / sizeof(_sensor_combined.gyro_timestamp[0])); i++) {
@@ -1317,6 +1339,11 @@ void AttitudePositionEstimatorEKF::pollData()
_ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 1]; _ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 1];
_ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 2]; _ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 2];
} else { } 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.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.y = 0.5f * (_ekf->angRate.y + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 1]) * _ekf->dtIMU;
_ekf->dAngIMU.z = 0.5f * (_ekf->angRate.z + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2]) * _ekf->dtIMU; _ekf->dAngIMU.z = 0.5f * (_ekf->angRate.z + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2]) * _ekf->dtIMU;
@@ -1389,20 +1416,8 @@ void AttitudePositionEstimatorEKF::pollData()
_vibration_warning_timestamp = 0; _vibration_warning_timestamp = 0;
} }
IMUusec = _sensor_combined.timestamp;
float deltaT = (_sensor_combined.timestamp - _last_sensor_timestamp) / 1e6f;
_last_sensor_timestamp = _sensor_combined.timestamp; _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 // XXX this is for assessing the filter performance
// leave this in as long as larger improvements are still being made. // leave this in as long as larger improvements are still being made.
#if 0 #if 0
@@ -1414,7 +1429,7 @@ void AttitudePositionEstimatorEKF::pollData()
static unsigned dtoverflow10 = 0; static unsigned dtoverflow10 = 0;
static hrt_abstime lastprint = 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", 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, (double)deltaT, (double)deltaTIntegral, (double)deltaTIntAcc,
dtoverflow5, dtoverflow10); dtoverflow5, dtoverflow10);
@@ -1433,7 +1448,15 @@ void AttitudePositionEstimatorEKF::pollData()
(double)(_sensor_combined.gyro_rad_s[0] * deltaT), (double)(_sensor_combined.gyro_rad_s[2] * deltaT), (double)(_sensor_combined.gyro_rad_s[0] * deltaT), (double)(_sensor_combined.gyro_rad_s[2] * deltaT),
(double)(_sensor_combined.accelerometer_m_s2[0] * deltaT), (double)(_sensor_combined.accelerometer_m_s2[0] * deltaT),
(double)(_sensor_combined.accelerometer_m_s2[1] * 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(); lastprint = hrt_absolute_time();
} }
@@ -1721,8 +1744,6 @@ int ekf_att_pos_estimator_main(int argc, char *argv[])
PX4_INFO("."); PX4_INFO(".");
} }
PX4_INFO(" ");
return 0; return 0;
} }
src/modules/ekf_att_pos_estimator/estimator_22states.cpp
+1 -1
View File
@@ -2827,7 +2827,7 @@ bool AttPosEKF::VelNEDDiverged()
Vector3f delta = current_vel - gps_vel; Vector3f delta = current_vel - gps_vel;
float delta_len = delta.length(); 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.error |= excessive;
current_ekf_state.velOffsetExcessive = excessive; current_ekf_state.velOffsetExcessive = excessive;
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+6
View File
@@ -1102,6 +1102,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
if (_mTecs.getEnabled()) { if (_mTecs.getEnabled()) {
_mTecs.resetIntegrators(); _mTecs.resetIntegrators();
_mTecs.resetDerivatives(_ctrl_state.airspeed); _mTecs.resetDerivatives(_ctrl_state.airspeed);
} else {
_tecs.reset_state();
} }
} }
_control_mode_current = FW_POSCTRL_MODE_AUTO; _control_mode_current = FW_POSCTRL_MODE_AUTO;
@@ -1461,6 +1463,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
if (_mTecs.getEnabled()) { if (_mTecs.getEnabled()) {
_mTecs.resetIntegrators(); _mTecs.resetIntegrators();
_mTecs.resetDerivatives(_ctrl_state.airspeed); _mTecs.resetDerivatives(_ctrl_state.airspeed);
} else {
_tecs.reset_state();
} }
} }
_control_mode_current = FW_POSCTRL_MODE_POSITION; _control_mode_current = FW_POSCTRL_MODE_POSITION;
@@ -1572,6 +1576,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
if (_mTecs.getEnabled()) { if (_mTecs.getEnabled()) {
_mTecs.resetIntegrators(); _mTecs.resetIntegrators();
_mTecs.resetDerivatives(_ctrl_state.airspeed); _mTecs.resetDerivatives(_ctrl_state.airspeed);
} else {
_tecs.reset_state();
} }
} }
_control_mode_current = FW_POSCTRL_MODE_ALTITUDE; _control_mode_current = FW_POSCTRL_MODE_ALTITUDE;
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 { } else {
_is_usb_uart = true; _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) #if defined (__PX4_LINUX) || defined (__PX4_DARWIN)
@@ -2094,8 +2097,12 @@ Mavlink::display_status()
printf("3DR RADIO\n"); printf("3DR RADIO\n");
break; break;
case telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB:
printf("USB CDC\n");
break;
default: default:
printf("UNKNOWN RADIO\n"); printf("GENERIC LINK OR RADIO\n");
break; break;
} }
src/modules/mavlink/mavlink_main.h
+2
View File
@@ -288,6 +288,8 @@ public:
float get_rate_mult(); float get_rate_mult();
float get_baudrate() { return _baudrate; }
/* Functions for waiting to start transmission until message received. */ /* Functions for waiting to start transmission until message received. */
void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } void set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
bool get_has_received_messages() { return _received_messages; } bool get_has_received_messages() { return _received_messages; }
src/modules/mavlink/mavlink_mission.cpp
+5 -6
View File
@@ -422,7 +422,9 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg)
} else { } else {
_mavlink->send_statustext_critical("REJ. WP CMD: partner id mismatch"); _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) { if (_state == MAVLINK_WPM_STATE_IDLE || _state == MAVLINK_WPM_STATE_SENDLIST) {
_time_last_recv = hrt_absolute_time(); _time_last_recv = hrt_absolute_time();
if (_count > 0) {
_state = MAVLINK_WPM_STATE_SENDLIST; _state = MAVLINK_WPM_STATE_SENDLIST;
_transfer_seq = 0; _transfer_seq = 0;
_transfer_count = _count; _transfer_count = _count;
_transfer_partner_sysid = msg->sysid; _transfer_partner_sysid = msg->sysid;
_transfer_partner_compid = msg->compid; _transfer_partner_compid = msg->compid;
if (_count > 0) {
if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK, %u mission items to send", _transfer_count); } if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK, %u mission items to send", _transfer_count); }
} else { } else {
@@ -598,9 +600,8 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
/* alternate dataman ID anyway to let navigator know about changes */ /* alternate dataman ID anyway to let navigator know about changes */
update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0); 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; _transfer_in_progress = false;
return; return;
} }
@@ -712,8 +713,6 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)
/* got all new mission items successfully */ /* 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); } 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; _state = MAVLINK_WPM_STATE_IDLE;
if (update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq) == OK) { if (update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq) == OK) {
src/modules/mavlink/mavlink_receiver.cpp
+39 -34
View File
@@ -1116,7 +1116,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
/* yaw */ /* yaw */
rc.values[2] = man.r / 2 + 1500; rc.values[2] = man.r / 2 + 1500;
/* throttle */ /* 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 */ /* decode all switches which fit into the channel mask */
unsigned max_switch = (sizeof(man.buttons) * 8); unsigned max_switch = (sizeof(man.buttons) * 8);
@@ -1307,18 +1307,22 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
float dt; float dt;
if (_hil_last_frame == 0 || if (_hil_last_frame == 0 ||
(imu.time_usec - _hil_last_frame) > (0.1f * 1e6f) || (timestamp <= _hil_last_frame) ||
(_hil_last_frame >= imu.time_usec)) { (timestamp - _hil_last_frame) > (0.1f * 1e6f) ||
(_hil_last_frame >= timestamp)) {
dt = 0.01f; /* default to 100 Hz */ 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 { } 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 */ /* airspeed */
{ {
struct airspeed_s airspeed; struct airspeed_s airspeed = {};
memset(&airspeed, 0, sizeof(airspeed));
float ias = calc_indicated_airspeed(imu.diff_pressure * 1e2f); float ias = calc_indicated_airspeed(imu.diff_pressure * 1e2f);
// XXX need to fix this // XXX need to fix this
@@ -1338,8 +1342,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
/* gyro */ /* gyro */
{ {
struct gyro_report gyro; struct gyro_report gyro = {};
memset(&gyro, 0, sizeof(gyro));
gyro.timestamp = timestamp; gyro.timestamp = timestamp;
gyro.x_raw = imu.xgyro * 1000.0f; gyro.x_raw = imu.xgyro * 1000.0f;
@@ -1360,8 +1363,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
/* accelerometer */ /* accelerometer */
{ {
struct accel_report accel; struct accel_report accel = {};
memset(&accel, 0, sizeof(accel));
accel.timestamp = timestamp; accel.timestamp = timestamp;
accel.x_raw = imu.xacc / mg2ms2; accel.x_raw = imu.xacc / mg2ms2;
@@ -1382,8 +1384,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
/* magnetometer */ /* magnetometer */
{ {
struct mag_report mag; struct mag_report mag = {};
memset(&mag, 0, sizeof(mag));
mag.timestamp = timestamp; mag.timestamp = timestamp;
mag.x_raw = imu.xmag * 1000.0f; mag.x_raw = imu.xmag * 1000.0f;
@@ -1404,8 +1405,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
/* baro */ /* baro */
{ {
struct baro_report baro; struct baro_report baro = {};
memset(&baro, 0, sizeof(baro));
baro.timestamp = timestamp; baro.timestamp = timestamp;
baro.pressure = imu.abs_pressure; baro.pressure = imu.abs_pressure;
@@ -1422,21 +1422,20 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
/* sensor combined */ /* sensor combined */
{ {
struct sensor_combined_s hil_sensors; struct sensor_combined_s hil_sensors = {};
memset(&hil_sensors, 0, sizeof(hil_sensors));
hil_sensors.timestamp = timestamp; hil_sensors.timestamp = timestamp;
hil_sensors.gyro_raw[0] = imu.xgyro * 1000.0f; hil_sensors.gyro_raw[0] = imu.xgyro * 1000.0f;
hil_sensors.gyro_raw[1] = imu.ygyro * 1000.0f; hil_sensors.gyro_raw[1] = imu.ygyro * 1000.0f;
hil_sensors.gyro_raw[2] = imu.zgyro * 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[1] = imu.ygyro;
hil_sensors.gyro_rad_s[2] = imu.zgyro; 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[0] = 0.5f * (imu.xgyro + _hil_prev_gyro[0]) * dt;
hil_sensors.gyro_integral_rad[1] = (hil_sensors.gyro_rad_s[1] * dt + _hil_prev_gyro[1]) / 2.0f; hil_sensors.gyro_integral_rad[1] = 0.5f * (imu.ygyro + _hil_prev_gyro[1]) * dt;
hil_sensors.gyro_integral_rad[2] = (hil_sensors.gyro_rad_s[2] * dt + _hil_prev_gyro[2]) / 2.0f; hil_sensors.gyro_integral_rad[2] = 0.5f * (imu.zgyro + _hil_prev_gyro[2]) * dt;
memcpy(&_hil_prev_gyro[0], &hil_sensors.gyro_integral_rad[0], sizeof(_hil_prev_gyro)); 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_integral_dt[0] = dt * 1e6f;
hil_sensors.gyro_timestamp[0] = timestamp; hil_sensors.gyro_timestamp[0] = timestamp;
hil_sensors.gyro_priority[0] = ORB_PRIO_HIGH; 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[0] = imu.xacc;
hil_sensors.accelerometer_m_s2[1] = imu.yacc; hil_sensors.accelerometer_m_s2[1] = imu.yacc;
hil_sensors.accelerometer_m_s2[2] = imu.zacc; 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[0] = 0.5f * (imu.xacc + _hil_prev_accel[0]) * dt;
hil_sensors.accelerometer_integral_m_s[1] = (imu.yacc * dt + _hil_prev_accel[1]) / 2.0f; hil_sensors.accelerometer_integral_m_s[1] = 0.5f * (imu.yacc + _hil_prev_accel[1]) * dt;
hil_sensors.accelerometer_integral_m_s[2] = (imu.zacc * dt + _hil_prev_accel[2]) / 2.0f; hil_sensors.accelerometer_integral_m_s[2] = 0.5f * (imu.zacc + _hil_prev_accel[2]) * dt;
memcpy(&_hil_prev_accel[0], &hil_sensors.accelerometer_integral_m_s[0], sizeof(_hil_prev_accel)); 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_integral_dt[0] = dt * 1e6f;
hil_sensors.accelerometer_mode[0] = 0; // TODO what is this? hil_sensors.accelerometer_mode[0] = 0; // TODO what is this?
hil_sensors.accelerometer_range_m_s2[0] = 32.7f; // int16 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 */ /* battery status */
{ {
struct battery_status_s hil_battery_status; struct battery_status_s hil_battery_status = {};
memset(&hil_battery_status, 0, sizeof(hil_battery_status));
hil_battery_status.timestamp = timestamp; hil_battery_status.timestamp = timestamp;
hil_battery_status.voltage_v = 11.1f; hil_battery_status.voltage_v = 11.5f;
hil_battery_status.voltage_filtered_v = 11.1f; hil_battery_status.voltage_filtered_v = 11.5f;
hil_battery_status.current_a = 10.0f; hil_battery_status.current_a = 10.0f;
hil_battery_status.discharged_mah = -1.0f; hil_battery_status.discharged_mah = -1.0f;
@@ -1759,7 +1757,7 @@ MavlinkReceiver::receive_thread(void *arg)
uint8_t buf[1600]; uint8_t buf[1600];
#else #else
/* the serial port buffers internally as well, we just need to fit a small chunk */ /* the serial port buffers internally as well, we just need to fit a small chunk */
uint8_t buf[32]; uint8_t buf[64];
#endif #endif
mavlink_message_t msg; mavlink_message_t msg;
@@ -1799,10 +1797,17 @@ MavlinkReceiver::receive_thread(void *arg)
while (!_mavlink->_task_should_exit) { while (!_mavlink->_task_should_exit) {
if (poll(&fds[0], 1, timeout) > 0) { if (poll(&fds[0], 1, timeout) > 0) {
if (_mavlink->get_protocol() == SERIAL) { 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 */ /* non-blocking read. read may return negative values */
if ((nread = ::read(uart_fd, buf, sizeof(buf))) < (ssize_t)sizeof(buf)) { if ((nread = ::read(uart_fd, buf, sizeof(buf))) < (ssize_t)character_count) {
/* to avoid reading very small chunks wait for data before reading */ unsigned sleeptime = (1.0f / (_mavlink->get_baudrate() / 10)) * character_count * 1000000;
usleep(1000); usleep(sleeptime);
} }
} }
#ifdef __PX4_POSIX #ifdef __PX4_POSIX
src/modules/mc_att_control/CMakeLists.txt
-1
View File
@@ -40,7 +40,6 @@ px4_add_module(
COMPILE_FLAGS ${MODULE_CFLAGS} COMPILE_FLAGS ${MODULE_CFLAGS}
SRCS SRCS
mc_att_control_main.cpp mc_att_control_main.cpp
mc_att_control_params.c
DEPENDS DEPENDS
platforms__common platforms__common
) )
src/modules/mc_att_control/mc_att_control_main.cpp
+19 -23
View File
@@ -39,7 +39,6 @@
* Daniel Mellinger and Vijay Kumar. Minimum Snap Trajectory Generation and Control for Quadrotors. * Daniel Mellinger and Vijay Kumar. Minimum Snap Trajectory Generation and Control for Quadrotors.
* Int. Conf. on Robotics and Automation, Shanghai, China, May 2011. * Int. Conf. on Robotics and Automation, Shanghai, China, May 2011.
* *
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lorenz@px4.io> * @author Lorenz Meier <lorenz@px4.io>
* @author Anton Babushkin <anton.babushkin@me.com> * @author Anton Babushkin <anton.babushkin@me.com>
* *
@@ -190,12 +189,10 @@ private:
param_t pitch_rate_max; param_t pitch_rate_max;
param_t yaw_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_roll_max;
param_t acro_pitch_max; param_t acro_pitch_max;
param_t acro_yaw_max; param_t acro_yaw_max;
param_t rattitude_thres;
} _params_handles; /**< handles for interesting parameters */ } _params_handles; /**< handles for interesting parameters */
@@ -212,11 +209,8 @@ private:
float yaw_rate_max; float yaw_rate_max;
math::Vector<3> mc_rate_max; /**< attitude rate limits in stabilized modes */ 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 */ math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */
float rattitude_thres;
} _params; } _params;
/** /**
@@ -346,11 +340,9 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_params.roll_rate_max = 0.0f; _params.roll_rate_max = 0.0f;
_params.pitch_rate_max = 0.0f; _params.pitch_rate_max = 0.0f;
_params.yaw_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.mc_rate_max.zero();
_params.acro_rate_max.zero(); _params.acro_rate_max.zero();
_params.rattitude_thres = 1.0f;
_rates_prev.zero(); _rates_prev.zero();
_rates_sp.zero(); _rates_sp.zero();
@@ -380,12 +372,10 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_params_handles.roll_rate_max = param_find("MC_ROLLRATE_MAX"); _params_handles.roll_rate_max = param_find("MC_ROLLRATE_MAX");
_params_handles.pitch_rate_max = param_find("MC_PITCHRATE_MAX"); _params_handles.pitch_rate_max = param_find("MC_PITCHRATE_MAX");
_params_handles.yaw_rate_max = param_find("MC_YAWRATE_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_roll_max = param_find("MC_ACRO_R_MAX");
_params_handles.acro_pitch_max = param_find("MC_ACRO_P_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 */ /* fetch initial parameter values */
parameters_update(); parameters_update();
@@ -466,14 +456,6 @@ MulticopterAttitudeControl::parameters_update()
param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max); param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max);
_params.mc_rate_max(2) = math::radians(_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 */ /* manual rate control scale and auto mode roll/pitch rate limits */
param_get(_params_handles.acro_roll_max, &v); param_get(_params_handles.acro_roll_max, &v);
_params.acro_rate_max(0) = math::radians(v); _params.acro_rate_max(0) = math::radians(v);
@@ -482,6 +464,9 @@ MulticopterAttitudeControl::parameters_update()
param_get(_params_handles.acro_yaw_max, &v); param_get(_params_handles.acro_yaw_max, &v);
_params.acro_rate_max(2) = math::radians(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); _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
return OK; return OK;
@@ -809,7 +794,18 @@ MulticopterAttitudeControl::task_main()
vehicle_status_poll(); vehicle_status_poll();
vehicle_motor_limits_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) { if (_v_control_mode.flag_control_attitude_enabled) {
control_attitude(dt); control_attitude(dt);
/* publish attitude rates setpoint */ /* publish attitude rates setpoint */
@@ -825,7 +821,7 @@ MulticopterAttitudeControl::task_main()
} else if (_rates_sp_id) { } else if (_rates_sp_id) {
_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
} }
//}
} else { } else {
/* attitude controller disabled, poll rates setpoint topic */ /* attitude controller disabled, poll rates setpoint topic */
if (_v_control_mode.flag_control_manual_enabled) { if (_v_control_mode.flag_control_manual_enabled) {
src/modules/mc_att_control/mc_att_control_params.c
+16 -6
View File
@@ -35,13 +35,10 @@
* @file mc_att_control_params.c * @file mc_att_control_params.c
* Parameters for multicopter attitude controller. * Parameters for multicopter attitude controller.
* *
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lorenz@px4.io> * @author Lorenz Meier <lorenz@px4.io>
* @author Anton Babushkin <anton@px4.io> * @author Anton Babushkin <anton@px4.io>
*/ */
#include <systemlib/param/param.h>
/** /**
* Roll P gain * Roll P gain
* *
@@ -251,7 +248,7 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 60.0f);
* @max 360.0 * @max 360.0
* @group Multicopter Attitude Control * @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 * Max acro pitch rate
@@ -261,7 +258,7 @@ PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 90.0f);
* @max 360.0 * @max 360.0
* @group Multicopter Attitude Control * @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 * Max acro yaw rate
@@ -270,4 +267,17 @@ PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 90.0f);
* @min 0.0 * @min 0.0
* @group Multicopter Attitude Control * @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);

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