diff --git a/.gitmodules b/.gitmodules index 3e6e75cb0e..cb9a6ccf05 100644 --- a/.gitmodules +++ b/.gitmodules @@ -19,3 +19,6 @@ [submodule "src/lib/eigen"] path = src/lib/eigen url = https://github.com/PX4/eigen.git +[submodule "src/lib/dspal"] + path = src/lib/dspal + url = https://github.com/mcharleb/dspal.git diff --git a/.travis.yml b/.travis.yml index ecc69a0c16..20b15d323d 100644 --- a/.travis.yml +++ b/.travis.yml @@ -56,6 +56,9 @@ before_script: - ln -s /usr/bin/ccache ~/bin/g++-4.8 - ln -s /usr/bin/ccache ~/bin/gcc-4.8 - export PATH=~/bin:$PATH +# grab astyle 2.05.1 + - wget -O ~/bin/astyle https://github.com/PX4/astyle/releases/download/2.05.1/astyle-linux && chmod +x ~/bin/astyle + - astyle --version env: global: @@ -66,6 +69,7 @@ env: - PX4_AWS_BUCKET=px4-travis script: + - make check_format - ccache -z - arm-none-eabi-gcc --version - echo 'Building POSIX Firmware..' && echo -en 'travis_fold:start:script.1\\r' diff --git a/Makefile b/Makefile index bf399d19bf..3c47aad3e0 100644 --- a/Makefile +++ b/Makefile @@ -222,9 +222,9 @@ qurtrun: tests: generateuorbtopicheaders $(Q) (mkdir -p $(PX4_BASE)/unittests/build && cd $(PX4_BASE)/unittests/build && cmake .. && $(MAKE) --no-print-directory unittests) -.PHONY: format check_format +.PHONY: check_format check_format: - $(Q) (./Tools/check_code_style.sh | sort -n) + $(Q) (./Tools/check_code_style.sh) # # Cleanup targets. 'clean' should remove all built products and force diff --git a/NuttX b/NuttX index 8891d035df..3009fb230e 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 8891d035df45c8be570cfbd9419b438679faf7ee +Subproject commit 3009fb230ec8232c4c4fd7798fd70ea9632692ec diff --git a/README.md b/README.md index 1abab0df13..7cb07907ff 100644 --- a/README.md +++ b/README.md @@ -16,7 +16,7 @@ This repository contains the PX4 Flight Core, with the main applications located * [Downloads](http://px4.io/firmware/downloads) * Releases * [Downloads](https://github.com/PX4/Firmware/releases) -* Mailing list: [Google Groups](http://groups.google.com/group/px4users) +* Forum / Mailing list: [Google Groups](http://groups.google.com/group/px4users) ### Users ### @@ -29,7 +29,7 @@ Contributing guide: * [PX4 Contribution Guide](http://px4.io/dev/contributing) Software in the Loop guide: -Use software in the loop [to get started with the codebase](https://github.com/PX4/Firmware/tree/master/posix-configs/SITL) +Use software in the loop [to get started with the codebase](https://pixhawk.org/dev/simulation/native_sitl). Developer guide: http://px4.io/dev/ @@ -38,8 +38,10 @@ Testing guide: http://px4.io/dev/unit_tests This repository contains code supporting these boards: + * [Snapdragon Flight](https://www.intrinsyc.com/qualcomm-snapdragon-flight/) * FMUv1.x - * FMUv2.x + * FMUv2.x (Pixhawk) + * FMUv3.x (Pixhawk 2) * AeroCore (v1 and v2) * STM32F4Discovery (basic support) [Tutorial](https://pixhawk.org/modules/stm32f4discovery) diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil index 4100cdb4a3..7938c47bae 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -39,8 +39,5 @@ then param set FW_RR_P 0.3 fi -# Enable gamepad / joystick support -param set COM_RC_IN_MODE 2 - set HIL yes set MIXER AERT diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil index 52c512ed8c..e1f2cdfe8d 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil @@ -11,7 +11,4 @@ sh /etc/init.d/rc.mc_defaults set MIXER quad_x -# Enable gamepad / joystick support -param set COM_RC_IN_MODE 2 - set HIL yes diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil index c590f9ac86..7aa888c763 100644 --- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -11,7 +11,4 @@ sh /etc/init.d/rc.mc_defaults set MIXER quad_+ -# Enable gamepad / joystick support -param set COM_RC_IN_MODE 2 - set HIL yes diff --git a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil index 0369da5980..57bcd24d02 100644 --- a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil +++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil @@ -11,7 +11,4 @@ sh /etc/init.d/rc.fw_defaults set HIL yes -# Enable gamepad / joystick support -param set COM_RC_IN_MODE 2 - set MIXER AERT diff --git a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil index 278df193aa..5e0b6cd746 100644 --- a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil +++ b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil @@ -36,8 +36,5 @@ fi set HIL yes -# Enable gamepad / joystick support -param set COM_RC_IN_MODE 2 - # Set the AERT mixer for HIL (even if the malolo is a flying wing) set MIXER AERT diff --git a/ROMFS/px4fmu_common/init.d/2101_fw_AERT b/ROMFS/px4fmu_common/init.d/2101_fw_AERT index 19a47cda8c..3631a1e36c 100644 --- a/ROMFS/px4fmu_common/init.d/2101_fw_AERT +++ b/ROMFS/px4fmu_common/init.d/2101_fw_AERT @@ -20,3 +20,7 @@ sh /etc/init.d/rc.fw_defaults set MIXER AERT + +# The ESC requires a specific pulse to arm. +set PWM_OUT 4 +set PWM_DISARMED p:PWM_DISARMED diff --git a/ROMFS/px4fmu_common/init.d/2104_fw_AETR b/ROMFS/px4fmu_common/init.d/2104_fw_AETR index 083eb42330..3113ede87a 100644 --- a/ROMFS/px4fmu_common/init.d/2104_fw_AETR +++ b/ROMFS/px4fmu_common/init.d/2104_fw_AETR @@ -20,3 +20,7 @@ sh /etc/init.d/rc.fw_defaults set MIXER AETR + +# The ESC requires a specific pulse to arm. +set PWM_OUT 3 +set PWM_DISARMED p:PWM_DISARMED diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index ff1a8092be..517c77b872 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -110,8 +110,6 @@ else fi fi -# -# Start sensors -# +# Wait 20 ms for sensors (because we need to wait for the HRT and work queue callbacks to fire) +usleep 20000 sensors start - diff --git a/ROMFS/px4fmu_common/init.d/rc.uavcan b/ROMFS/px4fmu_common/init.d/rc.uavcan index 37e6893bd1..ebecea2de4 100644 --- a/ROMFS/px4fmu_common/init.d/rc.uavcan +++ b/ROMFS/px4fmu_common/init.d/rc.uavcan @@ -3,26 +3,10 @@ # UAVCAN initialization script. # -# -# Mirriring the UAVCAN_ENABLE param value to an eponymous environment variable. -# TODO there should be a smarter way. -# -set UAVCAN_ENABLE 0 -if param compare UAVCAN_ENABLE 1 -then - set UAVCAN_ENABLE 1 -fi -if param compare UAVCAN_ENABLE 2 -then - set UAVCAN_ENABLE 2 -fi - -echo "[i] UAVCAN_ENABLE is $UAVCAN_ENABLE" - # # Starting stuff according to UAVCAN_ENABLE value # -if [ $UAVCAN_ENABLE -ge 1 ] +if param greater UAVCAN_ENABLE 0 then if uavcan start then @@ -33,7 +17,7 @@ then fi fi -if [ $UAVCAN_ENABLE -ge 2 ] +if param greater UAVCAN_ENABLE 1 then if uavcan start fw then @@ -43,10 +27,3 @@ then tone_alarm $TUNE_ERR fi fi - -if [ $UAVCAN_ENABLE -ge 1 ] -then - # First sensor publisher to initialize takes lowest instance ID - # This delay ensures that UAVCAN-interfaced sensors will be allocated on lowest instance IDs - sleep 8 -fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 85cbdeb59e..f14dd3015d 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -365,11 +365,6 @@ then then fi - # - # UAVCAN - # - sh /etc/init.d/rc.uavcan - # # Sensors System (start before Commander so Preflight checks are properly run) # @@ -389,8 +384,6 @@ then unset GPS_FAKE # Needs to be this early for in-air-restarts - # Wait 10 ms for sensors (workaround for airspeed, to be removed) - usleep 10000 commander start # @@ -578,6 +571,11 @@ then fi fi + # + # UAVCAN + # + sh /etc/init.d/rc.uavcan + # # Logging # diff --git a/Tools/check_code_style.sh b/Tools/check_code_style.sh index b8806f3164..db140fca45 100755 --- a/Tools/check_code_style.sh +++ b/Tools/check_code_style.sh @@ -1,16 +1,45 @@ #!/usr/bin/env bash set -eu failed=0 -for fn in $(find . -path './src/lib/uavcan' -prune -o \ - -path './src/lib/mathlib/CMSIS' -prune -o \ - -path './src/lib/eigen' -prune -o \ - -path './src/modules/attitude_estimator_ekf/codegen' -prune -o \ - -path './NuttX' -prune -o \ +for fn in $(find src/examples \ + src/systemcmds \ + src/include \ + src/drivers/blinkm \ + src/drivers/bma180 \ + src/drivers/pca9685 \ + src/drivers/pca8574 \ + src/drivers/md25 \ + src/drivers/ms5611 \ + src/drivers/stm32 \ + src/drivers/px4io \ + src/drivers/px4fmu \ + src/lib/launchdetection \ + src/modules/bottle_drop \ + src/modules/dataman \ + src/modules/fixedwing_backside \ + src/modules/segway \ + src/modules/unit_test \ + src/modules/systemlib \ -path './Build' -prune -o \ -path './mavlink' -prune -o \ - -path './unittests/gtest' -prune -o \ + -path './NuttX' -prune -o \ + -path './src/lib/eigen' -prune -o \ + -path './src/lib/mathlib/CMSIS' -prune -o \ + -path './src/lib/uavcan' -prune -o \ + -path './src/modules/attitude_estimator_ekf/codegen' -prune -o \ + -path './src/modules/ekf_att_pos_estimator' -prune -o \ + -path './src/modules/sdlog2' -prune -o \ + -path './src/modules/uORB' -prune -o \ + -path './src/modules/vtol_att_control' -prune -o \ -path './unittests/build' -prune -o \ - -name '*.c' -o -name '*.cpp' -o -name '*.hpp' -o -name '*.h' -type f); do + -path './unittests/gtest' -prune -o \ + -name '*.c' -o -name '*.cpp' -o -name '*.hpp' -o -name '*.h' \ + -not -name '*generated*' \ + -not -name '*uthash.h' \ + -not -name '*utstring.h' \ + -not -name '*utlist.h' \ + -not -name '*utarray.h' \ + -type f); do if [ -f "$fn" ]; then ./Tools/fix_code_style.sh --quiet < $fn > $fn.pretty @@ -18,7 +47,7 @@ for fn in $(find . -path './src/lib/uavcan' -prune -o \ rm -f $fn.pretty if [ $diffsize -ne 0 ]; then failed=1 - echo $diffsize $fn + echo $fn 'bad formatting, please run "./Tools/fix_code_style.sh' $fn'"' fi fi done @@ -27,6 +56,6 @@ if [ $failed -eq 0 ]; then echo "Format checks passed" exit 0 else - echo "Format checks failed; please run ./Tools/fix_code_style.sh on each file" + echo "Format checks failed" exit 1 fi diff --git a/Tools/fix_code_style.sh b/Tools/fix_code_style.sh index f1818f2f54..d8b424ae56 100755 --- a/Tools/fix_code_style.sh +++ b/Tools/fix_code_style.sh @@ -1,4 +1,14 @@ -#!/bin/sh +#!/bin/bash + +ASTYLE_VER=`astyle --version` +ASTYLE_VER_REQUIRED="Artistic Style Version 2.05.1" + +if [ "$ASTYLE_VER" != "$ASTYLE_VER_REQUIRED" ]; then + echo "Error: you're using ${ASTYLE_VER}, but PX4 requires ${ASTYLE_VER_REQUIRED}" + echo "You can get the correct version here: https://github.com/PX4/astyle/releases/tag/2.05.1" + exit 1 +fi + DIR=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd ) astyle \ --options=$DIR/astylerc \ diff --git a/Tools/px4params/srcparser.py b/Tools/px4params/srcparser.py index c69699cad7..2201fac6dd 100644 --- a/Tools/px4params/srcparser.py +++ b/Tools/px4params/srcparser.py @@ -200,7 +200,7 @@ class SourceParser(object): m = self.re_px4_parameter_definition.match(line) if m: tp, name = m.group(1, 2) - if default_var.has_key(name+'_DEFAULT'): + if (name+'_DEFAULT') in default_var: defval = default_var[name+'_DEFAULT'] if tp is not None: # Remove trailing type specifier from numbers: 0.1f => 0.1 diff --git a/Tools/px_romfs_pruner.py b/Tools/px_romfs_pruner.py index aef1cc7a3b..78e4e69f72 100644 --- a/Tools/px_romfs_pruner.py +++ b/Tools/px_romfs_pruner.py @@ -76,7 +76,7 @@ def main(): pruned_content += line # overwrite old scratch file - with open(file_path, "w") as f: + with open(file_path, "wb") as f: f.write(pruned_content) diff --git a/Vagrantfile b/Vagrantfile index 25ba1ce35c..086884adb7 100644 --- a/Vagrantfile +++ b/Vagrantfile @@ -73,12 +73,12 @@ Vagrant.configure(2) do |config| # Enable provisioning with a shell script. Additional provisioners such as # Puppet, Chef, Ansible, Salt, and Docker are also available. Please see the # documentation for more information about their specific syntax and use. - config.vm.provision "shell", inline: <<-SHELL + config.vm.provision "shell", privileged: false, inline: <<-SHELL # Ensure we start in the Firmware folder echo "cd /Firmware" >> ~/.bashrc # Install software sudo apt-get update - sudo apt-get install -y build-essential ccache cmake clang-3.5 lldb-3.5 g++-4.8 gcc-4.8 genromfs libc6-i386 libncurses5-dev python-argparse python-empy python-serial s3cmd texinfo zlib1g-dev git-core + sudo apt-get install -y build-essential ccache cmake clang-3.5 lldb-3.5 g++-4.8 gcc-4.8 genromfs libc6-i386 libncurses5-dev python-argparse python-empy python-serial s3cmd texinfo zlib1g-dev git-core zip pushd . cd ~ wget -q https://launchpadlibrarian.net/186124160/gcc-arm-none-eabi-4_8-2014q3-20140805-linux.tar.bz2 diff --git a/launch/ardrone.launch b/launch/ardrone.launch index f43bbf4709..db980731fe 100644 --- a/launch/ardrone.launch +++ b/launch/ardrone.launch @@ -1,8 +1,10 @@ + + diff --git a/launch/iris.launch b/launch/iris.launch index 5231e3215b..798b761d76 100644 --- a/launch/iris.launch +++ b/launch/iris.launch @@ -1,8 +1,10 @@ + + diff --git a/launch/multi_uav.launch b/launch/multi_uav.launch new file mode 100644 index 0000000000..6fb1589e80 --- /dev/null +++ b/launch/multi_uav.launch @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/multicopter.launch b/launch/multicopter.launch index 6882f24137..bf68da95dd 100644 --- a/launch/multicopter.launch +++ b/launch/multicopter.launch @@ -1,5 +1,6 @@ + @@ -10,7 +11,9 @@ - + + + diff --git a/launch/multicopter_w.launch b/launch/multicopter_w.launch index 66c30d186f..617357452f 100644 --- a/launch/multicopter_w.launch +++ b/launch/multicopter_w.launch @@ -1,8 +1,10 @@ + + diff --git a/launch/multicopter_x.launch b/launch/multicopter_x.launch index c686eba390..b7db46dcb5 100644 --- a/launch/multicopter_x.launch +++ b/launch/multicopter_x.launch @@ -1,8 +1,10 @@ + + diff --git a/makefiles/nuttx/config_px4fmu-v2_default.mk b/makefiles/nuttx/config_px4fmu-v2_default.mk index 142da2d581..8d16e538a0 100644 --- a/makefiles/nuttx/config_px4fmu-v2_default.mk +++ b/makefiles/nuttx/config_px4fmu-v2_default.mk @@ -79,7 +79,7 @@ MODULES += modules/land_detector # # Estimation modules (EKF/ SO3 / other filters) # -# Too high RAM usage due to static allocations +# Removed from build due to large static allocations #MODULES += modules/attitude_estimator_ekf MODULES += modules/attitude_estimator_q MODULES += modules/ekf_att_pos_estimator diff --git a/makefiles/posix-arm/config_eagle_adsp.mk b/makefiles/posix-arm/config_eagle_adsp.mk new file mode 100644 index 0000000000..e7e0d8cf6f --- /dev/null +++ b/makefiles/posix-arm/config_eagle_adsp.mk @@ -0,0 +1,57 @@ +# +# Makefile for the EAGLE *default* configuration +# + +# +# Board support modules +# +MODULES += drivers/device + +# +# System commands +# +MODULES += systemcmds/param +MODULES += systemcmds/ver + +# +# General system control +# +MODULES += modules/mavlink + +# +# Estimation modules (EKF/ SO3 / other filters) +# + +# +# Vehicle Control +# + +# +# Library modules +# +MODULES += modules/systemlib +MODULES += modules/uORB +MODULES += modules/dataman + +# +# Libraries +# +MODULES += lib/mathlib +MODULES += lib/mathlib/math/filter +MODULES += lib/geo +MODULES += lib/geo_lookup +MODULES += lib/conversion +# +# Linux port +# +MODULES += platforms/posix/px4_layer +MODULES += platforms/posix/work_queue + +# +# Unit tests +# + +# +# muorb fastrpc changes. +# +MODULES += modules/muorb/krait diff --git a/makefiles/posix-arm/config_eagle_hil.mk b/makefiles/posix-arm/config_eagle_hil.mk index d79213dec0..522549d05b 100644 --- a/makefiles/posix-arm/config_eagle_hil.mk +++ b/makefiles/posix-arm/config_eagle_hil.mk @@ -6,6 +6,7 @@ # Board support modules # MODULES += drivers/device +MODULES += drivers/boards/sitl #MODULES += drivers/blinkm #MODULES += drivers/pwm_out_sim #MODULES += drivers/rgbled diff --git a/makefiles/posix-arm/toolchain_gnu-arm-linux-gnueabihf.mk b/makefiles/posix-arm/toolchain_gnu-arm-linux-gnueabihf.mk index 12ea6fc29d..4eb3b0fd19 100644 --- a/makefiles/posix-arm/toolchain_gnu-arm-linux-gnueabihf.mk +++ b/makefiles/posix-arm/toolchain_gnu-arm-linux-gnueabihf.mk @@ -39,14 +39,17 @@ # CROSSDEV = arm-linux-gnueabihf- -CC = $(CROSSDEV)gcc -CXX = $(CROSSDEV)g++ -CPP = $(CROSSDEV)gcc -E -LD = $(CROSSDEV)ld -AR = $(CROSSDEV)ar rcs -NM = $(CROSSDEV)nm -OBJCOPY = $(CROSSDEV)objcopy -OBJDUMP = $(CROSSDEV)objdump +CC ?= $(CROSSDEV)gcc +CXX ?= $(CROSSDEV)g++ +CPP ?= $(CROSSDEV)gcc -E +LD ?= $(CROSSDEV)ld +AR ?= $(CROSSDEV)ar rcs +NM ?= $(CROSSDEV)nm +OBJCOPY ?= $(CROSSDEV)objcopy +OBJDUMP ?= $(CROSSDEV)objdump +ifdef OECORE_NATIVE_SYSROOT +AR := $(AR) rcs +endif # Check if the right version of the toolchain is available # @@ -57,7 +60,9 @@ ifeq (,$(findstring $(CROSSDEV_VER_FOUND), $(CROSSDEV_VER_SUPPORTED))) $(error Unsupported version of $(CC), found: $(CROSSDEV_VER_FOUND) instead of one in: $(CROSSDEV_VER_SUPPORTED)) endif -EXT_MUORB_LIB_ROOT = /opt/muorb_libs +ifndef POSIX_EXT_LIB_ROOT +$(error POSIX_EXT_LIB_ROOT is not set) +endif # XXX this is pulled pretty directly from the fmu Make.defs - needs cleanup @@ -71,37 +76,6 @@ ARCHCPUFLAGS_CORTEXA8 = -mtune=cortex-a8 \ -mfloat-abi=hard \ -mfpu=neon -ARCHCPUFLAGS_CORTEXM4F = -mcpu=cortex-m4 \ - -mthumb \ - -march=armv7e-m \ - -mfpu=fpv4-sp-d16 \ - -mfloat-abi=hard - -ARCHCPUFLAGS_CORTEXM4 = -mcpu=cortex-m4 \ - -mthumb \ - -march=armv7e-m \ - -mfloat-abi=soft - -ARCHCPUFLAGS_CORTEXM3 = -mcpu=cortex-m3 \ - -mthumb \ - -march=armv7-m \ - -mfloat-abi=soft - -# Enabling stack checks if OS was build with them -# -TEST_FILE_STACKCHECK=$(WORK_DIR)nuttx-export/include/nuttx/config.h -TEST_VALUE_STACKCHECK=CONFIG_ARMV7M_STACKCHECK\ 1 -ENABLE_STACK_CHECKS=$(shell $(GREP) -q "$(TEST_VALUE_STACKCHECK)" $(TEST_FILE_STACKCHECK); echo $$?;) -ifeq ("$(ENABLE_STACK_CHECKS)","0") -ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -finstrument-functions -ffixed-r10 -ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -finstrument-functions -ffixed-r10 -ARCHINSTRUMENTATIONDEFINES_CORTEXM3 = -else -ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -ARCHINSTRUMENTATIONDEFINES_CORTEXM3 = -endif - # Pick the right set of flags for the architecture. # ARCHCPUFLAGS = $(ARCHCPUFLAGS_$(CONFIG_ARCH)) @@ -115,13 +89,13 @@ ifeq ($(CONFIG_BOARD),) $(error Board config does not define CONFIG_BOARD) endif ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) \ - -D__PX4_LINUX -D__PX4_POSIX \ - -Dnoreturn_function= \ - -I$(PX4_BASE)/src/modules/systemlib \ - -I$(PX4_BASE)/src/lib/eigen \ - -I$(PX4_BASE)/src/platforms/posix/include \ - -I$(PX4_BASE)/mavlink/include/mavlink \ - -Wno-error=shadow + -D__PX4_LINUX -D__PX4_POSIX \ + -Dnoreturn_function= \ + -I$(PX4_BASE)/src/modules/systemlib \ + -I$(PX4_BASE)/src/lib/eigen \ + -I$(PX4_BASE)/src/platforms/posix/include \ + -I$(PX4_BASE)/mavlink/include/mavlink \ + -Wno-error=shadow # optimisation flags # @@ -189,7 +163,8 @@ LIBM := $(shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a) EXTRA_LIBS += -lpx4muorb -ladsprpc EXTRA_LIBS += -pthread -lm -lrt -LIB_DIRS += $(EXT_MUORB_LIB_ROOT)/krait/libs +LIB_DIRS += $(POSIX_EXT_LIB_ROOT)/libs +INCLUDE_DIRS += $(POSIX_EXT_LIB_ROOT)/inc # Flags we pass to the C compiler # diff --git a/makefiles/posix/config_posix_sitl.mk b/makefiles/posix/config_posix_sitl.mk index 57ede0b78b..b745d0e263 100644 --- a/makefiles/posix/config_posix_sitl.mk +++ b/makefiles/posix/config_posix_sitl.mk @@ -5,6 +5,7 @@ # # Board support modules # +MODULES += drivers/boards/sitl MODULES += drivers/device MODULES += drivers/blinkm MODULES += drivers/pwm_out_sim @@ -96,7 +97,7 @@ MODULES += platforms/posix/drivers/gpssim # Unit tests # #MODULES += platforms/posix/tests/hello -#MODULES += platforms/posix/tests/vcdev_test +MODULES += platforms/posix/tests/vcdev_test #MODULES += platforms/posix/tests/hrt_test #MODULES += platforms/posix/tests/wqueue diff --git a/makefiles/qurt/config_qurt_adsp.mk b/makefiles/qurt/config_qurt_adsp.mk index 6a1ce1d5f2..fcd63eef57 100644 --- a/makefiles/qurt/config_qurt_adsp.mk +++ b/makefiles/qurt/config_qurt_adsp.mk @@ -1,8 +1,19 @@ #Added configuration specific flags here. +ifndef HEXAGON_DRIVERS_ROOT +$(error HEXAGON_DRIVERS_ROOT is not set) +endif +ifndef EAGLE_DRIVERS_SRC +$(error EAGLE_DRIVERS_SRC is not set) +endif + +INCLUDE_DIRS += $(HEXAGON_DRIVERS_ROOT)/inc + # For Actual flight we need to link against the driver dynamic libraries -LDFLAGS += -L${DSPAL_ROOT}/mpu_spi/hexagon_Debug_dynamic_toolv64/ship -lmpu9x50 -LDFLAGS += -L${DSPAL_ROOT}/uart_esc/hexagon_Debug_dynamic_toolv64/ship -luart_esc +LDFLAGS += -L${HEXAGON_DRIVERS_ROOT}/libs -lmpu9x50 +LDFLAGS += -luart_esc +LDFLAGS += -lcsr_gps +LDFLAGS += -lrc_receiver # # Makefile for the EAGLE QuRT *default* configuration @@ -13,8 +24,10 @@ LDFLAGS += -L${DSPAL_ROOT}/uart_esc/hexagon_Debug_dynamic_toolv64/ship -luart_es # MODULES += drivers/device MODULES += modules/sensors -#MODULES += platforms/qurt/drivers/mpu9x50 -#MODULES += platforms/qurt/drivers/uart_esc +MODULES += $(EAGLE_DRIVERS_SRC)/mpu9x50 +MODULES += $(EAGLE_DRIVERS_SRC)/uart_esc +MODULES += $(EAGLE_DRIVERS_SRC)/rc_receiver +MODULES += $(EAGLE_DRIVERS_SRC)/csr_gps # # System commands @@ -47,6 +60,7 @@ MODULES += modules/systemlib/mixer MODULES += modules/uORB #MODULES += modules/dataman MODULES += modules/commander +MODULES += modules/controllib # # Libraries @@ -61,6 +75,7 @@ MODULES += lib/conversion # QuRT port # MODULES += platforms/qurt/px4_layer +MODULES += platforms/posix/work_queue # # Unit tests diff --git a/makefiles/qurt/config_qurt_hil.mk b/makefiles/qurt/config_qurt_hil.mk index 0536d7a4ad..6f14dd6a00 100644 --- a/makefiles/qurt/config_qurt_hil.mk +++ b/makefiles/qurt/config_qurt_hil.mk @@ -6,6 +6,7 @@ # Board support modules # MODULES += drivers/device +MODULES += drivers/boards/sitl #MODULES += drivers/blinkm MODULES += drivers/pwm_out_sim MODULES += drivers/led diff --git a/makefiles/qurt/firmware_qurt.mk b/makefiles/qurt/firmware_qurt.mk index 50c4b267b4..ac87683487 100644 --- a/makefiles/qurt/firmware_qurt.mk +++ b/makefiles/qurt/firmware_qurt.mk @@ -53,8 +53,7 @@ $(FIRMWARES): $(BUILD_DIR)%.build/firmware.a: generateuorbtopicheaders WORK_DIR=$(work_dir) \ $(FIRMWARE_GOAL) -HEXAGON_TOOLS_ROOT = /opt/6.4.05 -#V_ARCH = v4 +HEXAGON_TOOLS_ROOT ?= /opt/6.4.03 V_ARCH = v5 HEXAGON_CLANG_BIN = $(addsuffix /qc/bin,$(HEXAGON_TOOLS_ROOT)) SIM = $(HEXAGON_CLANG_BIN)/hexagon-sim diff --git a/makefiles/qurt/toolchain_hexagon.mk b/makefiles/qurt/toolchain_hexagon.mk index c54a74f203..77a9d4e838 100644 --- a/makefiles/qurt/toolchain_hexagon.mk +++ b/makefiles/qurt/toolchain_hexagon.mk @@ -35,14 +35,6 @@ #$(info TOOLCHAIN gnu-arm-eabi) -# -# Stop making if ADSP_LIB_ROOT is not set. This defines the path to -# DspAL headers and driver headers -# -ifndef DSPAL_ROOT -$(error DSPAL_ROOT is not set) -endif - # Toolchain commands. Normally only used inside this file. # HEXAGON_TOOLS_ROOT ?= /opt/6.4.03 @@ -57,7 +49,7 @@ HEXAGON_ISS_DIR = $(HEXAGON_TOOLS_ROOT)/qc/lib/iss TOOLSLIB = $(HEXAGON_TOOLS_ROOT)/dinkumware/lib/$(V_ARCH)/G0 QCTOOLSLIB = $(HEXAGON_TOOLS_ROOT)/qc/lib/$(V_ARCH)/G0 QURTLIB = $(HEXAGON_SDK_ROOT)/lib/common/qurt/ADSP$(V_ARCH)MP/lib -#DSPAL = $(PX4_BASE)/../dspal_libs/libdspal.a +DSPAL_INCS ?= $(PX4_BASE)/src/lib/dspal CC = $(HEXAGON_CLANG_BIN)/$(CROSSDEV)clang @@ -93,8 +85,7 @@ DYNAMIC_LIBS = \ # Check if the right version of the toolchain is available # -CROSSDEV_VER_SUPPORTED = 6.4.03 -#CROSSDEV_VER_SUPPORTED = 6.4.05 +CROSSDEV_VER_SUPPORTED = 6.4.03 6.4.05 CROSSDEV_VER_FOUND = $(shell $(CC) --version | sed -n 's/^.*version \([\. 0-9]*\),.*$$/\1/p') ifeq (,$(findstring $(CROSSDEV_VER_FOUND), $(CROSSDEV_VER_SUPPORTED))) @@ -122,18 +113,15 @@ ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) \ -Dnoreturn_function= \ -D__EXPORT= \ -Drestrict= \ - -D_DEBUG \ - -I$(DSPAL_ROOT)/ \ - -I$(DSPAL_ROOT)/dspal/include \ - -I$(DSPAL_ROOT)/dspal/sys \ - -I$(DSPAL_ROOT)/dspal/sys/sys \ - -I$(DSPAL_ROOT)/mpu_spi/inc/ \ - -I$(DSPAL_ROOT)/uart_esc/inc/ \ + -D_DEBUG \ + -I$(DSPAL_INCS)/include \ + -I$(DSPAL_INCS)/sys \ -I$(HEXAGON_TOOLS_ROOT)/gnu/hexagon/include \ -I$(PX4_BASE)/src/lib/eigen \ -I$(PX4_BASE)/src/platforms/qurt/include \ -I$(PX4_BASE)/src/platforms/posix/include \ -I$(PX4_BASE)/mavlink/include/mavlink \ + -I$(PX4_BASE)/../inc \ -I$(QURTLIB)/..//include \ -I$(HEXAGON_SDK_ROOT)/inc \ -I$(HEXAGON_SDK_ROOT)/inc/stddef \ diff --git a/msg/input_rc.msg b/msg/input_rc.msg index c07d229503..8e15a9a186 100644 --- a/msg/input_rc.msg +++ b/msg/input_rc.msg @@ -5,6 +5,7 @@ uint8 RC_INPUT_SOURCE_PX4IO_SPEKTRUM = 3 uint8 RC_INPUT_SOURCE_PX4IO_SBUS = 4 uint8 RC_INPUT_SOURCE_PX4IO_ST24 = 5 uint8 RC_INPUT_SOURCE_MAVLINK = 6 +uint8 RC_INPUT_SOURCE_QURT = 7 uint8 RC_INPUT_MAX_CHANNELS = 18 # Maximum number of R/C input channels in the system. S.Bus has up to 18 channels. diff --git a/msg/sensor_accel.msg b/msg/sensor_accel.msg index fdae381aba..2eca557465 100644 --- a/msg/sensor_accel.msg +++ b/msg/sensor_accel.msg @@ -1,8 +1,12 @@ uint64 timestamp +uint64 integral_dt # integration time uint64 error_count float32 x # acceleration in the NED X board axis in m/s^2 float32 y # acceleration in the NED Y board axis in m/s^2 float32 z # acceleration in the NED Z board axis in m/s^2 +float32 x_integral # velocity in the NED X board axis in m/s over the integration time frame +float32 y_integral # velocity in the NED Y board axis in m/s over the integration time frame +float32 z_integral # velocity in the NED Z board axis in m/s over the integration time frame float32 temperature # temperature in degrees celsius float32 range_m_s2 # range in m/s^2 (+- this value) float32 scaling diff --git a/msg/sensor_combined.msg b/msg/sensor_combined.msg index 876ccd7c4a..ec58e6f923 100644 --- a/msg/sensor_combined.msg +++ b/msg/sensor_combined.msg @@ -26,96 +26,49 @@ uint32 SENSOR_PRIO_MAX = 255 # NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only uint64 timestamp # Timestamp in microseconds since boot, from gyro -# -int16[3] gyro_raw # Raw sensor values of angular velocity -float32[3] gyro_rad_s # Angular velocity in radian per seconds -uint32 gyro_priority # Sensor priority -uint32 gyro_errcount # Error counter for gyro 0 -float32 gyro_temp # Temperature of gyro 0 +uint64[3] gyro_timestamp # Gyro timestamps +int16[9] gyro_raw # Raw sensor values of angular velocity +float32[9] gyro_rad_s # Angular velocity in radian per seconds +uint32[3] gyro_priority # Sensor priority +float32[9] gyro_integral_rad # delta angle in radians +uint64[3] gyro_integral_dt # delta time for gyro integral in us +uint32[3] gyro_errcount # Error counter for gyro 0 +float32[3] gyro_temp # Temperature of gyro 0 -int16[3] accelerometer_raw # Raw acceleration in NED body frame -float32[3] accelerometer_m_s2 # Acceleration in NED body frame, in m/s^2 -int16 accelerometer_mode # Accelerometer measurement mode -float32 accelerometer_range_m_s2 # Accelerometer measurement range in m/s^2 -uint64 accelerometer_timestamp # Accelerometer timestamp -uint32 accelerometer_priority # Sensor priority -uint32 accelerometer_errcount # Error counter for accel 0 -float32 accelerometer_temp # Temperature of accel 0 +int16[9] accelerometer_raw # Raw acceleration in NED body frame +float32[9] accelerometer_m_s2 # Acceleration in NED body frame, in m/s^2 +float32[9] accelerometer_integral_m_s # velocity in NED body frame, in m/s^2 +uint64[3] accelerometer_integral_dt # delta time for accel integral in us +int16[3] accelerometer_mode # Accelerometer measurement mode +float32[3] accelerometer_range_m_s2 # Accelerometer measurement range in m/s^2 +uint64[3] accelerometer_timestamp # Accelerometer timestamp +uint32[3] accelerometer_priority # Sensor priority +uint32[3] accelerometer_errcount # Error counter for accel 0 +float32[3] accelerometer_temp # Temperature of accel 0 -int16[3] magnetometer_raw # Raw magnetic field in NED body frame -float32[3] magnetometer_ga # Magnetic field in NED body frame, in Gauss -int16 magnetometer_mode # Magnetometer measurement mode -float32 magnetometer_range_ga # measurement range in Gauss -float32 magnetometer_cuttoff_freq_hz # Internal analog low pass frequency of sensor -uint64 magnetometer_timestamp # Magnetometer timestamp -uint32 magnetometer_priority # Sensor priority -uint32 magnetometer_errcount # Error counter for mag 0 -float32 magnetometer_temp # Temperature of mag 0 +int16[9] magnetometer_raw # Raw magnetic field in NED body frame +float32[9] magnetometer_ga # Magnetic field in NED body frame, in Gauss +int16[3] magnetometer_mode # Magnetometer measurement mode +float32[3] magnetometer_range_ga # measurement range in Gauss +float32[3] magnetometer_cuttoff_freq_hz # Internal analog low pass frequency of sensor +uint64[3] magnetometer_timestamp # Magnetometer timestamp +uint32[3] magnetometer_priority # Sensor priority +uint32[3] magnetometer_errcount # Error counter for mag 0 +float32[3] magnetometer_temp # Temperature of mag 0 -int16[3] gyro1_raw # Raw sensor values of angular velocity -float32[3] gyro1_rad_s # Angular velocity in radian per seconds -uint64 gyro1_timestamp # Gyro timestamp -uint32 gyro1_priority # Sensor priority -uint32 gyro1_errcount # Error counter for gyro 1 -float32 gyro1_temp # Temperature of gyro 1 - -int16[3] accelerometer1_raw # Raw acceleration in NED body frame -float32[3] accelerometer1_m_s2 # Acceleration in NED body frame, in m/s^2 -uint64 accelerometer1_timestamp # Accelerometer timestamp -uint32 accelerometer1_priority # Sensor priority -uint32 accelerometer1_errcount # Error counter for accel 1 -float32 accelerometer1_temp # Temperature of accel 1 - -int16[3] magnetometer1_raw # Raw magnetic field in NED body frame -float32[3] magnetometer1_ga # Magnetic field in NED body frame, in Gauss -uint64 magnetometer1_timestamp # Magnetometer timestamp -uint32 magnetometer1_priority # Sensor priority -uint32 magnetometer1_errcount # Error counter for mag 1 -float32 magnetometer1_temp # Temperature of mag 1 - -int16[3] gyro2_raw # Raw sensor values of angular velocity -float32[3] gyro2_rad_s # Angular velocity in radian per seconds -uint64 gyro2_timestamp # Gyro timestamp -uint32 gyro2_priority # Sensor priority -uint32 gyro2_errcount # Error counter for gyro 1 -float32 gyro2_temp # Temperature of gyro 1 - -int16[3] accelerometer2_raw # Raw acceleration in NED body frame -float32[3] accelerometer2_m_s2 # Acceleration in NED body frame, in m/s^2 -uint64 accelerometer2_timestamp # Accelerometer timestamp -uint32 accelerometer2_priority # Sensor priority -uint32 accelerometer2_errcount # Error counter for accel 2 -float32 accelerometer2_temp # Temperature of accel 2 - -int16[3] magnetometer2_raw # Raw magnetic field in NED body frame -float32[3] magnetometer2_ga # Magnetic field in NED body frame, in Gauss -uint64 magnetometer2_timestamp # Magnetometer timestamp -uint32 magnetometer2_priority # Sensor priority -uint32 magnetometer2_errcount # Error counter for mag 2 -float32 magnetometer2_temp # Temperature of mag 2 - -float32 baro_pres_mbar # Barometric pressure, already temp. comp. -float32 baro_alt_meter # Altitude, already temp. comp. -float32 baro_temp_celcius # Temperature in degrees celsius -uint64 baro_timestamp # Barometer timestamp -uint32 baro_priority # Sensor priority - -float32 baro1_pres_mbar # Barometric pressure, already temp. comp. -float32 baro1_alt_meter # Altitude, already temp. comp. -float32 baro1_temp_celcius # Temperature in degrees celsius -uint64 baro1_timestamp # Barometer timestamp -uint32 baro1_priority # Sensor priority +float32[3] baro_pres_mbar # Barometric pressure, already temp. comp. +float32[3] baro_alt_meter # Altitude, already temp. comp. +float32[3] baro_temp_celcius # Temperature in degrees celsius +uint64[3] baro_timestamp # Barometer timestamp +uint32[3] baro_priority # Sensor priority +uint32[3] baro_errcount # Error count in communication float32[10] adc_voltage_v # ADC voltages of ADC Chan 10/11/12/13 or -1 uint16[10] adc_mapping # Channel indices of each of these values float32 mcu_temp_celcius # Internal temperature measurement of MCU -float32 differential_pressure_pa # Airspeed sensor differential pressure -uint64 differential_pressure_timestamp # Last measurement timestamp -float32 differential_pressure_filtered_pa # Low pass filtered airspeed sensor differential pressure reading -uint32 differential_pressure_priority # Sensor priority - -float32 differential_pressure1_pa # Airspeed sensor differential pressure -uint64 differential_pressure1_timestamp # Last measurement timestamp -float32 differential_pressure1_filtered_pa # Low pass filtered airspeed sensor differential pressure reading -uint32 differential_pressure1_priority # Sensor priority +float32[3] differential_pressure_pa # Airspeed sensor differential pressure +uint64[3] differential_pressure_timestamp # Last measurement timestamp +float32[3] differential_pressure_filtered_pa # Low pass filtered airspeed sensor differential pressure reading +uint32[3] differential_pressure_priority # Sensor priority +uint32[3] differential_pressure_errcount # Error count in communication diff --git a/msg/sensor_gyro.msg b/msg/sensor_gyro.msg index 4a252269e8..65aa4a8706 100644 --- a/msg/sensor_gyro.msg +++ b/msg/sensor_gyro.msg @@ -1,8 +1,12 @@ uint64 timestamp +uint64 integral_dt # integration time uint64 error_count float32 x # angular velocity in the NED X board axis in rad/s float32 y # angular velocity in the NED Y board axis in rad/s float32 z # angular velocity in the NED Z board axis in rad/s +float32 x_integral # delta angle in the NED X board axis in rad/s in the integration time frame +float32 y_integral # delta angle in the NED Y board axis in rad/s in the integration time frame +float32 z_integral # delta angle in the NED Z board axis in rad/s in the integration time frame float32 temperature # temperature in degrees celcius float32 range_rad_s float32 scaling diff --git a/msg/vehicle_attitude.msg b/msg/vehicle_attitude.msg index 63879149ad..0ee90cd61d 100644 --- a/msg/vehicle_attitude.msg +++ b/msg/vehicle_attitude.msg @@ -10,6 +10,9 @@ float32 yawspeed # Yaw body angular rate (rad/s, x forward/y right/z down) float32 rollacc # Roll angular accelration (rad/s^2, x forward/y right/z down) float32 pitchacc # Pitch angular acceleration (rad/s^2, x forward/y right/z down) float32 yawacc # Yaw angular acceleration (rad/s^2, x forward/y right/z down) +float32 rate_vibration # Value between 0 and 1 indicating vibration. A value of 0 means no vibration, a value of 1 indicates unbearable vibration levels. +float32 accel_vibration # Value between 0 and 1 indicating vibration. A value of 0 means no vibration, a value of 1 indicates unbearable vibration levels. +float32 mag_vibration # Value between 0 and 1 indicating vibration. A value of 0 means no vibration, a value of 1 indicates unbearable vibration levels. float32[3] rate_offsets # Offsets of the body angular rates from zero float32[9] R # Rotation matrix, body to world, (Tait-Bryan, NED) float32[4] q # Quaternion (NED) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 59abc22fb6..8f6af5d6b4 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -551,8 +551,8 @@ CONFIG_UART7_SERIAL_CONSOLE=y # # USART1 Configuration # -CONFIG_USART1_RXBUFSIZE=600 -CONFIG_USART1_TXBUFSIZE=600 +CONFIG_USART1_RXBUFSIZE=128 +CONFIG_USART1_TXBUFSIZE=32 CONFIG_USART1_BAUD=115200 CONFIG_USART1_BITS=8 CONFIG_USART1_PARITY=0 diff --git a/posix-configs/SITL/init/rc.fixed_wing b/posix-configs/SITL/init/rc.fixed_wing index afd5a38117..7cdad2f9dc 100644 --- a/posix-configs/SITL/init/rc.fixed_wing +++ b/posix-configs/SITL/init/rc.fixed_wing @@ -4,7 +4,6 @@ param load param set MAV_TYPE 1 param set SYS_AUTOSTART 3033 param set SYS_RESTART_TYPE 2 -param set COM_RC_IN_MODE 2 dataman start param set CAL_GYRO0_ID 2293760 param set CAL_ACC0_ID 1376256 @@ -22,6 +21,7 @@ param set CAL_MAG0_XOFF 0.01 param set MPC_XY_P 0.4 param set MPC_XY_VEL_P 0.2 param set MPC_XY_VEL_D 0.005 +param set COM_RC_IN_MODE 2 rgbled start tone_alarm start gyrosim start @@ -29,9 +29,11 @@ accelsim start barosim start adcsim start gpssim start +measairspeedsim start pwm_out_sim mode_pwm -commander start +sleep 1 sensors start +commander start land_detector start fixedwing navigator start ekf_att_pos_estimator start @@ -44,3 +46,4 @@ mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14556 mavlink stream -r 50 -s ATTITUDE -u 14556 mavlink stream -r 50 -s ATTITUDE_TARGET -u 14556 mavlink boot_complete +sdlog2 start -r 100 -e -t -a diff --git a/posix-configs/SITL/init/rcS b/posix-configs/SITL/init/rcS index 6a9025bc97..11b1af9a9a 100644 --- a/posix-configs/SITL/init/rcS +++ b/posix-configs/SITL/init/rcS @@ -8,7 +8,6 @@ param set MC_YAW_P 2.0 param set MC_YAWRATE_P 0.35 param set SYS_AUTOSTART 4010 param set SYS_RESTART_TYPE 2 -param set COM_RC_IN_MODE 2 dataman start param set CAL_GYRO0_ID 2293760 param set CAL_ACC0_ID 1376256 @@ -27,6 +26,7 @@ param set MPC_XY_P 0.4 param set MPC_XY_VEL_P 0.2 param set MPC_XY_VEL_D 0.005 param set SENS_BOARD_ROT 0 +param set COM_RC_IN_MODE 2 rgbled start tone_alarm start gyrosim start @@ -35,8 +35,9 @@ barosim start adcsim start gpssim start pwm_out_sim mode_pwm -commander start +sleep 1 sensors start +commander start land_detector start multicopter navigator start attitude_estimator_q start @@ -52,4 +53,6 @@ mavlink stream -r 80 -s ATTITUDE -u 14556 mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556 mavlink stream -r 20 -s RC_CHANNELS -u 14556 mavlink stream -r 250 -s HIGHRES_IMU -u 14556 +mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 mavlink boot_complete +sdlog2 start -r 100 -e -t -a diff --git a/posix-configs/SITL/init/rc_iris_ros b/posix-configs/SITL/init/rc_iris_ros index 558fa6464b..ffef510989 100644 --- a/posix-configs/SITL/init/rc_iris_ros +++ b/posix-configs/SITL/init/rc_iris_ros @@ -8,7 +8,6 @@ param set MC_YAW_P 2.0 param set MC_YAWRATE_P 0.35 param set SYS_AUTOSTART 4010 param set SYS_RESTART_TYPE 2 -param set COM_RC_IN_MODE 2 dataman start param set CAL_GYRO0_ID 2293760 param set CAL_ACC0_ID 1376256 @@ -41,6 +40,7 @@ param set MP_ROLLRATE_I 0.001 param set MP_ROLLRATE_D 0.001 param set MP_PITCH_P 4 param set MP_PITCHRATE_P 0.3 +param set COM_RC_IN_MODE 2 rgbled start tone_alarm start gyrosim start @@ -49,8 +49,9 @@ barosim start adcsim start gpssim start pwm_out_sim mode_pwm -commander start +sleep 1 sensors start +commander start land_detector start multicopter navigator start attitude_estimator_q start diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 17cd868cdc..78fce9162e 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -244,7 +244,7 @@ private: /* for now, we only support one BlinkM */ namespace { - BlinkM *g_blinkm; +BlinkM *g_blinkm; } /* list of script names, must match script ID numbers */ @@ -277,9 +277,9 @@ extern "C" __EXPORT int blinkm_main(int argc, char *argv[]); BlinkM::BlinkM(int bus, int blinkm) : I2C("blinkm", BLINKM0_DEVICE_PATH, bus, blinkm #ifdef __PX4_NUTTX - , 100000 + , 100000 #endif - ), + ), led_color_1(LED_OFF), led_color_2(LED_OFF), led_color_3(LED_OFF), @@ -325,7 +325,7 @@ BlinkM::init() } stop_script(); - set_rgb(0,0,0); + set_rgb(0, 0, 0); return OK; } @@ -333,13 +333,14 @@ BlinkM::init() int BlinkM::setMode(int mode) { - if(mode == 1) { - if(systemstate_run == false) { + if (mode == 1) { + if (systemstate_run == false) { stop_script(); - set_rgb(0,0,0); + set_rgb(0, 0, 0); systemstate_run = true; work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, 1); } + } else { systemstate_run = false; } @@ -355,8 +356,9 @@ BlinkM::probe() ret = get_firmware_version(version); - if (ret == OK) + if (ret == OK) { DEVICE_DEBUG("found BlinkM firmware version %c%c", version[1], version[0]); + } return ret; } @@ -372,6 +374,7 @@ BlinkM::ioctl(device::file_t *filp, int cmd, unsigned long arg) ret = EINVAL; break; } + ret = play_script((const char *)arg); break; @@ -380,25 +383,31 @@ BlinkM::ioctl(device::file_t *filp, int cmd, unsigned long arg) break; case BLINKM_SET_USER_SCRIPT: { - if (arg == 0) { - ret = EINVAL; + if (arg == 0) { + ret = EINVAL; + break; + } + + unsigned lines = 0; + const uint8_t *script = (const uint8_t *)arg; + + while ((lines < 50) && (script[1] != 0)) { + ret = write_script_line(lines, script[0], script[1], script[2], script[3], script[4]); + + if (ret != OK) { + break; + } + + script += 5; + } + + if (ret == OK) { + ret = set_script(lines, 0); + } + break; } - unsigned lines = 0; - const uint8_t *script = (const uint8_t *)arg; - - while ((lines < 50) && (script[1] != 0)) { - ret = write_script_line(lines, script[0], script[1], script[2], script[3], script[4]); - if (ret != OK) - break; - script += 5; - } - if (ret == OK) - ret = set_script(lines, 0); - break; - } - default: break; } @@ -421,7 +430,7 @@ void BlinkM::led() { - if(!topic_initialized) { + if (!topic_initialized) { vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(vehicle_status_sub_fd, 250); @@ -441,29 +450,36 @@ BlinkM::led() topic_initialized = true; } - if(led_thread_ready == true) { - if(!detected_cells_blinked) { - if(num_of_cells > 0) { + if (led_thread_ready == true) { + if (!detected_cells_blinked) { + if (num_of_cells > 0) { t_led_color[0] = LED_PURPLE; } - if(num_of_cells > 1) { + + if (num_of_cells > 1) { t_led_color[1] = LED_PURPLE; } - if(num_of_cells > 2) { + + if (num_of_cells > 2) { t_led_color[2] = LED_PURPLE; } - if(num_of_cells > 3) { + + if (num_of_cells > 3) { t_led_color[3] = LED_PURPLE; } - if(num_of_cells > 4) { + + if (num_of_cells > 4) { t_led_color[4] = LED_PURPLE; } - if(num_of_cells > 5) { + + if (num_of_cells > 5) { t_led_color[5] = LED_PURPLE; } + t_led_color[6] = LED_OFF; t_led_color[7] = LED_OFF; t_led_blink = LED_BLINK; + } else { t_led_color[0] = led_color_1; t_led_color[1] = led_color_2; @@ -475,13 +491,17 @@ BlinkM::led() t_led_color[7] = led_color_8; t_led_blink = led_blink; } + led_thread_ready = false; } if (led_thread_runcount & 1) { - if (t_led_blink) + if (t_led_blink) { setLEDColor(LED_OFF); + } + led_interval = LED_OFFTIME; + } else { setLEDColor(t_led_color[(led_thread_runcount / 2) % 8]); //led_interval = (led_thread_runcount & 1) : LED_ONTIME; @@ -516,10 +536,13 @@ BlinkM::led() if (new_data_vehicle_status) { orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw); no_data_vehicle_status = 0; + } else { no_data_vehicle_status++; - if(no_data_vehicle_status >= 3) + + if (no_data_vehicle_status >= 3) { no_data_vehicle_status = 3; + } } orb_check(vehicle_control_mode_sub_fd, &new_data_vehicle_control_mode); @@ -527,10 +550,13 @@ BlinkM::led() if (new_data_vehicle_control_mode) { orb_copy(ORB_ID(vehicle_control_mode), vehicle_control_mode_sub_fd, &vehicle_control_mode); no_data_vehicle_control_mode = 0; + } else { no_data_vehicle_control_mode++; - if(no_data_vehicle_control_mode >= 3) + + if (no_data_vehicle_control_mode >= 3) { no_data_vehicle_control_mode = 3; + } } orb_check(actuator_armed_sub_fd, &new_data_actuator_armed); @@ -538,10 +564,13 @@ BlinkM::led() if (new_data_actuator_armed) { orb_copy(ORB_ID(actuator_armed), actuator_armed_sub_fd, &actuator_armed); no_data_actuator_armed = 0; + } else { no_data_actuator_armed++; - if(no_data_actuator_armed >= 3) + + if (no_data_actuator_armed >= 3) { no_data_actuator_armed = 3; + } } orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position); @@ -549,10 +578,13 @@ BlinkM::led() if (new_data_vehicle_gps_position) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub_fd, &vehicle_gps_position_raw); no_data_vehicle_gps_position = 0; + } else { no_data_vehicle_gps_position++; - if(no_data_vehicle_gps_position >= 3) + + if (no_data_vehicle_gps_position >= 3) { no_data_vehicle_gps_position = 3; + } } /* update safety topic */ @@ -569,13 +601,15 @@ BlinkM::led() if (num_of_cells == 0) { /* looking for lipo cells that are connected */ printf(" checking cells\n"); - for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) { - if(vehicle_status_raw.battery_voltage < num_of_cells * MAX_CELL_VOLTAGE) break; + + for (num_of_cells = 2; num_of_cells < 7; num_of_cells++) { + if (vehicle_status_raw.battery_voltage < num_of_cells * MAX_CELL_VOLTAGE) { break; } } + printf(" cells found:%d\n", num_of_cells); } else { - if(vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) { + if (vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) { /* LED Pattern for battery critical alerting */ led_color_1 = LED_RED; led_color_2 = LED_RED; @@ -587,7 +621,7 @@ BlinkM::led() led_color_8 = LED_RED; led_blink = LED_BLINK; - } else if(vehicle_status_raw.rc_signal_lost) { + } else if (vehicle_status_raw.rc_signal_lost) { /* LED Pattern for FAILSAFE */ led_color_1 = LED_BLUE; led_color_2 = LED_BLUE; @@ -599,7 +633,7 @@ BlinkM::led() led_color_8 = LED_BLUE; led_blink = LED_BLINK; - } else if(vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW) { + } else if (vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW) { /* LED Pattern for battery low warning */ led_color_1 = LED_YELLOW; led_color_2 = LED_YELLOW; @@ -614,9 +648,9 @@ BlinkM::led() } else { /* no battery warnings here */ - if(actuator_armed.armed == false) { + if (actuator_armed.armed == false) { /* system not armed */ - if(safety.safety_off){ + if (safety.safety_off) { led_color_1 = LED_ORANGE; led_color_2 = LED_ORANGE; led_color_3 = LED_ORANGE; @@ -626,7 +660,8 @@ BlinkM::led() led_color_7 = LED_ORANGE; led_color_8 = LED_ORANGE; led_blink = LED_BLINK; - }else{ + + } else { led_color_1 = LED_CYAN; led_color_2 = LED_CYAN; led_color_3 = LED_CYAN; @@ -637,6 +672,7 @@ BlinkM::led() led_color_8 = LED_CYAN; led_blink = LED_NOBLINK; } + } else { /* armed system - initial led pattern */ led_color_1 = LED_RED; @@ -649,32 +685,41 @@ BlinkM::led() led_color_8 = LED_OFF; led_blink = LED_BLINK; - if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) { + if (new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) { /* indicate main control state */ - if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_POSCTL) + if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_POSCTL) { led_color_4 = LED_GREEN; + } + /* TODO: add other Auto modes */ - else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_AUTO_MISSION) + else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_AUTO_MISSION) { led_color_4 = LED_BLUE; - else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_ALTCTL) + + } else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_ALTCTL) { led_color_4 = LED_YELLOW; - else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_MANUAL) + + } else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_MANUAL) { led_color_4 = LED_WHITE; - else + + } else { led_color_4 = LED_OFF; + } + led_color_5 = led_color_4; } - if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) { + if (new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) { /* handling used satus */ - if(num_of_used_sats >= 7) { + if (num_of_used_sats >= 7) { led_color_1 = LED_OFF; led_color_2 = LED_OFF; led_color_3 = LED_OFF; - } else if(num_of_used_sats == 6) { + + } else if (num_of_used_sats == 6) { led_color_2 = LED_OFF; led_color_3 = LED_OFF; - } else if(num_of_used_sats == 5) { + + } else if (num_of_used_sats == 5) { led_color_3 = LED_OFF; } @@ -689,6 +734,7 @@ BlinkM::led() } } } + } else { /* LED Pattern for general Error - no vehicle_status can retrieved */ led_color_1 = LED_WHITE; @@ -716,12 +762,13 @@ BlinkM::led() vehicle_gps_position_raw.satellites_visible); */ - led_thread_runcount=0; + led_thread_runcount = 0; led_thread_ready = true; led_interval = LED_OFFTIME; - if(detected_cells_runcount < 4){ + if (detected_cells_runcount < 4) { detected_cells_runcount++; + } else { detected_cells_blinked = true; } @@ -730,47 +777,58 @@ BlinkM::led() led_thread_runcount++; } - if(systemstate_run == true) { + if (systemstate_run == true) { /* re-queue ourselves to run again later */ work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, led_interval); + } else { stop_script(); - set_rgb(0,0,0); + set_rgb(0, 0, 0); } } -void BlinkM::setLEDColor(int ledcolor) { +void BlinkM::setLEDColor(int ledcolor) +{ switch (ledcolor) { - case LED_OFF: // off - set_rgb(0,0,0); - break; - case LED_RED: // red - set_rgb(255,0,0); - break; - case LED_ORANGE: // orange - set_rgb(255,150,0); - break; - case LED_YELLOW: // yellow - set_rgb(200,200,0); - break; - case LED_PURPLE: // purple - set_rgb(255,0,255); - break; - case LED_GREEN: // green - set_rgb(0,255,0); - break; - case LED_BLUE: // blue - set_rgb(0,0,255); - break; - case LED_CYAN: // cyan - set_rgb(0,128,128); - break; - case LED_WHITE: // white - set_rgb(255,255,255); - break; - case LED_AMBER: // amber - set_rgb(255,65,0); - break; + case LED_OFF: // off + set_rgb(0, 0, 0); + break; + + case LED_RED: // red + set_rgb(255, 0, 0); + break; + + case LED_ORANGE: // orange + set_rgb(255, 150, 0); + break; + + case LED_YELLOW: // yellow + set_rgb(200, 200, 0); + break; + + case LED_PURPLE: // purple + set_rgb(255, 0, 255); + break; + + case LED_GREEN: // green + set_rgb(0, 255, 0); + break; + + case LED_BLUE: // blue + set_rgb(0, 0, 255); + break; + + case LED_CYAN: // cyan + set_rgb(0, 128, 128); + break; + + case LED_WHITE: // white + set_rgb(255, 255, 255); + break; + + case LED_AMBER: // amber + set_rgb(255, 65, 0); + break; } } @@ -855,8 +913,9 @@ BlinkM::play_script(const char *script_name) } for (unsigned i = 0; script_names[i] != nullptr; i++) - if (!strcasecmp(script_name, script_names[i])) + if (!strcasecmp(script_name, script_names[i])) { return play_script(i); + } return -1; } @@ -931,7 +990,8 @@ BlinkM::get_firmware_version(uint8_t version[2]) void blinkm_usage(); -void blinkm_usage() { +void blinkm_usage() +{ warnx("missing command: try 'start', 'systemstate', 'ledoff', 'list' or a script name {options}"); warnx("options:"); warnx("\t-b --bus i2cbus (3)"); @@ -951,6 +1011,7 @@ blinkm_main(int argc, char *argv[]) blinkm_usage(); return 1; } + for (x = 1; x < argc; x++) { if (strcmp(argv[x], "-b") == 0 || strcmp(argv[x], "--bus") == 0) { if (argc > x + 1) { @@ -1008,22 +1069,27 @@ blinkm_main(int argc, char *argv[]) if (!strcmp(argv[1], "list")) { - for (unsigned i = 0; BlinkM::script_names[i] != nullptr; i++) + for (unsigned i = 0; BlinkM::script_names[i] != nullptr; i++) { fprintf(stderr, " %s\n", BlinkM::script_names[i]); + } + fprintf(stderr, " \n"); return 0; } /* things that require access to the device */ int fd = px4_open(BLINKM0_DEVICE_PATH, 0); + if (fd < 0) { warn("can't open BlinkM device"); return 1; } g_blinkm->setMode(0); - if (px4_ioctl(fd, BLINKM_PLAY_SCRIPT_NAMED, (unsigned long)argv[1]) == OK) + + if (px4_ioctl(fd, BLINKM_PLAY_SCRIPT_NAMED, (unsigned long)argv[1]) == OK) { return 0; + } px4_close(fd); diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp index d83cfeb4d4..6557746c1a 100644 --- a/src/drivers/bma180/bma180.cpp +++ b/src/drivers/bma180/bma180.cpp @@ -262,8 +262,9 @@ BMA180::~BMA180() stop(); /* free any existing reports */ - if (_reports != nullptr) + if (_reports != nullptr) { delete _reports; + } /* delete the perf counter */ perf_free(_sample_perf); @@ -275,14 +276,16 @@ BMA180::init() int ret = ERROR; /* do SPI init (and probe) first */ - if (SPI::init() != OK) + if (SPI::init() != OK) { goto out; + } /* allocate basic report buffers */ _reports = new RingBuffer(2, sizeof(accel_report)); - if (_reports == nullptr) + if (_reports == nullptr) { goto out; + } /* perform soft reset (p48) */ write_reg(ADDR_RESET, SOFT_RESET); @@ -308,9 +311,9 @@ BMA180::init() /* disable writing to chip config */ modify_reg(ADDR_CTRL_REG0, REG0_WRITE_ENABLE, 0); - if (set_range(4)) warnx("Failed setting range"); + if (set_range(4)) { warnx("Failed setting range"); } - if (set_lowpass(75)) warnx("Failed setting lowpass"); + if (set_lowpass(75)) { warnx("Failed setting lowpass"); } if (read_reg(ADDR_CHIP_ID) == CHIP_ID) { ret = OK; @@ -342,8 +345,9 @@ BMA180::probe() /* dummy read to ensure SPI state machine is sane */ read_reg(ADDR_CHIP_ID); - if (read_reg(ADDR_CHIP_ID) == CHIP_ID) + if (read_reg(ADDR_CHIP_ID) == CHIP_ID) { return OK; + } return -EIO; } @@ -356,8 +360,9 @@ BMA180::read(struct file *filp, char *buffer, size_t buflen) int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is enabled */ if (_call_interval > 0) { @@ -383,8 +388,9 @@ BMA180::read(struct file *filp, char *buffer, size_t buflen) measure(); /* measurement will have generated a report, copy it out */ - if (_reports->get(arp)) + if (_reports->get(arp)) { ret = sizeof(*arp); + } return ret; } @@ -397,27 +403,27 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCSPOLLRATE: { switch (arg) { - /* switching to manual polling */ + /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _call_interval = 0; return OK; - /* external signalling not supported */ + /* external signalling not supported */ case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ + /* zero would be bad */ case 0: return -EINVAL; - /* set default/max polling rate */ + /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: /* With internal low pass filters enabled, 250 Hz is sufficient */ return ioctl(filp, SENSORIOCSPOLLRATE, 250); - /* adjust to a legal polling interval in Hz */ + /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ bool want_start = (_call_interval == 0); @@ -426,16 +432,18 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg) unsigned ticks = 1000000 / arg; /* check against maximum sane rate */ - if (ticks < 1000) + if (ticks < 1000) { return -EINVAL; + } /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ _call.period = _call_interval = ticks; /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } @@ -443,25 +451,29 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_call_interval == 0) + if (_call_interval == 0) { return SENSOR_POLLRATE_MANUAL; + } return 1000000 / _call_interval; case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 2) || (arg > 100)) - return -EINVAL; - - irqstate_t flags = irqsave(); - if (!_reports->resize(arg)) { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } + irqrestore(flags); - return -ENOMEM; + + return OK; } - irqrestore(flags); - - return OK; - } case SENSORIOCGQUEUEDEPTH: return _reports->size(); @@ -543,11 +555,13 @@ BMA180::set_range(unsigned max_g) { uint8_t rangebits; - if (max_g == 0) + if (max_g == 0) { max_g = 16; + } - if (max_g > 16) + if (max_g > 16) { return -ERANGE; + } if (max_g <= 2) { _current_range = 2; @@ -700,7 +714,7 @@ BMA180::measure() * measurement flow without using the external interrupt. */ report.timestamp = hrt_absolute_time(); - report.error_count = 0; + report.error_count = 0; /* * y of board is x of sensor and x of board is -y of sensor * perform only the axis assignment here. @@ -733,8 +747,9 @@ BMA180::measure() poll_notify(POLLIN); /* publish for subscribers */ - if (_accel_topic != nullptr && !(_pub_blocked)) + if (_accel_topic != nullptr && !(_pub_blocked)) { orb_publish(ORB_ID(sensor_accel), _accel_topic, &report); + } /* stop the perf counter */ perf_end(_sample_perf); @@ -768,26 +783,31 @@ start() { int fd; - if (g_dev != nullptr) + if (g_dev != nullptr) { errx(1, "already started"); + } /* create the driver */ g_dev = new BMA180(1 /* XXX magic number */, (spi_dev_e)PX4_SPIDEV_ACCEL); - if (g_dev == nullptr) + if (g_dev == nullptr) { goto fail; + } - if (OK != g_dev->init()) + if (OK != g_dev->init()) { goto fail; + } /* set the poll rate to default, starts automatic data collection */ fd = open(ACCEL_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { goto fail; + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { goto fail; + } exit(0); fail: @@ -820,14 +840,16 @@ test() ACCEL_DEVICE_PATH); /* reset to manual polling */ - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) { err(1, "reset to manual polling"); + } /* do a simple demand read */ sz = read(fd, &a_report, sizeof(a_report)); - if (sz != sizeof(a_report)) + if (sz != sizeof(a_report)) { err(1, "immediate acc read failed"); + } warnx("single read"); warnx("time: %lld", a_report.timestamp); @@ -854,14 +876,17 @@ reset() { int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "failed "); + } - if (ioctl(fd, SENSORIOCRESET, 0) < 0) + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { err(1, "driver reset failed"); + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "driver poll restart failed"); + } exit(0); } @@ -872,8 +897,9 @@ reset() void info() { - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "BMA180: driver not running"); + } printf("state @ %p\n", g_dev); g_dev->print_info(); @@ -891,26 +917,30 @@ bma180_main(int argc, char *argv[]) * Start/load the driver. */ - if (!strcmp(argv[1], "start")) + if (!strcmp(argv[1], "start")) { bma180::start(); + } /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) + if (!strcmp(argv[1], "test")) { bma180::test(); + } /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) + if (!strcmp(argv[1], "reset")) { bma180::reset(); + } /* * Print driver information. */ - if (!strcmp(argv[1], "info")) + if (!strcmp(argv[1], "info")) { bma180::info(); + } errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'"); } diff --git a/src/drivers/boards/sitl/module.mk b/src/drivers/boards/sitl/module.mk new file mode 100644 index 0000000000..c67272fc53 --- /dev/null +++ b/src/drivers/boards/sitl/module.mk @@ -0,0 +1,8 @@ +# +# Board-specific startup code for SITL +# + +SRCS = \ + sitl_led.c + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/boards/sitl/sitl_led.c b/src/drivers/boards/sitl/sitl_led.c new file mode 100644 index 0000000000..53d033234f --- /dev/null +++ b/src/drivers/boards/sitl/sitl_led.c @@ -0,0 +1,81 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file sitl_led.c + * + * sitl LED backend. + */ + +#include +#include +#include + +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +extern void led_toggle(int led); +__END_DECLS + +static bool _led_state[2] = { false , false }; + +__EXPORT void led_init() +{ + PX4_DEBUG("LED_INIT"); +} + +__EXPORT void led_on(int led) +{ + if (led == 1 || led == 0) { + PX4_DEBUG("LED%d_ON", led); + _led_state[led] = true; + } +} + +__EXPORT void led_off(int led) +{ + if (led == 1 || led == 0) { + PX4_DEBUG("LED%d_OFF", led); + _led_state[led] = false; + } +} + +__EXPORT void led_toggle(int led) +{ + if (led == 1 || led == 0) { + _led_state[led] = !_led_state[led]; + PX4_DEBUG("LED%d_TOGGLE: %s", led, _led_state[led] ? "ON" : "OFF"); + + } +} diff --git a/src/drivers/device/integrator.h b/src/drivers/device/integrator.h new file mode 100644 index 0000000000..6dc886de96 --- /dev/null +++ b/src/drivers/device/integrator.h @@ -0,0 +1,183 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file integrator.h + * + * A resettable integrator + * + * @author Lorenz Meier + */ + +#pragma once + +#include + +#include + +class Integrator { +public: + Integrator(uint64_t auto_reset_interval = 4000 /* 250 Hz */, bool coning_compensation = false); + virtual ~Integrator(); + + /** + * Put an item into the integral. + * + * @param timestamp Timestamp of the current value + * @param val Item to put + * @param integral Current integral in case the integrator did reset, else the value will not be modified + * @return true if putting the item triggered an integral reset + * and the integral should be published + */ + bool put(hrt_abstime timestamp, math::Vector<3> &val, math::Vector<3> &integral, uint64_t &integral_dt); + + /** + * Get the current integral value + * + * @return the integral since the last auto-reset + */ + math::Vector<3> get() { return _integral_auto; } + + /** + * Read from the integral + * + * @param auto_reset Reset the integral to zero on read + * @return the integral since the last read-reset + */ + math::Vector<3> read(bool auto_reset); + + /** + * Get current integral start time + */ + hrt_abstime current_integral_start() { return _last_auto; } + +private: + hrt_abstime _auto_reset_interval; /**< the interval after which the content will be published and the integrator reset */ + hrt_abstime _last_integration; /**< timestamp of the last integration step */ + hrt_abstime _last_auto; /**< last auto-announcement of integral value */ + math::Vector<3> _integral_auto; /**< the integrated value which auto-resets after _auto_reset_interval */ + math::Vector<3> _integral_read; /**< the integrated value since the last read */ + math::Vector<3> _last_val; /**< previously integrated last value */ + math::Vector<3> _last_delta; /**< last local delta */ + void (*_auto_callback)(hrt_abstime, math::Vector<3>); /**< the function callback for auto-reset */ + bool _coning_comp_on; /**< coning compensation */ + + /* we don't want this class to be copied */ + Integrator(const Integrator&); + Integrator operator=(const Integrator&); +}; + +Integrator::Integrator(hrt_abstime auto_reset_interval, bool coning_compensation) : + _auto_reset_interval(auto_reset_interval), + _last_integration(0), + _last_auto(0), + _integral_auto(0.0f, 0.0f, 0.0f), + _integral_read(0.0f, 0.0f, 0.0f), + _last_val(0.0f, 0.0f, 0.0f), + _last_delta(0.0f, 0.0f, 0.0f), + _auto_callback(nullptr), + _coning_comp_on(coning_compensation) +{ + +} + +Integrator::~Integrator() +{ + +} + +bool +Integrator::put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integral, uint64_t &integral_dt) +{ + bool auto_reset = false; + + if (_last_integration == 0) { + /* this is the first item in the integrator */ + _last_integration = timestamp; + _last_auto = timestamp; + _last_val = val; + return false; + } + + // Integrate + double dt = (double)(timestamp - _last_integration) / 1000000.0; + math::Vector<3> i = (val + _last_val) * dt * 0.5f; + + // Apply coning compensation if required + if (_coning_comp_on) { + // Coning compensation derived by Paul Riseborough and Jonathan Challinger, + // following: + // Tian et al (2010) Three-loop Integration of GPS and Strapdown INS with Coning and Sculling Compensation + // Available: http://www.sage.unsw.edu.au/snap/publications/tian_etal2010b.pdf + + i += ((_integral_auto + _last_delta * (1.0f / 6.0f)) % i) * 0.5f; + } + + _integral_auto += i; + _integral_read += i; + + _last_integration = timestamp; + _last_val = val; + _last_delta = i; + + if ((timestamp - _last_auto) > _auto_reset_interval) { + if (_auto_callback) { + /* call the callback */ + _auto_callback(timestamp, _integral_auto); + } + + integral = _integral_auto; + integral_dt = (timestamp - _last_auto); + + auto_reset = true; + _last_auto = timestamp; + _integral_auto(0) = 0.0f; + _integral_auto(1) = 0.0f; + _integral_auto(2) = 0.0f; + } + + return auto_reset; +} + +math::Vector<3> +Integrator::read(bool auto_reset) +{ + math::Vector<3> val = _integral_read; + if (auto_reset) { + _integral_read(0) = 0.0f; + _integral_read(1) = 0.0f; + _integral_read(2) = 0.0f; + } + + return val; +} diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index 86c2230526..10e136ed8e 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -195,7 +195,7 @@ int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) { sem_t sem; int count = 0; - int ret; + int ret = -1; unsigned int i; PX4_DEBUG("Called px4_poll timeout = %d", timeout); @@ -222,7 +222,7 @@ int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) if (ret >= 0) { - if (timeout >= 0) + if (timeout > 0) { // Use a work queue task work_s _hpwork; @@ -234,7 +234,7 @@ int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) // out of scope hrt_work_cancel(&_hpwork); } - else + else if (timeout < 0) { sem_wait(&sem); } @@ -270,7 +270,7 @@ int px4_fsync(int fd) int px4_access(const char *pathname, int mode) { - if (mode == F_OK) { + if (mode != F_OK) { errno = EINVAL; return -1; } diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index de22a888d3..d8650c382e 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -178,12 +178,12 @@ void frsky_send_frame1(int uart) roundf(raw.accelerometer_m_s2[2] * 1000.0f)); frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP, - raw.baro_alt_meter); + raw.baro_alt_meter[0]); frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP, - roundf(frac(raw.baro_alt_meter) * 100.0f)); + roundf(frac(raw.baro_alt_meter[0]) * 100.0f)); frsky_send_data(uart, FRSKY_ID_TEMP1, - roundf(raw.baro_temp_celcius)); + roundf(raw.baro_temp_celcius[0])); frsky_send_data(uart, FRSKY_ID_VFAS, roundf(battery.voltage_v * 10.0f)); diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp index ef841c3f85..61daa302fb 100644 --- a/src/drivers/hott/messages.cpp +++ b/src/drivers/hott/messages.cpp @@ -148,12 +148,12 @@ build_eam_response(uint8_t *buffer, size_t *size) msg.eam_sensor_id = EAM_SENSOR_ID; msg.sensor_text_id = EAM_SENSOR_TEXT_ID; - msg.temperature1 = (uint8_t)(raw.baro_temp_celcius + 20); + msg.temperature1 = (uint8_t)(raw.baro_temp_celcius[0] + 20); msg.temperature2 = msg.temperature1 - BOARD_TEMP_OFFSET_DEG; msg.main_voltage_L = (uint8_t)(battery.voltage_v * 10); - uint16_t alt = (uint16_t)(raw.baro_alt_meter + 500); + uint16_t alt = (uint16_t)(raw.baro_alt_meter[0] + 500); msg.altitude_L = (uint8_t)alt & 0xff; msg.altitude_H = (uint8_t)(alt >> 8) & 0xff; diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 7dcd5a0004..43a24359eb 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -66,6 +66,7 @@ #include #include #include +#include #include #include @@ -175,6 +176,7 @@ static const int ERROR = -1; #define L3GD20_DEFAULT_RATE 760 #define L3G4200D_DEFAULT_RATE 800 +#define L3GD20_MAX_OUTPUT_RATE 280 #define L3GD20_DEFAULT_RANGE_DPS 2000 #define L3GD20_DEFAULT_FILTER_FREQ 30 #define L3GD20_TEMP_OFFSET_CELSIUS 40 @@ -256,6 +258,8 @@ private: math::LowPassFilter2p _gyro_filter_y; math::LowPassFilter2p _gyro_filter_z; + Integrator _gyro_int; + /* true if an L3G4200D is detected */ bool _is_l3g4200d; @@ -427,6 +431,7 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati _gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), + _gyro_int(1000000 / L3GD20_MAX_OUTPUT_RATE, true), _is_l3g4200d(false), _rotation(rotation), _checked_next(0) @@ -1029,13 +1034,21 @@ L3GD20::measure() // apply user specified rotation rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); - report.x = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; - report.y = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; - report.z = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; + float xin = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; + float yin = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; + float zin = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; - report.x = _gyro_filter_x.apply(report.x); - report.y = _gyro_filter_y.apply(report.y); - report.z = _gyro_filter_z.apply(report.z); + report.x = _gyro_filter_x.apply(xin); + report.y = _gyro_filter_y.apply(yin); + report.z = _gyro_filter_z.apply(zin); + + math::Vector<3> gval(xin, yin, zin); + math::Vector<3> gval_integrated; + + bool gyro_notify = _gyro_int.put(report.timestamp, gval, gval_integrated, report.integral_dt); + report.x_integral = gval_integrated(0); + report.y_integral = gval_integrated(1); + report.z_integral = gval_integrated(2); report.temperature = L3GD20_TEMP_OFFSET_CELSIUS - raw_report.temp; @@ -1044,13 +1057,15 @@ L3GD20::measure() _reports->force(&report); - /* notify anyone waiting for data */ - poll_notify(POLLIN); + if (gyro_notify) { + /* notify anyone waiting for data */ + poll_notify(POLLIN); - /* publish for subscribers */ - if (!(_pub_blocked)) { - /* publish it */ - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report); + /* publish for subscribers */ + if (!(_pub_blocked)) { + /* publish it */ + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report); + } } _read++; diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index 71f4134790..a4c8720dbc 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -113,6 +113,10 @@ LidarLite * get_dev(const bool use_i2c, const int bus) { */ void start(const bool use_i2c, const int bus) { + if (g_dev_int != nullptr || g_dev_ext != nullptr || g_dev_pwm != nullptr) { + errx(1,"driver already started"); + } + if (use_i2c) { /* create the driver, attempt expansion bus first */ if (bus == -1 || bus == PX4_I2C_BUS_EXPANSION) { @@ -464,7 +468,6 @@ ll40ls_main(int argc, char *argv[]) /* Start/load the driver. */ if (!strcmp(verb, "start")) { - ll40ls::start(use_i2c, bus); } diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 52e514a3e1..4235ecc6f5 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -65,6 +65,7 @@ #include #include #include +#include #include #include @@ -206,6 +207,7 @@ static const int ERROR = -1; #define LSM303D_ACCEL_DEFAULT_RATE 800 #define LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ 50 #define LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 +#define LSM303D_ACCEL_MAX_OUTPUT_RATE 280 #define LSM303D_MAG_DEFAULT_RANGE_GA 2 #define LSM303D_MAG_DEFAULT_RATE 100 @@ -308,6 +310,8 @@ private: math::LowPassFilter2p _accel_filter_y; math::LowPassFilter2p _accel_filter_z; + Integrator _accel_int; + enum Rotation _rotation; // values used to @@ -577,6 +581,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota _accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _accel_int(1000000 / LSM303D_ACCEL_MAX_OUTPUT_RATE, true), _rotation(rotation), _constant_accel_count(0), _last_temperature(0), @@ -1411,6 +1416,13 @@ LSM303D::stop() { hrt_cancel(&_accel_call); hrt_cancel(&_mag_call); + + /* reset internal states */ + memset(_last_accel, 0, sizeof(_last_accel)); + + /* discard unread data in the buffers */ + _accel_reports->flush(); + _mag_reports->flush(); } void @@ -1575,17 +1587,27 @@ LSM303D::measure() accel_report.y = _accel_filter_y.apply(y_in_new); accel_report.z = _accel_filter_z.apply(z_in_new); + math::Vector<3> aval(x_in_new, y_in_new, z_in_new); + math::Vector<3> aval_integrated; + + bool accel_notify = _accel_int.put(accel_report.timestamp, aval, aval_integrated, accel_report.integral_dt); + accel_report.x_integral = aval_integrated(0); + accel_report.y_integral = aval_integrated(1); + accel_report.z_integral = aval_integrated(2); + accel_report.scaling = _accel_range_scale; accel_report.range_m_s2 = _accel_range_m_s2; _accel_reports->force(&accel_report); /* notify anyone waiting for data */ - poll_notify(POLLIN); + if (accel_notify) { + poll_notify(POLLIN); - if (!(_pub_blocked)) { - /* publish it */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); + if (!(_pub_blocked)) { + /* publish it */ + orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); + } } _accel_read++; @@ -1841,7 +1863,7 @@ namespace lsm303d LSM303D *g_dev; -void start(bool external_bus, enum Rotation rotation); +void start(bool external_bus, enum Rotation rotation, unsigned range); void test(); void reset(); void info(); @@ -1856,11 +1878,12 @@ void test_error(); * up and running or failed to detect the sensor. */ void -start(bool external_bus, enum Rotation rotation) +start(bool external_bus, enum Rotation rotation, unsigned range) { int fd, fd_mag; - if (g_dev != nullptr) + if (g_dev != nullptr) { errx(0, "already started"); + } /* create the driver */ if (external_bus) { @@ -1884,11 +1907,17 @@ start(bool external_bus, enum Rotation rotation) /* set the poll rate to default, starts automatic data collection */ fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY); - if (fd < 0) + if (fd < 0) { goto fail; + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { goto fail; + } + + if (ioctl(fd, ACCELIOCSRANGE, range) < 0) { + goto fail; + } fd_mag = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY); @@ -1980,7 +2009,10 @@ test() warnx("mag z: \t%d\traw", (int)m_report.z_raw); warnx("mag range: %8.4f ga", (double)m_report.range_ga); - /* XXX add poll-rate tests here too */ + /* reset to default polling */ + if (ioctl(fd_accel, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + err(1, "reset to default polling"); + } close(fd_accel); close(fd_mag); @@ -2084,9 +2116,10 @@ lsm303d_main(int argc, char *argv[]) bool external_bus = false; int ch; enum Rotation rotation = ROTATION_NONE; + int accel_range = 8; /* jump over start/off/etc and look at options first */ - while ((ch = getopt(argc, argv, "XR:")) != EOF) { + while ((ch = getopt(argc, argv, "XR:a:")) != EOF) { switch (ch) { case 'X': external_bus = true; @@ -2094,6 +2127,9 @@ lsm303d_main(int argc, char *argv[]) case 'R': rotation = (enum Rotation)atoi(optarg); break; + case 'a': + accel_range = atoi(optarg); + break; default: lsm303d::usage(); exit(0); @@ -2107,7 +2143,7 @@ lsm303d_main(int argc, char *argv[]) */ if (!strcmp(verb, "start")) - lsm303d::start(external_bus, rotation); + lsm303d::start(external_bus, rotation, accel_range); /* * Test the driver/device. diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp index 5d1f58b854..041772b4bf 100644 --- a/src/drivers/md25/md25.cpp +++ b/src/drivers/md25/md25.cpp @@ -333,7 +333,7 @@ void MD25::update() // check for exit condition every second // note "::poll" is required to distinguish global // poll from member function for driver - if (::poll(&_controlPoll, 1, 1000) < 0) return; // poll error + if (::poll(&_controlPoll, 1, 1000) < 0) { return; } // poll error // if new data, send to motors if (_actuators.updated()) { @@ -350,7 +350,7 @@ int MD25::probe() int ret = OK; // try initial address first, if good, then done - if (readData() == OK) return ret; + if (readData() == OK) { return ret; } // try all other addresses uint8_t testAddress = 0; @@ -451,9 +451,9 @@ float MD25::_uint8ToNorm(uint8_t value) uint8_t MD25::_normToUint8(float value) { - if (value > 1) value = 1; + if (value > 1) { value = 1; } - if (value < -1) value = -1; + if (value < -1) { value = -1; } // TODO, should go from 0 to 255 // possibly should handle this differently @@ -494,7 +494,7 @@ int md25Test(const char *deviceName, uint8_t bus, uint8_t address) break; } - if (t > 2.0f) break; + if (t > 2.0f) { break; } } md25.setMotor1Speed(0); @@ -514,7 +514,7 @@ int md25Test(const char *deviceName, uint8_t bus, uint8_t address) break; } - if (t > 2.0f) break; + if (t > 2.0f) { break; } } md25.setMotor1Speed(0); @@ -536,7 +536,7 @@ int md25Test(const char *deviceName, uint8_t bus, uint8_t address) break; } - if (t > 2.0f) break; + if (t > 2.0f) { break; } } md25.setMotor2Speed(0); @@ -556,7 +556,7 @@ int md25Test(const char *deviceName, uint8_t bus, uint8_t address) break; } - if (t > 2.0f) break; + if (t > 2.0f) { break; } } md25.setMotor2Speed(0); @@ -592,13 +592,14 @@ int md25Sine(const char *deviceName, uint8_t bus, uint8_t address, float amplitu // sine wave for motor 1 md25.resetEncoders(); + while (true) { // input uint64_t timestamp = hrt_absolute_time(); - float t = timestamp/1000000.0f; + float t = timestamp / 1000000.0f; - float input_value = amplitude*sinf(2*M_PI*frequency*t); + float input_value = amplitude * sinf(2 * M_PI * frequency * t); md25.setMotor1Speed(input_value); // output @@ -613,11 +614,11 @@ int md25Sine(const char *deviceName, uint8_t bus, uint8_t address, float amplitu // send output message strncpy(debug_msg.key, "md25 out ", 10); - debug_msg.timestamp_ms = 1000*timestamp; + debug_msg.timestamp_ms = 1000 * timestamp; debug_msg.value = current_revolution; debug_msg.update(); - if (t > t_final) break; + if (t > t_final) { break; } // update for next step prev_revolution = current_revolution; @@ -625,6 +626,7 @@ int md25Sine(const char *deviceName, uint8_t bus, uint8_t address, float amplitu // sleep usleep(1000000 * dt); } + md25.setMotor1Speed(0); printf("md25 sine complete\n"); diff --git a/src/drivers/md25/md25_main.cpp b/src/drivers/md25/md25_main.cpp index a20bf7c7c6..5b889bf03a 100644 --- a/src/drivers/md25/md25_main.cpp +++ b/src/drivers/md25/md25_main.cpp @@ -79,8 +79,9 @@ static void usage(const char *reason); static void usage(const char *reason) { - if (reason) + if (reason) { fprintf(stderr, "%s\n", reason); + } fprintf(stderr, "usage: md25 {start|stop|read|status|search|test|change_address}\n\n"); exit(1); @@ -111,11 +112,11 @@ int md25_main(int argc, char *argv[]) thread_should_exit = false; deamon_task = px4_task_spawn_cmd("md25", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 10, - 2048, - md25_thread_main, - (const char **)argv); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 10, + 2048, + md25_thread_main, + (const char **)argv); exit(0); } @@ -206,7 +207,7 @@ int md25_main(int argc, char *argv[]) exit(0); } - + if (!strcmp(argv[1], "search")) { if (argc < 3) { diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 7c7bc43d3e..4af1bec586 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -69,6 +69,7 @@ #include #include +#include #include #include #include @@ -167,10 +168,13 @@ #define MPU6000_ACCEL_DEFAULT_RANGE_G 8 #define MPU6000_ACCEL_DEFAULT_RATE 1000 +#define MPU6000_ACCEL_MAX_OUTPUT_RATE 280 #define MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 #define MPU6000_GYRO_DEFAULT_RANGE_G 8 #define MPU6000_GYRO_DEFAULT_RATE 1000 +/* rates need to be the same between accel and gyro */ +#define MPU6000_GYRO_MAX_OUTPUT_RATE MPU6000_ACCEL_MAX_OUTPUT_RATE #define MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30 #define MPU6000_DEFAULT_ONCHIP_FILTER_FREQ 42 @@ -281,6 +285,9 @@ private: math::LowPassFilter2p _gyro_filter_y; math::LowPassFilter2p _gyro_filter_z; + Integrator _accel_int; + Integrator _gyro_int; + enum Rotation _rotation; // this is used to support runtime checking of key @@ -535,6 +542,8 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _gyro_filter_x(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _gyro_filter_y(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), + _accel_int(1000000 / MPU6000_ACCEL_MAX_OUTPUT_RATE), + _gyro_int(1000000 / MPU6000_GYRO_MAX_OUTPUT_RATE, true), _rotation(rotation), _checked_next(0), _in_factory_test(false), @@ -1525,6 +1534,13 @@ void MPU6000::stop() { hrt_cancel(&_call); + + /* reset internal states */ + memset(_last_accel, 0, sizeof(_last_accel)); + + /* discard unread data in the buffers */ + _accel_reports->flush(); + _gyro_reports->flush(); } void @@ -1759,6 +1775,14 @@ MPU6000::measure() arb.y = _accel_filter_y.apply(y_in_new); arb.z = _accel_filter_z.apply(z_in_new); + math::Vector<3> aval(x_in_new, y_in_new, z_in_new); + math::Vector<3> aval_integrated; + + bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt); + arb.x_integral = aval_integrated(0); + arb.y_integral = aval_integrated(1); + arb.z_integral = aval_integrated(2); + arb.scaling = _accel_range_scale; arb.range_m_s2 = _accel_range_m_s2; @@ -1786,6 +1810,14 @@ MPU6000::measure() grb.y = _gyro_filter_y.apply(y_gyro_in_new); grb.z = _gyro_filter_z.apply(z_gyro_in_new); + math::Vector<3> gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new); + math::Vector<3> gval_integrated; + + bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt); + grb.x_integral = gval_integrated(0); + grb.y_integral = gval_integrated(1); + grb.z_integral = gval_integrated(2); + grb.scaling = _gyro_range_scale; grb.range_rad_s = _gyro_range_rad_s; @@ -1796,10 +1828,15 @@ MPU6000::measure() _gyro_reports->force(&grb); /* notify anyone waiting for data */ - poll_notify(POLLIN); - _gyro->parent_poll_notify(); + if (accel_notify) { + poll_notify(POLLIN); + } - if (!(_pub_blocked)) { + if (gyro_notify) { + _gyro->parent_poll_notify(); + } + + if (accel_notify && !(_pub_blocked)) { /* log the time of this report */ perf_begin(_controller_latency_perf); perf_begin(_system_latency_perf); @@ -1807,7 +1844,7 @@ MPU6000::measure() orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); } - if (!(_pub_blocked)) { + if (gyro_notify && !(_pub_blocked)) { /* publish it */ orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); } @@ -1925,7 +1962,7 @@ namespace mpu6000 MPU6000 *g_dev_int; // on internal bus MPU6000 *g_dev_ext; // on external bus -void start(bool, enum Rotation); +void start(bool, enum Rotation, int range); void stop(bool); void test(bool); void reset(bool); @@ -1942,7 +1979,7 @@ void usage(); * or failed to detect the sensor. */ void -start(bool external_bus, enum Rotation rotation) +start(bool external_bus, enum Rotation rotation, int range) { int fd; MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; @@ -1979,6 +2016,9 @@ start(bool external_bus, enum Rotation rotation) if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) goto fail; + if (ioctl(fd, ACCELIOCSRANGE, range) < 0) + goto fail; + close(fd); exit(0); @@ -2076,6 +2116,12 @@ test(bool external_bus) warnx("temp: \t%8.4f\tdeg celsius", (double)a_report.temperature); warnx("temp: \t%d\traw 0x%0x", (short)a_report.temperature_raw, (unsigned short)a_report.temperature_raw); + /* reset to default polling */ + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + err(1, "reset to default polling"); + + close(fd); + close(fd_gyro); /* XXX add poll-rate tests here too */ @@ -2175,6 +2221,7 @@ usage() warnx("options:"); warnx(" -X (external bus)"); warnx(" -R rotation"); + warnx(" -a accel range (in g)"); } } // namespace @@ -2185,9 +2232,10 @@ mpu6000_main(int argc, char *argv[]) bool external_bus = false; int ch; enum Rotation rotation = ROTATION_NONE; + int accel_range = 8; /* jump over start/off/etc and look at options first */ - while ((ch = getopt(argc, argv, "XR:")) != EOF) { + while ((ch = getopt(argc, argv, "XR:a:")) != EOF) { switch (ch) { case 'X': external_bus = true; @@ -2195,6 +2243,9 @@ mpu6000_main(int argc, char *argv[]) case 'R': rotation = (enum Rotation)atoi(optarg); break; + case 'a': + accel_range = atoi(optarg); + break; default: mpu6000::usage(); exit(0); @@ -2208,7 +2259,7 @@ mpu6000_main(int argc, char *argv[]) */ if (!strcmp(verb, "start")) { - mpu6000::start(external_bus, rotation); + mpu6000::start(external_bus, rotation, accel_range); } if (!strcmp(verb, "stop")) { diff --git a/src/drivers/ms5611/ms5611.h b/src/drivers/ms5611/ms5611.h index 1d6239467d..a7e305b2aa 100644 --- a/src/drivers/ms5611/ms5611.h +++ b/src/drivers/ms5611/ms5611.h @@ -83,4 +83,4 @@ extern bool crc4(uint16_t *n_prom); extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, uint8_t busnum) __attribute__((weak)); extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint8_t busnum) __attribute__((weak)); extern device::Device *MS5611_sim_interface(ms5611::prom_u &prom_buf, uint8_t busnum) __attribute__((weak)); -typedef device::Device* (*MS5611_constructor)(ms5611::prom_u &prom_buf, uint8_t busnum); +typedef device::Device *(*MS5611_constructor)(ms5611::prom_u &prom_buf, uint8_t busnum); diff --git a/src/drivers/ms5611/ms5611_i2c.cpp b/src/drivers/ms5611/ms5611_i2c.cpp index 8f66304065..74f67c0f26 100644 --- a/src/drivers/ms5611/ms5611_i2c.cpp +++ b/src/drivers/ms5611/ms5611_i2c.cpp @@ -31,11 +31,11 @@ * ****************************************************************************/ - /** - * @file ms5611_i2c.cpp - * - * I2C interface for MS5611 - */ +/** + * @file ms5611_i2c.cpp + * + * I2C interface for MS5611 + */ /* XXX trim includes */ #include @@ -116,13 +116,13 @@ MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint8_t busnum) } MS5611_I2C::MS5611_I2C(uint8_t bus, ms5611::prom_u &prom) : - I2C("MS5611_I2C", + I2C("MS5611_I2C", #ifdef __PX4_NUTTX -nullptr, bus, 0, 400000 + nullptr, bus, 0, 400000 #else -"/dev/MS5611_I2C", bus, 0 + "/dev/MS5611_I2C", bus, 0 #endif -), + ), _prom(prom) { } @@ -150,6 +150,7 @@ MS5611_I2C::read(device::file_t *handlep, char *data, size_t buflen) /* read the most recent measurement */ uint8_t cmd = 0; int ret = transfer(&cmd, 1, &buf[0], 3); + if (ret == PX4_OK) { /* fetch the raw value */ cvt->b[0] = buf[2]; @@ -191,9 +192,9 @@ MS5611_I2C::probe() if ((PX4_OK == _probe_address(MS5611_ADDRESS_1)) || (PX4_OK == _probe_address(MS5611_ADDRESS_2))) { /* - * Disable retries; we may enable them selectively in some cases, + * Disable retries; we may enable them selectively in some cases, * but the device gets confused if we retry some of the commands. - */ + */ _retries = 0; return PX4_OK; } @@ -208,12 +209,14 @@ MS5611_I2C::_probe_address(uint8_t address) set_address(address); /* send reset command */ - if (PX4_OK != _reset()) + if (PX4_OK != _reset()) { return -EIO; + } /* read PROM */ - if (PX4_OK != _read_prom()) + if (PX4_OK != _read_prom()) { return -EIO; + } return PX4_OK; } @@ -239,7 +242,7 @@ int MS5611_I2C::_measure(unsigned addr) { /* - * Disable retries on this command; we can't know whether failure + * Disable retries on this command; we can't know whether failure * means the device did or did not see the command. */ _retries = 0; @@ -267,8 +270,9 @@ MS5611_I2C::_read_prom() for (int i = 0; i < 8; i++) { uint8_t cmd = ADDR_PROM_SETUP + (i * 2); - if (PX4_OK != transfer(&cmd, 1, &prom_buf[0], 2)) + if (PX4_OK != transfer(&cmd, 1, &prom_buf[0], 2)) { break; + } /* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */ cvt.b[0] = prom_buf[1]; diff --git a/src/drivers/ms5611/ms5611_nuttx.cpp b/src/drivers/ms5611/ms5611_nuttx.cpp index 4a304e130b..7c336dfcab 100644 --- a/src/drivers/ms5611/ms5611_nuttx.cpp +++ b/src/drivers/ms5611/ms5611_nuttx.cpp @@ -106,7 +106,7 @@ static const int ERROR = -1; class MS5611 : public device::CDev { public: - MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char* path); + MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path); ~MS5611(); virtual int init(); @@ -214,7 +214,7 @@ protected: */ extern "C" __EXPORT int ms5611_main(int argc, char *argv[]); -MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char* path) : +MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path) : CDev("MS5611", path), _interface(interface), _prom(prom_buf.s), @@ -243,12 +243,14 @@ MS5611::~MS5611() /* make sure we are truly inactive */ stop_cycle(); - if (_class_instance != -1) + if (_class_instance != -1) { unregister_class_devname(get_devname(), _class_instance); + } /* free any existing reports */ - if (_reports != nullptr) + if (_reports != nullptr) { delete _reports; + } // free perf counters perf_free(_sample_perf); @@ -265,13 +267,14 @@ MS5611::init() int ret; ret = CDev::init(); + if (ret != OK) { DEVICE_DEBUG("CDev init failed"); goto out; } /* allocate basic report buffers */ - _reports = new ringbuffer::RingBuffer(2, sizeof(baro_report)); + _reports = new ringbuffer::RingBuffer(2, sizeof(sensor_baro_s)); if (_reports == nullptr) { DEVICE_DEBUG("can't get memory for reports"); @@ -321,7 +324,7 @@ MS5611::init() ret = OK; _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp, - &_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT); + &_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT); if (_baro_topic == nullptr) { @@ -342,8 +345,9 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is enabled */ if (_measure_ticks > 0) { @@ -396,8 +400,9 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) } /* state machine will have generated a report, copy it out */ - if (_reports->get(brp)) + if (_reports->get(brp)) { ret = sizeof(*brp); + } } while (0); @@ -412,20 +417,20 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCSPOLLRATE: { switch (arg) { - /* switching to manual polling */ + /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop_cycle(); _measure_ticks = 0; return OK; - /* external signalling not supported */ + /* external signalling not supported */ case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ + /* zero would be bad */ case 0: return -EINVAL; - /* set default/max polling rate */ + /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: { /* do we need to start internal polling? */ @@ -435,13 +440,14 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) _measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL); /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start_cycle(); + } return OK; } - /* adjust to a legal polling interval in Hz */ + /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ bool want_start = (_measure_ticks == 0); @@ -450,15 +456,17 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) unsigned ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ - if (ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL)) + if (ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL)) { return -EINVAL; + } /* update interval for next measurement */ _measure_ticks = ticks; /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start_cycle(); + } return OK; } @@ -466,24 +474,28 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) + if (_measure_ticks == 0) { return SENSOR_POLLRATE_MANUAL; + } return (1000 / _measure_ticks); case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } - irqstate_t flags = irqsave(); - if (!_reports->resize(arg)) { irqrestore(flags); - return -ENOMEM; + return OK; } - irqrestore(flags); - return OK; - } case SENSORIOCGQUEUEDEPTH: return _reports->size(); @@ -498,8 +510,9 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) case BAROIOCSMSLPRESSURE: /* range-check for sanity */ - if ((arg < 80000) || (arg > 120000)) + if ((arg < 80000) || (arg > 120000)) { return -EINVAL; + } _msl_pressure = arg; return OK; @@ -554,6 +567,7 @@ MS5611::cycle() /* perform collection */ ret = collect(); + if (ret != OK) { if (ret == -6) { /* @@ -564,6 +578,7 @@ MS5611::cycle() } else { //DEVICE_LOG("collection error %d", ret); } + /* issue a reset command to the sensor */ _interface->ioctl(IOCTL_RESET, dummy); /* reset the collection state machine and try again - we need @@ -598,6 +613,7 @@ MS5611::cycle() /* measurement phase */ ret = measure(); + if (ret != OK) { /* issue a reset command to the sensor */ _interface->ioctl(IOCTL_RESET, dummy); @@ -633,8 +649,10 @@ MS5611::measure() * Send the command to begin measuring. */ ret = _interface->ioctl(IOCTL_MEASURE, addr); - if (OK != ret) + + if (OK != ret) { perf_count(_comms_errors); + } perf_end(_measure_perf); @@ -652,10 +670,11 @@ MS5611::collect() struct baro_report report; /* this should be fairly close to the end of the conversion, so the best approximation of the time */ report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); + report.error_count = perf_event_count(_comms_errors); /* read the most recent measurement - read offset/size are hardcoded in the interface */ ret = _interface->read(0, (void *)&raw, 0); + if (ret < 0) { perf_count(_comms_errors); perf_end(_sample_perf); @@ -883,11 +902,13 @@ crc4(uint16_t *n_prom) bool start_bus(struct ms5611_bus_option &bus) { - if (bus.dev != nullptr) - errx(1,"bus option already started"); + if (bus.dev != nullptr) { + errx(1, "bus option already started"); + } prom_u prom_buf; device::Device *interface = bus.interface_constructor(prom_buf, bus.busnum); + if (interface->init() != OK) { delete interface; warnx("no device on bus %u", (unsigned)bus.busid); @@ -895,6 +916,7 @@ start_bus(struct ms5611_bus_option &bus) } bus.dev = new MS5611(interface, prom_buf, bus.devpath); + if (bus.dev != nullptr && OK != bus.dev->init()) { delete bus.dev; bus.dev = NULL; @@ -907,6 +929,7 @@ start_bus(struct ms5611_bus_option &bus) if (fd == -1) { errx(1, "can't open baro device"); } + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { close(fd); errx(1, "failed setting default poll rate"); @@ -929,20 +952,23 @@ start(enum MS5611_BUS busid) uint8_t i; bool started = false; - for (i=0; iprint_info(); } } + exit(0); } @@ -1084,12 +1124,14 @@ calibrate(unsigned altitude, enum MS5611_BUS busid) fd = open(bus.devpath, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "open failed (try 'ms5611 start' if the driver is not running)"); + } /* start the sensor polling at max */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX)) + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX)) { errx(1, "failed to set poll rate"); + } /* average a few measurements */ pressure = 0.0f; @@ -1104,14 +1146,16 @@ calibrate(unsigned altitude, enum MS5611_BUS busid) fds.events = POLLIN; ret = poll(&fds, 1, 1000); - if (ret != 1) + if (ret != 1) { errx(1, "timed out waiting for sensor data"); + } /* now go get it */ sz = read(fd, &report, sizeof(report)); - if (sz != sizeof(report)) + if (sz != sizeof(report)) { err(1, "sensor read failed"); + } pressure += report.pressure; } @@ -1134,8 +1178,9 @@ calibrate(unsigned altitude, enum MS5611_BUS busid) /* save as integer Pa */ p1 *= 1000.0f; - if (ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK) + if (ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK) { err(1, "BAROIOCSMSLPRESSURE"); + } close(fd); exit(0); @@ -1166,15 +1211,19 @@ ms5611_main(int argc, char *argv[]) case 'X': busid = MS5611_BUS_I2C_EXTERNAL; break; + case 'I': busid = MS5611_BUS_I2C_INTERNAL; break; + case 'S': busid = MS5611_BUS_SPI_EXTERNAL; break; + case 's': busid = MS5611_BUS_SPI_INTERNAL; break; + default: ms5611::usage(); exit(0); @@ -1215,10 +1264,11 @@ ms5611_main(int argc, char *argv[]) * Perform MSL pressure calibration given an altitude in metres */ if (!strcmp(verb, "calibrate")) { - if (argc < 2) + if (argc < 2) { errx(1, "missing altitude"); + } - long altitude = strtol(argv[optind+1], nullptr, 10); + long altitude = strtol(argv[optind + 1], nullptr, 10); ms5611::calibrate(altitude, busid); } diff --git a/src/drivers/ms5611/ms5611_posix.cpp b/src/drivers/ms5611/ms5611_posix.cpp index d41a186c38..b4b22f6c35 100644 --- a/src/drivers/ms5611/ms5611_posix.cpp +++ b/src/drivers/ms5611/ms5611_posix.cpp @@ -105,7 +105,7 @@ static const int ERROR = -1; class MS5611 : public device::VDev { public: - MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char* path); + MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path); ~MS5611(); virtual int init(); @@ -211,7 +211,7 @@ protected: */ extern "C" __EXPORT int ms5611_main(int argc, char *argv[]); -MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char* path) : +MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path) : VDev("MS5611", path), _interface(interface), _prom(prom_buf.s), @@ -240,12 +240,14 @@ MS5611::~MS5611() /* make sure we are truly inactive */ stop_cycle(); - if (_class_instance != -1) + if (_class_instance != -1) { unregister_class_devname(get_devname(), _class_instance); + } /* free any existing reports */ - if (_reports != nullptr) + if (_reports != nullptr) { delete _reports; + } // free perf counters perf_free(_sample_perf); @@ -263,6 +265,7 @@ MS5611::init() warnx("MS5611::init"); ret = VDev::init(); + if (ret != OK) { DEVICE_DEBUG("VDev init failed"); goto out; @@ -323,11 +326,12 @@ MS5611::init() ret = OK; _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp, - &_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT); + &_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT); if (_baro_topic == nullptr) { warnx("failed to create sensor_baro publication"); } + //warnx("sensor_baro publication %ld", _baro_topic); } while (0); @@ -344,8 +348,9 @@ MS5611::read(device::file_t *handlep, char *buffer, size_t buflen) int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is enabled */ if (_measure_ticks > 0) { @@ -398,8 +403,9 @@ MS5611::read(device::file_t *handlep, char *buffer, size_t buflen) } /* state machine will have generated a report, copy it out */ - if (_reports->get(brp)) + if (_reports->get(brp)) { ret = sizeof(*brp); + } } while (0); @@ -414,20 +420,20 @@ MS5611::ioctl(device::file_t *handlep, int cmd, unsigned long arg) case SENSORIOCSPOLLRATE: { switch (arg) { - /* switching to manual polling */ + /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop_cycle(); _measure_ticks = 0; return OK; - /* external signalling not supported */ + /* external signalling not supported */ case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ + /* zero would be bad */ case 0: return -EINVAL; - /* set default/max polling rate */ + /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: { /* do we need to start internal polling? */ @@ -437,13 +443,14 @@ MS5611::ioctl(device::file_t *handlep, int cmd, unsigned long arg) _measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL); /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start_cycle(); + } return OK; } - /* adjust to a legal polling interval in Hz */ + /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ bool want_start = (_measure_ticks == 0); @@ -452,15 +459,17 @@ MS5611::ioctl(device::file_t *handlep, int cmd, unsigned long arg) unsigned ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ - if ((unsigned long)ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL)) + if ((unsigned long)ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL)) { return -EINVAL; + } /* update interval for next measurement */ _measure_ticks = ticks; /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start_cycle(); + } return OK; } @@ -468,21 +477,24 @@ MS5611::ioctl(device::file_t *handlep, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) + if (_measure_ticks == 0) { return SENSOR_POLLRATE_MANUAL; + } return (1000 / _measure_ticks); case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } - if (!_reports->resize(arg)) { - return -ENOMEM; + if (!_reports->resize(arg)) { + return -ENOMEM; + } + + return OK; } - return OK; - } case SENSORIOCGQUEUEDEPTH: return _reports->size(); @@ -497,8 +509,9 @@ MS5611::ioctl(device::file_t *handlep, int cmd, unsigned long arg) case BAROIOCSMSLPRESSURE: /* range-check for sanity */ - if ((arg < 80000) || (arg > 120000)) + if ((arg < 80000) || (arg > 120000)) { return -EINVAL; + } _msl_pressure = arg; return OK; @@ -553,6 +566,7 @@ MS5611::cycle() /* perform collection */ ret = collect(); + if (ret != OK) { if (ret == -6) { /* @@ -563,6 +577,7 @@ MS5611::cycle() } else { //DEVICE_LOG("collection error %d", ret); } + /* issue a reset command to the sensor */ _interface->dev_ioctl(IOCTL_RESET, dummy); /* reset the collection state machine and try again */ @@ -594,6 +609,7 @@ MS5611::cycle() /* measurement phase */ ret = measure(); + if (ret != OK) { //DEVICE_LOG("measure error %d", ret); /* issue a reset command to the sensor */ @@ -630,8 +646,10 @@ MS5611::measure() * Send the command to begin measuring. */ ret = _interface->dev_ioctl(IOCTL_MEASURE, addr); - if (OK != ret) + + if (OK != ret) { perf_count(_comms_errors); + } perf_end(_measure_perf); @@ -649,10 +667,11 @@ MS5611::collect() struct baro_report report; /* this should be fairly close to the end of the conversion, so the best approximation of the time */ report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); + report.error_count = perf_event_count(_comms_errors); /* read the most recent measurement - read offset/size are hardcoded in the interface */ ret = _interface->dev_read(0, (void *)&raw, 0); + if (ret < 0) { perf_count(_comms_errors); perf_end(_sample_perf); @@ -745,8 +764,8 @@ MS5611::collect() if (_baro_topic != (orb_advert_t)(-1)) { /* publish it */ orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); - } - else { + + } else { printf("MS5611::collect _baro_topic not initialized\n"); } } @@ -897,6 +916,7 @@ start_bus(struct ms5611_bus_option &bus) prom_u prom_buf; device::Device *interface = bus.interface_constructor(prom_buf, bus.busnum); + if (interface->init() != OK) { delete interface; warnx("no device on bus %u", (unsigned)bus.busid); @@ -904,13 +924,14 @@ start_bus(struct ms5611_bus_option &bus) } bus.dev = new MS5611(interface, prom_buf, bus.devpath); + if (bus.dev != nullptr && OK != bus.dev->init()) { delete bus.dev; bus.dev = NULL; warnx("bus init failed %p", bus.dev); return false; } - + int fd = px4_open(bus.devpath, O_RDONLY); /* set the poll rate to default, starts automatic data collection */ @@ -918,6 +939,7 @@ start_bus(struct ms5611_bus_option &bus) warnx("can't open baro device"); return false; } + if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { px4_close(fd); warnx("failed setting default poll rate"); @@ -941,15 +963,17 @@ start(enum MS5611_BUS busid) uint8_t i; bool started = false; - for (i=0; iprint_info(); } } + return 0; } @@ -1118,12 +1151,15 @@ int calibrate(unsigned altitude, enum MS5611_BUS busid) { struct ms5611_bus_option bus; + if (!find_bus(busid, bus)) { return 1; } struct baro_report report; + float pressure; + float p1; int fd; @@ -1224,18 +1260,23 @@ ms5611_main(int argc, char *argv[]) /* jump over start/off/etc and look at options first */ int myoptind = 1; const char *myoptarg = NULL; + while ((ch = px4_getopt(argc, argv, "XIS", &myoptind, &myoptarg)) != EOF) { printf("ch = %d\n", ch); + switch (ch) { case 'X': busid = MS5611_BUS_I2C_EXTERNAL; break; + case 'I': busid = MS5611_BUS_I2C_INTERNAL; break; + case 'S': busid = MS5611_BUS_SIM_EXTERNAL; break; + default: ms5611::usage(); return 1; @@ -1247,42 +1288,48 @@ ms5611_main(int argc, char *argv[]) /* * Start/load the driver. */ - if (!strcmp(verb, "start")) + if (!strcmp(verb, "start")) { ret = ms5611::start(busid); + } /* * Test the driver/device. */ - else if (!strcmp(verb, "test")) + else if (!strcmp(verb, "test")) { ret = ms5611::test(busid); + } /* * Reset the driver. */ - else if (!strcmp(verb, "reset")) + else if (!strcmp(verb, "reset")) { ret = ms5611::reset(busid); + } /* * Print driver information. */ - else if (!strcmp(verb, "info")) + else if (!strcmp(verb, "info")) { ret = ms5611::info(); + } /* * Perform MSL pressure calibration given an altitude in metres */ else if (!strcmp(verb, "calibrate")) { - if (argc < 2) - PX4_WARN("missing altitude"); + if (argc < 2) { + PX4_WARN("missing altitude"); + } - long altitude = strtol(argv[optind+1], nullptr, 10); + long altitude = strtol(argv[optind + 1], nullptr, 10); ret = ms5611::calibrate(altitude, busid); - } - else { + + } else { ms5611::usage(); warnx("unrecognised command, try 'start', 'test', 'reset' or 'info'"); return 1; } + return ret; } diff --git a/src/drivers/ms5611/ms5611_sim.cpp b/src/drivers/ms5611/ms5611_sim.cpp index ab7656b9c3..72c02f6b38 100644 --- a/src/drivers/ms5611/ms5611_sim.cpp +++ b/src/drivers/ms5611/ms5611_sim.cpp @@ -31,11 +31,11 @@ * ****************************************************************************/ - /** - * @file ms5611_i2c.cpp - * - * SIM interface for MS5611 - */ +/** + * @file ms5611_i2c.cpp + * + * SIM interface for MS5611 + */ /* XXX trim includes */ #include @@ -127,6 +127,7 @@ MS5611_SIM::dev_read(unsigned offset, void *data, unsigned count) /* read the most recent measurement */ uint8_t cmd = 0; int ret = transfer(&cmd, 1, &buf[0], 3); + if (ret == PX4_OK) { /* fetch the raw value */ cvt->b[0] = buf[2]; @@ -178,7 +179,7 @@ int MS5611_SIM::_measure(unsigned addr) { /* - * Disable retries on this command; we can't know whether failure + * Disable retries on this command; we can't know whether failure * means the device did or did not see the command. */ _retries = 0; @@ -197,7 +198,7 @@ MS5611_SIM::_read_prom() int MS5611_SIM::transfer(const uint8_t *send, unsigned send_len, - uint8_t *recv, unsigned recv_len) + uint8_t *recv, unsigned recv_len) { // TODO add Simulation data connection so calls retrieve // data from the simulator diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 37c9fb4c12..78192c3a98 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -31,11 +31,11 @@ * ****************************************************************************/ - /** - * @file ms5611_spi.cpp - * - * SPI interface for MS5611 - */ +/** + * @file ms5611_spi.cpp + * + * SPI interface for MS5611 + */ /* XXX trim includes */ #include @@ -117,6 +117,7 @@ device::Device * MS5611_spi_interface(ms5611::prom_u &prom_buf, uint8_t busnum) { #ifdef PX4_SPI_BUS_EXT + if (busnum == PX4_SPI_BUS_EXT) { #ifdef PX4_SPIDEV_EXT_BARO return new MS5611_SPI(busnum, (spi_dev_e)PX4_SPIDEV_EXT_BARO, prom_buf); @@ -124,12 +125,13 @@ MS5611_spi_interface(ms5611::prom_u &prom_buf, uint8_t busnum) return nullptr; #endif } + #endif return new MS5611_SPI(busnum, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf); } MS5611_SPI::MS5611_SPI(uint8_t bus, spi_dev_e device, ms5611::prom_u &prom_buf) : - SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz */), + SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 11 * 1000 * 1000 /* will be rounded to 10.4 MHz */), _prom(prom_buf) { } @@ -144,6 +146,7 @@ MS5611_SPI::init() int ret; ret = SPI::init(); + if (ret != OK) { DEVICE_DEBUG("SPI init failed"); goto out; @@ -151,6 +154,7 @@ MS5611_SPI::init() /* send reset command */ ret = _reset(); + if (ret != OK) { DEVICE_DEBUG("reset failed"); goto out; @@ -158,6 +162,7 @@ MS5611_SPI::init() /* read PROM */ ret = _read_prom(); + if (ret != OK) { DEVICE_DEBUG("prom readout failed"); goto out; @@ -214,6 +219,7 @@ MS5611_SPI::ioctl(unsigned operation, unsigned &arg) errno = ret; return -1; } + return 0; } @@ -244,25 +250,32 @@ MS5611_SPI::_read_prom() usleep(3000); /* read and convert PROM words */ - bool all_zero = true; + bool all_zero = true; + for (int i = 0; i < 8; i++) { uint8_t cmd = (ADDR_PROM_SETUP + (i * 2)); _prom.c[i] = _reg16(cmd); - if (_prom.c[i] != 0) + + if (_prom.c[i] != 0) { all_zero = false; - //DEVICE_DEBUG("prom[%u]=0x%x", (unsigned)i, (unsigned)_prom.c[i]); + } + + //DEVICE_DEBUG("prom[%u]=0x%x", (unsigned)i, (unsigned)_prom.c[i]); } /* calculate CRC and return success/failure accordingly */ int ret = ms5611::crc4(&_prom.c[0]) ? OK : -EIO; - if (ret != OK) { + + if (ret != OK) { DEVICE_DEBUG("crc failed"); - } - if (all_zero) { + } + + if (all_zero) { DEVICE_DEBUG("prom all zero"); ret = -EIO; - } - return ret; + } + + return ret; } uint16_t diff --git a/src/drivers/pca8574/pca8574.cpp b/src/drivers/pca8574/pca8574.cpp index c697125a71..14ccaab7b2 100644 --- a/src/drivers/pca8574/pca8574.cpp +++ b/src/drivers/pca8574/pca8574.cpp @@ -188,6 +188,7 @@ PCA8574::ioctl(struct file *filp, int cmd, unsigned long arg) if (_values_out) { _mode = IOX_MODE_ON; } + send_led_values(); } @@ -277,15 +278,18 @@ PCA8574::led() _update_out = true; _should_run = true; + } else if (_mode == IOX_MODE_OFF) { _update_out = true; _should_run = false; + } else { // Any of the normal modes if (_blinking > 0) { /* we need to be running to blink */ _should_run = true; + } else { _should_run = false; } @@ -304,6 +308,7 @@ PCA8574::led() msg |= ((_blink_phase) ? _blinking : 0); _blink_phase = !_blink_phase; + } else { msg = _values_out; } @@ -513,6 +518,7 @@ pca8574_main(int argc, char *argv[]) printf("."); fflush(stdout); } + printf("\n"); fflush(stdout); @@ -520,6 +526,7 @@ pca8574_main(int argc, char *argv[]) delete g_pca8574; g_pca8574 = nullptr; exit(0); + } else { warnx("stop failed."); exit(1); @@ -542,9 +549,11 @@ pca8574_main(int argc, char *argv[]) if (channel < 8) { ret = ioctl(fd, (IOX_SET_VALUE + channel), val); + } else { ret = -1; } + close(fd); exit(ret); } diff --git a/src/drivers/pca9685/pca9685.cpp b/src/drivers/pca9685/pca9685.cpp index 7f0a3de031..051b4602f7 100644 --- a/src/drivers/pca9685/pca9685.cpp +++ b/src/drivers/pca9685/pca9685.cpp @@ -117,7 +117,7 @@ static const int ERROR = -1; class PCA9685 : public device::I2C { public: - PCA9685(int bus=PCA9685_BUS, uint8_t address=ADDR); + PCA9685(int bus = PCA9685_BUS, uint8_t address = ADDR); virtual ~PCA9685(); @@ -192,7 +192,7 @@ PCA9685::PCA9685(int bus, uint8_t address) : I2C("pca9685", PCA9685_DEVICE_PATH, bus, address, 100000), _mode(IOX_MODE_OFF), _running(false), - _i2cpwm_interval(SEC2TICK(1.0f/60.0f)), + _i2cpwm_interval(SEC2TICK(1.0f / 60.0f)), _should_run(false), _comms_errors(perf_alloc(PC_COUNT, "actuator_controls_2_comms_errors")), _actuator_controls_sub(-1), @@ -213,11 +213,13 @@ PCA9685::init() { int ret; ret = I2C::init(); + if (ret != OK) { return ret; } ret = reset(); + if (ret != OK) { return ret; } @@ -231,6 +233,7 @@ int PCA9685::ioctl(struct file *filp, int cmd, unsigned long arg) { int ret = -EINVAL; + switch (cmd) { case IOX_SET_MODE: @@ -241,9 +244,11 @@ PCA9685::ioctl(struct file *filp, int cmd, unsigned long arg) case IOX_MODE_OFF: warnx("shutting down"); break; + case IOX_MODE_ON: warnx("starting"); break; + case IOX_MODE_TEST_OUT: warnx("test starting"); break; @@ -280,6 +285,7 @@ PCA9685::info() if (is_running()) { warnx("Driver is running, mode: %u", _mode); + } else { warnx("Driver started but not running"); } @@ -304,8 +310,10 @@ PCA9685::i2cpwm() if (_mode == IOX_MODE_TEST_OUT) { setPin(0, PCA9685_PWMCENTER); _should_run = true; + } else if (_mode == IOX_MODE_OFF) { _should_run = false; + } else { if (!_mode_on_initialized) { /* Subscribe to actuator control 2 (payload group for gimbal) */ @@ -319,25 +327,29 @@ PCA9685::i2cpwm() /* Read the servo setpoints from the actuator control topics (gimbal) */ bool updated; orb_check(_actuator_controls_sub, &updated); + if (updated) { orb_copy(ORB_ID(actuator_controls_2), _actuator_controls_sub, &_actuator_controls); + for (int i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { /* Scale the controls to PWM, first multiply by pi to get rad, * the control[i] values are on the range -1 ... 1 */ uint16_t new_value = PCA9685_PWMCENTER + - (_actuator_controls.control[i] * M_PI_F * PCA9685_SCALE); + (_actuator_controls.control[i] * M_PI_F * PCA9685_SCALE); DEVICE_DEBUG("%d: current: %u, new %u, control %.2f", i, _current_values[i], new_value, - (double)_actuator_controls.control[i]); + (double)_actuator_controls.control[i]); + if (new_value != _current_values[i] && - isfinite(new_value) && - new_value >= PCA9685_PWMMIN && - new_value <= PCA9685_PWMMAX) { + isfinite(new_value) && + new_value >= PCA9685_PWMMIN && + new_value <= PCA9685_PWMMAX) { /* This value was updated, send the command to adjust the PWM value */ setPin(i, new_value); _current_values[i] = new_value; } } } + _should_run = true; } @@ -381,23 +393,29 @@ PCA9685::setPin(uint8_t num, uint16_t val, bool invert) if (val > 4095) { val = 4095; } + if (invert) { if (val == 0) { // Special value for signal fully on. return setPWM(num, 4096, 0); + } else if (val == 4095) { // Special value for signal fully off. return setPWM(num, 0, 4096); + } else { - return setPWM(num, 0, 4095-val); + return setPWM(num, 0, 4095 - val); } + } else { if (val == 4095) { // Special value for signal fully on. return setPWM(num, 4096, 0); + } else if (val == 0) { // Special value for signal fully off. return setPWM(num, 0, 4096); + } else { return setPWM(num, 0, val); } @@ -419,20 +437,27 @@ PCA9685::setPWMFreq(float freq) uint8_t prescale = uint8_t(prescaleval + 0.5f); //implicit floor() uint8_t oldmode; ret = read8(PCA9685_MODE1, oldmode); + if (ret != OK) { return ret; } - uint8_t newmode = (oldmode&0x7F) | 0x10; // sleep + + uint8_t newmode = (oldmode & 0x7F) | 0x10; // sleep ret = write8(PCA9685_MODE1, newmode); // go to sleep + if (ret != OK) { return ret; } + ret = write8(PCA9685_PRESCALE, prescale); // set the prescaler + if (ret != OK) { return ret; } + ret = write8(PCA9685_MODE1, oldmode); + if (ret != OK) { return ret; } @@ -440,6 +465,7 @@ PCA9685::setPWMFreq(float freq) usleep(5000); //5ms delay (from arduino driver) ret = write8(PCA9685_MODE1, oldmode | 0xa1); // This sets the MODE1 register to turn on auto increment. + if (ret != OK) { return ret; } @@ -455,12 +481,14 @@ PCA9685::read8(uint8_t addr, uint8_t &value) /* send addr */ ret = transfer(&addr, sizeof(addr), nullptr, 0); + if (ret != OK) { goto fail_read; } /* get value */ ret = transfer(nullptr, 0, &value, 1); + if (ret != OK) { goto fail_read; } @@ -474,23 +502,27 @@ fail_read: return ret; } -int PCA9685::reset(void) { +int PCA9685::reset(void) +{ warnx("resetting"); return write8(PCA9685_MODE1, 0x0); } /* Wrapper to wite a byte to addr */ int -PCA9685::write8(uint8_t addr, uint8_t value) { +PCA9685::write8(uint8_t addr, uint8_t value) +{ int ret = OK; _msg[0] = addr; _msg[1] = value; /* send addr and value */ ret = transfer(_msg, 2, nullptr, 0); + if (ret != OK) { perf_count(_comms_errors); DEVICE_LOG("i2c::transfer returned %d", ret); } + return ret; } @@ -571,10 +603,13 @@ pca9685_main(int argc, char *argv[]) errx(1, "init failed"); } } + fd = open(PCA9685_DEVICE_PATH, 0); + if (fd == -1) { errx(1, "Unable to open " PCA9685_DEVICE_PATH); } + ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_ON); close(fd); @@ -632,14 +667,16 @@ pca9685_main(int argc, char *argv[]) printf("."); fflush(stdout); } + printf("\n"); fflush(stdout); if (!g_pca9685->is_running()) { delete g_pca9685; - g_pca9685= nullptr; + g_pca9685 = nullptr; warnx("stopped, exiting"); exit(0); + } else { warnx("stop failed."); exit(1); diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index c1acccfaa1..3db4fb5e13 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -197,8 +197,8 @@ private: int gpio_ioctl(file *filp, int cmd, unsigned long arg); /* do not allow to copy due to ptr data members */ - PX4FMU(const PX4FMU&); - PX4FMU operator=(const PX4FMU&); + PX4FMU(const PX4FMU &); + PX4FMU operator=(const PX4FMU &); }; const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { @@ -273,7 +273,7 @@ PX4FMU::PX4FMU() : _mixers(nullptr), _groups_required(0), _groups_subscribed(0), - _control_subs{-1}, + _control_subs{ -1}, _poll_fds_num(0), _failsafe_pwm{0}, _disarmed_pwm{0}, @@ -335,8 +335,9 @@ PX4FMU::init() /* do regular cdev init */ ret = CDev::init(); - if (ret != OK) + if (ret != OK) { return ret; + } /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ _class_instance = register_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH); @@ -352,11 +353,11 @@ PX4FMU::init() /* start the IO interface task */ _task = px4_task_spawn_cmd("fmuservo", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 1200, - (main_t)&PX4FMU::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 1200, + (main_t)&PX4FMU::task_main_trampoline, + nullptr); if (_task < 0) { DEVICE_DEBUG("task start failed: %d", errno); @@ -426,6 +427,7 @@ PX4FMU::set_mode(Mode mode) break; #ifdef CONFIG_ARCH_BOARD_AEROCORE + case MODE_8PWM: // AeroCore PWMs as 8 PWM outs DEVICE_DEBUG("MODE_8PWM"); /* default output rates */ @@ -470,8 +472,9 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate // get the channel mask for this rate group uint32_t mask = up_pwm_servo_get_rate_group(group); - if (mask == 0) + if (mask == 0) { continue; + } // all channels in the group must be either default or alt-rate uint32_t alt = rate_map & mask; @@ -534,11 +537,13 @@ PX4FMU::subscribe() 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)) { DEVICE_DEBUG("subscribe to actuator_controls_%d", i); _control_subs[i] = orb_subscribe(_control_topics[i]); } + if (unsub_groups & (1 << i)) { DEVICE_DEBUG("unsubscribe from actuator_controls_%d", i); ::close(_control_subs[i]); @@ -574,7 +579,8 @@ PX4FMU::update_pwm_rev_mask() } void -PX4FMU::publish_pwm_outputs(uint16_t *values, size_t numvalues) { +PX4FMU::publish_pwm_outputs(uint16_t *values, size_t numvalues) +{ actuator_outputs_s outputs; outputs.noutputs = numvalues; outputs.timestamp = hrt_absolute_time(); @@ -586,6 +592,7 @@ PX4FMU::publish_pwm_outputs(uint16_t *values, size_t numvalues) { if (_outputs_pub == nullptr) { int instance = -1; _outputs_pub = orb_advertise_multi(ORB_ID(actuator_outputs), &outputs, &instance, ORB_PRIO_DEFAULT); + } else { orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs); } @@ -647,6 +654,7 @@ PX4FMU::task_main() } DEVICE_DEBUG("adjusted actuator update interval to %ums", 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); @@ -674,11 +682,13 @@ PX4FMU::task_main() /* 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++; } } @@ -704,6 +714,7 @@ PX4FMU::task_main() case MODE_8PWM: num_outputs = 8; break; + default: num_outputs = 0; break; @@ -723,7 +734,8 @@ PX4FMU::task_main() uint16_t pwm_limited[_max_actuators]; /* the PWM limit call takes care of out of band errors, NaN and constrains */ - pwm_limit_calc(_servo_armed, arm_nothrottle(), num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm, outputs, pwm_limited, &_pwm_limit); + pwm_limit_calc(_servo_armed, arm_nothrottle(), num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm, + outputs, pwm_limited, &_pwm_limit); /* output to the servos */ for (size_t i = 0; i < num_outputs; i++) { @@ -758,6 +770,7 @@ PX4FMU::task_main() } orb_check(_param_sub, &updated); + if (updated) { parameter_update_s pupdate; orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate); @@ -809,6 +822,7 @@ PX4FMU::task_main() _control_subs[i] = -1; } } + ::close(_armed_sub); ::close(_param_sub); @@ -837,6 +851,7 @@ PX4FMU::control_callback(uintptr_t handle, /* limit control input */ if (input > 1.0f) { input = 1.0f; + } else if (input < -1.0f) { input = -1.0f; } @@ -844,7 +859,7 @@ PX4FMU::control_callback(uintptr_t handle, /* motor spinup phase - lock throttle to zero */ if (_pwm_limit.state == PWM_LIMIT_STATE_RAMP) { if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && - control_index == actuator_controls_s::INDEX_THROTTLE) { + control_index == actuator_controls_s::INDEX_THROTTLE) { /* limit the throttle output to zero during motor spinup, * as the motors cannot follow any demand yet */ @@ -855,7 +870,7 @@ PX4FMU::control_callback(uintptr_t handle, /* throttle not arming - mark throttle input as invalid */ if (arm_nothrottle()) { if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && - control_index == actuator_controls_s::INDEX_THROTTLE) { + control_index == actuator_controls_s::INDEX_THROTTLE) { /* set the throttle to an invalid value */ input = NAN_VALUE; } @@ -973,8 +988,9 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) _num_failsafe_set = 0; for (unsigned i = 0; i < _max_actuators; i++) { - if (_failsafe_pwm[i] > 0) + if (_failsafe_pwm[i] > 0) { _num_failsafe_set++; + } } break; @@ -1021,8 +1037,9 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) _num_disarmed_set = 0; for (unsigned i = 0; i < _max_actuators; i++) { - if (_disarmed_pwm[i] > 0) + if (_disarmed_pwm[i] > 0) { _num_disarmed_set++; + } } break; @@ -1116,12 +1133,14 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) } #ifdef CONFIG_ARCH_BOARD_AEROCORE + case PWM_SERVO_SET(7): case PWM_SERVO_SET(6): if (_mode < MODE_8PWM) { ret = -EINVAL; break; } + #endif case PWM_SERVO_SET(5): @@ -1152,12 +1171,14 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; #ifdef CONFIG_ARCH_BOARD_AEROCORE + case PWM_SERVO_GET(7): case PWM_SERVO_GET(6): if (_mode < MODE_8PWM) { ret = -EINVAL; break; } + #endif case PWM_SERVO_GET(5): @@ -1198,6 +1219,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) case MIXERIOCGETOUTPUTCOUNT: switch (_mode) { #ifdef CONFIG_ARCH_BOARD_AEROCORE + case MODE_8PWM: *(unsigned *)arg = 8; break; @@ -1223,43 +1245,46 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; case PWM_SERVO_SET_COUNT: { - /* change the number of outputs that are enabled for - * PWM. This is used to change the split between GPIO - * and PWM under control of the flight config - * parameters. Note that this does not allow for - * changing a set of pins to be used for serial on - * FMUv1 - */ - switch (arg) { - case 0: - set_mode(MODE_NONE); - break; + /* change the number of outputs that are enabled for + * PWM. This is used to change the split between GPIO + * and PWM under control of the flight config + * parameters. Note that this does not allow for + * changing a set of pins to be used for serial on + * FMUv1 + */ + switch (arg) { + case 0: + set_mode(MODE_NONE); + break; - case 2: - set_mode(MODE_2PWM); - break; + case 2: + set_mode(MODE_2PWM); + break; - case 4: - set_mode(MODE_4PWM); - break; + case 4: + set_mode(MODE_4PWM); + break; #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) - case 6: - set_mode(MODE_6PWM); - break; + + case 6: + set_mode(MODE_6PWM); + break; #endif #if defined(CONFIG_ARCH_BOARD_AEROCORE) - case 8: - set_mode(MODE_8PWM); - break; + + case 8: + set_mode(MODE_8PWM); + break; #endif - default: - ret = -EINVAL; + default: + ret = -EINVAL; + break; + } + break; } - break; - } case MIXERIOCRESET: if (_mixers != nullptr) { @@ -1297,8 +1322,9 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) const char *buf = (const char *)arg; unsigned buflen = strnlen(buf, 1024); - if (_mixers == nullptr) + if (_mixers == nullptr) { _mixers = new MixerGroup(control_callback, (uintptr_t)_controls); + } if (_mixers == nullptr) { _groups_required = 0; @@ -1314,6 +1340,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) _mixers = nullptr; _groups_required = 0; ret = -EINVAL; + } else { _mixers->groups_required(_groups_required); @@ -1344,15 +1371,19 @@ PX4FMU::write(file *filp, const char *buffer, size_t len) uint16_t values[6]; #ifdef CONFIG_ARCH_BOARD_AEROCORE + if (count > 8) { // we have at most 8 outputs count = 8; } + #else + if (count > 6) { // we have at most 6 outputs count = 6; } + #endif // allow for misaligned values @@ -1507,8 +1538,9 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function) gpios |= 3; /* flip the buffer to output mode if required */ - if (GPIO_SET_OUTPUT == function) + if (GPIO_SET_OUTPUT == function) { stm32_gpiowrite(GPIO_GPIO_DIR, 1); + } } #endif @@ -1526,8 +1558,9 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function) break; case GPIO_SET_ALT_1: - if (_gpio_tab[i].alt != 0) + if (_gpio_tab[i].alt != 0) { stm32_configgpio(_gpio_tab[i].alt); + } break; } @@ -1537,8 +1570,9 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function) #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) /* flip buffer to input mode if required */ - if ((GPIO_SET_INPUT == function) && (gpios & 3)) + if ((GPIO_SET_INPUT == function) && (gpios & 3)) { stm32_gpiowrite(GPIO_GPIO_DIR, 0); + } #endif } @@ -1549,8 +1583,9 @@ PX4FMU::gpio_write(uint32_t gpios, int function) int value = (function == GPIO_SET) ? 1 : 0; for (unsigned i = 0; i < _ngpio; i++) - if (gpios & (1 << i)) + if (gpios & (1 << i)) { stm32_gpiowrite(_gpio_tab[i].output, value); + } } uint32_t @@ -1559,8 +1594,9 @@ PX4FMU::gpio_read(void) uint32_t bits = 0; for (unsigned i = 0; i < _ngpio; i++) - if (stm32_gpioread(_gpio_tab[i].input)) + if (stm32_gpioread(_gpio_tab[i].input)) { bits |= (1 << i); + } return bits; } @@ -1664,6 +1700,7 @@ fmu_new_mode(PortMode new_mode) break; #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + case PORT_PWM4: /* select 4-pin PWM mode */ servo_mode = PX4FMU::MODE_4PWM; @@ -1701,8 +1738,9 @@ fmu_new_mode(PortMode new_mode) } /* adjust GPIO config for serial mode(s) */ - if (gpio_bits != 0) + if (gpio_bits != 0) { g_fmu->ioctl(0, GPIO_SET_ALT_1, gpio_bits); + } /* (re)set the PWM output mode */ g_fmu->set_mode(servo_mode); @@ -1801,10 +1839,11 @@ test(void) fd = open(PX4FMU_DEVICE_PATH, O_RDWR); - if (fd < 0) + if (fd < 0) { errx(1, "open fail"); + } - if (ioctl(fd, PWM_SERVO_ARM, 0) < 0) err(1, "servo arm failed"); + if (ioctl(fd, PWM_SERVO_ARM, 0) < 0) { err(1, "servo arm failed"); } if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count) != 0) { err(1, "Unable to get servo count\n"); @@ -1822,8 +1861,9 @@ test(void) /* sweep all servos between 1000..2000 */ servo_position_t servos[servo_count]; - for (unsigned i = 0; i < servo_count; i++) + for (unsigned i = 0; i < servo_count; i++) { servos[i] = pwm_value; + } if (direction == 1) { // use ioctl interface for one direction @@ -1837,8 +1877,9 @@ test(void) // and use write interface for the other direction ret = write(fd, servos, sizeof(servos)); - if (ret != (int)sizeof(servos)) + if (ret != (int)sizeof(servos)) { err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret); + } } if (direction > 0) { @@ -1862,11 +1903,13 @@ test(void) for (unsigned i = 0; i < servo_count; i++) { servo_position_t value; - if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value)) + if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value)) { err(1, "error reading PWM servo %d", i); + } - if (value != servos[i]) + if (value != servos[i]) { errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]); + } } /* Check if user wants to quit */ @@ -1892,8 +1935,9 @@ test(void) void fake(int argc, char *argv[]) { - if (argc < 5) + if (argc < 5) { errx(1, "fmu fake (values -100 .. 100)"); + } actuator_controls_s ac; @@ -1907,8 +1951,9 @@ fake(int argc, char *argv[]) orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac); - if (handle == nullptr) + if (handle == nullptr) { errx(1, "advertise failed"); + } actuator_armed_s aa; @@ -1917,8 +1962,9 @@ fake(int argc, char *argv[]) handle = orb_advertise(ORB_ID(actuator_armed), &aa); - if (handle == nullptr) + if (handle == nullptr) { errx(1, "advertise failed 2"); + } exit(0); } @@ -1948,8 +1994,9 @@ fmu_main(int argc, char *argv[]) } - if (fmu_start() != OK) + if (fmu_start() != OK) { errx(1, "failed to start the FMU driver"); + } /* * Mode switches. @@ -1961,6 +2008,7 @@ fmu_main(int argc, char *argv[]) new_mode = PORT_FULL_PWM; #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + } else if (!strcmp(verb, "mode_pwm4")) { new_mode = PORT_PWM4; #endif @@ -1984,19 +2032,22 @@ fmu_main(int argc, char *argv[]) if (new_mode != PORT_MODE_UNSET) { /* yes but it's the same mode */ - if (new_mode == g_port_mode) + if (new_mode == g_port_mode) { return OK; + } /* switch modes */ int ret = fmu_new_mode(new_mode); exit(ret == OK ? 0 : 1); } - if (!strcmp(verb, "test")) + if (!strcmp(verb, "test")) { test(); + } - if (!strcmp(verb, "fake")) + if (!strcmp(verb, "fake")) { fake(argc - 1, argv + 1); + } if (!strcmp(verb, "sensor_reset")) { if (argc > 2) { @@ -2035,6 +2086,7 @@ fmu_main(int argc, char *argv[]) } exit(0); + } else { warnx("i2c cmd args: "); } @@ -2042,7 +2094,8 @@ fmu_main(int argc, char *argv[]) fprintf(stderr, "FMU: unrecognised command %s, try:\n", verb); #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) - fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test, fake, sensor_reset, id\n"); + fprintf(stderr, + " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test, fake, sensor_reset, id\n"); #elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_AEROCORE) fprintf(stderr, " mode_gpio, mode_pwm, mode_pwm4, test, sensor_reset [milliseconds], i2c \n"); #endif diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index e53bdbd0a4..9a91f35a16 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -228,7 +228,8 @@ public: * * @param[in] enable true=DSM satellite VCC is controlled by relay1, false=DSM satellite VCC not controlled */ - inline void set_dsm_vcc_ctl(bool enable) { + inline void set_dsm_vcc_ctl(bool enable) + { _dsm_vcc_ctl = enable; }; @@ -237,7 +238,8 @@ public: * * @return true=DSM satellite VCC is controlled by relay1, false=DSM satellite VCC not controlled */ - inline bool get_dsm_vcc_ctl() { + inline bool get_dsm_vcc_ctl() + { return _dsm_vcc_ctl; }; #endif @@ -476,8 +478,8 @@ private: void io_handle_vservo(uint16_t vservo, uint16_t vrssi); /* do not allow to copy this class due to ptr data members */ - PX4IO(const PX4IO&); - PX4IO operator=(const PX4IO&); + PX4IO(const PX4IO &); + PX4IO operator=(const PX4IO &); }; namespace @@ -560,11 +562,13 @@ PX4IO::~PX4IO() } /* well, kill it anyway, though this will probably crash */ - if (_task != -1) + if (_task != -1) { task_delete(_task); + } - if (_interface != nullptr) + if (_interface != nullptr) { delete _interface; + } /* deallocate perfs */ perf_free(_perf_update); @@ -584,8 +588,9 @@ PX4IO::detect() /* do regular cdev init */ ret = CDev::init(); - if (ret != OK) + if (ret != OK) { return ret; + } /* get some parameters */ unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); @@ -609,7 +614,8 @@ PX4IO::detect() } int -PX4IO::init(bool rc_handling_disabled) { +PX4IO::init(bool rc_handling_disabled) +{ _rc_handling_disabled = rc_handling_disabled; return init(); } @@ -624,6 +630,7 @@ PX4IO::init() ASSERT(_task == -1); sys_restart_param = param_find("SYS_RESTART_TYPE"); + if (sys_restart_param != PARAM_INVALID) { /* Indicate restart type is unknown */ param_set(sys_restart_param, &sys_restart_val); @@ -632,8 +639,9 @@ PX4IO::init() /* do regular cdev init */ ret = CDev::init(); - if (ret != OK) + if (ret != OK) { return ret; + } /* get some parameters */ unsigned protocol; @@ -672,8 +680,9 @@ PX4IO::init() return -1; } - if (_max_rc_input > input_rc_s::RC_INPUT_MAX_CHANNELS) + if (_max_rc_input > input_rc_s::RC_INPUT_MAX_CHANNELS) { _max_rc_input = input_rc_s::RC_INPUT_MAX_CHANNELS; + } param_get(param_find("RC_RSSI_PWM_CHAN"), &_rssi_pwm_chan); param_get(param_find("RC_RSSI_PWM_MAX"), &_rssi_pwm_max); @@ -691,8 +700,9 @@ PX4IO::init() /* get IO's last seen FMU state */ ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, ®, sizeof(reg)); - if (ret != OK) + if (ret != OK) { return ret; + } /* * in-air restart is only tried if the IO board reports it is @@ -857,11 +867,11 @@ PX4IO::init() /* start the IO interface task */ _task = px4_task_spawn_cmd("px4io", - SCHED_DEFAULT, - SCHED_PRIORITY_ACTUATOR_OUTPUTS, - 1800, - (main_t)&PX4IO::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_ACTUATOR_OUTPUTS, + 1800, + (main_t)&PX4IO::task_main_trampoline, + nullptr); if (_task < 0) { DEVICE_DEBUG("task start failed: %d", errno); @@ -929,11 +939,13 @@ PX4IO::task_main() /* adjust update interval */ if (_update_interval != 0) { - if (_update_interval < UPDATE_INTERVAL_MIN) + if (_update_interval < UPDATE_INTERVAL_MIN) { _update_interval = UPDATE_INTERVAL_MIN; + } - if (_update_interval > 100) + if (_update_interval > 100) { _update_interval = 100; + } orb_set_interval(_t_actuator_controls_0, _update_interval); /* @@ -984,8 +996,9 @@ PX4IO::task_main() orb_check_last = now; /* try to claim the MAVLink log FD */ - if (_mavlink_fd < 0) + if (_mavlink_fd < 0) { _mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); + } /* check updates on uORB topics and handle it */ bool updated = false; @@ -1068,6 +1081,7 @@ PX4IO::task_main() uint16_t failsafe_thr = failsafe_param_val; pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RC_THR_FAILSAFE_US, &failsafe_thr, 1); + if (pret != OK) { mavlink_and_console_log_critical(_mavlink_fd, "failsafe upload failed, FS: %d us", (int)failsafe_thr); } @@ -1119,18 +1133,21 @@ PX4IO::task_main() param_t parm_handle; parm_handle = param_find("TRIM_ROLL"); + if (parm_handle != PARAM_INVALID) { param_get(parm_handle, &trim_val); (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_ROLL, FLOAT_TO_REG(trim_val)); } parm_handle = param_find("TRIM_PITCH"); + if (parm_handle != PARAM_INVALID) { param_get(parm_handle, &trim_val); (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_PITCH, FLOAT_TO_REG(trim_val)); } parm_handle = param_find("TRIM_YAW"); + if (parm_handle != PARAM_INVALID) { param_get(parm_handle, &trim_val); (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_YAW, FLOAT_TO_REG(trim_val)); @@ -1139,17 +1156,22 @@ PX4IO::task_main() /* S.BUS output */ int sbus_mode; parm_handle = param_find("PWM_SBUS_MODE"); + if (parm_handle != PARAM_INVALID) { param_get(parm_handle, &sbus_mode); + if (sbus_mode == 1) { /* enable S.BUS 1 */ (void)io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS1_OUT); + } else if (sbus_mode == 2) { /* enable S.BUS 2 */ (void)io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + } else { /* disable S.BUS */ - (void)io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT), 0); + (void)io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, + (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT), 0); } } } @@ -1165,8 +1187,9 @@ out: DEVICE_DEBUG("exiting"); /* clean up the alternate device node */ - if (_primary_pwm_device) + if (_primary_pwm_device) { unregister_driver(PWM_OUTPUT0_DEVICE_PATH); + } /* tell the dtor that we are exiting */ _task = -1; @@ -1196,8 +1219,7 @@ PX4IO::io_set_control_state(unsigned group) bool changed = false; switch (group) { - case 0: - { + case 0: { orb_check(_t_actuator_controls_0, &changed); if (changed) { @@ -1206,8 +1228,8 @@ PX4IO::io_set_control_state(unsigned group) } } break; - case 1: - { + + case 1: { orb_check(_t_actuator_controls_1, &changed); if (changed) { @@ -1215,8 +1237,8 @@ PX4IO::io_set_control_state(unsigned group) } } break; - case 2: - { + + case 2: { orb_check(_t_actuator_controls_2, &changed); if (changed) { @@ -1224,8 +1246,8 @@ PX4IO::io_set_control_state(unsigned group) } } break; - case 3: - { + + case 3: { orb_check(_t_actuator_controls_3, &changed); if (changed) { @@ -1253,6 +1275,7 @@ PX4IO::io_set_control_state(unsigned group) if (ctrl < -1.0f) { ctrl = -1.0f; + } else if (ctrl > 1.0f) { ctrl = 1.0f; } @@ -1280,14 +1303,17 @@ PX4IO::io_set_arming_state() if (have_armed == OK) { _in_esc_calibration_mode = armed.in_esc_calibration_mode; + if (armed.armed || _in_esc_calibration_mode) { set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; + } else { clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } if (armed.lockdown && !_lockdown_override) { set |= PX4IO_P_SETUP_ARMING_LOCKDOWN; + } else { clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN; } @@ -1295,6 +1321,7 @@ PX4IO::io_set_arming_state() /* Do not set failsafe if circuit breaker is enabled */ if (armed.force_failsafe && !_cb_flighttermination) { set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; + } else { clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; } @@ -1318,6 +1345,7 @@ PX4IO::io_set_arming_state() if (have_control_mode == OK) { if (control_mode.flag_external_manual_override_ok) { set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; + } else { clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; } @@ -1357,8 +1385,9 @@ PX4IO::io_set_rc_config() */ /* fill the mapping with an error condition triggering value */ - for (unsigned i = 0; i < _max_rc_input; i++) + for (unsigned i = 0; i < _max_rc_input; i++) { input_map[i] = UINT8_MAX; + } /* * NOTE: The indices for mapped channels are 1-based @@ -1368,54 +1397,63 @@ PX4IO::io_set_rc_config() /* ROLL */ param_get(param_find("RC_MAP_ROLL"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { input_map[ichan - 1] = 0; } /* PITCH */ param_get(param_find("RC_MAP_PITCH"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { input_map[ichan - 1] = 1; } /* YAW */ param_get(param_find("RC_MAP_YAW"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { input_map[ichan - 1] = 2; } /* THROTTLE */ param_get(param_find("RC_MAP_THROTTLE"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { input_map[ichan - 1] = 3; } /* FLAPS */ param_get(param_find("RC_MAP_FLAPS"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { input_map[ichan - 1] = 4; } /* AUX 1*/ param_get(param_find("RC_MAP_AUX1"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { input_map[ichan - 1] = 5; } /* AUX 2*/ param_get(param_find("RC_MAP_AUX2"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { input_map[ichan - 1] = 6; } /* AUX 3*/ param_get(param_find("RC_MAP_AUX3"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { input_map[ichan - 1] = 7; } /* MAIN MODE SWITCH */ param_get(param_find("RC_MAP_MODE_SW"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { /* use out of normal bounds index to indicate special channel */ input_map[ichan - 1] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH; @@ -1501,7 +1539,8 @@ PX4IO::io_handle_status(uint16_t status) if (_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF && !(status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && !(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { /* set the arming flag */ - ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_SAFETY_OFF | PX4IO_P_STATUS_FLAGS_ARM_SYNC); + ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, + PX4IO_P_STATUS_FLAGS_SAFETY_OFF | PX4IO_P_STATUS_FLAGS_ARM_SYNC); /* set new status */ _status = status; @@ -1552,10 +1591,12 @@ PX4IO::dsm_bind_ioctl(int dsmMode) { if (!(_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { mavlink_log_info(_mavlink_fd, "[IO] binding DSM%s RX", (dsmMode == 0) ? "2" : ((dsmMode == 1) ? "-X" : "-X8")); - int ret = ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : ((dsmMode == 1) ? DSMX_BIND_PULSES : DSMX8_BIND_PULSES)); + int ret = ioctl(nullptr, DSM_BIND_START, + (dsmMode == 0) ? DSM2_BIND_PULSES : ((dsmMode == 1) ? DSMX_BIND_PULSES : DSMX8_BIND_PULSES)); - if (ret) + if (ret) { mavlink_log_critical(_mavlink_fd, "binding failed."); + } } else { mavlink_log_info(_mavlink_fd, "[IO] system armed, bind request rejected"); @@ -1651,8 +1692,9 @@ PX4IO::io_get_status() * in that order */ ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, ®s[0], sizeof(regs) / sizeof(regs[0])); - if (ret != OK) + if (ret != OK) { return ret; + } io_handle_status(regs[0]); io_handle_alarms(regs[1]); @@ -1687,8 +1729,9 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) */ ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, ®s[0], prolog + 9); - if (ret != OK) + if (ret != OK) { return ret; + } /* * Get the channel count any any extra channels. This is no more expensive than reading the @@ -1726,8 +1769,9 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) if (channel_count > 9) { ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, ®s[prolog + 9], channel_count - 9); - if (ret != OK) + if (ret != OK) { return ret; + } } /* last thing set are the actual channel values as 16 bit values */ @@ -1738,7 +1782,7 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) /* get RSSI from input channel */ if (_rssi_pwm_chan > 0 && _rssi_pwm_chan <= input_rc_s::RC_INPUT_MAX_CHANNELS && _rssi_pwm_max - _rssi_pwm_min != 0) { int rssi = (input_rc.values[_rssi_pwm_chan - 1] - _rssi_pwm_min) / - ((_rssi_pwm_max - _rssi_pwm_min) / 100); + ((_rssi_pwm_max - _rssi_pwm_min) / 100); rssi = rssi > 100 ? 100 : rssi; rssi = rssi < 0 ? 0 : rssi; input_rc.rssi = rssi; @@ -1759,8 +1803,9 @@ PX4IO::io_publish_raw_rc() int ret = io_get_raw_rc_input(rc_val); - if (ret != OK) + if (ret != OK) { return ret; + } /* sort out the source of the values */ if (_status & PX4IO_P_STATUS_FLAGS_RC_PPM) { @@ -1809,12 +1854,14 @@ PX4IO::io_publish_pwm_outputs() uint16_t ctl[_max_actuators]; int ret = io_reg_get(PX4IO_PAGE_SERVOS, 0, ctl, _max_actuators); - if (ret != OK) + if (ret != OK) { return ret; + } /* convert from register format to float */ - for (unsigned i = 0; i < _max_actuators; i++) + for (unsigned i = 0; i < _max_actuators; i++) { outputs.output[i] = ctl[i]; + } outputs.noutputs = _max_actuators; @@ -1822,7 +1869,7 @@ PX4IO::io_publish_pwm_outputs() if (_to_outputs == nullptr) { int instance; _to_outputs = orb_advertise_multi(ORB_ID(actuator_outputs), - &outputs, &instance, ORB_PRIO_MAX); + &outputs, &instance, ORB_PRIO_MAX); } else { orb_publish(ORB_ID(actuator_outputs), _to_outputs, &outputs); @@ -1830,17 +1877,19 @@ PX4IO::io_publish_pwm_outputs() /* get mixer status flags from IO */ uint16_t mixer_status; - ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_MIXER, &mixer_status,sizeof(mixer_status)/sizeof(uint16_t)); - memcpy(&motor_limits,&mixer_status,sizeof(motor_limits)); + ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_MIXER, &mixer_status, sizeof(mixer_status) / sizeof(uint16_t)); + memcpy(&motor_limits, &mixer_status, sizeof(motor_limits)); - if (ret != OK) + if (ret != OK) { return ret; + } /* publish mixer status */ - if(_to_mixer_status == nullptr) { + if (_to_mixer_status == nullptr) { _to_mixer_status = orb_advertise(ORB_ID(multirotor_motor_limits), &motor_limits); + } else { - orb_publish(ORB_ID(multirotor_motor_limits),_to_mixer_status, &motor_limits); + orb_publish(ORB_ID(multirotor_motor_limits), _to_mixer_status, &motor_limits); } return OK; @@ -1895,8 +1944,9 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset) { uint16_t value; - if (io_reg_get(page, offset, &value, 1) != OK) + if (io_reg_get(page, offset, &value, 1) != OK) { return _io_reg_get_error; + } return value; } @@ -1909,8 +1959,9 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t ret = io_reg_get(page, offset, &value, 1); - if (ret != OK) + if (ret != OK) { return ret; + } value &= ~clearbits; value |= setbits; @@ -1981,8 +2032,9 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) do { unsigned count = buflen; - if (count > max_len) + if (count > max_len) { count = max_len; + } if (count > 0) { memcpy(&msg->text[0], buf, count); @@ -2015,6 +2067,7 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) if (ret) { usleep(333); + } else { break; } @@ -2050,13 +2103,15 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) if (ret) { usleep(333); + } else { break; } } - if (ret) + if (ret) { return ret; + } retries--; @@ -2087,7 +2142,7 @@ PX4IO::print_status(bool extended_status) io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION), io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC), - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC+1)); + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC + 1)); printf("%u controls %u actuators %u R/C inputs %u analog inputs %u relays\n", io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT), io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT), @@ -2149,22 +2204,26 @@ PX4IO::print_status(bool extended_status) printf("actuators"); - for (unsigned i = 0; i < _max_actuators; i++) + for (unsigned i = 0; i < _max_actuators; i++) { printf(" %hi", int16_t(io_reg_get(PX4IO_PAGE_ACTUATORS, i))); + } printf("\n"); printf("servos"); - for (unsigned i = 0; i < _max_actuators; i++) + for (unsigned i = 0; i < _max_actuators; i++) { printf(" %u", io_reg_get(PX4IO_PAGE_SERVOS, i)); + } uint16_t pwm_invert_mask = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_REVERSE); printf("\n"); printf("reversed outputs: ["); + for (unsigned i = 0; i < _max_actuators; i++) { printf("%s", (pwm_invert_mask & (1 << i)) ? "x" : "_"); } + printf("]"); float trim_roll = REG_TO_FLOAT(io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_ROLL)); @@ -2172,24 +2231,25 @@ PX4IO::print_status(bool extended_status) float trim_yaw = REG_TO_FLOAT(io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_YAW)); printf(" trims: r: %8.4f p: %8.4f y: %8.4f\n", - (double)trim_roll, (double)trim_pitch, (double)trim_yaw); + (double)trim_roll, (double)trim_pitch, (double)trim_yaw); uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); printf("%d raw R/C inputs", raw_inputs); - for (unsigned i = 0; i < raw_inputs; i++) + for (unsigned i = 0; i < raw_inputs; i++) { printf(" %u", io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i)); + } printf("\n"); flags = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_FLAGS); printf("R/C flags: 0x%04x%s%s%s%s%s\n", flags, - (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11))) ? " DSM10" : ""), - (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11)) ? " DSM11" : ""), - ((flags & PX4IO_P_RAW_RC_FLAGS_FRAME_DROP) ? " FRAME_DROP" : ""), - ((flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) ? " FAILSAFE" : ""), - ((flags & PX4IO_P_RAW_RC_FLAGS_MAPPING_OK) ? " MAPPING_OK" : "") - ); + (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11))) ? " DSM10" : ""), + (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11)) ? " DSM11" : ""), + ((flags & PX4IO_P_RAW_RC_FLAGS_FRAME_DROP) ? " FRAME_DROP" : ""), + ((flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) ? " FAILSAFE" : ""), + ((flags & PX4IO_P_RAW_RC_FLAGS_MAPPING_OK) ? " MAPPING_OK" : "") + ); if ((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_PPM)) { int frame_len = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_DATA); @@ -2204,27 +2264,29 @@ PX4IO::print_status(bool extended_status) printf("mapped R/C inputs 0x%04x", mapped_inputs); for (unsigned i = 0; i < _max_rc_input; i++) { - if (mapped_inputs & (1 << i)) + if (mapped_inputs & (1 << i)) { printf(" %u:%d", i, REG_TO_SIGNED(io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_BASE + i))); + } } printf("\n"); uint16_t adc_inputs = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT); printf("ADC inputs"); - for (unsigned i = 0; i < adc_inputs; i++) + for (unsigned i = 0; i < adc_inputs; i++) { printf(" %u", io_reg_get(PX4IO_PAGE_RAW_ADC_INPUT, i)); + } printf("\n"); /* setup and state */ uint16_t features = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES); printf("features 0x%04x%s%s%s%s\n", features, - ((features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) ? " S.BUS1_OUT" : ""), - ((features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) ? " S.BUS2_OUT" : ""), - ((features & PX4IO_P_SETUP_FEATURES_PWM_RSSI) ? " RSSI_PWM" : ""), - ((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "") - ); + ((features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) ? " S.BUS1_OUT" : ""), + ((features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) ? " S.BUS2_OUT" : ""), + ((features & PX4IO_P_SETUP_FEATURES_PWM_RSSI) ? " RSSI_PWM" : ""), + ((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "") + ); uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); printf("arming 0x%04x%s%s%s%s%s%s%s%s%s%s\n", arming, @@ -2238,7 +2300,7 @@ PX4IO::print_status(bool extended_status) ((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) ? " FORCE_FAILSAFE" : ""), ((arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) ? " TERM_FAILSAFE" : ""), ((arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) ? " OVERRIDE_IMMEDIATE" : "") - ); + ); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 printf("rates 0x%04x default %u alt %u relays 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), @@ -2253,11 +2315,13 @@ PX4IO::print_status(bool extended_status) io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE)); #endif printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG)); + for (unsigned group = 0; group < 4; group++) { printf("controls %u:", group); - for (unsigned i = 0; i < _max_controls; i++) + for (unsigned i = 0; i < _max_controls; i++) { printf(" %d", (int16_t) io_reg_get(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT + i)); + } printf("\n"); } @@ -2281,19 +2345,21 @@ PX4IO::print_status(bool extended_status) printf("failsafe"); - for (unsigned i = 0; i < _max_actuators; i++) + for (unsigned i = 0; i < _max_actuators; i++) { printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); + } printf("\ndisarmed values"); - for (unsigned i = 0; i < _max_actuators; i++) + for (unsigned i = 0; i < _max_actuators; i++) { printf(" %u", io_reg_get(PX4IO_PAGE_DISARMED_PWM, i)); + } printf("\n"); } int -PX4IO::ioctl(file * filep, int cmd, unsigned long arg) +PX4IO::ioctl(file *filep, int cmd, unsigned long arg) { int ret = OK; @@ -2359,79 +2425,99 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) break; case PWM_SERVO_SET_FAILSAFE_PWM: { - struct pwm_output_values* pwm = (struct pwm_output_values*)arg; - if (pwm->channel_count > _max_actuators) - /* fail with error */ - return -E2BIG; + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - /* copy values to registers in IO */ - ret = io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm->values, pwm->channel_count); - break; - } + if (pwm->channel_count > _max_actuators) + /* fail with error */ + { + return -E2BIG; + } + + /* copy values to registers in IO */ + ret = io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm->values, pwm->channel_count); + break; + } case PWM_SERVO_GET_FAILSAFE_PWM: - ret = io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, 0, (uint16_t*)arg, _max_actuators); + ret = io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, 0, (uint16_t *)arg, _max_actuators); + if (ret != OK) { ret = -EIO; } + break; case PWM_SERVO_SET_DISARMED_PWM: { - struct pwm_output_values* pwm = (struct pwm_output_values*)arg; - if (pwm->channel_count > _max_actuators) - /* fail with error */ - return -E2BIG; + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - /* copy values to registers in IO */ - ret = io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, pwm->values, pwm->channel_count); - break; - } + if (pwm->channel_count > _max_actuators) + /* fail with error */ + { + return -E2BIG; + } + + /* copy values to registers in IO */ + ret = io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, pwm->values, pwm->channel_count); + break; + } case PWM_SERVO_GET_DISARMED_PWM: - ret = io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, (uint16_t*)arg, _max_actuators); + ret = io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, (uint16_t *)arg, _max_actuators); + if (ret != OK) { ret = -EIO; } + break; case PWM_SERVO_SET_MIN_PWM: { - struct pwm_output_values* pwm = (struct pwm_output_values*)arg; - if (pwm->channel_count > _max_actuators) - /* fail with error */ - return -E2BIG; + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - /* copy values to registers in IO */ - ret = io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, pwm->values, pwm->channel_count); - break; - } + if (pwm->channel_count > _max_actuators) + /* fail with error */ + { + return -E2BIG; + } + + /* copy values to registers in IO */ + ret = io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, pwm->values, pwm->channel_count); + break; + } case PWM_SERVO_GET_MIN_PWM: - ret = io_reg_get(PX4IO_PAGE_CONTROL_MIN_PWM, 0, (uint16_t*)arg, _max_actuators); + ret = io_reg_get(PX4IO_PAGE_CONTROL_MIN_PWM, 0, (uint16_t *)arg, _max_actuators); + if (ret != OK) { ret = -EIO; } + break; case PWM_SERVO_SET_MAX_PWM: { - struct pwm_output_values* pwm = (struct pwm_output_values*)arg; - if (pwm->channel_count > _max_actuators) - /* fail with error */ - return -E2BIG; + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - /* copy values to registers in IO */ - ret = io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, pwm->values, pwm->channel_count); - break; - } + if (pwm->channel_count > _max_actuators) + /* fail with error */ + { + return -E2BIG; + } + + /* copy values to registers in IO */ + ret = io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, pwm->values, pwm->channel_count); + break; + } case PWM_SERVO_GET_MAX_PWM: - ret = io_reg_get(PX4IO_PAGE_CONTROL_MAX_PWM, 0, (uint16_t*)arg, _max_actuators); + ret = io_reg_get(PX4IO_PAGE_CONTROL_MAX_PWM, 0, (uint16_t *)arg, _max_actuators); + if (ret != OK) { ret = -EIO; } + break; case PWM_SERVO_GET_COUNT: @@ -2457,38 +2543,47 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) break; case PWM_SERVO_SET_FORCE_FAILSAFE: + /* force failsafe mode instantly */ if (arg == 0) { /* clear force failsafe flag */ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE, 0); + } else { /* set force failsafe flag */ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE); } + break; case PWM_SERVO_SET_TERMINATION_FAILSAFE: + /* if failsafe occurs, do not allow the system to recover */ if (arg == 0) { /* clear termination failsafe flag */ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE, 0); + } else { /* set termination failsafe flag */ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE); } + break; case PWM_SERVO_SET_OVERRIDE_IMMEDIATE: + /* control whether override on FMU failure is - immediate or waits for override threshold on mode - switch */ + immediate or waits for override threshold on mode + switch */ if (arg == 0) { /* clear override immediate flag */ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE, 0); + } else { /* set override immediate flag */ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE); } + break; case DSM_BIND_START: @@ -2507,9 +2602,11 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart); ret = OK; + } else { ret = -EINVAL; } + break; case DSM_BIND_POWER_UP: @@ -2560,8 +2657,9 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) *(uint32_t *)arg = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + channel); - if (*(uint32_t *)arg == _io_reg_get_error) + if (*(uint32_t *)arg == _io_reg_get_error) { ret = -EIO; + } break; } @@ -2571,8 +2669,9 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) uint32_t bits = (1 << _max_relays) - 1; /* don't touch relay1 if it's controlling RX vcc */ - if (_dsm_vcc_ctl) + if (_dsm_vcc_ctl) { bits &= ~PX4IO_P_SETUP_RELAYS_POWER1; + } ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, bits, 0); #endif @@ -2620,8 +2719,9 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 *(uint32_t *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS); - if (*(uint32_t *)arg == _io_reg_get_error) + if (*(uint32_t *)arg == _io_reg_get_error) { ret = -EIO; + } #endif #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 @@ -2649,8 +2749,9 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &status, 1); - if (ret != OK) + if (ret != OK) { break; + } /* if no R/C input, don't try to fetch anything */ if (!(status & PX4IO_P_STATUS_FLAGS_RC_OK)) { @@ -2686,8 +2787,9 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) break; case PX4IO_REBOOT_BOOTLOADER: - if (system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + if (system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { return -EINVAL; + } /* reboot into bootloader - arg must be PX4IO_REBOOT_BL_MAGIC */ io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_REBOOT_BL, arg); @@ -2696,17 +2798,21 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) break; case PX4IO_CHECK_CRC: { - /* check IO firmware CRC against passed value */ - uint32_t io_crc = 0; - ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC, (uint16_t *)&io_crc, 2); - if (ret != OK) - return ret; - if (io_crc != arg) { - DEVICE_DEBUG("crc mismatch 0x%08x 0x%08x", (unsigned)io_crc, arg); - return -EINVAL; + /* check IO firmware CRC against passed value */ + uint32_t io_crc = 0; + ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC, (uint16_t *)&io_crc, 2); + + if (ret != OK) { + return ret; + } + + if (io_crc != arg) { + DEVICE_DEBUG("crc mismatch 0x%08x 0x%08x", (unsigned)io_crc, arg); + return -EINVAL; + } + + break; } - break; - } case PX4IO_INAIR_RESTART_ENABLE: @@ -2724,6 +2830,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) if (arg) { ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_ADC_RSSI); + } else { ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, PX4IO_P_SETUP_FEATURES_ADC_RSSI, 0); } @@ -2734,6 +2841,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) if (arg) { ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_PWM_RSSI); + } else { ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, PX4IO_P_SETUP_FEATURES_PWM_RSSI, 0); } @@ -2744,39 +2852,45 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) if (arg == 1) { ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS1_OUT); + } else if (arg == 2) { ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + } else { - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT), 0); + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, + (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT), 0); } break; case PWM_SERVO_SET_RC_CONFIG: { - /* enable setting of RC configuration without relying - on param_get() - */ - struct pwm_output_rc_config* config = (struct pwm_output_rc_config*)arg; - if (config->channel >= input_rc_s::RC_INPUT_MAX_CHANNELS) { - /* fail with error */ - return -E2BIG; - } + /* enable setting of RC configuration without relying + on param_get() + */ + struct pwm_output_rc_config *config = (struct pwm_output_rc_config *)arg; - /* copy values to registers in IO */ - uint16_t regs[PX4IO_P_RC_CONFIG_STRIDE]; - uint16_t offset = config->channel * PX4IO_P_RC_CONFIG_STRIDE; - regs[PX4IO_P_RC_CONFIG_MIN] = config->rc_min; - regs[PX4IO_P_RC_CONFIG_CENTER] = config->rc_trim; - regs[PX4IO_P_RC_CONFIG_MAX] = config->rc_max; - regs[PX4IO_P_RC_CONFIG_DEADZONE] = config->rc_dz; - regs[PX4IO_P_RC_CONFIG_ASSIGNMENT] = config->rc_assignment; - regs[PX4IO_P_RC_CONFIG_OPTIONS] = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; - if (config->rc_reverse) { - regs[PX4IO_P_RC_CONFIG_OPTIONS] |= PX4IO_P_RC_CONFIG_OPTIONS_REVERSE; + if (config->channel >= input_rc_s::RC_INPUT_MAX_CHANNELS) { + /* fail with error */ + return -E2BIG; + } + + /* copy values to registers in IO */ + uint16_t regs[PX4IO_P_RC_CONFIG_STRIDE]; + uint16_t offset = config->channel * PX4IO_P_RC_CONFIG_STRIDE; + regs[PX4IO_P_RC_CONFIG_MIN] = config->rc_min; + regs[PX4IO_P_RC_CONFIG_CENTER] = config->rc_trim; + regs[PX4IO_P_RC_CONFIG_MAX] = config->rc_max; + regs[PX4IO_P_RC_CONFIG_DEADZONE] = config->rc_dz; + regs[PX4IO_P_RC_CONFIG_ASSIGNMENT] = config->rc_assignment; + regs[PX4IO_P_RC_CONFIG_OPTIONS] = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + + if (config->rc_reverse) { + regs[PX4IO_P_RC_CONFIG_OPTIONS] |= PX4IO_P_RC_CONFIG_OPTIONS_REVERSE; + } + + ret = io_reg_set(PX4IO_PAGE_RC_CONFIG, offset, regs, PX4IO_P_RC_CONFIG_STRIDE); + break; } - ret = io_reg_set(PX4IO_PAGE_RC_CONFIG, offset, regs, PX4IO_P_RC_CONFIG_STRIDE); - break; - } case PWM_SERVO_SET_OVERRIDE_OK: /* set the 'OVERRIDE OK' bit */ @@ -2803,8 +2917,9 @@ PX4IO::write(file * /*filp*/, const char *buffer, size_t len) { unsigned count = len / 2; - if (count > _max_actuators) + if (count > _max_actuators) { count = _max_actuators; + } if (count > 0) { @@ -2812,8 +2927,9 @@ PX4IO::write(file * /*filp*/, const char *buffer, size_t len) int ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count); perf_end(_perf_write); - if (ret != OK) + if (ret != OK) { return ret; + } } return count * 2; @@ -2858,20 +2974,24 @@ get_interface() #ifndef CONFIG_ARCH_BOARD_PX4FMU_V1 /* try for a serial interface */ - if (PX4IO_serial_interface != nullptr) + if (PX4IO_serial_interface != nullptr) { interface = PX4IO_serial_interface(); + } - if (interface != nullptr) + if (interface != nullptr) { goto got; + } #endif /* try for an I2C interface if we haven't got a serial one */ - if (PX4IO_i2c_interface != nullptr) + if (PX4IO_i2c_interface != nullptr) { interface = PX4IO_i2c_interface(); + } - if (interface != nullptr) + if (interface != nullptr) { goto got; + } errx(1, "cannot alloc interface"); @@ -2888,8 +3008,9 @@ got: void start(int argc, char *argv[]) { - if (g_dev != nullptr) + if (g_dev != nullptr) { errx(0, "already loaded"); + } /* allocate the interface */ device::Device *interface = get_interface(); @@ -2938,8 +3059,9 @@ start(int argc, char *argv[]) void detect(int argc, char *argv[]) { - if (g_dev != nullptr) + if (g_dev != nullptr) { errx(0, "already loaded"); + } /* allocate the interface */ device::Device *interface = get_interface(); @@ -2947,8 +3069,9 @@ detect(int argc, char *argv[]) /* create the driver - it will set g_dev */ (void)new PX4IO(interface); - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "driver alloc failed"); + } int ret = g_dev->detect(); @@ -2976,8 +3099,10 @@ checkcrc(int argc, char *argv[]) /* create the driver - it will set g_dev */ (void)new PX4IO(interface); - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "driver alloc failed"); + } + } else { /* its already running, don't kill the driver */ keep_running = true; @@ -2990,22 +3115,30 @@ checkcrc(int argc, char *argv[]) warnx("usage: px4io checkcrc filename"); exit(1); } + int fd = open(argv[1], O_RDONLY); + if (fd == -1) { warnx("open of %s failed: %d", argv[1], errno); exit(1); } + const uint32_t app_size_max = 0xf000; uint32_t fw_crc = 0; uint32_t nbytes = 0; + while (true) { uint8_t buf[16]; int n = read(fd, buf, sizeof(buf)); - if (n <= 0) break; + + if (n <= 0) { break; } + fw_crc = crc32part(buf, n, fw_crc); nbytes += n; } + close(fd); + while (nbytes < app_size_max) { uint8_t b = 0xff; fw_crc = crc32part(&b, 1, fw_crc); @@ -3023,6 +3156,7 @@ checkcrc(int argc, char *argv[]) warn("check CRC failed: %d", ret); exit(1); } + warnx("CRCs match"); exit(0); } @@ -3032,32 +3166,43 @@ bind(int argc, char *argv[]) { int pulses; - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "px4io must be started first"); + } #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 - if (!g_dev->get_dsm_vcc_ctl()) + if (!g_dev->get_dsm_vcc_ctl()) { errx(1, "DSM bind feature not enabled"); + } #endif - if (argc < 3) + if (argc < 3) { errx(0, "needs argument, use dsm2, dsmx or dsmx8"); + } - if (!strcmp(argv[2], "dsm2")) + if (!strcmp(argv[2], "dsm2")) { pulses = DSM2_BIND_PULSES; - else if (!strcmp(argv[2], "dsmx")) + + } else if (!strcmp(argv[2], "dsmx")) { pulses = DSMX_BIND_PULSES; - else if (!strcmp(argv[2], "dsmx8")) + + } else if (!strcmp(argv[2], "dsmx8")) { pulses = DSMX8_BIND_PULSES; - else + + } else { errx(1, "unknown parameter %s, use dsm2, dsmx or dsmx8", argv[2]); + } + // Test for custom pulse parameter - if (argc > 3) + if (argc > 3) { pulses = atoi(argv[3]); - if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + } + + if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { errx(1, "system must not be armed"); + } #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 warnx("This command will only bind DSM if satellite VCC (red wire) is controlled by relay 1."); @@ -3079,23 +3224,29 @@ test(void) fd = open(PX4IO_DEVICE_PATH, O_WRONLY); - if (fd < 0) + if (fd < 0) { err(1, "failed to open device"); + } - if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count)) + if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count)) { err(1, "failed to get servo count"); + } /* tell IO that its ok to disable its safety with the switch */ ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); - if (ret != OK) + if (ret != OK) { err(1, "PWM_SERVO_SET_ARM_OK"); + } - if (ioctl(fd, PWM_SERVO_ARM, 0)) + if (ioctl(fd, PWM_SERVO_ARM, 0)) { err(1, "failed to arm servos"); + } struct pollfd fds; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; warnx("Press CTRL-C or 'c' to abort."); @@ -3105,13 +3256,15 @@ test(void) /* sweep all servos between 1000..2000 */ servo_position_t servos[servo_count]; - for (unsigned i = 0; i < servo_count; i++) + for (unsigned i = 0; i < servo_count; i++) { servos[i] = pwm_value; + } ret = write(fd, servos, sizeof(servos)); - if (ret != (int)sizeof(servos)) + if (ret != (int)sizeof(servos)) { err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret); + } if (direction > 0) { if (pwm_value < 2000) { @@ -3134,11 +3287,13 @@ test(void) for (unsigned i = 0; i < servo_count; i++) { servo_position_t value; - if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value)) + if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value)) { err(1, "error reading PWM servo %d", i); + } - if (value != servos[i]) + if (value != servos[i]) { errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]); + } } /* Check if user wants to quit */ @@ -3170,6 +3325,7 @@ monitor(void) fds[0].fd = 0; fds[0].events = POLLIN; + if (poll(fds, 1, 2000) < 0) { errx(1, "poll fail"); } @@ -3207,6 +3363,7 @@ if_test(unsigned mode) if (interface) { result = interface->ioctl(1, mode); /* XXX magic numbers */ delete interface; + } else { errx(1, "interface not loaded, exiting"); } @@ -3219,53 +3376,57 @@ lockdown(int argc, char *argv[]) { if (g_dev != nullptr) { - if (argc > 2 && !strcmp(argv[2], "disable")) { + if (argc > 2 && !strcmp(argv[2], "disable")) { - warnx("WARNING: ACTUATORS WILL BE LIVE IN HIL! PROCEED?"); - warnx("Press 'y' to enable, any other key to abort."); + warnx("WARNING: ACTUATORS WILL BE LIVE IN HIL! PROCEED?"); + warnx("Press 'y' to enable, any other key to abort."); - /* check if user wants to abort */ - char c; + /* check if user wants to abort */ + char c; - struct pollfd fds; - int ret; - hrt_abstime start = hrt_absolute_time(); - const unsigned long timeout = 5000000; + struct pollfd fds; + int ret; + hrt_abstime start = hrt_absolute_time(); + const unsigned long timeout = 5000000; - while (hrt_elapsed_time(&start) < timeout) { - fds.fd = 0; /* stdin */ - fds.events = POLLIN; - ret = poll(&fds, 1, 0); + while (hrt_elapsed_time(&start) < timeout) { + fds.fd = 0; /* stdin */ + fds.events = POLLIN; + ret = poll(&fds, 1, 0); - if (ret > 0) { + if (ret > 0) { - if (read(0, &c, 1) > 0) { + if (read(0, &c, 1) > 0) { - if (c != 'y') { - exit(0); - } else if (c == 'y') { - break; - } + if (c != 'y') { + exit(0); + + } else if (c == 'y') { + break; } } - - usleep(10000); } - if (hrt_elapsed_time(&start) > timeout) - errx(1, "TIMEOUT! ABORTED WITHOUT CHANGES."); - - (void)g_dev->ioctl(0, PWM_SERVO_SET_DISABLE_LOCKDOWN, 1); - - warnx("WARNING: ACTUATORS ARE NOW LIVE IN HIL!"); - } else { - (void)g_dev->ioctl(0, PWM_SERVO_SET_DISABLE_LOCKDOWN, 0); - warnx("ACTUATORS ARE NOW SAFE IN HIL."); + usleep(10000); } + if (hrt_elapsed_time(&start) > timeout) { + errx(1, "TIMEOUT! ABORTED WITHOUT CHANGES."); + } + + (void)g_dev->ioctl(0, PWM_SERVO_SET_DISABLE_LOCKDOWN, 1); + + warnx("WARNING: ACTUATORS ARE NOW LIVE IN HIL!"); + } else { - errx(1, "driver not loaded, exiting"); + (void)g_dev->ioctl(0, PWM_SERVO_SET_DISABLE_LOCKDOWN, 0); + warnx("ACTUATORS ARE NOW SAFE IN HIL."); } + + } else { + errx(1, "driver not loaded, exiting"); + } + exit(0); } @@ -3275,17 +3436,21 @@ int px4io_main(int argc, char *argv[]) { /* check for sufficient number of arguments */ - if (argc < 2) + if (argc < 2) { goto out; + } - if (!strcmp(argv[1], "start")) + if (!strcmp(argv[1], "start")) { start(argc - 1, argv + 1); + } - if (!strcmp(argv[1], "detect")) + if (!strcmp(argv[1], "detect")) { detect(argc - 1, argv + 1); + } - if (!strcmp(argv[1], "checkcrc")) + if (!strcmp(argv[1], "checkcrc")) { checkcrc(argc - 1, argv + 1); + } if (!strcmp(argv[1], "update")) { @@ -3349,8 +3514,9 @@ px4io_main(int argc, char *argv[]) } if (!strcmp(argv[1], "iftest")) { - if (g_dev != nullptr) + if (g_dev != nullptr) { errx(1, "can't iftest when started"); + } if_test((argc > 2) ? strtol(argv[2], NULL, 0) : 0); } @@ -3364,6 +3530,7 @@ px4io_main(int argc, char *argv[]) warnx("usage: px4io forceupdate MAGIC filename"); exit(1); } + if (g_dev == nullptr) { warnx("px4io is not started, still attempting upgrade"); @@ -3381,6 +3548,7 @@ px4io_main(int argc, char *argv[]) uint16_t arg = atol(argv[2]); int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg); + if (ret != OK) { warnx("reboot failed - %d", ret); exit(1); @@ -3402,8 +3570,9 @@ px4io_main(int argc, char *argv[]) /* commands below here require a started driver */ - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "not started"); + } if (!strcmp(argv[1], "limit")) { @@ -3412,6 +3581,7 @@ px4io_main(int argc, char *argv[]) if (limitrate > 0) { g_dev->set_update_rate(limitrate); + } else { errx(1, "invalid rate: %d", limitrate); } @@ -3438,19 +3608,23 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "safety_off")) { int ret = g_dev->ioctl(NULL, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0); + if (ret != OK) { warnx("failed to disable safety"); exit(1); } + exit(0); } if (!strcmp(argv[1], "safety_on")) { int ret = g_dev->ioctl(NULL, PWM_SERVO_SET_FORCE_SAFETY_ON, 0); + if (ret != OK) { warnx("failed to enable safety"); exit(1); } + exit(0); } @@ -3512,20 +3686,25 @@ px4io_main(int argc, char *argv[]) !strcmp(argv[1], "rx_dsm_10bit") || !strcmp(argv[1], "rx_dsm_11bit") || !strcmp(argv[1], "rx_sbus") || - !strcmp(argv[1], "rx_ppm")) + !strcmp(argv[1], "rx_ppm")) { errx(0, "receiver type is automatically detected, option '%s' is deprecated", argv[1]); + } - if (!strcmp(argv[1], "test")) + if (!strcmp(argv[1], "test")) { test(); + } - if (!strcmp(argv[1], "monitor")) + if (!strcmp(argv[1], "monitor")) { monitor(); + } - if (!strcmp(argv[1], "bind")) + if (!strcmp(argv[1], "bind")) { bind(argc, argv); + } - if (!strcmp(argv[1], "lockdown")) + if (!strcmp(argv[1], "lockdown")) { lockdown(argc, argv); + } if (!strcmp(argv[1], "sbus1_out")) { /* we can cheat and call the driver directly, as it @@ -3581,6 +3760,6 @@ px4io_main(int argc, char *argv[]) out: errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug ',\n" - "'recovery', 'limit ', 'current', 'bind', 'checkcrc', 'safety_on', 'safety_off',\n" - "'forceupdate', 'update', 'sbus1_out', 'sbus2_out', 'rssi_analog' or 'rssi_pwm'"); + "'recovery', 'limit ', 'current', 'bind', 'checkcrc', 'safety_on', 'safety_off',\n" + "'forceupdate', 'update', 'sbus1_out', 'sbus2_out', 'rssi_analog' or 'rssi_pwm'"); } diff --git a/src/drivers/px4io/px4io_i2c.cpp b/src/drivers/px4io/px4io_i2c.cpp index 4cceb5cf29..574a58f52a 100755 --- a/src/drivers/px4io/px4io_i2c.cpp +++ b/src/drivers/px4io/px4io_i2c.cpp @@ -31,11 +31,11 @@ * ****************************************************************************/ - /** - * @file px4io_i2c.cpp - * - * I2C interface for PX4IO - */ +/** + * @file px4io_i2c.cpp + * + * I2C interface for PX4IO + */ /* XXX trim includes */ #include @@ -94,8 +94,10 @@ PX4IO_I2C::init() int ret; ret = I2C::init(); - if (ret != OK) + + if (ret != OK) { goto out; + } /* XXX really should do something more here */ @@ -133,8 +135,11 @@ PX4IO_I2C::write(unsigned address, void *data, unsigned count) msgv[1].length = 2 * count; int ret = transfer(msgv, 2); - if (ret == OK) + + if (ret == OK) { ret = count; + } + return ret; } @@ -161,8 +166,11 @@ PX4IO_I2C::read(unsigned address, void *data, unsigned count) msgv[1].length = 2 * count; int ret = transfer(msgv, 2); - if (ret == OK) + + if (ret == OK) { ret = count; + } + return ret; } diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp index 566d43fcb7..93f55f9486 100644 --- a/src/drivers/px4io/px4io_serial.cpp +++ b/src/drivers/px4io/px4io_serial.cpp @@ -31,11 +31,11 @@ * ****************************************************************************/ - /** - * @file px4io_serial.cpp - * - * Serial interface for PX4IO - */ +/** + * @file px4io_serial.cpp + * + * Serial interface for PX4IO + */ /* XXX trim includes */ #include @@ -159,7 +159,7 @@ private: /* do not allow top copying this class */ PX4IO_serial(PX4IO_serial &); - PX4IO_serial& operator = (const PX4IO_serial &); + PX4IO_serial &operator = (const PX4IO_serial &); }; @@ -199,6 +199,7 @@ PX4IO_serial::~PX4IO_serial() stm32_dmastop(_tx_dma); stm32_dmafree(_tx_dma); } + if (_rx_dma != nullptr) { stm32_dmastop(_rx_dma); stm32_dmafree(_rx_dma); @@ -232,8 +233,9 @@ PX4IO_serial::~PX4IO_serial() perf_free(_pc_idle); perf_free(_pc_badidle); - if (g_interface == this) + if (g_interface == this) { g_interface = nullptr; + } } int @@ -243,6 +245,7 @@ PX4IO_serial::init() /* allocate DMA */ _tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP); _rx_dma = stm32_dmachannel(PX4IO_SERIAL_RX_DMAMAP); + if ((_tx_dma == nullptr) || (_rx_dma == nullptr)) { return -1; } @@ -305,19 +308,22 @@ PX4IO_serial::ioctl(unsigned operation, unsigned &arg) for (;;) { while (!(rSR & USART_SR_TXE)) ; + rDR = 0x55; } + return 0; - case 1: - { + case 1: { unsigned fails = 0; + for (unsigned count = 0;; count++) { uint16_t value = count & 0xffff; - if (write((PX4IO_PAGE_TEST << 8) | PX4IO_P_TEST_LED, &value, 1) != 0) + if (write((PX4IO_PAGE_TEST << 8) | PX4IO_P_TEST_LED, &value, 1) != 0) { fails++; - + } + if (count >= 5000) { lowsyslog("==== test 1 : %u failures ====\n", fails); perf_print_counter(_pc_txns); @@ -333,12 +339,15 @@ PX4IO_serial::ioctl(unsigned operation, unsigned &arg) count = 0; } } + return 0; } + case 2: lowsyslog("test 2\n"); return 0; } + default: break; } @@ -353,20 +362,24 @@ PX4IO_serial::write(unsigned address, void *data, unsigned count) uint8_t offset = address & 0xff; const uint16_t *values = reinterpret_cast(data); - if (count > PKT_MAX_REGS) + if (count > PKT_MAX_REGS) { return -EINVAL; + } sem_wait(&_bus_semaphore); int result; + for (unsigned retries = 0; retries < 3; retries++) { _dma_buffer.count_code = count | PKT_CODE_WRITE; _dma_buffer.page = page; _dma_buffer.offset = offset; memcpy((void *)&_dma_buffer.regs[0], (void *)values, (2 * count)); - for (unsigned i = count; i < PKT_MAX_REGS; i++) + + for (unsigned i = count; i < PKT_MAX_REGS; i++) { _dma_buffer.regs[i] = 0x55aa; + } /* XXX implement check byte */ @@ -386,13 +399,16 @@ PX4IO_serial::write(unsigned address, void *data, unsigned count) break; } + perf_count(_pc_retries); } sem_post(&_bus_semaphore); - if (result == OK) + if (result == OK) { result = count; + } + return result; } @@ -403,12 +419,14 @@ PX4IO_serial::read(unsigned address, void *data, unsigned count) uint8_t offset = address & 0xff; uint16_t *values = reinterpret_cast(data); - if (count > PKT_MAX_REGS) + if (count > PKT_MAX_REGS) { return -EINVAL; + } sem_wait(&_bus_semaphore); int result; + for (unsigned retries = 0; retries < 3; retries++) { _dma_buffer.count_code = count | PKT_CODE_READ; @@ -428,14 +446,16 @@ PX4IO_serial::read(unsigned address, void *data, unsigned count) result = -EINVAL; perf_count(_pc_protoerrs); - /* compare the received count with the expected count */ + /* compare the received count with the expected count */ + } else if (PKT_COUNT(_dma_buffer) != count) { /* IO returned the wrong number of registers - no point retrying */ result = -EIO; perf_count(_pc_protoerrs); - /* successful read */ + /* successful read */ + } else { /* copy back the result */ @@ -444,13 +464,16 @@ PX4IO_serial::read(unsigned address, void *data, unsigned count) break; } + perf_count(_pc_retries); } sem_post(&_bus_semaphore); - if (result == OK) + if (result == OK) { result = count; + } + return result; } @@ -517,14 +540,16 @@ PX4IO_serial::_wait_complete() /* compute the deadline for a 10ms timeout */ struct timespec abstime; clock_gettime(CLOCK_REALTIME, &abstime); - abstime.tv_nsec += 10*1000*1000; - if (abstime.tv_nsec >= 1000*1000*1000) { + abstime.tv_nsec += 10 * 1000 * 1000; + + if (abstime.tv_nsec >= 1000 * 1000 * 1000) { abstime.tv_sec++; - abstime.tv_nsec -= 1000*1000*1000; + abstime.tv_nsec -= 1000 * 1000 * 1000; } /* wait for the transaction to complete - 64 bytes @ 1.5Mbps ~426µs */ int ret; + for (;;) { ret = sem_timedwait(&_completion_semaphore, &abstime); @@ -539,6 +564,7 @@ PX4IO_serial::_wait_complete() /* check packet CRC - corrupt packet errors mean IO receive CRC error */ uint8_t crc = _dma_buffer.crc; _dma_buffer.crc = 0; + if ((crc != crc_packet(&_dma_buffer)) | (PKT_CODE(_dma_buffer) == PKT_CODE_CORRUPT)) { perf_count(_pc_crcerrs); ret = -EIO; @@ -588,6 +614,7 @@ PX4IO_serial::_do_rx_dma_callback(unsigned status) /* check for packet overrun - this will occur after DMA completes */ uint32_t sr = rSR; + if (sr & (USART_SR_ORE | USART_SR_RXNE)) { (void)rDR; status = DMA_STATUS_TEIF; @@ -607,8 +634,10 @@ PX4IO_serial::_do_rx_dma_callback(unsigned status) int PX4IO_serial::_interrupt(int irq, void *context) { - if (g_interface != nullptr) + if (g_interface != nullptr) { g_interface->_do_interrupt(); + } + return 0; } @@ -619,10 +648,10 @@ PX4IO_serial::_do_interrupt() (void)rDR; /* read DR to clear status */ if (sr & (USART_SR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */ - USART_SR_NE | /* noise error - we have lost a byte due to noise */ - USART_SR_FE)) { /* framing error - start/stop bit lost or line break */ - - /* + USART_SR_NE | /* noise error - we have lost a byte due to noise */ + USART_SR_FE)) { /* framing error - start/stop bit lost or line break */ + + /* * If we are in the process of listening for something, these are all fatal; * abort the DMA with an error. */ @@ -649,6 +678,7 @@ PX4IO_serial::_do_interrupt() /* verify that the received packet is complete */ size_t length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma); + if ((length < 1) || (length < PKT_SIZE(_dma_buffer))) { perf_count(_pc_badidle); diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index 027253905f..70337c5a0c 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -126,8 +126,10 @@ PX4IO_Uploader::upload(const char *filenames[]) /* look for the bootloader for 150 ms */ for (int i = 0; i < 15; i++) { ret = sync(); + if (ret == OK) { break; + } else { usleep(10000); } @@ -143,6 +145,7 @@ PX4IO_Uploader::upload(const char *filenames[]) } struct stat st; + if (stat(filename, &st) != 0) { log("Failed to stat %s - %d\n", filename, (int)errno); tcsetattr(_io_fd, TCSANOW, &t_original); @@ -150,6 +153,7 @@ PX4IO_Uploader::upload(const char *filenames[]) _io_fd = -1; return -errno; } + fw_size = st.st_size; if (_fw_fd == -1) { @@ -180,6 +184,7 @@ PX4IO_Uploader::upload(const char *filenames[]) if (ret == OK) { if (bl_rev <= BL_REV) { log("found bootloader revision: %d", bl_rev); + } else { log("found unsupported bootloader revision %d, exiting", bl_rev); tcsetattr(_io_fd, TCSANOW, &t_original); @@ -205,6 +210,7 @@ PX4IO_Uploader::upload(const char *filenames[]) if (bl_rev <= 2) { ret = verify_rev2(fw_size); + } else { /* verify rev 3 and higher. Every version *needs* to be verified. */ ret = verify_rev3(fw_size); @@ -240,7 +246,7 @@ PX4IO_Uploader::upload(const char *filenames[]) // sleep for enough time for the IO chip to boot. This makes // forceupdate more reliably startup IO again after update - up_udelay(100*1000); + up_udelay(100 * 1000); return ret; } @@ -274,12 +280,15 @@ int PX4IO_Uploader::recv_bytes(uint8_t *p, unsigned count) { int ret = OK; + while (count--) { ret = recv_byte_with_timeout(p++, 5000); - if (ret != OK) + if (ret != OK) { break; + } } + return ret; } @@ -296,9 +305,11 @@ PX4IO_Uploader::drain() ret = recv_byte_with_timeout(&c, 40); #ifdef UDEBUG + if (ret == OK) { log("discard 0x%02x", c); } + #endif } while (ret == OK); } @@ -309,8 +320,11 @@ PX4IO_Uploader::send(uint8_t c) #ifdef UDEBUG log("send 0x%02x", c); #endif - if (write(_io_fd, &c, 1) != 1) + + if (write(_io_fd, &c, 1) != 1) { return -errno; + } + return OK; } @@ -318,11 +332,15 @@ int PX4IO_Uploader::send(uint8_t *p, unsigned count) { int ret; + while (count--) { ret = send(*p++); - if (ret != OK) + + if (ret != OK) { break; + } } + return ret; } @@ -334,13 +352,15 @@ PX4IO_Uploader::get_sync(unsigned timeout) ret = recv_byte_with_timeout(c, timeout); - if (ret != OK) + if (ret != OK) { return ret; + } ret = recv_byte_with_timeout(c + 1, timeout); - if (ret != OK) + if (ret != OK) { return ret; + } if ((c[0] != PROTO_INSYNC) || (c[1] != PROTO_OK)) { log("bad sync 0x%02x,0x%02x", c[0], c[1]); @@ -356,8 +376,9 @@ PX4IO_Uploader::sync() drain(); /* complete any pending program operation */ - for (unsigned i = 0; i < (PROG_MULTI_MAX + 6); i++) + for (unsigned i = 0; i < (PROG_MULTI_MAX + 6); i++) { send(0); + } send(PROTO_GET_SYNC); send(PROTO_EOC); @@ -375,8 +396,9 @@ PX4IO_Uploader::get_info(int param, uint32_t &val) ret = recv_bytes((uint8_t *)&val, sizeof(val)); - if (ret != OK) + if (ret != OK) { return ret; + } return get_sync(); } @@ -395,14 +417,17 @@ static int read_with_retry(int fd, void *buf, size_t n) { int ret; uint8_t retries = 0; + do { ret = read(fd, buf, n); } while (ret == -1 && retries++ < 100); + if (retries != 0) { printf("read of %u bytes needed %u retries\n", (unsigned)n, (unsigned)retries); } + return ret; } @@ -415,6 +440,7 @@ PX4IO_Uploader::program(size_t fw_size) size_t sent = 0; file_buf = new uint8_t[PROG_MULTI_MAX]; + if (!file_buf) { log("Can't allocate program buffer"); return -ENOMEM; @@ -430,13 +456,15 @@ PX4IO_Uploader::program(size_t fw_size) while (sent < fw_size) { /* get more bytes to program */ size_t n = fw_size - sent; + if (n > PROG_MULTI_MAX) { n = PROG_MULTI_MAX; } + count = read_with_retry(_fw_fd, file_buf, n); if (count != (ssize_t)n) { - log("firmware read of %u bytes at %u failed -> %d errno %d", + log("firmware read of %u bytes at %u failed -> %d errno %d", (unsigned)n, (unsigned)sent, (int)count, @@ -478,32 +506,37 @@ PX4IO_Uploader::verify_rev2(size_t fw_size) send(PROTO_EOC); ret = get_sync(); - if (ret != OK) + if (ret != OK) { return ret; + } while (sent < fw_size) { /* get more bytes to verify */ size_t n = fw_size - sent; + if (n > sizeof(file_buf)) { n = sizeof(file_buf); } + count = read_with_retry(_fw_fd, file_buf, n); if (count != (ssize_t)n) { - log("firmware read of %u bytes at %u failed -> %d errno %d", + log("firmware read of %u bytes at %u failed -> %d errno %d", (unsigned)n, (unsigned)sent, (int)count, (int)errno); } - if (count == 0) + if (count == 0) { break; + } sent += count; - if (count < 0) + if (count < 0) { return -errno; + } ASSERT((count % 4) == 0); @@ -564,13 +597,15 @@ PX4IO_Uploader::verify_rev3(size_t fw_size_local) /* read through the firmware file again and calculate the checksum*/ while (bytes_read < fw_size_local) { size_t n = fw_size_local - bytes_read; + if (n > sizeof(file_buf)) { n = sizeof(file_buf); } + count = read_with_retry(_fw_fd, file_buf, n); if (count != (ssize_t)n) { - log("firmware read of %u bytes at %u failed -> %d errno %d", + log("firmware read of %u bytes at %u failed -> %d errno %d", (unsigned)n, (unsigned)bytes_read, (int)count, @@ -581,9 +616,11 @@ PX4IO_Uploader::verify_rev3(size_t fw_size_local) if (count == 0) { break; } + /* stop if the file cannot be read */ - if (count < 0) + if (count < 0) { return -errno; + } /* calculate crc32 sum */ sum = crc32part((uint8_t *)&file_buf, sizeof(file_buf), sum); @@ -601,7 +638,7 @@ PX4IO_Uploader::verify_rev3(size_t fw_size_local) send(PROTO_GET_CRC); send(PROTO_EOC); - ret = recv_bytes((uint8_t*)(&crc), sizeof(crc)); + ret = recv_bytes((uint8_t *)(&crc), sizeof(crc)); if (ret != OK) { log("did not receive CRC checksum"); @@ -621,7 +658,7 @@ int PX4IO_Uploader::reboot() { send(PROTO_REBOOT); - up_udelay(100*1000); // Ensure the farend is in wait for char. + up_udelay(100 * 1000); // Ensure the farend is in wait for char. send(PROTO_EOC); return OK; diff --git a/src/drivers/rgbled/module.mk b/src/drivers/rgbled/module.mk index 924624cebb..f4f4e41427 100644 --- a/src/drivers/rgbled/module.mk +++ b/src/drivers/rgbled/module.mk @@ -4,6 +4,7 @@ MODULE_COMMAND = rgbled -SRCS = rgbled.cpp +SRCS = rgbled.cpp \ + rgbled_params.c MAXOPTIMIZATION = -Os diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 8b5be151de..d379b20b60 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -101,11 +101,13 @@ private: uint8_t _g; uint8_t _b; float _brightness; + float _max_brightness; bool _running; int _led_interval; bool _should_run; int _counter; + int _param_sub; void set_color(rgbled_color_t ledcolor); void set_mode(rgbled_mode_t mode); @@ -117,6 +119,7 @@ private: int send_led_enable(bool enable); int send_led_rgb(); int get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b); + void update_params(); }; /* for now, we only support one RGBLED */ @@ -140,10 +143,12 @@ RGBLED::RGBLED(int bus, int rgbled) : _g(0), _b(0), _brightness(1.0f), + _max_brightness(1.0f), _running(false), _led_interval(0), _should_run(false), - _counter(0) + _counter(0), + _param_sub(-1) { memset(&_work, 0, sizeof(_work)); memset(&_pattern, 0, sizeof(_pattern)); @@ -285,6 +290,22 @@ RGBLED::led() return; } + if (_param_sub < 0) { + _param_sub = orb_subscribe(ORB_ID(parameter_update)); + } + + if (_param_sub >= 0) { + bool updated = false; + orb_check(_param_sub, &updated); + if (updated) { + parameter_update_s pupdate; + orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate); + update_params(); + // Immediately update to change brightness + send_led_rgb(); + } + } + switch (_mode) { case RGBLED_MODE_BLINK_SLOW: case RGBLED_MODE_BLINK_NORMAL: @@ -546,9 +567,9 @@ RGBLED::send_led_rgb() { /* To scale from 0..255 -> 0..15 shift right by 4 bits */ const uint8_t msg[6] = { - SUB_ADDR_PWM0, (uint8_t)((int)(_b * _brightness) >> 4), - SUB_ADDR_PWM1, (uint8_t)((int)(_g * _brightness) >> 4), - SUB_ADDR_PWM2, (uint8_t)((int)(_r * _brightness) >> 4) + SUB_ADDR_PWM0, static_cast((_b >> 4) * _brightness * _max_brightness + 0.5f), + SUB_ADDR_PWM1, static_cast((_g >> 4) * _brightness * _max_brightness + 0.5f), + SUB_ADDR_PWM2, static_cast((_r >> 4) * _brightness * _max_brightness + 0.5f) }; return transfer(msg, sizeof(msg), nullptr, 0); } @@ -573,6 +594,21 @@ RGBLED::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b) return ret; } +void +RGBLED::update_params() +{ + int32_t maxbrt = 15; + param_get(param_find("LED_RGB_MAXBRT"), &maxbrt); + maxbrt = maxbrt > 15 ? 15 : maxbrt; + maxbrt = maxbrt < 0 ? 0 : maxbrt; + + // A minimum of 2 "on" steps is required for breathe effect + if (maxbrt == 1) { + maxbrt = 2; + } + _max_brightness = maxbrt / 15.0f; +} + void rgbled_usage() { diff --git a/src/modules/uavcan/uavcan_clock.cpp b/src/drivers/rgbled/rgbled_params.c similarity index 69% rename from src/modules/uavcan/uavcan_clock.cpp rename to src/drivers/rgbled/rgbled_params.c index fe8ba406a5..5ed2c9de38 100644 --- a/src/modules/uavcan/uavcan_clock.cpp +++ b/src/drivers/rgbled/rgbled_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,51 +31,24 @@ * ****************************************************************************/ -#include -#include - -/** - * @file uavcan_clock.cpp +/* + * @file rgbled_params.c * - * Implements a clock for the CAN node. + * Parameters defined by the RBG led driver * - * @author Pavel Kirienko + * @author Nate Weibley */ -namespace uavcan_stm32 -{ -namespace clock -{ -uavcan::MonotonicTime getMonotonic() -{ - return uavcan::MonotonicTime::fromUSec(hrt_absolute_time()); -} - -uavcan::UtcTime getUtc() -{ - return uavcan::UtcTime(); -} - -void adjustUtc(uavcan::UtcDuration adjustment) -{ - (void)adjustment; -} - -uavcan::uint64_t getUtcUSecFromCanInterrupt(); - -uavcan::uint64_t getUtcUSecFromCanInterrupt() -{ - return 0; -} - -} // namespace clock - -SystemClock &SystemClock::instance() -{ - static SystemClock inst; - return inst; -} - -} +#include +#include +/** + * RGB Led brightness limit + * + * Set to 0 to disable, 1 for minimum brightness up to 15 (max) + * + * @min 0 + * @max 15 + */ +PARAM_DEFINE_INT32(LED_RGB_MAXBRT, 15); diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index a6be9d5c24..ab458895a6 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -115,7 +115,7 @@ protected: private: static const hrt_abstime _tickrate = 10000; /**< 100Hz base rate */ - + hrt_call _call; perf_counter_t _sample_perf; @@ -161,11 +161,13 @@ ADC::ADC(uint32_t channels) : _channel_count++; } } + _samples = new adc_msg_s[_channel_count]; /* prefill the channel numbers in the sample array */ if (_samples != nullptr) { unsigned index = 0; + for (unsigned i = 0; i < 32; i++) { if (channels & (1 << i)) { _samples[index].am_channel = i; @@ -178,8 +180,9 @@ ADC::ADC(uint32_t channels) : ADC::~ADC() { - if (_samples != nullptr) + if (_samples != nullptr) { delete _samples; + } } int @@ -189,8 +192,11 @@ ADC::init() #ifdef ADC_CR2_CAL rCR2 |= ADC_CR2_CAL; usleep(100); - if (rCR2 & ADC_CR2_CAL) + + if (rCR2 & ADC_CR2_CAL) { return -1; + } + #endif /* arbitrarily configure all channels for 55 cycle sample time */ @@ -201,7 +207,7 @@ ADC::init() rCR1 = 0; /* enable the temperature sensor / Vrefint channel if supported*/ - rCR2 = + rCR2 = #ifdef ADC_CR2_TSVREFE /* enable the temperature sensor in CR2 */ ADC_CR2_TSVREFE | @@ -216,7 +222,7 @@ ADC::init() /* configure for a single-channel sequence */ rSQR1 = 0; rSQR2 = 0; - rSQR3 = 0; /* will be updated with the channel each tick */ + rSQR3 = 0; /* will be updated with the channel each tick */ /* power-cycle the ADC and turn it on */ rCR2 &= ~ADC_CR2_ADON; @@ -229,6 +235,7 @@ ADC::init() /* kick off a sample and wait for it to complete */ hrt_abstime now = hrt_absolute_time(); rCR2 |= ADC_CR2_SWSTART; + while (!(rSR & ADC_SR_EOC)) { /* don't wait for more than 500us, since that means something broke - should reset here if we see this */ @@ -256,8 +263,9 @@ ADC::read(file *filp, char *buffer, size_t len) { const size_t maxsize = sizeof(adc_msg_s) * _channel_count; - if (len > maxsize) + if (len > maxsize) { len = maxsize; + } /* block interrupts while copying samples to avoid racing with an update */ irqstate_t flags = irqsave(); @@ -296,8 +304,10 @@ void ADC::_tick() { /* scan the channel set and sample each */ - for (unsigned i = 0; i < _channel_count; i++) + for (unsigned i = 0; i < _channel_count; i++) { _samples[i].am_data = _sample(_samples[i].am_channel); + } + update_system_power(); } @@ -309,6 +319,7 @@ ADC::update_system_power(void) system_power.timestamp = hrt_absolute_time(); system_power.voltage5V_v = 0; + for (unsigned i = 0; i < _channel_count; i++) { if (_samples[i].am_channel == 4) { // it is 2:1 scaled @@ -331,9 +342,11 @@ ADC::update_system_power(void) /* lazily publish */ if (_to_system_power != nullptr) { orb_publish(ORB_ID(system_power), _to_system_power, &system_power); + } else { _to_system_power = orb_advertise(ORB_ID(system_power), &system_power); } + #endif // CONFIG_ARCH_BOARD_PX4FMU_V2 } @@ -343,8 +356,9 @@ ADC::_sample(unsigned channel) perf_begin(_sample_perf); /* clear any previous EOC */ - if (rSR & ADC_SR_EOC) + if (rSR & ADC_SR_EOC) { rSR &= ~ADC_SR_EOC; + } /* run a single conversion right now - should take about 60 cycles (a few microseconds) max */ rSQR3 = channel; @@ -352,6 +366,7 @@ ADC::_sample(unsigned channel) /* wait for the conversion to complete */ hrt_abstime now = hrt_absolute_time(); + while (!(rSR & ADC_SR_EOC)) { /* don't wait for more than 50us, since that means something broke - should reset here if we see this */ @@ -382,20 +397,23 @@ test(void) { int fd = open(ADC0_DEVICE_PATH, O_RDONLY); - if (fd < 0) + + if (fd < 0) { err(1, "can't open ADC device"); + } for (unsigned i = 0; i < 50; i++) { adc_msg_s data[12]; ssize_t count = read(fd, data, sizeof(data)); - if (count < 0) + if (count < 0) { errx(1, "read error"); + } unsigned channels = count / sizeof(data[0]); for (unsigned j = 0; j < channels; j++) { - printf ("%d: %u ", data[j].am_channel, data[j].am_data); + printf("%d: %u ", data[j].am_channel, data[j].am_data); } printf("\n"); @@ -410,11 +428,12 @@ int adc_main(int argc, char *argv[]) { if (g_adc == nullptr) { - /* XXX this hardcodes the default channel set for the board in board_config.h - should be configurable */ - g_adc = new ADC(ADC_CHANNELS); + /* XXX this hardcodes the default channel set for the board in board_config.h - should be configurable */ + g_adc = new ADC(ADC_CHANNELS); - if (g_adc == nullptr) + if (g_adc == nullptr) { errx(1, "couldn't allocate the ADC driver"); + } if (g_adc->init() != OK) { delete g_adc; @@ -423,8 +442,9 @@ adc_main(int argc, char *argv[]) } if (argc > 1) { - if (!strcmp(argv[1], "test")) + if (!strcmp(argv[1], "test")) { test(); + } } exit(0); diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index 74c81b7a0e..ba148c1335 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -455,8 +455,9 @@ hrt_ppm_decode(uint32_t status) unsigned i; /* if we missed an edge, we have to give up */ - if (status & SR_OVF_PPM) + if (status & SR_OVF_PPM) { goto error; + } /* how long since the last edge? - this handles counter wrapping implicitely. */ width = count - ppm.last_edge; @@ -464,8 +465,10 @@ hrt_ppm_decode(uint32_t status) #if PPM_DEBUG ppm_edge_history[ppm_edge_next++] = width; - if (ppm_edge_next >= 32) + if (ppm_edge_next >= 32) { ppm_edge_next = 0; + } + #endif /* @@ -501,8 +504,9 @@ hrt_ppm_decode(uint32_t status) } else { /* frame channel count matches expected, let's use it */ if (ppm.next_channel > PPM_MIN_CHANNELS) { - for (i = 0; i < ppm.next_channel; i++) + for (i = 0; i < ppm.next_channel; i++) { ppm_buffer[i] = ppm_temp_buffer[i]; + } ppm_last_valid_decode = hrt_absolute_time(); @@ -527,8 +531,9 @@ hrt_ppm_decode(uint32_t status) case ARM: /* we expect a pulse giving us the first mark */ - if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH) - goto error; /* pulse was too short or too long */ + if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH) { + goto error; /* pulse was too short or too long */ + } /* record the mark timing, expect an inactive edge */ ppm.last_mark = ppm.last_edge; @@ -542,8 +547,9 @@ hrt_ppm_decode(uint32_t status) case INACTIVE: /* we expect a short pulse */ - if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH) - goto error; /* pulse was too short or too long */ + if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH) { + goto error; /* pulse was too short or too long */ + } /* this edge is not interesting, but now we are ready for the next mark */ ppm.phase = ACTIVE; @@ -557,17 +563,21 @@ hrt_ppm_decode(uint32_t status) #if PPM_DEBUG ppm_pulse_history[ppm_pulse_next++] = interval; - if (ppm_pulse_next >= 32) + if (ppm_pulse_next >= 32) { ppm_pulse_next = 0; + } + #endif /* if the mark-mark timing is out of bounds, abandon the frame */ - if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE)) + if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE)) { goto error; + } /* if we have room to store the value, do so */ - if (ppm.next_channel < PPM_MAX_CHANNELS) + if (ppm.next_channel < PPM_MAX_CHANNELS) { ppm_temp_buffer[ppm.next_channel++] = interval; + } ppm.phase = INACTIVE; break; @@ -668,8 +678,9 @@ hrt_absolute_time(void) * This simple test is sufficient due to the guarantee that * we are always called at least once per counter period. */ - if (count < last_count) + if (count < last_count) { base_time += HRT_COUNTER_PERIOD; + } /* save the count for next time */ last_count = count; @@ -800,8 +811,9 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte queue for the uninitialised entry->link but we don't do anything actually unsafe. */ - if (entry->deadline != 0) + if (entry->deadline != 0) { sq_rem(&entry->link, &callout_queue); + } entry->deadline = deadline; entry->period = interval; @@ -883,11 +895,13 @@ hrt_call_invoke(void) call = (struct hrt_call *)sq_peek(&callout_queue); - if (call == NULL) + if (call == NULL) { break; + } - if (call->deadline > now) + if (call->deadline > now) { break; + } sq_rem(&call->link, &callout_queue); //lldbg("call pop\n"); diff --git a/src/drivers/stm32/drv_pwm_servo.c b/src/drivers/stm32/drv_pwm_servo.c index 77f5387749..3a24f19922 100644 --- a/src/drivers/stm32/drv_pwm_servo.c +++ b/src/drivers/stm32/drv_pwm_servo.c @@ -174,19 +174,22 @@ pwm_channel_init(unsigned channel) int up_pwm_servo_set(unsigned channel, servo_position_t value) { - if (channel >= PWM_SERVO_MAX_CHANNELS) + if (channel >= PWM_SERVO_MAX_CHANNELS) { return -1; + } unsigned timer = pwm_channels[channel].timer_index; /* test timer for validity */ if ((pwm_timers[timer].base == 0) || - (pwm_channels[channel].gpio == 0)) + (pwm_channels[channel].gpio == 0)) { return -1; + } /* configure the channel */ - if (value > 0) + if (value > 0) { value--; + } switch (pwm_channels[channel].timer_channel) { case 1: @@ -215,16 +218,18 @@ up_pwm_servo_set(unsigned channel, servo_position_t value) servo_position_t up_pwm_servo_get(unsigned channel) { - if (channel >= PWM_SERVO_MAX_CHANNELS) + if (channel >= PWM_SERVO_MAX_CHANNELS) { return 0; + } unsigned timer = pwm_channels[channel].timer_index; servo_position_t value = 0; /* test timer for validity */ if ((pwm_timers[timer].base == 0) || - (pwm_channels[channel].timer_channel == 0)) + (pwm_channels[channel].timer_channel == 0)) { return 0; + } /* configure the channel */ switch (pwm_channels[channel].timer_channel) { @@ -253,15 +258,17 @@ up_pwm_servo_init(uint32_t channel_mask) { /* do basic timer initialisation first */ for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) { - if (pwm_timers[i].base != 0) + if (pwm_timers[i].base != 0) { pwm_timer_init(i); + } } /* now init channels */ for (unsigned i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) { /* don't do init for disabled channels; this leaves the pin configs alone */ - if (((1 << i) & channel_mask) && (pwm_channels[i].timer_channel != 0)) + if (((1 << i) & channel_mask) && (pwm_channels[i].timer_channel != 0)) { pwm_channel_init(i); + } } return OK; @@ -278,13 +285,17 @@ int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate) { /* limit update rate to 1..10000Hz; somewhat arbitrary but safe */ - if (rate < 1) - return -ERANGE; - if (rate > 10000) + if (rate < 1) { return -ERANGE; + } - if ((group >= PWM_SERVO_MAX_TIMERS) || (pwm_timers[group].base == 0)) + if (rate > 10000) { + return -ERANGE; + } + + if ((group >= PWM_SERVO_MAX_TIMERS) || (pwm_timers[group].base == 0)) { return ERROR; + } pwm_timer_set_rate(group, rate); @@ -294,8 +305,9 @@ up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate) int up_pwm_servo_set_rate(unsigned rate) { - for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) + for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) { up_pwm_servo_set_rate_group_update(i, rate); + } return 0; } @@ -306,9 +318,11 @@ up_pwm_servo_get_rate_group(unsigned group) unsigned channels = 0; for (unsigned i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) { - if ((pwm_channels[i].gpio != 0) && (pwm_channels[i].timer_index == group)) + if ((pwm_channels[i].gpio != 0) && (pwm_channels[i].timer_index == group)) { channels |= (1 << i); + } } + return channels; } diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index 023ce426cc..55b36f8bb4 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -47,16 +47,16 @@ * From Wikibooks: * * PLAY "[string expression]" - * + * * Used to play notes and a score ... The tones are indicated by letters A through G. - * Accidentals are indicated with a "+" or "#" (for sharp) or "-" (for flat) + * Accidentals are indicated with a "+" or "#" (for sharp) or "-" (for flat) * immediately after the note letter. See this example: - * + * * PLAY "C C# C C#" * * Whitespaces are ignored inside the string expression. There are also codes that - * set the duration, octave and tempo. They are all case-insensitive. PLAY executes - * the commands or notes the order in which they appear in the string. Any indicators + * set the duration, octave and tempo. They are all case-insensitive. PLAY executes + * the commands or notes the order in which they appear in the string. Any indicators * that change the properties are effective for the notes following that indicator. * * Ln Sets the duration (length) of the notes. The variable n does not indicate an actual duration @@ -66,15 +66,15 @@ * The shorthand notation of length is also provided for a note. For example, "L4 CDE L8 FG L4 AB" * can be shortened to "L4 CDE F8G8 AB". F and G play as eighth notes while others play as quarter notes. * On Sets the current octave. Valid values for n are 0 through 6. An octave begins with C and ends with B. - * Remember that C- is equivalent to B. + * Remember that C- is equivalent to B. * < > Changes the current octave respectively down or up one level. * Nn Plays a specified note in the seven-octave range. Valid values are from 0 to 84. (0 is a pause.) * Cannot use with sharp and flat. Cannot use with the shorthand notation neither. * MN Stand for Music Normal. Note duration is 7/8ths of the length indicated by Ln. It is the default mode. * ML Stand for Music Legato. Note duration is full length of that indicated by Ln. * MS Stand for Music Staccato. Note duration is 3/4ths of the length indicated by Ln. - * Pn Causes a silence (pause) for the length of note indicated (same as Ln). - * Tn Sets the number of "L4"s per minute (tempo). Valid values are from 32 to 255. The default value is T120. + * Pn Causes a silence (pause) for the length of note indicated (same as Ln). + * Tn Sets the number of "L4"s per minute (tempo). Valid values are from 32 to 255. The default value is T120. * . When placed after a note, it causes the duration of the note to be 3/2 of the set duration. * This is how to get "dotted" notes. "L4 C#." would play C sharp as a dotted quarter note. * It can be used for a pause as well. @@ -117,8 +117,23 @@ #include +/* Check that tone alarm and HRT timers are different */ +#if defined(TONE_ALARM_TIMER) && defined(HRT_TIMER) +# if TONE_ALARM_TIMER == HRT_TIMER +# error TONE_ALARM_TIMER and HRT_TIMER must use different timers. +# endif +#endif + /* Tone alarm configuration */ -#if TONE_ALARM_TIMER == 2 +#if TONE_ALARM_TIMER == 1 +# define TONE_ALARM_BASE STM32_TIM1_BASE +# define TONE_ALARM_CLOCK STM32_APB2_TIM1_CLKIN +# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB2ENR +# define TONE_ALARM_CLOCK_ENABLE RCC_APB2ENR_TIM1EN +# ifdef CONFIG_STM32_TIM1 +# error Must not set CONFIG_STM32_TIM1 when TONE_ALARM_TIMER is 1 +# endif +#elif TONE_ALARM_TIMER == 2 # define TONE_ALARM_BASE STM32_TIM2_BASE # define TONE_ALARM_CLOCK STM32_APB1_TIM2_CLKIN # define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB1ENR @@ -150,6 +165,14 @@ # ifdef CONFIG_STM32_TIM5 # error Must not set CONFIG_STM32_TIM5 when TONE_ALARM_TIMER is 5 # endif +#elif TONE_ALARM_TIMER == 8 +# define TONE_ALARM_BASE STM32_TIM8_BASE +# define TONE_ALARM_CLOCK STM32_APB2_TIM8_CLKIN +# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB2ENR +# define TONE_ALARM_CLOCK_ENABLE RCC_APB2ENR_TIM8EN +# ifdef CONFIG_STM32_TIM8 +# error Must not set CONFIG_STM32_TIM8 when TONE_ALARM_TIMER is 8 +# endif #elif TONE_ALARM_TIMER == 9 # define TONE_ALARM_BASE STM32_TIM9_BASE # define TONE_ALARM_CLOCK STM32_APB2_TIM9_CLKIN @@ -175,7 +198,7 @@ # error Must not set CONFIG_STM32_TIM11 when TONE_ALARM_TIMER is 11 # endif #else -# error Must set TONE_ALARM_TIMER to a generic timer in order to use this driver. +# error Must set TONE_ALARM_TIMER to one of the timers between 1 and 11 (inclusive) to use this driver. #endif #if TONE_ALARM_CHANNEL == 1 @@ -208,24 +231,47 @@ */ #define REG(_reg) (*(volatile uint32_t *)(TONE_ALARM_BASE + _reg)) -#define rCR1 REG(STM32_GTIM_CR1_OFFSET) -#define rCR2 REG(STM32_GTIM_CR2_OFFSET) -#define rSMCR REG(STM32_GTIM_SMCR_OFFSET) -#define rDIER REG(STM32_GTIM_DIER_OFFSET) -#define rSR REG(STM32_GTIM_SR_OFFSET) -#define rEGR REG(STM32_GTIM_EGR_OFFSET) -#define rCCMR1 REG(STM32_GTIM_CCMR1_OFFSET) -#define rCCMR2 REG(STM32_GTIM_CCMR2_OFFSET) -#define rCCER REG(STM32_GTIM_CCER_OFFSET) -#define rCNT REG(STM32_GTIM_CNT_OFFSET) -#define rPSC REG(STM32_GTIM_PSC_OFFSET) -#define rARR REG(STM32_GTIM_ARR_OFFSET) -#define rCCR1 REG(STM32_GTIM_CCR1_OFFSET) -#define rCCR2 REG(STM32_GTIM_CCR2_OFFSET) -#define rCCR3 REG(STM32_GTIM_CCR3_OFFSET) -#define rCCR4 REG(STM32_GTIM_CCR4_OFFSET) -#define rDCR REG(STM32_GTIM_DCR_OFFSET) -#define rDMAR REG(STM32_GTIM_DMAR_OFFSET) +#if TONE_ALARM_TIMER == 1 || TONE_ALARM_TIMER == 8 // Note: If using TIM1 or TIM8, then you are using the ADVANCED timers and NOT the GENERAL TIMERS, therefore different registers +# define rCR1 REG(STM32_ATIM_CR1_OFFSET) +# define rCR2 REG(STM32_ATIM_CR2_OFFSET) +# define rSMCR REG(STM32_ATIM_SMCR_OFFSET) +# define rDIER REG(STM32_ATIM_DIER_OFFSET) +# define rSR REG(STM32_ATIM_SR_OFFSET) +# define rEGR REG(STM32_ATIM_EGR_OFFSET) +# define rCCMR1 REG(STM32_ATIM_CCMR1_OFFSET) +# define rCCMR2 REG(STM32_ATIM_CCMR2_OFFSET) +# define rCCER REG(STM32_ATIM_CCER_OFFSET) +# define rCNT REG(STM32_ATIM_CNT_OFFSET) +# define rPSC REG(STM32_ATIM_PSC_OFFSET) +# define rARR REG(STM32_ATIM_ARR_OFFSET) +# define rRCR REG(STM32_ATIM_RCR_OFFSET) +# define rCCR1 REG(STM32_ATIM_CCR1_OFFSET) +# define rCCR2 REG(STM32_ATIM_CCR2_OFFSET) +# define rCCR3 REG(STM32_ATIM_CCR3_OFFSET) +# define rCCR4 REG(STM32_ATIM_CCR4_OFFSET) +# define rBDTR REG(STM32_ATIM_BDTR_OFFSET) +# define rDCR REG(STM32_ATIM_DCR_OFFSET) +# define rDMAR REG(STM32_ATIM_DMAR_OFFSET) +#else +# define rCR1 REG(STM32_GTIM_CR1_OFFSET) +# define rCR2 REG(STM32_GTIM_CR2_OFFSET) +# define rSMCR REG(STM32_GTIM_SMCR_OFFSET) +# define rDIER REG(STM32_GTIM_DIER_OFFSET) +# define rSR REG(STM32_GTIM_SR_OFFSET) +# define rEGR REG(STM32_GTIM_EGR_OFFSET) +# define rCCMR1 REG(STM32_GTIM_CCMR1_OFFSET) +# define rCCMR2 REG(STM32_GTIM_CCMR2_OFFSET) +# define rCCER REG(STM32_GTIM_CCER_OFFSET) +# define rCNT REG(STM32_GTIM_CNT_OFFSET) +# define rPSC REG(STM32_GTIM_PSC_OFFSET) +# define rARR REG(STM32_GTIM_ARR_OFFSET) +# define rCCR1 REG(STM32_GTIM_CCR1_OFFSET) +# define rCCR2 REG(STM32_GTIM_CCR2_OFFSET) +# define rCCR3 REG(STM32_GTIM_CCR3_OFFSET) +# define rCCR4 REG(STM32_GTIM_CCR4_OFFSET) +# define rDCR REG(STM32_GTIM_DCR_OFFSET) +# define rDMAR REG(STM32_GTIM_DMAR_OFFSET) +#endif class ToneAlarm : public device::CDev { @@ -237,14 +283,15 @@ public: virtual int ioctl(file *filp, int cmd, unsigned long arg); virtual ssize_t write(file *filp, const char *buffer, size_t len); - inline const char *name(int tune) { + inline const char *name(int tune) + { return _tune_names[tune]; } private: static const unsigned _tune_max = 1024 * 8; // be reasonable about user tunes - const char * _default_tunes[TONE_NUMBER_OF_TUNES]; - const char * _tune_names[TONE_NUMBER_OF_TUNES]; + const char *_default_tunes[TONE_NUMBER_OF_TUNES]; + const char *_tune_names[TONE_NUMBER_OF_TUNES]; static const uint8_t _note_tab[]; unsigned _default_tune_number; // number of currently playing default tune (0 for none) @@ -268,8 +315,8 @@ private: // unsigned note_to_divisor(unsigned note); - // Calculate the duration in microseconds of play and silence for a - // note given the current tempo, length and mode and the number of + // Calculate the duration in microseconds of play and silence for a + // note given the current tempo, length and mode and the number of // dots following in the play string. // unsigned note_duration(unsigned &silence, unsigned note_length, unsigned dots); @@ -376,14 +423,15 @@ ToneAlarm::init() ret = CDev::init(); - if (ret != OK) + if (ret != OK) { return ret; + } /* configure the GPIO to the idle state */ stm32_configgpio(GPIO_TONE_ALARM_IDLE); /* clock/power on our timer */ - modifyreg32(TONE_ALARM_CLOCK_POWER_REG, 0, TONE_ALARM_CLOCK_ENABLE); + modifyreg32(TONE_ALARM_CLOCK_POWER_REG, 0, TONE_ALARM_CLOCK_ENABLE); /* initialise the timer */ rCR1 = 0; @@ -396,6 +444,10 @@ ToneAlarm::init() rCCER = TONE_CCER; rDCR = 0; +#ifdef rBDTR // If using an advanced timer, you need to activate the output + rBDTR = ATIM_BDTR_MOE; // enable the main output of the advanced timer +#endif + /* toggle the CC output each time the count passes 1 */ TONE_rCCR = 1; @@ -428,25 +480,31 @@ ToneAlarm::note_duration(unsigned &silence, unsigned note_length, unsigned dots) { unsigned whole_note_period = (60 * 1000000 * 4) / _tempo; - if (note_length == 0) + if (note_length == 0) { note_length = 1; + } + unsigned note_period = whole_note_period / note_length; switch (_note_mode) { case MODE_NORMAL: silence = note_period / 8; break; + case MODE_STACCATO: silence = note_period / 4; break; + default: case MODE_LEGATO: silence = 0; break; } + note_period -= silence; unsigned dot_extension = note_period / 2; + while (dots--) { note_period += dot_extension; dot_extension /= 2; @@ -460,12 +518,14 @@ ToneAlarm::rest_duration(unsigned rest_length, unsigned dots) { unsigned whole_note_period = (60 * 1000000 * 4) / _tempo; - if (rest_length == 0) + if (rest_length == 0) { rest_length = 1; + } unsigned rest_period = whole_note_period / rest_length; unsigned dot_extension = rest_period / 2; + while (dots--) { rest_period += dot_extension; dot_extension /= 2; @@ -555,115 +615,155 @@ ToneAlarm::next_note() while (note == 0) { // we always need at least one character from the string int c = next_char(); - if (c == 0) + + if (c == 0) { goto tune_end; + } + _next++; switch (c) { case 'L': // select note length _note_length = next_number(); - if (_note_length < 1) + + if (_note_length < 1) { goto tune_error; + } + break; case 'O': // select octave _octave = next_number(); - if (_octave > 6) + + if (_octave > 6) { _octave = 6; + } + break; case '<': // decrease octave - if (_octave > 0) + if (_octave > 0) { _octave--; + } + break; case '>': // increase octave - if (_octave < 6) + if (_octave < 6) { _octave++; + } + break; case 'M': // select inter-note gap c = next_char(); - if (c == 0) + + if (c == 0) { goto tune_error; + } + _next++; + switch (c) { case 'N': _note_mode = MODE_NORMAL; break; + case 'L': _note_mode = MODE_LEGATO; break; + case 'S': _note_mode = MODE_STACCATO; break; + case 'F': _repeat = false; break; + case 'B': _repeat = true; break; + default: goto tune_error; } + break; case 'P': // pause for a note length stop_note(); - hrt_call_after(&_note_call, - (hrt_abstime)rest_duration(next_number(), next_dots()), - (hrt_callout)next_trampoline, - this); + hrt_call_after(&_note_call, + (hrt_abstime)rest_duration(next_number(), next_dots()), + (hrt_callout)next_trampoline, + this); return; case 'T': { // change tempo - unsigned nt = next_number(); + unsigned nt = next_number(); - if ((nt >= 32) && (nt <= 255)) { - _tempo = nt; - } else { - goto tune_error; + if ((nt >= 32) && (nt <= 255)) { + _tempo = nt; + + } else { + goto tune_error; + } + + break; } - break; - } case 'N': // play an arbitrary note note = next_number(); - if (note > 84) + + if (note > 84) { goto tune_error; + } + if (note == 0) { // this is a rest - pause for the current note length hrt_call_after(&_note_call, - (hrt_abstime)rest_duration(_note_length, next_dots()), - (hrt_callout)next_trampoline, - this); - return; + (hrt_abstime)rest_duration(_note_length, next_dots()), + (hrt_callout)next_trampoline, + this); + return; } + break; case 'A'...'G': // play a note in the current octave note = _note_tab[c - 'A'] + (_octave * 12) + 1; c = next_char(); + switch (c) { case '#': // up a semitone case '+': - if (note < 84) + if (note < 84) { note++; + } + _next++; break; + case '-': // down a semitone - if (note > 1) + if (note > 1) { note--; + } + _next++; break; + default: // 0 / no next char here is OK break; } + // shorthand length notation note_length = next_number(); - if (note_length == 0) + + if (note_length == 0) { note_length = _note_length; + } + break; default: @@ -689,12 +789,15 @@ tune_error: // stop (and potentially restart) the tune tune_end: stop_note(); + if (_repeat) { start_tune(_tune); + } else { _tune = nullptr; _default_tune_number = 0; } + return; } @@ -704,6 +807,7 @@ ToneAlarm::next_char() while (isspace(*_next)) { _next++; } + return toupper(*_next); } @@ -715,8 +819,11 @@ ToneAlarm::next_number() for (;;) { c = next_char(); - if (!isdigit(c)) + + if (!isdigit(c)) { return number; + } + _next++; number = (number * 10) + (c - '0'); } @@ -731,6 +838,7 @@ ToneAlarm::next_dots() _next++; dots++; } + return dots; } @@ -764,6 +872,7 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) _next = nullptr; _repeat = false; _default_tune_number = 0; + } else { /* always interrupt alarms, unless they are repeating and already playing */ if (!(_repeat && _default_tune_number == arg)) { @@ -772,6 +881,7 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) start_tune(_default_tunes[arg]); } } + } else { result = -EINVAL; } @@ -786,8 +896,9 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) // irqrestore(flags); /* give it to the superclass if we didn't like it */ - if (result == -ENOTTY) + if (result == -ENOTTY) { result = CDev::ioctl(filp, cmd, arg); + } return result; } @@ -796,8 +907,9 @@ int ToneAlarm::write(file *filp, const char *buffer, size_t len) { // sanity-check the buffer for length and nul-termination - if (len > _tune_max) + if (len > _tune_max) { return -EFBIG; + } // if we have an existing user tune, free it if (_user_tune != nullptr) { @@ -814,13 +926,16 @@ ToneAlarm::write(file *filp, const char *buffer, size_t len) } // if the new tune is empty, we're done - if (buffer[0] == '\0') + if (buffer[0] == '\0') { return OK; + } // allocate a copy of the new tune _user_tune = strndup(buffer, len); - if (_user_tune == nullptr) + + if (_user_tune == nullptr) { return -ENOMEM; + } // and play it start_tune(_user_tune); @@ -843,14 +958,16 @@ play_tune(unsigned tune) fd = open(TONEALARM0_DEVICE_PATH, 0); - if (fd < 0) + if (fd < 0) { err(1, TONEALARM0_DEVICE_PATH); + } ret = ioctl(fd, TONE_SET_ALARM, tune); close(fd); - if (ret != 0) + if (ret != 0) { err(1, "TONE_SET_ALARM"); + } exit(0); } @@ -862,17 +979,21 @@ play_string(const char *str, bool free_buffer) fd = open(TONEALARM0_DEVICE_PATH, O_WRONLY); - if (fd < 0) + if (fd < 0) { err(1, TONEALARM0_DEVICE_PATH); + } ret = write(fd, str, strlen(str) + 1); close(fd); - if (free_buffer) + if (free_buffer) { free((void *)str); + } - if (ret < 0) + if (ret < 0) { err(1, "play tune"); + } + exit(0); } @@ -887,8 +1008,9 @@ tone_alarm_main(int argc, char *argv[]) if (g_dev == nullptr) { g_dev = new ToneAlarm; - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "couldn't allocate the ToneAlarm driver"); + } if (g_dev->init() != OK) { delete g_dev; @@ -903,30 +1025,39 @@ tone_alarm_main(int argc, char *argv[]) play_tune(TONE_STOP_TUNE); } - if (!strcmp(argv1, "stop")) + if (!strcmp(argv1, "stop")) { play_tune(TONE_STOP_TUNE); + } - if ((tune = strtol(argv1, nullptr, 10)) != 0) + if ((tune = strtol(argv1, nullptr, 10)) != 0) { play_tune(tune); + } /* It might be a tune name */ for (tune = 1; tune < TONE_NUMBER_OF_TUNES; tune++) - if (!strcmp(g_dev->name(tune), argv1)) + if (!strcmp(g_dev->name(tune), argv1)) { play_tune(tune); + } /* If it is a file name then load and play it as a string */ if (*argv1 == '/') { FILE *fd = fopen(argv1, "r"); int sz; char *buffer; - if (fd == nullptr) + + if (fd == nullptr) { errx(1, "couldn't open '%s'", argv1); + } + fseek(fd, 0, SEEK_END); sz = ftell(fd); fseek(fd, 0, SEEK_SET); buffer = (char *)malloc(sz + 1); - if (buffer == nullptr) + + if (buffer == nullptr) { errx(1, "not enough memory memory"); + } + fread(buffer, sz, 1, fd); /* terminate the string */ buffer[sz] = 0; diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index d6e78fd821..3d6aa88269 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -432,11 +432,11 @@ int ex_fixedwing_control_main(int argc, char *argv[]) thread_should_exit = false; deamon_task = px4_task_spawn_cmd("ex_fixedwing_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 20, - 2048, - fixedwing_control_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 20, + 2048, + fixedwing_control_thread_main, + (argv) ? (char *const *)&argv[2] : (char *const *)NULL); thread_running = true; exit(0); } diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c index 027511df20..bcf13c4de5 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -112,11 +112,11 @@ int flow_position_estimator_main(int argc, char *argv[]) thread_should_exit = false; daemon_task = px4_task_spawn_cmd("flow_position_estimator", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 4000, - flow_position_estimator_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 4000, + flow_position_estimator_thread_main, + (argv) ? (char *const *)&argv[2] : (char *const *)NULL); exit(0); } diff --git a/src/examples/matlab_csv_serial/matlab_csv_serial.c b/src/examples/matlab_csv_serial/matlab_csv_serial.c index 139a182b22..b949400e47 100644 --- a/src/examples/matlab_csv_serial/matlab_csv_serial.c +++ b/src/examples/matlab_csv_serial/matlab_csv_serial.c @@ -104,11 +104,11 @@ int matlab_csv_serial_main(int argc, char *argv[]) thread_should_exit = false; daemon_task = px4_task_spawn_cmd("matlab_csv_serial", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - matlab_csv_serial_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + matlab_csv_serial_thread_main, + (argv) ? (char *const *)&argv[2] : (char *const *)NULL); exit(0); } diff --git a/src/examples/publisher/publisher_start_nuttx.cpp b/src/examples/publisher/publisher_start_nuttx.cpp index 2379d7aa68..b07207e8a5 100644 --- a/src/examples/publisher/publisher_start_nuttx.cpp +++ b/src/examples/publisher/publisher_start_nuttx.cpp @@ -69,11 +69,11 @@ int publisher_main(int argc, char *argv[]) task_should_exit = false; daemon_task = px4_task_spawn_cmd("publisher", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - main, - (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + main, + (argv) ? (char *const *)&argv[2] : (char *const *)NULL); exit(0); } diff --git a/src/examples/px4_daemon_app/px4_daemon_app.c b/src/examples/px4_daemon_app/px4_daemon_app.c index 9154c7e5fc..1495b1122d 100644 --- a/src/examples/px4_daemon_app/px4_daemon_app.c +++ b/src/examples/px4_daemon_app/px4_daemon_app.c @@ -103,11 +103,11 @@ int px4_daemon_app_main(int argc, char *argv[]) thread_should_exit = false; daemon_task = px4_task_spawn_cmd("daemon", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2000, - px4_daemon_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2000, + px4_daemon_thread_main, + (argv) ? (char *const *)&argv[2] : (char *const *)NULL); return 0; } diff --git a/src/examples/rover_steering_control/main.cpp b/src/examples/rover_steering_control/main.cpp index 887ea86309..7a227c2366 100644 --- a/src/examples/rover_steering_control/main.cpp +++ b/src/examples/rover_steering_control/main.cpp @@ -426,11 +426,11 @@ int rover_steering_control_main(int argc, char *argv[]) thread_should_exit = false; deamon_task = px4_task_spawn_cmd("rover_steering_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 20, - 2048, - rover_steering_control_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 20, + 2048, + rover_steering_control_thread_main, + (argv) ? (char *const *)&argv[2] : (char *const *)NULL); thread_running = true; exit(0); } diff --git a/src/examples/subscriber/subscriber_start_nuttx.cpp b/src/examples/subscriber/subscriber_start_nuttx.cpp index 424e49e9c1..16644280be 100644 --- a/src/examples/subscriber/subscriber_start_nuttx.cpp +++ b/src/examples/subscriber/subscriber_start_nuttx.cpp @@ -69,11 +69,11 @@ int subscriber_main(int argc, char *argv[]) task_should_exit = false; daemon_task = px4_task_spawn_cmd("subscriber", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - main, - (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + main, + (argv) ? (char *const *)&argv[2] : (char *const *)NULL); exit(0); } diff --git a/src/include/containers/List.hpp b/src/include/containers/List.hpp index 1906030abb..6046502e3f 100644 --- a/src/include/containers/List.hpp +++ b/src/include/containers/List.hpp @@ -43,31 +43,35 @@ template class __EXPORT ListNode { public: - ListNode() : _sibling(nullptr) { + ListNode() : _sibling(nullptr) + { } virtual ~ListNode() {}; void setSibling(T sibling) { _sibling = sibling; } T getSibling() { return _sibling; } - T get() { + T get() + { return _sibling; } protected: T _sibling; private: // forbid copy - ListNode(const ListNode& other); + ListNode(const ListNode &other); // forbid assignment - ListNode & operator = (const ListNode &); + ListNode &operator = (const ListNode &); }; template class __EXPORT List { public: - List() : _head() { + List() : _head() + { } virtual ~List() {}; - void add(T newNode) { + void add(T newNode) + { newNode->setSibling(getHead()); setHead(newNode); } @@ -77,7 +81,7 @@ protected: T _head; private: // forbid copy - List(const List& other); + List(const List &other); // forbid assignment - List& operator = (const List &); + List &operator = (const List &); }; diff --git a/src/include/mavlink/mavlink_log.h b/src/include/mavlink/mavlink_log.h index 4ba2f98ad5..13a6b16b34 100644 --- a/src/include/mavlink/mavlink_log.h +++ b/src/include/mavlink/mavlink_log.h @@ -107,9 +107,9 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...); * @param _text The text to log; */ #define mavlink_and_console_log_emergency(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__); \ - fprintf(stderr, "telem> "); \ - fprintf(stderr, _text, ##__VA_ARGS__); \ - fprintf(stderr, "\n"); + fprintf(stderr, "telem> "); \ + fprintf(stderr, _text, ##__VA_ARGS__); \ + fprintf(stderr, "\n"); /** * Send a mavlink critical message and print to console. @@ -118,9 +118,9 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...); * @param _text The text to log; */ #define mavlink_and_console_log_critical(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__); \ - fprintf(stderr, "telem> "); \ - fprintf(stderr, _text, ##__VA_ARGS__); \ - fprintf(stderr, "\n"); + fprintf(stderr, "telem> "); \ + fprintf(stderr, _text, ##__VA_ARGS__); \ + fprintf(stderr, "\n"); /** * Send a mavlink emergency message and print to console. @@ -129,9 +129,9 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...); * @param _text The text to log; */ #define mavlink_and_console_log_info(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__); \ - fprintf(stderr, "telem> "); \ - fprintf(stderr, _text, ##__VA_ARGS__); \ - fprintf(stderr, "\n"); + fprintf(stderr, "telem> "); \ + fprintf(stderr, _text, ##__VA_ARGS__); \ + fprintf(stderr, "\n"); struct mavlink_logmessage { char text[MAVLINK_LOG_MAXLEN + 1]; diff --git a/src/lib/dspal b/src/lib/dspal new file mode 160000 index 0000000000..229f2f4d84 --- /dev/null +++ b/src/lib/dspal @@ -0,0 +1 @@ +Subproject commit 229f2f4d8471564f01fe8330e5de1554a9b7aeb6 diff --git a/src/lib/ecl/ecl.h b/src/lib/ecl/ecl.h index aa3c5000a4..39e1f50809 100644 --- a/src/lib/ecl/ecl.h +++ b/src/lib/ecl/ecl.h @@ -38,6 +38,9 @@ */ #include +#include #define ecl_absolute_time hrt_absolute_time #define ecl_elapsed_time hrt_elapsed_time +#define ECL_WARN PX4_WARN +#define ECL_INFO PX4_INFO diff --git a/src/lib/ecl/module.mk b/src/lib/ecl/module.mk index 03974c9503..747a7c5be0 100644 --- a/src/lib/ecl/module.mk +++ b/src/lib/ecl/module.mk @@ -39,6 +39,8 @@ SRCS = attitude_fw/ecl_controller.cpp \ attitude_fw/ecl_pitch_controller.cpp \ attitude_fw/ecl_roll_controller.cpp \ attitude_fw/ecl_yaw_controller.cpp \ - l1/ecl_l1_pos_controller.cpp + l1/ecl_l1_pos_controller.cpp \ + validation/data_validator.cpp \ + validation/data_validator_group.cpp MAXOPTIMIZATION = -Os diff --git a/src/lib/ecl/validation/data_validator.cpp b/src/lib/ecl/validation/data_validator.cpp new file mode 100644 index 0000000000..737d9ea1a3 --- /dev/null +++ b/src/lib/ecl/validation/data_validator.cpp @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file data_validator.c + * + * A data validation class to identify anomalies in data streams + * + * @author Lorenz Meier + */ + +#include "data_validator.h" +#include + +DataValidator::DataValidator(DataValidator *prev_sibling) : + _time_last(0), + _timeout_interval(20000), + _event_count(0), + _error_count(0), + _error_density(0), + _priority(0), + _mean{0.0f}, + _lp{0.0f}, + _M2{0.0f}, + _rms{0.0f}, + _value{0.0f}, + _value_equal_count(0), + _sibling(prev_sibling) +{ + +} + +DataValidator::~DataValidator() +{ + +} + +void +DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in, int priority_in) +{ + _event_count++; + + if (error_count_in > _error_count) { + _error_density += (error_count_in - _error_count); + } else if (_error_density > 0) { + _error_density--; + } + + _error_count = error_count_in; + _priority = priority_in; + + for (unsigned i = 0; i < _dimensions; i++) { + if (_time_last == 0) { + _mean[i] = 0; + _lp[i] = val[i]; + _M2[i] = 0; + } else { + float lp_val = val[i] - _lp[i]; + + float delta_val = lp_val - _mean[i]; + _mean[i] += delta_val / _event_count; + _M2[i] += delta_val * (lp_val - _mean[i]); + _rms[i] = sqrtf(_M2[i] / (_event_count - 1)); + + if (fabsf(_value[i] - val[i]) < 0.000001f) { + _value_equal_count++; + } else { + _value_equal_count = 0; + } + } + + // XXX replace with better filter, make it auto-tune to update rate + _lp[i] = _lp[i] * 0.5f + val[i] * 0.5f; + + _value[i] = val[i]; + } + + _time_last = timestamp; +} + +float +DataValidator::confidence(uint64_t timestamp) +{ + /* check if we have any data */ + if (_time_last == 0) { + return 0.0f; + } + + /* check error count limit */ + if (_error_count > NORETURN_ERRCOUNT) { + return 0.0f; + } + + /* we got the exact same sensor value N times in a row */ + if (_value_equal_count > VALUE_EQUAL_COUNT_MAX) { + return 0.0f; + } + + /* timed out - that's it */ + if (timestamp - _time_last > _timeout_interval) { + return 0.0f; + } + + /* cap error density counter at window size */ + if (_error_density > ERROR_DENSITY_WINDOW) { + _error_density = ERROR_DENSITY_WINDOW; + } + + /* return local error density for last N measurements */ + return 1.0f - (_error_density / ERROR_DENSITY_WINDOW); +} + +int +DataValidator::priority() +{ + return _priority; +} + +void +DataValidator::print() +{ + if (_time_last == 0) { + ECL_INFO("\tno data"); + return; + } + + for (unsigned i = 0; i < _dimensions; i++) { + ECL_INFO("\tval: %8.4f, lp: %8.4f mean dev: %8.4f RMS: %8.4f conf: %8.4f", + (double) _value[i], (double)_lp[i], (double)_mean[i], + (double)_rms[i], (double)confidence(hrt_absolute_time())); + } +} diff --git a/src/lib/ecl/validation/data_validator.h b/src/lib/ecl/validation/data_validator.h new file mode 100644 index 0000000000..dde9cb51aa --- /dev/null +++ b/src/lib/ecl/validation/data_validator.h @@ -0,0 +1,137 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file data_validator.h + * + * A data validation class to identify anomalies in data streams + * + * @author Lorenz Meier + */ + +#pragma once + +#include +#include + +class __EXPORT DataValidator { +public: + DataValidator(DataValidator *prev_sibling = nullptr); + virtual ~DataValidator(); + + /** + * Put an item into the validator. + * + * @param val Item to put + */ + void put(uint64_t timestamp, float val[3], uint64_t error_count, int priority); + + /** + * Get the next sibling in the group + * + * @return the next sibling + */ + DataValidator* sibling() { return _sibling; } + + /** + * Get the confidence of this validator + * @return the confidence between 0 and 1 + */ + float confidence(uint64_t timestamp); + + /** + * Get the error count of this validator + * @return the error count + */ + uint64_t error_count() { return _error_count; } + + /** + * Get the values of this validator + * @return the stored value + */ + float* value() { return _value; } + + /** + * Get the used status of this validator + * @return true if this validator ever saw data + */ + bool used() { return (_time_last > 0); } + + /** + * Get the priority of this validator + * @return the stored priority + */ + int priority(); + + /** + * Get the RMS values of this validator + * @return the stored RMS + */ + float* rms() { return _rms; } + + /** + * Print the validator value + * + */ + void print(); + + /** + * Set the timeout value + * + * @param timeout_interval_us The timeout interval in microseconds + */ + void set_timeout(uint64_t timeout_interval_us) { _timeout_interval = timeout_interval_us; } + +private: + static const unsigned _dimensions = 3; + uint64_t _time_last; /**< last timestamp */ + uint64_t _timeout_interval; /**< interval in which the datastream times out in us */ + uint64_t _event_count; /**< total data counter */ + uint64_t _error_count; /**< error count */ + int _error_density; /**< ratio between successful reads and errors */ + int _priority; /**< sensor nominal priority */ + float _mean[_dimensions]; /**< mean of value */ + float _lp[3]; /**< low pass value */ + float _M2[3]; /**< RMS component value */ + float _rms[3]; /**< root mean square error */ + float _value[3]; /**< last value */ + float _value_equal_count; /**< equal values in a row */ + DataValidator *_sibling; /**< sibling in the group */ + const unsigned NORETURN_ERRCOUNT = 10000; /**< if the error count reaches this value, return sensor as invalid */ + const float ERROR_DENSITY_WINDOW = 100.0f; /**< window in measurement counts for errors */ + const unsigned VALUE_EQUAL_COUNT_MAX = 100; /**< if the sensor value is the same (accumulated also between axes) this many times, flag it */ + + /* we don't want this class to be copied */ + DataValidator(const DataValidator&); + DataValidator operator=(const DataValidator&); +}; diff --git a/src/lib/ecl/validation/data_validator_group.cpp b/src/lib/ecl/validation/data_validator_group.cpp new file mode 100644 index 0000000000..ac4bcc8b9b --- /dev/null +++ b/src/lib/ecl/validation/data_validator_group.cpp @@ -0,0 +1,224 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file data_validator_group.cpp + * + * A data validation group to identify anomalies in data streams + * + * @author Lorenz Meier + */ + +#include "data_validator_group.h" +#include + +DataValidatorGroup::DataValidatorGroup(unsigned siblings) : + _first(nullptr), + _curr_best(-1), + _prev_best(-1), + _first_failover_time(0), + _toggle_count(0) +{ + DataValidator *next = _first; + + for (unsigned i = 0; i < siblings; i++) { + next = new DataValidator(next); + } + + _first = next; +} + +DataValidatorGroup::~DataValidatorGroup() +{ + +} + +void +DataValidatorGroup::set_timeout(uint64_t timeout_interval_us) +{ + DataValidator *next = _first; + + while (next != nullptr) { + next->set_timeout(timeout_interval_us); + next = next->sibling(); + } +} + +void +DataValidatorGroup::put(unsigned index, uint64_t timestamp, float val[3], uint64_t error_count, int priority) +{ + DataValidator *next = _first; + unsigned i = 0; + + while (next != nullptr) { + if (i == index) { + next->put(timestamp, val, error_count, priority); + break; + } + next = next->sibling(); + i++; + } +} + +float* +DataValidatorGroup::get_best(uint64_t timestamp, int *index) +{ + DataValidator *next = _first; + + // XXX This should eventually also include voting + int pre_check_best = _curr_best; + float pre_check_confidence = 1.0f; + int pre_check_prio = -1; + float max_confidence = -1.0f; + int max_priority = -1000; + int max_index = -1; + DataValidator *best = nullptr; + + unsigned i = 0; + + while (next != nullptr) { + float confidence = next->confidence(timestamp); + + if (static_cast(i) == pre_check_best) { + pre_check_prio = next->priority(); + pre_check_confidence = confidence; + } + + /* + * Switch if: + * 1) the confidence is higher and priority is equal or higher + * 2) the confidence is no less than 1% different and the priority is higher + */ + if (((max_confidence < MIN_REGULAR_CONFIDENCE) && (confidence >= MIN_REGULAR_CONFIDENCE)) || + (confidence > max_confidence && (next->priority() >= max_priority)) || + (fabsf(confidence - max_confidence) < 0.01f && (next->priority() > max_priority)) + ) { + max_index = i; + max_confidence = confidence; + max_priority = next->priority(); + best = next; + } + + next = next->sibling(); + i++; + } + + /* the current best sensor is not matching the previous best sensor */ + if (max_index != _curr_best) { + + bool true_failsafe = true; + + /* check wether the switch was a failsafe or preferring a higher priority sensor */ + if (pre_check_prio != -1 && pre_check_prio < max_priority && + fabsf(pre_check_confidence - max_confidence) < 0.1f) { + /* this is not a failover */ + true_failsafe = false; + } + + /* if we're no initialized, initialize the bookkeeping but do not count a failsafe */ + if (_curr_best < 0) { + _prev_best = max_index; + } else { + /* we were initialized before, this is a real failsafe */ + _prev_best = pre_check_best; + + if (true_failsafe) { + _toggle_count++; + + /* if this is the first time, log when we failed */ + if (_first_failover_time == 0) { + _first_failover_time = timestamp; + } + } + } + + /* for all cases we want to keep a record of the best index */ + _curr_best = max_index; + } + *index = max_index; + return (best) ? best->value() : nullptr; +} + +float +DataValidatorGroup::get_vibration_factor(uint64_t timestamp) +{ + DataValidator *next = _first; + + float vibe = 0.0f; + + /* find the best RMS value of a non-timed out sensor */ + while (next != nullptr) { + + if (next->confidence(timestamp) > 0.5f) { + float* rms = next->rms(); + + for (unsigned j = 0; j < 3; j++) { + if (rms[j] > vibe) { + vibe = rms[j]; + } + } + } + + next = next->sibling(); + } + + return vibe; +} + +void +DataValidatorGroup::print() +{ + /* print the group's state */ + ECL_INFO("validator: best: %d, prev best: %d, failsafe: %s (# %u)", + _curr_best, _prev_best, (_toggle_count > 0) ? "YES" : "NO", + _toggle_count); + + + DataValidator *next = _first; + unsigned i = 0; + + while (next != nullptr) { + if (next->used()) { + ECL_INFO("sensor #%u, prio: %d", i, next->priority()); + next->print(); + } + next = next->sibling(); + i++; + } +} + +unsigned +DataValidatorGroup::failover_count() +{ + return _toggle_count; +} diff --git a/src/lib/ecl/validation/data_validator_group.h b/src/lib/ecl/validation/data_validator_group.h new file mode 100644 index 0000000000..3756be2638 --- /dev/null +++ b/src/lib/ecl/validation/data_validator_group.h @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file data_validator_group.h + * + * A data validation group to identify anomalies in data streams + * + * @author Lorenz Meier + */ + +#pragma once + +#include "data_validator.h" + +class __EXPORT DataValidatorGroup { +public: + DataValidatorGroup(unsigned siblings); + virtual ~DataValidatorGroup(); + + /** + * Put an item into the validator group. + * + * @param index Sensor index + * @param timestamp The timestamp of the measurement + * @param val The 3D vector + * @param error_count The current error count of the sensor + * @param priority The priority of the sensor + */ + void put(unsigned index, uint64_t timestamp, + float val[3], uint64_t error_count, int priority); + + /** + * Get the best data triplet of the group + * + * @return pointer to the array of best values + */ + float* get_best(uint64_t timestamp, int *index); + + /** + * Get the RMS / vibration factor + * + * @return float value representing the RMS, which a valid indicator for vibration + */ + float get_vibration_factor(uint64_t timestamp); + + /** + * Get the number of failover events + * + * @return the number of failovers + */ + unsigned failover_count(); + + /** + * Print the validator value + * + */ + void print(); + + /** + * Set the timeout value + * + * @param timeout_interval_us The timeout interval in microseconds + */ + void set_timeout(uint64_t timeout_interval_us); + +private: + DataValidator *_first; /**< sibling in the group */ + int _curr_best; /**< currently best index */ + int _prev_best; /**< the previous best index */ + uint64_t _first_failover_time; /**< timestamp where the first failover occured or zero if none occured */ + unsigned _toggle_count; /**< number of back and forth switches between two sensors */ + static constexpr float MIN_REGULAR_CONFIDENCE = 0.9f; + + /* we don't want this class to be copied */ + DataValidatorGroup(const DataValidatorGroup&); + DataValidatorGroup operator=(const DataValidatorGroup&); +}; diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp index 2ea1c414bd..d514d895f4 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.cpp +++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp @@ -58,7 +58,8 @@ CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) : } -CatapultLaunchMethod::~CatapultLaunchMethod() { +CatapultLaunchMethod::~CatapultLaunchMethod() +{ } @@ -69,34 +70,41 @@ void CatapultLaunchMethod::update(float accel_x) switch (state) { case LAUNCHDETECTION_RES_NONE: + /* Detect a acceleration that is longer and stronger as the minimum given by the params */ if (accel_x > thresholdAccel.get()) { integrator += dt; + if (integrator > thresholdTime.get()) { if (motorDelay.get() > 0.0f) { state = LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL; warnx("Launch detected: state: enablecontrol, waiting %.2fs until using full" - " throttle", (double)motorDelay.get()); + " throttle", (double)motorDelay.get()); + } else { /* No motor delay set: go directly to enablemotors state */ state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; warnx("Launch detected: state: enablemotors (delay not activated)"); } } + } else { /* reset */ reset(); } + break; case LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL: - /* Vehicle is currently controlling attitude but not with full throttle. Waiting undtil delay is + /* Vehicle is currently controlling attitude but not with full throttle. Waiting until delay is * over to allow full throttle */ motorDelayCounter += dt; + if (motorDelayCounter > motorDelay.get()) { warnx("Launch detected: state enablemotors"); state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; } + break; default: @@ -119,10 +127,12 @@ void CatapultLaunchMethod::reset() state = LAUNCHDETECTION_RES_NONE; } -float CatapultLaunchMethod::getPitchMax(float pitchMaxDefault) { +float CatapultLaunchMethod::getPitchMax(float pitchMaxDefault) +{ /* If motor is turned on do not impose the extra limit on maximum pitch */ if (state == LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { return pitchMaxDefault; + } else { return pitchMaxPreThrottle.get(); } diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp index 52f5c32576..38e556e88c 100644 --- a/src/lib/launchdetection/LaunchDetector.cpp +++ b/src/lib/launchdetection/LaunchDetector.cpp @@ -66,7 +66,7 @@ LaunchDetector::~LaunchDetector() void LaunchDetector::reset() { /* Reset all detectors */ - for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) { + for (unsigned i = 0; i < (sizeof(launchMethods) / sizeof(launchMethods[0])); i++) { launchMethods[i]->reset(); } @@ -79,7 +79,7 @@ void LaunchDetector::reset() void LaunchDetector::update(float accel_x) { if (launchdetection_on.get() == 1) { - for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) { + for (unsigned i = 0; i < (sizeof(launchMethods) / sizeof(launchMethods[0])); i++) { launchMethods[i]->update(accel_x); } } @@ -89,14 +89,15 @@ LaunchDetectionResult LaunchDetector::getLaunchDetected() { if (launchdetection_on.get() == 1) { if (activeLaunchDetectionMethodIndex < 0) { - /* None of the active launchmethods has detected a launch, check all launchmethods */ - for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) { - if(launchMethods[i]->getLaunchDetected() != LAUNCHDETECTION_RES_NONE) { + /* None of the active launchmethods has detected a launch, check all launchmethods */ + for (unsigned i = 0; i < (sizeof(launchMethods) / sizeof(launchMethods[0])); i++) { + if (launchMethods[i]->getLaunchDetected() != LAUNCHDETECTION_RES_NONE) { warnx("selecting launchmethod %d", i); activeLaunchDetectionMethodIndex = i; // from now on only check this method return launchMethods[i]->getLaunchDetected(); } } + } else { return launchMethods[activeLaunchDetectionMethodIndex]->getLaunchDetected(); } @@ -105,7 +106,8 @@ LaunchDetectionResult LaunchDetector::getLaunchDetected() return LAUNCHDETECTION_RES_NONE; } -float LaunchDetector::getPitchMax(float pitchMaxDefault) { +float LaunchDetector::getPitchMax(float pitchMaxDefault) +{ if (!launchdetection_on.get()) { return pitchMaxDefault; } @@ -113,11 +115,13 @@ float LaunchDetector::getPitchMax(float pitchMaxDefault) { /* if a lauchdetectionmethod is active or only one exists return the pitch limit from this method, * otherwise use the default limit */ if (activeLaunchDetectionMethodIndex < 0) { - if (sizeof(launchMethods)/sizeof(LaunchMethod) > 1) { + if (sizeof(launchMethods) / sizeof(LaunchMethod *) > 1) { return pitchMaxDefault; + } else { return launchMethods[0]->getPitchMax(pitchMaxDefault); } + } else { return launchMethods[activeLaunchDetectionMethodIndex]->getPitchMax(pitchMaxDefault); } diff --git a/src/lib/uavcan b/src/lib/uavcan index 3ae5400aa5..50dc08663a 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit 3ae5400aa5ead18139106d30f730114d5e9b65dd +Subproject commit 50dc08663af2d9b55eae0ccf5f07c63db0ee8907 diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index a320ffe518..14f5470425 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -383,10 +383,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) uint8_t update_vect[3] = {0, 0, 0}; /* Fill in gyro measurements */ - if (sensor_last_timestamp[0] != raw.timestamp) { + if (sensor_last_timestamp[0] != raw.gyro_timestamp[0]) { update_vect[0] = 1; // sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); - sensor_last_timestamp[0] = raw.timestamp; + sensor_last_timestamp[0] = raw.gyro_timestamp[0]; } z_k[0] = raw.gyro_rad_s[0] - gyro_offsets[0]; @@ -394,10 +394,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; /* update accelerometer measurements */ - if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) { + if (sensor_last_timestamp[1] != raw.accelerometer_timestamp[0]) { update_vect[1] = 1; // sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); - sensor_last_timestamp[1] = raw.accelerometer_timestamp; + sensor_last_timestamp[1] = raw.accelerometer_timestamp[0]; } hrt_abstime vel_t = 0; @@ -437,14 +437,14 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) z_k[5] = raw.accelerometer_m_s2[2] - acc(2); /* update magnetometer measurements */ - if (sensor_last_timestamp[2] != raw.magnetometer_timestamp && + if (sensor_last_timestamp[2] != raw.magnetometer_timestamp[0] && /* check that the mag vector is > 0 */ fabsf(sqrtf(raw.magnetometer_ga[0] * raw.magnetometer_ga[0] + raw.magnetometer_ga[1] * raw.magnetometer_ga[1] + raw.magnetometer_ga[2] * raw.magnetometer_ga[2])) > 0.1f) { update_vect[2] = 1; // sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); - sensor_last_timestamp[2] = raw.magnetometer_timestamp; + sensor_last_timestamp[2] = raw.magnetometer_timestamp[0]; } bool vision_updated = false; diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk index 20eda27690..716b0aefe4 100644 --- a/src/modules/attitude_estimator_ekf/module.mk +++ b/src/modules/attitude_estimator_ekf/module.mk @@ -46,5 +46,5 @@ MODULE_STACKSIZE = 1200 EXTRACFLAGS = -Wno-float-equal -Wframe-larger-than=3700 ifeq ($(PX4_TARGET_OS),nuttx) -EXTRACXXFLAGS = -Wframe-larger-than=2400 +EXTRACXXFLAGS = -Wframe-larger-than=2600 endif diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index 138a8c7e5c..9b60701413 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -51,7 +51,6 @@ #include #include #include -#include #include #include #include @@ -59,8 +58,11 @@ #include #include -#include +#include +#include #include +#include +#include #include #include @@ -103,6 +105,8 @@ public: void task_main(); + void print(); + private: static constexpr float _dt_max = 0.02; bool _task_should_exit = false; /**< if true, task should exit */ @@ -121,6 +125,7 @@ private: param_t mag_decl_auto; param_t acc_comp; param_t bias_max; + param_t vibe_thresh; } _params_handles; /**< handles for interesting parameters */ float _w_accel = 0.0f; @@ -130,6 +135,8 @@ private: bool _mag_decl_auto = false; bool _acc_comp = false; float _bias_max = 0.0f; + float _vibration_warning_threshold = 1.0f; + hrt_abstime _vibration_warning_timestamp = 0; Vector<3> _gyro; Vector<3> _accel; @@ -142,10 +149,23 @@ private: vehicle_global_position_s _gpos = {}; Vector<3> _vel_prev; Vector<3> _pos_acc; + + DataValidatorGroup _voter_gyro; + DataValidatorGroup _voter_accel; + DataValidatorGroup _voter_mag; + + /* Low pass filter for attitude rates */ + math::LowPassFilter2p _lp_roll_rate; + math::LowPassFilter2p _lp_pitch_rate; + hrt_abstime _vel_prev_t = 0; bool _inited = false; bool _data_good = false; + bool _failsafe = false; + bool _vibration_warning = false; + + int _mavlink_fd = -1; perf_counter_t _update_perf; perf_counter_t _loop_perf; @@ -160,7 +180,15 @@ private: }; -AttitudeEstimatorQ::AttitudeEstimatorQ() { +AttitudeEstimatorQ::AttitudeEstimatorQ() : + _voter_gyro(3), + _voter_accel(3), + _voter_mag(3), + _lp_roll_rate(250.0f, 20.0f), + _lp_pitch_rate(250.0f, 20.0f) +{ + _voter_mag.set_timeout(200000); + _params_handles.w_acc = param_find("ATT_W_ACC"); _params_handles.w_mag = param_find("ATT_W_MAG"); _params_handles.w_gyro_bias = param_find("ATT_W_GYRO_BIAS"); @@ -168,12 +196,14 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() { _params_handles.mag_decl_auto = param_find("ATT_MAG_DECL_A"); _params_handles.acc_comp = param_find("ATT_ACC_COMP"); _params_handles.bias_max = param_find("ATT_BIAS_MAX"); + _params_handles.vibe_thresh = param_find("ATT_VIBE_THRESH"); } /** * Destructor, also kills task. */ -AttitudeEstimatorQ::~AttitudeEstimatorQ() { +AttitudeEstimatorQ::~AttitudeEstimatorQ() +{ if (_control_task != -1) { /* task wakes up every 100ms or so at the longest */ _task_should_exit = true; @@ -196,14 +226,15 @@ AttitudeEstimatorQ::~AttitudeEstimatorQ() { attitude_estimator_q::instance = nullptr; } -int AttitudeEstimatorQ::start() { +int AttitudeEstimatorQ::start() +{ ASSERT(_control_task == -1); /* start the task */ _control_task = px4_task_spawn_cmd("attitude_estimator_q", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 2000, + 2100, (px4_main_t)&AttitudeEstimatorQ::task_main_trampoline, nullptr); @@ -215,12 +246,23 @@ int AttitudeEstimatorQ::start() { return OK; } -void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[]) { +void AttitudeEstimatorQ::print() +{ + warnx("gyro status:"); + _voter_gyro.print(); + warnx("accel status:"); + _voter_accel.print(); + warnx("mag status:"); + _voter_mag.print(); +} + +void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[]) +{ attitude_estimator_q::instance->task_main(); } -void AttitudeEstimatorQ::task_main() { - +void AttitudeEstimatorQ::task_main() +{ _sensors_sub = orb_subscribe(ORB_ID(sensor_combined)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); @@ -236,6 +278,10 @@ void AttitudeEstimatorQ::task_main() { while (!_task_should_exit) { int ret = px4_poll(fds, 1, 1000); + if (_mavlink_fd < 0) { + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + } + if (ret < 0) { // Poll error, sleep and try again usleep(10000); @@ -250,11 +296,71 @@ void AttitudeEstimatorQ::task_main() { // Update sensors sensor_combined_s sensors; if (!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)) { - _gyro.set(sensors.gyro_rad_s); - _accel.set(sensors.accelerometer_m_s2); - _mag.set(sensors.magnetometer_ga); + // Feed validator with recent sensor data + + for (unsigned i = 0; i < (sizeof(sensors.gyro_timestamp) / sizeof(sensors.gyro_timestamp[0])); i++) { + + /* ignore empty fields */ + if (sensors.gyro_timestamp[i] > 0) { + + float gyro[3]; + + for (unsigned j = 0; j < 3; j++) { + if (sensors.gyro_integral_dt[i] > 0) { + gyro[j] = (double)sensors.gyro_integral_rad[i * 3 + j] / (sensors.gyro_integral_dt[i] / 1e6); + } else { + /* fall back to angular rate */ + gyro[j] = sensors.gyro_rad_s[i * 3 + j]; + } + } + + _voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]); + } + _voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3], + sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]); + _voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3], + sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]); + } + + int best_gyro, best_accel, best_mag; + + // Get best measurement values + hrt_abstime curr_time = hrt_absolute_time(); + _gyro.set(_voter_gyro.get_best(curr_time, &best_gyro)); + _accel.set(_voter_accel.get_best(curr_time, &best_accel)); + _mag.set(_voter_mag.get_best(curr_time, &best_mag)); + + if (_accel.length() < 0.01f || _mag.length() < 0.01f) { + warnx("WARNING: degenerate accel / mag!"); + continue; + } _data_good = true; + + if (!_failsafe && (_voter_gyro.failover_count() > 0 || + _voter_accel.failover_count() > 0 || + _voter_mag.failover_count() > 0)) { + + _failsafe = true; + mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY"); + } + + if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold || + _voter_accel.get_vibration_factor(curr_time) > _vibration_warning_threshold || + _voter_mag.get_vibration_factor(curr_time) > _vibration_warning_threshold)) { + + if (_vibration_warning_timestamp == 0) { + _vibration_warning_timestamp = curr_time; + } else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000000) { + _vibration_warning = true; + mavlink_and_console_log_critical(_mavlink_fd, "HIGH VIBRATION! g: %d a: %d m: %d", + (int)(100 * _voter_gyro.get_vibration_factor(curr_time)), + (int)(100 * _voter_accel.get_vibration_factor(curr_time)), + (int)(100 * _voter_mag.get_vibration_factor(curr_time))); + } + } else { + _vibration_warning_timestamp = 0; + } } bool gpos_updated; @@ -311,8 +417,11 @@ void AttitudeEstimatorQ::task_main() { att.pitch = euler(1); att.yaw = euler(2); - att.rollspeed = _rates(0); - att.pitchspeed = _rates(1); + /* the complimentary filter should reflect the true system + * state, but we need smoother outputs for the control system + */ + att.rollspeed = _lp_roll_rate.apply(_rates(0)); + att.pitchspeed = _lp_pitch_rate.apply(_rates(1)); att.yawspeed = _rates(2); for (int i = 0; i < 3; i++) { @@ -328,6 +437,10 @@ void AttitudeEstimatorQ::task_main() { memcpy(&att.R[0], R.data, sizeof(att.R)); att.R_valid = true; + att.rate_vibration = _voter_gyro.get_vibration_factor(hrt_absolute_time()); + att.accel_vibration = _voter_accel.get_vibration_factor(hrt_absolute_time()); + att.mag_vibration = _voter_mag.get_vibration_factor(hrt_absolute_time()); + if (_att_pub == nullptr) { _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); } else { @@ -358,6 +471,7 @@ void AttitudeEstimatorQ::update_parameters(bool force) { param_get(_params_handles.acc_comp, &acc_comp_int); _acc_comp = acc_comp_int != 0; param_get(_params_handles.bias_max, &_bias_max); + param_get(_params_handles.vibe_thresh, &_vibration_warning_threshold); } } @@ -501,6 +615,7 @@ int attitude_estimator_q_main(int argc, char *argv[]) { if (!strcmp(argv[1], "status")) { if (attitude_estimator_q::instance) { + attitude_estimator_q::instance->print(); warnx("running"); return 0; diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c index 730d7a4e8b..d72f62dc61 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c @@ -108,3 +108,12 @@ PARAM_DEFINE_INT32(ATT_ACC_COMP, 2); * @unit rad/s */ PARAM_DEFINE_FLOAT(ATT_BIAS_MAX, 0.05f); + +/** + * Threshold (of RMS) to warn about high vibration levels + * + * @group Attitude Q estimator + * @min 0.001 + * @max 100 + */ +PARAM_DEFINE_FLOAT(ATT_VIBE_THRESH, 0.2f); diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index cd2897655d..8d2621ff62 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -226,11 +226,11 @@ BottleDrop::start() /* start the task */ _main_task = px4_task_spawn_cmd("bottle_drop", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT + 15, - 1500, - (main_t)&BottleDrop::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT + 15, + 1500, + (main_t)&BottleDrop::task_main_trampoline, + nullptr); if (_main_task < 0) { warn("task start failed"); @@ -256,6 +256,7 @@ BottleDrop::open_bay() if (_doors_opened == 0) { _doors_opened = hrt_absolute_time(); } + warnx("open doors"); actuators_publish(); @@ -326,8 +327,10 @@ BottleDrop::actuators_publish() } else { _actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators); + if (_actuator_pub != nullptr) { return OK; + } else { return -1; } @@ -459,6 +462,7 @@ BottleDrop::task_main() } orb_check(vehicle_global_position_sub, &updated); + if (updated) { /* copy global position */ orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos); @@ -478,12 +482,14 @@ BottleDrop::task_main() // Get wind estimate orb_check(_wind_estimate_sub, &updated); + if (updated) { orb_copy(ORB_ID(wind_estimate), _wind_estimate_sub, &wind); } // Get vehicle position orb_check(vehicle_global_position_sub, &updated); + if (updated) { // copy global position orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos); @@ -491,6 +497,7 @@ BottleDrop::task_main() // Get parameter updates orb_check(parameter_update_sub, &updated); + if (updated) { // copy global position orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); @@ -502,6 +509,7 @@ BottleDrop::task_main() } orb_check(_command_sub, &updated); + if (updated) { orb_copy(ORB_ID(vehicle_command), _command_sub, &_command); handle_command(&_command); @@ -515,25 +523,27 @@ BottleDrop::task_main() // Distance to drop position and angle error to approach vector // are relevant in all states greater than target valid (which calculates these positions) if (_drop_state > DROP_STATE_TARGET_VALID) { - distance_real = fabsf(get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, _drop_position.lat, _drop_position.lon)); + distance_real = fabsf(get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, _drop_position.lat, + _drop_position.lon)); float ground_direction = atan2f(_global_pos.vel_e, _global_pos.vel_n); - float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon); + float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, + flight_vector_e.lon); approach_error = _wrap_pi(ground_direction - approach_direction); if (counter % 90 == 0) { - mavlink_log_info(_mavlink_fd, "drop distance %u, heading error %u", (unsigned)distance_real, (unsigned)math::degrees(approach_error)); + mavlink_log_info(_mavlink_fd, "drop distance %u, heading error %u", (unsigned)distance_real, + (unsigned)math::degrees(approach_error)); } } switch (_drop_state) { - case DROP_STATE_INIT: - // do nothing - break; + case DROP_STATE_INIT: + // do nothing + break; - case DROP_STATE_TARGET_VALID: - { + case DROP_STATE_TARGET_VALID: { az = g; // acceleration in z direction[m/s^2] vz = 0; // velocity in z direction [m/s] @@ -626,27 +636,30 @@ BottleDrop::task_main() _onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &_onboard_mission); } - float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon); - mavlink_log_critical(_mavlink_fd, "position set, approach heading: %u", (unsigned)distance_real, (unsigned)math::degrees(approach_direction + M_PI_F)); + float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, + flight_vector_e.lon); + mavlink_log_critical(_mavlink_fd, "position set, approach heading: %u", (unsigned)distance_real, + (unsigned)math::degrees(approach_direction + M_PI_F)); _drop_state = DROP_STATE_TARGET_SET; } break; - case DROP_STATE_TARGET_SET: - { - float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon); + case DROP_STATE_TARGET_SET: { + float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, + flight_vector_e.lon); if (distance_wp2 < distance_real) { _onboard_mission.current_seq = 0; orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); + } else { // We're close enough - open the bay distance_open_door = math::max(10.0f, 3.0f * fabsf(t_door * groundspeed_body)); if (isfinite(distance_real) && distance_real < distance_open_door && - fabsf(approach_error) < math::radians(20.0f)) { + fabsf(approach_error) < math::radians(20.0f)) { open_bay(); _drop_state = DROP_STATE_BAY_OPEN; mavlink_log_info(_mavlink_fd, "#audio: opening bay"); @@ -655,52 +668,55 @@ BottleDrop::task_main() } break; - case DROP_STATE_BAY_OPEN: - { - if (_drop_approval) { - map_projection_project(&ref, _global_pos.lat, _global_pos.lon, &x_l, &y_l); - x_f = x_l + _global_pos.vel_n * dt_runs; - y_f = y_l + _global_pos.vel_e * dt_runs; - map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED); - future_distance = get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon); + case DROP_STATE_BAY_OPEN: { + if (_drop_approval) { + map_projection_project(&ref, _global_pos.lat, _global_pos.lon, &x_l, &y_l); + x_f = x_l + _global_pos.vel_n * dt_runs; + y_f = y_l + _global_pos.vel_e * dt_runs; + map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED); + future_distance = get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon); - if (isfinite(distance_real) && - (distance_real < precision) && ((distance_real < future_distance))) { - drop(); - _drop_state = DROP_STATE_DROPPED; - mavlink_log_info(_mavlink_fd, "#audio: payload dropped"); - } else { + if (isfinite(distance_real) && + (distance_real < precision) && ((distance_real < future_distance))) { + drop(); + _drop_state = DROP_STATE_DROPPED; + mavlink_log_info(_mavlink_fd, "#audio: payload dropped"); - float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon); + } else { - if (distance_wp2 < distance_real) { - _onboard_mission.current_seq = 0; - orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); - } + float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, + flight_vector_e.lon); + + if (distance_wp2 < distance_real) { + _onboard_mission.current_seq = 0; + orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); } } } - break; + } + break; - case DROP_STATE_DROPPED: - /* 2s after drop, reset and close everything again */ - if ((hrt_elapsed_time(&_doors_opened) > 2 * 1000 * 1000)) { - _drop_state = DROP_STATE_INIT; - _drop_approval = false; - lock_release(); - close_bay(); - mavlink_log_info(_mavlink_fd, "#audio: closing bay"); + case DROP_STATE_DROPPED: - // remove onboard mission - _onboard_mission.current_seq = -1; - _onboard_mission.count = 0; - orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); - } - break; + /* 2s after drop, reset and close everything again */ + if ((hrt_elapsed_time(&_doors_opened) > 2 * 1000 * 1000)) { + _drop_state = DROP_STATE_INIT; + _drop_approval = false; + lock_release(); + close_bay(); + mavlink_log_info(_mavlink_fd, "#audio: closing bay"); - case DROP_STATE_BAY_CLOSED: - // do nothing - break; + // remove onboard mission + _onboard_mission.current_seq = -1; + _onboard_mission.count = 0; + orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); + } + + break; + + case DROP_STATE_BAY_CLOSED: + // do nothing + break; } counter++; @@ -726,6 +742,7 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) { switch (cmd->command) { case vehicle_command_s::VEHICLE_CMD_CUSTOM_0: + /* * param1 and param2 set to 1: open and drop * param1 set to 1: open @@ -775,7 +792,7 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) _target_position.alt = cmd->param7; _drop_state = DROP_STATE_TARGET_VALID; mavlink_log_info(_mavlink_fd, "got target: %8.4f, %8.4f, %8.4f", (double)_target_position.lat, - (double)_target_position.lon, (double)_target_position.alt); + (double)_target_position.lon, (double)_target_position.alt); map_projection_init(&ref, _target_position.lat, _target_position.lon); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); break; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 397083a3b5..f9fdd6dadb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1220,13 +1220,17 @@ int commander_thread_main(int argc, char *argv[]) } // Run preflight check + int32_t rc_in_off = 0; param_get(_param_autostart_id, &autostart_id); if (is_hil_setup(autostart_id)) { // HIL configuration selected: real sensors will be disabled status.condition_system_sensors_initialized = false; set_tune_override(TONE_STARTUP_TUNE); //normal boot tune } else { - status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !status.rc_input_mode, !status.circuit_breaker_engaged_gpsfailure_check); + param_get(_param_rc_in_off, &rc_in_off); + status.rc_input_mode = rc_in_off; + status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, + checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status.circuit_breaker_engaged_gpsfailure_check); if (!status.condition_system_sensors_initialized) { set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune } @@ -1242,7 +1246,6 @@ int commander_thread_main(int argc, char *argv[]) int32_t datalink_loss_enabled = false; int32_t datalink_loss_timeout = 10; float rc_loss_timeout = 0.5; - int32_t rc_in_off = 0; int32_t datalink_regain_timeout = 0; /* Thresholds for engine failure detection */ @@ -1435,7 +1438,7 @@ int commander_thread_main(int argc, char *argv[]) * vertical separation from other airtraffic the operator has to know when the * barometer is inoperational. * */ - if (hrt_elapsed_time(&sensors.baro_timestamp) < FAILSAFE_DEFAULT_TIMEOUT) { + if (hrt_elapsed_time(&sensors.baro_timestamp[0]) < FAILSAFE_DEFAULT_TIMEOUT) { /* handle the case where baro was regained */ if (status.barometer_failure) { status.barometer_failure = false; @@ -1863,17 +1866,16 @@ int commander_thread_main(int argc, char *argv[]) } /* RC input check */ - if (!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF) && !status.rc_input_blocked && sp_man.timestamp != 0 && - hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) { + if (!status.rc_input_blocked && sp_man.timestamp != 0 && + (hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f))) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; - mavlink_log_info(mavlink_fd, "Detected radio control"); status_changed = true; } else { if (status.rc_signal_lost) { - mavlink_log_info(mavlink_fd, "RC SIGNAL REGAINED after %llums", + mavlink_log_info(mavlink_fd, "MANUAL CONTROL REGAINED after %llums", (hrt_absolute_time() - status.rc_signal_lost_timestamp) / 1000); status_changed = true; } @@ -1913,8 +1915,7 @@ int commander_thread_main(int argc, char *argv[]) } /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ - if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY && - sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { + if (sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* we check outside of the transition function here because the requirement @@ -1925,13 +1926,15 @@ int commander_thread_main(int argc, char *argv[]) (status.main_state != vehicle_status_s::MAIN_STATE_STAB)) { print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); - } else { + } else if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; } + } else { + print_reject_arm("NOT ARMING: Configuration error"); } stick_on_counter = 0; @@ -1978,8 +1981,8 @@ int commander_thread_main(int argc, char *argv[]) } } else { - if (!status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST (at t=%llums)", hrt_absolute_time() / 1000); + if (!status.rc_input_blocked && !status.rc_signal_lost) { + mavlink_log_critical(mavlink_fd, "MANUAL CONTROL LOST (at t=%llums)", hrt_absolute_time() / 1000); status.rc_signal_lost = true; status.rc_signal_lost_timestamp = sp_man.timestamp; status_changed = true; diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index c16a606711..e4fcea2c69 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -55,6 +55,6 @@ MODULE_STACKSIZE = 5000 MAXOPTIMIZATION = -Os ifeq ($(PX4_TARGET_OS),nuttx) -EXTRACXXFLAGS = -Wframe-larger-than=2200 +EXTRACXXFLAGS = -Wframe-larger-than=2400 endif diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 67864ebf38..9227a141de 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -245,9 +245,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR) && (!status->condition_system_sensors_initialized)) { - if (!fRunPreArmChecks) { - mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection"); - } + mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection"); feedback_provided = true; valid_transition = false; status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; diff --git a/src/modules/controllib/block/BlockParam.cpp b/src/modules/controllib/block/BlockParam.cpp index 532a037d40..80f1bae1b0 100644 --- a/src/modules/controllib/block/BlockParam.cpp +++ b/src/modules/controllib/block/BlockParam.cpp @@ -97,6 +97,11 @@ void BlockParam::update() { if (_handle != PARAM_INVALID) param_get(_handle, &_val); } +template +void BlockParam::commit() { + if (_handle != PARAM_INVALID) param_set(_handle, &_val); +} + template BlockParam::~BlockParam() {}; diff --git a/src/modules/controllib/block/BlockParam.hpp b/src/modules/controllib/block/BlockParam.hpp index cab28c65fd..db035f9f91 100644 --- a/src/modules/controllib/block/BlockParam.hpp +++ b/src/modules/controllib/block/BlockParam.hpp @@ -79,6 +79,7 @@ public: BlockParam(Block *block, const char *name, bool parent_prefix = true); T get(); + void commit(); void set(T val); void update(); virtual ~BlockParam(); diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 1594e340c8..6c9e9f0ee6 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -63,7 +63,8 @@ __EXPORT int dataman_main(int argc, char *argv[]); __EXPORT ssize_t dm_read(dm_item_t item, unsigned char index, void *buffer, size_t buflen); -__EXPORT ssize_t dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const void *buffer, size_t buflen); +__EXPORT ssize_t dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const void *buffer, + size_t buflen); __EXPORT int dm_clear(dm_item_t item); __EXPORT void dm_lock(dm_item_t item); __EXPORT void dm_unlock(dm_item_t item); @@ -188,32 +189,40 @@ create_work_item(void) /* Try to reuse item from free item queue */ lock_queue(&g_free_q); - if ((item = (work_q_item_t *)sq_remfirst(&(g_free_q.q)))) + if ((item = (work_q_item_t *)sq_remfirst(&(g_free_q.q)))) { g_free_q.size--; + } unlock_queue(&g_free_q); /* If we there weren't any free items then obtain memory for a new ones */ if (item == NULL) { item = (work_q_item_t *)malloc(k_work_item_allocation_chunk_size * sizeof(work_q_item_t)); + if (item) { item->first = 1; lock_queue(&g_free_q); + for (size_t i = 1; i < k_work_item_allocation_chunk_size; i++) { (item + i)->first = 0; sq_addfirst(&(item + i)->link, &(g_free_q.q)); } + /* Update the queue size and potentially the maximum queue size */ g_free_q.size += k_work_item_allocation_chunk_size - 1; - if (g_free_q.size > g_free_q.max_size) + + if (g_free_q.size > g_free_q.max_size) { g_free_q.max_size = g_free_q.size; + } + unlock_queue(&g_free_q); } } /* If we got one then lock the item*/ - if (item) - sem_init(&item->wait_sem, 1, 0); /* Caller will wait on this... initially locked */ + if (item) { + sem_init(&item->wait_sem, 1, 0); /* Caller will wait on this... initially locked */ + } /* return the item pointer, or NULL if all failed */ return item; @@ -230,8 +239,9 @@ destroy_work_item(work_q_item_t *item) sq_addfirst(&item->link, &(g_free_q.q)); /* Update the queue size and potentially the maximum queue size */ - if (++g_free_q.size > g_free_q.max_size) + if (++g_free_q.size > g_free_q.max_size) { g_free_q.max_size = g_free_q.size; + } unlock_queue(&g_free_q); } @@ -244,8 +254,9 @@ dequeue_work_item(void) /* retrieve the 1st item on the work queue */ lock_queue(&g_work_q); - if ((work = (work_q_item_t *)sq_remfirst(&g_work_q.q))) + if ((work = (work_q_item_t *)sq_remfirst(&g_work_q.q))) { g_work_q.size--; + } unlock_queue(&g_work_q); return work; @@ -259,8 +270,9 @@ enqueue_work_item_and_wait_for_result(work_q_item_t *item) sq_addlast(&item->link, &(g_work_q.q)); /* Adjust the queue size and potentially the maximum queue size */ - if (++g_work_q.size > g_work_q.max_size) + if (++g_work_q.size > g_work_q.max_size) { g_work_q.max_size = g_work_q.size; + } unlock_queue(&g_work_q); @@ -283,12 +295,14 @@ calculate_offset(dm_item_t item, unsigned char index) { /* Make sure the item type is valid */ - if (item >= DM_KEY_NUM_KEYS) + if (item >= DM_KEY_NUM_KEYS) { return -1; + } /* Make sure the index for this item type is valid */ - if (index >= g_per_item_max_index[item]) + if (index >= g_per_item_max_index[item]) { return -1; + } /* Calculate and return the item index based on type and index */ return g_key_offsets[item] + (index * k_sector_size); @@ -317,33 +331,39 @@ _write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const v offset = calculate_offset(item, index); /* If item type or index out of range, return error */ - if (offset < 0) + if (offset < 0) { return -1; + } /* Make sure caller has not given us more data than we can handle */ - if (count > DM_MAX_DATA_SIZE) + if (count > DM_MAX_DATA_SIZE) { return -1; + } /* Write out the data, prefixed with length and persistence level */ buffer[0] = count; buffer[1] = persistence; buffer[2] = 0; buffer[3] = 0; + if (count > 0) { memcpy(buffer + DM_SECTOR_HDR_SIZE, buf, count); } + count += DM_SECTOR_HDR_SIZE; len = -1; /* Seek to the right spot in the data manager file and write the data item */ if (lseek(g_task_fd, offset, SEEK_SET) == offset) - if ((len = write(g_task_fd, buffer, count)) == count) - fsync(g_task_fd); /* Make sure data is written to physical media */ + if ((len = write(g_task_fd, buffer, count)) == count) { + fsync(g_task_fd); /* Make sure data is written to physical media */ + } /* Make sure the write succeeded */ - if (len != count) + if (len != count) { return -1; + } /* All is well... return the number of user data written */ return count - DM_SECTOR_HDR_SIZE; @@ -360,32 +380,38 @@ _read(dm_item_t item, unsigned char index, void *buf, size_t count) offset = calculate_offset(item, index); /* If item type or index out of range, return error */ - if (offset < 0) + if (offset < 0) { return -1; + } /* Make sure the caller hasn't asked for more data than we can handle */ - if (count > DM_MAX_DATA_SIZE) + if (count > DM_MAX_DATA_SIZE) { return -1; + } /* Read the prefix and data */ len = -1; - if (lseek(g_task_fd, offset, SEEK_SET) == offset) + if (lseek(g_task_fd, offset, SEEK_SET) == offset) { len = read(g_task_fd, buffer, count + DM_SECTOR_HDR_SIZE); + } /* Check for read error */ - if (len < 0) + if (len < 0) { return -1; + } /* A zero length entry is a empty entry */ - if (len == 0) + if (len == 0) { buffer[0] = 0; + } /* See if we got data */ if (buffer[0] > 0) { /* We got more than requested!!! */ - if (buffer[0] > count) + if (buffer[0] > count) { return -1; + } /* Looks good, copy it to the caller's buffer */ memcpy(buf, buffer + DM_SECTOR_HDR_SIZE, buffer[0]); @@ -404,8 +430,9 @@ _clear(dm_item_t item) int offset = calculate_offset(item, 0); /* Check for item type out of range */ - if (offset < 0) + if (offset < 0) { return -1; + } /* Clear all items of this type */ for (i = 0; (unsigned)i < g_per_item_max_index[item]; i++) { @@ -417,8 +444,9 @@ _clear(dm_item_t item) } /* Avoid SD flash wear by only doing writes where necessary */ - if (read(g_task_fd, buf, 1) < 1) + if (read(g_task_fd, buf, 1) < 1) { break; + } /* If item has length greater than 0 it needs to be overwritten */ if (buf[0]) { @@ -519,12 +547,14 @@ dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const work_q_item_t *work; /* Make sure data manager has been started and is not shutting down */ - if ((g_fd < 0) || g_task_should_exit) + if ((g_fd < 0) || g_task_should_exit) { return -1; + } /* get a work item and queue up a write request */ - if ((work = create_work_item()) == NULL) + if ((work = create_work_item()) == NULL) { return -1; + } work->func = dm_write_func; work->write_params.item = item; @@ -544,12 +574,14 @@ dm_read(dm_item_t item, unsigned char index, void *buf, size_t count) work_q_item_t *work; /* Make sure data manager has been started and is not shutting down */ - if ((g_fd < 0) || g_task_should_exit) + if ((g_fd < 0) || g_task_should_exit) { return -1; + } /* get a work item and queue up a read request */ - if ((work = create_work_item()) == NULL) + if ((work = create_work_item()) == NULL) { return -1; + } work->func = dm_read_func; work->read_params.item = item; @@ -567,12 +599,14 @@ dm_clear(dm_item_t item) work_q_item_t *work; /* Make sure data manager has been started and is not shutting down */ - if ((g_fd < 0) || g_task_should_exit) + if ((g_fd < 0) || g_task_should_exit) { return -1; + } /* get a work item and queue up a clear request */ - if ((work = create_work_item()) == NULL) + if ((work = create_work_item()) == NULL) { return -1; + } work->func = dm_clear_func; work->clear_params.item = item; @@ -585,10 +619,14 @@ __EXPORT void dm_lock(dm_item_t item) { /* Make sure data manager has been started and is not shutting down */ - if ((g_fd < 0) || g_task_should_exit) + if ((g_fd < 0) || g_task_should_exit) { return; - if (item >= DM_KEY_NUM_KEYS) + } + + if (item >= DM_KEY_NUM_KEYS) { return; + } + if (g_item_locks[item]) { sem_wait(g_item_locks[item]); } @@ -598,10 +636,14 @@ __EXPORT void dm_unlock(dm_item_t item) { /* Make sure data manager has been started and is not shutting down */ - if ((g_fd < 0) || g_task_should_exit) + if ((g_fd < 0) || g_task_should_exit) { return; - if (item >= DM_KEY_NUM_KEYS) + } + + if (item >= DM_KEY_NUM_KEYS) { return; + } + if (g_item_locks[item]) { sem_post(g_item_locks[item]); } @@ -614,12 +656,14 @@ dm_restart(dm_reset_reason reason) work_q_item_t *work; /* Make sure data manager has been started and is not shutting down */ - if ((g_fd < 0) || g_task_should_exit) + if ((g_fd < 0) || g_task_should_exit) { return -1; + } /* get a work item and queue up a restart request */ - if ((work = create_work_item()) == NULL) + if ((work = create_work_item()) == NULL) { return -1; + } work->func = dm_restart_func; work->restart_params.reason = reason; @@ -636,18 +680,23 @@ task_main(int argc, char *argv[]) /* Initialize global variables */ g_key_offsets[0] = 0; - for (unsigned i = 0; i < (DM_KEY_NUM_KEYS - 1); i++) + for (unsigned i = 0; i < (DM_KEY_NUM_KEYS - 1); i++) { g_key_offsets[i + 1] = g_key_offsets[i] + (g_per_item_max_index[i] * k_sector_size); + } unsigned max_offset = g_key_offsets[DM_KEY_NUM_KEYS - 1] + (g_per_item_max_index[DM_KEY_NUM_KEYS - 1] * k_sector_size); - for (unsigned i = 0; i < dm_number_of_funcs; i++) + for (unsigned i = 0; i < dm_number_of_funcs; i++) { g_func_counts[i] = 0; + } /* Initialize the item type locks, for now only DM_KEY_MISSION_STATE supports locking */ sem_init(&g_sys_state_mutex, 1, 1); /* Initially unlocked */ - for (unsigned i = 0; i < DM_KEY_NUM_KEYS; i++) + + for (unsigned i = 0; i < DM_KEY_NUM_KEYS; i++) { g_item_locks[i] = NULL; + } + g_item_locks[DM_KEY_MISSION_STATE] = &g_sys_state_mutex; g_task_should_exit = false; @@ -659,14 +708,17 @@ task_main(int argc, char *argv[]) /* See if the data manage file exists and is a multiple of the sector size */ g_task_fd = open(k_data_manager_device_path, O_RDONLY | O_BINARY); + if (g_task_fd >= 0) { /* File exists, check its size */ int file_size = lseek(g_task_fd, 0, SEEK_END); + if ((file_size % k_sector_size) != 0) { warnx("Incompatible data manager file %s, resetting it", k_data_manager_device_path); warnx("Size: %u, sector size: %d", file_size, k_sector_size); close(g_task_fd); unlink(k_data_manager_device_path); + } else { close(g_task_fd); } @@ -693,16 +745,20 @@ task_main(int argc, char *argv[]) printf("dataman: "); /* see if we need to erase any items based on restart type */ int sys_restart_val; + if (param_get(param_find("SYS_RESTART_TYPE"), &sys_restart_val) == OK) { if (sys_restart_val == DM_INIT_REASON_POWER_ON) { printf("Power on restart"); _restart(DM_INIT_REASON_POWER_ON); + } else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) { printf("In flight restart"); _restart(DM_INIT_REASON_IN_FLIGHT); + } else { printf("Unknown restart"); } + } else { printf("Unknown restart"); } @@ -739,7 +795,8 @@ task_main(int argc, char *argv[]) case dm_write_func: g_func_counts[dm_write_func]++; work->result = - _write(work->write_params.item, work->write_params.index, work->write_params.persistence, work->write_params.buf, work->write_params.count); + _write(work->write_params.item, work->write_params.index, work->write_params.persistence, work->write_params.buf, + work->write_params.count); break; case dm_read_func: @@ -768,8 +825,9 @@ task_main(int argc, char *argv[]) } /* time to go???? */ - if ((g_task_should_exit) && (g_fd < 0)) + if ((g_task_should_exit) && (g_fd < 0)) { break; + } } close(g_task_fd); @@ -777,10 +835,13 @@ task_main(int argc, char *argv[]) /* The work queue is now empty, empty the free queue */ for (;;) { - if ((work = (work_q_item_t *)sq_remfirst(&(g_free_q.q))) == NULL) + if ((work = (work_q_item_t *)sq_remfirst(&(g_free_q.q))) == NULL) { break; - if (work->first) + } + + if (work->first) { free(work); + } } destroy_q(&g_work_q); @@ -850,11 +911,12 @@ dataman_main(int argc, char *argv[]) warnx("dataman already running"); return -1; } - if (argc == 4 && strcmp(argv[2],"-f") == 0) { + + if (argc == 4 && strcmp(argv[2], "-f") == 0) { k_data_manager_device_path = strdup(argv[3]); warnx("dataman file set to: %s\n", k_data_manager_device_path); - } - else { + + } else { k_data_manager_device_path = strdup(default_device_path); } @@ -881,14 +943,17 @@ dataman_main(int argc, char *argv[]) stop(); free(k_data_manager_device_path); k_data_manager_device_path = NULL; - } - else if (!strcmp(argv[1], "status")) + + } else if (!strcmp(argv[1], "status")) { status(); - else if (!strcmp(argv[1], "poweronrestart")) + + } else if (!strcmp(argv[1], "poweronrestart")) { dm_restart(DM_INIT_REASON_POWER_ON); - else if (!strcmp(argv[1], "inflightrestart")) + + } else if (!strcmp(argv[1], "inflightrestart")) { dm_restart(DM_INIT_REASON_IN_FLIGHT); - else { + + } else { usage(); return -1; } diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index 7ea93b87e9..a29a29c735 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -47,92 +47,92 @@ extern "C" { #endif - /** Types of items that the data manager can store */ - typedef enum { - DM_KEY_SAFE_POINTS = 0, /* Safe points coordinates, safe point 0 is home point */ - DM_KEY_FENCE_POINTS, /* Fence vertex coordinates */ - DM_KEY_WAYPOINTS_OFFBOARD_0, /* Mission way point coordinates sent over mavlink */ - DM_KEY_WAYPOINTS_OFFBOARD_1, /* (alernate between 0 and 1) */ - DM_KEY_WAYPOINTS_ONBOARD, /* Mission way point coordinates generated onboard */ - DM_KEY_MISSION_STATE, /* Persistent mission state */ - DM_KEY_NUM_KEYS /* Total number of item types defined */ - } dm_item_t; +/** Types of items that the data manager can store */ +typedef enum { + DM_KEY_SAFE_POINTS = 0, /* Safe points coordinates, safe point 0 is home point */ + DM_KEY_FENCE_POINTS, /* Fence vertex coordinates */ + DM_KEY_WAYPOINTS_OFFBOARD_0, /* Mission way point coordinates sent over mavlink */ + DM_KEY_WAYPOINTS_OFFBOARD_1, /* (alernate between 0 and 1) */ + DM_KEY_WAYPOINTS_ONBOARD, /* Mission way point coordinates generated onboard */ + DM_KEY_MISSION_STATE, /* Persistent mission state */ + DM_KEY_NUM_KEYS /* Total number of item types defined */ +} dm_item_t; - #define DM_KEY_WAYPOINTS_OFFBOARD(_id) (_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1) +#define DM_KEY_WAYPOINTS_OFFBOARD(_id) (_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1) - /** The maximum number of instances for each item type */ - enum { - DM_KEY_SAFE_POINTS_MAX = 8, - #ifdef __cplusplus - DM_KEY_FENCE_POINTS_MAX = fence_s::GEOFENCE_MAX_VERTICES, - #else - DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES, - #endif - DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED, - DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED, - DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED, - DM_KEY_MISSION_STATE_MAX = 1 - }; +/** The maximum number of instances for each item type */ +enum { + DM_KEY_SAFE_POINTS_MAX = 8, +#ifdef __cplusplus + DM_KEY_FENCE_POINTS_MAX = fence_s::GEOFENCE_MAX_VERTICES, +#else + DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES, +#endif + DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED, + DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED, + DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED, + DM_KEY_MISSION_STATE_MAX = 1 +}; - /** Data persistence levels */ - typedef enum { - DM_PERSIST_POWER_ON_RESET = 0, /* Data survives all resets */ - DM_PERSIST_IN_FLIGHT_RESET, /* Data survives in-flight resets only */ - DM_PERSIST_VOLATILE /* Data does not survive resets */ - } dm_persitence_t; +/** Data persistence levels */ +typedef enum { + DM_PERSIST_POWER_ON_RESET = 0, /* Data survives all resets */ + DM_PERSIST_IN_FLIGHT_RESET, /* Data survives in-flight resets only */ + DM_PERSIST_VOLATILE /* Data does not survive resets */ +} dm_persitence_t; - /** The reason for the last reset */ - typedef enum { - DM_INIT_REASON_POWER_ON = 0, /* Data survives resets */ - DM_INIT_REASON_IN_FLIGHT, /* Data survives in-flight resets only */ - DM_INIT_REASON_VOLATILE /* Data does not survive reset */ - } dm_reset_reason; +/** The reason for the last reset */ +typedef enum { + DM_INIT_REASON_POWER_ON = 0, /* Data survives resets */ + DM_INIT_REASON_IN_FLIGHT, /* Data survives in-flight resets only */ + DM_INIT_REASON_VOLATILE /* Data does not survive reset */ +} dm_reset_reason; - /** Maximum size in bytes of a single item instance */ - #define DM_MAX_DATA_SIZE 124 +/** Maximum size in bytes of a single item instance */ +#define DM_MAX_DATA_SIZE 124 - /** Retrieve from the data manager store */ - __EXPORT ssize_t - dm_read( - dm_item_t item, /* The item type to retrieve */ - unsigned char index, /* The index of the item */ - void *buffer, /* Pointer to caller data buffer */ - size_t buflen /* Length in bytes of data to retrieve */ - ); +/** Retrieve from the data manager store */ +__EXPORT ssize_t +dm_read( + dm_item_t item, /* The item type to retrieve */ + unsigned char index, /* The index of the item */ + void *buffer, /* Pointer to caller data buffer */ + size_t buflen /* Length in bytes of data to retrieve */ +); - /** write to the data manager store */ - __EXPORT ssize_t - dm_write( - dm_item_t item, /* The item type to store */ - unsigned char index, /* The index of the item */ - dm_persitence_t persistence, /* The persistence level of this item */ - const void *buffer, /* Pointer to caller data buffer */ - size_t buflen /* Length in bytes of data to retrieve */ - ); +/** write to the data manager store */ +__EXPORT ssize_t +dm_write( + dm_item_t item, /* The item type to store */ + unsigned char index, /* The index of the item */ + dm_persitence_t persistence, /* The persistence level of this item */ + const void *buffer, /* Pointer to caller data buffer */ + size_t buflen /* Length in bytes of data to retrieve */ +); - /** Lock all items of this type */ - __EXPORT void - dm_lock( - dm_item_t item /* The item type to clear */ - ); +/** Lock all items of this type */ +__EXPORT void +dm_lock( + dm_item_t item /* The item type to clear */ +); - /** Unlock all items of this type */ - __EXPORT void - dm_unlock( - dm_item_t item /* The item type to clear */ - ); +/** Unlock all items of this type */ +__EXPORT void +dm_unlock( + dm_item_t item /* The item type to clear */ +); - /** Erase all items of this type */ - __EXPORT int - dm_clear( - dm_item_t item /* The item type to clear */ - ); +/** Erase all items of this type */ +__EXPORT int +dm_clear( + dm_item_t item /* The item type to clear */ +); - /** Tell the data manager about the type of the last reset */ - __EXPORT int - dm_restart( - dm_reset_reason restart_type /* The last reset type */ - ); +/** Tell the data manager about the type of the last reset */ +__EXPORT int +dm_restart( + dm_reset_reason restart_type /* The last reset type */ +); #ifdef __cplusplus } diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index 28b23f2069..e4d924396a 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -52,7 +52,6 @@ #include #include #include -#include #include #include #include @@ -70,11 +69,16 @@ #include #include +#include +#include "estimator_22states.h" + +#include +#include //Forward declaration class AttPosEKF; -class AttitudePositionEstimatorEKF +class AttitudePositionEstimatorEKF : public control::SuperBlock { public: /** @@ -168,9 +172,8 @@ private: struct vehicle_land_detected_s _landDetector; struct actuator_armed_s _armed; - struct gyro_scale _gyro_offsets[3]; - struct accel_scale _accel_offsets[3]; - struct mag_scale _mag_offsets[3]; + hrt_abstime _last_accel; + hrt_abstime _last_mag; struct sensor_combined_s _sensor_combined; @@ -179,6 +182,8 @@ private: float _filter_ref_offset; /**< offset between initial baro reference and GPS init baro altitude */ float _baro_gps_offset; /**< offset between baro altitude (at GPS init time) and GPS altitude */ hrt_abstime _last_debug_print = 0; + float _vibration_warning_threshold = 1.0f; + hrt_abstime _vibration_warning_timestamp = 0; perf_counter_t _loop_perf; ///< loop performance counter perf_counter_t _loop_intvl; ///< loop rate counter @@ -198,14 +203,16 @@ private: bool _gps_initialized; hrt_abstime _filter_start_time; hrt_abstime _last_sensor_timestamp; - hrt_abstime _last_run; hrt_abstime _distance_last_valid; - bool _gyro_valid; - bool _accel_valid; - bool _mag_valid; + DataValidatorGroup _voter_gyro; + DataValidatorGroup _voter_accel; + DataValidatorGroup _voter_mag; int _gyro_main; ///< index of the main gyroscope int _accel_main; ///< index of the main accelerometer int _mag_main; ///< index of the main magnetometer + bool _data_good; ///< all required filter data is ok + bool _failsafe; ///< failsafe on one of the sensors + bool _vibration_warning; ///< high vibration levels detected bool _ekf_logging; ///< log EKF state unsigned _debug; ///< debug level - default 0 @@ -216,6 +223,10 @@ private: int _mavlink_fd; + control::BlockParamFloat _mag_offset_x; + control::BlockParamFloat _mag_offset_y; + control::BlockParamFloat _mag_offset_z; + struct { int32_t vel_delay_ms; int32_t pos_delay_ms; diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 5f3634d22b..d23dd1b9e9 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -68,7 +68,6 @@ #include #include -static uint64_t IMUmsec = 0; static uint64_t IMUusec = 0; //Constants @@ -88,13 +87,18 @@ static constexpr float EPV_LARGE_VALUE = 1000.0f; */ extern "C" __EXPORT int ekf_att_pos_estimator_main(int argc, char *argv[]); -__EXPORT uint32_t millis(); - -__EXPORT uint64_t getMicros(); +uint32_t millis(); +uint64_t getMicros(); +uint32_t getMillis(); uint32_t millis() { - return IMUmsec; + return getMillis(); +} + +uint32_t getMillis() +{ + return getMicros() / 1000; } uint64_t getMicros() @@ -115,6 +119,7 @@ AttitudePositionEstimatorEKF *g_estimator = nullptr; } AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : + SuperBlock(NULL, "PE"), _task_should_exit(false), _task_running(false), _estimator_task(-1), @@ -133,84 +138,88 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _landDetectorSub(-1), _armedSub(-1), -/* publications */ + /* publications */ _att_pub(nullptr), _global_pos_pub(nullptr), _local_pos_pub(nullptr), _estimator_status_pub(nullptr), _wind_pub(nullptr), - _att({}), - _gyro({}), - _accel({}), - _mag({}), - _airspeed({}), - _baro({}), - _vstatus({}), - _global_pos({}), - _local_pos({}), - _gps({}), - _wind({}), - _distance {}, - _landDetector {}, - _armed {}, + _att{}, + _gyro{}, + _accel{}, + _mag{}, + _airspeed{}, + _baro{}, + _vstatus{}, + _global_pos{}, + _local_pos{}, + _gps{}, + _wind{}, + _distance{}, + _landDetector{}, + _armed{}, - _gyro_offsets({}), - _accel_offsets({}), - _mag_offsets({}), + _last_accel(0), + _last_mag(0), - _sensor_combined {}, + _sensor_combined{}, - _pos_ref{}, - _filter_ref_offset(0.0f), - _baro_gps_offset(0.0f), + _pos_ref{}, + _filter_ref_offset(0.0f), + _baro_gps_offset(0.0f), - /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")), - _loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")), - _perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")), - _perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")), - _perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")), - _perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")), - _perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")), - _perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")), + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")), + _loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")), + _perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")), + _perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")), + _perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")), + _perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")), + _perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")), + _perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")), - /* states */ - _gps_alt_filt(0.0f), - _baro_alt_filt(0.0f), - _covariancePredictionDt(0.0f), - _gpsIsGood(false), - _previousGPSTimestamp(0), - _baro_init(false), - _gps_initialized(false), - _filter_start_time(0), - _last_sensor_timestamp(0), - _last_run(0), - _distance_last_valid(0), - _gyro_valid(false), - _accel_valid(false), - _mag_valid(false), - _gyro_main(0), - _accel_main(0), - _mag_main(0), - _ekf_logging(true), - _debug(0), + /* states */ + _gps_alt_filt(0.0f), + _baro_alt_filt(0.0f), + _covariancePredictionDt(0.0f), + _gpsIsGood(false), + _previousGPSTimestamp(0), + _baro_init(false), + _gps_initialized(false), + _filter_start_time(0), + _last_sensor_timestamp(hrt_absolute_time()), + _distance_last_valid(0), + _voter_gyro(3), + _voter_accel(3), + _voter_mag(3), + _gyro_main(-1), + _accel_main(-1), + _mag_main(-1), + _data_good(false), + _failsafe(false), + _vibration_warning(false), + _ekf_logging(true), + _debug(0), - _newHgtData(false), - _newAdsData(false), - _newDataMag(false), - _newRangeData(false), + _newHgtData(false), + _newAdsData(false), + _newDataMag(false), + _newRangeData(false), - _mavlink_fd(-1), - _parameters{}, - _parameter_handles{}, - _ekf(nullptr), + _mavlink_fd(-1), + _mag_offset_x(this, "MAGB_X"), + _mag_offset_y(this, "MAGB_Y"), + _mag_offset_z(this, "MAGB_Z"), + _parameters{}, + _parameter_handles{}, + _ekf(nullptr), - _LP_att_P(100.0f, 10.0f), - _LP_att_Q(100.0f, 10.0f), - _LP_att_R(100.0f, 10.0f) + _LP_att_P(250.0f, 20.0f), + _LP_att_Q(250.0f, 20.0f), + _LP_att_R(250.0f, 20.0f) { - _last_run = hrt_absolute_time(); + _voter_mag.set_timeout(200000); _parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS"); _parameter_handles.pos_delay_ms = param_find("PE_POS_DELAY_MS"); @@ -237,48 +246,6 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : /* fetch initial parameter values */ parameters_update(); - - /* get offsets */ - int fd, res; - - for (unsigned s = 0; s < 3; s++) { - char str[30]; - (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); - fd = px4_open(str, O_RDONLY); - - if (fd >= 0) { - res = px4_ioctl(fd, GYROIOCGSCALE, (long unsigned int)&_gyro_offsets[s]); - px4_close(fd); - - if (res) { - PX4_WARN("G%u SCALE FAIL", s); - } - } - - (void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s); - fd = px4_open(str, O_RDONLY); - - if (fd >= 0) { - res = px4_ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&_accel_offsets[s]); - px4_close(fd); - - if (res) { - PX4_WARN("A%u SCALE FAIL", s); - } - } - - (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s); - fd = px4_open(str, O_RDONLY); - - if (fd >= 0) { - res = px4_ioctl(fd, MAGIOCGSCALE, (long unsigned int)&_mag_offsets[s]); - px4_close(fd); - - if (res) { - PX4_WARN("M%u SCALE FAIL", s); - } - } - } } AttitudePositionEstimatorEKF::~AttitudePositionEstimatorEKF() @@ -317,6 +284,8 @@ int AttitudePositionEstimatorEKF::enable_logging(bool logging) int AttitudePositionEstimatorEKF::parameters_update() { + /* update C++ param system */ + updateParams(); param_get(_parameter_handles.vel_delay_ms, &(_parameters.vel_delay_ms)); param_get(_parameter_handles.pos_delay_ms, &(_parameters.pos_delay_ms)); @@ -354,6 +323,13 @@ int AttitudePositionEstimatorEKF::parameters_update() _ekf->accelProcessNoise = _parameters.acc_pnoise; _ekf->airspeedMeasurementSigma = _parameters.eas_noise; _ekf->rngFinderPitch = 0.0f; // XXX base on SENS_BOARD_Y_OFF + #if 0 + // Initially disable loading until + // convergence is flight-test proven + _ekf->magBias.x = _mag_offset_x.get(); + _ekf->magBias.y = _mag_offset_y.get(); + _ekf->magBias.z = _mag_offset_z.get(); + #endif } return OK; @@ -366,12 +342,24 @@ void AttitudePositionEstimatorEKF::vehicle_status_poll() /* Check HIL state if vehicle status has changed */ orb_check(_vstatus_sub, &vstatus_updated); + bool landed = _vstatus.condition_landed; + if (vstatus_updated) { orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus); - //Tell EKF that the vehicle is a fixed wing or multi-rotor + // Tell EKF that the vehicle is a fixed wing or multi-rotor _ekf->setIsFixedWing(!_vstatus.is_rotary_wing); + + // Save params on landed + if (!landed && _vstatus.condition_landed) { + _mag_offset_x.set(_ekf->magBias.x); + _mag_offset_x.commit(); + _mag_offset_y.set(_ekf->magBias.y); + _mag_offset_y.commit(); + _mag_offset_z.set(_ekf->magBias.z); + _mag_offset_z.commit(); + } } } @@ -541,8 +529,6 @@ void AttitudePositionEstimatorEKF::task_main() orb_set_interval(_vstatus_sub, 200); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); - /* XXX remove this!, BUT increase the data buffer size! */ - orb_set_interval(_sensor_combined_sub, 9); /* sets also parameters in the EKF object */ parameters_update(); @@ -610,15 +596,14 @@ void AttitudePositionEstimatorEKF::task_main() orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); /* set sensors to de-initialized state */ - _gyro_valid = false; - _accel_valid = false; - _mag_valid = false; + _gyro_main = -1; + _accel_main = -1; + _mag_main = -1; _baro_init = false; _gps_initialized = false; _last_sensor_timestamp = hrt_absolute_time(); - _last_run = _last_sensor_timestamp; _ekf->ZeroVariables(); _ekf->dtIMU = 0.01f; @@ -646,7 +631,7 @@ void AttitudePositionEstimatorEKF::task_main() * We run the filter only once all data has been fetched **/ - if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) { + if (_baro_init && (_gyro_main >= 0) && (_accel_main >= 0) && (_mag_main >= 0)) { // maintain filtered baro and gps altitudes to calculate weather offset // baro sample rate is ~70Hz and measurement bandwidth is high @@ -836,9 +821,9 @@ void AttitudePositionEstimatorEKF::publishAttitude() _att.pitch = euler(1); _att.yaw = euler(2); - _att.rollspeed = _LP_att_P.apply(_ekf->angRate.x) - _ekf->states[10] / _ekf->dtIMUfilt; - _att.pitchspeed = _LP_att_Q.apply(_ekf->angRate.y) - _ekf->states[11] / _ekf->dtIMUfilt; - _att.yawspeed = _LP_att_R.apply(_ekf->angRate.z) - _ekf->states[12] / _ekf->dtIMUfilt; + _att.rollspeed = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt; + _att.pitchspeed = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt; + _att.yawspeed = _LP_att_R.apply(_ekf->dAngIMU.z / _ekf->dtIMU) - _ekf->states[12] / _ekf->dtIMUfilt; // gyro offsets _att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMUfilt; @@ -1009,7 +994,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const _ekf->UpdateStrapdownEquationsNED(); // store the predicted states for subsequent use by measurement fusion - _ekf->StoreStates(IMUmsec); + _ekf->StoreStates(getMillis()); // sum delta angles and time used by covariance prediction _ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng; @@ -1035,8 +1020,8 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const _ekf->fusePosData = true; // recall states stored at time of measurement after adjusting for delays - _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); - _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); + _ekf->RecallStates(_ekf->statesAtVelTime, (getMillis() - _parameters.vel_delay_ms)); + _ekf->RecallStates(_ekf->statesAtPosTime, (getMillis() - _parameters.pos_delay_ms)); // run the fusion step _ekf->FuseVelposNED(); @@ -1059,8 +1044,8 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const _ekf->fusePosData = true; // recall states stored at time of measurement after adjusting for delays - _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); - _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); + _ekf->RecallStates(_ekf->statesAtVelTime, (getMillis() - _parameters.vel_delay_ms)); + _ekf->RecallStates(_ekf->statesAtPosTime, (getMillis() - _parameters.pos_delay_ms)); // run the fusion step _ekf->FuseVelposNED(); @@ -1076,7 +1061,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const _ekf->fuseHgtData = true; // recall states stored at time of measurement after adjusting for delays - _ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); + _ekf->RecallStates(_ekf->statesAtHgtTime, (getMillis() - _parameters.height_delay_ms)); // run the fusion step _ekf->FuseVelposNED(); @@ -1089,7 +1074,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const if (fuseMag) { _ekf->fuseMagData = true; _ekf->RecallStates(_ekf->statesAtMagMeasTime, - (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data + (getMillis() - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data _ekf->magstate.obsIndex = 0; _ekf->FuseMagnetometer(); @@ -1104,7 +1089,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const if (fuseAirSpeed && _airspeed.true_airspeed_m_s > 5.0f) { _ekf->fuseVtasData = true; _ekf->RecallStates(_ekf->statesAtVtasMeasTime, - (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data + (getMillis() - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data _ekf->FuseAirspeed(); } else { @@ -1117,7 +1102,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const // _ekf->rngMea is set in sensor readout already _ekf->fuseRngData = true; _ekf->fuseOptFlowData = false; - _ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f)); + _ekf->RecallStates(_ekf->statesAtRngTime, (getMillis() - 100.0f)); _ekf->OpticalFlowEKF(); _ekf->fuseRngData = false; } @@ -1131,7 +1116,7 @@ int AttitudePositionEstimatorEKF::start() /* start the task */ _estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator", SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 40, + SCHED_PRIORITY_MAX - 20, 4800, (px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline, nullptr); @@ -1163,7 +1148,7 @@ void AttitudePositionEstimatorEKF::print_status() // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) - PX4_INFO("dtIMU: %8.6f filt: %8.6f IMUmsec: %d", (double)_ekf->dtIMU, (double)_ekf->dtIMUfilt, (int)IMUmsec); + PX4_INFO("dtIMU: %8.6f filt: %8.6f IMU (ms): %d", (double)_ekf->dtIMU, (double)_ekf->dtIMUfilt, (int)getMillis()); PX4_INFO("alt RAW: baro alt: %8.4f GPS alt: %8.4f", (double)_baro.altitude, (double)_ekf->gpsHgt); PX4_INFO("alt EST: local alt: %8.4f (NED), AMSL alt: %8.4f (ENU)", (double)(_local_pos.z), (double)_global_pos.alt); PX4_INFO("filter ref offset: %8.4f baro GPS offset: %8.4f", (double)_filter_ref_offset, @@ -1210,6 +1195,13 @@ void AttitudePositionEstimatorEKF::print_status() (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS", (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); + + PX4_INFO("gyro status:"); + _voter_gyro.print(); + PX4_INFO("accel status:"); + _voter_accel.print(); + PX4_INFO("mag status:"); + _voter_mag.print(); } void AttitudePositionEstimatorEKF::pollData() @@ -1222,125 +1214,153 @@ void AttitudePositionEstimatorEKF::pollData() orb_copy(ORB_ID(actuator_armed), _armedSub, &_armed); } - //Update Gyro and Accelerometer - static Vector3f lastAngRate; - static Vector3f lastAccel; - bool accel_updated = false; - orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); - static hrt_abstime last_accel = 0; - static hrt_abstime last_mag = 0; + // Feed validator with recent sensor data - if (last_accel != _sensor_combined.accelerometer_timestamp) { - accel_updated = true; - - } else { - accel_updated = false; + for (unsigned i = 0; i < (sizeof(_sensor_combined.gyro_timestamp) / sizeof(_sensor_combined.gyro_timestamp[0])); i++) { + _voter_gyro.put(i, _sensor_combined.gyro_timestamp[i], &_sensor_combined.gyro_rad_s[i * 3], + _sensor_combined.gyro_errcount[i], _sensor_combined.gyro_priority[i]); + _voter_accel.put(i, _sensor_combined.accelerometer_timestamp[i], &_sensor_combined.accelerometer_m_s2[i * 3], + _sensor_combined.accelerometer_errcount[i], _sensor_combined.accelerometer_priority[i]); + _voter_mag.put(i, _sensor_combined.magnetometer_timestamp[i], &_sensor_combined.magnetometer_ga[i * 3], + _sensor_combined.magnetometer_errcount[i], _sensor_combined.magnetometer_priority[i]); } - last_accel = _sensor_combined.accelerometer_timestamp; + // Get best measurement values + hrt_abstime curr_time = hrt_absolute_time(); + (void)_voter_gyro.get_best(curr_time, &_gyro_main); + if (_gyro_main >= 0) { + // Use pre-integrated values if possible + if (_sensor_combined.gyro_integral_dt[_gyro_main] > 0) { + _ekf->dAngIMU.x = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 0]; + _ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 1]; + _ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 2]; + } else { + _ekf->dAngIMU.x = 0.5f * (_ekf->angRate.x + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0]); + _ekf->dAngIMU.y = 0.5f * (_ekf->angRate.y + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 1]); + _ekf->dAngIMU.z = 0.5f * (_ekf->angRate.z + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2]); + } + + _ekf->angRate.x = _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0]; + _ekf->angRate.y = _sensor_combined.gyro_rad_s[_gyro_main * 3 + 1]; + _ekf->angRate.z = _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2]; + perf_count(_perf_gyro); + } + + (void)_voter_accel.get_best(curr_time, &_accel_main); + if (_accel_main >= 0 && (_last_accel != _sensor_combined.accelerometer_timestamp[_accel_main])) { + + // Use pre-integrated values if possible + if (_sensor_combined.accelerometer_integral_dt[_accel_main] > 0) { + _ekf->dVelIMU.x = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 0]; + _ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 1]; + _ekf->dVelIMU.z = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 2]; + } else { + _ekf->dVelIMU.x = 0.5f * (_ekf->accel.x + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]); + _ekf->dVelIMU.y = 0.5f * (_ekf->accel.y + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1]); + _ekf->dVelIMU.z = 0.5f * (_ekf->accel.z + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]); + } + + _ekf->accel.x = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]; + _ekf->accel.y = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1]; + _ekf->accel.z = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]; + _last_accel = _sensor_combined.accelerometer_timestamp[_accel_main]; + } + + (void)_voter_mag.get_best(curr_time, &_mag_main); + if (_mag_main >= 0) { + Vector3f mag(_sensor_combined.magnetometer_ga[_mag_main * 3 + 0], _sensor_combined.magnetometer_ga[_mag_main * 3 + 1], + _sensor_combined.magnetometer_ga[_mag_main * 3 + 2]); + + /* fail over to the 2nd mag if we know the first is down */ + if (mag.length() > 0.1f && (_last_mag != _sensor_combined.magnetometer_timestamp[_mag_main])) { + _ekf->magData.x = mag.x; + _ekf->magData.y = mag.y; + _ekf->magData.z = mag.z; + _newDataMag = true; + _last_mag = _sensor_combined.magnetometer_timestamp[_mag_main]; + perf_count(_perf_mag); + } + } + + if (!_failsafe && (_voter_gyro.failover_count() > 0 || + _voter_accel.failover_count() > 0 || + _voter_mag.failover_count() > 0)) { + + _failsafe = true; + mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY"); + } + + if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold || + _voter_accel.get_vibration_factor(curr_time) > _vibration_warning_threshold || + _voter_mag.get_vibration_factor(curr_time) > _vibration_warning_threshold)) { + + if (_vibration_warning_timestamp == 0) { + _vibration_warning_timestamp = curr_time; + } else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000000) { + _vibration_warning = true; + mavlink_and_console_log_critical(_mavlink_fd, "HIGH VIBRATION! g: %d a: %d m: %d", + (int)(100 * _voter_gyro.get_vibration_factor(curr_time)), + (int)(100 * _voter_accel.get_vibration_factor(curr_time)), + (int)(100 * _voter_mag.get_vibration_factor(curr_time))); + } + } else { + _vibration_warning_timestamp = 0; + } - // Copy gyro and accel - _last_sensor_timestamp = _sensor_combined.timestamp; - IMUmsec = _sensor_combined.timestamp / 1e3; IMUusec = _sensor_combined.timestamp; - float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f; + float deltaT = (_sensor_combined.timestamp - _last_sensor_timestamp) / 1e6f; + _last_sensor_timestamp = _sensor_combined.timestamp; /* guard against too large deltaT's */ if (!PX4_ISFINITE(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { deltaT = 0.01f; } - _last_run = _sensor_combined.timestamp; - // Always store data, independent of init status /* fill in last data set */ _ekf->dtIMU = deltaT; - int last_gyro_main = _gyro_main; + // XXX this is for assessing the filter performance + // leave this in as long as larger improvements are still being made. + #if 0 - if (PX4_ISFINITE(_sensor_combined.gyro_rad_s[0]) && - PX4_ISFINITE(_sensor_combined.gyro_rad_s[1]) && - PX4_ISFINITE(_sensor_combined.gyro_rad_s[2]) && - (_sensor_combined.gyro_errcount <= (_sensor_combined.gyro1_errcount + GYRO_SWITCH_HYSTERESIS))) { + float deltaTIntegral = (_sensor_combined.gyro_integral_dt) / 1e6f; + float deltaTIntAcc = (_sensor_combined.accelerometer_integral_dt) / 1e6f; - _ekf->angRate.x = _sensor_combined.gyro_rad_s[0]; - _ekf->angRate.y = _sensor_combined.gyro_rad_s[1]; - _ekf->angRate.z = _sensor_combined.gyro_rad_s[2]; - _gyro_main = 0; - _gyro_valid = true; + static unsigned dtoverflow5 = 0; + static unsigned dtoverflow10 = 0; + static hrt_abstime lastprint = 0; - } else if (PX4_ISFINITE(_sensor_combined.gyro1_rad_s[0]) && - PX4_ISFINITE(_sensor_combined.gyro1_rad_s[1]) && - PX4_ISFINITE(_sensor_combined.gyro1_rad_s[2])) { + if (hrt_elapsed_time(&lastprint) > 1000000) { + warnx("dt: %8.6f, dtg: %8.6f, dta: %8.6f, dt > 5 ms: %u, dt > 10 ms: %u", + (double)deltaT, (double)deltaTIntegral, (double)deltaTIntAcc, + dtoverflow5, dtoverflow10); - _ekf->angRate.x = _sensor_combined.gyro1_rad_s[0]; - _ekf->angRate.y = _sensor_combined.gyro1_rad_s[1]; - _ekf->angRate.z = _sensor_combined.gyro1_rad_s[2]; - _gyro_main = 1; - _gyro_valid = true; + warnx("DRV: dang: %8.4f %8.4f dvel: %8.4f %8.4f", + (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.z, + (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.z); - } else { - _gyro_valid = false; + warnx("EKF: dang: %8.4f %8.4f dvel: %8.4f %8.4f", + (double)(_sensor_combined.gyro_rad_s[0] * deltaT), (double)(_sensor_combined.gyro_rad_s[2] * deltaT), + (double)(_sensor_combined.accelerometer_m_s2[0] * deltaT), (double)(_sensor_combined.accelerometer_m_s2[2] * deltaT)); + + lastprint = hrt_absolute_time(); } - if (last_gyro_main != _gyro_main) { - mavlink_and_console_log_emergency(_mavlink_fd, "GYRO FAILED! Switched from #%d to %d", last_gyro_main, _gyro_main); + if (deltaT > 0.005f) { + dtoverflow5++; } - if (!_gyro_valid) { - /* keep last value if he hit an out of band value */ - lastAngRate = _ekf->angRate; - - } else { - perf_count(_perf_gyro); + if (deltaT > 0.01f) { + dtoverflow10++; } + #endif - if (accel_updated) { - - int last_accel_main = _accel_main; - - /* fail over to the 2nd accel if we know the first is down */ - if (_sensor_combined.accelerometer_errcount <= (_sensor_combined.accelerometer1_errcount + ACCEL_SWITCH_HYSTERESIS)) { - _ekf->accel.x = _sensor_combined.accelerometer_m_s2[0]; - _ekf->accel.y = _sensor_combined.accelerometer_m_s2[1]; - _ekf->accel.z = _sensor_combined.accelerometer_m_s2[2]; - _accel_main = 0; - - } else { - _ekf->accel.x = _sensor_combined.accelerometer1_m_s2[0]; - _ekf->accel.y = _sensor_combined.accelerometer1_m_s2[1]; - _ekf->accel.z = _sensor_combined.accelerometer1_m_s2[2]; - _accel_main = 1; - } - - if (!_accel_valid) { - lastAccel = _ekf->accel; - } - - if (last_accel_main != _accel_main) { - mavlink_and_console_log_emergency(_mavlink_fd, "ACCEL FAILED! Switched from #%d to %d", last_accel_main, _accel_main); - } - - _accel_valid = true; - } - - _ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU; - lastAngRate = _ekf->angRate; - _ekf->dVelIMU = 0.5f * (_ekf->accel + lastAccel) * _ekf->dtIMU; - lastAccel = _ekf->accel; - - if (last_mag != _sensor_combined.magnetometer_timestamp) { - _newDataMag = true; - - } else { - _newDataMag = false; - } - - last_mag = _sensor_combined.magnetometer_timestamp; + _data_good = true; //PX4_INFO("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z); @@ -1362,7 +1382,6 @@ void AttitudePositionEstimatorEKF::pollData() _ekf->VtasMeas = _airspeed.true_airspeed_unfiltered_m_s; } - bool gps_update; orb_check(_gps_sub, &gps_update); @@ -1516,57 +1535,6 @@ void AttitudePositionEstimatorEKF::pollData() perf_count(_perf_baro); } - //Update Magnetometer - if (_newDataMag) { - - _mag_valid = true; - - perf_count(_perf_mag); - - int last_mag_main = _mag_main; - - Vector3f mag0(_sensor_combined.magnetometer_ga[0], _sensor_combined.magnetometer_ga[1], - _sensor_combined.magnetometer_ga[2]); - - Vector3f mag1(_sensor_combined.magnetometer1_ga[0], _sensor_combined.magnetometer1_ga[1], - _sensor_combined.magnetometer1_ga[2]); - - const unsigned mag_timeout_us = 200000; - - /* fail over to the 2nd mag if we know the first is down */ - if (hrt_elapsed_time(&_sensor_combined.magnetometer_timestamp) < mag_timeout_us && - _sensor_combined.magnetometer_errcount <= (_sensor_combined.magnetometer1_errcount + MAG_SWITCH_HYSTERESIS) && - mag0.length() > 0.1f) { - _ekf->magData.x = mag0.x; - _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset - - _ekf->magData.y = mag0.y; - _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset - - _ekf->magData.z = mag0.z; - _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset - _mag_main = 0; - - } else if (hrt_elapsed_time(&_sensor_combined.magnetometer1_timestamp) < mag_timeout_us && - mag1.length() > 0.1f) { - _ekf->magData.x = mag1.x; - _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset - - _ekf->magData.y = mag1.y; - _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset - - _ekf->magData.z = mag1.z; - _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset - _mag_main = 1; - } else { - _mag_valid = false; - } - - if (last_mag_main != _mag_main) { - mavlink_and_console_log_emergency(_mavlink_fd, "MAG FAILED! Failover from unit %d to unit %d", last_mag_main, _mag_main); - } - } - //Update range data orb_check(_distance_sub, &_newRangeData); diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c index 943365e499..0d33e28c91 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c @@ -258,6 +258,42 @@ PARAM_DEFINE_FLOAT(PE_MAGE_PNOISE, 0.0003f); */ PARAM_DEFINE_FLOAT(PE_MAGB_PNOISE, 0.0003f); +/** + * Magnetometer X bias + * + * The magnetometer bias. This bias is learnt by the filter over time and + * persists between boots. + * + * @min -0.6 + * @max 0.6 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_MAGB_X, 0.0f); + +/** + * Magnetometer Y bias + * + * The magnetometer bias. This bias is learnt by the filter over time and + * persists between boots. + * + * @min -0.6 + * @max 0.6 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_MAGB_Y, 0.0f); + +/** + * Magnetometer Z bias + * + * The magnetometer bias. This bias is learnt by the filter over time and + * persists between boots. + * + * @min -0.6 + * @max 0.6 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_MAGB_Z, 0.0f); + /** * Threshold for filter initialization. * diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index bbb39f20f0..7984bb12af 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -58,8 +58,8 @@ BlockYawDamper::~BlockYawDamper() {}; void BlockYawDamper::update(float rCmd, float r, float outputScale) { - _rudder = outputScale*_r2Rdr.update(rCmd - - _rWashout.update(_rLowPass.update(r))); + _rudder = outputScale * _r2Rdr.update(rCmd - + _rWashout.update(_rLowPass.update(r))); } BlockStabilization::BlockStabilization(SuperBlock *parent, const char *name) : @@ -79,9 +79,9 @@ BlockStabilization::~BlockStabilization() {}; void BlockStabilization::update(float pCmd, float qCmd, float rCmd, float p, float q, float r, float outputScale) { - _aileron = outputScale*_p2Ail.update( + _aileron = outputScale * _p2Ail.update( pCmd - _pLowPass.update(p)); - _elevator = outputScale*_q2Elv.update( + _elevator = outputScale * _q2Elv.update( qCmd - _qLowPass.update(q)); _yawDamper.update(rCmd, r, outputScale); } @@ -127,7 +127,7 @@ BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *par void BlockMultiModeBacksideAutopilot::update() { // wait for a sensor update, check for exit condition every 100 ms - if (poll(&_attPoll, 1, 100) < 0) return; // poll error + if (poll(&_attPoll, 1, 100) < 0) { return; } // poll error uint64_t newTimeStamp = hrt_absolute_time(); float dt = (newTimeStamp - _timeStamp) / 1.0e6f; @@ -135,7 +135,7 @@ void BlockMultiModeBacksideAutopilot::update() // check for sane values of dt // to prevent large control responses - if (dt > 1.0f || dt < 0) return; + if (dt > 1.0f || dt < 0) { return; } // set dt for all child blocks setDt(dt); @@ -146,14 +146,15 @@ void BlockMultiModeBacksideAutopilot::update() } // check for new updates - if (_param_update.updated()) updateParams(); + if (_param_update.updated()) { updateParams(); } // get new information from subscriptions updateSubscriptions(); // default all output to zero unless handled by mode - for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++) + for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++) { _actuators.control[i] = 0.0f; + } // only update guidance in auto mode if (_status.main_state == MAIN_STATE_AUTO) { @@ -170,13 +171,13 @@ void BlockMultiModeBacksideAutopilot::update() if (_status.main_state == MAIN_STATE_AUTO) { // calculate velocity, XXX should be airspeed, - // but using ground speed for now for the purpose + // but using ground speed for now for the purpose // of control we will limit the velocity feedback between // the min/max velocity float v = _vLimit.update(sqrtf( - _pos.vel_n * _pos.vel_n + - _pos.vel_e * _pos.vel_e + - _pos.vel_d * _pos.vel_d)); + _pos.vel_n * _pos.vel_n + + _pos.vel_e * _pos.vel_e + + _pos.vel_d * _pos.vel_d)); // limit velocity command between min/max velocity float vCmd = _vLimit.update(_vCmd.get()); @@ -198,8 +199,8 @@ void BlockMultiModeBacksideAutopilot::update() float rCmd = 0; // stabilization - float velocityRatio = _trimV.get()/v; - float outputScale = velocityRatio*velocityRatio; + float velocityRatio = _trimV.get() / v; + float outputScale = velocityRatio * velocityRatio; // this term scales the output based on the dynamic pressure change from trim _stabilization.update(pCmd, qCmd, rCmd, _att.rollspeed, _att.pitchspeed, _att.yawspeed, @@ -230,15 +231,15 @@ void BlockMultiModeBacksideAutopilot::update() _actuators.control[CH_THR] = _manual.throttle; } else if (_status.main_state == MAIN_STATE_ALTCTL || - _status.main_state == MAIN_STATE_POSCTL /* TODO, implement pos control */) { + _status.main_state == MAIN_STATE_POSCTL /* TODO, implement pos control */) { // calculate velocity, XXX should be airspeed, but using ground speed for now // for the purpose of control we will limit the velocity feedback between // the min/max velocity float v = _vLimit.update(sqrtf( - _pos.vel_n * _pos.vel_n + - _pos.vel_e * _pos.vel_e + - _pos.vel_d * _pos.vel_d)); + _pos.vel_n * _pos.vel_n + + _pos.vel_e * _pos.vel_e + + _pos.vel_d * _pos.vel_d)); // pitch channel -> rate of climb // TODO, might want to put a gain on this, otherwise commanding @@ -253,8 +254,8 @@ void BlockMultiModeBacksideAutopilot::update() // throttle channel -> velocity // negative sign because nose over to increase speed float vCmd = _vLimit.update(_manual.throttle * - (_vLimit.getMax() - _vLimit.getMin()) + - _vLimit.getMin()); + (_vLimit.getMax() - _vLimit.getMin()) + + _vLimit.getMin()); float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v)); float qCmd = _theta2Q.update(thetaCmd - _att.pitch); @@ -263,7 +264,7 @@ void BlockMultiModeBacksideAutopilot::update() // stabilization _stabilization.update(pCmd, qCmd, rCmd, - _att.rollspeed, _att.pitchspeed, _att.yawspeed); + _att.rollspeed, _att.pitchspeed, _att.yawspeed); // output _actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get(); @@ -284,14 +285,17 @@ void BlockMultiModeBacksideAutopilot::update() if (_status.hil_state != HIL_STATE_ON) { /* limit to value of manual throttle */ _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? - _actuators.control[CH_THR] : _manual.throttle; + _actuators.control[CH_THR] : _manual.throttle; } - // body rates controller, disabled for now - // TODO - } else if (0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) { // TODO use vehicle_control_mode here? + + // body rates controller, disabled for now + // TODO + + } else if ( + 0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) { // TODO use vehicle_control_mode here? _stabilization.update(_manual.roll, _manual.pitch, _manual.yaw, - _att.rollspeed, _att.pitchspeed, _att.yawspeed); + _att.rollspeed, _att.pitchspeed, _att.yawspeed); _actuators.control[CH_AIL] = _stabilization.getAileron(); _actuators.control[CH_ELV] = _stabilization.getElevator(); @@ -307,8 +311,9 @@ BlockMultiModeBacksideAutopilot::~BlockMultiModeBacksideAutopilot() { // send one last publication when destroyed, setting // all output to zero - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { _actuators.control[i] = 0.0f; + } updatePublications(); } diff --git a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp index 06559de979..fe9a1066bd 100644 --- a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp +++ b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp @@ -79,8 +79,9 @@ static void usage(const char *reason); static void usage(const char *reason) { - if (reason) + if (reason) { fprintf(stderr, "%s\n", reason); + } fprintf(stderr, "usage: fixedwing_backside {start|stop|status} [-p ]\n\n"); exit(1); @@ -112,11 +113,11 @@ int fixedwing_backside_main(int argc, char *argv[]) thread_should_exit = false; deamon_task = px4_task_spawn_cmd("fixedwing_backside", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 10, - 5120, - control_demo_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 10, + 5120, + control_demo_thread_main, + (argv) ? (char *const *)&argv[2] : (char *const *)NULL); exit(0); } diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 4a02b93495..c3816f5374 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1227,9 +1227,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed_2d); - /* limit roll motion to prevent wings from touching the ground first */ - _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); - land_noreturn_horizontal = true; } else { @@ -1241,6 +1238,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); + if (land_noreturn_horizontal) { + /* limit roll motion to prevent wings from touching the ground first */ + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); + } /* Vertical landing control */ //xxx: using the tecs altitude controller for slope control for now @@ -1363,7 +1364,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Detect launch */ launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); - /* update our copy of the laucn detection state */ + /* update our copy of the launch detection state */ launch_detection_state = launchDetector.getLaunchDetected(); } else { /* no takeoff detection --> fly */ diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 48ea9eec18..ca5cf307e5 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -94,6 +94,7 @@ #endif static const int ERROR = -1; +#define DEFAULT_REMOTE_PORT_UDP 14550 ///< GCS port per MAVLink spec #define DEFAULT_DEVICE_NAME "/dev/ttyS1" #define MAX_DATA_RATE 10000000 ///< max data rate in bytes/s #define MAIN_LOOP_DELAY 10000 ///< 100 Hz @ 1000 bytes/s data rate @@ -153,7 +154,6 @@ Mavlink::Mavlink() : _receive_thread {}, _verbose(false), _forwarding_on(false), - _passing_on(false), _ftp_on(false), #ifndef __PX4_POSIX _uart_fd(-1), @@ -885,6 +885,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID ret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr)); } else if (get_protocol() == TCP) { // not implemented, but possible to do so + warnx("TCP transport pending implementation"); } #endif @@ -974,11 +975,12 @@ Mavlink::init_udp() return; } - unsigned char inbuf[256]; - socklen_t addrlen = sizeof(_src_addr); + // set default target address + memset((char *)&_src_addr, 0, sizeof(_src_addr)); + _src_addr.sin_family = AF_INET; + inet_aton("127.0.0.1", &_src_addr.sin_addr); + _src_addr.sin_port = htons(DEFAULT_REMOTE_PORT_UDP); - // wait for client to connect to socket - recvfrom(_socket_fd,inbuf,sizeof(inbuf),0,(struct sockaddr *)&_src_addr,&addrlen); #endif } @@ -1319,7 +1321,7 @@ Mavlink::message_buffer_mark_read(int n) void Mavlink::pass_message(const mavlink_message_t *msg) { - if (_passing_on) { + if (_forwarding_on) { /* size is 8 bytes plus variable payload */ int size = MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len; pthread_mutex_lock(&_message_buffer_mutex); @@ -1496,10 +1498,6 @@ Mavlink::task_main(int argc, char *argv[]) _forwarding_on = true; break; - case 'p': - _passing_on = true; - break; - case 'v': _verbose = true; break; @@ -1565,7 +1563,7 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_logbuffer_init(&_logbuffer, 5); /* if we are passing on mavlink messages, we need to prepare a buffer for this instance */ - if (_passing_on || _ftp_on) { + if (_forwarding_on || _ftp_on) { /* initialize message buffer if multiplexing is on or its needed for FTP. * make space for two messages plus off-by-one space as we use the empty element * marker ring buffer approach. @@ -1639,14 +1637,16 @@ Mavlink::task_main(int argc, char *argv[]) switch (_mode) { case MAVLINK_MODE_NORMAL: configure_stream("SYS_STATUS", 1.0f); - configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); + configure_stream("HOME_POSITION", 0.5f); configure_stream("HIGHRES_IMU", 2.0f); configure_stream("ATTITUDE", 20.0f); configure_stream("VFR_HUD", 8.0f); configure_stream("GPS_RAW_INT", 1.0f); configure_stream("GLOBAL_POSITION_INT", 3.0f); configure_stream("LOCAL_POSITION_NED", 3.0f); - configure_stream("RC_CHANNELS", 4.0f); + configure_stream("NAMED_VALUE_FLOAT", 2.0f); + configure_stream("RC_CHANNELS", 1.0f); + configure_stream("SERVO_OUTPUT_RAW_0", 1.0f); configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f); configure_stream("ATTITUDE_TARGET", 8.0f); configure_stream("DISTANCE_SENSOR", 0.5f); @@ -1661,13 +1661,16 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("GPS_RAW_INT", 5.0f); configure_stream("GLOBAL_POSITION_INT", 50.0f); configure_stream("LOCAL_POSITION_NED", 30.0f); + configure_stream("NAMED_VALUE_FLOAT", 10.0f); configure_stream("CAMERA_CAPTURE", 2.0f); + configure_stream("HOME_POSITION", 0.5f); configure_stream("ATTITUDE_TARGET", 10.0f); configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f); configure_stream("POSITION_TARGET_LOCAL_NED", 10.0f); configure_stream("DISTANCE_SENSOR", 10.0f); configure_stream("OPTICAL_FLOW_RAD", 10.0f); configure_stream("RC_CHANNELS", 20.0f); + configure_stream("SERVO_OUTPUT_RAW_0", 10.0f); configure_stream("VFR_HUD", 10.0f); configure_stream("SYSTEM_TIME", 1.0f); configure_stream("TIMESYNC", 10.0f); @@ -1683,30 +1686,36 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("VFR_HUD", 5.0f); configure_stream("GPS_RAW_INT", 1.0f); configure_stream("GLOBAL_POSITION_INT", 10.0f); + configure_stream("HOME_POSITION", 0.5f); configure_stream("ATTITUDE_TARGET", 10.0f); configure_stream("BATTERY_STATUS", 1.0f); configure_stream("SYSTEM_TIME", 1.0f); configure_stream("RC_CHANNELS", 5.0f); + configure_stream("SERVO_OUTPUT_RAW_0", 1.0f); configure_stream("VTOL_STATE", 0.5f); break; case MAVLINK_MODE_CONFIG: // Enable a number of interesting streams we want via USB + configure_stream("SYS_STATUS", 1.0f); + configure_stream("HOME_POSITION", 0.5f); + configure_stream("GLOBAL_POSITION_INT", 10.0f); + configure_stream("ATTITUDE_TARGET", 8.0f); configure_stream("PARAM_VALUE", 300.0f); configure_stream("MISSION_ITEM", 50.0f); - configure_stream("NAMED_VALUE_FLOAT", 10.0f); + configure_stream("NAMED_VALUE_FLOAT", 50.0f); configure_stream("OPTICAL_FLOW_RAD", 10.0f); configure_stream("DISTANCE_SENSOR", 10.0f); configure_stream("VFR_HUD", 20.0f); configure_stream("ATTITUDE", 100.0f); configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f); - configure_stream("RC_CHANNELS_RAW", 5.0f); + configure_stream("RC_CHANNELS", 5.0f); configure_stream("SERVO_OUTPUT_RAW_0", 20.0f); configure_stream("SERVO_OUTPUT_RAW_1", 20.0f); configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f); configure_stream("LOCAL_POSITION_NED", 30.0f); configure_stream("MANUAL_CONTROL", 5.0f); - configure_stream("HIGHRES_IMU", 100.0f); + configure_stream("HIGHRES_IMU", 50.0f); configure_stream("GPS_RAW_INT", 20.0f); configure_stream("CAMERA_TRIGGER", 500.0f); configure_stream("VTOL_STATE", 2.0f); @@ -1837,7 +1846,7 @@ Mavlink::task_main(int argc, char *argv[]) } /* pass messages from other UARTs or FTP worker */ - if (_passing_on || _ftp_on) { + if (_forwarding_on || _ftp_on) { bool is_part; uint8_t *read_ptr; @@ -1946,7 +1955,7 @@ Mavlink::task_main(int argc, char *argv[]) /* close mavlink logging device */ px4_close(_mavlink_fd); - if (_passing_on || _ftp_on) { + if (_forwarding_on || _ftp_on) { message_buffer_destroy(); pthread_mutex_destroy(&_message_buffer_mutex); } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 00716480cf..4d28b1c6e5 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -47,6 +47,7 @@ #else #include #include +#include #include #endif #include @@ -333,7 +334,9 @@ public: unsigned short get_network_port() { return _network_port; } int get_socket_fd () { return _socket_fd; }; - +#ifdef __PX4_POSIX + struct sockaddr_in * get_client_source_address() {return &_src_addr;}; +#endif static bool boot_complete() { return _boot_complete; } protected: @@ -376,7 +379,6 @@ private: bool _verbose; bool _forwarding_on; - bool _passing_on; bool _ftp_on; #ifndef __PX4_QURT int _uart_fd; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 67809ed7be..de589a8bac 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -643,28 +643,28 @@ protected: if (_sensor_sub->update(&_sensor_time, &sensor)) { uint16_t fields_updated = 0; - if (_accel_timestamp != sensor.accelerometer_timestamp) { + if (_accel_timestamp != sensor.accelerometer_timestamp[0]) { /* mark first three dimensions as changed */ fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); - _accel_timestamp = sensor.accelerometer_timestamp; + _accel_timestamp = sensor.accelerometer_timestamp[0]; } - if (_gyro_timestamp != sensor.timestamp) { + if (_gyro_timestamp != sensor.gyro_timestamp[0]) { /* mark second group dimensions as changed */ fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); - _gyro_timestamp = sensor.timestamp; + _gyro_timestamp = sensor.gyro_timestamp[0]; } - if (_mag_timestamp != sensor.magnetometer_timestamp) { + if (_mag_timestamp != sensor.magnetometer_timestamp[0]) { /* mark third group dimensions as changed */ fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); - _mag_timestamp = sensor.magnetometer_timestamp; + _mag_timestamp = sensor.magnetometer_timestamp[0]; } - if (_baro_timestamp != sensor.baro_timestamp) { + if (_baro_timestamp != sensor.baro_timestamp[0]) { /* mark last group dimensions as changed */ fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); - _baro_timestamp = sensor.baro_timestamp; + _baro_timestamp = sensor.baro_timestamp[0]; } mavlink_highres_imu_t msg; @@ -679,10 +679,10 @@ protected: msg.xmag = sensor.magnetometer_ga[0]; msg.ymag = sensor.magnetometer_ga[1]; msg.zmag = sensor.magnetometer_ga[2]; - msg.abs_pressure = sensor.baro_pres_mbar; - msg.diff_pressure = sensor.differential_pressure_pa; - msg.pressure_alt = sensor.baro_alt_meter; - msg.temperature = sensor.baro_temp_celcius; + msg.abs_pressure = sensor.baro_pres_mbar[0]; + msg.diff_pressure = sensor.differential_pressure_pa[0]; + msg.pressure_alt = sensor.baro_alt_meter[0]; + msg.temperature = sensor.baro_temp_celcius[0]; msg.fields_updated = fields_updated; _mavlink->send_message(MAVLINK_MSG_ID_HIGHRES_IMU, &msg); @@ -1326,43 +1326,43 @@ protected: }; -class MavlinkStreamGPSGlobalOrigin : public MavlinkStream +class MavlinkStreamHomePosition : public MavlinkStream { public: const char *get_name() const { - return MavlinkStreamGPSGlobalOrigin::get_name_static(); + return MavlinkStreamHomePosition::get_name_static(); } static const char *get_name_static() { - return "GPS_GLOBAL_ORIGIN"; + return "HOME_POSITION"; } uint8_t get_id() { - return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; + return MAVLINK_MSG_ID_HOME_POSITION; } static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamGPSGlobalOrigin(mavlink); + return new MavlinkStreamHomePosition(mavlink); } unsigned get_size() { - return _home_sub->is_published() ? (MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + return _home_sub->is_published() ? (MAVLINK_MSG_ID_HOME_POSITION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: MavlinkOrbSubscription *_home_sub; /* do not allow top copying this class */ - MavlinkStreamGPSGlobalOrigin(MavlinkStreamGPSGlobalOrigin &); - MavlinkStreamGPSGlobalOrigin& operator = (const MavlinkStreamGPSGlobalOrigin &); + MavlinkStreamHomePosition(MavlinkStreamHomePosition &); + MavlinkStreamHomePosition& operator = (const MavlinkStreamHomePosition &); protected: - explicit MavlinkStreamGPSGlobalOrigin(Mavlink *mavlink) : MavlinkStream(mavlink), + explicit MavlinkStreamHomePosition(Mavlink *mavlink) : MavlinkStream(mavlink), _home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))) {} @@ -1374,13 +1374,26 @@ protected: struct home_position_s home; if (_home_sub->update(&home)) { - mavlink_gps_global_origin_t msg; + mavlink_home_position_t msg; msg.latitude = home.lat * 1e7; msg.longitude = home.lon * 1e7; msg.altitude = home.alt * 1e3f; - _mavlink->send_message(MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, &msg); + msg.x = home.x; + msg.y = home.y; + msg.z = home.z; + + msg.q[0] = 1.0f; + msg.q[1] = 0.0f; + msg.q[2] = 0.0f; + msg.q[3] = 0.0f; + + msg.approach_x = 0.0f; + msg.approach_y = 0.0f; + msg.approach_z = 0.0f; + + _mavlink->send_message(MAVLINK_MSG_ID_HOME_POSITION, &msg); } } } @@ -2057,7 +2070,14 @@ protected: msg.y = manual.y * 1000; msg.z = manual.z * 1000; msg.r = manual.r * 1000; + unsigned shift = 2; msg.buttons = 0; + msg.buttons |= (manual.mode_switch << (shift * 0)); + msg.buttons |= (manual.return_switch << (shift * 1)); + msg.buttons |= (manual.posctl_switch << (shift * 2)); + msg.buttons |= (manual.loiter_switch << (shift * 3)); + msg.buttons |= (manual.acro_switch << (shift * 4)); + msg.buttons |= (manual.offboard_switch << (shift * 5)); _mavlink->send_message(MAVLINK_MSG_ID_MANUAL_CONTROL, &msg); } @@ -2430,7 +2450,7 @@ const StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static), new StreamListItem(&MavlinkStreamAttPosMocap::new_instance, &MavlinkStreamAttPosMocap::get_name_static), - new StreamListItem(&MavlinkStreamGPSGlobalOrigin::new_instance, &MavlinkStreamGPSGlobalOrigin::get_name_static), + new StreamListItem(&MavlinkStreamHomePosition::new_instance, &MavlinkStreamHomePosition::get_name_static), new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static), new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static), new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static), diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 2c1a3d88f6..883fcd0f34 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,9 +35,9 @@ * @file mavlink_mission.cpp * MAVLink mission manager implementation. * - * @author Lorenz Meier - * @author Julian Oes - * @author Anton Babushkin + * @author Lorenz Meier + * @author Julian Oes + * @author Anton Babushkin */ #include "mavlink_mission.h" @@ -77,6 +77,7 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(m _action_timeout(MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT), _retry_timeout(MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT), _max_count(DM_KEY_WAYPOINTS_OFFBOARD_0_MAX), + _filesystem_errcount(0), _my_dataman_id(0), _transfer_dataman_id(0), _transfer_count(0), @@ -169,8 +170,10 @@ MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int return OK; } else { - warnx("ERROR: can't save mission state"); - _mavlink->send_statustext(MAV_SEVERITY_CRITICAL, "ERROR: can't save mission state"); + warnx("WPM: ERROR: can't save mission state"); + if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) { + _mavlink->send_statustext_critical("Mission storage: Unable to write to microSD"); + } return ERROR; } @@ -253,7 +256,9 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t } else { send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); - _mavlink->send_statustext_critical("Unable to read from micro SD"); + if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) { + _mavlink->send_statustext_critical("Mission storage: Unable to read from microSD"); + } if (_verbose) { warnx("WPM: Send MISSION_ITEM ERROR: could not read seq %u from dataman ID %i", seq, _dataman_id); } } @@ -479,8 +484,6 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg) } else { if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK nothing to send, mission is empty"); } - - _mavlink->send_statustext_info("WPM: mission is empty"); } send_mission_count(msg->sysid, msg->compid, _count); diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index 54af7dc466..c37322d37b 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,9 +35,9 @@ * @file mavlink_mission.h * MAVLink mission manager interface definition. * - * @author Lorenz Meier - * @author Julian Oes - * @author Anton Babushkin + * @author Lorenz Meier + * @author Julian Oes + * @author Anton Babushkin */ #pragma once @@ -108,13 +108,14 @@ private: uint32_t _action_timeout; uint32_t _retry_timeout; unsigned _max_count; ///< Maximum number of mission items + unsigned _filesystem_errcount; ///< File system error count static int _dataman_id; ///< Global Dataman storage ID for active mission - int _my_dataman_id; ///< class Dataman storage ID + int _my_dataman_id; ///< class Dataman storage ID static bool _dataman_init; ///< Dataman initialized static unsigned _count; ///< Count of items in active mission - static int _current_seq; ///< Current item sequence in active mission + static int _current_seq; ///< Current item sequence in active mission int _transfer_dataman_id; ///< Dataman storage ID for current transmission unsigned _transfer_count; ///< Items count in current transmission @@ -122,7 +123,7 @@ private: unsigned _transfer_current_seq; ///< Current item ID for current transmission (-1 means not initialized) unsigned _transfer_partner_sysid; ///< Partner system ID for current transmission unsigned _transfer_partner_compid; ///< Partner component ID for current transmission - static bool _transfer_in_progress; ///< Global variable checking for current transmission + static bool _transfer_in_progress; ///< Global variable checking for current transmission int _offboard_mission_sub; int _mission_result_sub; @@ -132,6 +133,8 @@ private: bool _verbose; + static constexpr unsigned int FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT = 2; ///< Error count limit before stopping to report FS errors + /* do not allow top copying this class */ MavlinkMissionManager(MavlinkMissionManager &); MavlinkMissionManager& operator = (const MavlinkMissionManager &); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 08b24f339d..8d97b6a8c8 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -309,6 +309,10 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) } else if (cmd_mavlink.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) { /* send autopilot version message */ _mavlink->send_autopilot_capabilites(); + + } else if (cmd_mavlink.command == MAV_CMD_GET_HOME_POSITION) { + _mavlink->configure_stream_threadsafe("HOME_POSITION", 0.5f); + } else { if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) { @@ -365,6 +369,13 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) /* terminate other threads and this thread */ _mavlink->_task_should_exit = true; + } else if (cmd_mavlink.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) { + /* send autopilot version message */ + _mavlink->send_autopilot_capabilites(); + + } else if (cmd_mavlink.command == MAV_CMD_GET_HOME_POSITION) { + _mavlink->configure_stream_threadsafe("HOME_POSITION", 0.5f); + } else { if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) { @@ -990,8 +1001,8 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) switch_pos_t MavlinkReceiver::decode_switch_pos(uint16_t buttons, unsigned sw) { - // XXX non-standard 3 pos switch decoding - return (buttons >> (sw * 2)) & 3; + // This 2-bit method should be used in the future: (buttons >> (sw * 2)) & 3; + return (buttons & (1 << sw)) ? manual_control_setpoint_s::SWITCH_POS_ON : manual_control_setpoint_s::SWITCH_POS_OFF; } int @@ -1101,26 +1112,10 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) rc.values[0] = man.x / 2 + 1500; /* pitch */ rc.values[1] = man.y / 2 + 1500; - - /* - * yaw needs special handling as some joysticks have a circular mechanical mask, - * which makes the corner positions unreachable. - * scale yaw up and clip it to overcome this. - */ - rc.values[2] = man.r / 1.1f + 1500; - if (rc.values[2] > 2000) { - rc.values[2] = 2000; - } else if (rc.values[2] < 1000) { - rc.values[2] = 1000; - } - + /* yaw */ + rc.values[2] = man.r / 2 + 1500; /* throttle */ - rc.values[3] = man.z / 0.9f + 1000; - if (rc.values[3] > 2000) { - rc.values[3] = 2000; - } else if (rc.values[3] < 1000) { - rc.values[3] = 1000; - } + rc.values[3] = man.z / 1 + 1000; /* decode all switches which fit into the channel mask */ unsigned max_switch = (sizeof(man.buttons) * 8); @@ -1426,6 +1421,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_sensors.gyro_rad_s[0] = imu.xgyro; hil_sensors.gyro_rad_s[1] = imu.ygyro; hil_sensors.gyro_rad_s[2] = imu.zgyro; + hil_sensors.gyro_timestamp[0] = timestamp; hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2; hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2; @@ -1433,9 +1429,9 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_sensors.accelerometer_m_s2[0] = imu.xacc; hil_sensors.accelerometer_m_s2[1] = imu.yacc; hil_sensors.accelerometer_m_s2[2] = imu.zacc; - hil_sensors.accelerometer_mode = 0; // TODO what is this? - hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16 - hil_sensors.accelerometer_timestamp = timestamp; + hil_sensors.accelerometer_mode[0] = 0; // TODO what is this? + hil_sensors.accelerometer_range_m_s2[0] = 32.7f; // int16 + hil_sensors.accelerometer_timestamp[0] = timestamp; hil_sensors.adc_voltage_v[0] = 0.0f; hil_sensors.adc_voltage_v[1] = 0.0f; @@ -1447,19 +1443,19 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_sensors.magnetometer_ga[0] = imu.xmag; hil_sensors.magnetometer_ga[1] = imu.ymag; hil_sensors.magnetometer_ga[2] = imu.zmag; - hil_sensors.magnetometer_range_ga = 32.7f; // int16 - hil_sensors.magnetometer_mode = 0; // TODO what is this - hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f; - hil_sensors.magnetometer_timestamp = timestamp; + hil_sensors.magnetometer_range_ga[0] = 32.7f; // int16 + hil_sensors.magnetometer_mode[0] = 0; // TODO what is this + hil_sensors.magnetometer_cuttoff_freq_hz[0] = 50.0f; + hil_sensors.magnetometer_timestamp[0] = timestamp; - hil_sensors.baro_pres_mbar = imu.abs_pressure; - hil_sensors.baro_alt_meter = imu.pressure_alt; - hil_sensors.baro_temp_celcius = imu.temperature; - hil_sensors.baro_timestamp = timestamp; + hil_sensors.baro_pres_mbar[0] = imu.abs_pressure; + hil_sensors.baro_alt_meter[0] = imu.pressure_alt; + hil_sensors.baro_temp_celcius[0] = imu.temperature; + hil_sensors.baro_timestamp[0] = timestamp; - hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa - hil_sensors.differential_pressure_filtered_pa = hil_sensors.differential_pressure_pa; - hil_sensors.differential_pressure_timestamp = timestamp; + hil_sensors.differential_pressure_pa[0] = imu.diff_pressure * 1e2f; //from hPa to Pa + hil_sensors.differential_pressure_filtered_pa[0] = hil_sensors.differential_pressure_pa[0]; + hil_sensors.differential_pressure_timestamp[0] = timestamp; /* publish combined sensor topic */ if (_sensors_pub == nullptr) { @@ -1792,6 +1788,13 @@ MavlinkReceiver::receive_thread(void *arg) } else { // could be TCP or other protocol } + + struct sockaddr_in * srcaddr_last = _mavlink->get_client_source_address(); + int localhost = (127 << 24) + 1; + if (srcaddr_last->sin_addr.s_addr == htonl(localhost) && srcaddr.sin_addr.s_addr != htonl(localhost)) { + // if we were sending to localhost before but have a new host then accept him + memcpy(srcaddr_last, &srcaddr, sizeof(srcaddr)); + } #endif /* if read failed, this loop won't execute */ for (ssize_t i = 0; i < nread; i++) { diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 30a50480e0..8d9fb6b911 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -215,7 +215,7 @@ PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f); * @max 360.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ROLLRATE_MAX, 360.0f); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_MAX, 220.0f); /** * Max pitch rate @@ -227,7 +227,7 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_MAX, 360.0f); * @max 360.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_PITCHRATE_MAX, 360.0f); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_MAX, 220.0f); /** * Max yaw rate diff --git a/src/modules/muorb/krait/uORBKraitFastRpcChannel.hpp b/src/modules/muorb/krait/uORBKraitFastRpcChannel.hpp index d729e6cd7b..a2f6850cbe 100644 --- a/src/modules/muorb/krait/uORBKraitFastRpcChannel.hpp +++ b/src/modules/muorb/krait/uORBKraitFastRpcChannel.hpp @@ -38,7 +38,7 @@ #include #include #include "uORB/uORBCommunicator.hpp" -#include +#include #include #include "drivers/drv_hrt.h" diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index d63f5fe338..50b8746a71 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -406,7 +406,7 @@ Navigator::task_main() /* Check geofence violation */ static hrt_abstime last_geofence_check = 0; if (have_geofence_position_data && hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL) { - bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos, _home_position_set); + bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter[0], _home_pos, _home_position_set); last_geofence_check = hrt_absolute_time(); have_geofence_position_data = false; if (!inside) { @@ -524,7 +524,7 @@ Navigator::start() /* start the task */ _navigator_task = px4_task_spawn_cmd("navigator", SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT - 5, + SCHED_PRIORITY_DEFAULT + 5, 1500, (px4_main_t)&Navigator::task_main_trampoline, nullptr); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 892e50f3a1..ee8359f0c3 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -399,13 +399,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (fds_init[0].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - if (wait_baro && sensor.baro_timestamp != baro_timestamp) { - baro_timestamp = sensor.baro_timestamp; + if (wait_baro && sensor.baro_timestamp[0] != baro_timestamp) { + baro_timestamp = sensor.baro_timestamp[0]; /* mean calculation over several measurements */ if (baro_init_cnt < baro_init_num) { - if (PX4_ISFINITE(sensor.baro_alt_meter)) { - baro_offset += sensor.baro_alt_meter; + if (PX4_ISFINITE(sensor.baro_alt_meter[0])) { + baro_offset += sensor.baro_alt_meter[0]; baro_init_cnt++; } @@ -474,7 +474,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - if (sensor.accelerometer_timestamp != accel_timestamp) { + if (sensor.accelerometer_timestamp[0] != accel_timestamp) { if (att.R_valid) { /* correct accel bias */ sensor.accelerometer_m_s2[0] -= acc_bias[0]; @@ -496,13 +496,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memset(acc, 0, sizeof(acc)); } - accel_timestamp = sensor.accelerometer_timestamp; + accel_timestamp = sensor.accelerometer_timestamp[0]; accel_updates++; } - if (sensor.baro_timestamp != baro_timestamp) { - corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0]; - baro_timestamp = sensor.baro_timestamp; + if (sensor.baro_timestamp[0] != baro_timestamp) { + corr_baro = baro_offset - sensor.baro_alt_meter[0] - z_est[0]; + baro_timestamp = sensor.baro_timestamp[0]; baro_updates++; } } diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk index d22d0a9d93..69afb39099 100644 --- a/src/modules/sdlog2/module.mk +++ b/src/modules/sdlog2/module.mk @@ -47,5 +47,5 @@ MODULE_STACKSIZE = 1200 MAXOPTIMIZATION = -Os ifeq ($(PX4_TARGET_OS),nuttx) -EXTRACFLAGS = -Wframe-larger-than=1400 +EXTRACFLAGS = -Wframe-larger-than=1600 endif diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index c6d13f8756..cd08f4a386 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1272,30 +1272,26 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.sat_info_sub = -1; - /* close non-needed fd's */ +#ifdef __PX4_NUTTX + /* close non-needed fd's. We cannot do this for posix since the file + descriptors will also be closed for the parent process + */ /* close stdin */ close(0); /* close stdout */ close(1); - +#endif /* initialize thread synchronization */ pthread_mutex_init(&logbuffer_mutex, NULL); pthread_cond_init(&logbuffer_cond, NULL); /* track changes in sensor_combined topic */ - hrt_abstime gyro_timestamp = 0; - hrt_abstime accelerometer_timestamp = 0; - hrt_abstime magnetometer_timestamp = 0; - hrt_abstime barometer_timestamp = 0; - hrt_abstime differential_pressure_timestamp = 0; - hrt_abstime barometer1_timestamp = 0; - hrt_abstime gyro1_timestamp = 0; - hrt_abstime accelerometer1_timestamp = 0; - hrt_abstime magnetometer1_timestamp = 0; - hrt_abstime gyro2_timestamp = 0; - hrt_abstime accelerometer2_timestamp = 0; - hrt_abstime magnetometer2_timestamp = 0; + hrt_abstime gyro_timestamp[3] = {0, 0, 0}; + hrt_abstime accelerometer_timestamp[3] = {0, 0, 0}; + hrt_abstime magnetometer_timestamp[3] = {0, 0, 0}; + hrt_abstime barometer_timestamp[3] = {0, 0, 0}; + hrt_abstime differential_pressure_timestamp[3] = {0, 0, 0}; /* initialize calculated mean SNR */ float snr_mean = 0.0f; @@ -1443,144 +1439,86 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- SENSOR COMBINED --- */ if (copy_if_updated(ORB_ID(sensor_combined), &subs.sensor_sub, &buf.sensor)) { - bool write_IMU = false; - bool write_IMU1 = false; - bool write_IMU2 = false; - bool write_SENS = false; - bool write_SENS1 = false; + - if (buf.sensor.timestamp != gyro_timestamp) { - gyro_timestamp = buf.sensor.timestamp; - write_IMU = true; + for (unsigned i = 0; i < 3; i++) { + bool write_IMU = false; + bool write_SENS = false; + + if (buf.sensor.gyro_timestamp[i] != gyro_timestamp[i]) { + gyro_timestamp[i] = buf.sensor.gyro_timestamp[i]; + write_IMU = true; + } + + if (buf.sensor.accelerometer_timestamp[i] != accelerometer_timestamp[i]) { + accelerometer_timestamp[i] = buf.sensor.accelerometer_timestamp[i]; + write_IMU = true; + } + + if (buf.sensor.magnetometer_timestamp[i] != magnetometer_timestamp[i]) { + magnetometer_timestamp[i] = buf.sensor.magnetometer_timestamp[i]; + write_IMU = true; + } + + if (buf.sensor.baro_timestamp[i] != barometer_timestamp[i]) { + barometer_timestamp[i] = buf.sensor.baro_timestamp[i]; + write_SENS = true; + } + + if (buf.sensor.differential_pressure_timestamp[i] != differential_pressure_timestamp[i]) { + differential_pressure_timestamp[i] = buf.sensor.differential_pressure_timestamp[i]; + write_SENS = true; + } + + if (write_IMU) { + switch (i) { + case 0: + log_msg.msg_type = LOG_IMU_MSG; + break; + case 1: + log_msg.msg_type = LOG_IMU1_MSG; + break; + case 2: + log_msg.msg_type = LOG_IMU2_MSG; + break; + } + + log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad_s[i * 3 + 0]; + log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad_s[i * 3 + 1]; + log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad_s[i * 3 + 2]; + log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[i * 3 + 0]; + log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[i * 3 + 1]; + log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[i * 3 + 2]; + log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[i * 3 + 0]; + log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[i * 3 + 1]; + log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[i * 3 + 2]; + log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro_temp[i * 3 + 0]; + log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer_temp[i * 3 + 0]; + log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer_temp[i * 3 + 0]; + LOGBUFFER_WRITE_AND_COUNT(IMU); + } + + if (write_SENS) { + switch (i) { + case 0: + log_msg.msg_type = LOG_SENS_MSG; + break; + case 1: + log_msg.msg_type = LOG_AIR1_MSG; + break; + case 2: + continue; + break; + } + + log_msg.body.log_SENS.baro_pres = buf.sensor.baro_pres_mbar[i]; + log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter[i]; + log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius[i]; + log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa[i]; + log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure_filtered_pa[i]; + LOGBUFFER_WRITE_AND_COUNT(SENS); + } } - - if (buf.sensor.accelerometer_timestamp != accelerometer_timestamp) { - accelerometer_timestamp = buf.sensor.accelerometer_timestamp; - write_IMU = true; - } - - if (buf.sensor.magnetometer_timestamp != magnetometer_timestamp) { - magnetometer_timestamp = buf.sensor.magnetometer_timestamp; - write_IMU = true; - } - - if (buf.sensor.baro_timestamp != barometer_timestamp) { - barometer_timestamp = buf.sensor.baro_timestamp; - write_SENS = true; - } - - if (buf.sensor.differential_pressure_timestamp != differential_pressure_timestamp) { - differential_pressure_timestamp = buf.sensor.differential_pressure_timestamp; - write_SENS = true; - } - - if (write_IMU) { - log_msg.msg_type = LOG_IMU_MSG; - log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad_s[0]; - log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad_s[1]; - log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad_s[2]; - log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[0]; - log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[1]; - log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[2]; - log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[0]; - log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[1]; - log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[2]; - log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro_temp; - log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer_temp; - log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer_temp; - LOGBUFFER_WRITE_AND_COUNT(IMU); - } - - if (write_SENS) { - log_msg.msg_type = LOG_SENS_MSG; - log_msg.body.log_SENS.baro_pres = buf.sensor.baro_pres_mbar; - log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter; - log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius; - log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa; - log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure_filtered_pa; - LOGBUFFER_WRITE_AND_COUNT(SENS); - } - - if (buf.sensor.baro1_timestamp != barometer1_timestamp) { - barometer1_timestamp = buf.sensor.baro1_timestamp; - write_SENS1 = true; - } - - if (write_SENS1) { - log_msg.msg_type = LOG_AIR1_MSG; - log_msg.body.log_SENS.baro_pres = buf.sensor.baro1_pres_mbar; - log_msg.body.log_SENS.baro_alt = buf.sensor.baro1_alt_meter; - log_msg.body.log_SENS.baro_temp = buf.sensor.baro1_temp_celcius; - log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure1_pa; - log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure1_filtered_pa; - // XXX moving to AIR0-AIR2 instead of SENS - LOGBUFFER_WRITE_AND_COUNT(SENS); - } - - if (buf.sensor.accelerometer1_timestamp != accelerometer1_timestamp) { - accelerometer1_timestamp = buf.sensor.accelerometer1_timestamp; - write_IMU1 = true; - } - - if (buf.sensor.gyro1_timestamp != gyro1_timestamp) { - gyro1_timestamp = buf.sensor.gyro1_timestamp; - write_IMU1 = true; - } - - if (buf.sensor.magnetometer1_timestamp != magnetometer1_timestamp) { - magnetometer1_timestamp = buf.sensor.magnetometer1_timestamp; - write_IMU1 = true; - } - - if (write_IMU1) { - log_msg.msg_type = LOG_IMU1_MSG; - log_msg.body.log_IMU.gyro_x = buf.sensor.gyro1_rad_s[0]; - log_msg.body.log_IMU.gyro_y = buf.sensor.gyro1_rad_s[1]; - log_msg.body.log_IMU.gyro_z = buf.sensor.gyro1_rad_s[2]; - log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer1_m_s2[0]; - log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer1_m_s2[1]; - log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer1_m_s2[2]; - log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer1_ga[0]; - log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer1_ga[1]; - log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer1_ga[2]; - log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro1_temp; - log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer1_temp; - log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer1_temp; - LOGBUFFER_WRITE_AND_COUNT(IMU); - } - - if (buf.sensor.accelerometer2_timestamp != accelerometer2_timestamp) { - accelerometer2_timestamp = buf.sensor.accelerometer2_timestamp; - write_IMU2 = true; - } - - if (buf.sensor.gyro2_timestamp != gyro2_timestamp) { - gyro2_timestamp = buf.sensor.gyro2_timestamp; - write_IMU2 = true; - } - - if (buf.sensor.magnetometer2_timestamp != magnetometer2_timestamp) { - magnetometer2_timestamp = buf.sensor.magnetometer2_timestamp; - write_IMU2 = true; - } - - if (write_IMU2) { - log_msg.msg_type = LOG_IMU2_MSG; - log_msg.body.log_IMU.gyro_x = buf.sensor.gyro2_rad_s[0]; - log_msg.body.log_IMU.gyro_y = buf.sensor.gyro2_rad_s[1]; - log_msg.body.log_IMU.gyro_z = buf.sensor.gyro2_rad_s[2]; - log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer2_m_s2[0]; - log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer2_m_s2[1]; - log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer2_m_s2[2]; - log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer2_ga[0]; - log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer2_ga[1]; - log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer2_ga[2]; - log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro2_temp; - log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer2_temp; - log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer2_temp; - LOGBUFFER_WRITE_AND_COUNT(IMU); - } - } /* --- ATTITUDE --- */ @@ -1922,11 +1860,11 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp; log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative; log_msg.body.log_TECS.totalEnergyError = buf.tecs_status.totalEnergyError; - log_msg.body.log_TECS.energyDistributionError = buf.tecs_status.energyDistributionError; log_msg.body.log_TECS.totalEnergyRateError = buf.tecs_status.totalEnergyRateError; + log_msg.body.log_TECS.energyDistributionError = buf.tecs_status.energyDistributionError; log_msg.body.log_TECS.energyDistributionRateError = buf.tecs_status.energyDistributionRateError; - log_msg.body.log_TECS.throttle_integ = buf.tecs_status.throttle_integ; log_msg.body.log_TECS.pitch_integ = buf.tecs_status.pitch_integ; + log_msg.body.log_TECS.throttle_integ = buf.tecs_status.throttle_integ; log_msg.body.log_TECS.mode = (uint8_t)buf.tecs_status.mode; LOGBUFFER_WRITE_AND_COUNT(TECS); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 7f77a58e7b..1f1b979288 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -545,7 +545,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), - LOG_FORMAT(TECS, "ffffffffffffffB", "ASP,AF,FSP,F,AsSP,AsF,AsDSP,AsD,EE,EDE,ERE,EDRE,PtchI,ThrI,M"), + LOG_FORMAT(TECS, "ffffffffffffffB", "ASP,AF,FSP,F,AsSP,AsF,AsDSP,AsD,EE,ERE,EDE,EDRE,PtchI,ThrI,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), LOG_FORMAT(ENCD, "qfqf", "cnt0,vel0,cnt1,vel1"), LOG_FORMAT(TSYN, "Q", "TimeOffset"), diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp index 91230a37cb..ba7665201a 100644 --- a/src/modules/segway/BlockSegwayController.cpp +++ b/src/modules/segway/BlockSegwayController.cpp @@ -1,8 +1,9 @@ #include "BlockSegwayController.hpp" -void BlockSegwayController::update() { +void BlockSegwayController::update() +{ // wait for a sensor update, check for exit condition every 100 ms - if (poll(&_attPoll, 1, 100) < 0) return; // poll error + if (poll(&_attPoll, 1, 100) < 0) { return; } // poll error uint64_t newTimeStamp = hrt_absolute_time(); float dt = (newTimeStamp - _timeStamp) / 1.0e6f; @@ -10,20 +11,21 @@ void BlockSegwayController::update() { // check for sane values of dt // to prevent large control responses - if (dt > 1.0f || dt < 0) return; + if (dt > 1.0f || dt < 0) { return; } // set dt for all child blocks setDt(dt); // check for new updates - if (_param_update.updated()) updateParams(); + if (_param_update.updated()) { updateParams(); } // get new information from subscriptions updateSubscriptions(); // default all output to zero unless handled by mode - for (unsigned i = 2; i < NUM_ACTUATOR_CONTROLS; i++) + for (unsigned i = 2; i < NUM_ACTUATOR_CONTROLS; i++) { _actuators.control[i] = 0.0f; + } // only update guidance in auto mode if (_status.main_state == MAIN_STATE_AUTO) { diff --git a/src/modules/segway/BlockSegwayController.hpp b/src/modules/segway/BlockSegwayController.hpp index 4a01f785c5..4413a3cff8 100644 --- a/src/modules/segway/BlockSegwayController.hpp +++ b/src/modules/segway/BlockSegwayController.hpp @@ -4,10 +4,11 @@ using namespace control; -class BlockSegwayController : public control::BlockUorbEnabledAutopilot { +class BlockSegwayController : public control::BlockUorbEnabledAutopilot +{ public: BlockSegwayController() : - BlockUorbEnabledAutopilot(NULL,"SEG"), + BlockUorbEnabledAutopilot(NULL, "SEG"), th2v(this, "TH2V"), q2v(this, "Q2V"), _attPoll(), diff --git a/src/modules/segway/segway_main.cpp b/src/modules/segway/segway_main.cpp index 67acd94bf7..667b2ad3fe 100644 --- a/src/modules/segway/segway_main.cpp +++ b/src/modules/segway/segway_main.cpp @@ -74,8 +74,9 @@ static void usage(const char *reason); static void usage(const char *reason) { - if (reason) + if (reason) { fprintf(stderr, "%s\n", reason); + } fprintf(stderr, "usage: segway {start|stop|status} [-p ]\n\n"); exit(1); @@ -107,11 +108,11 @@ int segway_main(int argc, char *argv[]) thread_should_exit = false; deamon_task = px4_task_spawn_cmd("segway", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 10, - 5120, - segway_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 10, + 5120, + segway_thread_main, + (argv) ? (char *const *)&argv[2] : (char *const *)NULL); exit(0); } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 26f7b1da30..ce6f7939ad 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -129,6 +129,8 @@ #define STICK_ON_OFF_LIMIT 0.75f #define MAG_ROT_VAL_INTERNAL -1 +#define SENSOR_COUNT_MAX 3 + /* oddly, ERROR is not defined for c++ */ #ifdef ERROR # undef ERROR @@ -200,25 +202,23 @@ private: bool _hil_enabled; /**< if true, HIL is active */ bool _publishing; /**< if true, we are publishing sensor data */ + bool _armed; /**< arming status of the vehicle */ + + int _gyro_sub[SENSOR_COUNT_MAX]; /**< raw gyro data subscription */ + int _accel_sub[SENSOR_COUNT_MAX]; /**< raw accel data subscription */ + int _mag_sub[SENSOR_COUNT_MAX]; /**< raw mag data subscription */ + int _baro_sub[SENSOR_COUNT_MAX]; /**< raw baro data subscription */ + unsigned _gyro_count; /**< raw gyro data count */ + unsigned _accel_count; /**< raw accel data count */ + unsigned _mag_count; /**< raw mag data count */ + unsigned _baro_count; /**< raw baro data count */ - int _gyro_sub; /**< raw gyro0 data subscription */ - int _accel_sub; /**< raw accel0 data subscription */ - int _mag_sub; /**< raw mag0 data subscription */ - int _gyro1_sub; /**< raw gyro1 data subscription */ - int _accel1_sub; /**< raw accel1 data subscription */ - int _mag1_sub; /**< raw mag1 data subscription */ - int _gyro2_sub; /**< raw gyro2 data subscription */ - int _accel2_sub; /**< raw accel2 data subscription */ - int _mag2_sub; /**< raw mag2 data subscription */ int _rc_sub; /**< raw rc channels data subscription */ - int _baro_sub; /**< raw baro0 data subscription */ - int _baro1_sub; /**< raw baro1 data subscription */ - //int _airspeed_sub; /**< airspeed subscription */ int _diff_pres_sub; /**< raw differential pressure subscription */ - int _vcontrol_mode_sub; /**< vehicle control mode subscription */ + int _vcontrol_mode_sub; /**< vehicle control mode subscription */ int _params_sub; /**< notification of parameter updates */ - int _rc_parameter_map_sub; /**< rc parameter map subscription */ - int _manual_control_sub; /**< notification of manual control updates */ + int _rc_parameter_map_sub; /**< rc parameter map subscription */ + int _manual_control_sub; /**< notification of manual control updates */ orb_advert_t _sensor_pub; /**< combined sensor data topic */ orb_advert_t _manual_control_pub; /**< manual control signal topic */ @@ -228,18 +228,6 @@ private: orb_advert_t _airspeed_pub; /**< airspeed */ orb_advert_t _diff_pres_pub; /**< differential_pressure */ - int32_t _gyro_prio; /**< gyro0 sensor priority */ - int32_t _accel_prio; /**< accel0 sensor priority */ - int32_t _mag_prio; /**< mag0 sensor priority */ - int32_t _gyro1_prio; /**< gyro1 sensor priority */ - int32_t _accel1_prio; /** _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ - math::Matrix<3, 3> _mag_rotation[3]; /**< rotation matrix for the orientation that the external mag0 is mounted */ + math::Matrix<3, 3> _mag_rotation[3]; /**< rotation matrix for the orientation that the external mag0 is mounted */ uint64_t _battery_discharged; /**< battery discharged current in mA*ms */ hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */ @@ -377,6 +365,9 @@ private: } _parameter_handles; /**< handles for interesting parameters */ + int init_sensor_class(const struct orb_metadata *meta, int *subs, + unsigned *priorities, unsigned *errcount); + /** * Update our local parameter cache. */ @@ -495,20 +486,18 @@ Sensors::Sensors() : _sensors_task(-1), _hil_enabled(false), _publishing(true), + _armed(false), /* subscriptions */ - _gyro_sub(-1), - _accel_sub(-1), - _mag_sub(-1), - _gyro1_sub(-1), - _accel1_sub(-1), - _mag1_sub(-1), - _gyro2_sub(-1), - _accel2_sub(-1), - _mag2_sub(-1), + _gyro_sub{-1, -1, -1}, + _accel_sub{-1, -1, -1}, + _mag_sub{-1, -1, -1}, + _baro_sub{-1, -1, -1}, + _gyro_count(0), + _accel_count(0), + _mag_count(0), + _baro_count(0), _rc_sub(-1), - _baro_sub(-1), - _baro1_sub(-1), _vcontrol_mode_sub(-1), _params_sub(-1), _rc_parameter_map_sub(-1), @@ -523,19 +512,6 @@ Sensors::Sensors() : _airspeed_pub(nullptr), _diff_pres_pub(nullptr), - /* sensor priorities */ - _gyro_prio(-1), - _accel_prio(-1), - _mag_prio(-1), - _gyro1_prio(-1), - _accel1_prio(-1), - _mag1_prio(-1), - _gyro2_prio(-1), - _accel2_prio(-1), - _mag2_prio(-1), - _baro_prio(-1), - _baro1_prio(-1), - /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), @@ -546,6 +522,14 @@ Sensors::Sensors() : _battery_discharged(0), _battery_current_timestamp(0) { + /* initialize subscriptions */ + for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) { + _gyro_sub[i] = -1; + _accel_sub[i] = -1; + _mag_sub[i] = -1; + _baro_sub[i] = -1; + } + memset(&_rc, 0, sizeof(_rc)); memset(&_diff_pres, 0, sizeof(_diff_pres)); memset(&_rc_parameter_map, 0, sizeof(_rc_parameter_map)); @@ -1032,269 +1016,132 @@ Sensors::adc_init() void Sensors::accel_poll(struct sensor_combined_s &raw) { - bool accel_updated; - orb_check(_accel_sub, &accel_updated); + for (unsigned i = 0; i < _accel_count; i++) { + bool accel_updated; + orb_check(_accel_sub[i], &accel_updated); - if (accel_updated) { - struct accel_report accel_report; + if (accel_updated) { + struct accel_report accel_report; - orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); + orb_copy(ORB_ID(sensor_accel), _accel_sub[i], &accel_report); - math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); - vect = _board_rotation * vect; + math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); + vect = _board_rotation * vect; - raw.accelerometer_m_s2[0] = vect(0); - raw.accelerometer_m_s2[1] = vect(1); - raw.accelerometer_m_s2[2] = vect(2); + raw.accelerometer_m_s2[i * 3 + 0] = vect(0); + raw.accelerometer_m_s2[i * 3 + 1] = vect(1); + raw.accelerometer_m_s2[i * 3 + 2] = vect(2); - raw.accelerometer_raw[0] = accel_report.x_raw; - raw.accelerometer_raw[1] = accel_report.y_raw; - raw.accelerometer_raw[2] = accel_report.z_raw; + math::Vector<3> vect_int(accel_report.x_integral, accel_report.y_integral, accel_report.z_integral); + vect_int = _board_rotation * vect_int; - raw.accelerometer_timestamp = accel_report.timestamp; - raw.accelerometer_priority = _accel_prio; - raw.accelerometer_errcount = accel_report.error_count; - raw.accelerometer_temp = accel_report.temperature; - } + raw.accelerometer_integral_m_s[i * 3 + 0] = vect_int(0); + raw.accelerometer_integral_m_s[i * 3 + 1] = vect_int(1); + raw.accelerometer_integral_m_s[i * 3 + 2] = vect_int(2); - orb_check(_accel1_sub, &accel_updated); + raw.accelerometer_integral_dt[i] = accel_report.integral_dt; - if (accel_updated) { - struct accel_report accel_report; + raw.accelerometer_raw[i * 3 + 0] = accel_report.x_raw; + raw.accelerometer_raw[i * 3 + 1] = accel_report.y_raw; + raw.accelerometer_raw[i * 3 + 2] = accel_report.z_raw; - orb_copy(ORB_ID(sensor_accel), _accel1_sub, &accel_report); - - math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); - vect = _board_rotation * vect; - - raw.accelerometer1_m_s2[0] = vect(0); - raw.accelerometer1_m_s2[1] = vect(1); - raw.accelerometer1_m_s2[2] = vect(2); - - raw.accelerometer1_raw[0] = accel_report.x_raw; - raw.accelerometer1_raw[1] = accel_report.y_raw; - raw.accelerometer1_raw[2] = accel_report.z_raw; - - raw.accelerometer1_timestamp = accel_report.timestamp; - raw.accelerometer1_priority = _accel1_prio; - raw.accelerometer1_errcount = accel_report.error_count; - raw.accelerometer1_temp = accel_report.temperature; - } - - orb_check(_accel2_sub, &accel_updated); - - if (accel_updated) { - struct accel_report accel_report; - - orb_copy(ORB_ID(sensor_accel), _accel2_sub, &accel_report); - - math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); - vect = _board_rotation * vect; - - raw.accelerometer2_m_s2[0] = vect(0); - raw.accelerometer2_m_s2[1] = vect(1); - raw.accelerometer2_m_s2[2] = vect(2); - - raw.accelerometer2_raw[0] = accel_report.x_raw; - raw.accelerometer2_raw[1] = accel_report.y_raw; - raw.accelerometer2_raw[2] = accel_report.z_raw; - - raw.accelerometer2_timestamp = accel_report.timestamp; - raw.accelerometer2_priority = _accel2_prio; - raw.accelerometer2_errcount = accel_report.error_count; - raw.accelerometer2_temp = accel_report.temperature; + raw.accelerometer_timestamp[i] = accel_report.timestamp; + raw.accelerometer_errcount[i] = accel_report.error_count; + raw.accelerometer_temp[i] = accel_report.temperature; + } } } void Sensors::gyro_poll(struct sensor_combined_s &raw) { - bool gyro_updated; - orb_check(_gyro_sub, &gyro_updated); + for (unsigned i = 0; i < _gyro_count; i++) { + bool gyro_updated; + orb_check(_gyro_sub[i], &gyro_updated); - if (gyro_updated) { - struct gyro_report gyro_report; + if (gyro_updated) { + struct gyro_report gyro_report; - orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); + orb_copy(ORB_ID(sensor_gyro), _gyro_sub[i], &gyro_report); - math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); - vect = _board_rotation * vect; + math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); + vect = _board_rotation * vect; - raw.gyro_rad_s[0] = vect(0); - raw.gyro_rad_s[1] = vect(1); - raw.gyro_rad_s[2] = vect(2); + raw.gyro_rad_s[i * 3 + 0] = vect(0); + raw.gyro_rad_s[i * 3 + 1] = vect(1); + raw.gyro_rad_s[i * 3 + 2] = vect(2); - raw.gyro_raw[0] = gyro_report.x_raw; - raw.gyro_raw[1] = gyro_report.y_raw; - raw.gyro_raw[2] = gyro_report.z_raw; + math::Vector<3> vect_int(gyro_report.x_integral, gyro_report.y_integral, gyro_report.z_integral); + vect_int = _board_rotation * vect_int; - raw.timestamp = gyro_report.timestamp; - raw.gyro_priority = _gyro_prio; - raw.gyro_errcount = gyro_report.error_count; - raw.gyro_temp = gyro_report.temperature; - } + raw.gyro_integral_rad[i * 3 + 0] = vect_int(0); + raw.gyro_integral_rad[i * 3 + 1] = vect_int(1); + raw.gyro_integral_rad[i * 3 + 2] = vect_int(2); - orb_check(_gyro1_sub, &gyro_updated); + raw.gyro_integral_dt[i] = gyro_report.integral_dt; - if (gyro_updated) { - struct gyro_report gyro_report; + raw.gyro_raw[i * 3 + 0] = gyro_report.x_raw; + raw.gyro_raw[i * 3 + 1] = gyro_report.y_raw; + raw.gyro_raw[i * 3 + 2] = gyro_report.z_raw; - orb_copy(ORB_ID(sensor_gyro), _gyro1_sub, &gyro_report); - - math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); - vect = _board_rotation * vect; - - raw.gyro1_rad_s[0] = vect(0); - raw.gyro1_rad_s[1] = vect(1); - raw.gyro1_rad_s[2] = vect(2); - - raw.gyro1_raw[0] = gyro_report.x_raw; - raw.gyro1_raw[1] = gyro_report.y_raw; - raw.gyro1_raw[2] = gyro_report.z_raw; - - raw.gyro1_timestamp = gyro_report.timestamp; - raw.gyro1_priority = _gyro1_prio; - raw.gyro1_errcount = gyro_report.error_count; - raw.gyro1_temp = gyro_report.temperature; - } - - orb_check(_gyro2_sub, &gyro_updated); - - if (gyro_updated) { - struct gyro_report gyro_report; - - orb_copy(ORB_ID(sensor_gyro), _gyro2_sub, &gyro_report); - - math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); - vect = _board_rotation * vect; - - raw.gyro2_rad_s[0] = vect(0); - raw.gyro2_rad_s[1] = vect(1); - raw.gyro2_rad_s[2] = vect(2); - - raw.gyro2_raw[0] = gyro_report.x_raw; - raw.gyro2_raw[1] = gyro_report.y_raw; - raw.gyro2_raw[2] = gyro_report.z_raw; - - raw.gyro2_timestamp = gyro_report.timestamp; - raw.gyro2_priority = _gyro2_prio; - raw.gyro2_errcount = gyro_report.error_count; - raw.gyro2_temp = gyro_report.temperature; + raw.gyro_timestamp[i] = gyro_report.timestamp; + if (i == 0) { + raw.timestamp = gyro_report.timestamp; + } + raw.gyro_errcount[i] = gyro_report.error_count; + raw.gyro_temp[i] = gyro_report.temperature; + } } } void Sensors::mag_poll(struct sensor_combined_s &raw) { - bool mag_updated; - orb_check(_mag_sub, &mag_updated); + for (unsigned i = 0; i < _mag_count; i++) { + bool mag_updated; + orb_check(_mag_sub[i], &mag_updated); - if (mag_updated) { - struct mag_report mag_report; + if (mag_updated) { + struct mag_report mag_report; - orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); + orb_copy(ORB_ID(sensor_mag), _mag_sub[i], &mag_report); - math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); + math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); - vect = _mag_rotation[0] * vect; + vect = _mag_rotation[i] * vect; - raw.magnetometer_ga[0] = vect(0); - raw.magnetometer_ga[1] = vect(1); - raw.magnetometer_ga[2] = vect(2); + raw.magnetometer_ga[i * 3 + 0] = vect(0); + raw.magnetometer_ga[i * 3 + 1] = vect(1); + raw.magnetometer_ga[i * 3 + 2] = vect(2); - raw.magnetometer_raw[0] = mag_report.x_raw; - raw.magnetometer_raw[1] = mag_report.y_raw; - raw.magnetometer_raw[2] = mag_report.z_raw; + raw.magnetometer_raw[i * 3 + 0] = mag_report.x_raw; + raw.magnetometer_raw[i * 3 + 1] = mag_report.y_raw; + raw.magnetometer_raw[i * 3 + 2] = mag_report.z_raw; - raw.magnetometer_timestamp = mag_report.timestamp; - raw.magnetometer_priority = _mag_prio; - raw.magnetometer_errcount = mag_report.error_count; - raw.magnetometer_temp = mag_report.temperature; - } - - orb_check(_mag1_sub, &mag_updated); - - if (mag_updated) { - struct mag_report mag_report; - - orb_copy(ORB_ID(sensor_mag), _mag1_sub, &mag_report); - - math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); - - vect = _mag_rotation[1] * vect; - - raw.magnetometer1_ga[0] = vect(0); - raw.magnetometer1_ga[1] = vect(1); - raw.magnetometer1_ga[2] = vect(2); - - raw.magnetometer1_raw[0] = mag_report.x_raw; - raw.magnetometer1_raw[1] = mag_report.y_raw; - raw.magnetometer1_raw[2] = mag_report.z_raw; - - raw.magnetometer1_timestamp = mag_report.timestamp; - raw.magnetometer1_priority = _mag1_prio; - raw.magnetometer1_errcount = mag_report.error_count; - raw.magnetometer1_temp = mag_report.temperature; - } - - orb_check(_mag2_sub, &mag_updated); - - if (mag_updated) { - struct mag_report mag_report; - - orb_copy(ORB_ID(sensor_mag), _mag2_sub, &mag_report); - - math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); - - vect = _mag_rotation[2] * vect; - - raw.magnetometer2_ga[0] = vect(0); - raw.magnetometer2_ga[1] = vect(1); - raw.magnetometer2_ga[2] = vect(2); - - raw.magnetometer2_raw[0] = mag_report.x_raw; - raw.magnetometer2_raw[1] = mag_report.y_raw; - raw.magnetometer2_raw[2] = mag_report.z_raw; - - raw.magnetometer2_timestamp = mag_report.timestamp; - raw.magnetometer2_priority = _mag2_prio; - raw.magnetometer2_errcount = mag_report.error_count; - raw.magnetometer2_temp = mag_report.temperature; + raw.magnetometer_timestamp[i] = mag_report.timestamp; + raw.magnetometer_errcount[i] = mag_report.error_count; + raw.magnetometer_temp[i] = mag_report.temperature; + } } } void Sensors::baro_poll(struct sensor_combined_s &raw) { - bool baro_updated; - orb_check(_baro_sub, &baro_updated); + for (unsigned i = 0; i < _baro_count; i++) { + bool baro_updated; + orb_check(_baro_sub[i], &baro_updated); - if (baro_updated) { + if (baro_updated) { - orb_copy(ORB_ID(sensor_baro), _baro_sub, &_barometer); + orb_copy(ORB_ID(sensor_baro), _baro_sub[i], &_barometer); - raw.baro_pres_mbar = _barometer.pressure; // Pressure in mbar - raw.baro_alt_meter = _barometer.altitude; // Altitude in meters - raw.baro_temp_celcius = _barometer.temperature; // Temperature in degrees celcius + raw.baro_pres_mbar[i] = _barometer.pressure; // Pressure in mbar + raw.baro_alt_meter[i] = _barometer.altitude; // Altitude in meters + raw.baro_temp_celcius[i] = _barometer.temperature; // Temperature in degrees celcius - raw.baro_timestamp = _barometer.timestamp; - raw.baro_priority = _baro_prio; - } - - orb_check(_baro1_sub, &baro_updated); - - if (baro_updated) { - - struct baro_report baro_report; - - orb_copy(ORB_ID(sensor_baro), _baro1_sub, &baro_report); - - raw.baro1_pres_mbar = baro_report.pressure; // Pressure in mbar - raw.baro1_alt_meter = baro_report.altitude; // Altitude in meters - raw.baro1_temp_celcius = baro_report.temperature; // Temperature in degrees celcius - - raw.baro1_timestamp = baro_report.timestamp; - raw.baro1_priority = _baro1_prio; + raw.baro_timestamp[i] = _barometer.timestamp; + } } } @@ -1307,12 +1154,12 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) if (updated) { orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres); - raw.differential_pressure_pa = _diff_pres.differential_pressure_raw_pa; - raw.differential_pressure_timestamp = _diff_pres.timestamp; - raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa; + raw.differential_pressure_pa[0] = _diff_pres.differential_pressure_raw_pa; + raw.differential_pressure_timestamp[0] = _diff_pres.timestamp; + raw.differential_pressure_filtered_pa[0] = _diff_pres.differential_pressure_filtered_pa; float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : - (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); + (raw.baro_temp_celcius[0] - PCB_TEMP_ESTIMATE_DEG); _airspeed.timestamp = _diff_pres.timestamp; @@ -1320,11 +1167,11 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) _airspeed.indicated_airspeed_m_s = math::max(0.0f, calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa)); _airspeed.true_airspeed_m_s = math::max(0.0f, - calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f, - raw.baro_pres_mbar * 1e2f, air_temperature_celsius)); + calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar[0] * 1e2f, + raw.baro_pres_mbar[0] * 1e2f, air_temperature_celsius)); _airspeed.true_airspeed_unfiltered_m_s = math::max(0.0f, - calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + raw.baro_pres_mbar * 1e2f, - raw.baro_pres_mbar * 1e2f, air_temperature_celsius)); + calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + raw.baro_pres_mbar[0] * 1e2f, + raw.baro_pres_mbar[0] * 1e2f, air_temperature_celsius)); _airspeed.air_temperature_celsius = air_temperature_celsius; @@ -1352,16 +1199,17 @@ Sensors::vehicle_control_mode_poll() orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode); /* switching from non-HIL to HIL mode */ - //printf("[sensors] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled); if (vcontrol_mode.flag_system_hil_enabled && !_hil_enabled) { _hil_enabled = true; _publishing = false; + _armed = vcontrol_mode.flag_armed; /* switching from HIL to non-HIL mode */ } else if (!_publishing && !_hil_enabled) { _hil_enabled = false; _publishing = true; + _armed = vcontrol_mode.flag_armed; } } } @@ -1391,7 +1239,7 @@ Sensors::parameter_update_poll(bool forced) unsigned accel_count = 0; /* run through all gyro sensors */ - for (unsigned s = 0; s < 3; s++) { + for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) { res = ERROR; (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); @@ -1405,7 +1253,7 @@ Sensors::parameter_update_poll(bool forced) bool config_ok = false; /* run through all stored calibrations */ - for (unsigned i = 0; i < 3; i++) { + for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) { /* initially status is ok per config */ failed = false; @@ -1457,7 +1305,7 @@ Sensors::parameter_update_poll(bool forced) } /* run through all accel sensors */ - for (unsigned s = 0; s < 3; s++) { + for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) { res = ERROR; (void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s); @@ -1471,7 +1319,7 @@ Sensors::parameter_update_poll(bool forced) bool config_ok = false; /* run through all stored calibrations */ - for (unsigned i = 0; i < 3; i++) { + for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) { /* initially status is ok per config */ failed = false; @@ -1523,7 +1371,7 @@ Sensors::parameter_update_poll(bool forced) } /* run through all mag sensors */ - for (unsigned s = 0; s < 3; s++) { + for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) { /* set a valid default rotation (same as board). * if the mag is configured, this might be replaced @@ -1544,7 +1392,7 @@ Sensors::parameter_update_poll(bool forced) bool config_ok = false; /* run through all stored calibrations */ - for (unsigned i = 0; i < 3; i++) { + for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) { /* initially status is ok per config */ failed = false; @@ -2099,6 +1947,26 @@ Sensors::task_main_trampoline(int argc, char *argv[]) sensors::g_sensors->task_main(); } +int +Sensors::init_sensor_class(const struct orb_metadata *meta, int *subs, + unsigned *priorities, unsigned *errcount) +{ + unsigned group_count = orb_group_count(meta); + + if (group_count > SENSOR_COUNT_MAX) { + group_count = SENSOR_COUNT_MAX; + } + + for (unsigned i = 0; i < group_count; i++) { + if (subs[i] < 0) { + subs[i] = orb_subscribe_multi(meta, i); + orb_priority(subs[i], (int32_t*)&priorities[i]); + } + } + + return group_count; +} + void Sensors::task_main() { @@ -2120,7 +1988,7 @@ Sensors::task_main() } while (0); if (ret) { - warnx("Sensor initialization failed"); + warnx("sensor initialization failed"); _sensors_task = -1; if (_fd_adc >=0) { px4_close(_fd_adc); @@ -2129,74 +1997,47 @@ Sensors::task_main() return; } + struct sensor_combined_s raw = {}; + + /* ensure no overflows can occur */ + static_assert((sizeof(raw.gyro_timestamp) / sizeof(raw.gyro_timestamp[0])) >= SENSOR_COUNT_MAX, + "SENSOR_COUNT_MAX larger than sensor_combined datastructure fields. Overflow would occur"); + /* * do subscriptions */ - _gyro_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 0); - _accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0); - _mag_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 0); - _gyro1_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 1); - _accel1_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 1); - _mag1_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 1); - _gyro2_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 2); - _accel2_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 2); - _mag2_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 2); + + _gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0], + &raw.gyro_priority[0], &raw.gyro_errcount[0]); + + _mag_count = init_sensor_class(ORB_ID(sensor_mag), &_mag_sub[0], + &raw.magnetometer_priority[0], &raw.magnetometer_errcount[0]); + + _accel_count = init_sensor_class(ORB_ID(sensor_accel), &_accel_sub[0], + &raw.accelerometer_priority[0], &raw.accelerometer_errcount[0]); + + _baro_count = init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0], + &raw.baro_priority[0], &raw.baro_errcount[0]); + _rc_sub = orb_subscribe(ORB_ID(input_rc)); - _baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0); - _baro1_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 1); _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _rc_parameter_map_sub = orb_subscribe(ORB_ID(rc_parameter_map)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - /* - * get sensor priorities - */ - orb_priority(_gyro_sub, &_gyro_prio); - orb_priority(_accel_sub, &_accel_prio); - orb_priority(_mag_sub, &_mag_prio); - orb_priority(_gyro1_sub, &_gyro1_prio); - orb_priority(_accel1_sub, &_accel1_prio); - orb_priority(_mag1_sub, &_mag1_prio); - orb_priority(_gyro2_sub, &_gyro2_prio); - orb_priority(_accel2_sub, &_accel2_prio); - orb_priority(_mag2_sub, &_mag2_prio); - orb_priority(_baro_sub, &_baro_prio); - orb_priority(_baro1_sub, &_baro1_prio); - /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vcontrol_mode_sub, 200); - /* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */ - orb_set_interval(_gyro_sub, 4); - /* * do advertisements */ - struct sensor_combined_s raw; - memset(&raw, 0, sizeof(raw)); raw.timestamp = hrt_absolute_time(); raw.adc_voltage_v[0] = 0.0f; raw.adc_voltage_v[1] = 0.0f; raw.adc_voltage_v[2] = 0.0f; raw.adc_voltage_v[3] = 0.0f; - /* set high initial error counts to deselect gyros */ - raw.gyro_errcount = 100000; - raw.gyro1_errcount = 100000; - raw.gyro2_errcount = 100000; - - /* set high initial error counts to deselect accels */ - raw.accelerometer_errcount = 100000; - raw.accelerometer1_errcount = 100000; - raw.accelerometer2_errcount = 100000; - - /* set high initial error counts to deselect mags */ - raw.magnetometer_errcount = 100000; - raw.magnetometer1_errcount = 100000; - raw.magnetometer2_errcount = 100000; - memset(&_battery_status, 0, sizeof(_battery_status)); _battery_status.voltage_v = -1.0f; _battery_status.voltage_filtered_v = -1.0f; @@ -2219,14 +2060,16 @@ Sensors::task_main() /* wakeup source(s) */ px4_pollfd_struct_t fds[1]; - /* use the gyro to pace output - XXX BROKEN if we are using the L3GD20 */ - fds[0].fd = _gyro_sub; + /* use the gyro to pace output */ + fds[0].fd = _gyro_sub[0]; fds[0].events = POLLIN; _task_should_exit = false; raw.timestamp = 0; + uint64_t _last_config_update = hrt_absolute_time(); + while (!_task_should_exit) { /* wait for up to 50ms for data */ @@ -2236,7 +2079,7 @@ Sensors::task_main() /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { - warn("poll error %d, %d", pret, errno); + warnx("poll error %d, %d", pret, errno); continue; } @@ -2253,13 +2096,13 @@ Sensors::task_main() baro_poll(raw); /* work out if main gyro timed out and fail over to alternate gyro */ - if (hrt_elapsed_time(&raw.timestamp) > 20 * 1000) { + if (hrt_elapsed_time(&raw.gyro_timestamp[0]) > 20 * 1000) { /* if the secondary failed as well, go to the tertiary */ - if (hrt_elapsed_time(&raw.gyro1_timestamp) > 20 * 1000) { - fds[0].fd = _gyro2_sub; + if (hrt_elapsed_time(&raw.gyro_timestamp[1]) > 20 * 1000) { + fds[0].fd = _gyro_sub[2]; } else { - fds[0].fd = _gyro1_sub; + fds[0].fd = _gyro_sub[1]; } } @@ -2273,11 +2116,32 @@ Sensors::task_main() orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw); } - /* check parameters for updates */ - parameter_update_poll(); + /* keep adding sensors as long as we are not armed, + * when not adding sensors poll for param updates + */ + if (!_armed && hrt_elapsed_time(&_last_config_update) > 500 * 1000) { + _gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0], + &raw.gyro_priority[0], &raw.gyro_errcount[0]); - /* check rc parameter map for updates */ - rc_parameter_map_poll(); + _mag_count = init_sensor_class(ORB_ID(sensor_mag), &_mag_sub[0], + &raw.magnetometer_priority[0], &raw.magnetometer_errcount[0]); + + _accel_count = init_sensor_class(ORB_ID(sensor_accel), &_accel_sub[0], + &raw.accelerometer_priority[0], &raw.accelerometer_errcount[0]); + + _baro_count = init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0], + &raw.baro_priority[0], &raw.baro_errcount[0]); + + _last_config_update = hrt_absolute_time(); + + } else { + + /* check parameters for updates */ + parameter_update_poll(); + + /* check rc parameter map for updates */ + rc_parameter_map_poll(); + } /* Look for new r/c input data */ rc_poll(); @@ -2296,7 +2160,7 @@ Sensors::start() ASSERT(_sensors_task == -1); /* start the task */ - _sensors_task = px4_task_spawn_cmd("sensors_task", + _sensors_task = px4_task_spawn_cmd("sensors", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index f0345ba036..5d9e29be3a 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -86,6 +86,11 @@ bool Simulator::getGPSSample(uint8_t *buf, int len) return _gps.copyData(buf, len); } +bool Simulator::getAirspeedSample(uint8_t *buf, int len) +{ + return _airspeed.copyData(buf, len); +} + void Simulator::write_MPU_data(void *buf) { _mpu.writeData(buf); } @@ -106,6 +111,10 @@ void Simulator::write_gps_data(void *buf) { _gps.writeData(buf); } +void Simulator::write_airspeed_data(void *buf) { + _airspeed.writeData(buf); +} + int Simulator::start(int argc, char *argv[]) { int ret = 0; @@ -141,10 +150,6 @@ static void usage() __BEGIN_DECLS extern int simulator_main(int argc, char *argv[]); -extern void led_init(void); -extern void led_on(int led); -extern void led_off(int led); -extern void led_toggle(int led); __END_DECLS extern "C" { @@ -198,39 +203,3 @@ int simulator_main(int argc, char *argv[]) } } - -bool static _led_state[2] = { false , false }; - -__EXPORT void led_init() -{ - PX4_DEBUG("LED_INIT"); -} - -__EXPORT void led_on(int led) -{ - if (led == 1 || led == 0) - { - PX4_DEBUG("LED%d_ON", led); - _led_state[led] = true; - } -} - -__EXPORT void led_off(int led) -{ - if (led == 1 || led == 0) - { - PX4_DEBUG("LED%d_OFF", led); - _led_state[led] = false; - } -} - -__EXPORT void led_toggle(int led) -{ - if (led == 1 || led == 0) - { - _led_state[led] = !_led_state[led]; - PX4_DEBUG("LED%d_TOGGLE: %s", led, _led_state[led] ? "ON" : "OFF"); - - } -} - diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index dbb3d5b783..5d8eedac2d 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -51,6 +51,7 @@ #include #include #include +#include #include #include namespace simulator { @@ -94,6 +95,13 @@ struct RawBaroData { }; #pragma pack(pop) +#pragma pack(push, 1) +struct RawAirspeedData { + float temperature; + float diff_pressure; +}; +#pragma pack(pop) + #pragma pack(push, 1) struct RawGPSData { int32_t lat; @@ -191,12 +199,14 @@ public: bool getMPUReport(uint8_t *buf, int len); bool getBaroSample(uint8_t *buf, int len); bool getGPSSample(uint8_t *buf, int len); + bool getAirspeedSample(uint8_t *buf, int len); void write_MPU_data(void *buf); void write_accel_data(void *buf); void write_mag_data(void *buf); void write_baro_data(void *buf); void write_gps_data(void *buf); + void write_airspeed_data(void *buf); bool isInitialized() { return _initialized; } @@ -207,6 +217,7 @@ private: _baro(1), _mag(1), _gps(1), + _airspeed(1), _accel_pub(nullptr), _baro_pub(nullptr), _gyro_pub(nullptr), @@ -238,17 +249,20 @@ private: simulator::Report _baro; simulator::Report _mag; simulator::Report _gps; + simulator::Report _airspeed; // uORB publisher handlers orb_advert_t _accel_pub; orb_advert_t _baro_pub; orb_advert_t _gyro_pub; orb_advert_t _mag_pub; + orb_advert_t _flow_pub; bool _initialized; // class methods int publish_sensor_topics(mavlink_hil_sensor_t *imu); + int publish_flow_topic(mavlink_hil_optical_flow_t *flow); #ifndef __PX4_QURT // uORB publisher handlers diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index ec3a7c8a24..82dc7217b4 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -187,11 +187,17 @@ void Simulator::update_sensors(mavlink_hil_sensor_t *imu) { RawBaroData baro; // calculate air pressure from altitude (valid for low altitude) - baro.pressure = PRESS_GROUND - GRAVITY * DENSITY * imu->pressure_alt; + baro.pressure = (PRESS_GROUND - GRAVITY * DENSITY * imu->pressure_alt) / 100.0f; // convert from Pa to mbar baro.altitude = imu->pressure_alt; baro.temperature = imu->temperature; write_baro_data((void *)&baro); + + RawAirspeedData airspeed; + airspeed.temperature = imu->temperature; + airspeed.diff_pressure = imu->diff_pressure; + + write_airspeed_data((void *)&airspeed); } void Simulator::update_gps(mavlink_hil_gps_t *gps_sim) { @@ -223,6 +229,12 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish) { update_sensors(&imu); break; + case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW: + mavlink_hil_optical_flow_t flow; + mavlink_msg_hil_optical_flow_decode(msg, &flow); + publish_flow_topic(&flow); + break; + case MAVLINK_MSG_ID_HIL_GPS: mavlink_hil_gps_t gps_sim; mavlink_msg_hil_gps_decode(msg, &gps_sim); @@ -702,5 +714,40 @@ int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu) { } } - return OK; + return OK; +} + +int Simulator::publish_flow_topic(mavlink_hil_optical_flow_t* flow_mavlink) +{ + uint64_t timestamp = hrt_absolute_time(); + + /* gyro */ + { + struct optical_flow_s flow; + memset(&flow, 0, sizeof(flow)); + + flow.sensor_id = flow_mavlink->sensor_id; + flow.timestamp = timestamp; + flow.time_since_last_sonar_update = 0; + flow.frame_count_since_last_readout = 0; // ? + flow.integration_timespan = flow_mavlink->integration_time_us; + + flow.ground_distance_m = flow_mavlink->distance; + flow.gyro_temperature = flow_mavlink->temperature; + flow.gyro_x_rate_integral = flow_mavlink->integrated_xgyro; + flow.gyro_y_rate_integral = flow_mavlink->integrated_ygyro; + flow.gyro_z_rate_integral = flow_mavlink->integrated_zgyro; + flow.pixel_flow_x_integral = flow_mavlink->integrated_x; + flow.pixel_flow_x_integral = flow_mavlink->integrated_y; + flow.quality = flow_mavlink->quality; + + if (_flow_pub == nullptr) { + _flow_pub = orb_advertise(ORB_ID(optical_flow), &flow); + + } else { + orb_publish(ORB_ID(optical_flow), _flow_pub, &flow); + } + } + + return OK; } diff --git a/src/modules/systemlib/airspeed.c b/src/modules/systemlib/airspeed.c index 1a479c2058..fa5db7c89b 100644 --- a/src/modules/systemlib/airspeed.c +++ b/src/modules/systemlib/airspeed.c @@ -60,13 +60,14 @@ float calc_indicated_airspeed(float differential_pressure) { if (differential_pressure > 0.0f) { - return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + return sqrtf((2.0f * differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + } else { - return -sqrtf((2.0f*fabsf(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + return -sqrtf((2.0f * fabsf(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); } } - + /** * Calculate true airspeed from indicated airspeed. * @@ -79,9 +80,10 @@ float calc_indicated_airspeed(float differential_pressure) */ float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_ambient, float temperature_celsius) { - return speed_indicated * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / get_air_density(pressure_ambient, temperature_celsius)); + return speed_indicated * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / get_air_density(pressure_ambient, + temperature_celsius)); } - + /** * Directly calculate true airspeed * @@ -103,9 +105,10 @@ float calc_true_airspeed(float total_pressure, float static_pressure, float temp float pressure_difference = total_pressure - static_pressure; if (pressure_difference > 0) { - return sqrtf((2.0f*(pressure_difference)) / density); + return sqrtf((2.0f * (pressure_difference)) / density); + } else { - return -sqrtf((2.0f*fabsf(pressure_difference)) / density); + return -sqrtf((2.0f * fabsf(pressure_difference)) / density); } } diff --git a/src/modules/systemlib/airspeed.h b/src/modules/systemlib/airspeed.h index 8dccaab9c5..2a2b0873da 100644 --- a/src/modules/systemlib/airspeed.h +++ b/src/modules/systemlib/airspeed.h @@ -46,51 +46,52 @@ #include "math.h" #include "conversions.h" - __BEGIN_DECLS - - /** - * Calculate indicated airspeed. - * - * Note that the indicated airspeed is not the true airspeed because it - * lacks the air density compensation. Use the calc_true_airspeed functions to get - * the true airspeed. - * - * @param total_pressure pressure inside the pitot/prandtl tube - * @param static_pressure pressure at the side of the tube/airplane - * @return indicated airspeed in m/s - */ - __EXPORT float calc_indicated_airspeed(float differential_pressure); - - /** - * Calculate true airspeed from indicated airspeed. - * - * Note that the true airspeed is NOT the groundspeed, because of the effects of wind - * - * @param speed_indicated current indicated airspeed - * @param pressure_ambient pressure at the side of the tube/airplane - * @param temperature_celsius air temperature in degrees celcius - * @return true airspeed in m/s - */ - __EXPORT float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_ambient, float temperature_celsius); - - /** - * Directly calculate true airspeed - * - * Note that the true airspeed is NOT the groundspeed, because of the effects of wind - * - * @param total_pressure pressure inside the pitot/prandtl tube - * @param static_pressure pressure at the side of the tube/airplane - * @param temperature_celsius air temperature in degrees celcius - * @return true airspeed in m/s - */ - __EXPORT float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius); +__BEGIN_DECLS - /** - * Calculates air density. +/** + * Calculate indicated airspeed. * - * @param static_pressure ambient pressure in millibar - * @param temperature_celcius air / ambient temperature in celcius + * Note that the indicated airspeed is not the true airspeed because it + * lacks the air density compensation. Use the calc_true_airspeed functions to get + * the true airspeed. + * + * @param total_pressure pressure inside the pitot/prandtl tube + * @param static_pressure pressure at the side of the tube/airplane + * @return indicated airspeed in m/s */ +__EXPORT float calc_indicated_airspeed(float differential_pressure); + +/** + * Calculate true airspeed from indicated airspeed. + * + * Note that the true airspeed is NOT the groundspeed, because of the effects of wind + * + * @param speed_indicated current indicated airspeed + * @param pressure_ambient pressure at the side of the tube/airplane + * @param temperature_celsius air temperature in degrees celcius + * @return true airspeed in m/s + */ +__EXPORT float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_ambient, + float temperature_celsius); + +/** + * Directly calculate true airspeed + * + * Note that the true airspeed is NOT the groundspeed, because of the effects of wind + * + * @param total_pressure pressure inside the pitot/prandtl tube + * @param static_pressure pressure at the side of the tube/airplane + * @param temperature_celsius air temperature in degrees celcius + * @return true airspeed in m/s + */ +__EXPORT float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius); + +/** +* Calculates air density. +* +* @param static_pressure ambient pressure in millibar +* @param temperature_celcius air / ambient temperature in celcius +*/ __EXPORT float get_air_density(float static_pressure, float temperature_celsius); __END_DECLS diff --git a/src/modules/systemlib/bson/tinybson.c b/src/modules/systemlib/bson/tinybson.c index 2378a2d033..e83fd67999 100644 --- a/src/modules/systemlib/bson/tinybson.c +++ b/src/modules/systemlib/bson/tinybson.c @@ -75,14 +75,19 @@ read_x(bson_decoder_t decoder, void *p, size_t s) if (decoder->buf != NULL) { /* staged operations to avoid integer overflow for corrupt data */ - if (s >= decoder->bufsize) + if (s >= decoder->bufsize) { CODER_KILL(decoder, "buffer too small for read"); - if ((decoder->bufsize - s) < decoder->bufpos) + } + + if ((decoder->bufsize - s) < decoder->bufpos) { CODER_KILL(decoder, "not enough data for read"); + } + memcpy(p, (decoder->buf + decoder->bufpos), s); decoder->bufpos += s; return 0; } + debug("no source"); return -1; } @@ -126,31 +131,37 @@ bson_decoder_init_file(bson_decoder_t decoder, int fd, bson_decoder_callback cal decoder->node.type = BSON_UNDEFINED; /* read and discard document size */ - if (read_int32(decoder, &junk)) + if (read_int32(decoder, &junk)) { CODER_KILL(decoder, "failed discarding length"); + } /* ready for decoding */ return 0; } int -bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_decoder_callback callback, void *private) +bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_decoder_callback callback, + void *private) { int32_t len; /* argument sanity */ - if ((buf == NULL) || (callback == NULL)) + if ((buf == NULL) || (callback == NULL)) { return -1; + } decoder->fd = -1; decoder->buf = (uint8_t *)buf; decoder->dead = false; + if (bufsize == 0) { decoder->bufsize = *(uint32_t *)buf; debug("auto-detected %u byte object", decoder->bufsize); + } else { decoder->bufsize = bufsize; } + decoder->bufpos = 0; decoder->callback = callback; decoder->private = private; @@ -159,10 +170,13 @@ bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_ decoder->node.type = BSON_UNDEFINED; /* read and discard document size */ - if (read_int32(decoder, &len)) + if (read_int32(decoder, &len)) { CODER_KILL(decoder, "failed reading length"); - if ((len > 0) && (len > (int)decoder->bufsize)) + } + + if ((len > 0) && (len > (int)decoder->bufsize)) { CODER_KILL(decoder, "document length larger than buffer"); + } /* ready for decoding */ return 0; @@ -179,8 +193,9 @@ bson_decoder_next(bson_decoder_t decoder) /* if the previous node was EOO, pop a nesting level */ if (decoder->node.type == BSON_EOO) { - if (decoder->nesting > 0) + if (decoder->nesting > 0) { decoder->nesting--; + } /* if the nesting level is now zero, the top-level document is done */ if (decoder->nesting == 0) { @@ -195,15 +210,17 @@ bson_decoder_next(bson_decoder_t decoder) /* if there are unread bytes pending in the stream, discard them */ while (decoder->pending > 0) { - if (read_int8(decoder, &tbyte)) + if (read_int8(decoder, &tbyte)) { CODER_KILL(decoder, "read error discarding pending bytes"); + } decoder->pending--; } /* get the type byte */ - if (read_int8(decoder, &tbyte)) + if (read_int8(decoder, &tbyte)) { CODER_KILL(decoder, "read error on type byte"); + } decoder->node.type = tbyte; decoder->pending = 0; @@ -213,20 +230,24 @@ bson_decoder_next(bson_decoder_t decoder) /* EOO is special; it has no name/data following */ if (decoder->node.type == BSON_EOO) { decoder->node.name[0] = '\0'; + } else { /* get the node name */ nlen = 0; for (;;) { - if (nlen >= BSON_MAXNAME) + if (nlen >= BSON_MAXNAME) { CODER_KILL(decoder, "node name overflow"); + } - if (read_int8(decoder, (int8_t *)&decoder->node.name[nlen])) + if (read_int8(decoder, (int8_t *)&decoder->node.name[nlen])) { CODER_KILL(decoder, "read error on node name"); + } - if (decoder->node.name[nlen] == '\0') + if (decoder->node.name[nlen] == '\0') { break; + } nlen++; } @@ -235,45 +256,55 @@ bson_decoder_next(bson_decoder_t decoder) switch (decoder->node.type) { case BSON_BOOL: - if (read_int8(decoder, &tbyte)) + if (read_int8(decoder, &tbyte)) { CODER_KILL(decoder, "read error on BSON_BOOL"); + } + decoder->node.b = (tbyte != 0); break; case BSON_INT32: - if (read_int32(decoder, &tint)) + if (read_int32(decoder, &tint)) { CODER_KILL(decoder, "read error on BSON_INT"); + } + decoder->node.i = tint; break; case BSON_INT64: - if (read_int64(decoder, &decoder->node.i)) + if (read_int64(decoder, &decoder->node.i)) { CODER_KILL(decoder, "read error on BSON_INT"); + } break; case BSON_DOUBLE: - if (read_double(decoder, &decoder->node.d)) + if (read_double(decoder, &decoder->node.d)) { CODER_KILL(decoder, "read error on BSON_DOUBLE"); + } break; case BSON_STRING: - if (read_int32(decoder, &decoder->pending)) + if (read_int32(decoder, &decoder->pending)) { CODER_KILL(decoder, "read error on BSON_STRING length"); + } + break; case BSON_BINDATA: - if (read_int32(decoder, &decoder->pending)) + if (read_int32(decoder, &decoder->pending)) { CODER_KILL(decoder, "read error on BSON_BINDATA size"); + } - if (read_int8(decoder, &tbyte)) + if (read_int8(decoder, &tbyte)) { CODER_KILL(decoder, "read error on BSON_BINDATA subtype"); + } decoder->node.subtype = tbyte; break; - /* XXX currently not supporting other types */ + /* XXX currently not supporting other types */ default: CODER_KILL(decoder, "unsupported node type"); } @@ -293,8 +324,9 @@ bson_decoder_copy_data(bson_decoder_t decoder, void *buf) /* copy data */ result = read_x(decoder, buf, decoder->pending); - if (result != 0) + if (result != 0) { CODER_KILL(decoder, "read error on copy_data"); + } /* pending count is discharged */ decoder->pending = 0; @@ -312,17 +344,21 @@ write_x(bson_encoder_t encoder, const void *p, size_t s) { CODER_CHECK(encoder); - if (encoder->fd > -1) + if (encoder->fd > -1) { return (BSON_WRITE(encoder->fd, p, s) == (int)s) ? 0 : -1; + } /* do we need to extend the buffer? */ while ((encoder->bufpos + s) > encoder->bufsize) { - if (!encoder->realloc_ok) + if (!encoder->realloc_ok) { CODER_KILL(encoder, "fixed-size buffer overflow"); + } uint8_t *newbuf = realloc(encoder->buf, encoder->bufsize + BSON_BUF_INCREMENT); - if (newbuf == NULL) + + if (newbuf == NULL) { CODER_KILL(encoder, "could not grow buffer"); + } encoder->buf = newbuf; encoder->bufsize += BSON_BUF_INCREMENT; @@ -365,8 +401,9 @@ write_name(bson_encoder_t encoder, const char *name) { size_t len = strlen(name); - if (len > BSON_MAXNAME) + if (len > BSON_MAXNAME) { CODER_KILL(encoder, "node name too long"); + } return write_x(encoder, name, len + 1); } @@ -378,8 +415,9 @@ bson_encoder_init_file(bson_encoder_t encoder, int fd) encoder->buf = NULL; encoder->dead = false; - if (write_int32(encoder, 0)) + if (write_int32(encoder, 0)) { CODER_KILL(encoder, "write error on document length"); + } return 0; } @@ -391,18 +429,21 @@ bson_encoder_init_buf(bson_encoder_t encoder, void *buf, unsigned bufsize) encoder->buf = (uint8_t *)buf; encoder->bufpos = 0; encoder->dead = false; + if (encoder->buf == NULL) { encoder->bufsize = 0; encoder->realloc_ok = true; + } else { encoder->bufsize = bufsize; encoder->realloc_ok = false; } - if (write_int32(encoder, 0)) + if (write_int32(encoder, 0)) { CODER_KILL(encoder, "write error on document length"); + } - return 0; + return 0; } int @@ -410,8 +451,9 @@ bson_encoder_fini(bson_encoder_t encoder) { CODER_CHECK(encoder); - if (write_int8(encoder, BSON_EOO)) + if (write_int8(encoder, BSON_EOO)) { CODER_KILL(encoder, "write error on document terminator"); + } /* hack to fix up length for in-buffer documents */ if (encoder->buf != NULL) { @@ -430,8 +472,9 @@ bson_encoder_buf_size(bson_encoder_t encoder) { CODER_CHECK(encoder); - if (encoder->fd > -1) + if (encoder->fd > -1) { return -1; + } return encoder->bufpos; } @@ -441,8 +484,9 @@ bson_encoder_buf_data(bson_encoder_t encoder) { /* note, no CODER_CHECK here as the caller has to clean up dead buffers */ - if (encoder->fd > -1) + if (encoder->fd > -1) { return NULL; + } return encoder->buf; } @@ -453,8 +497,9 @@ int bson_encoder_append_bool(bson_encoder_t encoder, const char *name, bool valu if (write_int8(encoder, BSON_BOOL) || write_name(encoder, name) || - write_int8(encoder, value ? 1 : 0)) + write_int8(encoder, value ? 1 : 0)) { CODER_KILL(encoder, "write error on BSON_BOOL"); + } return 0; } @@ -472,14 +517,17 @@ bson_encoder_append_int(bson_encoder_t encoder, const char *name, int64_t value) result = write_int8(encoder, BSON_INT32) || write_name(encoder, name) || write_int32(encoder, value); + } else { debug("encoding %lld as int64", value); result = write_int8(encoder, BSON_INT64) || write_name(encoder, name) || write_int64(encoder, value); } - if (result) + + if (result) { CODER_KILL(encoder, "write error on BSON_INT"); + } return 0; } @@ -491,8 +539,9 @@ bson_encoder_append_double(bson_encoder_t encoder, const char *name, double valu if (write_int8(encoder, BSON_DOUBLE) || write_name(encoder, name) || - write_double(encoder, value)) + write_double(encoder, value)) { CODER_KILL(encoder, "write error on BSON_DOUBLE"); + } return 0; @@ -510,14 +559,16 @@ bson_encoder_append_string(bson_encoder_t encoder, const char *name, const char if (write_int8(encoder, BSON_STRING) || write_name(encoder, name) || write_int32(encoder, len) || - write_x(encoder, string, len)) + write_x(encoder, string, len)) { CODER_KILL(encoder, "write error on BSON_STRING"); + } return 0; } int -bson_encoder_append_binary(bson_encoder_t encoder, const char *name, bson_binary_subtype_t subtype, size_t size, const void *data) +bson_encoder_append_binary(bson_encoder_t encoder, const char *name, bson_binary_subtype_t subtype, size_t size, + const void *data) { CODER_CHECK(encoder); @@ -525,8 +576,9 @@ bson_encoder_append_binary(bson_encoder_t encoder, const char *name, bson_binary write_name(encoder, name) || write_int32(encoder, size) || write_int8(encoder, subtype) || - write_x(encoder, data, size)) + write_x(encoder, data, size)) { CODER_KILL(encoder, "write error on BSON_BINDATA"); + } return 0; } diff --git a/src/modules/systemlib/bson/tinybson.h b/src/modules/systemlib/bson/tinybson.h index 666f8191aa..25bd90a9eb 100644 --- a/src/modules/systemlib/bson/tinybson.h +++ b/src/modules/systemlib/bson/tinybson.h @@ -141,7 +141,8 @@ __EXPORT int bson_decoder_init_file(bson_decoder_t decoder, int fd, bson_decoder * @param private Callback private data, stored in node. * @return Zero on success. */ -__EXPORT int bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_decoder_callback callback, void *private); +__EXPORT int bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_decoder_callback callback, + void *private); /** * Process the next node from the stream and invoke the callback. @@ -198,7 +199,7 @@ __EXPORT int bson_encoder_init_file(bson_encoder_t encoder, int fd); * @param encoder Encoder state structure to be initialised. * @param buf Buffer pointer to use, or NULL if the buffer * should be allocated by the encoder. - * @param bufsize Maximum buffer size, or zero for no limit. If + * @param bufsize Maximum buffer size, or zero for no limit. If * the buffer is supplied, the size of the supplied buffer. * @return Zero on success. */ @@ -238,7 +239,7 @@ __EXPORT int bson_encoder_append_bool(bson_encoder_t encoder, const char *name, * * @param encoder Encoder state. * @param name Node name. - * @param value Value to be encoded. + * @param value Value to be encoded. */ __EXPORT int bson_encoder_append_int(bson_encoder_t encoder, const char *name, int64_t value); @@ -269,7 +270,8 @@ __EXPORT int bson_encoder_append_string(bson_encoder_t encoder, const char *name * @param size Data size. * @param data Buffer containing data to be encoded. */ -__EXPORT int bson_encoder_append_binary(bson_encoder_t encoder, const char *name, bson_binary_subtype_t subtype, size_t size, const void *data); +__EXPORT int bson_encoder_append_binary(bson_encoder_t encoder, const char *name, bson_binary_subtype_t subtype, + size_t size, const void *data); #endif diff --git a/src/modules/systemlib/circuit_breaker_params.c b/src/modules/systemlib/circuit_breaker_params.c index 054a1c3501..20b9638d6a 100644 --- a/src/modules/systemlib/circuit_breaker_params.c +++ b/src/modules/systemlib/circuit_breaker_params.c @@ -128,7 +128,7 @@ PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953); * If this check is enabled, then the sensor check will fail if the GPS module * is missing. It will also check for excessive signal noise on the GPS receiver * and warn the user if detected. - * + * * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK * * @min 0 diff --git a/src/modules/systemlib/err.c b/src/modules/systemlib/err.c index 3e1bdd0b19..0011bca206 100644 --- a/src/modules/systemlib/err.c +++ b/src/modules/systemlib/err.c @@ -77,11 +77,13 @@ warnerr_core(int errcode, const char *fmt, va_list args) vfprintf(stderr, fmt, args); /* convenience as many parts of NuttX use negative errno */ - if (errcode < 0) + if (errcode < 0) { errcode = -errcode; + } - if (errcode < NOCODE) + if (errcode < NOCODE) { fprintf(stderr, ": %s", strerror(errcode)); + } fprintf(stderr, "\n"); #elif CONFIG_ARCH_LOWPUTC @@ -89,11 +91,13 @@ warnerr_core(int errcode, const char *fmt, va_list args) lowvsyslog(fmt, args); /* convenience as many parts of NuttX use negative errno */ - if (errcode < 0) + if (errcode < 0) { errcode = -errcode; + } - if (errcode < NOCODE) + if (errcode < NOCODE) { lowsyslog(": %s", strerror(errcode)); + } lowsyslog("\n"); #endif diff --git a/src/modules/systemlib/getopt_long.c b/src/modules/systemlib/getopt_long.c index 27c38635f7..c715221a00 100644 --- a/src/modules/systemlib/getopt_long.c +++ b/src/modules/systemlib/getopt_long.c @@ -75,7 +75,7 @@ COPYRIGHT NOTICE AND DISCLAIMER: Copyright (C) 1997 Gregory Pietsch -This file and the accompanying getopt.h header file are hereby placed in the +This file and the accompanying getopt.h header file are hereby placed in the public domain without restrictions. Just give the author credit, don't claim you wrote it or prevent anyone else from using it. @@ -92,11 +92,10 @@ gpietsch@comcast.net /* macros */ /* types */ -typedef enum GETOPT_ORDERING_T -{ - PERMUTE, - RETURN_IN_ORDER, - REQUIRE_ORDER +typedef enum GETOPT_ORDERING_T { + PERMUTE, + RETURN_IN_ORDER, + REQUIRE_ORDER } GETOPT_ORDERING_T; /* globally-defined variables */ @@ -109,296 +108,320 @@ int optopt = '?'; /* reverse_argv_elements: reverses num elements starting at argv */ static void -reverse_argv_elements (char **argv, int num) +reverse_argv_elements(char **argv, int num) { - int i; - char *tmp; + int i; + char *tmp; - for (i = 0; i < (num >> 1); i++) - { - tmp = argv[i]; - argv[i] = argv[num - i - 1]; - argv[num - i - 1] = tmp; - } + for (i = 0; i < (num >> 1); i++) { + tmp = argv[i]; + argv[i] = argv[num - i - 1]; + argv[num - i - 1] = tmp; + } } /* permute: swap two blocks of argv-elements given their lengths */ static void -permute (char **argv, int len1, int len2) +permute(char **argv, int len1, int len2) { - reverse_argv_elements (argv, len1); - reverse_argv_elements (argv, len1 + len2); - reverse_argv_elements (argv, len2); + reverse_argv_elements(argv, len1); + reverse_argv_elements(argv, len1 + len2); + reverse_argv_elements(argv, len2); } /* is_option: is this argv-element an option or the end of the option list? */ static int -is_option (char *argv_element, int only) +is_option(char *argv_element, int only) { - return ((argv_element == NULL) - || (argv_element[0] == '-') || (only && argv_element[0] == '+')); + return ((argv_element == NULL) + || (argv_element[0] == '-') || (only && argv_element[0] == '+')); } /* getopt_internal: the function that does all the dirty work */ static int -getopt_internal (int argc, char **argv, const char *shortopts, - const GETOPT_LONG_OPTION_T * longopts, int *longind, int only) +getopt_internal(int argc, char **argv, const char *shortopts, + const GETOPT_LONG_OPTION_T *longopts, int *longind, int only) { - GETOPT_ORDERING_T ordering = PERMUTE; - static size_t optwhere = 0; - size_t permute_from = 0; - int num_nonopts = 0; - int optindex = 0; - size_t match_chars = 0; - char *possible_arg = NULL; - int longopt_match = -1; - int has_arg = -1; - char *cp = NULL; - int arg_next = 0; + GETOPT_ORDERING_T ordering = PERMUTE; + static size_t optwhere = 0; + size_t permute_from = 0; + int num_nonopts = 0; + int optindex = 0; + size_t match_chars = 0; + char *possible_arg = NULL; + int longopt_match = -1; + int has_arg = -1; + char *cp = NULL; + int arg_next = 0; - /* first, deal with silly parameters and easy stuff */ - if (argc == 0 || argv == NULL || (shortopts == NULL && longopts == NULL) - || optind >= argc || argv[optind] == NULL) - return EOF; - if (strcmp (argv[optind], "--") == 0) - { - optind++; - return EOF; - } - /* if this is our first time through */ - if (optind == 0) - optind = optwhere = 1; + /* first, deal with silly parameters and easy stuff */ + if (argc == 0 || argv == NULL || (shortopts == NULL && longopts == NULL) + || optind >= argc || argv[optind] == NULL) { + return EOF; + } - /* define ordering */ - if (shortopts != NULL && (*shortopts == '-' || *shortopts == '+')) - { - ordering = (*shortopts == '-') ? RETURN_IN_ORDER : REQUIRE_ORDER; - shortopts++; - } - else - ordering = /*(getenv ("POSIXLY_CORRECT") != NULL) ? REQUIRE_ORDER :*/ PERMUTE; + if (strcmp(argv[optind], "--") == 0) { + optind++; + return EOF; + } - /* - * based on ordering, find our next option, if we're at the beginning of - * one - */ - if (optwhere == 1) - { - switch (ordering) - { - default: /* shouldn't happen */ - case PERMUTE: - permute_from = optind; - num_nonopts = 0; - while (!is_option (argv[optind], only)) - { - optind++; - num_nonopts++; - } - if (argv[optind] == NULL) - { - /* no more options */ - optind = permute_from; - return EOF; - } - else if (strcmp (argv[optind], "--") == 0) - { - /* no more options, but have to get `--' out of the way */ - permute (argv + permute_from, num_nonopts, 1); - optind = permute_from + 1; - return EOF; - } - break; - case RETURN_IN_ORDER: - if (!is_option (argv[optind], only)) - { - optarg = argv[optind++]; - return (optopt = 1); - } - break; - case REQUIRE_ORDER: - if (!is_option (argv[optind], only)) - return EOF; - break; - } - } - /* we've got an option, so parse it */ + /* if this is our first time through */ + if (optind == 0) { + optind = optwhere = 1; + } - /* first, is it a long option? */ - if (longopts != NULL - && (memcmp (argv[optind], "--", 2) == 0 - || (only && argv[optind][0] == '+')) && optwhere == 1) - { - /* handle long options */ - if (memcmp (argv[optind], "--", 2) == 0) - optwhere = 2; - longopt_match = -1; - possible_arg = strchr (argv[optind] + optwhere, '='); - if (possible_arg == NULL) - { - /* no =, so next argv might be arg */ - match_chars = strlen (argv[optind]); - possible_arg = argv[optind] + match_chars; - match_chars = match_chars - optwhere; - } - else - match_chars = (possible_arg - argv[optind]) - optwhere; - for (optindex = 0; longopts[optindex].name != NULL; optindex++) - { - if (memcmp (argv[optind] + optwhere, - longopts[optindex].name, match_chars) == 0) - { - /* do we have an exact match? */ - if (match_chars == (unsigned) (strlen (longopts[optindex].name))) - { - longopt_match = optindex; - break; - } - /* do any characters match? */ - else - { - if (longopt_match < 0) - longopt_match = optindex; - else - { - /* we have ambiguous options */ - if (opterr) - fprintf (stderr, "%s: option `%s' is ambiguous " - "(could be `--%s' or `--%s')\n", - argv[0], - argv[optind], - longopts[longopt_match].name, - longopts[optindex].name); - return (optopt = '?'); - } - } - } - } - if (longopt_match >= 0) - has_arg = longopts[longopt_match].has_arg; - } - /* if we didn't find a long option, is it a short option? */ - if (longopt_match < 0 && shortopts != NULL) - { - cp = strchr (shortopts, argv[optind][optwhere]); - if (cp == NULL) - { - /* couldn't find option in shortopts */ - if (opterr) - fprintf (stderr, - "%s: invalid option -- `-%c'\n", - argv[0], argv[optind][optwhere]); - optwhere++; - if (argv[optind][optwhere] == '\0') - { - optind++; - optwhere = 1; - } - return (optopt = '?'); - } - has_arg = ((cp[1] == ':') - ? ((cp[2] == ':') ? OPTIONAL_ARG : REQUIRED_ARG) : NO_ARG); - possible_arg = argv[optind] + optwhere + 1; - optopt = *cp; - } - /* get argument and reset optwhere */ - arg_next = 0; - switch (has_arg) - { - case OPTIONAL_ARG: - if (*possible_arg == '=') - possible_arg++; - optarg = (*possible_arg != '\0') ? possible_arg : 0; - optwhere = 1; - break; - case REQUIRED_ARG: - if (*possible_arg == '=') - possible_arg++; - if (*possible_arg != '\0') - { - optarg = possible_arg; - optwhere = 1; - } - else if (optind + 1 >= argc) - { - if (opterr) - { - fprintf (stderr, "%s: argument required for option `", argv[0]); - if (longopt_match >= 0) - fprintf (stderr, "--%s'\n", longopts[longopt_match].name); - else - fprintf (stderr, "-%c'\n", *cp); - } - optind++; - return (optopt = ':'); - } - else - { - optarg = argv[optind + 1]; - arg_next = 1; - optwhere = 1; - } - break; - default: /* shouldn't happen */ - case NO_ARG: - if (longopt_match < 0) - { - optwhere++; - if (argv[optind][optwhere] == '\0') - optwhere = 1; - } - else - optwhere = 1; - optarg = NULL; - break; - } + /* define ordering */ + if (shortopts != NULL && (*shortopts == '-' || *shortopts == '+')) { + ordering = (*shortopts == '-') ? RETURN_IN_ORDER : REQUIRE_ORDER; + shortopts++; - /* do we have to permute or otherwise modify optind? */ - if (ordering == PERMUTE && optwhere == 1 && num_nonopts != 0) - { - permute (argv + permute_from, num_nonopts, 1 + arg_next); - optind = permute_from + 1 + arg_next; - } - else if (optwhere == 1) - optind = optind + 1 + arg_next; + } else { + ordering = /*(getenv ("POSIXLY_CORRECT") != NULL) ? REQUIRE_ORDER :*/ PERMUTE; + } - /* finally return */ - if (longopt_match >= 0) - { - if (longind != NULL) - *longind = longopt_match; - if (longopts[longopt_match].flag != NULL) - { - *(longopts[longopt_match].flag) = longopts[longopt_match].val; - return 0; - } - else - return longopts[longopt_match].val; - } - else - return optopt; + /* + * based on ordering, find our next option, if we're at the beginning of + * one + */ + if (optwhere == 1) { + switch (ordering) { + default: /* shouldn't happen */ + case PERMUTE: + permute_from = optind; + num_nonopts = 0; + + while (!is_option(argv[optind], only)) { + optind++; + num_nonopts++; + } + + if (argv[optind] == NULL) { + /* no more options */ + optind = permute_from; + return EOF; + + } else if (strcmp(argv[optind], "--") == 0) { + /* no more options, but have to get `--' out of the way */ + permute(argv + permute_from, num_nonopts, 1); + optind = permute_from + 1; + return EOF; + } + + break; + + case RETURN_IN_ORDER: + if (!is_option(argv[optind], only)) { + optarg = argv[optind++]; + return (optopt = 1); + } + + break; + + case REQUIRE_ORDER: + if (!is_option(argv[optind], only)) { + return EOF; + } + + break; + } + } + + /* we've got an option, so parse it */ + + /* first, is it a long option? */ + if (longopts != NULL + && (memcmp(argv[optind], "--", 2) == 0 + || (only && argv[optind][0] == '+')) && optwhere == 1) { + /* handle long options */ + if (memcmp(argv[optind], "--", 2) == 0) { + optwhere = 2; + } + + longopt_match = -1; + possible_arg = strchr(argv[optind] + optwhere, '='); + + if (possible_arg == NULL) { + /* no =, so next argv might be arg */ + match_chars = strlen(argv[optind]); + possible_arg = argv[optind] + match_chars; + match_chars = match_chars - optwhere; + + } else { + match_chars = (possible_arg - argv[optind]) - optwhere; + } + + for (optindex = 0; longopts[optindex].name != NULL; optindex++) { + if (memcmp(argv[optind] + optwhere, + longopts[optindex].name, match_chars) == 0) { + /* do we have an exact match? */ + if (match_chars == (unsigned)(strlen(longopts[optindex].name))) { + longopt_match = optindex; + break; + } + + /* do any characters match? */ + else { + if (longopt_match < 0) { + longopt_match = optindex; + + } else { + /* we have ambiguous options */ + if (opterr) + fprintf(stderr, "%s: option `%s' is ambiguous " + "(could be `--%s' or `--%s')\n", + argv[0], + argv[optind], + longopts[longopt_match].name, + longopts[optindex].name); + + return (optopt = '?'); + } + } + } + } + + if (longopt_match >= 0) { + has_arg = longopts[longopt_match].has_arg; + } + } + + /* if we didn't find a long option, is it a short option? */ + if (longopt_match < 0 && shortopts != NULL) { + cp = strchr(shortopts, argv[optind][optwhere]); + + if (cp == NULL) { + /* couldn't find option in shortopts */ + if (opterr) + fprintf(stderr, + "%s: invalid option -- `-%c'\n", + argv[0], argv[optind][optwhere]); + + optwhere++; + + if (argv[optind][optwhere] == '\0') { + optind++; + optwhere = 1; + } + + return (optopt = '?'); + } + + has_arg = ((cp[1] == ':') + ? ((cp[2] == ':') ? OPTIONAL_ARG : REQUIRED_ARG) : NO_ARG); + possible_arg = argv[optind] + optwhere + 1; + optopt = *cp; + } + + /* get argument and reset optwhere */ + arg_next = 0; + + switch (has_arg) { + case OPTIONAL_ARG: + if (*possible_arg == '=') { + possible_arg++; + } + + optarg = (*possible_arg != '\0') ? possible_arg : 0; + optwhere = 1; + break; + + case REQUIRED_ARG: + if (*possible_arg == '=') { + possible_arg++; + } + + if (*possible_arg != '\0') { + optarg = possible_arg; + optwhere = 1; + + } else if (optind + 1 >= argc) { + if (opterr) { + fprintf(stderr, "%s: argument required for option `", argv[0]); + + if (longopt_match >= 0) { + fprintf(stderr, "--%s'\n", longopts[longopt_match].name); + + } else { + fprintf(stderr, "-%c'\n", *cp); + } + } + + optind++; + return (optopt = ':'); + + } else { + optarg = argv[optind + 1]; + arg_next = 1; + optwhere = 1; + } + + break; + + default: /* shouldn't happen */ + case NO_ARG: + if (longopt_match < 0) { + optwhere++; + + if (argv[optind][optwhere] == '\0') { + optwhere = 1; + } + + } else { + optwhere = 1; + } + + optarg = NULL; + break; + } + + /* do we have to permute or otherwise modify optind? */ + if (ordering == PERMUTE && optwhere == 1 && num_nonopts != 0) { + permute(argv + permute_from, num_nonopts, 1 + arg_next); + optind = permute_from + 1 + arg_next; + + } else if (optwhere == 1) { + optind = optind + 1 + arg_next; + } + + /* finally return */ + if (longopt_match >= 0) { + if (longind != NULL) { + *longind = longopt_match; + } + + if (longopts[longopt_match].flag != NULL) { + *(longopts[longopt_match].flag) = longopts[longopt_match].val; + return 0; + + } else { + return longopts[longopt_match].val; + } + + } else { + return optopt; + } } #if 0 int -getopt (int argc, char **argv, char *optstring) +getopt(int argc, char **argv, char *optstring) { - return getopt_internal (argc, argv, optstring, NULL, NULL, 0); + return getopt_internal(argc, argv, optstring, NULL, NULL, 0); } #endif int -getopt_long (int argc, char **argv, const char *shortopts, - const GETOPT_LONG_OPTION_T * longopts, int *longind) +getopt_long(int argc, char **argv, const char *shortopts, + const GETOPT_LONG_OPTION_T *longopts, int *longind) { - return getopt_internal (argc, argv, shortopts, longopts, longind, 0); + return getopt_internal(argc, argv, shortopts, longopts, longind, 0); } int -getopt_long_only (int argc, char **argv, const char *shortopts, - const GETOPT_LONG_OPTION_T * longopts, int *longind) +getopt_long_only(int argc, char **argv, const char *shortopts, + const GETOPT_LONG_OPTION_T *longopts, int *longind) { - return getopt_internal (argc, argv, shortopts, longopts, longind, 1); + return getopt_internal(argc, argv, shortopts, longopts, longind, 1); } /* end of file GETOPT.C */ diff --git a/src/modules/systemlib/getopt_long.h b/src/modules/systemlib/getopt_long.h index 61e8e76f31..ead667c73a 100644 --- a/src/modules/systemlib/getopt_long.h +++ b/src/modules/systemlib/getopt_long.h @@ -74,8 +74,8 @@ COPYRIGHT NOTICE AND DISCLAIMER: Copyright (C) 1997 Gregory Pietsch -This file and the accompanying getopt.c implementation file are hereby -placed in the public domain without restrictions. Just give the author +This file and the accompanying getopt.c implementation file are hereby +placed in the public domain without restrictions. Just give the author credit, don't claim you wrote it or prevent anyone else from using it. Gregory Pietsch's current e-mail address: @@ -95,17 +95,16 @@ gpietsch@comcast.net /* types defined by this include file */ /* GETOPT_LONG_OPTION_T: The type of long option */ -typedef struct GETOPT_LONG_OPTION_T -{ - char *name; /* the name of the long option */ - int has_arg; /* one of the above macros */ - int *flag; /* determines if getopt_long() returns a +typedef struct GETOPT_LONG_OPTION_T { + char *name; /* the name of the long option */ + int has_arg; /* one of the above macros */ + int *flag; /* determines if getopt_long() returns a * value for a long option; if it is * non-NULL, 0 is returned as a function * value and the value of val is stored in * the area pointed to by flag. Otherwise, * val is returned. */ - int val; /* determines the value to return if flag is + int val; /* determines the value to return if flag is * NULL. */ } GETOPT_LONG_OPTION_T; @@ -114,20 +113,20 @@ extern "C" { #endif - /* externally-defined variables */ - extern char *optarg; - extern int optind; - extern int opterr; - extern int optopt; +/* externally-defined variables */ +extern char *optarg; +extern int optind; +extern int opterr; +extern int optopt; - /* function prototypes */ +/* function prototypes */ #if 0 - int getopt (int argc, char **argv, char *optstring); +int getopt(int argc, char **argv, char *optstring); #endif - __EXPORT int getopt_long (int argc, char **argv, const char *shortopts, - const GETOPT_LONG_OPTION_T * longopts, int *longind); - __EXPORT int getopt_long_only (int argc, char **argv, const char *shortopts, - const GETOPT_LONG_OPTION_T * longopts, int *longind); +__EXPORT int getopt_long(int argc, char **argv, const char *shortopts, + const GETOPT_LONG_OPTION_T *longopts, int *longind); +__EXPORT int getopt_long_only(int argc, char **argv, const char *shortopts, + const GETOPT_LONG_OPTION_T *longopts, int *longind); #ifdef __cplusplus }; diff --git a/src/modules/systemlib/git_version.h b/src/modules/systemlib/git_version.h index 5f8d411088..f9bc3cbedd 100644 --- a/src/modules/systemlib/git_version.h +++ b/src/modules/systemlib/git_version.h @@ -45,7 +45,7 @@ __BEGIN_DECLS -__EXPORT extern const char* px4_git_version; +__EXPORT extern const char *px4_git_version; __EXPORT extern const uint64_t px4_git_version_binary; __END_DECLS diff --git a/src/modules/systemlib/hx_stream.c b/src/modules/systemlib/hx_stream.c index 52ae77de55..8f388e4078 100644 --- a/src/modules/systemlib/hx_stream.c +++ b/src/modules/systemlib/hx_stream.c @@ -92,8 +92,9 @@ static int hx_rx_frame(hx_stream_t stream); static void hx_tx_raw(hx_stream_t stream, uint8_t c) { - if (write(stream->fd, &c, 1) != 1) + if (write(stream->fd, &c, 1) != 1) { stream->tx_error = true; + } } static int @@ -111,8 +112,9 @@ hx_rx_frame(hx_stream_t stream) /* not a real frame - too short */ if (length < 4) { - if (length > 1) + if (length > 1) { perf_count(stream->pc_rx_errors); + } return 0; } @@ -190,11 +192,12 @@ hx_stream_reset(hx_stream_t stream) int hx_stream_start(hx_stream_t stream, - const void *data, - size_t count) + const void *data, + size_t count) { - if (count > HX_STREAM_MAX_FRAME) + if (count > HX_STREAM_MAX_FRAME) { return -EINVAL; + } stream->tx_buf = data; stream->tx_resid = count; @@ -224,6 +227,7 @@ hx_stream_send_next(hx_stream_t stream) stream->tx_state = TX_SENT_ESCAPE; return CEO; } + break; case TX_SENT_ESCAPE: @@ -251,12 +255,14 @@ hx_stream_send_next(hx_stream_t stream) /* was the buffer the frame CRC? */ if (stream->tx_buf == (pcrc + sizeof(stream->tx_crc))) { stream->tx_state = TX_SEND_END; + } else { /* no, it was the payload - switch to sending the CRC */ stream->tx_buf = pcrc; stream->tx_resid = sizeof(stream->tx_crc); } } + return c; } @@ -268,12 +274,16 @@ hx_stream_send(hx_stream_t stream, int result; result = hx_stream_start(stream, data, count); - if (result != OK) + + if (result != OK) { return result; + } int c; - while ((c = hx_stream_send_next(stream)) >= 0) + + while ((c = hx_stream_send_next(stream)) >= 0) { hx_tx_raw(stream, c); + } /* check for transmit error */ if (stream->tx_error) { @@ -306,6 +316,7 @@ hx_stream_rx(hx_stream_t stream, uint8_t c) } /* save for later */ - if (stream->rx_frame_bytes < sizeof(stream->rx_buf)) + if (stream->rx_frame_bytes < sizeof(stream->rx_buf)) { stream->rx_buf[stream->rx_frame_bytes++] = c; + } } diff --git a/src/modules/systemlib/mcu_version.c b/src/modules/systemlib/mcu_version.c index 9b979b112c..e4623c2ce0 100644 --- a/src/modules/systemlib/mcu_version.c +++ b/src/modules/systemlib/mcu_version.c @@ -33,7 +33,7 @@ /** * @file mcu_version.c - * + * * Read out the microcontroller version from the board * * @author Lorenz Meier @@ -64,8 +64,8 @@ void mcu_unique_id(uint32_t *uid_96_bit) { #ifdef __PX4_NUTTX uid_96_bit[0] = getreg32(UNIQUE_ID); - uid_96_bit[1] = getreg32(UNIQUE_ID+4); - uid_96_bit[2] = getreg32(UNIQUE_ID+8); + uid_96_bit[1] = getreg32(UNIQUE_ID + 4); + uid_96_bit[2] = getreg32(UNIQUE_ID + 8); #else uid_96_bit[0] = 0; uid_96_bit[1] = 1; @@ -73,7 +73,7 @@ void mcu_unique_id(uint32_t *uid_96_bit) #endif } -int mcu_version(char* rev, char** revstr) +int mcu_version(char *rev, char **revstr) { #ifdef CONFIG_ARCH_CHIP_STM32 uint32_t abc = getreg32(DBGMCU_IDCODE); @@ -85,9 +85,11 @@ int mcu_version(char* rev, char** revstr) case STM32F40x_41x: *revstr = "STM32F40x"; break; + case STM32F42x_43x: *revstr = "STM32F42x"; break; + default: *revstr = "STM32F???"; break; @@ -95,25 +97,30 @@ int mcu_version(char* rev, char** revstr) switch (revid) { - case MCU_REV_STM32F4_REV_A: - *rev = 'A'; - break; - case MCU_REV_STM32F4_REV_Z: - *rev = 'Z'; - break; - case MCU_REV_STM32F4_REV_Y: - *rev = 'Y'; - break; - case MCU_REV_STM32F4_REV_1: - *rev = '1'; - break; - case MCU_REV_STM32F4_REV_3: - *rev = '3'; - break; - default: - *rev = '?'; - revid = -1; - break; + case MCU_REV_STM32F4_REV_A: + *rev = 'A'; + break; + + case MCU_REV_STM32F4_REV_Z: + *rev = 'Z'; + break; + + case MCU_REV_STM32F4_REV_Y: + *rev = 'Y'; + break; + + case MCU_REV_STM32F4_REV_1: + *rev = '1'; + break; + + case MCU_REV_STM32F4_REV_3: + *rev = '3'; + break; + + default: + *rev = '?'; + revid = -1; + break; } return revid; diff --git a/src/modules/systemlib/otp.c b/src/modules/systemlib/otp.c index a866c843d7..89329d0b55 100644 --- a/src/modules/systemlib/otp.c +++ b/src/modules/systemlib/otp.c @@ -76,29 +76,43 @@ int write_otp(uint8_t id_type, uint32_t vid, uint32_t pid, char *signature) int errors = 0; // descriptor - if (F_write_byte(ADDR_OTP_START, 'P')) + if (F_write_byte(ADDR_OTP_START, 'P')) { errors++; - // write the 'P' from PX4. to first byte in OTP - if (F_write_byte(ADDR_OTP_START + 1, 'X')) - errors++; // write the 'P' from PX4. to first byte in OTP - if (F_write_byte(ADDR_OTP_START + 2, '4')) + } + + // write the 'P' from PX4. to first byte in OTP + if (F_write_byte(ADDR_OTP_START + 1, 'X')) { + errors++; // write the 'P' from PX4. to first byte in OTP + } + + if (F_write_byte(ADDR_OTP_START + 2, '4')) { errors++; - if (F_write_byte(ADDR_OTP_START + 3, '\0')) + } + + if (F_write_byte(ADDR_OTP_START + 3, '\0')) { errors++; + } + //id_type - if (F_write_byte(ADDR_OTP_START + 4, id_type)) + if (F_write_byte(ADDR_OTP_START + 4, id_type)) { errors++; + } + // vid and pid are 4 bytes each - if (F_write_word(ADDR_OTP_START + 5, vid)) + if (F_write_word(ADDR_OTP_START + 5, vid)) { errors++; - if (F_write_word(ADDR_OTP_START + 9, pid)) + } + + if (F_write_word(ADDR_OTP_START + 9, pid)) { errors++; + } // leave some 19 bytes of space, and go to the next block... // then the auth sig starts for (int i = 0 ; i < 128 ; i++) { - if (F_write_byte(ADDR_OTP_START + 32 + i, signature[i])) + if (F_write_byte(ADDR_OTP_START + 32 + i, signature[i])) { errors++; + } } return errors; @@ -123,8 +137,9 @@ int lock_otp(void) // or just realise it's exctly 5x 32byte blocks we need to lock. 1 block for ID,type,vid,pid, and 4 blocks for certificate, which is 128 bytes. for (int i = 0 ; i < locksize ; i++) { - if (F_write_byte(ADDR_OTP_LOCK_START + i, OTP_LOCK_LOCKED)) + if (F_write_byte(ADDR_OTP_LOCK_START + i, OTP_LOCK_LOCKED)) { errors++; + } } return errors; diff --git a/src/modules/systemlib/param/param.h b/src/modules/systemlib/param/param.h index af9585eb42..caa0635dce 100644 --- a/src/modules/systemlib/param/param.h +++ b/src/modules/systemlib/param/param.h @@ -393,7 +393,7 @@ union param_value_u { struct param_info_s { const char *name -// GCC 4.8 and higher don't implement proper alignment of static data on +// GCC 4.8 and higher don't implement proper alignment of static data on // 64-bit. This means that the 24-byte param_info_s variables are // 16 byte aligned by GCC and that messes up the assumption that // sequential items in the __param segment can be addressed as an array. @@ -404,9 +404,9 @@ struct param_info_s { // The following hack is for GCC >=4.8 only. Clang works fine without // this. #ifdef __PX4_POSIX - __attribute__((aligned(16))); + __attribute__((aligned(16))); #else - ; + ; #endif param_type_t type; union param_value_u val; diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c index b0a0b01d03..8ec9b1a444 100644 --- a/src/modules/systemlib/perf_counter.c +++ b/src/modules/systemlib/perf_counter.c @@ -46,7 +46,7 @@ #include "perf_counter.h" #ifdef __PX4_QURT -#define dprintf(...) +#define dprintf(...) #endif /** @@ -144,11 +144,13 @@ perf_alloc_once(enum perf_counter_type type, const char *name) if (type == handle->type) { /* they are the same counter */ return handle; + } else { /* same name but different type, assuming this is an error and not intended */ return NULL; } } + handle = (perf_counter_t)sq_next(&handle->link); } @@ -159,8 +161,9 @@ perf_alloc_once(enum perf_counter_type type, const char *name) void perf_free(perf_counter_t handle) { - if (handle == NULL) + if (handle == NULL) { return; + } sq_rem(&handle->link, &perf_counters); free(handle); @@ -169,8 +172,9 @@ perf_free(perf_counter_t handle) void perf_count(perf_counter_t handle) { - if (handle == NULL) + if (handle == NULL) { return; + } switch (handle->type) { case PC_COUNT: @@ -178,38 +182,46 @@ perf_count(perf_counter_t handle) break; case PC_INTERVAL: { - struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; - hrt_abstime now = hrt_absolute_time(); + struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; + hrt_abstime now = hrt_absolute_time(); - switch (pci->event_count) { - case 0: - pci->time_first = now; - break; - case 1: - pci->time_least = now - pci->time_last; - pci->time_most = now - pci->time_last; - pci->mean = pci->time_least / 1e6f; - pci->M2 = 0; - break; - default: { - hrt_abstime interval = now - pci->time_last; - if (interval < pci->time_least) - pci->time_least = interval; - if (interval > pci->time_most) - pci->time_most = interval; - // maintain mean and variance of interval in seconds - // Knuth/Welford recursive mean and variance of update intervals (via Wikipedia) - float dt = interval / 1e6f; - float delta_intvl = dt - pci->mean; - pci->mean += delta_intvl / pci->event_count; - pci->M2 += delta_intvl * (dt - pci->mean); + switch (pci->event_count) { + case 0: + pci->time_first = now; break; + + case 1: + pci->time_least = now - pci->time_last; + pci->time_most = now - pci->time_last; + pci->mean = pci->time_least / 1e6f; + pci->M2 = 0; + break; + + default: { + hrt_abstime interval = now - pci->time_last; + + if (interval < pci->time_least) { + pci->time_least = interval; + } + + if (interval > pci->time_most) { + pci->time_most = interval; + } + + // maintain mean and variance of interval in seconds + // Knuth/Welford recursive mean and variance of update intervals (via Wikipedia) + float dt = interval / 1e6f; + float delta_intvl = dt - pci->mean; + pci->mean += delta_intvl / pci->event_count; + pci->M2 += delta_intvl * (dt - pci->mean); + break; + } } + + pci->time_last = now; + pci->event_count++; + break; } - pci->time_last = now; - pci->event_count++; - break; - } default: break; @@ -219,8 +231,9 @@ perf_count(perf_counter_t handle) void perf_begin(perf_counter_t handle) { - if (handle == NULL) + if (handle == NULL) { return; + } switch (handle->type) { case PC_ELAPSED: @@ -235,8 +248,9 @@ perf_begin(perf_counter_t handle) void perf_end(perf_counter_t handle) { - if (handle == NULL) + if (handle == NULL) { return; + } switch (handle->type) { case PC_ELAPSED: { @@ -247,16 +261,19 @@ perf_end(perf_counter_t handle) if (elapsed < 0) { pce->event_overruns++; + } else { pce->event_count++; pce->time_total += elapsed; - if ((pce->time_least > (uint64_t)elapsed) || (pce->time_least == 0)) + if ((pce->time_least > (uint64_t)elapsed) || (pce->time_least == 0)) { pce->time_least = elapsed; + } - if (pce->time_most < (uint64_t)elapsed) + if (pce->time_most < (uint64_t)elapsed) { pce->time_most = elapsed; + } // maintain mean and variance of the elapsed time in seconds // Knuth/Welford recursive mean and variance of update intervals (via Wikipedia) @@ -291,16 +308,19 @@ perf_set(perf_counter_t handle, int64_t elapsed) if (elapsed < 0) { pce->event_overruns++; + } else { pce->event_count++; pce->time_total += elapsed; - if ((pce->time_least > (uint64_t)elapsed) || (pce->time_least == 0)) + if ((pce->time_least > (uint64_t)elapsed) || (pce->time_least == 0)) { pce->time_least = elapsed; + } - if (pce->time_most < (uint64_t)elapsed) + if (pce->time_most < (uint64_t)elapsed) { pce->time_most = elapsed; + } // maintain mean and variance of the elapsed time in seconds // Knuth/Welford recursive mean and variance of update intervals (via Wikipedia) @@ -322,8 +342,9 @@ perf_set(perf_counter_t handle, int64_t elapsed) void perf_cancel(perf_counter_t handle) { - if (handle == NULL) + if (handle == NULL) { return; + } switch (handle->type) { case PC_ELAPSED: { @@ -343,8 +364,9 @@ perf_cancel(perf_counter_t handle) void perf_reset(perf_counter_t handle) { - if (handle == NULL) + if (handle == NULL) { return; + } switch (handle->type) { case PC_COUNT: @@ -352,25 +374,25 @@ perf_reset(perf_counter_t handle) break; case PC_ELAPSED: { - struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; - pce->event_count = 0; - pce->time_start = 0; - pce->time_total = 0; - pce->time_least = 0; - pce->time_most = 0; - break; - } + struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; + pce->event_count = 0; + pce->time_start = 0; + pce->time_total = 0; + pce->time_least = 0; + pce->time_most = 0; + break; + } case PC_INTERVAL: { - struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; - pci->event_count = 0; - pci->time_event = 0; - pci->time_first = 0; - pci->time_last = 0; - pci->time_least = 0; - pci->time_most = 0; - break; - } + struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; + pci->event_count = 0; + pci->time_event = 0; + pci->time_first = 0; + pci->time_last = 0; + pci->time_least = 0; + pci->time_most = 0; + break; + } } } @@ -383,45 +405,45 @@ perf_print_counter(perf_counter_t handle) void perf_print_counter_fd(int fd, perf_counter_t handle) { - if (handle == NULL) + if (handle == NULL) { return; + } switch (handle->type) { case PC_COUNT: dprintf(fd, "%s: %llu events\n", - handle->name, - (unsigned long long)((struct perf_ctr_count *)handle)->event_count); + handle->name, + (unsigned long long)((struct perf_ctr_count *)handle)->event_count); break; case PC_ELAPSED: { - struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; - float rms = sqrtf(pce->M2 / (pce->event_count-1)); - - dprintf(fd, "%s: %llu events, %llu overruns, %lluus elapsed, %lluus avg, min %lluus max %lluus %5.3fus rms\n", - handle->name, - (unsigned long long)pce->event_count, - (unsigned long long)pce->event_overruns, - (unsigned long long)pce->time_total, - (unsigned long long)pce->time_total / pce->event_count, - (unsigned long long)pce->time_least, - (unsigned long long)pce->time_most, - (double)(1e6f * rms)); - break; - } + struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; + float rms = sqrtf(pce->M2 / (pce->event_count - 1)); + dprintf(fd, "%s: %llu events, %llu overruns, %lluus elapsed, %lluus avg, min %lluus max %lluus %5.3fus rms\n", + handle->name, + (unsigned long long)pce->event_count, + (unsigned long long)pce->event_overruns, + (unsigned long long)pce->time_total, + pce->event_count == 0 ? 0 : (unsigned long long)pce->time_total / pce->event_count, + (unsigned long long)pce->time_least, + (unsigned long long)pce->time_most, + (double)(1e6f * rms)); + break; + } case PC_INTERVAL: { - struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; - float rms = sqrtf(pci->M2 / (pci->event_count-1)); + struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; + float rms = sqrtf(pci->M2 / (pci->event_count - 1)); - dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus %5.3fus rms\n", - handle->name, - (unsigned long long)pci->event_count, - (unsigned long long)(pci->time_last - pci->time_first) / pci->event_count, - (unsigned long long)pci->time_least, - (unsigned long long)pci->time_most, - (double)(1e6f * rms)); - break; - } + dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus %5.3fus rms\n", + handle->name, + (unsigned long long)pci->event_count, + (unsigned long long)(pci->time_last - pci->time_first) / pci->event_count, + (unsigned long long)pci->time_least, + (unsigned long long)pci->time_most, + (double)(1e6f * rms)); + break; + } default: break; @@ -431,26 +453,28 @@ perf_print_counter_fd(int fd, perf_counter_t handle) uint64_t perf_event_count(perf_counter_t handle) { - if (handle == NULL) + if (handle == NULL) { return 0; + } switch (handle->type) { case PC_COUNT: return ((struct perf_ctr_count *)handle)->event_count; case PC_ELAPSED: { - struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; - return pce->event_count; - } + struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; + return pce->event_count; + } case PC_INTERVAL: { - struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; - return pci->event_count; - } + struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; + return pci->event_count; + } default: break; } + return 0; } @@ -473,11 +497,13 @@ void perf_print_latency(int fd) { dprintf(fd, "bucket : events\n"); + for (int i = 0; i < latency_bucket_count; i++) { printf(" %4i : %li\n", latency_buckets[i], (long int)latency_counters[i]); } + // print the overflow bucket value - dprintf(fd, " >%4i : %i\n", latency_buckets[latency_bucket_count-1], latency_counters[latency_bucket_count]); + dprintf(fd, " >%4i : %i\n", latency_buckets[latency_bucket_count - 1], latency_counters[latency_bucket_count]); } void @@ -489,6 +515,7 @@ perf_reset_all(void) perf_reset(handle); handle = (perf_counter_t)sq_next(&handle->link); } + for (int i = 0; i <= latency_bucket_count; i++) { latency_counters[i] = 0; } diff --git a/src/modules/systemlib/ppm_decode.c b/src/modules/systemlib/ppm_decode.c index fe4ddde190..06845a043b 100644 --- a/src/modules/systemlib/ppm_decode.c +++ b/src/modules/systemlib/ppm_decode.c @@ -127,14 +127,16 @@ ppm_input_decode(bool reset, unsigned count) unsigned i; /* if we missed an edge, we have to give up */ - if (reset) + if (reset) { goto error; + } /* how long since the last edge? */ width = count - ppm.last_edge; - if (count < ppm.last_edge) - width += ppm.count_max; /* handle wrapped count */ + if (count < ppm.last_edge) { + width += ppm.count_max; /* handle wrapped count */ + } ppm.last_edge = count; @@ -175,8 +177,9 @@ ppm_input_decode(bool reset, unsigned count) } else { /* frame channel count matches expected, let's use it */ if (ppm.next_channel > PPM_MIN_CHANNELS) { - for (i = 0; i < ppm.next_channel; i++) + for (i = 0; i < ppm.next_channel; i++) { ppm_buffer[i] = ppm_temp_buffer[i]; + } ppm_last_valid_decode = hrt_absolute_time(); } @@ -199,8 +202,9 @@ ppm_input_decode(bool reset, unsigned count) case ARM: /* we expect a pulse giving us the first mark */ - if (width > PPM_MAX_PULSE_WIDTH) - goto error; /* pulse was too long */ + if (width > PPM_MAX_PULSE_WIDTH) { + goto error; /* pulse was too long */ + } /* record the mark timing, expect an inactive edge */ ppm.last_mark = count; @@ -218,20 +222,23 @@ ppm_input_decode(bool reset, unsigned count) case ACTIVE: /* we expect a well-formed pulse */ - if (width > PPM_MAX_PULSE_WIDTH) - goto error; /* pulse was too long */ + if (width > PPM_MAX_PULSE_WIDTH) { + goto error; /* pulse was too long */ + } /* determine the interval from the last mark */ interval = count - ppm.last_mark; ppm.last_mark = count; /* if the mark-mark timing is out of bounds, abandon the frame */ - if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE)) + if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE)) { goto error; + } /* if we have room to store the value, do so */ - if (ppm.next_channel < PPM_MAX_CHANNELS) + if (ppm.next_channel < PPM_MAX_CHANNELS) { ppm_temp_buffer[ppm.next_channel++] = interval; + } ppm.phase = INACTIVE; return; diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index cf71d7e335..e64b8d635d 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -56,48 +56,57 @@ void pwm_limit_init(pwm_limit_t *limit) } void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels, const uint16_t reverse_mask, - const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, - const float *output, uint16_t *effective_pwm, pwm_limit_t *limit) + const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, + const float *output, uint16_t *effective_pwm, pwm_limit_t *limit) { /* first evaluate state changes */ switch (limit->state) { - case PWM_LIMIT_STATE_INIT: + case PWM_LIMIT_STATE_INIT: - if (armed) { + if (armed) { - /* set arming time for the first call */ - if (limit->time_armed == 0) { - limit->time_armed = hrt_absolute_time(); - } - - if (hrt_elapsed_time(&limit->time_armed) >= INIT_TIME_US) { - limit->state = PWM_LIMIT_STATE_OFF; - } - } - break; - case PWM_LIMIT_STATE_OFF: - if (armed) { - limit->state = PWM_LIMIT_STATE_RAMP; - - /* reset arming time, used for ramp timing */ + /* set arming time for the first call */ + if (limit->time_armed == 0) { limit->time_armed = hrt_absolute_time(); } - break; - case PWM_LIMIT_STATE_RAMP: - if (!armed) { - limit->state = PWM_LIMIT_STATE_OFF; - } else if (hrt_elapsed_time(&limit->time_armed) >= RAMP_TIME_US) { - limit->state = PWM_LIMIT_STATE_ON; - } - break; - case PWM_LIMIT_STATE_ON: - if (!armed) { + + if (hrt_elapsed_time(&limit->time_armed) >= INIT_TIME_US) { limit->state = PWM_LIMIT_STATE_OFF; } - break; - default: - break; + } + + break; + + case PWM_LIMIT_STATE_OFF: + if (armed) { + limit->state = PWM_LIMIT_STATE_RAMP; + + /* reset arming time, used for ramp timing */ + limit->time_armed = hrt_absolute_time(); + } + + break; + + case PWM_LIMIT_STATE_RAMP: + if (!armed) { + limit->state = PWM_LIMIT_STATE_OFF; + + } else if (hrt_elapsed_time(&limit->time_armed) >= RAMP_TIME_US) { + limit->state = PWM_LIMIT_STATE_ON; + } + + break; + + case PWM_LIMIT_STATE_ON: + if (!armed) { + limit->state = PWM_LIMIT_STATE_OFF; + } + + break; + + default: + break; } /* if the system is pre-armed, the limit state is temporarily on, @@ -107,7 +116,7 @@ void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_c * regular arming time. */ - unsigned local_limit_state = limit->state; + unsigned local_limit_state = limit->state; if (pre_armed) { local_limit_state = PWM_LIMIT_STATE_ON; @@ -117,69 +126,24 @@ void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_c /* then set effective_pwm based on state */ switch (local_limit_state) { - case PWM_LIMIT_STATE_OFF: - case PWM_LIMIT_STATE_INIT: - for (unsigned i=0; itime_armed); + + progress = diff * PROGRESS_INT_SCALING / RAMP_TIME_US; + + if (progress > PROGRESS_INT_SCALING) { + progress = PROGRESS_INT_SCALING; } - break; - case PWM_LIMIT_STATE_RAMP: - { - hrt_abstime diff = hrt_elapsed_time(&limit->time_armed); - progress = diff * PROGRESS_INT_SCALING / RAMP_TIME_US; - - if (progress > PROGRESS_INT_SCALING) { - progress = PROGRESS_INT_SCALING; - } - - for (unsigned i=0; i 0) { - - /* safeguard against overflows */ - unsigned disarmed = disarmed_pwm[i]; - if (disarmed > min_pwm[i]) { - disarmed = min_pwm[i]; - } - - unsigned disarmed_min_diff = min_pwm[i] - disarmed; - ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / PROGRESS_INT_SCALING; - - } else { - - /* no disarmed pwm value set, choose min pwm */ - ramp_min_pwm = min_pwm[i]; - } - - if (reverse_mask & (1 << i)) { - control_value = -1.0f * control_value; - } - - effective_pwm[i] = control_value * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2; - - /* last line of defense against invalid inputs */ - if (effective_pwm[i] < ramp_min_pwm) { - effective_pwm[i] = ramp_min_pwm; - } else if (effective_pwm[i] > max_pwm[i]) { - effective_pwm[i] = max_pwm[i]; - } - } - } - break; - case PWM_LIMIT_STATE_ON: - for (unsigned i=0; i 0) { + + /* safeguard against overflows */ + unsigned disarmed = disarmed_pwm[i]; + + if (disarmed > min_pwm[i]) { + disarmed = min_pwm[i]; + } + + unsigned disarmed_min_diff = min_pwm[i] - disarmed; + ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / PROGRESS_INT_SCALING; + + } else { + + /* no disarmed pwm value set, choose min pwm */ + ramp_min_pwm = min_pwm[i]; + } + if (reverse_mask & (1 << i)) { control_value = -1.0f * control_value; } - effective_pwm[i] = control_value * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2; + effective_pwm[i] = control_value * (max_pwm[i] - ramp_min_pwm) / 2 + (max_pwm[i] + ramp_min_pwm) / 2; /* last line of defense against invalid inputs */ - if (effective_pwm[i] < min_pwm[i]) { - effective_pwm[i] = min_pwm[i]; + if (effective_pwm[i] < ramp_min_pwm) { + effective_pwm[i] = ramp_min_pwm; + } else if (effective_pwm[i] > max_pwm[i]) { effective_pwm[i] = max_pwm[i]; } } - break; - default: - break; + } + break; + + case PWM_LIMIT_STATE_ON: + for (unsigned i = 0; i < num_channels; i++) { + + float control_value = output[i]; + + /* check for invalid / disabled channels */ + if (!isfinite(control_value)) { + effective_pwm[i] = disarmed_pwm[i]; + continue; + } + + if (reverse_mask & (1 << i)) { + control_value = -1.0f * control_value; + } + + effective_pwm[i] = control_value * (max_pwm[i] - min_pwm[i]) / 2 + (max_pwm[i] + min_pwm[i]) / 2; + + /* last line of defense against invalid inputs */ + if (effective_pwm[i] < min_pwm[i]) { + effective_pwm[i] = min_pwm[i]; + + } else if (effective_pwm[i] > max_pwm[i]) { + effective_pwm[i] = max_pwm[i]; + } + } + + break; + + default: + break; } + return; } diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.h b/src/modules/systemlib/pwm_limit/pwm_limit.h index 48cf686e8e..23adc9ee3a 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.h +++ b/src/modules/systemlib/pwm_limit/pwm_limit.h @@ -71,7 +71,8 @@ typedef struct { __EXPORT void pwm_limit_init(pwm_limit_t *limit); -__EXPORT void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels, const uint16_t reverse_mask, const uint16_t *disarmed_pwm, +__EXPORT void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels, + const uint16_t reverse_mask, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output, uint16_t *effective_pwm, pwm_limit_t *limit); __END_DECLS diff --git a/src/modules/systemlib/state_table.h b/src/modules/systemlib/state_table.h index 100c5ac499..f28b9180c1 100644 --- a/src/modules/systemlib/state_table.h +++ b/src/modules/systemlib/state_table.h @@ -51,16 +51,17 @@ public: }; StateTable(Tran const *table, unsigned nStates, unsigned nSignals) - : myTable(table), myNsignals(nSignals) {} + : myTable(table), myNsignals(nSignals) {} - #define NO_ACTION &StateTable::doNothing - #define ACTION(_target) StateTable::Action(_target) +#define NO_ACTION &StateTable::doNothing +#define ACTION(_target) StateTable::Action(_target) virtual ~StateTable() {} - void dispatch(unsigned const sig) { + void dispatch(unsigned const sig) + { /* get transition using state table */ - Tran const *t = myTable + myState*myNsignals + sig; + Tran const *t = myTable + myState * myNsignals + sig; /* accept new state */ myState = t->nextState; @@ -68,7 +69,8 @@ public: /* */ (this->*(t->action))(); } - void doNothing() { + void doNothing() + { return; } protected: diff --git a/src/modules/systemlib/up_cxxinitialize.c b/src/modules/systemlib/up_cxxinitialize.c index 41707360e4..6460b03c9d 100644 --- a/src/modules/systemlib/up_cxxinitialize.c +++ b/src/modules/systemlib/up_cxxinitialize.c @@ -126,27 +126,25 @@ extern uint32_t _etext; __EXPORT void up_cxxinitialize(void); void up_cxxinitialize(void) { - initializer_t *initp; + initializer_t *initp; - cxxdbg("_sinit: %p _einit: %p _stext: %p _etext: %p\n", - &_sinit, &_einit, &_stext, &_etext); + cxxdbg("_sinit: %p _einit: %p _stext: %p _etext: %p\n", + &_sinit, &_einit, &_stext, &_etext); - /* Visit each entry in the initialzation table */ + /* Visit each entry in the initialzation table */ - for (initp = &_sinit; initp != &_einit; initp++) - { - initializer_t initializer = *initp; - cxxdbg("initp: %p initializer: %p\n", initp, initializer); + for (initp = &_sinit; initp != &_einit; initp++) { + initializer_t initializer = *initp; + cxxdbg("initp: %p initializer: %p\n", initp, initializer); - /* Make sure that the address is non-NULL and lies in the text region - * defined by the linker script. Some toolchains may put NULL values - * or counts in the initialization table - */ + /* Make sure that the address is non-NULL and lies in the text region + * defined by the linker script. Some toolchains may put NULL values + * or counts in the initialization table + */ - if ((void*)initializer > (void*)&_stext && (void*)initializer < (void*)&_etext) - { - cxxdbg("Calling %p\n", initializer); - initializer(); - } - } + if ((void *)initializer > (void *)&_stext && (void *)initializer < (void *)&_etext) { + cxxdbg("Calling %p\n", initializer); + initializer(); + } + } } diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index c8920607ac..5a13a864c1 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -284,7 +284,7 @@ int orb_group_count(const struct orb_metadata *meta) * priority, independent of the startup order of the associated publishers. * @return OK on success, ERROR otherwise with errno set accordingly. */ -int orb_priority(int handle, int *priority) +int orb_priority(int handle, int32_t *priority) { return uORB::Manager::get_instance()->orb_priority(handle, priority); } diff --git a/src/modules/uORB/uORBManager.hpp b/src/modules/uORB/uORBManager.hpp index f998c8252c..04158234bd 100644 --- a/src/modules/uORB/uORBManager.hpp +++ b/src/modules/uORB/uORBManager.hpp @@ -269,7 +269,7 @@ public: * priority, independent of the startup order of the associated publishers. * @return OK on success, ERROR otherwise with errno set accordingly. */ - int orb_priority(int handle, int *priority) ; + int orb_priority(int handle, int32_t *priority) ; /** * Set the minimum interval between which updates are seen for a subscription. diff --git a/src/modules/uORB/uORBManager_nuttx.cpp b/src/modules/uORB/uORBManager_nuttx.cpp index 8b2051aacf..3c4d886023 100644 --- a/src/modules/uORB/uORBManager_nuttx.cpp +++ b/src/modules/uORB/uORBManager_nuttx.cpp @@ -166,7 +166,7 @@ int uORB::Manager::orb_stat(int handle, uint64_t *time) return ioctl(handle, ORBIOCLASTUPDATE, (unsigned long)(uintptr_t)time); } -int uORB::Manager::orb_priority(int handle, int *priority) +int uORB::Manager::orb_priority(int handle, int32_t *priority) { return ioctl(handle, ORBIOCGPRIORITY, (unsigned long)(uintptr_t)priority); } diff --git a/src/modules/uORB/uORBManager_posix.cpp b/src/modules/uORB/uORBManager_posix.cpp index 68bfa3f7fe..7fe3f6a687 100644 --- a/src/modules/uORB/uORBManager_posix.cpp +++ b/src/modules/uORB/uORBManager_posix.cpp @@ -172,7 +172,7 @@ int uORB::Manager::orb_stat(int handle, uint64_t *time) return px4_ioctl(handle, ORBIOCLASTUPDATE, (unsigned long)(uintptr_t)time); } -int uORB::Manager::orb_priority(int handle, int *priority) +int uORB::Manager::orb_priority(int handle, int32_t *priority) { return px4_ioctl(handle, ORBIOCGPRIORITY, (unsigned long)(uintptr_t)priority); } diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index da62e55634..9873d87fbe 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -40,12 +40,11 @@ MODULE_COMMAND = uavcan MAXOPTIMIZATION = -O3 MODULE_STACKSIZE = 3200 -WFRAME_LARGER_THAN = 1400 +WFRAME_LARGER_THAN = 1416 # Main SRCS += uavcan_main.cpp \ - uavcan_servers.cpp \ - uavcan_clock.cpp \ + uavcan_servers.cpp \ uavcan_params.c # Actuators @@ -70,7 +69,8 @@ override EXTRADEFINES := $(EXTRADEFINES) \ -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 \ -DUAVCAN_NO_ASSERTIONS \ -DUAVCAN_MEM_POOL_BLOCK_SIZE=48 \ --DUAVCAN_MAX_NETWORK_SIZE_HINT=16 +-DUAVCAN_MAX_NETWORK_SIZE_HINT=16 \ +-DUAVCAN_STM32_TIMER_NUMBER=5 # # libuavcan drivers for STM32 diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index 1986de4247..694a3988a1 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -159,7 +159,14 @@ void UavcanBarometerBridge::air_pressure_sub_cb(const { baro_report report; - report.timestamp = msg.getMonotonicTimestamp().toUSec(); + /* + * FIXME HACK + * This code used to rely on msg.getMonotonicTimestamp().toUSec() instead of HRT. + * It stopped working when the time sync feature has been introduced, because it caused libuavcan + * to use an independent time source (based on hardware TIM5) instead of HRT. + * The proper solution is to be developed. + */ + report.timestamp = hrt_absolute_time(); report.temperature = last_temperature_kelvin - 273.15F; report.pressure = msg.static_pressure / 100.0F; // Convert to millibar report.error_count = 0; diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index 0830aba4ee..d80b4b425d 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -96,7 +96,15 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure +#include + //todo:The Inclusion of file_server_backend is killing // #include and leaving OK undefined # define OK 0 @@ -77,7 +79,11 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys CDev("uavcan", UAVCAN_DEVICE_PATH), _node(can_driver, system_clock), _node_mutex(), - _esc_controller(_node) + _esc_controller(_node), + _time_sync_master(_node), + _time_sync_slave(_node), + _master_timer(_node), + _setget_response(0) { _task_should_exit = false; _fw_server_action = None; @@ -186,6 +192,239 @@ int UavcanNode::getHardwareVersion(uavcan::protocol::HardwareVersion &hwver) return rv; } +int UavcanNode::print_params(uavcan::protocol::param::GetSet::Response &resp) +{ + if (resp.value.is(uavcan::protocol::param::Value::Tag::integer_value)) { + return std::printf("name: %s %lld\n", resp.name.c_str(), + resp.value.to()); + + } else if (resp.value.is(uavcan::protocol::param::Value::Tag::real_value)) { + return std::printf("name: %s %.4f\n", resp.name.c_str(), + static_cast(resp.value.to())); + + } else if (resp.value.is(uavcan::protocol::param::Value::Tag::boolean_value)) { + return std::printf("name: %s %d\n", resp.name.c_str(), + resp.value.to()); + + } else if (resp.value.is(uavcan::protocol::param::Value::Tag::string_value)) { + return std::printf("name: %s '%s'\n", resp.name.c_str(), + resp.value.to().c_str()); + } + + return -1; +} + +void UavcanNode::cb_opcode(const uavcan::ServiceCallResult &result) +{ + uavcan::protocol::param::ExecuteOpcode::Response resp; + _callback_success = result.isSuccessful(); + resp = result.getResponse(); + _callback_success &= resp.ok; +} + +int UavcanNode::save_params(int remote_node_id) +{ + uavcan::protocol::param::ExecuteOpcode::Request opcode_req; + opcode_req.opcode = opcode_req.OPCODE_SAVE; + uavcan::ServiceClient client(_node); + client.setCallback(ExecuteOpcodeCallback(this, &UavcanNode::cb_opcode)); + _callback_success = false; + int call_res = client.call(remote_node_id, opcode_req); + + if (call_res >= 0) { + while (client.hasPendingCalls()) { + usleep(10000); + } + } + + if (!_callback_success) { + std::printf("Failed to save parameters: %d\n", call_res); + return -1; + } + + return 0; +} + +void UavcanNode::cb_restart(const uavcan::ServiceCallResult &result) +{ + uavcan::protocol::RestartNode::Response resp; + _callback_success = result.isSuccessful(); + resp = result.getResponse(); + _callback_success &= resp.ok; +} + +int UavcanNode::reset_node(int remote_node_id) +{ + uavcan::protocol::RestartNode::Request restart_req; + restart_req.magic_number = restart_req.MAGIC_NUMBER; + uavcan::ServiceClient client(_node); + client.setCallback(RestartNodeCallback(this, &UavcanNode::cb_restart)); + _callback_success = false; + int call_res = client.call(remote_node_id, restart_req); + + if (call_res >= 0) { + while (client.hasPendingCalls()) { + usleep(10000); + } + } + + if (!call_res) { + std::printf("Failed to reset node: %d\n", remote_node_id); + return -1; + } + + return 0; +} + +int UavcanNode::list_params(int remote_node_id) +{ + int rv = 0; + int index = 0; + uavcan::protocol::param::GetSet::Response resp; + set_setget_response(&resp); + + while (true) { + uavcan::protocol::param::GetSet::Request req; + req.index = index++; + _callback_success = false; + int call_res = get_set_param(remote_node_id, nullptr, req); + + if (call_res < 0 || !_callback_success) { + std::printf("Failed to get param: %d\n", call_res); + rv = -1; + break; + } + + if (resp.name.empty()) { // Empty name means no such param, which means we're finished + break; + } + + print_params(resp); + } + + free_setget_response(); + return rv; +} + + +void UavcanNode::cb_setget(const uavcan::ServiceCallResult &result) +{ + _callback_success = result.isSuccessful(); + *_setget_response = result.getResponse(); +} + +int UavcanNode::get_set_param(int remote_node_id, const char *name, uavcan::protocol::param::GetSet::Request &req) +{ + if (name != nullptr) { + req.name = name; + } + + uavcan::ServiceClient client(_node); + client.setCallback(GetSetCallback(this, &UavcanNode::cb_setget)); + _callback_success = false; + int call_res = client.call(remote_node_id, req); + + if (call_res >= 0) { + + while (client.hasPendingCalls()) { + usleep(10000); + } + + if (!_callback_success) { + call_res = -1; + } + } + + return call_res; +} + +int UavcanNode::set_param(int remote_node_id, const char *name, char *value) +{ + uavcan::protocol::param::GetSet::Request req; + uavcan::protocol::param::GetSet::Response resp; + set_setget_response(&resp); + int rv = get_set_param(remote_node_id, name, req); + + if (rv < 0 || resp.name.empty()) { + std::printf("Failed to retrieve param: %s\n", name); + rv = -1; + + } else { + + rv = 0; + req = {}; + + if (resp.value.is(uavcan::protocol::param::Value::Tag::integer_value)) { + int64_t i = std::strtoull(value, NULL, 10); + int64_t min = resp.min_value.to(); + int64_t max = resp.max_value.to(); + + if (i >= min && i <= max) { + req.value.to() = i; + + } else { + std::printf("Invalid value for: %s must be between %lld and %lld\n", name, min, max); + rv = -1; + } + + } else if (resp.value.is(uavcan::protocol::param::Value::Tag::real_value)) { + float f = static_cast(std::atof(value)); + float min = resp.min_value.to(); + float max = resp.max_value.to(); + + if (f >= min && f <= max) { + req.value.to() = f; + + } else { + std::printf("Invalid value for: %s must be between %.4f and %.4f\n", name, static_cast(min), + static_cast(max)); + rv = -1; + } + + } else if (resp.value.is(uavcan::protocol::param::Value::Tag::boolean_value)) { + int8_t i = (value[0] == '1' || value[0] == 't') ? 1 : 0; + req.value.to() = i; + + } else if (resp.value.is(uavcan::protocol::param::Value::Tag::string_value)) { + req.value.to() = value; + } + + if (rv == 0) { + rv = get_set_param(remote_node_id, name, req); + + if (rv < 0 || resp.name.empty()) { + std::printf("Failed to set param: %s\n", name); + return -1; + } + + return 0; + } + } + + free_setget_response(); + return rv; +} + +int UavcanNode::get_param(int remote_node_id, const char *name) +{ + uavcan::protocol::param::GetSet::Request req; + uavcan::protocol::param::GetSet::Response resp; + set_setget_response(&resp); + int rv = get_set_param(remote_node_id, name, req); + + if (rv < 0 || resp.name.empty()) { + std::printf("Failed to get param: %s\n", name); + rv = -1; + + } else { + print_params(resp); + rv = 0; + } + + free_setget_response(); + return rv; +} + int UavcanNode::start_fw_server() { int rv = -1; @@ -194,7 +433,7 @@ int UavcanNode::start_fw_server() if (_servers == nullptr) { - rv = UavcanServers::start(2, _node); + rv = UavcanServers::start(_node); if (rv >= 0) { /* @@ -459,6 +698,48 @@ int UavcanNode::add_poll_fd(int fd) } +void UavcanNode::handle_time_sync(const uavcan::TimerEvent &) +{ + + /* + * Check whether there are higher priority masters in the network. + * If there are, we need to activate the local slave in order to sync with them. + */ + if (_time_sync_slave.isActive()) { // "Active" means that the slave tracks at least one remote master in the network + if (_node.getNodeID() < _time_sync_slave.getMasterNodeID()) { + /* + * We're the highest priority master in the network. + * We need to suppress the slave now to prevent it from picking up unwanted sync messages from + * lower priority masters. + */ + _time_sync_slave.suppress(true); // SUPPRESS + + } else { + /* + * There is at least one higher priority master in the network. + * We need to allow the slave to adjust our local clock in order to be in sync. + */ + _time_sync_slave.suppress(false); // UNSUPPRESS + } + + } else { + /* + * There are no other time sync masters in the network, so we're the only time source. + * The slave must be suppressed anyway to prevent it from disrupting the local clock if a new + * lower priority master suddenly appears in the network. + */ + _time_sync_slave.suppress(true); + } + + /* + * Publish the sync message now, even if we're not a higher priority master. + * Other nodes will be able to pick the right master anyway. + */ + _time_sync_master.publish(); +} + + + int UavcanNode::run() { (void)pthread_mutex_lock(&_node_mutex); @@ -472,6 +753,28 @@ int UavcanNode::run() memset(&_outputs, 0, sizeof(_outputs)); + /* + * Set up the time synchronization + */ + + const int slave_init_res = _time_sync_slave.start(); + + if (slave_init_res < 0) { + warnx("Failed to start time_sync_slave"); + _task_should_exit = true; + } + + /* When we have a system wide notion of time update (i.e the transition from the initial + * System RTC setting to the GPS) we would call uavcan_stm32::clock::setUtc() when that + * happens, but for now we use adjustUtc with a correction of the hrt so that the + * time bases are the same + */ + uavcan_stm32::clock::adjustUtc(uavcan::UtcDuration::fromUSec(hrt_absolute_time())); + _master_timer.setCallback(TimerCallback(this, &UavcanNode::handle_time_sync)); + _master_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000)); + + + const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0); if (busevent_fd < 0) { @@ -870,7 +1173,7 @@ UavcanNode::print_info() static void print_usage() { warnx("usage: \n" - "\tuavcan {start [fw]|status|stop [all|fw]|arm|disarm|update fw}"); + "\tuavcan {start [fw]|status|stop [all|fw]|arm|disarm|update fw|param [set|get|list|save] nodeid [name] [value]|reset nodeid}"); } extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); @@ -956,6 +1259,60 @@ int uavcan_main(int argc, char *argv[]) ::exit(0); } + /* + * Parameter setting commands + * + * uavcan param list + * uavcan param save + * uavcan param get + * uavcan param set + * + */ + int node_arg = !std::strcmp(argv[1], "reset") ? 2 : 3; + + if (!std::strcmp(argv[1], "param") || node_arg == 2) { + if (argc < node_arg + 1) { + errx(1, "Node id required"); + } + + int nodeid = atoi(argv[node_arg]); + + if (nodeid == 0 || nodeid > 127 || nodeid == inst->get_node().getNodeID().get()) { + errx(1, "Invalid Node id"); + } + + if (node_arg == 2) { + + return inst->reset_node(nodeid); + + } else if (!std::strcmp(argv[2], "list")) { + + return inst->list_params(nodeid); + + } else if (!std::strcmp(argv[2], "save")) { + + return inst->save_params(nodeid); + + } else if (!std::strcmp(argv[2], "get")) { + if (argc < 5) { + errx(1, "Name required"); + } + + return inst->get_param(nodeid, argv[4]); + + } else if (!std::strcmp(argv[2], "set")) { + if (argc < 5) { + errx(1, "Name required"); + } + + if (argc < 6) { + errx(1, "Value required"); + } + + return inst->set_param(nodeid, argv[4], argv[5]); + } + } + if (!std::strcmp(argv[1], "stop")) { if (fw) { diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index acc1c32843..23e9c89417 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -36,6 +36,12 @@ #include #include +#include +#include +#include +#include +#include + #include #include @@ -48,7 +54,7 @@ #include "actuators/esc.hpp" #include "sensors/sensor_bridge.hpp" -# include "uavcan_servers.hpp" +#include "uavcan_servers.hpp" /** * @file uavcan_main.hpp @@ -119,8 +125,13 @@ public: static int getHardwareVersion(uavcan::protocol::HardwareVersion &hwver); int fw_server(eServerAction action); void attachITxQueueInjector(ITxQueueInjector *injector) {_tx_injector = injector;} - + int list_params(int remote_node_id); + int save_params(int remote_node_id); + int set_param(int remote_node_id, const char *name, char *value); + int get_param(int remote_node_id, const char *name); + int reset_node(int remote_node_id); private: + void fill_node_info(); int init(uavcan::NodeID node_id); void node_spin_once(); @@ -129,6 +140,16 @@ private: int start_fw_server(); int stop_fw_server(); int request_fw_check(); + int print_params(uavcan::protocol::param::GetSet::Response &resp); + int get_set_param(int nodeid, const char *name, uavcan::protocol::param::GetSet::Request &req); + void set_setget_response(uavcan::protocol::param::GetSet::Response *resp) + { + _setget_response = resp; + } + void free_setget_response(void) + { + _setget_response = nullptr; + } int _task = -1; ///< handle to the OS task bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver @@ -150,6 +171,8 @@ private: pthread_mutex_t _node_mutex; sem_t _server_command_sem; UavcanEscController _esc_controller; + uavcan::GlobalTimeSyncMaster _time_sync_master; + uavcan::GlobalTimeSyncSlave _time_sync_slave; List _sensor_bridges; ///< List of active sensor bridges @@ -175,4 +198,22 @@ private: perf_counter_t _perfcnt_node_spin_elapsed = perf_alloc(PC_ELAPSED, "uavcan_node_spin_elapsed"); perf_counter_t _perfcnt_esc_mixer_output_elapsed = perf_alloc(PC_ELAPSED, "uavcan_esc_mixer_output_elapsed"); perf_counter_t _perfcnt_esc_mixer_total_elapsed = perf_alloc(PC_ELAPSED, "uavcan_esc_mixer_total_elapsed"); + + void handle_time_sync(const uavcan::TimerEvent &); + + typedef uavcan::MethodBinder TimerCallback; + uavcan::TimerEventForwarder _master_timer; + + bool _callback_success; + uavcan::protocol::param::GetSet::Response *_setget_response; + typedef uavcan::MethodBinder &)> GetSetCallback; + typedef uavcan::MethodBinder &)> ExecuteOpcodeCallback; + typedef uavcan::MethodBinder &)> RestartNodeCallback; + void cb_setget(const uavcan::ServiceCallResult &result); + void cb_opcode(const uavcan::ServiceCallResult &result); + void cb_restart(const uavcan::ServiceCallResult &result); + }; diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp index 1713b282e7..824b5182e2 100644 --- a/src/modules/uavcan/uavcan_servers.cpp +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -76,7 +76,7 @@ UavcanServers *UavcanServers::_instance; UavcanServers::UavcanServers(uavcan::INode &main_node) : _subnode_thread(-1), - _vdriver(UAVCAN_STM32_NUM_IFACES, uavcan_stm32::SystemClock::instance()), + _vdriver(NumIfaces, uavcan_stm32::SystemClock::instance()), _subnode(_vdriver, uavcan_stm32::SystemClock::instance()), _main_node(main_node), _tracer(), @@ -98,10 +98,11 @@ UavcanServers::~UavcanServers() if (_mutex_inited) { (void)Lock::deinit(_subnode_mutex); } + _main_node.getDispatcher().removeRxFrameListener(); } -int UavcanServers::stop(void) +int UavcanServers::stop() { UavcanServers *server = instance(); @@ -123,7 +124,7 @@ int UavcanServers::stop(void) return 0; } -int UavcanServers::start(unsigned num_ifaces, uavcan::INode &main_node) +int UavcanServers::start(uavcan::INode &main_node) { if (_instance != nullptr) { warnx("Already started"); @@ -140,7 +141,7 @@ int UavcanServers::start(unsigned num_ifaces, uavcan::INode &main_node) return -2; } - int rv = _instance->init(num_ifaces); + int rv = _instance->init(); if (rv < 0) { warnx("Node init failed: %d", rv); @@ -164,9 +165,9 @@ int UavcanServers::start(unsigned num_ifaces, uavcan::INode &main_node) rv = pthread_create(&_instance->_subnode_thread, &tattr, static_cast(run_trampoline), NULL); - if (rv < 0) { - warnx("pthread_create() failed: %d", errno); - rv = -errno; + if (rv != 0) { + rv = -rv; + warnx("pthread_create() failed: %d", rv); delete _instance; _instance = nullptr; } @@ -174,7 +175,7 @@ int UavcanServers::start(unsigned num_ifaces, uavcan::INode &main_node) return rv; } -int UavcanServers::init(unsigned num_ifaces) +int UavcanServers::init() { errno = 0; @@ -268,6 +269,7 @@ int UavcanServers::init(unsigned num_ifaces) return OK; } + __attribute__((optimize("-O0"))) pthread_addr_t UavcanServers::run(pthread_addr_t) { diff --git a/src/modules/uavcan/uavcan_servers.hpp b/src/modules/uavcan/uavcan_servers.hpp index 9a68a8cc25..9d21b8f93f 100644 --- a/src/modules/uavcan/uavcan_servers.hpp +++ b/src/modules/uavcan/uavcan_servers.hpp @@ -72,9 +72,11 @@ */ class UavcanServers { + static constexpr unsigned NumIfaces = 1; // UAVCAN_STM32_NUM_IFACES + static constexpr unsigned MemPoolSize = 64 * uavcan::MemPoolBlockSize; - static constexpr unsigned MaxCanFramsPerTransfer = 63; + static constexpr unsigned MaxCanFramesPerTransfer = 63; /** * This number is based on the worst case max number of frames per interface. With @@ -84,7 +86,7 @@ class UavcanServers * 1 instead of UAVCAN_STM32_NUM_IFACES into the constructor of the virtual CAN driver. */ static constexpr unsigned QueuePoolSize = - (UAVCAN_STM32_NUM_IFACES * uavcan::MemPoolBlockSize *MaxCanFramsPerTransfer); + (NumIfaces * uavcan::MemPoolBlockSize * MaxCanFramesPerTransfer); static constexpr unsigned StackSize = 3500; static constexpr unsigned Priority = 120; @@ -96,8 +98,8 @@ public: virtual ~UavcanServers(); - static int start(unsigned num_ifaces, uavcan::INode &main_node); - static int stop(void); + static int start(uavcan::INode &main_node); + static int stop(); SubNode &get_node() { return _subnode; } @@ -116,8 +118,7 @@ private: pthread_t _subnode_thread; pthread_mutex_t _subnode_mutex; - int init(unsigned num_ifaces); - void deinit(void); + int init(); pthread_addr_t run(pthread_addr_t); diff --git a/src/modules/unit_test/unit_test.h b/src/modules/unit_test/unit_test.h index 8ed4efadf9..bd021ff54a 100644 --- a/src/modules/unit_test/unit_test.h +++ b/src/modules/unit_test/unit_test.h @@ -44,14 +44,14 @@ public: UnitTest(); virtual ~UnitTest(); - + /// @brief Override to run your unit tests. Unit tests should be called using ut_run_test macro. /// @return true: all unit tests succeeded, false: one or more unit tests failed virtual bool run_tests(void) = 0; - + /// @brief Prints results from running of unit tests. void print_results(void); - + /// @brief Macro to create a function which will run a unit test class and print results. #define ut_declare_test(test_function, test_class) \ bool test_function(void) \ @@ -61,9 +61,9 @@ public: test->print_results(); \ return success; \ } - + protected: - + /// @brief Runs a single unit test. Unit tests must have the function signature of bool test(void). The unit /// test should return true if it succeeded, false for fail. #define ut_run_test(test) \ @@ -80,7 +80,7 @@ protected: } \ _cleanup(); \ } while (0) - + /// @brief Used to assert a value within a unit test. #define ut_assert(message, test) \ do { \ @@ -91,7 +91,7 @@ protected: _assertions++; \ } \ } while (0) - + /// @brief Used to compare two integer values within a unit test. If possible use ut_compare instead of ut_assert /// since it will give you better error reporting of the actual values being compared. #define ut_compare(message, v1, v2) \ @@ -105,12 +105,13 @@ protected: _assertions++; \ } \ } while (0) - + virtual void _init(void) { }; ///< Run before each unit test. Override to provide custom behavior. virtual void _cleanup(void) { }; ///< Run after each unit test. Override to provide custom behavior. - - void _print_assert(const char* msg, const char* test, const char* file, int line); - void _print_compare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line); + + void _print_assert(const char *msg, const char *test, const char *file, int line); + void _print_compare(const char *msg, const char *v1_text, int v1, const char *v2_text, int v2, const char *file, + int line); int _tests_run; ///< The number of individual unit tests run int _tests_failed; ///< The number of unit tests which failed diff --git a/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp b/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp index 43cd080628..9cb0ac73d2 100644 --- a/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp +++ b/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp @@ -35,14 +35,12 @@ * @file ets_airspeed.cpp * @author Simon Wilks * @author Mark Charlebois (simulator) - * - * Driver for the Simulated Airspeed Sensor based on Eagle Tree Airspeed V3. + * @author Roman Bapst (simulator) + * Driver for the Simulated AirspeedSim Sensor based on Eagle Tree AirspeedSim V3. */ #include -#include - #include #include #include @@ -56,8 +54,6 @@ #include #include -#include - #include #include #include @@ -71,12 +67,15 @@ #include #include -#include +#include -Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char* path) : - I2C("Airspeed", path, bus, address), +#include "airspeedsim.h" + +AirspeedSim::AirspeedSim(int bus, int address, unsigned conversion_interval, const char* path) : + VDev("AIRSPEEDSIM", path), _reports(nullptr), _buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")), + _retries(0), _max_differential_pressure_pa(0), _sensor_ok(false), _last_published_sensor_ok(true), /* initialize differently to force publication */ @@ -97,7 +96,7 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const cha memset(&_work, 0, sizeof(_work)); } -Airspeed::~Airspeed() +AirspeedSim::~AirspeedSim() { /* make sure we are truly inactive */ stop(); @@ -116,18 +115,21 @@ Airspeed::~Airspeed() } int -Airspeed::init() +AirspeedSim::init() { int ret = ERROR; - /* do I2C init (and probe) first */ - if (I2C::init() != OK) + /* init base class */ + if (VDev::init() != OK) { + DEVICE_DEBUG("VDev init failed"); goto out; + } /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(differential_pressure_s)); - if (_reports == nullptr) + if (_reports == nullptr) { goto out; + } /* register alternate interfaces if we have to */ _class_instance = register_class_devname(AIRSPEED_BASE_DEVICE_PATH); @@ -154,7 +156,7 @@ out: } int -Airspeed::probe() +AirspeedSim::probe() { /* on initial power up the device may need more than one retry for detection. Once it is running the number of retries can @@ -169,7 +171,7 @@ Airspeed::probe() } int -Airspeed::ioctl(device::file_t *filp, int cmd, unsigned long arg) +AirspeedSim::ioctl(device::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { @@ -272,12 +274,13 @@ Airspeed::ioctl(device::file_t *filp, int cmd, unsigned long arg) default: /* give it to the superclass */ - return I2C::ioctl(filp, cmd, arg); + //return I2C::ioctl(filp, cmd, arg); XXX SIM should be super class + return 0; } } ssize_t -Airspeed::read(device::file_t *filp, char *buffer, size_t buflen) +AirspeedSim::read(device::file_t *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(differential_pressure_s); differential_pressure_s *abuf = reinterpret_cast(buffer); @@ -336,24 +339,24 @@ Airspeed::read(device::file_t *filp, char *buffer, size_t buflen) } void -Airspeed::start() +AirspeedSim::start() { /* reset the report ring and state machine */ _collect_phase = false; _reports->flush(); /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&Airspeed::cycle_trampoline, this, 1); + work_queue(HPWORK, &_work, (worker_t)&AirspeedSim::cycle_trampoline, this, 1); } void -Airspeed::stop() +AirspeedSim::stop() { work_cancel(HPWORK, &_work); } void -Airspeed::update_status() +AirspeedSim::update_status() { if (_sensor_ok != _last_published_sensor_ok) { /* notify about state change */ @@ -375,9 +378,9 @@ Airspeed::update_status() } void -Airspeed::cycle_trampoline(void *arg) +AirspeedSim::cycle_trampoline(void *arg) { - Airspeed *dev = (Airspeed *)arg; + AirspeedSim *dev = (AirspeedSim *)arg; dev->cycle(); // XXX we do not know if this is @@ -387,7 +390,7 @@ Airspeed::cycle_trampoline(void *arg) } void -Airspeed::print_info() +AirspeedSim::print_info() { perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); @@ -397,8 +400,26 @@ Airspeed::print_info() } void -Airspeed::new_report(const differential_pressure_s &report) +AirspeedSim::new_report(const differential_pressure_s &report) { if (!_reports->force(&report)) perf_count(_buffer_overflows); } + +int +AirspeedSim::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) { + if (recv_len > 0) { + // this is equivalent to the collect phase + Simulator *sim = Simulator::getInstance(); + if (sim == NULL) { + PX4_ERR("Error BARO_SIM::transfer no simulator"); + return -ENODEV; + } + PX4_DEBUG("BARO_SIM::transfer getting sample"); + sim->getAirspeedSample(recv, recv_len); + } else { + // we don't need measure phase + } + + return 0; +} \ No newline at end of file diff --git a/src/platforms/posix/drivers/airspeedsim/airspeedsim.h b/src/platforms/posix/drivers/airspeedsim/airspeedsim.h index 66f4c58627..2c668fc97f 100644 --- a/src/platforms/posix/drivers/airspeedsim/airspeedsim.h +++ b/src/platforms/posix/drivers/airspeedsim/airspeedsim.h @@ -40,7 +40,7 @@ #include -#include +//#include #include #include @@ -55,10 +55,11 @@ #include #include -#include -#include -#include +//#include +//#include +//#include +#include #include #include @@ -69,6 +70,7 @@ #include #include #include +#include #include #include @@ -87,7 +89,7 @@ static const int ERROR = -1; # error This requires CONFIG_SCHED_WORKQUEUE. #endif -class __EXPORT AirspeedSim : public device::I2C +class __EXPORT AirspeedSim : public device::VDev { public: AirspeedSim(int bus, int address, unsigned conversion_interval, const char *path); @@ -95,8 +97,8 @@ public: virtual int init(); - virtual ssize_t read(px4_dev_handle_t *handlep, char *buffer, size_t buflen); - virtual int ioctl(px4_dev_handle_t *handlep, int cmd, unsigned long arg); + virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); /** * Diagnostics - print some basic information about the driver. @@ -104,9 +106,11 @@ public: virtual void print_info(); private: - RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; perf_counter_t _buffer_overflows; + unsigned _retries; // XXX this should come from the SIM class + /* this class has pointer data members and should not be copied */ AirspeedSim(const AirspeedSim &); AirspeedSim &operator=(const AirspeedSim &); @@ -122,16 +126,19 @@ protected: virtual int measure() = 0; virtual int collect() = 0; + virtual int transfer(const uint8_t *send, unsigned send_len, + uint8_t *recv, unsigned recv_len); + /** * Update the subsystem status */ void update_status(); - work_s _work; + struct work_s _work; float _max_differential_pressure_pa; bool _sensor_ok; bool _last_published_sensor_ok; - int _measure_ticks; + unsigned _measure_ticks; bool _collect_phase; float _diff_pres_offset; diff --git a/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp b/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp new file mode 100644 index 0000000000..5aca386acb --- /dev/null +++ b/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp @@ -0,0 +1,631 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file meas_airspeed_sim.cpp + * @author Lorenz Meier + * @author Sarthak Kaingade + * @author Simon Wilks + * @author Thomas Gubler + * @author Roman Bapst + * + * Driver for a simulated airspeed sensor. + * + */ + + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include + +#include "airspeedsim.h" + +/* I2C bus address is 1010001x */ +#define I2C_ADDRESS_MS4525DO 0x28 /**< 7-bit address. Depends on the order code (this is for code "I") */ +#define PATH_MS4525 "/dev/ms4525" +/* The MS5525DSO address is 111011Cx, where C is the complementary value of the pin CSB */ +#define I2C_ADDRESS_MS5525DSO 0x77 //0x77/* 7-bit address, addr. pin pulled low */ +#define PATH_MS5525 "/dev/ms5525" + +/* Register address */ +#define ADDR_READ_MR 0x00 /* write to this address to start conversion */ + +/* Measurement rate is 100Hz */ +#define MEAS_RATE 100 +#define MEAS_DRIVER_FILTER_FREQ 1.2f +#define CONVERSION_INTERVAL (1000000 / MEAS_RATE) /* microseconds */ + +class MEASAirspeedSim : public AirspeedSim +{ +public: + MEASAirspeedSim(int bus, int address = I2C_ADDRESS_MS4525DO, const char *path = PATH_MS4525); + +protected: + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + virtual void cycle(); + virtual int measure(); + virtual int collect(); + + math::LowPassFilter2p _filter; + + /** + * Correct for 5V rail voltage variations + */ + void voltage_correction(float &diff_pres_pa, float &temperature); + + int _t_system_power; + struct system_power_s system_power; +}; + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int measairspeedsim_main(int argc, char *argv[]); + +MEASAirspeedSim::MEASAirspeedSim(int bus, int address, const char *path) : AirspeedSim(bus, address, + CONVERSION_INTERVAL, path), + _filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ), + _t_system_power(-1), + system_power{} +{} + +int +MEASAirspeedSim::measure() +{ + int ret; + + /* + * Send the command to begin a measurement. + */ + uint8_t cmd = 0; + ret = transfer(&cmd, 1, nullptr, 0); + + if (OK != ret) { + perf_count(_comms_errors); + } + + return ret; +} + +int +MEASAirspeedSim::collect() +{ + int ret = -EIO; + + /* read from the sensor */ + #pragma pack(push, 1) + struct { + float temperature; + float diff_pressure; + } airspeed_report; + #pragma pack(pop) + + + perf_begin(_sample_perf); + + ret = transfer(nullptr, 0, (uint8_t *)&airspeed_report, sizeof(airspeed_report)); + + if (ret < 0) { + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; + } + + uint8_t status = 0; + + switch (status) { + case 0: + break; + + case 1: + + /* fallthrough */ + case 2: + + /* fallthrough */ + case 3: + perf_count(_comms_errors); + perf_end(_sample_perf); + return -EAGAIN; + } + + float temperature = airspeed_report.temperature; + float diff_press_pa_raw = airspeed_report.diff_pressure * 100.0f; // convert from millibar to bar + + struct differential_pressure_s report; + + /* track maximum differential pressure measured (so we can work out top speed). */ + if (diff_press_pa_raw > _max_differential_pressure_pa) { + _max_differential_pressure_pa = diff_press_pa_raw; + } + + report.timestamp = hrt_absolute_time(); + report.error_count = perf_event_count(_comms_errors); + report.temperature = temperature; + report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw); + + report.differential_pressure_raw_pa = diff_press_pa_raw; + report.max_differential_pressure_pa = _max_differential_pressure_pa; + + if (_airspeed_pub != nullptr && !(_pub_blocked)) { + /* publish it */ + orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); + } + + new_report(report); + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + ret = OK; + + perf_end(_sample_perf); + + return ret; +} + +void +MEASAirspeedSim::cycle() +{ + int ret; + + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + ret = collect(); + + if (OK != ret) { + /* restart the measurement state machine */ + start(); + _sensor_ok = false; + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + */ + if (_measure_ticks > USEC2TICK(CONVERSION_INTERVAL)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&AirspeedSim::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + ret = measure(); + + if (OK != ret) { + DEVICE_DEBUG("measure error"); + } + + _sensor_ok = (ret == OK); + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&AirspeedSim::cycle_trampoline, + this, + USEC2TICK(CONVERSION_INTERVAL)); +} + +/** + correct for 5V rail voltage if the system_power ORB topic is + available + + See http://uav.tridgell.net/MS4525/MS4525-offset.png for a graph of + offset versus voltage for 3 sensors + */ +void +MEASAirspeedSim::voltage_correction(float &diff_press_pa, float &temperature) +{ +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + + if (_t_system_power == -1) { + _t_system_power = orb_subscribe(ORB_ID(system_power)); + } + + if (_t_system_power == -1) { + // not available + return; + } + + bool updated = false; + orb_check(_t_system_power, &updated); + + if (updated) { + orb_copy(ORB_ID(system_power), _t_system_power, &system_power); + } + + if (system_power.voltage5V_v < 3.0f || system_power.voltage5V_v > 6.0f) { + // not valid, skip correction + return; + } + + const float slope = 65.0f; + /* + apply a piecewise linear correction, flattening at 0.5V from 5V + */ + float voltage_diff = system_power.voltage5V_v - 5.0f; + + if (voltage_diff > 0.5f) { + voltage_diff = 0.5f; + } + + if (voltage_diff < -0.5f) { + voltage_diff = -0.5f; + } + + diff_press_pa -= voltage_diff * slope; + + /* + the temperature masurement varies as well + */ + const float temp_slope = 0.887f; + voltage_diff = system_power.voltage5V_v - 5.0f; + + if (voltage_diff > 0.5f) { + voltage_diff = 0.5f; + } + + if (voltage_diff < -1.0f) { + voltage_diff = -1.0f; + } + + temperature -= voltage_diff * temp_slope; +#endif // CONFIG_ARCH_BOARD_PX4FMU_V2 +} + +/** + * Local functions in support of the shell command. + */ +namespace meas_airspeed_sim +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +MEASAirspeedSim *g_dev = nullptr; + +int start(int i2c_bus); +int stop(); +int test(); +int reset(); +int info(); + +/** + * Start the driver. + * + * This function call only returns once the driver is up and running + * or failed to detect the sensor. + */ +int +start(int i2c_bus) +{ + int fd; + + if (g_dev != nullptr) { + PX4_WARN("already started"); + } + + /* create the driver, try the MS4525DO first */ + g_dev = new MEASAirspeedSim(i2c_bus, I2C_ADDRESS_MS4525DO, PATH_MS4525); + + /* check if the MS4525DO was instantiated */ + if (g_dev == nullptr) { + goto fail; + } + + /* try the MS5525DSO next if init fails */ + if (OK != g_dev->AirspeedSim::init()) { + delete g_dev; + g_dev = new MEASAirspeedSim(i2c_bus, I2C_ADDRESS_MS5525DSO, PATH_MS5525); + + /* check if the MS5525DSO was instantiated */ + if (g_dev == nullptr) { + goto fail; + } + + /* both versions failed if the init for the MS5525DSO fails, give up */ + if (OK != g_dev->AirspeedSim::init()) { + goto fail; + } + } + + /* set the poll rate to default, starts automatic data collection */ + fd = px4_open(PATH_MS4525, O_RDONLY); + + if (fd < 0) { + goto fail; + } + + if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + goto fail; + } + + return 0; + +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + PX4_ERR("no MS4525 airspeedSim sensor connected"); + return 1; +} + +/** + * Stop the driver + */ +int +stop() +{ + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + + } else { + PX4_ERR("driver not running"); + } + + return 0; +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +int +test() +{ + struct differential_pressure_s report; + ssize_t sz; + int ret; + + int fd = px4_open(PATH_MS4525, O_RDONLY); + + if (fd < 0) { + PX4_ERR("%s open failed (try 'meas_airspeed_sim start' if the driver is not running", PATH_MS4525); + return 1; + } + + /* do a simple demand read */ + sz = px4_read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + PX4_ERR("immediate read failed"); + return 1; + } + + PX4_WARN("single read"); + PX4_WARN("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa); + + /* start the sensor polling at 2Hz */ + if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) { + PX4_WARN("failed to set 2Hz poll rate"); + } + + /* read the sensor 5x and report each value */ + for (unsigned i = 0; i < 5; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 2000); + + if (ret != 1) { + warnx("timed out"); + } + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + PX4_WARN("periodic read failed"); + } + + PX4_WARN("periodic read %u", i); + PX4_WARN("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa); + PX4_WARN("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature); + } + + /* reset the sensor polling to its default rate */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { + PX4_ERR("failed to set default rate"); + return 1; + } + + PX4_WARN("PASS"); + + return 0; +} + +/** + * Reset the driver. + */ +int +reset() +{ + int fd = open(PATH_MS4525, O_RDONLY); + + if (fd < 0) { + PX4_ERR("failed "); + return 1; + } + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { + PX4_ERR("driver reset failed"); + return 1; + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + PX4_ERR("driver poll restart failed"); + return 1; + } + + return 0; +} + +/** + * Print a little info about the driver. + */ +int +info() +{ + if (g_dev == nullptr) { + PX4_WARN("driver not running"); + } + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + return 0; +} + +} // namespace + + +static void +meas_airspeed_usage() +{ + PX4_WARN("usage: meas_airspeed_sim command [options]"); + PX4_WARN("options:"); + PX4_WARN("\t-b --bus i2cbus (%d)", 1); + PX4_WARN("command:"); + PX4_WARN("\tstart|stop|reset|test|info"); +} + +int +measairspeedsim_main(int argc, char *argv[]) +{ + int i2c_bus = 1;//PX4_I2C_BUS_DEFAULT; + + int i; + + for (i = 1; i < argc; i++) { + if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) { + if (argc > i + 1) { + i2c_bus = atoi(argv[i + 1]); + } + } + } + + int ret; + + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) { + ret = meas_airspeed_sim::start(i2c_bus); + } + + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) { + ret = meas_airspeed_sim::stop(); + } + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) { + ret = meas_airspeed_sim::test(); + } + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) { + ret = meas_airspeed_sim::reset(); + } + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { + ret = meas_airspeed_sim::info(); + } + + meas_airspeed_usage(); + return ret; +} diff --git a/src/platforms/posix/drivers/airspeedsim/module.mk b/src/platforms/posix/drivers/airspeedsim/module.mk index 2d971e4d28..fb12fa04a2 100644 --- a/src/platforms/posix/drivers/airspeedsim/module.mk +++ b/src/platforms/posix/drivers/airspeedsim/module.mk @@ -34,7 +34,8 @@ # # Makefile to build the generic airspeed driver. # +MODULE_COMMAND = measairspeedsim -SRCS = airspeedsim.cpp +SRCS = airspeedsim.cpp meas_airspeed_sim.cpp MAXOPTIMIZATION = -Os diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index 1c34b281af..be2eae5090 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -65,11 +65,15 @@ extern "C" { } } +static void print_prompt() +{ + cout << "pxh> "; +} + static void run_cmd(const vector &appargs) { // command is appargs[0] string command = appargs[0]; - cout << "----------------------------------\n"; if (apps.find(command) != apps.end()) { const char *arg[appargs.size() + 2]; @@ -82,15 +86,20 @@ static void run_cmd(const vector &appargs) } arg[i] = (char *)0; - cout << "Running: " << command << "\n"; apps[command](i, (char **)arg); - usleep(40000); - cout << "Returning: " << command << "\n"; + usleep(65000); + + } else if (command.compare("help") == 0) { + list_builtins(); + + } else if (command.length() == 0) { + // Do nothing } else { - cout << "Invalid command: " << command << endl; - list_builtins(); + cout << "Invalid command: " << command << "\ntype 'help' for a list of commands" << endl; + } + print_prompt(); } static void usage() @@ -177,16 +186,26 @@ int main(int argc, char **argv) if (!daemon_mode) { string mystr; + print_prompt(); + while (!_ExitFlag) { - cout << "Enter a command and its args:" << endl; - getline(cin, mystr); - process_line(mystr); - mystr = ""; + + struct pollfd fds; + int ret; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; + ret = poll(&fds, 1, 100); + + if (ret > 0) { + getline(cin, mystr); + process_line(mystr); + mystr = ""; + } } } else { while (!_ExitFlag) { - sleep(1000000); + usleep(100000); } } diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp index 129709d3d7..c8f8d801c5 100644 --- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp @@ -143,7 +143,7 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int // Must add NULL at end of argv taskdata->argv[argc] = (char *)0; - PX4_WARN("starting task %s", name); + PX4_DEBUG("starting task %s", name); rv = pthread_attr_init(&attr); if (rv != 0) { @@ -203,7 +203,7 @@ int px4_task_delete(px4_task_t id) { int rv = 0; pthread_t pid; - PX4_WARN("Called px4_task_delete"); + PX4_DEBUG("Called px4_task_delete"); if (id < PX4_MAX_TASKS && taskmap[id].isused) pid = taskmap[id].pid; diff --git a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp index dc3111a40a..ec709cc0ef 100644 --- a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp +++ b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp @@ -51,54 +51,128 @@ px4::AppState VCDevExample::appState; using namespace device; -#define TESTDEV "/dev/vdevex" +#define TESTDEV "/dev/vdevtest" + +static bool g_exit = false; static int writer_main(int argc, char *argv[]) { - char buf[1] = { '1' }; + char buf[1]; - int fd = px4_open(TESTDEV, PX4_F_RDONLY); + int fd = px4_open(TESTDEV, PX4_F_WRONLY); if (fd < 0) { - PX4_INFO("--- Open failed %d %d", fd, px4_errno); + PX4_INFO("Writer: Open failed %d %d", fd, px4_errno); return -px4_errno; } int ret; int i=0; - while (i<3) { - // Wait for 3 seconds - PX4_INFO("--- Sleeping for 4 sec\n"); - ret = sleep(4); + while (!g_exit) { + // Wait for 2 seconds + PX4_INFO("Writer: Sleeping for 2 sec"); + ret = sleep(2); if (ret < 0) { - PX4_INFO("--- sleep failed %d %d\n", ret, errno); + PX4_INFO("Writer: sleep failed %d %d", ret, errno); return ret; } - PX4_INFO("--- writing to fd\n"); + buf[0] = 'A'+(char)(i % 26); + PX4_INFO("Writer: writing char '%c'", buf[0]); ret = px4_write(fd, buf, 1); ++i; } px4_close(fd); + PX4_INFO("Writer: stopped"); return ret; } +class PrivData { +public: + PrivData() : _read_offset(0) {} + ~PrivData() {} + + size_t _read_offset; +}; + class VCDevNode : public VDev { public: - VCDevNode() : VDev("vcdevtest", TESTDEV) {}; + VCDevNode() : + VDev("vcdevtest", TESTDEV), + _is_open_for_write(false), + _write_offset(0) {}; ~VCDevNode() {} + virtual int open(device::file_t *handlep); + virtual int close(device::file_t *handlep); virtual ssize_t write(device::file_t *handlep, const char *buffer, size_t buflen); + virtual ssize_t read(device::file_t *handlep, char *buffer, size_t buflen); +private: + bool _is_open_for_write; + size_t _write_offset; + char _buf[1000]; }; +int VCDevNode::open(device::file_t *handlep) +{ + // Only allow one writer + if (_is_open_for_write && (handlep->flags & PX4_F_WRONLY)) { + errno = EBUSY; + return -1; + } + int ret = VDev::open(handlep); + if (ret != 0) { + return ret; + } + handlep->priv = new PrivData; + + if (_is_open_for_write && (handlep->flags & PX4_F_WRONLY)) { + _is_open_for_write = true; + } + + return 0; +} + +int VCDevNode::close(device::file_t *handlep) +{ + delete (PrivData *)handlep->priv; + handlep->priv = nullptr; + VDev::close(handlep); + + // Enable a new writer of the device is re-opened for write + if ((handlep->flags & PX4_F_WRONLY) && _is_open_for_write) { + _is_open_for_write = false; + } + return 0; +} + ssize_t VCDevNode::write(device::file_t *handlep, const char *buffer, size_t buflen) { + for (size_t i=0; ipriv; + ssize_t chars_read = 0; + PX4_INFO("read %zu write %zu", p->_read_offset, _write_offset); + for (size_t i=0; i_read_offset < _write_offset); i++) { + buffer[i] = _buf[p->_read_offset]; + p->_read_offset++; + chars_read++; + } + + return chars_read; +} + VCDevExample::~VCDevExample() { if (_node) { delete _node; @@ -119,11 +193,57 @@ static int test_pub_block(int fd, unsigned long blocked) PX4_INFO("ioctl PX4_DEVIOCGPUBBLOCK failed %d %d", ret, px4_errno); return -px4_errno; } - PX4_INFO("pub_blocked = %d %s\n", ret, (unsigned long)ret == blocked ? "PASS" : "FAIL"); + PX4_INFO("pub_blocked = %d %s", ret, (unsigned long)ret == blocked ? "PASS" : "FAIL"); return 0; } +int VCDevExample::do_poll(int fd, int timeout, int iterations, int delayms_after_poll) +{ + int pollret, readret; + int loop_count = 0; + char readbuf[10]; + px4_pollfd_struct_t fds[1]; + + fds[0].fd = fd; + fds[0].events = POLLIN; + fds[0].revents = 0; + + bool mustblock = (timeout < 0); + + // Test indefinte blocking poll + while ((!appState.exitRequested()) && (loop_count < iterations)) { + pollret = px4_poll(fds, 1, timeout); + if (pollret < 0) { + PX4_ERR("Reader: px4_poll failed %d %d FAIL", pollret, px4_errno); + goto fail; + } + PX4_INFO("Reader: px4_poll returned %d", pollret); + if (pollret) { + readret = px4_read(fd, readbuf, 10); + if (readret != 1) { + if (mustblock) { + PX4_ERR("Reader: read failed %d FAIL", readret); + goto fail; + } + else { + PX4_INFO("Reader: read failed %d FAIL", readret); + } + } + else { + readbuf[readret] = '\0'; + PX4_INFO("Reader: px4_poll returned %d, read '%s' PASS", pollret, readbuf); + } + } + if (delayms_after_poll) { + usleep(delayms_after_poll*1000); + } + loop_count++; + } + return 0; +fail: + return 1; +} int VCDevExample::main() { appState.setRunning(true); @@ -131,12 +251,12 @@ int VCDevExample::main() _node = new VCDevNode(); if (_node == 0) { - PX4_INFO("Failed to allocate VCDevNode\n"); + PX4_INFO("Failed to allocate VCDevNode"); return -ENOMEM; } if (_node->init() != PX4_OK) { - PX4_INFO("Failed to init VCDevNode\n"); + PX4_INFO("Failed to init VCDevNode"); return 1; } @@ -153,7 +273,7 @@ int VCDevExample::main() PX4_INFO("ioctl DIOC_GETPRIV failed %d %d", ret, px4_errno); return -px4_errno; } - PX4_INFO("priv data = %p %s\n", p, p == (void *)_node ? "PASS" : "FAIL"); + PX4_INFO("priv data = %p %s", p, p == (void *)_node ? "PASS" : "FAIL"); ret = test_pub_block(fd, 1); if (ret < 0) @@ -162,10 +282,8 @@ int VCDevExample::main() if (ret < 0) return ret; - int i=0; - px4_pollfd_struct_t fds[1]; - // Start a task that will write something in 3 seconds + // Start a task that will write something in 4 seconds (void)px4_task_spawn_cmd("writer", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 6, @@ -173,42 +291,33 @@ int VCDevExample::main() writer_main, (char* const*)NULL); - while (!appState.exitRequested() && i<13) { - PX4_INFO("=====================\n"); - PX4_INFO("==== sleeping 2 sec ====\n"); - sleep(2); + ret = 0; - fds[0].fd = fd; - fds[0].events = POLLIN; - fds[0].revents = 0; - - PX4_INFO("==== Calling Poll\n"); - ret = px4_poll(fds, 1, 1000); - PX4_INFO("==== Done poll\n"); - if (ret < 0) { - PX4_INFO("==== poll failed %d %d\n", ret, px4_errno); - px4_close(fd); - } - else if (i > 0) { - if (ret == 0) { - PX4_INFO("==== Nothing to read - PASS\n"); - } - else { - PX4_INFO("==== poll returned %d\n", ret); - } - } - else if (i == 0) { - if (ret == 1) { - PX4_INFO("==== %d to read - %s\n", ret, fds[0].revents & POLLIN ? "PASS" : "FAIL"); - } - else { - PX4_INFO("==== %d to read - FAIL\n", ret); - } - - } - ++i; + PX4_INFO("TEST: BLOCKING POLL ---------------"); + if (do_poll(fd, -1, 3, 0)) { + ret = 1; + goto fail2; + } + PX4_INFO("TEST: ZERO TIMEOUT POLL -----------"); + if(do_poll(fd, 0, 3, 0)) { + ret = 1; + goto fail2; + } + PX4_INFO("TEST: 100ms TIMEOUT POLL -----------"); + if(do_poll(fd, 0, 30, 100)) { + ret = 1; + goto fail2; + } + PX4_INFO("TEST: 1 SEC TIMOUT POLL ------------"); + if(do_poll(fd, 1000, 3, 0)) { + ret = 1; + goto fail2; } - px4_close(fd); - return 0; +fail2: + g_exit = true; + px4_close(fd); + PX4_INFO("TEST: waiting for writer to stop"); + sleep(3); + return ret; } diff --git a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.h b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.h index 4898210dff..10befc795c 100644 --- a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.h +++ b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.h @@ -54,5 +54,7 @@ public: static px4::AppState appState; /* track requests to terminate app */ private: + int do_poll(int fd, int timeout, int iterations, int delayms_after_poll); + VCDevNode *_node; }; diff --git a/src/platforms/posix/work_queue/hrt_thread.c b/src/platforms/posix/work_queue/hrt_thread.c index 49a0a17a6a..9ba01d9eee 100644 --- a/src/platforms/posix/work_queue/hrt_thread.c +++ b/src/platforms/posix/work_queue/hrt_thread.c @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -73,6 +74,26 @@ sem_t _hrt_work_lock; ****************************************************************************/ static void hrt_work_process(void); +static void _sighandler(int sig_num); + +/**************************************************************************** + * Name: _sighandler + * + * Description: + * This is the handler for the signal to wake the queue processing thread + * + * Input parameters: + * sig_num - the received signal + * + * Returned Value: + * None + * + ****************************************************************************/ +static void _sighandler(int sig_num) +{ + PX4_DEBUG("RECEIVED SIGNAL %d", sig_num); +} + /**************************************************************************** * Name: work_process * @@ -251,5 +272,11 @@ void hrt_work_queue_init(void) work_hrtthread, (char *const *)NULL); + +#ifdef __PX4_QURT + signal(SIGALRM, _sighandler); +#else + signal(SIGCONT, _sighandler); +#endif } diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index d071893b0c..0c1063883b 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -63,7 +63,7 @@ static inline void do_nothing(int level, ...) #define PX4_ERR(...) ROS_WARN(__VA_ARGS__) #define PX4_WARN(...) ROS_WARN(__VA_ARGS__) #define PX4_INFO(...) ROS_WARN(__VA_ARGS__) -#define PX4_DEBUG(...) +#define PX4_DEBUG(...) #elif defined(__PX4_QURT) #include "qurt_log.h" @@ -127,6 +127,7 @@ __EXPORT extern uint64_t hrt_absolute_time(void); __EXPORT extern const char *__px4_log_level_str[5]; __EXPORT extern int __px4_log_level_current; +__END_DECLS // __px4_log_level_current will be initialized to PX4_LOG_LEVEL_AT_RUN_TIME #define PX4_LOG_LEVEL_AT_RUN_TIME _PX4_LOG_LEVEL_WARN @@ -136,13 +137,13 @@ __EXPORT extern int __px4_log_level_current; * * To write to a specific stream for each message type, open the streams and * set __px4__log_startline to something like: - * if (level <= __px4_log_level_current) printf(_px4_fd[level], + * if (level <= __px4_log_level_current) printf(_px4_fd[level], * * Additional behavior can be added using "{\" for __px4__log_startline and * "}" for __px4__log_endline and any other required setup or teardown steps ****************************************************************************/ -#define __px4__log_startcond(cond) if (cond) printf( -#define __px4__log_startline(level) if (level <= __px4_log_level_current) printf( +#define __px4__log_printcond(cond, ...) if (cond) printf(__VA_ARGS__) +#define __px4__log_printline(level, ...) if (level <= __px4_log_level_current) printf(__VA_ARGS__) #define __px4__log_timestamp_fmt "%-10" PRIu64 " " #define __px4__log_timestamp_arg ,hrt_absolute_time() @@ -171,28 +172,12 @@ __EXPORT extern int __px4_log_level_current; * if the first arg/condition is true. ****************************************************************************/ #define __px4_log_named_cond(name, cond, FMT, ...) \ - __px4__log_startcond(cond)\ + __px4__log_printcond(cond,\ "%s " \ FMT\ __px4__log_end_fmt \ ,name, ##__VA_ARGS__\ - __px4__log_endline - -/**************************************************************************** - * __px4_log_named_cond: - * Convert a message in the form: - * PX4_LOG_COND(__dbg_enabled, "val is %d", val); - * to - * printf("%-5s val is %d\n", "LOG", val); - * if the first arg/condition is true. - ****************************************************************************/ -#define __px4_log_named_cond(name, cond, FMT, ...) \ - __px4__log_startcond(cond)\ - "%s " \ - FMT\ - __px4__log_end_fmt \ - ,name, ##__VA_ARGS__\ - __px4__log_endline + ) /**************************************************************************** * __px4_log: @@ -202,12 +187,12 @@ __EXPORT extern int __px4_log_level_current; * printf("%-5s val is %d\n", __px4_log_level_str[3], val); ****************************************************************************/ #define __px4_log(level, FMT, ...) \ - __px4__log_startline(level)\ + __px4__log_printline(level,\ __px4__log_level_fmt \ FMT\ __px4__log_end_fmt \ __px4__log_level_arg(level), ##__VA_ARGS__\ - __px4__log_endline + ) /**************************************************************************** * __px4_log_timestamp: @@ -218,7 +203,7 @@ __EXPORT extern int __px4_log_level_current; * hrt_absolute_time(), val); ****************************************************************************/ #define __px4_log_timestamp(level, FMT, ...) \ - __px4__log_startline(level)\ + __px4__log_printline(level,\ __px4__log_level_fmt\ __px4__log_timestamp_fmt\ FMT\ @@ -226,7 +211,7 @@ __EXPORT extern int __px4_log_level_current; __px4__log_level_arg(level)\ __px4__log_timestamp_arg\ , ##__VA_ARGS__\ - __px4__log_endline + ) /**************************************************************************** * __px4_log_timestamp_thread: @@ -237,7 +222,7 @@ __EXPORT extern int __px4_log_level_current; * hrt_absolute_time(), pthread_self(), val); ****************************************************************************/ #define __px4_log_timestamp_thread(level, FMT, ...) \ - __px4__log_startline(level)\ + __px4__log_printline(level,\ __px4__log_level_fmt\ __px4__log_timestamp_fmt\ __px4__log_thread_fmt\ @@ -247,18 +232,18 @@ __EXPORT extern int __px4_log_level_current; __px4__log_timestamp_arg\ __px4__log_thread_arg\ , ##__VA_ARGS__\ - __px4__log_endline + ) /**************************************************************************** * __px4_log_file_and_line: * Convert a message in the form: * PX4_WARN("val is %d", val); * to - * printf("%-5s val is %d (file %s line %u)\n", + * printf("%-5s val is %d (file %s line %u)\n", * __px4_log_level_str[3], val, __FILE__, __LINE__); ****************************************************************************/ #define __px4_log_file_and_line(level, FMT, ...) \ - __px4__log_startline(level)\ + __px4__log_printline(level,\ __px4__log_level_fmt\ __px4__log_timestamp_fmt\ FMT\ @@ -268,19 +253,19 @@ __EXPORT extern int __px4_log_level_current; __px4__log_timestamp_arg\ , ##__VA_ARGS__\ __px4__log_file_and_line_arg\ - __px4__log_endline + ) /**************************************************************************** * __px4_log_timestamp_file_and_line: * Convert a message in the form: * PX4_WARN("val is %d", val); * to - * printf("%-5s %-10lu val is %d (file %s line %u)\n", + * printf("%-5s %-10lu val is %d (file %s line %u)\n", * __px4_log_level_str[3], hrt_absolute_time(), * val, __FILE__, __LINE__); ****************************************************************************/ #define __px4_log_timestamp_file_and_line(level, FMT, ...) \ - __px4__log_startline(level)\ + __px4__log_printline(level,\ __px4__log_level_fmt\ __px4__log_timestamp_fmt\ FMT\ @@ -290,19 +275,19 @@ __EXPORT extern int __px4_log_level_current; __px4__log_timestamp_arg\ , ##__VA_ARGS__\ __px4__log_file_and_line_arg\ - __px4__log_endline + ) /**************************************************************************** * __px4_log_thread_file_and_line: * Convert a message in the form: * PX4_WARN("val is %d", val); * to - * printf("%-5s %#X val is %d (file %s line %u)\n", - * __px4_log_level_str[3], pthread_self(), + * printf("%-5s %#X val is %d (file %s line %u)\n", + * __px4_log_level_str[3], pthread_self(), * val, __FILE__, __LINE__); ****************************************************************************/ #define __px4_log_thread_file_and_line(level, FMT, ...) \ - __px4__log_startline(level)\ + __px4__log_printline(level,\ __px4__log_level_fmt\ __px4__log_thread_fmt\ FMT\ @@ -312,19 +297,19 @@ __EXPORT extern int __px4_log_level_current; __px4__log_thread_arg\ , ##__VA_ARGS__\ __px4__log_file_and_line_arg\ - __px4__log_endline + ) /**************************************************************************** * __px4_log_timestamp_thread_file_and_line: * Convert a message in the form: * PX4_WARN("val is %d", val); * to - * printf("%-5s %-10lu %#X val is %d (file %s line %u)\n", - * __px4_log_level_str[3], hrt_absolute_time(), + * printf("%-5s %-10lu %#X val is %d (file %s line %u)\n", + * __px4_log_level_str[3], hrt_absolute_time(), * pthread_self(), val, __FILE__, __LINE__); ****************************************************************************/ #define __px4_log_timestamp_thread_file_and_line(level, FMT, ...) \ - __px4__log_startline(level)\ + __px4__log_printline(level,\ __px4__log_level_fmt\ __px4__log_timestamp_fmt\ __px4__log_thread_fmt\ @@ -336,7 +321,7 @@ __EXPORT extern int __px4_log_level_current; __px4__log_thread_arg\ , ##__VA_ARGS__\ __px4__log_file_and_line_arg\ - __px4__log_endline + ) /**************************************************************************** @@ -389,5 +374,4 @@ __EXPORT extern int __px4_log_level_current; #endif #define PX4_LOG_NAMED(name, FMT, ...) __px4_log_named_cond(name, true, FMT, ##__VA_ARGS__) #define PX4_LOG_NAMED_COND(name, cond, FMT, ...) __px4_log_named_cond(name, cond, FMT, ##__VA_ARGS__) -__END_DECLS #endif diff --git a/src/platforms/px4_tasks.h b/src/platforms/px4_tasks.h index 941c1ceb37..3d36a65cfd 100644 --- a/src/platforms/px4_tasks.h +++ b/src/platforms/px4_tasks.h @@ -67,11 +67,11 @@ typedef int px4_task_t; #ifdef __PX4_LINUX #define SCHED_PRIORITY_MAX sched_get_priority_max(SCHED_FIFO) #define SCHED_PRIORITY_MIN sched_get_priority_min(SCHED_FIFO) -#define SCHED_PRIORITY_DEFAULT sched_get_priority_max(SCHED_FIFO) +#define SCHED_PRIORITY_DEFAULT (((sched_get_priority_max(SCHED_FIFO) - sched_get_priority_min(SCHED_FIFO)) / 2) + sched_get_priority_min(SCHED_FIFO)) #elif defined(__PX4_DARWIN) #define SCHED_PRIORITY_MAX sched_get_priority_max(SCHED_FIFO) #define SCHED_PRIORITY_MIN sched_get_priority_min(SCHED_FIFO) -#define SCHED_PRIORITY_DEFAULT sched_get_priority_max(SCHED_FIFO) +#define SCHED_PRIORITY_DEFAULT (((sched_get_priority_max(SCHED_FIFO) - sched_get_priority_min(SCHED_FIFO)) / 2) + sched_get_priority_min(SCHED_FIFO)) #elif defined(__PX4_QURT) #define SCHED_PRIORITY_MAX 0 #define SCHED_PRIORITY_MIN 0 diff --git a/src/platforms/qurt/eagle_px4_firmware_patch.patch b/src/platforms/qurt/eagle_px4_firmware_patch.patch deleted file mode 100644 index 41ac284b51..0000000000 --- a/src/platforms/qurt/eagle_px4_firmware_patch.patch +++ /dev/null @@ -1,190 +0,0 @@ -diff --git a/src/include/mavlink/mavlink_log.h b/src/include/mavlink/mavlink_log.h -index 4ba2f98..a7b664e 100644 ---- a/src/include/mavlink/mavlink_log.h -+++ b/src/include/mavlink/mavlink_log.h -@@ -106,10 +106,11 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...); - * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); - * @param _text The text to log; - */ --#define mavlink_and_console_log_emergency(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__); \ -- fprintf(stderr, "telem> "); \ -+#define mavlink_and_console_log_emergency(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__); -+/* fprintf(stderr, "telem> "); \ - fprintf(stderr, _text, ##__VA_ARGS__); \ - fprintf(stderr, "\n"); -+*/ - - /** - * Send a mavlink critical message and print to console. -@@ -117,10 +118,11 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...); - * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); - * @param _text The text to log; - */ --#define mavlink_and_console_log_critical(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__); \ -- fprintf(stderr, "telem> "); \ -+#define mavlink_and_console_log_critical(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__) -+/* fprintf(stderr, "telem> "); \ - fprintf(stderr, _text, ##__VA_ARGS__); \ - fprintf(stderr, "\n"); -+*/ - - /** - * Send a mavlink emergency message and print to console. -@@ -128,11 +130,11 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...); - * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); - * @param _text The text to log; - */ --#define mavlink_and_console_log_info(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__); \ -- fprintf(stderr, "telem> "); \ -+#define mavlink_and_console_log_info(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__); -+/* fprintf(stderr, "telem> "); \ - fprintf(stderr, _text, ##__VA_ARGS__); \ - fprintf(stderr, "\n"); -- -+*/ - struct mavlink_logmessage { - char text[MAVLINK_LOG_MAXLEN + 1]; - unsigned char severity; -diff --git a/src/lib/eigen b/src/lib/eigen ---- a/src/lib/eigen -+++ b/src/lib/eigen -@@ -1 +1 @@ --Subproject commit e7850ed81f9c469e02df496ef09ae32ec0379b71 -+Subproject commit e7850ed81f9c469e02df496ef09ae32ec0379b71-dirty -diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp -index bbf5f8e..50c7d8b 100644 ---- a/src/modules/commander/PreflightCheck.cpp -+++ b/src/modules/commander/PreflightCheck.cpp -@@ -378,6 +378,14 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro - } - } - -+ -+#ifdef __PX4_QURT -+ // WARNING: Preflight checks are important and should be added back when -+ // all the sensors are supported -+ PX4_WARN("WARNING: Preflight checks PASS always."); -+ failed = false; -+#endif -+ - /* Report status */ - return !failed; - } -diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp -index 971e086..c165322 100644 ---- a/src/modules/commander/commander.cpp -+++ b/src/modules/commander/commander.cpp -@@ -1836,8 +1836,13 @@ int commander_thread_main(int argc, char *argv[]) - } - - /* RC input check */ -+#ifndef __PX4_QURT - if (!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF) && !status.rc_input_blocked && sp_man.timestamp != 0 && -- hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) { -+ hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) { -+#else -+ // HACK: remove old data check due to timestamp issue in QURT -+ if (!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF) && !status.rc_input_blocked && sp_man.timestamp != 0 ) { -+#endif - /* handle the case where RC signal was regained */ - if (!status.rc_signal_found_once) { - status.rc_signal_found_once = true; -@@ -2526,7 +2531,7 @@ set_control_mode() - /* set vehicle_control_mode according to set_navigation_state */ - control_mode.flag_armed = armed.armed; - control_mode.flag_external_manual_override_ok = (!status.is_rotary_wing && !status.is_vtol); -- control_mode.flag_system_hil_enabled = status.hil_state == vehicle_status_s::HIL_STATE_ON; -+ //control_mode.flag_system_hil_enabled = status.hil_state == vehicle_status_s::HIL_STATE_ON; - control_mode.flag_control_offboard_enabled = false; - - switch (status.nav_state) { -diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c -index 892e50f..7ac6776 100644 ---- a/src/modules/position_estimator_inav/position_estimator_inav_main.c -+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c -@@ -384,7 +384,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) - }; - - /* wait for initial baro value */ -+#ifdef __PX4_QURT -+ // WARNING skipping -+ PX4_WARN("Hacked to skip baro initialization for testing."); -+ bool wait_baro = false; -+#else - bool wait_baro = true; -+#endif - - thread_running = true; - -diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp -index 3fa4458..0ef1883 100644 ---- a/src/modules/sensors/sensors.cpp -+++ b/src/modules/sensors/sensors.cpp -@@ -1099,7 +1099,12 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) - raw.gyro_raw[1] = gyro_report.y_raw; - raw.gyro_raw[2] = gyro_report.z_raw; - -+#ifdef __PX4_QURT -+// Temporary fix required due to lack of time sync between apps and dsp cores -+ raw.timestamp = hrt_absolute_time(); -+#else - raw.timestamp = gyro_report.timestamp; -+#endif - raw.gyro_errcount = gyro_report.error_count; - raw.gyro_temp = gyro_report.temperature; - } -@@ -2068,6 +2073,7 @@ Sensors::task_main() - - /* start individual sensors */ - int ret = 0; -+#if 0 - do { /* create a scope to handle exit with break */ - ret = accel_init(); - if (ret) break; -@@ -2091,7 +2097,7 @@ Sensors::task_main() - } - return; - } -- -+#endif - /* - * do subscriptions - */ -diff --git a/src/systemcmds/mixer/mixer.cpp b/src/systemcmds/mixer/mixer.cpp -index 83a9378..ec86faf 100644 ---- a/src/systemcmds/mixer/mixer.cpp -+++ b/src/systemcmds/mixer/mixer.cpp -@@ -118,6 +118,8 @@ load(const char *devname, const char *fname) - return 1; - } - -+#ifndef __PX4_QURT -+ - if (load_mixer_file(fname, &buf[0], sizeof(buf)) < 0) { - warnx("can't load mixer: %s", fname); - return 1; -@@ -125,6 +127,24 @@ load(const char *devname, const char *fname) - - /* XXX pass the buffer to the device */ - int ret = px4_ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf); -+#else -+ char newbuf[] = -+ "R: 4x 10000 10000 10000 0\n" -+ "M: 1\n" -+ "O: 10000 10000 0 -10000 10000\n" -+ "S: 0 4 10000 10000 0 -10000 10000\n" -+ "M: 1\n" -+ "O: 10000 10000 0 -10000 10000\n" -+ "S: 0 5 10000 10000 0 -10000 10000\n" -+ "M: 1\n" -+ "O: 10000 10000 0 -10000 10000\n" -+ "S: 0 6 10000 10000 0 -10000 10000\n" -+ "M: 1\n" -+ "O: 10000 10000 0 -10000 10000\n" -+ "S: 0 7 10000 10000 0 -10000 10000\n"; -+ /* XXX pass the buffer to the device */ -+ int ret = px4_ioctl(dev, MIXERIOCLOADBUF, (unsigned long)newbuf); -+#endif - - if (ret < 0) { - warnx("error loading mixers from %s", fname); diff --git a/src/platforms/qurt/px4_layer/commands_adsp.c b/src/platforms/qurt/px4_layer/commands_adsp.c new file mode 100644 index 0000000000..0e6ee8402c --- /dev/null +++ b/src/platforms/qurt/px4_layer/commands_adsp.c @@ -0,0 +1,175 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file commands_muorb_test.c + * Commands to run for the "qurt_muorb_test" config + * + * @author Mark Charlebois + */ + +const char *get_commands() +{ + + static const char *commands = + "uorb start\n" + "param set CAL_GYRO0_ID 2293760\n" + "param set CAL_ACC0_ID 1310720\n" + "param set CAL_ACC1_ID 1376256\n" + "param set CAL_MAG0_ID 196608\n" + "commander start\n" + "param set RC_RECEIVER_TYPE 1\n" + "rc_receiver start -D /dev/tty-1\n" + "attitude_estimator_q start\n" + "position_estimator_inav start\n" + //"ekf_att_pos_estimator start\n" + "mc_pos_control start\n" + "mc_att_control start\n" + "sleep 1\n" + + // Channel mapping for Spektrum DX8 + "param set RC_MAP_THROTTLE 1\n" + "param set RC_MAP_ROLL 2\n" + "param set RC_MAP_PITCH 3\n" + "param set RC_MAP_YAW 4\n" + "param set RC_MAP_MODE_SW 5\n" + "param set RC_MAP_POSCTL_SW 6\n" + + // RC calibration for Spektrum DX8 + "param set RC1_MAX 852\n" + "param set RC1_MIN 171\n" + "param set RC1_TRIM 171\n" + "param set RC1_REV 1\n" + "param set RC2_MAX 852\n" + "param set RC2_MIN 171\n" + "param set RC2_TRIM 512\n" + "param set RC2_REV -1\n" + "param set RC3_MAX 852\n" + "param set RC3_MIN 171\n" + "param set RC3_TRIM 512\n" + "param set RC3_REV 1\n" + "param set RC4_MAX 852\n" + "param set RC4_MIN 171\n" + "param set RC4_TRIM 514\n" + "param set RC4_REV -1\n" + "param set RC5_MAX 852\n" + "param set RC5_MIN 171\n" + "param set RC5_TRIM 512\n" + "param set RC5_REV 1\n" + "param set RC6_MAX 852\n" + "param set RC6_MIN 171\n" + "param set RC6_TRIM 171\n" + "param set RC6_REV 1\n" + + // // RC calibration for DX6i + // "param set RC1_MAX 2015\n" + // "param set RC1_MIN 996\n" + // "param set RC1_TRIM 1502\n" + // "param set RC1_REV -1\n" + // "param set RC2_MAX 2016 \n" + // "param set RC2_MIN 995\n" + // "param set RC2_TRIM 1500\n" + // "param set RC3_MAX 2003\n" + // "param set RC3_MIN 992\n" + // "param set RC3_TRIM 992\n" + // "param set RC4_MAX 2011\n" + // "param set RC4_MIN 997\n" + // "param set RC4_TRIM 1504\n" + // "param set RC4_REV -1\n" + // "param set RC6_MAX 2016\n" + // "param set RC6_MIN 992\n" + // "param set RC6_TRIM 1504\n" + // "param set RC_CHAN_CNT 8\n" + // "param set RC_MAP_MODE_SW 5\n" + // "param set RC_MAP_POSCTL_SW 7\n" + // "param set RC_MAP_RETURN_SW 8\n" + + "sensors start\n" + "param set MC_YAW_P 7.0\n" + "param set MC_YAWRATE_P 0.1125\n" + "param set MC_YAWRATE_I 0.0\n" + "param set MC_YAWRATE_D 0\n" + "param set MC_PITCH_P 6.0\n" + "param set MC_PITCHRATE_P 0.125\n" + "param set MC_PITCHRATE_I 0.0\n" + "param set MC_PITCHRATE_D 0.0\n" + "param set MC_ROLL_P 6.0\n" + "param set MC_ROLLRATE_P 0.1125\n" + "param set MC_ROLLRATE_I 0.0\n" + "param set MC_ROLLRATE_D 0.0\n" + "param set ATT_W_MAG 0.00\n" + "param set PE_MAG_NOISE 1.0f\n" // ekf_att_pos only + "param set SENS_BOARD_ROT 6\n" + + "param set CAL_GYRO0_XOFF 0.0\n" + "param set CAL_GYRO0_YOFF 0.0\n" + "param set CAL_GYRO0_ZOFF 0.0\n" + "param set CAL_GYRO0_XSCALE 1.000000\n" + "param set CAL_GYRO0_YSCALE 1.000000\n" + "param set CAL_GYRO0_ZSCALE 1.000000\n" + "param set CAL_ACC0_XOFF 0.0\n" + "param set CAL_ACC0_YOFF 0.0\n" + "param set CAL_ACC0_ZOFF 0.0\n" + "param set CAL_ACC0_XSCALE 1.0\n" + "param set CAL_ACC0_YSCALE 1.0\n" + "param set CAL_ACC0_ZSCALE 1.0\n" + // "param set CAL_ACC0_XOFF 0.064189\n" + // "param set CAL_ACC0_YOFF 0.153990\n" + // "param set CAL_ACC0_ZOFF -0.000567\n" + "param set MPU_GYRO_LPF_ENUM 4\n" + "param set MPU_ACC_LPF_ENUM 4\n" + "param set MPU_SAMPLE_RATE_ENUM 2\n" + "sleep 1\n" + "mpu9x50 start -D /dev/spi-1\n" + "uart_esc start -D /dev/tty-2\n" + "csr_gps start -D /dev/tty-3\n" + "param set MAV_TYPE 2\n" + "list_devices\n" + "list_files\n" + "list_tasks\n" + "list_topics\n" + + ; + + return commands; + +} + +/* +simulator numbers + "param set MC_YAW_P 1.5\n" + "param set MC_PITCH_P 3.0\n" + "param set MC_ROLL_P 3.0\n" + "param set MC_YAWRATE_P 0.2\n" + "param set MC_PITCHRATE_P 0.03\n" + "param set MC_ROLLRATE_P 0.03\n" +*/ diff --git a/src/platforms/qurt/px4_layer/commands_hil.c b/src/platforms/qurt/px4_layer/commands_hil.c index 3f6c23b732..b338aeb7ed 100644 --- a/src/platforms/qurt/px4_layer/commands_hil.c +++ b/src/platforms/qurt/px4_layer/commands_hil.c @@ -56,7 +56,7 @@ const char *get_commands() "mc_pos_control start\n" "mc_att_control start\n" "sleep 1\n" - "hil mode_pwm\n" + "pwm_out_sim mode_pwm\n" "param set RC1_MAX 2015\n" "param set RC1_MIN 996\n" "param set RC1_TRIM 1502\n" diff --git a/src/platforms/qurt/px4_layer/main.cpp b/src/platforms/qurt/px4_layer/main.cpp index f4a538a13a..45a6e4cec8 100644 --- a/src/platforms/qurt/px4_layer/main.cpp +++ b/src/platforms/qurt/px4_layer/main.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** * @file main.cpp - * Basic shell to execute builtin "apps" + * Basic shell to execute builtin "apps" * * @author Mark Charlebois */ @@ -56,6 +56,8 @@ static px4_task_t g_dspal_task = -1; __BEGIN_DECLS // The commands to run are specified in a target file: commands_.c extern const char *get_commands(void); +// Enable external library hook +void qurt_external_hook(void) __attribute__((weak)); __END_DECLS static void run_cmd(map &apps, const vector &appargs) { @@ -108,7 +110,7 @@ static void process_commands(map &apps, const char *cmds) // Eat leading whitespace eat_whitespace(b, i); - + for(;;) { // End of command line @@ -156,15 +158,17 @@ __END_DECLS int dspal_entry( int argc, char* argv[] ) { - //const char *argv[2] = { "dspal_client", NULL }; - //int argc = 1; - PX4_INFO("In main\n"); map apps; init_app_map(apps); px4::init_once(); px4::init(argc, (char **)argv, "mainapp"); process_commands(apps, get_commands()); + usleep( 1000000 ); // give time for all commands to execute before starting external function + if(qurt_external_hook) + { + qurt_external_hook(); + } for( ;; ){ usleep( 1000000 ); } diff --git a/src/platforms/qurt/px4_layer/module.mk b/src/platforms/qurt/px4_layer/module.mk index a10df59d36..47902057cc 100644 --- a/src/platforms/qurt/px4_layer/module.mk +++ b/src/platforms/qurt/px4_layer/module.mk @@ -58,6 +58,9 @@ endif ifeq ($(CONFIG),qurt_hil) SRCS += commands_hil.c endif +ifeq ($(CONFIG),qurt_adsp) +SRCS += commands_adsp.c +endif MAXOPTIMIZATION = -Os diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp index c92dd0843d..567acc200e 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.cpp +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -46,7 +46,7 @@ using namespace px4; -Mavlink::Mavlink() : +Mavlink::Mavlink(std::string mavlink_fcu_url) : _n(), _v_att_sub(_n.subscribe("vehicle_attitude", 1, &Mavlink::VehicleAttitudeCallback, this)), _v_local_pos_sub(_n.subscribe("vehicle_local_position", 1, &Mavlink::VehicleLocalPositionCallback, this)), @@ -55,7 +55,7 @@ Mavlink::Mavlink() : _offboard_control_mode_pub(_n.advertise("offboard_control_mode", 1)), _force_sp_pub(_n.advertise("vehicle_force_setpoint", 1)) { - _link = mavconn::MAVConnInterface::open_url("udp://localhost:14565@localhost:14560"); + _link = mavconn::MAVConnInterface::open_url(mavlink_fcu_url); _link->message_received.connect(boost::bind(&Mavlink::handle_msg, this, _1, _2, _3)); _att_sp = {}; _offboard_control_mode = {}; @@ -64,7 +64,10 @@ Mavlink::Mavlink() : int main(int argc, char **argv) { ros::init(argc, argv, "mavlink"); - Mavlink m; + ros::NodeHandle privateNh("~"); + std::string mavlink_fcu_url; + privateNh.param("mavlink_fcu_url", mavlink_fcu_url, std::string("udp://localhost:14565@localhost:14560")); + Mavlink m(mavlink_fcu_url); ros::spin(); return 0; } diff --git a/src/platforms/ros/nodes/mavlink/mavlink.h b/src/platforms/ros/nodes/mavlink/mavlink.h index 8b7d08d242..af9e019993 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.h +++ b/src/platforms/ros/nodes/mavlink/mavlink.h @@ -55,7 +55,7 @@ namespace px4 class Mavlink { public: - Mavlink(); + Mavlink(std::string mavlink_fcu_url); ~Mavlink() {} diff --git a/src/systemcmds/bl_update/bl_update.c b/src/systemcmds/bl_update/bl_update.c index 082865a866..24271c3315 100644 --- a/src/systemcmds/bl_update/bl_update.c +++ b/src/systemcmds/bl_update/bl_update.c @@ -61,33 +61,40 @@ static void setopt(void); int bl_update_main(int argc, char *argv[]) { - if (argc != 2) + if (argc != 2) { errx(1, "missing firmware filename or command"); + } - if (!strcmp(argv[1], "setopt")) + if (!strcmp(argv[1], "setopt")) { setopt(); + } int fd = open(argv[1], O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "open %s", argv[1]); + } struct stat s; - if (stat(argv[1], &s) != 0) + if (stat(argv[1], &s) != 0) { err(1, "stat %s", argv[1]); + } /* sanity-check file size */ - if (s.st_size > BL_FILE_SIZE_LIMIT) + if (s.st_size > BL_FILE_SIZE_LIMIT) { errx(1, "%s: file too large (limit: %u, actual: %d)", argv[1], BL_FILE_SIZE_LIMIT, s.st_size); + } uint8_t *buf = malloc(s.st_size); - if (buf == NULL) + if (buf == NULL) { errx(1, "failed to allocate %u bytes for firmware buffer", s.st_size); + } - if (read(fd, buf, s.st_size) != s.st_size) + if (read(fd, buf, s.st_size) != s.st_size) { err(1, "firmware read error"); + } close(fd); @@ -193,24 +200,27 @@ setopt(void) const uint16_t opt_mask = (3 << 2); /* BOR_LEV bitmask */ const uint16_t opt_bits = (0 << 2); /* BOR = 0, setting for 2.7-3.6V operation */ - if ((*optcr & opt_mask) == opt_bits) + if ((*optcr & opt_mask) == opt_bits) { errx(0, "option bits are already set as required"); + } /* unlock the control register */ volatile uint32_t *optkeyr = (volatile uint32_t *)0x40023c08; *optkeyr = 0x08192a3bU; *optkeyr = 0x4c5d6e7fU; - if (*optcr & 1) + if (*optcr & 1) { errx(1, "option control register unlock failed"); + } /* program the new option value */ *optcr = (*optcr & ~opt_mask) | opt_bits | (1 << 1); usleep(1000); - if ((*optcr & opt_mask) == opt_bits) + if ((*optcr & opt_mask) == opt_bits) { errx(0, "option bits set"); + } errx(1, "option bits setting failed; readback 0x%04x", *optcr); diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 210b4caa1f..362af65353 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -71,12 +71,15 @@ int config_main(int argc, char *argv[]) { if (argc >= 2) { - if (!strncmp(argv[1], "/dev/gyro",9)) { + if (!strncmp(argv[1], "/dev/gyro", 9)) { do_gyro(argc - 1, argv + 1); - } else if (!strncmp(argv[1], "/dev/accel",10)) { + + } else if (!strncmp(argv[1], "/dev/accel", 10)) { do_accel(argc - 1, argv + 1); - } else if (!strncmp(argv[1], "/dev/mag",8)) { + + } else if (!strncmp(argv[1], "/dev/mag", 8)) { do_mag(argc - 1, argv + 1); + } else { do_device(argc - 1, argv + 1); } @@ -109,16 +112,18 @@ do_device(int argc, char *argv[]) /* disable the device publications */ ret = ioctl(fd, DEVIOCSPUBBLOCK, 1); - if (ret) - errx(ret,"uORB publications could not be blocked"); + if (ret) { + errx(ret, "uORB publications could not be blocked"); + } } else if (argc == 2 && !strcmp(argv[1], "unblock")) { /* enable the device publications */ ret = ioctl(fd, DEVIOCSPUBBLOCK, 0); - if (ret) - errx(ret,"uORB publications could not be unblocked"); + if (ret) { + errx(ret, "uORB publications could not be unblocked"); + } } else { errx(1, "no valid command: %s", argv[1]); @@ -148,24 +153,27 @@ do_gyro(int argc, char *argv[]) /* set the gyro internal sampling rate up to at least i Hz */ ret = ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[2], NULL, 0)); - if (ret) - errx(ret,"sampling rate could not be set"); + if (ret) { + errx(ret, "sampling rate could not be set"); + } } else if (argc == 3 && !strcmp(argv[1], "rate")) { /* set the driver to poll at i Hz */ ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0)); - if (ret) - errx(ret,"pollrate could not be set"); + if (ret) { + errx(ret, "pollrate could not be set"); + } } else if (argc == 3 && !strcmp(argv[1], "range")) { /* set the range to i dps */ ret = ioctl(fd, GYROIOCSRANGE, strtoul(argv[2], NULL, 0)); - if (ret) - errx(ret,"range could not be set"); + if (ret) { + errx(ret, "range could not be set"); + } } else if (argc == 2 && !strcmp(argv[1], "check")) { ret = ioctl(fd, GYROIOCSELFTEST, 0); @@ -181,6 +189,7 @@ do_gyro(int argc, char *argv[]) warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_offset, (double)scale.y_offset, (double)scale.z_offset); warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_scale, (double)scale.y_scale, (double)scale.z_scale); + } else { warnx("gyro calibration and self test OK"); } @@ -192,12 +201,13 @@ do_gyro(int argc, char *argv[]) int srate = ioctl(fd, GYROIOCGSAMPLERATE, 0); int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); int range = ioctl(fd, GYROIOCGRANGE, 0); - int id = ioctl(fd, DEVIOCGDEVICEID,0); + int id = ioctl(fd, DEVIOCGDEVICEID, 0); int32_t calibration_id = 0; param_get(param_find("CAL_GYRO0_ID"), &(calibration_id)); - warnx("gyro: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d dps", id, calibration_id, srate, prate, range); + warnx("gyro: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d dps", + id, calibration_id, srate, prate, range); close(fd); } @@ -225,26 +235,29 @@ do_mag(int argc, char *argv[]) /* set the mag internal sampling rate up to at least i Hz */ ret = ioctl(fd, MAGIOCSSAMPLERATE, strtoul(argv[2], NULL, 0)); - if (ret) - errx(ret,"sampling rate could not be set"); + if (ret) { + errx(ret, "sampling rate could not be set"); + } } else if (argc == 3 && !strcmp(argv[1], "rate")) { /* set the driver to poll at i Hz */ ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0)); - if (ret) - errx(ret,"pollrate could not be set"); + if (ret) { + errx(ret, "pollrate could not be set"); + } } else if (argc == 3 && !strcmp(argv[1], "range")) { /* set the range to i G */ ret = ioctl(fd, MAGIOCSRANGE, strtoul(argv[2], NULL, 0)); - if (ret) - errx(ret,"range could not be set"); + if (ret) { + errx(ret, "range could not be set"); + } - } else if(argc == 2 && !strcmp(argv[1], "check")) { + } else if (argc == 2 && !strcmp(argv[1], "check")) { ret = ioctl(fd, MAGIOCSELFTEST, 0); if (ret) { @@ -258,6 +271,7 @@ do_mag(int argc, char *argv[]) warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_offset, (double)scale.y_offset, (double)scale.z_offset); warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_scale, (double)scale.y_scale, (double)scale.z_scale); + } else { warnx("mag calibration and self test OK"); } @@ -269,12 +283,13 @@ do_mag(int argc, char *argv[]) int srate = ioctl(fd, MAGIOCGSAMPLERATE, 0); int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); int range = ioctl(fd, MAGIOCGRANGE, 0); - int id = ioctl(fd, DEVIOCGDEVICEID,0); + int id = ioctl(fd, DEVIOCGDEVICEID, 0); int32_t calibration_id = 0; param_get(param_find("CAL_MAG0_ID"), &(calibration_id)); - warnx("mag: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", id, calibration_id, srate, prate, range); + warnx("mag: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", + id, calibration_id, srate, prate, range); close(fd); } @@ -302,26 +317,29 @@ do_accel(int argc, char *argv[]) /* set the accel internal sampling rate up to at least i Hz */ ret = ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[2], NULL, 0)); - if (ret) - errx(ret,"sampling rate could not be set"); + if (ret) { + errx(ret, "sampling rate could not be set"); + } } else if (argc == 3 && !strcmp(argv[1], "rate")) { /* set the driver to poll at i Hz */ ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0)); - if (ret) - errx(ret,"pollrate could not be set"); + if (ret) { + errx(ret, "pollrate could not be set"); + } } else if (argc == 3 && !strcmp(argv[1], "range")) { /* set the range to i G */ ret = ioctl(fd, ACCELIOCSRANGE, strtoul(argv[2], NULL, 0)); - if (ret) - errx(ret,"range could not be set"); + if (ret) { + errx(ret, "range could not be set"); + } - } else if(argc == 2 && !strcmp(argv[1], "check")) { + } else if (argc == 2 && !strcmp(argv[1], "check")) { ret = ioctl(fd, ACCELIOCSELFTEST, 0); if (ret) { @@ -335,23 +353,25 @@ do_accel(int argc, char *argv[]) warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_offset, (double)scale.y_offset, (double)scale.z_offset); warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_scale, (double)scale.y_scale, (double)scale.z_scale); + } else { warnx("accel calibration and self test OK"); } } else { - errx(1,"no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 4' to set measurement range to 4 G\n\t"); + errx(1, "no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 4' to set measurement range to 4 G\n\t"); } int srate = ioctl(fd, ACCELIOCGSAMPLERATE, 0); int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); int range = ioctl(fd, ACCELIOCGRANGE, 0); - int id = ioctl(fd, DEVIOCGDEVICEID,0); + int id = ioctl(fd, DEVIOCGDEVICEID, 0); int32_t calibration_id = 0; param_get(param_find("CAL_ACC0_ID"), &(calibration_id)); - warnx("accel: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G", id, calibration_id, srate, prate, range); + warnx("accel: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G", + id, calibration_id, srate, prate, range); close(fd); } diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c index 8b432f1638..86939ff686 100644 --- a/src/systemcmds/esc_calib/esc_calib.c +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -71,8 +71,9 @@ __EXPORT int esc_calib_main(int argc, char *argv[]); static void usage(const char *reason) { - if (reason != NULL) + if (reason != NULL) { PX4_ERR("%s", reason); + } PX4_ERR( "usage:\n" @@ -117,6 +118,7 @@ esc_calib_main(int argc, char *argv[]) int myoptind = 1; const char *myoptarg = NULL; + while ((ch = px4_getopt(argc, argv, "d:c:m:al:h:", &myoptind, &myoptarg)) != EOF) { switch (ch) { @@ -131,43 +133,54 @@ esc_calib_main(int argc, char *argv[]) while ((single_ch = channels % 10)) { - set_mask |= 1<<(single_ch-1); + set_mask |= 1 << (single_ch - 1); channels /= 10; } + break; case 'm': /* Read in mask directly */ set_mask = strtoul(myoptarg, &ep, 0); + if (*ep != '\0') { usage("bad set_mask value"); return 1; } + break; case 'a': + /* Choose all channels */ - for (unsigned i = 0; i PWM_HIGHEST_MIN) { usage("low PWM invalid"); return 1; } + break; + case 'h': /* Read in custom high value */ pwm_high = strtoul(myoptarg, &ep, 0); + if (*ep != '\0' || pwm_high > PWM_HIGHEST_MAX || pwm_high < PWM_LOWEST_MAX) { usage("high PWM invalid"); return 1; } + break; + default: usage(NULL); return 1; @@ -220,7 +233,10 @@ esc_calib_main(int argc, char *argv[]) ret = poll(&fds, 1, 0); if (ret > 0) { - read(0, &c, 1); + if (read(0, &c, 1) <= 0) { + printf("ESC calibration read error\n"); + return 0; + } if (c == 'y' || c == 'Y') { break; @@ -253,6 +269,7 @@ esc_calib_main(int argc, char *argv[]) /* get number of channels available on the device */ ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&max_channels); + if (ret != OK) { PX4_ERR("PWM_SERVO_GET_COUNT"); return 1; @@ -260,12 +277,15 @@ esc_calib_main(int argc, char *argv[]) /* tell IO/FMU that its ok to disable its safety with the switch */ ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); + if (ret != OK) { PX4_ERR("PWM_SERVO_SET_ARM_OK"); return 1; } + /* tell IO/FMU that the system is armed (it will output values if safety is off) */ ret = ioctl(fd, PWM_SERVO_ARM, 0); + if (ret != OK) { PX4_ERR("PWM_SERVO_ARM"); return 1; @@ -285,11 +305,11 @@ esc_calib_main(int argc, char *argv[]) /* set max PWM */ for (unsigned i = 0; i < max_channels; i++) { - if (set_mask & 1< 0) { - read(0, &c, 1); + if (read(0, &c, 1) <= 0) { + printf("ESC calibration read error\n"); + goto done; + } if (c == 13) { break; @@ -322,7 +345,7 @@ esc_calib_main(int argc, char *argv[]) /* set disarmed PWM */ for (unsigned i = 0; i < max_channels; i++) { - if (set_mask & 1< 0) { - read(0, &c, 1); + if (read(0, &c, 1) <= 0) { + printf("ESC calibration read error\n"); + goto done; + } if (c == 13) { break; @@ -352,6 +378,7 @@ esc_calib_main(int argc, char *argv[]) /* disarm */ ret = ioctl(fd, PWM_SERVO_DISARM, 0); + if (ret != OK) { PX4_ERR("PWM_SERVO_DISARM"); goto cleanup; diff --git a/src/systemcmds/i2c/i2c.c b/src/systemcmds/i2c/i2c.c index 2991456cf1..1fa60a7e47 100644 --- a/src/systemcmds/i2c/i2c.c +++ b/src/systemcmds/i2c/i2c.c @@ -75,21 +75,25 @@ int i2c_main(int argc, char *argv[]) /* find the right I2C */ i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD); - if (i2c == NULL) + if (i2c == NULL) { errx(1, "failed to locate I2C bus"); + } usleep(100000); uint8_t buf[] = { 0, 4}; int ret = transfer(PX4_I2C_OBDEV_PX4IO, buf, sizeof(buf), NULL, 0); - if (ret) + if (ret) { errx(1, "send failed - %d", ret); + } uint32_t val; ret = transfer(PX4_I2C_OBDEV_PX4IO, NULL, 0, (uint8_t *)&val, sizeof(val)); - if (ret) + + if (ret) { errx(1, "recive failed - %d", ret); + } errx(0, "got 0x%08x", val); } @@ -121,8 +125,9 @@ transfer(uint8_t address, uint8_t *send, unsigned send_len, uint8_t *recv, unsig msgs++; } - if (msgs == 0) + if (msgs == 0) { return -1; + } /* * I2C architecture means there is an unavoidable race here @@ -133,8 +138,9 @@ transfer(uint8_t address, uint8_t *send, unsigned send_len, uint8_t *recv, unsig ret = I2C_TRANSFER(i2c, &msgv[0], msgs); // reset the I2C bus to unwedge on error - if (ret != OK) + if (ret != OK) { up_i2creset(i2c); + } return ret; } diff --git a/src/systemcmds/mixer/mixer.cpp b/src/systemcmds/mixer/mixer.cpp index 83a9378619..56419714b5 100644 --- a/src/systemcmds/mixer/mixer.cpp +++ b/src/systemcmds/mixer/mixer.cpp @@ -76,22 +76,26 @@ mixer_main(int argc, char *argv[]) } int ret = load(argv[2], argv[3]); - if(ret !=0) { + + if (ret != 0) { warnx("failed to load mixer"); return 1; } + } else { usage("Unknown command"); return 1; } + return 0; } static void usage(const char *reason) { - if (reason) + if (reason) { PX4_INFO("%s\n", reason); + } PX4_INFO("usage:\n"); PX4_INFO(" mixer load \n"); @@ -102,7 +106,7 @@ load(const char *devname, const char *fname) { // sleep a while to ensure device has been set up usleep(20000); - + int dev; char buf[2048]; @@ -130,5 +134,6 @@ load(const char *devname, const char *fname) warnx("error loading mixers from %s", fname); return 1; } + return 0; } diff --git a/src/systemcmds/motor_test/motor_test.c b/src/systemcmds/motor_test/motor_test.c index 2108b2004e..3581c2004b 100644 --- a/src/systemcmds/motor_test/motor_test.c +++ b/src/systemcmds/motor_test/motor_test.c @@ -69,13 +69,14 @@ void motor_test(unsigned channel, float value) _test_motor.timestamp = hrt_absolute_time(); _test_motor.value = value; - if (_test_motor_pub != nullptr) { - /* publish test state */ - orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor); - } else { - /* advertise and publish */ - _test_motor_pub = orb_advertise(ORB_ID(test_motor), &_test_motor); - } + if (_test_motor_pub != nullptr) { + /* publish test state */ + orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor); + + } else { + /* advertise and publish */ + _test_motor_pub = orb_advertise(ORB_ID(test_motor), &_test_motor); + } } static void usage(const char *reason) @@ -85,10 +86,10 @@ static void usage(const char *reason) } errx(1, - "usage:\n" - "motor_test\n" - " -m Motor to test (0..7)\n" - " -p Power (0..100)\n"); + "usage:\n" + "motor_test\n" + " -m Motor to test (0..7)\n" + " -p Power (0..100)\n"); } int motor_test_main(int argc, char *argv[]) @@ -114,11 +115,13 @@ int motor_test_main(int argc, char *argv[]) /* Read in power value */ lval = strtoul(optarg, NULL, 0); - if (lval > 100) + if (lval > 100) { usage("value invalid"); + } - value = ((float)lval)/100.f; + value = ((float)lval) / 100.f; break; + default: usage(NULL); } diff --git a/src/systemcmds/mtd/24xxxx_mtd.c b/src/systemcmds/mtd/24xxxx_mtd.c index 2aba9dbb69..9fc2786831 100644 --- a/src/systemcmds/mtd/24xxxx_mtd.c +++ b/src/systemcmds/mtd/24xxxx_mtd.c @@ -239,11 +239,13 @@ void at24c_test(void) for (count = 0; count < 10000; count++) { ssize_t result = at24c_bread(&g_at24c.mtd, 0, 1, buf); + if (result == ERROR) { if (errors++ > 2) { vdbg("too many errors\n"); return; } + } else if (result != 1) { vdbg("unexpected %u\n", result); } @@ -311,8 +313,9 @@ static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock, ret = I2C_TRANSFER(priv->dev, &msgv[0], 2); perf_end(priv->perf_transfers); - if (ret >= 0) + if (ret >= 0) { break; + } fvdbg("read stall"); usleep(1000); @@ -396,8 +399,9 @@ static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, size_t ret = I2C_TRANSFER(priv->dev, &msgv[0], 1); perf_end(priv->perf_transfers); - if (ret >= 0) + if (ret >= 0) { break; + } fvdbg("write stall"); usleep(1000); @@ -503,7 +507,8 @@ static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg) * ************************************************************************************/ -FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev) { +FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev) +{ FAR struct at24c_dev_s *priv; fvdbg("dev: %p\n", dev); diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c index c1dbcad709..0f54c9c4c3 100644 --- a/src/systemcmds/mtd/mtd.c +++ b/src/systemcmds/mtd/mtd.c @@ -92,8 +92,8 @@ static void mtd_erase(char *partition_names[], unsigned n_partitions); static void mtd_readtest(char *partition_names[], unsigned n_partitions); static void mtd_rwtest(char *partition_names[], unsigned n_partitions); static void mtd_print_info(void); -static int mtd_get_geometry(unsigned long *blocksize, unsigned long *erasesize, unsigned long *neraseblocks, - unsigned *blkpererase, unsigned *nblocks, unsigned *partsize, unsigned n_partitions); +static int mtd_get_geometry(unsigned long *blocksize, unsigned long *erasesize, unsigned long *neraseblocks, + unsigned *blkpererase, unsigned *nblocks, unsigned *partsize, unsigned n_partitions); static bool attached = false; static bool started = false; @@ -107,9 +107,10 @@ static const int n_partitions_default = sizeof(partition_names_default) / sizeof static void mtd_status(void) { - if (!attached) + if (!attached) { errx(1, "MTD driver not started"); - + } + mtd_print_info(); exit(0); } @@ -122,40 +123,46 @@ int mtd_main(int argc, char *argv[]) /* start mapping according to user request */ if (argc >= 3) { mtd_start(argv + 2, argc - 2); + } else { mtd_start(partition_names_default, n_partitions_default); } } - if (!strcmp(argv[1], "test")) + if (!strcmp(argv[1], "test")) { mtd_test(); + } if (!strcmp(argv[1], "readtest")) { if (argc >= 3) { mtd_readtest(argv + 2, argc - 2); + } else { mtd_readtest(partition_names_default, n_partitions_default); } - } + } if (!strcmp(argv[1], "rwtest")) { if (argc >= 3) { mtd_rwtest(argv + 2, argc - 2); + } else { mtd_rwtest(partition_names_default, n_partitions_default); } - } + } - if (!strcmp(argv[1], "status")) + if (!strcmp(argv[1], "status")) { mtd_status(); + } if (!strcmp(argv[1], "erase")) { if (argc >= 3) { mtd_erase(argv + 2, argc - 2); + } else { mtd_erase(partition_names_default, n_partitions_default); } - } + } } errx(1, "expected a command, try 'start', 'erase', 'status', 'readtest', 'rwtest' or 'test'"); @@ -163,7 +170,7 @@ int mtd_main(int argc, char *argv[]) struct mtd_dev_s *ramtron_initialize(FAR struct spi_dev_s *dev); struct mtd_dev_s *mtd_partition(FAR struct mtd_dev_s *mtd, - off_t firstblock, off_t nblocks); + off_t firstblock, off_t nblocks); #ifdef CONFIG_MTD_RAMTRON static void @@ -181,8 +188,9 @@ ramtron_attach(void) SPI_SETMODE(spi, SPIDEV_MODE3); SPI_SELECT(spi, SPIDEV_FLASH, false); - if (spi == NULL) + if (spi == NULL) { errx(1, "failed to locate spi bus"); + } /* start the RAMTRON driver, attempt 5 times */ for (int i = 0; i < 5; i++) { @@ -199,10 +207,12 @@ ramtron_attach(void) } /* if last attempt is still unsuccessful, abort */ - if (mtd_dev == NULL) + if (mtd_dev == NULL) { errx(1, "failed to initialize mtd driver"); + } + + int ret = mtd_dev->ioctl(mtd_dev, MTDIOC_SETSPEED, (unsigned long)10 * 1000 * 1000); - int ret = mtd_dev->ioctl(mtd_dev, MTDIOC_SETSPEED, (unsigned long)10*1000*1000); if (ret != OK) { // FIXME: From the previous warnx call, it looked like this should have been an errx instead. Tried // that but setting the bug speed does fail all the time. Which was then exiting and the board would @@ -222,24 +232,28 @@ at24xxx_attach(void) /* this resets the I2C bus, set correct bus speed again */ I2C_SETFREQUENCY(i2c, 400000); - if (i2c == NULL) + if (i2c == NULL) { errx(1, "failed to locate I2C bus"); + } /* start the MTD driver, attempt 5 times */ for (int i = 0; i < 5; i++) { mtd_dev = at24c_initialize(i2c); + if (mtd_dev) { /* abort on first valid result */ if (i > 0) { - warnx("warning: EEPROM needed %d attempts to attach", i+1); + warnx("warning: EEPROM needed %d attempts to attach", i + 1); } + break; } } /* if last attempt is still unsuccessful, abort */ - if (mtd_dev == NULL) + if (mtd_dev == NULL) { errx(1, "failed to initialize EEPROM driver"); + } attached = true; } @@ -250,15 +264,16 @@ mtd_start(char *partition_names[], unsigned n_partitions) { int ret; - if (started) + if (started) { errx(1, "mtd already mounted"); + } if (!attached) { - #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 at24xxx_attach(); - #else +#else ramtron_attach(); - #endif +#endif } if (!mtd_dev) { @@ -270,8 +285,10 @@ mtd_start(char *partition_names[], unsigned n_partitions) unsigned blkpererase, nblocks, partsize; ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, n_partitions); - if (ret) + + if (ret) { exit(3); + } /* Now create MTD FLASH partitions */ @@ -320,10 +337,10 @@ mtd_start(char *partition_names[], unsigned n_partitions) exit(0); } -int mtd_get_geometry(unsigned long *blocksize, unsigned long *erasesize, unsigned long *neraseblocks, - unsigned *blkpererase, unsigned *nblocks, unsigned *partsize, unsigned n_partitions) +int mtd_get_geometry(unsigned long *blocksize, unsigned long *erasesize, unsigned long *neraseblocks, + unsigned *blkpererase, unsigned *nblocks, unsigned *partsize, unsigned n_partitions) { - /* Get the geometry of the FLASH device */ + /* Get the geometry of the FLASH device */ FAR struct mtd_geometry_s geo; @@ -358,24 +375,31 @@ static ssize_t mtd_get_partition_size(void) unsigned long blocksize, erasesize, neraseblocks; unsigned blkpererase, nblocks, partsize = 0; - int ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, n_partitions_current); + int ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, + n_partitions_current); + if (ret != OK) { errx(1, "Failed to get geometry"); } + return partsize; } void mtd_print_info(void) { - if (!attached) + if (!attached) { exit(1); + } unsigned long blocksize, erasesize, neraseblocks; unsigned blkpererase, nblocks, partsize; - int ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, n_partitions_current); - if (ret) + int ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, + n_partitions_current); + + if (ret) { exit(3); + } warnx("Flash Geometry:"); @@ -400,19 +424,24 @@ mtd_erase(char *partition_names[], unsigned n_partitions) { uint8_t v[64]; memset(v, 0xFF, sizeof(v)); + for (uint8_t i = 0; i < n_partitions; i++) { uint32_t count = 0; printf("Erasing %s\n", partition_names[i]); int fd = open(partition_names[i], O_WRONLY); + if (fd == -1) { errx(1, "Failed to open partition"); } + while (write(fd, v, sizeof(v)) == sizeof(v)) { count += sizeof(v); } + printf("Erased %lu bytes\n", (unsigned long)count); close(fd); } + exit(0); } @@ -427,21 +456,27 @@ mtd_readtest(char *partition_names[], unsigned n_partitions) ssize_t expected_size = mtd_get_partition_size(); uint8_t v[128]; + for (uint8_t i = 0; i < n_partitions; i++) { ssize_t count = 0; printf("reading %s expecting %u bytes\n", partition_names[i], expected_size); int fd = open(partition_names[i], O_RDONLY); + if (fd == -1) { errx(1, "Failed to open partition"); } + while (read(fd, v, sizeof(v)) == sizeof(v)) { count += sizeof(v); } + if (count != expected_size) { - errx(1,"Failed to read partition - got %u/%u bytes", count, expected_size); + errx(1, "Failed to read partition - got %u/%u bytes", count, expected_size); } + close(fd); } + printf("readtest OK\n"); exit(0); } @@ -458,38 +493,50 @@ mtd_rwtest(char *partition_names[], unsigned n_partitions) ssize_t expected_size = mtd_get_partition_size(); uint8_t v[128], v2[128]; + for (uint8_t i = 0; i < n_partitions; i++) { ssize_t count = 0; - off_t offset = 0; + off_t offset = 0; printf("rwtest %s testing %u bytes\n", partition_names[i], expected_size); int fd = open(partition_names[i], O_RDWR); + if (fd == -1) { errx(1, "Failed to open partition"); } + while (read(fd, v, sizeof(v)) == sizeof(v)) { count += sizeof(v); - if (lseek(fd, offset, SEEK_SET) != offset) { - errx(1, "seek failed"); - } - if (write(fd, v, sizeof(v)) != sizeof(v)) { - errx(1, "write failed"); - } - if (lseek(fd, offset, SEEK_SET) != offset) { - errx(1, "seek failed"); - } - if (read(fd, v2, sizeof(v2)) != sizeof(v2)) { - errx(1, "read failed"); - } - if (memcmp(v, v2, sizeof(v2)) != 0) { - errx(1, "memcmp failed"); - } - offset += sizeof(v); + + if (lseek(fd, offset, SEEK_SET) != offset) { + errx(1, "seek failed"); + } + + if (write(fd, v, sizeof(v)) != sizeof(v)) { + errx(1, "write failed"); + } + + if (lseek(fd, offset, SEEK_SET) != offset) { + errx(1, "seek failed"); + } + + if (read(fd, v2, sizeof(v2)) != sizeof(v2)) { + errx(1, "read failed"); + } + + if (memcmp(v, v2, sizeof(v2)) != 0) { + errx(1, "memcmp failed"); + } + + offset += sizeof(v); } + if (count != expected_size) { - errx(1,"Failed to read partition - got %u/%u bytes", count, expected_size); + errx(1, "Failed to read partition - got %u/%u bytes", count, expected_size); } + close(fd); } + printf("rwtest OK\n"); exit(0); } diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index 784c64bf1f..56d4f9f3af 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -59,87 +59,92 @@ __EXPORT int nshterm_main(int argc, char *argv[]); int nshterm_main(int argc, char *argv[]) { - if (argc < 2) { - printf("Usage: nshterm \n"); - exit(1); - } - unsigned retries = 0; - int fd = -1; - int armed_fd = orb_subscribe(ORB_ID(actuator_armed)); - struct actuator_armed_s armed; + if (argc < 2) { + printf("Usage: nshterm \n"); + exit(1); + } - /* back off 1800 ms to avoid running into the USB setup timing */ - while (hrt_absolute_time() < 1800U * 1000U) { - usleep(50000); - } + unsigned retries = 0; + int fd = -1; + int armed_fd = orb_subscribe(ORB_ID(actuator_armed)); + struct actuator_armed_s armed; - /* try to bring up the console - stop doing so if the system gets armed */ - while (true) { + /* back off 1800 ms to avoid running into the USB setup timing */ + while (hrt_absolute_time() < 1800U * 1000U) { + usleep(50000); + } - /* abort if an arming topic is published and system is armed */ - bool updated = false; - orb_check(armed_fd, &updated); - if (updated) { - /* the system is now providing arming status feedback. - * instead of timing out, we resort to abort bringing - * up the terminal. - */ - orb_copy(ORB_ID(actuator_armed), armed_fd, &armed); + /* try to bring up the console - stop doing so if the system gets armed */ + while (true) { - if (armed.armed) { - /* this is not an error, but we are done */ - exit(0); - } - } + /* abort if an arming topic is published and system is armed */ + bool updated = false; + orb_check(armed_fd, &updated); - /* the retries are to cope with the behaviour of /dev/ttyACM0 */ - /* which may not be ready immediately. */ - fd = open(argv[1], O_RDWR); - if (fd != -1) { - close(armed_fd); - break; - } - usleep(100000); - retries++; - } - if (fd == -1) { - perror(argv[1]); - exit(1); - } + if (updated) { + /* the system is now providing arming status feedback. + * instead of timing out, we resort to abort bringing + * up the terminal. + */ + orb_copy(ORB_ID(actuator_armed), armed_fd, &armed); - /* set up the serial port with output processing */ - - /* Try to set baud rate */ - struct termios uart_config; - int termios_state; + if (armed.armed) { + /* this is not an error, but we are done */ + exit(0); + } + } - /* Back up the original uart configuration to restore it after exit */ - if ((termios_state = tcgetattr(fd, &uart_config)) < 0) { - warnx("ERR get config %s: %d\n", argv[1], termios_state); - close(fd); - return -1; - } + /* the retries are to cope with the behaviour of /dev/ttyACM0 */ + /* which may not be ready immediately. */ + fd = open(argv[1], O_RDWR); - /* Set ONLCR flag (which appends a CR for every LF) */ - uart_config.c_oflag |= (ONLCR | OPOST); + if (fd != -1) { + close(armed_fd); + break; + } - if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { - warnx("ERR set config %s\n", argv[1]); - close(fd); - return -1; - } + usleep(100000); + retries++; + } - /* setup standard file descriptors */ - close(0); - close(1); - close(2); - dup2(fd, 0); - dup2(fd, 1); - dup2(fd, 2); + if (fd == -1) { + perror(argv[1]); + exit(1); + } - nsh_consolemain(0, NULL); + /* set up the serial port with output processing */ - close(fd); + /* Try to set baud rate */ + struct termios uart_config; + int termios_state; - return OK; + /* Back up the original uart configuration to restore it after exit */ + if ((termios_state = tcgetattr(fd, &uart_config)) < 0) { + warnx("ERR get config %s: %d\n", argv[1], termios_state); + close(fd); + return -1; + } + + /* Set ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag |= (ONLCR | OPOST); + + if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { + warnx("ERR set config %s\n", argv[1]); + close(fd); + return -1; + } + + /* setup standard file descriptors */ + close(0); + close(1); + close(2); + dup2(fd, 0); + dup2(fd, 1); + dup2(fd, 2); + + nsh_consolemain(0, NULL); + + close(fd); + + return OK; } diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 614c7dc0d6..69d56dffcd 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -191,6 +191,7 @@ param_main(int argc, char *argv[]) if (!strcmp(argv[1], "index_used")) { if (argc >= 3) { return do_show_index(argv[2], true); + } else { warnx("no index provided"); return 1; @@ -200,6 +201,7 @@ param_main(int argc, char *argv[]) if (!strcmp(argv[1], "index")) { if (argc >= 3) { return do_show_index(argv[2], false); + } else { warnx("no index provided"); return 1; @@ -299,6 +301,7 @@ do_show_index(const char *index, bool used_index) if (used_index) { param = param_for_used_index(i); + } else { param = param_for_index(i); } @@ -326,6 +329,7 @@ do_show_index(const char *index, bool used_index) } break; + default: printf("\n", 0 + param_type(param)); } @@ -537,7 +541,7 @@ do_compare(const char *name, char *vals[], unsigned comparisons, enum COMPARE_OP float g = strtod(vals[k], &end); - if (((cmp_op == COMPARE_OPERATOR_EQUAL) && (fabsf(f - g) < 1e-7f)) || + if (((cmp_op == COMPARE_OPERATOR_EQUAL) && (fabsf(f - g) < 1e-7f)) || ((cmp_op == COMPARE_OPERATOR_GREATER) && (f > g))) { printf(" %4.4f: ", (double)f); ret = 0; @@ -575,6 +579,7 @@ do_reset(const char *excludes[], int num_excludes) warnx("Param export failed."); return 1; } + return 0; } @@ -602,5 +607,6 @@ do_reset_nostart(const char *excludes[], int num_excludes) return 1; } + return 0; } diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 024a54060c..a119952963 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -187,8 +187,7 @@ pwm_main(int argc, char *argv[]) break; - case 'p': - { + case 'p': { /* check if this is a param name */ if (strncmp("p:", optarg, 2) == 0) { @@ -203,12 +202,15 @@ pwm_main(int argc, char *argv[]) if (gret == 0) { pwm_value = pwm_parm; + } else { usage("PARAM LOAD FAIL"); } + } else { usage("PARAM NAME NOT FOUND"); } + } else { pwm_value = strtoul(optarg, &ep, 0); diff --git a/src/systemcmds/reboot/reboot.c b/src/systemcmds/reboot/reboot.c index 8de6b09985..8309d74ea4 100644 --- a/src/systemcmds/reboot/reboot.c +++ b/src/systemcmds/reboot/reboot.c @@ -52,6 +52,7 @@ int reboot_main(int argc, char *argv[]) int myoptind = 1; const char *myoptarg = NULL; + while ((ch = px4_getopt(argc, argv, "b", &myoptind, &myoptarg)) != -1) { switch (ch) { case 'b': @@ -60,7 +61,7 @@ int reboot_main(int argc, char *argv[]) default: PX4_ERR("usage: reboot [-b]\n" - " -b reboot into the bootloader"); + " -b reboot into the bootloader"); } } diff --git a/src/systemcmds/reflect/reflect.c b/src/systemcmds/reflect/reflect.c index fe5f0f0359..683ff7c6ea 100644 --- a/src/systemcmds/reflect/reflect.c +++ b/src/systemcmds/reflect/reflect.c @@ -54,7 +54,7 @@ __EXPORT int reflect_main(int argc, char *argv[]); #define MAX_BLOCKS 1000 static uint32_t nblocks; struct block { - uint32_t v[256]; + uint32_t v[256]; }; static struct block *blocks[MAX_BLOCKS]; @@ -62,50 +62,59 @@ static struct block *blocks[MAX_BLOCKS]; static void allocate_blocks(void) { - while (nblocks < MAX_BLOCKS) { - blocks[nblocks] = calloc(1, sizeof(struct block)); - if (blocks[nblocks] == NULL) { - break; - } - for (uint32_t i=0; iv)/sizeof(uint32_t); i++) { - blocks[nblocks]->v[i] = VALUE(i); - } - nblocks++; - } - printf("Allocated %u blocks\n", nblocks); + while (nblocks < MAX_BLOCKS) { + blocks[nblocks] = calloc(1, sizeof(struct block)); + + if (blocks[nblocks] == NULL) { + break; + } + + for (uint32_t i = 0; i < sizeof(blocks[nblocks]->v) / sizeof(uint32_t); i++) { + blocks[nblocks]->v[i] = VALUE(i); + } + + nblocks++; + } + + printf("Allocated %u blocks\n", nblocks); } static void check_blocks(void) { - for (uint32_t n=0; nv)/sizeof(uint32_t); i++) { - assert(blocks[n]->v[i] == VALUE(i)); - } - } + for (uint32_t n = 0; n < nblocks; n++) { + for (uint32_t i = 0; i < sizeof(blocks[nblocks]->v) / sizeof(uint32_t); i++) { + assert(blocks[n]->v[i] == VALUE(i)); + } + } } int reflect_main(int argc, char *argv[]) { - uint32_t total = 0; - printf("Starting reflector\n"); + uint32_t total = 0; + printf("Starting reflector\n"); - allocate_blocks(); + allocate_blocks(); - while (true) { - char buf[128]; - ssize_t n = read(0, buf, sizeof(buf)); - if (n < 0) { - break; - } - if (n > 0) { - write(1, buf, n); - } - total += n; - if (total > 1024000) { - check_blocks(); - total = 0; - } - } - return OK; + while (true) { + char buf[128]; + ssize_t n = read(0, buf, sizeof(buf)); + + if (n < 0) { + break; + } + + if (n > 0) { + write(1, buf, n); + } + + total += n; + + if (total > 1024000) { + check_blocks(); + total = 0; + } + } + + return OK; } diff --git a/src/systemcmds/tests/test_bson.c b/src/systemcmds/tests/test_bson.c index bba1ae4f12..945b00aa51 100644 --- a/src/systemcmds/tests/test_bson.c +++ b/src/systemcmds/tests/test_bson.c @@ -39,7 +39,7 @@ #define __STDC_FORMAT_MACROS #include - + #include #include #include @@ -111,8 +111,8 @@ decode_callback(bson_decoder_t decoder, void *private, bson_node_t node) if (node->b != sample_bool) { PX4_ERR("FAIL: decoder: bool1 value %s, expected %s", - (node->b ? "true" : "false"), - (sample_bool ? "true" : "false")); + (node->b ? "true" : "false"), + (sample_bool ? "true" : "false")); return 1; } diff --git a/src/systemcmds/tests/test_conv.cpp b/src/systemcmds/tests/test_conv.cpp index 8718342eb9..e451755959 100644 --- a/src/systemcmds/tests/test_conv.cpp +++ b/src/systemcmds/tests/test_conv.cpp @@ -67,7 +67,7 @@ int test_conv(int argc, char *argv[]) if (fabsf(f - fres) > 0.0001f) { PX4_ERR("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", (double)f, REG_TO_SIGNED(FLOAT_TO_REG(f)), - (double)fres); + (double)fres); return 1; } } diff --git a/src/systemcmds/tests/test_eigen.cpp b/src/systemcmds/tests/test_eigen.cpp index 0f71260e5d..ba3922784b 100644 --- a/src/systemcmds/tests/test_eigen.cpp +++ b/src/systemcmds/tests/test_eigen.cpp @@ -89,9 +89,9 @@ uint8_t err_num; #define EXPECT_QUATERNION(q_exp, q_act, epsilon) \ (fabsf(q_exp.x() - q_act.x()) <= epsilon && \ - fabsf(q_exp.y() - q_act.y()) <= epsilon && \ - fabsf(q_exp.z() - q_act.z()) <= epsilon && \ - fabsf(q_exp.w() - q_act.w()) <= epsilon) + fabsf(q_exp.y() - q_act.y()) <= epsilon && \ + fabsf(q_exp.z() - q_act.z()) <= epsilon && \ + fabsf(q_exp.w() - q_act.w()) <= epsilon) #define EXPECT_NEAR(expected, actual, epsilon) \ (fabsf(expected - actual) <= epsilon) @@ -130,12 +130,13 @@ math::Quaternion px4qFromEigenq(const Eigen::Quaternionf &q); * Construct new Eigen::Quaternion from euler angles * Right order is YPR. **/ -Eigen::Quaternionf quatFromEuler(const Eigen::Vector3f &rpy){ +Eigen::Quaternionf quatFromEuler(const Eigen::Vector3f &rpy) +{ return Eigen::Quaternionf( - Eigen::AngleAxisf(rpy.z(), Eigen::Vector3f::UnitZ()) * - Eigen::AngleAxisf(rpy.y(), Eigen::Vector3f::UnitY()) * - Eigen::AngleAxisf(rpy.x(), Eigen::Vector3f::UnitX()) - ); + Eigen::AngleAxisf(rpy.z(), Eigen::Vector3f::UnitZ()) * + Eigen::AngleAxisf(rpy.y(), Eigen::Vector3f::UnitY()) * + Eigen::AngleAxisf(rpy.x(), Eigen::Vector3f::UnitX()) + ); } /** @@ -143,7 +144,8 @@ Eigen::Quaternionf quatFromEuler(const Eigen::Vector3f &rpy){ * Construct new Eigen::Vector3f of euler angles from quaternion * Right order is YPR. **/ -Eigen::Vector3f eulerFromQuat(const Eigen::Quaternionf &q){ +Eigen::Vector3f eulerFromQuat(const Eigen::Quaternionf &q) +{ return q.toRotationMatrix().eulerAngles(2, 1, 0).reverse(); } @@ -151,7 +153,8 @@ Eigen::Vector3f eulerFromQuat(const Eigen::Quaternionf &q){ * @brief * Construct new Eigen::Matrix3f from euler angles **/ -Eigen::Matrix3f matrixFromEuler(const Eigen::Vector3f &rpy){ +Eigen::Matrix3f matrixFromEuler(const Eigen::Vector3f &rpy) +{ return quatFromEuler(rpy).toRotationMatrix(); } @@ -159,7 +162,8 @@ Eigen::Matrix3f matrixFromEuler(const Eigen::Vector3f &rpy){ * @brief * Adjust PX4 math::quaternion to Eigen::Quaternionf **/ -Eigen::Quaternionf eigenqFromPx4q(const math::Quaternion &q){ +Eigen::Quaternionf eigenqFromPx4q(const math::Quaternion &q) +{ return Eigen::Quaternionf(q.data[1], q.data[2], q.data[3], q.data[0]); } @@ -167,7 +171,8 @@ Eigen::Quaternionf eigenqFromPx4q(const math::Quaternion &q){ * @brief * Adjust Eigen::Quaternionf to PX4 math::quaternion **/ -math::Quaternion px4qFromEigenq(const Eigen::Quaternionf &q){ +math::Quaternion px4qFromEigenq(const Eigen::Quaternionf &q) +{ return math::Quaternion(q.w(), q.x(), q.y(), q.z()); } @@ -175,7 +180,8 @@ math::Quaternion px4qFromEigenq(const Eigen::Quaternionf &q){ * @brief * Testing main routine **/ -int test_eigen(int argc, char *argv[]) { +int test_eigen(int argc, char *argv[]) +{ int rc = 0; warnx("Testing Eigen math..."); @@ -285,7 +291,7 @@ int test_eigen(int argc, char *argv[]) { const Eigen::Matrix m1_orig = (Eigen::Matrix() << 1, 2, 3, - 4, 5, 6).finished(); + 4, 5, 6).finished(); Eigen::Matrix m1(m1_orig); @@ -362,6 +368,7 @@ int test_eigen(int argc, char *argv[]) { ++err_num; rc = 1; } + /********************************************************************************************************/ /******************************************** TEST 2 ****************************************************/ q_true = {1.0f, 0.0f, 0.0f, 0.0f}; @@ -376,6 +383,7 @@ int test_eigen(int argc, char *argv[]) { ++err_num; rc = 1; } + /********************************************************************************************************/ /******************************************** TEST 3 ****************************************************/ q_true = quatFromEuler(Eigen::Vector3f(0.3f, 0.2f, 0.1f)); @@ -388,6 +396,7 @@ int test_eigen(int argc, char *argv[]) { ++err_num; rc = 1; } + /********************************************************************************************************/ /******************************************** TEST 4 ****************************************************/ q_true = quatFromEuler(Eigen::Vector3f(-1.5f, -0.2f, 0.5f)); @@ -400,6 +409,7 @@ int test_eigen(int argc, char *argv[]) { ++err_num; rc = 1; } + /********************************************************************************************************/ /******************************************** TEST 5 ****************************************************/ q_true = quatFromEuler(Eigen::Vector3f(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3)); @@ -407,13 +417,15 @@ int test_eigen(int argc, char *argv[]) { for (size_t i = 0; i < 4; i++) { if (!EXPECT_QUATERNION(q_true, q, FLT_EPSILON)) { - printf("%llu: It[%d]: Value of: Quaternion [0.6830, 0.1830, -0.6830, 0.1830]\n", (unsigned long long)hrt_absolute_time(), (int)i); + printf("%llu: It[%d]: Value of: Quaternion [0.6830, 0.1830, -0.6830, 0.1830]\n", + (unsigned long long)hrt_absolute_time(), (int)i); printf("Actual: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q.w(), q.x(), q.y(), q.z()); printf("Expected: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_true.w(), q_true.x(), q_true.y(), q_true.z()); ++err_num; rc = 1; } } + /********************************************************************************************************/ /******************************************** TEST 6 ****************************************************/ printf("%llu: Conversion method: Testing transformation range...\n", (unsigned long long)hrt_absolute_time()); @@ -430,7 +442,8 @@ int test_eigen(int argc, char *argv[]) { for (size_t i = 0; i < 3; i++) { for (size_t j = 0; j < 3; j++) { if (!EXPECT_NEAR(R_orig(i, j), R(i, j), epsilon_f)) { - printf("%llu: (%d, %d) Value of: Quaternion constructor or 'toRotationMatrix'\n", (unsigned long long)hrt_absolute_time(), (int)i, (int)j); + printf("%llu: (%d, %d) Value of: Quaternion constructor or 'toRotationMatrix'\n", + (unsigned long long)hrt_absolute_time(), (int)i, (int)j); printf("Actual: \t%8.5f\n", R(i, j)); printf("Expected: \t%8.5f\n", R_orig(i, j)); ++err_num; @@ -457,7 +470,7 @@ int test_eigen(int argc, char *argv[]) { /******************************************** TEST 1 ****************************************************/ q = quatFromEuler(Eigen::Vector3f(0.0f, 0.0f, M_PI_2_F)); vector_q = q._transformVector(vector); - Eigen::Vector3f vector_true = {-1.00f, 1.00f, 1.00f}; + Eigen::Vector3f vector_true = { -1.00f, 1.00f, 1.00f}; for (size_t i = 0; i < 3; i++) { if (!EXPECT_NEAR(vector_true(i), vector_q(i), FLT_EPSILON)) { @@ -468,6 +481,7 @@ int test_eigen(int argc, char *argv[]) { rc = 1; } } + /********************************************************************************************************/ /******************************************** TEST 2 ****************************************************/ q = quatFromEuler(Eigen::Vector3f(0.1f, 0.2f, 0.3f)); @@ -483,6 +497,7 @@ int test_eigen(int argc, char *argv[]) { rc = 1; } } + /********************************************************************************************************/ /******************************************** TEST 3 ****************************************************/ q = quatFromEuler(Eigen::Vector3f(0.5f, -0.2f, -1.5f)); @@ -498,11 +513,12 @@ int test_eigen(int argc, char *argv[]) { rc = 1; } } + /********************************************************************************************************/ /******************************************** TEST 4 ****************************************************/ q = quatFromEuler(Eigen::Vector3f(-M_PI_F / 3.0f, -M_PI_2_F, M_PI_2_F)); vector_q = q._transformVector(vector); - vector_true = {-1.366030f, 0.366025f, 1.000000f}; + vector_true = { -1.366030f, 0.366025f, 1.000000f}; for (size_t i = 0; i < 3; i++) { if (!EXPECT_NEAR(vector_true(i), vector_q(i), epsilon_f)) { @@ -513,6 +529,7 @@ int test_eigen(int argc, char *argv[]) { rc = 1; } } + /********************************************************************************************************/ /******************************************** TEST 5 ****************************************************/ printf("%llu: Rotation method: Testing transformation range...\n", (unsigned long long)hrt_absolute_time()); diff --git a/src/systemcmds/tests/test_file2.c b/src/systemcmds/tests/test_file2.c index 630b9adc48..56d9f0d83a 100644 --- a/src/systemcmds/tests/test_file2.c +++ b/src/systemcmds/tests/test_file2.c @@ -176,6 +176,7 @@ int test_file2(int argc, char *argv[]) int myoptind = 1; const char *myoptarg = NULL; + while ((opt = px4_getopt(argc, argv, "c:s:FLh", &myoptind, &myoptarg)) != EOF) { switch (opt) { case 'F': diff --git a/src/systemcmds/tests/test_hott_telemetry.c b/src/systemcmds/tests/test_hott_telemetry.c index 430d0445d8..d1e9281191 100644 --- a/src/systemcmds/tests/test_hott_telemetry.c +++ b/src/systemcmds/tests/test_hott_telemetry.c @@ -195,7 +195,7 @@ int test_hott_telemetry(int argc, char *argv[]) } else { PX4_WARN("WARN: Received %d out of %d byte pairs of which %d were valid from the HoTT receiver device.", received_count, - max_polls, valid_count); + max_polls, valid_count); } } else { diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index a8a4878ccb..98b65f10d1 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -204,7 +204,7 @@ int test_mixer(int argc, char *argv[]) mixed = mixer_group.mix(&outputs[0], output_max, NULL); pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, - r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); for (unsigned i = 0; i < mixed; i++) { @@ -216,6 +216,7 @@ int test_mixer(int argc, char *argv[]) warnx("active servo < min"); return 1; } + } else { if (r_page_servos[i] != r_page_servo_disarmed[i]) { warnx("throttle output != 0 (this check assumed the IO pass mixer!)"); @@ -244,7 +245,7 @@ int test_mixer(int argc, char *argv[]) mixed = mixer_group.mix(&outputs[0], output_max, NULL); pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, - r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); for (unsigned i = 0; i < mixed; i++) { @@ -287,7 +288,8 @@ int test_mixer(int argc, char *argv[]) /* mix */ mixed = mixer_group.mix(&outputs[0], output_max, NULL); - pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, + pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, + r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); PX4_INFO("mixed %d outputs (max %d)", mixed, output_max); @@ -314,7 +316,8 @@ int test_mixer(int argc, char *argv[]) /* mix */ mixed = mixer_group.mix(&outputs[0], output_max, NULL); - pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, + pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, + r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); @@ -351,7 +354,8 @@ int test_mixer(int argc, char *argv[]) /* mix */ mixed = mixer_group.mix(&outputs[0], output_max, NULL); - pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, + pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, + r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); @@ -435,7 +439,7 @@ mixer_callback(uintptr_t handle, control = actuator_controls[control_index]; if (should_prearm && control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && - control_index == actuator_controls_s::INDEX_THROTTLE) { + control_index == actuator_controls_s::INDEX_THROTTLE) { control = NAN_VALUE; } diff --git a/src/systemcmds/tests/test_mount.c b/src/systemcmds/tests/test_mount.c index 64eefa0b47..f92c5f6316 100644 --- a/src/systemcmds/tests/test_mount.c +++ b/src/systemcmds/tests/test_mount.c @@ -132,7 +132,7 @@ test_mount(int argc, char *argv[]) } PX4_INFO("Iterations left: #%d / #%d of %d / %d\n(%s)", it_left_fsync, it_left_abort, - fsync_tries, abort_tries, buf); + fsync_tries, abort_tries, buf); int it_left_fsync_prev = it_left_fsync; diff --git a/src/systemcmds/tests/test_uart_baudchange.c b/src/systemcmds/tests/test_uart_baudchange.c index 8c9a5ff35f..7c138efca1 100644 --- a/src/systemcmds/tests/test_uart_baudchange.c +++ b/src/systemcmds/tests/test_uart_baudchange.c @@ -165,5 +165,5 @@ int test_uart_baudchange(int argc, char *argv[]) cleanup: close(uart2); return ret; - + } diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index 0328621b3d..28cd988627 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -58,7 +58,7 @@ static const char sz_ver_all_str[] = "all"; static const char mcu_ver_str[] = "mcu"; static const char mcu_uid_str[] = "uid"; -const char* px4_git_version = PX4_GIT_VERSION_STR; +const char *px4_git_version = PX4_GIT_VERSION_STR; const uint64_t px4_git_version_binary = PX4_GIT_VERSION_BINARY; static void usage(const char *reason) @@ -89,6 +89,7 @@ int ver_main(int argc, char *argv[]) if (ret == 0) { printf("ver hwcmp match: %s\n", HW_ARCH); } + return ret; } else { @@ -127,7 +128,7 @@ int ver_main(int argc, char *argv[]) if (show_all || !strncmp(argv[1], mcu_ver_str, sizeof(mcu_ver_str))) { char rev; - char* revstr; + char *revstr; int chip_version = mcu_version(&rev, &revstr); @@ -139,9 +140,9 @@ int ver_main(int argc, char *argv[]) if (chip_version < MCU_REV_STM32F4_REV_3) { printf("\nWARNING WARNING WARNING!\n" - "Revision %c has a silicon errata\n" - "This device can only utilize a maximum of 1MB flash safely!\n" - "http://px4.io/help/errata\n\n", rev); + "Revision %c has a silicon errata\n" + "This device can only utilize a maximum of 1MB flash safely!\n" + "http://px4.io/help/errata\n\n", rev); } } @@ -153,7 +154,7 @@ int ver_main(int argc, char *argv[]) mcu_unique_id(uid); - printf("UID: %X:%X:%X \n",uid[0],uid[1],uid[2]); + printf("UID: %X:%X:%X \n", uid[0], uid[1], uid[2]); ret = 0; }