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

Merge branch 'master' into ekf_voting

Browse Source
This commit is contained in:
Lorenz Meier
2015-09-05 17:41:21 +02:00
parent 9950d5e950 329f7c0130
commit d532e372ac
54 changed files with 1091 additions and 243 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.travis.yml
+7
View File
@@ -13,6 +13,7 @@ cache:
addons:
apt:
sources:
- kubuntu-backports
- ubuntu-toolchain-r-test
packages:
- build-essential
@@ -46,6 +47,12 @@ 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
Makefile
+1 -1
View File
@@ -220,7 +220,7 @@ 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
check_format:
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.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
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/toolchain_gnu-arm-linux-gnueabihf.mk
-2
View File
@@ -157,8 +157,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 \
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/firmware_qurt.mk
+1 -1
View File
@@ -47,7 +47,7 @@ $(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) \
makefiles/qurt/toolchain_hexagon.mk
+1 -1
View File
@@ -47,7 +47,7 @@ endif
#
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))
posix-configs/SITL/init/rc.fixed_wing
+4 -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
@@ -29,18 +28,20 @@ accelsim start
barosim start
adcsim start
gpssim start
measairspeedsim start
pwm_out_sim mode_pwm
commander start
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
+2 -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
@@ -35,8 +34,8 @@ barosim start
adcsim start
gpssim start
pwm_out_sim mode_pwm
commander start
sensors start
commander start
land_detector start multicopter
navigator start
attitude_estimator_q start
@@ -53,3 +52,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
+1 -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
@@ -49,8 +48,8 @@ barosim start
adcsim start
gpssim start
pwm_out_sim mode_pwm
commander start
sensors start
commander start
land_detector start multicopter
navigator start
attitude_estimator_q start
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/stm32/tone_alarm/tone_alarm.cpp
+70 -20
View File
@@ -117,8 +117,23 @@
#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
@@ -150,6 +165,14 @@
# ifdef CONFIG_STM32_TIM5
# error Must not set CONFIG_STM32_TIM5 when TONE_ALARM_TIMER is 5
# endif
#elif TONE_ALARM_TIMER == 8
# define TONE_ALARM_BASE STM32_TIM8_BASE
# define TONE_ALARM_CLOCK STM32_APB2_TIM8_CLKIN
# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB2ENR
# define TONE_ALARM_CLOCK_ENABLE RCC_APB2ENR_TIM8EN
# ifdef CONFIG_STM32_TIM8
# error Must not set CONFIG_STM32_TIM8 when TONE_ALARM_TIMER is 8
# endif
#elif TONE_ALARM_TIMER == 9
# define TONE_ALARM_BASE STM32_TIM9_BASE
# define TONE_ALARM_CLOCK STM32_APB2_TIM9_CLKIN
@@ -175,7 +198,7 @@
# error Must not set CONFIG_STM32_TIM11 when TONE_ALARM_TIMER is 11
# endif
#else
# error Must set TONE_ALARM_TIMER to a generic timer in order to use this driver.
# error Must set TONE_ALARM_TIMER to one of the timers between 1 and 11 (inclusive) to use this driver.
#endif
#if TONE_ALARM_CHANNEL == 1
@@ -208,24 +231,47 @@
*/
#define REG(_reg) (*(volatile uint32_t *)(TONE_ALARM_BASE + _reg))
#define rCR1 REG(STM32_GTIM_CR1_OFFSET)
#define rCR2 REG(STM32_GTIM_CR2_OFFSET)
#define rSMCR REG(STM32_GTIM_SMCR_OFFSET)
#define rDIER REG(STM32_GTIM_DIER_OFFSET)
#define rSR REG(STM32_GTIM_SR_OFFSET)
#define rEGR REG(STM32_GTIM_EGR_OFFSET)
#define rCCMR1 REG(STM32_GTIM_CCMR1_OFFSET)
#define rCCMR2 REG(STM32_GTIM_CCMR2_OFFSET)
#define rCCER REG(STM32_GTIM_CCER_OFFSET)
#define rCNT REG(STM32_GTIM_CNT_OFFSET)
#define rPSC REG(STM32_GTIM_PSC_OFFSET)
#define rARR REG(STM32_GTIM_ARR_OFFSET)
#define rCCR1 REG(STM32_GTIM_CCR1_OFFSET)
#define rCCR2 REG(STM32_GTIM_CCR2_OFFSET)
#define rCCR3 REG(STM32_GTIM_CCR3_OFFSET)
#define rCCR4 REG(STM32_GTIM_CCR4_OFFSET)
#define rDCR REG(STM32_GTIM_DCR_OFFSET)
#define rDMAR REG(STM32_GTIM_DMAR_OFFSET)
#if TONE_ALARM_TIMER == 1 || TONE_ALARM_TIMER == 8 // Note: If using TIM1 or TIM8, then you are using the ADVANCED timers and NOT the GENERAL TIMERS, therefore different registers
# define rCR1 REG(STM32_ATIM_CR1_OFFSET)
# define rCR2 REG(STM32_ATIM_CR2_OFFSET)
# define rSMCR REG(STM32_ATIM_SMCR_OFFSET)
# define rDIER REG(STM32_ATIM_DIER_OFFSET)
# define rSR REG(STM32_ATIM_SR_OFFSET)
# define rEGR REG(STM32_ATIM_EGR_OFFSET)
# define rCCMR1 REG(STM32_ATIM_CCMR1_OFFSET)
# define rCCMR2 REG(STM32_ATIM_CCMR2_OFFSET)
# define rCCER REG(STM32_ATIM_CCER_OFFSET)
# define rCNT REG(STM32_ATIM_CNT_OFFSET)
# define rPSC REG(STM32_ATIM_PSC_OFFSET)
# define rARR REG(STM32_ATIM_ARR_OFFSET)
# define rRCR REG(STM32_ATIM_RCR_OFFSET)
# define rCCR1 REG(STM32_ATIM_CCR1_OFFSET)
# define rCCR2 REG(STM32_ATIM_CCR2_OFFSET)
# define rCCR3 REG(STM32_ATIM_CCR3_OFFSET)
# define rCCR4 REG(STM32_ATIM_CCR4_OFFSET)
# define rBDTR REG(STM32_ATIM_BDTR_OFFSET)
# define rDCR REG(STM32_ATIM_DCR_OFFSET)
# define rDMAR REG(STM32_ATIM_DMAR_OFFSET)
#else
# define rCR1 REG(STM32_GTIM_CR1_OFFSET)
# define rCR2 REG(STM32_GTIM_CR2_OFFSET)
# define rSMCR REG(STM32_GTIM_SMCR_OFFSET)
# define rDIER REG(STM32_GTIM_DIER_OFFSET)
# define rSR REG(STM32_GTIM_SR_OFFSET)
# define rEGR REG(STM32_GTIM_EGR_OFFSET)
# define rCCMR1 REG(STM32_GTIM_CCMR1_OFFSET)
# define rCCMR2 REG(STM32_GTIM_CCMR2_OFFSET)
# define rCCER REG(STM32_GTIM_CCER_OFFSET)
# define rCNT REG(STM32_GTIM_CNT_OFFSET)
# define rPSC REG(STM32_GTIM_PSC_OFFSET)
# define rARR REG(STM32_GTIM_ARR_OFFSET)
# define rCCR1 REG(STM32_GTIM_CCR1_OFFSET)
# define rCCR2 REG(STM32_GTIM_CCR2_OFFSET)
# define rCCR3 REG(STM32_GTIM_CCR3_OFFSET)
# define rCCR4 REG(STM32_GTIM_CCR4_OFFSET)
# define rDCR REG(STM32_GTIM_DCR_OFFSET)
# define rDMAR REG(STM32_GTIM_DMAR_OFFSET)
#endif
class ToneAlarm : public device::CDev
{
@@ -396,6 +442,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;
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/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/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
+2 -2
View File
@@ -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
+16 -18
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.
@@ -1724,6 +1717,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 +1833,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 +1942,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
-2
View File
@@ -479,8 +479,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_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);
@@ -1762,10 +1746,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;
@@ -1780,13 +1771,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
+4 -4
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
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);
src/modules/simulator/simulator.cpp
+9
View File
@@ -86,6 +86,11 @@ bool Simulator::getGPSSample(uint8_t *buf, int len)
return _gps.copyData(buf, len);
}
bool Simulator::getAirspeedSample(uint8_t *buf, int len)
{
return _airspeed.copyData(buf, len);
}
void Simulator::write_MPU_data(void *buf) {
_mpu.writeData(buf);
}
@@ -106,6 +111,10 @@ void Simulator::write_gps_data(void *buf) {
_gps.writeData(buf);
}
void Simulator::write_airspeed_data(void *buf) {
_airspeed.writeData(buf);
}
int Simulator::start(int argc, char *argv[])
{
int ret = 0;
src/modules/simulator/simulator.h
+18 -2
View File
@@ -43,6 +43,7 @@
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_status.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>
#include <drivers/drv_baro.h>
@@ -93,6 +94,13 @@ struct RawBaroData {
};
#pragma pack(pop)
#pragma pack(push, 1)
struct RawAirspeedData {
float temperature;
float diff_pressure;
};
#pragma pack(pop)
#pragma pack(push, 1)
struct RawGPSData {
int32_t lat;
@@ -190,12 +198,14 @@ public:
bool getMPUReport(uint8_t *buf, int len);
bool getBaroSample(uint8_t *buf, int len);
bool getGPSSample(uint8_t *buf, int len);
bool getAirspeedSample(uint8_t *buf, int len);
void write_MPU_data(void *buf);
void write_accel_data(void *buf);
void write_mag_data(void *buf);
void write_baro_data(void *buf);
void write_gps_data(void *buf);
void write_airspeed_data(void *buf);
bool isInitialized() { return _initialized; }
@@ -206,6 +216,7 @@ private:
_baro(1),
_mag(1),
_gps(1),
_airspeed(1),
_accel_pub(nullptr),
_baro_pub(nullptr),
_gyro_pub(nullptr),
@@ -217,10 +228,12 @@ private:
_actuator_outputs_sub(-1),
_vehicle_attitude_sub(-1),
_manual_sub(-1),
_vehicle_status_sub(-1),
_rc_input{},
_actuators{},
_attitude{},
_manual{}
_manual{},
_vehicle_status{}
#endif
{}
~Simulator() { _instance=NULL; }
@@ -235,6 +248,7 @@ private:
simulator::Report<simulator::RawBaroData> _baro;
simulator::Report<simulator::RawMagData> _mag;
simulator::Report<simulator::RawGPSData> _gps;
simulator::Report<simulator::RawAirspeedData> _airspeed;
// uORB publisher handlers
orb_advert_t _accel_pub;
@@ -255,14 +269,16 @@ private:
int _actuator_outputs_sub;
int _vehicle_attitude_sub;
int _manual_sub;
int _vehicle_status_sub;
// uORB data containers
struct rc_input_values _rc_input;
struct actuator_outputs_s _actuators;
struct vehicle_attitude_s _attitude;
struct manual_control_setpoint_s _manual;
struct vehicle_status_s _vehicle_status;
void poll_actuators();
void poll_topics();
void handle_message(mavlink_message_t *msg, bool publish);
void send_controls();
void pollForMAVLinkMessages(bool publish);
src/modules/simulator/simulator_mavlink.cpp
+42 -18
View File
@@ -64,33 +64,46 @@ static socklen_t _addrlen = sizeof(_srcaddr);
using namespace simulator;
void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) {
float out[8];
float out[8] = {};
const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2;
// for now we only support quadrotors
unsigned n = 4;
for (unsigned i = 0; i < 8; i++) {
if (_actuators.output[i] > PWM_LOWEST_MIN / 2) {
if (i < n) {
// scale PWM out 900..2100 us to 0..1 for rotors */
out[i] = (_actuators.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
if (_vehicle_status.is_rotary_wing) {
for (unsigned i = 0; i < 8; i++) {
if (_actuators.output[i] > PWM_LOWEST_MIN / 2) {
if (i < n) {
// scale PWM out 900..2100 us to 0..1 for rotors */
out[i] = (_actuators.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
} else {
// scale PWM out 900..2100 us to -1..1 for other channels */
out[i] = (_actuators.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
}
} else {
// scale PWM out 900..2100 us to -1..1 for other channels */
out[i] = (_actuators.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
// send 0 when disarmed and for disabled channels */
out[i] = 0.0f;
}
} else {
// send 0 when disarmed and for disabled channels */
out[i] = 0.0f;
}
} else {
// convert back to range [-1, 1]
for (unsigned i = 0; i < 8; i++) {
out[i] = (_actuators.output[i] - 1500) / 600.0f;
}
}
// if vehicle status has not yet been updated, set actuator commands to zero
// this is to prevent the simulation getting into a bad state
if (_vehicle_status.timestamp == 0) {
memset(out, 0, sizeof(out));
}
actuator_msg.time_usec = hrt_absolute_time();
actuator_msg.roll_ailerons = out[0];
actuator_msg.pitch_elevator = out[1];
actuator_msg.pitch_elevator = _vehicle_status.is_rotary_wing ? out[1] : -out[1];
actuator_msg.yaw_rudder = out[2];
actuator_msg.throttle = out[3];
actuator_msg.aux1 = out[4];
@@ -174,11 +187,17 @@ void Simulator::update_sensors(mavlink_hil_sensor_t *imu) {
RawBaroData baro;
// calculate air pressure from altitude (valid for low altitude)
baro.pressure = PRESS_GROUND - GRAVITY * DENSITY * imu->pressure_alt;
baro.pressure = (PRESS_GROUND - GRAVITY * DENSITY * imu->pressure_alt) / 100.0f; // convert from Pa to mbar
baro.altitude = imu->pressure_alt;
baro.temperature = imu->temperature;
write_baro_data((void *)&baro);
RawAirspeedData airspeed;
airspeed.temperature = imu->temperature;
airspeed.diff_pressure = imu->diff_pressure;
write_airspeed_data((void *)&airspeed);
}
void Simulator::update_gps(mavlink_hil_gps_t *gps_sim) {
@@ -270,14 +289,18 @@ void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8
}
}
void Simulator::poll_actuators() {
void Simulator::poll_topics() {
// copy new actuator data if available
bool updated;
orb_check(_actuator_outputs_sub, &updated);
if(updated) {
//PX4_WARN("Received actuator_output0 orb_topic");
if (updated) {
orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuators);
}
orb_check(_vehicle_status_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
}
}
void *Simulator::sending_trampoline(void *) {
@@ -310,7 +333,7 @@ void Simulator::send() {
if (fds[0].revents & POLLIN) {
// got new data to read, update all topics
poll_actuators();
poll_topics();
send_controls();
}
}
@@ -419,6 +442,7 @@ void Simulator::pollForMAVLinkMessages(bool publish)
// subscribe to topics
_actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs), 0);
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
// got data from simulator, now activate the sending thread
pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, NULL);
src/modules/systemlib/perf_counter.c
+1 -2
View File
@@ -396,13 +396,12 @@ perf_print_counter_fd(int fd, perf_counter_t handle)
case PC_ELAPSED: {
struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
float rms = sqrtf(pce->M2 / (pce->event_count-1));
dprintf(fd, "%s: %llu events, %llu overruns, %lluus elapsed, %lluus avg, min %lluus max %lluus %5.3fus rms\n",
handle->name,
(unsigned long long)pce->event_count,
(unsigned long long)pce->event_overruns,
(unsigned long long)pce->time_total,
(unsigned long long)pce->time_total / pce->event_count,
pce->event_count == 0 ? 0 : (unsigned long long)pce->time_total / pce->event_count,
(unsigned long long)pce->time_least,
(unsigned long long)pce->time_most,
(double)(1e6f * rms));
src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp
+47 -26
View File
@@ -35,14 +35,12 @@
* @file ets_airspeed.cpp
* @author Simon Wilks
* @author Mark Charlebois (simulator)
*
* Driver for the Simulated Airspeed Sensor based on Eagle Tree Airspeed V3.
* @author Roman Bapst (simulator)
* Driver for the Simulated AirspeedSim Sensor based on Eagle Tree AirspeedSim V3.
*/
#include <px4_config.h>
#include <drivers/device/i2c.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdlib.h>
@@ -56,8 +54,6 @@
#include <math.h>
#include <unistd.h>
#include <arch/board/board.h>
#include <systemlib/airspeed.h>
#include <systemlib/err.h>
#include <systemlib/param/param.h>
@@ -71,12 +67,15 @@
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/subsystem_info.h>
#include <drivers/airspeed/airspeed.h>
#include <simulator/simulator.h>
Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char* path) :
I2C("Airspeed", path, bus, address),
#include "airspeedsim.h"
AirspeedSim::AirspeedSim(int bus, int address, unsigned conversion_interval, const char* path) :
VDev("AIRSPEEDSIM", path),
_reports(nullptr),
_buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")),
_retries(0),
_max_differential_pressure_pa(0),
_sensor_ok(false),
_last_published_sensor_ok(true), /* initialize differently to force publication */
@@ -97,7 +96,7 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const cha
memset(&_work, 0, sizeof(_work));
}
Airspeed::~Airspeed()
AirspeedSim::~AirspeedSim()
{
/* make sure we are truly inactive */
stop();
@@ -116,18 +115,21 @@ Airspeed::~Airspeed()
}
int
Airspeed::init()
AirspeedSim::init()
{
int ret = ERROR;
/* do I2C init (and probe) first */
if (I2C::init() != OK)
/* init base class */
if (VDev::init() != OK) {
DEVICE_DEBUG("VDev init failed");
goto out;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(differential_pressure_s));
if (_reports == nullptr)
if (_reports == nullptr) {
goto out;
}
/* register alternate interfaces if we have to */
_class_instance = register_class_devname(AIRSPEED_BASE_DEVICE_PATH);
@@ -154,7 +156,7 @@ out:
}
int
Airspeed::probe()
AirspeedSim::probe()
{
/* on initial power up the device may need more than one retry
for detection. Once it is running the number of retries can
@@ -169,7 +171,7 @@ Airspeed::probe()
}
int
Airspeed::ioctl(device::file_t *filp, int cmd, unsigned long arg)
AirspeedSim::ioctl(device::file_t *filp, int cmd, unsigned long arg)
{
switch (cmd) {
@@ -272,12 +274,13 @@ Airspeed::ioctl(device::file_t *filp, int cmd, unsigned long arg)
default:
/* give it to the superclass */
return I2C::ioctl(filp, cmd, arg);
//return I2C::ioctl(filp, cmd, arg); XXX SIM should be super class
return 0;
}
}
ssize_t
Airspeed::read(device::file_t *filp, char *buffer, size_t buflen)
AirspeedSim::read(device::file_t *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(differential_pressure_s);
differential_pressure_s *abuf = reinterpret_cast<differential_pressure_s *>(buffer);
@@ -336,24 +339,24 @@ Airspeed::read(device::file_t *filp, char *buffer, size_t buflen)
}
void
Airspeed::start()
AirspeedSim::start()
{
/* reset the report ring and state machine */
_collect_phase = false;
_reports->flush();
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&Airspeed::cycle_trampoline, this, 1);
work_queue(HPWORK, &_work, (worker_t)&AirspeedSim::cycle_trampoline, this, 1);
}
void
Airspeed::stop()
AirspeedSim::stop()
{
work_cancel(HPWORK, &_work);
}
void
Airspeed::update_status()
AirspeedSim::update_status()
{
if (_sensor_ok != _last_published_sensor_ok) {
/* notify about state change */
@@ -375,9 +378,9 @@ Airspeed::update_status()
}
void
Airspeed::cycle_trampoline(void *arg)
AirspeedSim::cycle_trampoline(void *arg)
{
Airspeed *dev = (Airspeed *)arg;
AirspeedSim *dev = (AirspeedSim *)arg;
dev->cycle();
// XXX we do not know if this is
@@ -387,7 +390,7 @@ Airspeed::cycle_trampoline(void *arg)
}
void
Airspeed::print_info()
AirspeedSim::print_info()
{
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
@@ -397,8 +400,26 @@ Airspeed::print_info()
}
void
Airspeed::new_report(const differential_pressure_s &report)
AirspeedSim::new_report(const differential_pressure_s &report)
{
if (!_reports->force(&report))
perf_count(_buffer_overflows);
}
int
AirspeedSim::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) {
if (recv_len > 0) {
// this is equivalent to the collect phase
Simulator *sim = Simulator::getInstance();
if (sim == NULL) {
PX4_ERR("Error BARO_SIM::transfer no simulator");
return -ENODEV;
}
PX4_DEBUG("BARO_SIM::transfer getting sample");
sim->getAirspeedSample(recv, recv_len);
} else {
// we don't need measure phase
}
return 0;
}
src/platforms/posix/drivers/airspeedsim/airspeedsim.h
+17 -10
View File
@@ -40,7 +40,7 @@
#include <px4_config.h>
#include <drivers/device/i2c.h>
//#include <drivers/device/i2c.h>
#include <sys/types.h>
#include <stdint.h>
@@ -55,10 +55,11 @@
#include <math.h>
#include <unistd.h>
#include <nuttx/arch.h>
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
//#include <nuttx/arch.h>
//#include <nuttx/wqueue.h>
//#include <nuttx/clock.h>
#include <px4_workqueue.h>
#include <arch/board/board.h>
#include <systemlib/airspeed.h>
@@ -69,6 +70,7 @@
#include <drivers/drv_airspeed.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/ringbuffer.h>
#include <drivers/device/device.h>
#include <uORB/uORB.h>
#include <uORB/topics/differential_pressure.h>
@@ -87,7 +89,7 @@ static const int ERROR = -1;
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
class __EXPORT AirspeedSim : public device::I2C
class __EXPORT AirspeedSim : public device::VDev
{
public:
AirspeedSim(int bus, int address, unsigned conversion_interval, const char *path);
@@ -95,8 +97,8 @@ public:
virtual int init();
virtual ssize_t read(px4_dev_handle_t *handlep, char *buffer, size_t buflen);
virtual int ioctl(px4_dev_handle_t *handlep, int cmd, unsigned long arg);
virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen);
virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg);
/**
* Diagnostics - print some basic information about the driver.
@@ -104,9 +106,11 @@ public:
virtual void print_info();
private:
RingBuffer *_reports;
ringbuffer::RingBuffer *_reports;
perf_counter_t _buffer_overflows;
unsigned _retries; // XXX this should come from the SIM class
/* this class has pointer data members and should not be copied */
AirspeedSim(const AirspeedSim &);
AirspeedSim &operator=(const AirspeedSim &);
@@ -122,16 +126,19 @@ protected:
virtual int measure() = 0;
virtual int collect() = 0;
virtual int transfer(const uint8_t *send, unsigned send_len,
uint8_t *recv, unsigned recv_len);
/**
* Update the subsystem status
*/
void update_status();
work_s _work;
struct work_s _work;
float _max_differential_pressure_pa;
bool _sensor_ok;
bool _last_published_sensor_ok;
int _measure_ticks;
unsigned _measure_ticks;
bool _collect_phase;
float _diff_pres_offset;
src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp
+631
View File
@@ -0,0 +1,631 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file meas_airspeed_sim.cpp
* @author Lorenz Meier
* @author Sarthak Kaingade
* @author Simon Wilks
* @author Thomas Gubler
* @author Roman Bapst
*
* Driver for a simulated airspeed sensor.
*
*/
#include <px4_config.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <systemlib/airspeed.h>
#include <systemlib/err.h>
#include <systemlib/param/param.h>
#include <systemlib/perf_counter.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <drivers/drv_airspeed.h>
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/system_power.h>
#include "airspeedsim.h"
/* I2C bus address is 1010001x */
#define I2C_ADDRESS_MS4525DO 0x28 /**< 7-bit address. Depends on the order code (this is for code "I") */
#define PATH_MS4525 "/dev/ms4525"
/* The MS5525DSO address is 111011Cx, where C is the complementary value of the pin CSB */
#define I2C_ADDRESS_MS5525DSO 0x77 //0x77/* 7-bit address, addr. pin pulled low */
#define PATH_MS5525 "/dev/ms5525"
/* Register address */
#define ADDR_READ_MR 0x00 /* write to this address to start conversion */
/* Measurement rate is 100Hz */
#define MEAS_RATE 100
#define MEAS_DRIVER_FILTER_FREQ 1.2f
#define CONVERSION_INTERVAL (1000000 / MEAS_RATE) /* microseconds */
class MEASAirspeedSim : public AirspeedSim
{
public:
MEASAirspeedSim(int bus, int address = I2C_ADDRESS_MS4525DO, const char *path = PATH_MS4525);
protected:
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
virtual void cycle();
virtual int measure();
virtual int collect();
math::LowPassFilter2p _filter;
/**
* Correct for 5V rail voltage variations
*/
void voltage_correction(float &diff_pres_pa, float &temperature);
int _t_system_power;
struct system_power_s system_power;
};
/*
* Driver 'main' command.
*/
extern "C" __EXPORT int measairspeedsim_main(int argc, char *argv[]);
MEASAirspeedSim::MEASAirspeedSim(int bus, int address, const char *path) : AirspeedSim(bus, address,
CONVERSION_INTERVAL, path),
_filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ),
_t_system_power(-1),
system_power{}
{}
int
MEASAirspeedSim::measure()
{
int ret;
/*
* Send the command to begin a measurement.
*/
uint8_t cmd = 0;
ret = transfer(&cmd, 1, nullptr, 0);
if (OK != ret) {
perf_count(_comms_errors);
}
return ret;
}
int
MEASAirspeedSim::collect()
{
int ret = -EIO;
/* read from the sensor */
#pragma pack(push, 1)
struct {
float temperature;
float diff_pressure;
} airspeed_report;
#pragma pack(pop)
perf_begin(_sample_perf);
ret = transfer(nullptr, 0, (uint8_t *)&airspeed_report, sizeof(airspeed_report));
if (ret < 0) {
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
}
uint8_t status = 0;
switch (status) {
case 0:
break;
case 1:
/* fallthrough */
case 2:
/* fallthrough */
case 3:
perf_count(_comms_errors);
perf_end(_sample_perf);
return -EAGAIN;
}
float temperature = airspeed_report.temperature;
float diff_press_pa_raw = airspeed_report.diff_pressure * 100.0f; // convert from millibar to bar
struct differential_pressure_s report;
/* track maximum differential pressure measured (so we can work out top speed). */
if (diff_press_pa_raw > _max_differential_pressure_pa) {
_max_differential_pressure_pa = diff_press_pa_raw;
}
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.temperature = temperature;
report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw);
report.differential_pressure_raw_pa = diff_press_pa_raw;
report.max_differential_pressure_pa = _max_differential_pressure_pa;
if (_airspeed_pub != nullptr && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
}
new_report(report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
perf_end(_sample_perf);
return ret;
}
void
MEASAirspeedSim::cycle()
{
int ret;
/* collection phase? */
if (_collect_phase) {
/* perform collection */
ret = collect();
if (OK != ret) {
/* restart the measurement state machine */
start();
_sensor_ok = false;
return;
}
/* next phase is measurement */
_collect_phase = false;
/*
* Is there a collect->measure gap?
*/
if (_measure_ticks > USEC2TICK(CONVERSION_INTERVAL)) {
/* schedule a fresh cycle call when we are ready to measure again */
work_queue(HPWORK,
&_work,
(worker_t)&AirspeedSim::cycle_trampoline,
this,
_measure_ticks - USEC2TICK(CONVERSION_INTERVAL));
return;
}
}
/* measurement phase */
ret = measure();
if (OK != ret) {
DEVICE_DEBUG("measure error");
}
_sensor_ok = (ret == OK);
/* next phase is collection */
_collect_phase = true;
/* schedule a fresh cycle call when the measurement is done */
work_queue(HPWORK,
&_work,
(worker_t)&AirspeedSim::cycle_trampoline,
this,
USEC2TICK(CONVERSION_INTERVAL));
}
/**
correct for 5V rail voltage if the system_power ORB topic is
available
See http://uav.tridgell.net/MS4525/MS4525-offset.png for a graph of
offset versus voltage for 3 sensors
*/
void
MEASAirspeedSim::voltage_correction(float &diff_press_pa, float &temperature)
{
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
if (_t_system_power == -1) {
_t_system_power = orb_subscribe(ORB_ID(system_power));
}
if (_t_system_power == -1) {
// not available
return;
}
bool updated = false;
orb_check(_t_system_power, &updated);
if (updated) {
orb_copy(ORB_ID(system_power), _t_system_power, &system_power);
}
if (system_power.voltage5V_v < 3.0f || system_power.voltage5V_v > 6.0f) {
// not valid, skip correction
return;
}
const float slope = 65.0f;
/*
apply a piecewise linear correction, flattening at 0.5V from 5V
*/
float voltage_diff = system_power.voltage5V_v - 5.0f;
if (voltage_diff > 0.5f) {
voltage_diff = 0.5f;
}
if (voltage_diff < -0.5f) {
voltage_diff = -0.5f;
}
diff_press_pa -= voltage_diff * slope;
/*
the temperature masurement varies as well
*/
const float temp_slope = 0.887f;
voltage_diff = system_power.voltage5V_v - 5.0f;
if (voltage_diff > 0.5f) {
voltage_diff = 0.5f;
}
if (voltage_diff < -1.0f) {
voltage_diff = -1.0f;
}
temperature -= voltage_diff * temp_slope;
#endif // CONFIG_ARCH_BOARD_PX4FMU_V2
}
/**
* Local functions in support of the shell command.
*/
namespace meas_airspeed_sim
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
const int ERROR = -1;
MEASAirspeedSim *g_dev = nullptr;
int start(int i2c_bus);
int stop();
int test();
int reset();
int info();
/**
* Start the driver.
*
* This function call only returns once the driver is up and running
* or failed to detect the sensor.
*/
int
start(int i2c_bus)
{
int fd;
if (g_dev != nullptr) {
PX4_WARN("already started");
}
/* create the driver, try the MS4525DO first */
g_dev = new MEASAirspeedSim(i2c_bus, I2C_ADDRESS_MS4525DO, PATH_MS4525);
/* check if the MS4525DO was instantiated */
if (g_dev == nullptr) {
goto fail;
}
/* try the MS5525DSO next if init fails */
if (OK != g_dev->AirspeedSim::init()) {
delete g_dev;
g_dev = new MEASAirspeedSim(i2c_bus, I2C_ADDRESS_MS5525DSO, PATH_MS5525);
/* check if the MS5525DSO was instantiated */
if (g_dev == nullptr) {
goto fail;
}
/* both versions failed if the init for the MS5525DSO fails, give up */
if (OK != g_dev->AirspeedSim::init()) {
goto fail;
}
}
/* set the poll rate to default, starts automatic data collection */
fd = px4_open(PATH_MS4525, O_RDONLY);
if (fd < 0) {
goto fail;
}
if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
goto fail;
}
return 0;
fail:
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
PX4_ERR("no MS4525 airspeedSim sensor connected");
return 1;
}
/**
* Stop the driver
*/
int
stop()
{
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
} else {
PX4_ERR("driver not running");
}
return 0;
}
/**
* Perform some basic functional tests on the driver;
* make sure we can collect data from the sensor in polled
* and automatic modes.
*/
int
test()
{
struct differential_pressure_s report;
ssize_t sz;
int ret;
int fd = px4_open(PATH_MS4525, O_RDONLY);
if (fd < 0) {
PX4_ERR("%s open failed (try 'meas_airspeed_sim start' if the driver is not running", PATH_MS4525);
return 1;
}
/* do a simple demand read */
sz = px4_read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
PX4_ERR("immediate read failed");
return 1;
}
PX4_WARN("single read");
PX4_WARN("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa);
/* start the sensor polling at 2Hz */
if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
PX4_WARN("failed to set 2Hz poll rate");
}
/* read the sensor 5x and report each value */
for (unsigned i = 0; i < 5; i++) {
struct pollfd fds;
/* wait for data to be ready */
fds.fd = fd;
fds.events = POLLIN;
ret = poll(&fds, 1, 2000);
if (ret != 1) {
warnx("timed out");
}
/* now go get it */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
PX4_WARN("periodic read failed");
}
PX4_WARN("periodic read %u", i);
PX4_WARN("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa);
PX4_WARN("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature);
}
/* reset the sensor polling to its default rate */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
PX4_ERR("failed to set default rate");
return 1;
}
PX4_WARN("PASS");
return 0;
}
/**
* Reset the driver.
*/
int
reset()
{
int fd = open(PATH_MS4525, O_RDONLY);
if (fd < 0) {
PX4_ERR("failed ");
return 1;
}
if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
PX4_ERR("driver reset failed");
return 1;
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
PX4_ERR("driver poll restart failed");
return 1;
}
return 0;
}
/**
* Print a little info about the driver.
*/
int
info()
{
if (g_dev == nullptr) {
PX4_WARN("driver not running");
}
printf("state @ %p\n", g_dev);
g_dev->print_info();
return 0;
}
} // namespace
static void
meas_airspeed_usage()
{
PX4_WARN("usage: meas_airspeed_sim command [options]");
PX4_WARN("options:");
PX4_WARN("\t-b --bus i2cbus (%d)", 1);
PX4_WARN("command:");
PX4_WARN("\tstart|stop|reset|test|info");
}
int
measairspeedsim_main(int argc, char *argv[])
{
int i2c_bus = 1;//PX4_I2C_BUS_DEFAULT;
int i;
for (i = 1; i < argc; i++) {
if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) {
if (argc > i + 1) {
i2c_bus = atoi(argv[i + 1]);
}
}
}
int ret;
/*
* Start/load the driver.
*/
if (!strcmp(argv[1], "start")) {
ret = meas_airspeed_sim::start(i2c_bus);
}
/*
* Stop the driver
*/
if (!strcmp(argv[1], "stop")) {
ret = meas_airspeed_sim::stop();
}
/*
* Test the driver/device.
*/
if (!strcmp(argv[1], "test")) {
ret = meas_airspeed_sim::test();
}
/*
* Reset the driver.
*/
if (!strcmp(argv[1], "reset")) {
ret = meas_airspeed_sim::reset();
}
/*
* Print driver information.
*/
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
ret = meas_airspeed_sim::info();
}
meas_airspeed_usage();
return ret;
}
src/platforms/posix/drivers/airspeedsim/module.mk
+2 -1
View File
@@ -34,7 +34,8 @@
#
# Makefile to build the generic airspeed driver.
#
MODULE_COMMAND = measairspeedsim
SRCS = airspeedsim.cpp
SRCS = airspeedsim.cpp meas_airspeed_sim.cpp
MAXOPTIMIZATION = -Os
src/platforms/posix/main.cpp
+109 -24
View File
@@ -32,7 +32,7 @@
****************************************************************************/
/**
* @file main.cpp
* Basic shell to execute builtin "apps"
* Basic shell to execute builtin "apps"
*
* @author Mark Charlebois <charlebm@gmail.com>
*/
@@ -42,8 +42,10 @@
#include <string>
#include <sstream>
#include <vector>
#include <signal.h>
namespace px4 {
namespace px4
{
void init_once(void);
}
@@ -54,21 +56,34 @@ typedef int (*px4_main_t)(int argc, char *argv[]);
#include "apps.h"
#include "px4_middleware.h"
static void run_cmd(const vector<string> &appargs) {
static bool _ExitFlag = false;
extern "C" {
void _SigIntHandler(int sig_num);
void _SigIntHandler(int sig_num)
{
_ExitFlag = true;
}
}
static void run_cmd(const vector<string> &appargs)
{
// command is appargs[0]
string command = appargs[0];
cout << "----------------------------------\n";
if (apps.find(command) != apps.end()) {
const char *arg[appargs.size()+2];
const char *arg[appargs.size() + 2];
unsigned int i = 0;
while (i < appargs.size() && appargs[i] != "") {
arg[i] = (char *)appargs[i].c_str();
++i;
}
arg[i] = (char *)0;
cout << "Running: " << command << "\n";
apps[command](i,(char **)arg);
apps[command](i, (char **)arg);
usleep(40000);
cout << "Returning: " << command << "\n";
@@ -78,40 +93,110 @@ static void run_cmd(const vector<string> &appargs) {
}
}
static void usage()
{
cout << "./mainapp [-d] [startup_config] -h" << std::endl;
cout << " -d - Optional flag to run the app in daemon mode and does not take listen for user input." <<
std::endl;
cout << " This is needed if mainapp is intended to be run as a upstart job on linux" << std::endl;
cout << "<startup_config> - config file for starting/stopping px4 modules" << std::endl;
cout << " -h - help/usage information" << std::endl;
}
static void process_line(string &line)
{
vector<string> appargs(8);
stringstream(line) >> appargs[0] >> appargs[1] >> appargs[2] >> appargs[3] >> appargs[4] >> appargs[5] >> appargs[6] >> appargs[7];
stringstream(line) >> appargs[0] >> appargs[1] >> appargs[2] >> appargs[3] >> appargs[4] >> appargs[5] >> appargs[6] >>
appargs[7];
run_cmd(appargs);
}
int main(int argc, char **argv)
{
px4::init_once();
bool daemon_mode = false;
signal(SIGINT, _SigIntHandler);
px4::init(argc, argv, "mainapp");
int index = 1;
bool error_detected = false;
char *commands_file = nullptr;
// Execute a command list of provided
if (argc == 2) {
ifstream infile(argv[1]);
while (index < argc) {
if (argv[index][0] == '-') {
// the arg starts with -
if (strcmp(argv[index], "-d") == 0) {
daemon_mode = true;
if (!infile) {
cout << "failed opening script" << argv[1] << std::endl;
return 1;
} else if (strcmp(argv[index], "-h") == 0) {
usage();
return 0;
} else {
PX4_WARN("Unknown/unhandled parameter: %s", argv[index]);
return 1;
}
} else {
// this is an argument that does not have '-' prefix; treat it like a file name
ifstream infile(argv[index]);
if (infile.good()) {
infile.close();
commands_file = argv[index];
} else {
PX4_WARN("Error opening file: %s", argv[index]);
error_detected = true;
break;
}
}
for (string line; getline(infile, line, '\n'); ) {
process_line(line);
}
++index;
}
string mystr;
while(1) {
cout << "Enter a command and its args:" << endl;
getline (cin,mystr);
process_line(mystr);
mystr = "";
if (!error_detected) {
px4::init_once();
px4::init(argc, argv, "mainapp");
//if commandfile is present, process the commands from the file
if (commands_file != nullptr) {
ifstream infile(commands_file);
if (infile.is_open()) {
for (string line; getline(infile, line, '\n');) {
process_line(line);
}
} else {
PX4_WARN("Error opening file: %s", commands_file);
}
}
if (!daemon_mode) {
string mystr;
while (!_ExitFlag) {
cout << "Enter a command and its args:" << endl;
getline(cin, mystr);
process_line(mystr);
mystr = "";
}
} else {
while (!_ExitFlag) {
sleep(1000000);
}
}
if (px4_task_is_running("muorb")) {
// sending muorb stop is needed if it is running to exit cleanly
vector<string> muorb_stop_cmd = { "muorb", "stop" };
run_cmd(muorb_stop_cmd);
}
vector<string> shutdown_cmd = { "shutdown" };
run_cmd(shutdown_cmd);
}
}
src/platforms/posix/px4_layer/px4_posix_tasks.cpp
+11
View File
@@ -280,6 +280,17 @@ void px4_show_tasks()
}
bool px4_task_is_running(const char *taskname)
{
int idx;
for (idx=0; idx < PX4_MAX_TASKS; idx++)
{
if (taskmap[idx].isused && (strcmp(taskmap[idx].name.c_str(), taskname) == 0)) {
return true;
}
}
return false;
}
__BEGIN_DECLS
unsigned long px4_getpid()
src/platforms/px4_tasks.h
+3
View File
@@ -117,5 +117,8 @@ __EXPORT void px4_task_exit(int ret);
/** Show a list of running tasks **/
__EXPORT void px4_show_tasks(void);
/** See if a task is running **/
__EXPORT bool px4_task_is_running(const char *taskname);
__END_DECLS
unittests/CMakeLists.txt
+8 -2
View File
@@ -11,6 +11,13 @@ CMAKE_FORCE_CXX_COMPILER( clang++ Clang )
#set( CMAKE_FIND_ROOT_PATH_MODE_LIBARARY_ONLY )
#set( CMAKE_FIND_ROOT_PATH_MODE_INCLUDE_ONLY )
if("${CMAKE_C_COMPILER_ID}" STREQUAL "Clang")
add_compile_options(-Qunused-arguments)
endif()
if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
add_compile_options(-Qunused-arguments)
endif()
project(unittests)
enable_testing()
@@ -123,8 +130,7 @@ add_executable(mixer_test mixer_test.cpp hrt.cpp
${PX_SRC}/modules/systemlib/mixer/mixer_simple.cpp
${PX_SRC}/modules/systemlib/pwm_limit/pwm_limit.c
${PX_SRC}/systemcmds/tests/test_mixer.cpp)
target_link_libraries( mixer_test LINK_PUBLIC px4_platform )
target_link_libraries( mixer_test px4_platform )
add_gtest(mixer_test)
unittests/sbus2_test.cpp
+1 -1
View File
@@ -33,7 +33,7 @@ TEST(SBUS2Test, SBUS2)
// Init the parser
uint8_t frame[30];
unsigned partial_frame_count = 0;
uint16_t rc_values[18];
//uint16_t rc_values[18];
//uint16_t num_values;
//bool sbus_failsafe;
//bool sbus_frame_drop;
unittests/st24_test.cpp
+2 -3
View File
@@ -41,7 +41,7 @@ TEST(ST24Test, ST24)
last_time = f;
// Pipe the data into the parser
hrt_abstime now = hrt_absolute_time();
//hrt_abstime now = hrt_absolute_time();
uint8_t rssi;
uint8_t rx_count;
@@ -53,8 +53,7 @@ TEST(ST24Test, ST24)
//warnx("decoded: %u channels (converted to PPM range)", (unsigned)channel_count);
for (unsigned i = 0; i < channel_count; i++) {
int16_t val = channels[i];
//int16_t val = channels[i];
//warnx("channel %u: %d 0x%03X", i, static_cast<int>(val), static_cast<int>(val));
}
}
unittests/sumd_test.cpp
+2 -3
View File
@@ -41,7 +41,7 @@ TEST(SUMDTest, SUMD)
last_time = f;
// Pipe the data into the parser
hrt_abstime now = hrt_absolute_time();
//hrt_abstime now = hrt_absolute_time();
uint8_t rssi;
uint8_t rx_count;
@@ -53,8 +53,7 @@ TEST(SUMDTest, SUMD)
//warnx("decoded: %u channels (converted to PPM range)", (unsigned)channel_count);
for (unsigned i = 0; i < channel_count; i++) {
int16_t val = channels[i];
//int16_t val = channels[i];
//warnx("channel %u: %d 0x%03X", i, static_cast<int>(val), static_cast<int>(val));
}
}
unittests/uorb_unittests/uORBCommunicatorMock.hpp
+2 -2
View File
@@ -126,8 +126,8 @@ class uORB_test::uORBCommunicatorMock : public uORBCommunicator::IChannel
private:
uORBCommunicator::IChannelRxHandler* _rx_handler;
int _sub_topicA_copy_fd;
int _sub_topicB_copy_fd;
//int _sub_topicA_copy_fd;
//int _sub_topicB_copy_fd;
std::map<std::string, std::string> _topic_translation_map;