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 134cdcca67..20b15d323d 100644 --- a/.travis.yml +++ b/.travis.yml @@ -13,6 +13,7 @@ cache: addons: apt: sources: + - kubuntu-backports - ubuntu-toolchain-r-test packages: - build-essential @@ -46,9 +47,18 @@ before_script: - mkdir -p ~/bin - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-g++ - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-gcc + - ln -s /usr/bin/ccache ~/bin/clang++ + - ln -s /usr/bin/ccache ~/bin/clang++-3.4 + - ln -s /usr/bin/ccache ~/bin/clang++-3.5 + - ln -s /usr/bin/ccache ~/bin/clang + - ln -s /usr/bin/ccache ~/bin/clang-3.4 + - ln -s /usr/bin/ccache ~/bin/clang-3.5 - ln -s /usr/bin/ccache ~/bin/g++-4.8 - ln -s /usr/bin/ccache ~/bin/gcc-4.8 - export PATH=~/bin:$PATH +# 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: @@ -59,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 a0dc23f705..3c47aad3e0 100644 --- a/Makefile +++ b/Makefile @@ -220,11 +220,11 @@ qurtrun: # unit tests. .PHONY: tests tests: generateuorbtopicheaders - $(Q) (mkdir -p $(PX4_BASE)/unittests/build && cd $(PX4_BASE)/unittests/build && cmake .. && $(MAKE) unittests) + $(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/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/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index a2536e5418..ffaa935055 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -13,11 +13,11 @@ if [ $AUTOCNF == yes ] then # TODO tune roll/pitch separately param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.13 + param set MC_ROLLRATE_P 0.15 param set MC_ROLLRATE_I 0.05 param set MC_ROLLRATE_D 0.004 param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.13 + param set MC_PITCHRATE_P 0.15 param set MC_PITCHRATE_I 0.05 param set MC_PITCHRATE_D 0.004 param set MC_YAW_P 2.5 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/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 4303320ffe..aea5292a24 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -16,11 +16,11 @@ sh /etc/init.d/4001_quad_x if [ $AUTOCNF == yes ] then param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.13 + param set MC_ROLLRATE_P 0.15 param set MC_ROLLRATE_I 0.05 param set MC_ROLLRATE_D 0.003 param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.13 + param set MC_PITCHRATE_P 0.15 param set MC_PITCHRATE_I 0.05 param set MC_PITCHRATE_D 0.003 param set MC_YAW_P 2.8 diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index 408707ec0b..c0bd5b1c85 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -16,8 +16,6 @@ then param set PE_VELD_NOISE 0.35 param set PE_POSNE_NOISE 0.5 param set PE_POSD_NOISE 1.0 - param set PE_GBIAS_PNOISE 0.000001 - param set PE_ABIAS_PNOISE 0.0002 fi # This is the gimbal pass mixer diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index eccee07e7b..1080121618 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -8,8 +8,6 @@ then param set PE_VELD_NOISE 0.35 param set PE_POSNE_NOISE 0.5 param set PE_POSD_NOISE 1.25 - param set PE_GBIAS_PNOISE 0.000001 - param set PE_ABIAS_PNOISE 0.0001 param set NAV_ACC_RAD 2.0 param set RTL_RETURN_ALT 30.0 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.vtol_defaults b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults index c2340a7b16..1b7b6528cf 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults @@ -37,7 +37,6 @@ then param set PE_VELD_NOISE 0.3 param set PE_POSNE_NOISE 0.5 param set PE_POSD_NOISE 1.25 - param set PE_GBIAS_PNOISE 0.000001 param set PE_ABIAS_PNOISE 0.0001 fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 85cbdeb59e..8e7df42743 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -389,8 +389,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 # 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/makefiles/firmware.mk b/makefiles/firmware.mk index c59b66b21f..b8c8d4a85b 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -106,8 +106,6 @@ ifneq ($(words $(PX4_BASE)),1) $(error Cannot build when the PX4_BASE path contains one or more space characters.) endif -$(info % GIT_DESC = $(GIT_DESC)) - # # Set a default target so that included makefiles or errors here don't # cause confusion. @@ -232,7 +230,7 @@ $(MODULE_OBJS): workdir = $(@D) $(MODULE_OBJS): $(GLOBAL_DEPS) $(NUTTX_CONFIG_HEADER) $(Q) $(MKDIR) -p $(workdir) $(Q) $(MAKE) -r -f $(PX4_MK_DIR)module.mk \ - -C $(workdir) \ + --no-print-directory -C $(workdir) \ MODULE_WORK_DIR=$(workdir) \ MODULE_OBJ=$@ \ MODULE_MK=$(mkfile) \ @@ -292,7 +290,7 @@ $(LIBRARY_LIBS): workdir = $(@D) $(LIBRARY_LIBS): $(GLOBAL_DEPS) $(NUTTX_CONFIG_HEADER) $(Q) $(MKDIR) -p $(workdir) $(Q) $(MAKE) -r -f $(PX4_MK_DIR)library.mk \ - -C $(workdir) \ + --no-print-directory -C $(workdir) \ LIBRARY_WORK_DIR=$(workdir) \ LIBRARY_LIB=$@ \ LIBRARY_MK=$(mkfile) \ diff --git a/makefiles/module.mk b/makefiles/module.mk index 3c9ca35026..c92abaec3f 100644 --- a/makefiles/module.mk +++ b/makefiles/module.mk @@ -113,7 +113,9 @@ ifeq ($(MODULE_MK),) $(error No module makefile specified) endif +ifeq ($(V),1) $(info %% MODULE_MK = $(MODULE_MK)) +endif # # Get the board/toolchain config @@ -125,10 +127,12 @@ include $(BOARD_FILE) # include $(MODULE_MK) MODULE_SRC := $(dir $(MODULE_MK)) +ifeq ($(V),1) $(info % MODULE_NAME = $(MODULE_NAME)) $(info % MODULE_SRC = $(MODULE_SRC)) $(info % MODULE_OBJ = $(MODULE_OBJ)) $(info % MODULE_WORK_DIR = $(MODULE_WORK_DIR)) +endif # # Things that, if they change, might affect everything diff --git a/makefiles/nuttx/firmware_nuttx.mk b/makefiles/nuttx/firmware_nuttx.mk index 644c3a0e72..ca4831b8bd 100644 --- a/makefiles/nuttx/firmware_nuttx.mk +++ b/makefiles/nuttx/firmware_nuttx.mk @@ -30,7 +30,7 @@ $(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: checkgitversion generateuorbtopi @$(ECHO) %%%% Building $(config) in $(work_dir) @$(ECHO) %%%% $(Q) $(MKDIR) -p $(work_dir) - $(Q) $(MAKE) -r -C $(work_dir) \ + $(Q) $(MAKE) -r --no-print-directory -C $(work_dir) \ -f $(PX4_MK_DIR)firmware.mk \ CONFIG=$(config) \ WORK_DIR=$(work_dir) \ @@ -77,11 +77,11 @@ $(ARCHIVE_DIR)%.export: configuration = nsh $(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) @$(ECHO) %% Configuring NuttX for $(board) $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) - $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) $(MAKE) -r -j$(J) --no-print-directory -C $(NUTTX_SRC) -r $(MQUIET) distclean $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(board) .) $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(board)/$(configuration)) @$(ECHO) %% Exporting NuttX for $(board) - $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) CONFIG_ARCH_BOARD=$(board) export + $(Q) $(MAKE) -r -j$(J) --no-print-directory -C $(NUTTX_SRC) -r $(MQUIET) CONFIG_ARCH_BOARD=$(board) export $(Q) $(MKDIR) -p $(dir $@) $(Q) $(COPY) $(NUTTX_SRC)nuttx-export.zip $@ $(Q) (cd $(NUTTX_SRC)/configs && $(RMDIR) $(board)) @@ -98,12 +98,12 @@ BOARD = $(BOARDS) menuconfig: $(NUTTX_SRC) @$(ECHO) %% Configuring NuttX for $(BOARD) $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) - $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) $(MAKE) -r -j$(J) --no-print-directory -C $(NUTTX_SRC) -r $(MQUIET) distclean $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(BOARD) .) $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(BOARD)/nsh) @$(ECHO) %% Running menuconfig for $(BOARD) - $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) oldconfig - $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) menuconfig + $(Q) $(MAKE) -r -j$(J) --no-print-directory -C $(NUTTX_SRC) -r $(MQUIET) oldconfig + $(Q) $(MAKE) -r -j$(J) --no-print-directory -C $(NUTTX_SRC) -r $(MQUIET) menuconfig @$(ECHO) %% Saving configuration file $(Q)$(COPY) $(NUTTX_SRC).config $(PX4_BASE)nuttx-configs/$(BOARD)/nsh/defconfig else diff --git a/makefiles/nuttx/toolchain_gnu-arm-eabi.mk b/makefiles/nuttx/toolchain_gnu-arm-eabi.mk index 27be31da8e..868c3d161e 100644 --- a/makefiles/nuttx/toolchain_gnu-arm-eabi.mk +++ b/makefiles/nuttx/toolchain_gnu-arm-eabi.mk @@ -147,7 +147,6 @@ ARCHWARNINGS = -Wall \ -Wshadow \ -Wfloat-equal \ -Wpointer-arith \ - -Wlogical-op \ -Wmissing-declarations \ -Wpacked \ -Wno-unused-parameter \ @@ -271,7 +270,6 @@ define PRELINK @$(ECHO) "PRELINK: $1" @$(MKDIR) -p $(dir $1) $(Q) $(LD) -Ur -Map $1.map -o $1 $2 && $(OBJCOPY) --localize-hidden $1 - #$(Q) $(LD) -Ur -Map $1.map -o $1 $2 && $(OBJCOPY) --localize-hidden $1 endef # Update the archive $1 with the files in $2 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 2811512ecd..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 # @@ -157,8 +131,6 @@ ARCHWARNINGS = -Wall \ -Werror=reorder \ -Werror=uninitialized \ -Werror=init-self \ - -Wno-error=logical-op \ - -Wlogical-op \ -Wformat=1 \ -Werror=unused-but-set-variable \ -Wno-error=double-promotion \ @@ -191,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..d77082a89a 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 diff --git a/makefiles/posix/firmware_posix.mk b/makefiles/posix/firmware_posix.mk index f88caf569a..f6f087d449 100644 --- a/makefiles/posix/firmware_posix.mk +++ b/makefiles/posix/firmware_posix.mk @@ -47,7 +47,7 @@ $(FIRMWARES): $(BUILD_DIR)%.build/firmware.a: checkgitversion generateuorbtopich @$(ECHO) %%%% Building $(config) in $(work_dir) @$(ECHO) %%%% $(Q) $(MKDIR) -p $(work_dir) - $(Q) $(MAKE) -r -C $(work_dir) \ + $(Q) $(MAKE) -r --no-print-directory -C $(work_dir) \ -f $(PX4_MK_DIR)firmware.mk \ CONFIG=$(config) \ WORK_DIR=$(work_dir) \ diff --git a/makefiles/posix/toolchain_native.mk b/makefiles/posix/toolchain_native.mk index eeba6d8f91..c3a766d5ef 100644 --- a/makefiles/posix/toolchain_native.mk +++ b/makefiles/posix/toolchain_native.mk @@ -183,9 +183,8 @@ ARCHWARNINGS = -Wall \ # Add compiler specific options ifeq ($(USE_GCC),1) -ARCHDEFINES += -Wno-error=logical-op +ARCHDEFINES += ARCHWARNINGS += -Wdouble-promotion \ - -Wlogical-op \ -Wformat=1 \ -Werror=unused-but-set-variable \ -Werror=double-promotion @@ -348,7 +347,6 @@ endef define LINK_A @$(ECHO) "LINK_A: $1" @$(MKDIR) -p $(dir $1) - echo "$(Q) $(AR) $1 $2" $(Q) $(AR) $1 $2 endef @@ -357,7 +355,6 @@ endef define LINK_SO @$(ECHO) "LINK_SO: $1" @$(MKDIR) -p $(dir $1) - echo "$(Q) $(CXX) $(LDFLAGS) -shared -Wl,-soname,`basename $1`.1 -o $1 $2 $(LIBS) $(EXTRA_LIBS)" $(Q) $(CXX) $(LDFLAGS) -shared -Wl,-soname,`basename $1`.1 -o $1 $2 $(LIBS) -pthread -lc endef 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 a367e4a327..ac87683487 100644 --- a/makefiles/qurt/firmware_qurt.mk +++ b/makefiles/qurt/firmware_qurt.mk @@ -47,14 +47,13 @@ $(FIRMWARES): $(BUILD_DIR)%.build/firmware.a: generateuorbtopicheaders @$(ECHO) %%%% Building $(config) in $(work_dir) @$(ECHO) %%%% $(Q) $(MKDIR) -p $(work_dir) - $(Q) $(MAKE) -r -C $(work_dir) \ + $(Q) $(MAKE) -r --no-print-directory -C $(work_dir) \ -f $(PX4_MK_DIR)firmware.mk \ CONFIG=$(config) \ 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 038c299904..77a9d4e838 100644 --- a/makefiles/qurt/toolchain_hexagon.mk +++ b/makefiles/qurt/toolchain_hexagon.mk @@ -35,19 +35,11 @@ #$(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 #HEXAGON_TOOLS_ROOT = /opt/6.4.05 -HEXAGON_SDK_ROOT = /opt/Hexagon_SDK/2.0 +HEXAGON_SDK_ROOT ?= /opt/Hexagon_SDK/2.0 V_ARCH = v5 CROSSDEV = hexagon- HEXAGON_BIN = $(addsuffix /gnu/bin,$(HEXAGON_TOOLS_ROOT)) @@ -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/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 65b69e4d0b..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,18 +29,21 @@ 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 fw_att_control start fw_pos_control_l1 start -mixer load /dev/pwm_output0 ../../ROMFS/px4fmu_common/mixers/quad_x.main.mix +mixer load /dev/pwm_output0 ../../ROMFS/px4fmu_common/mixers/IO_pass.main.mix mavlink start -u 14556 -r 60000 mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556 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..b340096721 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 @@ -53,3 +54,4 @@ 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 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/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index 86c2230526..44aea614cc 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -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/irlock/irlock.cpp b/src/drivers/irlock/irlock.cpp index 4aeb8c7497..19b0de6d5f 100644 --- a/src/drivers/irlock/irlock.cpp +++ b/src/drivers/irlock/irlock.cpp @@ -110,7 +110,7 @@ private: int read_device_block(struct irlock_s *block); /** internal variables **/ - RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; bool _sensor_ok; work_s _work; uint32_t _read_failures; @@ -158,7 +158,7 @@ int IRLOCK::init() } /** allocate buffer storing values read from sensor **/ - _reports = new RingBuffer(IRLOCK_OBJECTS_MAX, sizeof(struct irlock_s)); + _reports = new ringbuffer::RingBuffer(IRLOCK_OBJECTS_MAX, sizeof(struct irlock_s)); if (_reports == nullptr) { return ENOTTY; 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/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/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..847d3e3a03 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,6 +267,7 @@ MS5611::init() int ret; ret = CDev::init(); + if (ret != OK) { DEVICE_DEBUG("CDev init failed"); goto out; @@ -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/pwm_input/pwm_input.cpp b/src/drivers/pwm_input/pwm_input.cpp index c137517591..06d660d92c 100644 --- a/src/drivers/pwm_input/pwm_input.cpp +++ b/src/drivers/pwm_input/pwm_input.cpp @@ -105,7 +105,7 @@ #elif PWMIN_TIMER == 2 # define PWMIN_TIMER_BASE STM32_TIM2_BASE # define PWMIN_TIMER_POWER_REG STM32_RCC_APB1ENR -# define PWMIN_TIMER_POWER_BIT RCC_APB2ENR_TIM2EN +# define PWMIN_TIMER_POWER_BIT RCC_APB1ENR_TIM2EN # define PWMIN_TIMER_VECTOR STM32_IRQ_TIM2 # define PWMIN_TIMER_CLOCK STM32_APB1_TIM2_CLKIN #elif PWMIN_TIMER == 3 @@ -123,7 +123,7 @@ #elif PWMIN_TIMER == 5 # define PWMIN_TIMER_BASE STM32_TIM5_BASE # define PWMIN_TIMER_POWER_REG STM32_RCC_APB1ENR -# define PWMIN_TIMER_POWER_BIT RCC_APB2ENR_TIM5EN +# define PWMIN_TIMER_POWER_BIT RCC_APB1ENR_TIM5EN # define PWMIN_TIMER_VECTOR STM32_IRQ_TIM5 # define PWMIN_TIMER_CLOCK STM32_APB1_TIM5_CLKIN #elif PWMIN_TIMER == 8 @@ -134,28 +134,28 @@ # define PWMIN_TIMER_CLOCK STM32_APB2_TIM8_CLKIN #elif PWMIN_TIMER == 9 # define PWMIN_TIMER_BASE STM32_TIM9_BASE -# define PWMIN_TIMER_POWER_REG STM32_RCC_APB1ENR +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB2ENR # define PWMIN_TIMER_POWER_BIT RCC_APB2ENR_TIM9EN # define PWMIN_TIMER_VECTOR STM32_IRQ_TIM1BRK -# define PWMIN_TIMER_CLOCK STM32_APB1_TIM9_CLKIN +# define PWMIN_TIMER_CLOCK STM32_APB2_TIM9_CLKIN #elif PWMIN_TIMER == 10 # define PWMIN_TIMER_BASE STM32_TIM10_BASE -# define PWMIN_TIMER_POWER_REG STM32_RCC_APB1ENR +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB2ENR # define PWMIN_TIMER_POWER_BIT RCC_APB2ENR_TIM10EN # define PWMIN_TIMER_VECTOR STM32_IRQ_TIM1UP # define PWMIN_TIMER_CLOCK STM32_APB2_TIM10_CLKIN #elif PWMIN_TIMER == 11 # define PWMIN_TIMER_BASE STM32_TIM11_BASE -# define PWMIN_TIMER_POWER_REG STM32_RCC_APB1ENR +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB2ENR # define PWMIN_TIMER_POWER_BIT RCC_APB2ENR_TIM11EN # define PWMIN_TIMER_VECTOR STM32_IRQ_TIM1TRGCOM # define PWMIN_TIMER_CLOCK STM32_APB2_TIM11_CLKIN #elif PWMIN_TIMER == 12 # define PWMIN_TIMER_BASE STM32_TIM12_BASE # define PWMIN_TIMER_POWER_REG STM32_RCC_APB1ENR -# define PWMIN_TIMER_POWER_BIT RCC_APB2ENR_TIM12EN -# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM1TRGCOM -# define PWMIN_TIMER_CLOCK STM32_APB2_TIM12_CLKIN +# define PWMIN_TIMER_POWER_BIT RCC_APB1ENR_TIM12EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM8BRK +# define PWMIN_TIMER_CLOCK STM32_APB1_TIM12_CLKIN #else # error PWMIN_TIMER must be a value between 1 and 12 #endif 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/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 e37b750fe8..ba148c1335 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -85,7 +85,7 @@ #elif HRT_TIMER == 2 # define HRT_TIMER_BASE STM32_TIM2_BASE # define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR -# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM2EN +# define HRT_TIMER_POWER_BIT RCC_APB1ENR_TIM2EN # define HRT_TIMER_VECTOR STM32_IRQ_TIM2 # define HRT_TIMER_CLOCK STM32_APB1_TIM2_CLKIN # if CONFIG_STM32_TIM2 @@ -103,7 +103,7 @@ #elif HRT_TIMER == 4 # define HRT_TIMER_BASE STM32_TIM4_BASE # define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR -# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM4EN +# define HRT_TIMER_POWER_BIT RCC_APB1ENR_TIM4EN # define HRT_TIMER_VECTOR STM32_IRQ_TIM4 # define HRT_TIMER_CLOCK STM32_APB1_TIM4_CLKIN # if CONFIG_STM32_TIM4 @@ -112,7 +112,7 @@ #elif HRT_TIMER == 5 # define HRT_TIMER_BASE STM32_TIM5_BASE # define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR -# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM5EN +# define HRT_TIMER_POWER_BIT RCC_APB1ENR_TIM5EN # define HRT_TIMER_VECTOR STM32_IRQ_TIM5 # define HRT_TIMER_CLOCK STM32_APB1_TIM5_CLKIN # if CONFIG_STM32_TIM5 @@ -129,16 +129,16 @@ # endif #elif HRT_TIMER == 9 # define HRT_TIMER_BASE STM32_TIM9_BASE -# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR +# define HRT_TIMER_POWER_REG STM32_RCC_APB2ENR # define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM9EN # define HRT_TIMER_VECTOR STM32_IRQ_TIM1BRK -# define HRT_TIMER_CLOCK STM32_APB1_TIM9_CLKIN +# define HRT_TIMER_CLOCK STM32_APB2_TIM9_CLKIN # if CONFIG_STM32_TIM9 # error must not set CONFIG_STM32_TIM9=y and HRT_TIMER=9 # endif #elif HRT_TIMER == 10 # define HRT_TIMER_BASE STM32_TIM10_BASE -# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR +# define HRT_TIMER_POWER_REG STM32_RCC_APB2ENR # define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM10EN # define HRT_TIMER_VECTOR STM32_IRQ_TIM1UP # define HRT_TIMER_CLOCK STM32_APB2_TIM10_CLKIN @@ -147,7 +147,7 @@ # endif #elif HRT_TIMER == 11 # define HRT_TIMER_BASE STM32_TIM11_BASE -# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR +# define HRT_TIMER_POWER_REG STM32_RCC_APB2ENR # define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM11EN # define HRT_TIMER_VECTOR STM32_IRQ_TIM1TRGCOM # define HRT_TIMER_CLOCK STM32_APB2_TIM11_CLKIN @@ -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 17f4c9362b..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,10 +117,26 @@ #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 # define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM2EN # ifdef CONFIG_STM32_TIM2 # error Must not set CONFIG_STM32_TIM2 when TONE_ALARM_TIMER is 2 @@ -128,6 +144,7 @@ #elif TONE_ALARM_TIMER == 3 # define TONE_ALARM_BASE STM32_TIM3_BASE # define TONE_ALARM_CLOCK STM32_APB1_TIM3_CLKIN +# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB1ENR # define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM3EN # ifdef CONFIG_STM32_TIM3 # error Must not set CONFIG_STM32_TIM3 when TONE_ALARM_TIMER is 3 @@ -135,6 +152,7 @@ #elif TONE_ALARM_TIMER == 4 # define TONE_ALARM_BASE STM32_TIM4_BASE # define TONE_ALARM_CLOCK STM32_APB1_TIM4_CLKIN +# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB1ENR # define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM4EN # ifdef CONFIG_STM32_TIM4 # error Must not set CONFIG_STM32_TIM4 when TONE_ALARM_TIMER is 4 @@ -142,33 +160,45 @@ #elif TONE_ALARM_TIMER == 5 # define TONE_ALARM_BASE STM32_TIM5_BASE # define TONE_ALARM_CLOCK STM32_APB1_TIM5_CLKIN +# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB1ENR # define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM5EN # 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_APB1_TIM9_CLKIN -# define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM9EN +# define TONE_ALARM_CLOCK STM32_APB2_TIM9_CLKIN +# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB2ENR +# define TONE_ALARM_CLOCK_ENABLE RCC_APB2ENR_TIM9EN # ifdef CONFIG_STM32_TIM9 # error Must not set CONFIG_STM32_TIM9 when TONE_ALARM_TIMER is 9 # endif #elif TONE_ALARM_TIMER == 10 # define TONE_ALARM_BASE STM32_TIM10_BASE -# define TONE_ALARM_CLOCK STM32_APB1_TIM10_CLKIN -# define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM10EN +# define TONE_ALARM_CLOCK STM32_APB2_TIM10_CLKIN +# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB2ENR +# define TONE_ALARM_CLOCK_ENABLE RCC_APB2ENR_TIM10EN # ifdef CONFIG_STM32_TIM10 # error Must not set CONFIG_STM32_TIM10 when TONE_ALARM_TIMER is 10 # endif #elif TONE_ALARM_TIMER == 11 # define TONE_ALARM_BASE STM32_TIM11_BASE -# define TONE_ALARM_CLOCK STM32_APB1_TIM11_CLKIN -# define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM11EN +# define TONE_ALARM_CLOCK STM32_APB2_TIM11_CLKIN +# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB2ENR +# define TONE_ALARM_CLOCK_ENABLE RCC_APB2ENR_TIM11EN # ifdef CONFIG_STM32_TIM11 # 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 @@ -201,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 { @@ -230,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) @@ -261,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); @@ -369,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(STM32_RCC_APB1ENR, 0, TONE_ALARM_CLOCK_ENABLE); + modifyreg32(TONE_ALARM_CLOCK_POWER_REG, 0, TONE_ALARM_CLOCK_ENABLE); /* initialise the timer */ rCR1 = 0; @@ -389,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; @@ -421,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; @@ -453,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; @@ -548,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: @@ -682,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; } @@ -697,6 +807,7 @@ ToneAlarm::next_char() while (isspace(*_next)) { _next++; } + return toupper(*_next); } @@ -708,8 +819,11 @@ ToneAlarm::next_number() for (;;) { c = next_char(); - if (!isdigit(c)) + + if (!isdigit(c)) { return number; + } + _next++; number = (number * 10) + (c - '0'); } @@ -724,6 +838,7 @@ ToneAlarm::next_dots() _next++; dots++; } + return dots; } @@ -757,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)) { @@ -765,6 +881,7 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) start_tune(_default_tunes[arg]); } } + } else { result = -EINVAL; } @@ -779,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; } @@ -789,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) { @@ -807,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); @@ -836,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); } @@ -855,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); } @@ -880,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; @@ -896,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..a88d55925c --- /dev/null +++ b/src/lib/dspal @@ -0,0 +1 @@ +Subproject commit a88d55925cc21d4f9ebc728deab85cbcbb218a52 diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index ec2d64ad83..cca01e6a3a 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -49,8 +49,7 @@ void TECS::update_state(float baro_altitude, float airspeed, const math::Matrix< bool reset_altitude = false; if (_update_50hz_last_usec == 0 || DT > DT_MAX) { - DT = 0.02f; // when first starting TECS, use a - // small time constant + DT = DT_DEFAULT; // when first starting TECS, use small time constant reset_altitude = true; } @@ -132,14 +131,6 @@ void TECS::_update_speed(float airspeed_demand, float indicated_airspeed, _TASmax = indicated_airspeed_max * EAS2TAS; _TASmin = indicated_airspeed_min * EAS2TAS; - // Reset states of time since last update is too large - if (_update_speed_last_usec == 0 || DT > 1.0f || !_in_air) { - _integ5_state = (_EAS * EAS2TAS); - _integ4_state = 0.0f; - DT = 0.1f; // when first starting TECS, use a - // small time constant - } - // Get airspeed or default to halfway between min and max if // airspeed is not being used and set speed rate to zero if (!PX4_ISFINITE(indicated_airspeed) || !airspeed_sensor_enabled()) { @@ -150,6 +141,16 @@ void TECS::_update_speed(float airspeed_demand, float indicated_airspeed, _EAS = indicated_airspeed; } + // Reset states on initial execution or if not active + if (_update_speed_last_usec == 0 || !_in_air) { + _integ4_state = 0.0f; + _integ5_state = (_EAS * EAS2TAS); + } + + if (DT < DT_MIN || DT > DT_MAX) { + DT = DT_DEFAULT; // when first starting TECS, use small time constant + } + // Implement a second order complementary filter to obtain a // smoothed airspeed estimate // airspeed estimate is held in _integ5_state @@ -440,9 +441,9 @@ void TECS::_update_pitch(void) float SPE_weighting = 2.0f - SKE_weighting; // Calculate Specific Energy Balance demand, and error - float SEB_dem = _SPE_dem * SPE_weighting - _SKE_dem * SKE_weighting; - float SEBdot_dem = _SPEdot_dem * SPE_weighting - _SKEdot_dem * SKE_weighting; - _SEB_error = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting); + float SEB_dem = _SPE_dem * SPE_weighting - _SKE_dem * SKE_weighting; + float SEBdot_dem = _SPEdot_dem * SPE_weighting - _SKEdot_dem * SKE_weighting; + _SEB_error = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting); _SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting); // Calculate integrator state, constraining input if pitch limits are exceeded @@ -495,22 +496,27 @@ void TECS::_update_pitch(void) void TECS::_initialise_states(float pitch, float throttle_cruise, float baro_altitude, float ptchMinCO_rad) { // Initialise states and variables if DT > 1 second or in climbout - if (_update_pitch_throttle_last_usec == 0 || _DT > 1.0f || !_in_air || !_states_initalized) { - _integ6_state = 0.0f; - _integ7_state = 0.0f; + if (_update_pitch_throttle_last_usec == 0 || _DT > DT_MAX || !_in_air || !_states_initalized) { + _integ1_state = 0.0f; + _integ2_state = 0.0f; + _integ3_state = baro_altitude; + _integ4_state = 0.0f; + _integ5_state = _EAS; + _integ6_state = 0.0f; + _integ7_state = 0.0f; _last_throttle_dem = throttle_cruise; - _last_pitch_dem = pitch; - _hgt_dem_adj_last = baro_altitude; - _hgt_dem_adj = _hgt_dem_adj_last; - _hgt_dem_prev = _hgt_dem_adj_last; - _hgt_dem_in_old = _hgt_dem_adj_last; - _TAS_dem_last = _TAS_dem; - _TAS_dem_adj = _TAS_dem; - _underspeed = false; - _badDescent = false; + _last_pitch_dem = pitch; + _hgt_dem_adj_last = baro_altitude; + _hgt_dem_adj = _hgt_dem_adj_last; + _hgt_dem_prev = _hgt_dem_adj_last; + _hgt_dem_in_old = _hgt_dem_adj_last; + _TAS_dem_last = _TAS_dem; + _TAS_dem_adj = _TAS_dem; + _underspeed = false; + _badDescent = false; - if (_DT > 1.0f || _DT < 0.001f) { - _DT = DT_MIN; + if (_DT > DT_MAX || _DT < DT_MIN) { + _DT = DT_DEFAULT; } } else if (_climbOutDem) { @@ -549,8 +555,8 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f // _DT, pitch, baro_altitude, hgt_dem, EAS_dem, indicated_airspeed, EAS2TAS, (climbOutDem) ? "climb" : "level", ptchMinCO, throttle_min, throttle_max, throttle_cruise, pitch_limit_min, pitch_limit_max); // Convert inputs - _THRmaxf = throttle_max; - _THRminf = throttle_min; + _THRmaxf = throttle_max; + _THRminf = throttle_min; _PITCHmaxf = pitch_limit_max; _PITCHminf = pitch_limit_min; _climbOutDem = climbOutDem; diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 2599e7a152..914e41eba0 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -60,6 +60,7 @@ public: _integ7_state(0.0f), _last_pitch_dem(0.0f), _vel_dot(0.0f), + _EAS(0.0f), _TAS_dem(0.0f), _TAS_dem_last(0.0f), _hgt_dem_in_old(0.0f), @@ -396,7 +397,8 @@ private: // Time since last update of main TECS loop (seconds) float _DT; - static constexpr float DT_MIN = 0.1f; + static constexpr float DT_MIN = 0.001f; + static constexpr float DT_DEFAULT = 0.02f; static constexpr float DT_MAX = 1.0f; bool _airspeed_enabled; 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/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index a39cca335b..b81f8868ec 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -375,7 +375,7 @@ public: printf("[ "); for (unsigned int j = 0; j < N; j++) - printf("%.3f\t", data[i][j]); + printf("%.3f\t", (double)data[i][j]); printf(" ]\n"); } 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..f9477d6f7f 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 */ @@ -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/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/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/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 3c5eb1c04a..5f3634d22b 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 @@ -944,9 +944,14 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() const float dtLastGoodGPS = static_cast(hrt_absolute_time() - _previousGPSTimestamp) / 1e6f; - if (_gps.timestamp_position == 0 || (dtLastGoodGPS >= POS_RESET_THRESHOLD)) { + if (!_local_pos.xy_global || + !_local_pos.v_xy_valid || + _gps.timestamp_position == 0 || + (dtLastGoodGPS >= POS_RESET_THRESHOLD)) { + _global_pos.eph = EPH_LARGE_VALUE; _global_pos.epv = EPV_LARGE_VALUE; + } else { _global_pos.eph = _gps.eph; _global_pos.epv = _gps.epv; 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 f496f9ed96..943365e499 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 @@ -214,23 +214,23 @@ PARAM_DEFINE_FLOAT(PE_ACC_PNOISE, 0.25f); * Generic defaults: 1e-07f, multicopters: 1e-07f, ground vehicles: 1e-07f. * Increasing this value will make the gyro bias converge faster but noisier. * - * @min 0.0000001 + * @min 0.00000005 * @max 0.00001 * @group Position Estimator */ -PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-06f); +PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-07f); /** * Accelerometer bias estimate process noise * - * Generic defaults: 0.0001f, multicopters: 0.0001f, ground vehicles: 0.0001f. + * Generic defaults: 0.00001f, multicopters: 0.00001f, ground vehicles: 0.00001f. * Increasing this value makes the bias estimation faster and noisier. * * @min 0.00001 * @max 0.001 * @group Position Estimator */ -PARAM_DEFINE_FLOAT(PE_ABIAS_PNOISE, 0.0002f); +PARAM_DEFINE_FLOAT(PE_ABIAS_PNOISE, 1e-05f); /** * Magnetometer earth frame offsets process noise 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_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index b6265ffc06..d06e9b9a74 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -648,8 +648,8 @@ FixedwingAttitudeControl::task_main() vehicle_manual_poll(); vehicle_status_poll(); - /* wakeup source(s) */ - struct pollfd fds[2]; + /* wakeup source */ + px4_pollfd_struct_t fds[2]; /* Setup of loop */ fds[0].fd = _params_sub; @@ -660,11 +660,10 @@ FixedwingAttitudeControl::task_main() _task_running = true; while (!_task_should_exit) { - static int loop_counter = 0; /* wait for up to 500ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit, etc. */ if (pret == 0) { @@ -691,7 +690,6 @@ FixedwingAttitudeControl::task_main() /* only run controller if attitude changed */ if (fds[1].revents & POLLIN) { - static uint64_t last_run = 0; float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); @@ -703,6 +701,7 @@ FixedwingAttitudeControl::task_main() /* load local copies */ orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + if (_vehicle_status.is_vtol && _parameters.vtol_type == 0) { /* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode * 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 73c2bb07ac..d5815629f0 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 @@ -1363,7 +1363,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 */ @@ -1719,7 +1719,7 @@ FixedwingPositionControl::task_main() } /* wakeup source(s) */ - struct pollfd fds[2]; + px4_pollfd_struct_t fds[2]; /* Setup of loop */ fds[0].fd = _params_sub; @@ -1732,7 +1732,7 @@ FixedwingPositionControl::task_main() while (!_task_should_exit) { /* wait for up to 500ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit, etc. */ if (pret == 0) { diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 6fa5918fc5..ea810c8233 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); @@ -1494,10 +1496,6 @@ Mavlink::task_main(int argc, char *argv[]) _forwarding_on = true; break; - case 'p': - _passing_on = true; - break; - case 'v': _verbose = true; break; @@ -1543,11 +1541,6 @@ Mavlink::task_main(int argc, char *argv[]) /* flush stdout in case MAVLink is about to take it over */ fflush(stdout); - /* init socket if necessary */ - if (get_protocol() == UDP) { - init_udp(); - } - #ifndef __PX4_POSIX struct termios uart_config_original; @@ -1568,7 +1561,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. @@ -1649,7 +1642,8 @@ Mavlink::task_main(int argc, char *argv[]) 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("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); @@ -1671,6 +1665,7 @@ Mavlink::task_main(int argc, char *argv[]) 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); @@ -1690,6 +1685,7 @@ Mavlink::task_main(int argc, char *argv[]) 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; @@ -1703,7 +1699,7 @@ Mavlink::task_main(int argc, char *argv[]) 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); @@ -1724,6 +1720,11 @@ Mavlink::task_main(int argc, char *argv[]) /* now the instance is fully initialized and we can bump the instance count */ LL_APPEND(_mavlink_instances, this); + /* init socket if necessary */ + if (get_protocol() == UDP) { + init_udp(); + } + /* if the protocol is serial, we send the system version blindly */ if (get_protocol() == SERIAL) { send_autopilot_capabilites(); @@ -1835,7 +1836,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; @@ -1944,7 +1945,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..2829b97cf3 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -2057,7 +2057,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); } 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 24d0940477..58124f2133 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -990,8 +990,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 +1101,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); @@ -1761,10 +1745,17 @@ MavlinkReceiver::receive_thread(void *arg) #ifdef __PX4_POSIX struct sockaddr_in srcaddr; socklen_t addrlen = sizeof(srcaddr); + if (_mavlink->get_protocol() == UDP || _mavlink->get_protocol() == TCP) { + // make sure mavlink app has booted before we start using the socket + while (!_mavlink->boot_complete()) { + usleep(100000); + } + fds[0].fd = _mavlink->get_socket_fd(); fds[0].events = POLLIN; } + #endif ssize_t nread = 0; @@ -1779,13 +1770,19 @@ MavlinkReceiver::receive_thread(void *arg) } #ifdef __PX4_POSIX if (_mavlink->get_protocol() == UDP) { - if (fds[0].revents & POLLIN) { nread = recvfrom(_mavlink->get_socket_fd(), buf, sizeof(buf), 0, (struct sockaddr *)&srcaddr, &addrlen); } } 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 9af68a4e1e..8d9fb6b911 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -36,8 +36,8 @@ * Parameters for multicopter attitude controller. * * @author Tobias Naegeli - * @author Lorenz Meier - * @author Anton Babushkin + * @author Lorenz Meier + * @author Anton Babushkin */ #include @@ -60,7 +60,7 @@ PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.5f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.12f); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.15f); /** * Roll rate I gain @@ -111,7 +111,7 @@ PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.5f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.12f); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.15f); /** * Pitch rate I gain @@ -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/mission.cpp b/src/modules/navigator/mission.cpp index c988faab7d..e4068cac38 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -625,26 +625,29 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s { /* select onboard/offboard mission */ int *mission_index_ptr; - struct mission_s *mission; dm_item_t dm_item; - int mission_index_next; + + struct mission_s *mission = (onboard) ? &_onboard_mission : &_offboard_mission; + int mission_index_next = (onboard) ? _current_onboard_mission_index : _current_offboard_mission_index; + + /* do not work on empty missions */ + if (mission->count == 0) { + return false; + } + + /* move to next item if there is one */ + if (mission_index_next < ((int)mission->count - 1)) { + mission_index_next++; + } if (onboard) { /* onboard mission */ - mission_index_next = _current_onboard_mission_index + 1; mission_index_ptr = is_current ? &_current_onboard_mission_index : &mission_index_next; - - mission = &_onboard_mission; - dm_item = DM_KEY_WAYPOINTS_ONBOARD; } else { /* offboard mission */ - mission_index_next = _current_offboard_mission_index + 1; mission_index_ptr = is_current ? &_current_offboard_mission_index : &mission_index_next; - - mission = &_offboard_mission; - dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); } @@ -654,7 +657,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) { /* mission item index out of bounds */ - mavlink_log_critical(_navigator->get_mavlink_fd(), "[wpm] err: index: %d, max: %d", *mission_index_ptr, (int)mission->count); + mavlink_and_console_log_critical(_navigator->get_mavlink_fd(), "[wpm] err: index: %d, max: %d", *mission_index_ptr, (int)mission->count); return false; } @@ -666,7 +669,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s /* read mission item from datamanager */ if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - mavlink_log_critical(_navigator->get_mavlink_fd(), + mavlink_and_console_log_critical(_navigator->get_mavlink_fd(), "ERROR waypoint could not be read"); return false; } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index d63f5fe338..454326a923 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -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/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index c6d13f8756..a81ab9fdb1 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1272,13 +1272,16 @@ 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); 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/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 0009d9e1de..af4b8a746c 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -43,6 +43,7 @@ #include #include #include +#include #include #include #include @@ -93,6 +94,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; @@ -190,12 +198,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; } @@ -206,6 +216,7 @@ private: _baro(1), _mag(1), _gps(1), + _airspeed(1), _accel_pub(nullptr), _baro_pub(nullptr), _gyro_pub(nullptr), @@ -217,10 +228,12 @@ private: _actuator_outputs_sub(-1), _vehicle_attitude_sub(-1), _manual_sub(-1), + _vehicle_status_sub(-1), _rc_input{}, _actuators{}, _attitude{}, - _manual{} + _manual{}, + _vehicle_status{} #endif {} ~Simulator() { _instance=NULL; } @@ -235,6 +248,7 @@ private: simulator::Report _baro; simulator::Report _mag; simulator::Report _gps; + simulator::Report _airspeed; // uORB publisher handlers orb_advert_t _accel_pub; @@ -255,14 +269,16 @@ private: int _actuator_outputs_sub; int _vehicle_attitude_sub; int _manual_sub; + int _vehicle_status_sub; // uORB data containers struct rc_input_values _rc_input; struct actuator_outputs_s _actuators; struct vehicle_attitude_s _attitude; struct manual_control_setpoint_s _manual; + struct vehicle_status_s _vehicle_status; - void poll_actuators(); + void poll_topics(); void handle_message(mavlink_message_t *msg, bool publish); void send_controls(); void pollForMAVLinkMessages(bool publish); diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 7b6b4594f0..ddbc79db76 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -64,33 +64,46 @@ static socklen_t _addrlen = sizeof(_srcaddr); using namespace simulator; void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) { - float out[8]; + float out[8] = {}; const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; // for now we only support quadrotors unsigned n = 4; - for (unsigned i = 0; i < 8; i++) { - if (_actuators.output[i] > PWM_LOWEST_MIN / 2) { - if (i < n) { - // scale PWM out 900..2100 us to 0..1 for rotors */ - out[i] = (_actuators.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + if (_vehicle_status.is_rotary_wing) { + for (unsigned i = 0; i < 8; i++) { + if (_actuators.output[i] > PWM_LOWEST_MIN / 2) { + if (i < n) { + // scale PWM out 900..2100 us to 0..1 for rotors */ + out[i] = (_actuators.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + + } else { + // scale PWM out 900..2100 us to -1..1 for other channels */ + out[i] = (_actuators.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); + } } else { - // scale PWM out 900..2100 us to -1..1 for other channels */ - out[i] = (_actuators.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); + // send 0 when disarmed and for disabled channels */ + out[i] = 0.0f; } - - } else { - // send 0 when disarmed and for disabled channels */ - out[i] = 0.0f; } + } else { + // convert back to range [-1, 1] + for (unsigned i = 0; i < 8; i++) { + out[i] = (_actuators.output[i] - 1500) / 600.0f; + } + } + + // if vehicle status has not yet been updated, set actuator commands to zero + // this is to prevent the simulation getting into a bad state + if (_vehicle_status.timestamp == 0) { + memset(out, 0, sizeof(out)); } actuator_msg.time_usec = hrt_absolute_time(); actuator_msg.roll_ailerons = out[0]; - actuator_msg.pitch_elevator = out[1]; + actuator_msg.pitch_elevator = _vehicle_status.is_rotary_wing ? out[1] : -out[1]; actuator_msg.yaw_rudder = out[2]; actuator_msg.throttle = out[3]; actuator_msg.aux1 = out[4]; @@ -174,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) { @@ -270,14 +289,18 @@ void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8 } } -void Simulator::poll_actuators() { +void Simulator::poll_topics() { // copy new actuator data if available bool updated; orb_check(_actuator_outputs_sub, &updated); - if(updated) { - //PX4_WARN("Received actuator_output0 orb_topic"); + if (updated) { orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuators); } + + orb_check(_vehicle_status_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + } } void *Simulator::sending_trampoline(void *) { @@ -310,7 +333,7 @@ void Simulator::send() { if (fds[0].revents & POLLIN) { // got new data to read, update all topics - poll_actuators(); + poll_topics(); send_controls(); } } @@ -419,6 +442,7 @@ void Simulator::pollForMAVLinkMessages(bool publish) // subscribe to topics _actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs), 0); + _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); // got data from simulator, now activate the sending thread pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, NULL); 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 097903d21f..20b9638d6a 100644 --- a/src/modules/systemlib/circuit_breaker_params.c +++ b/src/modules/systemlib/circuit_breaker_params.c @@ -73,7 +73,7 @@ PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0); /** * Circuit breaker for IO safety * - * Setting this parameter to 894281 will disable IO safety. + * Setting this parameter to 22027 will disable IO safety. * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK * * @min 0 @@ -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 1344accb84..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,76 +374,76 @@ 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; + } } } void perf_print_counter(perf_counter_t handle) { - perf_print_counter_fd(0, handle); + perf_print_counter_fd(1, 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 d292628be0..5a13a864c1 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -260,6 +260,20 @@ int orb_exists(const struct orb_metadata *meta, int instance) return uORB::Manager::get_instance()->orb_exists(meta, instance); } +/** + * Get the number of published instances of a topic group + * + * @param meta ORB topic metadata. + * @return The number of published instances of this topic + */ +int orb_group_count(const struct orb_metadata *meta) +{ + unsigned group_count = 0; + while (!uORB::Manager::get_instance()->orb_exists(meta, group_count++)) {}; + + return group_count; +} + /** * Return the priority of the topic * @@ -270,7 +284,7 @@ int orb_exists(const struct orb_metadata *meta, int instance) * 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/uORB.h b/src/modules/uORB/uORB.h index 7b40cff219..c887925fcf 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -328,6 +328,14 @@ extern int orb_stat(int handle, uint64_t *time) __EXPORT; */ extern int orb_exists(const struct orb_metadata *meta, int instance) __EXPORT; +/** + * Get the number of published instances of a topic group + * + * @param meta ORB topic metadata. + * @return The number of published instances of this topic + */ +extern int orb_group_count(const struct orb_metadata *meta) __EXPORT; + /** * Return the priority of the topic * 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/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 9f914ed693..be2eae5090 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** * @file main.cpp - * Basic shell to execute builtin "apps" + * Basic shell to execute builtin "apps" * * @author Mark Charlebois */ @@ -42,8 +42,10 @@ #include #include #include +#include -namespace px4 { +namespace px4 +{ void init_once(void); } @@ -54,64 +56,166 @@ typedef int (*px4_main_t)(int argc, char *argv[]); #include "apps.h" #include "px4_middleware.h" -static void run_cmd(const vector &appargs) { +static bool _ExitFlag = false; +extern "C" { + void _SigIntHandler(int sig_num); + void _SigIntHandler(int sig_num) + { + _ExitFlag = true; + } +} + +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]; + const char *arg[appargs.size() + 2]; unsigned int i = 0; + while (i < appargs.size() && appargs[i] != "") { arg[i] = (char *)appargs[i].c_str(); ++i; } + arg[i] = (char *)0; - cout << "Running: " << command << "\n"; - apps[command](i,(char **)arg); - usleep(40000); - cout << "Returning: " << command << "\n"; + apps[command](i, (char **)arg); + 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() +{ + + cout << "./mainapp [-d] [startup_config] -h" << std::endl; + cout << " -d - Optional flag to run the app in daemon mode and does not take listen for user input." << + std::endl; + cout << " This is needed if mainapp is intended to be run as a upstart job on linux" << std::endl; + cout << " - config file for starting/stopping px4 modules" << std::endl; + cout << " -h - help/usage information" << std::endl; } static void process_line(string &line) { vector appargs(8); - stringstream(line) >> appargs[0] >> appargs[1] >> appargs[2] >> appargs[3] >> appargs[4] >> appargs[5] >> appargs[6] >> appargs[7]; + stringstream(line) >> appargs[0] >> appargs[1] >> appargs[2] >> appargs[3] >> appargs[4] >> appargs[5] >> appargs[6] >> + appargs[7]; run_cmd(appargs); } int main(int argc, char **argv) { - px4::init_once(); + bool daemon_mode = false; + signal(SIGINT, _SigIntHandler); - px4::init(argc, argv, "mainapp"); + int index = 1; + bool error_detected = false; + char *commands_file = nullptr; - // Execute a command list of provided - if (argc == 2) { - ifstream infile(argv[1]); + while (index < argc) { + if (argv[index][0] == '-') { + // the arg starts with - + if (strcmp(argv[index], "-d") == 0) { + daemon_mode = true; - if (!infile) { - cout << "failed opening script" << argv[1] << std::endl; - return 1; + } else if (strcmp(argv[index], "-h") == 0) { + usage(); + return 0; + + } else { + PX4_WARN("Unknown/unhandled parameter: %s", argv[index]); + return 1; + } + + } else { + // this is an argument that does not have '-' prefix; treat it like a file name + ifstream infile(argv[index]); + + if (infile.good()) { + infile.close(); + commands_file = argv[index]; + + } else { + PX4_WARN("Error opening file: %s", argv[index]); + error_detected = true; + break; + } } - for (string line; getline(infile, line, '\n'); ) { - process_line(line); - } + ++index; } - string mystr; - - while(1) { - cout << "Enter a command and its args:" << endl; - getline (cin,mystr); - process_line(mystr); - mystr = ""; + if (!error_detected) { + px4::init_once(); + + px4::init(argc, argv, "mainapp"); + + //if commandfile is present, process the commands from the file + if (commands_file != nullptr) { + ifstream infile(commands_file); + + if (infile.is_open()) { + for (string line; getline(infile, line, '\n');) { + process_line(line); + } + + } else { + PX4_WARN("Error opening file: %s", commands_file); + } + } + + if (!daemon_mode) { + string mystr; + + print_prompt(); + + while (!_ExitFlag) { + + 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) { + usleep(100000); + } + } + + if (px4_task_is_running("muorb")) { + // sending muorb stop is needed if it is running to exit cleanly + vector muorb_stop_cmd = { "muorb", "stop" }; + run_cmd(muorb_stop_cmd); + } + + vector shutdown_cmd = { "shutdown" }; + run_cmd(shutdown_cmd); } } diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp index 44f9ceff88..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; @@ -280,6 +280,17 @@ void px4_show_tasks() } +bool px4_task_is_running(const char *taskname) +{ + int idx; + for (idx=0; idx < PX4_MAX_TASKS; idx++) + { + if (taskmap[idx].isused && (strcmp(taskmap[idx].name.c_str(), taskname) == 0)) { + return true; + } + } + return false; +} __BEGIN_DECLS unsigned long px4_getpid() diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index d071893b0c..a8208313e6 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_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 @@ -162,22 +163,6 @@ __EXPORT extern int __px4_log_level_current; ****************************************************************************/ -/**************************************************************************** - * __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_named_cond: * Convert a message in the form: @@ -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 8dde7cfdd5..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 @@ -117,5 +117,8 @@ __EXPORT void px4_task_exit(int ret); /** Show a list of running tasks **/ __EXPORT void px4_show_tasks(void); +/** See if a task is running **/ +__EXPORT bool px4_task_is_running(const char *taskname); + __END_DECLS 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/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..8da9580207 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; @@ -253,6 +266,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 +274,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 +302,11 @@ esc_calib_main(int argc, char *argv[]) /* set max PWM */ for (unsigned i = 0; i < max_channels; i++) { - if (set_mask & 1< \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; } diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index 80eb8c24e6..4285520b0c 100644 --- a/unittests/CMakeLists.txt +++ b/unittests/CMakeLists.txt @@ -11,6 +11,13 @@ CMAKE_FORCE_CXX_COMPILER( clang++ Clang ) #set( CMAKE_FIND_ROOT_PATH_MODE_LIBARARY_ONLY ) #set( CMAKE_FIND_ROOT_PATH_MODE_INCLUDE_ONLY ) +if("${CMAKE_C_COMPILER_ID}" STREQUAL "Clang") + add_compile_options(-Qunused-arguments) +endif() +if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") + add_compile_options(-Qunused-arguments) +endif() + project(unittests) enable_testing() @@ -123,8 +130,7 @@ add_executable(mixer_test mixer_test.cpp hrt.cpp ${PX_SRC}/modules/systemlib/mixer/mixer_simple.cpp ${PX_SRC}/modules/systemlib/pwm_limit/pwm_limit.c ${PX_SRC}/systemcmds/tests/test_mixer.cpp) - -target_link_libraries( mixer_test LINK_PUBLIC px4_platform ) +target_link_libraries( mixer_test px4_platform ) add_gtest(mixer_test) diff --git a/unittests/sbus2_test.cpp b/unittests/sbus2_test.cpp index 1c32e3bf84..54c7fc412b 100644 --- a/unittests/sbus2_test.cpp +++ b/unittests/sbus2_test.cpp @@ -33,7 +33,7 @@ TEST(SBUS2Test, SBUS2) // Init the parser uint8_t frame[30]; unsigned partial_frame_count = 0; - uint16_t rc_values[18]; + //uint16_t rc_values[18]; //uint16_t num_values; //bool sbus_failsafe; //bool sbus_frame_drop; diff --git a/unittests/st24_test.cpp b/unittests/st24_test.cpp index 42fa07ae74..c7fded68d4 100644 --- a/unittests/st24_test.cpp +++ b/unittests/st24_test.cpp @@ -41,7 +41,7 @@ TEST(ST24Test, ST24) last_time = f; // Pipe the data into the parser - hrt_abstime now = hrt_absolute_time(); + //hrt_abstime now = hrt_absolute_time(); uint8_t rssi; uint8_t rx_count; @@ -53,8 +53,7 @@ TEST(ST24Test, ST24) //warnx("decoded: %u channels (converted to PPM range)", (unsigned)channel_count); for (unsigned i = 0; i < channel_count; i++) { - - int16_t val = channels[i]; + //int16_t val = channels[i]; //warnx("channel %u: %d 0x%03X", i, static_cast(val), static_cast(val)); } } diff --git a/unittests/sumd_test.cpp b/unittests/sumd_test.cpp index bf99709475..65c944676c 100644 --- a/unittests/sumd_test.cpp +++ b/unittests/sumd_test.cpp @@ -41,7 +41,7 @@ TEST(SUMDTest, SUMD) last_time = f; // Pipe the data into the parser - hrt_abstime now = hrt_absolute_time(); + //hrt_abstime now = hrt_absolute_time(); uint8_t rssi; uint8_t rx_count; @@ -53,8 +53,7 @@ TEST(SUMDTest, SUMD) //warnx("decoded: %u channels (converted to PPM range)", (unsigned)channel_count); for (unsigned i = 0; i < channel_count; i++) { - - int16_t val = channels[i]; + //int16_t val = channels[i]; //warnx("channel %u: %d 0x%03X", i, static_cast(val), static_cast(val)); } } diff --git a/unittests/uorb_unittests/uORBCommunicatorMock.hpp b/unittests/uorb_unittests/uORBCommunicatorMock.hpp index 8c5b861b1e..acffbdb7ab 100644 --- a/unittests/uorb_unittests/uORBCommunicatorMock.hpp +++ b/unittests/uorb_unittests/uORBCommunicatorMock.hpp @@ -126,8 +126,8 @@ class uORB_test::uORBCommunicatorMock : public uORBCommunicator::IChannel private: uORBCommunicator::IChannelRxHandler* _rx_handler; - int _sub_topicA_copy_fd; - int _sub_topicB_copy_fd; + //int _sub_topicA_copy_fd; + //int _sub_topicB_copy_fd; std::map _topic_translation_map;