fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-06 13:38:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'multirotor' into fixedwing_l1

Browse Source
This commit is contained in:
Lorenz Meier
2013-08-27 16:11:41 +02:00
parent a897b3d88e 3380d40a7d
commit 0104f070c6
66 changed files with 1893 additions and 1320 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Makefile
+36 -20
View File
@@ -40,14 +40,16 @@ export PX4_BASE := $(realpath $(dir $(lastword $(MAKEFILE_LIST))))/
include $(PX4_BASE)makefiles/setup.mk
#
# Canned firmware configurations that we build.
# Canned firmware configurations that we (know how to) build.
#
CONFIGS ?= $(subst config_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)config_*.mk))))
KNOWN_CONFIGS := $(subst config_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)config_*.mk))))
CONFIGS ?= $(KNOWN_CONFIGS)
#
# Boards that we build NuttX export kits for.
# Boards that we (know how to) build NuttX export kits for.
#
BOARDS := $(subst board_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)board_*.mk))))
KNOWN_BOARDS := $(subst board_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)board_*.mk))))
BOARDS ?= $(KNOWN_BOARDS)
#
# Debugging
@@ -87,10 +89,11 @@ endif
#
# Built products
#
STAGED_FIRMWARES = $(foreach config,$(CONFIGS),$(IMAGE_DIR)$(config).px4)
FIRMWARES = $(foreach config,$(CONFIGS),$(BUILD_DIR)$(config).build/firmware.px4)
DESIRED_FIRMWARES = $(foreach config,$(CONFIGS),$(IMAGE_DIR)$(config).px4)
STAGED_FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(IMAGE_DIR)$(config).px4)
FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(BUILD_DIR)$(config).build/firmware.px4)
all: $(STAGED_FIRMWARES)
all: $(DESIRED_FIRMWARES)
#
# Copy FIRMWARES into the image directory.
@@ -114,13 +117,26 @@ $(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4:
@$(ECHO) %%%%
@$(ECHO) %%%% Building $(config) in $(work_dir)
@$(ECHO) %%%%
$(Q) mkdir -p $(work_dir)
$(Q) make -r -C $(work_dir) \
$(Q) $(MKDIR) -p $(work_dir)
$(Q) $(MAKE) -r -C $(work_dir) \
-f $(PX4_MK_DIR)firmware.mk \
CONFIG=$(config) \
WORK_DIR=$(work_dir) \
$(FIRMWARE_GOAL)
#
# Make FMU firmwares depend on the corresponding IO firmware.
#
# This is a pretty vile hack, since it hard-codes knowledge of the FMU->IO dependency
# and forces the _default config in all cases. There has to be a better way to do this...
#
FMU_VERSION = $(patsubst px4fmu-%,%,$(word 1, $(subst _, ,$(1))))
define FMU_DEP
$(BUILD_DIR)$(1).build/firmware.px4: $(IMAGE_DIR)px4io-$(call FMU_VERSION,$(1))_default.px4
endef
FMU_CONFIGS := $(filter px4fmu%,$(CONFIGS))
$(foreach config,$(FMU_CONFIGS),$(eval $(call FMU_DEP,$(config))))
#
# Build the NuttX export archives.
#
@@ -147,12 +163,12 @@ $(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 -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean
$(Q) $(MAKE) -r -j1 -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 -j1 -C $(NUTTX_SRC) -r $(MQUIET) CONFIG_ARCH_BOARD=$(board) export
$(Q) mkdir -p $(dir $@)
$(Q) $(MAKE) -r -j1 -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))
@@ -168,11 +184,11 @@ BOARD = $(BOARDS)
menuconfig: $(NUTTX_SRC)
@$(ECHO) %% Configuring NuttX for $(BOARD)
$(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export)
$(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean
$(Q) $(MAKE) -r -j1 -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 -j1 -C $(NUTTX_SRC) -r $(MQUIET) menuconfig
$(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) menuconfig
@$(ECHO) %% Saving configuration file
$(Q)$(COPY) $(NUTTX_SRC).config $(PX4_BASE)nuttx-configs/$(BOARD)/nsh/defconfig
else
@@ -191,7 +207,7 @@ $(NUTTX_SRC):
# Testing targets
#
testbuild:
$(Q) (cd $(PX4_BASE) && make distclean && make archives && make -j8)
$(Q) (cd $(PX4_BASE) && $(MAKE) distclean && $(MAKE) archives && $(MAKE) -j8)
#
# Cleanup targets. 'clean' should remove all built products and force
@@ -206,8 +222,8 @@ clean:
.PHONY: distclean
distclean: clean
$(Q) $(REMOVE) $(ARCHIVE_DIR)*.export
$(Q) make -C $(NUTTX_SRC) -r $(MQUIET) distclean
$(Q) (cd $(NUTTX_SRC)/configs && find . -maxdepth 1 -type l -delete)
$(Q) $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) distclean
$(Q) (cd $(NUTTX_SRC)/configs && $(FIND) . -maxdepth 1 -type l -delete)
#
# Print some help text
@@ -229,9 +245,9 @@ help:
@$(ECHO) " A limited set of configs can be built with CONFIGS=<list-of-configs>"
@$(ECHO) ""
@for config in $(CONFIGS); do \
echo " $$config"; \
echo " Build just the $$config firmware configuration."; \
echo ""; \
$(ECHO) " $$config"; \
$(ECHO) " Build just the $$config firmware configuration."; \
$(ECHO) ""; \
done
@$(ECHO) " clean"
@$(ECHO) " Remove all firmware build pieces."
ROMFS/px4fmu_common/init.d/01_fmu_quad_x
+18 -59
View File
@@ -1,28 +1,6 @@
#!nsh
#
# Flight startup script for PX4FMU with PWM outputs.
#
# disable USB and autostart
set USB no
set MODE custom
echo "[init] doing PX4FMU Quad startup..."
#
# Start the ORB
#
uorb start
#
# Load microSD params
#
echo "[init] loading microSD params"
param select /fs/microsd/params
if [ -f /fs/microsd/params ]
then
param load /fs/microsd/params
fi
echo "[init] 01_fmu_quad_x: PX4FMU Quad X with PWM outputs"
#
# Load default params for this platform
@@ -52,54 +30,35 @@ fi
#
# Force some key parameters to sane values
# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor
# see https://pixhawk.ethz.ch/mavlink/
# MAV_TYPE 2 = quadrotor
#
param set MAV_TYPE 2
#
# Start MAVLink
#
mavlink start -d /dev/ttyS0 -b 57600
usleep 5000
#
# Start the sensors and test them.
#
sh /etc/init.d/rc.sensors
#
# Start the commander.
#
commander start
#
# Start GPS interface (depends on orb)
# Start PWM output
#
gps start
#
# Start the attitude estimator
#
attitude_estimator_ekf start
echo "[init] starting PWM output"
fmu mode_pwm
#
# Load mixer
#
mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
#
# Set PWM output frequency
#
pwm -u 400 -m 0xff
#
# Start common for all multirotors apps
#
sh /etc/init.d/rc.multirotor
#
# Start attitude control
#
multirotor_att_control start
#
# Start logging
#
sdlog2 start -r 50 -a -b 14
# Try to get an USB console
nshterm /dev/ttyACM0 &
# Exit, because /dev/ttyS0 is needed for MAVLink
exit
ROMFS/px4fmu_common/init.d/02_io_quad_x
+18 -72
View File
@@ -1,26 +1,6 @@
#!nsh
#
# Flight startup script for PX4FMU+PX4IO
#
# disable USB and autostart
set USB no
set MODE custom
#
# Start the ORB (first app to start)
#
uorb start
#
# Load microSD params
#
echo "[init] loading microSD params"
param select /fs/microsd/params
if [ -f /fs/microsd/params ]
then
param load /fs/microsd/params
fi
echo "[init] 02_io_quad_x: PX4FMU+PX4IO Quad X with PWM outputs"
#
# Load default params for this platform
@@ -28,74 +8,40 @@ fi
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
# TODO
param set SYS_AUTOCONFIG 0
param save /fs/microsd/params
fi
#
# Force some key parameters to sane values
# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor
# see https://pixhawk.ethz.ch/mavlink/
# MAV_TYPE 2 = quadrotor
#
param set MAV_TYPE 2
#
# Start MAVLink (depends on orb)
# Start MAVLink
#
mavlink start -d /dev/ttyS1 -b 57600
usleep 5000
#
# Start the commander (depends on orb, mavlink)
#
commander start
#
# Start PX4IO interface (depends on orb, commander)
#
px4io start
#
# Allow PX4IO to recover from midair restarts.
# this is very unlikely, but quite safe and robust.
px4io recovery
#
# Disable px4io topic limiting
# Start and configure PX4IO interface
#
px4io limit 200
sh /etc/init.d/rc.io
#
# Start the sensors (depends on orb, px4io)
#
sh /etc/init.d/rc.sensors
#
# Start GPS interface (depends on orb)
#
gps start
#
# Start the attitude estimator (depends on orb)
#
attitude_estimator_ekf start
#
# Load mixer and start controllers (depends on px4io)
# Load mixer
#
mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
#
# Set PWM output frequency
#
pwm -u 400 -m 0xff
multirotor_att_control start
#
# Start logging once armed
# Start common for all multirotors apps
#
sdlog2 start -r 50 -a -b 14
#
# Start system state
#
if blinkm start
then
blinkm systemstate
fi
sh /etc/init.d/rc.multirotor
ROMFS/px4fmu_common/init.d/08_ardrone
+19 -60
View File
@@ -1,86 +1,45 @@
#!nsh
#
# Flight startup script for PX4FMU on PX4IOAR carrier board.
#
echo "[init] doing PX4IOAR startup..."
echo "[init] 08_ardrone: PX4FMU on PX4IOAR carrier board"
#
# Start the ORB
# Load default params for this platform
#
uorb start
#
# Load microSD params
#
echo "[init] loading microSD params"
param select /fs/microsd/params
if [ -f /fs/microsd/params ]
if param compare SYS_AUTOCONFIG 1
then
param load /fs/microsd/params
# Set all params here, then disable autoconfig
# TODO
param set SYS_AUTOCONFIG 0
param save /fs/microsd/params
fi
#
# Force some key parameters to sane values
# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor
# see https://pixhawk.ethz.ch/mavlink/
# MAV_TYPE 2 = quadrotor
#
param set MAV_TYPE 2
#
# Start MAVLink
#
mavlink start -d /dev/ttyS0 -b 57600
usleep 5000
#
# Configure PX4FMU for operation with PX4IOAR
#
fmu mode_gpio_serial
#
# Start the sensors.
#
sh /etc/init.d/rc.sensors
#
# Start MAVLink
#
mavlink start -d /dev/ttyS0 -b 57600
usleep 5000
#
# Start the commander.
#
commander start
#
# Start the attitude estimator
#
attitude_estimator_ekf start
#
# Fire up the multi rotor attitude controller
#
multirotor_att_control start
#
# Fire up the AR.Drone interface.
#
ardrone_interface start -d /dev/ttyS1
#
# Start logging once armed
#
sdlog2 start -r 50 -a -b 14
#
# Start GPS capture
#
gps start
#
# startup is done; we don't want the shell because we
# use the same UART for telemetry
# Start common for all multirotors apps
#
echo "[init] startup done"
# Try to get an USB console
nshterm /dev/ttyACM0 &
sh /etc/init.d/rc.multirotor
# Exit, because /dev/ttyS0 is needed for MAVLink
exit
ROMFS/px4fmu_common/init.d/09_ardrone_flow
+23 -44
View File
@@ -1,49 +1,47 @@
#!nsh
echo "[init] 09_ardrone_flow: PX4FMU on PX4IOAR carrier board with PX4FLOW"
#
# Flight startup script for PX4FMU on PX4IOAR carrier board.
# Load default params for this platform
#
echo "[init] doing PX4IOAR startup..."
#
# Start the ORB
#
uorb start
#
# Load microSD params
#
echo "[init] loading microSD params"
param select /fs/microsd/params
if [ -f /fs/microsd/params ]
if param compare SYS_AUTOCONFIG 1
then
param load /fs/microsd/params
# Set all params here, then disable autoconfig
# TODO
param set SYS_AUTOCONFIG 0
param save /fs/microsd/params
fi
#
# Force some key parameters to sane values
# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor
# see https://pixhawk.ethz.ch/mavlink/
# MAV_TYPE 2 = quadrotor
#
param set MAV_TYPE 2
#
# Start MAVLink and MAVLink Onboard (PX4FLOW Sensor)
#
mavlink start -d /dev/ttyS0 -b 57600
mavlink_onboard start -d /dev/ttyS3 -b 115200
usleep 5000
#
# Configure PX4FMU for operation with PX4IOAR
#
fmu mode_gpio_serial
#
# Fire up the AR.Drone interface.
#
ardrone_interface start -d /dev/ttyS1
#
# Start the sensors.
#
sh /etc/init.d/rc.sensors
#
# Start MAVLink and MAVLink Onboard (Flow Sensor)
#
mavlink start -d /dev/ttyS0 -b 57600
mavlink_onboard start -d /dev/ttyS3 -b 115200
usleep 5000
#
# Start the commander.
#
@@ -73,25 +71,6 @@ flow_position_control start
# Fire up the flow speed controller
#
flow_speed_control start
#
# Fire up the AR.Drone interface.
#
ardrone_interface start -d /dev/ttyS1
#
# Start logging once armed
#
sdlog2 start -r 50 -a -b 14
#
# startup is done; we don't want the shell because we
# use the same UART for telemetry
#
echo "[init] startup done"
# Try to get an USB console
nshterm /dev/ttyACM0 &
# Exit, because /dev/ttyS0 is needed for MAVLink
exit
ROMFS/px4fmu_common/init.d/10_io_f330
+28 -78
View File
@@ -1,12 +1,6 @@
#!nsh
#
# Flight startup script for PX4FMU+PX4IO on an F330 quad.
#
#
# Start the ORB (first app to start)
#
uorb start
echo "[init] 10_io_f330: PX4FMU+PX4IO on a DJI F330 quad frame"
#
# Load default params for this platform
@@ -16,27 +10,26 @@ then
# Set all params here, then disable autoconfig
param set SYS_AUTOCONFIG 0
param set MC_ATTRATE_D 0.005
param set MC_ATTRATE_D 0.004
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_P 0.1
param set MC_ATTRATE_P 0.12
param set MC_ATT_D 0.0
param set MC_ATT_I 0.0
param set MC_ATT_P 4.5
param set MC_ATT_P 7.0
param set MC_RCLOSS_THR 0.0
param set MC_YAWPOS_D 0.0
param set MC_YAWPOS_I 0.3
param set MC_YAWPOS_P 0.6
param set MC_YAWRATE_D 0.0
param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_P 0.1
param set MC_YAWPOS_I 0.0
param set MC_YAWPOS_P 2.0
param set MC_YAWRATE_D 0.005
param set MC_YAWRATE_I 0.2
param set MC_YAWRATE_P 0.3
param save /fs/microsd/params
fi
#
# Force some key parameters to sane values
# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor
# see https://pixhawk.ethz.ch/mavlink/
# MAV_TYPE 2 = quadrotor
#
param set MAV_TYPE 2
@@ -47,71 +40,28 @@ mavlink start
usleep 5000
#
# Start the commander (depends on orb, mavlink)
# Start and configure PX4IO interface
#
commander start
sh /etc/init.d/rc.io
#
# Start PX4IO interface (depends on orb, commander)
# Set PWM values for DJI ESCs
#
if px4io start
then
#
# This sets a PWM right after startup (regardless of safety button)
#
px4io idle 900 900 900 900
pwm -u 400 -m 0xff
#
# Allow PX4IO to recover from midair restarts.
# this is very unlikely, but quite safe and robust.
px4io recovery
#
# The values are for spinning motors when armed using DJI ESCs
#
px4io min 1200 1200 1200 1200
#
# Upper limits could be higher, this is on the safe side
#
px4io max 1800 1800 1800 1800
mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
else
# SOS
tone_alarm 6
fi
#
# Start the sensors (depends on orb, px4io)
#
sh /etc/init.d/rc.sensors
#
# Start GPS interface (depends on orb)
#
gps start
#
# Start the attitude estimator (depends on orb)
#
attitude_estimator_ekf start
multirotor_att_control start
px4io idle 900 900 900 900
px4io min 1200 1200 1200 1200
px4io max 1800 1800 1800 1800
#
# Disable px4io topic limiting and start logging
# Load mixer
#
if [ $BOARD == fmuv1 ]
then
px4io limit 200
sdlog2 start -r 50 -a -b 16
else
px4io limit 400
sdlog2 start -r 200 -a -b 16
fi
mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
#
# Set PWM output frequency
#
pwm -u 400 -m 0xff
#
# Start common for all multirotors apps
#
sh /etc/init.d/rc.multirotor
ROMFS/px4fmu_common/init.d/15_io_tbs
+10 -81
View File
@@ -1,27 +1,6 @@
#!nsh
#
# Flight startup script for PX4FMU+PX4IO on a Team Blacksheep Discovery quad
# with stock DJI ESCs, motors and props.
#
# disable USB and autostart
set USB no
set MODE custom
#
# Start the ORB (first app to start)
#
uorb start
#
# Load microSD params
#
echo "[init] loading microSD params"
param select /fs/microsd/params
if [ -f /fs/microsd/params ]
then
param load /fs/microsd/params
fi
echo "[init] 15_io_tbs: PX4FMU+PX4IO on a Team Blacksheep Discovery quad"
#
# Load default params for this platform
@@ -47,11 +26,10 @@ then
param save /fs/microsd/params
fi
#
# Force some key parameters to sane values
# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor
# see https://pixhawk.ethz.ch/mavlink/
# MAV_TYPE 2 = quadrotor
#
param set MAV_TYPE 2
@@ -62,77 +40,28 @@ mavlink start
usleep 5000
#
# Start the commander (depends on orb, mavlink)
# Start and configure PX4IO interface
#
commander start
sh /etc/init.d/rc.io
#
# Start PX4IO interface (depends on orb, commander)
#
px4io start
pwm -u 400 -m 0xff
#
# Allow PX4IO to recover from midair restarts.
# This is very unlikely, but quite safe and robust.
px4io recovery
#
# This sets a PWM right after startup (regardless of safety button)
# Set PWM values for DJI ESCs
#
px4io idle 900 900 900 900
#
# The values are for spinning motors when armed using DJI ESCs
#
px4io min 1180 1180 1180 1180
#
# Upper limits could be higher, this is on the safe side
#
px4io max 1800 1800 1800 1800
#
# Load the mixer for a quad with wide arms
#
mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix
#
# Start the sensors (depends on orb, px4io)
#
sh /etc/init.d/rc.sensors
#
# Start GPS interface (depends on orb)
#
gps start
#
# Start the attitude estimator (depends on orb)
#
attitude_estimator_ekf start
#
# Start the controllers (depends on orb)
# Set PWM output frequency
#
multirotor_att_control start
pwm -u 400 -m 0xff
#
# Disable px4io topic limiting and start logging
# Start common for all multirotors apps
#
if [ $BOARD == fmuv1 ]
then
px4io limit 200
sdlog2 start -r 50 -a -b 16
if blinkm start
then
blinkm systemstate
fi
else
px4io limit 400
sdlog2 start -r 200 -a -b 16
if rgbled start
then
#rgbled systemstate
fi
fi
sh /etc/init.d/rc.multirotor
ROMFS/px4fmu_common/init.d/30_io_camflyer
+11 -60
View File
@@ -1,26 +1,6 @@
#!nsh
#
# Flight startup script for PX4FMU+PX4IO
#
# disable USB and autostart
set USB no
set MODE custom
#
# Start the ORB (first app to start)
#
uorb start
#
# Load microSD params
#
echo "[init] loading microSD params"
param select /fs/microsd/params
if [ -f /fs/microsd/params ]
then
param load /fs/microsd/params
fi
echo "[init] 30_io_camflyer: PX4FMU+PX4IO on Camflyer"
#
# Load default params for this platform
@@ -28,39 +8,18 @@ fi
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
# TODO
param set SYS_AUTOCONFIG 0
param save /fs/microsd/params
fi
#
# Force some key parameters to sane values
# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor
# see https://pixhawk.ethz.ch/mavlink/
# MAV_TYPE 1 = fixed wing
#
param set MAV_TYPE 1
#
# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell)
#
if [ -f /fs/microsd/px4io.bin ]
then
echo "PX4IO Firmware found. Checking Upgrade.."
if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current
then
echo "No newer version, skipping upgrade."
else
echo "Loading /fs/microsd/px4io.bin"
if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io_update.log
then
cp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current
echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io_update.log
else
echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io_update.log
echo "Failed to upgrade PX4IO firmware - check if PX4IO is in bootloader mode"
fi
fi
fi
#
# Start MAVLink (depends on orb)
#
@@ -90,6 +49,11 @@ px4io limit 100
# Start the sensors (depends on orb, px4io)
#
sh /etc/init.d/rc.sensors
#
# Start logging (depends on sensors)
#
sh /etc/init.d/rc.logging
#
# Start GPS interface (depends on orb)
@@ -105,17 +69,4 @@ kalman_demo start
# Load mixer and start controllers (depends on px4io)
#
mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
control_demo start
#
# Start logging
#
sdlog2 start -r 50 -a -b 14
#
# Start system state
#
if blinkm start
then
blinkm systemstate
fi
fw_att_control start
ROMFS/px4fmu_common/init.d/31_io_phantom
+24 -78
View File
@@ -1,26 +1,6 @@
#!nsh
#
# Flight startup script for PX4FMU+PX4IO
#
# disable USB and autostart
set USB no
set MODE custom
#
# Start the ORB (first app to start)
#
uorb start
#
# Load microSD params
#
echo "[init] loading microSD params"
param select /fs/microsd/params
if [ -f /fs/microsd/params ]
then
param load /fs/microsd/params
fi
echo "[init] 31_io_phantom: PX4FMU+PX4IO on Phantom"
#
# Load default params for this platform
@@ -28,76 +8,55 @@ fi
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
# TODO
param set SYS_AUTOCONFIG 0
param save /fs/microsd/params
fi
#
# Force some key parameters to sane values
# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor
# see https://pixhawk.ethz.ch/mavlink/
# MAV_TYPE 1 = fixed wing
#
param set MAV_TYPE 1
#
# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell)
#
if [ -f /fs/microsd/px4io.bin ]
then
echo "PX4IO Firmware found. Checking Upgrade.."
if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current
then
echo "No newer version, skipping upgrade."
else
echo "Loading /fs/microsd/px4io.bin"
if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io_update.log
then
cp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current
echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io_update.log
else
echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io_update.log
echo "Failed to upgrade PX4IO firmware - check if PX4IO is in bootloader mode"
fi
fi
fi
#
# Start MAVLink (depends on orb)
#
mavlink start -d /dev/ttyS1 -b 57600
usleep 5000
#
# Start the commander (depends on orb, mavlink)
# Start and configure PX4IO interface
#
commander start
#
# Start PX4IO interface (depends on orb, commander)
#
px4io start
#
# Allow PX4IO to recover from midair restarts.
# this is very unlikely, but quite safe and robust.
px4io recovery
sh /etc/init.d/rc.io
#
# Set actuator limit to 100 Hz update (50 Hz PWM)
px4io limit 100
#
# Start the sensors (depends on orb, px4io)
# Start the commander
#
sh /etc/init.d/rc.sensors
commander start
#
# Start GPS interface (depends on orb)
# Start the sensors
#
sh /etc/init.d/rc.sensors
#
# Start logging (depends on sensors)
#
sh /etc/init.d/rc.logging
#
# Start GPS interface
#
gps start
#
# Start the attitude estimator (depends on orb)
# Start the attitude estimator
#
kalman_demo start
@@ -105,17 +64,4 @@ kalman_demo start
# Load mixer and start controllers (depends on px4io)
#
mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
control_demo start
#
# Start logging
#
sdlog2 start -r 50 -a -b 14
#
# Start system state
#
if blinkm start
then
blinkm systemstate
fi
fw_att_control start
ROMFS/px4fmu_common/init.d/40_io_segway
+9 -75
View File
@@ -1,26 +1,4 @@
#!nsh
#
# Flight startup script for PX4FMU+PX4IO
#
# disable USB and autostart
set USB no
set MODE custom
#
# Start the ORB (first app to start)
#
uorb start
#
# Load microSD params
#
echo "[init] loading microSD params"
param select /fs/microsd/params
if [ -f /fs/microsd/params ]
then
param load /fs/microsd/params
fi
#
# Load default params for this platform
@@ -28,65 +6,34 @@ fi
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
# TODO
param set SYS_AUTOCONFIG 0
param save /fs/microsd/params
fi
#
# Force some key parameters to sane values
# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor
# see https://pixhawk.ethz.ch/mavlink/
# MAV_TYPE 10 = ground rover
#
param set MAV_TYPE 10
#
# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell)
#
if [ -f /fs/microsd/px4io.bin ]
then
echo "PX4IO Firmware found. Checking Upgrade.."
if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current
then
echo "No newer version, skipping upgrade."
else
echo "Loading /fs/microsd/px4io.bin"
if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io_update.log
then
cp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current
echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io_update.log
else
echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io_update.log
echo "Failed to upgrade PX4IO firmware - check if PX4IO is in bootloader mode"
fi
fi
fi
#
# Start MAVLink (depends on orb)
#
mavlink start -d /dev/ttyS1 -b 57600
usleep 5000
#
# Start and configure PX4IO interface
#
sh /etc/init.d/rc.io
#
# Start the commander (depends on orb, mavlink)
#
commander start
#
# Start PX4IO interface (depends on orb, commander)
#
px4io start
#
# Allow PX4IO to recover from midair restarts.
# this is very unlikely, but quite safe and robust.
px4io recovery
#
# Disable px4io topic limiting
#
px4io limit 200
#
# Start the sensors (depends on orb, px4io)
#
@@ -107,16 +54,3 @@ attitude_estimator_ekf start
#
md25 start 3 0x58
segway start
#
# Start logging
#
sdlog2 start -r 50 -a -b 14
#
# Start system state
#
if blinkm start
then
blinkm systemstate
fi
ROMFS/px4fmu_common/init.d/666_fmu_q_x550
+64
View File
@@ -0,0 +1,64 @@
#!nsh
echo "[init] 666_fmu_q_x550: PX4FMU Quad X550 with PWM outputs"
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
param set MC_ATTRATE_P 0.14
param set MC_ATTRATE_I 0
param set MC_ATTRATE_D 0.006
param set MC_ATT_P 5.5
param set MC_ATT_I 0
param set MC_ATT_D 0
param set MC_YAWPOS_D 0
param set MC_YAWPOS_I 0
param set MC_YAWPOS_P 0.6
param set MC_YAWRATE_D 0
param set MC_YAWRATE_I 0
param set MC_YAWRATE_P 0.08
param set RC_SCALE_PITCH 1
param set RC_SCALE_ROLL 1
param set RC_SCALE_YAW 3
param set SYS_AUTOCONFIG 0
param save /fs/microsd/params
fi
#
# Force some key parameters to sane values
# MAV_TYPE 2 = quadrotor
#
param set MAV_TYPE 2
#
# Start MAVLink
#
mavlink start -d /dev/ttyS0 -b 57600
usleep 5000
#
# Start PWM output
#
fmu mode_pwm
#
# Load mixer
#
mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
#
# Set PWM output frequency
#
pwm -u 400 -m 0xff
#
# Start common for all multirotors apps
#
sh /etc/init.d/rc.multirotor
# Exit, because /dev/ttyS0 is needed for MAVLink
exit
ROMFS/px4fmu_common/init.d/rc.io
+23
View File
@@ -0,0 +1,23 @@
#
# Start PX4IO interface (depends on orb, commander)
#
if px4io start
then
#
# Allow PX4IO to recover from midair restarts.
# this is very unlikely, but quite safe and robust.
px4io recovery
#
# Disable px4io topic limiting
#
if [ $BOARD == fmuv1 ]
then
px4io limit 200
else
px4io limit 400
fi
else
# SOS
tone_alarm 6
fi
ROMFS/px4fmu_common/init.d/rc.logging
+6 -1
View File
@@ -5,5 +5,10 @@
if [ -d /fs/microsd ]
then
sdlog start
if [ $BOARD == fmuv1 ]
then
sdlog2 start -r 50 -a -b 16
else
sdlog2 start -r 200 -a -b 16
fi
fi
ROMFS/px4fmu_common/init.d/rc.multirotor
+44
View File
@@ -0,0 +1,44 @@
#!nsh
#
# Standard everything needed for multirotors except mixer, output and mavlink
#
#
# Start the sensors and test them.
#
sh /etc/init.d/rc.sensors
#
# Start logging (depends on sensors)
#
sh /etc/init.d/rc.logging
#
# Start the commander.
#
commander start
#
# Start GPS interface (depends on orb)
#
gps start
#
# Start the attitude estimator
#
attitude_estimator_ekf start
#
# Start position estimator
#
position_estimator_inav start
#
# Start attitude control
#
multirotor_att_control start
#
# Start position control
#
multirotor_pos_control start
ROMFS/px4fmu_common/init.d/rcS
+153 -149
View File
@@ -57,162 +57,166 @@ fi
if [ $MODE == autostart ]
then
#
# Start terminal
#
if sercon
then
echo "USB connected"
else
# second attempt
sercon &
fi
#
# Start the ORB (first app to start)
#
uorb start
#
# Load microSD params
#
if ramtron start
then
param select /ramtron/params
if [ -f /ramtron/params ]
#
# Start terminal
#
if sercon
then
param load /ramtron/params
fi
else
param select /fs/microsd/params
if [ -f /fs/microsd/params ]
then
param load /fs/microsd/params
fi
fi
#
# Start system state indicator
#
if rgbled start
then
echo "Using external RGB Led"
else
if blinkm start
then
blinkm systemstate
fi
fi
#
# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell)
#
if [ -f /fs/microsd/px4io.bin ]
then
echo "PX4IO Firmware found. Checking Upgrade.."
if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.cur
then
echo "No newer version, skipping upgrade."
echo "USB connected"
else
echo "Loading /fs/microsd/px4io.bin"
if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io.log
# second attempt
sercon &
fi
#
# Start the ORB (first app to start)
#
uorb start
#
# Load microSD params
#
if ramtron start
then
param select /ramtron/params
if [ -f /ramtron/params ]
then
cp /fs/microsd/px4io.bin /fs/microsd/px4io.cur
echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io.log
param load /ramtron/params
fi
else
param select /fs/microsd/params
if [ -f /fs/microsd/params ]
then
param load /fs/microsd/params
fi
fi
#
# Start system state indicator
#
if rgbled start
then
echo "Using external RGB Led"
else
if blinkm start
then
blinkm systemstate
fi
fi
# Try to get an USB console
nshterm /dev/ttyACM0 &
#
# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell)
#
if [ -f /fs/microsd/px4io.bin ]
then
echo "PX4IO Firmware found. Checking Upgrade.."
if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.cur
then
echo "No newer version, skipping upgrade."
else
echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io.log
echo "Failed to upgrade px4io firmware - check if px4io is in bootloader mode"
echo "Loading /fs/microsd/px4io.bin"
if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io.log
then
cp /fs/microsd/px4io.bin /fs/microsd/px4io.cur
echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io.log
else
echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io.log
echo "Failed to upgrade px4io firmware - check if px4io is in bootloader mode"
fi
fi
fi
fi
#
# Check if auto-setup from one of the standard scripts is wanted
# SYS_AUTOSTART = 0 means no autostart (default)
#
if param compare SYS_AUTOSTART 1
then
sh /etc/init.d/01_fmu_quad_x
set MODE custom
fi
if param compare SYS_AUTOSTART 2
then
sh /etc/init.d/02_io_quad_x
set MODE custom
fi
if param compare SYS_AUTOSTART 8
then
sh /etc/init.d/08_ardrone
set MODE custom
fi
if param compare SYS_AUTOSTART 9
then
sh /etc/init.d/09_ardrone_flow
set MODE custom
fi
if param compare SYS_AUTOSTART 10
then
sh /etc/init.d/10_io_f330
set MODE custom
fi
if param compare SYS_AUTOSTART 15
then
sh /etc/init.d/15_io_tbs
set MODE custom
fi
if param compare SYS_AUTOSTART 30
then
sh /etc/init.d/30_io_camflyer
set MODE custom
fi
if param compare SYS_AUTOSTART 31
then
sh /etc/init.d/31_io_phantom
set MODE custom
fi
# Try to get an USB console
nshterm /dev/ttyACM0 &
# If none of the autostart scripts triggered, get a minimal setup
if [ $MODE == autostart ]
then
# Telemetry port is on both FMU boards ttyS1
mavlink start -b 57600 -d /dev/ttyS1
usleep 5000
# Start commander
commander start
# Start px4io if present
if px4io start
#
# Check if auto-setup from one of the standard scripts is wanted
# SYS_AUTOSTART = 0 means no autostart (default)
#
if param compare SYS_AUTOSTART 1
then
echo "PX4IO driver started"
else
if fmu mode_serial
then
echo "FMU driver started"
fi
sh /etc/init.d/01_fmu_quad_x
set MODE custom
fi
if param compare SYS_AUTOSTART 2
then
sh /etc/init.d/02_io_quad_x
set MODE custom
fi
if param compare SYS_AUTOSTART 8
then
sh /etc/init.d/08_ardrone
set MODE custom
fi
if param compare SYS_AUTOSTART 9
then
sh /etc/init.d/09_ardrone_flow
set MODE custom
fi
if param compare SYS_AUTOSTART 10
then
sh /etc/init.d/10_io_f330
set MODE custom
fi
if param compare SYS_AUTOSTART 15
then
sh /etc/init.d/15_io_tbs
set MODE custom
fi
if param compare SYS_AUTOSTART 30
then
sh /etc/init.d/30_io_camflyer
set MODE custom
fi
if param compare SYS_AUTOSTART 31
then
sh /etc/init.d/31_io_phantom
set MODE custom
fi
# Start any custom extensions that might be missing
if [ -f /fs/microsd/etc/rc.local ]
then
sh /fs/microsd/etc/rc.local
fi
# If none of the autostart scripts triggered, get a minimal setup
if [ $MODE == autostart ]
then
# Telemetry port is on both FMU boards ttyS1
mavlink start -b 57600 -d /dev/ttyS1
usleep 5000
# Start commander
commander start
# Start px4io if present
if px4io start
then
echo "PX4IO driver started"
else
if fmu mode_serial
then
echo "FMU driver started"
fi
fi
# Start sensors
sh /etc/init.d/rc.sensors
# Start one of the estimators
attitude_estimator_ekf start
# Start GPS
gps start
fi
# Start sensors
sh /etc/init.d/rc.sensors
# Start one of the estimators
attitude_estimator_ekf start
# Start GPS
gps start
fi
# End of autostart
fi
makefiles/config_px4fmu-v1_default.mk
+7
View File
@@ -6,6 +6,7 @@
# Use the configuration's ROMFS.
#
ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common
ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v1_default.bin
#
# Board support modules
@@ -90,6 +91,12 @@ MODULES += examples/flow_speed_control
#
MODULES += modules/sdlog2
#
# Unit tests
#
MODULES += modules/unit_test
MODULES += modules/commander/commander_tests
#
# Library modules
#
makefiles/config_px4fmu-v2_default.mk
+9 -1
View File
@@ -3,9 +3,11 @@
#
#
# Use the configuration's ROMFS.
# Use the configuration's ROMFS, copy the px4iov2 firmware into
# the ROMFS if it's available
#
ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common
ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v2_default.bin
#
# Board support modules
@@ -84,6 +86,12 @@ MODULES += modules/multirotor_pos_control
#
MODULES += modules/sdlog2
#
# Unit tests
#
MODULES += modules/unit_test
MODULES += modules/commander/commander_tests
#
# Library modules
#
makefiles/firmware.mk
+19 -3
View File
@@ -322,7 +322,7 @@ endif
# a root from several templates. That would be a nice feature.
#
# Add dependencies on anything in the ROMFS root
# Add dependencies on anything in the ROMFS root directory
ROMFS_FILES += $(wildcard \
$(ROMFS_ROOT)/* \
$(ROMFS_ROOT)/*/* \
@@ -334,7 +334,14 @@ ifeq ($(ROMFS_FILES),)
$(error ROMFS_ROOT $(ROMFS_ROOT) specifies a directory containing no files)
endif
ROMFS_DEPS += $(ROMFS_FILES)
# Extra files that may be copied into the ROMFS /extras directory
# ROMFS_EXTRA_FILES are required, ROMFS_OPTIONAL_FILES are optional
ROMFS_EXTRA_FILES += $(wildcard $(ROMFS_OPTIONAL_FILES))
ROMFS_DEPS += $(ROMFS_EXTRA_FILES)
ROMFS_IMG = romfs.img
ROMFS_SCRATCH = romfs_scratch
ROMFS_CSRC = $(ROMFS_IMG:.img=.c)
ROMFS_OBJ = $(ROMFS_CSRC:.c=.o)
LIBS += $(ROMFS_OBJ)
@@ -345,9 +352,18 @@ $(ROMFS_OBJ): $(ROMFS_IMG) $(GLOBAL_DEPS)
$(call BIN_TO_OBJ,$<,$@,romfs_img)
# Generate the ROMFS image from the root
$(ROMFS_IMG): $(ROMFS_DEPS) $(GLOBAL_DEPS)
$(ROMFS_IMG): $(ROMFS_SCRATCH) $(ROMFS_DEPS) $(GLOBAL_DEPS)
@$(ECHO) "ROMFS: $@"
$(Q) $(GENROMFS) -f $@ -d $(ROMFS_ROOT) -V "NSHInitVol"
$(Q) $(GENROMFS) -f $@ -d $(ROMFS_SCRATCH) -V "NSHInitVol"
# Construct the ROMFS scratch root from the canonical root
$(ROMFS_SCRATCH): $(ROMFS_DEPS) $(GLOBAL_DEPS)
$(Q) $(MKDIR) -p $(ROMFS_SCRATCH)
$(Q) $(COPYDIR) $(ROMFS_ROOT)/* $(ROMFS_SCRATCH)
ifneq ($(ROMFS_EXTRA_FILES),)
$(Q) $(MKDIR) -p $(ROMFS_SCRATCH)/extras
$(Q) $(COPY) $(ROMFS_EXTRA_FILES) $(ROMFS_SCRATCH)/extras
endif
EXTRA_CLEANS += $(ROMGS_OBJ) $(ROMFS_IMG)
makefiles/setup.mk
+1
View File
@@ -73,6 +73,7 @@ export RMDIR = rm -rf
export GENROMFS = genromfs
export TOUCH = touch
export MKDIR = mkdir
export FIND = find
export ECHO = echo
export UNZIP_CMD = unzip
export PYTHON = python
nuttx-configs/px4fmu-v1/nsh/defconfig
+4 -4
View File
@@ -405,8 +405,8 @@ CONFIG_SIG_SIGWORK=4
CONFIG_MAX_TASKS=32
CONFIG_MAX_TASK_ARGS=10
CONFIG_NPTHREAD_KEYS=4
CONFIG_NFILE_DESCRIPTORS=32
CONFIG_NFILE_STREAMS=25
CONFIG_NFILE_DESCRIPTORS=24
CONFIG_NFILE_STREAMS=8
CONFIG_NAME_MAX=32
CONFIG_PREALLOC_MQ_MSGS=4
CONFIG_MQ_MAXMSGSIZE=32
@@ -564,8 +564,8 @@ CONFIG_CDCACM_EPBULKIN_HSSIZE=512
CONFIG_CDCACM_NWRREQS=4
CONFIG_CDCACM_NRDREQS=4
CONFIG_CDCACM_BULKIN_REQLEN=96
CONFIG_CDCACM_RXBUFSIZE=256
CONFIG_CDCACM_TXBUFSIZE=256
CONFIG_CDCACM_RXBUFSIZE=512
CONFIG_CDCACM_TXBUFSIZE=512
CONFIG_CDCACM_VENDORID=0x26ac
CONFIG_CDCACM_PRODUCTID=0x0010
CONFIG_CDCACM_VENDORSTR="3D Robotics"
nuttx-configs/px4fmu-v2/nsh/defconfig
+2 -2
View File
@@ -451,8 +451,8 @@ CONFIG_SIG_SIGWORK=4
CONFIG_MAX_TASKS=32
CONFIG_MAX_TASK_ARGS=10
CONFIG_NPTHREAD_KEYS=4
CONFIG_NFILE_DESCRIPTORS=32
CONFIG_NFILE_STREAMS=25
CONFIG_NFILE_DESCRIPTORS=24
CONFIG_NFILE_STREAMS=8
CONFIG_NAME_MAX=32
CONFIG_PREALLOC_MQ_MSGS=4
CONFIG_MQ_MAXMSGSIZE=32
src/drivers/airspeed/airspeed.cpp
+8 -1
View File
@@ -146,7 +146,14 @@ out:
int
Airspeed::probe()
{
return measure();
/* on initial power up the device needs more than one retry
for detection. Once it is running then retries aren't
needed
*/
_retries = 4;
int ret = measure();
_retries = 0;
return ret;
}
int
src/drivers/stm32/tone_alarm/tone_alarm.cpp
+3 -8
View File
@@ -879,14 +879,9 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg)
_tune = nullptr;
_next = nullptr;
} else {
/* don't interrupt alarms unless they are repeated */
if (_tune != nullptr && !_repeat) {
/* abort and let the current tune finish */
result = -EBUSY;
} else if (_repeat && _default_tune_number == arg) {
/* requested repeating tune already playing */
} else {
// play the selected tune
/* always interrupt alarms, unless they are repeating and already playing */
if (!(_repeat && _default_tune_number == arg)) {
/* play the selected tune */
_default_tune_number = arg;
start_tune(_default_tunes[arg - 1]);
}
src/modules/att_pos_estimator_ekf/KalmanNav.cpp
+1 -1
View File
@@ -325,7 +325,7 @@ void KalmanNav::updatePublications()
_pos.vx = vN;
_pos.vy = vE;
_pos.vz = vD;
_pos.hdg = psi;
_pos.yaw = psi;
// attitude publication
_att.timestamp = _pubTimeStamp;
src/modules/commander/accelerometer_calibration.cpp
+32 -20
View File
@@ -133,9 +133,10 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp
int mat_invert3(float src[3][3], float dst[3][3]);
int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g);
void do_accel_calibration(int mavlink_fd) {
int do_accel_calibration(int mavlink_fd) {
/* announce change */
mavlink_log_info(mavlink_fd, "accel calibration started");
mavlink_log_info(mavlink_fd, "accel cal progress <0> percent");
/* measure and calculate offsets & scales */
float accel_offs[3];
@@ -176,11 +177,11 @@ void do_accel_calibration(int mavlink_fd) {
}
mavlink_log_info(mavlink_fd, "accel calibration done");
tune_positive();
return OK;
} else {
/* measurements error */
mavlink_log_info(mavlink_fd, "accel calibration aborted");
tune_negative();
return ERROR;
}
/* exit accel calibration mode */
@@ -211,39 +212,50 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
}
int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
unsigned done_count = 0;
while (true) {
bool done = true;
char str[80];
int str_ptr;
str_ptr = sprintf(str, "keep vehicle still:");
unsigned old_done_count = done_count;
done_count = 0;
for (int i = 0; i < 6; i++) {
if (!data_collected[i]) {
str_ptr += sprintf(&(str[str_ptr]), " %s", orientation_strs[i]);
done = false;
}
}
mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s",
(!data_collected[0]) ? "x+ " : "",
(!data_collected[1]) ? "x- " : "",
(!data_collected[2]) ? "y+ " : "",
(!data_collected[3]) ? "y- " : "",
(!data_collected[4]) ? "z+ " : "",
(!data_collected[5]) ? "z- " : "");
if (old_done_count != done_count)
mavlink_log_info(mavlink_fd, "accel cal progress <%u> percent", 17 * done_count);
if (done)
break;
mavlink_log_info(mavlink_fd, str);
int orient = detect_orientation(mavlink_fd, sensor_combined_sub);
if (orient < 0)
return ERROR;
if (data_collected[orient]) {
sprintf(str, "%s direction already measured, please rotate", orientation_strs[orient]);
mavlink_log_info(mavlink_fd, str);
mavlink_log_info(mavlink_fd, "%s done, please rotate to a different axis", orientation_strs[orient]);
continue;
}
sprintf(str, "meas started: %s", orientation_strs[orient]);
mavlink_log_info(mavlink_fd, str);
mavlink_log_info(mavlink_fd, "accel measurement started: %s axis", orientation_strs[orient]);
read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num);
str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient],
mavlink_log_info(mavlink_fd, "result for %s axis: [ %.2f %.2f %.2f ]", orientation_strs[orient],
(double)accel_ref[orient][0],
(double)accel_ref[orient][1],
(double)accel_ref[orient][2]);
mavlink_log_info(mavlink_fd, str);
data_collected[orient] = true;
tune_neutral();
}
@@ -253,7 +265,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
float accel_T[3][3];
int res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
if (res != 0) {
mavlink_log_info(mavlink_fd, "ERROR: calibration values calc error");
mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error");
return ERROR;
}
@@ -286,7 +298,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
/* set accel error threshold to 5m/s^2 */
float accel_err_thr = 5.0f;
/* still time required in us */
int64_t still_time = 2000000;
hrt_abstime still_time = 2000000;
struct pollfd fds[1];
fds[0].fd = sub_sensor_combined;
fds[0].events = POLLIN;
@@ -325,12 +337,12 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
/* is still now */
if (t_still == 0) {
/* first time */
mavlink_log_info(mavlink_fd, "still...");
mavlink_log_info(mavlink_fd, "detected rest position, waiting...");
t_still = t;
t_timeout = t + timeout;
} else {
/* still since t_still */
if ((int64_t) t - (int64_t) t_still > still_time) {
if (t > t_still + still_time) {
/* vehicle is still, exit from the loop to detection of its orientation */
break;
}
@@ -340,7 +352,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
accel_disp[2] > still_thr2 * 2.0f) {
/* not still, reset still start time */
if (t_still != 0) {
mavlink_log_info(mavlink_fd, "moving...");
mavlink_log_info(mavlink_fd, "detected motion, please hold still...");
t_still = 0;
}
}
@@ -352,7 +364,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
}
if (poll_errcount > 1000) {
mavlink_log_info(mavlink_fd, "ERROR: failed reading accel");
mavlink_log_info(mavlink_fd, "ERROR: Failed reading sensor");
return -1;
}
}
src/modules/commander/accelerometer_calibration.h
+1 -1
View File
@@ -45,6 +45,6 @@
#include <stdint.h>
void do_accel_calibration(int mavlink_fd);
int do_accel_calibration(int mavlink_fd);
#endif /* ACCELEROMETER_CALIBRATION_H_ */
src/modules/commander/airspeed_calibration.cpp
+13 -5
View File
@@ -49,7 +49,13 @@
#include <systemlib/param/param.h>
#include <systemlib/err.h>
void do_airspeed_calibration(int mavlink_fd)
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
int do_airspeed_calibration(int mavlink_fd)
{
/* give directions */
mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still");
@@ -79,7 +85,7 @@ void do_airspeed_calibration(int mavlink_fd)
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
mavlink_log_info(mavlink_fd, "airspeed calibration aborted");
return;
return ERROR;
}
}
@@ -89,6 +95,7 @@ void do_airspeed_calibration(int mavlink_fd)
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, "Setting offs failed!");
return ERROR;
}
/* auto-save to EEPROM */
@@ -96,6 +103,8 @@ void do_airspeed_calibration(int mavlink_fd)
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
mavlink_log_info(mavlink_fd, "FAILED storing calibration");
return ERROR;
}
//char buf[50];
@@ -103,11 +112,10 @@ void do_airspeed_calibration(int mavlink_fd)
//mavlink_log_info(mavlink_fd, buf);
mavlink_log_info(mavlink_fd, "airspeed calibration done");
tune_positive();
return OK;
} else {
mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)");
return ERROR;
}
close(diff_pres_sub);
}
src/modules/commander/airspeed_calibration.h
+2 -2
View File
@@ -41,6 +41,6 @@
#include <stdint.h>
void do_airspeed_calibration(int mavlink_fd);
int do_airspeed_calibration(int mavlink_fd);
#endif /* AIRSPEED_CALIBRATION_H_ */
#endif /* AIRSPEED_CALIBRATION_H_ */
src/modules/commander/baro_calibration.cpp
+8 -2
View File
@@ -47,8 +47,14 @@
#include <mavlink/mavlink_log.h>
#include <systemlib/param/param.h>
void do_baro_calibration(int mavlink_fd)
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
int do_baro_calibration(int mavlink_fd)
{
// TODO implement this
return;
return ERROR;
}
src/modules/commander/baro_calibration.h
+2 -2
View File
@@ -41,6 +41,6 @@
#include <stdint.h>
void do_baro_calibration(int mavlink_fd);
int do_baro_calibration(int mavlink_fd);
#endif /* BARO_CALIBRATION_H_ */
#endif /* BARO_CALIBRATION_H_ */
src/modules/commander/commander.cpp
+164 -138
View File
@@ -84,6 +84,7 @@
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
#include <systemlib/cpuload.h>
#include <systemlib/rc_check.h>
#include "px4_custom_mode.h"
#include "commander_helper.h"
@@ -209,7 +210,7 @@ void print_reject_arm(const char *msg);
void print_status();
transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode);
transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
/**
* Loop that runs at a lower rate and priority for calibration and parameter tasks.
@@ -322,10 +323,10 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
switch (cmd->command) {
case VEHICLE_CMD_DO_SET_MODE: {
uint8_t base_mode = (uint8_t) cmd->param1;
uint32_t custom_mode = (uint32_t) cmd->param2;
uint8_t custom_main_mode = (uint8_t) cmd->param2;
// TODO remove debug code
mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_mode);
mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode);
/* set arming state */
transition_result_t arming_res = TRANSITION_NOT_CHANGED;
@@ -355,19 +356,19 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
/* use autopilot-specific mode */
if (custom_mode == PX4_CUSTOM_MODE_MANUAL) {
if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
/* MANUAL */
main_res = main_state_transition(status, MAIN_STATE_MANUAL);
} else if (custom_mode == PX4_CUSTOM_MODE_SEATBELT) {
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) {
/* SEATBELT */
main_res = main_state_transition(status, MAIN_STATE_SEATBELT);
} else if (custom_mode == PX4_CUSTOM_MODE_EASY) {
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) {
/* EASY */
main_res = main_state_transition(status, MAIN_STATE_EASY);
} else if (custom_mode == PX4_CUSTOM_MODE_AUTO) {
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
/* AUTO */
main_res = main_state_transition(status, MAIN_STATE_AUTO);
}
@@ -502,7 +503,13 @@ int commander_thread_main(int argc, char *argv[])
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
if (mavlink_fd < 0) {
warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.");
/* try again later */
usleep(20000);
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
if (mavlink_fd < 0) {
warnx("ERROR: Failed to open MAVLink log stream again, start mavlink app first.");
}
}
/* Main state machine */
@@ -610,6 +617,8 @@ int commander_thread_main(int argc, char *argv[])
bool updated = false;
bool rc_calibration_ok = (OK == rc_calibration_check());
/* Subscribe to safety topic */
int safety_sub = orb_subscribe(ORB_ID(safety));
memset(&safety, 0, sizeof(safety));
@@ -720,6 +729,9 @@ int commander_thread_main(int argc, char *argv[])
param_get(_param_system_id, &(status.system_id));
param_get(_param_component_id, &(status.component_id));
status_changed = true;
/* Re-check RC calibration */
rc_calibration_ok = (OK == rc_calibration_check());
}
}
@@ -793,9 +805,9 @@ int commander_thread_main(int argc, char *argv[])
status.condition_landed = local_position.landed;
status_changed = true;
if (status.condition_landed) {
mavlink_log_info(mavlink_fd, "[cmd] LANDED");
mavlink_log_critical(mavlink_fd, "[cmd] LANDED");
} else {
mavlink_log_info(mavlink_fd, "[cmd] IN AIR");
mavlink_log_critical(mavlink_fd, "[cmd] IN AIR");
}
}
}
@@ -1010,46 +1022,42 @@ int commander_thread_main(int argc, char *argv[])
/* arm/disarm by RC */
res = TRANSITION_NOT_CHANGED;
/* check if left stick is in lower left position and we are in MANUAL or AUTO mode -> disarm
/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm
* do it only for rotary wings */
if (status.is_rotary_wing &&
(status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
(status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY)) {
if (sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
res = arming_state_transition(&status, &safety, new_arming_state, &armed);
stick_off_counter = 0;
} else {
stick_off_counter++;
}
stick_on_counter = 0;
(status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY ||
(status.condition_landed && (
status.navigation_state == NAVIGATION_STATE_ALTHOLD ||
status.navigation_state == NAVIGATION_STATE_VECTOR
))) && sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
res = arming_state_transition(&status, &safety, new_arming_state, &armed);
stick_off_counter = 0;
} else {
stick_off_counter = 0;
stick_off_counter++;
}
} else {
stick_off_counter = 0;
}
/* check if left stick is in lower right position and we're in manual mode -> arm */
/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
if (status.arming_state == ARMING_STATE_STANDBY &&
status.main_state == MAIN_STATE_MANUAL) {
if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
stick_on_counter = 0;
} else {
stick_on_counter++;
}
stick_off_counter = 0;
status.main_state == MAIN_STATE_MANUAL &&
sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
stick_on_counter = 0;
} else {
stick_on_counter = 0;
stick_on_counter++;
}
} else {
stick_on_counter = 0;
}
if (res == TRANSITION_CHANGED) {
@@ -1206,7 +1214,7 @@ int commander_thread_main(int argc, char *argv[])
}
/* evaluate the navigation state machine */
transition_result_t res = check_navigation_state_machine(&status, &control_mode);
transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position);
if (res == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */
@@ -1564,7 +1572,7 @@ print_reject_arm(const char *msg)
}
transition_result_t
check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode)
check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos)
{
transition_result_t res = TRANSITION_DENIED;
@@ -1582,11 +1590,15 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v
break;
case MAIN_STATE_AUTO:
if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
/* don't act while taking off */
res = TRANSITION_NOT_CHANGED;
} else {
if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) {
if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
/* don't switch to other states until takeoff not completed */
if (local_pos->z > -5.0f || status.condition_landed) {
res = TRANSITION_NOT_CHANGED;
break;
}
}
/* check again, state can be changed */
if (current_status->condition_landed) {
/* if landed: transitions only to AUTO_READY are allowed */
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode);
@@ -1613,12 +1625,13 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v
}
} else {
/* always switch to loiter in air when no RC control */
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
/* switch to mission in air when no RC control */
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
}
}
} else {
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode);
}
break;
default:
@@ -1628,6 +1641,33 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v
return res;
}
void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result)
{
switch (result) {
case VEHICLE_CMD_RESULT_ACCEPTED:
tune_positive();
break;
case VEHICLE_CMD_RESULT_DENIED:
mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command);
tune_negative();
break;
case VEHICLE_CMD_RESULT_FAILED:
mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command);
tune_negative();
break;
case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command);
tune_negative();
break;
case VEHICLE_CMD_RESULT_UNSUPPORTED:
mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command);
tune_negative();
break;
default:
break;
}
}
void *commander_low_prio_loop(void *arg)
{
/* Set thread name */
@@ -1665,128 +1705,114 @@ void *commander_low_prio_loop(void *arg)
/* ignore commands the high-prio loop handles */
if (cmd.command == VEHICLE_CMD_DO_SET_MODE ||
cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM)
cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM ||
cmd.command == VEHICLE_CMD_NAV_TAKEOFF)
continue;
/* result of the command */
uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED;
/* only handle low-priority commands here */
switch (cmd.command) {
case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
if (is_safe(&status, &safety, &armed)) {
if (is_safe(&status, &safety, &armed)) {
if (((int)(cmd.param1)) == 1) {
/* reboot */
systemreset(false);
} else if (((int)(cmd.param1)) == 3) {
/* reboot to bootloader */
systemreset(true);
} else {
result = VEHICLE_CMD_RESULT_DENIED;
}
if (((int)(cmd.param1)) == 1) {
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
usleep(1000000);
/* reboot */
systemreset(false);
} else if (((int)(cmd.param1)) == 3) {
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
usleep(1000000);
/* reboot to bootloader */
systemreset(true);
} else {
result = VEHICLE_CMD_RESULT_DENIED;
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
}
break;
} else {
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
}
break;
case VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
/* try to go to INIT/PREFLIGHT arming state */
int calib_ret = ERROR;
// XXX disable interrupts in arming_state_transition
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) {
result = VEHICLE_CMD_RESULT_DENIED;
break;
}
if ((int)(cmd.param1) == 1) {
/* gyro calibration */
do_gyro_calibration(mavlink_fd);
result = VEHICLE_CMD_RESULT_ACCEPTED;
} else if ((int)(cmd.param2) == 1) {
/* magnetometer calibration */
do_mag_calibration(mavlink_fd);
result = VEHICLE_CMD_RESULT_ACCEPTED;
} else if ((int)(cmd.param3) == 1) {
/* zero-altitude pressure calibration */
result = VEHICLE_CMD_RESULT_DENIED;
} else if ((int)(cmd.param4) == 1) {
/* RC calibration */
result = VEHICLE_CMD_RESULT_DENIED;
} else if ((int)(cmd.param5) == 1) {
/* accelerometer calibration */
do_accel_calibration(mavlink_fd);
result = VEHICLE_CMD_RESULT_ACCEPTED;
} else if ((int)(cmd.param6) == 1) {
/* airspeed calibration */
do_airspeed_calibration(mavlink_fd);
result = VEHICLE_CMD_RESULT_ACCEPTED;
}
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
/* try to go to INIT/PREFLIGHT arming state */
// XXX disable interrupts in arming_state_transition
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) {
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
break;
}
case VEHICLE_CMD_PREFLIGHT_STORAGE: {
if ((int)(cmd.param1) == 1) {
/* gyro calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
calib_ret = do_gyro_calibration(mavlink_fd);
if (((int)(cmd.param1)) == 0) {
if (0 == param_load_default()) {
mavlink_log_info(mavlink_fd, "[cmd] parameters loaded");
result = VEHICLE_CMD_RESULT_ACCEPTED;
} else if ((int)(cmd.param2) == 1) {
/* magnetometer calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
calib_ret = do_mag_calibration(mavlink_fd);
} else {
mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR");
tune_error();
result = VEHICLE_CMD_RESULT_FAILED;
}
} else if ((int)(cmd.param3) == 1) {
/* zero-altitude pressure calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
} else if (((int)(cmd.param1)) == 1) {
if (0 == param_save_default()) {
mavlink_log_info(mavlink_fd, "[cmd] parameters saved");
result = VEHICLE_CMD_RESULT_ACCEPTED;
} else if ((int)(cmd.param4) == 1) {
/* RC calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
} else {
mavlink_log_critical(mavlink_fd, "[cmd] parameters save error");
tune_error();
result = VEHICLE_CMD_RESULT_FAILED;
}
}
} else if ((int)(cmd.param5) == 1) {
/* accelerometer calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
calib_ret = do_accel_calibration(mavlink_fd);
break;
} else if ((int)(cmd.param6) == 1) {
/* airspeed calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
calib_ret = do_airspeed_calibration(mavlink_fd);
}
default:
if (calib_ret == OK)
tune_positive();
else
tune_negative();
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
break;
}
/* supported command handling stop */
if (result == VEHICLE_CMD_RESULT_ACCEPTED) {
tune_positive();
case VEHICLE_CMD_PREFLIGHT_STORAGE: {
} else {
tune_negative();
if (((int)(cmd.param1)) == 0) {
if (0 == param_load_default()) {
mavlink_log_info(mavlink_fd, "[cmd] parameters loaded");
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
if (result == VEHICLE_CMD_RESULT_DENIED) {
mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command);
} else {
mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR");
answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
}
} else if (result == VEHICLE_CMD_RESULT_FAILED) {
mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command);
} else if (((int)(cmd.param1)) == 1) {
if (0 == param_save_default()) {
mavlink_log_info(mavlink_fd, "[cmd] parameters saved");
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
} else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) {
mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command);
} else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) {
mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command);
} else {
mavlink_log_critical(mavlink_fd, "[cmd] parameters save error");
answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
}
}
break;
}
default:
answer_command(cmd, VEHICLE_CMD_RESULT_UNSUPPORTED);
break;
}
/* send any requested ACKs */
src/modules/commander/commander_tests/commander_tests.cpp
+55
View File
@@ -0,0 +1,55 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Simon Wilks <sjwilks@gmail.com>
*
* 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 commander_tests.cpp
* Commander unit tests. Run the tests as follows:
* nsh> commander_tests
*
*/
#include <systemlib/err.h>
#include "state_machine_helper_test.h"
extern "C" __EXPORT int commander_tests_main(int argc, char *argv[]);
int commander_tests_main(int argc, char *argv[])
{
state_machine_helper_test();
//state_machine_test();
return 0;
}
src/modules/commander/commander_tests/module.mk
+41
View File
@@ -0,0 +1,41 @@
############################################################################
#
# 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.
#
############################################################################
#
# System state machine tests.
#
MODULE_COMMAND = commander_tests
SRCS = commander_tests.cpp \
state_machine_helper_test.cpp \
../state_machine_helper.cpp
src/modules/commander/commander_tests/state_machine_helper_test.cpp
+247
View File
@@ -0,0 +1,247 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Simon Wilks <sjwilks@gmail.com>
*
* 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 state_machine_helper_test.cpp
* System state machine unit test.
*
*/
#include "state_machine_helper_test.h"
#include "../state_machine_helper.h"
#include <unit_test/unit_test.h>
class StateMachineHelperTest : public UnitTest
{
public:
StateMachineHelperTest();
virtual ~StateMachineHelperTest();
virtual const char* run_tests();
private:
const char* arming_state_transition_test();
const char* arming_state_transition_arm_disarm_test();
const char* main_state_transition_test();
const char* is_safe_test();
};
StateMachineHelperTest::StateMachineHelperTest() {
}
StateMachineHelperTest::~StateMachineHelperTest() {
}
const char*
StateMachineHelperTest::arming_state_transition_test()
{
struct vehicle_status_s status;
struct safety_s safety;
arming_state_t new_arming_state;
struct actuator_armed_s armed;
// Identical states.
status.arming_state = ARMING_STATE_INIT;
new_arming_state = ARMING_STATE_INIT;
mu_assert("no transition: identical states",
TRANSITION_NOT_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed));
// INIT to STANDBY.
armed.armed = false;
armed.ready_to_arm = false;
status.arming_state = ARMING_STATE_INIT;
status.condition_system_sensors_initialized = true;
new_arming_state = ARMING_STATE_STANDBY;
mu_assert("transition: init to standby",
TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed));
mu_assert("current state: standby", ARMING_STATE_STANDBY == status.arming_state);
mu_assert("not armed", !armed.armed);
mu_assert("ready to arm", armed.ready_to_arm);
// INIT to STANDBY, sensors not initialized.
armed.armed = false;
armed.ready_to_arm = false;
status.arming_state = ARMING_STATE_INIT;
status.condition_system_sensors_initialized = false;
new_arming_state = ARMING_STATE_STANDBY;
mu_assert("no transition: sensors not initialized",
TRANSITION_DENIED == arming_state_transition(&status, &safety, new_arming_state, &armed));
mu_assert("current state: init", ARMING_STATE_INIT == status.arming_state);
mu_assert("not armed", !armed.armed);
mu_assert("not ready to arm", !armed.ready_to_arm);
return 0;
}
const char*
StateMachineHelperTest::arming_state_transition_arm_disarm_test()
{
struct vehicle_status_s status;
struct safety_s safety;
arming_state_t new_arming_state;
struct actuator_armed_s armed;
// TODO(sjwilks): ARM then DISARM.
return 0;
}
const char*
StateMachineHelperTest::main_state_transition_test()
{
struct vehicle_status_s current_state;
main_state_t new_main_state;
// Identical states.
current_state.main_state = MAIN_STATE_MANUAL;
new_main_state = MAIN_STATE_MANUAL;
mu_assert("no transition: identical states",
TRANSITION_NOT_CHANGED == main_state_transition(&current_state, new_main_state));
mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
// AUTO to MANUAL.
current_state.main_state = MAIN_STATE_AUTO;
new_main_state = MAIN_STATE_MANUAL;
mu_assert("transition changed: auto to manual",
TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
mu_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state);
// MANUAL to SEATBELT.
current_state.main_state = MAIN_STATE_MANUAL;
current_state.condition_local_altitude_valid = true;
new_main_state = MAIN_STATE_SEATBELT;
mu_assert("tranisition: manual to seatbelt",
TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
mu_assert("new state: seatbelt", MAIN_STATE_SEATBELT == current_state.main_state);
// MANUAL to SEATBELT, invalid local altitude.
current_state.main_state = MAIN_STATE_MANUAL;
current_state.condition_local_altitude_valid = false;
new_main_state = MAIN_STATE_SEATBELT;
mu_assert("no transition: invalid local altitude",
TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
// MANUAL to EASY.
current_state.main_state = MAIN_STATE_MANUAL;
current_state.condition_local_position_valid = true;
new_main_state = MAIN_STATE_EASY;
mu_assert("transition: manual to easy",
TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
mu_assert("current state: easy", MAIN_STATE_EASY == current_state.main_state);
// MANUAL to EASY, invalid local position.
current_state.main_state = MAIN_STATE_MANUAL;
current_state.condition_local_position_valid = false;
new_main_state = MAIN_STATE_EASY;
mu_assert("no transition: invalid position",
TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
// MANUAL to AUTO.
current_state.main_state = MAIN_STATE_MANUAL;
current_state.condition_global_position_valid = true;
new_main_state = MAIN_STATE_AUTO;
mu_assert("transition: manual to auto",
TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
mu_assert("current state: auto", MAIN_STATE_AUTO == current_state.main_state);
// MANUAL to AUTO, invalid global position.
current_state.main_state = MAIN_STATE_MANUAL;
current_state.condition_global_position_valid = false;
new_main_state = MAIN_STATE_AUTO;
mu_assert("no transition: invalid global position",
TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
return 0;
}
const char*
StateMachineHelperTest::is_safe_test()
{
struct vehicle_status_s current_state;
struct safety_s safety;
struct actuator_armed_s armed;
armed.armed = false;
armed.lockdown = false;
safety.safety_switch_available = true;
safety.safety_off = false;
mu_assert("is safe: not armed", is_safe(&current_state, &safety, &armed));
armed.armed = false;
armed.lockdown = true;
safety.safety_switch_available = true;
safety.safety_off = true;
mu_assert("is safe: software lockdown", is_safe(&current_state, &safety, &armed));
armed.armed = true;
armed.lockdown = false;
safety.safety_switch_available = true;
safety.safety_off = true;
mu_assert("not safe: safety off", !is_safe(&current_state, &safety, &armed));
armed.armed = true;
armed.lockdown = false;
safety.safety_switch_available = true;
safety.safety_off = false;
mu_assert("is safe: safety off", is_safe(&current_state, &safety, &armed));
armed.armed = true;
armed.lockdown = false;
safety.safety_switch_available = false;
safety.safety_off = false;
mu_assert("not safe: no safety switch", !is_safe(&current_state, &safety, &armed));
return 0;
}
const char*
StateMachineHelperTest::run_tests()
{
mu_run_test(arming_state_transition_test);
mu_run_test(arming_state_transition_arm_disarm_test);
mu_run_test(main_state_transition_test);
mu_run_test(is_safe_test);
return 0;
}
void
state_machine_helper_test()
{
StateMachineHelperTest* test = new StateMachineHelperTest();
test->UnitTest::print_results(test->run_tests());
}
src/modules/commander/commander_tests/state_machine_helper_test.h
+44
View File
@@ -0,0 +1,44 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Simon Wilks <sjwilks@gmail.com>
*
* 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 state_machine_helper_test.h
*/
#ifndef STATE_MACHINE_HELPER_TEST_H_
#define STATE_MACHINE_HELPER_TEST_
void state_machine_helper_test();
#endif /* STATE_MACHINE_HELPER_TEST_H_ */
src/modules/commander/gyro_calibration.cpp
+26 -20
View File
@@ -50,10 +50,15 @@
#include <systemlib/param/param.h>
#include <systemlib/err.h>
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
void do_gyro_calibration(int mavlink_fd)
int do_gyro_calibration(int mavlink_fd)
{
mavlink_log_info(mavlink_fd, "gyro calibration starting, hold still");
mavlink_log_info(mavlink_fd, "Gyro calibration starting, do not move unit.");
const int calibration_count = 5000;
@@ -79,6 +84,8 @@ void do_gyro_calibration(int mavlink_fd)
close(fd);
unsigned poll_errcount = 0;
while (calibration_counter < calibration_count) {
/* wait blocking for new data */
@@ -88,17 +95,20 @@ void do_gyro_calibration(int mavlink_fd)
int poll_ret = poll(fds, 1, 1000);
if (poll_ret) {
if (poll_ret > 0) {
orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
gyro_offset[0] += raw.gyro_rad_s[0];
gyro_offset[1] += raw.gyro_rad_s[1];
gyro_offset[2] += raw.gyro_rad_s[2];
calibration_counter++;
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry");
return;
} else {
poll_errcount++;
}
if (poll_errcount > 1000) {
mavlink_log_info(mavlink_fd, "ERROR: Failed reading gyro sensor");
return ERROR;
}
}
@@ -137,18 +147,17 @@ void do_gyro_calibration(int mavlink_fd)
if (save_ret != 0) {
warnx("WARNING: auto-save of params to storage failed");
mavlink_log_critical(mavlink_fd, "gyro store failed");
// XXX negative tune
return;
return ERROR;
}
mavlink_log_info(mavlink_fd, "gyro calibration done");
tune_positive();
tune_neutral();
/* third beep by cal end routine */
} else {
mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)");
return;
return ERROR;
}
@@ -180,8 +189,7 @@ void do_gyro_calibration(int mavlink_fd)
&& (hrt_absolute_time() - start_time > 5 * 1e6)) {
mavlink_log_info(mavlink_fd, "gyro scale calibration skipped");
mavlink_log_info(mavlink_fd, "gyro calibration done");
tune_positive();
return;
return OK;
}
/* wait blocking for new data */
@@ -221,7 +229,7 @@ void do_gyro_calibration(int mavlink_fd)
// operating near the 1e6/1e8 max sane resolution of float.
gyro_integral += (raw.gyro_rad_s[2] * dt_ms) / 1e3f;
warnx("dbg: b: %6.4f, g: %6.4f", baseline_integral, gyro_integral);
// warnx("dbg: b: %6.4f, g: %6.4f", (double)baseline_integral, (double)gyro_integral);
// } else if (poll_ret == 0) {
// /* any poll failure for 1s is a reason to abort */
@@ -232,8 +240,8 @@ void do_gyro_calibration(int mavlink_fd)
float gyro_scale = baseline_integral / gyro_integral;
float gyro_scales[] = { gyro_scale, gyro_scale, gyro_scale };
warnx("gyro scale: yaw (z): %6.4f", gyro_scale);
mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", gyro_scale);
warnx("gyro scale: yaw (z): %6.4f", (double)gyro_scale);
mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale);
if (isfinite(gyro_scales[0]) && isfinite(gyro_scales[1]) && isfinite(gyro_scales[2])) {
@@ -272,12 +280,10 @@ void do_gyro_calibration(int mavlink_fd)
// mavlink_log_info(mavlink_fd, buf);
mavlink_log_info(mavlink_fd, "gyro calibration done");
tune_positive();
/* third beep by cal end routine */
return OK;
} else {
mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)");
return ERROR;
}
close(sub_sensor_combined);
}
src/modules/commander/gyro_calibration.h
+2 -2
View File
@@ -41,6 +41,6 @@
#include <stdint.h>
void do_gyro_calibration(int mavlink_fd);
int do_gyro_calibration(int mavlink_fd);
#endif /* GYRO_CALIBRATION_H_ */
#endif /* GYRO_CALIBRATION_H_ */
src/modules/commander/mag_calibration.cpp
+34 -16
View File
@@ -53,10 +53,15 @@
#include <systemlib/param/param.h>
#include <systemlib/err.h>
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
void do_mag_calibration(int mavlink_fd)
int do_mag_calibration(int mavlink_fd)
{
mavlink_log_info(mavlink_fd, "mag calibration starting, hold still");
mavlink_log_info(mavlink_fd, "please put the system in a rest position and wait.");
int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
struct mag_report mag;
@@ -95,6 +100,8 @@ void do_mag_calibration(int mavlink_fd)
close(fd);
mavlink_log_info(mavlink_fd, "mag cal progress <20> percent");
/* calibrate offsets */
// uint64_t calibration_start = hrt_absolute_time();
@@ -113,9 +120,13 @@ void do_mag_calibration(int mavlink_fd)
warnx("mag cal failed: out of memory");
mavlink_log_info(mavlink_fd, "mag cal failed: out of memory");
warnx("x:%p y:%p z:%p\n", x, y, z);
return;
return ERROR;
}
mavlink_log_info(mavlink_fd, "scale calibration completed, dynamic calibration starting..");
unsigned poll_errcount = 0;
while (hrt_absolute_time() < calibration_deadline &&
calibration_counter < calibration_maxcount) {
@@ -130,9 +141,8 @@ void do_mag_calibration(int mavlink_fd)
axis_index++;
char buf[50];
sprintf(buf, "please rotate around %c", axislabels[axis_index]);
mavlink_log_info(mavlink_fd, buf);
mavlink_log_info(mavlink_fd, "please rotate in a figure 8 or around %c axis.", axislabels[axis_index]);
mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_maxcount * 50) / calibration_counter);
tune_neutral();
axis_deadline += calibration_interval / 3;
@@ -152,7 +162,7 @@ void do_mag_calibration(int mavlink_fd)
int poll_ret = poll(fds, 1, 1000);
if (poll_ret) {
if (poll_ret > 0) {
orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
x[calibration_counter] = mag.x;
@@ -184,11 +194,16 @@ void do_mag_calibration(int mavlink_fd)
calibration_counter++;
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
mavlink_log_info(mavlink_fd, "mag cal canceled (timed out)");
break;
} else {
poll_errcount++;
}
if (poll_errcount > 1000) {
mavlink_log_info(mavlink_fd, "ERROR: Failed reading mag sensor");
return ERROR;
}
}
float sphere_x;
@@ -246,12 +261,15 @@ void do_mag_calibration(int mavlink_fd)
warnx("Setting Z mag scale failed!\n");
}
mavlink_log_info(mavlink_fd, "mag cal progress <90> percent");
/* auto-save to EEPROM */
int save_ret = param_save_default();
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
mavlink_log_info(mavlink_fd, "FAILED storing calibration");
return ERROR;
}
warnx("\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n",
@@ -267,14 +285,14 @@ void do_mag_calibration(int mavlink_fd)
(double)mscale.y_scale, (double)mscale.z_scale);
mavlink_log_info(mavlink_fd, buf);
mavlink_log_info(mavlink_fd, "mag calibration done");
mavlink_log_info(mavlink_fd, "magnetometer calibration completed");
mavlink_log_info(mavlink_fd, "mag cal progress <100> percent");
tune_positive();
return OK;
/* third beep by cal end routine */
} else {
mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)");
return ERROR;
}
close(sub_mag);
}
}
src/modules/commander/mag_calibration.h
+2 -2
View File
@@ -41,6 +41,6 @@
#include <stdint.h>
void do_mag_calibration(int mavlink_fd);
int do_mag_calibration(int mavlink_fd);
#endif /* MAG_CALIBRATION_H_ */
#endif /* MAG_CALIBRATION_H_ */
src/modules/commander/px4_custom_mode.h
+24 -5
View File
@@ -8,11 +8,30 @@
#ifndef PX4_CUSTOM_MODE_H_
#define PX4_CUSTOM_MODE_H_
enum PX4_CUSTOM_MODE {
PX4_CUSTOM_MODE_MANUAL = 1,
PX4_CUSTOM_MODE_SEATBELT,
PX4_CUSTOM_MODE_EASY,
PX4_CUSTOM_MODE_AUTO,
enum PX4_CUSTOM_MAIN_MODE {
PX4_CUSTOM_MAIN_MODE_MANUAL = 1,
PX4_CUSTOM_MAIN_MODE_SEATBELT,
PX4_CUSTOM_MAIN_MODE_EASY,
PX4_CUSTOM_MAIN_MODE_AUTO,
};
enum PX4_CUSTOM_SUB_MODE_AUTO {
PX4_CUSTOM_SUB_MODE_AUTO_READY = 1,
PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF,
PX4_CUSTOM_SUB_MODE_AUTO_LOITER,
PX4_CUSTOM_SUB_MODE_AUTO_MISSION,
PX4_CUSTOM_SUB_MODE_AUTO_RTL,
PX4_CUSTOM_SUB_MODE_AUTO_LAND,
};
union px4_custom_mode {
struct {
uint16_t reserved;
uint8_t main_mode;
uint8_t sub_mode;
};
uint32_t data;
float data_float;
};
#endif /* PX4_CUSTOM_MODE_H_ */
src/modules/commander/rc_calibration.cpp
+9 -4
View File
@@ -46,8 +46,13 @@
#include <systemlib/param/param.h>
#include <systemlib/err.h>
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
void do_rc_calibration(int mavlink_fd)
int do_rc_calibration(int mavlink_fd)
{
mavlink_log_info(mavlink_fd, "trim calibration starting");
@@ -75,9 +80,9 @@ void do_rc_calibration(int mavlink_fd)
if (save_ret != 0) {
mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed");
return ERROR;
}
tune_positive();
mavlink_log_info(mavlink_fd, "trim calibration done");
}
return OK;
}
src/modules/commander/rc_calibration.h
+2 -2
View File
@@ -41,6 +41,6 @@
#include <stdint.h>
void do_rc_calibration(int mavlink_fd);
int do_rc_calibration(int mavlink_fd);
#endif /* RC_CALIBRATION_H_ */
#endif /* RC_CALIBRATION_H_ */
src/modules/commander/state_machine_helper.cpp
+1
View File
@@ -413,6 +413,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
if (ret == TRANSITION_CHANGED) {
current_status->navigation_state = new_navigation_state;
control_mode->auto_state = current_status->navigation_state;
navigation_state_changed = true;
}
}
src/modules/mavlink/mavlink.c
+20 -4
View File
@@ -205,19 +205,35 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u
/* main state */
*mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
union px4_custom_mode custom_mode;
custom_mode.data = 0;
if (v_status.main_state == MAIN_STATE_MANUAL) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
*mavlink_custom_mode = PX4_CUSTOM_MODE_MANUAL;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
} else if (v_status.main_state == MAIN_STATE_SEATBELT) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
*mavlink_custom_mode = PX4_CUSTOM_MODE_SEATBELT;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;
} else if (v_status.main_state == MAIN_STATE_EASY) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
*mavlink_custom_mode = PX4_CUSTOM_MODE_EASY;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY;
} else if (v_status.main_state == MAIN_STATE_AUTO) {
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
*mavlink_custom_mode = PX4_CUSTOM_MODE_AUTO;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
if (v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF;
} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
}
}
*mavlink_custom_mode = custom_mode.data;
/**
* Set mavlink state
src/modules/mavlink/mavlink_receiver.cpp
+4 -1
View File
@@ -71,6 +71,7 @@
#include <systemlib/err.h>
#include <systemlib/airspeed.h>
#include <mavlink/mavlink_log.h>
#include <commander/px4_custom_mode.h>
__BEGIN_DECLS
@@ -196,9 +197,11 @@ handle_message(mavlink_message_t *msg)
mavlink_set_mode_t new_mode;
mavlink_msg_set_mode_decode(msg, &new_mode);
union px4_custom_mode custom_mode;
custom_mode.data = new_mode.custom_mode;
/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
vcmd.param1 = new_mode.base_mode;
vcmd.param2 = new_mode.custom_mode;
vcmd.param2 = custom_mode.main_mode;
vcmd.param3 = 0;
vcmd.param4 = 0;
vcmd.param5 = 0;
src/modules/mavlink/orb_listener.c
+5 -5
View File
@@ -341,7 +341,7 @@ l_global_position(const struct listener *l)
int16_t vz = (int16_t)(global_pos.vz * 100.0f);
/* heading in degrees * 10, from 0 to 36.000) */
uint16_t hdg = (global_pos.hdg / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f);
uint16_t hdg = (global_pos.yaw / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f);
mavlink_msg_global_position_int_send(MAVLINK_COMM_0,
timestamp / 1000,
@@ -666,12 +666,12 @@ l_airspeed(const struct listener *l)
orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed);
float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f;
float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1);
float alt = global_pos.alt;
float climb = global_pos.vz;
float alt = global_pos.relative_alt;
float climb = -global_pos.vz;
mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed,
((att.yaw + M_PI_F) / M_PI_F) * 180.0f, throttle, alt, climb);
mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, alt, climb);
}
static void *
src/modules/mavlink/waypoints.c
+17 -23
View File
@@ -294,16 +294,13 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq)
*/
float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt)
{
//TODO: implement for z once altidude contoller is implemented
static uint16_t counter;
// if(counter % 10 == 0) printf(" x = %.10f, y = %.10f\n", x, y);
if (seq < wpm->size) {
mavlink_mission_item_t *cur = &(wpm->waypoints[seq]);
mavlink_mission_item_t *wp = &(wpm->waypoints[seq]);
double current_x_rad = cur->x / 180.0 * M_PI;
double current_y_rad = cur->y / 180.0 * M_PI;
double current_x_rad = wp->x / 180.0 * M_PI;
double current_y_rad = wp->y / 180.0 * M_PI;
double x_rad = lat / 180.0 * M_PI;
double y_rad = lon / 180.0 * M_PI;
@@ -315,7 +312,10 @@ float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float
const double radius_earth = 6371000.0;
return radius_earth * c;
float dxy = radius_earth * c;
float dz = alt - wp->z;
return sqrtf(dxy * dxy + dz * dz);
} else {
return -1.0f;
@@ -383,21 +383,19 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
// XXX TODO
}
if (dist >= 0.f && dist <= orbit /*&& wpm->yaw_reached*/) { //TODO implement yaw
if (dist >= 0.f && dist <= orbit) {
wpm->pos_reached = true;
}
// else
// {
// if(counter % 100 == 0)
// printf("Setpoint not reached yet: %0.4f, orbit: %.4f, coordinate frame: %d\n",dist, orbit, coordinate_frame);
// }
// check if required yaw reached
float yaw_sp = _wrap_pi(wpm->waypoints[wpm->current_active_wp_id].param4 / 180.0f * FM_PI);
float yaw_err = _wrap_pi(yaw_sp - local_pos->yaw);
if (fabsf(yaw_err) < 0.05f) {
wpm->yaw_reached = true;
}
}
//check if the current waypoint was reached
if (wpm->pos_reached /*wpm->yaw_reached &&*/ && !wpm->idle) {
if (wpm->pos_reached && /*wpm->yaw_reached &&*/ !wpm->idle) {
if (wpm->current_active_wp_id < wpm->size) {
mavlink_mission_item_t *cur_wp = &(wpm->waypoints[wpm->current_active_wp_id]);
@@ -412,11 +410,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
bool time_elapsed = false;
if (cur_wp->command == (int)MAV_CMD_NAV_LOITER_TIME) {
if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) {
time_elapsed = true;
}
} else if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) {
if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) {
time_elapsed = true;
} else if (cur_wp->command == (int)MAV_CMD_NAV_TAKEOFF) {
time_elapsed = true;
@@ -493,7 +487,7 @@ int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_positio
// mavlink_wpm_send_setpoint(wpm->current_active_wp_id);
// }
check_waypoints_reached(now, global_position , local_position);
check_waypoints_reached(now, global_position, local_position);
return OK;
}
src/modules/multirotor_att_control/multirotor_att_control_main.c
+66 -61
View File
@@ -81,6 +81,8 @@ __EXPORT int multirotor_att_control_main(int argc, char *argv[]);
static bool thread_should_exit;
static int mc_task;
static bool motor_test_mode = false;
static const float min_takeoff_throttle = 0.3f;
static const float yaw_deadzone = 0.01f;
static int
mc_thread_main(int argc, char *argv[])
@@ -145,16 +147,14 @@ mc_thread_main(int argc, char *argv[])
warnx("starting");
/* store last control mode to detect mode switches */
bool flag_control_manual_enabled = false;
bool flag_control_attitude_enabled = false;
bool control_yaw_position = true;
bool reset_yaw_sp = true;
bool failsafe_first_time = true;
/* prepare the handle for the failsafe throttle */
param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR");
float failsafe_throttle = 0.0f;
param_get(failsafe_throttle_handle, &failsafe_throttle);
while (!thread_should_exit) {
@@ -176,7 +176,7 @@ mc_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(parameter_update), param_sub, &update);
/* update parameters */
// XXX no params here yet
param_get(failsafe_throttle_handle, &failsafe_throttle);
}
/* only run controller if attitude changed */
@@ -208,6 +208,14 @@ mc_thread_main(int argc, char *argv[])
/* get a local copy of the current sensor values */
orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
/* set flag to safe value */
control_yaw_position = true;
/* reset yaw setpoint if not armed */
if (!control_mode.flag_armed) {
reset_yaw_sp = true;
}
/* define which input is the dominating control input */
if (control_mode.flag_control_offboard_enabled) {
/* offboard inputs */
@@ -225,47 +233,43 @@ mc_thread_main(int argc, char *argv[])
att_sp.yaw_body = offboard_sp.p3;
att_sp.thrust = offboard_sp.p4;
att_sp.timestamp = hrt_absolute_time();
/* STEP 2: publish the result to the vehicle actuators */
/* publish the result to the vehicle actuators */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
}
/* reset yaw setpoint after offboard control */
reset_yaw_sp = true;
} else if (control_mode.flag_control_manual_enabled) {
/* direct manual input */
/* manual input */
if (control_mode.flag_control_attitude_enabled) {
/* control attitude, update attitude setpoint depending on mode */
/* initialize to current yaw if switching to manual or att control */
if (control_mode.flag_control_attitude_enabled != flag_control_attitude_enabled ||
control_mode.flag_control_manual_enabled != flag_control_manual_enabled) {
att_sp.yaw_body = att.yaw;
}
static bool rc_loss_first_time = true;
/* if the RC signal is lost, try to stay level and go slowly back down to ground */
if (control_mode.failsave_highlevel) {
failsafe_first_time = false;
if (!control_mode.flag_control_velocity_enabled) {
/* Don't reset attitude setpoint in position control mode, it's handled by position controller. */
/* don't reset attitude setpoint in position control mode, it's handled by position controller. */
att_sp.roll_body = 0.0f;
att_sp.pitch_body = 0.0f;
}
if (!control_mode.flag_control_climb_rate_enabled) {
/* Don't touch throttle in modes with altitude hold, it's handled by position controller.
*
* Only go to failsafe throttle if last known throttle was
* high enough to create some lift to make hovering state likely.
*
* This is to prevent that someone landing, but not disarming his
* multicopter (throttle = 0) does not make it jump up in the air
* if shutting down his remote.
*/
if (isfinite(manual.throttle) && manual.throttle > 0.2f) { // TODO use landed status instead of throttle
/* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */
param_get(failsafe_throttle_handle, &failsafe_throttle);
att_sp.thrust = failsafe_throttle;
if (!control_mode.flag_control_climb_rate_enabled) {
/* don't touch throttle in modes with altitude hold, it's handled by position controller.
*
* Only go to failsafe throttle if last known throttle was
* high enough to create some lift to make hovering state likely.
*
* This is to prevent that someone landing, but not disarming his
* multicopter (throttle = 0) does not make it jump up in the air
* if shutting down his remote.
*/
if (isfinite(manual.throttle) && manual.throttle > min_takeoff_throttle) { // TODO use landed status instead of throttle
/* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */
att_sp.thrust = failsafe_throttle;
} else {
att_sp.thrust = 0.0f;
}
} else {
att_sp.thrust = 0.0f;
}
}
@@ -273,46 +277,41 @@ mc_thread_main(int argc, char *argv[])
* since if the pilot regains RC control, he will be lost regarding
* the current orientation.
*/
if (rc_loss_first_time)
att_sp.yaw_body = att.yaw;
rc_loss_first_time = false;
} else {
rc_loss_first_time = true;
/* control yaw in all manual / assisted modes */
/* set yaw if arming or switching to attitude stabilized mode */
if (!flag_control_attitude_enabled) {
if (failsafe_first_time) {
reset_yaw_sp = true;
}
/* only move setpoint if manual input is != 0 */
if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { // TODO use landed status instead of throttle
rates_sp.yaw = manual.yaw;
} else {
failsafe_first_time = true;
/* only move yaw setpoint if manual input is != 0 and not landed */
if (manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) {
/* control yaw rate */
control_yaw_position = false;
reset_yaw_sp = true;
rates_sp.yaw = manual.yaw;
reset_yaw_sp = true; // has no effect on control, just for beautiful log
} else {
if (reset_yaw_sp) {
att_sp.yaw_body = att.yaw;
reset_yaw_sp = false;
}
control_yaw_position = true;
}
if (!control_mode.flag_control_velocity_enabled) {
/* don't update attitude setpoint in position control mode */
/* update attitude setpoint if not in position control mode */
att_sp.roll_body = manual.roll;
att_sp.pitch_body = manual.pitch;
if (!control_mode.flag_control_climb_rate_enabled) {
/* don't set throttle in altitude hold modes */
/* pass throttle directly if not in altitude control mode */
att_sp.thrust = manual.throttle;
}
}
att_sp.timestamp = hrt_absolute_time();
}
/* reset yaw setpint to current position if needed */
if (reset_yaw_sp) {
att_sp.yaw_body = att.yaw;
reset_yaw_sp = false;
}
if (motor_test_mode) {
@@ -321,10 +320,11 @@ mc_thread_main(int argc, char *argv[])
att_sp.pitch_body = 0.0f;
att_sp.yaw_body = 0.0f;
att_sp.thrust = 0.1f;
att_sp.timestamp = hrt_absolute_time();
}
/* STEP 2: publish the controller output */
att_sp.timestamp = hrt_absolute_time();
/* publish the controller output */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
} else {
@@ -336,7 +336,14 @@ mc_thread_main(int argc, char *argv[])
rates_sp.thrust = manual.throttle;
rates_sp.timestamp = hrt_absolute_time();
}
/* reset yaw setpoint after ACRO */
reset_yaw_sp = true;
}
} else {
/* reset yaw setpoint after non-manual control */
reset_yaw_sp = true;
}
/* check if we should we reset integrals */
@@ -367,6 +374,7 @@ mc_thread_main(int argc, char *argv[])
rates[1] = att.pitchspeed;
rates[2] = att.yawspeed;
multirotor_control_rates(&rates_sp, rates, &actuators, reset_integral);
} else {
/* rates controller disabled, set actuators to zero for safety */
actuators.control[0] = 0.0f;
@@ -374,13 +382,10 @@ mc_thread_main(int argc, char *argv[])
actuators.control[2] = 0.0f;
actuators.control[3] = 0.0f;
}
actuators.timestamp = hrt_absolute_time();
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
/* update state */
flag_control_attitude_enabled = control_mode.flag_control_attitude_enabled;
flag_control_manual_enabled = control_mode.flag_control_manual_enabled;
perf_end(mc_loop_perf);
} /* end of poll call for attitude updates */
} /* end of poll return value check */
src/modules/multirotor_att_control/multirotor_attitude_control.c
+2
View File
@@ -247,6 +247,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
}
rates_sp->thrust = att_sp->thrust;
//need to update the timestamp now that we've touched rates_sp
rates_sp->timestamp = hrt_absolute_time();
motor_skip_counter++;
}
src/modules/multirotor_pos_control/multirotor_pos_control.c
+85 -45
View File
@@ -54,6 +54,7 @@
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/manual_control_setpoint.h>
@@ -221,11 +222,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
bool reset_int_xy = true;
bool was_armed = false;
bool reset_integral = true;
bool reset_auto_pos = true;
hrt_abstime t_prev = 0;
/* integrate in NED frame to estimate wind but not attitude offset */
const float alt_ctl_dz = 0.2f;
const float pos_ctl_dz = 0.05f;
const float takeoff_alt_default = 10.0f;
float ref_alt = 0.0f;
hrt_abstime ref_alt_t = 0;
uint64_t local_ref_timestamp = 0;
@@ -403,61 +405,95 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
}
}
/* publish local position setpoint */
orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
local_pos_sp.yaw = att_sp.yaw_body;
/* local position setpoint is valid and can be used for loiter after position controlled mode */
local_pos_sp_valid = control_mode.flag_control_position_enabled;
reset_auto_pos = true;
/* force reprojection of global setpoint after manual mode */
global_pos_sp_reproject = true;
} else {
/* non-manual mode, use global setpoint */
/* init local projection using local position ref */
if (local_pos.ref_timestamp != local_ref_timestamp) {
global_pos_sp_reproject = true;
local_ref_timestamp = local_pos.ref_timestamp;
double lat_home = local_pos.ref_lat * 1e-7;
double lon_home = local_pos.ref_lon * 1e-7;
map_projection_init(lat_home, lon_home);
mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", lat_home, lon_home);
}
if (global_pos_sp_reproject) {
/* update global setpoint projection */
global_pos_sp_reproject = false;
if (global_pos_sp_valid) {
/* global position setpoint valid, use it */
double sp_lat = global_pos_sp.lat * 1e-7;
double sp_lon = global_pos_sp.lon * 1e-7;
/* project global setpoint to local setpoint */
map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y));
if (global_pos_sp.altitude_is_relative) {
local_pos_sp.z = -global_pos_sp.altitude;
} else {
local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
}
mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y);
} else {
if (!local_pos_sp_valid) {
/* local position setpoint is invalid,
* use current position as setpoint for loiter */
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
local_pos_sp.z = local_pos.z;
}
mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y);
if (control_mode.auto_state == NAVIGATION_STATE_AUTO_READY) {
reset_auto_pos = true;
} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
if (reset_auto_pos) {
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
local_pos_sp.z = -takeoff_alt_default;
local_pos_sp.yaw = att.yaw;
local_pos_sp_valid = true;
att_sp.yaw_body = att.yaw;
reset_auto_pos = false;
mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", local_pos_sp.x, local_pos_sp.y, local_pos_sp.z);
}
} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_LOITER) {
if (reset_auto_pos) {
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
local_pos_sp.z = local_pos.z;
local_pos_sp.yaw = att.yaw;
local_pos_sp_valid = true;
att_sp.yaw_body = att.yaw;
reset_auto_pos = false;
}
} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) {
// TODO
reset_auto_pos = true;
} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_MISSION) {
/* init local projection using local position ref */
if (local_pos.ref_timestamp != local_ref_timestamp) {
global_pos_sp_reproject = true;
local_ref_timestamp = local_pos.ref_timestamp;
double lat_home = local_pos.ref_lat * 1e-7;
double lon_home = local_pos.ref_lon * 1e-7;
map_projection_init(lat_home, lon_home);
mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", lat_home, lon_home);
}
/* publish local position setpoint as projection of global position setpoint */
orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
if (global_pos_sp_reproject) {
/* update global setpoint projection */
global_pos_sp_reproject = false;
if (global_pos_sp_valid) {
/* global position setpoint valid, use it */
double sp_lat = global_pos_sp.lat * 1e-7;
double sp_lon = global_pos_sp.lon * 1e-7;
/* project global setpoint to local setpoint */
map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y));
if (global_pos_sp.altitude_is_relative) {
local_pos_sp.z = -global_pos_sp.altitude;
} else {
local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
}
local_pos_sp.yaw = global_pos_sp.yaw;
att_sp.yaw_body = global_pos_sp.yaw;
mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y);
} else {
if (!local_pos_sp_valid) {
/* local position setpoint is invalid,
* use current position as setpoint for loiter */
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
local_pos_sp.z = local_pos.z;
local_pos_sp.yaw = att.yaw;
local_pos_sp_valid = true;
}
mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y);
}
}
reset_auto_pos = true;
}
if (control_mode.auto_state != NAVIGATION_STATE_AUTO_MISSION) {
global_pos_sp_reproject = true;
}
/* reset setpoints after non-manual modes */
@@ -465,6 +501,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
reset_sp_z = true;
}
/* publish local position setpoint */
orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
/* run position & altitude controllers, calculate velocity setpoint */
if (control_mode.flag_control_altitude_enabled) {
global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2];
@@ -575,6 +614,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
reset_int_z = true;
reset_int_xy = true;
global_pos_sp_reproject = true;
reset_auto_pos = true;
}
/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
src/modules/position_estimator_inav/position_estimator_inav_main.c
+19 -8
View File
@@ -75,8 +75,11 @@ static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int position_estimator_inav_task; /**< Handle of deamon task / thread */
static bool verbose_mode = false;
static hrt_abstime gps_timeout = 1000000; // GPS timeout = 1s
static hrt_abstime flow_timeout = 1000000; // optical flow timeout = 1s
static const hrt_abstime gps_timeout = 1000000; // GPS timeout = 1s
static const hrt_abstime flow_timeout = 1000000; // optical flow timeout = 1s
static const uint32_t updates_counter_len = 1000000;
static const uint32_t pub_interval = 4000; // limit publish rate to 250 Hz
__EXPORT int position_estimator_inav_main(int argc, char *argv[]);
@@ -172,6 +175,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float alt_avg = 0.0f;
bool landed = true;
hrt_abstime landed_time = 0;
bool flag_armed = false;
uint32_t accel_counter = 0;
uint32_t baro_counter = 0;
@@ -275,10 +279,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
uint16_t flow_updates = 0;
hrt_abstime updates_counter_start = hrt_absolute_time();
uint32_t updates_counter_len = 1000000;
hrt_abstime pub_last = hrt_absolute_time();
uint32_t pub_interval = 4000; // limit publish rate to 250 Hz
/* acceleration in NED frame */
float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G };
@@ -407,7 +408,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} else {
// new ground level
baro_alt0 += sonar_corr;
warnx("new home: alt = %.3f", baro_alt0);
mavlink_log_info(mavlink_fd, "[inav] new home: alt = %.3f", baro_alt0);
local_pos.ref_alt = baro_alt0;
local_pos.ref_timestamp = hrt_absolute_time();
@@ -429,7 +429,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (fds[6].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout) {
if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout && gps.eph_m < 5.0f) {
/* initialize reference position if needed */
if (!ref_xy_inited) {
if (ref_xy_init_start == 0) {
@@ -486,6 +486,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
t_prev = t;
/* reset ground level on arm */
if (armed.armed && !flag_armed) {
baro_alt0 -= z_est[0];
z_est[0] = 0.0f;
local_pos.ref_alt = baro_alt0;
local_pos.ref_timestamp = hrt_absolute_time();
mavlink_log_info(mavlink_fd, "[inav] new home on arm: alt = %.3f", baro_alt0);
}
/* accel bias correction, now only for Z
* not strictly correct, but stable and works */
accel_bias[2] += (accel_NED[2] + CONSTANTS_ONE_G) * params.w_acc_bias * dt;
@@ -591,6 +600,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
local_pos.z = z_est[0];
local_pos.vz = z_est[1];
local_pos.landed = landed;
local_pos.yaw = att.yaw;
orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);
@@ -609,7 +619,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (local_pos.v_xy_valid) {
global_pos.vx = local_pos.vx;
global_pos.vy = local_pos.vy;
global_pos.hdg = atan2f(local_pos.vy, local_pos.vx);
}
if (local_pos.z_valid) {
@@ -623,11 +632,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (local_pos.v_z_valid) {
global_pos.vz = local_pos.vz;
}
global_pos.yaw = local_pos.yaw;
global_pos.timestamp = t;
orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos);
}
flag_armed = armed.armed;
}
warnx("exiting.");
src/modules/sdlog2/sdlog2.c
+2
View File
@@ -1182,6 +1182,8 @@ int sdlog2_thread_main(int argc, char *argv[])
pthread_mutex_destroy(&logbuffer_mutex);
pthread_cond_destroy(&logbuffer_cond);
free(lb.data);
warnx("exiting.");
thread_running = false;
src/modules/sensors/sensor_params.c
+5 -9
View File
@@ -157,7 +157,6 @@ PARAM_DEFINE_FLOAT(RC14_MAX, 2000);
PARAM_DEFINE_FLOAT(RC14_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f);
PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */
PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */
PARAM_DEFINE_INT32(RC_DSM_BIND, 0); /* 0 = Idle, 1 = Start DSM2 bind, 2 = Start DSMX bind */
@@ -174,8 +173,8 @@ PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3);
PARAM_DEFINE_INT32(RC_MAP_YAW, 4);
PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5);
PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 6);
PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0);
PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0);
PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 6);
PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0);
//PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
@@ -184,10 +183,7 @@ PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0);
PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); /**< default function: camera yaw / azimuth */
PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera pitch / tilt */
PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); /**< default function: camera trigger */
PARAM_DEFINE_INT32(RC_MAP_AUX4, 0); /**< default function: camera roll */
PARAM_DEFINE_INT32(RC_MAP_AUX5, 0); /**< default function: payload drop */
PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.4f);
PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.4f);
PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 1.0f);
PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f);
PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f);
PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f);
src/modules/sensors/sensors.cpp
+5 -32
View File
@@ -305,8 +305,6 @@ private:
int board_rotation;
int external_mag_rotation;
int rc_type;
int rc_map_roll;
int rc_map_pitch;
int rc_map_yaw;
@@ -342,9 +340,6 @@ private:
param_t max[_rc_max_chan_count];
param_t rev[_rc_max_chan_count];
param_t dz[_rc_max_chan_count];
param_t rc_type;
param_t rc_demix;
param_t gyro_offset[3];
param_t gyro_scale[3];
@@ -566,8 +561,6 @@ Sensors::Sensors() :
}
_parameter_handles.rc_type = param_find("RC_TYPE");
/* mandatory input switched, mapped to channels 1-4 per default */
_parameter_handles.rc_map_roll = param_find("RC_MAP_ROLL");
_parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH");
@@ -692,11 +685,6 @@ Sensors::parameters_update()
if (!rc_valid)
warnx("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n");
/* remote control type */
if (param_get(_parameter_handles.rc_type, &(_parameters.rc_type)) != OK) {
warnx("Failed getting remote control type");
}
/* channel mapping */
if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) {
warnx("Failed getting roll chan index");
@@ -738,26 +726,11 @@ Sensors::parameters_update()
// warnx("Failed getting offboard control mode sw chan index");
// }
if (param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)) != OK) {
warnx("Failed getting mode aux 1 index");
}
if (param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2)) != OK) {
warnx("Failed getting mode aux 2 index");
}
if (param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)) != OK) {
warnx("Failed getting mode aux 3 index");
}
if (param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)) != OK) {
warnx("Failed getting mode aux 4 index");
}
if (param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)) != OK) {
warnx("Failed getting mode aux 5 index");
}
param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1));
param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2));
param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3));
param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4));
param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5));
param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll));
param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch));
param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw));
src/modules/systemlib/module.mk
+2 -1
View File
@@ -48,4 +48,5 @@ SRCS = err.c \
systemlib.c \
airspeed.c \
system_params.c \
mavlink_log.c
mavlink_log.c \
rc_check.c
src/modules/systemlib/rc_check.c
+148
View File
@@ -0,0 +1,148 @@
/****************************************************************************
*
* 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 rc_check.c
*
* RC calibration check
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <fcntl.h>
#include <systemlib/rc_check.h>
#include <systemlib/param/param.h>
#include <mavlink/mavlink_log.h>
#include <uORB/topics/rc_channels.h>
int rc_calibration_check(void) {
char nbuf[20];
param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max,
_parameter_handles_rev, _parameter_handles_dz;
int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
float param_min, param_max, param_trim, param_rev, param_dz;
/* first check channel mappings */
/* check which map param applies */
// if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) {
// mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1);
// /* give system time to flush error message in case there are more */
// usleep(100000);
// count++;
// }
for (int i = 0; i < RC_CHANNELS_MAX; i++) {
/* should the channel be enabled? */
uint8_t count = 0;
/* min values */
sprintf(nbuf, "RC%d_MIN", i + 1);
_parameter_handles_min = param_find(nbuf);
param_get(_parameter_handles_min, &param_min);
/* trim values */
sprintf(nbuf, "RC%d_TRIM", i + 1);
_parameter_handles_trim = param_find(nbuf);
param_get(_parameter_handles_trim, &param_trim);
/* max values */
sprintf(nbuf, "RC%d_MAX", i + 1);
_parameter_handles_max = param_find(nbuf);
param_get(_parameter_handles_max, &param_max);
/* channel reverse */
sprintf(nbuf, "RC%d_REV", i + 1);
_parameter_handles_rev = param_find(nbuf);
param_get(_parameter_handles_rev, &param_rev);
/* channel deadzone */
sprintf(nbuf, "RC%d_DZ", i + 1);
_parameter_handles_dz = param_find(nbuf);
param_get(_parameter_handles_dz, &param_dz);
/* assert min..center..max ordering */
if (param_min < 500) {
count++;
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MIN < 500", i+1);
/* give system time to flush error message in case there are more */
usleep(100000);
}
if (param_max > 2500) {
count++;
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAX > 2500", i+1);
/* give system time to flush error message in case there are more */
usleep(100000);
}
if (param_trim < param_min) {
count++;
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN", i+1);
/* give system time to flush error message in case there are more */
usleep(100000);
}
if (param_trim > param_max) {
count++;
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX", i+1);
/* give system time to flush error message in case there are more */
usleep(100000);
}
/* assert deadzone is sane */
if (param_dz > 500) {
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_DZ > 500", i+1);
/* give system time to flush error message in case there are more */
usleep(100000);
count++;
}
/* check which map param applies */
// if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) {
// mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1);
// /* give system time to flush error message in case there are more */
// usleep(100000);
// count++;
// }
/* sanity checks pass, enable channel */
if (count) {
mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1));
usleep(100000);
}
}
}
src/modules/systemlib/rc_check.h
+52
View File
@@ -0,0 +1,52 @@
/****************************************************************************
*
* 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 rc_check.h
*
* RC calibration check
*/
#pragma once
__BEGIN_DECLS
/**
* Check the RC calibration
*
* @return 0 / OK if RC calibration is ok, index + 1 of the first
* channel that failed else (so 1 == first channel failed)
*/
__EXPORT int rc_calibration_check(void);
__END_DECLS
src/modules/test/foo.c
-4
View File
@@ -1,4 +0,0 @@
int test_main(void)
{
return 0;
}
src/modules/test/module.mk
-4
View File
@@ -1,4 +0,0 @@
MODULE_NAME = test
SRCS = foo.c
src/modules/uORB/topics/vehicle_control_mode.h
+2
View File
@@ -82,6 +82,8 @@ struct vehicle_control_mode_s
bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */
bool failsave_highlevel; /**< Set to true if high-level failsafe mode is enabled */
uint8_t auto_state; // TEMP navigation state for AUTO modes
};
/**
src/modules/uORB/topics/vehicle_global_position.h
+9 -10
View File
@@ -62,18 +62,17 @@
struct vehicle_global_position_s
{
uint64_t timestamp; /**< time of this estimate, in microseconds since system start */
uint64_t time_gps_usec; /**< GPS timestamp in microseconds */
uint64_t time_gps_usec; /**< GPS timestamp in microseconds */
bool valid; /**< true if position satisfies validity criteria of estimator */
int32_t lat; /**< Latitude in 1E7 degrees LOGME */
int32_t lon; /**< Longitude in 1E7 degrees LOGME */
float alt; /**< Altitude in meters LOGME */
float relative_alt; /**< Altitude above home position in meters, LOGME */
float vx; /**< Ground X Speed (Latitude), m/s in ENU LOGME */
float vy; /**< Ground Y Speed (Longitude), m/s in ENU LOGME */
float vz; /**< Ground Z Speed (Altitude), m/s in ENU LOGME */
float hdg; /**< Compass heading in radians -PI..+PI. */
int32_t lat; /**< Latitude in 1E7 degrees */
int32_t lon; /**< Longitude in 1E7 degrees */
float alt; /**< Altitude in meters */
float relative_alt; /**< Altitude above home position in meters, */
float vx; /**< Ground X velocity, m/s in NED */
float vy; /**< Ground Y velocity, m/s in NED */
float vz; /**< Ground Z velocity, m/s in NED */
float yaw; /**< Compass heading in radians -PI..+PI. */
};
/**
src/modules/uORB/topics/vehicle_local_position.h
+2
View File
@@ -67,6 +67,8 @@ struct vehicle_local_position_s
float vx; /**< Ground X Speed (Latitude), m/s in NED */
float vy; /**< Ground Y Speed (Longitude), m/s in NED */
float vz; /**< Ground Z Speed (Altitude), m/s in NED */
/* Heading */
float yaw;
/* Reference position in GPS / WGS84 frame */
bool global_xy; /**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */
bool global_z; /**< true if z is valid and has valid global reference (ref_alt) */
src/modules/unit_test/module.mk
+39
View File
@@ -0,0 +1,39 @@
############################################################################
#
# 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.
#
############################################################################
#
# Makefile to build the unit test library.
#
SRCS = unit_test.cpp
src/modules/unit_test/unit_test.cpp
+65
View File
@@ -0,0 +1,65 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Simon Wilks <sjwilks@gmail.com>
*
* 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 unit_test.cpp
* A unit test library.
*
*/
#include "unit_test.h"
#include <systemlib/err.h>
UnitTest::UnitTest()
{
}
UnitTest::~UnitTest()
{
}
void
UnitTest::print_results(const char* result)
{
if (result != 0) {
warnx("Failed: %s:%d", mu_last_test(), mu_line());
warnx("%s", result);
} else {
warnx("ALL TESTS PASSED");
warnx(" Tests run : %d", mu_tests_run());
warnx(" Assertion : %d", mu_assertion());
}
}
src/modules/unit_test/unit_test.h
+93
View File
@@ -0,0 +1,93 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Simon Wilks <sjwilks@gmail.com>
*
* 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 unit_test.h
* A unit test library based on MinUnit (http://www.jera.com/techinfo/jtns/jtn002.html).
*
*/
#ifndef UNIT_TEST_H_
#define UNIT_TEST_
#include <systemlib/err.h>
class __EXPORT UnitTest
{
public:
#define xstr(s) str(s)
#define str(s) #s
#define INLINE_GLOBAL(type,func) inline type& func() { static type x; return x; }
INLINE_GLOBAL(int, mu_tests_run)
INLINE_GLOBAL(int, mu_assertion)
INLINE_GLOBAL(int, mu_line)
INLINE_GLOBAL(const char*, mu_last_test)
#define mu_assert(message, test) \
do \
{ \
if (!(test)) \
return __FILE__ ":" xstr(__LINE__) " " message " (" #test ")"; \
else \
mu_assertion()++; \
} while (0)
#define mu_run_test(test) \
do \
{ \
const char *message; \
mu_last_test() = #test; \
mu_line() = __LINE__; \
message = test(); \
mu_tests_run()++; \
if (message) \
return message; \
} while (0)
public:
UnitTest();
virtual ~UnitTest();
virtual const char* run_tests() = 0;
virtual void print_results(const char* result);
};
#endif /* UNIT_TEST_H_ */
src/systemcmds/preflight_check/preflight_check.c
+2 -95
View File
@@ -57,6 +57,7 @@
#include <drivers/drv_baro.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/rc_check.h>
__EXPORT int preflight_check_main(int argc, char *argv[]);
static int led_toggle(int leds, int led);
@@ -139,101 +140,7 @@ int preflight_check_main(int argc, char *argv[])
/* ---- RC CALIBRATION ---- */
param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max,
_parameter_handles_rev, _parameter_handles_dz;
float param_min, param_max, param_trim, param_rev, param_dz;
bool rc_ok = true;
char nbuf[20];
/* first check channel mappings */
/* check which map param applies */
// if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) {
// mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1);
// /* give system time to flush error message in case there are more */
// usleep(100000);
// count++;
// }
for (int i = 0; i < 12; i++) {
/* should the channel be enabled? */
uint8_t count = 0;
/* min values */
sprintf(nbuf, "RC%d_MIN", i + 1);
_parameter_handles_min = param_find(nbuf);
param_get(_parameter_handles_min, &param_min);
/* trim values */
sprintf(nbuf, "RC%d_TRIM", i + 1);
_parameter_handles_trim = param_find(nbuf);
param_get(_parameter_handles_trim, &param_trim);
/* max values */
sprintf(nbuf, "RC%d_MAX", i + 1);
_parameter_handles_max = param_find(nbuf);
param_get(_parameter_handles_max, &param_max);
/* channel reverse */
sprintf(nbuf, "RC%d_REV", i + 1);
_parameter_handles_rev = param_find(nbuf);
param_get(_parameter_handles_rev, &param_rev);
/* channel deadzone */
sprintf(nbuf, "RC%d_DZ", i + 1);
_parameter_handles_dz = param_find(nbuf);
param_get(_parameter_handles_dz, &param_dz);
/* assert min..center..max ordering */
if (param_min < 500) {
count++;
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MIN < 500", i+1);
/* give system time to flush error message in case there are more */
usleep(100000);
}
if (param_max > 2500) {
count++;
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAX > 2500", i+1);
/* give system time to flush error message in case there are more */
usleep(100000);
}
if (param_trim < param_min) {
count++;
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN", i+1);
/* give system time to flush error message in case there are more */
usleep(100000);
}
if (param_trim > param_max) {
count++;
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX", i+1);
/* give system time to flush error message in case there are more */
usleep(100000);
}
/* assert deadzone is sane */
if (param_dz > 500) {
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_DZ > 500", i+1);
/* give system time to flush error message in case there are more */
usleep(100000);
count++;
}
/* check which map param applies */
// if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) {
// mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1);
// /* give system time to flush error message in case there are more */
// usleep(100000);
// count++;
// }
/* sanity checks pass, enable channel */
if (count) {
mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1));
usleep(100000);
rc_ok = false;
}
}
bool rc_ok = (OK == rc_calibration_check());
/* require RC ok to keep system_ok */
system_ok &= rc_ok;