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

Merge branch 'master' of github.com:PX4/Firmware

Browse Source
This commit is contained in:
Youssef Demitri
2015-09-14 16:25:52 +02:00
parent 9d5ea4bceb 683df0eb46
commit fa2518ac22
171 changed files with 4825 additions and 2650 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitmodules
+3
View File
@@ -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
.travis.yml
+11
View File
@@ -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'
Makefile
+3 -3
View File
@@ -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
ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil
-3
View File
@@ -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
ROMFS/px4fmu_common/init.d/10016_3dr_iris
+2 -2
View File
@@ -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
ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil
-3
View File
@@ -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
ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil
-3
View File
@@ -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
ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil
-3
View File
@@ -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
ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil
-3
View File
@@ -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
ROMFS/px4fmu_common/init.d/4010_dji_f330
+2 -2
View File
@@ -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
ROMFS/px4fmu_common/init.d/rc.fw_defaults
-2
View File
@@ -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
ROMFS/px4fmu_common/init.d/rc.mc_defaults
-2
View File
@@ -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
ROMFS/px4fmu_common/init.d/rc.sensors
+2 -4
View File
@@ -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
ROMFS/px4fmu_common/init.d/rc.vtol_defaults
-1
View File
@@ -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
ROMFS/px4fmu_common/init.d/rcS
-2
View File
@@ -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
#
Tools/check_code_style.sh
+38 -9
View File
@@ -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
Tools/fix_code_style.sh
+11 -1
View File
@@ -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 \
makefiles/firmware.mk
+2 -4
View File
@@ -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) \
makefiles/module.mk
+4
View File
@@ -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
makefiles/nuttx/firmware_nuttx.mk
+6 -6
View File
@@ -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
makefiles/nuttx/toolchain_gnu-arm-eabi.mk
-2
View File
@@ -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
makefiles/posix-arm/config_eagle_adsp.mk
+57
View File
@@ -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
makefiles/posix-arm/config_eagle_hil.mk
+1
View File
@@ -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
makefiles/posix-arm/toolchain_gnu-arm-linux-gnueabihf.mk
+23 -50
View File
@@ -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
#
makefiles/posix/config_posix_sitl.mk
+1
View File
@@ -5,6 +5,7 @@
#
# Board support modules
#
MODULES += drivers/boards/sitl
MODULES += drivers/device
MODULES += drivers/blinkm
MODULES += drivers/pwm_out_sim
makefiles/posix/firmware_posix.mk
+1 -1
View File
@@ -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) \
makefiles/posix/toolchain_native.mk
+1 -4
View File
@@ -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
makefiles/qurt/config_qurt_adsp.mk
+19 -4
View File
@@ -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
makefiles/qurt/config_qurt_hil.mk
+1
View File
@@ -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
makefiles/qurt/firmware_qurt.mk
+2 -3
View File
@@ -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
makefiles/qurt/toolchain_hexagon.mk
+7 -19
View File
@@ -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 \
msg/input_rc.msg
+1
View File
@@ -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.
nuttx-configs/px4fmu-v2/nsh/defconfig
+2 -2
View File
@@ -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
posix-configs/SITL/init/rc.fixed_wing
+6 -3
View File
@@ -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
posix-configs/SITL/init/rcS
+4 -2
View File
@@ -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
posix-configs/SITL/init/rc_iris_ros
+3 -2
View File
@@ -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
src/drivers/blinkm/blinkm.cpp
+161 -95
View File
@@ -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("<blinkm> 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("<blinkm> 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, " <html color number>\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);
src/drivers/bma180/bma180.cpp
+76 -46
View File
@@ -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'");
}
src/drivers/boards/sitl/module.mk
+8
View File
@@ -0,0 +1,8 @@
#
# Board-specific startup code for SITL
#
SRCS = \
sitl_led.c
MAXOPTIMIZATION = -Os
src/drivers/boards/sitl/sitl_led.c
+81
View File
@@ -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 <px4_config.h>
#include <px4_log.h>
#include <stdbool.h>
__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");
}
}
src/drivers/device/vdev_posix.cpp
+1 -1
View File
@@ -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;
}
src/drivers/irlock/irlock.cpp
+2 -2
View File
@@ -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;
src/drivers/ll40ls/ll40ls.cpp
+4 -1
View File
@@ -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);
}
src/drivers/md25/md25.cpp
+14 -12
View File
@@ -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");
src/drivers/md25/md25_main.cpp
+8 -7
View File
@@ -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) {
src/drivers/ms5611/ms5611.h
+1 -1
View File
@@ -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);
src/drivers/ms5611/ms5611_i2c.cpp
+19 -15
View File
@@ -31,11 +31,11 @@
*
****************************************************************************/
/**
* @file ms5611_i2c.cpp
*
* I2C interface for MS5611
*/
/**
* @file ms5611_i2c.cpp
*
* I2C interface for MS5611
*/
/* XXX trim includes */
#include <px4_config.h>
@@ -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];
src/drivers/ms5611/ms5611_nuttx.cpp
+101 -51
View File
@@ -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; i<NUM_BUS_OPTIONS; i++) {
for (i = 0; i < NUM_BUS_OPTIONS; i++) {
if (busid == MS5611_BUS_ALL && bus_options[i].dev != NULL) {
// this device is already started
continue;
}
if (busid != MS5611_BUS_ALL && bus_options[i].busid != busid) {
// not the one that is asked for
continue;
}
started |= start_bus(bus_options[i]);
}
if (!started)
if (!started) {
errx(1, "driver start failed");
}
// one or more drivers started OK
exit(0);
@@ -954,13 +980,14 @@ start(enum MS5611_BUS busid)
*/
struct ms5611_bus_option &find_bus(enum MS5611_BUS busid)
{
for (uint8_t i=0; i<NUM_BUS_OPTIONS; i++) {
for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) {
if ((busid == MS5611_BUS_ALL ||
busid == bus_options[i].busid) && bus_options[i].dev != NULL) {
return bus_options[i];
}
}
errx(1,"bus %u not started", (unsigned)busid);
errx(1, "bus %u not started", (unsigned)busid);
}
/**
@@ -979,14 +1006,17 @@ test(enum MS5611_BUS busid)
int fd;
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)");
}
/* do a simple demand read */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report))
if (sz != sizeof(report)) {
err(1, "immediate read failed");
}
warnx("single read");
warnx("pressure: %10.4f", (double)report.pressure);
@@ -995,12 +1025,14 @@ test(enum MS5611_BUS busid)
warnx("time: %lld", report.timestamp);
/* set the queue depth to 10 */
if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10))
if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) {
errx(1, "failed to set queue depth");
}
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
errx(1, "failed to set 2Hz poll rate");
}
/* read the sensor 5x and report each value */
for (unsigned i = 0; i < 5; i++) {
@@ -1011,14 +1043,16 @@ test(enum MS5611_BUS busid)
fds.events = POLLIN;
ret = poll(&fds, 1, 2000);
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, "periodic read failed");
}
warnx("periodic read %u", i);
warnx("pressure: %10.4f", (double)report.pressure);
@@ -1041,14 +1075,18 @@ reset(enum MS5611_BUS busid)
int fd;
fd = open(bus.devpath, 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);
}
@@ -1059,13 +1097,15 @@ reset(enum MS5611_BUS busid)
void
info()
{
for (uint8_t i=0; i<NUM_BUS_OPTIONS; i++) {
for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) {
struct ms5611_bus_option &bus = bus_options[i];
if (bus.dev != nullptr) {
warnx("%s", bus.devpath);
bus.dev->print_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);
}
src/drivers/ms5611/ms5611_posix.cpp
+90 -43
View File
@@ -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; i<NUM_BUS_OPTIONS; i++) {
for (i = 0; i < NUM_BUS_OPTIONS; i++) {
if (busid == MS5611_BUS_ALL && bus_options[i].dev != NULL) {
// this device is already started
continue;
}
if (busid != MS5611_BUS_ALL && bus_options[i].busid != busid) {
// not the one that is asked for
continue;
}
started |= start_bus(bus_options[i]);
}
@@ -968,15 +992,15 @@ start(enum MS5611_BUS busid)
*/
bool find_bus(enum MS5611_BUS busid, ms5611_bus_option &bus)
{
for (uint8_t i=0; i<NUM_BUS_OPTIONS; i++) {
for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) {
if ((busid == MS5611_BUS_ALL ||
busid == bus_options[i].busid) && bus_options[i].dev != NULL) {
bus = bus_options[i];
return true;
return true;
}
}
PX4_WARN("bus %u not started", (unsigned)busid);
PX4_WARN("bus %u not started", (unsigned)busid);
return false;
}
@@ -989,17 +1013,21 @@ int
test(enum MS5611_BUS busid)
{
struct ms5611_bus_option bus;
if (!find_bus(busid, bus)) {
return 1;
}
struct baro_report report;
ssize_t sz;
int ret;
int fd;
fd = px4_open(bus.devpath, O_RDONLY);
if (fd < 0) {
warn("open failed (try 'ms5611 start' if the driver is not running)");
return 1;
@@ -1071,6 +1099,7 @@ int
reset(enum MS5611_BUS busid)
{
struct ms5611_bus_option bus;
if (!find_bus(busid, bus)) {
return 1;
}
@@ -1078,6 +1107,7 @@ reset(enum MS5611_BUS busid)
int fd;
fd = open(bus.devpath, O_RDONLY);
if (fd < 0) {
warn("failed ");
return 1;
@@ -1092,6 +1122,7 @@ reset(enum MS5611_BUS busid)
warn("driver poll restart failed");
return 1;
}
return 0;
}
@@ -1101,13 +1132,15 @@ reset(enum MS5611_BUS busid)
int
info()
{
for (uint8_t i=0; i<NUM_BUS_OPTIONS; i++) {
for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) {
struct ms5611_bus_option &bus = bus_options[i];
if (bus.dev != nullptr) {
warnx("%s", bus.devpath);
bus.dev->print_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;
}
src/drivers/ms5611/ms5611_sim.cpp
+8 -7
View File
@@ -31,11 +31,11 @@
*
****************************************************************************/
/**
* @file ms5611_i2c.cpp
*
* SIM interface for MS5611
*/
/**
* @file ms5611_i2c.cpp
*
* SIM interface for MS5611
*/
/* XXX trim includes */
#include <px4_config.h>
@@ -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
src/drivers/ms5611/ms5611_spi.cpp
+27 -14
View File
@@ -31,11 +31,11 @@
*
****************************************************************************/
/**
* @file ms5611_spi.cpp
*
* SPI interface for MS5611
*/
/**
* @file ms5611_spi.cpp
*
* SPI interface for MS5611
*/
/* XXX trim includes */
#include <px4_config.h>
@@ -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
src/drivers/pca8574/pca8574.cpp
+9
View File
@@ -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);
}
src/drivers/pca9685/pca9685.cpp
+49 -12
View File
@@ -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);
src/drivers/pwm_input/pwm_input.cpp
+9 -9
View File
@@ -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
src/drivers/px4fmu/fmu.cpp
+117 -64
View File
@@ -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 <roll> <pitch> <yaw> <thrust> (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: <bus id> <clock Hz>");
}
@@ -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 <bus> <hz>\n");
#endif
src/drivers/px4io/px4io.cpp
+401 -222
View File
File diff suppressed because it is too large Load Diff
src/drivers/px4io/px4io_i2c.cpp
+16 -8
View File
@@ -31,11 +31,11 @@
*
****************************************************************************/
/**
* @file px4io_i2c.cpp
*
* I2C interface for PX4IO
*/
/**
* @file px4io_i2c.cpp
*
* I2C interface for PX4IO
*/
/* XXX trim includes */
#include <px4_config.h>
@@ -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;
}
src/drivers/px4io/px4io_serial.cpp
+56 -26
View File
@@ -31,11 +31,11 @@
*
****************************************************************************/
/**
* @file px4io_serial.cpp
*
* Serial interface for PX4IO
*/
/**
* @file px4io_serial.cpp
*
* Serial interface for PX4IO
*/
/* XXX trim includes */
#include <px4_config.h>
@@ -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<const uint16_t *>(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<uint16_t *>(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);
src/drivers/px4io/px4io_uploader.cpp
+54 -17
View File
@@ -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;
src/drivers/stm32/adc/adc.cpp
+35 -15
View File
@@ -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);
src/drivers/stm32/drv_hrt.c
+35 -21
View File
@@ -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");
src/drivers/stm32/drv_pwm_servo.c
+27 -13
View File
@@ -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;
}
src/drivers/stm32/tone_alarm/tone_alarm.cpp
+222 -84
View File
@@ -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 <systemlib/err.h>
/* 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;
src/examples/fixedwing_control/main.c
+5 -5
View File
@@ -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);
}
src/examples/flow_position_estimator/flow_position_estimator_main.c
+5 -5
View File
@@ -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);
}
src/examples/matlab_csv_serial/matlab_csv_serial.c
+5 -5
View File
@@ -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);
}
src/examples/publisher/publisher_start_nuttx.cpp
+5 -5
View File
@@ -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);
}
src/examples/px4_daemon_app/px4_daemon_app.c
+5 -5
View File
@@ -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;
}
src/examples/rover_steering_control/main.cpp
+5 -5
View File
@@ -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);
}
src/examples/subscriber/subscriber_start_nuttx.cpp
+5 -5
View File
@@ -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);
}
src/include/containers/List.hpp
+12 -8
View File
@@ -43,31 +43,35 @@ template<class T>
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 T>
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 &);
};
src/include/mavlink/mavlink_log.h
+9 -9
View File
@@ -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];
src/lib/dspal
Submodule
+1
Submodule src/lib/dspal added at a88d55925c
src/lib/external_lgpl/tecs/tecs.cpp
+35 -29
View File
@@ -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;
src/lib/external_lgpl/tecs/tecs.h
+3 -1
View File
@@ -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;
src/lib/launchdetection/CatapultLaunchMethod.cpp
+14 -4
View File
@@ -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();
}
src/lib/launchdetection/LaunchDetector.cpp
+11 -7
View File
@@ -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);
}
src/lib/mathlib/math/Matrix.hpp
+1 -1
View File
@@ -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");
}
src/modules/bottle_drop/bottle_drop.cpp
+74 -57
View File
@@ -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;
src/modules/commander/commander.cpp
+14 -11
View File
@@ -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;
src/modules/commander/state_machine_helper.cpp
+1 -3
View File
@@ -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;
src/modules/dataman/dataman.c
+115 -50
View File
@@ -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;
}
src/modules/dataman/dataman.h
+75 -75
View File
@@ -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
}
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+6 -1
View File
@@ -944,9 +944,14 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
const float dtLastGoodGPS = static_cast<float>(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;
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c
+4 -4
View File
@@ -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
src/modules/fixedwing_backside/fixedwing.cpp
+32 -27
View File
@@ -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();
}
src/modules/fixedwing_backside/fixedwing_backside_main.cpp
+7 -6
View File
@@ -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 <additional params>]\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);
}
src/modules/fw_att_control/fw_att_control_main.cpp
+4 -5
View File
@@ -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
*
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+3 -3
View File
@@ -1363,7 +1363,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_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) {
src/modules/mavlink/mavlink_main.cpp
+21 -20
View File
@@ -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);
}
src/modules/mavlink/mavlink_main.h
+4 -2
View File
@@ -47,6 +47,7 @@
#else
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <drivers/device/device.h>
#endif
#include <systemlib/param/param.h>
@@ -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;
src/modules/mavlink/mavlink_messages.cpp
+7
View File
@@ -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);
}
src/modules/mavlink/mavlink_mission.cpp
+12 -9
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -35,9 +35,9 @@
* @file mavlink_mission.cpp
* MAVLink mission manager implementation.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Lorenz Meier <lorenz@px4.io>
* @author Julian Oes <julian@px4.io>
* @author Anton Babushkin <anton@px4.io>
*/
#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);
src/modules/mavlink/mavlink_mission.h
+10 -7
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -35,9 +35,9 @@
* @file mavlink_mission.h
* MAVLink mission manager interface definition.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Lorenz Meier <lorenz@px4.io>
* @author Julian Oes <julian@px4.io>
* @author Anton Babushkin <anton@px4.io>
*/
#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 &);
src/modules/mavlink/mavlink_receiver.cpp
+19 -22
View File
@@ -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++) {
src/modules/mc_att_control/mc_att_control_params.c
+6 -6
View File
@@ -36,8 +36,8 @@
* Parameters for multicopter attitude controller.
*
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Lorenz Meier <lorenz@px4.io>
* @author Anton Babushkin <anton@px4.io>
*/
#include <systemlib/param/param.h>
@@ -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
src/modules/muorb/krait/uORBKraitFastRpcChannel.hpp
+1 -1
View File
@@ -38,7 +38,7 @@
#include <string>
#include <pthread.h>
#include "uORB/uORBCommunicator.hpp"
#include <px4_muorb/px4muorb_KraitRpcWrapper.hpp>
#include <px4muorb_KraitRpcWrapper.hpp>
#include <map>
#include "drivers/drv_hrt.h"
src/modules/navigator/mission.cpp
+15 -12
View File
@@ -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;
}
src/modules/navigator/navigator_main.cpp
+1 -1
View File
@@ -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);
src/modules/sdlog2/sdlog2.c
+5 -2
View File
@@ -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);

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