diff --git a/.gitignore b/.gitignore
index 76f45281ce767801c90bee6b070ad3e10962ead1..3e94cf62054ba6d1243e154385fdac3d4c0f6845 100644
--- a/.gitignore
+++ b/.gitignore
@@ -19,14 +19,19 @@ Archives/*
Build/*
core
cscope.out
-dot.gdbinit
Firmware.sublime-workspace
Images/*.bin
Images/*.px4
mavlink/include/mavlink/v0.9/
+/nuttx-configs/px4io-v2/src/.depend
+/nuttx-configs/px4io-v2/src/Make.dep
+/nuttx-configs/px4io-v2/src/libboard.a
+/nuttx-configs/px4io-v1/src/.depend
+/nuttx-configs/px4io-v1/src/Make.dep
+/nuttx-configs/px4io-v1/src/libboard.a
/NuttX
/Documentation/doxy.log
/Documentation/html/
/Documentation/doxygen*objdb*tmp
.tags
-.tags_sorted_by_file
\ No newline at end of file
+.tags_sorted_by_file
diff --git a/Debug/PX4 b/Debug/PX4
index 085cffe434c0e773231f0d8a8b368b190fbee37c..e99228ee0c88986bd471263730aa0b0123cd7c6e 100644
--- a/Debug/PX4
+++ b/Debug/PX4
@@ -27,7 +27,7 @@ define _perf_print
# PC_COUNT
if $hdr->type == 0
set $count = (struct perf_ctr_count *)$hdr
- printf "%llu events,\n", $count->event_count;
+ printf "%llu events\n", $count->event_count
end
# PC_ELPASED
if $hdr->type == 1
diff --git a/Debug/dot.gdbinit b/Debug/dot.gdbinit
new file mode 100644
index 0000000000000000000000000000000000000000..d70410bc78b36a294aaf2179e607d080d3c373af
--- /dev/null
+++ b/Debug/dot.gdbinit
@@ -0,0 +1,13 @@
+# copy the file to .gdbinit in your Firmware tree, and adjust the path
+# below to match your system
+# For example:
+# target extended /dev/serial/by-id/usb-Black_Sphere_Technologies_Black_Magic_Probe_DDE5A1C4-if00
+# target extended /dev/ttyACM4
+
+
+monitor swdp_scan
+attach 1
+monitor vector_catch disable hard
+set mem inaccessible-by-default off
+set print pretty
+source Debug/PX4
diff --git a/Debug/olimex-px4fmu-debug.cfg b/Debug/olimex-px4fmu-debug.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..61d70070dbb041d749f65c4476eb8812fe9b43c5
--- /dev/null
+++ b/Debug/olimex-px4fmu-debug.cfg
@@ -0,0 +1,22 @@
+# program a bootable device load on a mavstation
+# To run type openocd -f mavprogram.cfg
+
+source [find interface/olimex-arm-usb-ocd-h.cfg]
+source [find px4fmu-v1-board.cfg]
+
+init
+halt
+
+# Find the flash inside this CPU
+flash probe 0
+
+# erase it (128 pages) then program and exit
+
+#flash erase_sector 0 0 127
+# stm32f1x mass_erase 0
+
+# It seems that Pat's image has a start address offset of 0x1000 but the vectors need to be at zero, so fixbin.sh moves things around
+#flash write_bank 0 fixed.bin 0
+#flash write_image firmware.elf
+#shutdown
+
diff --git a/Debug/openocd.gdbinit b/Debug/openocd.gdbinit
new file mode 100644
index 0000000000000000000000000000000000000000..92d78b58dbb1fd7c9283f2259c18e0eeb8a01c1d
--- /dev/null
+++ b/Debug/openocd.gdbinit
@@ -0,0 +1,21 @@
+target remote :3333
+
+# Don't let GDB get confused while stepping
+define hook-step
+ mon cortex_m maskisr on
+end
+define hookpost-step
+ mon cortex_m maskisr off
+end
+
+mon init
+mon stm32_init
+# mon reset halt
+mon poll
+mon cortex_m maskisr auto
+set mem inaccessible-by-default off
+set print pretty
+source Debug/PX4
+
+echo PX4 resumed, press ctrl-c to interrupt\n
+continue
diff --git a/Debug/px4fmu-v1-board.cfg b/Debug/px4fmu-v1-board.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..19b862a2d17669575828f07ae8e2615274abc6fb
--- /dev/null
+++ b/Debug/px4fmu-v1-board.cfg
@@ -0,0 +1,38 @@
+# The latest defaults in OpenOCD 0.7.0 are actually prettymuch correct for the px4fmu
+
+# increase working area to 32KB for faster flash programming
+set WORKAREASIZE 0x8000
+
+source [find target/stm32f4x.cfg]
+
+# needed for px4
+reset_config trst_only
+
+proc stm32_reset {} {
+ reset halt
+# FIXME - needed to init periphs on reset
+# 0x40023800 RCC base
+# 0x24 RCC_APB2 0x75933
+# RCC_APB2 0
+}
+
+# perform init that is required on each connection to the target
+proc stm32_init {} {
+
+ # force jtag to not shutdown during sleep
+ #uint32_t cr = getreg32(STM32_DBGMCU_CR);
+ #cr |= DBGMCU_CR_STANDBY | DBGMCU_CR_STOP | DBGMCU_CR_SLEEP;
+ #putreg32(cr, STM32_DBGMCU_CR);
+ mww 0xe0042004 00000007
+}
+
+# if srst is not fitted use SYSRESETREQ to
+# perform a soft reset
+cortex_m reset_config sysresetreq
+
+# Let GDB directly program elf binaries
+gdb_memory_map enable
+
+# doesn't work yet
+gdb_flash_program disable
+
diff --git a/Debug/runopenocd.sh b/Debug/runopenocd.sh
new file mode 100755
index 0000000000000000000000000000000000000000..6258fccfb6c5f9f6080854f2c04037b3ce7abf75
--- /dev/null
+++ b/Debug/runopenocd.sh
@@ -0,0 +1,5 @@
+#!/bin/bash
+
+DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
+
+openocd -f interface/olimex-arm-usb-ocd-h.cfg -f $DIR/px4fmu-v1-board.cfg
diff --git a/Debug/stm32f4x.cfg b/Debug/stm32f4x.cfg
deleted file mode 100644
index 28bfcfbbbc6c8ef7db2760595b42b6c64aa4b2fc..0000000000000000000000000000000000000000
--- a/Debug/stm32f4x.cfg
+++ /dev/null
@@ -1,64 +0,0 @@
-# script for stm32f2xxx
-
-if { [info exists CHIPNAME] } {
- set _CHIPNAME $CHIPNAME
-} else {
- set _CHIPNAME stm32f4xxx
-}
-
-if { [info exists ENDIAN] } {
- set _ENDIAN $ENDIAN
-} else {
- set _ENDIAN little
-}
-
-# Work-area is a space in RAM used for flash programming
-# By default use 64kB
-if { [info exists WORKAREASIZE] } {
- set _WORKAREASIZE $WORKAREASIZE
-} else {
- set _WORKAREASIZE 0x10000
-}
-
-# JTAG speed should be <= F_CPU/6. F_CPU after reset is 8MHz, so use F_JTAG = 1MHz
-#
-# Since we may be running of an RC oscilator, we crank down the speed a
-# bit more to be on the safe side. Perhaps superstition, but if are
-# running off a crystal, we can run closer to the limit. Note
-# that there can be a pretty wide band where things are more or less stable.
-jtag_khz 1000
-
-jtag_nsrst_delay 100
-jtag_ntrst_delay 100
-
-#jtag scan chain
-if { [info exists CPUTAPID ] } {
- set _CPUTAPID $CPUTAPID
-} else {
- # See STM Document RM0033
- # Section 32.6.3 - corresponds to Cortex-M3 r2p0
- set _CPUTAPID 0x4ba00477
-}
-jtag newtap $_CHIPNAME cpu -irlen 4 -ircapture 0x1 -irmask 0xf -expected-id $_CPUTAPID
-
-if { [info exists BSTAPID ] } {
- set _BSTAPID $BSTAPID
-} else {
- # See STM Document RM0033
- # Section 32.6.2
- #
- set _BSTAPID 0x06413041
-}
-jtag newtap $_CHIPNAME bs -irlen 5 -expected-id $_BSTAPID
-
-set _TARGETNAME $_CHIPNAME.cpu
-target create $_TARGETNAME cortex_m3 -endian $_ENDIAN -chain-position $_TARGETNAME -rtos auto
-
-$_TARGETNAME configure -work-area-phys 0x20000000 -work-area-size $_WORKAREASIZE -work-area-backup 0
-
-set _FLASHNAME $_CHIPNAME.flash
-flash bank $_FLASHNAME stm32f2x 0 0 0 0 $_TARGETNAME
-
-# if srst is not fitted use SYSRESETREQ to
-# perform a soft reset
-cortex_m3 reset_config sysresetreq
diff --git a/Documentation/flight_mode_state_machine.pdf b/Documentation/flight_mode_state_machine.pdf
index cd234ada73ce021351db8fdc8952dc20aad29ca8..af41953eec12e70623e824f8dfeb882b60d46882 100644
Binary files a/Documentation/flight_mode_state_machine.pdf and b/Documentation/flight_mode_state_machine.pdf differ
diff --git a/Documentation/rc_mode_switch.odg b/Documentation/rc_mode_switch.odg
index a8a6f93f31f90c26e4e6d2ae4914fcefd53301aa..29d738c39c9598003b84fe5bd1565cfdff644ee0 100644
Binary files a/Documentation/rc_mode_switch.odg and b/Documentation/rc_mode_switch.odg differ
diff --git a/Documentation/rc_mode_switch.pdf b/Documentation/rc_mode_switch.pdf
index 3141eed7fff618b07a1b84db8da60bdc2cffc9bb..856dd55c59af22e3595048dcfb5dd322c8cb2326 100644
Binary files a/Documentation/rc_mode_switch.pdf and b/Documentation/rc_mode_switch.pdf differ
diff --git a/Images/px4fmu-v2.prototype b/Images/px4fmu-v2.prototype
new file mode 100644
index 0000000000000000000000000000000000000000..5109b77d1e3b92477228933b54686b6ffb58319c
--- /dev/null
+++ b/Images/px4fmu-v2.prototype
@@ -0,0 +1,12 @@
+{
+ "board_id": 9,
+ "magic": "PX4FWv1",
+ "description": "Firmware for the PX4FMUv2 board",
+ "image": "",
+ "build_time": 0,
+ "summary": "PX4FMUv2",
+ "version": "0.1",
+ "image_size": 0,
+ "git_identity": "",
+ "board_revision": 0
+}
diff --git a/Images/px4io-v2.prototype b/Images/px4io-v2.prototype
new file mode 100644
index 0000000000000000000000000000000000000000..af87924e90dda7fe0b9ba1ba7a0ed018c1e405d3
--- /dev/null
+++ b/Images/px4io-v2.prototype
@@ -0,0 +1,12 @@
+{
+ "board_id": 10,
+ "magic": "PX4FWv2",
+ "description": "Firmware for the PX4IOv2 board",
+ "image": "",
+ "build_time": 0,
+ "summary": "PX4IOv2",
+ "version": "2.0",
+ "image_size": 0,
+ "git_identity": "",
+ "board_revision": 0
+}
diff --git a/Images/px4iov2.prototype b/Images/px4iov2.prototype
new file mode 100644
index 0000000000000000000000000000000000000000..af87924e90dda7fe0b9ba1ba7a0ed018c1e405d3
--- /dev/null
+++ b/Images/px4iov2.prototype
@@ -0,0 +1,12 @@
+{
+ "board_id": 10,
+ "magic": "PX4FWv2",
+ "description": "Firmware for the PX4IOv2 board",
+ "image": "",
+ "build_time": 0,
+ "summary": "PX4IOv2",
+ "version": "2.0",
+ "image_size": 0,
+ "git_identity": "",
+ "board_revision": 0
+}
diff --git a/Makefile b/Makefile
index 778395cee99150c1115f15910c5089c1fec02011..83a6f3ce9f0eaaa601499aec36f0d40d2eb8e022 100644
--- a/Makefile
+++ b/Makefile
@@ -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,7 +222,8 @@ clean:
.PHONY: distclean
distclean: clean
$(Q) $(REMOVE) $(ARCHIVE_DIR)*.export
- $(Q) make -C $(NUTTX_SRC) -r $(MQUIET) distclean
+ $(Q) $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) distclean
+ $(Q) (cd $(NUTTX_SRC)/configs && $(FIND) . -maxdepth 1 -type l -delete)
#
# Print some help text
@@ -228,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."
diff --git a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x
index 58a970eba5ac10c35128111398fd926aacb43e9e..f57e4bd68da3ce2e4a7254c285559e6aa3e3bd4f 100644
--- a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x
+++ b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x
@@ -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,56 +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
-
+fmu mode_pwm
+
#
-# Start the attitude estimator
+# Load mixer
#
-attitude_estimator_ekf start
-
-echo "[init] starting PWM output"
-fmu mode_pwm
mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
-pwm -u 400 -m 0xff
-
+
#
-# Start attitude control
+# Set PWM output frequency
#
-multirotor_att_control start
+pwm -u 400 -m 0xff
#
-# Start logging
+# Start common for all multirotors apps
#
-sdlog2 start -r 50 -a -b 14
+sh /etc/init.d/rc.multirotor
-#
-# Start system state
-#
-if blinkm start
-then
- blinkm systemstate
-fi
+# Exit, because /dev/ttyS0 is needed for MAVLink
+exit
diff --git a/ROMFS/px4fmu_common/init.d/02_io_quad_x b/ROMFS/px4fmu_common/init.d/02_io_quad_x
index c63e92f6d32e6d9b7bef7a3a9d7e02676ad6fd33..a37c26ad1d3e7217fdef9f7fb018dd8edd37f391 100644
--- a/ROMFS/px4fmu_common/init.d/02_io_quad_x
+++ b/ROMFS/px4fmu_common/init.d/02_io_quad_x
@@ -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
-#
-px4io limit 200
-
-#
-# 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)
+# Start and configure PX4IO interface
#
-attitude_estimator_ekf start
-
+sh /etc/init.d/rc.io
+
#
-# Load mixer and start controllers (depends on px4io)
+# Load mixer
#
mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
-pwm -u 400 -m 0xff
-multirotor_att_control start
-
+
#
-# Start logging
+# Set PWM output frequency
#
-sdlog2 start -r 50 -a -b 14
-
+pwm -u 400 -m 0xff
+
#
-# Start system state
+# Start common for all multirotors apps
#
-if blinkm start
-then
- blinkm systemstate
-fi
+sh /etc/init.d/rc.multirotor
diff --git a/ROMFS/px4fmu_common/init.d/08_ardrone b/ROMFS/px4fmu_common/init.d/08_ardrone
index f55ac2ae3449c8a603ef7fed1442d5ae6483ac70..eb9f82f771045dcecb7b2c00853782ceed6e06d9 100644
--- a/ROMFS/px4fmu_common/init.d/08_ardrone
+++ b/ROMFS/px4fmu_common/init.d/08_ardrone
@@ -1,99 +1,45 @@
#!nsh
-#
-# Flight startup script for PX4FMU on PX4IOAR carrier board.
-#
-
-# Disable the USB interface
-set USB no
-# Disable autostarting other apps
-set MODE ardrone
-
-echo "[init] doing PX4IOAR startup..."
-
-#
-# Start the ORB
-#
-uorb start
+echo "[init] 08_ardrone: PX4FMU on PX4IOAR carrier board"
#
-# Load microSD params
+# Load default params for this platform
#
-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
-#
-# 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
+# Configure PX4FMU for operation with PX4IOAR
#
-multirotor_att_control start
+fmu mode_gpio_serial
#
# Fire up the AR.Drone interface.
#
ardrone_interface start -d /dev/ttyS1
-
-#
-# Start logging
-#
-sdlog start -s 10
-
-#
-# Start GPS capture
-#
-gps start
-
-#
-# Start system state
-#
-if blinkm start
-then
- echo "using BlinkM for state indication"
- blinkm systemstate
-else
- echo "no BlinkM found, OK."
-fi
#
-# 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"
-
+sh /etc/init.d/rc.multirotor
+
+# Exit, because /dev/ttyS0 is needed for MAVLink
exit
diff --git a/ROMFS/px4fmu_common/init.d/09_ardrone_flow b/ROMFS/px4fmu_common/init.d/09_ardrone_flow
index e7173f6e6efb8fd0deb0a6c7b2f191c8ff297589..44fbb79b7b806322031c7d4d80754e2d962509ad 100644
--- a/ROMFS/px4fmu_common/init.d/09_ardrone_flow
+++ b/ROMFS/px4fmu_common/init.d/09_ardrone_flow
@@ -1,55 +1,47 @@
#!nsh
-#
-# Flight startup script for PX4FMU on PX4IOAR carrier board.
-#
-# Disable the USB interface
-set USB no
+echo "[init] 09_ardrone_flow: PX4FMU on PX4IOAR carrier board with PX4FLOW"
-# Disable autostarting other apps
-set MODE ardrone
-
-echo "[init] doing PX4IOAR startup..."
-
#
-# 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 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.
#
@@ -79,16 +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
-#
-# startup is done; we don't want the shell because we
-# use the same UART for telemetry
-#
-echo "[init] startup done"
-
+# Exit, because /dev/ttyS0 is needed for MAVLink
exit
diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330
index 4450eb50db7d4bdfc1c73aadd9a1c1942a3e0756..7b6509bf882c44c991883ebe0664bcd8603f94e7 100644
--- a/ROMFS/px4fmu_common/init.d/10_io_f330
+++ b/ROMFS/px4fmu_common/init.d/10_io_f330
@@ -1,11 +1,6 @@
#!nsh
-#
-# Flight startup script for PX4FMU+PX4IO
-#
-
-# disable USB and autostart
-set USB no
-set MODE custom
+
+echo "[init] 10_io_f330: PX4FMU+PX4IO on a DJI F330 quad frame"
#
# Load default params for this platform
@@ -15,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
@@ -44,73 +38,30 @@ param set MAV_TYPE 2
#
mavlink start
usleep 5000
-
-#
-# 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
#
-# Disable px4io topic limiting
+# Start and configure PX4IO interface
#
-px4io limit 200
+sh /etc/init.d/rc.io
#
-# 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 1200 1200 1200 1200
-
-#
-# Upper limits could be higher, this is on the safe side
-#
px4io max 1800 1800 1800 1800
-
-#
-# Start the sensors (depends on orb, px4io)
-#
-sh /etc/init.d/rc.sensors
#
-# Start the commander (depends on orb, mavlink)
-#
-commander start
-
-#
-# 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
-multirotor_att_control start
-
+
#
-# Start logging
+# Set PWM output frequency
#
-sdlog2 start -r 20 -a -b 16
-
+pwm -u 400 -m 0xff
+
#
-# Start system state
+# Start common for all multirotors apps
#
-if blinkm start
-then
- blinkm systemstate
-fi
+sh /etc/init.d/rc.multirotor
diff --git a/ROMFS/px4fmu_common/init.d/15_io_tbs b/ROMFS/px4fmu_common/init.d/15_io_tbs
new file mode 100644
index 0000000000000000000000000000000000000000..b4f063e5241125a41875bdcf3cf8b93871868fb8
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/15_io_tbs
@@ -0,0 +1,67 @@
+#!nsh
+
+echo "[init] 15_io_tbs: PX4FMU+PX4IO on a Team Blacksheep Discovery quad"
+
+#
+# Load default params for this platform
+#
+if param compare SYS_AUTOCONFIG 1
+then
+ # Set all params here, then disable autoconfig
+ param set SYS_AUTOCONFIG 0
+
+ param set MC_ATTRATE_D 0.006
+ param set MC_ATTRATE_I 0.0
+ param set MC_ATTRATE_P 0.17
+ param set MC_ATT_D 0.0
+ param set MC_ATT_I 0.0
+ param set MC_ATT_P 5.0
+ param set MC_RCLOSS_THR 0.0
+ param set MC_YAWPOS_D 0.0
+ param set MC_YAWPOS_I 0.15
+ param set MC_YAWPOS_P 0.5
+ param set MC_YAWRATE_D 0.0
+ param set MC_YAWRATE_I 0.0
+ param set MC_YAWRATE_P 0.2
+
+ param save /fs/microsd/params
+fi
+
+#
+# Force some key parameters to sane values
+# MAV_TYPE 2 = quadrotor
+#
+param set MAV_TYPE 2
+
+#
+# Start MAVLink (depends on orb)
+#
+mavlink start
+usleep 5000
+
+#
+# Start and configure PX4IO interface
+#
+sh /etc/init.d/rc.io
+
+#
+# Set PWM values for DJI ESCs
+#
+px4io idle 900 900 900 900
+px4io min 1180 1180 1180 1180
+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
+
+#
+# Set PWM output frequency
+#
+pwm -u 400 -m 0xff
+
+#
+# Start common for all multirotors apps
+#
+sh /etc/init.d/rc.multirotor
diff --git a/ROMFS/px4fmu_common/init.d/30_io_camflyer b/ROMFS/px4fmu_common/init.d/30_io_camflyer
index 5090b98a426224045535851a2a27853d512bd083..6a0bd4da8251d751c685c153cb3121eebf3ead80 100644
--- a/ROMFS/px4fmu_common/init.d/30_io_camflyer
+++ b/ROMFS/px4fmu_common/init.d/30_io_camflyer
@@ -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
diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom
index 5090b98a426224045535851a2a27853d512bd083..71831386227beb31fe80d57b92ee7091029631a0 100644
--- a/ROMFS/px4fmu_common/init.d/31_io_phantom
+++ b/ROMFS/px4fmu_common/init.d/31_io_phantom
@@ -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,94 +8,60 @@ 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)
-#
-commander start
-
-#
-# Start PX4IO interface (depends on orb, commander)
+
#
-px4io start
-
+# Start and configure PX4IO interface
#
-# 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
#
-gps start
-
+sh /etc/init.d/rc.sensors
+
#
-# Start the attitude estimator (depends on orb)
+# Start logging (depends on sensors)
#
-kalman_demo start
+sh /etc/init.d/rc.logging
#
-# Load mixer and start controllers (depends on px4io)
+# Start GPS interface
#
-mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
-control_demo start
+gps start
#
-# Start logging
+# Start the attitude estimator
#
-sdlog2 start -r 50 -a -b 14
+kalman_demo start
#
-# Start system state
+# Load mixer and start controllers (depends on px4io)
#
-if blinkm start
-then
- blinkm systemstate
-fi
+mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
+fw_att_control start
diff --git a/ROMFS/px4fmu_common/init.d/40_io_segway b/ROMFS/px4fmu_common/init.d/40_io_segway
index 5742d685aa43175878aa7a6b301a060c5e5c95b0..2890f43bece0eaff4e7ba9f6cbda595485bb0d25 100644
--- a/ROMFS/px4fmu_common/init.d/40_io_segway
+++ b/ROMFS/px4fmu_common/init.d/40_io_segway
@@ -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,39 +6,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 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)
#
@@ -68,25 +25,15 @@ mavlink start -d /dev/ttyS1 -b 57600
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
-
+# Start the commander (depends on orb, mavlink)
#
-# Allow PX4IO to recover from midair restarts.
-# this is very unlikely, but quite safe and robust.
-px4io recovery
+commander start
-#
-# 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
diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550
new file mode 100644
index 0000000000000000000000000000000000000000..2c821801317ec72929d12552f0065c8cf0ecc654
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550
@@ -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
diff --git a/ROMFS/px4fmu_common/init.d/rc.hil b/ROMFS/px4fmu_common/init.d/rc.hil
index 3517a5bd82177571ad07a42f500df3b1ac21ff4f..a843b7ffbca962841a8f97383a1411d4ddf911d6 100644
--- a/ROMFS/px4fmu_common/init.d/rc.hil
+++ b/ROMFS/px4fmu_common/init.d/rc.hil
@@ -8,7 +8,7 @@ echo "[HIL] starting.."
uorb start
# Tell MAVLink that this link is "fast"
-mavlink start -b 230400 -d /dev/console
+mavlink start -b 230400 -d /dev/ttyS1
# Create a fake HIL /dev/pwm_output interface
hil mode_pwm
@@ -38,13 +38,13 @@ commander start
#
# Check if we got an IO
#
-if [ px4io start ]
+if px4io start
then
echo "IO started"
else
fmu mode_serial
echo "FMU started"
-end
+fi
#
# Start the sensors (depends on orb, px4io)
@@ -60,9 +60,7 @@ att_pos_estimator_ekf start
# Load mixer and start controllers (depends on px4io)
#
mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix
-fixedwing_backside start
+fw_pos_control_l1 start
+fw_att_control start
echo "[HIL] setup done, running"
-
-# Exit shell to make it available to MAVLink
-exit
diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io
new file mode 100644
index 0000000000000000000000000000000000000000..85f00e582e78406a15953810aaf9699be81f5978
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/rc.io
@@ -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
diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging
index 09c2d00d195375f2b31c815364bdcacd0c9451a5..dc4be8055a84a5ee215caeb7c3d7fbd8bd8fdb60 100644
--- a/ROMFS/px4fmu_common/init.d/rc.logging
+++ b/ROMFS/px4fmu_common/init.d/rc.logging
@@ -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
diff --git a/ROMFS/px4fmu_common/init.d/rc.multirotor b/ROMFS/px4fmu_common/init.d/rc.multirotor
new file mode 100644
index 0000000000000000000000000000000000000000..e3638e3d1fc7f72581cb4db8d4d8cdb439d3eaad
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/rc.multirotor
@@ -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
diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors
index 5e80ddc2f412077028c8a4e7b6cf3aecc0894b43..61bb0972811ab229267011d7726eb71f9ccf12b9 100644
--- a/ROMFS/px4fmu_common/init.d/rc.sensors
+++ b/ROMFS/px4fmu_common/init.d/rc.sensors
@@ -18,14 +18,21 @@ fi
ms5611 start
adc start
+# mag might be external
+if hmc5883 start
+then
+ echo "using HMC5883"
+fi
+
if mpu6000 start
then
- echo "using MPU6000 and HMC5883L"
- hmc5883 start
+ echo "using MPU6000"
+ set BOARD fmuv1
else
echo "using L3GD20 and LSM303D"
l3gd20 start
lsm303d start
+ set BOARD fmuv2
fi
#
@@ -36,8 +43,5 @@ fi
#
if sensors start
then
- #
- # Check sensors - run AFTER 'sensors start'
- #
preflight_check &
fi
diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb
index 5b1bd272e9d819448a27fb2c85710f18a7d29250..abeed0ca3cd43f3e6a6fe6778748a3a7263d69b0 100644
--- a/ROMFS/px4fmu_common/init.d/rc.usb
+++ b/ROMFS/px4fmu_common/init.d/rc.usb
@@ -16,12 +16,26 @@ fi
sleep 2
mavlink start -b 230400 -d /dev/ttyACM0
+# Stop commander
+if commander stop
+then
+ echo "Commander stopped"
+fi
+sleep 1
+
# Start the commander
if commander start
then
echo "Commander started"
fi
+# Stop px4io
+if px4io stop
+then
+ echo "PX4IO stopped"
+fi
+sleep 1
+
# Start px4io if present
if px4io start
then
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index f0ee1a0c6e726071e52b2b408b1b194d2ccd1e00..c4abd07d2ecd7c8ff87be41d8f9348e6f02a92fe 100755
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -57,90 +57,166 @@ fi
if [ $MODE == autostart ]
then
-
-#
-# 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
+ echo "USB connected"
+ else
+ # second attempt
+ sercon &
fi
-else
- param select /fs/microsd/params
- if [ -f /fs/microsd/params ]
+
+ #
+ # Start the ORB (first app to start)
+ #
+ uorb start
+
+ #
+ # Load microSD params
+ #
+ if ramtron start
then
- param load /fs/microsd/params
+ param select /ramtron/params
+ if [ -f /ramtron/params ]
+ 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
-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
+
+ #
+ # Start system state indicator
+ #
+ if rgbled start
then
- echo "No newer version, skipping upgrade."
+ echo "Using external RGB Led"
else
- echo "Loading /fs/microsd/px4io.bin"
- if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io.log
+ if blinkm start
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"
+ blinkm systemstate
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
-fi
-
-if param compare SYS_AUTOSTART 2
-then
- sh /etc/init.d/02_io_quad_x
-fi
-
-if param compare SYS_AUTOSTART 8
-then
- sh /etc/init.d/08_ardrone
-fi
-
-if param compare SYS_AUTOSTART 9
-then
- sh /etc/init.d/09_ardrone_flow
-fi
-
-if param compare SYS_AUTOSTART 10
-then
- sh /etc/init.d/10_io_f330
-fi
-
-if param compare SYS_AUTOSTART 30
-then
- sh /etc/init.d/30_io_camflyer
-fi
-
-if param compare SYS_AUTOSTART 31
-then
- sh /etc/init.d/31_io_phantom
-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 "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
+
+ #
+ # 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
+
+ # 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
# End of autostart
fi
diff --git a/ROMFS/px4fmu_test/init.d/rc.standalone b/ROMFS/px4fmu_test/init.d/rc.standalone
new file mode 100644
index 0000000000000000000000000000000000000000..67e95215b9d4a2782df393f531eab133afee53cf
--- /dev/null
+++ b/ROMFS/px4fmu_test/init.d/rc.standalone
@@ -0,0 +1,13 @@
+#!nsh
+#
+# Flight startup script for PX4FMU standalone configuration.
+#
+
+echo "[init] doing standalone PX4FMU startup..."
+
+#
+# Start the ORB
+#
+uorb start
+
+echo "[init] startup done"
diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS
new file mode 100755
index 0000000000000000000000000000000000000000..7f161b053aef457a2fc3694cb4e728188a599b96
--- /dev/null
+++ b/ROMFS/px4fmu_test/init.d/rcS
@@ -0,0 +1,4 @@
+#!nsh
+#
+# PX4FMU startup script for test hackery.
+#
diff --git a/makefiles/board_px4fmu-v2.mk b/makefiles/board_px4fmu-v2.mk
new file mode 100644
index 0000000000000000000000000000000000000000..e9a2985b7fc2be6bd9c44f981905f6a8815a0dba
--- /dev/null
+++ b/makefiles/board_px4fmu-v2.mk
@@ -0,0 +1,11 @@
+#
+# Board-specific definitions for the PX4FMUv2
+#
+
+#
+# Configure the toolchain
+#
+CONFIG_ARCH = CORTEXM4F
+CONFIG_BOARD = PX4FMU_V2
+
+include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk
diff --git a/makefiles/board_px4io-v2.mk b/makefiles/board_px4io-v2.mk
new file mode 100644
index 0000000000000000000000000000000000000000..50a4068fb2b8a6b511ee7df7a64ec9144f97d0c4
--- /dev/null
+++ b/makefiles/board_px4io-v2.mk
@@ -0,0 +1,11 @@
+#
+# Board-specific definitions for the PX4IOv2
+#
+
+#
+# Configure the toolchain
+#
+CONFIG_ARCH = CORTEXM3
+CONFIG_BOARD = PX4IO_V2
+
+include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk
diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk
index 40eebcb52e3049d0b49a0a3fa9adff3257d3168d..fff43c6b475b8607dde6d50ed60eb6492fa59c8c 100644
--- a/makefiles/config_px4fmu-v1_default.mk
+++ b/makefiles/config_px4fmu-v1_default.mk
@@ -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
@@ -17,7 +18,7 @@ MODULES += drivers/stm32/tone_alarm
MODULES += drivers/led
MODULES += drivers/px4io
MODULES += drivers/px4fmu
-MODULES += drivers/boards/px4fmu
+MODULES += drivers/boards/px4fmu-v1
MODULES += drivers/ardrone_interface
MODULES += drivers/l3gd20
MODULES += drivers/bma180
@@ -30,6 +31,7 @@ MODULES += drivers/hil
MODULES += drivers/hott/hott_telemetry
MODULES += drivers/hott/hott_sensors
MODULES += drivers/blinkm
+MODULES += drivers/rgbled
MODULES += drivers/mkblctrl
MODULES += drivers/md25
MODULES += drivers/airspeed
@@ -50,10 +52,12 @@ MODULES += systemcmds/param
MODULES += systemcmds/perf
MODULES += systemcmds/preflight_check
MODULES += systemcmds/pwm
+MODULES += systemcmds/esc_calib
MODULES += systemcmds/reboot
MODULES += systemcmds/top
MODULES += systemcmds/tests
MODULES += systemcmds/config
+MODULES += systemcmds/nshterm
#
# General system control
@@ -67,18 +71,16 @@ MODULES += modules/gpio_led
# Estimation modules (EKF / other filters)
#
MODULES += modules/attitude_estimator_ekf
-MODULES += modules/attitude_estimator_so3_comp
-MODULES += modules/position_estimator
MODULES += modules/att_pos_estimator_ekf
+MODULES += modules/position_estimator_inav
MODULES += examples/flow_position_estimator
#
# Vehicle Control
#
-MODULES += modules/segway
-MODULES += modules/fixedwing_backside
-MODULES += modules/fixedwing_att_control
-MODULES += modules/fixedwing_pos_control
+#MODULES += modules/segway # XXX needs state machine update
+#MODULES += modules/fw_pos_control_l1
+#MODULES += modules/fw_att_control
MODULES += modules/multirotor_att_control
MODULES += modules/multirotor_pos_control
MODULES += examples/flow_position_control
@@ -89,20 +91,28 @@ MODULES += examples/flow_speed_control
#
MODULES += modules/sdlog2
+#
+# Unit tests
+#
+MODULES += modules/unit_test
+MODULES += modules/commander/commander_tests
+
#
# Library modules
#
MODULES += modules/systemlib
MODULES += modules/systemlib/mixer
-MODULES += modules/mathlib
-MODULES += modules/mathlib/math/filter
MODULES += modules/controllib
MODULES += modules/uORB
#
# Libraries
#
-LIBRARIES += modules/mathlib/CMSIS
+LIBRARIES += lib/mathlib/CMSIS
+MODULES += lib/mathlib
+MODULES += lib/mathlib/math/filter
+#MODULES += lib/ecl
+MODULES += lib/geo
#
# Demo apps
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
new file mode 100644
index 0000000000000000000000000000000000000000..ab0da355962d6edd0f0a5b9a6285e006345dfd8c
--- /dev/null
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -0,0 +1,142 @@
+#
+# Makefile for the px4fmu_default configuration
+#
+
+#
+# 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
+#
+MODULES += drivers/device
+MODULES += drivers/stm32
+MODULES += drivers/stm32/adc
+MODULES += drivers/stm32/tone_alarm
+MODULES += drivers/led
+MODULES += drivers/px4fmu
+MODULES += drivers/px4io
+MODULES += drivers/boards/px4fmu-v2
+MODULES += drivers/rgbled
+MODULES += drivers/lsm303d
+MODULES += drivers/l3gd20
+MODULES += drivers/hmc5883
+MODULES += drivers/ms5611
+MODULES += drivers/mb12xx
+MODULES += drivers/gps
+MODULES += drivers/hil
+MODULES += drivers/hott/hott_telemetry
+MODULES += drivers/hott/hott_sensors
+MODULES += drivers/blinkm
+MODULES += drivers/airspeed
+MODULES += drivers/ets_airspeed
+MODULES += drivers/meas_airspeed
+MODULES += modules/sensors
+
+# Needs to be burned to the ground and re-written; for now,
+# just don't build it.
+#MODULES += drivers/mkblctrl
+
+#
+# System commands
+#
+MODULES += systemcmds/ramtron
+MODULES += systemcmds/bl_update
+MODULES += systemcmds/boardinfo
+MODULES += systemcmds/mixer
+MODULES += systemcmds/param
+MODULES += systemcmds/perf
+MODULES += systemcmds/preflight_check
+MODULES += systemcmds/pwm
+MODULES += systemcmds/reboot
+MODULES += systemcmds/top
+MODULES += systemcmds/tests
+MODULES += systemcmds/config
+MODULES += systemcmds/nshterm
+
+#
+# General system control
+#
+MODULES += modules/commander
+MODULES += modules/mavlink
+MODULES += modules/mavlink_onboard
+
+#
+# Estimation modules (EKF / other filters)
+#
+MODULES += modules/attitude_estimator_ekf
+MODULES += modules/att_pos_estimator_ekf
+MODULES += modules/position_estimator_inav
+MODULES += examples/flow_position_estimator
+
+#
+# Vehicle Control
+#
+#MODULES += modules/fw_pos_control_l1
+#MODULES += modules/fw_att_control
+MODULES += modules/multirotor_att_control
+MODULES += modules/multirotor_pos_control
+
+#
+# Logging
+#
+MODULES += modules/sdlog2
+
+#
+# Unit tests
+#
+MODULES += modules/unit_test
+MODULES += modules/commander/commander_tests
+
+#
+# Library modules
+#
+MODULES += modules/systemlib
+MODULES += modules/systemlib/mixer
+MODULES += modules/controllib
+MODULES += modules/uORB
+
+#
+# Libraries
+#
+LIBRARIES += lib/mathlib/CMSIS
+MODULES += lib/mathlib
+MODULES += lib/mathlib/math/filter
+#MODULES += lib/ecl
+MODULES += lib/geo
+
+#
+# Demo apps
+#
+#MODULES += examples/math_demo
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/hello_sky
+#MODULES += examples/px4_simple_app
+
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/daemon
+#MODULES += examples/px4_daemon_app
+
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/debug_values
+#MODULES += examples/px4_mavlink_debug
+
+#
+# Transitional support - add commands from the NuttX export archive.
+#
+# In general, these should move to modules over time.
+#
+# Each entry here is <command>.<priority>.<stacksize>.<entrypoint> but we use a helper macro
+# to make the table a bit more readable.
+#
+define _B
+ $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4)
+endef
+
+# command priority stack entrypoint
+BUILTIN_COMMANDS := \
+ $(call _B, sercon, , 2048, sercon_main ) \
+ $(call _B, serdis, , 2048, serdis_main )
diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk
new file mode 100644
index 0000000000000000000000000000000000000000..0f60e88b5ec15c268918a6e4a2e20f1e6d0cfe6e
--- /dev/null
+++ b/makefiles/config_px4fmu-v2_test.mk
@@ -0,0 +1,41 @@
+#
+# Makefile for the px4fmu_default configuration
+#
+
+#
+# Use the configuration's ROMFS.
+#
+ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_test
+
+#
+# Board support modules
+#
+MODULES += drivers/device
+MODULES += drivers/stm32
+MODULES += drivers/led
+MODULES += drivers/boards/px4fmu-v2
+MODULES += systemcmds/perf
+MODULES += systemcmds/reboot
+
+#
+# Library modules
+#
+MODULES += modules/systemlib
+MODULES += modules/uORB
+
+#
+# Transitional support - add commands from the NuttX export archive.
+#
+# In general, these should move to modules over time.
+#
+# Each entry here is <command>.<priority>.<stacksize>.<entrypoint> but we use a helper macro
+# to make the table a bit more readable.
+#
+define _B
+ $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4)
+endef
+
+# command priority stack entrypoint
+BUILTIN_COMMANDS := \
+ $(call _B, sercon, , 2048, sercon_main ) \
+ $(call _B, serdis, , 2048, serdis_main )
diff --git a/makefiles/config_px4io-v1_default.mk b/makefiles/config_px4io-v1_default.mk
index cf70391bcf901fdbe5d45fd794c66573026344fe..73f8adf20217245b40768aec2de4914ff0f24c2a 100644
--- a/makefiles/config_px4io-v1_default.mk
+++ b/makefiles/config_px4io-v1_default.mk
@@ -6,5 +6,5 @@
# Board support modules
#
MODULES += drivers/stm32
-MODULES += drivers/boards/px4io
+MODULES += drivers/boards/px4io-v1
MODULES += modules/px4iofirmware
\ No newline at end of file
diff --git a/makefiles/config_px4io-v2_default.mk b/makefiles/config_px4io-v2_default.mk
new file mode 100644
index 0000000000000000000000000000000000000000..dbeaba3d376f0f584c476b000c6e4caca985e1b2
--- /dev/null
+++ b/makefiles/config_px4io-v2_default.mk
@@ -0,0 +1,10 @@
+#
+# Makefile for the px4iov2_default configuration
+#
+
+#
+# Board support modules
+#
+MODULES += drivers/stm32
+MODULES += drivers/boards/px4io-v2
+MODULES += modules/px4iofirmware
\ No newline at end of file
diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk
index ecff77db9a9f6168f79841c8d7579d6ab6659953..b3e50501c7e1d5540e84524e1839381fb1404dab 100644
--- a/makefiles/firmware.mk
+++ b/makefiles/firmware.mk
@@ -177,6 +177,11 @@ GLOBAL_DEPS += $(MAKEFILE_LIST)
#
EXTRA_CLEANS =
+#
+# Append the per-board driver directory to the header search path.
+#
+INCLUDE_DIRS += $(PX4_MODULE_SRC)drivers/boards/$(BOARD)
+
################################################################################
# NuttX libraries and paths
################################################################################
@@ -317,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)/*/* \
@@ -329,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)
@@ -340,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)
diff --git a/makefiles/setup.mk b/makefiles/setup.mk
index 168e41a5cb448edad6c5104961657807fc785950..183b143d6cb5167a16ce43bef519078126d15c05 100644
--- a/makefiles/setup.mk
+++ b/makefiles/setup.mk
@@ -43,6 +43,7 @@
#
export PX4_INCLUDE_DIR = $(abspath $(PX4_BASE)/src/include)/
export PX4_MODULE_SRC = $(abspath $(PX4_BASE)/src)/
+export PX4_LIB_DIR = $(abspath $(PX4_BASE)/src/lib)/
export PX4_MK_DIR = $(abspath $(PX4_BASE)/makefiles)/
export NUTTX_SRC = $(abspath $(PX4_BASE)/NuttX/nuttx)/
export NUTTX_APP_SRC = $(abspath $(PX4_BASE)/NuttX/apps)/
@@ -57,7 +58,8 @@ export ARCHIVE_DIR = $(abspath $(PX4_BASE)/Archives)/
#
export INCLUDE_DIRS := $(PX4_MODULE_SRC) \
$(PX4_MODULE_SRC)/modules/ \
- $(PX4_INCLUDE_DIR)
+ $(PX4_INCLUDE_DIR) \
+ $(PX4_LIB_DIR)
#
# Tools
@@ -71,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
diff --git a/makefiles/upload.mk b/makefiles/upload.mk
index 226a228848e40bfc59edf905f070a9151d0af351..470ddfdf16d5ca9ecca0f41f9b2a4d73dd07b9c6 100644
--- a/makefiles/upload.mk
+++ b/makefiles/upload.mk
@@ -27,6 +27,8 @@ all: upload-$(METHOD)-$(BOARD)
upload-serial-px4fmu-v1: $(BUNDLE) $(UPLOADER)
$(Q) $(PYTHON) -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE)
+upload-serial-px4fmu-v2: $(BUNDLE) $(UPLOADER)
+ $(Q) $(PYTHON) -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE)
#
# JTAG firmware uploading with OpenOCD
diff --git a/nuttx-configs/px4fmu-v1/include/board.h b/nuttx-configs/px4fmu-v1/include/board.h
index 839631b3a2a66800b5edb7e95e069272079149cc..27ace4b7dbd2707ed7eee9815f9a43b260ed758c 100644
--- a/nuttx-configs/px4fmu-v1/include/board.h
+++ b/nuttx-configs/px4fmu-v1/include/board.h
@@ -156,11 +156,6 @@
#define STM32_TIM18_FREQUENCY (2*STM32_PCLK2_FREQUENCY)
#define STM32_TIM27_FREQUENCY (2*STM32_PCLK1_FREQUENCY)
-/* High-resolution timer
- */
-#define HRT_TIMER 1 /* use timer1 for the HRT */
-#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */
-
/* LED definitions ******************************************************************/
/* If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in any
* way. The following definitions are used to access individual LEDs.
@@ -215,33 +210,6 @@
#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2
#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2
-/*
- * PWM
- *
- * Four PWM outputs can be configured on pins otherwise shared with
- * USART2; two can take the flow control pins if they are not being used.
- *
- * Pins:
- *
- * CTS - PA0 - TIM2CH1
- * RTS - PA1 - TIM2CH2
- * TX - PA2 - TIM2CH3
- * RX - PA3 - TIM2CH4
- *
- */
-#define GPIO_TIM2_CH1OUT GPIO_TIM2_CH1OUT_1
-#define GPIO_TIM2_CH2OUT GPIO_TIM2_CH2OUT_1
-#define GPIO_TIM2_CH3OUT GPIO_TIM2_CH3OUT_1
-#define GPIO_TIM2_CH4OUT GPIO_TIM2_CH4OUT_1
-
-/*
- * PPM
- *
- * PPM input is handled by the HRT timer.
- */
-#define HRT_PPM_CHANNEL 3 /* use capture/compare channel 3 */
-#define GPIO_PPM_IN (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN10)
-
/*
* CAN
*
@@ -273,25 +241,6 @@
#define GPIO_I2C3_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN8)
#define GPIO_I2C3_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN9)
-/*
- * I2C busses
- */
-#define PX4_I2C_BUS_ESC 1
-#define PX4_I2C_BUS_ONBOARD 2
-#define PX4_I2C_BUS_EXPANSION 3
-
-/*
- * Devices on the onboard bus.
- *
- * Note that these are unshifted addresses.
- */
-#define PX4_I2C_OBDEV_HMC5883 0x1e
-#define PX4_I2C_OBDEV_MS5611 0x76
-#define PX4_I2C_OBDEV_EEPROM NOTDEFINED
-
-#define PX4_I2C_OBDEV_PX4IO_BL 0x18
-#define PX4_I2C_OBDEV_PX4IO 0x1a
-
/*
* SPI
*
@@ -316,27 +265,6 @@
#define DMACHAN_SPI1_RX DMAMAP_SPI1_RX_1
#define DMACHAN_SPI1_TX DMAMAP_SPI1_TX_2
-/*
- * Use these in place of the spi_dev_e enumeration to
- * select a specific SPI device on SPI1
- */
-#define PX4_SPIDEV_GYRO 1
-#define PX4_SPIDEV_ACCEL 2
-#define PX4_SPIDEV_MPU 3
-
-/*
- * Optional devices on IO's external port
- */
-#define PX4_SPIDEV_ACCEL_MAG 2
-
-/*
- * Tone alarm output
- */
-#define TONE_ALARM_TIMER 3 /* timer 3 */
-#define TONE_ALARM_CHANNEL 3 /* channel 3 */
-#define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN8)
-#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF2|GPIO_SPEED_2MHz|GPIO_FLOAT|GPIO_PUSHPULL|GPIO_PORTC|GPIO_PIN8)
-
/************************************************************************************
* Public Data
************************************************************************************/
diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig
index 5b91930c918a7f0fd75676a1ff802036e18f76dd..a6f914a6473dc28a7e08c7017140fccbe38fa3bb 100644
--- a/nuttx-configs/px4fmu-v1/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v1/nsh/defconfig
@@ -368,7 +368,7 @@ CONFIG_TASK_NAME_SIZE=24
CONFIG_START_YEAR=1970
CONFIG_START_MONTH=1
CONFIG_START_DAY=1
-# CONFIG_DEV_CONSOLE is not set
+CONFIG_DEV_CONSOLE=y
# CONFIG_MUTEX_TYPES is not set
CONFIG_PRIORITY_INHERITANCE=y
CONFIG_SEM_PREALLOCHOLDERS=0
@@ -406,7 +406,7 @@ CONFIG_MAX_TASKS=32
CONFIG_MAX_TASK_ARGS=10
CONFIG_NPTHREAD_KEYS=4
CONFIG_NFILE_DESCRIPTORS=32
-CONFIG_NFILE_STREAMS=25
+CONFIG_NFILE_STREAMS=8
CONFIG_NAME_MAX=32
CONFIG_PREALLOC_MQ_MSGS=4
CONFIG_MQ_MAXMSGSIZE=32
@@ -476,11 +476,11 @@ CONFIG_ARCH_HAVE_USART6=y
CONFIG_MCU_SERIAL=y
CONFIG_STANDARD_SERIAL=y
CONFIG_SERIAL_NPOLLWAITERS=2
-# CONFIG_USART1_SERIAL_CONSOLE is not set
+CONFIG_USART1_SERIAL_CONSOLE=y
# CONFIG_USART2_SERIAL_CONSOLE is not set
# CONFIG_UART5_SERIAL_CONSOLE is not set
# CONFIG_USART6_SERIAL_CONSOLE is not set
-CONFIG_NO_SERIAL_CONSOLE=y
+# CONFIG_NO_SERIAL_CONSOLE is not set
#
# USART1 Configuration
@@ -538,10 +538,9 @@ CONFIG_USBDEV=y
#
# CONFIG_USBDEV_ISOCHRONOUS is not set
# CONFIG_USBDEV_DUALSPEED is not set
-# CONFIG_USBDEV_SELFPOWERED is not set
-CONFIG_USBDEV_BUSPOWERED=y
+CONFIG_USBDEV_SELFPOWERED=y
+# CONFIG_USBDEV_BUSPOWERED is not set
CONFIG_USBDEV_MAXPOWER=500
-# CONFIG_USBDEV_REMOTEWAKEUP is not set
# CONFIG_USBDEV_DMA is not set
# CONFIG_USBDEV_TRACE is not set
@@ -551,7 +550,7 @@ CONFIG_USBDEV_MAXPOWER=500
# CONFIG_USBDEV_COMPOSITE is not set
# CONFIG_PL2303 is not set
CONFIG_CDCACM=y
-CONFIG_CDCACM_CONSOLE=y
+CONFIG_CDCACM_CONSOLE=n
CONFIG_CDCACM_EP0MAXPACKET=64
CONFIG_CDCACM_EPINTIN=1
CONFIG_CDCACM_EPINTIN_FSSIZE=64
@@ -565,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"
diff --git a/nuttx-configs/px4fmu-v2/include/board.h b/nuttx-configs/px4fmu-v2/include/board.h
new file mode 100755
index 0000000000000000000000000000000000000000..507df70a235a10b206b84de1a2fcabc8599898a0
--- /dev/null
+++ b/nuttx-configs/px4fmu-v2/include/board.h
@@ -0,0 +1,306 @@
+/************************************************************************************
+ * configs/px4fmu/include/board.h
+ * include/arch/board/board.h
+ *
+ * Copyright (C) 2009 Gregory Nutt. All rights reserved.
+ * Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX 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.
+ *
+ ************************************************************************************/
+
+#ifndef __ARCH_BOARD_BOARD_H
+#define __ARCH_BOARD_BOARD_H
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+#ifndef __ASSEMBLY__
+# include <stdint.h>
+#endif
+
+#include <stm32.h>
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/* Clocking *************************************************************************/
+/* The PX4FMUV2 uses a 24MHz crystal connected to the HSE.
+ *
+ * This is the "standard" configuration as set up by arch/arm/src/stm32f40xx_rcc.c:
+ * System Clock source : PLL (HSE)
+ * SYSCLK(Hz) : 168000000 Determined by PLL configuration
+ * HCLK(Hz) : 168000000 (STM32_RCC_CFGR_HPRE)
+ * AHB Prescaler : 1 (STM32_RCC_CFGR_HPRE)
+ * APB1 Prescaler : 4 (STM32_RCC_CFGR_PPRE1)
+ * APB2 Prescaler : 2 (STM32_RCC_CFGR_PPRE2)
+ * HSE Frequency(Hz) : 24000000 (STM32_BOARD_XTAL)
+ * PLLM : 24 (STM32_PLLCFG_PLLM)
+ * PLLN : 336 (STM32_PLLCFG_PLLN)
+ * PLLP : 2 (STM32_PLLCFG_PLLP)
+ * PLLQ : 7 (STM32_PLLCFG_PPQ)
+ * Main regulator output voltage : Scale1 mode Needed for high speed SYSCLK
+ * Flash Latency(WS) : 5
+ * Prefetch Buffer : OFF
+ * Instruction cache : ON
+ * Data cache : ON
+ * Require 48MHz for USB OTG FS, : Enabled
+ * SDIO and RNG clock
+ */
+
+/* HSI - 16 MHz RC factory-trimmed
+ * LSI - 32 KHz RC
+ * HSE - On-board crystal frequency is 24MHz
+ * LSE - not installed
+ */
+
+#define STM32_BOARD_XTAL 24000000ul
+
+#define STM32_HSI_FREQUENCY 16000000ul
+#define STM32_LSI_FREQUENCY 32000
+#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL
+//#define STM32_LSE_FREQUENCY 32768
+
+/* Main PLL Configuration.
+ *
+ * PLL source is HSE
+ * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN
+ * = (25,000,000 / 25) * 336
+ * = 336,000,000
+ * SYSCLK = PLL_VCO / PLLP
+ * = 336,000,000 / 2 = 168,000,000
+ * USB OTG FS, SDIO and RNG Clock
+ * = PLL_VCO / PLLQ
+ * = 48,000,000
+ */
+
+#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(24)
+#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(336)
+#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_2
+#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(7)
+
+#define STM32_SYSCLK_FREQUENCY 168000000ul
+
+/* AHB clock (HCLK) is SYSCLK (168MHz) */
+
+#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK /* HCLK = SYSCLK / 1 */
+#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY
+#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */
+
+/* APB1 clock (PCLK1) is HCLK/4 (42MHz) */
+
+#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd4 /* PCLK1 = HCLK / 4 */
+#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/4)
+
+/* Timers driven from APB1 will be twice PCLK1 */
+
+#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY)
+
+/* APB2 clock (PCLK2) is HCLK/2 (84MHz) */
+
+#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */
+#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2)
+
+/* Timers driven from APB2 will be twice PCLK2 */
+
+#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM9_CLKIN (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK1_FREQUENCY)
+
+/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx
+ * otherwise frequency is 2xAPBx.
+ * Note: TIM1,8 are on APB2, others on APB1
+ */
+
+#define STM32_TIM18_FREQUENCY (2*STM32_PCLK2_FREQUENCY)
+#define STM32_TIM27_FREQUENCY (2*STM32_PCLK1_FREQUENCY)
+
+/* SDIO dividers. Note that slower clocking is required when DMA is disabled
+ * in order to avoid RX overrun/TX underrun errors due to delayed responses
+ * to service FIFOs in interrupt driven mode. These values have not been
+ * tuned!!!
+ *
+ * HCLK=72MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(178+2)=400 KHz
+ */
+
+#define SDIO_INIT_CLKDIV (178 << SDIO_CLKCR_CLKDIV_SHIFT)
+
+/* DMA ON: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(2+2)=18 MHz
+ * DMA OFF: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(3+2)=14.4 MHz
+ */
+
+#ifdef CONFIG_SDIO_DMA
+# define SDIO_MMCXFR_CLKDIV (2 << SDIO_CLKCR_CLKDIV_SHIFT)
+#else
+# define SDIO_MMCXFR_CLKDIV (3 << SDIO_CLKCR_CLKDIV_SHIFT)
+#endif
+
+/* DMA ON: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(1+2)=24 MHz
+ * DMA OFF: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(3+2)=14.4 MHz
+ */
+
+#ifdef CONFIG_SDIO_DMA
+# define SDIO_SDXFR_CLKDIV (1 << SDIO_CLKCR_CLKDIV_SHIFT)
+#else
+# define SDIO_SDXFR_CLKDIV (3 << SDIO_CLKCR_CLKDIV_SHIFT)
+#endif
+
+/* DMA Channl/Stream Selections *****************************************************/
+/* Stream selections are arbitrary for now but might become important in the future
+ * is we set aside more DMA channels/streams.
+ *
+ * SDIO DMA
+ * Â DMAMAP_SDIO_1 = Channel 4, Stream 3 <- may later be used by SPI DMA
+ * Â DMAMAP_SDIO_2 = Channel 4, Stream 6
+ */
+
+#define DMAMAP_SDIO DMAMAP_SDIO_1
+
+/* Alternate function pin selections ************************************************/
+
+/*
+ * UARTs.
+ */
+#define GPIO_USART1_RX GPIO_USART1_RX_1 /* console in from IO */
+#define GPIO_USART1_TX 0 /* USART1 is RX-only */
+
+#define GPIO_USART2_RX GPIO_USART2_RX_2
+#define GPIO_USART2_TX GPIO_USART2_TX_2
+#define GPIO_USART2_RTS GPIO_USART2_RTS_2
+#define GPIO_USART2_CTS GPIO_USART2_CTS_2
+
+#define GPIO_USART3_RX GPIO_USART3_RX_3
+#define GPIO_USART3_TX GPIO_USART3_TX_3
+#define GPIO_USART2_RTS GPIO_USART2_RTS_2
+#define GPIO_USART2_CTS GPIO_USART2_CTS_2
+
+#define GPIO_UART4_RX GPIO_UART4_RX_1
+#define GPIO_UART4_TX GPIO_UART4_TX_1
+
+#define GPIO_USART6_RX GPIO_USART6_RX_1
+#define GPIO_USART6_TX GPIO_USART6_TX_1
+
+#define GPIO_UART7_RX GPIO_UART7_RX_1
+#define GPIO_UART7_TX GPIO_UART7_TX_1
+
+/* UART8 has no alternate pin config */
+
+/* UART RX DMA configurations */
+#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2
+#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2
+
+/*
+ * CAN
+ *
+ * CAN1 is routed to the onboard transceiver.
+ * CAN2 is routed to the expansion connector.
+ */
+#define GPIO_CAN1_RX GPIO_CAN1_RX_3
+#define GPIO_CAN1_TX GPIO_CAN1_TX_3
+#define GPIO_CAN2_RX GPIO_CAN2_RX_1
+#define GPIO_CAN2_TX GPIO_CAN2_TX_2
+
+/*
+ * I2C
+ *
+ * The optional _GPIO configurations allow the I2C driver to manually
+ * reset the bus to clear stuck slaves. They match the pin configuration,
+ * but are normally-high GPIOs.
+ */
+#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2
+#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2
+#define GPIO_I2C1_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN8)
+#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9)
+
+#define GPIO_I2C2_SCL GPIO_I2C2_SCL_1
+#define GPIO_I2C2_SDA GPIO_I2C2_SDA_1
+#define GPIO_I2C2_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN10)
+#define GPIO_I2C2_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11)
+
+/*
+ * SPI
+ *
+ * There are sensors on SPI1, and SPI2 is connected to the FRAM.
+ */
+#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1
+#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1
+#define GPIO_SPI1_SCK GPIO_SPI1_SCK_1
+
+#define GPIO_SPI2_MISO GPIO_SPI2_MISO_1
+#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_1
+#define GPIO_SPI2_SCK GPIO_SPI2_SCK_2
+
+/************************************************************************************
+ * Public Data
+ ************************************************************************************/
+
+#ifndef __ASSEMBLY__
+
+#undef EXTERN
+#if defined(__cplusplus)
+#define EXTERN extern "C"
+extern "C" {
+#else
+#define EXTERN extern
+#endif
+
+/************************************************************************************
+ * Public Function Prototypes
+ ************************************************************************************/
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ * All STM32 architectures must provide the following entry point. This entry point
+ * is called early in the intitialization -- after all memory has been configured
+ * and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+EXTERN void stm32_boardinitialize(void);
+
+#undef EXTERN
+#if defined(__cplusplus)
+}
+#endif
+
+#endif /* __ASSEMBLY__ */
+#endif /* __ARCH_BOARD_BOARD_H */
diff --git a/nuttx-configs/px4fmu-v2/include/nsh_romfsimg.h b/nuttx-configs/px4fmu-v2/include/nsh_romfsimg.h
new file mode 100644
index 0000000000000000000000000000000000000000..15e4e7a8d5a6cdc29fcaf1e67edd8a2087f5178a
--- /dev/null
+++ b/nuttx-configs/px4fmu-v2/include/nsh_romfsimg.h
@@ -0,0 +1,42 @@
+/****************************************************************************
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+/**
+ * nsh_romfsetc.h
+ *
+ * This file is a stub for 'make export' purposes; the actual ROMFS
+ * must be supplied by the library client.
+ */
+
+extern unsigned char romfs_img[];
+extern unsigned int romfs_img_len;
diff --git a/nuttx-configs/px4fmu-v2/nsh/Make.defs b/nuttx-configs/px4fmu-v2/nsh/Make.defs
new file mode 100644
index 0000000000000000000000000000000000000000..e70320aaa4c9d1e1fca8980712f4b49acfa479aa
--- /dev/null
+++ b/nuttx-configs/px4fmu-v2/nsh/Make.defs
@@ -0,0 +1,179 @@
+############################################################################
+# configs/px4fmu-v2/nsh/Make.defs
+#
+# Copyright (C) 2011 Gregory Nutt. All rights reserved.
+# Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# 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 NuttX 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.
+#
+############################################################################
+
+include ${TOPDIR}/.config
+include ${TOPDIR}/tools/Config.mk
+
+#
+# We only support building with the ARM bare-metal toolchain from
+# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS.
+#
+CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI
+
+include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs
+
+CC = $(CROSSDEV)gcc
+CXX = $(CROSSDEV)g++
+CPP = $(CROSSDEV)gcc -E
+LD = $(CROSSDEV)ld
+AR = $(CROSSDEV)ar rcs
+NM = $(CROSSDEV)nm
+OBJCOPY = $(CROSSDEV)objcopy
+OBJDUMP = $(CROSSDEV)objdump
+
+MAXOPTIMIZATION = -O3
+ARCHCPUFLAGS = -mcpu=cortex-m4 \
+ -mthumb \
+ -march=armv7e-m \
+ -mfpu=fpv4-sp-d16 \
+ -mfloat-abi=hard
+
+
+# enable precise stack overflow tracking
+INSTRUMENTATIONDEFINES = -finstrument-functions \
+ -ffixed-r10
+
+# pull in *just* libm from the toolchain ... this is grody
+LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}"
+EXTRA_LIBS += $(LIBM)
+
+# use our linker script
+LDSCRIPT = ld.script
+
+ifeq ($(WINTOOL),y)
+ # Windows-native toolchains
+ DIRLINK = $(TOPDIR)/tools/copydir.sh
+ DIRUNLINK = $(TOPDIR)/tools/unlink.sh
+ MKDEP = $(TOPDIR)/tools/mknulldeps.sh
+ ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}"
+ ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}"
+ ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}"
+else
+ ifeq ($(PX4_WINTOOL),y)
+ # Windows-native toolchains (MSYS)
+ DIRLINK = $(TOPDIR)/tools/copydir.sh
+ DIRUNLINK = $(TOPDIR)/tools/unlink.sh
+ MKDEP = $(TOPDIR)/tools/mknulldeps.sh
+ ARCHINCLUDES = -I. -isystem $(TOPDIR)/include
+ ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx
+ ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)
+ else
+ # Linux/Cygwin-native toolchain
+ MKDEP = $(TOPDIR)/tools/mkdeps.sh
+ ARCHINCLUDES = -I. -isystem $(TOPDIR)/include
+ ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx
+ ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)
+ endif
+endif
+
+# tool versions
+ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'}
+ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1}
+
+# optimisation flags
+ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \
+ -fno-strict-aliasing \
+ -fno-strength-reduce \
+ -fomit-frame-pointer \
+ -funsafe-math-optimizations \
+ -fno-builtin-printf \
+ -ffunction-sections \
+ -fdata-sections
+
+ifeq ("${CONFIG_DEBUG_SYMBOLS}","y")
+ARCHOPTIMIZATION += -g
+endif
+
+ARCHCFLAGS = -std=gnu99
+ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x
+ARCHWARNINGS = -Wall \
+ -Wextra \
+ -Wdouble-promotion \
+ -Wshadow \
+ -Wfloat-equal \
+ -Wframe-larger-than=1024 \
+ -Wpointer-arith \
+ -Wlogical-op \
+ -Wmissing-declarations \
+ -Wpacked \
+ -Wno-unused-parameter
+# -Wcast-qual - generates spurious noreturn attribute warnings, try again later
+# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code
+# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives
+
+ARCHCWARNINGS = $(ARCHWARNINGS) \
+ -Wbad-function-cast \
+ -Wstrict-prototypes \
+ -Wold-style-declaration \
+ -Wmissing-parameter-type \
+ -Wmissing-prototypes \
+ -Wnested-externs \
+ -Wunsuffixed-float-constants
+ARCHWARNINGSXX = $(ARCHWARNINGS) \
+ -Wno-psabi
+ARCHDEFINES =
+ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10
+
+# this seems to be the only way to add linker flags
+EXTRA_LIBS += --warn-common \
+ --gc-sections
+
+CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common
+CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS)
+CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe
+CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS)
+CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES)
+AFLAGS = $(CFLAGS) -D__ASSEMBLY__
+
+NXFLATLDFLAGS1 = -r -d -warn-common
+NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections
+LDNXFLATFLAGS = -e main -s 2048
+
+OBJEXT = .o
+LIBEXT = .a
+EXEEXT =
+
+
+# produce partially-linked $1 from files in $2
+define PRELINK
+ @echo "PRELINK: $1"
+ $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1
+endef
+
+HOSTCC = gcc
+HOSTINCLUDES = -I.
+HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe
+HOSTLDFLAGS =
+
diff --git a/nuttx-configs/px4fmu-v2/nsh/appconfig b/nuttx-configs/px4fmu-v2/nsh/appconfig
new file mode 100644
index 0000000000000000000000000000000000000000..0e18aa8ef10a26b4200a5719887fda69d3184e31
--- /dev/null
+++ b/nuttx-configs/px4fmu-v2/nsh/appconfig
@@ -0,0 +1,52 @@
+############################################################################
+# configs/px4fmu/nsh/appconfig
+#
+# Copyright (C) 2011 Gregory Nutt. All rights reserved.
+# Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# 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 NuttX 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.
+#
+############################################################################
+
+# Path to example in apps/examples containing the user_start entry point
+
+CONFIGURED_APPS += examples/nsh
+
+# The NSH application library
+CONFIGURED_APPS += nshlib
+CONFIGURED_APPS += system/readline
+
+ifeq ($(CONFIG_CAN),y)
+#CONFIGURED_APPS += examples/can
+endif
+
+#ifeq ($(CONFIG_USBDEV),y)
+#ifeq ($(CONFIG_CDCACM),y)
+CONFIGURED_APPS += examples/cdcacm
+#endif
+#endif
diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig
new file mode 100644
index 0000000000000000000000000000000000000000..e507c89baf1508c3c0ca7becc25b3b1b6054762d
--- /dev/null
+++ b/nuttx-configs/px4fmu-v2/nsh/defconfig
@@ -0,0 +1,1063 @@
+#
+# Automatically generated file; DO NOT EDIT.
+# Nuttx/ Configuration
+#
+CONFIG_NUTTX_NEWCONFIG=y
+
+#
+# Build Setup
+#
+# CONFIG_EXPERIMENTAL is not set
+# CONFIG_HOST_LINUX is not set
+CONFIG_HOST_OSX=y
+# CONFIG_HOST_WINDOWS is not set
+# CONFIG_HOST_OTHER is not set
+
+#
+# Build Configuration
+#
+CONFIG_APPS_DIR="../apps"
+# CONFIG_BUILD_2PASS is not set
+
+#
+# Binary Output Formats
+#
+# CONFIG_RRLOAD_BINARY is not set
+# CONFIG_INTELHEX_BINARY is not set
+# CONFIG_MOTOROLA_SREC is not set
+CONFIG_RAW_BINARY=y
+
+#
+# Customize Header Files
+#
+# CONFIG_ARCH_STDBOOL_H is not set
+CONFIG_ARCH_MATH_H=y
+# CONFIG_ARCH_FLOAT_H is not set
+# CONFIG_ARCH_STDARG_H is not set
+
+#
+# Debug Options
+#
+# CONFIG_DEBUG is not set
+CONFIG_DEBUG_SYMBOLS=y
+
+#
+# System Type
+#
+# CONFIG_ARCH_8051 is not set
+CONFIG_ARCH_ARM=y
+# CONFIG_ARCH_AVR is not set
+# CONFIG_ARCH_HC is not set
+# CONFIG_ARCH_MIPS is not set
+# CONFIG_ARCH_RGMP is not set
+# CONFIG_ARCH_SH is not set
+# CONFIG_ARCH_SIM is not set
+# CONFIG_ARCH_X86 is not set
+# CONFIG_ARCH_Z16 is not set
+# CONFIG_ARCH_Z80 is not set
+CONFIG_ARCH="arm"
+
+#
+# ARM Options
+#
+# CONFIG_ARCH_CHIP_C5471 is not set
+# CONFIG_ARCH_CHIP_CALYPSO is not set
+# CONFIG_ARCH_CHIP_DM320 is not set
+# CONFIG_ARCH_CHIP_IMX is not set
+# CONFIG_ARCH_CHIP_KINETIS is not set
+# CONFIG_ARCH_CHIP_KL is not set
+# CONFIG_ARCH_CHIP_LM is not set
+# CONFIG_ARCH_CHIP_LPC17XX is not set
+# CONFIG_ARCH_CHIP_LPC214X is not set
+# CONFIG_ARCH_CHIP_LPC2378 is not set
+# CONFIG_ARCH_CHIP_LPC31XX is not set
+# CONFIG_ARCH_CHIP_LPC43XX is not set
+# CONFIG_ARCH_CHIP_NUC1XX is not set
+# CONFIG_ARCH_CHIP_SAM34 is not set
+CONFIG_ARCH_CHIP_STM32=y
+# CONFIG_ARCH_CHIP_STR71X is not set
+CONFIG_ARCH_CORTEXM4=y
+CONFIG_ARCH_FAMILY="armv7-m"
+CONFIG_ARCH_CHIP="stm32"
+CONFIG_ARMV7M_USEBASEPRI=y
+CONFIG_ARCH_HAVE_CMNVECTOR=y
+CONFIG_ARMV7M_CMNVECTOR=y
+CONFIG_ARCH_HAVE_FPU=y
+CONFIG_ARCH_FPU=y
+CONFIG_ARCH_HAVE_MPU=y
+# CONFIG_ARMV7M_MPU is not set
+
+#
+# ARMV7M Configuration Options
+#
+# CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set
+CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y
+CONFIG_ARMV7M_STACKCHECK=y
+CONFIG_SERIAL_TERMIOS=y
+CONFIG_SERIAL_DISABLE_REORDERING=y
+
+#
+# STM32 Configuration Options
+#
+# CONFIG_ARCH_CHIP_STM32L151C6 is not set
+# CONFIG_ARCH_CHIP_STM32L151C8 is not set
+# CONFIG_ARCH_CHIP_STM32L151CB is not set
+# CONFIG_ARCH_CHIP_STM32L151R6 is not set
+# CONFIG_ARCH_CHIP_STM32L151R8 is not set
+# CONFIG_ARCH_CHIP_STM32L151RB is not set
+# CONFIG_ARCH_CHIP_STM32L151V6 is not set
+# CONFIG_ARCH_CHIP_STM32L151V8 is not set
+# CONFIG_ARCH_CHIP_STM32L151VB is not set
+# CONFIG_ARCH_CHIP_STM32L152C6 is not set
+# CONFIG_ARCH_CHIP_STM32L152C8 is not set
+# CONFIG_ARCH_CHIP_STM32L152CB is not set
+# CONFIG_ARCH_CHIP_STM32L152R6 is not set
+# CONFIG_ARCH_CHIP_STM32L152R8 is not set
+# CONFIG_ARCH_CHIP_STM32L152RB is not set
+# CONFIG_ARCH_CHIP_STM32L152V6 is not set
+# CONFIG_ARCH_CHIP_STM32L152V8 is not set
+# CONFIG_ARCH_CHIP_STM32L152VB is not set
+# CONFIG_ARCH_CHIP_STM32F100C8 is not set
+# CONFIG_ARCH_CHIP_STM32F100CB is not set
+# CONFIG_ARCH_CHIP_STM32F100R8 is not set
+# CONFIG_ARCH_CHIP_STM32F100RB is not set
+# CONFIG_ARCH_CHIP_STM32F100RC is not set
+# CONFIG_ARCH_CHIP_STM32F100RD is not set
+# CONFIG_ARCH_CHIP_STM32F100RE is not set
+# CONFIG_ARCH_CHIP_STM32F100V8 is not set
+# CONFIG_ARCH_CHIP_STM32F100VB is not set
+# CONFIG_ARCH_CHIP_STM32F100VC is not set
+# CONFIG_ARCH_CHIP_STM32F100VD is not set
+# CONFIG_ARCH_CHIP_STM32F100VE is not set
+# CONFIG_ARCH_CHIP_STM32F103C4 is not set
+# CONFIG_ARCH_CHIP_STM32F103C8 is not set
+# CONFIG_ARCH_CHIP_STM32F103RET6 is not set
+# CONFIG_ARCH_CHIP_STM32F103VCT6 is not set
+# CONFIG_ARCH_CHIP_STM32F103VET6 is not set
+# CONFIG_ARCH_CHIP_STM32F103ZET6 is not set
+# CONFIG_ARCH_CHIP_STM32F105VBT7 is not set
+# CONFIG_ARCH_CHIP_STM32F107VC is not set
+# CONFIG_ARCH_CHIP_STM32F207IG is not set
+# CONFIG_ARCH_CHIP_STM32F302CB is not set
+# CONFIG_ARCH_CHIP_STM32F302CC is not set
+# CONFIG_ARCH_CHIP_STM32F302RB is not set
+# CONFIG_ARCH_CHIP_STM32F302RC is not set
+# CONFIG_ARCH_CHIP_STM32F302VB is not set
+# CONFIG_ARCH_CHIP_STM32F302VC is not set
+# CONFIG_ARCH_CHIP_STM32F303CB is not set
+# CONFIG_ARCH_CHIP_STM32F303CC is not set
+# CONFIG_ARCH_CHIP_STM32F303RB is not set
+# CONFIG_ARCH_CHIP_STM32F303RC is not set
+# CONFIG_ARCH_CHIP_STM32F303VB is not set
+# CONFIG_ARCH_CHIP_STM32F303VC is not set
+# CONFIG_ARCH_CHIP_STM32F405RG is not set
+# CONFIG_ARCH_CHIP_STM32F405VG is not set
+# CONFIG_ARCH_CHIP_STM32F405ZG is not set
+# CONFIG_ARCH_CHIP_STM32F407VE is not set
+# CONFIG_ARCH_CHIP_STM32F407VG is not set
+# CONFIG_ARCH_CHIP_STM32F407ZE is not set
+# CONFIG_ARCH_CHIP_STM32F407ZG is not set
+# CONFIG_ARCH_CHIP_STM32F407IE is not set
+# CONFIG_ARCH_CHIP_STM32F407IG is not set
+CONFIG_ARCH_CHIP_STM32F427V=y
+# CONFIG_ARCH_CHIP_STM32F427Z is not set
+# CONFIG_ARCH_CHIP_STM32F427I is not set
+# CONFIG_STM32_STM32L15XX is not set
+# CONFIG_STM32_ENERGYLITE is not set
+# CONFIG_STM32_STM32F10XX is not set
+# CONFIG_STM32_VALUELINE is not set
+# CONFIG_STM32_CONNECTIVITYLINE is not set
+# CONFIG_STM32_PERFORMANCELINE is not set
+# CONFIG_STM32_HIGHDENSITY is not set
+# CONFIG_STM32_MEDIUMDENSITY is not set
+# CONFIG_STM32_LOWDENSITY is not set
+# CONFIG_STM32_STM32F20XX is not set
+# CONFIG_STM32_STM32F30XX is not set
+CONFIG_STM32_STM32F40XX=y
+CONFIG_STM32_STM32F427=y
+# CONFIG_STM32_DFU is not set
+
+#
+# STM32 Peripheral Support
+#
+CONFIG_STM32_ADC1=y
+# CONFIG_STM32_ADC2 is not set
+# CONFIG_STM32_ADC3 is not set
+CONFIG_STM32_BKPSRAM=y
+# CONFIG_STM32_CAN1 is not set
+# CONFIG_STM32_CAN2 is not set
+CONFIG_STM32_CCMDATARAM=y
+# CONFIG_STM32_CRC is not set
+# CONFIG_STM32_CRYP is not set
+CONFIG_STM32_DMA1=y
+CONFIG_STM32_DMA2=y
+# CONFIG_STM32_DAC1 is not set
+# CONFIG_STM32_DAC2 is not set
+# CONFIG_STM32_DCMI is not set
+# CONFIG_STM32_ETHMAC is not set
+# CONFIG_STM32_FSMC is not set
+# CONFIG_STM32_HASH is not set
+CONFIG_STM32_I2C1=y
+CONFIG_STM32_I2C2=y
+# CONFIG_STM32_I2C3 is not set
+CONFIG_STM32_OTGFS=y
+# CONFIG_STM32_OTGHS is not set
+CONFIG_STM32_PWR=y
+# CONFIG_STM32_RNG is not set
+CONFIG_STM32_SDIO=y
+CONFIG_STM32_SPI1=y
+CONFIG_STM32_SPI2=y
+# CONFIG_STM32_SPI3 is not set
+# CONFIG_STM32_SPI4 is not set
+# CONFIG_STM32_SPI5 is not set
+# CONFIG_STM32_SPI6 is not set
+CONFIG_STM32_SYSCFG=y
+CONFIG_STM32_TIM1=y
+# CONFIG_STM32_TIM2 is not set
+CONFIG_STM32_TIM3=y
+CONFIG_STM32_TIM4=y
+# CONFIG_STM32_TIM5 is not set
+# CONFIG_STM32_TIM6 is not set
+# CONFIG_STM32_TIM7 is not set
+# CONFIG_STM32_TIM8 is not set
+CONFIG_STM32_TIM9=y
+CONFIG_STM32_TIM10=y
+CONFIG_STM32_TIM11=y
+# CONFIG_STM32_TIM12 is not set
+# CONFIG_STM32_TIM13 is not set
+# CONFIG_STM32_TIM14 is not set
+CONFIG_STM32_USART1=y
+CONFIG_STM32_USART2=y
+CONFIG_STM32_USART3=y
+CONFIG_STM32_UART4=y
+# CONFIG_STM32_UART5 is not set
+CONFIG_STM32_USART6=y
+CONFIG_STM32_UART7=y
+CONFIG_STM32_UART8=y
+# CONFIG_STM32_IWDG is not set
+CONFIG_STM32_WWDG=y
+CONFIG_STM32_ADC=y
+CONFIG_STM32_SPI=y
+CONFIG_STM32_I2C=y
+
+CONFIG_ARCH_HAVE_LEDS=y
+# CONFIG_ARCH_LEDS is not set
+
+#
+# Alternate Pin Mapping
+#
+CONFIG_STM32_FLASH_PREFETCH=y
+# CONFIG_STM32_JTAG_DISABLE is not set
+# CONFIG_STM32_JTAG_FULL_ENABLE is not set
+# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set
+CONFIG_STM32_JTAG_SW_ENABLE=y
+CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y
+# CONFIG_STM32_FORCEPOWER is not set
+# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set
+CONFIG_STM32_CCMEXCLUDE=y
+CONFIG_STM32_DMACAPABLE=y
+# CONFIG_STM32_TIM1_PWM is not set
+# CONFIG_STM32_TIM3_PWM is not set
+# CONFIG_STM32_TIM4_PWM is not set
+# CONFIG_STM32_TIM9_PWM is not set
+# CONFIG_STM32_TIM10_PWM is not set
+# CONFIG_STM32_TIM11_PWM is not set
+# CONFIG_STM32_TIM1_ADC is not set
+# CONFIG_STM32_TIM3_ADC is not set
+# CONFIG_STM32_TIM4_ADC is not set
+CONFIG_STM32_USART=y
+
+#
+# U[S]ART Configuration
+#
+# CONFIG_USART1_RS485 is not set
+CONFIG_USART1_RXDMA=y
+# CONFIG_USART2_RS485 is not set
+CONFIG_USART2_RXDMA=y
+# CONFIG_USART3_RS485 is not set
+# CONFIG_USART3_RXDMA is not set
+# CONFIG_UART4_RS485 is not set
+# CONFIG_UART4_RXDMA is not set
+# CONFIG_UART5_RXDMA is not set
+# CONFIG_USART6_RS485 is not set
+# CONFIG_USART6_RXDMA is not set
+# CONFIG_USART7_RXDMA is not set
+CONFIG_USART8_RXDMA=y
+CONFIG_STM32_USART_SINGLEWIRE=y
+
+
+# Previous:
+## CONFIG_USART1_RS485 is not set
+#CONFIG_USART1_RXDMA=y
+## CONFIG_USART2_RS485 is not set
+#CONFIG_USART2_RXDMA=y
+## CONFIG_USART3_RS485 is not set
+#CONFIG_USART3_RXDMA=y
+## CONFIG_UART4_RS485 is not set
+#CONFIG_UART4_RXDMA=y
+## CONFIG_UART5_RXDMA is not set
+## CONFIG_USART6_RS485 is not set
+## CONFIG_USART6_RXDMA is not set
+## CONFIG_USART7_RXDMA is not set
+#CONFIG_USART8_RXDMA=y
+#CONFIG_STM32_USART_SINGLEWIRE=y
+
+
+
+
+#
+# SPI Configuration
+#
+# CONFIG_STM32_SPI_INTERRUPTS is not set
+# CONFIG_STM32_SPI_DMA is not set
+
+#
+# I2C Configuration
+#
+# CONFIG_STM32_I2C_DYNTIMEO is not set
+CONFIG_STM32_I2CTIMEOSEC=0
+CONFIG_STM32_I2CTIMEOMS=10
+CONFIG_STM32_I2CTIMEOTICKS=500
+# CONFIG_STM32_I2C_DUTY16_9 is not set
+
+#
+# SDIO Configuration
+#
+
+#
+# Maintain legacy build behavior (revisit)
+#
+
+CONFIG_MMCSD=y
+#CONFIG_MMCSD_SPI=y
+CONFIG_MMCSD_SDIO=y
+CONFIG_MTD=y
+
+#
+# STM32 SDIO-based MMC/SD driver
+#
+CONFIG_SDIO_DMA=y
+# CONFIG_SDIO_DMAPRIO is not set
+# CONFIG_SDIO_WIDTH_D1_ONLY is not set
+CONFIG_MMCSD_MULTIBLOCK_DISABLE=y
+CONFIG_MMCSD_MMCSUPPORT=n
+CONFIG_MMCSD_HAVECARDDETECT=n
+
+#
+# USB Host Configuration
+#
+
+#
+# USB Device Configuration
+#
+
+#
+# External Memory Configuration
+#
+
+#
+# Architecture Options
+#
+# CONFIG_ARCH_NOINTC is not set
+# CONFIG_ARCH_VECNOTIRQ is not set
+CONFIG_ARCH_DMA=y
+CONFIG_ARCH_IRQPRIO=y
+# CONFIG_CUSTOM_STACK is not set
+# CONFIG_ADDRENV is not set
+CONFIG_ARCH_HAVE_VFORK=y
+CONFIG_ARCH_STACKDUMP=y
+# CONFIG_ENDIAN_BIG is not set
+# CONFIG_ARCH_HAVE_RAMFUNCS is not set
+CONFIG_ARCH_HAVE_RAMVECTORS=y
+# CONFIG_ARCH_RAMVECTORS is not set
+
+#
+# Board Settings
+#
+CONFIG_BOARD_LOOPSPERMSEC=16717
+# CONFIG_ARCH_CALIBRATION is not set
+CONFIG_DRAM_START=0x20000000
+CONFIG_DRAM_SIZE=262144
+CONFIG_ARCH_HAVE_INTERRUPTSTACK=y
+CONFIG_ARCH_INTERRUPTSTACK=2048
+
+#
+# Boot options
+#
+# CONFIG_BOOT_RUNFROMEXTSRAM is not set
+CONFIG_BOOT_RUNFROMFLASH=y
+# CONFIG_BOOT_RUNFROMISRAM is not set
+# CONFIG_BOOT_RUNFROMSDRAM is not set
+# CONFIG_BOOT_COPYTORAM is not set
+
+#
+# Board Selection
+#
+CONFIG_ARCH_BOARD_PX4FMU_V2=y
+# CONFIG_ARCH_BOARD_CUSTOM is not set
+CONFIG_ARCH_BOARD="px4fmu-v2"
+
+#
+# Common Board Options
+#
+CONFIG_NSH_MMCSDSLOTNO=0
+CONFIG_NSH_MMCSDMINOR=0
+
+#
+# Board-Specific Options
+#
+
+#
+# RTOS Features
+#
+# CONFIG_BOARD_INITIALIZE is not set
+CONFIG_MSEC_PER_TICK=1
+CONFIG_RR_INTERVAL=0
+CONFIG_SCHED_INSTRUMENTATION=y
+CONFIG_TASK_NAME_SIZE=24
+# CONFIG_SCHED_HAVE_PARENT is not set
+# CONFIG_JULIAN_TIME is not set
+CONFIG_START_YEAR=1970
+CONFIG_START_MONTH=1
+CONFIG_START_DAY=1
+CONFIG_DEV_CONSOLE=y
+# CONFIG_MUTEX_TYPES is not set
+CONFIG_PRIORITY_INHERITANCE=y
+CONFIG_SEM_PREALLOCHOLDERS=8
+CONFIG_SEM_NNESTPRIO=8
+# CONFIG_FDCLONE_DISABLE is not set
+CONFIG_FDCLONE_STDIO=y
+CONFIG_SDCLONE_DISABLE=y
+CONFIG_SCHED_WAITPID=y
+# CONFIG_SCHED_STARTHOOK is not set
+CONFIG_SCHED_ATEXIT=y
+CONFIG_SCHED_ATEXIT_MAX=1
+# CONFIG_SCHED_ONEXIT is not set
+CONFIG_USER_ENTRYPOINT="nsh_main"
+# CONFIG_DISABLE_OS_API is not set
+
+#
+# Signal Numbers
+#
+CONFIG_SIG_SIGUSR1=1
+CONFIG_SIG_SIGUSR2=2
+CONFIG_SIG_SIGALARM=3
+CONFIG_SIG_SIGCONDTIMEDOUT=16
+CONFIG_SIG_SIGWORK=4
+
+#
+# Sizes of configurable things (0 disables)
+#
+CONFIG_MAX_TASKS=32
+CONFIG_MAX_TASK_ARGS=10
+CONFIG_NPTHREAD_KEYS=4
+CONFIG_NFILE_DESCRIPTORS=32
+CONFIG_NFILE_STREAMS=8
+CONFIG_NAME_MAX=32
+CONFIG_PREALLOC_MQ_MSGS=4
+CONFIG_MQ_MAXMSGSIZE=32
+CONFIG_MAX_WDOGPARMS=2
+CONFIG_PREALLOC_WDOGS=50
+CONFIG_PREALLOC_TIMERS=50
+
+#
+# Stack and heap information
+#
+CONFIG_IDLETHREAD_STACKSIZE=6000
+CONFIG_USERMAIN_STACKSIZE=4096
+CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_PTHREAD_STACK_DEFAULT=2048
+
+#
+# Device Drivers
+#
+# CONFIG_DISABLE_POLL is not set
+CONFIG_DEV_NULL=y
+# CONFIG_DEV_ZERO is not set
+# CONFIG_LOOP is not set
+# CONFIG_RAMDISK is not set
+# CONFIG_CAN is not set
+# CONFIG_PWM is not set
+CONFIG_I2C=y
+# CONFIG_I2C_SLAVE is not set
+CONFIG_I2C_TRANSFER=y
+# CONFIG_I2C_WRITEREAD is not set
+# CONFIG_I2C_POLLED is not set
+# CONFIG_I2C_TRACE is not set
+CONFIG_ARCH_HAVE_I2CRESET=y
+CONFIG_I2C_RESET=y
+CONFIG_SPI=y
+# CONFIG_SPI_OWNBUS is not set
+CONFIG_SPI_EXCHANGE=y
+# CONFIG_SPI_CMDDATA is not set
+# CONFIG_RTC is not set
+CONFIG_WATCHDOG=y
+# CONFIG_ANALOG is not set
+# CONFIG_AUDIO_DEVICES is not set
+# CONFIG_BCH is not set
+# CONFIG_INPUT is not set
+# CONFIG_LCD is not set
+# CONFIG_MMCSD is not set
+CONFIG_MTD=y
+
+#
+# MTD Configuration
+#
+# CONFIG_MTD_PARTITION is not set
+# CONFIG_MTD_BYTE_WRITE is not set
+
+#
+# MTD Device Drivers
+#
+# CONFIG_RAMMTD is not set
+# CONFIG_MTD_AT24XX is not set
+# CONFIG_MTD_AT45DB is not set
+# CONFIG_MTD_M25P is not set
+# CONFIG_MTD_SMART is not set
+CONFIG_MTD_RAMTRON=y
+# CONFIG_MTD_SST25 is not set
+# CONFIG_MTD_SST39FV is not set
+# CONFIG_MTD_W25 is not set
+CONFIG_PIPES=y
+# CONFIG_PM is not set
+# CONFIG_POWER is not set
+# CONFIG_SENSORS is not set
+CONFIG_SERIAL=y
+CONFIG_DEV_LOWCONSOLE=y
+CONFIG_SERIAL_REMOVABLE=y
+# CONFIG_16550_UART is not set
+CONFIG_ARCH_HAVE_UART4=y
+CONFIG_ARCH_HAVE_UART7=y
+CONFIG_ARCH_HAVE_UART8=y
+CONFIG_ARCH_HAVE_USART1=y
+CONFIG_ARCH_HAVE_USART2=y
+CONFIG_ARCH_HAVE_USART3=y
+CONFIG_ARCH_HAVE_USART6=y
+CONFIG_MCU_SERIAL=y
+CONFIG_STANDARD_SERIAL=y
+CONFIG_SERIAL_NPOLLWAITERS=2
+# CONFIG_USART1_SERIAL_CONSOLE is not set
+# CONFIG_USART2_SERIAL_CONSOLE is not set
+# CONFIG_USART3_SERIAL_CONSOLE is not set
+# CONFIG_UART4_SERIAL_CONSOLE is not set
+# CONFIG_USART6_SERIAL_CONSOLE is not set
+# CONFIG_UART7_SERIAL_CONSOLE is not set
+CONFIG_UART8_SERIAL_CONSOLE=y
+# CONFIG_NO_SERIAL_CONSOLE is not set
+
+#
+# USART1 Configuration
+#
+CONFIG_USART1_RXBUFSIZE=512
+CONFIG_USART1_TXBUFSIZE=512
+CONFIG_USART1_BAUD=115200
+CONFIG_USART1_BITS=8
+CONFIG_USART1_PARITY=0
+CONFIG_USART1_2STOP=0
+# CONFIG_USART1_IFLOWCONTROL is not set
+# CONFIG_USART1_OFLOWCONTROL is not set
+
+#
+# USART2 Configuration
+#
+CONFIG_USART2_RXBUFSIZE=512
+CONFIG_USART2_TXBUFSIZE=512
+CONFIG_USART2_BAUD=57600
+CONFIG_USART2_BITS=8
+CONFIG_USART2_PARITY=0
+CONFIG_USART2_2STOP=0
+CONFIG_USART2_IFLOWCONTROL=y
+CONFIG_USART2_OFLOWCONTROL=y
+
+#
+# USART3 Configuration
+#
+CONFIG_USART3_RXBUFSIZE=512
+CONFIG_USART3_TXBUFSIZE=512
+CONFIG_USART3_BAUD=57600
+CONFIG_USART3_BITS=8
+CONFIG_USART3_PARITY=0
+CONFIG_USART3_2STOP=0
+CONFIG_USART3_IFLOWCONTROL=y
+CONFIG_USART3_OFLOWCONTROL=y
+
+#
+# UART4 Configuration
+#
+CONFIG_UART4_RXBUFSIZE=128
+CONFIG_UART4_TXBUFSIZE=128
+CONFIG_UART4_BAUD=57600
+CONFIG_UART4_BITS=8
+CONFIG_UART4_PARITY=0
+CONFIG_UART4_2STOP=0
+# CONFIG_UART4_IFLOWCONTROL is not set
+# CONFIG_UART4_OFLOWCONTROL is not set
+
+#
+# USART6 Configuration
+#
+CONFIG_USART6_RXBUFSIZE=256
+CONFIG_USART6_TXBUFSIZE=256
+CONFIG_USART6_BAUD=57600
+CONFIG_USART6_BITS=8
+CONFIG_USART6_PARITY=0
+CONFIG_USART6_2STOP=0
+# CONFIG_USART6_IFLOWCONTROL is not set
+# CONFIG_USART6_OFLOWCONTROL is not set
+
+#
+# UART7 Configuration
+#
+CONFIG_UART7_RXBUFSIZE=256
+CONFIG_UART7_TXBUFSIZE=256
+CONFIG_UART7_BAUD=57600
+CONFIG_UART7_BITS=8
+CONFIG_UART7_PARITY=0
+CONFIG_UART7_2STOP=0
+# CONFIG_UART7_IFLOWCONTROL is not set
+# CONFIG_UART7_OFLOWCONTROL is not set
+
+#
+# UART8 Configuration
+#
+CONFIG_UART8_RXBUFSIZE=256
+CONFIG_UART8_TXBUFSIZE=256
+CONFIG_UART8_BAUD=57600
+CONFIG_UART8_BITS=8
+CONFIG_UART8_PARITY=0
+CONFIG_UART8_2STOP=0
+# CONFIG_UART8_IFLOWCONTROL is not set
+# CONFIG_UART8_OFLOWCONTROL is not set
+CONFIG_SERIAL_IFLOWCONTROL=y
+CONFIG_SERIAL_OFLOWCONTROL=y
+CONFIG_USBDEV=y
+
+#
+# USB Device Controller Driver Options
+#
+# CONFIG_USBDEV_ISOCHRONOUS is not set
+# CONFIG_USBDEV_DUALSPEED is not set
+# CONFIG_USBDEV_SELFPOWERED is not set
+CONFIG_USBDEV_BUSPOWERED=y
+CONFIG_USBDEV_MAXPOWER=500
+# CONFIG_USBDEV_REMOTEWAKEUP is not set
+# CONFIG_USBDEV_DMA is not set
+# CONFIG_USBDEV_TRACE is not set
+
+#
+# USB Device Class Driver Options
+#
+# CONFIG_USBDEV_COMPOSITE is not set
+# CONFIG_PL2303 is not set
+CONFIG_CDCACM=y
+CONFIG_CDCACM_CONSOLE=n
+CONFIG_CDCACM_EP0MAXPACKET=64
+CONFIG_CDCACM_EPINTIN=1
+CONFIG_CDCACM_EPINTIN_FSSIZE=64
+CONFIG_CDCACM_EPINTIN_HSSIZE=64
+CONFIG_CDCACM_EPBULKOUT=3
+CONFIG_CDCACM_EPBULKOUT_FSSIZE=64
+CONFIG_CDCACM_EPBULKOUT_HSSIZE=512
+CONFIG_CDCACM_EPBULKIN=2
+CONFIG_CDCACM_EPBULKIN_FSSIZE=64
+CONFIG_CDCACM_EPBULKIN_HSSIZE=512
+CONFIG_CDCACM_NWRREQS=4
+CONFIG_CDCACM_NRDREQS=4
+CONFIG_CDCACM_BULKIN_REQLEN=96
+CONFIG_CDCACM_RXBUFSIZE=512
+CONFIG_CDCACM_TXBUFSIZE=512
+CONFIG_CDCACM_VENDORID=0x26ac
+CONFIG_CDCACM_PRODUCTID=0x0011
+CONFIG_CDCACM_VENDORSTR="3D Robotics"
+CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v2.x"
+# CONFIG_USBMSC is not set
+# CONFIG_USBHOST is not set
+# CONFIG_WIRELESS is not set
+
+#
+# System Logging Device Options
+#
+
+#
+# System Logging
+#
+# CONFIG_RAMLOG is not set
+
+#
+# Networking Support
+#
+# CONFIG_NET is not set
+
+#
+# File Systems
+#
+
+#
+# File system configuration
+#
+# CONFIG_DISABLE_MOUNTPOINT is not set
+# CONFIG_FS_RAMMAP is not set
+CONFIG_FS_FAT=y
+CONFIG_FAT_LCNAMES=y
+CONFIG_FAT_LFN=y
+CONFIG_FAT_MAXFNAME=32
+CONFIG_FS_FATTIME=y
+# CONFIG_FAT_DMAMEMORY is not set
+CONFIG_FS_NXFFS=y
+CONFIG_NXFFS_PREALLOCATED=y
+CONFIG_NXFFS_ERASEDSTATE=0xff
+CONFIG_NXFFS_PACKTHRESHOLD=32
+CONFIG_NXFFS_MAXNAMLEN=32
+CONFIG_NXFFS_TAILTHRESHOLD=2048
+CONFIG_FS_ROMFS=y
+# CONFIG_FS_SMARTFS is not set
+CONFIG_FS_BINFS=y
+
+#
+# System Logging
+#
+CONFIG_SYSLOG_ENABLE=y
+CONFIG_SYSLOG=y
+CONFIG_SYSLOG_CHAR=y
+CONFIG_SYSLOG_DEVPATH="/dev/ttyS6"
+
+#
+# Graphics Support
+#
+# CONFIG_NX is not set
+
+#
+# Memory Management
+#
+# CONFIG_MM_MULTIHEAP is not set
+# CONFIG_MM_SMALL is not set
+CONFIG_MM_REGIONS=2
+CONFIG_GRAN=y
+CONFIG_GRAN_SINGLE=y
+CONFIG_GRAN_INTR=y
+
+#
+# Audio Support
+#
+# CONFIG_AUDIO is not set
+
+#
+# Binary Formats
+#
+# CONFIG_BINFMT_DISABLE is not set
+# CONFIG_BINFMT_EXEPATH is not set
+# CONFIG_NXFLAT is not set
+# CONFIG_ELF is not set
+CONFIG_BUILTIN=y
+# CONFIG_PIC is not set
+# CONFIG_SYMTAB_ORDEREDBYNAME is not set
+
+#
+# Library Routines
+#
+
+#
+# Standard C Library Options
+#
+CONFIG_STDIO_BUFFER_SIZE=32
+CONFIG_STDIO_LINEBUFFER=y
+CONFIG_NUNGET_CHARS=2
+CONFIG_LIB_HOMEDIR="/"
+# CONFIG_NOPRINTF_FIELDWIDTH is not set
+CONFIG_LIBC_FLOATINGPOINT=y
+CONFIG_LIB_RAND_ORDER=1
+# CONFIG_EOL_IS_CR is not set
+# CONFIG_EOL_IS_LF is not set
+# CONFIG_EOL_IS_BOTH_CRLF is not set
+CONFIG_EOL_IS_EITHER_CRLF=y
+# CONFIG_LIBC_EXECFUNCS is not set
+CONFIG_POSIX_SPAWN_PROXY_STACKSIZE=1024
+CONFIG_TASK_SPAWN_DEFAULT_STACKSIZE=2048
+CONFIG_LIBC_STRERROR=y
+# CONFIG_LIBC_STRERROR_SHORT is not set
+# CONFIG_LIBC_PERROR_STDOUT is not set
+CONFIG_ARCH_LOWPUTC=y
+CONFIG_LIB_SENDFILE_BUFSIZE=512
+# CONFIG_ARCH_ROMGETC is not set
+CONFIG_ARCH_OPTIMIZED_FUNCTIONS=y
+CONFIG_ARCH_MEMCPY=y
+# CONFIG_ARCH_MEMCMP is not set
+# CONFIG_ARCH_MEMMOVE is not set
+# CONFIG_ARCH_MEMSET is not set
+# CONFIG_MEMSET_OPTSPEED is not set
+# CONFIG_ARCH_STRCHR is not set
+# CONFIG_ARCH_STRCMP is not set
+# CONFIG_ARCH_STRCPY is not set
+# CONFIG_ARCH_STRNCPY is not set
+# CONFIG_ARCH_STRLEN is not set
+# CONFIG_ARCH_STRNLEN is not set
+# CONFIG_ARCH_BZERO is not set
+
+#
+# Non-standard Library Support
+#
+CONFIG_SCHED_WORKQUEUE=y
+CONFIG_SCHED_HPWORK=y
+CONFIG_SCHED_WORKPRIORITY=192
+CONFIG_SCHED_WORKPERIOD=5000
+CONFIG_SCHED_WORKSTACKSIZE=2048
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKPERIOD=50000
+CONFIG_SCHED_LPWORKSTACKSIZE=2048
+# CONFIG_LIB_KBDCODEC is not set
+# CONFIG_LIB_SLCDCODEC is not set
+
+#
+# Basic CXX Support
+#
+CONFIG_C99_BOOL8=y
+CONFIG_HAVE_CXX=y
+CONFIG_HAVE_CXXINITIALIZE=y
+# CONFIG_CXX_NEWLONG is not set
+
+#
+# uClibc++ Standard C++ Library
+#
+# CONFIG_UCLIBCXX is not set
+
+#
+# Application Configuration
+#
+
+#
+# Built-In Applications
+#
+CONFIG_BUILTIN_PROXY_STACKSIZE=1024
+
+#
+# Examples
+#
+# CONFIG_EXAMPLES_BUTTONS is not set
+# CONFIG_EXAMPLES_CAN is not set
+CONFIG_EXAMPLES_CDCACM=y
+# CONFIG_EXAMPLES_COMPOSITE is not set
+# CONFIG_EXAMPLES_CXXTEST is not set
+# CONFIG_EXAMPLES_DHCPD is not set
+# CONFIG_EXAMPLES_ELF is not set
+# CONFIG_EXAMPLES_FTPC is not set
+# CONFIG_EXAMPLES_FTPD is not set
+# CONFIG_EXAMPLES_HELLO is not set
+# CONFIG_EXAMPLES_HELLOXX is not set
+# CONFIG_EXAMPLES_JSON is not set
+# CONFIG_EXAMPLES_HIDKBD is not set
+# CONFIG_EXAMPLES_KEYPADTEST is not set
+# CONFIG_EXAMPLES_IGMP is not set
+# CONFIG_EXAMPLES_LCDRW is not set
+# CONFIG_EXAMPLES_MM is not set
+# CONFIG_EXAMPLES_MODBUS is not set
+CONFIG_EXAMPLES_MOUNT=y
+# CONFIG_EXAMPLES_NRF24L01TERM is not set
+CONFIG_EXAMPLES_NSH=y
+# CONFIG_EXAMPLES_NULL is not set
+# CONFIG_EXAMPLES_NX is not set
+# CONFIG_EXAMPLES_NXCONSOLE is not set
+# CONFIG_EXAMPLES_NXFFS is not set
+# CONFIG_EXAMPLES_NXFLAT is not set
+# CONFIG_EXAMPLES_NXHELLO is not set
+# CONFIG_EXAMPLES_NXIMAGE is not set
+# CONFIG_EXAMPLES_NXLINES is not set
+# CONFIG_EXAMPLES_NXTEXT is not set
+# CONFIG_EXAMPLES_OSTEST is not set
+# CONFIG_EXAMPLES_PASHELLO is not set
+# CONFIG_EXAMPLES_PIPE is not set
+# CONFIG_EXAMPLES_POSIXSPAWN is not set
+# CONFIG_EXAMPLES_QENCODER is not set
+# CONFIG_EXAMPLES_RGMP is not set
+# CONFIG_EXAMPLES_ROMFS is not set
+# CONFIG_EXAMPLES_SENDMAIL is not set
+# CONFIG_EXAMPLES_SERLOOP is not set
+# CONFIG_EXAMPLES_SLCD is not set
+# CONFIG_EXAMPLES_SMART_TEST is not set
+# CONFIG_EXAMPLES_SMART is not set
+# CONFIG_EXAMPLES_TCPECHO is not set
+# CONFIG_EXAMPLES_TELNETD is not set
+# CONFIG_EXAMPLES_THTTPD is not set
+# CONFIG_EXAMPLES_TIFF is not set
+# CONFIG_EXAMPLES_TOUCHSCREEN is not set
+# CONFIG_EXAMPLES_UDP is not set
+# CONFIG_EXAMPLES_UIP is not set
+# CONFIG_EXAMPLES_USBSERIAL is not set
+# CONFIG_EXAMPLES_USBMSC is not set
+# CONFIG_EXAMPLES_USBTERM is not set
+# CONFIG_EXAMPLES_WATCHDOG is not set
+
+#
+# Graphics Support
+#
+# CONFIG_TIFF is not set
+
+#
+# Interpreters
+#
+# CONFIG_INTERPRETERS_FICL is not set
+# CONFIG_INTERPRETERS_PCODE is not set
+
+#
+# Network Utilities
+#
+
+#
+# Networking Utilities
+#
+# CONFIG_NETUTILS_CODECS is not set
+# CONFIG_NETUTILS_DHCPC is not set
+# CONFIG_NETUTILS_DHCPD is not set
+# CONFIG_NETUTILS_FTPC is not set
+# CONFIG_NETUTILS_FTPD is not set
+# CONFIG_NETUTILS_JSON is not set
+# CONFIG_NETUTILS_RESOLV is not set
+# CONFIG_NETUTILS_SMTP is not set
+# CONFIG_NETUTILS_TELNETD is not set
+# CONFIG_NETUTILS_TFTPC is not set
+# CONFIG_NETUTILS_THTTPD is not set
+# CONFIG_NETUTILS_UIPLIB is not set
+# CONFIG_NETUTILS_WEBCLIENT is not set
+
+#
+# FreeModBus
+#
+# CONFIG_MODBUS is not set
+
+#
+# NSH Library
+#
+CONFIG_NSH_LIBRARY=y
+CONFIG_NSH_BUILTIN_APPS=y
+
+#
+# Disable Individual commands
+#
+# CONFIG_NSH_DISABLE_CAT is not set
+# CONFIG_NSH_DISABLE_CD is not set
+# CONFIG_NSH_DISABLE_CP is not set
+# CONFIG_NSH_DISABLE_DD is not set
+# CONFIG_NSH_DISABLE_ECHO is not set
+# CONFIG_NSH_DISABLE_EXEC is not set
+# CONFIG_NSH_DISABLE_EXIT is not set
+# CONFIG_NSH_DISABLE_FREE is not set
+# CONFIG_NSH_DISABLE_GET is not set
+# CONFIG_NSH_DISABLE_HELP is not set
+# CONFIG_NSH_DISABLE_HEXDUMP is not set
+# CONFIG_NSH_DISABLE_IFCONFIG is not set
+# CONFIG_NSH_DISABLE_KILL is not set
+# CONFIG_NSH_DISABLE_LOSETUP is not set
+# CONFIG_NSH_DISABLE_LS is not set
+# CONFIG_NSH_DISABLE_MB is not set
+# CONFIG_NSH_DISABLE_MKDIR is not set
+# CONFIG_NSH_DISABLE_MKFATFS is not set
+# CONFIG_NSH_DISABLE_MKFIFO is not set
+# CONFIG_NSH_DISABLE_MKRD is not set
+# CONFIG_NSH_DISABLE_MH is not set
+# CONFIG_NSH_DISABLE_MOUNT is not set
+# CONFIG_NSH_DISABLE_MW is not set
+# CONFIG_NSH_DISABLE_NSFMOUNT is not set
+# CONFIG_NSH_DISABLE_PS is not set
+# CONFIG_NSH_DISABLE_PING is not set
+# CONFIG_NSH_DISABLE_PUT is not set
+# CONFIG_NSH_DISABLE_PWD is not set
+# CONFIG_NSH_DISABLE_RM is not set
+# CONFIG_NSH_DISABLE_RMDIR is not set
+# CONFIG_NSH_DISABLE_SET is not set
+# CONFIG_NSH_DISABLE_SH is not set
+# CONFIG_NSH_DISABLE_SLEEP is not set
+# CONFIG_NSH_DISABLE_TEST is not set
+# CONFIG_NSH_DISABLE_UMOUNT is not set
+# CONFIG_NSH_DISABLE_UNSET is not set
+# CONFIG_NSH_DISABLE_USLEEP is not set
+# CONFIG_NSH_DISABLE_WGET is not set
+# CONFIG_NSH_DISABLE_XD is not set
+
+#
+# Configure Command Options
+#
+# CONFIG_NSH_CMDOPT_DF_H is not set
+CONFIG_NSH_CODECS_BUFSIZE=128
+CONFIG_NSH_FILEIOSIZE=512
+CONFIG_NSH_STRERROR=y
+CONFIG_NSH_LINELEN=128
+CONFIG_NSH_MAXARGUMENTS=12
+CONFIG_NSH_NESTDEPTH=8
+# CONFIG_NSH_DISABLESCRIPT is not set
+# CONFIG_NSH_DISABLEBG is not set
+CONFIG_NSH_ROMFSETC=y
+# CONFIG_NSH_ROMFSRC is not set
+CONFIG_NSH_ROMFSMOUNTPT="/etc"
+CONFIG_NSH_INITSCRIPT="init.d/rcS"
+CONFIG_NSH_ROMFSDEVNO=0
+CONFIG_NSH_ROMFSSECTSIZE=128
+CONFIG_NSH_ARCHROMFS=y
+CONFIG_NSH_FATDEVNO=1
+CONFIG_NSH_FATSECTSIZE=512
+CONFIG_NSH_FATNSECTORS=1024
+CONFIG_NSH_FATMOUNTPT="/tmp"
+CONFIG_NSH_CONSOLE=y
+# CONFIG_NSH_USBCONSOLE is not set
+
+#
+# USB Trace Support
+#
+# CONFIG_NSH_CONDEV is not set
+CONFIG_NSH_ARCHINIT=y
+
+#
+# NxWidgets/NxWM
+#
+
+#
+# System NSH Add-Ons
+#
+
+#
+# Custom Free Memory Command
+#
+# CONFIG_SYSTEM_FREE is not set
+
+#
+# I2C tool
+#
+# CONFIG_SYSTEM_I2CTOOL is not set
+
+#
+# FLASH Program Installation
+#
+# CONFIG_SYSTEM_INSTALL is not set
+
+#
+# FLASH Erase-all Command
+#
+
+#
+# readline()
+#
+CONFIG_SYSTEM_READLINE=y
+CONFIG_READLINE_ECHO=y
+
+#
+# Power Off
+#
+# CONFIG_SYSTEM_POWEROFF is not set
+
+#
+# RAMTRON
+#
+# CONFIG_SYSTEM_RAMTRON is not set
+
+#
+# SD Card
+#
+# CONFIG_SYSTEM_SDCARD is not set
+
+#
+# Sysinfo
+#
+CONFIG_SYSTEM_SYSINFO=y
+
+#
+# USB Monitor
+#
diff --git a/nuttx-configs/px4fmu-v2/nsh/setenv.sh b/nuttx-configs/px4fmu-v2/nsh/setenv.sh
new file mode 100755
index 0000000000000000000000000000000000000000..265520997da2f80bfee0a9a44881216fe012b691
--- /dev/null
+++ b/nuttx-configs/px4fmu-v2/nsh/setenv.sh
@@ -0,0 +1,67 @@
+#!/bin/bash
+# configs/stm3240g-eval/nsh/setenv.sh
+#
+# Copyright (C) 2011 Gregory Nutt. All rights reserved.
+# Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# 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 NuttX 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.
+#
+
+if [ "$_" = "$0" ] ; then
+ echo "You must source this script, not run it!" 1>&2
+ exit 1
+fi
+
+WD=`pwd`
+if [ ! -x "setenv.sh" ]; then
+ echo "This script must be executed from the top-level NuttX build directory"
+ exit 1
+fi
+
+if [ -z "${PATH_ORIG}" ]; then
+ export PATH_ORIG="${PATH}"
+fi
+
+# This the Cygwin path to the location where I installed the RIDE
+# toolchain under windows. You will also have to edit this if you install
+# the RIDE toolchain in any other location
+#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/Raisonance/Ride/arm-gcc/bin"
+
+# This the Cygwin path to the location where I installed the CodeSourcery
+# toolchain under windows. You will also have to edit this if you install
+# the CodeSourcery toolchain in any other location
+export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery G++ Lite/bin"
+
+# This the Cygwin path to the location where I build the buildroot
+# toolchain.
+#export TOOLCHAIN_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin"
+
+# Add the path to the toolchain to the PATH varialble
+export PATH="${TOOLCHAIN_BIN}:/sbin:/usr/sbin:${PATH_ORIG}"
+
+echo "PATH : ${PATH}"
diff --git a/nuttx-configs/px4fmu-v2/scripts/ld.script b/nuttx-configs/px4fmu-v2/scripts/ld.script
new file mode 100644
index 0000000000000000000000000000000000000000..1be39fb87f0ab04f90fc76d4100ecdcef328a962
--- /dev/null
+++ b/nuttx-configs/px4fmu-v2/scripts/ld.script
@@ -0,0 +1,150 @@
+/****************************************************************************
+ * configs/px4fmu/common/ld.script
+ *
+ * Copyright (C) 2011 Gregory Nutt. All rights reserved.
+ * Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX 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.
+ *
+ ****************************************************************************/
+
+/* The STM32F427 has 2048Kb of FLASH beginning at address 0x0800:0000 and
+ * 256Kb of SRAM. SRAM is split up into three blocks:
+ *
+ * 1) 112Kb of SRAM beginning at address 0x2000:0000
+ * 2) 16Kb of SRAM beginning at address 0x2001:c000
+ * 3) 64Kb of SRAM beginning at address 0x2002:0000
+ * 4) 64Kb of TCM SRAM beginning at address 0x1000:0000
+ *
+ * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000
+ * where the code expects to begin execution by jumping to the entry point in
+ * the 0x0800:0000 address range.
+ *
+ * The first 0x4000 of flash is reserved for the bootloader.
+ */
+
+MEMORY
+{
+ flash (rx) : ORIGIN = 0x08004000, LENGTH = 2032K
+ sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K
+ ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K
+}
+
+OUTPUT_ARCH(arm)
+
+ENTRY(__start) /* treat __start as the anchor for dead code stripping */
+EXTERN(_vectors) /* force the vectors to be included in the output */
+
+/*
+ * Ensure that abort() is present in the final object. The exception handling
+ * code pulled in by libgcc.a requires it (and that code cannot be easily avoided).
+ */
+EXTERN(abort)
+
+SECTIONS
+{
+ .text : {
+ _stext = ABSOLUTE(.);
+ *(.vectors)
+ *(.text .text.*)
+ *(.fixup)
+ *(.gnu.warning)
+ *(.rodata .rodata.*)
+ *(.gnu.linkonce.t.*)
+ *(.got)
+ *(.gcc_except_table)
+ *(.gnu.linkonce.r.*)
+ _etext = ABSOLUTE(.);
+
+ /*
+ * This is a hack to make the newlib libm __errno() call
+ * use the NuttX get_errno_ptr() function.
+ */
+ __errno = get_errno_ptr;
+ } > flash
+
+ /*
+ * Init functions (static constructors and the like)
+ */
+ .init_section : {
+ _sinit = ABSOLUTE(.);
+ KEEP(*(.init_array .init_array.*))
+ _einit = ABSOLUTE(.);
+ } > flash
+
+ /*
+ * Construction data for parameters.
+ */
+ __param ALIGN(4): {
+ __param_start = ABSOLUTE(.);
+ KEEP(*(__param*))
+ __param_end = ABSOLUTE(.);
+ } > flash
+
+ .ARM.extab : {
+ *(.ARM.extab*)
+ } > flash
+
+ __exidx_start = ABSOLUTE(.);
+ .ARM.exidx : {
+ *(.ARM.exidx*)
+ } > flash
+ __exidx_end = ABSOLUTE(.);
+
+ _eronly = ABSOLUTE(.);
+
+ .data : {
+ _sdata = ABSOLUTE(.);
+ *(.data .data.*)
+ *(.gnu.linkonce.d.*)
+ CONSTRUCTORS
+ _edata = ABSOLUTE(.);
+ } > sram AT > flash
+
+ .bss : {
+ _sbss = ABSOLUTE(.);
+ *(.bss .bss.*)
+ *(.gnu.linkonce.b.*)
+ *(COMMON)
+ _ebss = ABSOLUTE(.);
+ } > sram
+
+ /* Stabs debugging sections. */
+ .stab 0 : { *(.stab) }
+ .stabstr 0 : { *(.stabstr) }
+ .stab.excl 0 : { *(.stab.excl) }
+ .stab.exclstr 0 : { *(.stab.exclstr) }
+ .stab.index 0 : { *(.stab.index) }
+ .stab.indexstr 0 : { *(.stab.indexstr) }
+ .comment 0 : { *(.comment) }
+ .debug_abbrev 0 : { *(.debug_abbrev) }
+ .debug_info 0 : { *(.debug_info) }
+ .debug_line 0 : { *(.debug_line) }
+ .debug_pubnames 0 : { *(.debug_pubnames) }
+ .debug_aranges 0 : { *(.debug_aranges) }
+}
diff --git a/nuttx-configs/px4fmu-v2/src/Makefile b/nuttx-configs/px4fmu-v2/src/Makefile
new file mode 100644
index 0000000000000000000000000000000000000000..d4276f7fc5e05a6c8d4aaa937d366bcfd07dd1df
--- /dev/null
+++ b/nuttx-configs/px4fmu-v2/src/Makefile
@@ -0,0 +1,84 @@
+############################################################################
+# configs/px4fmu/src/Makefile
+#
+# Copyright (C) 2011 Gregory Nutt. All rights reserved.
+# Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# 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 NuttX 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.
+#
+############################################################################
+
+-include $(TOPDIR)/Make.defs
+
+CFLAGS += -I$(TOPDIR)/sched
+
+ASRCS =
+AOBJS = $(ASRCS:.S=$(OBJEXT))
+
+CSRCS = empty.c
+COBJS = $(CSRCS:.c=$(OBJEXT))
+
+SRCS = $(ASRCS) $(CSRCS)
+OBJS = $(AOBJS) $(COBJS)
+
+ARCH_SRCDIR = $(TOPDIR)/arch/$(CONFIG_ARCH)/src
+ifeq ($(WINTOOL),y)
+ CFLAGS += -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \
+ -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \
+ -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}"
+else
+ CFLAGS += -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m
+endif
+
+all: libboard$(LIBEXT)
+
+$(AOBJS): %$(OBJEXT): %.S
+ $(call ASSEMBLE, $<, $@)
+
+$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c
+ $(call COMPILE, $<, $@)
+
+libboard$(LIBEXT): $(OBJS)
+ $(call ARCHIVE, $@, $(OBJS))
+
+.depend: Makefile $(SRCS)
+ $(Q) $(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
+ $(Q) touch $@
+
+depend: .depend
+
+clean:
+ $(call DELFILE, libboard$(LIBEXT))
+ $(call CLEAN)
+
+distclean: clean
+ $(call DELFILE, Make.dep)
+ $(call DELFILE, .depend)
+
+-include Make.dep
+
diff --git a/nuttx-configs/px4fmu-v2/src/empty.c b/nuttx-configs/px4fmu-v2/src/empty.c
new file mode 100644
index 0000000000000000000000000000000000000000..ace900866c5f3e8da1bf3587ae249133d9b644fa
--- /dev/null
+++ b/nuttx-configs/px4fmu-v2/src/empty.c
@@ -0,0 +1,4 @@
+/*
+ * There are no source files here, but libboard.a can't be empty, so
+ * we have this empty source file to keep it company.
+ */
diff --git a/nuttx-configs/px4io-v1/include/board.h b/nuttx-configs/px4io-v1/include/board.h
index 6503a94cd069c6a1427e8acc238a207c23b3a22b..815079266511b6bebe000c9a34b1ad5ca6ebbd79 100755
--- a/nuttx-configs/px4io-v1/include/board.h
+++ b/nuttx-configs/px4io-v1/include/board.h
@@ -115,22 +115,6 @@
#undef GPIO_USART3_RTS
#define GPIO_USART3_RTS 0xffffffff
-/*
- * High-resolution timer
- */
-#define HRT_TIMER 1 /* use timer1 for the HRT */
-#define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */
-
-/*
- * PPM
- *
- * PPM input is handled by the HRT timer.
- *
- * Pin is PA8, timer 1, channel 1
- */
-#define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 */
-#define GPIO_PPM_IN GPIO_TIM1_CH1IN
-
/************************************************************************************
* Public Data
************************************************************************************/
diff --git a/nuttx-configs/px4io-v2/include/board.h b/nuttx-configs/px4io-v2/include/board.h
new file mode 100755
index 0000000000000000000000000000000000000000..4b9c9063853811032432492c48a3d03a725277d4
--- /dev/null
+++ b/nuttx-configs/px4io-v2/include/board.h
@@ -0,0 +1,149 @@
+/************************************************************************************
+ * configs/px4io/include/board.h
+ * include/arch/board/board.h
+ *
+ * Copyright (C) 2009 Gregory Nutt. All rights reserved.
+ * Author: Gregory Nutt <gnutt@nuttx.org>
+ * Copyright (C) 2012 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 NuttX 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.
+ *
+ ************************************************************************************/
+
+#ifndef __ARCH_BOARD_BOARD_H
+#define __ARCH_BOARD_BOARD_H
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+#ifndef __ASSEMBLY__
+# include <stdint.h>
+#endif
+#include <stm32.h>
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/* Clocking *************************************************************************/
+
+/* On-board crystal frequency is 24MHz (HSE) */
+
+#define STM32_BOARD_XTAL 24000000ul
+
+/* Use the HSE output as the system clock */
+
+#define STM32_SYSCLK_SW RCC_CFGR_SW_HSE
+#define STM32_SYSCLK_SWS RCC_CFGR_SWS_HSE
+#define STM32_SYSCLK_FREQUENCY STM32_BOARD_XTAL
+
+/* AHB clock (HCLK) is SYSCLK (24MHz) */
+
+#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK
+#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY
+#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */
+
+/* APB2 clock (PCLK2) is HCLK (24MHz) */
+
+#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLK
+#define STM32_PCLK2_FREQUENCY STM32_HCLK_FREQUENCY
+#define STM32_APB2_CLKIN (STM32_PCLK2_FREQUENCY) /* Timers 2-4 */
+
+/* APB2 timer 1 will receive PCLK2. */
+
+#define STM32_APB2_TIM1_CLKIN (STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM8_CLKIN (STM32_PCLK2_FREQUENCY)
+
+/* APB1 clock (PCLK1) is HCLK (24MHz) */
+
+#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLK
+#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY)
+
+/* All timers run off PCLK */
+
+#define STM32_APB1_TIM1_CLKIN (STM32_PCLK2_FREQUENCY)
+#define STM32_APB1_TIM2_CLKIN (STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM3_CLKIN (STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM4_CLKIN (STM32_PCLK1_FREQUENCY)
+
+/*
+ * Some of the USART pins are not available; override the GPIO
+ * definitions with an invalid pin configuration.
+ */
+#undef GPIO_USART2_CTS
+#define GPIO_USART2_CTS 0xffffffff
+#undef GPIO_USART2_RTS
+#define GPIO_USART2_RTS 0xffffffff
+#undef GPIO_USART2_CK
+#define GPIO_USART2_CK 0xffffffff
+#undef GPIO_USART3_TX
+#define GPIO_USART3_TX 0xffffffff
+#undef GPIO_USART3_CK
+#define GPIO_USART3_CK 0xffffffff
+#undef GPIO_USART3_CTS
+#define GPIO_USART3_CTS 0xffffffff
+#undef GPIO_USART3_RTS
+#define GPIO_USART3_RTS 0xffffffff
+
+/************************************************************************************
+ * Public Data
+ ************************************************************************************/
+
+#ifndef __ASSEMBLY__
+
+#undef EXTERN
+#if defined(__cplusplus)
+#define EXTERN extern "C"
+extern "C" {
+#else
+#define EXTERN extern
+#endif
+
+/************************************************************************************
+ * Public Function Prototypes
+ ************************************************************************************/
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ * All STM32 architectures must provide the following entry point. This entry point
+ * is called early in the intitialization -- after all memory has been configured
+ * and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+EXTERN void stm32_boardinitialize(void);
+
+#if defined(__cplusplus)
+}
+#endif
+
+#endif /* __ASSEMBLY__ */
+#endif /* __ARCH_BOARD_BOARD_H */
diff --git a/nuttx-configs/px4io-v2/nsh/Make.defs b/nuttx-configs/px4io-v2/nsh/Make.defs
new file mode 100644
index 0000000000000000000000000000000000000000..cd2d8eba3be80b122ff6322434bee77214f98259
--- /dev/null
+++ b/nuttx-configs/px4io-v2/nsh/Make.defs
@@ -0,0 +1,170 @@
+############################################################################
+# configs/px4io-v2/nsh/Make.defs
+#
+# Copyright (C) 2011 Gregory Nutt. All rights reserved.
+# Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# 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 NuttX 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.
+#
+############################################################################
+
+include ${TOPDIR}/.config
+include ${TOPDIR}/tools/Config.mk
+
+#
+# We only support building with the ARM bare-metal toolchain from
+# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS.
+#
+CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI
+
+include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs
+
+CC = $(CROSSDEV)gcc
+CXX = $(CROSSDEV)g++
+CPP = $(CROSSDEV)gcc -E
+LD = $(CROSSDEV)ld
+AR = $(CROSSDEV)ar rcs
+NM = $(CROSSDEV)nm
+OBJCOPY = $(CROSSDEV)objcopy
+OBJDUMP = $(CROSSDEV)objdump
+
+MAXOPTIMIZATION = -O3
+ARCHCPUFLAGS = -mcpu=cortex-m3 \
+ -mthumb \
+ -march=armv7-m
+
+# enable precise stack overflow tracking
+#INSTRUMENTATIONDEFINES = -finstrument-functions \
+# -ffixed-r10
+
+# use our linker script
+LDSCRIPT = ld.script
+
+ifeq ($(WINTOOL),y)
+ # Windows-native toolchains
+ DIRLINK = $(TOPDIR)/tools/copydir.sh
+ DIRUNLINK = $(TOPDIR)/tools/unlink.sh
+ MKDEP = $(TOPDIR)/tools/mknulldeps.sh
+ ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}"
+ ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}"
+ ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}"
+else
+ ifeq ($(PX4_WINTOOL),y)
+ # Windows-native toolchains (MSYS)
+ DIRLINK = $(TOPDIR)/tools/copydir.sh
+ DIRUNLINK = $(TOPDIR)/tools/unlink.sh
+ MKDEP = $(TOPDIR)/tools/mknulldeps.sh
+ ARCHINCLUDES = -I. -isystem $(TOPDIR)/include
+ ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx
+ ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)
+ else
+ # Linux/Cygwin-native toolchain
+ MKDEP = $(TOPDIR)/tools/mkdeps.sh
+ ARCHINCLUDES = -I. -isystem $(TOPDIR)/include
+ ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx
+ ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)
+ endif
+endif
+
+# tool versions
+ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'}
+ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1}
+
+# optimisation flags
+ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \
+ -fno-strict-aliasing \
+ -fno-strength-reduce \
+ -fomit-frame-pointer \
+ -funsafe-math-optimizations \
+ -fno-builtin-printf \
+ -ffunction-sections \
+ -fdata-sections
+
+ifeq ("${CONFIG_DEBUG_SYMBOLS}","y")
+ARCHOPTIMIZATION += -g
+endif
+
+ARCHCFLAGS = -std=gnu99
+ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x
+ARCHWARNINGS = -Wall \
+ -Wextra \
+ -Wdouble-promotion \
+ -Wshadow \
+ -Wfloat-equal \
+ -Wframe-larger-than=1024 \
+ -Wpointer-arith \
+ -Wlogical-op \
+ -Wmissing-declarations \
+ -Wpacked \
+ -Wno-unused-parameter
+# -Wcast-qual - generates spurious noreturn attribute warnings, try again later
+# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code
+# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives
+
+ARCHCWARNINGS = $(ARCHWARNINGS) \
+ -Wbad-function-cast \
+ -Wstrict-prototypes \
+ -Wold-style-declaration \
+ -Wmissing-parameter-type \
+ -Wmissing-prototypes \
+ -Wnested-externs \
+ -Wunsuffixed-float-constants
+ARCHWARNINGSXX = $(ARCHWARNINGS)
+ARCHDEFINES =
+ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10
+
+# this seems to be the only way to add linker flags
+EXTRA_LIBS += --warn-common \
+ --gc-sections
+
+CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common
+CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS)
+CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe
+CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS)
+CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES)
+AFLAGS = $(CFLAGS) -D__ASSEMBLY__
+
+NXFLATLDFLAGS1 = -r -d -warn-common
+NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections
+LDNXFLATFLAGS = -e main -s 2048
+
+OBJEXT = .o
+LIBEXT = .a
+EXEEXT =
+
+# produce partially-linked $1 from files in $2
+define PRELINK
+ @echo "PRELINK: $1"
+ $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1
+endef
+
+HOSTCC = gcc
+HOSTINCLUDES = -I.
+HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe
+HOSTLDFLAGS =
+
diff --git a/nuttx-configs/px4io-v2/nsh/appconfig b/nuttx-configs/px4io-v2/nsh/appconfig
new file mode 100644
index 0000000000000000000000000000000000000000..48a41bcdb8da5b1ebc08209ee9fb7bacb726b732
--- /dev/null
+++ b/nuttx-configs/px4io-v2/nsh/appconfig
@@ -0,0 +1,32 @@
+############################################################################
+#
+# Copyright (C) 2012 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.
+#
+############################################################################
diff --git a/nuttx-configs/px4io-v2/nsh/defconfig b/nuttx-configs/px4io-v2/nsh/defconfig
new file mode 100755
index 0000000000000000000000000000000000000000..4111e70ebaa99989ce4cdd9fde92a63d22f494b5
--- /dev/null
+++ b/nuttx-configs/px4io-v2/nsh/defconfig
@@ -0,0 +1,548 @@
+############################################################################
+# configs/px4io/nsh/defconfig
+#
+# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved.
+# Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# 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 NuttX 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.
+#
+############################################################################
+#
+# architecture selection
+#
+# CONFIG_ARCH - identifies the arch subdirectory and, hence, the
+# processor architecture.
+# CONFIG_ARCH_family - for use in C code. This identifies the
+# particular chip family that the architecture is implemented
+# in.
+# CONFIG_ARCH_architecture - for use in C code. This identifies the
+# specific architecture within the chip family.
+# CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory
+# CONFIG_ARCH_CHIP_name - For use in C code
+# CONFIG_ARCH_BOARD - identifies the configs subdirectory and, hence,
+# the board that supports the particular chip or SoC.
+# CONFIG_ARCH_BOARD_name - for use in C code
+# CONFIG_ENDIAN_BIG - define if big endian (default is little endian)
+# CONFIG_BOARD_LOOPSPERMSEC - for delay loops
+# CONFIG_DRAM_SIZE - Describes the installed DRAM.
+# CONFIG_DRAM_START - The start address of DRAM (physical)
+# CONFIG_ARCH_IRQPRIO - The ST32F100CB supports interrupt prioritization
+# CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt
+# stack. If defined, this symbol is the size of the interrupt
+# stack in bytes. If not defined, the user task stacks will be
+# used during interrupt handling.
+# CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions
+# CONFIG_ARCH_BOOTLOADER - Set if you are using a bootloader.
+# CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture.
+# CONFIG_ARCH_BUTTONS - Enable support for buttons. Unique to board architecture.
+# CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that
+# cause a 100 second delay during boot-up. This 100 second delay
+# serves no purpose other than it allows you to calibrate
+# CONFIG_BOARD_LOOPSPERMSEC. You simply use a stop watch to measure
+# the 100 second delay then adjust CONFIG_BOARD_LOOPSPERMSEC until
+# the delay actually is 100 seconds.
+# CONFIG_ARCH_DMA - Support DMA initialization
+#
+CONFIG_ARCH="arm"
+CONFIG_ARCH_ARM=y
+CONFIG_ARCH_CORTEXM3=y
+CONFIG_ARCH_CHIP="stm32"
+CONFIG_ARCH_CHIP_STM32F100C8=y
+CONFIG_ARCH_BOARD="px4io-v2"
+CONFIG_ARCH_BOARD_PX4IO_V2=y
+CONFIG_BOARD_LOOPSPERMSEC=2000
+CONFIG_DRAM_SIZE=0x00002000
+CONFIG_DRAM_START=0x20000000
+CONFIG_ARCH_IRQPRIO=y
+CONFIG_ARCH_INTERRUPTSTACK=n
+CONFIG_ARCH_STACKDUMP=y
+CONFIG_ARCH_BOOTLOADER=n
+CONFIG_ARCH_LEDS=n
+CONFIG_ARCH_BUTTONS=n
+CONFIG_ARCH_CALIBRATION=n
+CONFIG_ARCH_DMA=y
+CONFIG_ARCH_MATH_H=y
+
+CONFIG_ARMV7M_CMNVECTOR=y
+
+#
+# JTAG Enable settings (by default JTAG-DP and SW-DP are disabled):
+#
+# CONFIG_STM32_DFU - Use the DFU bootloader, not JTAG
+#
+# JTAG Enable options:
+#
+# CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP)
+# CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP)
+# but without JNTRST.
+# CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled
+#
+CONFIG_STM32_DFU=n
+CONFIG_STM32_JTAG_FULL_ENABLE=y
+CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n
+CONFIG_STM32_JTAG_SW_ENABLE=n
+
+#
+# Individual subsystems can be enabled:
+#
+# AHB:
+CONFIG_STM32_DMA1=y
+CONFIG_STM32_DMA2=n
+CONFIG_STM32_CRC=n
+# APB1:
+# Timers 2,3 and 4 are owned by the PWM driver
+CONFIG_STM32_TIM2=n
+CONFIG_STM32_TIM3=n
+CONFIG_STM32_TIM4=n
+CONFIG_STM32_TIM5=n
+CONFIG_STM32_TIM6=n
+CONFIG_STM32_TIM7=n
+CONFIG_STM32_WWDG=n
+CONFIG_STM32_SPI2=n
+CONFIG_STM32_USART2=y
+CONFIG_STM32_USART3=y
+CONFIG_STM32_I2C1=n
+CONFIG_STM32_I2C2=n
+CONFIG_STM32_BKP=n
+CONFIG_STM32_PWR=n
+CONFIG_STM32_DAC=n
+# APB2:
+# We use our own ADC driver, but leave this on for clocking purposes.
+CONFIG_STM32_ADC1=y
+CONFIG_STM32_ADC2=n
+# TIM1 is owned by the HRT
+CONFIG_STM32_TIM1=n
+CONFIG_STM32_SPI1=n
+CONFIG_STM32_TIM8=n
+CONFIG_STM32_USART1=y
+CONFIG_STM32_ADC3=n
+
+
+#
+# STM32F100 specific serial device driver settings
+#
+# CONFIG_USARTn_SERIAL_CONSOLE - selects the USARTn for the
+# console and ttys0 (default is the USART1).
+# CONFIG_USARTn_RXBUFSIZE - Characters are buffered as received.
+# This specific the size of the receive buffer
+# CONFIG_USARTn_TXBUFSIZE - Characters are buffered before
+# being sent. This specific the size of the transmit buffer
+# CONFIG_USARTn_BAUD - The configure BAUD of the UART. Must be
+# CONFIG_USARTn_BITS - The number of bits. Must be either 7 or 8.
+# CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity
+# CONFIG_USARTn_2STOP - Two stop bits
+#
+CONFIG_SERIAL_TERMIOS=y
+CONFIG_STANDARD_SERIAL=y
+
+CONFIG_USART1_SERIAL_CONSOLE=y
+CONFIG_USART2_SERIAL_CONSOLE=n
+CONFIG_USART3_SERIAL_CONSOLE=n
+
+CONFIG_USART1_TXBUFSIZE=64
+CONFIG_USART2_TXBUFSIZE=64
+CONFIG_USART3_TXBUFSIZE=64
+
+CONFIG_USART1_RXBUFSIZE=64
+CONFIG_USART2_RXBUFSIZE=64
+CONFIG_USART3_RXBUFSIZE=64
+
+CONFIG_USART1_BAUD=115200
+CONFIG_USART2_BAUD=115200
+CONFIG_USART3_BAUD=115200
+
+CONFIG_USART1_BITS=8
+CONFIG_USART2_BITS=8
+CONFIG_USART3_BITS=8
+
+CONFIG_USART1_PARITY=0
+CONFIG_USART2_PARITY=0
+CONFIG_USART3_PARITY=0
+
+CONFIG_USART1_2STOP=0
+CONFIG_USART2_2STOP=0
+CONFIG_USART3_2STOP=0
+
+CONFIG_USART1_RXDMA=y
+SERIAL_HAVE_CONSOLE_DMA=y
+# Conflicts with I2C1 DMA
+CONFIG_USART2_RXDMA=n
+CONFIG_USART3_RXDMA=y
+
+#
+# PX4IO specific driver settings
+#
+# CONFIG_HRT_TIMER
+# Enables the high-resolution timer. The board definition must
+# set HRT_TIMER and HRT_TIMER_CHANNEL to the timer and capture/
+# compare channels to be used.
+# CONFIG_HRT_PPM
+# Enables R/C PPM input using the HRT. The board definition must
+# set HRT_PPM_CHANNEL to the timer capture/compare channel to be
+# used, and define GPIO_PPM_IN to configure the appropriate timer
+# GPIO.
+# CONFIG_PWM_SERVO
+# Enables the PWM servo driver. The driver configuration must be
+# supplied by the board support at initialisation time.
+# Note that USART2 must be disabled on the PX4 board for this to
+# be available.
+#
+#
+CONFIG_HRT_TIMER=y
+CONFIG_HRT_PPM=y
+
+#
+# General build options
+#
+# CONFIG_RRLOAD_BINARY - make the rrload binary format used with
+# BSPs from www.ridgerun.com using the tools/mkimage.sh script
+# CONFIG_INTELHEX_BINARY - make the Intel HEX binary format
+# used with many different loaders using the GNU objcopy program
+# Should not be selected if you are not using the GNU toolchain.
+# CONFIG_MOTOROLA_SREC - make the Motorola S-Record binary format
+# used with many different loaders using the GNU objcopy program
+# Should not be selected if you are not using the GNU toolchain.
+# CONFIG_RAW_BINARY - make a raw binary format file used with many
+# different loaders using the GNU objcopy program. This option
+# should not be selected if you are not using the GNU toolchain.
+# CONFIG_HAVE_LIBM - toolchain supports libm.a
+#
+CONFIG_RRLOAD_BINARY=n
+CONFIG_INTELHEX_BINARY=n
+CONFIG_MOTOROLA_SREC=n
+CONFIG_RAW_BINARY=y
+CONFIG_HAVE_LIBM=n
+
+#
+# General OS setup
+#
+# CONFIG_APPS_DIR - Identifies the relative path to the directory
+# that builds the application to link with NuttX. Default: ../apps
+# CONFIG_DEBUG - enables built-in debug options
+# CONFIG_DEBUG_VERBOSE - enables verbose debug output
+# CONFIG_DEBUG_SYMBOLS - build without optimization and with
+# debug symbols (needed for use with a debugger).
+# CONFIG_HAVE_CXX - Enable support for C++
+# CONFIG_HAVE_CXXINITIALIZE - The platform-specific logic includes support
+# for initialization of static C++ instances for this architecture
+# and for the selected toolchain (via up_cxxinitialize()).
+# CONFIG_MM_REGIONS - If the architecture includes multiple
+# regions of memory to allocate from, this specifies the
+# number of memory regions that the memory manager must
+# handle and enables the API mm_addregion(start, end);
+# CONFIG_ARCH_LOWPUTC - architecture supports low-level, boot
+# time console output
+# CONFIG_MSEC_PER_TICK - The default system timer is 100Hz
+# or MSEC_PER_TICK=10. This setting may be defined to
+# inform NuttX that the processor hardware is providing
+# system timer interrupts at some interrupt interval other
+# than 10 msec.
+# CONFIG_RR_INTERVAL - The round robin timeslice will be set
+# this number of milliseconds; Round robin scheduling can
+# be disabled by setting this value to zero.
+# CONFIG_SCHED_INSTRUMENTATION - enables instrumentation in
+# scheduler to monitor system performance
+# CONFIG_TASK_NAME_SIZE - Spcifies that maximum size of a
+# task name to save in the TCB. Useful if scheduler
+# instrumentation is selected. Set to zero to disable.
+# CONFIG_START_YEAR, CONFIG_START_MONTH, CONFIG_START_DAY -
+# Used to initialize the internal time logic.
+# CONFIG_GREGORIAN_TIME - Enables Gregorian time conversions.
+# You would only need this if you are concerned about accurate
+# time conversions in the past or in the distant future.
+# CONFIG_JULIAN_TIME - Enables Julian time conversions. You
+# would only need this if you are concerned about accurate
+# time conversion in the distand past. You must also define
+# CONFIG_GREGORIAN_TIME in order to use Julian time.
+# CONFIG_DEV_CONSOLE - Set if architecture-specific logic
+# provides /dev/console. Enables stdout, stderr, stdin.
+# CONFIG_DEV_LOWCONSOLE - Use the simple, low-level serial console
+# driver (minimul support)
+# CONFIG_MUTEX_TYPES: Set to enable support for recursive and
+# errorcheck mutexes. Enables pthread_mutexattr_settype().
+# CONFIG_PRIORITY_INHERITANCE : Set to enable support for priority
+# inheritance on mutexes and semaphores.
+# CONFIG_SEM_PREALLOCHOLDERS: This setting is only used if priority
+# inheritance is enabled. It defines the maximum number of
+# different threads (minus one) that can take counts on a
+# semaphore with priority inheritance support. This may be
+# set to zero if priority inheritance is disabled OR if you
+# are only using semaphores as mutexes (only one holder) OR
+# if no more than two threads participate using a counting
+# semaphore.
+# CONFIG_SEM_NNESTPRIO. If priority inheritance is enabled,
+# then this setting is the maximum number of higher priority
+# threads (minus 1) than can be waiting for another thread
+# to release a count on a semaphore. This value may be set
+# to zero if no more than one thread is expected to wait for
+# a semaphore.
+# CONFIG_FDCLONE_DISABLE. Disable cloning of all file descriptors
+# by task_create() when a new task is started. If set, all
+# files/drivers will appear to be closed in the new task.
+# CONFIG_FDCLONE_STDIO. Disable cloning of all but the first
+# three file descriptors (stdin, stdout, stderr) by task_create()
+# when a new task is started. If set, all files/drivers will
+# appear to be closed in the new task except for stdin, stdout,
+# and stderr.
+# CONFIG_SDCLONE_DISABLE. Disable cloning of all socket
+# desciptors by task_create() when a new task is started. If
+# set, all sockets will appear to be closed in the new task.
+# CONFIG_SCHED_WORKQUEUE. Create a dedicated "worker" thread to
+# handle delayed processing from interrupt handlers. This feature
+# is required for some drivers but, if there are not complaints,
+# can be safely disabled. The worker thread also performs
+# garbage collection -- completing any delayed memory deallocations
+# from interrupt handlers. If the worker thread is disabled,
+# then that clean will be performed by the IDLE thread instead
+# (which runs at the lowest of priority and may not be appropriate
+# if memory reclamation is of high priority). If CONFIG_SCHED_WORKQUEUE
+# is enabled, then the following options can also be used:
+# CONFIG_SCHED_WORKPRIORITY - The execution priority of the worker
+# thread. Default: 50
+# CONFIG_SCHED_WORKPERIOD - How often the worker thread checks for
+# work in units of microseconds. Default: 50*1000 (50 MS).
+# CONFIG_SCHED_WORKSTACKSIZE - The stack size allocated for the worker
+# thread. Default: CONFIG_IDLETHREAD_STACKSIZE.
+# CONFIG_SIG_SIGWORK - The signal number that will be used to wake-up
+# the worker thread. Default: 4
+# CONFIG_SCHED_WAITPID - Enable the waitpid() API
+# CONFIG_SCHED_ATEXIT - Enabled the atexit() API
+#
+CONFIG_USER_ENTRYPOINT="user_start"
+#CONFIG_APPS_DIR=
+CONFIG_DEBUG=n
+CONFIG_DEBUG_VERBOSE=n
+CONFIG_DEBUG_SYMBOLS=y
+CONFIG_DEBUG_FS=n
+CONFIG_DEBUG_GRAPHICS=n
+CONFIG_DEBUG_LCD=n
+CONFIG_DEBUG_USB=n
+CONFIG_DEBUG_NET=n
+CONFIG_DEBUG_RTC=n
+CONFIG_DEBUG_ANALOG=n
+CONFIG_DEBUG_PWM=n
+CONFIG_DEBUG_CAN=n
+CONFIG_DEBUG_I2C=n
+CONFIG_DEBUG_INPUT=n
+
+CONFIG_MSEC_PER_TICK=1
+CONFIG_HAVE_CXX=y
+CONFIG_HAVE_CXXINITIALIZE=y
+CONFIG_MM_REGIONS=1
+CONFIG_MM_SMALL=y
+CONFIG_ARCH_LOWPUTC=y
+CONFIG_RR_INTERVAL=0
+CONFIG_SCHED_INSTRUMENTATION=n
+CONFIG_TASK_NAME_SIZE=8
+CONFIG_START_YEAR=1970
+CONFIG_START_MONTH=1
+CONFIG_START_DAY=1
+CONFIG_GREGORIAN_TIME=n
+CONFIG_JULIAN_TIME=n
+# this eats ~1KiB of RAM ... work out why
+CONFIG_DEV_CONSOLE=y
+CONFIG_DEV_LOWCONSOLE=n
+CONFIG_MUTEX_TYPES=n
+CONFIG_PRIORITY_INHERITANCE=n
+CONFIG_SEM_PREALLOCHOLDERS=0
+CONFIG_SEM_NNESTPRIO=0
+CONFIG_FDCLONE_DISABLE=y
+CONFIG_FDCLONE_STDIO=y
+CONFIG_SDCLONE_DISABLE=y
+CONFIG_SCHED_WORKQUEUE=n
+CONFIG_SCHED_WORKPRIORITY=50
+CONFIG_SCHED_WORKPERIOD=50000
+CONFIG_SCHED_WORKSTACKSIZE=1024
+CONFIG_SIG_SIGWORK=4
+CONFIG_SCHED_WAITPID=n
+CONFIG_SCHED_ATEXIT=n
+
+#
+# The following can be used to disable categories of
+# APIs supported by the OS. If the compiler supports
+# weak functions, then it should not be necessary to
+# disable functions unless you want to restrict usage
+# of those APIs.
+#
+# There are certain dependency relationships in these
+# features.
+#
+# o mq_notify logic depends on signals to awaken tasks
+# waiting for queues to become full or empty.
+# o pthread_condtimedwait() depends on signals to wake
+# up waiting tasks.
+#
+CONFIG_DISABLE_CLOCK=n
+CONFIG_DISABLE_POSIX_TIMERS=y
+CONFIG_DISABLE_PTHREAD=y
+CONFIG_DISABLE_SIGNALS=y
+CONFIG_DISABLE_MQUEUE=y
+CONFIG_DISABLE_MOUNTPOINT=y
+CONFIG_DISABLE_ENVIRON=y
+CONFIG_DISABLE_POLL=y
+
+#
+# Misc libc settings
+#
+# CONFIG_NOPRINTF_FIELDWIDTH - sprintf-related logic is a
+# little smaller if we do not support fieldwidthes
+#
+CONFIG_NOPRINTF_FIELDWIDTH=n
+
+#
+# Allow for architecture optimized implementations
+#
+# The architecture can provide optimized versions of the
+# following to improve system performance
+#
+CONFIG_ARCH_MEMCPY=n
+CONFIG_ARCH_MEMCMP=n
+CONFIG_ARCH_MEMMOVE=n
+CONFIG_ARCH_MEMSET=n
+CONFIG_ARCH_STRCMP=n
+CONFIG_ARCH_STRCPY=n
+CONFIG_ARCH_STRNCPY=n
+CONFIG_ARCH_STRLEN=n
+CONFIG_ARCH_STRNLEN=n
+CONFIG_ARCH_BZERO=n
+
+#
+# Sizes of configurable things (0 disables)
+#
+# CONFIG_MAX_TASKS - The maximum number of simultaneously
+# active tasks. This value must be a power of two.
+# CONFIG_MAX_TASK_ARGS - This controls the maximum number of
+# of parameters that a task may receive (i.e., maxmum value
+# of 'argc')
+# CONFIG_NPTHREAD_KEYS - The number of items of thread-
+# specific data that can be retained
+# CONFIG_NFILE_DESCRIPTORS - The maximum number of file
+# descriptors (one for each open)
+# CONFIG_NFILE_STREAMS - The maximum number of streams that
+# can be fopen'ed
+# CONFIG_NAME_MAX - The maximum size of a file name.
+# CONFIG_STDIO_BUFFER_SIZE - Size of the buffer to allocate
+# on fopen. (Only if CONFIG_NFILE_STREAMS > 0)
+# CONFIG_STDIO_LINEBUFFER - If standard C buffered I/O is enabled
+# (CONFIG_STDIO_BUFFER_SIZE > 0), then this option may be added
+# to force automatic, line-oriented flushing the output buffer
+# for putc(), fputc(), putchar(), puts(), fputs(), printf(),
+# fprintf(), and vfprintf(). When a newline is encountered in
+# the output string, the output buffer will be flushed. This
+# (slightly) increases the NuttX footprint but supports the kind
+# of behavior that people expect for printf().
+# CONFIG_NUNGET_CHARS - Number of characters that can be
+# buffered by ungetc() (Only if CONFIG_NFILE_STREAMS > 0)
+# CONFIG_PREALLOC_MQ_MSGS - The number of pre-allocated message
+# structures. The system manages a pool of preallocated
+# message structures to minimize dynamic allocations
+# CONFIG_MQ_MAXMSGSIZE - Message structures are allocated with
+# a fixed payload size given by this settin (does not include
+# other message structure overhead.
+# CONFIG_MAX_WDOGPARMS - Maximum number of parameters that
+# can be passed to a watchdog handler
+# CONFIG_PREALLOC_WDOGS - The number of pre-allocated watchdog
+# structures. The system manages a pool of preallocated
+# watchdog structures to minimize dynamic allocations
+# CONFIG_PREALLOC_TIMERS - The number of pre-allocated POSIX
+# timer structures. The system manages a pool of preallocated
+# timer structures to minimize dynamic allocations. Set to
+# zero for all dynamic allocations.
+#
+CONFIG_MAX_TASKS=4
+CONFIG_MAX_TASK_ARGS=4
+CONFIG_NPTHREAD_KEYS=2
+CONFIG_NFILE_DESCRIPTORS=8
+CONFIG_NFILE_STREAMS=0
+CONFIG_NAME_MAX=12
+CONFIG_STDIO_BUFFER_SIZE=32
+CONFIG_STDIO_LINEBUFFER=n
+CONFIG_NUNGET_CHARS=2
+CONFIG_PREALLOC_MQ_MSGS=4
+CONFIG_MQ_MAXMSGSIZE=32
+CONFIG_MAX_WDOGPARMS=2
+CONFIG_PREALLOC_WDOGS=4
+CONFIG_PREALLOC_TIMERS=0
+
+
+#
+# Settings for apps/nshlib
+#
+# CONFIG_NSH_BUILTIN_APPS - Support external registered,
+# "named" applications that can be executed from the NSH
+# command line (see apps/README.txt for more information).
+# CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer
+# CONFIG_NSH_STRERROR - Use strerror(errno)
+# CONFIG_NSH_LINELEN - Maximum length of one command line
+# CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi
+# CONFIG_NSH_DISABLESCRIPT - Disable scripting support
+# CONFIG_NSH_DISABLEBG - Disable background commands
+# CONFIG_NSH_ROMFSETC - Use startup script in /etc
+# CONFIG_NSH_CONSOLE - Use serial console front end
+# CONFIG_NSH_TELNET - Use telnetd console front end
+# CONFIG_NSH_ARCHINIT - Platform provides architecture
+# specific initialization (nsh_archinitialize()).
+#
+
+# Disable NSH completely
+CONFIG_NSH_CONSOLE=n
+
+#
+# Stack and heap information
+#
+# CONFIG_BOOT_RUNFROMFLASH - Some configurations support XIP
+# operation from FLASH but must copy initialized .data sections to RAM.
+# (should also be =n for the STM3210E-EVAL which always runs from flash)
+# CONFIG_BOOT_COPYTORAM - Some configurations boot in FLASH
+# but copy themselves entirely into RAM for better performance.
+# CONFIG_CUSTOM_STACK - The up_ implementation will handle
+# all stack operations outside of the nuttx model.
+# CONFIG_STACK_POINTER - The initial stack pointer (arm7tdmi only)
+# CONFIG_IDLETHREAD_STACKSIZE - The size of the initial stack.
+# This is the thread that (1) performs the inital boot of the system up
+# to the point where user_start() is spawned, and (2) there after is the
+# IDLE thread that executes only when there is no other thread ready to
+# run.
+# CONFIG_USERMAIN_STACKSIZE - The size of the stack to allocate
+# for the main user thread that begins at the user_start() entry point.
+# CONFIG_PTHREAD_STACK_MIN - Minimum pthread stack size
+# CONFIG_PTHREAD_STACK_DEFAULT - Default pthread stack size
+# CONFIG_HEAP_BASE - The beginning of the heap
+# CONFIG_HEAP_SIZE - The size of the heap
+#
+CONFIG_BOOT_RUNFROMFLASH=n
+CONFIG_BOOT_COPYTORAM=n
+CONFIG_CUSTOM_STACK=n
+CONFIG_STACK_POINTER=
+CONFIG_IDLETHREAD_STACKSIZE=1024
+CONFIG_USERMAIN_STACKSIZE=1200
+CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_PTHREAD_STACK_DEFAULT=1024
+CONFIG_HEAP_BASE=
+CONFIG_HEAP_SIZE=
diff --git a/nuttx-configs/px4io-v2/nsh/setenv.sh b/nuttx-configs/px4io-v2/nsh/setenv.sh
new file mode 100755
index 0000000000000000000000000000000000000000..ff9a4bf8ae42600e91039bb3af1affa6950d5fc9
--- /dev/null
+++ b/nuttx-configs/px4io-v2/nsh/setenv.sh
@@ -0,0 +1,47 @@
+#!/bin/bash
+# configs/stm3210e-eval/dfu/setenv.sh
+#
+# Copyright (C) 2009 Gregory Nutt. All rights reserved.
+# Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# 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 NuttX 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.
+#
+
+if [ "$(basename $0)" = "setenv.sh" ] ; then
+ echo "You must source this script, not run it!" 1>&2
+ exit 1
+fi
+
+if [ -z "${PATH_ORIG}" ]; then export PATH_ORIG="${PATH}"; fi
+
+WD=`pwd`
+export RIDE_BIN="/cygdrive/c/Program Files/Raisonance/Ride/arm-gcc/bin"
+export BUILDROOT_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin"
+export PATH="${BUILDROOT_BIN}:${RIDE_BIN}:/sbin:/usr/sbin:${PATH_ORIG}"
+
+echo "PATH : ${PATH}"
diff --git a/nuttx-configs/px4io-v2/scripts/ld.script b/nuttx-configs/px4io-v2/scripts/ld.script
new file mode 100755
index 0000000000000000000000000000000000000000..69c2f9cb2ed63a2a7552a0f613ef81780def3191
--- /dev/null
+++ b/nuttx-configs/px4io-v2/scripts/ld.script
@@ -0,0 +1,129 @@
+/****************************************************************************
+ * configs/stm3210e-eval/nsh/ld.script
+ *
+ * Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved.
+ * Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+ *
+ * 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 NuttX 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.
+ *
+ ****************************************************************************/
+
+/* The STM32F100C8 has 64Kb of FLASH beginning at address 0x0800:0000 and
+ * 8Kb of SRAM beginning at address 0x2000:0000. When booting from FLASH,
+ * FLASH memory is aliased to address 0x0000:0000 where the code expects to
+ * begin execution by jumping to the entry point in the 0x0800:0000 address
+ * range.
+ */
+
+MEMORY
+{
+ flash (rx) : ORIGIN = 0x08001000, LENGTH = 60K
+ sram (rwx) : ORIGIN = 0x20000000, LENGTH = 8K
+}
+
+OUTPUT_ARCH(arm)
+ENTRY(__start) /* treat __start as the anchor for dead code stripping */
+EXTERN(_vectors) /* force the vectors to be included in the output */
+
+/*
+ * Ensure that abort() is present in the final object. The exception handling
+ * code pulled in by libgcc.a requires it (and that code cannot be easily avoided).
+ */
+EXTERN(abort)
+
+SECTIONS
+{
+ .text : {
+ _stext = ABSOLUTE(.);
+ *(.vectors)
+ *(.text .text.*)
+ *(.fixup)
+ *(.gnu.warning)
+ *(.rodata .rodata.*)
+ *(.gnu.linkonce.t.*)
+ *(.glue_7)
+ *(.glue_7t)
+ *(.got)
+ *(.gcc_except_table)
+ *(.gnu.linkonce.r.*)
+ _etext = ABSOLUTE(.);
+ } > flash
+
+ /*
+ * Init functions (static constructors and the like)
+ */
+ .init_section : {
+ _sinit = ABSOLUTE(.);
+ KEEP(*(.init_array .init_array.*))
+ _einit = ABSOLUTE(.);
+ } > flash
+
+ .ARM.extab : {
+ *(.ARM.extab*)
+ } > flash
+
+ __exidx_start = ABSOLUTE(.);
+ .ARM.exidx : {
+ *(.ARM.exidx*)
+ } > flash
+ __exidx_end = ABSOLUTE(.);
+
+ _eronly = ABSOLUTE(.);
+
+ /* The STM32F100CB has 8Kb of SRAM beginning at the following address */
+
+ .data : {
+ _sdata = ABSOLUTE(.);
+ *(.data .data.*)
+ *(.gnu.linkonce.d.*)
+ CONSTRUCTORS
+ _edata = ABSOLUTE(.);
+ } > sram AT > flash
+
+ .bss : {
+ _sbss = ABSOLUTE(.);
+ *(.bss .bss.*)
+ *(.gnu.linkonce.b.*)
+ *(COMMON)
+ _ebss = ABSOLUTE(.);
+ } > sram
+
+ /* Stabs debugging sections. */
+ .stab 0 : { *(.stab) }
+ .stabstr 0 : { *(.stabstr) }
+ .stab.excl 0 : { *(.stab.excl) }
+ .stab.exclstr 0 : { *(.stab.exclstr) }
+ .stab.index 0 : { *(.stab.index) }
+ .stab.indexstr 0 : { *(.stab.indexstr) }
+ .comment 0 : { *(.comment) }
+ .debug_abbrev 0 : { *(.debug_abbrev) }
+ .debug_info 0 : { *(.debug_info) }
+ .debug_line 0 : { *(.debug_line) }
+ .debug_pubnames 0 : { *(.debug_pubnames) }
+ .debug_aranges 0 : { *(.debug_aranges) }
+}
diff --git a/nuttx-configs/px4io-v2/src/Makefile b/nuttx-configs/px4io-v2/src/Makefile
new file mode 100644
index 0000000000000000000000000000000000000000..bb9539d16a756425ddf2a637a2e4fecb70e9b5a3
--- /dev/null
+++ b/nuttx-configs/px4io-v2/src/Makefile
@@ -0,0 +1,84 @@
+############################################################################
+# configs/stm3210e-eval/src/Makefile
+#
+# Copyright (C) 2009-2010 Gregory Nutt. All rights reserved.
+# Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+#
+# 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 NuttX 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.
+#
+############################################################################
+
+-include $(TOPDIR)/Make.defs
+
+CFLAGS += -I$(TOPDIR)/sched
+
+ASRCS =
+AOBJS = $(ASRCS:.S=$(OBJEXT))
+
+CSRCS = empty.c
+
+COBJS = $(CSRCS:.c=$(OBJEXT))
+
+SRCS = $(ASRCS) $(CSRCS)
+OBJS = $(AOBJS) $(COBJS)
+
+ARCH_SRCDIR = $(TOPDIR)/arch/$(CONFIG_ARCH)/src
+ifeq ($(WINTOOL),y)
+ CFLAGS += -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \
+ -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \
+ -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}"
+else
+ CFLAGS += -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m
+endif
+
+all: libboard$(LIBEXT)
+
+$(AOBJS): %$(OBJEXT): %.S
+ $(call ASSEMBLE, $<, $@)
+
+$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c
+ $(call COMPILE, $<, $@)
+
+libboard$(LIBEXT): $(OBJS)
+ $(call ARCHIVE, $@, $(OBJS))
+
+.depend: Makefile $(SRCS)
+ @$(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
+ @touch $@
+
+depend: .depend
+
+clean:
+ $(call DELFILE, libboard$(LIBEXT))
+ $(call CLEAN)
+
+distclean: clean
+ $(call DELFILE, Make.dep)
+ $(call DELFILE, .depend)
+
+-include Make.dep
diff --git a/nuttx-configs/px4io-v2/src/empty.c b/nuttx-configs/px4io-v2/src/empty.c
new file mode 100644
index 0000000000000000000000000000000000000000..ace900866c5f3e8da1bf3587ae249133d9b644fa
--- /dev/null
+++ b/nuttx-configs/px4io-v2/src/empty.c
@@ -0,0 +1,4 @@
+/*
+ * There are no source files here, but libboard.a can't be empty, so
+ * we have this empty source file to keep it company.
+ */
diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp
index 5a8157debccc95f0a750dc180d9ea78036e7df8b..277d8249a3dcf952e98959833f71f6e109d42afd 100644
--- a/src/drivers/airspeed/airspeed.cpp
+++ b/src/drivers/airspeed/airspeed.cpp
@@ -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
diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c
index 735bdb41a5d0bb8991f9fe99ecfa62934c25de94..b88f61ce80a1ae33f0ff61945bfe4abfc242e321 100644
--- a/src/drivers/ardrone_interface/ardrone_interface.c
+++ b/src/drivers/ardrone_interface/ardrone_interface.c
@@ -53,8 +53,10 @@
#include <sys/prctl.h>
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/safety.h>
#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/vehicle_control_mode.h>
#include <systemlib/systemlib.h>
@@ -243,16 +245,14 @@ int ardrone_interface_thread_main(int argc, char *argv[])
int led_counter = 0;
/* declare and safely initialize all structs */
- struct vehicle_status_s state;
- memset(&state, 0, sizeof(state));
struct actuator_controls_s actuator_controls;
memset(&actuator_controls, 0, sizeof(actuator_controls));
struct actuator_armed_s armed;
+ //XXX is this necessairy?
armed.armed = false;
/* subscribe to attitude, motor setpoints and system state */
int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
- int state_sub = orb_subscribe(ORB_ID(vehicle_status));
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
printf("[ardrone_interface] Motors initialized - ready.\n");
@@ -322,8 +322,6 @@ int ardrone_interface_thread_main(int argc, char *argv[])
} else {
/* MAIN OPERATION MODE */
- /* get a local copy of the vehicle state */
- orb_copy(ORB_ID(vehicle_status), state_sub, &state);
/* get a local copy of the actuator controls */
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp
index 3fabfd9a54e4c98ad4a9614fa200f7f4522bb223..490002254db4ecdaa8238200b49a411c85fbe04c 100644
--- a/src/drivers/blinkm/blinkm.cpp
+++ b/src/drivers/blinkm/blinkm.cpp
@@ -92,9 +92,6 @@
#include <nuttx/config.h>
-#include <arch/board/board.h>
-#include <drivers/device/i2c.h>
-
#include <sys/types.h>
#include <stdint.h>
#include <string.h>
@@ -104,18 +101,23 @@
#include <unistd.h>
#include <stdio.h>
#include <ctype.h>
-
-#include <drivers/drv_blinkm.h>
+#include <poll.h>
#include <nuttx/wqueue.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
-
#include <systemlib/systemlib.h>
-#include <poll.h>
+
+#include <board_config.h>
+
+#include <drivers/device/i2c.h>
+#include <drivers/drv_blinkm.h>
+
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/vehicle_gps_position.h>
static const float MAX_CELL_VOLTAGE = 4.3f;
@@ -375,7 +377,9 @@ BlinkM::led()
{
static int vehicle_status_sub_fd;
+ static int vehicle_control_mode_sub_fd;
static int vehicle_gps_position_sub_fd;
+ static int actuator_armed_sub_fd;
static int num_of_cells = 0;
static int detected_cells_runcount = 0;
@@ -386,6 +390,8 @@ BlinkM::led()
static int led_interval = 1000;
static int no_data_vehicle_status = 0;
+ static int no_data_vehicle_control_mode = 0;
+ static int no_data_actuator_armed = 0;
static int no_data_vehicle_gps_position = 0;
static bool topic_initialized = false;
@@ -398,6 +404,12 @@ BlinkM::led()
vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status));
orb_set_interval(vehicle_status_sub_fd, 1000);
+ vehicle_control_mode_sub_fd = orb_subscribe(ORB_ID(vehicle_control_mode));
+ orb_set_interval(vehicle_control_mode_sub_fd, 1000);
+
+ actuator_armed_sub_fd = orb_subscribe(ORB_ID(actuator_armed));
+ orb_set_interval(actuator_armed_sub_fd, 1000);
+
vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position));
orb_set_interval(vehicle_gps_position_sub_fd, 1000);
@@ -452,12 +464,16 @@ BlinkM::led()
if (led_thread_runcount == 15) {
/* obtained data for the first file descriptor */
struct vehicle_status_s vehicle_status_raw;
+ struct vehicle_control_mode_s vehicle_control_mode;
+ struct actuator_armed_s actuator_armed;
struct vehicle_gps_position_s vehicle_gps_position_raw;
memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw));
memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw));
bool new_data_vehicle_status;
+ bool new_data_vehicle_control_mode;
+ bool new_data_actuator_armed;
bool new_data_vehicle_gps_position;
orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);
@@ -471,6 +487,28 @@ BlinkM::led()
no_data_vehicle_status = 3;
}
+ orb_check(vehicle_control_mode_sub_fd, &new_data_vehicle_control_mode);
+
+ if (new_data_vehicle_control_mode) {
+ orb_copy(ORB_ID(vehicle_control_mode), vehicle_control_mode_sub_fd, &vehicle_control_mode);
+ no_data_vehicle_control_mode = 0;
+ } else {
+ no_data_vehicle_control_mode++;
+ if(no_data_vehicle_control_mode >= 3)
+ no_data_vehicle_control_mode = 3;
+ }
+
+ orb_check(actuator_armed_sub_fd, &new_data_actuator_armed);
+
+ if (new_data_actuator_armed) {
+ orb_copy(ORB_ID(actuator_armed), actuator_armed_sub_fd, &actuator_armed);
+ no_data_actuator_armed = 0;
+ } else {
+ no_data_actuator_armed++;
+ if(no_data_actuator_armed >= 3)
+ no_data_actuator_armed = 3;
+ }
+
orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position);
if (new_data_vehicle_gps_position) {
@@ -486,19 +524,19 @@ BlinkM::led()
/* get number of used satellites in navigation */
num_of_used_sats = 0;
- //for(int satloop=0; satloop<20; satloop++) {
- for(int satloop=0; satloop<sizeof(vehicle_gps_position_raw.satellite_used); satloop++) {
+
+ for(unsigned satloop=0; satloop<sizeof(vehicle_gps_position_raw.satellite_used); satloop++) {
if(vehicle_gps_position_raw.satellite_used[satloop] == 1) {
num_of_used_sats++;
}
}
- if(new_data_vehicle_status || no_data_vehicle_status < 3){
- if(num_of_cells == 0) {
+ if (new_data_vehicle_status || no_data_vehicle_status < 3) {
+ if (num_of_cells == 0) {
/* looking for lipo cells that are connected */
printf("<blinkm> checking cells\n");
for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) {
- if(vehicle_status_raw.voltage_battery < num_of_cells * MAX_CELL_VOLTAGE) break;
+ if(vehicle_status_raw.battery_voltage < num_of_cells * MAX_CELL_VOLTAGE) break;
}
printf("<blinkm> cells found:%d\n", num_of_cells);
@@ -530,7 +568,7 @@ BlinkM::led()
} else {
/* no battery warnings here */
- if(vehicle_status_raw.flag_system_armed == false) {
+ if(actuator_armed.armed == false) {
/* system not armed */
led_color_1 = LED_RED;
led_color_2 = LED_RED;
@@ -554,27 +592,24 @@ BlinkM::led()
led_color_8 = LED_OFF;
led_blink = LED_BLINK;
- /* handle 4th led - flightmode indicator */
- switch((int)vehicle_status_raw.flight_mode) {
- case VEHICLE_FLIGHT_MODE_MANUAL:
- led_color_4 = LED_AMBER;
- break;
-
- case VEHICLE_FLIGHT_MODE_STAB:
- led_color_4 = LED_YELLOW;
- break;
-
- case VEHICLE_FLIGHT_MODE_HOLD:
- led_color_4 = LED_BLUE;
- break;
+ if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) {
- case VEHICLE_FLIGHT_MODE_AUTO:
+ //XXX please check
+ if (vehicle_control_mode.flag_control_position_enabled)
led_color_4 = LED_GREEN;
- break;
+ else if (vehicle_control_mode.flag_control_velocity_enabled)
+ led_color_4 = LED_BLUE;
+ else if (vehicle_control_mode.flag_control_attitude_enabled)
+ led_color_4 = LED_YELLOW;
+ else if (vehicle_control_mode.flag_control_manual_enabled)
+ led_color_4 = LED_AMBER;
+ else
+ led_color_4 = LED_OFF;
+
}
if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) {
- /* handling used sat´s */
+ /* handling used sat�s */
if(num_of_used_sats >= 7) {
led_color_1 = LED_OFF;
led_color_2 = LED_OFF;
@@ -831,11 +866,13 @@ BlinkM::get_firmware_version(uint8_t version[2])
return transfer(&msg, sizeof(msg), version, 2);
}
+void blinkm_usage();
+
void blinkm_usage() {
- fprintf(stderr, "missing command: try 'start', 'systemstate', 'ledoff', 'list' or a script name {options}\n");
- fprintf(stderr, "options:\n");
- fprintf(stderr, "\t-b --bus i2cbus (3)\n");
- fprintf(stderr, "\t-a --blinkmaddr blinkmaddr (9)\n");
+ warnx("missing command: try 'start', 'systemstate', 'ledoff', 'list' or a script name {options}");
+ warnx("options:");
+ warnx("\t-b --bus i2cbus (3)");
+ warnx("\t-a --blinkmaddr blinkmaddr (9)");
}
int
diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp
index 4409a8a9cbcb72193c00eb860a63abb6514bfbd1..cfb6256704e429267f3f1e3c960f36f5d695e713 100644
--- a/src/drivers/bma180/bma180.cpp
+++ b/src/drivers/bma180/bma180.cpp
@@ -59,7 +59,7 @@
#include <nuttx/clock.h>
#include <drivers/drv_hrt.h>
-#include <arch/board/board.h>
+#include <board_config.h>
#include <drivers/device/spi.h>
#include <drivers/drv_accel.h>
diff --git a/src/drivers/boards/px4fmu/px4fmu_internal.h b/src/drivers/boards/px4fmu-v1/board_config.h
similarity index 77%
rename from src/drivers/boards/px4fmu/px4fmu_internal.h
rename to src/drivers/boards/px4fmu-v1/board_config.h
index 383a046ffdb76d9860219f472c354444022583ae..27621211a16ba9a9d031795fbdad721d40e24dc5 100644
--- a/src/drivers/boards/px4fmu/px4fmu_internal.h
+++ b/src/drivers/boards/px4fmu-v1/board_config.h
@@ -32,7 +32,7 @@
****************************************************************************/
/**
- * @file px4fmu_internal.h
+ * @file board_config.h
*
* PX4FMU internal definitions
*/
@@ -51,13 +51,16 @@ __BEGIN_DECLS
/* these headers are not C++ safe */
#include <stm32.h>
-
+#include <arch/board/board.h>
/****************************************************************************************************
* Definitions
****************************************************************************************************/
/* Configuration ************************************************************************************/
+/* PX4IO connection configuration */
+#define PX4IO_SERIAL_DEVICE "/dev/ttyS2"
+
//#ifdef CONFIG_STM32_SPI2
//# error "SPI2 is not supported on this board"
//#endif
@@ -74,15 +77,47 @@ __BEGIN_DECLS
/* External interrupts */
#define GPIO_EXTI_COMPASS (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN1)
-// XXX MPU6000 DRDY?
/* SPI chip selects */
-
#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14)
#define GPIO_SPI_CS_ACCEL (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
#define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
#define GPIO_SPI_CS_SDCARD (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4)
+/*
+ * Use these in place of the spi_dev_e enumeration to
+ * select a specific SPI device on SPI1
+ */
+#define PX4_SPIDEV_GYRO 1
+#define PX4_SPIDEV_ACCEL 2
+#define PX4_SPIDEV_MPU 3
+
+/*
+ * Optional devices on IO's external port
+ */
+#define PX4_SPIDEV_ACCEL_MAG 2
+
+/*
+ * I2C busses
+ */
+#define PX4_I2C_BUS_ESC 1
+#define PX4_I2C_BUS_ONBOARD 2
+#define PX4_I2C_BUS_EXPANSION 3
+#define PX4_I2C_BUS_LED 3
+
+/*
+ * Devices on the onboard bus.
+ *
+ * Note that these are unshifted addresses.
+ */
+#define PX4_I2C_OBDEV_HMC5883 0x1e
+#define PX4_I2C_OBDEV_MS5611 0x76
+#define PX4_I2C_OBDEV_EEPROM NOTDEFINED
+#define PX4_I2C_OBDEV_LED 0x55
+
+#define PX4_I2C_OBDEV_PX4IO_BL 0x18
+#define PX4_I2C_OBDEV_PX4IO 0x1a
+
/* User GPIOs
*
* GPIO0-1 are the buffered high-power GPIOs.
@@ -107,31 +142,45 @@ __BEGIN_DECLS
#define GPIO_GPIO7_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN12)
#define GPIO_GPIO_DIR (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)
+/*
+ * Tone alarm output
+ */
+#define TONE_ALARM_TIMER 3 /* timer 3 */
+#define TONE_ALARM_CHANNEL 3 /* channel 3 */
+#define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN8)
+#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF2|GPIO_SPEED_2MHz|GPIO_FLOAT|GPIO_PUSHPULL|GPIO_PORTC|GPIO_PIN8)
+
+/*
+ * PWM
+ *
+ * Four PWM outputs can be configured on pins otherwise shared with
+ * USART2; two can take the flow control pins if they are not being used.
+ *
+ * Pins:
+ *
+ * CTS - PA0 - TIM2CH1
+ * RTS - PA1 - TIM2CH2
+ * TX - PA2 - TIM2CH3
+ * RX - PA3 - TIM2CH4
+ *
+ */
+#define GPIO_TIM2_CH1OUT GPIO_TIM2_CH1OUT_1
+#define GPIO_TIM2_CH2OUT GPIO_TIM2_CH2OUT_1
+#define GPIO_TIM2_CH3OUT GPIO_TIM2_CH3OUT_1
+#define GPIO_TIM2_CH4OUT GPIO_TIM2_CH4OUT_1
+
/* USB OTG FS
*
* PA9 OTG_FS_VBUS VBUS sensing (also connected to the green LED)
*/
#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9)
-/* PWM
- *
- * The PX4FMU has five PWM outputs, of which only the output on
- * pin PC8 is fixed assigned to this function. The other four possible
- * pwm sources are the TX, RX, RTS and CTS pins of USART2
- *
- * Alternate function mapping:
- * PC8 - BUZZER - TIM8_CH3/SDIO_D0 /TIM3_CH3/ USART6_CK / DCMI_D2
+/* High-resolution timer
*/
-
-#ifdef CONFIG_PWM
-# if defined(CONFIG_STM32_TIM3_PWM)
-# define BUZZER_PWMCHANNEL 3
-# define BUZZER_PWMTIMER 3
-# elif defined(CONFIG_STM32_TIM8_PWM)
-# define BUZZER_PWMCHANNEL 3
-# define BUZZER_PWMTIMER 8
-# endif
-#endif
+#define HRT_TIMER 1 /* use timer1 for the HRT */
+#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */
+#define HRT_PPM_CHANNEL 3 /* use capture/compare channel 3 */
+#define GPIO_PPM_IN (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN10)
/****************************************************************************************************
* Public Types
diff --git a/src/drivers/boards/px4fmu/module.mk b/src/drivers/boards/px4fmu-v1/module.mk
similarity index 100%
rename from src/drivers/boards/px4fmu/module.mk
rename to src/drivers/boards/px4fmu-v1/module.mk
diff --git a/src/drivers/boards/px4fmu/px4fmu_can.c b/src/drivers/boards/px4fmu-v1/px4fmu_can.c
similarity index 99%
rename from src/drivers/boards/px4fmu/px4fmu_can.c
rename to src/drivers/boards/px4fmu-v1/px4fmu_can.c
index 187689ff9a90fe6736cf4253d96958ee41c58546..1e1f100406267f4ac551ce8b27d683c49c6d46dd 100644
--- a/src/drivers/boards/px4fmu/px4fmu_can.c
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_can.c
@@ -54,7 +54,7 @@
#include "stm32.h"
#include "stm32_can.h"
-#include "px4fmu_internal.h"
+#include "board_config.h"
#ifdef CONFIG_CAN
diff --git a/src/drivers/boards/px4fmu/px4fmu_init.c b/src/drivers/boards/px4fmu-v1/px4fmu_init.c
similarity index 99%
rename from src/drivers/boards/px4fmu/px4fmu_init.c
rename to src/drivers/boards/px4fmu-v1/px4fmu_init.c
index 36af2298c27bb0f159c172645db412115c5022f2..964f5069c6ca5993c3192e0e0a742e62cabb2a1f 100644
--- a/src/drivers/boards/px4fmu/px4fmu_init.c
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_init.c
@@ -59,7 +59,7 @@
#include <nuttx/analog/adc.h>
#include "stm32.h"
-#include "px4fmu_internal.h"
+#include "board_config.h"
#include "stm32_uart.h"
#include <arch/board/board.h>
diff --git a/src/drivers/boards/px4fmu/px4fmu_led.c b/src/drivers/boards/px4fmu-v1/px4fmu_led.c
similarity index 88%
rename from src/drivers/boards/px4fmu/px4fmu_led.c
rename to src/drivers/boards/px4fmu-v1/px4fmu_led.c
index 31b25984e124cbdf751d93215153fd9977b371aa..ea91f34ad6e15702a0e5f17021ec9928f2b664f9 100644
--- a/src/drivers/boards/px4fmu/px4fmu_led.c
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_led.c
@@ -42,7 +42,7 @@
#include <stdbool.h>
#include "stm32.h"
-#include "px4fmu_internal.h"
+#include "board_config.h"
#include <arch/board/board.h>
@@ -57,6 +57,7 @@ __BEGIN_DECLS
extern void led_init();
extern void led_on(int led);
extern void led_off(int led);
+extern void led_toggle(int led);
__END_DECLS
__EXPORT void led_init()
@@ -94,3 +95,21 @@ __EXPORT void led_off(int led)
stm32_gpiowrite(GPIO_LED2, true);
}
}
+
+__EXPORT void led_toggle(int led)
+{
+ if (led == 0)
+ {
+ if (stm32_gpioread(GPIO_LED1))
+ stm32_gpiowrite(GPIO_LED1, false);
+ else
+ stm32_gpiowrite(GPIO_LED1, true);
+ }
+ if (led == 1)
+ {
+ if (stm32_gpioread(GPIO_LED2))
+ stm32_gpiowrite(GPIO_LED2, false);
+ else
+ stm32_gpiowrite(GPIO_LED2, true);
+ }
+}
diff --git a/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmu-v1/px4fmu_pwm_servo.c
similarity index 98%
rename from src/drivers/boards/px4fmu/px4fmu_pwm_servo.c
rename to src/drivers/boards/px4fmu-v1/px4fmu_pwm_servo.c
index d85131dd889348930d8168165bf464db75104644..848e21d79894a4d3b3ea868001bbe99896d45bcd 100644
--- a/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_pwm_servo.c
@@ -41,15 +41,15 @@
#include <stdint.h>
-#include <drivers/stm32/drv_pwm_servo.h>
-
-#include <arch/board/board.h>
-#include <drivers/drv_pwm_output.h>
-
#include <stm32.h>
#include <stm32_gpio.h>
#include <stm32_tim.h>
+#include <drivers/stm32/drv_pwm_servo.h>
+#include <drivers/drv_pwm_output.h>
+
+#include "board_config.h"
+
__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
{
.base = STM32_TIM2_BASE,
diff --git a/src/drivers/boards/px4fmu/px4fmu_spi.c b/src/drivers/boards/px4fmu-v1/px4fmu_spi.c
similarity index 99%
rename from src/drivers/boards/px4fmu/px4fmu_spi.c
rename to src/drivers/boards/px4fmu-v1/px4fmu_spi.c
index e05ddecf3eca50f1d9ece3ed4d0896d2bcaeb02c..17e6862f7cf13781d8f93e6249d77caf4b8bbe09 100644
--- a/src/drivers/boards/px4fmu/px4fmu_spi.c
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_spi.c
@@ -53,7 +53,7 @@
#include "up_arch.h"
#include "chip.h"
#include "stm32.h"
-#include "px4fmu_internal.h"
+#include "board_config.h"
/************************************************************************************
* Public Functions
diff --git a/src/drivers/boards/px4fmu/px4fmu_usb.c b/src/drivers/boards/px4fmu-v1/px4fmu_usb.c
similarity index 99%
rename from src/drivers/boards/px4fmu/px4fmu_usb.c
rename to src/drivers/boards/px4fmu-v1/px4fmu_usb.c
index 0be981c1e11525fc37ad1c18e0d3121290c02de3..0fc8569aa4a28c0b1f61ff3c76b405eba68e5ccb 100644
--- a/src/drivers/boards/px4fmu/px4fmu_usb.c
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_usb.c
@@ -53,7 +53,7 @@
#include "up_arch.h"
#include "stm32.h"
-#include "px4fmu_internal.h"
+#include "board_config.h"
/************************************************************************************
* Definitions
diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h
new file mode 100644
index 0000000000000000000000000000000000000000..ec8dde499329695f120bdb23bef278f90babc40a
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/board_config.h
@@ -0,0 +1,192 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 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 board_config.h
+ *
+ * PX4FMU internal definitions
+ */
+
+#pragma once
+
+/****************************************************************************************************
+ * Included Files
+ ****************************************************************************************************/
+
+#include <nuttx/config.h>
+#include <nuttx/compiler.h>
+#include <stdint.h>
+
+__BEGIN_DECLS
+
+/* these headers are not C++ safe */
+#include <stm32.h>
+#include <arch/board/board.h>
+
+/****************************************************************************************************
+ * Definitions
+ ****************************************************************************************************/
+/* Configuration ************************************************************************************/
+
+/* PX4IO connection configuration */
+#define PX4IO_SERIAL_DEVICE "/dev/ttyS4"
+#define PX4IO_SERIAL_TX_GPIO GPIO_USART6_TX
+#define PX4IO_SERIAL_RX_GPIO GPIO_USART6_RX
+#define PX4IO_SERIAL_BASE STM32_USART6_BASE /* hardwired on the board */
+#define PX4IO_SERIAL_VECTOR STM32_IRQ_USART6
+#define PX4IO_SERIAL_TX_DMAMAP DMAMAP_USART6_TX_2
+#define PX4IO_SERIAL_RX_DMAMAP DMAMAP_USART6_RX_2
+#define PX4IO_SERIAL_CLOCK STM32_PCLK2_FREQUENCY
+#define PX4IO_SERIAL_BITRATE 1500000 /* 1.5Mbps -> max rate for IO */
+
+
+/* PX4FMU GPIOs ***********************************************************************************/
+/* LEDs */
+
+#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12)
+
+/* External interrupts */
+#define GPIO_EXTI_GYRO_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN0)
+#define GPIO_EXTI_MAG_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN1)
+#define GPIO_EXTI_ACCEL_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN4)
+
+/* SPI chip selects */
+#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
+#define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
+#define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7)
+#define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10)
+
+/* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */
+#define PX4_SPIDEV_GYRO 1
+#define PX4_SPIDEV_ACCEL_MAG 2
+#define PX4_SPIDEV_BARO 3
+
+/* I2C busses */
+#define PX4_I2C_BUS_EXPANSION 1
+#define PX4_I2C_BUS_LED 2
+
+/* Devices on the onboard bus.
+ *
+ * Note that these are unshifted addresses.
+ */
+#define PX4_I2C_OBDEV_LED 0x55
+#define PX4_I2C_OBDEV_HMC5883 0x1e
+
+/* User GPIOs
+ *
+ * GPIO0-5 are the PWM servo outputs.
+ */
+#define GPIO_GPIO0_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN14)
+#define GPIO_GPIO1_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN13)
+#define GPIO_GPIO2_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN11)
+#define GPIO_GPIO3_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN9)
+#define GPIO_GPIO4_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN13)
+#define GPIO_GPIO5_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN14)
+#define GPIO_GPIO0_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN14)
+#define GPIO_GPIO1_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN13)
+#define GPIO_GPIO2_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11)
+#define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN9)
+#define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13)
+#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14)
+
+/* Power supply control and monitoring GPIOs */
+#define GPIO_VDD_5V_PERIPH_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8)
+#define GPIO_VDD_BRICK_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5)
+#define GPIO_VDD_SERVO_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN7)
+#define GPIO_VDD_3V3_SENSORS_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3)
+#define GPIO_VDD_5V_HIPOWER_OC (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN10)
+#define GPIO_VDD_5V_PERIPH_OC (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN15)
+
+/* Tone alarm output */
+#define TONE_ALARM_TIMER 2 /* timer 2 */
+#define TONE_ALARM_CHANNEL 1 /* channel 1 */
+#define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15)
+#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15)
+
+/* PWM
+ *
+ * Six PWM outputs are configured.
+ *
+ * Pins:
+ *
+ * CH1 : PE14 : TIM1_CH4
+ * CH2 : PE13 : TIM1_CH3
+ * CH3 : PE11 : TIM1_CH2
+ * CH4 : PE9 : TIM1_CH1
+ * CH5 : PD13 : TIM4_CH2
+ * CH6 : PD14 : TIM4_CH3
+ */
+#define GPIO_TIM1_CH1OUT GPIO_TIM1_CH1OUT_2
+#define GPIO_TIM1_CH2OUT GPIO_TIM1_CH2OUT_2
+#define GPIO_TIM1_CH3OUT GPIO_TIM1_CH3OUT_2
+#define GPIO_TIM1_CH4OUT GPIO_TIM1_CH4OUT_2
+#define GPIO_TIM4_CH2OUT GPIO_TIM4_CH2OUT_2
+#define GPIO_TIM4_CH3OUT GPIO_TIM4_CH3OUT_2
+
+/* USB OTG FS
+ *
+ * PA9 OTG_FS_VBUS VBUS sensing (also connected to the green LED)
+ */
+#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9)
+
+/* High-resolution timer */
+#define HRT_TIMER 8 /* use timer8 for the HRT */
+#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */
+
+/****************************************************************************************************
+ * Public Types
+ ****************************************************************************************************/
+
+/****************************************************************************************************
+ * Public data
+ ****************************************************************************************************/
+
+#ifndef __ASSEMBLY__
+
+/****************************************************************************************************
+ * Public Functions
+ ****************************************************************************************************/
+
+/****************************************************************************************************
+ * Name: stm32_spiinitialize
+ *
+ * Description:
+ * Called to configure SPI chip select GPIO pins for the PX4FMU board.
+ *
+ ****************************************************************************************************/
+
+extern void stm32_spiinitialize(void);
+
+#endif /* __ASSEMBLY__ */
+
+__END_DECLS
diff --git a/src/drivers/boards/px4fmu-v2/module.mk b/src/drivers/boards/px4fmu-v2/module.mk
new file mode 100644
index 0000000000000000000000000000000000000000..99d37eecac51f90da035eacd049264b19434c08a
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/module.mk
@@ -0,0 +1,10 @@
+#
+# Board-specific startup code for the PX4FMUv2
+#
+
+SRCS = px4fmu_can.c \
+ px4fmu2_init.c \
+ px4fmu_pwm_servo.c \
+ px4fmu_spi.c \
+ px4fmu_usb.c \
+ px4fmu2_led.c
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
new file mode 100644
index 0000000000000000000000000000000000000000..135767b26a9fdc3bdb2effd4e2c33c9eb2526c23
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
@@ -0,0 +1,262 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 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 px4fmu_init.c
+ *
+ * PX4FMU-specific early startup code. This file implements the
+ * nsh_archinitialize() function that is called early by nsh during startup.
+ *
+ * Code here is run before the rcS script is invoked; it should start required
+ * subsystems and perform board-specific initialisation.
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <debug.h>
+#include <errno.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/spi.h>
+#include <nuttx/i2c.h>
+#include <nuttx/sdio.h>
+#include <nuttx/mmcsd.h>
+#include <nuttx/analog/adc.h>
+
+#include <stm32.h>
+#include "board_config.h"
+#include <stm32_uart.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_led.h>
+
+#include <systemlib/cpuload.h>
+
+/****************************************************************************
+ * Pre-Processor Definitions
+ ****************************************************************************/
+
+/* Configuration ************************************************************/
+
+/* Debug ********************************************************************/
+
+#ifdef CONFIG_CPP_HAVE_VARARGS
+# ifdef CONFIG_DEBUG
+# define message(...) lowsyslog(__VA_ARGS__)
+# else
+# define message(...) printf(__VA_ARGS__)
+# endif
+#else
+# ifdef CONFIG_DEBUG
+# define message lowsyslog
+# else
+# define message printf
+# endif
+#endif
+
+/****************************************************************************
+ * Protected Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ * All STM32 architectures must provide the following entry point. This entry point
+ * is called early in the intitialization -- after all memory has been configured
+ * and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_boardinitialize(void)
+{
+ /* configure SPI interfaces */
+ stm32_spiinitialize();
+
+ /* configure LEDs */
+ up_ledinit();
+}
+
+/****************************************************************************
+ * Name: nsh_archinitialize
+ *
+ * Description:
+ * Perform architecture specific initialization
+ *
+ ****************************************************************************/
+
+static struct spi_dev_s *spi1;
+static struct spi_dev_s *spi2;
+static struct sdio_dev_s *sdio;
+
+#include <math.h>
+
+#ifdef __cplusplus
+__EXPORT int matherr(struct __exception *e)
+{
+ return 1;
+}
+#else
+__EXPORT int matherr(struct exception *e)
+{
+ return 1;
+}
+#endif
+
+__EXPORT int nsh_archinitialize(void)
+{
+
+ /* configure ADC pins */
+ stm32_configgpio(GPIO_ADC1_IN2); /* BATT_VOLTAGE_SENS */
+ stm32_configgpio(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */
+ stm32_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */
+ stm32_configgpio(GPIO_ADC1_IN10); /* unrouted */
+ stm32_configgpio(GPIO_ADC1_IN11); /* unrouted */
+ stm32_configgpio(GPIO_ADC1_IN12); /* unrouted */
+ stm32_configgpio(GPIO_ADC1_IN13); /* FMU_AUX_ADC_1 */
+ stm32_configgpio(GPIO_ADC1_IN14); /* FMU_AUX_ADC_2 */
+ stm32_configgpio(GPIO_ADC1_IN15); /* PRESSURE_SENS */
+
+ /* configure power supply control/sense pins */
+ stm32_configgpio(GPIO_VDD_5V_PERIPH_EN);
+ stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN);
+ stm32_configgpio(GPIO_VDD_BRICK_VALID);
+ stm32_configgpio(GPIO_VDD_SERVO_VALID);
+ stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC);
+ stm32_configgpio(GPIO_VDD_5V_PERIPH_OC);
+
+ /* configure the high-resolution time/callout interface */
+ hrt_init();
+
+ /* configure CPU load estimation */
+#ifdef CONFIG_SCHED_INSTRUMENTATION
+ cpuload_initialize_once();
+#endif
+
+ /* set up the serial DMA polling */
+ static struct hrt_call serial_dma_call;
+ struct timespec ts;
+
+ /*
+ * Poll at 1ms intervals for received bytes that have not triggered
+ * a DMA event.
+ */
+ ts.tv_sec = 0;
+ ts.tv_nsec = 1000000;
+
+ hrt_call_every(&serial_dma_call,
+ ts_to_abstime(&ts),
+ ts_to_abstime(&ts),
+ (hrt_callout)stm32_serial_dma_poll,
+ NULL);
+
+ /* initial LED state */
+ drv_led_start();
+ led_off(LED_AMBER);
+
+ /* Configure SPI-based devices */
+
+ spi1 = up_spiinitialize(1);
+
+ if (!spi1) {
+ message("[boot] FAILED to initialize SPI port 1\n");
+ up_ledon(LED_AMBER);
+ return -ENODEV;
+ }
+
+ /* Default SPI1 to 1MHz and de-assert the known chip selects. */
+ SPI_SETFREQUENCY(spi1, 10000000);
+ SPI_SETBITS(spi1, 8);
+ SPI_SETMODE(spi1, SPIDEV_MODE3);
+ SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false);
+ SPI_SELECT(spi1, PX4_SPIDEV_ACCEL_MAG, false);
+ SPI_SELECT(spi1, PX4_SPIDEV_BARO, false);
+ up_udelay(20);
+
+ message("[boot] Successfully initialized SPI port 1\n");
+
+ /* Get the SPI port for the FRAM */
+
+ spi2 = up_spiinitialize(2);
+
+ if (!spi2) {
+ message("[boot] FAILED to initialize SPI port 2\n");
+ up_ledon(LED_AMBER);
+ return -ENODEV;
+ }
+
+ /* Default SPI2 to 37.5 MHz (F4 max) and de-assert the known chip selects. */
+ SPI_SETFREQUENCY(spi2, 375000000);
+ SPI_SETBITS(spi2, 8);
+ SPI_SETMODE(spi2, SPIDEV_MODE3);
+ SPI_SELECT(spi2, SPIDEV_FLASH, false);
+
+ message("[boot] Successfully initialized SPI port 2\n");
+
+ #ifdef CONFIG_MMCSD
+ /* First, get an instance of the SDIO interface */
+
+ sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO);
+ if (!sdio) {
+ message("nsh_archinitialize: Failed to initialize SDIO slot %d\n",
+ CONFIG_NSH_MMCSDSLOTNO);
+ return -ENODEV;
+ }
+
+ /* Now bind the SDIO interface to the MMC/SD driver */
+ int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio);
+ if (ret != OK) {
+ message("nsh_archinitialize: Failed to bind SDIO to the MMC/SD driver: %d\n", ret);
+ return ret;
+ }
+
+ /* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */
+ sdio_mediachange(sdio, true);
+
+ message("[boot] Initialized SDIO\n");
+ #endif
+
+ return OK;
+}
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_led.c b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c
new file mode 100644
index 0000000000000000000000000000000000000000..a856ccb0272a30facaa6d301b5c700581b5dc35d
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c
@@ -0,0 +1,97 @@
+/****************************************************************************
+ *
+ * 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 px4fmu2_led.c
+ *
+ * PX4FMU LED backend.
+ */
+
+#include <nuttx/config.h>
+
+#include <stdbool.h>
+
+#include "stm32.h"
+#include "board_config.h"
+
+#include <arch/board/board.h>
+
+/*
+ * Ideally we'd be able to get these from up_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
+ */
+__BEGIN_DECLS
+extern void led_init();
+extern void led_on(int led);
+extern void led_off(int led);
+extern void led_toggle(int led);
+__END_DECLS
+
+__EXPORT void led_init()
+{
+ /* Configure LED1 GPIO for output */
+
+ stm32_configgpio(GPIO_LED1);
+}
+
+__EXPORT void led_on(int led)
+{
+ if (led == 1)
+ {
+ /* Pull down to switch on */
+ stm32_gpiowrite(GPIO_LED1, false);
+ }
+}
+
+__EXPORT void led_off(int led)
+{
+ if (led == 1)
+ {
+ /* Pull up to switch off */
+ stm32_gpiowrite(GPIO_LED1, true);
+ }
+}
+
+__EXPORT void led_toggle(int led)
+{
+ if (led == 1)
+ {
+ if (stm32_gpioread(GPIO_LED1))
+ stm32_gpiowrite(GPIO_LED1, false);
+ else
+ stm32_gpiowrite(GPIO_LED1, true);
+ }
+}
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_can.c b/src/drivers/boards/px4fmu-v2/px4fmu_can.c
new file mode 100644
index 0000000000000000000000000000000000000000..f66c7cd79f6cb0d75d2b81fafcb7289f9d531f5f
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/px4fmu_can.c
@@ -0,0 +1,144 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 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 px4fmu_can.c
+ *
+ * Board-specific CAN functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <errno.h>
+#include <debug.h>
+
+#include <nuttx/can.h>
+#include <arch/board/board.h>
+
+#include "chip.h"
+#include "up_arch.h"
+
+#include "stm32.h"
+#include "stm32_can.h"
+#include "board_config.h"
+
+#ifdef CONFIG_CAN
+
+/************************************************************************************
+ * Pre-processor Definitions
+ ************************************************************************************/
+/* Configuration ********************************************************************/
+
+#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2)
+# warning "Both CAN1 and CAN2 are enabled. Assuming only CAN1."
+# undef CONFIG_STM32_CAN2
+#endif
+
+#ifdef CONFIG_STM32_CAN1
+# define CAN_PORT 1
+#else
+# define CAN_PORT 2
+#endif
+
+/* Debug ***************************************************************************/
+/* Non-standard debug that may be enabled just for testing CAN */
+
+#ifdef CONFIG_DEBUG_CAN
+# define candbg dbg
+# define canvdbg vdbg
+# define canlldbg lldbg
+# define canllvdbg llvdbg
+#else
+# define candbg(x...)
+# define canvdbg(x...)
+# define canlldbg(x...)
+# define canllvdbg(x...)
+#endif
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: can_devinit
+ *
+ * Description:
+ * All STM32 architectures must provide the following interface to work with
+ * examples/can.
+ *
+ ************************************************************************************/
+
+int can_devinit(void)
+{
+ static bool initialized = false;
+ struct can_dev_s *can;
+ int ret;
+
+ /* Check if we have already initialized */
+
+ if (!initialized) {
+ /* Call stm32_caninitialize() to get an instance of the CAN interface */
+
+ can = stm32_caninitialize(CAN_PORT);
+
+ if (can == NULL) {
+ candbg("ERROR: Failed to get CAN interface\n");
+ return -ENODEV;
+ }
+
+ /* Register the CAN driver at "/dev/can0" */
+
+ ret = can_register("/dev/can0", can);
+
+ if (ret < 0) {
+ candbg("ERROR: can_register failed: %d\n", ret);
+ return ret;
+ }
+
+ /* Now we are initialized */
+
+ initialized = true;
+ }
+
+ return OK;
+}
+
+#endif
\ No newline at end of file
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmu-v2/px4fmu_pwm_servo.c
new file mode 100644
index 0000000000000000000000000000000000000000..600dfef412244148afaa3dfa0d8d76ab6412449f
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/px4fmu_pwm_servo.c
@@ -0,0 +1,105 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 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 px4fmu_pwm_servo.c
+ *
+ * Configuration data for the stm32 pwm_servo driver.
+ *
+ * Note that these arrays must always be fully-sized.
+ */
+
+#include <stdint.h>
+
+#include <stm32.h>
+#include <stm32_gpio.h>
+#include <stm32_tim.h>
+
+#include <drivers/stm32/drv_pwm_servo.h>
+#include <drivers/drv_pwm_output.h>
+
+#include "board_config.h"
+
+__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
+ {
+ .base = STM32_TIM1_BASE,
+ .clock_register = STM32_RCC_APB2ENR,
+ .clock_bit = RCC_APB2ENR_TIM1EN,
+ .clock_freq = STM32_APB2_TIM1_CLKIN
+ },
+ {
+ .base = STM32_TIM4_BASE,
+ .clock_register = STM32_RCC_APB1ENR,
+ .clock_bit = RCC_APB1ENR_TIM4EN,
+ .clock_freq = STM32_APB1_TIM4_CLKIN
+ }
+};
+
+__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
+ {
+ .gpio = GPIO_TIM1_CH4OUT,
+ .timer_index = 0,
+ .timer_channel = 4,
+ .default_value = 1000,
+ },
+ {
+ .gpio = GPIO_TIM1_CH3OUT,
+ .timer_index = 0,
+ .timer_channel = 3,
+ .default_value = 1000,
+ },
+ {
+ .gpio = GPIO_TIM1_CH2OUT,
+ .timer_index = 0,
+ .timer_channel = 2,
+ .default_value = 1000,
+ },
+ {
+ .gpio = GPIO_TIM1_CH1OUT,
+ .timer_index = 0,
+ .timer_channel = 1,
+ .default_value = 1000,
+ },
+ {
+ .gpio = GPIO_TIM4_CH2OUT,
+ .timer_index = 1,
+ .timer_channel = 2,
+ .default_value = 1000,
+ },
+ {
+ .gpio = GPIO_TIM4_CH3OUT,
+ .timer_index = 1,
+ .timer_channel = 3,
+ .default_value = 1000,
+ }
+};
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c
new file mode 100644
index 0000000000000000000000000000000000000000..09838d02fe4b53f2a14bc04f4b2494c48bc6122a
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c
@@ -0,0 +1,141 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 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 px4fmu_spi.c
+ *
+ * Board-specific SPI functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <debug.h>
+
+#include <nuttx/spi.h>
+#include <arch/board/board.h>
+
+#include <up_arch.h>
+#include <chip.h>
+#include <stm32.h>
+#include "board_config.h"
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_spiinitialize
+ *
+ * Description:
+ * Called to configure SPI chip select GPIO pins for the PX4FMU board.
+ *
+ ************************************************************************************/
+
+__EXPORT void weak_function stm32_spiinitialize(void)
+{
+#ifdef CONFIG_STM32_SPI1
+ stm32_configgpio(GPIO_SPI_CS_GYRO);
+ stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG);
+ stm32_configgpio(GPIO_SPI_CS_BARO);
+
+ /* De-activate all peripherals,
+ * required for some peripheral
+ * state machines
+ */
+ stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+ stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+ stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+#endif
+
+#ifdef CONFIG_STM32_SPI2
+ stm32_configgpio(GPIO_SPI_CS_FRAM);
+ stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1);
+#endif
+}
+
+__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+ /* SPI select is active low, so write !selected to select the device */
+
+ switch (devid) {
+ case PX4_SPIDEV_GYRO:
+ /* Making sure the other peripherals are not selected */
+ stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
+ stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+ stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+ break;
+
+ case PX4_SPIDEV_ACCEL_MAG:
+ /* Making sure the other peripherals are not selected */
+ stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+ stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected);
+ stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+ break;
+
+ case PX4_SPIDEV_BARO:
+ /* Making sure the other peripherals are not selected */
+ stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+ stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+ stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected);
+ break;
+
+ default:
+ break;
+ }
+}
+
+__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+ return SPI_STATUS_PRESENT;
+}
+
+
+#ifdef CONFIG_STM32_SPI2
+__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+ /* there can only be one device on this bus, so always select it */
+ stm32_gpiowrite(GPIO_SPI_CS_FRAM, !selected);
+}
+
+__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+ /* FRAM is always present */
+ return SPI_STATUS_PRESENT;
+}
+#endif
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_usb.c b/src/drivers/boards/px4fmu-v2/px4fmu_usb.c
new file mode 100644
index 0000000000000000000000000000000000000000..f329e06ff9bf140074000798fef9c92761796885
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/px4fmu_usb.c
@@ -0,0 +1,108 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 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 px4fmu_usb.c
+ *
+ * Board-specific USB functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <debug.h>
+
+#include <nuttx/usb/usbdev.h>
+#include <nuttx/usb/usbdev_trace.h>
+
+#include <up_arch.h>
+#include <stm32.h>
+#include "board_config.h"
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_usbinitialize
+ *
+ * Description:
+ * Called to setup USB-related GPIO pins for the PX4FMU board.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_usbinitialize(void)
+{
+ /* The OTG FS has an internal soft pull-up */
+
+ /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */
+
+#ifdef CONFIG_STM32_OTGFS
+ stm32_configgpio(GPIO_OTGFS_VBUS);
+ /* XXX We only support device mode
+ stm32_configgpio(GPIO_OTGFS_PWRON);
+ stm32_configgpio(GPIO_OTGFS_OVER);
+ */
+#endif
+}
+
+/************************************************************************************
+ * Name: stm32_usbsuspend
+ *
+ * Description:
+ * Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is
+ * used. This function is called whenever the USB enters or leaves suspend mode.
+ * This is an opportunity for the board logic to shutdown clocks, power, etc.
+ * while the USB is suspended.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume)
+{
+ ulldbg("resume: %d\n", resume);
+}
+
diff --git a/src/drivers/boards/px4io/px4io_internal.h b/src/drivers/boards/px4io-v1/board_config.h
similarity index 92%
rename from src/drivers/boards/px4io/px4io_internal.h
rename to src/drivers/boards/px4io-v1/board_config.h
index 6638e715efa48a07470ada2cd6246024773d86fb..48aadbd768cd3c86a6a2c781e30fe169522e6604 100644
--- a/src/drivers/boards/px4io/px4io_internal.h
+++ b/src/drivers/boards/px4io-v1/board_config.h
@@ -32,7 +32,7 @@
****************************************************************************/
/**
- * @file px4io_internal.h
+ * @file board_config.h
*
* PX4IO hardware definitions.
*/
@@ -47,7 +47,9 @@
#include <nuttx/compiler.h>
#include <stdint.h>
+/* these headers are not C++ safe */
#include <stm32.h>
+#include <arch/board/board.h>
/************************************************************************************
* Definitions
@@ -83,3 +85,11 @@
#define GPIO_ADC_VBATT (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4)
#define GPIO_ADC_IN5 (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5)
+
+/*
+ * High-resolution timer
+ */
+#define HRT_TIMER 1 /* use timer1 for the HRT */
+#define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */
+#define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 */
+#define GPIO_PPM_IN GPIO_TIM1_CH1IN
diff --git a/src/drivers/boards/px4io/module.mk b/src/drivers/boards/px4io-v1/module.mk
similarity index 100%
rename from src/drivers/boards/px4io/module.mk
rename to src/drivers/boards/px4io-v1/module.mk
diff --git a/src/drivers/boards/px4io/px4io_init.c b/src/drivers/boards/px4io-v1/px4io_init.c
similarity index 99%
rename from src/drivers/boards/px4io/px4io_init.c
rename to src/drivers/boards/px4io-v1/px4io_init.c
index 15c59e42365ba00d7cd8cb2a3bcf68a320b228ae..8292da9e1c42ff15aa6774a449917cdc29dc0ae8 100644
--- a/src/drivers/boards/px4io/px4io_init.c
+++ b/src/drivers/boards/px4io-v1/px4io_init.c
@@ -55,7 +55,7 @@
#include <nuttx/arch.h>
#include "stm32.h"
-#include "px4io_internal.h"
+#include "board_config.h"
#include "stm32_uart.h"
#include <arch/board/board.h>
diff --git a/src/drivers/boards/px4io/px4io_pwm_servo.c b/src/drivers/boards/px4io-v1/px4io_pwm_servo.c
similarity index 100%
rename from src/drivers/boards/px4io/px4io_pwm_servo.c
rename to src/drivers/boards/px4io-v1/px4io_pwm_servo.c
diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h
new file mode 100644
index 0000000000000000000000000000000000000000..818b644362cb8b1d5153cebad5ea2be6ea921f57
--- /dev/null
+++ b/src/drivers/boards/px4io-v2/board_config.h
@@ -0,0 +1,138 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 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 px4iov2_internal.h
+ *
+ * PX4IOV2 internal definitions
+ */
+
+#pragma once
+
+/******************************************************************************
+ * Included Files
+ ******************************************************************************/
+
+#include <nuttx/config.h>
+#include <nuttx/compiler.h>
+#include <stdint.h>
+
+/* these headers are not C++ safe */
+#include <stm32.h>
+#include <arch/board/board.h>
+
+/******************************************************************************
+ * Definitions
+ ******************************************************************************/
+/* Configuration **************************************************************/
+
+/******************************************************************************
+ * Serial
+ ******************************************************************************/
+#define PX4FMU_SERIAL_BASE STM32_USART2_BASE
+#define PX4FMU_SERIAL_VECTOR STM32_IRQ_USART2
+#define PX4FMU_SERIAL_TX_GPIO GPIO_USART2_TX
+#define PX4FMU_SERIAL_RX_GPIO GPIO_USART2_RX
+#define PX4FMU_SERIAL_TX_DMA DMACHAN_USART2_TX
+#define PX4FMU_SERIAL_RX_DMA DMACHAN_USART2_RX
+#define PX4FMU_SERIAL_CLOCK STM32_PCLK1_FREQUENCY
+#define PX4FMU_SERIAL_BITRATE 1500000
+
+/******************************************************************************
+ * GPIOS
+ ******************************************************************************/
+
+/* LEDS **********************************************************************/
+
+#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14)
+#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15)
+#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13)
+
+/* Safety switch button *******************************************************/
+
+#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5)
+
+/* Power switch controls ******************************************************/
+
+#define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)
+
+#define GPIO_SERVO_FAULT_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN15)
+
+/* Analog inputs **************************************************************/
+
+#define GPIO_ADC_VSERVO (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4)
+
+/* the same rssi signal goes to both an adc and a timer input */
+#define GPIO_ADC_RSSI (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5)
+#define GPIO_TIM_RSSI (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN12)
+
+/* PWM pins **************************************************************/
+
+#define GPIO_PPM (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN8)
+
+#define GPIO_PWM1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)
+#define GPIO_PWM2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1)
+#define GPIO_PWM3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8)
+#define GPIO_PWM4 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9)
+#define GPIO_PWM5 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN6)
+#define GPIO_PWM6 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7)
+#define GPIO_PWM7 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0)
+#define GPIO_PWM8 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1)
+
+/* SBUS pins *************************************************************/
+
+/* XXX these should be UART pins */
+#define GPIO_SBUS_INPUT (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN11)
+#define GPIO_SBUS_OUTPUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
+#define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN4)
+
+/*
+ * High-resolution timer
+ */
+#define HRT_TIMER 1 /* use timer1 for the HRT */
+#define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */
+#define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 */
+#define GPIO_PPM_IN GPIO_TIM1_CH1IN
+
+/* LED definitions ******************************************************************/
+/* PX4 has two LEDs that we will encode as: */
+
+#define LED_STARTED 0 /* LED? */
+#define LED_HEAPALLOCATE 1 /* LED? */
+#define LED_IRQSENABLED 2 /* LED? + LED? */
+#define LED_STACKCREATED 3 /* LED? */
+#define LED_INIRQ 4 /* LED? + LED? */
+#define LED_SIGNAL 5 /* LED? + LED? */
+#define LED_ASSERTION 6 /* LED? + LED? + LED? */
+#define LED_PANIC 7 /* N/C + N/C + N/C + LED? */
+
diff --git a/src/drivers/boards/px4io-v2/module.mk b/src/drivers/boards/px4io-v2/module.mk
new file mode 100644
index 0000000000000000000000000000000000000000..85f94e8be1c06e339fca407bd02dc88065078e6f
--- /dev/null
+++ b/src/drivers/boards/px4io-v2/module.mk
@@ -0,0 +1,6 @@
+#
+# Board-specific startup code for the PX4IOv2
+#
+
+SRCS = px4iov2_init.c \
+ px4iov2_pwm_servo.c
diff --git a/src/drivers/boards/px4io-v2/px4iov2_init.c b/src/drivers/boards/px4io-v2/px4iov2_init.c
new file mode 100644
index 0000000000000000000000000000000000000000..0ea95bded9171e96f0002e165fe0b7431874ae73
--- /dev/null
+++ b/src/drivers/boards/px4io-v2/px4iov2_init.c
@@ -0,0 +1,162 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 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 px4iov2_init.c
+ *
+ * PX4FMU-specific early startup code. This file implements the
+ * nsh_archinitialize() function that is called early by nsh during startup.
+ *
+ * Code here is run before the rcS script is invoked; it should start required
+ * subsystems and perform board-specific initialisation.
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <debug.h>
+#include <errno.h>
+
+#include <nuttx/arch.h>
+
+#include <stm32.h>
+#include "board_config.h"
+
+#include <arch/board/board.h>
+
+/****************************************************************************
+ * Pre-Processor Definitions
+ ****************************************************************************/
+
+/* Configuration ************************************************************/
+
+/* Debug ********************************************************************/
+
+#ifdef CONFIG_CPP_HAVE_VARARGS
+# ifdef CONFIG_DEBUG
+# define message(...) lowsyslog(__VA_ARGS__)
+# else
+# define message(...) printf(__VA_ARGS__)
+# endif
+#else
+# ifdef CONFIG_DEBUG
+# define message lowsyslog
+# else
+# define message printf
+# endif
+#endif
+
+/****************************************************************************
+ * Protected Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ * All STM32 architectures must provide the following entry point. This entry point
+ * is called early in the intitialization -- after all memory has been configured
+ * and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_boardinitialize(void)
+{
+
+ /* configure GPIOs */
+
+ /* LEDS - default to off */
+ stm32_configgpio(GPIO_LED1);
+ stm32_configgpio(GPIO_LED2);
+ stm32_configgpio(GPIO_LED3);
+
+ stm32_configgpio(GPIO_BTN_SAFETY);
+
+ /* spektrum power enable is active high - disable it by default */
+ /* XXX might not want to do this on warm restart? */
+ stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, false);
+ stm32_configgpio(GPIO_SPEKTRUM_PWR_EN);
+
+ stm32_configgpio(GPIO_SERVO_FAULT_DETECT);
+
+ /* RSSI inputs */
+ stm32_configgpio(GPIO_TIM_RSSI); /* xxx alternate function */
+ stm32_configgpio(GPIO_ADC_RSSI);
+
+ /* servo rail voltage */
+ stm32_configgpio(GPIO_ADC_VSERVO);
+
+ stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */
+
+ stm32_gpiowrite(GPIO_SBUS_OUTPUT, false);
+ stm32_configgpio(GPIO_SBUS_OUTPUT);
+
+ /* sbus output enable is active low - disable it by default */
+ stm32_gpiowrite(GPIO_SBUS_OENABLE, true);
+ stm32_configgpio(GPIO_SBUS_OENABLE);
+
+ stm32_configgpio(GPIO_PPM); /* xxx alternate function */
+
+ stm32_gpiowrite(GPIO_PWM1, false);
+ stm32_configgpio(GPIO_PWM1);
+
+ stm32_gpiowrite(GPIO_PWM2, false);
+ stm32_configgpio(GPIO_PWM2);
+
+ stm32_gpiowrite(GPIO_PWM3, false);
+ stm32_configgpio(GPIO_PWM3);
+
+ stm32_gpiowrite(GPIO_PWM4, false);
+ stm32_configgpio(GPIO_PWM4);
+
+ stm32_gpiowrite(GPIO_PWM5, false);
+ stm32_configgpio(GPIO_PWM5);
+
+ stm32_gpiowrite(GPIO_PWM6, false);
+ stm32_configgpio(GPIO_PWM6);
+
+ stm32_gpiowrite(GPIO_PWM7, false);
+ stm32_configgpio(GPIO_PWM7);
+
+ stm32_gpiowrite(GPIO_PWM8, false);
+ stm32_configgpio(GPIO_PWM8);
+}
diff --git a/src/drivers/boards/px4io-v2/px4iov2_pwm_servo.c b/src/drivers/boards/px4io-v2/px4iov2_pwm_servo.c
new file mode 100644
index 0000000000000000000000000000000000000000..4f1b9487cf3db4d05a049da788357f5d8ebd0c73
--- /dev/null
+++ b/src/drivers/boards/px4io-v2/px4iov2_pwm_servo.c
@@ -0,0 +1,123 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 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 px4iov2_pwm_servo.c
+ *
+ * Configuration data for the stm32 pwm_servo driver.
+ *
+ * Note that these arrays must always be fully-sized.
+ */
+
+#include <stdint.h>
+
+#include <drivers/stm32/drv_pwm_servo.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_pwm_output.h>
+
+#include <stm32.h>
+#include <stm32_gpio.h>
+#include <stm32_tim.h>
+
+__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
+ {
+ .base = STM32_TIM2_BASE,
+ .clock_register = STM32_RCC_APB1ENR,
+ .clock_bit = RCC_APB1ENR_TIM2EN,
+ .clock_freq = STM32_APB1_TIM2_CLKIN
+ },
+ {
+ .base = STM32_TIM3_BASE,
+ .clock_register = STM32_RCC_APB1ENR,
+ .clock_bit = RCC_APB1ENR_TIM3EN,
+ .clock_freq = STM32_APB1_TIM3_CLKIN
+ },
+ {
+ .base = STM32_TIM4_BASE,
+ .clock_register = STM32_RCC_APB1ENR,
+ .clock_bit = RCC_APB1ENR_TIM4EN,
+ .clock_freq = STM32_APB1_TIM4_CLKIN
+ }
+};
+
+__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
+ {
+ .gpio = GPIO_TIM2_CH1OUT,
+ .timer_index = 0,
+ .timer_channel = 1,
+ .default_value = 1000,
+ },
+ {
+ .gpio = GPIO_TIM2_CH2OUT,
+ .timer_index = 0,
+ .timer_channel = 2,
+ .default_value = 1000,
+ },
+ {
+ .gpio = GPIO_TIM4_CH3OUT,
+ .timer_index = 2,
+ .timer_channel = 3,
+ .default_value = 1000,
+ },
+ {
+ .gpio = GPIO_TIM4_CH4OUT,
+ .timer_index = 2,
+ .timer_channel = 4,
+ .default_value = 1000,
+ },
+ {
+ .gpio = GPIO_TIM3_CH1OUT,
+ .timer_index = 1,
+ .timer_channel = 1,
+ .default_value = 1000,
+ },
+ {
+ .gpio = GPIO_TIM3_CH2OUT,
+ .timer_index = 1,
+ .timer_channel = 2,
+ .default_value = 1000,
+ },
+ {
+ .gpio = GPIO_TIM3_CH3OUT,
+ .timer_index = 1,
+ .timer_channel = 3,
+ .default_value = 1000,
+ },
+ {
+ .gpio = GPIO_TIM3_CH4OUT,
+ .timer_index = 1,
+ .timer_channel = 4,
+ .default_value = 1000,
+ }
+};
diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h
index 510983d4bd23e2cd192bbfa46cf59519df5a7463..37af26d522145d7187d7b0357da26999511d0539 100644
--- a/src/drivers/drv_gpio.h
+++ b/src/drivers/drv_gpio.h
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ * Copyright (C) 2012 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
@@ -59,17 +59,47 @@
# define GPIO_CAN_RX (1<<7) /**< CAN2 RX */
/**
- * Default GPIO device - other devices may also support this protocol if
- * they also export GPIO-like things. This is always the GPIOs on the
- * main board.
+ * Device paths for things that support the GPIO ioctl protocol.
*/
# define PX4FMU_DEVICE_PATH "/dev/px4fmu"
# define PX4IO_DEVICE_PATH "/dev/px4io"
#endif
-#ifndef PX4IO_DEVICE_PATH
-# error No GPIO support for this board.
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+/*
+ * PX4FMUv2 GPIO numbers.
+ *
+ * There are no alternate functions on this board.
+ */
+# define GPIO_SERVO_1 (1<<0) /**< servo 1 output */
+# define GPIO_SERVO_2 (1<<1) /**< servo 2 output */
+# define GPIO_SERVO_3 (1<<2) /**< servo 3 output */
+# define GPIO_SERVO_4 (1<<3) /**< servo 4 output */
+# define GPIO_SERVO_5 (1<<4) /**< servo 5 output */
+# define GPIO_SERVO_6 (1<<5) /**< servo 6 output */
+
+# define GPIO_5V_PERIPH_EN (1<<6) /**< PA8 - !VDD_5V_PERIPH_EN */
+# define GPIO_3V3_SENSORS_EN (1<<7) /**< PE3 - VDD_3V3_SENSORS_EN */
+# define GPIO_BRICK_VALID (1<<8) /**< PB5 - !VDD_BRICK_VALID */
+# define GPIO_SERVO_VALID (1<<9) /**< PB7 - !VDD_SERVO_VALID */
+# define GPIO_5V_HIPOWER_OC (1<<10) /**< PE10 - !VDD_5V_HIPOWER_OC */
+# define GPIO_5V_PERIPH_OC (1<<11) /**< PE10 - !VDD_5V_PERIPH_OC */
+
+/**
+ * Device paths for things that support the GPIO ioctl protocol.
+ */
+# define PX4FMU_DEVICE_PATH "/dev/px4fmu"
+# define PX4IO_DEVICE_PATH "/dev/px4io"
+
+#endif
+
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
+/* no GPIO driver on the PX4IOv1 board */
+#endif
+
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
+/* no GPIO driver on the PX4IOv2 board */
#endif
/*
diff --git a/src/drivers/drv_led.h b/src/drivers/drv_led.h
index 21044f6200aae0ad03bf4e888426f193f08fedf4..4ce04696e04226c566d03aa73763875b25ee1fc4 100644
--- a/src/drivers/drv_led.h
+++ b/src/drivers/drv_led.h
@@ -54,12 +54,13 @@
#define LED_ON _IOC(_LED_BASE, 0)
#define LED_OFF _IOC(_LED_BASE, 1)
+#define LED_TOGGLE _IOC(_LED_BASE, 2)
__BEGIN_DECLS
/*
* Initialise the LED driver.
*/
-__EXPORT extern void drv_led_start(void);
+__EXPORT void drv_led_start(void);
__END_DECLS
diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h
index 9aab995a17b4635e939eb48584ce4d70b41a01d6..3de5625fd76b3ad057135ed4acde7ab81c29228a 100644
--- a/src/drivers/drv_mag.h
+++ b/src/drivers/drv_mag.h
@@ -90,25 +90,37 @@ ORB_DECLARE(sensor_mag);
/** set the mag internal sample rate to at least (arg) Hz */
#define MAGIOCSSAMPLERATE _MAGIOC(0)
+/** return the mag internal sample rate in Hz */
+#define MAGIOCGSAMPLERATE _MAGIOC(1)
+
/** set the mag internal lowpass filter to no lower than (arg) Hz */
-#define MAGIOCSLOWPASS _MAGIOC(1)
+#define MAGIOCSLOWPASS _MAGIOC(2)
+
+/** return the mag internal lowpass filter in Hz */
+#define MAGIOCGLOWPASS _MAGIOC(3)
/** set the mag scaling constants to the structure pointed to by (arg) */
-#define MAGIOCSSCALE _MAGIOC(2)
+#define MAGIOCSSCALE _MAGIOC(4)
/** copy the mag scaling constants to the structure pointed to by (arg) */
-#define MAGIOCGSCALE _MAGIOC(3)
+#define MAGIOCGSCALE _MAGIOC(5)
/** set the measurement range to handle (at least) arg Gauss */
-#define MAGIOCSRANGE _MAGIOC(4)
+#define MAGIOCSRANGE _MAGIOC(6)
+
+/** return the current mag measurement range in Gauss */
+#define MAGIOCGRANGE _MAGIOC(7)
/** perform self-calibration, update scale factors to canonical units */
-#define MAGIOCCALIBRATE _MAGIOC(5)
+#define MAGIOCCALIBRATE _MAGIOC(8)
/** excite strap */
-#define MAGIOCEXSTRAP _MAGIOC(6)
+#define MAGIOCEXSTRAP _MAGIOC(9)
/** perform self test and report status */
-#define MAGIOCSELFTEST _MAGIOC(7)
+#define MAGIOCSELFTEST _MAGIOC(10)
+
+/** determine if mag is external or onboard */
+#define MAGIOCGEXTERNAL _MAGIOC(11)
#endif /* _DRV_MAG_H */
diff --git a/src/drivers/drv_rgbled.h b/src/drivers/drv_rgbled.h
new file mode 100644
index 0000000000000000000000000000000000000000..3c8bdec5d33ab146339369917328f1138334e2a6
--- /dev/null
+++ b/src/drivers/drv_rgbled.h
@@ -0,0 +1,126 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012-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 drv_rgbled.h
+ *
+ * RGB led device API
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+/* more devices will be 1, 2, etc */
+#define RGBLED_DEVICE_PATH "/dev/rgbled0"
+
+/*
+ * ioctl() definitions
+ */
+
+#define _RGBLEDIOCBASE (0x2900)
+#define _RGBLEDIOC(_n) (_IOC(_RGBLEDIOCBASE, _n))
+
+/** play the named script in *(char *)arg, repeating forever */
+#define RGBLED_PLAY_SCRIPT_NAMED _RGBLEDIOC(1)
+
+/** play the numbered script in (arg), repeating forever */
+#define RGBLED_PLAY_SCRIPT _RGBLEDIOC(2)
+
+/**
+ * Set the user script; (arg) is a pointer to an array of script lines,
+ * where each line is an array of four bytes giving <duration>, <command>, arg[0-2]
+ *
+ * The script is terminated by a zero command.
+ */
+#define RGBLED_SET_USER_SCRIPT _RGBLEDIOC(3)
+
+/** set constant RGB values */
+#define RGBLED_SET_RGB _RGBLEDIOC(4)
+
+/** set color */
+#define RGBLED_SET_COLOR _RGBLEDIOC(5)
+
+/** set blink speed */
+#define RGBLED_SET_MODE _RGBLEDIOC(6)
+
+/** set pattern */
+#define RGBLED_SET_PATTERN _RGBLEDIOC(7)
+
+/*
+ structure passed to RGBLED_SET_RGB ioctl()
+ Note that the driver scales the brightness to 0 to 255, regardless
+ of the hardware scaling
+ */
+typedef struct {
+ uint8_t red;
+ uint8_t green;
+ uint8_t blue;
+} rgbled_rgbset_t;
+
+/* enum passed to RGBLED_SET_COLOR ioctl()*/
+typedef enum {
+ RGBLED_COLOR_OFF,
+ RGBLED_COLOR_RED,
+ RGBLED_COLOR_YELLOW,
+ RGBLED_COLOR_PURPLE,
+ RGBLED_COLOR_GREEN,
+ RGBLED_COLOR_BLUE,
+ RGBLED_COLOR_WHITE,
+ RGBLED_COLOR_AMBER,
+ RGBLED_COLOR_DIM_RED,
+ RGBLED_COLOR_DIM_YELLOW,
+ RGBLED_COLOR_DIM_PURPLE,
+ RGBLED_COLOR_DIM_GREEN,
+ RGBLED_COLOR_DIM_BLUE,
+ RGBLED_COLOR_DIM_WHITE,
+ RGBLED_COLOR_DIM_AMBER
+} rgbled_color_t;
+
+/* enum passed to RGBLED_SET_MODE ioctl()*/
+typedef enum {
+ RGBLED_MODE_OFF,
+ RGBLED_MODE_ON,
+ RGBLED_MODE_BLINK_SLOW,
+ RGBLED_MODE_BLINK_NORMAL,
+ RGBLED_MODE_BLINK_FAST,
+ RGBLED_MODE_PATTERN
+} rgbled_mode_t;
+
+#define RGBLED_PATTERN_LENGTH 20
+
+typedef struct {
+ rgbled_color_t color[RGBLED_PATTERN_LENGTH];
+ unsigned duration[RGBLED_PATTERN_LENGTH];
+} rgbled_pattern_t;
diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp
index cd72d9d23dcec09df73c7b1c9e70724e535c32da..257b41935c449456e9c55692f7ae7673190afea5 100644
--- a/src/drivers/ets_airspeed/ets_airspeed.cpp
+++ b/src/drivers/ets_airspeed/ets_airspeed.cpp
@@ -59,7 +59,7 @@
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
-#include <arch/board/board.h>
+#include <board_config.h>
#include <systemlib/airspeed.h>
#include <systemlib/err.h>
diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp
index bd027ce0b9259faf8c6ee1af41e50ce3a2fbe11d..3e30e329276bf514f96f47e26437750423ac1c86 100644
--- a/src/drivers/hil/hil.cpp
+++ b/src/drivers/hil/hil.cpp
@@ -75,6 +75,7 @@
#include <systemlib/mixer/mixer.h>
#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_outputs.h>
#include <systemlib/err.h>
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index ac3bdc13259f71b0b06d1e67c04c311b62f2d791..d77f03bb72da035550dab977434fdfac210fd94b 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -58,7 +58,7 @@
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
-#include <arch/board/board.h>
+#include <board_config.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
@@ -77,8 +77,8 @@
#define HMC5883L_ADDRESS PX4_I2C_OBDEV_HMC5883
-/* Max measurement rate is 160Hz */
-#define HMC5883_CONVERSION_INTERVAL (1000000 / 160) /* microseconds */
+/* Max measurement rate is 160Hz, however with 160 it will be set to 166 Hz, therefore workaround using 150 */
+#define HMC5883_CONVERSION_INTERVAL (1000000 / 150) /* microseconds */
#define ADDR_CONF_A 0x00
#define ADDR_CONF_B 0x01
@@ -167,6 +167,8 @@ private:
bool _sensor_ok; /**< sensor was found and reports ok */
bool _calibrated; /**< the calibration is valid */
+ int _bus; /**< the bus the device is connected to */
+
/**
* Test whether the device supported by the driver is present at a
* specific address.
@@ -189,6 +191,11 @@ private:
*/
void stop();
+ /**
+ * Reset the device
+ */
+ int reset();
+
/**
* Perform the on-sensor scale calibration routine.
*
@@ -326,7 +333,8 @@ HMC5883::HMC5883(int bus) :
_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")),
_sensor_ok(false),
- _calibrated(false)
+ _calibrated(false),
+ _bus(bus)
{
// enable debug() calls
_debug_enabled = false;
@@ -362,6 +370,9 @@ HMC5883::init()
if (I2C::init() != OK)
goto out;
+ /* reset the device configuration */
+ reset();
+
/* allocate basic report buffers */
_num_reports = 2;
_reports = new struct mag_report[_num_reports];
@@ -378,9 +389,6 @@ HMC5883::init()
if (_mag_topic < 0)
debug("failed to create sensor_mag object");
- /* set range */
- set_range(_range_ga);
-
ret = OK;
/* sensor is ok, but not calibrated */
_sensor_ok = true;
@@ -539,68 +547,67 @@ int
HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
-
case SENSORIOCSPOLLRATE: {
- switch (arg) {
+ switch (arg) {
- /* switching to manual polling */
- case SENSOR_POLLRATE_MANUAL:
- stop();
- _measure_ticks = 0;
- return OK;
+ /* switching to manual polling */
+ case SENSOR_POLLRATE_MANUAL:
+ stop();
+ _measure_ticks = 0;
+ return OK;
- /* external signalling (DRDY) not supported */
- case SENSOR_POLLRATE_EXTERNAL:
+ /* external signalling (DRDY) not supported */
+ case SENSOR_POLLRATE_EXTERNAL:
- /* zero would be bad */
- case 0:
- return -EINVAL;
+ /* zero would be bad */
+ case 0:
+ return -EINVAL;
- /* set default/max polling rate */
- case SENSOR_POLLRATE_MAX:
- case SENSOR_POLLRATE_DEFAULT: {
- /* do we need to start internal polling? */
- bool want_start = (_measure_ticks == 0);
+ /* set default/max polling rate */
+ case SENSOR_POLLRATE_MAX:
+ case SENSOR_POLLRATE_DEFAULT: {
+ /* do we need to start internal polling? */
+ bool want_start = (_measure_ticks == 0);
- /* set interval for next measurement to minimum legal value */
- _measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL);
+ /* set interval for next measurement to minimum legal value */
+ _measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL);
- /* if we need to start the poll state machine, do it */
- if (want_start)
- start();
+ /* if we need to start the poll state machine, do it */
+ if (want_start)
+ start();
- return OK;
- }
+ return OK;
+ }
- /* adjust to a legal polling interval in Hz */
- default: {
- /* do we need to start internal polling? */
- bool want_start = (_measure_ticks == 0);
+ /* adjust to a legal polling interval in Hz */
+ default: {
+ /* do we need to start internal polling? */
+ bool want_start = (_measure_ticks == 0);
- /* convert hz to tick interval via microseconds */
- unsigned ticks = USEC2TICK(1000000 / arg);
+ /* convert hz to tick interval via microseconds */
+ unsigned ticks = USEC2TICK(1000000 / arg);
- /* check against maximum rate */
- if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL))
- return -EINVAL;
+ /* check against maximum rate */
+ if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL))
+ return -EINVAL;
- /* update interval for next measurement */
- _measure_ticks = ticks;
+ /* update interval for next measurement */
+ _measure_ticks = ticks;
- /* if we need to start the poll state machine, do it */
- if (want_start)
- start();
+ /* if we need to start the poll state machine, do it */
+ if (want_start)
+ start();
- return OK;
- }
+ return OK;
}
}
+ }
case SENSORIOCGPOLLRATE:
if (_measure_ticks == 0)
return SENSOR_POLLRATE_MANUAL;
- return (1000 / _measure_ticks);
+ return 1000000/TICK2USEC(_measure_ticks);
case SENSORIOCSQUEUEDEPTH: {
/* add one to account for the sentinel in the ring */
@@ -630,17 +637,24 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
return _num_reports - 1;
case SENSORIOCRESET:
- /* XXX implement this */
- return -EINVAL;
+ return reset();
case MAGIOCSSAMPLERATE:
- /* not supported, always 1 sample per poll */
- return -EINVAL;
+ /* same as pollrate because device is in single measurement mode*/
+ return ioctl(filp, SENSORIOCSPOLLRATE, arg);
+
+ case MAGIOCGSAMPLERATE:
+ /* same as pollrate because device is in single measurement mode*/
+ return 1000000/TICK2USEC(_measure_ticks);
case MAGIOCSRANGE:
return set_range(arg);
+ case MAGIOCGRANGE:
+ return _range_ga;
+
case MAGIOCSLOWPASS:
+ case MAGIOCGLOWPASS:
/* not supported, no internal filtering */
return -EINVAL;
@@ -665,6 +679,12 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
case MAGIOCSELFTEST:
return check_calibration();
+ case MAGIOCGEXTERNAL:
+ if (_bus == PX4_I2C_BUS_EXPANSION)
+ return 1;
+ else
+ return 0;
+
default:
/* give it to the superclass */
return I2C::ioctl(filp, cmd, arg);
@@ -688,6 +708,13 @@ HMC5883::stop()
work_cancel(HPWORK, &_work);
}
+int
+HMC5883::reset()
+{
+ /* set range */
+ return set_range(_range_ga);
+}
+
void
HMC5883::cycle_trampoline(void *arg)
{
@@ -851,10 +878,11 @@ HMC5883::collect()
_reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
} else {
#endif
- /* XXX axis assignment of external sensor is yet unknown */
- _reports[_next_report].x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
+ /* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down,
+ * therefore switch x and y and invert y */
+ _reports[_next_report].x = ((((report.y == -32768) ? 32767 : -report.y) * _range_scale) - _scale.x_offset) * _scale.x_scale;
/* flip axes and negate value for y */
- _reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale;
+ _reports[_next_report].y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
/* z remains z */
_reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
#ifdef PX4_I2C_BUS_ONBOARD
@@ -1293,6 +1321,11 @@ test()
warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z);
warnx("time: %lld", report.timestamp);
+ /* check if mag is onboard or external */
+ if ((ret = ioctl(fd, MAGIOCGEXTERNAL, 0)) < 0)
+ errx(1, "failed to get if mag is onboard or external");
+ warnx("device active: %s", ret ? "external" : "onboard");
+
/* set the queue depth to 10 */
if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10))
errx(1, "failed to set queue depth");
diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp
index 57c25633945f23bf8049b5b2c2f2ea18d8dbc20d..bb8d45beab9a0005ed0006f7131a0a9edbedf3c6 100644
--- a/src/drivers/hott/messages.cpp
+++ b/src/drivers/hott/messages.cpp
@@ -42,7 +42,7 @@
#include <math.h>
#include <stdio.h>
#include <string.h>
-#include <systemlib/geo/geo.h>
+#include <geo/geo.h>
#include <unistd.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 744abfa006e9bff4cd342823dc1f8ddf0b931c9e..5e0a2119a1a4ef44b87566394f422ec25be49da0 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -59,11 +59,11 @@
#include <nuttx/clock.h>
#include <drivers/drv_hrt.h>
-#include <arch/board/board.h>
-
#include <drivers/device/spi.h>
#include <drivers/drv_gyro.h>
+#include <board_config.h>
+#include <mathlib/math/filter/LowPassFilter2p.hpp>
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
@@ -71,6 +71,12 @@
#endif
static const int ERROR = -1;
+/* Orientation on board */
+#define SENSOR_BOARD_ROTATION_000_DEG 0
+#define SENSOR_BOARD_ROTATION_090_DEG 1
+#define SENSOR_BOARD_ROTATION_180_DEG 2
+#define SENSOR_BOARD_ROTATION_270_DEG 3
+
/* SPI protocol address bits */
#define DIR_READ (1<<7)
#define DIR_WRITE (0<<7)
@@ -78,6 +84,7 @@ static const int ERROR = -1;
/* register addresses */
#define ADDR_WHO_AM_I 0x0F
+#define WHO_I_AM_H 0xD7
#define WHO_I_AM 0xD4
#define ADDR_CTRL_REG1 0x20
@@ -85,8 +92,8 @@ static const int ERROR = -1;
/* keep lowpass low to avoid noise issues */
#define RATE_95HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4))
#define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4))
-#define RATE_380HZ_LP_30HZ ((1<<7) | (0<<6) | (1<<5) | (1<<4))
-#define RATE_760HZ_LP_30HZ ((1<<7) | (1<<6) | (1<<5) | (1<<4))
+#define RATE_380HZ_LP_20HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4))
+#define RATE_760HZ_LP_30HZ ((1<<7) | (1<<6) | (0<<5) | (0<<4))
#define ADDR_CTRL_REG2 0x21
#define ADDR_CTRL_REG3 0x22
@@ -147,6 +154,10 @@ static const int ERROR = -1;
#define FIFO_CTRL_STREAM_TO_FIFO_MODE (3<<5)
#define FIFO_CTRL_BYPASS_TO_STREAM_MODE (1<<7)
+#define L3GD20_DEFAULT_RATE 760
+#define L3GD20_DEFAULT_RANGE_DPS 2000
+#define L3GD20_DEFAULT_FILTER_FREQ 30
+
extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); }
class L3GD20 : public device::SPI
@@ -184,10 +195,16 @@ private:
orb_advert_t _gyro_topic;
unsigned _current_rate;
- unsigned _current_range;
+ unsigned _orientation;
+
+ unsigned _read;
perf_counter_t _sample_perf;
+ math::LowPassFilter2p _gyro_filter_x;
+ math::LowPassFilter2p _gyro_filter_y;
+ math::LowPassFilter2p _gyro_filter_z;
+
/**
* Start automatic measurement.
*/
@@ -198,6 +215,11 @@ private:
*/
void stop();
+ /**
+ * Reset the driver
+ */
+ void reset();
+
/**
* Static trampoline from the hrt_call context; because we don't have a
* generic hrt wrapper yet.
@@ -261,6 +283,14 @@ private:
*/
int set_samplerate(unsigned frequency);
+ /**
+ * Set the lowpass filter of the driver
+ *
+ * @param samplerate The current samplerate
+ * @param frequency The cutoff frequency for the lowpass filter
+ */
+ void set_driver_lowpass_filter(float samplerate, float bandwidth);
+
/**
* Self test
*
@@ -284,8 +314,12 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
_gyro_range_rad_s(0.0f),
_gyro_topic(-1),
_current_rate(0),
- _current_range(0),
- _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read"))
+ _orientation(SENSOR_BOARD_ROTATION_270_DEG),
+ _read(0),
+ _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")),
+ _gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
+ _gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
+ _gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ)
{
// enable debug() calls
_debug_enabled = true;
@@ -333,23 +367,7 @@ L3GD20::init()
memset(&_reports[0], 0, sizeof(_reports[0]));
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_reports[0]);
- /* set default configuration */
- write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
- write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */
- write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */
- write_reg(ADDR_CTRL_REG4, REG4_BDU);
- write_reg(ADDR_CTRL_REG5, 0);
-
-
- write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */
-
- /* disable FIFO. This makes things simpler and ensures we
- * aren't getting stale data. It means we must run the hrt
- * callback fast enough to not miss data. */
- write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE);
-
- set_range(500); /* default to 500dps */
- set_samplerate(0); /* max sample rate */
+ reset();
ret = OK;
out:
@@ -362,9 +380,24 @@ L3GD20::probe()
/* read dummy value to void to clear SPI statemachine on sensor */
(void)read_reg(ADDR_WHO_AM_I);
- /* verify that the device is attached and functioning */
- if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM)
+ /* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */
+ if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) {
+
+ #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+ _orientation = SENSOR_BOARD_ROTATION_270_DEG;
+ #elif CONFIG_ARCH_BOARD_PX4FMU_V2
+ _orientation = SENSOR_BOARD_ROTATION_270_DEG;
+ #else
+ #error This driver needs a board selection, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2
+ #endif
+ return OK;
+ }
+
+
+ if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) {
+ _orientation = SENSOR_BOARD_ROTATION_180_DEG;
return OK;
+ }
return -EIO;
}
@@ -434,8 +467,7 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT:
- /* With internal low pass filters enabled, 250 Hz is sufficient */
- return ioctl(filp, SENSORIOCSPOLLRATE, 250);
+ return ioctl(filp, SENSORIOCSPOLLRATE, L3GD20_DEFAULT_RATE);
/* adjust to a legal polling interval in Hz */
default: {
@@ -453,6 +485,11 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
/* XXX this is a bit shady, but no other way to adjust... */
_call.period = _call_interval = ticks;
+ /* adjust filters */
+ float cutoff_freq_hz = _gyro_filter_x.get_cutoff_freq();
+ float sample_rate = 1.0e6f/ticks;
+ set_driver_lowpass_filter(sample_rate, cutoff_freq_hz);
+
/* if we need to start the poll state machine, do it */
if (want_start)
start();
@@ -496,8 +533,8 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
return _num_reports - 1;
case SENSORIOCRESET:
- /* XXX implement */
- return -EINVAL;
+ reset();
+ return OK;
case GYROIOCSSAMPLERATE:
return set_samplerate(arg);
@@ -505,10 +542,16 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
case GYROIOCGSAMPLERATE:
return _current_rate;
- case GYROIOCSLOWPASS:
+ case GYROIOCSLOWPASS: {
+ float cutoff_freq_hz = arg;
+ float sample_rate = 1.0e6f / _call_interval;
+ set_driver_lowpass_filter(sample_rate, cutoff_freq_hz);
+
+ return OK;
+ }
+
case GYROIOCGLOWPASS:
- /* XXX not implemented due to wacky interaction with samplerate */
- return -EINVAL;
+ return _gyro_filter_x.get_cutoff_freq();
case GYROIOCSSCALE:
/* copy scale in */
@@ -521,10 +564,12 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
return OK;
case GYROIOCSRANGE:
+ /* arg should be in dps */
return set_range(arg);
case GYROIOCGRANGE:
- return _current_range;
+ /* convert to dps and round */
+ return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f);
case GYROIOCSELFTEST:
return self_test();
@@ -573,28 +618,33 @@ int
L3GD20::set_range(unsigned max_dps)
{
uint8_t bits = REG4_BDU;
+ float new_range_scale_dps_digit;
+ float new_range;
- if (max_dps == 0)
+ if (max_dps == 0) {
max_dps = 2000;
-
+ }
if (max_dps <= 250) {
- _current_range = 250;
+ new_range = 250;
bits |= RANGE_250DPS;
+ new_range_scale_dps_digit = 8.75e-3f;
} else if (max_dps <= 500) {
- _current_range = 500;
+ new_range = 500;
bits |= RANGE_500DPS;
+ new_range_scale_dps_digit = 17.5e-3f;
} else if (max_dps <= 2000) {
- _current_range = 2000;
+ new_range = 2000;
bits |= RANGE_2000DPS;
+ new_range_scale_dps_digit = 70e-3f;
} else {
return -EINVAL;
}
- _gyro_range_rad_s = _current_range / 180.0f * M_PI_F;
- _gyro_range_scale = _gyro_range_rad_s / 32768.0f;
+ _gyro_range_rad_s = new_range / 180.0f * M_PI_F;
+ _gyro_range_scale = new_range_scale_dps_digit / 180.0f * M_PI_F;
write_reg(ADDR_CTRL_REG4, bits);
return OK;
@@ -608,19 +658,20 @@ L3GD20::set_samplerate(unsigned frequency)
if (frequency == 0)
frequency = 760;
- if (frequency <= 95) {
+ /* use limits good for H or non-H models */
+ if (frequency <= 100) {
_current_rate = 95;
bits |= RATE_95HZ_LP_25HZ;
- } else if (frequency <= 190) {
+ } else if (frequency <= 200) {
_current_rate = 190;
bits |= RATE_190HZ_LP_25HZ;
- } else if (frequency <= 380) {
+ } else if (frequency <= 400) {
_current_rate = 380;
- bits |= RATE_380HZ_LP_30HZ;
+ bits |= RATE_380HZ_LP_20HZ;
- } else if (frequency <= 760) {
+ } else if (frequency <= 800) {
_current_rate = 760;
bits |= RATE_760HZ_LP_30HZ;
@@ -633,6 +684,14 @@ L3GD20::set_samplerate(unsigned frequency)
return OK;
}
+void
+L3GD20::set_driver_lowpass_filter(float samplerate, float bandwidth)
+{
+ _gyro_filter_x.set_cutoff_frequency(samplerate, bandwidth);
+ _gyro_filter_y.set_cutoff_frequency(samplerate, bandwidth);
+ _gyro_filter_z.set_cutoff_frequency(samplerate, bandwidth);
+}
+
void
L3GD20::start()
{
@@ -652,6 +711,30 @@ L3GD20::stop()
hrt_cancel(&_call);
}
+void
+L3GD20::reset()
+{
+ /* set default configuration */
+ write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
+ write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */
+ write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */
+ write_reg(ADDR_CTRL_REG4, REG4_BDU);
+ write_reg(ADDR_CTRL_REG5, 0);
+
+ write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */
+
+ /* disable FIFO. This makes things simpler and ensures we
+ * aren't getting stale data. It means we must run the hrt
+ * callback fast enough to not miss data. */
+ write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE);
+
+ set_samplerate(L3GD20_DEFAULT_RATE);
+ set_range(L3GD20_DEFAULT_RANGE_DPS);
+ set_driver_lowpass_filter(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ);
+
+ _read = 0;
+}
+
void
L3GD20::measure_trampoline(void *arg)
{
@@ -701,14 +784,43 @@ L3GD20::measure()
*/
report->timestamp = hrt_absolute_time();
- /* swap x and y and negate y */
- report->x_raw = raw_report.y;
- report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
+ switch (_orientation) {
+
+ case SENSOR_BOARD_ROTATION_000_DEG:
+ /* keep axes in place */
+ report->x_raw = raw_report.x;
+ report->y_raw = raw_report.y;
+ break;
+
+ case SENSOR_BOARD_ROTATION_090_DEG:
+ /* swap x and y */
+ report->x_raw = raw_report.y;
+ report->y_raw = raw_report.x;
+ break;
+
+ case SENSOR_BOARD_ROTATION_180_DEG:
+ /* swap x and y and negate both */
+ report->x_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
+ report->y_raw = ((raw_report.y == -32768) ? 32767 : -raw_report.y);
+ break;
+
+ case SENSOR_BOARD_ROTATION_270_DEG:
+ /* swap x and y and negate y */
+ report->x_raw = raw_report.y;
+ report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
+ break;
+ }
+
report->z_raw = raw_report.z;
report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
report->y = ((report->y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
report->z = ((report->z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
+
+ report->x = _gyro_filter_x.apply(report->x);
+ report->y = _gyro_filter_y.apply(report->y);
+ report->z = _gyro_filter_z.apply(report->z);
+
report->scaling = _gyro_range_scale;
report->range_rad_s = _gyro_range_rad_s;
@@ -726,6 +838,8 @@ L3GD20::measure()
if (_gyro_topic > 0)
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report);
+ _read++;
+
/* stop the perf counter */
perf_end(_sample_perf);
}
@@ -733,6 +847,7 @@ L3GD20::measure()
void
L3GD20::print_info()
{
+ printf("gyro reads: %u\n", _read);
perf_print_counter(_sample_perf);
printf("report queue: %u (%u/%u @ %p)\n",
_num_reports, _oldest_report, _next_report, _reports);
@@ -782,7 +897,7 @@ start()
int fd;
if (g_dev != nullptr)
- errx(1, "already started");
+ errx(0, "already started");
/* create the driver */
g_dev = new L3GD20(1 /* XXX magic number */, GYRO_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO);
@@ -871,7 +986,7 @@ reset()
err(1, "driver reset failed");
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
- err(1, "driver poll restart failed");
+ err(1, "accel pollrate reset failed");
exit(0);
}
diff --git a/src/drivers/led/led.cpp b/src/drivers/led/led.cpp
index 04b565358ca73b5ca69f091fca85e181c99abc05..a37eaca533d98ff5bd4ddc01ef84bf8e6450d728 100644
--- a/src/drivers/led/led.cpp
+++ b/src/drivers/led/led.cpp
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * 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
@@ -52,6 +52,7 @@ __BEGIN_DECLS
extern void led_init();
extern void led_on(int led);
extern void led_off(int led);
+extern void led_toggle(int led);
__END_DECLS
class LED : device::CDev
@@ -98,6 +99,11 @@ LED::ioctl(struct file *filp, int cmd, unsigned long arg)
led_off(arg);
break;
+ case LED_TOGGLE:
+ led_toggle(arg);
+ break;
+
+
default:
result = CDev::ioctl(filp, cmd, arg);
}
@@ -110,11 +116,11 @@ LED *gLED;
}
void
-drv_led_start()
+drv_led_start(void)
{
if (gLED == nullptr) {
gLED = new LED;
if (gLED != nullptr)
gLED->init();
}
-}
\ No newline at end of file
+}
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cf5f8d94cedbe1813162fa1b53c33cebea4bae05
--- /dev/null
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -0,0 +1,1659 @@
+/****************************************************************************
+ *
+ * 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 lsm303d.cpp
+ * Driver for the ST LSM303D MEMS accelerometer / magnetometer connected via SPI.
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/clock.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/device/spi.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_mag.h>
+
+#include <board_config.h>
+#include <mathlib/math/filter/LowPassFilter2p.hpp>
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+/* SPI protocol address bits */
+#define DIR_READ (1<<7)
+#define DIR_WRITE (0<<7)
+#define ADDR_INCREMENT (1<<6)
+
+
+
+/* register addresses: A: accel, M: mag, T: temp */
+#define ADDR_WHO_AM_I 0x0F
+#define WHO_I_AM 0x49
+
+#define ADDR_OUT_L_T 0x05
+#define ADDR_OUT_H_T 0x06
+#define ADDR_STATUS_M 0x07
+#define ADDR_OUT_X_L_M 0x08
+#define ADDR_OUT_X_H_M 0x09
+#define ADDR_OUT_Y_L_M 0x0A
+#define ADDR_OUT_Y_H_M 0x0B
+#define ADDR_OUT_Z_L_M 0x0C
+#define ADDR_OUT_Z_H_M 0x0D
+
+#define ADDR_OUT_TEMP_A 0x26
+#define ADDR_STATUS_A 0x27
+#define ADDR_OUT_X_L_A 0x28
+#define ADDR_OUT_X_H_A 0x29
+#define ADDR_OUT_Y_L_A 0x2A
+#define ADDR_OUT_Y_H_A 0x2B
+#define ADDR_OUT_Z_L_A 0x2C
+#define ADDR_OUT_Z_H_A 0x2D
+
+#define ADDR_CTRL_REG0 0x1F
+#define ADDR_CTRL_REG1 0x20
+#define ADDR_CTRL_REG2 0x21
+#define ADDR_CTRL_REG3 0x22
+#define ADDR_CTRL_REG4 0x23
+#define ADDR_CTRL_REG5 0x24
+#define ADDR_CTRL_REG6 0x25
+#define ADDR_CTRL_REG7 0x26
+
+#define REG1_RATE_BITS_A ((1<<7) | (1<<6) | (1<<5) | (1<<4))
+#define REG1_POWERDOWN_A ((0<<7) | (0<<6) | (0<<5) | (0<<4))
+#define REG1_RATE_3_125HZ_A ((0<<7) | (0<<6) | (0<<5) | (1<<4))
+#define REG1_RATE_6_25HZ_A ((0<<7) | (0<<6) | (1<<5) | (0<<4))
+#define REG1_RATE_12_5HZ_A ((0<<7) | (0<<6) | (1<<5) | (1<<4))
+#define REG1_RATE_25HZ_A ((0<<7) | (1<<6) | (0<<5) | (0<<4))
+#define REG1_RATE_50HZ_A ((0<<7) | (1<<6) | (0<<5) | (1<<4))
+#define REG1_RATE_100HZ_A ((0<<7) | (1<<6) | (1<<5) | (0<<4))
+#define REG1_RATE_200HZ_A ((0<<7) | (1<<6) | (1<<5) | (1<<4))
+#define REG1_RATE_400HZ_A ((1<<7) | (0<<6) | (0<<5) | (0<<4))
+#define REG1_RATE_800HZ_A ((1<<7) | (0<<6) | (0<<5) | (1<<4))
+#define REG1_RATE_1600HZ_A ((1<<7) | (0<<6) | (1<<5) | (0<<4))
+
+#define REG1_BDU_UPDATE (1<<3)
+#define REG1_Z_ENABLE_A (1<<2)
+#define REG1_Y_ENABLE_A (1<<1)
+#define REG1_X_ENABLE_A (1<<0)
+
+#define REG2_ANTIALIAS_FILTER_BW_BITS_A ((1<<7) | (1<<6))
+#define REG2_AA_FILTER_BW_773HZ_A ((0<<7) | (0<<6))
+#define REG2_AA_FILTER_BW_194HZ_A ((0<<7) | (1<<6))
+#define REG2_AA_FILTER_BW_362HZ_A ((1<<7) | (0<<6))
+#define REG2_AA_FILTER_BW_50HZ_A ((1<<7) | (1<<6))
+
+#define REG2_FULL_SCALE_BITS_A ((1<<5) | (1<<4) | (1<<3))
+#define REG2_FULL_SCALE_2G_A ((0<<5) | (0<<4) | (0<<3))
+#define REG2_FULL_SCALE_4G_A ((0<<5) | (0<<4) | (1<<3))
+#define REG2_FULL_SCALE_6G_A ((0<<5) | (1<<4) | (0<<3))
+#define REG2_FULL_SCALE_8G_A ((0<<5) | (1<<4) | (1<<3))
+#define REG2_FULL_SCALE_16G_A ((1<<5) | (0<<4) | (0<<3))
+
+#define REG5_ENABLE_T (1<<7)
+
+#define REG5_RES_HIGH_M ((1<<6) | (1<<5))
+#define REG5_RES_LOW_M ((0<<6) | (0<<5))
+
+#define REG5_RATE_BITS_M ((1<<4) | (1<<3) | (1<<2))
+#define REG5_RATE_3_125HZ_M ((0<<4) | (0<<3) | (0<<2))
+#define REG5_RATE_6_25HZ_M ((0<<4) | (0<<3) | (1<<2))
+#define REG5_RATE_12_5HZ_M ((0<<4) | (1<<3) | (0<<2))
+#define REG5_RATE_25HZ_M ((0<<4) | (1<<3) | (1<<2))
+#define REG5_RATE_50HZ_M ((1<<4) | (0<<3) | (0<<2))
+#define REG5_RATE_100HZ_M ((1<<4) | (0<<3) | (1<<2))
+#define REG5_RATE_DO_NOT_USE_M ((1<<4) | (1<<3) | (0<<2))
+
+#define REG6_FULL_SCALE_BITS_M ((1<<6) | (1<<5))
+#define REG6_FULL_SCALE_2GA_M ((0<<6) | (0<<5))
+#define REG6_FULL_SCALE_4GA_M ((0<<6) | (1<<5))
+#define REG6_FULL_SCALE_8GA_M ((1<<6) | (0<<5))
+#define REG6_FULL_SCALE_12GA_M ((1<<6) | (1<<5))
+
+#define REG7_CONT_MODE_M ((0<<1) | (0<<0))
+
+
+#define INT_CTRL_M 0x12
+#define INT_SRC_M 0x13
+
+/* default values for this device */
+#define LSM303D_ACCEL_DEFAULT_RANGE_G 8
+#define LSM303D_ACCEL_DEFAULT_RATE 800
+#define LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ 50
+#define LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30
+
+#define LSM303D_MAG_DEFAULT_RANGE_GA 2
+#define LSM303D_MAG_DEFAULT_RATE 100
+
+#define LSM303D_ONE_G 9.80665f
+
+extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); }
+
+
+class LSM303D_mag;
+
+class LSM303D : public device::SPI
+{
+public:
+ LSM303D(int bus, const char* path, spi_dev_e device);
+ virtual ~LSM303D();
+
+ virtual int init();
+
+ virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
+ virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
+
+ /**
+ * Diagnostics - print some basic information about the driver.
+ */
+ void print_info();
+
+protected:
+ virtual int probe();
+
+ friend class LSM303D_mag;
+
+ virtual ssize_t mag_read(struct file *filp, char *buffer, size_t buflen);
+ virtual int mag_ioctl(struct file *filp, int cmd, unsigned long arg);
+
+private:
+
+ LSM303D_mag *_mag;
+
+ struct hrt_call _accel_call;
+ struct hrt_call _mag_call;
+
+ unsigned _call_accel_interval;
+ unsigned _call_mag_interval;
+
+ unsigned _num_accel_reports;
+ volatile unsigned _next_accel_report;
+ volatile unsigned _oldest_accel_report;
+ struct accel_report *_accel_reports;
+
+ unsigned _num_mag_reports;
+ volatile unsigned _next_mag_report;
+ volatile unsigned _oldest_mag_report;
+ struct mag_report *_mag_reports;
+
+ struct accel_scale _accel_scale;
+ unsigned _accel_range_m_s2;
+ float _accel_range_scale;
+ unsigned _accel_samplerate;
+ unsigned _accel_onchip_filter_bandwith;
+
+ struct mag_scale _mag_scale;
+ unsigned _mag_range_ga;
+ float _mag_range_scale;
+ unsigned _mag_samplerate;
+
+ orb_advert_t _accel_topic;
+ orb_advert_t _mag_topic;
+
+ unsigned _accel_read;
+ unsigned _mag_read;
+
+ perf_counter_t _accel_sample_perf;
+ perf_counter_t _mag_sample_perf;
+
+ math::LowPassFilter2p _accel_filter_x;
+ math::LowPassFilter2p _accel_filter_y;
+ math::LowPassFilter2p _accel_filter_z;
+
+ /**
+ * Start automatic measurement.
+ */
+ void start();
+
+ /**
+ * Stop automatic measurement.
+ */
+ void stop();
+
+ /**
+ * Reset chip.
+ *
+ * Resets the chip and measurements ranges, but not scale and offset.
+ */
+ void reset();
+
+ /**
+ * Static trampoline from the hrt_call context; because we don't have a
+ * generic hrt wrapper yet.
+ *
+ * Called by the HRT in interrupt context at the specified rate if
+ * automatic polling is enabled.
+ *
+ * @param arg Instance pointer for the driver that is polling.
+ */
+ static void measure_trampoline(void *arg);
+
+ /**
+ * Static trampoline for the mag because it runs at a lower rate
+ *
+ * @param arg Instance pointer for the driver that is polling.
+ */
+ static void mag_measure_trampoline(void *arg);
+
+ /**
+ * Fetch accel measurements from the sensor and update the report ring.
+ */
+ void measure();
+
+ /**
+ * Fetch mag measurements from the sensor and update the report ring.
+ */
+ void mag_measure();
+
+ /**
+ * Accel self test
+ *
+ * @return 0 on success, 1 on failure
+ */
+ int accel_self_test();
+
+ /**
+ * Mag self test
+ *
+ * @return 0 on success, 1 on failure
+ */
+ int mag_self_test();
+
+ /**
+ * Read a register from the LSM303D
+ *
+ * @param The register to read.
+ * @return The value that was read.
+ */
+ uint8_t read_reg(unsigned reg);
+
+ /**
+ * Write a register in the LSM303D
+ *
+ * @param reg The register to write.
+ * @param value The new value to write.
+ */
+ void write_reg(unsigned reg, uint8_t value);
+
+ /**
+ * Modify a register in the LSM303D
+ *
+ * Bits are cleared before bits are set.
+ *
+ * @param reg The register to modify.
+ * @param clearbits Bits in the register to clear.
+ * @param setbits Bits in the register to set.
+ */
+ void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
+
+ /**
+ * Set the LSM303D accel measurement range.
+ *
+ * @param max_g The measurement range of the accel is in g (9.81m/s^2)
+ * Zero selects the maximum supported range.
+ * @return OK if the value can be supported, -ERANGE otherwise.
+ */
+ int accel_set_range(unsigned max_g);
+
+ /**
+ * Set the LSM303D mag measurement range.
+ *
+ * @param max_ga The measurement range of the mag is in Ga
+ * Zero selects the maximum supported range.
+ * @return OK if the value can be supported, -ERANGE otherwise.
+ */
+ int mag_set_range(unsigned max_g);
+
+ /**
+ * Set the LSM303D on-chip anti-alias filter bandwith.
+ *
+ * @param bandwidth The anti-alias filter bandwidth in Hz
+ * Zero selects the highest bandwidth
+ * @return OK if the value can be supported, -ERANGE otherwise.
+ */
+ int accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth);
+
+ /**
+ * Set the driver lowpass filter bandwidth.
+ *
+ * @param bandwidth The anti-alias filter bandwidth in Hz
+ * Zero selects the highest bandwidth
+ * @return OK if the value can be supported, -ERANGE otherwise.
+ */
+ int accel_set_driver_lowpass_filter(float samplerate, float bandwidth);
+
+ /**
+ * Set the LSM303D internal accel sampling frequency.
+ *
+ * @param frequency The internal accel sampling frequency is set to not less than
+ * this value.
+ * Zero selects the maximum rate supported.
+ * @return OK if the value can be supported.
+ */
+ int accel_set_samplerate(unsigned frequency);
+
+ /**
+ * Set the LSM303D internal mag sampling frequency.
+ *
+ * @param frequency The internal mag sampling frequency is set to not less than
+ * this value.
+ * Zero selects the maximum rate supported.
+ * @return OK if the value can be supported.
+ */
+ int mag_set_samplerate(unsigned frequency);
+};
+
+/**
+ * Helper class implementing the mag driver node.
+ */
+class LSM303D_mag : public device::CDev
+{
+public:
+ LSM303D_mag(LSM303D *parent);
+ ~LSM303D_mag();
+
+ virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
+ virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
+
+protected:
+ friend class LSM303D;
+
+ void parent_poll_notify();
+private:
+ LSM303D *_parent;
+
+ void measure();
+
+ void measure_trampoline(void *arg);
+};
+
+
+/* helper macro for handling report buffer indices */
+#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0)
+
+
+LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
+ SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 8000000),
+ _mag(new LSM303D_mag(this)),
+ _call_accel_interval(0),
+ _call_mag_interval(0),
+ _num_accel_reports(0),
+ _next_accel_report(0),
+ _oldest_accel_report(0),
+ _accel_reports(nullptr),
+ _num_mag_reports(0),
+ _next_mag_report(0),
+ _oldest_mag_report(0),
+ _mag_reports(nullptr),
+ _accel_range_m_s2(0.0f),
+ _accel_range_scale(0.0f),
+ _accel_samplerate(0),
+ _accel_onchip_filter_bandwith(0),
+ _mag_range_ga(0.0f),
+ _mag_range_scale(0.0f),
+ _mag_samplerate(0),
+ _accel_topic(-1),
+ _mag_topic(-1),
+ _accel_read(0),
+ _mag_read(0),
+ _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
+ _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")),
+ _accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+ _accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+ _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ)
+{
+ // enable debug() calls
+ _debug_enabled = true;
+
+ // default scale factors
+ _accel_scale.x_offset = 0.0f;
+ _accel_scale.x_scale = 1.0f;
+ _accel_scale.y_offset = 0.0f;
+ _accel_scale.y_scale = 1.0f;
+ _accel_scale.z_offset = 0.0f;
+ _accel_scale.z_scale = 1.0f;
+
+ _mag_scale.x_offset = 0.0f;
+ _mag_scale.x_scale = 1.0f;
+ _mag_scale.y_offset = 0.0f;
+ _mag_scale.y_scale = 1.0f;
+ _mag_scale.z_offset = 0.0f;
+ _mag_scale.z_scale = 1.0f;
+}
+
+LSM303D::~LSM303D()
+{
+ /* make sure we are truly inactive */
+ stop();
+
+ /* free any existing reports */
+ if (_accel_reports != nullptr)
+ delete[] _accel_reports;
+ if (_mag_reports != nullptr)
+ delete[] _mag_reports;
+
+ delete _mag;
+
+ /* delete the perf counter */
+ perf_free(_accel_sample_perf);
+ perf_free(_mag_sample_perf);
+}
+
+int
+LSM303D::init()
+{
+ int ret = ERROR;
+ int mag_ret;
+
+ /* do SPI init (and probe) first */
+ if (SPI::init() != OK)
+ goto out;
+
+ /* allocate basic report buffers */
+ _num_accel_reports = 2;
+ _oldest_accel_report = _next_accel_report = 0;
+ _accel_reports = new struct accel_report[_num_accel_reports];
+
+ if (_accel_reports == nullptr)
+ goto out;
+
+ /* advertise accel topic */
+ memset(&_accel_reports[0], 0, sizeof(_accel_reports[0]));
+ _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_reports[0]);
+
+ _num_mag_reports = 2;
+ _oldest_mag_report = _next_mag_report = 0;
+ _mag_reports = new struct mag_report[_num_mag_reports];
+
+ if (_mag_reports == nullptr)
+ goto out;
+
+ reset();
+
+ /* advertise mag topic */
+ memset(&_mag_reports[0], 0, sizeof(_mag_reports[0]));
+ _mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]);
+
+ /* do CDev init for the mag device node, keep it optional */
+ mag_ret = _mag->init();
+
+ if (mag_ret != OK) {
+ _mag_topic = -1;
+ }
+
+ ret = OK;
+out:
+ return ret;
+}
+
+void
+LSM303D::reset()
+{
+ /* enable accel*/
+ write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE);
+
+ /* enable mag */
+ write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M);
+ write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M);
+
+ accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G);
+ accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE);
+ accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ);
+ accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ);
+
+ mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA);
+ mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE);
+
+ _accel_read = 0;
+ _mag_read = 0;
+}
+
+int
+LSM303D::probe()
+{
+ /* read dummy value to void to clear SPI statemachine on sensor */
+ (void)read_reg(ADDR_WHO_AM_I);
+
+ /* verify that the device is attached and functioning */
+ if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM)
+ return OK;
+
+ return -EIO;
+}
+
+ssize_t
+LSM303D::read(struct file *filp, char *buffer, size_t buflen)
+{
+ unsigned count = buflen / sizeof(struct accel_report);
+ int ret = 0;
+
+ /* buffer must be large enough */
+ if (count < 1)
+ return -ENOSPC;
+
+ /* if automatic measurement is enabled */
+ if (_call_accel_interval > 0) {
+
+ /*
+ * While there is space in the caller's buffer, and reports, copy them.
+ * Note that we may be pre-empted by the measurement code while we are doing this;
+ * we are careful to avoid racing with it.
+ */
+ while (count--) {
+ if (_oldest_accel_report != _next_accel_report) {
+ memcpy(buffer, _accel_reports + _oldest_accel_report, sizeof(*_accel_reports));
+ ret += sizeof(_accel_reports[0]);
+ INCREMENT(_oldest_accel_report, _num_accel_reports);
+ }
+ }
+
+ /* if there was no data, warn the caller */
+ return ret ? ret : -EAGAIN;
+ }
+
+ /* manual measurement */
+ _oldest_accel_report = _next_accel_report = 0;
+ measure();
+
+ /* measurement will have generated a report, copy it out */
+ memcpy(buffer, _accel_reports, sizeof(*_accel_reports));
+ ret = sizeof(*_accel_reports);
+
+ return ret;
+}
+
+ssize_t
+LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen)
+{
+ unsigned count = buflen / sizeof(struct mag_report);
+ int ret = 0;
+
+ /* buffer must be large enough */
+ if (count < 1)
+ return -ENOSPC;
+
+ /* if automatic measurement is enabled */
+ if (_call_mag_interval > 0) {
+
+ /*
+ * While there is space in the caller's buffer, and reports, copy them.
+ * Note that we may be pre-empted by the measurement code while we are doing this;
+ * we are careful to avoid racing with it.
+ */
+ while (count--) {
+ if (_oldest_mag_report != _next_mag_report) {
+ memcpy(buffer, _mag_reports + _oldest_mag_report, sizeof(*_mag_reports));
+ ret += sizeof(_mag_reports[0]);
+ INCREMENT(_oldest_mag_report, _num_mag_reports);
+ }
+ }
+
+ /* if there was no data, warn the caller */
+ return ret ? ret : -EAGAIN;
+ }
+
+ /* manual measurement */
+ _oldest_mag_report = _next_mag_report = 0;
+ measure();
+
+ /* measurement will have generated a report, copy it out */
+ memcpy(buffer, _mag_reports, sizeof(*_mag_reports));
+ ret = sizeof(*_mag_reports);
+
+ return ret;
+}
+
+int
+LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+ switch (cmd) {
+
+ case SENSORIOCSPOLLRATE: {
+ switch (arg) {
+
+ /* switching to manual polling */
+ case SENSOR_POLLRATE_MANUAL:
+ stop();
+ _call_accel_interval = 0;
+ return OK;
+
+ /* external signalling not supported */
+ case SENSOR_POLLRATE_EXTERNAL:
+
+ /* zero would be bad */
+ case 0:
+ return -EINVAL;
+
+ /* set default/max polling rate */
+ case SENSOR_POLLRATE_MAX:
+ return ioctl(filp, SENSORIOCSPOLLRATE, 1600);
+
+ case SENSOR_POLLRATE_DEFAULT:
+ return ioctl(filp, SENSORIOCSPOLLRATE, LSM303D_ACCEL_DEFAULT_RATE);
+
+ /* adjust to a legal polling interval in Hz */
+ default: {
+ /* do we need to start internal polling? */
+ bool want_start = (_call_accel_interval == 0);
+
+ /* convert hz to hrt interval via microseconds */
+ unsigned ticks = 1000000 / arg;
+
+ /* check against maximum sane rate */
+ if (ticks < 500)
+ return -EINVAL;
+
+ /* adjust filters */
+ accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq());
+
+ /* update interval for next measurement */
+ /* XXX this is a bit shady, but no other way to adjust... */
+ _accel_call.period = _call_accel_interval = ticks;
+
+ /* if we need to start the poll state machine, do it */
+ if (want_start)
+ start();
+
+ return OK;
+ }
+ }
+ }
+
+ case SENSORIOCGPOLLRATE:
+ if (_call_accel_interval == 0)
+ return SENSOR_POLLRATE_MANUAL;
+
+ return 1000000 / _call_accel_interval;
+
+ case SENSORIOCSQUEUEDEPTH: {
+ /* account for sentinel in the ring */
+ arg++;
+
+ /* lower bound is mandatory, upper bound is a sanity check */
+ if ((arg < 2) || (arg > 100))
+ return -EINVAL;
+
+ /* allocate new buffer */
+ struct accel_report *buf = new struct accel_report[arg];
+
+ if (nullptr == buf)
+ return -ENOMEM;
+
+ /* reset the measurement state machine with the new buffer, free the old */
+ stop();
+ delete[] _accel_reports;
+ _num_accel_reports = arg;
+ _accel_reports = buf;
+ start();
+
+ return OK;
+ }
+
+ case SENSORIOCGQUEUEDEPTH:
+ return _num_accel_reports - 1;
+
+ case SENSORIOCRESET:
+ reset();
+ return OK;
+
+ case ACCELIOCSSAMPLERATE:
+ return accel_set_samplerate(arg);
+
+ case ACCELIOCGSAMPLERATE:
+ return _accel_samplerate;
+
+ case ACCELIOCSLOWPASS: {
+ return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)arg);
+ }
+
+ case ACCELIOCGLOWPASS:
+ return _accel_filter_x.get_cutoff_freq();
+
+ case ACCELIOCSSCALE: {
+ /* copy scale, but only if off by a few percent */
+ struct accel_scale *s = (struct accel_scale *) arg;
+ float sum = s->x_scale + s->y_scale + s->z_scale;
+ if (sum > 2.0f && sum < 4.0f) {
+ memcpy(&_accel_scale, s, sizeof(_accel_scale));
+ return OK;
+ } else {
+ return -EINVAL;
+ }
+ }
+
+ case ACCELIOCSRANGE:
+ /* arg needs to be in G */
+ return accel_set_range(arg);
+
+ case ACCELIOCGRANGE:
+ /* convert to m/s^2 and return rounded in G */
+ return (unsigned long)((_accel_range_m_s2)/LSM303D_ONE_G + 0.5f);
+
+ case ACCELIOCGSCALE:
+ /* copy scale out */
+ memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale));
+ return OK;
+
+ case ACCELIOCSELFTEST:
+ return accel_self_test();
+
+ default:
+ /* give it to the superclass */
+ return SPI::ioctl(filp, cmd, arg);
+ }
+}
+
+int
+LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+ switch (cmd) {
+
+ case SENSORIOCSPOLLRATE: {
+ switch (arg) {
+
+ /* switching to manual polling */
+ case SENSOR_POLLRATE_MANUAL:
+ stop();
+ _call_mag_interval = 0;
+ return OK;
+
+ /* external signalling not supported */
+ case SENSOR_POLLRATE_EXTERNAL:
+
+ /* zero would be bad */
+ case 0:
+ return -EINVAL;
+
+ /* set default/max polling rate */
+ case SENSOR_POLLRATE_MAX:
+ case SENSOR_POLLRATE_DEFAULT:
+ /* 100 Hz is max for mag */
+ return mag_ioctl(filp, SENSORIOCSPOLLRATE, 100);
+
+ /* adjust to a legal polling interval in Hz */
+ default: {
+ /* do we need to start internal polling? */
+ bool want_start = (_call_mag_interval == 0);
+
+ /* convert hz to hrt interval via microseconds */
+ unsigned ticks = 1000000 / arg;
+
+ /* check against maximum sane rate */
+ if (ticks < 1000)
+ return -EINVAL;
+
+ /* update interval for next measurement */
+ /* XXX this is a bit shady, but no other way to adjust... */
+ _mag_call.period = _call_mag_interval = ticks;
+
+ /* if we need to start the poll state machine, do it */
+ if (want_start)
+ start();
+
+ return OK;
+ }
+ }
+ }
+
+ case SENSORIOCGPOLLRATE:
+ if (_call_mag_interval == 0)
+ return SENSOR_POLLRATE_MANUAL;
+
+ return 1000000 / _call_mag_interval;
+
+ case SENSORIOCSQUEUEDEPTH: {
+ /* account for sentinel in the ring */
+ arg++;
+
+ /* lower bound is mandatory, upper bound is a sanity check */
+ if ((arg < 2) || (arg > 100))
+ return -EINVAL;
+
+ /* allocate new buffer */
+ struct mag_report *buf = new struct mag_report[arg];
+
+ if (nullptr == buf)
+ return -ENOMEM;
+
+ /* reset the measurement state machine with the new buffer, free the old */
+ stop();
+ delete[] _mag_reports;
+ _num_mag_reports = arg;
+ _mag_reports = buf;
+ start();
+
+ return OK;
+ }
+
+ case SENSORIOCGQUEUEDEPTH:
+ return _num_mag_reports - 1;
+
+ case SENSORIOCRESET:
+ reset();
+ return OK;
+
+ case MAGIOCSSAMPLERATE:
+ return mag_set_samplerate(arg);
+
+ case MAGIOCGSAMPLERATE:
+ return _mag_samplerate;
+
+ case MAGIOCSLOWPASS:
+ case MAGIOCGLOWPASS:
+ /* not supported, no internal filtering */
+ return -EINVAL;
+
+ case MAGIOCSSCALE:
+ /* copy scale in */
+ memcpy(&_mag_scale, (struct mag_scale *) arg, sizeof(_mag_scale));
+ return OK;
+
+ case MAGIOCGSCALE:
+ /* copy scale out */
+ memcpy((struct mag_scale *) arg, &_mag_scale, sizeof(_mag_scale));
+ return OK;
+
+ case MAGIOCSRANGE:
+ return mag_set_range(arg);
+
+ case MAGIOCGRANGE:
+ return _mag_range_ga;
+
+ case MAGIOCSELFTEST:
+ return mag_self_test();
+
+ case MAGIOCGEXTERNAL:
+ /* no external mag board yet */
+ return 0;
+
+ default:
+ /* give it to the superclass */
+ return SPI::ioctl(filp, cmd, arg);
+ }
+}
+
+int
+LSM303D::accel_self_test()
+{
+ if (_accel_read == 0)
+ return 1;
+
+ /* inspect accel offsets */
+ if (fabsf(_accel_scale.x_offset) < 0.000001f)
+ return 1;
+ if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f)
+ return 1;
+
+ if (fabsf(_accel_scale.y_offset) < 0.000001f)
+ return 1;
+ if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f)
+ return 1;
+
+ if (fabsf(_accel_scale.z_offset) < 0.000001f)
+ return 1;
+ if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f)
+ return 1;
+
+ return 0;
+}
+
+int
+LSM303D::mag_self_test()
+{
+ if (_mag_read == 0)
+ return 1;
+
+ /**
+ * inspect mag offsets
+ * don't check mag scale because it seems this is calibrated on chip
+ */
+ if (fabsf(_mag_scale.x_offset) < 0.000001f)
+ return 1;
+
+ if (fabsf(_mag_scale.y_offset) < 0.000001f)
+ return 1;
+
+ if (fabsf(_mag_scale.z_offset) < 0.000001f)
+ return 1;
+
+ return 0;
+}
+
+uint8_t
+LSM303D::read_reg(unsigned reg)
+{
+ uint8_t cmd[2];
+
+ cmd[0] = reg | DIR_READ;
+
+ transfer(cmd, cmd, sizeof(cmd));
+
+ return cmd[1];
+}
+
+void
+LSM303D::write_reg(unsigned reg, uint8_t value)
+{
+ uint8_t cmd[2];
+
+ cmd[0] = reg | DIR_WRITE;
+ cmd[1] = value;
+
+ transfer(cmd, nullptr, sizeof(cmd));
+}
+
+void
+LSM303D::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
+{
+ uint8_t val;
+
+ val = read_reg(reg);
+ val &= ~clearbits;
+ val |= setbits;
+ write_reg(reg, val);
+}
+
+int
+LSM303D::accel_set_range(unsigned max_g)
+{
+ uint8_t setbits = 0;
+ uint8_t clearbits = REG2_FULL_SCALE_BITS_A;
+ float new_scale_g_digit = 0.0f;
+
+ if (max_g == 0)
+ max_g = 16;
+
+ if (max_g <= 2) {
+ _accel_range_m_s2 = 2.0f*LSM303D_ONE_G;
+ setbits |= REG2_FULL_SCALE_2G_A;
+ new_scale_g_digit = 0.061e-3f;
+
+ } else if (max_g <= 4) {
+ _accel_range_m_s2 = 4.0f*LSM303D_ONE_G;
+ setbits |= REG2_FULL_SCALE_4G_A;
+ new_scale_g_digit = 0.122e-3f;
+
+ } else if (max_g <= 6) {
+ _accel_range_m_s2 = 6.0f*LSM303D_ONE_G;
+ setbits |= REG2_FULL_SCALE_6G_A;
+ new_scale_g_digit = 0.183e-3f;
+
+ } else if (max_g <= 8) {
+ _accel_range_m_s2 = 8.0f*LSM303D_ONE_G;
+ setbits |= REG2_FULL_SCALE_8G_A;
+ new_scale_g_digit = 0.244e-3f;
+
+ } else if (max_g <= 16) {
+ _accel_range_m_s2 = 16.0f*LSM303D_ONE_G;
+ setbits |= REG2_FULL_SCALE_16G_A;
+ new_scale_g_digit = 0.732e-3f;
+
+ } else {
+ return -EINVAL;
+ }
+
+ _accel_range_scale = new_scale_g_digit * LSM303D_ONE_G;
+
+ modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
+
+ return OK;
+}
+
+int
+LSM303D::mag_set_range(unsigned max_ga)
+{
+ uint8_t setbits = 0;
+ uint8_t clearbits = REG6_FULL_SCALE_BITS_M;
+ float new_scale_ga_digit = 0.0f;
+
+ if (max_ga == 0)
+ max_ga = 12;
+
+ if (max_ga <= 2) {
+ _mag_range_ga = 2;
+ setbits |= REG6_FULL_SCALE_2GA_M;
+ new_scale_ga_digit = 0.080e-3f;
+
+ } else if (max_ga <= 4) {
+ _mag_range_ga = 4;
+ setbits |= REG6_FULL_SCALE_4GA_M;
+ new_scale_ga_digit = 0.160e-3f;
+
+ } else if (max_ga <= 8) {
+ _mag_range_ga = 8;
+ setbits |= REG6_FULL_SCALE_8GA_M;
+ new_scale_ga_digit = 0.320e-3f;
+
+ } else if (max_ga <= 12) {
+ _mag_range_ga = 12;
+ setbits |= REG6_FULL_SCALE_12GA_M;
+ new_scale_ga_digit = 0.479e-3f;
+
+ } else {
+ return -EINVAL;
+ }
+
+ _mag_range_scale = new_scale_ga_digit;
+
+ modify_reg(ADDR_CTRL_REG6, clearbits, setbits);
+
+ return OK;
+}
+
+int
+LSM303D::accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth)
+{
+ uint8_t setbits = 0;
+ uint8_t clearbits = REG2_ANTIALIAS_FILTER_BW_BITS_A;
+
+ if (bandwidth == 0)
+ bandwidth = 773;
+
+ if (bandwidth <= 50) {
+ setbits |= REG2_AA_FILTER_BW_50HZ_A;
+ _accel_onchip_filter_bandwith = 50;
+
+ } else if (bandwidth <= 194) {
+ setbits |= REG2_AA_FILTER_BW_194HZ_A;
+ _accel_onchip_filter_bandwith = 194;
+
+ } else if (bandwidth <= 362) {
+ setbits |= REG2_AA_FILTER_BW_362HZ_A;
+ _accel_onchip_filter_bandwith = 362;
+
+ } else if (bandwidth <= 773) {
+ setbits |= REG2_AA_FILTER_BW_773HZ_A;
+ _accel_onchip_filter_bandwith = 773;
+
+ } else {
+ return -EINVAL;
+ }
+
+ modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
+
+ return OK;
+}
+
+int
+LSM303D::accel_set_driver_lowpass_filter(float samplerate, float bandwidth)
+{
+ _accel_filter_x.set_cutoff_frequency(samplerate, bandwidth);
+ _accel_filter_y.set_cutoff_frequency(samplerate, bandwidth);
+ _accel_filter_z.set_cutoff_frequency(samplerate, bandwidth);
+
+ return OK;
+}
+
+int
+LSM303D::accel_set_samplerate(unsigned frequency)
+{
+ uint8_t setbits = 0;
+ uint8_t clearbits = REG1_RATE_BITS_A;
+
+ if (frequency == 0)
+ frequency = 1600;
+
+ if (frequency <= 100) {
+ setbits |= REG1_RATE_100HZ_A;
+ _accel_samplerate = 100;
+
+ } else if (frequency <= 200) {
+ setbits |= REG1_RATE_200HZ_A;
+ _accel_samplerate = 200;
+
+ } else if (frequency <= 400) {
+ setbits |= REG1_RATE_400HZ_A;
+ _accel_samplerate = 400;
+
+ } else if (frequency <= 800) {
+ setbits |= REG1_RATE_800HZ_A;
+ _accel_samplerate = 800;
+
+ } else if (frequency <= 1600) {
+ setbits |= REG1_RATE_1600HZ_A;
+ _accel_samplerate = 1600;
+
+ } else {
+ return -EINVAL;
+ }
+
+ modify_reg(ADDR_CTRL_REG1, clearbits, setbits);
+
+ return OK;
+}
+
+int
+LSM303D::mag_set_samplerate(unsigned frequency)
+{
+ uint8_t setbits = 0;
+ uint8_t clearbits = REG5_RATE_BITS_M;
+
+ if (frequency == 0)
+ frequency = 100;
+
+ if (frequency <= 25) {
+ setbits |= REG5_RATE_25HZ_M;
+ _mag_samplerate = 25;
+
+ } else if (frequency <= 50) {
+ setbits |= REG5_RATE_50HZ_M;
+ _mag_samplerate = 50;
+
+ } else if (frequency <= 100) {
+ setbits |= REG5_RATE_100HZ_M;
+ _mag_samplerate = 100;
+
+ } else {
+ return -EINVAL;
+ }
+
+ modify_reg(ADDR_CTRL_REG5, clearbits, setbits);
+
+ return OK;
+}
+
+void
+LSM303D::start()
+{
+ /* make sure we are stopped first */
+ stop();
+
+ /* reset the report ring */
+ _oldest_accel_report = _next_accel_report = 0;
+ _oldest_mag_report = _next_mag_report = 0;
+
+ /* start polling at the specified rate */
+ hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&LSM303D::measure_trampoline, this);
+ hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&LSM303D::mag_measure_trampoline, this);
+}
+
+void
+LSM303D::stop()
+{
+ hrt_cancel(&_accel_call);
+ hrt_cancel(&_mag_call);
+}
+
+void
+LSM303D::measure_trampoline(void *arg)
+{
+ LSM303D *dev = (LSM303D *)arg;
+
+ /* make another measurement */
+ dev->measure();
+}
+
+void
+LSM303D::mag_measure_trampoline(void *arg)
+{
+ LSM303D *dev = (LSM303D *)arg;
+
+ /* make another measurement */
+ dev->mag_measure();
+}
+
+void
+LSM303D::measure()
+{
+ /* status register and data as read back from the device */
+
+#pragma pack(push, 1)
+ struct {
+ uint8_t cmd;
+ uint8_t status;
+ int16_t x;
+ int16_t y;
+ int16_t z;
+ } raw_accel_report;
+#pragma pack(pop)
+
+ accel_report *accel_report = &_accel_reports[_next_accel_report];
+
+ /* start the performance counter */
+ perf_begin(_accel_sample_perf);
+
+ /* fetch data from the sensor */
+ raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT;
+ transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report));
+
+ /*
+ * 1) Scale raw value to SI units using scaling from datasheet.
+ * 2) Subtract static offset (in SI units)
+ * 3) Scale the statically calibrated values with a linear
+ * dynamically obtained factor
+ *
+ * Note: the static sensor offset is the number the sensor outputs
+ * at a nominally 'zero' input. Therefore the offset has to
+ * be subtracted.
+ *
+ * Example: A gyro outputs a value of 74 at zero angular rate
+ * the offset is 74 from the origin and subtracting
+ * 74 from all measurements centers them around zero.
+ */
+
+
+ accel_report->timestamp = hrt_absolute_time();
+
+ accel_report->x_raw = raw_accel_report.x;
+ accel_report->y_raw = raw_accel_report.y;
+ accel_report->z_raw = raw_accel_report.z;
+
+ float x_in_new = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
+ float y_in_new = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
+ float z_in_new = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
+
+ accel_report->x = _accel_filter_x.apply(x_in_new);
+ accel_report->y = _accel_filter_y.apply(y_in_new);
+ accel_report->z = _accel_filter_z.apply(z_in_new);
+
+ accel_report->scaling = _accel_range_scale;
+ accel_report->range_m_s2 = _accel_range_m_s2;
+
+ /* post a report to the ring - note, not locked */
+ INCREMENT(_next_accel_report, _num_accel_reports);
+
+ /* if we are running up against the oldest report, fix it */
+ if (_next_accel_report == _oldest_accel_report)
+ INCREMENT(_oldest_accel_report, _num_accel_reports);
+
+ /* notify anyone waiting for data */
+ poll_notify(POLLIN);
+
+ /* publish for subscribers */
+ orb_publish(ORB_ID(sensor_accel), _accel_topic, accel_report);
+
+ _accel_read++;
+
+ /* stop the perf counter */
+ perf_end(_accel_sample_perf);
+}
+
+void
+LSM303D::mag_measure()
+{
+ /* status register and data as read back from the device */
+#pragma pack(push, 1)
+ struct {
+ uint8_t cmd;
+ uint8_t status;
+ int16_t x;
+ int16_t y;
+ int16_t z;
+ } raw_mag_report;
+#pragma pack(pop)
+
+ mag_report *mag_report = &_mag_reports[_next_mag_report];
+
+ /* start the performance counter */
+ perf_begin(_mag_sample_perf);
+
+ /* fetch data from the sensor */
+ raw_mag_report.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT;
+ transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report));
+
+ /*
+ * 1) Scale raw value to SI units using scaling from datasheet.
+ * 2) Subtract static offset (in SI units)
+ * 3) Scale the statically calibrated values with a linear
+ * dynamically obtained factor
+ *
+ * Note: the static sensor offset is the number the sensor outputs
+ * at a nominally 'zero' input. Therefore the offset has to
+ * be subtracted.
+ *
+ * Example: A gyro outputs a value of 74 at zero angular rate
+ * the offset is 74 from the origin and subtracting
+ * 74 from all measurements centers them around zero.
+ */
+
+
+ mag_report->timestamp = hrt_absolute_time();
+
+ mag_report->x_raw = raw_mag_report.x;
+ mag_report->y_raw = raw_mag_report.y;
+ mag_report->z_raw = raw_mag_report.z;
+ mag_report->x = ((mag_report->x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale;
+ mag_report->y = ((mag_report->y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale;
+ mag_report->z = ((mag_report->z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale;
+ mag_report->scaling = _mag_range_scale;
+ mag_report->range_ga = (float)_mag_range_ga;
+
+ /* post a report to the ring - note, not locked */
+ INCREMENT(_next_mag_report, _num_mag_reports);
+
+ /* if we are running up against the oldest report, fix it */
+ if (_next_mag_report == _oldest_mag_report)
+ INCREMENT(_oldest_mag_report, _num_mag_reports);
+
+ /* XXX please check this poll_notify, is it the right one? */
+ /* notify anyone waiting for data */
+ poll_notify(POLLIN);
+
+ /* publish for subscribers */
+ orb_publish(ORB_ID(sensor_mag), _mag_topic, mag_report);
+
+ _mag_read++;
+
+ /* stop the perf counter */
+ perf_end(_mag_sample_perf);
+}
+
+void
+LSM303D::print_info()
+{
+ printf("accel reads: %u\n", _accel_read);
+ printf("mag reads: %u\n", _mag_read);
+ perf_print_counter(_accel_sample_perf);
+ printf("report queue: %u (%u/%u @ %p)\n",
+ _num_accel_reports, _oldest_accel_report, _next_accel_report, _accel_reports);
+ perf_print_counter(_mag_sample_perf);
+ printf("report queue: %u (%u/%u @ %p)\n",
+ _num_mag_reports, _oldest_mag_report, _next_mag_report, _mag_reports);
+}
+
+LSM303D_mag::LSM303D_mag(LSM303D *parent) :
+ CDev("LSM303D_mag", MAG_DEVICE_PATH),
+ _parent(parent)
+{
+}
+
+LSM303D_mag::~LSM303D_mag()
+{
+}
+
+void
+LSM303D_mag::parent_poll_notify()
+{
+ poll_notify(POLLIN);
+}
+
+ssize_t
+LSM303D_mag::read(struct file *filp, char *buffer, size_t buflen)
+{
+ return _parent->mag_read(filp, buffer, buflen);
+}
+
+int
+LSM303D_mag::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+ return _parent->mag_ioctl(filp, cmd, arg);
+}
+
+void
+LSM303D_mag::measure()
+{
+ _parent->mag_measure();
+}
+
+void
+LSM303D_mag::measure_trampoline(void *arg)
+{
+ _parent->mag_measure_trampoline(arg);
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace lsm303d
+{
+
+LSM303D *g_dev;
+
+void start();
+void test();
+void reset();
+void info();
+
+/**
+ * Start the driver.
+ */
+void
+start()
+{
+ int fd, fd_mag;
+
+ if (g_dev != nullptr)
+ errx(0, "already started");
+
+ /* create the driver */
+ g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG);
+
+ if (g_dev == nullptr)
+ goto fail;
+
+ if (OK != g_dev->init())
+ goto fail;
+
+ /* set the poll rate to default, starts automatic data collection */
+ fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
+ if (fd < 0)
+ goto fail;
+
+ if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+ goto fail;
+
+ fd_mag = open(MAG_DEVICE_PATH, O_RDONLY);
+
+ /* don't fail if open cannot be opened */
+ if (0 <= fd_mag) {
+ if (ioctl(fd_mag, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+ goto fail;
+ }
+ }
+
+
+ exit(0);
+fail:
+
+ if (g_dev != nullptr) {
+ delete g_dev;
+ g_dev = nullptr;
+ }
+
+ errx(1, "driver start failed");
+}
+
+/**
+ * Perform some basic functional tests on the driver;
+ * make sure we can collect data from the sensor in polled
+ * and automatic modes.
+ */
+void
+test()
+{
+ int fd_accel = -1;
+ struct accel_report accel_report;
+ ssize_t sz;
+ int ret;
+
+ /* get the driver */
+ fd_accel = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
+ if (fd_accel < 0)
+ err(1, "%s open failed", ACCEL_DEVICE_PATH);
+
+ /* do a simple demand read */
+ sz = read(fd_accel, &accel_report, sizeof(accel_report));
+
+ if (sz != sizeof(accel_report))
+ err(1, "immediate read failed");
+
+
+ warnx("accel x: \t% 9.5f\tm/s^2", (double)accel_report.x);
+ warnx("accel y: \t% 9.5f\tm/s^2", (double)accel_report.y);
+ warnx("accel z: \t% 9.5f\tm/s^2", (double)accel_report.z);
+ warnx("accel x: \t%d\traw", (int)accel_report.x_raw);
+ warnx("accel y: \t%d\traw", (int)accel_report.y_raw);
+ warnx("accel z: \t%d\traw", (int)accel_report.z_raw);
+
+ warnx("accel range: %8.4f m/s^2", (double)accel_report.range_m_s2);
+ if (ERROR == (ret = ioctl(fd_accel, ACCELIOCGLOWPASS, 0)))
+ warnx("accel antialias filter bandwidth: fail");
+ else
+ warnx("accel antialias filter bandwidth: %d Hz", ret);
+
+ int fd_mag = -1;
+ struct mag_report m_report;
+
+ /* get the driver */
+ fd_mag = open(MAG_DEVICE_PATH, O_RDONLY);
+
+ if (fd_mag < 0)
+ err(1, "%s open failed", MAG_DEVICE_PATH);
+
+ /* check if mag is onboard or external */
+ if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0)
+ errx(1, "failed to get if mag is onboard or external");
+ warnx("mag device active: %s", ret ? "external" : "onboard");
+
+ /* do a simple demand read */
+ sz = read(fd_mag, &m_report, sizeof(m_report));
+
+ if (sz != sizeof(m_report))
+ err(1, "immediate read failed");
+
+ warnx("mag x: \t% 9.5f\tga", (double)m_report.x);
+ warnx("mag y: \t% 9.5f\tga", (double)m_report.y);
+ warnx("mag z: \t% 9.5f\tga", (double)m_report.z);
+ warnx("mag x: \t%d\traw", (int)m_report.x_raw);
+ warnx("mag y: \t%d\traw", (int)m_report.y_raw);
+ warnx("mag z: \t%d\traw", (int)m_report.z_raw);
+ warnx("mag range: %8.4f ga", (double)m_report.range_ga);
+
+ /* XXX add poll-rate tests here too */
+
+ reset();
+ errx(0, "PASS");
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset()
+{
+ int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
+ if (fd < 0)
+ err(1, "failed ");
+
+ if (ioctl(fd, SENSORIOCRESET, 0) < 0)
+ err(1, "driver reset failed");
+
+ if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+ err(1, "accel pollrate reset failed");
+
+ fd = open(MAG_DEVICE_PATH, O_RDONLY);
+
+ if (fd < 0) {
+ warnx("mag could not be opened, external mag might be used");
+ } else {
+ /* no need to reset the mag as well, the reset() is the same */
+ if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+ err(1, "mag pollrate reset failed");
+ }
+
+ exit(0);
+}
+
+/**
+ * Print a little info about the driver.
+ */
+void
+info()
+{
+ if (g_dev == nullptr)
+ errx(1, "driver not running\n");
+
+ printf("state @ %p\n", g_dev);
+ g_dev->print_info();
+
+ exit(0);
+}
+
+
+} // namespace
+
+int
+lsm303d_main(int argc, char *argv[])
+{
+ /*
+ * Start/load the driver.
+
+ */
+ if (!strcmp(argv[1], "start"))
+ lsm303d::start();
+
+ /*
+ * Test the driver/device.
+ */
+ if (!strcmp(argv[1], "test"))
+ lsm303d::test();
+
+ /*
+ * Reset the driver.
+ */
+ if (!strcmp(argv[1], "reset"))
+ lsm303d::reset();
+
+ /*
+ * Print driver information.
+ */
+ if (!strcmp(argv[1], "info"))
+ lsm303d::info();
+
+ errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
+}
diff --git a/src/drivers/lsm303d/module.mk b/src/drivers/lsm303d/module.mk
new file mode 100644
index 0000000000000000000000000000000000000000..e40f718c54b45eb1c76ca702a532ddb740ea4fae
--- /dev/null
+++ b/src/drivers/lsm303d/module.mk
@@ -0,0 +1,8 @@
+#
+# LSM303D accel/mag driver
+#
+
+MODULE_COMMAND = lsm303d
+SRCS = lsm303d.cpp
+
+
diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp
index 397686e8bfacda71c781d230065073ffdbc71969..c5f49fb36e987dd25b9018e6fffc1c6de9342146 100644
--- a/src/drivers/mb12xx/mb12xx.cpp
+++ b/src/drivers/mb12xx/mb12xx.cpp
@@ -59,8 +59,6 @@
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
-#include <arch/board/board.h>
-
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
@@ -70,6 +68,8 @@
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
+#include <board_config.h>
+
/* Configuration Constants */
#define MB12XX_BUS PX4_I2C_BUS_EXPANSION
#define MB12XX_BASEADDR 0x70 /* 7-bit address. 8-bit address is 0xE0 */
diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index 68d2c5d65590cbd4a316fb87102edd6b304305b9..666bd30e642a348252187085c8401848080bfab8 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -70,7 +70,7 @@
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
-#include <arch/board/board.h>
+#include <board_config.h>
#include <systemlib/airspeed.h>
#include <systemlib/err.h>
diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
index e78d96569871db3832964ce75a4a5db491366010..1bc3e97a411ae37fd1a38786e631ad3993e97b1e 100644
--- a/src/drivers/mkblctrl/mkblctrl.cpp
+++ b/src/drivers/mkblctrl/mkblctrl.cpp
@@ -59,10 +59,11 @@
#include <nuttx/arch.h>
#include <nuttx/i2c.h>
+#include <board_config.h>
+
#include <drivers/device/device.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_gpio.h>
-#include <drivers/boards/px4fmu/px4fmu_internal.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_rc_input.h>
@@ -74,6 +75,7 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/esc_status.h>
#include <systemlib/err.h>
@@ -134,7 +136,7 @@ private:
int _current_update_rate;
int _task;
int _t_actuators;
- int _t_armed;
+ int _t_actuator_armed;
unsigned int _motor;
int _px4mode;
int _frametype;
@@ -247,7 +249,7 @@ MK::MK(int bus) :
_update_rate(50),
_task(-1),
_t_actuators(-1),
- _t_armed(-1),
+ _t_actuator_armed(-1),
_t_outputs(0),
_t_actuators_effective(0),
_t_esc_status(0),
@@ -512,8 +514,8 @@ MK::task_main()
/* force a reset of the update rate */
_current_update_rate = 0;
- _t_armed = orb_subscribe(ORB_ID(actuator_armed));
- orb_set_interval(_t_armed, 200); /* 5Hz update rate */
+ _t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed));
+ orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */
/* advertise the mixed control outputs */
actuator_outputs_s outputs;
@@ -539,7 +541,7 @@ MK::task_main()
pollfd fds[2];
fds[0].fd = _t_actuators;
fds[0].events = POLLIN;
- fds[1].fd = _t_armed;
+ fds[1].fd = _t_actuator_armed;
fds[1].events = POLLIN;
log("starting");
@@ -653,7 +655,7 @@ MK::task_main()
actuator_armed_s aa;
/* get new value */
- orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
+ orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa);
/* update PWM servo armed status if armed and not locked down */
mk_servo_arm(aa.armed && !aa.lockdown);
@@ -699,7 +701,7 @@ MK::task_main()
//::close(_t_esc_status);
::close(_t_actuators);
::close(_t_actuators_effective);
- ::close(_t_armed);
+ ::close(_t_actuator_armed);
/* make sure servos are off */
@@ -1345,44 +1347,6 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
gpio_bits = 0;
servo_mode = MK::MODE_NONE;
- switch (new_mode) {
- case PORT_FULL_GPIO:
- case PORT_MODE_UNSET:
- /* nothing more to do here */
- break;
-
- case PORT_FULL_SERIAL:
- /* set all multi-GPIOs to serial mode */
- gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4;
- break;
-
- case PORT_FULL_PWM:
- /* select 4-pin PWM mode */
- servo_mode = MK::MODE_4PWM;
- break;
-
- case PORT_GPIO_AND_SERIAL:
- /* set RX/TX multi-GPIOs to serial mode */
- gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4;
- break;
-
- case PORT_PWM_AND_SERIAL:
- /* select 2-pin PWM mode */
- servo_mode = MK::MODE_2PWM;
- /* set RX/TX multi-GPIOs to serial mode */
- gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4;
- break;
-
- case PORT_PWM_AND_GPIO:
- /* select 2-pin PWM mode */
- servo_mode = MK::MODE_2PWM;
- break;
- }
-
- /* adjust GPIO config for serial mode(s) */
- if (gpio_bits != 0)
- g_mk->ioctl(0, GPIO_SET_ALT_1, gpio_bits);
-
/* native PX4 addressing) */
g_mk->set_px4mode(px4mode);
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index 4f7075600615165ae661f90a087ebb8c91362a1f..14f8f44b82af01dfcdbfae06949a3b18b0183473 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -63,7 +63,7 @@
#include <nuttx/arch.h>
#include <nuttx/clock.h>
-#include <arch/board/board.h>
+#include <board_config.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/spi.h>
@@ -149,6 +149,17 @@
#define MPU6000_REV_D9 0x59
#define MPU6000_REV_D10 0x5A
+#define MPU6000_ACCEL_DEFAULT_RANGE_G 8
+#define MPU6000_ACCEL_DEFAULT_RATE 1000
+#define MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30
+
+#define MPU6000_GYRO_DEFAULT_RANGE_G 8
+#define MPU6000_GYRO_DEFAULT_RATE 1000
+#define MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30
+
+#define MPU6000_DEFAULT_ONCHIP_FILTER_FREQ 42
+
+#define MPU6000_ONE_G 9.80665f
class MPU6000_gyro;
@@ -357,12 +368,12 @@ MPU6000::MPU6000(int bus, spi_dev_e device) :
_reads(0),
_sample_rate(1000),
_sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")),
- _accel_filter_x(1000, 30),
- _accel_filter_y(1000, 30),
- _accel_filter_z(1000, 30),
- _gyro_filter_x(1000, 30),
- _gyro_filter_y(1000, 30),
- _gyro_filter_z(1000, 30)
+ _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+ _accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+ _accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+ _gyro_filter_x(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
+ _gyro_filter_y(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
+ _gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ)
{
// disable debug() calls
_debug_enabled = false;
@@ -485,14 +496,13 @@ void MPU6000::reset()
up_udelay(1000);
// SAMPLE RATE
- //write_reg(MPUREG_SMPLRT_DIV, 0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz
- _set_sample_rate(_sample_rate); // default sample rate = 200Hz
+ _set_sample_rate(_sample_rate);
usleep(1000);
// FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter)
// was 90 Hz, but this ruins quality and does not improve the
// system response
- _set_dlpf_filter(42);
+ _set_dlpf_filter(MPU6000_DEFAULT_ONCHIP_FILTER_FREQ);
usleep(1000);
// Gyro scale 2000 deg/s ()
write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS);
@@ -535,8 +545,8 @@ void MPU6000::reset()
// Correct accel scale factors of 4096 LSB/g
// scale to m/s^2 ( 1g = 9.81 m/s^2)
- _accel_range_scale = (9.81f / 4096.0f);
- _accel_range_m_s2 = 8.0f * 9.81f;
+ _accel_range_scale = (MPU6000_ONE_G / 4096.0f);
+ _accel_range_m_s2 = 8.0f * MPU6000_ONE_G;
usleep(1000);
@@ -777,9 +787,10 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
+ return ioctl(filp, SENSORIOCSPOLLRATE, 1000);
+
case SENSOR_POLLRATE_DEFAULT:
- /* set to same as sample rate per default */
- return ioctl(filp, SENSORIOCSPOLLRATE, _sample_rate);
+ return ioctl(filp, SENSORIOCSPOLLRATE, MPU6000_ACCEL_DEFAULT_RATE);
/* adjust to a legal polling interval in Hz */
default: {
@@ -867,9 +878,9 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
// XXX decide on relationship of both filters
// i.e. disable the on-chip filter
//_set_dlpf_filter((uint16_t)arg);
- _accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
- _accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
- _accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
+ _accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
+ _accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
+ _accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
return OK;
case ACCELIOCSSCALE:
@@ -897,7 +908,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
// _accel_range_m_s2 = 8.0f * 9.81f;
return -EINVAL;
case ACCELIOCGRANGE:
- return _accel_range_m_s2;
+ return (unsigned long)((_accel_range_m_s2)/MPU6000_ONE_G + 0.5f);
case ACCELIOCSELFTEST:
return accel_self_test();
@@ -920,28 +931,28 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
return ioctl(filp, cmd, arg);
case SENSORIOCSQUEUEDEPTH: {
- /* lower bound is mandatory, upper bound is a sanity check */
- if ((arg < 1) || (arg > 100))
- return -EINVAL;
-
- /* allocate new buffer */
- GyroReportBuffer *buf = new GyroReportBuffer(arg);
-
- if (nullptr == buf)
- return -ENOMEM;
- if (buf->size() == 0) {
- delete buf;
- return -ENOMEM;
- }
+ /* lower bound is mandatory, upper bound is a sanity check */
+ if ((arg < 1) || (arg > 100))
+ return -EINVAL;
+
+ /* allocate new buffer */
+ GyroReportBuffer *buf = new GyroReportBuffer(arg);
+
+ if (nullptr == buf)
+ return -ENOMEM;
+ if (buf->size() == 0) {
+ delete buf;
+ return -ENOMEM;
+ }
- /* reset the measurement state machine with the new buffer, free the old */
- stop();
- delete _gyro_reports;
- _gyro_reports = buf;
- start();
+ /* reset the measurement state machine with the new buffer, free the old */
+ stop();
+ delete _gyro_reports;
+ _gyro_reports = buf;
+ start();
- return OK;
- }
+ return OK;
+ }
case SENSORIOCGQUEUEDEPTH:
return _gyro_reports->size();
@@ -980,7 +991,7 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
// _gyro_range_rad_s = xx
return -EINVAL;
case GYROIOCGRANGE:
- return _gyro_range_rad_s;
+ return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f);
case GYROIOCSELFTEST:
return gyro_self_test();
@@ -1400,7 +1411,7 @@ test()
warnx("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw);
warnx("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw);
warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2,
- (double)(a_report.range_m_s2 / 9.81f));
+ (double)(a_report.range_m_s2 / MPU6000_ONE_G));
/* do a simple demand read */
sz = read(fd_gyro, &g_report, sizeof(g_report));
diff --git a/src/drivers/ms5611/module.mk b/src/drivers/ms5611/module.mk
index 3c4b0f093827d5494b4c8aefa4f6f312a888878a..20f8aa17372a32c56eaf33360f38a6a9cc512dff 100644
--- a/src/drivers/ms5611/module.mk
+++ b/src/drivers/ms5611/module.mk
@@ -37,4 +37,4 @@
MODULE_COMMAND = ms5611
-SRCS = ms5611.cpp
+SRCS = ms5611.cpp ms5611_spi.cpp ms5611_i2c.cpp
diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp
index c13b65d5aeba03b4ca8d73316398c06397e41f65..d9268c0b30550f9a7f00cecc01c6ceffe07a0d1a 100644
--- a/src/drivers/ms5611/ms5611.cpp
+++ b/src/drivers/ms5611/ms5611.cpp
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Copyright (C) 2012-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
@@ -33,13 +33,11 @@
/**
* @file ms5611.cpp
- * Driver for the MS5611 barometric pressure sensor connected via I2C.
+ * Driver for the MS5611 barometric pressure sensor connected via I2C or SPI.
*/
#include <nuttx/config.h>
-#include <drivers/device/i2c.h>
-
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
@@ -59,12 +57,14 @@
#include <arch/board/board.h>
+#include <drivers/device/device.h>
+#include <drivers/drv_baro.h>
#include <drivers/drv_hrt.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
-#include <drivers/drv_baro.h>
+#include "ms5611.h"
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
@@ -76,35 +76,25 @@ static const int ERROR = -1;
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
-/**
- * Calibration PROM as reported by the device.
- */
-#pragma pack(push,1)
-struct ms5611_prom_s {
- uint16_t factory_setup;
- uint16_t c1_pressure_sens;
- uint16_t c2_pressure_offset;
- uint16_t c3_temp_coeff_pres_sens;
- uint16_t c4_temp_coeff_pres_offset;
- uint16_t c5_reference_temp;
- uint16_t c6_temp_coeff_temp;
- uint16_t serial_and_crc;
-};
+/* helper macro for handling report buffer indices */
+#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0)
-/**
- * Grody hack for crc4()
+/* helper macro for arithmetic - returns the square of the argument */
+#define POW2(_x) ((_x) * (_x))
+
+/*
+ * MS5611 internal constants and data structures.
*/
-union ms5611_prom_u {
- uint16_t c[8];
- struct ms5611_prom_s s;
-};
-#pragma pack(pop)
-class MS5611 : public device::I2C
+/* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */
+#define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */
+#define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */
+
+class MS5611 : public device::CDev
{
public:
- MS5611(int bus);
- virtual ~MS5611();
+ MS5611(device::Device *interface, ms5611::prom_u &prom_buf);
+ ~MS5611();
virtual int init();
@@ -117,10 +107,9 @@ public:
void print_info();
protected:
- virtual int probe();
+ Device *_interface;
-private:
- union ms5611_prom_u _prom;
+ ms5611::prom_s _prom;
struct work_s _work;
unsigned _measure_ticks;
@@ -149,16 +138,7 @@ private:
perf_counter_t _buffer_overflows;
/**
- * Test whether the device supported by the driver is present at a
- * specific address.
- *
- * @param address The I2C bus address to probe.
- * @return True if the device is present.
- */
- int probe_address(uint8_t address);
-
- /**
- * Initialise the automatic measurement state machine and start it.
+ * Initialize the automatic measurement state machine and start it.
*
* @note This function is called at open and error time. It might make sense
* to make it more aggressive about resetting the bus in case of errors.
@@ -198,70 +178,23 @@ private:
*
* @return OK if the measurement command was successful.
*/
- int measure();
+ virtual int measure();
/**
* Collect the result of the most recent measurement.
*/
- int collect();
-
- /**
- * Send a reset command to the MS5611.
- *
- * This is required after any bus reset.
- */
- int cmd_reset();
-
- /**
- * Read the MS5611 PROM
- *
- * @return OK if the PROM reads successfully.
- */
- int read_prom();
-
- /**
- * PROM CRC routine ported from MS5611 application note
- *
- * @param n_prom Pointer to words read from PROM.
- * @return True if the CRC matches.
- */
- bool crc4(uint16_t *n_prom);
-
+ virtual int collect();
};
-/* helper macro for handling report buffer indices */
-#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0)
-
-/* helper macro for arithmetic - returns the square of the argument */
-#define POW2(_x) ((_x) * (_x))
-
-/*
- * MS5611 internal constants and data structures.
- */
-
-/* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */
-#define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */
-#define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */
-
-#define MS5611_BUS PX4_I2C_BUS_ONBOARD
-#define MS5611_ADDRESS_1 PX4_I2C_OBDEV_MS5611 /* address select pins pulled high (PX4FMU series v1.6+) */
-#define MS5611_ADDRESS_2 0x77 /* address select pins pulled low (PX4FMU prototypes) */
-
-#define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */
-#define ADDR_CMD_CONVERT_D1 0x48 /* write to this address to start temperature conversion */
-#define ADDR_CMD_CONVERT_D2 0x58 /* write to this address to start pressure conversion */
-#define ADDR_DATA 0x00 /* address of 3 bytes / 32bit pressure data */
-#define ADDR_PROM_SETUP 0xA0 /* address of 8x 2 bytes factory and calibration data */
-#define ADDR_PROM_C1 0xA2 /* address of 6x 2 bytes calibration data */
-
/*
* Driver 'main' command.
*/
extern "C" __EXPORT int ms5611_main(int argc, char *argv[]);
-
-MS5611::MS5611(int bus) :
- I2C("MS5611", BARO_DEVICE_PATH, bus, 0, 400000),
+MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) :
+ CDev("MS5611", BARO_DEVICE_PATH),
+ _interface(interface),
+ _prom(prom_buf.s),
_measure_ticks(0),
_num_reports(0),
_next_report(0),
@@ -279,10 +212,7 @@ MS5611::MS5611(int bus) :
_comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows"))
{
- // enable debug() calls
- _debug_enabled = true;
-
- // work_cancel in the dtor will explode if we don't do this...
+ // work_cancel in stop_cycle called from the dtor will explode if we don't do this...
memset(&_work, 0, sizeof(_work));
}
@@ -294,23 +224,30 @@ MS5611::~MS5611()
/* free any existing reports */
if (_reports != nullptr)
delete[] _reports;
+
+ delete _interface;
}
int
MS5611::init()
{
- int ret = ERROR;
+ int ret;
- /* do I2C init (and probe) first */
- if (I2C::init() != OK)
+ ret = CDev::init();
+ if (ret != OK) {
+ debug("CDev init failed");
goto out;
+ }
/* allocate basic report buffers */
_num_reports = 2;
_reports = new struct baro_report[_num_reports];
- if (_reports == nullptr)
+ if (_reports == nullptr) {
+ debug("can't get memory for reports");
+ ret = -ENOMEM;
goto out;
+ }
_oldest_report = _next_report = 0;
@@ -318,49 +255,17 @@ MS5611::init()
memset(&_reports[0], 0, sizeof(_reports[0]));
_baro_topic = orb_advertise(ORB_ID(sensor_baro), &_reports[0]);
- if (_baro_topic < 0)
+ if (_baro_topic < 0) {
debug("failed to create sensor_baro object");
+ ret = -ENOSPC;
+ goto out;
+ }
ret = OK;
out:
return ret;
}
-int
-MS5611::probe()
-{
- _retries = 10;
-
- if ((OK == probe_address(MS5611_ADDRESS_1)) ||
- (OK == probe_address(MS5611_ADDRESS_2))) {
- /*
- * Disable retries; we may enable them selectively in some cases,
- * but the device gets confused if we retry some of the commands.
- */
- _retries = 0;
- return OK;
- }
-
- return -EIO;
-}
-
-int
-MS5611::probe_address(uint8_t address)
-{
- /* select the address we are going to try */
- set_address(address);
-
- /* send reset command */
- if (OK != cmd_reset())
- return -EIO;
-
- /* read PROM */
- if (OK != read_prom())
- return -EIO;
-
- return OK;
-}
-
ssize_t
MS5611::read(struct file *filp, char *buffer, size_t buflen)
{
@@ -546,8 +451,9 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
break;
}
- /* give it to the superclass */
- return I2C::ioctl(filp, cmd, arg);
+ /* give it to the bus-specific superclass */
+ // return bus_ioctl(filp, cmd, arg);
+ return CDev::ioctl(filp, cmd, arg);
}
void
@@ -572,7 +478,7 @@ MS5611::stop_cycle()
void
MS5611::cycle_trampoline(void *arg)
{
- MS5611 *dev = (MS5611 *)arg;
+ MS5611 *dev = reinterpret_cast<MS5611 *>(arg);
dev->cycle();
}
@@ -592,7 +498,7 @@ MS5611::cycle()
/*
* The ms5611 seems to regularly fail to respond to
* its address; this happens often enough that we'd rather not
- * spam the console with the message.
+ * spam the console with a message for this.
*/
} else {
//log("collection error %d", ret);
@@ -654,17 +560,12 @@ MS5611::measure()
/*
* In phase zero, request temperature; in other phases, request pressure.
*/
- uint8_t cmd_data = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1;
+ unsigned addr = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1;
/*
* Send the command to begin measuring.
- *
- * Disable retries on this command; we can't know whether failure
- * means the device did or did not see the write.
*/
- _retries = 0;
- ret = transfer(&cmd_data, 1, nullptr, 0);
-
+ ret = _interface->ioctl(IOCTL_MEASURE, addr);
if (OK != ret)
perf_count(_comms_errors);
@@ -677,47 +578,33 @@ int
MS5611::collect()
{
int ret;
- uint8_t cmd;
- uint8_t data[3];
- union {
- uint8_t b[4];
- uint32_t w;
- } cvt;
-
- /* read the most recent measurement */
- cmd = 0;
+ uint32_t raw;
perf_begin(_sample_perf);
/* this should be fairly close to the end of the conversion, so the best approximation of the time */
_reports[_next_report].timestamp = hrt_absolute_time();
- ret = transfer(&cmd, 1, &data[0], 3);
- if (ret != OK) {
+ /* read the most recent measurement - read offset/size are hardcoded in the interface */
+ ret = _interface->read(0, (void *)&raw, 0);
+ if (ret < 0) {
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
}
- /* fetch the raw value */
- cvt.b[0] = data[2];
- cvt.b[1] = data[1];
- cvt.b[2] = data[0];
- cvt.b[3] = 0;
- uint32_t raw = cvt.w;
-
/* handle a measurement */
if (_measure_phase == 0) {
/* temperature offset (in ADC units) */
- int32_t dT = (int32_t)raw - ((int32_t)_prom.s.c5_reference_temp << 8);
+ int32_t dT = (int32_t)raw - ((int32_t)_prom.c5_reference_temp << 8);
/* absolute temperature in centidegrees - note intermediate value is outside 32-bit range */
- _TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.s.c6_temp_coeff_temp) >> 23);
+ _TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.c6_temp_coeff_temp) >> 23);
/* base sensor scale/offset values */
- _SENS = ((int64_t)_prom.s.c1_pressure_sens << 15) + (((int64_t)_prom.s.c3_temp_coeff_pres_sens * dT) >> 8);
- _OFF = ((int64_t)_prom.s.c2_pressure_offset << 16) + (((int64_t)_prom.s.c4_temp_coeff_pres_offset * dT) >> 7);
+ _SENS = ((int64_t)_prom.c1_pressure_sens << 15) + (((int64_t)_prom.c3_temp_coeff_pres_sens * dT) >> 8);
+ _OFF = ((int64_t)_prom.c2_pressure_offset << 16) + (((int64_t)_prom.c4_temp_coeff_pres_offset * dT) >> 7);
/* temperature compensation */
if (_TEMP < 2000) {
@@ -761,30 +648,7 @@ MS5611::collect()
* double precision: ms5611_read: 992 events, 258641us elapsed, min 202us max 305us
* single precision: ms5611_read: 963 events, 208066us elapsed, min 202us max 241us
*/
-#if 0/* USE_FLOAT */
- /* tropospheric properties (0-11km) for standard atmosphere */
- const float T1 = 15.0f + 273.15f; /* temperature at base height in Kelvin */
- const float a = -6.5f / 1000f; /* temperature gradient in degrees per metre */
- const float g = 9.80665f; /* gravity constant in m/s/s */
- const float R = 287.05f; /* ideal gas constant in J/kg/K */
-
- /* current pressure at MSL in kPa */
- float p1 = _msl_pressure / 1000.0f;
-
- /* measured pressure in kPa */
- float p = P / 1000.0f;
- /*
- * Solve:
- *
- * / -(aR / g) \
- * | (p / p1) . T1 | - T1
- * \ /
- * h = ------------------------------- + h1
- * a
- */
- _reports[_next_report].altitude = (((powf((p / p1), (-(a * R) / g))) * T1) - T1) / a;
-#else
/* tropospheric properties (0-11km) for standard atmosphere */
const double T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */
const double a = -6.5 / 1000; /* temperature gradient in degrees per metre */
@@ -807,7 +671,7 @@ MS5611::collect()
* a
*/
_reports[_next_report].altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
-#endif
+
/* publish it */
orb_publish(ORB_ID(sensor_baro), _baro_topic, &_reports[_next_report]);
@@ -832,55 +696,49 @@ MS5611::collect()
return OK;
}
-int
-MS5611::cmd_reset()
+void
+MS5611::print_info()
{
- unsigned old_retrycount = _retries;
- uint8_t cmd = ADDR_RESET_CMD;
- int result;
-
- /* bump the retry count */
- _retries = 10;
- result = transfer(&cmd, 1, nullptr, 0);
- _retries = old_retrycount;
+ perf_print_counter(_sample_perf);
+ perf_print_counter(_comms_errors);
+ perf_print_counter(_buffer_overflows);
+ printf("poll interval: %u ticks\n", _measure_ticks);
+ printf("report queue: %u (%u/%u @ %p)\n",
+ _num_reports, _oldest_report, _next_report, _reports);
+ printf("TEMP: %d\n", _TEMP);
+ printf("SENS: %lld\n", _SENS);
+ printf("OFF: %lld\n", _OFF);
+ printf("MSL pressure: %10.4f\n", (double)(_msl_pressure / 100.f));
- return result;
+ printf("factory_setup %u\n", _prom.factory_setup);
+ printf("c1_pressure_sens %u\n", _prom.c1_pressure_sens);
+ printf("c2_pressure_offset %u\n", _prom.c2_pressure_offset);
+ printf("c3_temp_coeff_pres_sens %u\n", _prom.c3_temp_coeff_pres_sens);
+ printf("c4_temp_coeff_pres_offset %u\n", _prom.c4_temp_coeff_pres_offset);
+ printf("c5_reference_temp %u\n", _prom.c5_reference_temp);
+ printf("c6_temp_coeff_temp %u\n", _prom.c6_temp_coeff_temp);
+ printf("serial_and_crc %u\n", _prom.serial_and_crc);
}
-int
-MS5611::read_prom()
+/**
+ * Local functions in support of the shell command.
+ */
+namespace ms5611
{
- uint8_t prom_buf[2];
- union {
- uint8_t b[2];
- uint16_t w;
- } cvt;
-
- /*
- * Wait for PROM contents to be in the device (2.8 ms) in the case we are
- * called immediately after reset.
- */
- usleep(3000);
- /* read and convert PROM words */
- for (int i = 0; i < 8; i++) {
- uint8_t cmd = ADDR_PROM_SETUP + (i * 2);
-
- if (OK != transfer(&cmd, 1, &prom_buf[0], 2))
- break;
-
- /* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */
- cvt.b[0] = prom_buf[1];
- cvt.b[1] = prom_buf[0];
- _prom.c[i] = cvt.w;
- }
+MS5611 *g_dev;
- /* calculate CRC and return success/failure accordingly */
- return crc4(&_prom.c[0]) ? OK : -EIO;
-}
+void start();
+void test();
+void reset();
+void info();
+void calibrate(unsigned altitude);
+/**
+ * MS5611 crc4 cribbed from the datasheet
+ */
bool
-MS5611::crc4(uint16_t *n_prom)
+crc4(uint16_t *n_prom)
{
int16_t cnt;
uint16_t n_rem;
@@ -922,43 +780,6 @@ MS5611::crc4(uint16_t *n_prom)
return (0x000F & crc_read) == (n_rem ^ 0x00);
}
-void
-MS5611::print_info()
-{
- perf_print_counter(_sample_perf);
- perf_print_counter(_comms_errors);
- perf_print_counter(_buffer_overflows);
- printf("poll interval: %u ticks\n", _measure_ticks);
- printf("report queue: %u (%u/%u @ %p)\n",
- _num_reports, _oldest_report, _next_report, _reports);
- printf("TEMP: %d\n", _TEMP);
- printf("SENS: %lld\n", _SENS);
- printf("OFF: %lld\n", _OFF);
- printf("MSL pressure: %10.4f\n", (double)(_msl_pressure / 100.f));
-
- printf("factory_setup %u\n", _prom.s.factory_setup);
- printf("c1_pressure_sens %u\n", _prom.s.c1_pressure_sens);
- printf("c2_pressure_offset %u\n", _prom.s.c2_pressure_offset);
- printf("c3_temp_coeff_pres_sens %u\n", _prom.s.c3_temp_coeff_pres_sens);
- printf("c4_temp_coeff_pres_offset %u\n", _prom.s.c4_temp_coeff_pres_offset);
- printf("c5_reference_temp %u\n", _prom.s.c5_reference_temp);
- printf("c6_temp_coeff_temp %u\n", _prom.s.c6_temp_coeff_temp);
- printf("serial_and_crc %u\n", _prom.s.serial_and_crc);
-}
-
-/**
- * Local functions in support of the shell command.
- */
-namespace ms5611
-{
-
-MS5611 *g_dev;
-
-void start();
-void test();
-void reset();
-void info();
-void calibrate(unsigned altitude);
/**
* Start the driver.
@@ -967,28 +788,46 @@ void
start()
{
int fd;
+ prom_u prom_buf;
if (g_dev != nullptr)
/* if already started, the still command succeeded */
errx(0, "already started");
- /* create the driver */
- g_dev = new MS5611(MS5611_BUS);
+ device::Device *interface = nullptr;
- if (g_dev == nullptr)
- goto fail;
+ /* create the driver, try SPI first, fall back to I2C if unsuccessful */
+ if (MS5611_spi_interface != nullptr)
+ interface = MS5611_spi_interface(prom_buf);
+ if (interface == nullptr && (MS5611_i2c_interface != nullptr))
+ interface = MS5611_i2c_interface(prom_buf);
- if (OK != g_dev->init())
+ if (interface == nullptr)
+ errx(1, "failed to allocate an interface");
+
+ if (interface->init() != OK) {
+ delete interface;
+ errx(1, "interface init failed");
+ }
+
+ g_dev = new MS5611(interface, prom_buf);
+ if (g_dev == nullptr) {
+ delete interface;
+ errx(1, "failed to allocate driver");
+ }
+ if (g_dev->init() != OK)
goto fail;
/* set the poll rate to default, starts automatic data collection */
fd = open(BARO_DEVICE_PATH, O_RDONLY);
-
- if (fd < 0)
+ if (fd < 0) {
+ warnx("can't open baro device");
goto fail;
-
- if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+ }
+ if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+ warnx("failed setting default poll rate");
goto fail;
+ }
exit(0);
@@ -1155,11 +994,11 @@ calibrate(unsigned altitude)
const float g = 9.80665f; /* gravity constant in m/s/s */
const float R = 287.05f; /* ideal gas constant in J/kg/K */
- warnx("averaged pressure %10.4fkPa at %um", pressure, altitude);
+ warnx("averaged pressure %10.4fkPa at %um", (double)pressure, altitude);
p1 = pressure * (powf(((T1 + (a * (float)altitude)) / T1), (g / (a * R))));
- warnx("calculated MSL pressure %10.4fkPa", p1);
+ warnx("calculated MSL pressure %10.4fkPa", (double)p1);
/* save as integer Pa */
p1 *= 1000.0f;
diff --git a/src/drivers/ms5611/ms5611.h b/src/drivers/ms5611/ms5611.h
new file mode 100644
index 0000000000000000000000000000000000000000..76fb84de8e65f2166577c40a1cad32f15522fe85
--- /dev/null
+++ b/src/drivers/ms5611/ms5611.h
@@ -0,0 +1,85 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012-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 ms5611.h
+ *
+ * Shared defines for the ms5611 driver.
+ */
+
+#define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */
+#define ADDR_CMD_CONVERT_D1 0x48 /* write to this address to start temperature conversion */
+#define ADDR_CMD_CONVERT_D2 0x58 /* write to this address to start pressure conversion */
+#define ADDR_DATA 0x00 /* address of 3 bytes / 32bit pressure data */
+#define ADDR_PROM_SETUP 0xA0 /* address of 8x 2 bytes factory and calibration data */
+#define ADDR_PROM_C1 0xA2 /* address of 6x 2 bytes calibration data */
+
+/* interface ioctls */
+#define IOCTL_RESET 2
+#define IOCTL_MEASURE 3
+
+namespace ms5611
+{
+
+/**
+ * Calibration PROM as reported by the device.
+ */
+#pragma pack(push,1)
+struct prom_s {
+ uint16_t factory_setup;
+ uint16_t c1_pressure_sens;
+ uint16_t c2_pressure_offset;
+ uint16_t c3_temp_coeff_pres_sens;
+ uint16_t c4_temp_coeff_pres_offset;
+ uint16_t c5_reference_temp;
+ uint16_t c6_temp_coeff_temp;
+ uint16_t serial_and_crc;
+};
+
+/**
+ * Grody hack for crc4()
+ */
+union prom_u {
+ uint16_t c[8];
+ prom_s s;
+};
+#pragma pack(pop)
+
+extern bool crc4(uint16_t *n_prom);
+
+} /* namespace */
+
+/* interface factories */
+extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf) weak_function;
+extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf) weak_function;
+
diff --git a/src/drivers/ms5611/ms5611_i2c.cpp b/src/drivers/ms5611/ms5611_i2c.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..87d9b94a6e9f2d14dee469b80a969b71be35996a
--- /dev/null
+++ b/src/drivers/ms5611/ms5611_i2c.cpp
@@ -0,0 +1,278 @@
+/****************************************************************************
+ *
+ * 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 ms5611_i2c.cpp
+ *
+ * I2C interface for MS5611
+ */
+
+/* XXX trim includes */
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <assert.h>
+#include <debug.h>
+#include <errno.h>
+#include <unistd.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/device/i2c.h>
+
+#include "ms5611.h"
+
+#include "board_config.h"
+
+#ifdef PX4_I2C_OBDEV_MS5611
+
+#ifndef PX4_I2C_BUS_ONBOARD
+ #define MS5611_BUS 1
+#else
+ #define MS5611_BUS PX4_I2C_BUS_ONBOARD
+#endif
+
+#define MS5611_ADDRESS_1 0x76 /* address select pins pulled high (PX4FMU series v1.6+) */
+#define MS5611_ADDRESS_2 0x77 /* address select pins pulled low (PX4FMU prototypes) */
+
+
+
+device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf);
+
+class MS5611_I2C : public device::I2C
+{
+public:
+ MS5611_I2C(int bus, ms5611::prom_u &prom_buf);
+ virtual ~MS5611_I2C();
+
+ virtual int init();
+ virtual int read(unsigned offset, void *data, unsigned count);
+ virtual int ioctl(unsigned operation, unsigned &arg);
+
+protected:
+ virtual int probe();
+
+private:
+ ms5611::prom_u &_prom;
+
+ int _probe_address(uint8_t address);
+
+ /**
+ * Send a reset command to the MS5611.
+ *
+ * This is required after any bus reset.
+ */
+ int _reset();
+
+ /**
+ * Send a measure command to the MS5611.
+ *
+ * @param addr Which address to use for the measure operation.
+ */
+ int _measure(unsigned addr);
+
+ /**
+ * Read the MS5611 PROM
+ *
+ * @return OK if the PROM reads successfully.
+ */
+ int _read_prom();
+
+};
+
+device::Device *
+MS5611_i2c_interface(ms5611::prom_u &prom_buf)
+{
+ return new MS5611_I2C(MS5611_BUS, prom_buf);
+}
+
+MS5611_I2C::MS5611_I2C(int bus, ms5611::prom_u &prom) :
+ I2C("MS5611_I2C", nullptr, bus, 0, 400000),
+ _prom(prom)
+{
+}
+
+MS5611_I2C::~MS5611_I2C()
+{
+}
+
+int
+MS5611_I2C::init()
+{
+ /* this will call probe(), and thereby _probe_address */
+ return I2C::init();
+}
+
+int
+MS5611_I2C::read(unsigned offset, void *data, unsigned count)
+{
+ union _cvt {
+ uint8_t b[4];
+ uint32_t w;
+ } *cvt = (_cvt *)data;
+ uint8_t buf[3];
+
+ /* read the most recent measurement */
+ uint8_t cmd = 0;
+ int ret = transfer(&cmd, 1, &buf[0], 3);
+ if (ret == OK) {
+ /* fetch the raw value */
+ cvt->b[0] = buf[2];
+ cvt->b[1] = buf[1];
+ cvt->b[2] = buf[0];
+ cvt->b[3] = 0;
+ }
+
+ return ret;
+}
+
+int
+MS5611_I2C::ioctl(unsigned operation, unsigned &arg)
+{
+ int ret;
+
+ switch (operation) {
+ case IOCTL_RESET:
+ ret = _reset();
+ break;
+
+ case IOCTL_MEASURE:
+ ret = _measure(arg);
+ break;
+
+ default:
+ ret = EINVAL;
+ }
+
+ return ret;
+}
+
+int
+MS5611_I2C::probe()
+{
+ _retries = 10;
+
+ if ((OK == _probe_address(MS5611_ADDRESS_1)) ||
+ (OK == _probe_address(MS5611_ADDRESS_2))) {
+ /*
+ * Disable retries; we may enable them selectively in some cases,
+ * but the device gets confused if we retry some of the commands.
+ */
+ _retries = 0;
+ return OK;
+ }
+
+ return -EIO;
+}
+
+int
+MS5611_I2C::_probe_address(uint8_t address)
+{
+ /* select the address we are going to try */
+ set_address(address);
+
+ /* send reset command */
+ if (OK != _reset())
+ return -EIO;
+
+ /* read PROM */
+ if (OK != _read_prom())
+ return -EIO;
+
+ return OK;
+}
+
+
+int
+MS5611_I2C::_reset()
+{
+ unsigned old_retrycount = _retries;
+ uint8_t cmd = ADDR_RESET_CMD;
+ int result;
+
+ /* bump the retry count */
+ _retries = 10;
+ result = transfer(&cmd, 1, nullptr, 0);
+ _retries = old_retrycount;
+
+ return result;
+}
+
+int
+MS5611_I2C::_measure(unsigned addr)
+{
+ /*
+ * Disable retries on this command; we can't know whether failure
+ * means the device did or did not see the command.
+ */
+ _retries = 0;
+
+ uint8_t cmd = addr;
+ return transfer(&cmd, 1, nullptr, 0);
+}
+
+int
+MS5611_I2C::_read_prom()
+{
+ uint8_t prom_buf[2];
+ union {
+ uint8_t b[2];
+ uint16_t w;
+ } cvt;
+
+ /*
+ * Wait for PROM contents to be in the device (2.8 ms) in the case we are
+ * called immediately after reset.
+ */
+ usleep(3000);
+
+ /* read and convert PROM words */
+ for (int i = 0; i < 8; i++) {
+ uint8_t cmd = ADDR_PROM_SETUP + (i * 2);
+
+ if (OK != transfer(&cmd, 1, &prom_buf[0], 2))
+ break;
+
+ /* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */
+ cvt.b[0] = prom_buf[1];
+ cvt.b[1] = prom_buf[0];
+ _prom.c[i] = cvt.w;
+ }
+
+ /* calculate CRC and return success/failure accordingly */
+ return ms5611::crc4(&_prom.c[0]) ? OK : -EIO;
+}
+
+#endif /* PX4_I2C_OBDEV_MS5611 */
diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f6c62434080e24e8cc34c4b73704d5889ee5c0be
--- /dev/null
+++ b/src/drivers/ms5611/ms5611_spi.cpp
@@ -0,0 +1,273 @@
+/****************************************************************************
+ *
+ * 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 ms5611_spi.cpp
+ *
+ * SPI interface for MS5611
+ */
+
+/* XXX trim includes */
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <assert.h>
+#include <debug.h>
+#include <errno.h>
+#include <unistd.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/device/spi.h>
+
+#include "ms5611.h"
+#include "board_config.h"
+
+/* SPI protocol address bits */
+#define DIR_READ (1<<7)
+#define DIR_WRITE (0<<7)
+#define ADDR_INCREMENT (1<<6)
+
+#ifdef PX4_SPIDEV_BARO
+
+device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf);
+
+class MS5611_SPI : public device::SPI
+{
+public:
+ MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf);
+ virtual ~MS5611_SPI();
+
+ virtual int init();
+ virtual int read(unsigned offset, void *data, unsigned count);
+ virtual int ioctl(unsigned operation, unsigned &arg);
+
+private:
+ ms5611::prom_u &_prom;
+
+ /**
+ * Send a reset command to the MS5611.
+ *
+ * This is required after any bus reset.
+ */
+ int _reset();
+
+ /**
+ * Send a measure command to the MS5611.
+ *
+ * @param addr Which address to use for the measure operation.
+ */
+ int _measure(unsigned addr);
+
+ /**
+ * Read the MS5611 PROM
+ *
+ * @return OK if the PROM reads successfully.
+ */
+ int _read_prom();
+
+ /**
+ * Read a 16-bit register value.
+ *
+ * @param reg The register to read.
+ */
+ uint16_t _reg16(unsigned reg);
+
+ /**
+ * Wrapper around transfer() that prevents interrupt-context transfers
+ * from pre-empting us. The sensor may (does) share a bus with sensors
+ * that are polled from interrupt context (or we may be pre-empted)
+ * so we need to guarantee that transfers complete without interruption.
+ */
+ int _transfer(uint8_t *send, uint8_t *recv, unsigned len);
+};
+
+device::Device *
+MS5611_spi_interface(ms5611::prom_u &prom_buf)
+{
+ return new MS5611_SPI(1 /* XXX MAGIC NUMBER */, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf);
+}
+
+MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) :
+ SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 2000000),
+ _prom(prom_buf)
+{
+}
+
+MS5611_SPI::~MS5611_SPI()
+{
+}
+
+int
+MS5611_SPI::init()
+{
+ int ret;
+
+ ret = SPI::init();
+ if (ret != OK) {
+ debug("SPI init failed");
+ goto out;
+ }
+
+ /* send reset command */
+ ret = _reset();
+ if (ret != OK) {
+ debug("reset failed");
+ goto out;
+ }
+
+ /* read PROM */
+ ret = _read_prom();
+ if (ret != OK) {
+ debug("prom readout failed");
+ goto out;
+ }
+
+out:
+ return ret;
+}
+
+int
+MS5611_SPI::read(unsigned offset, void *data, unsigned count)
+{
+ union _cvt {
+ uint8_t b[4];
+ uint32_t w;
+ } *cvt = (_cvt *)data;
+ uint8_t buf[4];
+
+ /* read the most recent measurement */
+ buf[0] = 0 | DIR_WRITE;
+ int ret = _transfer(&buf[0], &buf[0], sizeof(buf));
+
+ if (ret == OK) {
+ /* fetch the raw value */
+ cvt->b[0] = buf[3];
+ cvt->b[1] = buf[2];
+ cvt->b[2] = buf[1];
+ cvt->b[3] = 0;
+
+ ret = count;
+ }
+
+ return ret;
+}
+
+int
+MS5611_SPI::ioctl(unsigned operation, unsigned &arg)
+{
+ int ret;
+
+ switch (operation) {
+ case IOCTL_RESET:
+ ret = _reset();
+ break;
+
+ case IOCTL_MEASURE:
+ ret = _measure(arg);
+ break;
+
+ default:
+ ret = EINVAL;
+ }
+
+ if (ret != OK) {
+ errno = ret;
+ return -1;
+ }
+ return 0;
+}
+
+int
+MS5611_SPI::_reset()
+{
+ uint8_t cmd = ADDR_RESET_CMD | DIR_WRITE;
+
+ return _transfer(&cmd, nullptr, 1);
+}
+
+int
+MS5611_SPI::_measure(unsigned addr)
+{
+ uint8_t cmd = addr | DIR_WRITE;
+
+ return _transfer(&cmd, nullptr, 1);
+}
+
+
+int
+MS5611_SPI::_read_prom()
+{
+ /*
+ * Wait for PROM contents to be in the device (2.8 ms) in the case we are
+ * called immediately after reset.
+ */
+ usleep(3000);
+
+ /* read and convert PROM words */
+ for (int i = 0; i < 8; i++) {
+ uint8_t cmd = (ADDR_PROM_SETUP + (i * 2));
+ _prom.c[i] = _reg16(cmd);
+ }
+
+ /* calculate CRC and return success/failure accordingly */
+ return ms5611::crc4(&_prom.c[0]) ? OK : -EIO;
+}
+
+uint16_t
+MS5611_SPI::_reg16(unsigned reg)
+{
+ uint8_t cmd[3];
+
+ cmd[0] = reg | DIR_READ;
+
+ _transfer(cmd, cmd, sizeof(cmd));
+
+ return (uint16_t)(cmd[1] << 8) | cmd[2];
+}
+
+int
+MS5611_SPI::_transfer(uint8_t *send, uint8_t *recv, unsigned len)
+{
+ irqstate_t flags = irqsave();
+
+ int ret = transfer(send, recv, len);
+
+ irqrestore(flags);
+
+ return ret;
+}
+
+#endif /* PX4_SPIDEV_BARO */
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index a98b527b1b71a88fd08542b6a8cfe2c4dd59b97c..6d4019f249cd09526a70fc11bae796c447e0d8d2 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -34,7 +34,7 @@
/**
* @file fmu.cpp
*
- * Driver/configurator for the PX4 FMU multi-purpose port.
+ * Driver/configurator for the PX4 FMU multi-purpose port on v1 and v2 boards.
*/
#include <nuttx/config.h>
@@ -57,9 +57,10 @@
#include <drivers/device/device.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_gpio.h>
-#include <drivers/boards/px4fmu/px4fmu_internal.h>
#include <drivers/drv_hrt.h>
+# include <board_config.h>
+
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
#include <systemlib/mixer/mixer.h>
@@ -69,17 +70,21 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_armed.h>
#include <systemlib/err.h>
-#include <systemlib/ppm_decode.h>
+#ifdef HRT_PPM_CHANNEL
+# include <systemlib/ppm_decode.h>
+#endif
class PX4FMU : public device::CDev
{
public:
enum Mode {
+ MODE_NONE,
MODE_2PWM,
MODE_4PWM,
- MODE_NONE
+ MODE_6PWM,
};
PX4FMU();
virtual ~PX4FMU();
@@ -104,7 +109,7 @@ private:
unsigned _current_update_rate;
int _task;
int _t_actuators;
- int _t_armed;
+ int _t_actuator_armed;
orb_advert_t _t_outputs;
orb_advert_t _t_actuators_effective;
unsigned _num_outputs;
@@ -146,6 +151,7 @@ private:
};
const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = {
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
{GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0},
{GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0},
{GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1},
@@ -154,6 +160,22 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = {
{GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, GPIO_USART2_RX_1},
{GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2},
{GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2},
+#endif
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+ {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0},
+ {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0},
+ {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0},
+ {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0},
+ {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0},
+ {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0},
+
+ {0, GPIO_VDD_5V_PERIPH_EN, 0},
+ {0, GPIO_VDD_3V3_SENSORS_EN, 0},
+ {GPIO_VDD_BRICK_VALID, 0, 0},
+ {GPIO_VDD_SERVO_VALID, 0, 0},
+ {GPIO_VDD_5V_HIPOWER_OC, 0, 0},
+ {GPIO_VDD_5V_PERIPH_OC, 0, 0},
+#endif
};
const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]);
@@ -174,7 +196,7 @@ PX4FMU::PX4FMU() :
_current_update_rate(0),
_task(-1),
_t_actuators(-1),
- _t_armed(-1),
+ _t_actuator_armed(-1),
_t_outputs(0),
_t_actuators_effective(0),
_num_outputs(0),
@@ -271,9 +293,36 @@ PX4FMU::set_mode(Mode mode)
* are presented on the output pins.
*/
switch (mode) {
- case MODE_2PWM: // multi-port with flow control lines as PWM
- case MODE_4PWM: // multi-port as 4 PWM outs
- debug("MODE_%dPWM", (mode == MODE_2PWM) ? 2 : 4);
+ case MODE_2PWM: // v1 multi-port with flow control lines as PWM
+ debug("MODE_2PWM");
+
+ /* default output rates */
+ _pwm_default_rate = 50;
+ _pwm_alt_rate = 50;
+ _pwm_alt_rate_channels = 0;
+
+ /* XXX magic numbers */
+ up_pwm_servo_init(0x3);
+ set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
+
+ break;
+
+ case MODE_4PWM: // v1 multi-port as 4 PWM outs
+ debug("MODE_4PWM");
+
+ /* default output rates */
+ _pwm_default_rate = 50;
+ _pwm_alt_rate = 50;
+ _pwm_alt_rate_channels = 0;
+
+ /* XXX magic numbers */
+ up_pwm_servo_init(0xf);
+ set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
+
+ break;
+
+ case MODE_6PWM: // v2 PWMs as 6 PWM outs
+ debug("MODE_6PWM");
/* default output rates */
_pwm_default_rate = 50;
@@ -281,7 +330,7 @@ PX4FMU::set_mode(Mode mode)
_pwm_alt_rate_channels = 0;
/* XXX magic numbers */
- up_pwm_servo_init((mode == MODE_2PWM) ? 0x3 : 0xf);
+ up_pwm_servo_init(0x3f);
set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
break;
@@ -376,8 +425,8 @@ PX4FMU::task_main()
/* force a reset of the update rate */
_current_update_rate = 0;
- _t_armed = orb_subscribe(ORB_ID(actuator_armed));
- orb_set_interval(_t_armed, 200); /* 5Hz update rate */
+ _t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed));
+ orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */
/* advertise the mixed control outputs */
actuator_outputs_s outputs;
@@ -396,17 +445,17 @@ PX4FMU::task_main()
pollfd fds[2];
fds[0].fd = _t_actuators;
fds[0].events = POLLIN;
- fds[1].fd = _t_armed;
+ fds[1].fd = _t_actuator_armed;
fds[1].events = POLLIN;
- unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4;
-
+#ifdef HRT_PPM_CHANNEL
// rc input, published to ORB
struct rc_input_values rc_in;
orb_advert_t to_input_rc = 0;
memset(&rc_in, 0, sizeof(rc_in));
rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM;
+#endif
log("starting");
@@ -460,6 +509,23 @@ PX4FMU::task_main()
/* can we mix? */
if (_mixers != nullptr) {
+ unsigned num_outputs;
+
+ switch (_mode) {
+ case MODE_2PWM:
+ num_outputs = 2;
+ break;
+ case MODE_4PWM:
+ num_outputs = 4;
+ break;
+ case MODE_6PWM:
+ num_outputs = 6;
+ break;
+ default:
+ num_outputs = 0;
+ break;
+ }
+
/* do mixing */
outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
outputs.timestamp = hrt_absolute_time();
@@ -502,7 +568,7 @@ PX4FMU::task_main()
actuator_armed_s aa;
/* get new value */
- orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
+ orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa);
/* update PWM servo armed status if armed and not locked down */
bool set_armed = aa.armed && !aa.lockdown;
@@ -512,6 +578,7 @@ PX4FMU::task_main()
}
}
+#ifdef HRT_PPM_CHANNEL
// see if we have new PPM input data
if (ppm_last_valid_decode != rc_in.timestamp) {
// we have a new PPM frame. Publish it.
@@ -531,11 +598,13 @@ PX4FMU::task_main()
orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in);
}
}
+#endif
+
}
::close(_t_actuators);
::close(_t_actuators_effective);
- ::close(_t_armed);
+ ::close(_t_actuator_armed);
/* make sure servos are off */
up_pwm_servo_deinit();
@@ -579,6 +648,7 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg)
switch (_mode) {
case MODE_2PWM:
case MODE_4PWM:
+ case MODE_6PWM:
ret = pwm_ioctl(filp, cmd, arg);
break;
@@ -623,16 +693,24 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
ret = set_pwm_rate(arg, _pwm_default_rate, _pwm_alt_rate);
break;
- case PWM_SERVO_SET(2):
+ case PWM_SERVO_SET(5):
+ case PWM_SERVO_SET(4):
+ if (_mode < MODE_6PWM) {
+ ret = -EINVAL;
+ break;
+ }
+
+ /* FALLTHROUGH */
case PWM_SERVO_SET(3):
- if (_mode != MODE_4PWM) {
+ case PWM_SERVO_SET(2):
+ if (_mode < MODE_4PWM) {
ret = -EINVAL;
break;
}
/* FALLTHROUGH */
- case PWM_SERVO_SET(0):
case PWM_SERVO_SET(1):
+ case PWM_SERVO_SET(0):
if (arg < 2100) {
up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg);
} else {
@@ -641,16 +719,24 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
- case PWM_SERVO_GET(2):
+ case PWM_SERVO_GET(5):
+ case PWM_SERVO_GET(4):
+ if (_mode < MODE_6PWM) {
+ ret = -EINVAL;
+ break;
+ }
+
+ /* FALLTHROUGH */
case PWM_SERVO_GET(3):
- if (_mode != MODE_4PWM) {
+ case PWM_SERVO_GET(2):
+ if (_mode < MODE_4PWM) {
ret = -EINVAL;
break;
}
/* FALLTHROUGH */
- case PWM_SERVO_GET(0):
case PWM_SERVO_GET(1):
+ case PWM_SERVO_GET(0):
*(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0));
break;
@@ -658,16 +744,26 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
case PWM_SERVO_GET_RATEGROUP(1):
case PWM_SERVO_GET_RATEGROUP(2):
case PWM_SERVO_GET_RATEGROUP(3):
+ case PWM_SERVO_GET_RATEGROUP(4):
+ case PWM_SERVO_GET_RATEGROUP(5):
*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
break;
case PWM_SERVO_GET_COUNT:
case MIXERIOCGETOUTPUTCOUNT:
- if (_mode == MODE_4PWM) {
+ switch (_mode) {
+ case MODE_6PWM:
+ *(unsigned *)arg = 6;
+ break;
+ case MODE_4PWM:
*(unsigned *)arg = 4;
-
- } else {
+ break;
+ case MODE_2PWM:
*(unsigned *)arg = 2;
+ break;
+ default:
+ ret = -EINVAL;
+ break;
}
break;
@@ -743,17 +839,17 @@ ssize_t
PX4FMU::write(file *filp, const char *buffer, size_t len)
{
unsigned count = len / 2;
- uint16_t values[4];
+ uint16_t values[6];
- if (count > 4) {
- // we only have 4 PWM outputs on the FMU
- count = 4;
+ if (count > 6) {
+ // we have at most 6 outputs
+ count = 6;
}
// allow for misaligned values
- memcpy(values, buffer, count*2);
+ memcpy(values, buffer, count * 2);
- for (uint8_t i=0; i<count; i++) {
+ for (uint8_t i = 0; i < count; i++) {
up_pwm_servo_set(i, values[i]);
}
return count * 2;
@@ -763,19 +859,28 @@ void
PX4FMU::gpio_reset(void)
{
/*
- * Setup default GPIO config - all pins as GPIOs, GPIO driver chip
- * to input mode.
+ * Setup default GPIO config - all pins as GPIOs, input if
+ * possible otherwise output if possible.
*/
- for (unsigned i = 0; i < _ngpio; i++)
- stm32_configgpio(_gpio_tab[i].input);
+ for (unsigned i = 0; i < _ngpio; i++) {
+ if (_gpio_tab[i].input != 0) {
+ stm32_configgpio(_gpio_tab[i].input);
+ } else if (_gpio_tab[i].output != 0) {
+ stm32_configgpio(_gpio_tab[i].output);
+ }
+ }
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+ /* if we have a GPIO direction control, set it to zero (input) */
stm32_gpiowrite(GPIO_GPIO_DIR, 0);
stm32_configgpio(GPIO_GPIO_DIR);
+#endif
}
void
PX4FMU::gpio_set_function(uint32_t gpios, int function)
{
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
/*
* GPIOs 0 and 1 must have the same direction as they are buffered
* by a shared 2-port driver. Any attempt to set either sets both.
@@ -787,6 +892,7 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function)
if (GPIO_SET_OUTPUT == function)
stm32_gpiowrite(GPIO_GPIO_DIR, 1);
}
+#endif
/* configure selected GPIOs as required */
for (unsigned i = 0; i < _ngpio; i++) {
@@ -809,9 +915,11 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function)
}
}
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
/* flip buffer to input mode if required */
if ((GPIO_SET_INPUT == function) && (gpios & 3))
stm32_gpiowrite(GPIO_GPIO_DIR, 0);
+#endif
}
void
@@ -912,14 +1020,21 @@ fmu_new_mode(PortMode new_mode)
/* nothing more to do here */
break;
- case PORT_FULL_SERIAL:
- /* set all multi-GPIOs to serial mode */
- gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4;
- break;
-
case PORT_FULL_PWM:
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
/* select 4-pin PWM mode */
servo_mode = PX4FMU::MODE_4PWM;
+#endif
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+ servo_mode = PX4FMU::MODE_6PWM;
+#endif
+ break;
+
+ /* mixed modes supported on v1 board only */
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+ case PORT_FULL_SERIAL:
+ /* set all multi-GPIOs to serial mode */
+ gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4;
break;
case PORT_GPIO_AND_SERIAL:
@@ -938,6 +1053,9 @@ fmu_new_mode(PortMode new_mode)
/* select 2-pin PWM mode */
servo_mode = PX4FMU::MODE_2PWM;
break;
+#endif
+ default:
+ return -1;
}
/* adjust GPIO config for serial mode(s) */
@@ -979,22 +1097,84 @@ void
test(void)
{
int fd;
-
- fd = open(PWM_OUTPUT_DEVICE_PATH, 0);
+ unsigned servo_count = 0;
+ unsigned pwm_value = 1000;
+ int direction = 1;
+
+ fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
if (fd < 0)
errx(1, "open fail");
if (ioctl(fd, PWM_SERVO_ARM, 0) < 0) err(1, "servo arm failed");
- if (ioctl(fd, PWM_SERVO_SET(0), 1000) < 0) err(1, "servo 1 set failed");
+ if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count) != 0) {
+ err(1, "Unable to get servo count\n");
+ }
+
+ warnx("Testing %u servos", (unsigned)servo_count);
+
+ int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
+ if (!console)
+ err(1, "failed opening console");
+
+ warnx("Press CTRL-C or 'c' to abort.");
+
+ for (;;) {
+ /* sweep all servos between 1000..2000 */
+ servo_position_t servos[servo_count];
+ for (unsigned i = 0; i < servo_count; i++)
+ servos[i] = pwm_value;
+
+ if (direction == 1) {
+ // use ioctl interface for one direction
+ for (unsigned i=0; i < servo_count; i++) {
+ if (ioctl(fd, PWM_SERVO_SET(i), servos[i]) < 0) {
+ err(1, "servo %u set failed", i);
+ }
+ }
+ } else {
+ // and use write interface for the other direction
+ int ret = write(fd, servos, sizeof(servos));
+ if (ret != (int)sizeof(servos))
+ err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret);
+ }
+
+ if (direction > 0) {
+ if (pwm_value < 2000) {
+ pwm_value++;
+ } else {
+ direction = -1;
+ }
+ } else {
+ if (pwm_value > 1000) {
+ pwm_value--;
+ } else {
+ direction = 1;
+ }
+ }
- if (ioctl(fd, PWM_SERVO_SET(1), 1200) < 0) err(1, "servo 2 set failed");
+ /* readback servo values */
+ for (unsigned i = 0; i < servo_count; i++) {
+ servo_position_t value;
- if (ioctl(fd, PWM_SERVO_SET(2), 1400) < 0) err(1, "servo 3 set failed");
+ if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value))
+ err(1, "error reading PWM servo %d", i);
+ if (value != servos[i])
+ errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]);
+ }
- if (ioctl(fd, PWM_SERVO_SET(3), 1600) < 0) err(1, "servo 4 set failed");
+ /* Check if user wants to quit */
+ char c;
+ if (read(console, &c, 1) == 1) {
+ if (c == 0x03 || c == 0x63) {
+ warnx("User abort\n");
+ break;
+ }
+ }
+ }
+ close(console);
close(fd);
exit(0);
@@ -1053,12 +1233,13 @@ fmu_main(int argc, char *argv[])
if (!strcmp(verb, "mode_gpio")) {
new_mode = PORT_FULL_GPIO;
- } else if (!strcmp(verb, "mode_serial")) {
- new_mode = PORT_FULL_SERIAL;
-
} else if (!strcmp(verb, "mode_pwm")) {
new_mode = PORT_FULL_PWM;
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+ } else if (!strcmp(verb, "mode_serial")) {
+ new_mode = PORT_FULL_SERIAL;
+
} else if (!strcmp(verb, "mode_gpio_serial")) {
new_mode = PORT_GPIO_AND_SERIAL;
@@ -1067,6 +1248,7 @@ fmu_main(int argc, char *argv[])
} else if (!strcmp(verb, "mode_pwm_gpio")) {
new_mode = PORT_PWM_AND_GPIO;
+#endif
}
/* was a new mode set? */
@@ -1088,6 +1270,10 @@ fmu_main(int argc, char *argv[])
fake(argc - 1, argv + 1);
fprintf(stderr, "FMU: unrecognised command, try:\n");
- fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n");
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+ fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test\n");
+#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+ fprintf(stderr, " mode_gpio, mode_pwm, test\n");
+#endif
exit(1);
}
diff --git a/src/drivers/px4io/module.mk b/src/drivers/px4io/module.mk
index 328e5a684380ebc77c5d07a68335e0452c0191bd..2054faa12715adea1afb2b8dbb04038847d20578 100644
--- a/src/drivers/px4io/module.mk
+++ b/src/drivers/px4io/module.mk
@@ -38,4 +38,9 @@
MODULE_COMMAND = px4io
SRCS = px4io.cpp \
- uploader.cpp
+ px4io_uploader.cpp \
+ px4io_serial.cpp \
+ px4io_i2c.cpp
+
+# XXX prune to just get UART registers
+INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index fed83ea1ac7cacb5fa03a26442969c49b47a27c0..15873f79ba61eb9838e6b58ed6047450435fdcce 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -58,7 +58,6 @@
#include <arch/board/board.h>
#include <drivers/device/device.h>
-#include <drivers/device/i2c.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_gpio.h>
@@ -75,7 +74,9 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_outputs.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/safety.h>
+#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/battery_status.h>
@@ -83,9 +84,13 @@
#include <debug.h>
#include <mavlink/mavlink_log.h>
-#include "uploader.h"
#include <modules/px4iofirmware/protocol.h>
+#include "uploader.h"
+
+extern device::Device *PX4IO_i2c_interface() weak_function;
+extern device::Device *PX4IO_serial_interface() weak_function;
+
#define PX4IO_SET_DEBUG _IOC(0xff00, 0)
#define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1)
@@ -94,7 +99,7 @@
*
* Encapsulates PX4FMU to PX4IO communications modeled as file operations.
*/
-class PX4IO : public device::I2C
+class PX4IO : public device::CDev
{
public:
/**
@@ -102,7 +107,8 @@ public:
*
* Initialize all class variables.
*/
- PX4IO();
+ PX4IO(device::Device *interface);
+
/**
* Destructor.
*
@@ -143,8 +149,8 @@ public:
/**
* Set the update rate for actuator outputs from FMU to IO.
*
- * @param[in] rate The rate in Hz actuator output are sent to IO.
- * Min 10 Hz, max 400 Hz
+ * @param[in] rate The rate in Hz actuator outpus are sent to IO.
+ * Min 10 Hz, max 400 Hz
*/
int set_update_rate(int rate);
@@ -159,11 +165,26 @@ public:
/**
* Push failsafe values to IO.
*
- * @param[in] vals Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full)
- * @param[in] len Number of channels, could up to 8
+ * @param[in] vals Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full)
+ * @param[in] len Number of channels, could up to 8
*/
int set_failsafe_values(const uint16_t *vals, unsigned len);
+ /**
+ * Set the minimum PWM signals when armed
+ */
+ int set_min_values(const uint16_t *vals, unsigned len);
+
+ /**
+ * Set the maximum PWM signal when armed
+ */
+ int set_max_values(const uint16_t *vals, unsigned len);
+
+ /**
+ * Set an idle PWM signal that is active right after startup, even when SAFETY_SAFE
+ */
+ int set_idle_values(const uint16_t *vals, unsigned len);
+
/**
* Print IO status.
*
@@ -171,6 +192,11 @@ public:
*/
void print_status();
+ /**
+ * Disable RC input handling
+ */
+ int disable_rc_handling();
+
/**
* Set the DSM VCC is controlled by relay one flag
*
@@ -192,14 +218,18 @@ public:
};
private:
+ device::Device *_interface;
+
// XXX
+ unsigned _hardware; ///< Hardware revision
unsigned _max_actuators; ///<Maximum # of actuators supported by PX4IO
unsigned _max_controls; ///<Maximum # of controls supported by PX4IO
unsigned _max_rc_input; ///<Maximum receiver channels supported by PX4IO
unsigned _max_relays; ///<Maximum relays supported by PX4IO
unsigned _max_transfer; ///<Maximum number of I2C transfers supported by PX4IO
- unsigned _update_interval; ///<Subscription interval limiting send rate
+ unsigned _update_interval; ///< Subscription interval limiting send rate
+ bool _rc_handling_disabled; ///< If set, IO does not evaluate, but only forward the RC values
volatile int _task; ///<worker task id
volatile bool _task_should_exit; ///<worker terminate flag
@@ -213,16 +243,17 @@ private:
uint16_t _alarms; ///<Various IO alarms
/* subscribed topics */
- int _t_actuators; ///<actuator controls topic
- int _t_armed; ///<system armed control topic
- int _t_vstatus; ///<system / vehicle status
- int _t_param; ///<parameter update topic
+ int _t_actuators; ///< actuator controls topic
+ int _t_actuator_armed; ///< system armed control topic
+ int _t_vehicle_control_mode;///< vehicle control mode topic
+ int _t_param; ///< parameter update topic
/* advertised topics */
- orb_advert_t _to_input_rc; ///<rc inputs from IO topic
- orb_advert_t _to_actuators_effective; ///<effective actuator controls topic
- orb_advert_t _to_outputs; ///<mixed servo outputs topic
- orb_advert_t _to_battery; ///<battery status / voltage topic
+ orb_advert_t _to_input_rc; ///< rc inputs from io
+ orb_advert_t _to_actuators_effective; ///< effective actuator controls topic
+ orb_advert_t _to_outputs; ///< mixed servo outputs topic
+ orb_advert_t _to_battery; ///< battery status / voltage
+ orb_advert_t _to_safety; ///< status of safety
actuator_outputs_s _outputs; ///<mixed outputs
actuator_controls_effective_s _controls_effective; ///<effective controls
@@ -268,6 +299,11 @@ private:
*/
int io_get_status();
+ /**
+ * Disable RC input handling
+ */
+ int io_disable_rc_handling();
+
/**
* Fetch RC inputs from IO.
*
@@ -298,7 +334,7 @@ private:
* @param offset Register offset to start writing at.
* @param values Pointer to array of values to write.
* @param num_values The number of values to write.
- * @return Zero if all values were successfully written.
+ * @return OK if all values were successfully written.
*/
int io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values);
@@ -308,7 +344,7 @@ private:
* @param page Register page to write to.
* @param offset Register offset to write to.
* @param value Value to write.
- * @return Zero if the value was written successfully.
+ * @return OK if the value was written successfully.
*/
int io_reg_set(uint8_t page, uint8_t offset, const uint16_t value);
@@ -319,7 +355,7 @@ private:
* @param offset Register offset to start reading from.
* @param values Pointer to array where values should be stored.
* @param num_values The number of values to read.
- * @return Zero if all values were successfully read.
+ * @return OK if all values were successfully read.
*/
int io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values);
@@ -376,14 +412,17 @@ PX4IO *g_dev;
}
-PX4IO::PX4IO() :
- I2C("px4io", PX4IO_DEVICE_PATH, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000),
+PX4IO::PX4IO(device::Device *interface) :
+ CDev("px4io", PX4IO_DEVICE_PATH),
+ _interface(interface),
+ _hardware(0),
_max_actuators(0),
_max_controls(0),
_max_rc_input(0),
_max_relays(0),
_max_transfer(16), /* sensible default */
_update_interval(0),
+ _rc_handling_disabled(false),
_task(-1),
_task_should_exit(false),
_mavlink_fd(-1),
@@ -391,13 +430,14 @@ PX4IO::PX4IO() :
_status(0),
_alarms(0),
_t_actuators(-1),
- _t_armed(-1),
- _t_vstatus(-1),
+ _t_actuator_armed(-1),
+ _t_vehicle_control_mode(-1),
_t_param(-1),
_to_input_rc(0),
_to_actuators_effective(0),
_to_outputs(0),
_to_battery(0),
+ _to_safety(0),
_primary_pwm_device(false),
_battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor
_battery_amp_bias(0),
@@ -429,6 +469,9 @@ PX4IO::~PX4IO()
if (_task != -1)
task_delete(_task);
+ if (_interface != nullptr)
+ delete _interface;
+
g_dev = nullptr;
}
@@ -440,31 +483,30 @@ PX4IO::init()
ASSERT(_task == -1);
/* do regular cdev init */
- ret = I2C::init();
+ ret = CDev::init();
if (ret != OK)
return ret;
- /*
- * Enable a couple of retries for operations to IO.
- *
- * Register read/write operations are intentionally idempotent
- * so this is safe as designed.
- */
- _retries = 2;
-
/* get some parameters */
+ unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION);
+ if (protocol != PX4IO_PROTOCOL_VERSION) {
+ log("protocol/firmware mismatch");
+ mavlink_log_emergency(_mavlink_fd, "[IO] protocol/firmware mismatch, abort.");
+ return -1;
+ }
+ _hardware = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION);
_max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT);
- _max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT);
+ _max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT);
_max_relays = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT);
_max_transfer = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER) - 2;
_max_rc_input = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT);
if ((_max_actuators < 1) || (_max_actuators > 255) ||
- (_max_relays < 1) || (_max_relays > 255) ||
- (_max_transfer < 16) || (_max_transfer > 255) ||
+ (_max_relays > 32) ||
+ (_max_transfer < 16) || (_max_transfer > 255) ||
(_max_rc_input < 1) || (_max_rc_input > 255)) {
- log("failed getting parameters from PX4IO");
- mavlink_log_emergency(_mavlink_fd, "[IO] param read fail, abort.");
+ log("config read error");
+ mavlink_log_emergency(_mavlink_fd, "[IO] config read fail, abort.");
return -1;
}
if (_max_rc_input > RC_INPUT_MAX_CHANNELS)
@@ -492,26 +534,27 @@ PX4IO::init()
(reg & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART");
+ log("INAIR RESTART RECOVERY (needs commander app running)");
/* WARNING: COMMANDER app/vehicle status must be initialized.
* If this fails (or the app is not started), worst-case IO
* remains untouched (so manual override is still available).
*/
- int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
+ int safety_sub = orb_subscribe(ORB_ID(actuator_armed));
/* fill with initial values, clear updated flag */
- vehicle_status_s status;
+ struct actuator_armed_s safety;
uint64_t try_start_time = hrt_absolute_time();
bool updated = false;
- /* keep checking for an update, ensure we got a recent state,
+ /* keep checking for an update, ensure we got a arming information,
not something that was published a long time ago. */
do {
- orb_check(vstatus_sub, &updated);
+ orb_check(safety_sub, &updated);
if (updated) {
/* got data, copy and exit loop */
- orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status);
+ orb_copy(ORB_ID(actuator_armed), safety_sub, &safety);
break;
}
@@ -519,7 +562,7 @@ PX4IO::init()
usleep(10000);
/* abort after 5s */
- if ((hrt_absolute_time() - try_start_time)/1000 > 50000) {
+ if ((hrt_absolute_time() - try_start_time)/1000 > 3000) {
log("failed to recover from in-air restart (1), aborting IO driver init.");
return 1;
}
@@ -537,35 +580,41 @@ PX4IO::init()
cmd.param6 = 0;
cmd.param7 = 0;
cmd.command = VEHICLE_CMD_COMPONENT_ARM_DISARM;
- cmd.target_system = status.system_id;
- cmd.target_component = status.component_id;
- cmd.source_system = status.system_id;
- cmd.source_component = status.component_id;
+ // cmd.target_system = status.system_id;
+ // cmd.target_component = status.component_id;
+ // cmd.source_system = status.system_id;
+ // cmd.source_component = status.component_id;
/* ask to confirm command */
cmd.confirmation = 1;
/* send command once */
- (void)orb_advertise(ORB_ID(vehicle_command), &cmd);
+ orb_advert_t pub = orb_advertise(ORB_ID(vehicle_command), &cmd);
/* spin here until IO's state has propagated into the system */
do {
- orb_check(vstatus_sub, &updated);
+ orb_check(safety_sub, &updated);
if (updated) {
- orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status);
+ orb_copy(ORB_ID(actuator_armed), safety_sub, &safety);
}
- /* wait 10 ms */
- usleep(10000);
+ /* wait 50 ms */
+ usleep(50000);
/* abort after 5s */
- if ((hrt_absolute_time() - try_start_time)/1000 > 50000) {
+ if ((hrt_absolute_time() - try_start_time)/1000 > 2000) {
log("failed to recover from in-air restart (2), aborting IO driver init.");
return 1;
}
- /* keep waiting for state change for 10 s */
- } while (!status.flag_system_armed);
+ /* re-send if necessary */
+ if (!safety.armed) {
+ orb_publish(ORB_ID(vehicle_command), pub, &cmd);
+ log("re-sending arm cmd");
+ }
+
+ /* keep waiting for state change for 2 s */
+ } while (!safety.armed);
/* regular boot, no in-air restart, init IO */
} else {
@@ -575,14 +624,19 @@ PX4IO::init()
io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING,
PX4IO_P_SETUP_ARMING_FMU_ARMED |
PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK |
- PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, 0);
+ PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK |
+ PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE, 0);
- /* publish RC config to IO */
- ret = io_set_rc_config();
- if (ret != OK) {
- log("failed to update RC input config");
- mavlink_log_info(_mavlink_fd, "[IO] RC config upload fail");
- return ret;
+ if (_rc_handling_disabled) {
+ ret = io_disable_rc_handling();
+ } else {
+ /* publish RC config to IO */
+ ret = io_set_rc_config();
+ if (ret != OK) {
+ log("failed to update RC input config");
+ mavlink_log_info(_mavlink_fd, "[IO] RC config upload fail");
+ return ret;
+ }
}
}
@@ -596,7 +650,7 @@ PX4IO::init()
}
/* start the IO interface task */
- _task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr);
+ _task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 2048, (main_t)&PX4IO::task_main_trampoline, nullptr);
if (_task < 0) {
debug("task start failed: %d", errno);
@@ -631,22 +685,30 @@ PX4IO::task_main()
ORB_ID(actuator_controls_1));
orb_set_interval(_t_actuators, 20); /* default to 50Hz */
- _t_armed = orb_subscribe(ORB_ID(actuator_armed));
- orb_set_interval(_t_armed, 200); /* 5Hz update rate */
+ _t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed));
+ orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */
- _t_vstatus = orb_subscribe(ORB_ID(vehicle_status));
- orb_set_interval(_t_vstatus, 200); /* 5Hz update rate max. */
+ _t_vehicle_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
+ orb_set_interval(_t_vehicle_control_mode, 200); /* 5Hz update rate max. */
_t_param = orb_subscribe(ORB_ID(parameter_update));
orb_set_interval(_t_param, 500); /* 2Hz update rate max. */
+ if ((_t_actuators < 0) ||
+ (_t_actuator_armed < 0) ||
+ (_t_vehicle_control_mode < 0) ||
+ (_t_param < 0)) {
+ log("subscription(s) failed");
+ goto out;
+ }
+
/* poll descriptor */
pollfd fds[4];
fds[0].fd = _t_actuators;
fds[0].events = POLLIN;
- fds[1].fd = _t_armed;
+ fds[1].fd = _t_actuator_armed;
fds[1].events = POLLIN;
- fds[2].fd = _t_vstatus;
+ fds[2].fd = _t_vehicle_control_mode;
fds[2].events = POLLIN;
fds[3].fd = _t_param;
fds[3].events = POLLIN;
@@ -728,7 +790,7 @@ PX4IO::task_main()
// See if bind parameter has been set, and reset it to 0
param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val);
if (dsm_bind_val) {
- if (!(_status & PX4IO_P_STATUS_FLAGS_ARMED)) {
+ if (!(_status & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
if ((dsm_bind_val == 1) || (dsm_bind_val == 2)) {
mavlink_log_info(mavlink_fd, "[IO] binding dsm%c rx", dsm_bind_val == 1 ? '2' : 'x');
ioctl(nullptr, DSM_BIND_START, dsm_bind_val == 1 ? 3 : 7);
@@ -755,6 +817,7 @@ PX4IO::task_main()
unlock();
+out:
debug("exiting");
/* clean up the alternate device node */
@@ -794,14 +857,56 @@ PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len)
return io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, vals, len);
}
+int
+PX4IO::set_min_values(const uint16_t *vals, unsigned len)
+{
+ uint16_t regs[_max_actuators];
+
+ if (len > _max_actuators)
+ /* fail with error */
+ return E2BIG;
+
+ /* copy values to registers in IO */
+ return io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, vals, len);
+}
+
+int
+PX4IO::set_max_values(const uint16_t *vals, unsigned len)
+{
+ uint16_t regs[_max_actuators];
+
+ if (len > _max_actuators)
+ /* fail with error */
+ return E2BIG;
+
+ /* copy values to registers in IO */
+ return io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, vals, len);
+}
+
+int
+PX4IO::set_idle_values(const uint16_t *vals, unsigned len)
+{
+ uint16_t regs[_max_actuators];
+
+ if (len > _max_actuators)
+ /* fail with error */
+ return E2BIG;
+
+ printf("Sending IDLE values\n");
+
+ /* copy values to registers in IO */
+ return io_reg_set(PX4IO_PAGE_IDLE_PWM, 0, vals, len);
+}
+
+
int
PX4IO::io_set_arming_state()
{
actuator_armed_s armed; ///< system armed state
- vehicle_status_s vstatus; ///< overall system state
+ vehicle_control_mode_s control_mode; ///< vehicle_control_mode
- orb_copy(ORB_ID(actuator_armed), _t_armed, &armed);
- orb_copy(ORB_ID(vehicle_status), _t_vstatus, &vstatus);
+ orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed);
+ orb_copy(ORB_ID(vehicle_control_mode), _t_vehicle_control_mode, &control_mode);
uint16_t set = 0;
uint16_t clear = 0;
@@ -811,14 +916,13 @@ PX4IO::io_set_arming_state()
} else {
clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
}
-
if (armed.ready_to_arm) {
set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
} else {
clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
}
- if (vstatus.flag_external_manual_override_ok) {
+ if (control_mode.flag_external_manual_override_ok) {
set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
} else {
clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
@@ -827,6 +931,21 @@ PX4IO::io_set_arming_state()
return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set);
}
+int
+PX4IO::disable_rc_handling()
+{
+ return io_disable_rc_handling();
+}
+
+int
+PX4IO::io_disable_rc_handling()
+{
+ uint16_t set = PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED;
+ uint16_t clear = 0;
+
+ return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set);
+}
+
int
PX4IO::io_set_rc_config()
{
@@ -945,14 +1064,14 @@ PX4IO::io_handle_status(uint16_t status)
*/
/* check for IO reset - force it back to armed if necessary */
- if (_status & PX4IO_P_STATUS_FLAGS_ARMED && !(status & PX4IO_P_STATUS_FLAGS_ARMED)
+ if (_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF && !(status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
&& !(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
/* set the arming flag */
- ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARMED | PX4IO_P_STATUS_FLAGS_ARM_SYNC);
+ ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_SAFETY_OFF | PX4IO_P_STATUS_FLAGS_ARM_SYNC);
/* set new status */
_status = status;
- _status &= PX4IO_P_STATUS_FLAGS_ARMED;
+ _status &= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
} else if (!(_status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
/* set the sync flag */
@@ -967,6 +1086,27 @@ PX4IO::io_handle_status(uint16_t status)
_status = status;
}
+ /**
+ * Get and handle the safety status
+ */
+ struct safety_s safety;
+ safety.timestamp = hrt_absolute_time();
+
+ if (status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
+ safety.safety_off = true;
+ safety.safety_switch_available = true;
+ } else {
+ safety.safety_off = false;
+ safety.safety_switch_available = true;
+ }
+
+ /* lazily publish the safety status */
+ if (_to_safety > 0) {
+ orb_publish(ORB_ID(safety), _to_safety, &safety);
+ } else {
+ _to_safety = orb_advertise(ORB_ID(safety), &safety);
+ }
+
return ret;
}
@@ -996,7 +1136,7 @@ PX4IO::io_get_status()
io_handle_status(regs[0]);
io_handle_alarms(regs[1]);
-
+
/* only publish if battery has a valid minimum voltage */
if (regs[2] > 3300) {
battery_status_s battery_status;
@@ -1030,6 +1170,7 @@ PX4IO::io_get_status()
_to_battery = orb_advertise(ORB_ID(battery_status), &battery_status);
}
}
+
return ret;
}
@@ -1037,38 +1178,38 @@ int
PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
{
uint32_t channel_count;
- int ret = OK;
+ int ret;
/* we don't have the status bits, so input_source has to be set elsewhere */
input_rc.input_source = RC_INPUT_SOURCE_UNKNOWN;
+ static const unsigned prolog = (PX4IO_P_RAW_RC_BASE - PX4IO_P_RAW_RC_COUNT);
+ uint16_t regs[RC_INPUT_MAX_CHANNELS + prolog];
+
/*
- * XXX Because the channel count and channel data are fetched
- * separately, there is a risk of a race between the two
- * that could leave us with channel data and a count that
- * are out of sync.
- * Fixing this would require a guarantee of atomicity from
- * IO, and a single fetch for both count and channels.
+ * Read the channel count and the first 9 channels.
*
- * XXX Since IO has the input calibration info, we ought to be
- * able to get the pre-fixed-up controls directly.
- *
- * XXX can we do this more cheaply? If we knew we had DMA, it would
- * almost certainly be better to just get all the inputs...
+ * This should be the common case (9 channel R/C control being a reasonable upper bound).
*/
- channel_count = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT);
- if (channel_count == _io_reg_get_error)
- return -EIO;
- if (channel_count > RC_INPUT_MAX_CHANNELS)
- channel_count = RC_INPUT_MAX_CHANNELS;
- input_rc.channel_count = channel_count;
+ input_rc.timestamp = hrt_absolute_time();
+ ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, ®s[0], prolog + 9);
+ if (ret != OK)
+ return ret;
- if (channel_count > 0) {
- ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, input_rc.values, channel_count);
- if (ret == OK)
- input_rc.timestamp = hrt_absolute_time();
+ /*
+ * Get the channel count any any extra channels. This is no more expensive than reading the
+ * channel count once.
+ */
+ channel_count = regs[0];
+ if (channel_count > 9) {
+ ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, ®s[prolog + 9], channel_count - 9);
+ if (ret != OK)
+ return ret;
}
+ input_rc.channel_count = channel_count;
+ memcpy(input_rc.values, ®s[prolog], channel_count * 2);
+
return ret;
}
@@ -1198,25 +1339,12 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned
return -EINVAL;
}
- /* set up the transfer */
- uint8_t addr[2] = {
- page,
- offset
- };
- i2c_msg_s msgv[2];
-
- msgv[0].flags = 0;
- msgv[0].buffer = addr;
- msgv[0].length = 2;
- msgv[1].flags = I2C_M_NORESTART;
- msgv[1].buffer = (uint8_t *)values;
- msgv[1].length = num_values * sizeof(*values);
-
- /* perform the transfer */
- int ret = transfer(msgv, 2);
- if (ret != OK)
- debug("io_reg_set: error %d", ret);
- return ret;
+ int ret = _interface->write((page << 8) | offset, (void *)values, num_values);
+ if (ret != num_values) {
+ debug("io_reg_set(%u,%u,%u): error %d", page, offset, num_values, ret);
+ return -1;
+ }
+ return OK;
}
int
@@ -1228,25 +1356,18 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, uint16_t value)
int
PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values)
{
- /* set up the transfer */
- uint8_t addr[2] = {
- page,
- offset
- };
- i2c_msg_s msgv[2];
-
- msgv[0].flags = 0;
- msgv[0].buffer = addr;
- msgv[0].length = 2;
- msgv[1].flags = I2C_M_READ;
- msgv[1].buffer = (uint8_t *)values;
- msgv[1].length = num_values * sizeof(*values);
+ /* range check the transfer */
+ if (num_values > ((_max_transfer) / sizeof(*values))) {
+ debug("io_reg_get: too many registers (%u, max %u)", num_values, _max_transfer / 2);
+ return -EINVAL;
+ }
- /* perform the transfer */
- int ret = transfer(msgv, 2);
- if (ret != OK)
- debug("io_reg_get: data error %d", ret);
- return ret;
+ int ret = _interface->read((page << 8) | offset, reinterpret_cast<void *>(values), num_values);
+ if (ret != num_values) {
+ debug("io_reg_get(%u,%u,%u): data error %d", page, offset, num_values, ret);
+ return -1;
+ }
+ return OK;
}
uint32_t
@@ -1254,7 +1375,7 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset)
{
uint16_t value;
- if (io_reg_get(page, offset, &value, 1))
+ if (io_reg_get(page, offset, &value, 1) != OK)
return _io_reg_get_error;
return value;
@@ -1267,7 +1388,7 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t
uint16_t value;
ret = io_reg_get(page, offset, &value, 1);
- if (ret)
+ if (ret != OK)
return ret;
value &= ~clearbits;
value |= setbits;
@@ -1339,9 +1460,9 @@ void
PX4IO::print_status()
{
/* basic configuration */
- printf("protocol %u software %u bootloader %u buffer %uB\n",
+ printf("protocol %u hardware %u bootloader %u buffer %uB\n",
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION),
- io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_SOFTWARE_VERSION),
+ io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION),
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION),
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER));
printf("%u controls %u actuators %u R/C inputs %u analog inputs %u relays\n",
@@ -1357,7 +1478,8 @@ PX4IO::print_status()
uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS);
printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s%s\n",
flags,
- ((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " ARMED" : ""),
+ ((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""),
+ ((flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? " SAFETY_OFF" : " SAFETY_SAFE"),
((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""),
((flags & PX4IO_P_STATUS_FLAGS_RC_OK) ? " RC_OK" : " RC_FAIL"),
((flags & PX4IO_P_STATUS_FLAGS_RC_PPM) ? " PPM" : ""),
@@ -1365,13 +1487,13 @@ PX4IO::print_status()
(((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_STATUS_FLAGS_RC_DSM11)) ? " DSM11" : ""),
((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS) ? " SBUS" : ""),
((flags & PX4IO_P_STATUS_FLAGS_FMU_OK) ? " FMU_OK" : " FMU_FAIL"),
- ((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PPM" : ""),
+ ((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PWM_PASSTHROUGH" : ""),
((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"),
((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"),
((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL"),
((flags & PX4IO_P_STATUS_FLAGS_FAILSAFE) ? " FAILSAFE" : ""));
uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS);
- printf("alarms 0x%04x%s%s%s%s%s%s%s\n",
+ printf("alarms 0x%04x%s%s%s%s%s%s%s%s\n",
alarms,
((alarms & PX4IO_P_STATUS_ALARMS_VBATT_LOW) ? " VBATT_LOW" : ""),
((alarms & PX4IO_P_STATUS_ALARMS_TEMPERATURE) ? " TEMPERATURE" : ""),
@@ -1379,18 +1501,26 @@ PX4IO::print_status()
((alarms & PX4IO_P_STATUS_ALARMS_ACC_CURRENT) ? " ACC_CURRENT" : ""),
((alarms & PX4IO_P_STATUS_ALARMS_FMU_LOST) ? " FMU_LOST" : ""),
((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST) ? " RC_LOST" : ""),
- ((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) ? " PWM_ERROR" : ""));
+ ((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) ? " PWM_ERROR" : ""),
+ ((alarms & PX4IO_P_STATUS_ALARMS_VSERVO_FAULT) ? " VSERVO_FAULT" : ""));
/* now clear alarms */
io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, 0xFFFF);
- printf("vbatt %u ibatt %u vbatt scale %u\n",
- io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT),
- io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT),
- io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE));
- printf("amp_per_volt %.3f amp_offset %.3f mAh discharged %.3f\n",
- (double)_battery_amp_per_volt,
- (double)_battery_amp_bias,
- (double)_battery_mamphour_total);
+ if (_hardware == 1) {
+ printf("vbatt mV %u ibatt mV %u vbatt scale %u\n",
+ io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT),
+ io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT),
+ io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE));
+ printf("amp_per_volt %.3f amp_offset %.3f mAh discharged %.3f\n",
+ (double)_battery_amp_per_volt,
+ (double)_battery_amp_bias,
+ (double)_battery_mamphour_total);
+ } else if (_hardware == 2) {
+ printf("vservo %u mV vservo scale %u\n",
+ io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VSERVO),
+ io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VSERVO_SCALE));
+ printf("vrssi %u\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VRSSI));
+ }
printf("actuators");
for (unsigned i = 0; i < _max_actuators; i++)
printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i));
@@ -1420,12 +1550,14 @@ PX4IO::print_status()
/* setup and state */
printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES));
uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
- printf("arming 0x%04x%s%s%s%s\n",
+ printf("arming 0x%04x%s%s%s%s%s%s\n",
arming,
- ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : ""),
- ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : ""),
+ ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : " FMU_DISARMED"),
+ ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : " IO_ARM_DENIED"),
((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""),
- ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""));
+ ((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ? " FAILSAFE_CUSTOM" : ""),
+ ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""),
+ ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : ""));
printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE),
@@ -1452,7 +1584,10 @@ PX4IO::print_status()
}
printf("failsafe");
for (unsigned i = 0; i < _max_actuators; i++)
- printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i));
+ printf(" %u\n", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i));
+ printf("\nidle values");
+ for (unsigned i = 0; i < _max_actuators; i++)
+ printf(" %u", io_reg_get(PX4IO_PAGE_IDLE_PWM, i));
printf("\n");
}
@@ -1561,9 +1696,9 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0);
- uint32_t value = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + channel);
-
- *(uint32_t *)arg = value;
+ *(uint32_t *)arg = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + channel);
+ if (*(uint32_t *)arg == _io_reg_get_error)
+ ret = -EIO;
break;
}
@@ -1571,7 +1706,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
uint32_t bits = (1 << _max_relays) - 1;
/* don't touch relay1 if it's controlling RX vcc */
if (_dsm_vcc_ctl)
- bits &= ~PX4IO_RELAY1;
+ bits &= ~PX4IO_P_SETUP_RELAYS_POWER1;
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, bits, 0);
break;
}
@@ -1579,7 +1714,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
case GPIO_SET:
arg &= ((1 << _max_relays) - 1);
/* don't touch relay1 if it's controlling RX vcc */
- if (_dsm_vcc_ctl & (arg & PX4IO_RELAY1))
+ if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1))
ret = -EINVAL;
else
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg);
@@ -1588,7 +1723,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
case GPIO_CLEAR:
arg &= ((1 << _max_relays) - 1);
/* don't touch relay1 if it's controlling RX vcc */
- if (_dsm_vcc_ctl & (arg & PX4IO_RELAY1))
+ if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1))
ret = -EINVAL;
else
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0);
@@ -1686,8 +1821,8 @@ int
PX4IO::set_update_rate(int rate)
{
int interval_ms = 1000 / rate;
- if (interval_ms < 5) {
- interval_ms = 5;
+ if (interval_ms < 3) {
+ interval_ms = 3;
warnx("update rate too high, limiting interval to %d ms (%d Hz).", interval_ms, 1000 / interval_ms);
}
@@ -1712,14 +1847,47 @@ extern "C" __EXPORT int px4io_main(int argc, char *argv[]);
namespace
{
+device::Device *
+get_interface()
+{
+ device::Device *interface = nullptr;
+
+#ifndef CONFIG_ARCH_BOARD_PX4FMU_V1
+ /* try for a serial interface */
+ if (PX4IO_serial_interface != nullptr)
+ interface = PX4IO_serial_interface();
+ if (interface != nullptr)
+ goto got;
+#endif
+
+ /* try for an I2C interface if we haven't got a serial one */
+ if (PX4IO_i2c_interface != nullptr)
+ interface = PX4IO_i2c_interface();
+ if (interface != nullptr)
+ goto got;
+
+ errx(1, "cannot alloc interface");
+
+got:
+ if (interface->init() != OK) {
+ delete interface;
+ errx(1, "interface init failed");
+ }
+
+ return interface;
+}
+
void
start(int argc, char *argv[])
{
if (g_dev != nullptr)
errx(1, "already loaded");
+ /* allocate the interface */
+ device::Device *interface = get_interface();
+
/* create the driver - it will set g_dev */
- (void)new PX4IO();
+ (void)new PX4IO(interface);
if (g_dev == nullptr)
errx(1, "driver alloc failed");
@@ -1729,6 +1897,18 @@ start(int argc, char *argv[])
errx(1, "driver init failed");
}
+ /* disable RC handling on request */
+ if (argc > 1) {
+ if (!strcmp(argv[1], "norc")) {
+
+ if(g_dev->disable_rc_handling())
+ warnx("Failed disabling RC handling");
+
+ } else {
+ warnx("unknown argument: %s", argv[1]);
+ }
+ }
+
int dsm_vcc_ctl;
if (param_get(param_find("RC_RL1_DSM_VCC"), &dsm_vcc_ctl) == OK) {
@@ -1786,6 +1966,11 @@ test(void)
if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count))
err(1, "failed to get servo count");
+ /* tell IO that its ok to disable its safety with the switch */
+ ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
+ if (ret != OK)
+ err(1, "PWM_SERVO_SET_ARM_OK");
+
if (ioctl(fd, PWM_SERVO_ARM, 0))
err(1, "failed to arm servos");
@@ -1834,7 +2019,7 @@ test(void)
/* Check if user wants to quit */
char c;
if (read(console, &c, 1) == 1) {
- if (c == 0x03 || c == 0x63) {
+ if (c == 0x03 || c == 0x63 || c == 'q') {
warnx("User abort\n");
close(console);
exit(0);
@@ -1871,8 +2056,19 @@ monitor(void)
}
}
+void
+if_test(unsigned mode)
+{
+ device::Device *interface = get_interface();
+
+ int result = interface->ioctl(1, mode); /* XXX magic numbers */
+ delete interface;
+
+ errx(0, "test returned %d", result);
}
+} /* namespace */
+
int
px4io_main(int argc, char *argv[])
{
@@ -1883,28 +2079,87 @@ px4io_main(int argc, char *argv[])
if (!strcmp(argv[1], "start"))
start(argc - 1, argv + 1);
- if (!strcmp(argv[1], "limit")) {
+ if (!strcmp(argv[1], "update")) {
if (g_dev != nullptr) {
+ printf("[px4io] loaded, detaching first\n");
+ /* stop the driver */
+ delete g_dev;
+ }
- if ((argc > 2)) {
- g_dev->set_update_rate(atoi(argv[2]));
- } else {
- errx(1, "missing argument (50 - 200 Hz)");
- return 1;
- }
+ PX4IO_Uploader *up;
+ const char *fn[5];
+
+ /* work out what we're uploading... */
+ if (argc > 2) {
+ fn[0] = argv[2];
+ fn[1] = nullptr;
+
+ } else {
+ fn[0] = "/fs/microsd/px4io.bin";
+ fn[1] = "/etc/px4io.bin";
+ fn[2] = "/fs/microsd/px4io2.bin";
+ fn[3] = "/etc/px4io2.bin";
+ fn[4] = nullptr;
+ }
+
+ up = new PX4IO_Uploader;
+ int ret = up->upload(&fn[0]);
+ delete up;
+
+ switch (ret) {
+ case OK:
+ break;
+
+ case -ENOENT:
+ errx(1, "PX4IO firmware file not found");
+
+ case -EEXIST:
+ case -EIO:
+ errx(1, "error updating PX4IO - check that bootloader mode is enabled");
+
+ case -EINVAL:
+ errx(1, "verify failed - retry the update");
+
+ case -ETIMEDOUT:
+ errx(1, "timed out waiting for bootloader - power-cycle and try again");
+
+ default:
+ errx(1, "unexpected error %d", ret);
+ }
+
+ return ret;
+ }
+
+ if (!strcmp(argv[1], "iftest")) {
+ if (g_dev != nullptr)
+ errx(1, "can't iftest when started");
+
+ if_test((argc > 2) ? strtol(argv[2], NULL, 0) : 0);
+ }
+
+ /* commands below here require a started driver */
+
+ if (g_dev == nullptr)
+ errx(1, "not started");
+
+ if (!strcmp(argv[1], "limit")) {
+
+ if ((argc > 2)) {
+ g_dev->set_update_rate(atoi(argv[2]));
+ } else {
+ errx(1, "missing argument (50 - 400 Hz)");
+ return 1;
}
exit(0);
}
if (!strcmp(argv[1], "current")) {
- if (g_dev != nullptr) {
- if ((argc > 3)) {
- g_dev->set_battery_current_scaling(atof(argv[2]), atof(argv[3]));
- } else {
- errx(1, "missing argument (apm_per_volt, amp_offset)");
- return 1;
- }
+ if ((argc > 3)) {
+ g_dev->set_battery_current_scaling(atof(argv[2]), atof(argv[3]));
+ } else {
+ errx(1, "missing argument (apm_per_volt, amp_offset)");
+ return 1;
}
exit(0);
}
@@ -1915,70 +2170,158 @@ px4io_main(int argc, char *argv[])
errx(1, "failsafe command needs at least one channel value (ppm)");
}
+ /* set values for first 8 channels, fill unassigned channels with 1500. */
+ uint16_t failsafe[8];
+
+ for (int i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++) {
+
+ /* set channel to commandline argument or to 900 for non-provided channels */
+ if (argc > i + 2) {
+ failsafe[i] = atoi(argv[i+2]);
+ if (failsafe[i] < 800 || failsafe[i] > 2200) {
+ errx(1, "value out of range of 800 < value < 2200. Aborting.");
+ }
+ } else {
+ /* a zero value will result in stopping to output any pulse */
+ failsafe[i] = 0;
+ }
+ }
+
+ int ret = g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0]));
+
+ if (ret != OK)
+ errx(ret, "failed setting failsafe values");
+ exit(0);
+ }
+
+ if (!strcmp(argv[1], "min")) {
+
+ if (argc < 3) {
+ errx(1, "min command needs at least one channel value (PWM)");
+ }
+
if (g_dev != nullptr) {
- /* set values for first 8 channels, fill unassigned channels with 1500. */
- uint16_t failsafe[8];
+ /* set values for first 8 channels, fill unassigned channels with 900. */
+ uint16_t min[8];
- for (int i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++)
+ for (int i = 0; i < sizeof(min) / sizeof(min[0]); i++)
{
/* set channel to commanline argument or to 900 for non-provided channels */
if (argc > i + 2) {
- failsafe[i] = atoi(argv[i+2]);
- if (failsafe[i] < 800 || failsafe[i] > 2200) {
- errx(1, "value out of range of 800 < value < 2200. Aborting.");
+ min[i] = atoi(argv[i+2]);
+ if (min[i] < 900 || min[i] > 1200) {
+ errx(1, "value out of range of 900 < value < 1200. Aborting.");
}
} else {
- /* a zero value will result in stopping to output any pulse */
- failsafe[i] = 0;
+ /* a zero value will the default */
+ min[i] = 0;
}
}
- int ret = g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0]));
+ int ret = g_dev->set_min_values(min, sizeof(min) / sizeof(min[0]));
if (ret != OK)
- errx(ret, "failed setting failsafe values");
+ errx(ret, "failed setting min values");
} else {
errx(1, "not loaded");
}
exit(0);
}
- if (!strcmp(argv[1], "recovery")) {
+ if (!strcmp(argv[1], "max")) {
+
+ if (argc < 3) {
+ errx(1, "max command needs at least one channel value (PWM)");
+ }
if (g_dev != nullptr) {
- /*
- * Enable in-air restart support.
- * We can cheat and call the driver directly, as it
- * doesn't reference filp in ioctl()
- */
- g_dev->ioctl(nullptr, PX4IO_INAIR_RESTART_ENABLE, 1);
+
+ /* set values for first 8 channels, fill unassigned channels with 2100. */
+ uint16_t max[8];
+
+ for (int i = 0; i < sizeof(max) / sizeof(max[0]); i++)
+ {
+ /* set channel to commanline argument or to 2100 for non-provided channels */
+ if (argc > i + 2) {
+ max[i] = atoi(argv[i+2]);
+ if (max[i] < 1800 || max[i] > 2100) {
+ errx(1, "value out of range of 1800 < value < 2100. Aborting.");
+ }
+ } else {
+ /* a zero value will the default */
+ max[i] = 0;
+ }
+ }
+
+ int ret = g_dev->set_max_values(max, sizeof(max) / sizeof(max[0]));
+
+ if (ret != OK)
+ errx(ret, "failed setting max values");
} else {
errx(1, "not loaded");
}
exit(0);
}
- if (!strcmp(argv[1], "stop")) {
+ if (!strcmp(argv[1], "idle")) {
+
+ if (argc < 3) {
+ errx(1, "max command needs at least one channel value (PWM)");
+ }
if (g_dev != nullptr) {
- /* stop the driver */
- delete g_dev;
+
+ /* set values for first 8 channels, fill unassigned channels with 0. */
+ uint16_t idle[8];
+
+ for (int i = 0; i < sizeof(idle) / sizeof(idle[0]); i++)
+ {
+ /* set channel to commanline argument or to 0 for non-provided channels */
+ if (argc > i + 2) {
+ idle[i] = atoi(argv[i+2]);
+ if (idle[i] < 900 || idle[i] > 2100) {
+ errx(1, "value out of range of 900 < value < 2100. Aborting.");
+ }
+ } else {
+ /* a zero value will the default */
+ idle[i] = 0;
+ }
+ }
+
+ int ret = g_dev->set_idle_values(idle, sizeof(idle) / sizeof(idle[0]));
+
+ if (ret != OK)
+ errx(ret, "failed setting idle values");
} else {
errx(1, "not loaded");
}
exit(0);
}
+ if (!strcmp(argv[1], "recovery")) {
+
+ /*
+ * Enable in-air restart support.
+ * We can cheat and call the driver directly, as it
+ * doesn't reference filp in ioctl()
+ */
+ g_dev->ioctl(NULL, PX4IO_INAIR_RESTART_ENABLE, 1);
+ exit(0);
+ }
+
+ if (!strcmp(argv[1], "stop")) {
+
+ /* stop the driver */
+ delete g_dev;
+ exit(0);
+ }
+
if (!strcmp(argv[1], "status")) {
- if (g_dev != nullptr) {
- printf("[px4io] loaded\n");
- g_dev->print_status();
- } else {
- printf("[px4io] not loaded\n");
- }
+ printf("[px4io] loaded\n");
+ g_dev->print_status();
exit(0);
}
@@ -2005,58 +2348,6 @@ px4io_main(int argc, char *argv[])
exit(0);
}
- /* note, stop not currently implemented */
-
- if (!strcmp(argv[1], "update")) {
-
- if (g_dev != nullptr) {
- printf("[px4io] loaded, detaching first\n");
- /* stop the driver */
- delete g_dev;
- }
-
- PX4IO_Uploader *up;
- const char *fn[3];
-
- /* work out what we're uploading... */
- if (argc > 2) {
- fn[0] = argv[2];
- fn[1] = nullptr;
-
- } else {
- fn[0] = "/fs/microsd/px4io.bin";
- fn[1] = "/etc/px4io.bin";
- fn[2] = nullptr;
- }
-
- up = new PX4IO_Uploader;
- int ret = up->upload(&fn[0]);
- delete up;
-
- switch (ret) {
- case OK:
- break;
-
- case -ENOENT:
- errx(1, "PX4IO firmware file not found");
-
- case -EEXIST:
- case -EIO:
- errx(1, "error updating PX4IO - check that bootloader mode is enabled");
-
- case -EINVAL:
- errx(1, "verify failed - retry the update");
-
- case -ETIMEDOUT:
- errx(1, "timed out waiting for bootloader - power-cycle and try again");
-
- default:
- errx(1, "unexpected error %d", ret);
- }
-
- return ret;
- }
-
if (!strcmp(argv[1], "rx_dsm") ||
!strcmp(argv[1], "rx_dsm_10bit") ||
!strcmp(argv[1], "rx_dsm_11bit") ||
@@ -2074,5 +2365,5 @@ px4io_main(int argc, char *argv[])
bind(argc, argv);
out:
- errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe', 'bind', or 'update'");
+ errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'failsafe', 'min, 'max',\n 'idle', 'bind' or 'update'");
}
diff --git a/src/drivers/px4io/px4io_i2c.cpp b/src/drivers/px4io/px4io_i2c.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..19776c40a2a530c5095ea3d9ba7f1197d4b5db6d
--- /dev/null
+++ b/src/drivers/px4io/px4io_i2c.cpp
@@ -0,0 +1,169 @@
+/****************************************************************************
+ *
+ * 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 px4io_i2c.cpp
+ *
+ * I2C interface for PX4IO
+ */
+
+/* XXX trim includes */
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <assert.h>
+#include <debug.h>
+#include <errno.h>
+#include <unistd.h>
+
+#include <arch/board/board.h>
+#include <board_config.h>
+
+#include <drivers/device/i2c.h>
+
+#ifdef PX4_I2C_OBDEV_PX4IO
+
+device::Device *PX4IO_i2c_interface();
+
+class PX4IO_I2C : public device::I2C
+{
+public:
+ PX4IO_I2C(int bus, uint8_t address);
+ virtual ~PX4IO_I2C();
+
+ virtual int init();
+ virtual int read(unsigned offset, void *data, unsigned count = 1);
+ virtual int write(unsigned address, void *data, unsigned count = 1);
+ virtual int ioctl(unsigned operation, unsigned &arg);
+
+private:
+
+};
+
+device::Device
+*PX4IO_i2c_interface()
+{
+ return new PX4IO_I2C(PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO);
+}
+
+PX4IO_I2C::PX4IO_I2C(int bus, uint8_t address) :
+ I2C("PX4IO_i2c", nullptr, bus, address, 320000)
+{
+ _retries = 3;
+}
+
+PX4IO_I2C::~PX4IO_I2C()
+{
+}
+
+int
+PX4IO_I2C::init()
+{
+ int ret;
+
+ ret = I2C::init();
+ if (ret != OK)
+ goto out;
+
+ /* XXX really should do something more here */
+
+out:
+ return 0;
+}
+
+int
+PX4IO_I2C::ioctl(unsigned operation, unsigned &arg)
+{
+ return 0;
+}
+
+int
+PX4IO_I2C::write(unsigned address, void *data, unsigned count)
+{
+ uint8_t page = address >> 8;
+ uint8_t offset = address & 0xff;
+ const uint16_t *values = reinterpret_cast<const uint16_t *>(data);
+
+ /* set up the transfer */
+ uint8_t addr[2] = {
+ page,
+ offset
+ };
+
+ i2c_msg_s msgv[2];
+
+ msgv[0].flags = 0;
+ msgv[0].buffer = addr;
+ msgv[0].length = 2;
+
+ msgv[1].flags = I2C_M_NORESTART;
+ msgv[1].buffer = (uint8_t *)values;
+ msgv[1].length = 2 * count;
+
+ int ret = transfer(msgv, 2);
+ if (ret == OK)
+ ret = count;
+ return ret;
+}
+
+int
+PX4IO_I2C::read(unsigned address, void *data, unsigned count)
+{
+ uint8_t page = address >> 8;
+ uint8_t offset = address & 0xff;
+ const uint16_t *values = reinterpret_cast<const uint16_t *>(data);
+
+ /* set up the transfer */
+ uint8_t addr[2] = {
+ page,
+ offset
+ };
+ i2c_msg_s msgv[2];
+
+ msgv[0].flags = 0;
+ msgv[0].buffer = addr;
+ msgv[0].length = 2;
+
+ msgv[1].flags = I2C_M_READ;
+ msgv[1].buffer = (uint8_t *)values;
+ msgv[1].length = 2 * count;
+
+ int ret = transfer(msgv, 2);
+ if (ret == OK)
+ ret = count;
+ return ret;
+}
+
+#endif /* PX4_I2C_OBDEV_PX4IO */
diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..236cb918dcc0f9cf23c47f0cfa6a7a91c58fb7a2
--- /dev/null
+++ b/src/drivers/px4io/px4io_serial.cpp
@@ -0,0 +1,673 @@
+/****************************************************************************
+ *
+ * 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 px4io_serial.cpp
+ *
+ * Serial interface for PX4IO
+ */
+
+/* XXX trim includes */
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <assert.h>
+#include <debug.h>
+#include <time.h>
+#include <errno.h>
+#include <string.h>
+
+#include <arch/board/board.h>
+
+/* XXX might be able to prune these */
+#include <nuttx/arch.h>
+#include <chip.h>
+#include <up_internal.h>
+#include <up_arch.h>
+
+#include <debug.h>
+
+#include <drivers/device/device.h>
+#include <drivers/drv_hrt.h>
+#include <board_config.h>
+
+#include <systemlib/perf_counter.h>
+
+#include <modules/px4iofirmware/protocol.h>
+
+#ifdef PX4IO_SERIAL_BASE
+
+device::Device *PX4IO_serial_interface();
+
+/* serial register accessors */
+#define REG(_x) (*(volatile uint32_t *)(PX4IO_SERIAL_BASE + _x))
+#define rSR REG(STM32_USART_SR_OFFSET)
+#define rDR REG(STM32_USART_DR_OFFSET)
+#define rBRR REG(STM32_USART_BRR_OFFSET)
+#define rCR1 REG(STM32_USART_CR1_OFFSET)
+#define rCR2 REG(STM32_USART_CR2_OFFSET)
+#define rCR3 REG(STM32_USART_CR3_OFFSET)
+#define rGTPR REG(STM32_USART_GTPR_OFFSET)
+
+class PX4IO_serial : public device::Device
+{
+public:
+ PX4IO_serial();
+ virtual ~PX4IO_serial();
+
+ virtual int init();
+ virtual int read(unsigned offset, void *data, unsigned count = 1);
+ virtual int write(unsigned address, void *data, unsigned count = 1);
+ virtual int ioctl(unsigned operation, unsigned &arg);
+
+private:
+ /*
+ * XXX tune this value
+ *
+ * At 1.5Mbps each register takes 13.3µs, and we always transfer a full packet.
+ * Packet overhead is 26µs for the four-byte header.
+ *
+ * 32 registers = 451µs
+ *
+ * Maybe we can just send smaller packets (e.g. 8 regs) and loop for larger (less common)
+ * transfers? Could cause issues with any regs expecting to be written atomically...
+ */
+ static IOPacket _dma_buffer; // XXX static to ensure DMA-able memory
+
+ DMA_HANDLE _tx_dma;
+ DMA_HANDLE _rx_dma;
+
+ /** saved DMA status */
+ static const unsigned _dma_status_inactive = 0x80000000; // low bits overlap DMA_STATUS_* values
+ static const unsigned _dma_status_waiting = 0x00000000;
+ volatile unsigned _rx_dma_status;
+
+ /** bus-ownership lock */
+ sem_t _bus_semaphore;
+
+ /** client-waiting lock/signal */
+ sem_t _completion_semaphore;
+
+ /**
+ * Start the transaction with IO and wait for it to complete.
+ */
+ int _wait_complete();
+
+ /**
+ * DMA completion handler.
+ */
+ static void _dma_callback(DMA_HANDLE handle, uint8_t status, void *arg);
+ void _do_rx_dma_callback(unsigned status);
+
+ /**
+ * Serial interrupt handler.
+ */
+ static int _interrupt(int vector, void *context);
+ void _do_interrupt();
+
+ /**
+ * Cancel any DMA in progress with an error.
+ */
+ void _abort_dma();
+
+ /**
+ * Performance counters.
+ */
+ perf_counter_t _pc_txns;
+ perf_counter_t _pc_dmasetup;
+ perf_counter_t _pc_retries;
+ perf_counter_t _pc_timeouts;
+ perf_counter_t _pc_crcerrs;
+ perf_counter_t _pc_dmaerrs;
+ perf_counter_t _pc_protoerrs;
+ perf_counter_t _pc_uerrs;
+ perf_counter_t _pc_idle;
+ perf_counter_t _pc_badidle;
+
+};
+
+IOPacket PX4IO_serial::_dma_buffer;
+static PX4IO_serial *g_interface;
+
+device::Device
+*PX4IO_serial_interface()
+{
+ return new PX4IO_serial();
+}
+
+PX4IO_serial::PX4IO_serial() :
+ Device("PX4IO_serial"),
+ _tx_dma(nullptr),
+ _rx_dma(nullptr),
+ _rx_dma_status(_dma_status_inactive),
+ _pc_txns(perf_alloc(PC_ELAPSED, "io_txns ")),
+ _pc_dmasetup(perf_alloc(PC_ELAPSED, "io_dmasetup ")),
+ _pc_retries(perf_alloc(PC_COUNT, "io_retries ")),
+ _pc_timeouts(perf_alloc(PC_COUNT, "io_timeouts ")),
+ _pc_crcerrs(perf_alloc(PC_COUNT, "io_crcerrs ")),
+ _pc_dmaerrs(perf_alloc(PC_COUNT, "io_dmaerrs ")),
+ _pc_protoerrs(perf_alloc(PC_COUNT, "io_protoerrs")),
+ _pc_uerrs(perf_alloc(PC_COUNT, "io_uarterrs ")),
+ _pc_idle(perf_alloc(PC_COUNT, "io_idle ")),
+ _pc_badidle(perf_alloc(PC_COUNT, "io_badidle "))
+{
+ g_interface = this;
+}
+
+PX4IO_serial::~PX4IO_serial()
+{
+ if (_tx_dma != nullptr) {
+ stm32_dmastop(_tx_dma);
+ stm32_dmafree(_tx_dma);
+ }
+ if (_rx_dma != nullptr) {
+ stm32_dmastop(_rx_dma);
+ stm32_dmafree(_rx_dma);
+ }
+
+ /* reset the UART */
+ rCR1 = 0;
+ rCR2 = 0;
+ rCR3 = 0;
+
+ /* detach our interrupt handler */
+ up_disable_irq(PX4IO_SERIAL_VECTOR);
+ irq_detach(PX4IO_SERIAL_VECTOR);
+
+ /* restore the GPIOs */
+ stm32_unconfiggpio(PX4IO_SERIAL_TX_GPIO);
+ stm32_unconfiggpio(PX4IO_SERIAL_RX_GPIO);
+
+ /* and kill our semaphores */
+ sem_destroy(&_completion_semaphore);
+ sem_destroy(&_bus_semaphore);
+
+ perf_free(_pc_txns);
+ perf_free(_pc_dmasetup);
+ perf_free(_pc_retries);
+ perf_free(_pc_timeouts);
+ perf_free(_pc_crcerrs);
+ perf_free(_pc_dmaerrs);
+ perf_free(_pc_protoerrs);
+ perf_free(_pc_uerrs);
+ perf_free(_pc_idle);
+ perf_free(_pc_badidle);
+
+ if (g_interface == this)
+ g_interface = nullptr;
+}
+
+int
+PX4IO_serial::init()
+{
+ /* allocate DMA */
+ _tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP);
+ _rx_dma = stm32_dmachannel(PX4IO_SERIAL_RX_DMAMAP);
+ if ((_tx_dma == nullptr) || (_rx_dma == nullptr))
+ return -1;
+
+ /* configure pins for serial use */
+ stm32_configgpio(PX4IO_SERIAL_TX_GPIO);
+ stm32_configgpio(PX4IO_SERIAL_RX_GPIO);
+
+ /* reset & configure the UART */
+ rCR1 = 0;
+ rCR2 = 0;
+ rCR3 = 0;
+
+ /* eat any existing interrupt status */
+ (void)rSR;
+ (void)rDR;
+
+ /* configure line speed */
+ uint32_t usartdiv32 = PX4IO_SERIAL_CLOCK / (PX4IO_SERIAL_BITRATE / 2);
+ uint32_t mantissa = usartdiv32 >> 5;
+ uint32_t fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1;
+ rBRR = (mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT);
+
+ /* attach serial interrupt handler */
+ irq_attach(PX4IO_SERIAL_VECTOR, _interrupt);
+ up_enable_irq(PX4IO_SERIAL_VECTOR);
+
+ /* enable UART in DMA mode, enable error and line idle interrupts */
+ /* rCR3 = USART_CR3_EIE;*/
+ rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE;
+
+ /* create semaphores */
+ sem_init(&_completion_semaphore, 0, 0);
+ sem_init(&_bus_semaphore, 0, 1);
+
+
+ /* XXX this could try talking to IO */
+
+ return 0;
+}
+
+int
+PX4IO_serial::ioctl(unsigned operation, unsigned &arg)
+{
+
+ switch (operation) {
+
+ case 1: /* XXX magic number - test operation */
+ switch (arg) {
+ case 0:
+ lowsyslog("test 0\n");
+
+ /* kill DMA, this is a PIO test */
+ stm32_dmastop(_tx_dma);
+ stm32_dmastop(_rx_dma);
+ rCR3 &= ~(USART_CR3_DMAR | USART_CR3_DMAT);
+
+ for (;;) {
+ while (!(rSR & USART_SR_TXE))
+ ;
+ rDR = 0x55;
+ }
+ return 0;
+
+ case 1:
+ {
+ unsigned fails = 0;
+ for (unsigned count = 0;; count++) {
+ uint16_t value = count & 0xffff;
+
+ if (write((PX4IO_PAGE_TEST << 8) | PX4IO_P_TEST_LED, &value, 1) != 0)
+ fails++;
+
+ if (count >= 5000) {
+ lowsyslog("==== test 1 : %u failures ====\n", fails);
+ perf_print_counter(_pc_txns);
+ perf_print_counter(_pc_dmasetup);
+ perf_print_counter(_pc_retries);
+ perf_print_counter(_pc_timeouts);
+ perf_print_counter(_pc_crcerrs);
+ perf_print_counter(_pc_dmaerrs);
+ perf_print_counter(_pc_protoerrs);
+ perf_print_counter(_pc_uerrs);
+ perf_print_counter(_pc_idle);
+ perf_print_counter(_pc_badidle);
+ count = 0;
+ }
+ }
+ return 0;
+ }
+ case 2:
+ lowsyslog("test 2\n");
+ return 0;
+ }
+ default:
+ break;
+ }
+
+ return -1;
+}
+
+int
+PX4IO_serial::write(unsigned address, void *data, unsigned count)
+{
+ uint8_t page = address >> 8;
+ uint8_t offset = address & 0xff;
+ const uint16_t *values = reinterpret_cast<const uint16_t *>(data);
+
+ if (count > PKT_MAX_REGS)
+ return -EINVAL;
+
+ sem_wait(&_bus_semaphore);
+
+ int result;
+ for (unsigned retries = 0; retries < 3; retries++) {
+
+ _dma_buffer.count_code = count | PKT_CODE_WRITE;
+ _dma_buffer.page = page;
+ _dma_buffer.offset = offset;
+ memcpy((void *)&_dma_buffer.regs[0], (void *)values, (2 * count));
+ for (unsigned i = count; i < PKT_MAX_REGS; i++)
+ _dma_buffer.regs[i] = 0x55aa;
+
+ /* XXX implement check byte */
+
+ /* start the transaction and wait for it to complete */
+ result = _wait_complete();
+
+ /* successful transaction? */
+ if (result == OK) {
+
+ /* check result in packet */
+ if (PKT_CODE(_dma_buffer) == PKT_CODE_ERROR) {
+
+ /* IO didn't like it - no point retrying */
+ result = -EINVAL;
+ perf_count(_pc_protoerrs);
+ }
+
+ break;
+ }
+ perf_count(_pc_retries);
+ }
+
+ sem_post(&_bus_semaphore);
+
+ if (result == OK)
+ result = count;
+ return result;
+}
+
+int
+PX4IO_serial::read(unsigned address, void *data, unsigned count)
+{
+ uint8_t page = address >> 8;
+ uint8_t offset = address & 0xff;
+ uint16_t *values = reinterpret_cast<uint16_t *>(data);
+
+ if (count > PKT_MAX_REGS)
+ return -EINVAL;
+
+ sem_wait(&_bus_semaphore);
+
+ int result;
+ for (unsigned retries = 0; retries < 3; retries++) {
+
+ _dma_buffer.count_code = count | PKT_CODE_READ;
+ _dma_buffer.page = page;
+ _dma_buffer.offset = offset;
+
+ /* start the transaction and wait for it to complete */
+ result = _wait_complete();
+
+ /* successful transaction? */
+ if (result == OK) {
+
+ /* check result in packet */
+ if (PKT_CODE(_dma_buffer) == PKT_CODE_ERROR) {
+
+ /* IO didn't like it - no point retrying */
+ result = -EINVAL;
+ perf_count(_pc_protoerrs);
+
+ /* compare the received count with the expected count */
+ } else if (PKT_COUNT(_dma_buffer) != count) {
+
+ /* IO returned the wrong number of registers - no point retrying */
+ result = -EIO;
+ perf_count(_pc_protoerrs);
+
+ /* successful read */
+ } else {
+
+ /* copy back the result */
+ memcpy(values, &_dma_buffer.regs[0], (2 * count));
+ }
+
+ break;
+ }
+ perf_count(_pc_retries);
+ }
+
+ sem_post(&_bus_semaphore);
+
+ if (result == OK)
+ result = count;
+ return result;
+}
+
+int
+PX4IO_serial::_wait_complete()
+{
+ /* clear any lingering error status */
+ (void)rSR;
+ (void)rDR;
+
+ /* start RX DMA */
+ perf_begin(_pc_txns);
+ perf_begin(_pc_dmasetup);
+
+ /* DMA setup time ~3µs */
+ _rx_dma_status = _dma_status_waiting;
+
+ /*
+ * Note that we enable circular buffer mode as a workaround for
+ * there being no API to disable the DMA FIFO. We need direct mode
+ * because otherwise when the line idle interrupt fires there
+ * will be packet bytes still in the DMA FIFO, and we will assume
+ * that the idle was spurious.
+ *
+ * XXX this should be fixed with a NuttX change.
+ */
+ stm32_dmasetup(
+ _rx_dma,
+ PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET,
+ reinterpret_cast<uint32_t>(&_dma_buffer),
+ sizeof(_dma_buffer),
+ DMA_SCR_CIRC | /* XXX see note above */
+ DMA_SCR_DIR_P2M |
+ DMA_SCR_MINC |
+ DMA_SCR_PSIZE_8BITS |
+ DMA_SCR_MSIZE_8BITS |
+ DMA_SCR_PBURST_SINGLE |
+ DMA_SCR_MBURST_SINGLE);
+ stm32_dmastart(_rx_dma, _dma_callback, this, false);
+ rCR3 |= USART_CR3_DMAR;
+
+ /* start TX DMA - no callback if we also expect a reply */
+ /* DMA setup time ~3µs */
+ _dma_buffer.crc = 0;
+ _dma_buffer.crc = crc_packet(&_dma_buffer);
+ stm32_dmasetup(
+ _tx_dma,
+ PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET,
+ reinterpret_cast<uint32_t>(&_dma_buffer),
+ PKT_SIZE(_dma_buffer),
+ DMA_SCR_DIR_M2P |
+ DMA_SCR_MINC |
+ DMA_SCR_PSIZE_8BITS |
+ DMA_SCR_MSIZE_8BITS |
+ DMA_SCR_PBURST_SINGLE |
+ DMA_SCR_MBURST_SINGLE);
+ stm32_dmastart(_tx_dma, nullptr, nullptr, false);
+ rCR3 |= USART_CR3_DMAT;
+
+ perf_end(_pc_dmasetup);
+
+ /* compute the deadline for a 5ms timeout */
+ struct timespec abstime;
+ clock_gettime(CLOCK_REALTIME, &abstime);
+#if 0
+ abstime.tv_sec++; /* long timeout for testing */
+#else
+ abstime.tv_nsec += 10000000; /* 0ms timeout */
+ if (abstime.tv_nsec > 1000000000) {
+ abstime.tv_sec++;
+ abstime.tv_nsec -= 1000000000;
+ }
+#endif
+
+ /* wait for the transaction to complete - 64 bytes @ 1.5Mbps ~426µs */
+ int ret;
+ for (;;) {
+ ret = sem_timedwait(&_completion_semaphore, &abstime);
+
+ if (ret == OK) {
+ /* check for DMA errors */
+ if (_rx_dma_status & DMA_STATUS_TEIF) {
+ perf_count(_pc_dmaerrs);
+ ret = -EIO;
+ break;
+ }
+
+ /* check packet CRC - corrupt packet errors mean IO receive CRC error */
+ uint8_t crc = _dma_buffer.crc;
+ _dma_buffer.crc = 0;
+ if ((crc != crc_packet(&_dma_buffer)) | (PKT_CODE(_dma_buffer) == PKT_CODE_CORRUPT)) {
+ perf_count(_pc_crcerrs);
+ ret = -EIO;
+ break;
+ }
+
+ /* successful txn (may still be reporting an error) */
+ break;
+ }
+
+ if (errno == ETIMEDOUT) {
+ /* something has broken - clear out any partial DMA state and reconfigure */
+ _abort_dma();
+ perf_count(_pc_timeouts);
+ perf_cancel(_pc_txns); /* don't count this as a transaction */
+ break;
+ }
+
+ /* we might? see this for EINTR */
+ lowsyslog("unexpected ret %d/%d\n", ret, errno);
+ }
+
+ /* reset DMA status */
+ _rx_dma_status = _dma_status_inactive;
+
+ /* update counters */
+ perf_end(_pc_txns);
+
+ return ret;
+}
+
+void
+PX4IO_serial::_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg)
+{
+ if (arg != nullptr) {
+ PX4IO_serial *ps = reinterpret_cast<PX4IO_serial *>(arg);
+
+ ps->_do_rx_dma_callback(status);
+ }
+}
+
+void
+PX4IO_serial::_do_rx_dma_callback(unsigned status)
+{
+ /* on completion of a reply, wake the waiter */
+ if (_rx_dma_status == _dma_status_waiting) {
+
+ /* check for packet overrun - this will occur after DMA completes */
+ uint32_t sr = rSR;
+ if (sr & (USART_SR_ORE | USART_SR_RXNE)) {
+ (void)rDR;
+ status = DMA_STATUS_TEIF;
+ }
+
+ /* save RX status */
+ _rx_dma_status = status;
+
+ /* disable UART DMA */
+ rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
+
+ /* complete now */
+ sem_post(&_completion_semaphore);
+ }
+}
+
+int
+PX4IO_serial::_interrupt(int irq, void *context)
+{
+ if (g_interface != nullptr)
+ g_interface->_do_interrupt();
+ return 0;
+}
+
+void
+PX4IO_serial::_do_interrupt()
+{
+ uint32_t sr = rSR; /* get UART status register */
+ (void)rDR; /* read DR to clear status */
+
+ if (sr & (USART_SR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */
+ USART_SR_NE | /* noise error - we have lost a byte due to noise */
+ USART_SR_FE)) { /* framing error - start/stop bit lost or line break */
+
+ /*
+ * If we are in the process of listening for something, these are all fatal;
+ * abort the DMA with an error.
+ */
+ if (_rx_dma_status == _dma_status_waiting) {
+ _abort_dma();
+ perf_count(_pc_uerrs);
+
+ /* complete DMA as though in error */
+ _do_rx_dma_callback(DMA_STATUS_TEIF);
+
+ return;
+ }
+
+ /* XXX we might want to use FE / line break as an out-of-band handshake ... handle it here */
+
+ /* don't attempt to handle IDLE if it's set - things went bad */
+ return;
+ }
+
+ if (sr & USART_SR_IDLE) {
+
+ /* if there is DMA reception going on, this is a short packet */
+ if (_rx_dma_status == _dma_status_waiting) {
+
+ /* verify that the received packet is complete */
+ unsigned length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma);
+ if ((length < 1) || (length < PKT_SIZE(_dma_buffer))) {
+ perf_count(_pc_badidle);
+ return;
+ }
+
+ perf_count(_pc_idle);
+
+ /* stop the receive DMA */
+ stm32_dmastop(_rx_dma);
+
+ /* complete the short reception */
+ _do_rx_dma_callback(DMA_STATUS_TCIF);
+ }
+ }
+}
+
+void
+PX4IO_serial::_abort_dma()
+{
+ /* disable UART DMA */
+ rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
+ (void)rSR;
+ (void)rDR;
+ (void)rDR;
+
+ /* stop DMA */
+ stm32_dmastop(_tx_dma);
+ stm32_dmastop(_rx_dma);
+}
+
+#endif /* PX4IO_SERIAL_BASE */
\ No newline at end of file
diff --git a/src/drivers/px4io/uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp
similarity index 74%
rename from src/drivers/px4io/uploader.cpp
rename to src/drivers/px4io/px4io_uploader.cpp
index 9e3f041e3f983c7fbb022d7fe52ae14f88daa736..c7ce60b5ee047f7a88326f2fa931cfb49863765b 100644
--- a/src/drivers/px4io/uploader.cpp
+++ b/src/drivers/px4io/px4io_uploader.cpp
@@ -52,51 +52,14 @@
#include <termios.h>
#include <sys/stat.h>
+#include <crc32.h>
+
#include "uploader.h"
-static const uint32_t crctab[] =
-{
- 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3,
- 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91,
- 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
- 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5,
- 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b,
- 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
- 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f,
- 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d,
- 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
- 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01,
- 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457,
- 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
- 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb,
- 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9,
- 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
- 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad,
- 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683,
- 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
- 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7,
- 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5,
- 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
- 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79,
- 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f,
- 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
- 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713,
- 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21,
- 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
- 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45,
- 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db,
- 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
- 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf,
- 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d
-};
-
-static uint32_t
-crc32(const uint8_t *src, unsigned len, unsigned state)
-{
- for (unsigned i = 0; i < len; i++)
- state = crctab[(state ^ src[i]) & 0xff] ^ (state >> 8);
- return state;
-}
+#include <board_config.h>
+
+// define for comms logging
+//#define UDEBUG
PX4IO_Uploader::PX4IO_Uploader() :
_io_fd(-1),
@@ -115,7 +78,11 @@ PX4IO_Uploader::upload(const char *filenames[])
const char *filename = NULL;
size_t fw_size;
- _io_fd = open("/dev/ttyS2", O_RDWR);
+#ifndef PX4IO_SERIAL_DEVICE
+#error Must define PX4IO_SERIAL_DEVICE in board configuration to support firmware upload
+#endif
+
+ _io_fd = open(PX4IO_SERIAL_DEVICE, O_RDWR);
if (_io_fd < 0) {
log("could not open interface");
@@ -258,12 +225,16 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout)
int ret = ::poll(&fds[0], 1, timeout);
if (ret < 1) {
- //log("poll timeout %d", ret);
+#ifdef UDEBUG
+ log("poll timeout %d", ret);
+#endif
return -ETIMEDOUT;
}
read(_io_fd, &c, 1);
- //log("recv 0x%02x", c);
+#ifdef UDEBUG
+ log("recv 0x%02x", c);
+#endif
return OK;
}
@@ -289,16 +260,20 @@ PX4IO_Uploader::drain()
do {
ret = recv(c, 1000);
+#ifdef UDEBUG
if (ret == OK) {
- //log("discard 0x%02x", c);
+ log("discard 0x%02x", c);
}
+#endif
} while (ret == OK);
}
int
PX4IO_Uploader::send(uint8_t c)
{
- //log("send 0x%02x", c);
+#ifdef UDEBUG
+ log("send 0x%02x", c);
+#endif
if (write(_io_fd, &c, 1) != 1)
return -errno;
@@ -572,14 +547,14 @@ PX4IO_Uploader::verify_rev3(size_t fw_size_local)
return -errno;
/* calculate crc32 sum */
- sum = crc32((uint8_t *)&file_buf, sizeof(file_buf), sum);
+ sum = crc32part((uint8_t *)&file_buf, sizeof(file_buf), sum);
bytes_read += count;
}
/* fill the rest with 0xff */
while (bytes_read < fw_size_remote) {
- sum = crc32(&fill_blank, sizeof(fill_blank), sum);
+ sum = crc32part(&fill_blank, sizeof(fill_blank), sum);
bytes_read += sizeof(fill_blank);
}
diff --git a/src/drivers/rgbled/module.mk b/src/drivers/rgbled/module.mk
new file mode 100644
index 0000000000000000000000000000000000000000..39b53ec9ed925e14dd04cf786483b588f88b6bef
--- /dev/null
+++ b/src/drivers/rgbled/module.mk
@@ -0,0 +1,6 @@
+#
+# TCA62724FMG driver for RGB LED
+#
+
+MODULE_COMMAND = rgbled
+SRCS = rgbled.cpp
diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..05f079ede2f221e2d7d47b5ec8bc55a629ea1e75
--- /dev/null
+++ b/src/drivers/rgbled/rgbled.cpp
@@ -0,0 +1,566 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 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 rgbled.cpp
+ *
+ * Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C.
+ *
+ *
+ */
+
+#include <nuttx/config.h>
+
+#include <drivers/device/i2c.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <ctype.h>
+
+#include <nuttx/wqueue.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
+#include <board_config.h>
+
+#include <drivers/drv_rgbled.h>
+
+#define RGBLED_ONTIME 120
+#define RGBLED_OFFTIME 120
+
+#define ADDR PX4_I2C_OBDEV_LED /**< I2C adress of TCA62724FMG */
+#define SUB_ADDR_START 0x01 /**< write everything (with auto-increment) */
+#define SUB_ADDR_PWM0 0x81 /**< blue (without auto-increment) */
+#define SUB_ADDR_PWM1 0x82 /**< green (without auto-increment) */
+#define SUB_ADDR_PWM2 0x83 /**< red (without auto-increment) */
+#define SUB_ADDR_SETTINGS 0x84 /**< settings (without auto-increment)*/
+
+#define SETTING_NOT_POWERSAVE 0x01 /**< power-save mode not off */
+#define SETTING_ENABLE 0x02 /**< on */
+
+
+class RGBLED : public device::I2C
+{
+public:
+ RGBLED(int bus, int rgbled);
+ virtual ~RGBLED();
+
+
+ virtual int init();
+ virtual int probe();
+ virtual int info();
+ virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
+
+private:
+ work_s _work;
+
+ rgbled_color_t _color;
+ rgbled_mode_t _mode;
+ rgbled_pattern_t _pattern;
+
+ bool _should_run;
+ bool _running;
+ int _led_interval;
+ int _counter;
+
+ void set_color(rgbled_color_t ledcolor);
+ void set_mode(rgbled_mode_t mode);
+ void set_pattern(rgbled_pattern_t *pattern);
+
+ static void led_trampoline(void *arg);
+ void led();
+
+ int set(bool on, uint8_t r, uint8_t g, uint8_t b);
+ int set_on(bool on);
+ int set_rgb(uint8_t r, uint8_t g, uint8_t b);
+ int get(bool &on, bool ¬_powersave, uint8_t &r, uint8_t &g, uint8_t &b);
+};
+
+/* for now, we only support one RGBLED */
+namespace
+{
+ RGBLED *g_rgbled;
+}
+
+
+extern "C" __EXPORT int rgbled_main(int argc, char *argv[]);
+
+RGBLED::RGBLED(int bus, int rgbled) :
+ I2C("rgbled", RGBLED_DEVICE_PATH, bus, rgbled, 100000),
+ _color(RGBLED_COLOR_OFF),
+ _mode(RGBLED_MODE_OFF),
+ _running(false),
+ _led_interval(0),
+ _counter(0)
+{
+ memset(&_work, 0, sizeof(_work));
+ memset(&_pattern, 0, sizeof(_pattern));
+}
+
+RGBLED::~RGBLED()
+{
+}
+
+int
+RGBLED::init()
+{
+ int ret;
+ ret = I2C::init();
+
+ if (ret != OK) {
+ return ret;
+ }
+
+ /* start off */
+ set(false, 0, 0, 0);
+
+ return OK;
+}
+
+int
+RGBLED::probe()
+{
+ int ret;
+ bool on, not_powersave;
+ uint8_t r, g, b;
+
+ ret = get(on, not_powersave, r, g, b);
+
+ return ret;
+}
+
+int
+RGBLED::info()
+{
+ int ret;
+ bool on, not_powersave;
+ uint8_t r, g, b;
+
+ ret = get(on, not_powersave, r, g, b);
+
+ if (ret == OK) {
+ /* we don't care about power-save mode */
+ log("state: %s", on ? "ON" : "OFF");
+ log("red: %u, green: %u, blue: %u", (unsigned)r, (unsigned)g, (unsigned)b);
+ } else {
+ warnx("failed to read led");
+ }
+
+ return ret;
+}
+
+int
+RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+ int ret = ENOTTY;
+
+ switch (cmd) {
+ case RGBLED_SET_RGB:
+ /* set the specified RGB values */
+ rgbled_rgbset_t rgbset;
+ memcpy(&rgbset, (rgbled_rgbset_t*)arg, sizeof(rgbset));
+ set_rgb(rgbset.red, rgbset.green, rgbset.blue);
+ set_mode(RGBLED_MODE_ON);
+ return OK;
+
+ case RGBLED_SET_COLOR:
+ /* set the specified color name */
+ set_color((rgbled_color_t)arg);
+ return OK;
+
+ case RGBLED_SET_MODE:
+ /* set the specified blink speed */
+ set_mode((rgbled_mode_t)arg);
+ return OK;
+
+ case RGBLED_SET_PATTERN:
+ /* set a special pattern */
+ set_pattern((rgbled_pattern_t*)arg);
+ return OK;
+
+ default:
+ break;
+ }
+
+ return ret;
+}
+
+
+void
+RGBLED::led_trampoline(void *arg)
+{
+ RGBLED *rgbl = reinterpret_cast<RGBLED *>(arg);
+
+ rgbl->led();
+}
+
+
+
+void
+RGBLED::led()
+{
+ switch (_mode) {
+ case RGBLED_MODE_BLINK_SLOW:
+ case RGBLED_MODE_BLINK_NORMAL:
+ case RGBLED_MODE_BLINK_FAST:
+ if(_counter % 2 == 0)
+ set_on(true);
+ else
+ set_on(false);
+ break;
+ case RGBLED_MODE_PATTERN:
+ /* don't run out of the pattern array and stop if the next frame is 0 */
+ if (_counter >= RGBLED_PATTERN_LENGTH || _pattern.duration[_counter] <= 0)
+ _counter = 0;
+
+ set_color(_pattern.color[_counter]);
+ _led_interval = _pattern.duration[_counter];
+ break;
+ default:
+ break;
+ }
+
+ _counter++;
+
+ /* re-queue ourselves to run again later */
+ work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, _led_interval);
+}
+
+void
+RGBLED::set_color(rgbled_color_t color) {
+
+ _color = color;
+
+ switch (color) {
+ case RGBLED_COLOR_OFF: // off
+ set_rgb(0,0,0);
+ break;
+ case RGBLED_COLOR_RED: // red
+ set_rgb(255,0,0);
+ break;
+ case RGBLED_COLOR_YELLOW: // yellow
+ set_rgb(255,70,0);
+ break;
+ case RGBLED_COLOR_PURPLE: // purple
+ set_rgb(255,0,255);
+ break;
+ case RGBLED_COLOR_GREEN: // green
+ set_rgb(0,255,0);
+ break;
+ case RGBLED_COLOR_BLUE: // blue
+ set_rgb(0,0,255);
+ break;
+ case RGBLED_COLOR_WHITE: // white
+ set_rgb(255,255,255);
+ break;
+ case RGBLED_COLOR_AMBER: // amber
+ set_rgb(255,20,0);
+ break;
+ case RGBLED_COLOR_DIM_RED: // red
+ set_rgb(90,0,0);
+ break;
+ case RGBLED_COLOR_DIM_YELLOW: // yellow
+ set_rgb(80,30,0);
+ break;
+ case RGBLED_COLOR_DIM_PURPLE: // purple
+ set_rgb(45,0,45);
+ break;
+ case RGBLED_COLOR_DIM_GREEN: // green
+ set_rgb(0,90,0);
+ break;
+ case RGBLED_COLOR_DIM_BLUE: // blue
+ set_rgb(0,0,90);
+ break;
+ case RGBLED_COLOR_DIM_WHITE: // white
+ set_rgb(30,30,30);
+ break;
+ case RGBLED_COLOR_DIM_AMBER: // amber
+ set_rgb(80,20,0);
+ break;
+ default:
+ warnx("color unknown");
+ break;
+ }
+}
+
+void
+RGBLED::set_mode(rgbled_mode_t mode)
+{
+ _mode = mode;
+
+ switch (mode) {
+ case RGBLED_MODE_OFF:
+ _should_run = false;
+ set_on(false);
+ break;
+ case RGBLED_MODE_ON:
+ _should_run = false;
+ set_on(true);
+ break;
+ case RGBLED_MODE_BLINK_SLOW:
+ _should_run = true;
+ _led_interval = 2000;
+ break;
+ case RGBLED_MODE_BLINK_NORMAL:
+ _should_run = true;
+ _led_interval = 500;
+ break;
+ case RGBLED_MODE_BLINK_FAST:
+ _should_run = true;
+ _led_interval = 100;
+ break;
+ case RGBLED_MODE_PATTERN:
+ _should_run = true;
+ set_on(true);
+ _counter = 0;
+ break;
+ default:
+ warnx("mode unknown");
+ break;
+ }
+
+ /* if it should run now, start the workq */
+ if (_should_run && !_running) {
+ _running = true;
+ work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1);
+ }
+ /* if it should stop, then cancel the workq */
+ if (!_should_run && _running) {
+ _running = false;
+ work_cancel(LPWORK, &_work);
+ }
+}
+
+void
+RGBLED::set_pattern(rgbled_pattern_t *pattern)
+{
+ memcpy(&_pattern, pattern, sizeof(rgbled_pattern_t));
+
+ set_mode(RGBLED_MODE_PATTERN);
+}
+
+int
+RGBLED::set(bool on, uint8_t r, uint8_t g, uint8_t b)
+{
+ uint8_t settings_byte = 0;
+
+ if (on)
+ settings_byte |= SETTING_ENABLE;
+/* powersave not used */
+// if (not_powersave)
+ settings_byte |= SETTING_NOT_POWERSAVE;
+
+ const uint8_t msg[5] = { SUB_ADDR_START, (uint8_t)(b*15/255), (uint8_t)(g*15/255), (uint8_t)(r*15/255), settings_byte};
+
+ return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+RGBLED::set_on(bool on)
+{
+ uint8_t settings_byte = 0;
+
+ if (on)
+ settings_byte |= SETTING_ENABLE;
+
+/* powersave not used */
+// if (not_powersave)
+ settings_byte |= SETTING_NOT_POWERSAVE;
+
+ const uint8_t msg[2] = { SUB_ADDR_SETTINGS, settings_byte};
+
+ return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+RGBLED::set_rgb(uint8_t r, uint8_t g, uint8_t b)
+{
+ const uint8_t msg[6] = { SUB_ADDR_PWM0, (uint8_t)(b*15/255), SUB_ADDR_PWM1, (uint8_t)(g*15/255), SUB_ADDR_PWM2, (uint8_t)(r*15/255)};
+
+ return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+
+int
+RGBLED::get(bool &on, bool ¬_powersave, uint8_t &r, uint8_t &g, uint8_t &b)
+{
+ uint8_t result[2];
+ int ret;
+
+ ret = transfer(nullptr, 0, &result[0], 2);
+
+ if (ret == OK) {
+ on = result[0] & SETTING_ENABLE;
+ not_powersave = result[0] & SETTING_NOT_POWERSAVE;
+ /* XXX check, looks wrong */
+ r = (result[0] & 0x0f)*255/15;
+ g = (result[1] & 0xf0)*255/15;
+ b = (result[1] & 0x0f)*255/15;
+ }
+
+ return ret;
+}
+
+void rgbled_usage();
+
+
+void rgbled_usage() {
+ warnx("missing command: try 'start', 'test', 'info', 'stop'/'off', 'rgb 30 40 50'");
+ warnx("options:");
+ warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED);
+ errx(0, " -a addr (0x%x)", ADDR);
+}
+
+int
+rgbled_main(int argc, char *argv[])
+{
+ int i2cdevice = -1;
+ int rgbledadr = ADDR; /* 7bit */
+
+ int ch;
+ /* jump over start/off/etc and look at options first */
+ while ((ch = getopt(argc-1, &argv[1], "a:b:")) != EOF) {
+ switch (ch) {
+ case 'a':
+ rgbledadr = strtol(optarg, NULL, 0);
+ break;
+ case 'b':
+ i2cdevice = strtol(optarg, NULL, 0);
+ break;
+ default:
+ rgbled_usage();
+ }
+ }
+
+ const char *verb = argv[1];
+
+ int fd;
+ int ret;
+
+ if (!strcmp(verb, "start")) {
+ if (g_rgbled != nullptr)
+ errx(1, "already started");
+
+ if (i2cdevice == -1) {
+ // try the external bus first
+ i2cdevice = PX4_I2C_BUS_EXPANSION;
+ g_rgbled = new RGBLED(PX4_I2C_BUS_EXPANSION, rgbledadr);
+ if (g_rgbled != nullptr && OK != g_rgbled->init()) {
+ delete g_rgbled;
+ g_rgbled = nullptr;
+ }
+ if (g_rgbled == nullptr) {
+ // fall back to default bus
+ i2cdevice = PX4_I2C_BUS_LED;
+ }
+ }
+ if (g_rgbled == nullptr) {
+ g_rgbled = new RGBLED(i2cdevice, rgbledadr);
+ if (g_rgbled == nullptr)
+ errx(1, "new failed");
+
+ if (OK != g_rgbled->init()) {
+ delete g_rgbled;
+ g_rgbled = nullptr;
+ errx(1, "init failed");
+ }
+ }
+
+ exit(0);
+ }
+
+ /* need the driver past this point */
+ if (g_rgbled == nullptr) {
+ warnx("not started");
+ rgbled_usage();
+ exit(0);
+ }
+
+ if (!strcmp(verb, "test")) {
+ fd = open(RGBLED_DEVICE_PATH, 0);
+ if (fd == -1) {
+ errx(1, "Unable to open " RGBLED_DEVICE_PATH);
+ }
+
+ rgbled_pattern_t pattern = { {RGBLED_COLOR_RED, RGBLED_COLOR_GREEN, RGBLED_COLOR_BLUE, RGBLED_COLOR_OFF},
+ {200, 200, 200, 400 } };
+
+ ret = ioctl(fd, RGBLED_SET_PATTERN, (unsigned long)&pattern);
+
+ close(fd);
+ exit(ret);
+ }
+
+ if (!strcmp(verb, "info")) {
+ g_rgbled->info();
+ exit(0);
+ }
+
+ if (!strcmp(verb, "stop") || !strcmp(verb, "off")) {
+ /* although technically it doesn't stop, this is the excepted syntax */
+ fd = open(RGBLED_DEVICE_PATH, 0);
+ if (fd == -1) {
+ errx(1, "Unable to open " RGBLED_DEVICE_PATH);
+ }
+ ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF);
+ close(fd);
+ exit(ret);
+ }
+
+ if (!strcmp(verb, "rgb")) {
+ fd = open(RGBLED_DEVICE_PATH, 0);
+ if (fd == -1) {
+ errx(1, "Unable to open " RGBLED_DEVICE_PATH);
+ }
+ if (argc < 5) {
+ errx(1, "Usage: rgbled rgb <red> <green> <blue>");
+ }
+ rgbled_rgbset_t v;
+ v.red = strtol(argv[1], NULL, 0);
+ v.green = strtol(argv[2], NULL, 0);
+ v.blue = strtol(argv[3], NULL, 0);
+ ret = ioctl(fd, RGBLED_SET_RGB, (unsigned long)&v);
+ close(fd);
+ exit(ret);
+ }
+
+ rgbled_usage();
+}
diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp
index 1020eb9465f4b77e59f76497f216b2b6759eda72..00e46d6b82ae9fcc37616d67cabfd98e364429a2 100644
--- a/src/drivers/stm32/adc/adc.cpp
+++ b/src/drivers/stm32/adc/adc.cpp
@@ -227,7 +227,6 @@ ADC::init()
if ((hrt_absolute_time() - now) > 500) {
log("sample timeout");
return -1;
- return 0xffff;
}
}
@@ -282,7 +281,7 @@ ADC::close_last(struct file *filp)
void
ADC::_tick_trampoline(void *arg)
{
- ((ADC *)arg)->_tick();
+ (reinterpret_cast<ADC *>(arg))->_tick();
}
void
@@ -342,7 +341,7 @@ test(void)
err(1, "can't open ADC device");
for (unsigned i = 0; i < 50; i++) {
- adc_msg_s data[8];
+ adc_msg_s data[10];
ssize_t count = read(fd, data, sizeof(data));
if (count < 0)
@@ -366,8 +365,15 @@ int
adc_main(int argc, char *argv[])
{
if (g_adc == nullptr) {
- /* XXX this hardcodes the default channel set for PX4FMU - should be configurable */
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+ /* XXX this hardcodes the default channel set for PX4FMUv1 - should be configurable */
g_adc = new ADC((1 << 10) | (1 << 11) | (1 << 12) | (1 << 13));
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+ /* XXX this hardcodes the default channel set for PX4FMUv2 - should be configurable */
+ g_adc = new ADC((1 << 2) | (1 << 3) | (1 << 4) |
+ (1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) | (1 << 15));
+#endif
if (g_adc == nullptr)
errx(1, "couldn't allocate the ADC driver");
diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c
index 83a1a1abbb8737dcc26ff29d2483986e0f5ecd5b..e79d7e10a31531d51d3252441fc6dab48ad4ac0d 100644
--- a/src/drivers/stm32/drv_hrt.c
+++ b/src/drivers/stm32/drv_hrt.c
@@ -59,7 +59,7 @@
#include <errno.h>
#include <string.h>
-#include <arch/board/board.h>
+#include <board_config.h>
#include <drivers/drv_hrt.h>
#include "chip.h"
diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
index 2284be84de0352602b8e0d80a7c88d76da191549..ad21f7143ffb729e7f2fdcfbc0b67a5f99f7fdfb 100644
--- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp
+++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * 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
@@ -104,7 +104,7 @@
#include <math.h>
#include <ctype.h>
-#include <arch/board/board.h>
+#include <board_config.h>
#include <drivers/drv_hrt.h>
#include <arch/stm32/chip.h>
@@ -237,6 +237,8 @@ private:
static const unsigned _default_ntunes;
static const uint8_t _note_tab[];
+ unsigned _default_tune_number; // number of currently playing default tune (0 for none)
+
const char *_user_tune;
const char *_tune; // current tune string
@@ -452,6 +454,9 @@ const char * const ToneAlarm::_default_tunes[] = {
"O1B8O2G+8E8B8G+8O3E8O2B8O3E8O2B8O3G+8E8B8"
"O3G+8O4E4P8E16E16E8E8E8E8E4P8E16E4P8O2E16"
"O2E2P64",
+ "MNT75L1O2G", //arming warning
+ "MBNT100a8", //battery warning slow
+ "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8" //battery warning fast // XXX why is there a break before a repetition
};
const unsigned ToneAlarm::_default_ntunes = sizeof(_default_tunes) / sizeof(_default_tunes[0]);
@@ -467,6 +472,7 @@ extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]);
ToneAlarm::ToneAlarm() :
CDev("tone_alarm", "/dev/tone_alarm"),
+ _default_tune_number(0),
_user_tune(nullptr),
_tune(nullptr),
_next(nullptr)
@@ -799,8 +805,12 @@ tune_error:
// stop (and potentially restart) the tune
tune_end:
stop_note();
- if (_repeat)
+ if (_repeat) {
start_tune(_tune);
+ } else {
+ _tune = nullptr;
+ _default_tune_number = 0;
+ }
return;
}
@@ -869,8 +879,12 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg)
_tune = nullptr;
_next = nullptr;
} else {
- // play the selected tune
- start_tune(_default_tunes[arg - 1]);
+ /* 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]);
+ }
}
} else {
result = -EINVAL;
diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c
index d13ffe4148532461b0eb7e3327fffadd9fd96c8f..b286e00077e147cb252b873046f03ba70ccfeda0 100644
--- a/src/examples/fixedwing_control/main.c
+++ b/src/examples/fixedwing_control/main.c
@@ -66,7 +66,7 @@
#include <uORB/topics/parameter_update.h>
#include <systemlib/param/param.h>
#include <systemlib/pid/pid.h>
-#include <systemlib/geo/geo.h>
+#include <geo/geo.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
@@ -380,9 +380,10 @@ int fixedwing_control_thread_main(int argc, char *argv[])
/* control */
- /* if in auto mode, fly global position setpoint */
- if (vstatus.state_machine == SYSTEM_STATE_AUTO ||
- vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
+#warning fix this
+#if 0
+ if (vstatus.navigation_state == NAVIGATION_STATE_AUTO_ ||
+ vstatus.navigation_state == NAVIGATION_STATE_STABILIZED) {
/* simple heading control */
control_heading(&global_pos, &global_sp, &att, &att_sp);
@@ -400,6 +401,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
/* set flaps to zero */
actuators.control[4] = 0.0f;
+ } else if (vstatus.navigation_state == NAVIGATION_STATE_MANUAL) {
/* if in manual mode, decide between attitude stabilization (SAS) and full manual pass-through */
} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
@@ -465,6 +467,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
}
}
}
+#endif
/* publish rates */
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
diff --git a/src/examples/flow_position_control/flow_position_control_main.c b/src/examples/flow_position_control/flow_position_control_main.c
index c96f73155fa80e6f71abf07b508bf22e67a5c0c4..621d3253f074cfe04f9b289d619039d48fb8eb6b 100644
--- a/src/examples/flow_position_control/flow_position_control_main.c
+++ b/src/examples/flow_position_control/flow_position_control_main.c
@@ -55,7 +55,8 @@
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_local_position.h>
@@ -158,7 +159,8 @@ flow_position_control_thread_main(int argc, char *argv[])
const float time_scale = powf(10.0f,-6.0f);
/* structures */
- struct vehicle_status_s vstatus;
+ struct actuator_armed_s armed;
+ struct vehicle_control_mode_s control_mode;
struct vehicle_attitude_s att;
struct manual_control_setpoint_s manual;
struct filtered_bottom_flow_s filtered_flow;
@@ -169,7 +171,8 @@ flow_position_control_thread_main(int argc, char *argv[])
/* subscribe to attitude, motor setpoints and system state */
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
- int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+ int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+ int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow));
int vehicle_local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
@@ -258,7 +261,7 @@ flow_position_control_thread_main(int argc, char *argv[])
perf_begin(mc_loop_perf);
/* get a local copy of the vehicle state */
- orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
+ orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
/* get a local copy of manual setpoint */
orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual);
/* get a local copy of attitude */
@@ -268,7 +271,7 @@ flow_position_control_thread_main(int argc, char *argv[])
/* get a local copy of local position */
orb_copy(ORB_ID(vehicle_local_position), vehicle_local_position_sub, &local_pos);
- if (vstatus.state_machine == SYSTEM_STATE_AUTO)
+ if (control_mode.flag_control_velocity_enabled)
{
float manual_pitch = manual.pitch / params.rc_scale_pitch; // 0 to 1
float manual_roll = manual.roll / params.rc_scale_roll; // 0 to 1
@@ -490,7 +493,7 @@ flow_position_control_thread_main(int argc, char *argv[])
/* store actual height for speed estimation */
last_local_pos_z = local_pos.z;
- speed_sp.thrust_sp = thrust_control;
+ speed_sp.thrust_sp = thrust_control; //manual.throttle;
speed_sp.timestamp = hrt_absolute_time();
/* publish new speed setpoint */
@@ -527,7 +530,6 @@ flow_position_control_thread_main(int argc, char *argv[])
if(isfinite(manual.throttle))
speed_sp.thrust_sp = manual.throttle;
}
-
/* measure in what intervals the controller runs */
perf_count(mc_interval_perf);
perf_end(mc_loop_perf);
@@ -576,7 +578,8 @@ flow_position_control_thread_main(int argc, char *argv[])
close(parameter_update_sub);
close(vehicle_attitude_sub);
close(vehicle_local_position_sub);
- close(vehicle_status_sub);
+ close(armed_sub);
+ close(control_mode_sub);
close(manual_control_setpoint_sub);
close(speed_sp_pub);
diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c
index e40c9081d7a538ccf029fcda9a8b41ef95b77ab9..5e80066a7052ca7d37f9d37921c746be6a095eaa 100644
--- a/src/examples/flow_position_estimator/flow_position_estimator_main.c
+++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c
@@ -56,7 +56,8 @@
#include <math.h>
#include <uORB/uORB.h>
#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_local_position.h>
@@ -143,8 +144,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
printf("[flow position estimator] starting\n");
/* rotation matrix for transformation of optical flow speed vectors */
- static const int8_t rotM_flow_sensor[3][3] = {{ 0, 1, 0 },
- { -1, 0, 0 },
+ static const int8_t rotM_flow_sensor[3][3] = {{ 0, -1, 0 },
+ { 1, 0, 0 },
{ 0, 0, 1 }}; // 90deg rotated
const float time_scale = powf(10.0f,-6.0f);
static float speed[3] = {0.0f, 0.0f, 0.0f};
@@ -158,8 +159,10 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
static float sonar_lp = 0.0f;
/* subscribe to vehicle status, attitude, sensors and flow*/
- struct vehicle_status_s vstatus;
- memset(&vstatus, 0, sizeof(vstatus));
+ struct actuator_armed_s armed;
+ memset(&armed, 0, sizeof(armed));
+ struct vehicle_control_mode_s control_mode;
+ memset(&control_mode, 0, sizeof(control_mode));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_attitude_setpoint_s att_sp;
@@ -170,8 +173,11 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
/* subscribe to parameter changes */
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
- /* subscribe to vehicle status */
- int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+ /* subscribe to armed topic */
+ int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+
+ /* subscribe to safety topic */
+ int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
/* subscribe to attitude */
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
@@ -218,6 +224,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
while (!thread_should_exit)
{
+
if (sensors_ready)
{
/*This runs at the rate of the sensors */
@@ -263,7 +270,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
/* got flow, updating attitude and status as well */
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp);
- orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
+ orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
+ orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
/* vehicle state estimation */
float sonar_new = flow.ground_distance_m;
@@ -273,14 +281,15 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
* -> accept sonar measurements after reaching calibration distance (values between 0.3m and 1.0m for some time)
* -> minimum sonar value 0.3m
*/
+
if (!vehicle_liftoff)
{
- if (vstatus.flag_system_armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f)
+ if (armed.armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f)
vehicle_liftoff = true;
}
else
{
- if (!vstatus.flag_system_armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f))
+ if (!armed.armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f))
vehicle_liftoff = false;
}
@@ -347,7 +356,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
}
/* filtering ground distance */
- if (!vehicle_liftoff || !vstatus.flag_system_armed)
+ if (!vehicle_liftoff || !armed.armed)
{
/* not possible to fly */
sonar_valid = false;
@@ -444,7 +453,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
close(vehicle_attitude_setpoint_sub);
close(vehicle_attitude_sub);
- close(vehicle_status_sub);
+ close(armed_sub);
+ close(control_mode_sub);
close(parameter_update_sub);
close(optical_flow_sub);
diff --git a/src/examples/flow_speed_control/flow_speed_control_main.c b/src/examples/flow_speed_control/flow_speed_control_main.c
index 8b3881c435560e433b71c7dd89aa2d370f88df44..6af955cd7d7bbe787340e3b0d950be296292a038 100644
--- a/src/examples/flow_speed_control/flow_speed_control_main.c
+++ b/src/examples/flow_speed_control/flow_speed_control_main.c
@@ -55,7 +55,8 @@
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_bodyframe_speed_setpoint.h>
@@ -155,7 +156,8 @@ flow_speed_control_thread_main(int argc, char *argv[])
uint32_t counter = 0;
/* structures */
- struct vehicle_status_s vstatus;
+ struct actuator_armed_s armed;
+ struct vehicle_control_mode_s control_mode;
struct filtered_bottom_flow_s filtered_flow;
struct vehicle_bodyframe_speed_setpoint_s speed_sp;
@@ -164,7 +166,8 @@ flow_speed_control_thread_main(int argc, char *argv[])
/* subscribe to attitude, motor setpoints and system state */
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
- int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+ int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+ int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow));
int vehicle_bodyframe_speed_setpoint_sub = orb_subscribe(ORB_ID(vehicle_bodyframe_speed_setpoint));
@@ -226,14 +229,16 @@ flow_speed_control_thread_main(int argc, char *argv[])
{
perf_begin(mc_loop_perf);
- /* get a local copy of the vehicle state */
- orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
+ /* get a local copy of the armed topic */
+ orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
+ /* get a local copy of the control mode */
+ orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
/* get a local copy of filtered bottom flow */
orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow);
/* get a local copy of bodyframe speed setpoint */
orb_copy(ORB_ID(vehicle_bodyframe_speed_setpoint), vehicle_bodyframe_speed_setpoint_sub, &speed_sp);
- if (vstatus.state_machine == SYSTEM_STATE_AUTO)
+ if (control_mode.flag_control_velocity_enabled)
{
/* calc new roll/pitch */
float pitch_body = -(speed_sp.vx - filtered_flow.vx) * params.speed_p;
@@ -350,7 +355,8 @@ flow_speed_control_thread_main(int argc, char *argv[])
close(vehicle_attitude_sub);
close(vehicle_bodyframe_speed_setpoint_sub);
close(filtered_bottom_flow_sub);
- close(vehicle_status_sub);
+ close(armed_sub);
+ close(control_mode_sub);
close(att_sp_pub);
perf_print_counter(mc_loop_perf);
diff --git a/src/modules/systemlib/geo/geo.c b/src/lib/geo/geo.c
similarity index 99%
rename from src/modules/systemlib/geo/geo.c
rename to src/lib/geo/geo.c
index 6463e6489e41b180417b0e72c01ee2ad26504eb7..63792dda5286dba475f729fb58bc45dcaf91e035 100644
--- a/src/modules/systemlib/geo/geo.c
+++ b/src/lib/geo/geo.c
@@ -44,7 +44,7 @@
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
-#include <systemlib/geo/geo.h>
+#include <geo/geo.h>
#include <nuttx/config.h>
#include <unistd.h>
#include <pthread.h>
diff --git a/src/modules/systemlib/geo/geo.h b/src/lib/geo/geo.h
similarity index 100%
rename from src/modules/systemlib/geo/geo.h
rename to src/lib/geo/geo.h
diff --git a/src/lib/geo/module.mk b/src/lib/geo/module.mk
new file mode 100644
index 0000000000000000000000000000000000000000..30a2dc99fd370ab48d9feff0612a483b9eb5221e
--- /dev/null
+++ b/src/lib/geo/module.mk
@@ -0,0 +1,38 @@
+############################################################################
+#
+# Copyright (C) 2012 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.
+#
+############################################################################
+
+#
+# Geo library
+#
+
+SRCS = geo.c
diff --git a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h b/src/lib/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h
similarity index 100%
rename from src/modules/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h
rename to src/lib/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h
diff --git a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h b/src/lib/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h
similarity index 100%
rename from src/modules/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h
rename to src/lib/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h
diff --git a/src/modules/mathlib/CMSIS/Include/arm_common_tables.h b/src/lib/mathlib/CMSIS/Include/arm_common_tables.h
similarity index 100%
rename from src/modules/mathlib/CMSIS/Include/arm_common_tables.h
rename to src/lib/mathlib/CMSIS/Include/arm_common_tables.h
diff --git a/src/modules/mathlib/CMSIS/Include/arm_const_structs.h b/src/lib/mathlib/CMSIS/Include/arm_const_structs.h
similarity index 100%
rename from src/modules/mathlib/CMSIS/Include/arm_const_structs.h
rename to src/lib/mathlib/CMSIS/Include/arm_const_structs.h
diff --git a/src/modules/mathlib/CMSIS/Include/arm_math.h b/src/lib/mathlib/CMSIS/Include/arm_math.h
similarity index 100%
rename from src/modules/mathlib/CMSIS/Include/arm_math.h
rename to src/lib/mathlib/CMSIS/Include/arm_math.h
diff --git a/src/modules/mathlib/CMSIS/Include/core_cm3.h b/src/lib/mathlib/CMSIS/Include/core_cm3.h
similarity index 100%
rename from src/modules/mathlib/CMSIS/Include/core_cm3.h
rename to src/lib/mathlib/CMSIS/Include/core_cm3.h
diff --git a/src/modules/mathlib/CMSIS/Include/core_cm4.h b/src/lib/mathlib/CMSIS/Include/core_cm4.h
similarity index 100%
rename from src/modules/mathlib/CMSIS/Include/core_cm4.h
rename to src/lib/mathlib/CMSIS/Include/core_cm4.h
diff --git a/src/modules/mathlib/CMSIS/Include/core_cm4_simd.h b/src/lib/mathlib/CMSIS/Include/core_cm4_simd.h
similarity index 100%
rename from src/modules/mathlib/CMSIS/Include/core_cm4_simd.h
rename to src/lib/mathlib/CMSIS/Include/core_cm4_simd.h
diff --git a/src/modules/mathlib/CMSIS/Include/core_cmFunc.h b/src/lib/mathlib/CMSIS/Include/core_cmFunc.h
similarity index 100%
rename from src/modules/mathlib/CMSIS/Include/core_cmFunc.h
rename to src/lib/mathlib/CMSIS/Include/core_cmFunc.h
diff --git a/src/modules/mathlib/CMSIS/Include/core_cmInstr.h b/src/lib/mathlib/CMSIS/Include/core_cmInstr.h
similarity index 100%
rename from src/modules/mathlib/CMSIS/Include/core_cmInstr.h
rename to src/lib/mathlib/CMSIS/Include/core_cmInstr.h
diff --git a/src/modules/mathlib/CMSIS/libarm_cortexM3l_math.a b/src/lib/mathlib/CMSIS/libarm_cortexM3l_math.a
similarity index 100%
rename from src/modules/mathlib/CMSIS/libarm_cortexM3l_math.a
rename to src/lib/mathlib/CMSIS/libarm_cortexM3l_math.a
diff --git a/src/modules/mathlib/CMSIS/libarm_cortexM4l_math.a b/src/lib/mathlib/CMSIS/libarm_cortexM4l_math.a
similarity index 100%
rename from src/modules/mathlib/CMSIS/libarm_cortexM4l_math.a
rename to src/lib/mathlib/CMSIS/libarm_cortexM4l_math.a
diff --git a/src/modules/mathlib/CMSIS/libarm_cortexM4lf_math.a b/src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a
similarity index 100%
rename from src/modules/mathlib/CMSIS/libarm_cortexM4lf_math.a
rename to src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a
diff --git a/src/modules/mathlib/CMSIS/library.mk b/src/lib/mathlib/CMSIS/library.mk
similarity index 100%
rename from src/modules/mathlib/CMSIS/library.mk
rename to src/lib/mathlib/CMSIS/library.mk
diff --git a/src/modules/mathlib/CMSIS/license.txt b/src/lib/mathlib/CMSIS/license.txt
similarity index 100%
rename from src/modules/mathlib/CMSIS/license.txt
rename to src/lib/mathlib/CMSIS/license.txt
diff --git a/src/modules/mathlib/math/Dcm.cpp b/src/lib/mathlib/math/Dcm.cpp
similarity index 100%
rename from src/modules/mathlib/math/Dcm.cpp
rename to src/lib/mathlib/math/Dcm.cpp
diff --git a/src/modules/mathlib/math/Dcm.hpp b/src/lib/mathlib/math/Dcm.hpp
similarity index 100%
rename from src/modules/mathlib/math/Dcm.hpp
rename to src/lib/mathlib/math/Dcm.hpp
diff --git a/src/modules/mathlib/math/EulerAngles.cpp b/src/lib/mathlib/math/EulerAngles.cpp
similarity index 100%
rename from src/modules/mathlib/math/EulerAngles.cpp
rename to src/lib/mathlib/math/EulerAngles.cpp
diff --git a/src/modules/mathlib/math/EulerAngles.hpp b/src/lib/mathlib/math/EulerAngles.hpp
similarity index 100%
rename from src/modules/mathlib/math/EulerAngles.hpp
rename to src/lib/mathlib/math/EulerAngles.hpp
diff --git a/src/modules/mathlib/math/Limits.cpp b/src/lib/mathlib/math/Limits.cpp
similarity index 100%
rename from src/modules/mathlib/math/Limits.cpp
rename to src/lib/mathlib/math/Limits.cpp
diff --git a/src/modules/mathlib/math/Limits.hpp b/src/lib/mathlib/math/Limits.hpp
similarity index 100%
rename from src/modules/mathlib/math/Limits.hpp
rename to src/lib/mathlib/math/Limits.hpp
diff --git a/src/modules/mathlib/math/Matrix.cpp b/src/lib/mathlib/math/Matrix.cpp
similarity index 100%
rename from src/modules/mathlib/math/Matrix.cpp
rename to src/lib/mathlib/math/Matrix.cpp
diff --git a/src/modules/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp
similarity index 100%
rename from src/modules/mathlib/math/Matrix.hpp
rename to src/lib/mathlib/math/Matrix.hpp
diff --git a/src/modules/mathlib/math/Quaternion.cpp b/src/lib/mathlib/math/Quaternion.cpp
similarity index 100%
rename from src/modules/mathlib/math/Quaternion.cpp
rename to src/lib/mathlib/math/Quaternion.cpp
diff --git a/src/modules/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp
similarity index 100%
rename from src/modules/mathlib/math/Quaternion.hpp
rename to src/lib/mathlib/math/Quaternion.hpp
diff --git a/src/modules/mathlib/math/Vector.cpp b/src/lib/mathlib/math/Vector.cpp
similarity index 100%
rename from src/modules/mathlib/math/Vector.cpp
rename to src/lib/mathlib/math/Vector.cpp
diff --git a/src/modules/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp
similarity index 100%
rename from src/modules/mathlib/math/Vector.hpp
rename to src/lib/mathlib/math/Vector.hpp
diff --git a/src/modules/mathlib/math/Vector2f.cpp b/src/lib/mathlib/math/Vector2f.cpp
similarity index 100%
rename from src/modules/mathlib/math/Vector2f.cpp
rename to src/lib/mathlib/math/Vector2f.cpp
diff --git a/src/modules/mathlib/math/Vector2f.hpp b/src/lib/mathlib/math/Vector2f.hpp
similarity index 100%
rename from src/modules/mathlib/math/Vector2f.hpp
rename to src/lib/mathlib/math/Vector2f.hpp
diff --git a/src/modules/mathlib/math/Vector3.cpp b/src/lib/mathlib/math/Vector3.cpp
similarity index 100%
rename from src/modules/mathlib/math/Vector3.cpp
rename to src/lib/mathlib/math/Vector3.cpp
diff --git a/src/modules/mathlib/math/Vector3.hpp b/src/lib/mathlib/math/Vector3.hpp
similarity index 100%
rename from src/modules/mathlib/math/Vector3.hpp
rename to src/lib/mathlib/math/Vector3.hpp
diff --git a/src/modules/mathlib/math/arm/Matrix.cpp b/src/lib/mathlib/math/arm/Matrix.cpp
similarity index 100%
rename from src/modules/mathlib/math/arm/Matrix.cpp
rename to src/lib/mathlib/math/arm/Matrix.cpp
diff --git a/src/modules/mathlib/math/arm/Matrix.hpp b/src/lib/mathlib/math/arm/Matrix.hpp
similarity index 100%
rename from src/modules/mathlib/math/arm/Matrix.hpp
rename to src/lib/mathlib/math/arm/Matrix.hpp
diff --git a/src/modules/mathlib/math/arm/Vector.cpp b/src/lib/mathlib/math/arm/Vector.cpp
similarity index 100%
rename from src/modules/mathlib/math/arm/Vector.cpp
rename to src/lib/mathlib/math/arm/Vector.cpp
diff --git a/src/modules/mathlib/math/arm/Vector.hpp b/src/lib/mathlib/math/arm/Vector.hpp
similarity index 100%
rename from src/modules/mathlib/math/arm/Vector.hpp
rename to src/lib/mathlib/math/arm/Vector.hpp
diff --git a/src/modules/mathlib/math/filter/LowPassFilter2p.cpp b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp
similarity index 100%
rename from src/modules/mathlib/math/filter/LowPassFilter2p.cpp
rename to src/lib/mathlib/math/filter/LowPassFilter2p.cpp
diff --git a/src/modules/mathlib/math/filter/LowPassFilter2p.hpp b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp
similarity index 100%
rename from src/modules/mathlib/math/filter/LowPassFilter2p.hpp
rename to src/lib/mathlib/math/filter/LowPassFilter2p.hpp
diff --git a/src/modules/mathlib/math/filter/module.mk b/src/lib/mathlib/math/filter/module.mk
similarity index 100%
rename from src/modules/mathlib/math/filter/module.mk
rename to src/lib/mathlib/math/filter/module.mk
diff --git a/src/modules/mathlib/math/generic/Matrix.cpp b/src/lib/mathlib/math/generic/Matrix.cpp
similarity index 100%
rename from src/modules/mathlib/math/generic/Matrix.cpp
rename to src/lib/mathlib/math/generic/Matrix.cpp
diff --git a/src/modules/mathlib/math/generic/Matrix.hpp b/src/lib/mathlib/math/generic/Matrix.hpp
similarity index 100%
rename from src/modules/mathlib/math/generic/Matrix.hpp
rename to src/lib/mathlib/math/generic/Matrix.hpp
diff --git a/src/modules/mathlib/math/generic/Vector.cpp b/src/lib/mathlib/math/generic/Vector.cpp
similarity index 100%
rename from src/modules/mathlib/math/generic/Vector.cpp
rename to src/lib/mathlib/math/generic/Vector.cpp
diff --git a/src/modules/mathlib/math/generic/Vector.hpp b/src/lib/mathlib/math/generic/Vector.hpp
similarity index 100%
rename from src/modules/mathlib/math/generic/Vector.hpp
rename to src/lib/mathlib/math/generic/Vector.hpp
diff --git a/src/modules/mathlib/math/nasa_rotation_def.pdf b/src/lib/mathlib/math/nasa_rotation_def.pdf
similarity index 100%
rename from src/modules/mathlib/math/nasa_rotation_def.pdf
rename to src/lib/mathlib/math/nasa_rotation_def.pdf
diff --git a/src/modules/mathlib/math/test/test.cpp b/src/lib/mathlib/math/test/test.cpp
similarity index 100%
rename from src/modules/mathlib/math/test/test.cpp
rename to src/lib/mathlib/math/test/test.cpp
diff --git a/src/modules/mathlib/math/test/test.hpp b/src/lib/mathlib/math/test/test.hpp
similarity index 100%
rename from src/modules/mathlib/math/test/test.hpp
rename to src/lib/mathlib/math/test/test.hpp
diff --git a/src/modules/mathlib/math/test_math.sce b/src/lib/mathlib/math/test_math.sce
similarity index 100%
rename from src/modules/mathlib/math/test_math.sce
rename to src/lib/mathlib/math/test_math.sce
diff --git a/src/modules/mathlib/mathlib.h b/src/lib/mathlib/mathlib.h
similarity index 100%
rename from src/modules/mathlib/mathlib.h
rename to src/lib/mathlib/mathlib.h
diff --git a/src/modules/mathlib/module.mk b/src/lib/mathlib/module.mk
similarity index 100%
rename from src/modules/mathlib/module.mk
rename to src/lib/mathlib/module.mk
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
index 191d20f306d7cb3ff086a83d9a3581c8d19853d8..33879892ee705616e3478b0caf01b34503a5dbaa 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
@@ -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;
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index 9e533ccdfb8641ce4c24ae1e97ec5a2384c91d02..65abcde1e3ff15f1b51d81ba1ef65f675a4c4a9f 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -57,7 +57,7 @@
#include <uORB/topics/debug_key_value.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/parameter_update.h>
#include <drivers/drv_hrt.h>
@@ -216,8 +216,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
memset(&raw, 0, sizeof(raw));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
- struct vehicle_status_s state;
- memset(&state, 0, sizeof(state));
+ struct vehicle_control_mode_s control_mode;
+ memset(&control_mode, 0, sizeof(control_mode));
uint64_t last_data = 0;
uint64_t last_measurement = 0;
@@ -230,8 +230,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
/* subscribe to param changes */
int sub_params = orb_subscribe(ORB_ID(parameter_update));
- /* subscribe to system state*/
- int sub_state = orb_subscribe(ORB_ID(vehicle_status));
+ /* subscribe to control mode*/
+ int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
/* advertise attitude */
orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
@@ -282,9 +282,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
/* XXX this is seriously bad - should be an emergency */
} else if (ret == 0) {
/* check if we're in HIL - not getting sensor data is fine then */
- orb_copy(ORB_ID(vehicle_status), sub_state, &state);
+ orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode);
- if (!state.flag_hil_enabled) {
+ if (!control_mode.flag_system_hil_enabled) {
fprintf(stderr,
"[att ekf] WARNING: Not getting sensors - sensor app running?\n");
}
diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
index 107c2dfb12373126ab3657c7c22a934616e163c9..236052b5695a375b0bed04b3eab0ff58e7f496a2 100755
--- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
+++ b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
@@ -35,7 +35,7 @@
#include <uORB/topics/debug_key_value.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/parameter_update.h>
#include <drivers/drv_hrt.h>
@@ -547,8 +547,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
- struct vehicle_status_s state;
- memset(&state, 0, sizeof(state));
+ struct vehicle_control_mode_s control_mode;
+ memset(&control_mode, 0, sizeof(control_mode));
uint64_t last_data = 0;
uint64_t last_measurement = 0;
@@ -561,8 +561,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
/* subscribe to param changes */
int sub_params = orb_subscribe(ORB_ID(parameter_update));
- /* subscribe to system state*/
- int sub_state = orb_subscribe(ORB_ID(vehicle_status));
+ /* subscribe to control mode */
+ int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
/* advertise attitude */
orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
@@ -612,9 +612,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
/* XXX this is seriously bad - should be an emergency */
} else if (ret == 0) {
/* check if we're in HIL - not getting sensor data is fine then */
- orb_copy(ORB_ID(vehicle_status), sub_state, &state);
+ orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode);
- if (!state.flag_hil_enabled) {
+ if (!control_mode.flag_system_hil_enabled) {
fprintf(stderr,
"[att so3_comp] WARNING: Not getting sensors - sensor app running?\n");
}
diff --git a/src/modules/commander/accelerometer_calibration.c b/src/modules/commander/accelerometer_calibration.cpp
similarity index 80%
rename from src/modules/commander/accelerometer_calibration.c
rename to src/modules/commander/accelerometer_calibration.cpp
index fbb73d997db3763acb4dd23514b9ac9ae4a986ee..ed6707f0458d25eb2a5656209b088f0eaf244d39 100644
--- a/src/modules/commander/accelerometer_calibration.c
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -33,7 +33,7 @@
****************************************************************************/
/**
- * @file accelerometer_calibration.c
+ * @file accelerometer_calibration.cpp
*
* Implementation of accelerometer calibration.
*
@@ -104,32 +104,44 @@
*/
#include "accelerometer_calibration.h"
+#include "commander_helper.h"
+#include <unistd.h>
+#include <stdio.h>
#include <poll.h>
+#include <fcntl.h>
+#include <sys/prctl.h>
+#include <math.h>
+#include <string.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/sensor_combined.h>
#include <drivers/drv_accel.h>
-#include <systemlib/conversions.h>
+#include <geo/geo.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
#include <mavlink/mavlink_log.h>
-void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd);
-int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]);
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]);
int detect_orientation(int mavlink_fd, int sub_sensor_combined);
int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num);
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 status_pub, struct vehicle_status_s *status, int mavlink_fd) {
+int do_accel_calibration(int mavlink_fd) {
/* announce change */
mavlink_log_info(mavlink_fd, "accel calibration started");
- /* set to accel calibration mode */
- status->flag_preflight_accel_calibration = true;
- state_machine_publish(status_pub, status, mavlink_fd);
+ mavlink_log_info(mavlink_fd, "accel cal progress <0> percent");
/* measure and calculate offsets & scales */
float accel_offs[3];
float accel_scale[3];
- int res = do_accel_calibration_mesurements(mavlink_fd, accel_offs, accel_scale);
+ int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_scale);
if (res == OK) {
/* measurements complete successfully, set parameters */
@@ -165,24 +177,17 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int m
}
mavlink_log_info(mavlink_fd, "accel calibration done");
- tune_confirm();
- sleep(2);
- tune_confirm();
- sleep(2);
- /* third beep by cal end routine */
+ return OK;
} else {
/* measurements error */
mavlink_log_info(mavlink_fd, "accel calibration aborted");
- tune_error();
- sleep(2);
+ return ERROR;
}
/* exit accel calibration mode */
- status->flag_preflight_accel_calibration = false;
- state_machine_publish(status_pub, status, mavlink_fd);
}
-int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) {
+int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) {
const int samples_num = 2500;
float accel_ref[6][3];
bool data_collected[6] = { false, false, false, false, false, false };
@@ -207,38 +212,52 @@ int do_accel_calibration_mesurements(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], accel_ref[orient][0], accel_ref[orient][1], accel_ref[orient][2]);
- mavlink_log_info(mavlink_fd, str);
+ 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]);
+
data_collected[orient] = true;
- tune_confirm();
+ tune_neutral();
}
close(sensor_combined_sub);
@@ -246,7 +265,7 @@ int do_accel_calibration_mesurements(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;
}
@@ -279,8 +298,10 @@ 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;
- struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
+ hrt_abstime still_time = 2000000;
+ struct pollfd fds[1];
+ fds[0].fd = sub_sensor_combined;
+ fds[0].events = POLLIN;
hrt_abstime t_start = hrt_absolute_time();
/* set timeout to 30s */
@@ -316,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;
}
@@ -331,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;
}
}
@@ -343,34 +364,34 @@ 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;
}
}
- if ( fabs(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
- fabs(accel_ema[1]) < accel_err_thr &&
- fabs(accel_ema[2]) < accel_err_thr )
+ if ( fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
+ fabsf(accel_ema[1]) < accel_err_thr &&
+ fabsf(accel_ema[2]) < accel_err_thr )
return 0; // [ g, 0, 0 ]
- if ( fabs(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
- fabs(accel_ema[1]) < accel_err_thr &&
- fabs(accel_ema[2]) < accel_err_thr )
+ if ( fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
+ fabsf(accel_ema[1]) < accel_err_thr &&
+ fabsf(accel_ema[2]) < accel_err_thr )
return 1; // [ -g, 0, 0 ]
- if ( fabs(accel_ema[0]) < accel_err_thr &&
- fabs(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
- fabs(accel_ema[2]) < accel_err_thr )
+ if ( fabsf(accel_ema[0]) < accel_err_thr &&
+ fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
+ fabsf(accel_ema[2]) < accel_err_thr )
return 2; // [ 0, g, 0 ]
- if ( fabs(accel_ema[0]) < accel_err_thr &&
- fabs(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
- fabs(accel_ema[2]) < accel_err_thr )
+ if ( fabsf(accel_ema[0]) < accel_err_thr &&
+ fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
+ fabsf(accel_ema[2]) < accel_err_thr )
return 3; // [ 0, -g, 0 ]
- if ( fabs(accel_ema[0]) < accel_err_thr &&
- fabs(accel_ema[1]) < accel_err_thr &&
- fabs(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr )
+ if ( fabsf(accel_ema[0]) < accel_err_thr &&
+ fabsf(accel_ema[1]) < accel_err_thr &&
+ fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr )
return 4; // [ 0, 0, g ]
- if ( fabs(accel_ema[0]) < accel_err_thr &&
- fabs(accel_ema[1]) < accel_err_thr &&
- fabs(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr )
+ if ( fabsf(accel_ema[0]) < accel_err_thr &&
+ fabsf(accel_ema[1]) < accel_err_thr &&
+ fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr )
return 5; // [ 0, 0, -g ]
mavlink_log_info(mavlink_fd, "ERROR: invalid orientation");
@@ -382,7 +403,9 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
* Read specified number of accelerometer samples, calculate average and dispersion.
*/
int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num) {
- struct pollfd fds[1] = { { .fd = sensor_combined_sub, .events = POLLIN } };
+ struct pollfd fds[1];
+ fds[0].fd = sensor_combined_sub;
+ fds[0].events = POLLIN;
int count = 0;
float accel_sum[3] = { 0.0f, 0.0f, 0.0f };
@@ -416,7 +439,7 @@ int mat_invert3(float src[3][3], float dst[3][3]) {
float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) -
src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) +
src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]);
- if (det == 0.0)
+ if (det == 0.0f)
return ERROR; // Singular matrix
dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det;
diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h
index f93a867bae335474982cd52054ea0a1bd297946f..1cf9c0977542d43cf96c3bc484050fd5114df424 100644
--- a/src/modules/commander/accelerometer_calibration.h
+++ b/src/modules/commander/accelerometer_calibration.h
@@ -44,8 +44,7 @@
#define ACCELEROMETER_CALIBRATION_H_
#include <stdint.h>
-#include <uORB/topics/vehicle_status.h>
-void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd);
+int do_accel_calibration(int mavlink_fd);
#endif /* ACCELEROMETER_CALIBRATION_H_ */
diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e414e5f70739b69af3fd07f986640897b467e75b
--- /dev/null
+++ b/src/modules/commander/airspeed_calibration.cpp
@@ -0,0 +1,121 @@
+/****************************************************************************
+ *
+ * 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 airspeed_calibration.cpp
+ * Airspeed sensor calibration routine
+ */
+
+#include "airspeed_calibration.h"
+#include "commander_helper.h"
+
+#include <stdio.h>
+#include <poll.h>
+#include <math.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/differential_pressure.h>
+#include <mavlink/mavlink_log.h>
+#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;
+
+int do_airspeed_calibration(int mavlink_fd)
+{
+ /* give directions */
+ mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still");
+
+ const int calibration_count = 2500;
+
+ int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
+ struct differential_pressure_s diff_pres;
+
+ int calibration_counter = 0;
+ float diff_pres_offset = 0.0f;
+
+ while (calibration_counter < calibration_count) {
+
+ /* wait blocking for new data */
+ struct pollfd fds[1];
+ fds[0].fd = diff_pres_sub;
+ fds[0].events = POLLIN;
+
+ int poll_ret = poll(fds, 1, 1000);
+
+ if (poll_ret) {
+ orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
+ diff_pres_offset += diff_pres.differential_pressure_pa;
+ calibration_counter++;
+
+ } else if (poll_ret == 0) {
+ /* any poll failure for 1s is a reason to abort */
+ mavlink_log_info(mavlink_fd, "airspeed calibration aborted");
+ return ERROR;
+ }
+ }
+
+ diff_pres_offset = diff_pres_offset / calibration_count;
+
+ if (isfinite(diff_pres_offset)) {
+
+ 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 */
+ 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;
+ }
+
+ //char buf[50];
+ //sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]);
+ //mavlink_log_info(mavlink_fd, buf);
+ mavlink_log_info(mavlink_fd, "airspeed calibration done");
+
+ return OK;
+
+ } else {
+ mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)");
+ return ERROR;
+ }
+}
diff --git a/src/modules/multirotor_pos_control/position_control.h b/src/modules/commander/airspeed_calibration.h
similarity index 66%
rename from src/modules/multirotor_pos_control/position_control.h
rename to src/modules/commander/airspeed_calibration.h
index 2144ebc343244dfc245ad79edcbac2f5e79f6dfe..71c701fc2ebf2744a2276c87cb171c9cbb3989f1 100644
--- a/src/modules/multirotor_pos_control/position_control.h
+++ b/src/modules/commander/airspeed_calibration.h
@@ -1,10 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- * Author: @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Laurens Mackay <mackayl@student.ethz.ch>
- * @author Tobias Naegeli <naegelit@student.ethz.ch>
- * @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ * 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
@@ -36,15 +32,15 @@
****************************************************************************/
/**
- * @file multirotor_position_control.h
- * Definition of the position control for a multirotor VTOL
+ * @file gyro_calibration.h
+ * Airspeed sensor calibration routine
*/
-// #ifndef POSITION_CONTROL_H_
-// #define POSITION_CONTROL_H_
+#ifndef AIRSPEED_CALIBRATION_H_
+#define AIRSPEED_CALIBRATION_H_
-// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual,
-// const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos,
-// const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp);
+#include <stdint.h>
-// #endif /* POSITION_CONTROL_H_ */
+int do_airspeed_calibration(int mavlink_fd);
+
+#endif /* AIRSPEED_CALIBRATION_H_ */
diff --git a/src/modules/commander/baro_calibration.cpp b/src/modules/commander/baro_calibration.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3123c4087617204dd44e34410832eac118d11502
--- /dev/null
+++ b/src/modules/commander/baro_calibration.cpp
@@ -0,0 +1,60 @@
+/****************************************************************************
+ *
+ * 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 baro_calibration.cpp
+ * Barometer calibration routine
+ */
+
+#include "baro_calibration.h"
+
+#include <poll.h>
+#include <math.h>
+#include <fcntl.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/topics/sensor_combined.h>
+#include <drivers/drv_baro.h>
+#include <mavlink/mavlink_log.h>
+#include <systemlib/param/param.h>
+
+/* 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 ERROR;
+}
diff --git a/src/modules/commander/baro_calibration.h b/src/modules/commander/baro_calibration.h
new file mode 100644
index 0000000000000000000000000000000000000000..bc42bc6cfc530e8215b0cd689526541aaf99c31c
--- /dev/null
+++ b/src/modules/commander/baro_calibration.h
@@ -0,0 +1,46 @@
+/****************************************************************************
+ *
+ * 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 mag_calibration.h
+ * Barometer calibration routine
+ */
+
+#ifndef BARO_CALIBRATION_H_
+#define BARO_CALIBRATION_H_
+
+#include <stdint.h>
+
+int do_baro_calibration(int mavlink_fd);
+
+#endif /* BARO_CALIBRATION_H_ */
diff --git a/src/modules/commander/calibration_routines.c b/src/modules/commander/calibration_routines.cpp
similarity index 99%
rename from src/modules/commander/calibration_routines.c
rename to src/modules/commander/calibration_routines.cpp
index a2693863784193db2ab4d8c811a30608316008ce..be38ea104602637eb082647ced8ee51148b42256 100644
--- a/src/modules/commander/calibration_routines.c
+++ b/src/modules/commander/calibration_routines.cpp
@@ -33,7 +33,7 @@
****************************************************************************/
/**
- * @file calibration_routines.c
+ * @file calibration_routines.cpp
* Calibration routines implementations.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
@@ -217,3 +217,4 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[],
return 0;
}
+
diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c
deleted file mode 100644
index e9d1f39540be409b675db3ffdfba62456a33827f..0000000000000000000000000000000000000000
--- a/src/modules/commander/commander.c
+++ /dev/null
@@ -1,2097 +0,0 @@
-/****************************************************************************
- *
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
- * Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
- * Lorenz Meier <lm@inf.ethz.ch>
- * Thomas Gubler <thomasgubler@student.ethz.ch>
- * Julian Oes <joes@student.ethz.ch>
- *
- * 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.c
- * Main system state machine implementation.
- *
- * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
- *
- */
-
-#include "commander.h"
-
-#include <nuttx/config.h>
-#include <pthread.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <stdbool.h>
-#include <string.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <debug.h>
-#include <sys/prctl.h>
-#include <string.h>
-#include <drivers/drv_led.h>
-#include <drivers/drv_hrt.h>
-#include <drivers/drv_tone_alarm.h>
-#include "state_machine_helper.h"
-#include "systemlib/systemlib.h"
-#include <math.h>
-#include <poll.h>
-#include <uORB/uORB.h>
-#include <uORB/topics/sensor_combined.h>
-#include <uORB/topics/battery_status.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/offboard_control_setpoint.h>
-#include <uORB/topics/home_position.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/vehicle_gps_position.h>
-#include <uORB/topics/vehicle_command.h>
-#include <uORB/topics/subsystem_info.h>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/differential_pressure.h>
-#include <mavlink/mavlink_log.h>
-
-#include <systemlib/param/param.h>
-#include <systemlib/systemlib.h>
-#include <systemlib/err.h>
-
-/* XXX MOVE CALIBRATION TO SENSORS APP THREAD */
-#include <drivers/drv_accel.h>
-#include <drivers/drv_gyro.h>
-#include <drivers/drv_mag.h>
-#include <drivers/drv_baro.h>
-
-#include "calibration_routines.h"
-#include "accelerometer_calibration.h"
-
-PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */
-//PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0); /**< Go into high-level failsafe after 0 ms */
-PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
-PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
-PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
-
-#include <systemlib/cpuload.h>
-extern struct system_load_s system_load;
-
-/* Decouple update interval and hysteris counters, all depends on intervals */
-#define COMMANDER_MONITORING_INTERVAL 50000
-#define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f))
-#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
-#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
-
-#define STICK_ON_OFF_LIMIT 0.75f
-#define STICK_THRUST_RANGE 1.0f
-#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
-#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
-
-#define GPS_FIX_TYPE_2D 2
-#define GPS_FIX_TYPE_3D 3
-#define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000
-#define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
-
-/* File descriptors */
-static int leds;
-static int buzzer;
-static int mavlink_fd;
-static bool commander_initialized = false;
-static struct vehicle_status_s current_status; /**< Main state machine */
-static orb_advert_t stat_pub;
-
-// static uint16_t nofix_counter = 0;
-// static uint16_t gotfix_counter = 0;
-
-static unsigned int failsafe_lowlevel_timeout_ms;
-
-static bool thread_should_exit = false; /**< daemon exit flag */
-static bool thread_running = false; /**< daemon status flag */
-static int daemon_task; /**< Handle of daemon task / thread */
-
-/* pthread loops */
-static void *orb_receive_loop(void *arg);
-
-__EXPORT int commander_main(int argc, char *argv[]);
-
-/**
- * Mainloop of commander.
- */
-int commander_thread_main(int argc, char *argv[]);
-
-static int buzzer_init(void);
-static void buzzer_deinit(void);
-static int led_init(void);
-static void led_deinit(void);
-static int led_toggle(int led);
-static int led_on(int led);
-static int led_off(int led);
-static void do_gyro_calibration(int status_pub, struct vehicle_status_s *status);
-static void do_mag_calibration(int status_pub, struct vehicle_status_s *status);
-static void do_rc_calibration(int status_pub, struct vehicle_status_s *status);
-static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd);
-
-int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state);
-
-
-
-/**
- * Print the correct usage.
- */
-static void usage(const char *reason);
-
-/**
- * Sort calibration values.
- *
- * Sorts the calibration values with bubble sort.
- *
- * @param a The array to sort
- * @param n The number of entries in the array
- */
-// static void cal_bsort(float a[], int n);
-
-static int buzzer_init()
-{
- buzzer = open("/dev/tone_alarm", O_WRONLY);
-
- if (buzzer < 0) {
- warnx("Buzzer: open fail\n");
- return ERROR;
- }
-
- return 0;
-}
-
-static void buzzer_deinit()
-{
- close(buzzer);
-}
-
-
-static int led_init()
-{
- leds = open(LED_DEVICE_PATH, 0);
-
- if (leds < 0) {
- warnx("LED: open fail\n");
- return ERROR;
- }
-
- if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) {
- warnx("LED: ioctl fail\n");
- return ERROR;
- }
-
- return 0;
-}
-
-static void led_deinit()
-{
- close(leds);
-}
-
-static int led_toggle(int led)
-{
- static int last_blue = LED_ON;
- static int last_amber = LED_ON;
-
- if (led == LED_BLUE) last_blue = (last_blue == LED_ON) ? LED_OFF : LED_ON;
-
- if (led == LED_AMBER) last_amber = (last_amber == LED_ON) ? LED_OFF : LED_ON;
-
- return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led);
-}
-
-static int led_on(int led)
-{
- return ioctl(leds, LED_ON, led);
-}
-
-static int led_off(int led)
-{
- return ioctl(leds, LED_OFF, led);
-}
-
-enum AUDIO_PATTERN {
- AUDIO_PATTERN_ERROR = 2,
- AUDIO_PATTERN_NOTIFY_POSITIVE = 3,
- AUDIO_PATTERN_NOTIFY_NEUTRAL = 4,
- AUDIO_PATTERN_NOTIFY_NEGATIVE = 5,
- AUDIO_PATTERN_NOTIFY_CHARGE = 6
-};
-
-int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state)
-{
-
- /* Trigger alarm if going into any error state */
- if (((new_state == SYSTEM_STATE_GROUND_ERROR) && (old_state != SYSTEM_STATE_GROUND_ERROR)) ||
- ((new_state == SYSTEM_STATE_MISSION_ABORT) && (old_state != SYSTEM_STATE_MISSION_ABORT))) {
- ioctl(buzzer, TONE_SET_ALARM, 0);
- ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_ERROR);
- }
-
- /* Trigger neutral on arming / disarming */
- if (((new_state == SYSTEM_STATE_GROUND_READY) && (old_state != SYSTEM_STATE_GROUND_READY))) {
- ioctl(buzzer, TONE_SET_ALARM, 0);
- ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_NOTIFY_NEUTRAL);
- }
-
- /* Trigger Tetris on being bored */
-
- return 0;
-}
-
-void tune_confirm(void)
-{
- ioctl(buzzer, TONE_SET_ALARM, 3);
-}
-
-void tune_error(void)
-{
- ioctl(buzzer, TONE_SET_ALARM, 4);
-}
-
-void do_rc_calibration(int status_pub, struct vehicle_status_s *status)
-{
- if (current_status.rc_signal_lost) {
- mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal.");
- return;
- }
-
- int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
- struct manual_control_setpoint_s sp;
- orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);
-
- /* set parameters */
-
- float p = sp.roll;
- param_set(param_find("TRIM_ROLL"), &p);
- p = sp.pitch;
- param_set(param_find("TRIM_PITCH"), &p);
- p = sp.yaw;
- param_set(param_find("TRIM_YAW"), &p);
-
- /* store to permanent storage */
- /* auto-save to EEPROM */
- int save_ret = param_save_default();
-
- if (save_ret != 0) {
- mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed");
- }
-
- mavlink_log_info(mavlink_fd, "trim calibration done");
-}
-
-void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
-{
-
- /* set to mag calibration mode */
- status->flag_preflight_mag_calibration = true;
- state_machine_publish(status_pub, status, mavlink_fd);
-
- int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
- struct mag_report mag;
-
- /* 45 seconds */
- uint64_t calibration_interval = 45 * 1000 * 1000;
-
- /* maximum 2000 values */
- const unsigned int calibration_maxcount = 500;
- unsigned int calibration_counter = 0;
-
- /* limit update rate to get equally spaced measurements over time (in ms) */
- orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount);
-
- // XXX old cal
- // * FLT_MIN is not the most negative float number,
- // * but the smallest number by magnitude float can
- // * represent. Use -FLT_MAX to initialize the most
- // * negative number
-
- // float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX};
- // float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX};
-
- int fd = open(MAG_DEVICE_PATH, O_RDONLY);
-
- /* erase old calibration */
- struct mag_scale mscale_null = {
- 0.0f,
- 1.0f,
- 0.0f,
- 1.0f,
- 0.0f,
- 1.0f,
- };
-
- if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
- warn("WARNING: failed to set scale / offsets for mag");
- mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag");
- }
-
- /* calibrate range */
- if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) {
- warnx("failed to calibrate scale");
- }
-
- close(fd);
-
- /* calibrate offsets */
-
- // uint64_t calibration_start = hrt_absolute_time();
-
- uint64_t axis_deadline = hrt_absolute_time();
- uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
-
- const char axislabels[3] = { 'X', 'Y', 'Z'};
- int axis_index = -1;
-
- float *x = (float *)malloc(sizeof(float) * calibration_maxcount);
- float *y = (float *)malloc(sizeof(float) * calibration_maxcount);
- float *z = (float *)malloc(sizeof(float) * calibration_maxcount);
-
- if (x == NULL || y == NULL || z == NULL) {
- 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;
- }
-
- tune_confirm();
- sleep(2);
- tune_confirm();
-
- while (hrt_absolute_time() < calibration_deadline &&
- calibration_counter < calibration_maxcount) {
-
- /* wait blocking for new data */
- struct pollfd fds[1] = { { .fd = sub_mag, .events = POLLIN } };
-
- /* user guidance */
- if (hrt_absolute_time() >= axis_deadline &&
- axis_index < 3) {
-
- axis_index++;
-
- char buf[50];
- sprintf(buf, "Please rotate around %c", axislabels[axis_index]);
- mavlink_log_info(mavlink_fd, buf);
- tune_confirm();
-
- axis_deadline += calibration_interval / 3;
- }
-
- if (!(axis_index < 3)) {
- break;
- }
-
- // int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time();
-
- // if ((axis_left / 1000) == 0 && axis_left > 0) {
- // char buf[50];
- // sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]);
- // mavlink_log_info(mavlink_fd, buf);
- // }
-
- int poll_ret = poll(fds, 1, 1000);
-
- if (poll_ret) {
- orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
-
- x[calibration_counter] = mag.x;
- y[calibration_counter] = mag.y;
- z[calibration_counter] = mag.z;
-
- /* get min/max values */
-
- // if (mag.x < mag_min[0]) {
- // mag_min[0] = mag.x;
- // }
- // else if (mag.x > mag_max[0]) {
- // mag_max[0] = mag.x;
- // }
-
- // if (raw.magnetometer_ga[1] < mag_min[1]) {
- // mag_min[1] = raw.magnetometer_ga[1];
- // }
- // else if (raw.magnetometer_ga[1] > mag_max[1]) {
- // mag_max[1] = raw.magnetometer_ga[1];
- // }
-
- // if (raw.magnetometer_ga[2] < mag_min[2]) {
- // mag_min[2] = raw.magnetometer_ga[2];
- // }
- // else if (raw.magnetometer_ga[2] > mag_max[2]) {
- // mag_max[2] = raw.magnetometer_ga[2];
- // }
-
- 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;
- }
- }
-
- float sphere_x;
- float sphere_y;
- float sphere_z;
- float sphere_radius;
-
- sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
-
- free(x);
- free(y);
- free(z);
-
- if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) {
-
- fd = open(MAG_DEVICE_PATH, 0);
-
- struct mag_scale mscale;
-
- if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale))
- warn("WARNING: failed to get scale / offsets for mag");
-
- mscale.x_offset = sphere_x;
- mscale.y_offset = sphere_y;
- mscale.z_offset = sphere_z;
-
- if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale))
- warn("WARNING: failed to set scale / offsets for mag");
-
- close(fd);
-
- /* announce and set new offset */
-
- if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) {
- warnx("Setting X mag offset failed!\n");
- }
-
- if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) {
- warnx("Setting Y mag offset failed!\n");
- }
-
- if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) {
- warnx("Setting Z mag offset failed!\n");
- }
-
- if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) {
- warnx("Setting X mag scale failed!\n");
- }
-
- if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) {
- warnx("Setting Y mag scale failed!\n");
- }
-
- if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) {
- warnx("Setting Z mag scale failed!\n");
- }
-
- /* 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");
- }
-
- warnx("\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n",
- (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale,
- (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius);
-
- char buf[52];
- sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset,
- (double)mscale.y_offset, (double)mscale.z_offset);
- mavlink_log_info(mavlink_fd, buf);
-
- sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale,
- (double)mscale.y_scale, (double)mscale.z_scale);
- mavlink_log_info(mavlink_fd, buf);
-
- mavlink_log_info(mavlink_fd, "mag calibration done");
-
- tune_confirm();
- sleep(2);
- tune_confirm();
- sleep(2);
- /* third beep by cal end routine */
-
- } else {
- mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)");
- }
-
- /* disable calibration mode */
- status->flag_preflight_mag_calibration = false;
- state_machine_publish(status_pub, status, mavlink_fd);
-
- close(sub_mag);
-}
-
-void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
-{
- /* set to gyro calibration mode */
- status->flag_preflight_gyro_calibration = true;
- state_machine_publish(status_pub, status, mavlink_fd);
-
- const int calibration_count = 5000;
-
- int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
- struct sensor_combined_s raw;
-
- int calibration_counter = 0;
- float gyro_offset[3] = {0.0f, 0.0f, 0.0f};
-
- /* set offsets to zero */
- int fd = open(GYRO_DEVICE_PATH, 0);
- struct gyro_scale gscale_null = {
- 0.0f,
- 1.0f,
- 0.0f,
- 1.0f,
- 0.0f,
- 1.0f,
- };
-
- if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null))
- warn("WARNING: failed to set scale / offsets for gyro");
-
- close(fd);
-
- int errcount = 0;
-
- while (calibration_counter < calibration_count) {
-
- /* wait blocking for new data */
- struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
-
- int poll_ret = poll(fds, 1, 1000);
-
- if (poll_ret) {
- 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) {
- errcount++;
- }
-
- if (errcount > 1000) {
- /* any persisting poll error is a reason to abort */
- mavlink_log_info(mavlink_fd, "permanent gyro error, aborted.");
- return;
- }
- }
-
- gyro_offset[0] = gyro_offset[0] / calibration_count;
- gyro_offset[1] = gyro_offset[1] / calibration_count;
- gyro_offset[2] = gyro_offset[2] / calibration_count;
-
- /* exit gyro calibration mode */
- status->flag_preflight_gyro_calibration = false;
- state_machine_publish(status_pub, status, mavlink_fd);
-
- if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) {
-
- if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0]))
- || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1]))
- || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) {
- mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!");
- }
-
- /* set offsets to actual value */
- fd = open(GYRO_DEVICE_PATH, 0);
- struct gyro_scale gscale = {
- gyro_offset[0],
- 1.0f,
- gyro_offset[1],
- 1.0f,
- gyro_offset[2],
- 1.0f,
- };
-
- if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
- warn("WARNING: failed to set scale / offsets for gyro");
-
- close(fd);
-
- /* auto-save to EEPROM */
- int save_ret = param_save_default();
-
- if (save_ret != 0) {
- warn("WARNING: auto-save of params to storage failed");
- }
-
- // char buf[50];
- // sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]);
- // mavlink_log_info(mavlink_fd, buf);
- mavlink_log_info(mavlink_fd, "gyro calibration done");
-
- tune_confirm();
- sleep(2);
- tune_confirm();
- sleep(2);
- /* third beep by cal end routine */
-
- } else {
- mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)");
- }
-
- close(sub_sensor_combined);
-}
-
-void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status)
-{
- /* announce change */
-
- mavlink_log_info(mavlink_fd, "keep it still");
- /* set to accel calibration mode */
- status->flag_preflight_airspeed_calibration = true;
- state_machine_publish(status_pub, status, mavlink_fd);
-
- const int calibration_count = 2500;
-
- int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
- struct differential_pressure_s diff_pres;
-
- int calibration_counter = 0;
- float diff_pres_offset = 0.0f;
-
- while (calibration_counter < calibration_count) {
-
- /* wait blocking for new data */
- struct pollfd fds[1] = { { .fd = diff_pres_sub, .events = POLLIN } };
-
- int poll_ret = poll(fds, 1, 1000);
-
- if (poll_ret) {
- orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
- diff_pres_offset += diff_pres.differential_pressure_pa;
- calibration_counter++;
-
- } else if (poll_ret == 0) {
- /* any poll failure for 1s is a reason to abort */
- mavlink_log_info(mavlink_fd, "airspeed calibration aborted");
- return;
- }
- }
-
- diff_pres_offset = diff_pres_offset / calibration_count;
-
- if (isfinite(diff_pres_offset)) {
-
- if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
- mavlink_log_critical(mavlink_fd, "Setting offs failed!");
- }
-
- /* auto-save to EEPROM */
- int save_ret = param_save_default();
-
- if (save_ret != 0) {
- warn("WARNING: auto-save of params to storage failed");
- }
-
- //char buf[50];
- //sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]);
- //mavlink_log_info(mavlink_fd, buf);
- mavlink_log_info(mavlink_fd, "airspeed calibration done");
-
- tune_confirm();
- sleep(2);
- tune_confirm();
- sleep(2);
- /* third beep by cal end routine */
-
- } else {
- mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)");
- }
-
- /* exit airspeed calibration mode */
- status->flag_preflight_airspeed_calibration = false;
- state_machine_publish(status_pub, status, mavlink_fd);
-
- close(diff_pres_sub);
-}
-
-
-
-void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd)
-{
- /* result of the command */
- uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED;
-
- /* announce command handling */
- tune_confirm();
-
-
- /* supported command handling start */
-
- /* request to set different system mode */
- switch (cmd->command) {
- case VEHICLE_CMD_DO_SET_MODE: {
- if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, (uint8_t)cmd->param1)) {
- result = VEHICLE_CMD_RESULT_ACCEPTED;
-
- } else {
- result = VEHICLE_CMD_RESULT_DENIED;
- }
- }
- break;
-
- case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
- /* request to arm */
- if ((int)cmd->param1 == 1) {
- if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
- result = VEHICLE_CMD_RESULT_ACCEPTED;
-
- } else {
- result = VEHICLE_CMD_RESULT_DENIED;
- }
-
- /* request to disarm */
-
- } else if ((int)cmd->param1 == 0) {
- if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
- result = VEHICLE_CMD_RESULT_ACCEPTED;
-
- } else {
- result = VEHICLE_CMD_RESULT_DENIED;
- }
- }
- }
- break;
-
- /* request for an autopilot reboot */
- case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: {
- if ((int)cmd->param1 == 1) {
- if (OK == do_state_update(status_pub, current_vehicle_status, mavlink_fd, SYSTEM_STATE_REBOOT)) {
- /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */
- result = VEHICLE_CMD_RESULT_ACCEPTED;
-
- } else {
- /* system may return here */
- result = VEHICLE_CMD_RESULT_DENIED;
- }
- }
- }
- break;
-
-// /* request to land */
-// case VEHICLE_CMD_NAV_LAND:
-// {
-// //TODO: add check if landing possible
-// //TODO: add landing maneuver
-//
-// if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) {
-// result = VEHICLE_CMD_RESULT_ACCEPTED;
-// } }
-// break;
-//
-// /* request to takeoff */
-// case VEHICLE_CMD_NAV_TAKEOFF:
-// {
-// //TODO: add check if takeoff possible
-// //TODO: add takeoff maneuver
-//
-// if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) {
-// result = VEHICLE_CMD_RESULT_ACCEPTED;
-// }
-// }
-// break;
-//
- /* preflight calibration */
- case VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
- bool handled = false;
-
- /* gyro calibration */
- if ((int)(cmd->param1) == 1) {
- /* transition to calibration state */
- do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
- if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
- mavlink_log_info(mavlink_fd, "starting gyro cal");
- tune_confirm();
- do_gyro_calibration(status_pub, ¤t_status);
- mavlink_log_info(mavlink_fd, "finished gyro cal");
- tune_confirm();
- do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY);
- result = VEHICLE_CMD_RESULT_ACCEPTED;
-
- } else {
- mavlink_log_critical(mavlink_fd, "REJECTING gyro cal");
- result = VEHICLE_CMD_RESULT_DENIED;
- }
-
- handled = true;
- }
-
- /* magnetometer calibration */
- if ((int)(cmd->param2) == 1) {
- /* transition to calibration state */
- do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
- if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
- mavlink_log_info(mavlink_fd, "starting mag cal");
- tune_confirm();
- do_mag_calibration(status_pub, ¤t_status);
- mavlink_log_info(mavlink_fd, "finished mag cal");
- tune_confirm();
- do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY);
- result = VEHICLE_CMD_RESULT_ACCEPTED;
-
- } else {
- mavlink_log_critical(mavlink_fd, "REJECTING mag cal");
- result = VEHICLE_CMD_RESULT_DENIED;
- }
-
- handled = true;
- }
-
- /* zero-altitude pressure calibration */
- if ((int)(cmd->param3) == 1) {
- /* transition to calibration state */
- do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
- if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
- mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented");
- tune_confirm();
-
- } else {
- mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration");
- result = VEHICLE_CMD_RESULT_DENIED;
- }
-
- handled = true;
- }
-
- /* trim calibration */
- if ((int)(cmd->param4) == 1) {
- /* transition to calibration state */
- do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
- if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
- mavlink_log_info(mavlink_fd, "starting trim cal");
- tune_confirm();
- do_rc_calibration(status_pub, ¤t_status);
- mavlink_log_info(mavlink_fd, "finished trim cal");
- tune_confirm();
- do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY);
- result = VEHICLE_CMD_RESULT_ACCEPTED;
-
- } else {
- mavlink_log_critical(mavlink_fd, "REJECTING trim cal");
- result = VEHICLE_CMD_RESULT_DENIED;
- }
-
- handled = true;
- }
-
- /* accel calibration */
- if ((int)(cmd->param5) == 1) {
- /* transition to calibration state */
- do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
- if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
- mavlink_log_info(mavlink_fd, "CMD starting accel cal");
- tune_confirm();
- do_accel_calibration(status_pub, ¤t_status, mavlink_fd);
- tune_confirm();
- mavlink_log_info(mavlink_fd, "CMD finished accel cal");
- do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY);
- result = VEHICLE_CMD_RESULT_ACCEPTED;
-
- } else {
- mavlink_log_critical(mavlink_fd, "REJECTING accel cal");
- result = VEHICLE_CMD_RESULT_DENIED;
- }
-
- handled = true;
- }
-
- /* airspeed calibration */
- if ((int)(cmd->param6) == 1) { //xxx: this is not defined by the mavlink protocol
- /* transition to calibration state */
- do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
- if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
- mavlink_log_info(mavlink_fd, "CMD starting airspeed cal");
- tune_confirm();
- do_airspeed_calibration(status_pub, ¤t_status);
- tune_confirm();
- mavlink_log_info(mavlink_fd, "CMD finished airspeed cal");
- do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY);
- result = VEHICLE_CMD_RESULT_ACCEPTED;
-
- } else {
- mavlink_log_critical(mavlink_fd, "REJECTING airspeed cal");
- result = VEHICLE_CMD_RESULT_DENIED;
- }
-
- handled = true;
- }
-
- /* none found */
- if (!handled) {
- //warnx("refusing unsupported calibration request\n");
- mavlink_log_critical(mavlink_fd, "CMD refusing unsup. calib. request");
- result = VEHICLE_CMD_RESULT_UNSUPPORTED;
- }
- }
- break;
-
- case VEHICLE_CMD_PREFLIGHT_STORAGE: {
- if (current_status.flag_system_armed &&
- ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
- (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
- (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) {
- /* do not perform expensive memory tasks on multirotors in flight */
- // XXX this is over-safe, as soon as cmd is in low prio thread this can be allowed
- mavlink_log_info(mavlink_fd, "REJECTING save cmd while multicopter armed");
-
- } else {
-
- // XXX move this to LOW PRIO THREAD of commander app
- /* Read all parameters from EEPROM to RAM */
-
- if (((int)(cmd->param1)) == 0) {
-
- /* read all parameters from EEPROM to RAM */
- int read_ret = param_load_default();
-
- if (read_ret == OK) {
- //warnx("[mavlink pm] Loaded EEPROM params in RAM\n");
- mavlink_log_info(mavlink_fd, "OK loading params from");
- mavlink_log_info(mavlink_fd, param_get_default_file());
- result = VEHICLE_CMD_RESULT_ACCEPTED;
-
- } else if (read_ret == 1) {
- mavlink_log_info(mavlink_fd, "OK no changes in");
- mavlink_log_info(mavlink_fd, param_get_default_file());
- result = VEHICLE_CMD_RESULT_ACCEPTED;
-
- } else {
- if (read_ret < -1) {
- mavlink_log_info(mavlink_fd, "ERR loading params from");
- mavlink_log_info(mavlink_fd, param_get_default_file());
-
- } else {
- mavlink_log_info(mavlink_fd, "ERR no param file named");
- mavlink_log_info(mavlink_fd, param_get_default_file());
- }
-
- result = VEHICLE_CMD_RESULT_FAILED;
- }
-
- } else if (((int)(cmd->param1)) == 1) {
-
- /* write all parameters from RAM to EEPROM */
- int write_ret = param_save_default();
-
- if (write_ret == OK) {
- mavlink_log_info(mavlink_fd, "OK saved param file");
- mavlink_log_info(mavlink_fd, param_get_default_file());
- result = VEHICLE_CMD_RESULT_ACCEPTED;
-
- } else {
- if (write_ret < -1) {
- mavlink_log_info(mavlink_fd, "ERR params file does not exit:");
- mavlink_log_info(mavlink_fd, param_get_default_file());
-
- } else {
- mavlink_log_info(mavlink_fd, "ERR writing params to");
- mavlink_log_info(mavlink_fd, param_get_default_file());
- }
-
- result = VEHICLE_CMD_RESULT_FAILED;
- }
-
- } else {
- mavlink_log_info(mavlink_fd, "[pm] refusing unsupp. STOR request");
- result = VEHICLE_CMD_RESULT_UNSUPPORTED;
- }
- }
- }
- break;
-
- default: {
- mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command");
- result = VEHICLE_CMD_RESULT_UNSUPPORTED;
- /* announce command rejection */
- ioctl(buzzer, TONE_SET_ALARM, 4);
- }
- break;
- }
-
- /* supported command handling stop */
- if (result == VEHICLE_CMD_RESULT_FAILED ||
- result == VEHICLE_CMD_RESULT_DENIED ||
- result == VEHICLE_CMD_RESULT_UNSUPPORTED) {
- ioctl(buzzer, TONE_SET_ALARM, 5);
-
- } else if (result == VEHICLE_CMD_RESULT_ACCEPTED) {
- tune_confirm();
- }
-
- /* send any requested ACKs */
- if (cmd->confirmation > 0) {
- /* send acknowledge command */
- // XXX TODO
- }
-
-}
-
-static void *orb_receive_loop(void *arg) //handles status information coming from subsystems (present, enabled, health), these values do not indicate the quality (variance) of the signal
-{
- /* Set thread name */
- prctl(PR_SET_NAME, "commander orb rcv", getpid());
-
- /* Subscribe to command topic */
- int subsys_sub = orb_subscribe(ORB_ID(subsystem_info));
- struct subsystem_info_s info;
-
- struct vehicle_status_s *vstatus = (struct vehicle_status_s *)arg;
-
- while (!thread_should_exit) {
- struct pollfd fds[1] = { { .fd = subsys_sub, .events = POLLIN } };
-
- if (poll(fds, 1, 5000) == 0) {
- /* timeout, but this is no problem, silently ignore */
- } else {
- /* got command */
- orb_copy(ORB_ID(subsystem_info), subsys_sub, &info);
-
- warnx("Subsys changed: %d\n", (int)info.subsystem_type);
-
- /* mark / unmark as present */
- if (info.present) {
- vstatus->onboard_control_sensors_present |= info.subsystem_type;
-
- } else {
- vstatus->onboard_control_sensors_present &= ~info.subsystem_type;
- }
-
- /* mark / unmark as enabled */
- if (info.enabled) {
- vstatus->onboard_control_sensors_enabled |= info.subsystem_type;
-
- } else {
- vstatus->onboard_control_sensors_enabled &= ~info.subsystem_type;
- }
-
- /* mark / unmark as ok */
- if (info.ok) {
- vstatus->onboard_control_sensors_health |= info.subsystem_type;
-
- } else {
- vstatus->onboard_control_sensors_health &= ~info.subsystem_type;
- }
- }
- }
-
- close(subsys_sub);
-
- return NULL;
-}
-
-/*
- * Provides a coarse estimate of remaining battery power.
- *
- * The estimate is very basic and based on decharging voltage curves.
- *
- * @return the estimated remaining capacity in 0..1
- */
-float battery_remaining_estimate_voltage(float voltage);
-
-PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f);
-PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f);
-PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3);
-
-float battery_remaining_estimate_voltage(float voltage)
-{
- float ret = 0;
- static param_t bat_volt_empty;
- static param_t bat_volt_full;
- static param_t bat_n_cells;
- static bool initialized = false;
- static unsigned int counter = 0;
- static float ncells = 3;
- // XXX change cells to int (and param to INT32)
-
- if (!initialized) {
- bat_volt_empty = param_find("BAT_V_EMPTY");
- bat_volt_full = param_find("BAT_V_FULL");
- bat_n_cells = param_find("BAT_N_CELLS");
- initialized = true;
- }
-
- static float chemistry_voltage_empty = 3.2f;
- static float chemistry_voltage_full = 4.05f;
-
- if (counter % 100 == 0) {
- param_get(bat_volt_empty, &chemistry_voltage_empty);
- param_get(bat_volt_full, &chemistry_voltage_full);
- param_get(bat_n_cells, &ncells);
- }
-
- counter++;
-
- ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty));
-
- /* limit to sane values */
- ret = (ret < 0) ? 0 : ret;
- ret = (ret > 1) ? 1 : ret;
- return ret;
-}
-
-static void
-usage(const char *reason)
-{
- if (reason)
- fprintf(stderr, "%s\n", reason);
-
- fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
- exit(1);
-}
-
-/**
- * The daemon app only briefly exists to start
- * the background job. The stack size assigned in the
- * Makefile does only apply to this management task.
- *
- * The actual stack size should be set in the call
- * to task_create().
- */
-int commander_main(int argc, char *argv[])
-{
- if (argc < 1)
- usage("missing command");
-
- if (!strcmp(argv[1], "start")) {
-
- if (thread_running) {
- warnx("commander already running\n");
- /* this is not an error */
- exit(0);
- }
-
- thread_should_exit = false;
- daemon_task = task_spawn_cmd("commander",
- SCHED_DEFAULT,
- SCHED_PRIORITY_MAX - 40,
- 3000,
- commander_thread_main,
- (argv) ? (const char **)&argv[2] : (const char **)NULL);
- exit(0);
- }
-
- if (!strcmp(argv[1], "stop")) {
- thread_should_exit = true;
- exit(0);
- }
-
- if (!strcmp(argv[1], "status")) {
- if (thread_running) {
- warnx("\tcommander is running\n");
-
- } else {
- warnx("\tcommander not started\n");
- }
-
- exit(0);
- }
-
- usage("unrecognized command");
- exit(1);
-}
-
-int commander_thread_main(int argc, char *argv[])
-{
- /* not yet initialized */
- commander_initialized = false;
- bool home_position_set = false;
-
- /* set parameters */
- failsafe_lowlevel_timeout_ms = 0;
- param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms);
-
- param_t _param_sys_type = param_find("MAV_TYPE");
- param_t _param_system_id = param_find("MAV_SYS_ID");
- param_t _param_component_id = param_find("MAV_COMP_ID");
-
- /* welcome user */
- warnx("I am in command now!\n");
-
- /* pthreads for command and subsystem info handling */
- // pthread_t command_handling_thread;
- pthread_t subsystem_info_thread;
-
- /* initialize */
- if (led_init() != 0) {
- warnx("ERROR: Failed to initialize leds\n");
- }
-
- if (buzzer_init() != 0) {
- warnx("ERROR: Failed to initialize buzzer\n");
- }
-
- mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-
- if (mavlink_fd < 0) {
- warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.\n");
- }
-
- /* make sure we are in preflight state */
- memset(¤t_status, 0, sizeof(current_status));
- current_status.state_machine = SYSTEM_STATE_PREFLIGHT;
- current_status.flag_system_armed = false;
- /* neither manual nor offboard control commands have been received */
- current_status.offboard_control_signal_found_once = false;
- current_status.rc_signal_found_once = false;
- /* mark all signals lost as long as they haven't been found */
- current_status.rc_signal_lost = true;
- current_status.offboard_control_signal_lost = true;
- /* allow manual override initially */
- current_status.flag_external_manual_override_ok = true;
- /* flag position info as bad, do not allow auto mode */
- current_status.flag_vector_flight_mode_ok = false;
- /* set battery warning flag */
- current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE;
-
- /* advertise to ORB */
- stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status);
- /* publish current state machine */
- state_machine_publish(stat_pub, ¤t_status, mavlink_fd);
-
- /* home position */
- orb_advert_t home_pub = -1;
- struct home_position_s home;
- memset(&home, 0, sizeof(home));
-
- if (stat_pub < 0) {
- warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n");
- warnx("exiting.");
- exit(ERROR);
- }
-
- mavlink_log_info(mavlink_fd, "system is running");
-
- /* create pthreads */
- pthread_attr_t subsystem_info_attr;
- pthread_attr_init(&subsystem_info_attr);
- pthread_attr_setstacksize(&subsystem_info_attr, 2048);
- pthread_create(&subsystem_info_thread, &subsystem_info_attr, orb_receive_loop, ¤t_status);
-
- /* Start monitoring loop */
- uint16_t counter = 0;
- uint8_t flight_env;
-
- /* Initialize to 0.0V */
- float battery_voltage = 0.0f;
- bool battery_voltage_valid = false;
- bool low_battery_voltage_actions_done = false;
- bool critical_battery_voltage_actions_done = false;
- uint8_t low_voltage_counter = 0;
- uint16_t critical_voltage_counter = 0;
- int16_t mode_switch_rc_value;
- float bat_remain = 1.0f;
-
- uint16_t stick_off_counter = 0;
- uint16_t stick_on_counter = 0;
-
- /* Subscribe to manual control data */
- int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
- struct manual_control_setpoint_s sp_man;
- memset(&sp_man, 0, sizeof(sp_man));
-
- /* Subscribe to offboard control data */
- int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
- struct offboard_control_setpoint_s sp_offboard;
- memset(&sp_offboard, 0, sizeof(sp_offboard));
-
- int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
- struct vehicle_global_position_s global_position;
- memset(&global_position, 0, sizeof(global_position));
- uint64_t last_global_position_time = 0;
-
- int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
- struct vehicle_local_position_s local_position;
- memset(&local_position, 0, sizeof(local_position));
- uint64_t last_local_position_time = 0;
-
- /*
- * The home position is set based on GPS only, to prevent a dependency between
- * position estimator and commander. RAW GPS is more than good enough for a
- * non-flying vehicle.
- */
- int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
- struct vehicle_gps_position_s gps_position;
- memset(&gps_position, 0, sizeof(gps_position));
-
- int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
- struct sensor_combined_s sensors;
- memset(&sensors, 0, sizeof(sensors));
-
- int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
- struct differential_pressure_s diff_pres;
- memset(&diff_pres, 0, sizeof(diff_pres));
- uint64_t last_diff_pres_time = 0;
-
- /* Subscribe to command topic */
- int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
- struct vehicle_command_s cmd;
- memset(&cmd, 0, sizeof(cmd));
-
- /* Subscribe to parameters changed topic */
- int param_changed_sub = orb_subscribe(ORB_ID(parameter_update));
- struct parameter_update_s param_changed;
- memset(¶m_changed, 0, sizeof(param_changed));
-
- /* subscribe to battery topic */
- int battery_sub = orb_subscribe(ORB_ID(battery_status));
- struct battery_status_s battery;
- memset(&battery, 0, sizeof(battery));
- battery.voltage_v = 0.0f;
-
- // uint8_t vehicle_state_previous = current_status.state_machine;
- float voltage_previous = 0.0f;
-
- uint64_t last_idle_time = 0;
-
- /* now initialized */
- commander_initialized = true;
- thread_running = true;
-
- uint64_t start_time = hrt_absolute_time();
- uint64_t failsave_ll_start_time = 0;
-
- enum VEHICLE_MANUAL_SAS_MODE manual_sas_mode;
- bool state_changed = true;
- bool param_init_forced = true;
-
- while (!thread_should_exit) {
-
- /* Get current values */
- bool new_data;
- orb_check(sp_man_sub, &new_data);
-
- if (new_data) {
- orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man);
- }
-
- orb_check(sp_offboard_sub, &new_data);
-
- if (new_data) {
- orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard);
- }
-
- orb_check(sensor_sub, &new_data);
-
- if (new_data) {
- orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors);
- }
-
- orb_check(diff_pres_sub, &new_data);
-
- if (new_data) {
- orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
- last_diff_pres_time = diff_pres.timestamp;
- }
-
- orb_check(cmd_sub, &new_data);
-
- if (new_data) {
- /* got command */
- orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
-
- /* handle it */
- handle_command(stat_pub, ¤t_status, &cmd);
- }
-
- /* update parameters */
- orb_check(param_changed_sub, &new_data);
-
- if (new_data || param_init_forced) {
- param_init_forced = false;
- /* parameters changed */
- orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed);
-
-
- /* update parameters */
- if (!current_status.flag_system_armed) {
- if (param_get(_param_sys_type, &(current_status.system_type)) != OK) {
- warnx("failed setting new system type");
- }
-
- /* disable manual override for all systems that rely on electronic stabilization */
- if (current_status.system_type == VEHICLE_TYPE_QUADROTOR ||
- current_status.system_type == VEHICLE_TYPE_HEXAROTOR ||
- current_status.system_type == VEHICLE_TYPE_OCTOROTOR) {
- current_status.flag_external_manual_override_ok = false;
-
- } else {
- current_status.flag_external_manual_override_ok = true;
- }
-
- /* check and update system / component ID */
- param_get(_param_system_id, &(current_status.system_id));
- param_get(_param_component_id, &(current_status.component_id));
-
- }
- }
-
- /* update global position estimate */
- orb_check(global_position_sub, &new_data);
-
- if (new_data) {
- /* position changed */
- orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);
- last_global_position_time = global_position.timestamp;
- }
-
- /* update local position estimate */
- orb_check(local_position_sub, &new_data);
-
- if (new_data) {
- /* position changed */
- orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
- last_local_position_time = local_position.timestamp;
- }
-
- /* update battery status */
- orb_check(battery_sub, &new_data);
- if (new_data) {
- orb_copy(ORB_ID(battery_status), battery_sub, &battery);
- battery_voltage = battery.voltage_v;
- battery_voltage_valid = true;
-
- /*
- * Only update battery voltage estimate if system has
- * been running for two and a half seconds.
- */
- if (hrt_absolute_time() - start_time > 2500000) {
- bat_remain = battery_remaining_estimate_voltage(battery_voltage);
- }
- }
-
- /* Slow but important 8 Hz checks */
- if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) {
- /* toggle activity (blue) led at 1 Hz in standby, 10 Hz in armed mode */
- if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY ||
- current_status.state_machine == SYSTEM_STATE_AUTO ||
- current_status.state_machine == SYSTEM_STATE_MANUAL)) {
- /* armed, solid */
- led_on(LED_AMBER);
-
- } else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
- /* not armed */
- led_toggle(LED_AMBER);
- }
-
- if (hrt_absolute_time() - gps_position.timestamp_position < 2000000) {
-
- /* toggle GPS (blue) led at 1 Hz if GPS present but no lock, make is solid once locked */
- if ((hrt_absolute_time() - gps_position.timestamp_position < 2000000)
- && (gps_position.fix_type == GPS_FIX_TYPE_3D)) {
- /* GPS lock */
- led_on(LED_BLUE);
-
- } else if ((counter + 4) % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
- /* no GPS lock, but GPS module is aquiring lock */
- led_toggle(LED_BLUE);
- }
-
- } else {
- /* no GPS info, don't light the blue led */
- led_off(LED_BLUE);
- }
-
- /* toggle GPS led at 5 Hz in HIL mode */
- if (current_status.flag_hil_enabled) {
- /* hil enabled */
- led_toggle(LED_BLUE);
-
- } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) {
- /* toggle arming (red) at 5 Hz on low battery or error */
- led_toggle(LED_AMBER);
-
- } else {
- // /* Constant error indication in standby mode without GPS */
- // if (!current_status.gps_valid) {
- // led_on(LED_AMBER);
-
- // } else {
- // led_off(LED_AMBER);
- // }
- }
-
- if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
- /* compute system load */
- uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time;
-
- if (last_idle_time > 0)
- current_status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle
-
- last_idle_time = system_load.tasks[0].total_runtime;
- }
- }
-
- // // XXX Export patterns and threshold to parameters
- /* Trigger audio event for low battery */
- if (bat_remain < 0.1f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 4) == 0)) {
- /* For less than 10%, start be really annoying at 5 Hz */
- ioctl(buzzer, TONE_SET_ALARM, 0);
- ioctl(buzzer, TONE_SET_ALARM, 3);
-
- } else if (bat_remain < 0.1f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 4) == 2)) {
- ioctl(buzzer, TONE_SET_ALARM, 0);
-
- } else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 0)) {
- /* For less than 20%, start be slightly annoying at 1 Hz */
- ioctl(buzzer, TONE_SET_ALARM, 0);
- tune_confirm();
-
- } else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 2)) {
- ioctl(buzzer, TONE_SET_ALARM, 0);
- }
-
- /* Check battery voltage */
- /* write to sys_status */
- if (battery_voltage_valid) {
- current_status.voltage_battery = battery_voltage;
-
- } else {
- current_status.voltage_battery = 0.0f;
- }
-
- /* if battery voltage is getting lower, warn using buzzer, etc. */
- if (battery_voltage_valid && (bat_remain < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS
-
- if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
- low_battery_voltage_actions_done = true;
- mavlink_log_critical(mavlink_fd, "[cmd] WARNING! LOW BATTERY!");
- current_status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING;
- }
-
- low_voltage_counter++;
- }
-
- /* Critical, this is rather an emergency, kill signal to sdlog and change state machine */
- else if (battery_voltage_valid && (bat_remain < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) {
- if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
- critical_battery_voltage_actions_done = true;
- mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!");
- current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT;
- state_machine_emergency(stat_pub, ¤t_status, mavlink_fd);
- }
-
- critical_voltage_counter++;
-
- } else {
- low_voltage_counter = 0;
- critical_voltage_counter = 0;
- }
-
- /* End battery voltage check */
-
-
- /*
- * Check for valid position information.
- *
- * If the system has a valid position source from an onboard
- * position estimator, it is safe to operate it autonomously.
- * The flag_vector_flight_mode_ok flag indicates that a minimum
- * set of position measurements is available.
- */
-
- /* store current state to reason later about a state change */
- bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok;
- bool global_pos_valid = current_status.flag_global_position_valid;
- bool local_pos_valid = current_status.flag_local_position_valid;
- bool airspeed_valid = current_status.flag_airspeed_valid;
-
- /* check for global or local position updates, set a timeout of 2s */
- if (hrt_absolute_time() - last_global_position_time < 2000000) {
- current_status.flag_global_position_valid = true;
- // XXX check for controller status and home position as well
-
- } else {
- current_status.flag_global_position_valid = false;
- }
-
- if (hrt_absolute_time() - last_local_position_time < 2000000) {
- current_status.flag_local_position_valid = true;
- // XXX check for controller status and home position as well
-
- } else {
- current_status.flag_local_position_valid = false;
- }
-
- /* Check for valid airspeed/differential pressure measurements */
- if (hrt_absolute_time() - last_diff_pres_time < 2000000) {
- current_status.flag_airspeed_valid = true;
-
- } else {
- current_status.flag_airspeed_valid = false;
- }
-
- /*
- * Consolidate global position and local position valid flags
- * for vector flight mode.
- */
- if (current_status.flag_local_position_valid ||
- current_status.flag_global_position_valid) {
- current_status.flag_vector_flight_mode_ok = true;
-
- } else {
- current_status.flag_vector_flight_mode_ok = false;
- }
-
- /* consolidate state change, flag as changed if required */
- if (vector_flight_mode_ok != current_status.flag_vector_flight_mode_ok ||
- global_pos_valid != current_status.flag_global_position_valid ||
- local_pos_valid != current_status.flag_local_position_valid ||
- airspeed_valid != current_status.flag_airspeed_valid) {
- state_changed = true;
- }
-
- /*
- * Mark the position of the first position lock as return to launch (RTL)
- * position. The MAV will return here on command or emergency.
- *
- * Conditions:
- *
- * 1) The system aquired position lock just now
- * 2) The system has not aquired position lock before
- * 3) The system is not armed (on the ground)
- */
- if (!current_status.flag_valid_launch_position &&
- !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok &&
- !current_status.flag_system_armed) {
- /* first time a valid position, store it and emit it */
-
- // XXX implement storage and publication of RTL position
- current_status.flag_valid_launch_position = true;
- current_status.flag_auto_flight_mode_ok = true;
- state_changed = true;
- }
-
- if (orb_check(ORB_ID(vehicle_gps_position), &new_data)) {
-
- orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position);
-
- /* check for first, long-term and valid GPS lock -> set home position */
- float hdop_m = gps_position.eph_m;
- float vdop_m = gps_position.epv_m;
-
- /* check if gps fix is ok */
- // XXX magic number
- float hdop_threshold_m = 4.0f;
- float vdop_threshold_m = 8.0f;
-
- /*
- * If horizontal dilution of precision (hdop / eph)
- * and vertical diluation of precision (vdop / epv)
- * are below a certain threshold (e.g. 4 m), AND
- * home position is not yet set AND the last GPS
- * GPS measurement is not older than two seconds AND
- * the system is currently not armed, set home
- * position to the current position.
- */
-
- if (gps_position.fix_type == GPS_FIX_TYPE_3D
- && (hdop_m < hdop_threshold_m)
- && (vdop_m < vdop_threshold_m)
- && !home_position_set
- && (hrt_absolute_time() - gps_position.timestamp_position < 2000000)
- && !current_status.flag_system_armed) {
- warnx("setting home position");
-
- /* copy position data to uORB home message, store it locally as well */
- home.lat = gps_position.lat;
- home.lon = gps_position.lon;
- home.alt = gps_position.alt;
-
- home.eph_m = gps_position.eph_m;
- home.epv_m = gps_position.epv_m;
-
- home.s_variance_m_s = gps_position.s_variance_m_s;
- home.p_variance_m = gps_position.p_variance_m;
-
- /* announce new home position */
- if (home_pub > 0) {
- orb_publish(ORB_ID(home_position), home_pub, &home);
- } else {
- home_pub = orb_advertise(ORB_ID(home_position), &home);
- }
-
- /* mark home position as set */
- home_position_set = true;
- tune_confirm();
- }
- }
-
- /* ignore RC signals if in offboard control mode */
- if (!current_status.offboard_control_signal_found_once && sp_man.timestamp != 0) {
- /* Start RC state check */
-
- if ((hrt_absolute_time() - sp_man.timestamp) < 100000) {
-
- // /*
- // * Check if manual control modes have to be switched
- // */
- // if (!isfinite(sp_man.manual_mode_switch)) {
- // warnx("man mode sw not finite\n");
-
- // /* this switch is not properly mapped, set default */
- // if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
- // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
- // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) {
-
- // /* assuming a rotary wing, fall back to SAS */
- // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
- // current_status.flag_control_attitude_enabled = true;
- // current_status.flag_control_rates_enabled = true;
- // } else {
-
- // /* assuming a fixed wing, fall back to direct pass-through */
- // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
- // current_status.flag_control_attitude_enabled = false;
- // current_status.flag_control_rates_enabled = false;
- // }
-
- // } else if (sp_man.manual_mode_switch < -STICK_ON_OFF_LIMIT) {
-
- // /* bottom stick position, set direct mode for vehicles supporting it */
- // if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
- // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
- // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) {
-
- // /* assuming a rotary wing, fall back to SAS */
- // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
- // current_status.flag_control_attitude_enabled = true;
- // current_status.flag_control_rates_enabled = true;
- // } else {
-
- // /* assuming a fixed wing, set to direct pass-through as requested */
- // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
- // current_status.flag_control_attitude_enabled = false;
- // current_status.flag_control_rates_enabled = false;
- // }
-
- // } else if (sp_man.manual_mode_switch > STICK_ON_OFF_LIMIT) {
-
- // /* top stick position, set SAS for all vehicle types */
- // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
- // current_status.flag_control_attitude_enabled = true;
- // current_status.flag_control_rates_enabled = true;
-
- // } else {
- // /* center stick position, set rate control */
- // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_RATES;
- // current_status.flag_control_attitude_enabled = false;
- // current_status.flag_control_rates_enabled = true;
- // }
-
- // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode);
-
- /*
- * Check if manual stability control modes have to be switched
- */
- if (!isfinite(sp_man.manual_sas_switch)) {
-
- /* this switch is not properly mapped, set default */
- current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS;
-
- } else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) {
-
- /* bottom stick position, set default */
- /* this MUST be mapped to extremal position to switch easy in case of emergency */
- current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS;
-
- } else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) {
-
- /* top stick position */
- current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_SIMPLE;
-
- } else {
- /* center stick position, set altitude hold */
- current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE;
- }
-
- if (current_status.manual_sas_mode != manual_sas_mode) {
- /* publish SAS mode changes immediately */
- manual_sas_mode = current_status.manual_sas_mode;
- state_changed = true;
- }
-
- /*
- * Check if left stick is in lower left position --> switch to standby state.
- * Do this only for multirotors, not for fixed wing aircraft.
- */
- if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
- (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
- (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)
- ) &&
- current_status.flag_control_manual_enabled &&
- current_status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS &&
- sp_man.yaw < -STICK_ON_OFF_LIMIT &&
- sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
- if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
- update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd);
- stick_on_counter = 0;
-
- } else {
- stick_off_counter++;
- stick_on_counter = 0;
- }
- }
-
- /* check if left stick is in lower right position --> arm */
- if (current_status.flag_control_manual_enabled &&
- current_status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS &&
- sp_man.yaw > STICK_ON_OFF_LIMIT &&
- sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
- if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
- update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd);
- stick_on_counter = 0;
-
- } else {
- stick_on_counter++;
- stick_off_counter = 0;
- }
- }
-
- /* check manual override switch - switch to manual or auto mode */
- if (sp_man.manual_override_switch > STICK_ON_OFF_LIMIT) {
- /* enable manual override */
- update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd);
-
- } else if (sp_man.manual_override_switch < -STICK_ON_OFF_LIMIT) {
- // /* check auto mode switch for correct mode */
- // if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) {
- // /* enable guided mode */
- // update_state_machine_mode_guided(stat_pub, ¤t_status, mavlink_fd);
-
- // } else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) {
- // XXX hardcode to auto for now
- update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd);
-
- // }
-
- } else {
- /* center stick position, set SAS for all vehicle types */
- update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd);
- }
-
- /* handle the case where RC signal was regained */
- if (!current_status.rc_signal_found_once) {
- current_status.rc_signal_found_once = true;
- mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME.");
-
- } else {
- if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!");
- }
-
- current_status.rc_signal_cutting_off = false;
- current_status.rc_signal_lost = false;
- current_status.rc_signal_lost_interval = 0;
-
- } else {
- static uint64_t last_print_time = 0;
-
- /* print error message for first RC glitch and then every 5 s / 5000 ms) */
- if (!current_status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) {
- /* only complain if the offboard control is NOT active */
- current_status.rc_signal_cutting_off = true;
- mavlink_log_critical(mavlink_fd, "CRITICAL - NO REMOTE SIGNAL!");
- last_print_time = hrt_absolute_time();
- }
-
- /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */
- current_status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp;
-
- /* if the RC signal is gone for a full second, consider it lost */
- if (current_status.rc_signal_lost_interval > 1000000) {
- current_status.rc_signal_lost = true;
- current_status.failsave_lowlevel = true;
- state_changed = true;
- }
-
- // if (hrt_absolute_time() - current_status.failsave_ll_start_time > failsafe_lowlevel_timeout_ms*1000) {
- // publish_armed_status(¤t_status);
- // }
- }
- }
-
-
-
-
- /* End mode switch */
-
- /* END RC state check */
-
-
- /* State machine update for offboard control */
- if (!current_status.rc_signal_found_once && sp_offboard.timestamp != 0) {
- if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) {
-
- /* decide about attitude control flag, enable in att/pos/vel */
- bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE ||
- sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY ||
- sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION);
-
- /* decide about rate control flag, enable it always XXX (for now) */
- bool rates_ctrl_enabled = true;
-
- /* set up control mode */
- if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) {
- current_status.flag_control_attitude_enabled = attitude_ctrl_enabled;
- state_changed = true;
- }
-
- if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) {
- current_status.flag_control_rates_enabled = rates_ctrl_enabled;
- state_changed = true;
- }
-
- /* handle the case where offboard control signal was regained */
- if (!current_status.offboard_control_signal_found_once) {
- current_status.offboard_control_signal_found_once = true;
- /* enable offboard control, disable manual input */
- current_status.flag_control_manual_enabled = false;
- current_status.flag_control_offboard_enabled = true;
- state_changed = true;
- tune_confirm();
-
- mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST");
-
- } else {
- if (current_status.offboard_control_signal_lost) {
- mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL");
- state_changed = true;
- tune_confirm();
- }
- }
-
- current_status.offboard_control_signal_weak = false;
- current_status.offboard_control_signal_lost = false;
- current_status.offboard_control_signal_lost_interval = 0;
-
- /* arm / disarm on request */
- if (sp_offboard.armed && !current_status.flag_system_armed) {
- update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd);
- /* switch to stabilized mode = takeoff */
- update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd);
-
- } else if (!sp_offboard.armed && current_status.flag_system_armed) {
- update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd);
- }
-
- } else {
- static uint64_t last_print_time = 0;
-
- /* print error message for first RC glitch and then every 5 s / 5000 ms) */
- if (!current_status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) {
- current_status.offboard_control_signal_weak = true;
- mavlink_log_critical(mavlink_fd, "CRIT:NO OFFBOARD CONTROL!");
- last_print_time = hrt_absolute_time();
- }
-
- /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */
- current_status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp;
-
- /* if the signal is gone for 0.1 seconds, consider it lost */
- if (current_status.offboard_control_signal_lost_interval > 100000) {
- current_status.offboard_control_signal_lost = true;
- current_status.failsave_lowlevel_start_time = hrt_absolute_time();
- tune_confirm();
-
- /* kill motors after timeout */
- if (hrt_absolute_time() - current_status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) {
- current_status.failsave_lowlevel = true;
- state_changed = true;
- }
- }
- }
- }
-
-
- current_status.counter++;
- current_status.timestamp = hrt_absolute_time();
-
-
- /* If full run came back clean, transition to standby */
- if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT &&
- current_status.flag_preflight_gyro_calibration == false &&
- current_status.flag_preflight_mag_calibration == false &&
- current_status.flag_preflight_accel_calibration == false) {
- /* All ok, no calibration going on, go to standby */
- do_state_update(stat_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY);
- }
-
- /* publish at least with 1 Hz */
- if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) {
- publish_armed_status(¤t_status);
- orb_publish(ORB_ID(vehicle_status), stat_pub, ¤t_status);
- state_changed = false;
- }
-
- /* Store old modes to detect and act on state transitions */
- voltage_previous = current_status.voltage_battery;
-
- fflush(stdout);
- counter++;
- usleep(COMMANDER_MONITORING_INTERVAL);
- }
-
- /* wait for threads to complete */
- // pthread_join(command_handling_thread, NULL);
- pthread_join(subsystem_info_thread, NULL);
-
- /* close fds */
- led_deinit();
- buzzer_deinit();
- close(sp_man_sub);
- close(sp_offboard_sub);
- close(global_position_sub);
- close(sensor_sub);
- close(cmd_sub);
-
- warnx("exiting..\n");
- fflush(stdout);
-
- thread_running = false;
-
- return 0;
-}
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a548f943e24987ea4df3bb87671189f82e77816d
--- /dev/null
+++ b/src/modules/commander/commander.cpp
@@ -0,0 +1,1793 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ * Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
+ * Lorenz Meier <lm@inf.ethz.ch>
+ * Thomas Gubler <thomasgubler@student.ethz.ch>
+ * Julian Oes <joes@student.ethz.ch>
+ *
+ * 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.cpp
+ * Main system state machine implementation.
+ *
+ */
+
+#include <nuttx/config.h>
+#include <pthread.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+#include <sys/prctl.h>
+#include <sys/stat.h>
+#include <string.h>
+#include <math.h>
+#include <poll.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/offboard_control_setpoint.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/subsystem_info.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/differential_pressure.h>
+#include <uORB/topics/safety.h>
+
+#include <drivers/drv_led.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_tone_alarm.h>
+
+#include <mavlink/mavlink_log.h>
+#include <systemlib/param/param.h>
+#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"
+#include "state_machine_helper.h"
+#include "calibration_routines.h"
+#include "accelerometer_calibration.h"
+#include "gyro_calibration.h"
+#include "mag_calibration.h"
+#include "baro_calibration.h"
+#include "rc_calibration.h"
+#include "airspeed_calibration.h"
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+extern struct system_load_s system_load;
+
+#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f
+#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f
+
+/* Decouple update interval and hysteris counters, all depends on intervals */
+#define COMMANDER_MONITORING_INTERVAL 50000
+#define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f))
+#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
+#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
+
+#define STICK_ON_OFF_LIMIT 0.75f
+#define STICK_THRUST_RANGE 1.0f
+#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
+#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
+
+#define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */
+#define RC_TIMEOUT 100000
+#define DIFFPRESS_TIMEOUT 2000000
+
+#define PRINT_INTERVAL 5000000
+#define PRINT_MODE_REJECT_INTERVAL 2000000
+
+enum MAV_MODE_FLAG {
+ MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */
+ MAV_MODE_FLAG_TEST_ENABLED = 2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */
+ MAV_MODE_FLAG_AUTO_ENABLED = 4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */
+ MAV_MODE_FLAG_GUIDED_ENABLED = 8, /* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */
+ MAV_MODE_FLAG_STABILIZE_ENABLED = 16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */
+ MAV_MODE_FLAG_HIL_ENABLED = 32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */
+ MAV_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, /* 0b01000000 remote control input is enabled. | */
+ MAV_MODE_FLAG_SAFETY_ARMED = 128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. | */
+ MAV_MODE_FLAG_ENUM_END = 129, /* | */
+};
+
+/* Mavlink file descriptors */
+static int mavlink_fd;
+
+/* flags */
+static bool commander_initialized = false;
+static bool thread_should_exit = false; /**< daemon exit flag */
+static bool thread_running = false; /**< daemon status flag */
+static int daemon_task; /**< Handle of daemon task / thread */
+
+static unsigned int leds_counter;
+/* To remember when last notification was sent */
+static uint64_t last_print_mode_reject_time = 0;
+/* if connected via USB */
+static bool on_usb_power = false;
+
+
+/* tasks waiting for low prio thread */
+typedef enum {
+ LOW_PRIO_TASK_NONE = 0,
+ LOW_PRIO_TASK_PARAM_SAVE,
+ LOW_PRIO_TASK_PARAM_LOAD,
+ LOW_PRIO_TASK_GYRO_CALIBRATION,
+ LOW_PRIO_TASK_MAG_CALIBRATION,
+ LOW_PRIO_TASK_ALTITUDE_CALIBRATION,
+ LOW_PRIO_TASK_RC_CALIBRATION,
+ LOW_PRIO_TASK_ACCEL_CALIBRATION,
+ LOW_PRIO_TASK_AIRSPEED_CALIBRATION
+} low_prio_task_t;
+
+static low_prio_task_t low_prio_task = LOW_PRIO_TASK_NONE;
+
+/**
+ * The daemon app only briefly exists to start
+ * the background job. The stack size assigned in the
+ * Makefile does only apply to this management task.
+ *
+ * The actual stack size should be set in the call
+ * to task_create().
+ *
+ * @ingroup apps
+ */
+extern "C" __EXPORT int commander_main(int argc, char *argv[]);
+
+/**
+ * Print the correct usage.
+ */
+void usage(const char *reason);
+
+/**
+ * React to commands that are sent e.g. from the mavlink module.
+ */
+void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed);
+
+/**
+ * Mainloop of commander.
+ */
+int commander_thread_main(int argc, char *argv[]);
+
+void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed);
+
+void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
+
+void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status);
+
+transition_result_t check_main_state_machine(struct vehicle_status_s *current_status);
+
+void print_reject_mode(const char *msg);
+
+void print_reject_arm(const char *msg);
+
+void print_status();
+
+transition_result_t check_navigation_state_machine(struct vehicle_status_s *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.
+ */
+void *commander_low_prio_loop(void *arg);
+
+void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result);
+
+
+int commander_main(int argc, char *argv[])
+{
+ if (argc < 1)
+ usage("missing command");
+
+ if (!strcmp(argv[1], "start")) {
+
+ if (thread_running) {
+ warnx("commander already running\n");
+ /* this is not an error */
+ exit(0);
+ }
+
+ thread_should_exit = false;
+ daemon_task = task_spawn_cmd("commander",
+ SCHED_DEFAULT,
+ SCHED_PRIORITY_MAX - 40,
+ 3000,
+ commander_thread_main,
+ (argv) ? (const char **)&argv[2] : (const char **)NULL);
+ exit(0);
+ }
+
+ if (!strcmp(argv[1], "stop")) {
+ thread_should_exit = true;
+ exit(0);
+ }
+
+ if (!strcmp(argv[1], "status")) {
+ if (thread_running) {
+ warnx("\tcommander is running\n");
+ print_status();
+
+ } else {
+ warnx("\tcommander not started\n");
+ }
+
+ exit(0);
+ }
+
+ usage("unrecognized command");
+ exit(1);
+}
+
+void usage(const char *reason)
+{
+ if (reason)
+ fprintf(stderr, "%s\n", reason);
+
+ fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
+ exit(1);
+}
+
+void print_status()
+{
+ warnx("usb powered: %s", (on_usb_power) ? "yes" : "no");
+
+ /* read all relevant states */
+ int state_sub = orb_subscribe(ORB_ID(vehicle_status));
+ struct vehicle_status_s state;
+ orb_copy(ORB_ID(vehicle_status), state_sub, &state);
+
+ const char *armed_str;
+
+ switch (state.arming_state) {
+ case ARMING_STATE_INIT:
+ armed_str = "INIT";
+ break;
+
+ case ARMING_STATE_STANDBY:
+ armed_str = "STANDBY";
+ break;
+
+ case ARMING_STATE_ARMED:
+ armed_str = "ARMED";
+ break;
+
+ case ARMING_STATE_ARMED_ERROR:
+ armed_str = "ARMED_ERROR";
+ break;
+
+ case ARMING_STATE_STANDBY_ERROR:
+ armed_str = "STANDBY_ERROR";
+ break;
+
+ case ARMING_STATE_REBOOT:
+ armed_str = "REBOOT";
+ break;
+
+ case ARMING_STATE_IN_AIR_RESTORE:
+ armed_str = "IN_AIR_RESTORE";
+ break;
+
+ default:
+ armed_str = "ERR: UNKNOWN STATE";
+ break;
+ }
+
+
+ warnx("arming: %s", armed_str);
+}
+
+void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
+{
+ /* result of the command */
+ uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED;
+
+ /* only handle high-priority commands here */
+
+ /* request to set different system mode */
+ switch (cmd->command) {
+ case VEHICLE_CMD_DO_SET_MODE: {
+ uint8_t base_mode = (uint8_t) cmd->param1;
+ 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_main_mode);
+ /* set arming state */
+ transition_result_t arming_res = TRANSITION_NOT_CHANGED;
+
+ if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
+ arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
+
+ if (arming_res == TRANSITION_CHANGED) {
+ mavlink_log_info(mavlink_fd, "[cmd] ARMED by command");
+ }
+
+ } else {
+ if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) {
+ arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
+ arming_res = arming_state_transition(status, safety, new_arming_state, armed);
+
+ if (arming_res == TRANSITION_CHANGED) {
+ mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command");
+ }
+
+ } else {
+ arming_res = TRANSITION_NOT_CHANGED;
+ }
+ }
+
+ /* set main state */
+ transition_result_t main_res = TRANSITION_DENIED;
+
+ if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
+ /* use autopilot-specific mode */
+ if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
+ /* MANUAL */
+ main_res = main_state_transition(status, MAIN_STATE_MANUAL);
+
+ } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) {
+ /* SEATBELT */
+ main_res = main_state_transition(status, MAIN_STATE_SEATBELT);
+
+ } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) {
+ /* EASY */
+ main_res = main_state_transition(status, MAIN_STATE_EASY);
+
+ } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
+ /* AUTO */
+ main_res = main_state_transition(status, MAIN_STATE_AUTO);
+ }
+
+ } else {
+ /* use base mode */
+ if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
+ /* AUTO */
+ main_res = main_state_transition(status, MAIN_STATE_AUTO);
+
+ } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
+ if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
+ /* EASY */
+ main_res = main_state_transition(status, MAIN_STATE_EASY);
+
+ } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
+ /* MANUAL */
+ main_res = main_state_transition(status, MAIN_STATE_MANUAL);
+ }
+ }
+ }
+
+ if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) {
+ result = VEHICLE_CMD_RESULT_ACCEPTED;
+
+ } else {
+ result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+ }
+
+ break;
+ }
+
+ case VEHICLE_CMD_NAV_TAKEOFF: {
+ if (armed->armed) {
+ transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode);
+
+ if (nav_res == TRANSITION_CHANGED) {
+ mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command");
+ }
+
+ if (nav_res != TRANSITION_DENIED) {
+ result = VEHICLE_CMD_RESULT_ACCEPTED;
+
+ } else {
+ result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+ }
+
+ } else {
+ /* reject TAKEOFF not armed */
+ result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+ }
+
+ break;
+ }
+
+ case VEHICLE_CMD_COMPONENT_ARM_DISARM:
+ // XXX implement later
+ result = VEHICLE_CMD_RESULT_DENIED;
+ break;
+
+ default:
+ break;
+ }
+
+ /* supported command handling stop */
+ if (result == VEHICLE_CMD_RESULT_ACCEPTED) {
+ tune_positive();
+
+ } else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) {
+ /* we do not care in the high prio loop about commands we don't know */
+ } else {
+ tune_negative();
+
+ if (result == VEHICLE_CMD_RESULT_DENIED) {
+ mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd->command);
+
+ } else if (result == VEHICLE_CMD_RESULT_FAILED) {
+ mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd->command);
+
+ } else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) {
+ mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd->command);
+
+ }
+ }
+
+ /* send any requested ACKs */
+ if (cmd->confirmation > 0 && result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
+ /* send acknowledge command */
+ // XXX TODO
+ }
+
+}
+
+static struct vehicle_status_s status;
+
+/* armed topic */
+static struct actuator_armed_s armed;
+
+static struct safety_s safety;
+
+int commander_thread_main(int argc, char *argv[])
+{
+ /* not yet initialized */
+ commander_initialized = false;
+ bool home_position_set = false;
+
+ bool battery_tune_played = false;
+ bool arm_tune_played = false;
+
+ /* set parameters */
+ param_t _param_sys_type = param_find("MAV_TYPE");
+ param_t _param_system_id = param_find("MAV_SYS_ID");
+ param_t _param_component_id = param_find("MAV_COMP_ID");
+
+ /* welcome user */
+ warnx("[commander] starting");
+
+ /* pthread for slow low prio thread */
+ pthread_t commander_low_prio_thread;
+
+ /* initialize */
+ if (led_init() != 0) {
+ warnx("ERROR: Failed to initialize leds");
+ }
+
+ if (buzzer_init() != OK) {
+ warnx("ERROR: Failed to initialize buzzer");
+ }
+
+ mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+
+ if (mavlink_fd < 0) {
+ /* 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 */
+ orb_advert_t status_pub;
+ /* make sure we are in preflight state */
+ memset(&status, 0, sizeof(status));
+ status.condition_landed = true; // initialize to safe value
+
+ /* armed topic */
+ orb_advert_t armed_pub;
+ /* Initialize armed with all false */
+ memset(&armed, 0, sizeof(armed));
+
+ /* flags for control apps */
+ struct vehicle_control_mode_s control_mode;
+ orb_advert_t control_mode_pub;
+
+ /* Initialize all flags to false */
+ memset(&control_mode, 0, sizeof(control_mode));
+
+ status.main_state = MAIN_STATE_MANUAL;
+ status.navigation_state = NAVIGATION_STATE_DIRECT;
+ status.arming_state = ARMING_STATE_INIT;
+ status.hil_state = HIL_STATE_OFF;
+
+ /* neither manual nor offboard control commands have been received */
+ status.offboard_control_signal_found_once = false;
+ status.rc_signal_found_once = false;
+
+ /* mark all signals lost as long as they haven't been found */
+ status.rc_signal_lost = true;
+ status.offboard_control_signal_lost = true;
+
+ /* allow manual override initially */
+ control_mode.flag_external_manual_override_ok = true;
+
+ /* set battery warning flag */
+ status.battery_warning = VEHICLE_BATTERY_WARNING_NONE;
+ status.condition_battery_voltage_valid = false;
+
+ // XXX for now just set sensors as initialized
+ status.condition_system_sensors_initialized = true;
+
+ // XXX just disable offboard control for now
+ control_mode.flag_control_offboard_enabled = false;
+
+ /* advertise to ORB */
+ status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
+ /* publish current state machine */
+
+ /* publish initial state */
+ status.counter++;
+ status.timestamp = hrt_absolute_time();
+ orb_publish(ORB_ID(vehicle_status), status_pub, &status);
+
+ armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
+
+ control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode);
+
+ /* home position */
+ orb_advert_t home_pub = -1;
+ struct home_position_s home;
+ memset(&home, 0, sizeof(home));
+
+ if (status_pub < 0) {
+ warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n");
+ warnx("exiting.");
+ exit(ERROR);
+ }
+
+ mavlink_log_info(mavlink_fd, "[cmd] started");
+
+ pthread_attr_t commander_low_prio_attr;
+ pthread_attr_init(&commander_low_prio_attr);
+ pthread_attr_setstacksize(&commander_low_prio_attr, 2048);
+
+ struct sched_param param;
+ /* low priority */
+ param.sched_priority = SCHED_PRIORITY_DEFAULT - 50;
+ (void)pthread_attr_setschedparam(&commander_low_prio_attr, ¶m);
+ pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL);
+
+ /* Start monitoring loop */
+ unsigned counter = 0;
+ unsigned low_voltage_counter = 0;
+ unsigned critical_voltage_counter = 0;
+ unsigned stick_off_counter = 0;
+ unsigned stick_on_counter = 0;
+
+ bool low_battery_voltage_actions_done = false;
+ bool critical_battery_voltage_actions_done = false;
+
+ uint64_t last_idle_time = 0;
+
+ uint64_t start_time = 0;
+
+ bool status_changed = true;
+ bool param_init_forced = true;
+
+ 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));
+ safety.safety_switch_available = false;
+ safety.safety_off = false;
+
+ /* Subscribe to manual control data */
+ int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+ struct manual_control_setpoint_s sp_man;
+ memset(&sp_man, 0, sizeof(sp_man));
+
+ /* Subscribe to offboard control data */
+ int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
+ struct offboard_control_setpoint_s sp_offboard;
+ memset(&sp_offboard, 0, sizeof(sp_offboard));
+
+ /* Subscribe to global position */
+ int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+ struct vehicle_global_position_s global_position;
+ memset(&global_position, 0, sizeof(global_position));
+
+ /* Subscribe to local position data */
+ int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
+ struct vehicle_local_position_s local_position;
+ memset(&local_position, 0, sizeof(local_position));
+
+ /*
+ * The home position is set based on GPS only, to prevent a dependency between
+ * position estimator and commander. RAW GPS is more than good enough for a
+ * non-flying vehicle.
+ */
+
+ /* Subscribe to GPS topic */
+ int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+ struct vehicle_gps_position_s gps_position;
+ memset(&gps_position, 0, sizeof(gps_position));
+
+ /* Subscribe to sensor topic */
+ int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+ struct sensor_combined_s sensors;
+ memset(&sensors, 0, sizeof(sensors));
+
+ /* Subscribe to differential pressure topic */
+ int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
+ struct differential_pressure_s diff_pres;
+ memset(&diff_pres, 0, sizeof(diff_pres));
+
+ /* Subscribe to command topic */
+ int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
+ struct vehicle_command_s cmd;
+ memset(&cmd, 0, sizeof(cmd));
+
+ /* Subscribe to parameters changed topic */
+ int param_changed_sub = orb_subscribe(ORB_ID(parameter_update));
+ struct parameter_update_s param_changed;
+ memset(¶m_changed, 0, sizeof(param_changed));
+
+ /* Subscribe to battery topic */
+ int battery_sub = orb_subscribe(ORB_ID(battery_status));
+ struct battery_status_s battery;
+ memset(&battery, 0, sizeof(battery));
+ battery.voltage_v = 0.0f;
+
+ /* Subscribe to subsystem info topic */
+ int subsys_sub = orb_subscribe(ORB_ID(subsystem_info));
+ struct subsystem_info_s info;
+ memset(&info, 0, sizeof(info));
+
+ /* now initialized */
+ commander_initialized = true;
+ thread_running = true;
+
+ start_time = hrt_absolute_time();
+
+ while (!thread_should_exit) {
+
+ /* update parameters */
+ orb_check(param_changed_sub, &updated);
+
+ if (updated || param_init_forced) {
+ param_init_forced = false;
+ /* parameters changed */
+ orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed);
+
+ /* update parameters */
+ if (!armed.armed) {
+ if (param_get(_param_sys_type, &(status.system_type)) != OK) {
+ warnx("failed getting new system type");
+ }
+
+ /* disable manual override for all systems that rely on electronic stabilization */
+ if (status.system_type == VEHICLE_TYPE_COAXIAL ||
+ status.system_type == VEHICLE_TYPE_HELICOPTER ||
+ status.system_type == VEHICLE_TYPE_TRICOPTER ||
+ status.system_type == VEHICLE_TYPE_QUADROTOR ||
+ status.system_type == VEHICLE_TYPE_HEXAROTOR ||
+ status.system_type == VEHICLE_TYPE_OCTOROTOR) {
+ control_mode.flag_external_manual_override_ok = false;
+ status.is_rotary_wing = true;
+
+ } else {
+ control_mode.flag_external_manual_override_ok = true;
+ status.is_rotary_wing = false;
+ }
+
+ /* check and update system / component ID */
+ 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());
+ }
+ }
+
+ orb_check(sp_man_sub, &updated);
+
+ if (updated) {
+ orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man);
+ }
+
+ orb_check(sp_offboard_sub, &updated);
+
+ if (updated) {
+ orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard);
+ }
+
+ orb_check(sensor_sub, &updated);
+
+ if (updated) {
+ orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors);
+ }
+
+ orb_check(diff_pres_sub, &updated);
+
+ if (updated) {
+ orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
+ }
+
+ check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status.condition_airspeed_valid), &status_changed);
+
+ orb_check(cmd_sub, &updated);
+
+ if (updated) {
+ /* got command */
+ orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
+
+ /* handle it */
+ handle_command(&status, &safety, &control_mode, &cmd, &armed);
+ }
+
+ /* update safety topic */
+ orb_check(safety_sub, &updated);
+
+ if (updated) {
+ orb_copy(ORB_ID(safety), safety_sub, &safety);
+ }
+
+ /* update global position estimate */
+ orb_check(global_position_sub, &updated);
+
+ if (updated) {
+ /* position changed */
+ orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);
+ }
+
+ /* update condition_global_position_valid */
+ check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.valid, &(status.condition_global_position_valid), &status_changed);
+
+ /* update local position estimate */
+ orb_check(local_position_sub, &updated);
+
+ if (updated) {
+ /* position changed */
+ orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
+ }
+
+ /* update condition_local_position_valid and condition_local_altitude_valid */
+ check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed);
+ check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed);
+
+ if (status.condition_local_altitude_valid) {
+ if (status.condition_landed != local_position.landed) {
+ status.condition_landed = local_position.landed;
+ status_changed = true;
+
+ if (status.condition_landed) {
+ mavlink_log_critical(mavlink_fd, "[cmd] LANDED");
+
+ } else {
+ mavlink_log_critical(mavlink_fd, "[cmd] IN AIR");
+ }
+ }
+ }
+
+ /* update battery status */
+ orb_check(battery_sub, &updated);
+
+ if (updated) {
+ orb_copy(ORB_ID(battery_status), battery_sub, &battery);
+
+ // warnx("bat v: %2.2f", battery.voltage_v);
+
+ /* only consider battery voltage if system has been running 2s and battery voltage is not 0 */
+ if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 0.001f) {
+ status.battery_voltage = battery.voltage_v;
+ status.condition_battery_voltage_valid = true;
+ status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage);
+ }
+ }
+
+ /* update subsystem */
+ orb_check(subsys_sub, &updated);
+
+ if (updated) {
+ orb_copy(ORB_ID(subsystem_info), subsys_sub, &info);
+
+ warnx("subsystem changed: %d\n", (int)info.subsystem_type);
+
+ /* mark / unmark as present */
+ if (info.present) {
+ status.onboard_control_sensors_present |= info.subsystem_type;
+
+ } else {
+ status.onboard_control_sensors_present &= ~info.subsystem_type;
+ }
+
+ /* mark / unmark as enabled */
+ if (info.enabled) {
+ status.onboard_control_sensors_enabled |= info.subsystem_type;
+
+ } else {
+ status.onboard_control_sensors_enabled &= ~info.subsystem_type;
+ }
+
+ /* mark / unmark as ok */
+ if (info.ok) {
+ status.onboard_control_sensors_health |= info.subsystem_type;
+
+ } else {
+ status.onboard_control_sensors_health &= ~info.subsystem_type;
+ }
+
+ status_changed = true;
+ }
+
+ if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
+ /* compute system load */
+ uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time;
+
+ if (last_idle_time > 0)
+ status.load = 1.0f - ((float)interval_runtime / 1e6f); //system load is time spent in non-idle
+
+ last_idle_time = system_load.tasks[0].total_runtime;
+
+ /* check if board is connected via USB */
+ struct stat statbuf;
+ //on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0);
+ }
+
+ // XXX remove later
+ //warnx("bat remaining: %2.2f", status.battery_remaining);
+
+ /* if battery voltage is getting lower, warn using buzzer, etc. */
+ if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) {
+ //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS
+ if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
+ low_battery_voltage_actions_done = true;
+ mavlink_log_critical(mavlink_fd, "[cmd] WARNING: LOW BATTERY");
+ status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING;
+ status_changed = true;
+ battery_tune_played = false;
+ }
+
+ low_voltage_counter++;
+
+ } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
+ /* critical battery voltage, this is rather an emergency, change state machine */
+ if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
+ critical_battery_voltage_actions_done = true;
+ mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY");
+ status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT;
+ battery_tune_played = false;
+
+ if (armed.armed) {
+ // XXX not sure what should happen when voltage is low in flight
+ //arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
+ } else {
+ // XXX should we still allow to arm with critical battery?
+ //arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
+ }
+
+ status_changed = true;
+ }
+
+ critical_voltage_counter++;
+
+ } else {
+ low_voltage_counter = 0;
+ critical_voltage_counter = 0;
+ }
+
+ /* End battery voltage check */
+
+ /* If in INIT state, try to proceed to STANDBY state */
+ if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
+ // XXX check for sensors
+ arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
+
+ } else {
+ // XXX: Add emergency stuff if sensors are lost
+ }
+
+
+ /*
+ * Check for valid position information.
+ *
+ * If the system has a valid position source from an onboard
+ * position estimator, it is safe to operate it autonomously.
+ * The flag_vector_flight_mode_ok flag indicates that a minimum
+ * set of position measurements is available.
+ */
+
+ orb_check(gps_sub, &updated);
+
+ if (updated) {
+ orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position);
+ /* check if GPS fix is ok */
+ float hdop_threshold_m = 4.0f;
+ float vdop_threshold_m = 8.0f;
+
+ /*
+ * If horizontal dilution of precision (hdop / eph)
+ * and vertical diluation of precision (vdop / epv)
+ * are below a certain threshold (e.g. 4 m), AND
+ * home position is not yet set AND the last GPS
+ * GPS measurement is not older than two seconds AND
+ * the system is currently not armed, set home
+ * position to the current position.
+ */
+
+ if (!home_position_set && gps_position.fix_type >= 3 &&
+ (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk
+ (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed) {
+ /* copy position data to uORB home message, store it locally as well */
+ // TODO use global position estimate
+ home.lat = gps_position.lat;
+ home.lon = gps_position.lon;
+ home.alt = gps_position.alt;
+
+ home.eph_m = gps_position.eph_m;
+ home.epv_m = gps_position.epv_m;
+
+ home.s_variance_m_s = gps_position.s_variance_m_s;
+ home.p_variance_m = gps_position.p_variance_m;
+
+ double home_lat_d = home.lat * 1e-7;
+ double home_lon_d = home.lon * 1e-7;
+ warnx("home: lat = %.7f, lon = %.7f", home_lat_d, home_lon_d);
+ mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f", home_lat_d, home_lon_d);
+
+ /* announce new home position */
+ if (home_pub > 0) {
+ orb_publish(ORB_ID(home_position), home_pub, &home);
+
+ } else {
+ home_pub = orb_advertise(ORB_ID(home_position), &home);
+ }
+
+ /* mark home position as set */
+ home_position_set = true;
+ tune_positive();
+ }
+ }
+
+ /* ignore RC signals if in offboard control mode */
+ if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0) {
+ /* start RC input check */
+ if (hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
+ /* handle the case where RC signal was regained */
+ if (!status.rc_signal_found_once) {
+ status.rc_signal_found_once = true;
+ mavlink_log_critical(mavlink_fd, "[cmd] detected RC signal first time");
+ status_changed = true;
+
+ } else {
+ if (status.rc_signal_lost) {
+ mavlink_log_critical(mavlink_fd, "[cmd] RC signal regained");
+ status_changed = true;
+ }
+ }
+
+ status.rc_signal_lost = false;
+
+ transition_result_t res; // store all transitions results here
+
+ /* arm/disarm by RC */
+ res = TRANSITION_NOT_CHANGED;
+
+ /* 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 ||
+ (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++;
+ }
+
+ } else {
+ stick_off_counter = 0;
+ }
+
+ /* 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 &&
+ 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++;
+ }
+
+ } else {
+ stick_on_counter = 0;
+ }
+
+ if (res == TRANSITION_CHANGED) {
+ if (status.arming_state == ARMING_STATE_ARMED) {
+ mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC");
+
+ } else {
+ mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC");
+ }
+
+ } else if (res == TRANSITION_DENIED) {
+ /* DENIED here indicates safety switch not pressed */
+
+ if (!(!safety.safety_switch_available || safety.safety_off)) {
+ print_reject_arm("NOT ARMING: Press safety switch first.");
+
+ } else {
+ warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
+ mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
+ }
+ }
+
+ /* fill current_status according to mode switches */
+ check_mode_switches(&sp_man, &status);
+
+ /* evaluate the main state machine */
+ res = check_main_state_machine(&status);
+
+ if (res == TRANSITION_CHANGED) {
+ //mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state);
+ tune_positive();
+
+ } else if (res == TRANSITION_DENIED) {
+ /* DENIED here indicates bug in the commander */
+ warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
+ mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
+ }
+
+ } else {
+ if (!status.rc_signal_lost) {
+ mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: RC SIGNAL LOST");
+ status.rc_signal_lost = true;
+ status_changed = true;
+ }
+ }
+ }
+
+ /* evaluate the navigation state machine */
+ transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position);
+
+ if (res == TRANSITION_DENIED) {
+ /* DENIED here indicates bug in the commander */
+ warnx("ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state);
+ mavlink_log_critical(mavlink_fd, "[cmd] ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state);
+ }
+
+ /* check which state machines for changes, clear "changed" flag */
+ bool arming_state_changed = check_arming_state_changed();
+ bool main_state_changed = check_main_state_changed();
+ bool navigation_state_changed = check_navigation_state_changed();
+
+ if (arming_state_changed || main_state_changed || navigation_state_changed) {
+ mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state);
+ status_changed = true;
+
+ } else {
+ status_changed = false;
+ }
+
+ hrt_abstime t1 = hrt_absolute_time();
+
+ /* publish arming state */
+ if (arming_state_changed) {
+ armed.timestamp = t1;
+ orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
+ }
+
+ /* publish control mode */
+ if (navigation_state_changed || arming_state_changed) {
+ /* publish new navigation state */
+ control_mode.flag_armed = armed.armed; // copy armed state to vehicle_control_mode topic
+ control_mode.counter++;
+ control_mode.timestamp = t1;
+ orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
+ }
+
+ /* publish states (armed, control mode, vehicle status) at least with 5 Hz */
+ if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) {
+ status.counter++;
+ status.timestamp = t1;
+ orb_publish(ORB_ID(vehicle_status), status_pub, &status);
+ control_mode.timestamp = t1;
+ orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
+ armed.timestamp = t1;
+ orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
+ }
+
+ /* play arming and battery warning tunes */
+ if (!arm_tune_played && armed.armed) {
+ /* play tune when armed */
+ if (tune_arm() == OK)
+ arm_tune_played = true;
+
+ } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
+ /* play tune on battery warning */
+ if (tune_low_bat() == OK)
+ battery_tune_played = true;
+
+ } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
+ /* play tune on battery critical */
+ if (tune_critical_bat() == OK)
+ battery_tune_played = true;
+
+ } else if (battery_tune_played) {
+ tune_stop();
+ battery_tune_played = false;
+ }
+
+ /* reset arm_tune_played when disarmed */
+ if (!(armed.armed && (!safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) {
+ arm_tune_played = false;
+ }
+
+ fflush(stdout);
+ counter++;
+
+ toggle_status_leds(&status, &armed, arming_state_changed || status_changed);
+
+ usleep(COMMANDER_MONITORING_INTERVAL);
+ }
+
+ /* wait for threads to complete */
+ pthread_join(commander_low_prio_thread, NULL);
+
+ /* close fds */
+ led_deinit();
+ buzzer_deinit();
+ close(sp_man_sub);
+ close(sp_offboard_sub);
+ close(local_position_sub);
+ close(global_position_sub);
+ close(gps_sub);
+ close(sensor_sub);
+ close(safety_sub);
+ close(cmd_sub);
+ close(subsys_sub);
+ close(diff_pres_sub);
+ close(param_changed_sub);
+ close(battery_sub);
+
+ warnx("exiting");
+ fflush(stdout);
+
+ thread_running = false;
+
+ return 0;
+}
+
+void
+check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed)
+{
+ hrt_abstime t = hrt_absolute_time();
+ bool valid_new = (t < timestamp + timeout && t > timeout && valid_in);
+
+ if (*valid_out != valid_new) {
+ *valid_out = valid_new;
+ *changed = true;
+ }
+}
+
+void
+toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed)
+{
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+
+ /* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */
+ if (armed->armed) {
+ /* armed, solid */
+ led_on(LED_BLUE);
+
+ } else if (armed->ready_to_arm) {
+ /* ready to arm, blink at 1Hz */
+ if (leds_counter % 20 == 0)
+ led_toggle(LED_BLUE);
+
+ } else {
+ /* not ready to arm, blink at 10Hz */
+ if (leds_counter % 2 == 0)
+ led_toggle(LED_BLUE);
+ }
+
+#endif
+
+ if (changed) {
+
+ int i;
+ rgbled_pattern_t pattern;
+ memset(&pattern, 0, sizeof(pattern));
+
+ if (armed->armed) {
+ /* armed, solid */
+ if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
+ pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER;
+
+ } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
+ pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED;
+
+ } else {
+ pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN;
+ }
+
+ pattern.duration[0] = 1000;
+
+ } else if (armed->ready_to_arm) {
+ for (i = 0; i < 3; i++) {
+ if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
+ pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER;
+
+ } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
+ pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED;
+
+ } else {
+ pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN;
+ }
+
+ pattern.duration[i * 2] = 200;
+
+ pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF;
+ pattern.duration[i * 2 + 1] = 800;
+ }
+
+ if (status->condition_global_position_valid) {
+ pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE;
+ pattern.duration[i * 2] = 1000;
+ pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF;
+ pattern.duration[i * 2 + 1] = 800;
+
+ } else {
+ for (i = 3; i < 6; i++) {
+ pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE;
+ pattern.duration[i * 2] = 100;
+ pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF;
+ pattern.duration[i * 2 + 1] = 100;
+ }
+
+ pattern.color[6 * 2] = RGBLED_COLOR_OFF;
+ pattern.duration[6 * 2] = 700;
+ }
+
+ } else {
+ for (i = 0; i < 3; i++) {
+ pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED;
+ pattern.duration[i * 2] = 200;
+ pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF;
+ pattern.duration[i * 2 + 1] = 200;
+ }
+
+ /* not ready to arm, blink at 10Hz */
+ }
+
+ rgbled_set_pattern(&pattern);
+ }
+
+ /* give system warnings on error LED, XXX maybe add memory usage warning too */
+ if (status->load > 0.95f) {
+ if (leds_counter % 2 == 0)
+ led_toggle(LED_AMBER);
+
+ } else {
+ led_off(LED_AMBER);
+ }
+
+ leds_counter++;
+}
+
+void
+check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status)
+{
+ /* main mode switch */
+ if (!isfinite(sp_man->mode_switch)) {
+ warnx("mode sw not finite");
+ current_status->mode_switch = MODE_SWITCH_MANUAL;
+
+ } else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) {
+ current_status->mode_switch = MODE_SWITCH_AUTO;
+
+ } else if (sp_man->mode_switch < -STICK_ON_OFF_LIMIT) {
+ current_status->mode_switch = MODE_SWITCH_MANUAL;
+
+ } else {
+ current_status->mode_switch = MODE_SWITCH_ASSISTED;
+ }
+
+ /* land switch */
+ if (!isfinite(sp_man->return_switch)) {
+ current_status->return_switch = RETURN_SWITCH_NONE;
+
+ } else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) {
+ current_status->return_switch = RETURN_SWITCH_RETURN;
+
+ } else {
+ current_status->return_switch = RETURN_SWITCH_NONE;
+ }
+
+ /* assisted switch */
+ if (!isfinite(sp_man->assisted_switch)) {
+ current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
+
+ } else if (sp_man->assisted_switch > STICK_ON_OFF_LIMIT) {
+ current_status->assisted_switch = ASSISTED_SWITCH_EASY;
+
+ } else {
+ current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
+ }
+
+ /* mission switch */
+ if (!isfinite(sp_man->mission_switch)) {
+ current_status->mission_switch = MISSION_SWITCH_MISSION;
+
+ } else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) {
+ current_status->mission_switch = MISSION_SWITCH_NONE;
+
+ } else {
+ current_status->mission_switch = MISSION_SWITCH_MISSION;
+ }
+}
+
+transition_result_t
+check_main_state_machine(struct vehicle_status_s *current_status)
+{
+ /* evaluate the main state machine */
+ transition_result_t res = TRANSITION_DENIED;
+
+ switch (current_status->mode_switch) {
+ case MODE_SWITCH_MANUAL:
+ res = main_state_transition(current_status, MAIN_STATE_MANUAL);
+ // TRANSITION_DENIED is not possible here
+ break;
+
+ case MODE_SWITCH_ASSISTED:
+ if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) {
+ res = main_state_transition(current_status, MAIN_STATE_EASY);
+
+ if (res != TRANSITION_DENIED)
+ break; // changed successfully or already in this state
+
+ // else fallback to SEATBELT
+ print_reject_mode("EASY");
+ }
+
+ res = main_state_transition(current_status, MAIN_STATE_SEATBELT);
+
+ if (res != TRANSITION_DENIED)
+ break; // changed successfully or already in this mode
+
+ if (current_status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages
+ print_reject_mode("SEATBELT");
+
+ // else fallback to MANUAL
+ res = main_state_transition(current_status, MAIN_STATE_MANUAL);
+ // TRANSITION_DENIED is not possible here
+ break;
+
+ case MODE_SWITCH_AUTO:
+ res = main_state_transition(current_status, MAIN_STATE_AUTO);
+
+ if (res != TRANSITION_DENIED)
+ break; // changed successfully or already in this state
+
+ // else fallback to SEATBELT (EASY likely will not work too)
+ print_reject_mode("AUTO");
+ res = main_state_transition(current_status, MAIN_STATE_SEATBELT);
+
+ if (res != TRANSITION_DENIED)
+ break; // changed successfully or already in this state
+
+ // else fallback to MANUAL
+ res = main_state_transition(current_status, MAIN_STATE_MANUAL);
+ // TRANSITION_DENIED is not possible here
+ break;
+
+ default:
+ break;
+ }
+
+ return res;
+}
+
+void
+print_reject_mode(const char *msg)
+{
+ hrt_abstime t = hrt_absolute_time();
+
+ if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
+ last_print_mode_reject_time = t;
+ char s[80];
+ sprintf(s, "[cmd] WARNING: reject %s", msg);
+ mavlink_log_critical(mavlink_fd, s);
+ tune_negative();
+ }
+}
+
+void
+print_reject_arm(const char *msg)
+{
+ hrt_abstime t = hrt_absolute_time();
+
+ if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
+ last_print_mode_reject_time = t;
+ char s[80];
+ sprintf(s, "[cmd] %s", msg);
+ mavlink_log_critical(mavlink_fd, s);
+ tune_negative();
+ }
+}
+
+transition_result_t
+check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos)
+{
+ transition_result_t res = TRANSITION_DENIED;
+
+ if (status->main_state == MAIN_STATE_AUTO) {
+ if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) {
+ if (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;
+ }
+ }
+
+ if (res != TRANSITION_NOT_CHANGED) {
+ /* check again, state can be changed */
+ if (status->condition_landed) {
+ /* if landed: transitions only to AUTO_READY are allowed */
+ res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode);
+ // TRANSITION_DENIED is not possible here
+
+ } else {
+ /* not landed */
+ if (status->rc_signal_found_once && !status->rc_signal_lost) {
+ /* act depending on switches when manual control enabled */
+ if (status->return_switch == RETURN_SWITCH_RETURN) {
+ /* RTL */
+ res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_RTL, control_mode);
+
+ } else {
+ if (status->mission_switch == MISSION_SWITCH_MISSION) {
+ /* MISSION */
+ res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
+
+ } else {
+ /* LOITER */
+ res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
+ }
+ }
+
+ } else {
+ /* switch to MISSION in air when no RC control */
+ if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER ||
+ status->navigation_state == NAVIGATION_STATE_AUTO_MISSION ||
+ status->navigation_state == NAVIGATION_STATE_AUTO_RTL ||
+ status->navigation_state == NAVIGATION_STATE_AUTO_LAND) {
+ res = TRANSITION_NOT_CHANGED;
+
+ } else {
+ res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
+ }
+ }
+ }
+ }
+
+ } else {
+ /* disarmed, always switch to AUTO_READY */
+ res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode);
+ }
+
+ } else {
+ /* manual control modes */
+ if (status->rc_signal_lost && (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR)) {
+ /* switch to failsafe mode */
+ bool manual_control_old = control_mode->flag_control_manual_enabled;
+
+ if (!status->condition_landed) {
+ /* in air: try to hold position */
+ res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode);
+
+ } else {
+ /* landed: don't try to hold position but land (if taking off) */
+ res = TRANSITION_DENIED;
+ }
+
+ if (res == TRANSITION_DENIED) {
+ res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode);
+ }
+
+ control_mode->flag_control_manual_enabled = false;
+
+ if (res == TRANSITION_NOT_CHANGED && manual_control_old) {
+ /* mark navigation state as changed to force immediate flag publishing */
+ set_navigation_state_changed();
+ res = TRANSITION_CHANGED;
+ }
+
+ if (res == TRANSITION_CHANGED) {
+ if (control_mode->flag_control_position_enabled) {
+ mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: POS HOLD");
+
+ } else {
+ if (status->condition_landed) {
+ mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: ALT HOLD (LAND)");
+
+ } else {
+ mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: ALT HOLD");
+ }
+ }
+ }
+
+ } else {
+ switch (status->main_state) {
+ case MAIN_STATE_MANUAL:
+ res = navigation_state_transition(status, status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode);
+ break;
+
+ case MAIN_STATE_SEATBELT:
+ res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode);
+ break;
+
+ case MAIN_STATE_EASY:
+ res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode);
+ break;
+
+ default:
+ break;
+ }
+ }
+ }
+
+ 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 */
+ prctl(PR_SET_NAME, "commander_low_prio", getpid());
+
+ /* Subscribe to command topic */
+ int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
+ struct vehicle_command_s cmd;
+ memset(&cmd, 0, sizeof(cmd));
+
+ /* wakeup source(s) */
+ struct pollfd fds[1];
+
+ /* use the gyro to pace output - XXX BROKEN if we are using the L3GD20 */
+ fds[0].fd = cmd_sub;
+ fds[0].events = POLLIN;
+
+ while (!thread_should_exit) {
+
+ /* wait for up to 100ms for data */
+ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000);
+
+ /* timed out - periodic check for _task_should_exit, etc. */
+ if (pret == 0)
+ continue;
+
+ /* this is undesirable but not much we can do - might want to flag unhappy status */
+ if (pret < 0) {
+ warn("poll error %d, %d", pret, errno);
+ continue;
+ }
+
+ /* if we reach here, we have a valid command */
+ orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
+
+ /* 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_NAV_TAKEOFF)
+ continue;
+
+ /* only handle low-priority commands here */
+ switch (cmd.command) {
+
+ case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
+ if (is_safe(&status, &safety, &armed)) {
+
+ 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 {
+ answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
+ }
+
+ } else {
+ answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
+ }
+
+ break;
+
+ case VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
+
+ int calib_ret = ERROR;
+
+ /* 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;
+ }
+
+ if ((int)(cmd.param1) == 1) {
+ /* gyro calibration */
+ answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+ calib_ret = do_gyro_calibration(mavlink_fd);
+
+ } else if ((int)(cmd.param2) == 1) {
+ /* magnetometer calibration */
+ answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+ calib_ret = do_mag_calibration(mavlink_fd);
+
+ } else if ((int)(cmd.param3) == 1) {
+ /* zero-altitude pressure calibration */
+ answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
+
+ } else if ((int)(cmd.param4) == 1) {
+ /* RC calibration */
+ answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
+
+ } else if ((int)(cmd.param5) == 1) {
+ /* accelerometer calibration */
+ answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+ calib_ret = do_accel_calibration(mavlink_fd);
+
+ } else if ((int)(cmd.param6) == 1) {
+ /* airspeed calibration */
+ answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+ calib_ret = do_airspeed_calibration(mavlink_fd);
+ }
+
+ if (calib_ret == OK)
+ tune_positive();
+ else
+ tune_negative();
+
+ arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
+
+ break;
+ }
+
+ case VEHICLE_CMD_PREFLIGHT_STORAGE: {
+
+ 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);
+
+ } else {
+ mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR");
+ answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
+ }
+
+ } 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 {
+ 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 */
+ if (cmd.confirmation > 0 && cmd.command != VEHICLE_CMD_DO_SET_MODE
+ && cmd.command != VEHICLE_CMD_COMPONENT_ARM_DISARM) {
+ /* send acknowledge command */
+ // XXX TODO
+ }
+
+ }
+
+ return 0;
+}
diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5df5d8d0c3fb3123f2acf1ffdfde6186a7adb2b5
--- /dev/null
+++ b/src/modules/commander/commander_helper.cpp
@@ -0,0 +1,259 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ * Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ * Julian Oes <joes@student.ethz.ch>
+ *
+ * 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_helper.cpp
+ * Commander helper functions implementations
+ */
+
+#include <stdio.h>
+#include <unistd.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <fcntl.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <systemlib/err.h>
+#include <systemlib/param/param.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_tone_alarm.h>
+#include <drivers/drv_led.h>
+#include <drivers/drv_rgbled.h>
+
+
+#include "commander_helper.h"
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+bool is_multirotor(const struct vehicle_status_s *current_status)
+{
+ return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) ||
+ (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
+ (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) ||
+ (current_status->system_type == VEHICLE_TYPE_TRICOPTER));
+}
+
+bool is_rotary_wing(const struct vehicle_status_s *current_status)
+{
+ return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER)
+ || (current_status->system_type == VEHICLE_TYPE_COAXIAL);
+}
+
+static int buzzer;
+
+int buzzer_init()
+{
+ buzzer = open("/dev/tone_alarm", O_WRONLY);
+
+ if (buzzer < 0) {
+ warnx("Buzzer: open fail\n");
+ return ERROR;
+ }
+
+ return OK;
+}
+
+void buzzer_deinit()
+{
+ close(buzzer);
+}
+
+void tune_error()
+{
+ ioctl(buzzer, TONE_SET_ALARM, 2);
+}
+
+void tune_positive()
+{
+ ioctl(buzzer, TONE_SET_ALARM, 3);
+}
+
+void tune_neutral()
+{
+ ioctl(buzzer, TONE_SET_ALARM, 4);
+}
+
+void tune_negative()
+{
+ ioctl(buzzer, TONE_SET_ALARM, 5);
+}
+
+int tune_arm()
+{
+ return ioctl(buzzer, TONE_SET_ALARM, 12);
+}
+
+int tune_low_bat()
+{
+ return ioctl(buzzer, TONE_SET_ALARM, 13);
+}
+
+int tune_critical_bat()
+{
+ return ioctl(buzzer, TONE_SET_ALARM, 14);
+}
+
+
+
+void tune_stop()
+{
+ ioctl(buzzer, TONE_SET_ALARM, 0);
+}
+
+static int leds;
+static int rgbleds;
+
+int led_init()
+{
+ /* first open normal LEDs */
+ leds = open(LED_DEVICE_PATH, 0);
+
+ if (leds < 0) {
+ warnx("LED: open fail\n");
+ return ERROR;
+ }
+
+ /* the blue LED is only available on FMUv1 but not FMUv2 */
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+
+ if (ioctl(leds, LED_ON, LED_BLUE)) {
+ warnx("Blue LED: ioctl fail\n");
+ return ERROR;
+ }
+#endif
+
+ if (ioctl(leds, LED_ON, LED_AMBER)) {
+ warnx("Amber LED: ioctl fail\n");
+ return ERROR;
+ }
+
+ /* then try RGB LEDs, this can fail on FMUv1*/
+ rgbleds = open(RGBLED_DEVICE_PATH, 0);
+ if (rgbleds == -1) {
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+ errx(1, "Unable to open " RGBLED_DEVICE_PATH);
+#else
+ warnx("No RGB LED found");
+#endif
+ }
+
+ return 0;
+}
+
+void led_deinit()
+{
+ close(leds);
+
+ if (rgbleds != -1) {
+ close(rgbleds);
+ }
+}
+
+int led_toggle(int led)
+{
+ return ioctl(leds, LED_TOGGLE, led);
+}
+
+int led_on(int led)
+{
+ return ioctl(leds, LED_ON, led);
+}
+
+int led_off(int led)
+{
+ return ioctl(leds, LED_OFF, led);
+}
+
+void rgbled_set_color(rgbled_color_t color) {
+
+ if (rgbleds != -1)
+ ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color);
+}
+
+void rgbled_set_mode(rgbled_mode_t mode) {
+
+ if (rgbleds != -1)
+ ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode);
+}
+
+void rgbled_set_pattern(rgbled_pattern_t *pattern) {
+
+ if (rgbleds != -1)
+ ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern);
+}
+
+float battery_remaining_estimate_voltage(float voltage)
+{
+ float ret = 0;
+ static param_t bat_volt_empty;
+ static param_t bat_volt_full;
+ static param_t bat_n_cells;
+ static bool initialized = false;
+ static unsigned int counter = 0;
+ static float ncells = 3;
+ // XXX change cells to int (and param to INT32)
+
+ if (!initialized) {
+ bat_volt_empty = param_find("BAT_V_EMPTY");
+ bat_volt_full = param_find("BAT_V_FULL");
+ bat_n_cells = param_find("BAT_N_CELLS");
+ initialized = true;
+ }
+
+ static float chemistry_voltage_empty = 3.2f;
+ static float chemistry_voltage_full = 4.05f;
+
+ if (counter % 100 == 0) {
+ param_get(bat_volt_empty, &chemistry_voltage_empty);
+ param_get(bat_volt_full, &chemistry_voltage_full);
+ param_get(bat_n_cells, &ncells);
+ }
+
+ counter++;
+
+ ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty));
+
+ /* limit to sane values */
+ ret = (ret < 0.0f) ? 0.0f : ret;
+ ret = (ret > 1.0f) ? 1.0f : ret;
+ return ret;
+}
diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h
new file mode 100644
index 0000000000000000000000000000000000000000..027d0535f066f2a5f251ab3daeb82f2110b72090
--- /dev/null
+++ b/src/modules/commander/commander_helper.h
@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ * Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ * Julian Oes <joes@student.ethz.ch>
+ *
+ * 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_helper.h
+ * Commander helper functions definitions
+ */
+
+#ifndef COMMANDER_HELPER_H_
+#define COMMANDER_HELPER_H_
+
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <drivers/drv_rgbled.h>
+
+
+bool is_multirotor(const struct vehicle_status_s *current_status);
+bool is_rotary_wing(const struct vehicle_status_s *current_status);
+
+int buzzer_init(void);
+void buzzer_deinit(void);
+
+void tune_error(void);
+void tune_positive(void);
+void tune_neutral(void);
+void tune_negative(void);
+int tune_arm(void);
+int tune_low_bat(void);
+int tune_critical_bat(void);
+void tune_stop(void);
+
+int led_init(void);
+void led_deinit(void);
+int led_toggle(int led);
+int led_on(int led);
+int led_off(int led);
+
+void rgbled_set_color(rgbled_color_t color);
+
+void rgbled_set_mode(rgbled_mode_t mode);
+
+void rgbled_set_pattern(rgbled_pattern_t *pattern);
+
+/**
+ * Provides a coarse estimate of remaining battery power.
+ *
+ * The estimate is very basic and based on decharging voltage curves.
+ *
+ * @return the estimated remaining capacity in 0..1
+ */
+float battery_remaining_estimate_voltage(float voltage);
+
+#endif /* COMMANDER_HELPER_H_ */
diff --git a/src/modules/commander/commander.h b/src/modules/commander/commander_params.c
similarity index 74%
rename from src/modules/commander/commander.h
rename to src/modules/commander/commander_params.c
index 04b4e72ab31b0c28ac0de6bf1c42f4e76f96c297..0a1703b2e0cdeb8400bed8a93681d21278f9a424 100644
--- a/src/modules/commander/commander.h
+++ b/src/modules/commander/commander_params.c
@@ -1,10 +1,7 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
- * Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
- * Lorenz Meier <lm@inf.ethz.ch>
- * Thomas Gubler <thomasgubler@student.ethz.ch>
- * Julian Oes <joes@student.ethz.ch>
+ * Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ * Author: Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -36,23 +33,22 @@
****************************************************************************/
/**
- * @file commander.h
- * Main system state machine definition.
+ * @file commander_params.c
+ *
+ * Parameters defined by the sensors task.
*
- * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
- *
*/
-#ifndef COMMANDER_H_
-#define COMMANDER_H_
-
-#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f
-#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f
-
-void tune_confirm(void);
-void tune_error(void);
+#include <nuttx/config.h>
+#include <systemlib/param/param.h>
-#endif /* COMMANDER_H_ */
+PARAM_DEFINE_INT32(SYS_FAILSAFE_LL, 0); /**< Go into low-level failsafe after 0 ms */
+PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
+PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
+PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
+PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f);
+PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f);
+PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3);
diff --git a/src/modules/commander/commander_tests/commander_tests.cpp b/src/modules/commander/commander_tests/commander_tests.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6e72cf0d9c0cb1d40cfc2f2066896e3ebadb9d1d
--- /dev/null
+++ b/src/modules/commander/commander_tests/commander_tests.cpp
@@ -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;
+}
diff --git a/src/modules/commander/commander_tests/module.mk b/src/modules/commander/commander_tests/module.mk
new file mode 100644
index 0000000000000000000000000000000000000000..4d10275d1ecae26acb7845baee09dc78b11df630
--- /dev/null
+++ b/src/modules/commander/commander_tests/module.mk
@@ -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
diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..40bedd9f31bc3ca0f327204307d3c1e800dd0b67
--- /dev/null
+++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
@@ -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(¤t_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(¤t_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(¤t_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(¤t_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(¤t_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(¤t_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(¤t_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(¤t_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(¤t_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(¤t_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(¤t_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(¤t_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(¤t_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());
+}
diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.h b/src/modules/commander/commander_tests/state_machine_helper_test.h
new file mode 100644
index 0000000000000000000000000000000000000000..10a68e6028d6b757d0d2e4ea33fbdfb4ac1b0e65
--- /dev/null
+++ b/src/modules/commander/commander_tests/state_machine_helper_test.h
@@ -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_ */
diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..33566d4e53cc9b68f97abe3290ebc402750b4045
--- /dev/null
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -0,0 +1,289 @@
+/****************************************************************************
+ *
+ * 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 gyro_calibration.cpp
+ * Gyroscope calibration routine
+ */
+
+#include "gyro_calibration.h"
+#include "commander_helper.h"
+
+#include <stdio.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <math.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/topics/sensor_combined.h>
+#include <drivers/drv_gyro.h>
+#include <mavlink/mavlink_log.h>
+#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;
+
+int do_gyro_calibration(int mavlink_fd)
+{
+ mavlink_log_info(mavlink_fd, "Gyro calibration starting, do not move unit.");
+
+ const int calibration_count = 5000;
+
+ int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
+ struct sensor_combined_s raw;
+
+ int calibration_counter = 0;
+ float gyro_offset[3] = {0.0f, 0.0f, 0.0f};
+
+ /* set offsets to zero */
+ int fd = open(GYRO_DEVICE_PATH, 0);
+ struct gyro_scale gscale_null = {
+ 0.0f,
+ 1.0f,
+ 0.0f,
+ 1.0f,
+ 0.0f,
+ 1.0f,
+ };
+
+ if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null))
+ warn("WARNING: failed to set scale / offsets for gyro");
+
+ close(fd);
+
+ unsigned poll_errcount = 0;
+
+ while (calibration_counter < calibration_count) {
+
+ /* wait blocking for new data */
+ struct pollfd fds[1];
+ fds[0].fd = sub_sensor_combined;
+ fds[0].events = POLLIN;
+
+ int poll_ret = poll(fds, 1, 1000);
+
+ 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 {
+ poll_errcount++;
+ }
+
+ if (poll_errcount > 1000) {
+ mavlink_log_info(mavlink_fd, "ERROR: Failed reading gyro sensor");
+ return ERROR;
+ }
+ }
+
+ gyro_offset[0] = gyro_offset[0] / calibration_count;
+ gyro_offset[1] = gyro_offset[1] / calibration_count;
+ gyro_offset[2] = gyro_offset[2] / calibration_count;
+
+
+ if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) {
+
+ if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0]))
+ || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1]))
+ || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) {
+ mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!");
+ }
+
+ /* set offsets to actual value */
+ fd = open(GYRO_DEVICE_PATH, 0);
+ struct gyro_scale gscale = {
+ gyro_offset[0],
+ 1.0f,
+ gyro_offset[1],
+ 1.0f,
+ gyro_offset[2],
+ 1.0f,
+ };
+
+ if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
+ warn("WARNING: failed to set scale / offsets for gyro");
+
+ close(fd);
+
+ /* auto-save to EEPROM */
+ int save_ret = param_save_default();
+
+ if (save_ret != 0) {
+ warnx("WARNING: auto-save of params to storage failed");
+ mavlink_log_critical(mavlink_fd, "gyro store failed");
+ return ERROR;
+ }
+
+ mavlink_log_info(mavlink_fd, "gyro calibration done");
+
+ tune_neutral();
+ /* third beep by cal end routine */
+
+ } else {
+ mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)");
+ return ERROR;
+ }
+
+
+ /*** --- SCALING --- ***/
+
+ mavlink_log_info(mavlink_fd, "offset calibration finished. Rotate for scale 30x");
+ mavlink_log_info(mavlink_fd, "or do not rotate and wait for 5 seconds to skip.");
+ warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip.");
+
+ unsigned rotations_count = 30;
+ float gyro_integral = 0.0f;
+ float baseline_integral = 0.0f;
+
+ // XXX change to mag topic
+ orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
+
+ float mag_last = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]);
+ if (mag_last > M_PI_F) mag_last -= 2*M_PI_F;
+ if (mag_last < -M_PI_F) mag_last += 2*M_PI_F;
+
+
+ uint64_t last_time = hrt_absolute_time();
+ uint64_t start_time = hrt_absolute_time();
+
+ while ((int)fabsf(baseline_integral / (2.0f * M_PI_F)) < rotations_count) {
+
+ /* abort this loop if not rotated more than 180 degrees within 5 seconds */
+ if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f)
+ && (hrt_absolute_time() - start_time > 5 * 1e6)) {
+ mavlink_log_info(mavlink_fd, "gyro scale calibration skipped");
+ mavlink_log_info(mavlink_fd, "gyro calibration done");
+ return OK;
+ }
+
+ /* wait blocking for new data */
+ struct pollfd fds[1];
+ fds[0].fd = sub_sensor_combined;
+ fds[0].events = POLLIN;
+
+ int poll_ret = poll(fds, 1, 1000);
+
+ if (poll_ret) {
+
+ float dt_ms = (hrt_absolute_time() - last_time) / 1e3f;
+ last_time = hrt_absolute_time();
+
+ orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
+
+ // XXX this is just a proof of concept and needs world / body
+ // transformation and more
+
+ //math::Vector2f magNav(raw.magnetometer_ga);
+
+ // calculate error between estimate and measurement
+ // apply declination correction for true heading as well.
+ //float mag = -atan2f(magNav(1),magNav(0));
+ float mag = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]);
+ if (mag > M_PI_F) mag -= 2*M_PI_F;
+ if (mag < -M_PI_F) mag += 2*M_PI_F;
+
+ float diff = mag - mag_last;
+
+ if (diff > M_PI_F) diff -= 2*M_PI_F;
+ if (diff < -M_PI_F) diff += 2*M_PI_F;
+
+ baseline_integral += diff;
+ mag_last = mag;
+ // Jump through some timing scale hoops to avoid
+ // 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", (double)baseline_integral, (double)gyro_integral);
+
+ // } 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;
+ }
+ }
+
+ float gyro_scale = baseline_integral / gyro_integral;
+ float gyro_scales[] = { gyro_scale, gyro_scale, 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])) {
+
+ if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scales[0]))
+ || param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scales[1]))
+ || param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scales[2]))) {
+ mavlink_log_critical(mavlink_fd, "Setting gyro scale failed!");
+ }
+
+ /* set offsets to actual value */
+ fd = open(GYRO_DEVICE_PATH, 0);
+ struct gyro_scale gscale = {
+ gyro_offset[0],
+ gyro_scales[0],
+ gyro_offset[1],
+ gyro_scales[1],
+ gyro_offset[2],
+ gyro_scales[2],
+ };
+
+ if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
+ warn("WARNING: failed to set scale / offsets for gyro");
+
+ close(fd);
+
+ /* auto-save to EEPROM */
+ int save_ret = param_save_default();
+
+ if (save_ret != 0) {
+ warn("WARNING: auto-save of params to storage failed");
+ }
+
+ // char buf[50];
+ // sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]);
+ // mavlink_log_info(mavlink_fd, buf);
+ mavlink_log_info(mavlink_fd, "gyro calibration done");
+
+ /* third beep by cal end routine */
+ return OK;
+ } else {
+ mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)");
+ return ERROR;
+ }
+}
diff --git a/src/modules/commander/gyro_calibration.h b/src/modules/commander/gyro_calibration.h
new file mode 100644
index 0000000000000000000000000000000000000000..59c32a15e9d83ada80474186f1a44bae0c53c33b
--- /dev/null
+++ b/src/modules/commander/gyro_calibration.h
@@ -0,0 +1,46 @@
+/****************************************************************************
+ *
+ * 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 gyro_calibration.h
+ * Gyroscope calibration routine
+ */
+
+#ifndef GYRO_CALIBRATION_H_
+#define GYRO_CALIBRATION_H_
+
+#include <stdint.h>
+
+int do_gyro_calibration(int mavlink_fd);
+
+#endif /* GYRO_CALIBRATION_H_ */
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e38616027169ad9fac934dc9cb08acd0e2f46d45
--- /dev/null
+++ b/src/modules/commander/mag_calibration.cpp
@@ -0,0 +1,298 @@
+/****************************************************************************
+ *
+ * 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 mag_calibration.cpp
+ * Magnetometer calibration routine
+ */
+
+#include "mag_calibration.h"
+#include "commander_helper.h"
+#include "calibration_routines.h"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <poll.h>
+#include <math.h>
+#include <fcntl.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/topics/sensor_combined.h>
+#include <drivers/drv_mag.h>
+#include <mavlink/mavlink_log.h>
+#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;
+
+int do_mag_calibration(int mavlink_fd)
+{
+ 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;
+
+ /* 45 seconds */
+ uint64_t calibration_interval = 45 * 1000 * 1000;
+
+ /* maximum 2000 values */
+ const unsigned int calibration_maxcount = 500;
+ unsigned int calibration_counter = 0;
+
+ /* limit update rate to get equally spaced measurements over time (in ms) */
+ orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount);
+
+ int fd = open(MAG_DEVICE_PATH, O_RDONLY);
+
+ /* erase old calibration */
+ struct mag_scale mscale_null = {
+ 0.0f,
+ 1.0f,
+ 0.0f,
+ 1.0f,
+ 0.0f,
+ 1.0f,
+ };
+
+ if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
+ warn("WARNING: failed to set scale / offsets for mag");
+ mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag");
+ }
+
+ /* calibrate range */
+ if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) {
+ warnx("failed to calibrate scale");
+ }
+
+ close(fd);
+
+ mavlink_log_info(mavlink_fd, "mag cal progress <20> percent");
+
+ /* calibrate offsets */
+
+ // uint64_t calibration_start = hrt_absolute_time();
+
+ uint64_t axis_deadline = hrt_absolute_time();
+ uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
+
+ const char axislabels[3] = { 'X', 'Y', 'Z'};
+ int axis_index = -1;
+
+ float *x = (float *)malloc(sizeof(float) * calibration_maxcount);
+ float *y = (float *)malloc(sizeof(float) * calibration_maxcount);
+ float *z = (float *)malloc(sizeof(float) * calibration_maxcount);
+
+ if (x == NULL || y == NULL || z == NULL) {
+ 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 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) {
+
+ /* wait blocking for new data */
+ struct pollfd fds[1];
+ fds[0].fd = sub_mag;
+ fds[0].events = POLLIN;
+
+ /* user guidance */
+ if (hrt_absolute_time() >= axis_deadline &&
+ axis_index < 3) {
+
+ axis_index++;
+
+ 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;
+ }
+
+ if (!(axis_index < 3)) {
+ break;
+ }
+
+ // int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time();
+
+ // if ((axis_left / 1000) == 0 && axis_left > 0) {
+ // char buf[50];
+ // sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]);
+ // mavlink_log_info(mavlink_fd, buf);
+ // }
+
+ int poll_ret = poll(fds, 1, 1000);
+
+ if (poll_ret > 0) {
+ orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
+
+ x[calibration_counter] = mag.x;
+ y[calibration_counter] = mag.y;
+ z[calibration_counter] = mag.z;
+
+ /* get min/max values */
+
+ // if (mag.x < mag_min[0]) {
+ // mag_min[0] = mag.x;
+ // }
+ // else if (mag.x > mag_max[0]) {
+ // mag_max[0] = mag.x;
+ // }
+
+ // if (raw.magnetometer_ga[1] < mag_min[1]) {
+ // mag_min[1] = raw.magnetometer_ga[1];
+ // }
+ // else if (raw.magnetometer_ga[1] > mag_max[1]) {
+ // mag_max[1] = raw.magnetometer_ga[1];
+ // }
+
+ // if (raw.magnetometer_ga[2] < mag_min[2]) {
+ // mag_min[2] = raw.magnetometer_ga[2];
+ // }
+ // else if (raw.magnetometer_ga[2] > mag_max[2]) {
+ // mag_max[2] = raw.magnetometer_ga[2];
+ // }
+
+ calibration_counter++;
+
+ } else {
+ poll_errcount++;
+ }
+
+ if (poll_errcount > 1000) {
+ mavlink_log_info(mavlink_fd, "ERROR: Failed reading mag sensor");
+ return ERROR;
+ }
+
+
+ }
+
+ float sphere_x;
+ float sphere_y;
+ float sphere_z;
+ float sphere_radius;
+
+ sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
+
+ free(x);
+ free(y);
+ free(z);
+
+ if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) {
+
+ fd = open(MAG_DEVICE_PATH, 0);
+
+ struct mag_scale mscale;
+
+ if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale))
+ warn("WARNING: failed to get scale / offsets for mag");
+
+ mscale.x_offset = sphere_x;
+ mscale.y_offset = sphere_y;
+ mscale.z_offset = sphere_z;
+
+ if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale))
+ warn("WARNING: failed to set scale / offsets for mag");
+
+ close(fd);
+
+ /* announce and set new offset */
+
+ if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) {
+ warnx("Setting X mag offset failed!\n");
+ }
+
+ if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) {
+ warnx("Setting Y mag offset failed!\n");
+ }
+
+ if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) {
+ warnx("Setting Z mag offset failed!\n");
+ }
+
+ if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) {
+ warnx("Setting X mag scale failed!\n");
+ }
+
+ if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) {
+ warnx("Setting Y mag scale failed!\n");
+ }
+
+ if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) {
+ 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",
+ (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale,
+ (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius);
+
+ char buf[52];
+ sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset,
+ (double)mscale.y_offset, (double)mscale.z_offset);
+ mavlink_log_info(mavlink_fd, buf);
+
+ sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale,
+ (double)mscale.y_scale, (double)mscale.z_scale);
+ mavlink_log_info(mavlink_fd, buf);
+
+ mavlink_log_info(mavlink_fd, "magnetometer calibration completed");
+ mavlink_log_info(mavlink_fd, "mag cal progress <100> percent");
+
+ return OK;
+ /* third beep by cal end routine */
+
+ } else {
+ mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)");
+ return ERROR;
+ }
+}
diff --git a/src/modules/commander/mag_calibration.h b/src/modules/commander/mag_calibration.h
new file mode 100644
index 0000000000000000000000000000000000000000..a101cd7b1fffb392fe0b3b143c3d39b9b7b876f7
--- /dev/null
+++ b/src/modules/commander/mag_calibration.h
@@ -0,0 +1,46 @@
+/****************************************************************************
+ *
+ * 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 mag_calibration.h
+ * Magnetometer calibration routine
+ */
+
+#ifndef MAG_CALIBRATION_H_
+#define MAG_CALIBRATION_H_
+
+#include <stdint.h>
+
+int do_mag_calibration(int mavlink_fd);
+
+#endif /* MAG_CALIBRATION_H_ */
diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk
index fe44e955ad2c15362bd38a0d4ec273bd0639265a..91d75121eb475a41416f59923db5434bd3fad9d6 100644
--- a/src/modules/commander/module.mk
+++ b/src/modules/commander/module.mk
@@ -36,8 +36,15 @@
#
MODULE_COMMAND = commander
-SRCS = commander.c \
- state_machine_helper.c \
- calibration_routines.c \
- accelerometer_calibration.c
+SRCS = commander.cpp \
+ commander_params.c \
+ state_machine_helper.cpp \
+ commander_helper.cpp \
+ calibration_routines.cpp \
+ accelerometer_calibration.cpp \
+ gyro_calibration.cpp \
+ mag_calibration.cpp \
+ baro_calibration.cpp \
+ rc_calibration.cpp \
+ airspeed_calibration.cpp
diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h
new file mode 100644
index 0000000000000000000000000000000000000000..b60a7e0b9c8a30f63a9a7270f14c5803f11ede53
--- /dev/null
+++ b/src/modules/commander/px4_custom_mode.h
@@ -0,0 +1,37 @@
+/*
+ * px4_custom_mode.h
+ *
+ * Created on: 09.08.2013
+ * Author: ton
+ */
+
+#ifndef PX4_CUSTOM_MODE_H_
+#define PX4_CUSTOM_MODE_H_
+
+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_ */
diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fe87a3323b36ed6d7fae12e2e8a9566cb23899c2
--- /dev/null
+++ b/src/modules/commander/rc_calibration.cpp
@@ -0,0 +1,88 @@
+/****************************************************************************
+ *
+ * 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_calibration.cpp
+ * Remote Control calibration routine
+ */
+
+#include "rc_calibration.h"
+#include "commander_helper.h"
+
+#include <poll.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <mavlink/mavlink_log.h>
+#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;
+
+int do_rc_calibration(int mavlink_fd)
+{
+ mavlink_log_info(mavlink_fd, "trim calibration starting");
+
+ /* XXX fix this */
+ // if (current_status.rc_signal) {
+ // mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal.");
+ // return;
+ // }
+
+ int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
+ struct manual_control_setpoint_s sp;
+ orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);
+
+ /* set parameters */
+ float p = sp.roll;
+ param_set(param_find("TRIM_ROLL"), &p);
+ p = sp.pitch;
+ param_set(param_find("TRIM_PITCH"), &p);
+ p = sp.yaw;
+ param_set(param_find("TRIM_YAW"), &p);
+
+ /* store to permanent storage */
+ /* auto-save */
+ int save_ret = param_save_default();
+
+ if (save_ret != 0) {
+ mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed");
+ return ERROR;
+ }
+
+ mavlink_log_info(mavlink_fd, "trim calibration done");
+ return OK;
+}
diff --git a/src/modules/commander/rc_calibration.h b/src/modules/commander/rc_calibration.h
new file mode 100644
index 0000000000000000000000000000000000000000..9aa6faafa82b867af59ea9a7371c73c89edc93ea
--- /dev/null
+++ b/src/modules/commander/rc_calibration.h
@@ -0,0 +1,46 @@
+/****************************************************************************
+ *
+ * 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_calibration.h
+ * Remote Control calibration routine
+ */
+
+#ifndef RC_CALIBRATION_H_
+#define RC_CALIBRATION_H_
+
+#include <stdint.h>
+
+int do_rc_calibration(int mavlink_fd);
+
+#endif /* RC_CALIBRATION_H_ */
diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c
deleted file mode 100644
index ab728c7bbb8ca8ea09f94cfd8eedf399f598d5c8..0000000000000000000000000000000000000000
--- a/src/modules/commander/state_machine_helper.c
+++ /dev/null
@@ -1,757 +0,0 @@
-/****************************************************************************
- *
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
- * Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- * Julian Oes <joes@student.ethz.ch>
- *
- * 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.c
- * State machine helper functions implementations
- */
-
-#include <stdio.h>
-#include <unistd.h>
-
-#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/actuator_controls.h>
-#include <systemlib/systemlib.h>
-#include <drivers/drv_hrt.h>
-#include <mavlink/mavlink_log.h>
-
-#include "state_machine_helper.h"
-
-static const char *system_state_txt[] = {
- "SYSTEM_STATE_PREFLIGHT",
- "SYSTEM_STATE_STANDBY",
- "SYSTEM_STATE_GROUND_READY",
- "SYSTEM_STATE_MANUAL",
- "SYSTEM_STATE_STABILIZED",
- "SYSTEM_STATE_AUTO",
- "SYSTEM_STATE_MISSION_ABORT",
- "SYSTEM_STATE_EMCY_LANDING",
- "SYSTEM_STATE_EMCY_CUTOFF",
- "SYSTEM_STATE_GROUND_ERROR",
- "SYSTEM_STATE_REBOOT",
-
-};
-
-/**
- * Transition from one state to another
- */
-int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state)
-{
- int invalid_state = false;
- int ret = ERROR;
-
- commander_state_machine_t old_state = current_status->state_machine;
-
- switch (new_state) {
- case SYSTEM_STATE_MISSION_ABORT: {
- /* Indoor or outdoor */
- // if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
- ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING);
-
- // } else {
- // ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF);
- // }
- }
- break;
-
- case SYSTEM_STATE_EMCY_LANDING:
- /* Tell the controller to land */
-
- /* set system flags according to state */
- current_status->flag_system_armed = true;
-
- warnx("EMERGENCY LANDING!\n");
- mavlink_log_critical(mavlink_fd, "EMERGENCY LANDING!");
- break;
-
- case SYSTEM_STATE_EMCY_CUTOFF:
- /* Tell the controller to cutoff the motors (thrust = 0) */
-
- /* set system flags according to state */
- current_status->flag_system_armed = false;
-
- warnx("EMERGENCY MOTOR CUTOFF!\n");
- mavlink_log_critical(mavlink_fd, "EMERGENCY MOTOR CUTOFF!");
- break;
-
- case SYSTEM_STATE_GROUND_ERROR:
-
- /* set system flags according to state */
-
- /* prevent actuators from arming */
- current_status->flag_system_armed = false;
-
- warnx("GROUND ERROR, locking down propulsion system\n");
- mavlink_log_critical(mavlink_fd, "GROUND ERROR, locking down system");
- break;
-
- case SYSTEM_STATE_PREFLIGHT:
- if (current_status->state_machine == SYSTEM_STATE_STANDBY
- || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
- /* set system flags according to state */
- current_status->flag_system_armed = false;
- mavlink_log_critical(mavlink_fd, "Switched to PREFLIGHT state");
-
- } else {
- invalid_state = true;
- mavlink_log_critical(mavlink_fd, "REFUSED to switch to PREFLIGHT state");
- }
-
- break;
-
- case SYSTEM_STATE_REBOOT:
- if (current_status->state_machine == SYSTEM_STATE_STANDBY
- || current_status->state_machine == SYSTEM_STATE_PREFLIGHT
- || current_status->flag_hil_enabled) {
- invalid_state = false;
- /* set system flags according to state */
- current_status->flag_system_armed = false;
- mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM");
- usleep(500000);
- up_systemreset();
- /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */
-
- } else {
- invalid_state = true;
- mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT");
- }
-
- break;
-
- case SYSTEM_STATE_STANDBY:
- /* set system flags according to state */
-
- /* standby enforces disarmed */
- current_status->flag_system_armed = false;
-
- mavlink_log_critical(mavlink_fd, "Switched to STANDBY state");
- break;
-
- case SYSTEM_STATE_GROUND_READY:
-
- /* set system flags according to state */
-
- /* ground ready has motors / actuators armed */
- current_status->flag_system_armed = true;
-
- mavlink_log_critical(mavlink_fd, "Switched to GROUND READY state");
- break;
-
- case SYSTEM_STATE_AUTO:
-
- /* set system flags according to state */
-
- /* auto is airborne and in auto mode, motors armed */
- current_status->flag_system_armed = true;
-
- mavlink_log_critical(mavlink_fd, "Switched to FLYING / AUTO mode");
- break;
-
- case SYSTEM_STATE_STABILIZED:
-
- /* set system flags according to state */
- current_status->flag_system_armed = true;
-
- mavlink_log_critical(mavlink_fd, "Switched to FLYING / STABILIZED mode");
- break;
-
- case SYSTEM_STATE_MANUAL:
-
- /* set system flags according to state */
- current_status->flag_system_armed = true;
-
- mavlink_log_critical(mavlink_fd, "Switched to FLYING / MANUAL mode");
- break;
-
- default:
- invalid_state = true;
- break;
- }
-
- if (invalid_state == false || old_state != new_state) {
- current_status->state_machine = new_state;
- state_machine_publish(status_pub, current_status, mavlink_fd);
- publish_armed_status(current_status);
- ret = OK;
- }
-
- if (invalid_state) {
- mavlink_log_critical(mavlink_fd, "REJECTING invalid state transition");
- ret = ERROR;
- }
-
- return ret;
-}
-
-void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
- /* publish the new state */
- current_status->counter++;
- current_status->timestamp = hrt_absolute_time();
-
- /* assemble state vector based on flag values */
- if (current_status->flag_control_rates_enabled) {
- current_status->onboard_control_sensors_present |= 0x400;
-
- } else {
- current_status->onboard_control_sensors_present &= ~0x400;
- }
-
- current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0;
- current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0;
- current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0;
- current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0;
-
- current_status->onboard_control_sensors_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 0;
- current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0;
- current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0;
- current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0;
- current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0;
-
- orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
- printf("[cmd] new state: %s\n", system_state_txt[current_status->state_machine]);
-}
-
-void publish_armed_status(const struct vehicle_status_s *current_status)
-{
- struct actuator_armed_s armed;
- armed.armed = current_status->flag_system_armed;
-
- /* XXX allow arming by external components on multicopters only if not yet armed by RC */
- /* XXX allow arming only if core sensors are ok */
- armed.ready_to_arm = true;
-
- /* lock down actuators if required, only in HIL */
- armed.lockdown = (current_status->flag_hil_enabled) ? true : false;
- orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
- orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
-}
-
-
-/*
- * Private functions, update the state machine
- */
-void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
- warnx("EMERGENCY HANDLER\n");
- /* Depending on the current state go to one of the error states */
-
- if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
- do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR);
-
- } else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) {
-
- // DO NOT abort mission
- //do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT);
-
- } else {
- warnx("Unknown system state: #%d\n", current_status->state_machine);
- }
-}
-
-void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) //do not call state_machine_emergency_always_critical if we are in manual mode for these errors
-{
- if (current_status->state_machine != SYSTEM_STATE_MANUAL) { //if we are in manual: user can react to errors themself
- state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd);
-
- } else {
- //global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL);
- }
-
-}
-
-
-
-// /*
-// * Wrapper functions (to be used in the commander), all functions assume lock on current_status
-// */
-
-// /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR
-// *
-// * START SUBSYSTEM/EMERGENCY FUNCTIONS
-// * */
-
-// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// current_status->onboard_control_sensors_present |= 1 << *subsystem_type;
-// current_status->counter++;
-// current_status->timestamp = hrt_absolute_time();
-// orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-// }
-
-// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type);
-// current_status->counter++;
-// current_status->timestamp = hrt_absolute_time();
-// orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-
-// /* if a subsystem was removed something went completely wrong */
-
-// switch (*subsystem_type) {
-// case SUBSYSTEM_TYPE_GYRO:
-// //global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY);
-// state_machine_emergency_always_critical(status_pub, current_status);
-// break;
-
-// case SUBSYSTEM_TYPE_ACC:
-// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY);
-// state_machine_emergency_always_critical(status_pub, current_status);
-// break;
-
-// case SUBSYSTEM_TYPE_MAG:
-// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY);
-// state_machine_emergency_always_critical(status_pub, current_status);
-// break;
-
-// case SUBSYSTEM_TYPE_GPS:
-// {
-// uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV];
-
-// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
-// //global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY);
-// state_machine_emergency(status_pub, current_status);
-// }
-// }
-// break;
-
-// default:
-// break;
-// }
-
-// }
-
-// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type;
-// current_status->counter++;
-// current_status->timestamp = hrt_absolute_time();
-// orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-// }
-
-// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type);
-// current_status->counter++;
-// current_status->timestamp = hrt_absolute_time();
-// orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-
-// /* if a subsystem was disabled something went completely wrong */
-
-// switch (*subsystem_type) {
-// case SUBSYSTEM_TYPE_GYRO:
-// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY);
-// state_machine_emergency_always_critical(status_pub, current_status);
-// break;
-
-// case SUBSYSTEM_TYPE_ACC:
-// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY);
-// state_machine_emergency_always_critical(status_pub, current_status);
-// break;
-
-// case SUBSYSTEM_TYPE_MAG:
-// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY);
-// state_machine_emergency_always_critical(status_pub, current_status);
-// break;
-
-// case SUBSYSTEM_TYPE_GPS:
-// {
-// uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]);
-
-// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
-// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY);
-// state_machine_emergency(status_pub, current_status);
-// }
-// }
-// break;
-
-// default:
-// break;
-// }
-
-// }
-
-
-// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// current_status->onboard_control_sensors_health |= 1 << *subsystem_type;
-// current_status->counter++;
-// current_status->timestamp = hrt_absolute_time();
-// orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-
-// switch (*subsystem_type) {
-// case SUBSYSTEM_TYPE_GYRO:
-// //TODO state machine change (recovering)
-// break;
-
-// case SUBSYSTEM_TYPE_ACC:
-// //TODO state machine change
-// break;
-
-// case SUBSYSTEM_TYPE_MAG:
-// //TODO state machine change
-// break;
-
-// case SUBSYSTEM_TYPE_GPS:
-// //TODO state machine change
-// break;
-
-// default:
-// break;
-// }
-
-
-// }
-
-
-// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// bool previosly_healthy = (bool)(current_status->onboard_control_sensors_health & 1 << *subsystem_type);
-// current_status->onboard_control_sensors_health &= ~(1 << *subsystem_type);
-// current_status->counter++;
-// current_status->timestamp = hrt_absolute_time();
-// orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-
-// /* if we received unhealthy message more than *_HEALTH_COUNTER_LIMIT, switch to error state */
-
-// switch (*subsystem_type) {
-// case SUBSYSTEM_TYPE_GYRO:
-// //global_data_send_mavlink_statustext_message_out("Commander: gyro unhealthy", MAV_SEVERITY_CRITICAL);
-
-// if (previosly_healthy) //only throw emergency if previously healthy
-// state_machine_emergency_always_critical(status_pub, current_status);
-
-// break;
-
-// case SUBSYSTEM_TYPE_ACC:
-// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer unhealthy", MAV_SEVERITY_CRITICAL);
-
-// if (previosly_healthy) //only throw emergency if previously healthy
-// state_machine_emergency_always_critical(status_pub, current_status);
-
-// break;
-
-// case SUBSYSTEM_TYPE_MAG:
-// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer unhealthy", MAV_SEVERITY_CRITICAL);
-
-// if (previosly_healthy) //only throw emergency if previously healthy
-// state_machine_emergency_always_critical(status_pub, current_status);
-
-// break;
-
-// case SUBSYSTEM_TYPE_GPS:
-// // //TODO: remove this block
-// // break;
-// // ///////////////////
-// //global_data_send_mavlink_statustext_message_out("Commander: GPS unhealthy", MAV_SEVERITY_CRITICAL);
-
-// // printf("previosly_healthy = %u\n", previosly_healthy);
-// if (previosly_healthy) //only throw emergency if previously healthy
-// state_machine_emergency(status_pub, current_status);
-
-// break;
-
-// default:
-// break;
-// }
-
-// }
-
-
-/* END SUBSYSTEM/EMERGENCY FUNCTIONS*/
-
-
-void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
- /* Depending on the current state switch state */
- if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
- do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
- }
-}
-
-void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
- /* Depending on the current state switch state */
- if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_AUTO) {
- state_machine_emergency(status_pub, current_status, mavlink_fd);
- }
-}
-
-void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
- if (current_status->state_machine == SYSTEM_STATE_STANDBY) {
- printf("[cmd] arming\n");
- do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
- }
-}
-
-void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
- if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
- printf("[cmd] going standby\n");
- do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
-
- } else if (current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
- printf("[cmd] MISSION ABORT!\n");
- do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
- }
-}
-
-void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
- int old_mode = current_status->flight_mode;
- current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL;
-
- current_status->flag_control_manual_enabled = true;
-
- /* set behaviour based on airframe */
- if ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) ||
- (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
- (current_status->system_type == VEHICLE_TYPE_OCTOROTOR)) {
-
- /* assuming a rotary wing, set to SAS */
- current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
- current_status->flag_control_attitude_enabled = true;
- current_status->flag_control_rates_enabled = true;
-
- } else {
-
- /* assuming a fixed wing, set to direct pass-through */
- current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
- current_status->flag_control_attitude_enabled = false;
- current_status->flag_control_rates_enabled = false;
- }
-
- if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
-
- if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
- printf("[cmd] manual mode\n");
- do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
- }
-}
-
-void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
- if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
- int old_mode = current_status->flight_mode;
- int old_manual_control_mode = current_status->manual_control_mode;
- current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL;
- current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
- current_status->flag_control_attitude_enabled = true;
- current_status->flag_control_rates_enabled = true;
- current_status->flag_control_manual_enabled = true;
-
- if (old_mode != current_status->flight_mode ||
- old_manual_control_mode != current_status->manual_control_mode) {
- printf("[cmd] att stabilized mode\n");
- do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
- state_machine_publish(status_pub, current_status, mavlink_fd);
- }
-
- }
-}
-
-void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
- if (!current_status->flag_vector_flight_mode_ok) {
- mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. GUIDED MODE");
- tune_error();
- return;
- }
-
- if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
- printf("[cmd] position guided mode\n");
- int old_mode = current_status->flight_mode;
- current_status->flight_mode = VEHICLE_FLIGHT_MODE_STAB;
- current_status->flag_control_manual_enabled = false;
- current_status->flag_control_attitude_enabled = true;
- current_status->flag_control_rates_enabled = true;
- do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED);
-
- if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
-
- }
-}
-
-void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
- if (!current_status->flag_vector_flight_mode_ok) {
- mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. AUTO MODE");
- return;
- }
-
- if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) {
- printf("[cmd] auto mode\n");
- int old_mode = current_status->flight_mode;
- current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO;
- current_status->flag_control_manual_enabled = false;
- current_status->flag_control_attitude_enabled = true;
- current_status->flag_control_rates_enabled = true;
- do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO);
-
- if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
- }
-}
-
-
-uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode)
-{
- uint8_t ret = 1;
-
- /* Switch on HIL if in standby and not already in HIL mode */
- if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED)
- && !current_status->flag_hil_enabled) {
- if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) {
- /* Enable HIL on request */
- current_status->flag_hil_enabled = true;
- ret = OK;
- state_machine_publish(status_pub, current_status, mavlink_fd);
- publish_armed_status(current_status);
- printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n");
-
- } else if (current_status->state_machine != SYSTEM_STATE_STANDBY &&
- current_status->flag_system_armed) {
-
- mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!")
-
- } else {
-
- mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.")
- }
- }
-
- /* switch manual / auto */
- if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) {
- update_state_machine_mode_auto(status_pub, current_status, mavlink_fd);
-
- } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) {
- update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd);
-
- } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
- update_state_machine_mode_guided(status_pub, current_status, mavlink_fd);
-
- } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
- update_state_machine_mode_manual(status_pub, current_status, mavlink_fd);
- }
-
- /* vehicle is disarmed, mode requests arming */
- if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
- /* only arm in standby state */
- // XXX REMOVE
- if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
- do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
- ret = OK;
- printf("[cmd] arming due to command request\n");
- }
- }
-
- /* vehicle is armed, mode requests disarming */
- if (current_status->flag_system_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
- /* only disarm in ground ready */
- if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
- do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
- ret = OK;
- printf("[cmd] disarming due to command request\n");
- }
- }
-
- /* NEVER actually switch off HIL without reboot */
- if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
- warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n");
- mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL");
- ret = ERROR;
- }
-
- return ret;
-}
-
-uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode) //TODO: add more checks to avoid state switching in critical situations
-{
- commander_state_machine_t current_system_state = current_status->state_machine;
-
- uint8_t ret = 1;
-
- switch (custom_mode) {
- case SYSTEM_STATE_GROUND_READY:
- break;
-
- case SYSTEM_STATE_STANDBY:
- break;
-
- case SYSTEM_STATE_REBOOT:
- printf("try to reboot\n");
-
- if (current_system_state == SYSTEM_STATE_STANDBY
- || current_system_state == SYSTEM_STATE_PREFLIGHT
- || current_status->flag_hil_enabled) {
- printf("system will reboot\n");
- mavlink_log_critical(mavlink_fd, "Rebooting..");
- usleep(200000);
- do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_REBOOT);
- ret = 0;
- }
-
- break;
-
- case SYSTEM_STATE_AUTO:
- printf("try to switch to auto/takeoff\n");
-
- if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_MANUAL) {
- do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO);
- printf("state: auto\n");
- ret = 0;
- }
-
- break;
-
- case SYSTEM_STATE_MANUAL:
- printf("try to switch to manual\n");
-
- if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_AUTO) {
- do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
- printf("state: manual\n");
- ret = 0;
- }
-
- break;
-
- default:
- break;
- }
-
- return ret;
-}
-
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..674f3feda3978c67684429645e515d5baef56789
--- /dev/null
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -0,0 +1,703 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ * Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ * Julian Oes <joes@student.ethz.ch>
+ *
+ * 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.cpp
+ * State machine helper functions implementations
+ */
+
+#include <stdio.h>
+#include <unistd.h>
+#include <stdint.h>
+#include <stdbool.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+#include <mavlink/mavlink_log.h>
+
+#include "state_machine_helper.h"
+#include "commander_helper.h"
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+static bool arming_state_changed = true;
+static bool main_state_changed = true;
+static bool navigation_state_changed = true;
+
+transition_result_t
+arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, arming_state_t new_arming_state, struct actuator_armed_s *armed)
+{
+ /*
+ * Perform an atomic state update
+ */
+ irqstate_t flags = irqsave();
+
+ transition_result_t ret = TRANSITION_DENIED;
+
+ /* only check transition if the new state is actually different from the current one */
+ if (new_arming_state == status->arming_state) {
+ ret = TRANSITION_NOT_CHANGED;
+
+ } else {
+
+ switch (new_arming_state) {
+ case ARMING_STATE_INIT:
+
+ /* allow going back from INIT for calibration */
+ if (status->arming_state == ARMING_STATE_STANDBY) {
+ ret = TRANSITION_CHANGED;
+ armed->armed = false;
+ armed->ready_to_arm = false;
+ }
+
+ break;
+
+ case ARMING_STATE_STANDBY:
+
+ /* allow coming from INIT and disarming from ARMED */
+ if (status->arming_state == ARMING_STATE_INIT
+ || status->arming_state == ARMING_STATE_ARMED) {
+
+ /* sensors need to be initialized for STANDBY state */
+ if (status->condition_system_sensors_initialized) {
+ ret = TRANSITION_CHANGED;
+ armed->armed = false;
+ armed->ready_to_arm = true;
+ }
+ }
+
+ break;
+
+ case ARMING_STATE_ARMED:
+
+ /* allow arming from STANDBY and IN-AIR-RESTORE */
+ if ((status->arming_state == ARMING_STATE_STANDBY
+ || status->arming_state == ARMING_STATE_IN_AIR_RESTORE)
+ && (!safety->safety_switch_available || safety->safety_off)) { /* only allow arming if safety is off */
+ ret = TRANSITION_CHANGED;
+ armed->armed = true;
+ armed->ready_to_arm = true;
+ }
+
+ break;
+
+ case ARMING_STATE_ARMED_ERROR:
+
+ /* an armed error happens when ARMED obviously */
+ if (status->arming_state == ARMING_STATE_ARMED) {
+ ret = TRANSITION_CHANGED;
+ armed->armed = true;
+ armed->ready_to_arm = false;
+ }
+
+ break;
+
+ case ARMING_STATE_STANDBY_ERROR:
+
+ /* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */
+ if (status->arming_state == ARMING_STATE_STANDBY
+ || status->arming_state == ARMING_STATE_INIT
+ || status->arming_state == ARMING_STATE_ARMED_ERROR) {
+ ret = TRANSITION_CHANGED;
+ armed->armed = false;
+ armed->ready_to_arm = false;
+ }
+
+ break;
+
+ case ARMING_STATE_REBOOT:
+
+ /* an armed error happens when ARMED obviously */
+ if (status->arming_state == ARMING_STATE_INIT
+ || status->arming_state == ARMING_STATE_STANDBY
+ || status->arming_state == ARMING_STATE_STANDBY_ERROR) {
+ ret = TRANSITION_CHANGED;
+ armed->armed = false;
+ armed->ready_to_arm = false;
+ }
+
+ break;
+
+ case ARMING_STATE_IN_AIR_RESTORE:
+
+ /* XXX implement */
+ break;
+
+ default:
+ break;
+ }
+
+ if (ret == TRANSITION_CHANGED) {
+ status->arming_state = new_arming_state;
+ arming_state_changed = true;
+ }
+ }
+
+ /* end of atomic state update */
+ irqrestore(flags);
+
+ if (ret == TRANSITION_DENIED)
+ warnx("arming transition rejected");
+
+ return ret;
+}
+
+bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safety, const struct actuator_armed_s *armed)
+{
+ // System is safe if:
+ // 1) Not armed
+ // 2) Armed, but in software lockdown (HIL)
+ // 3) Safety switch is present AND engaged -> actuators locked
+ if (!armed->armed || (armed->armed && armed->lockdown) || (safety->safety_switch_available && !safety->safety_off)) {
+ return true;
+
+ } else {
+ return false;
+ }
+}
+
+bool
+check_arming_state_changed()
+{
+ if (arming_state_changed) {
+ arming_state_changed = false;
+ return true;
+
+ } else {
+ return false;
+ }
+}
+
+transition_result_t
+main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state)
+{
+ transition_result_t ret = TRANSITION_DENIED;
+
+ /* only check transition if the new state is actually different from the current one */
+ if (new_main_state == current_state->main_state) {
+ ret = TRANSITION_NOT_CHANGED;
+
+ } else {
+
+ switch (new_main_state) {
+ case MAIN_STATE_MANUAL:
+ ret = TRANSITION_CHANGED;
+ break;
+
+ case MAIN_STATE_SEATBELT:
+
+ /* need altitude estimate */
+ if (current_state->condition_local_altitude_valid) {
+ ret = TRANSITION_CHANGED;
+ }
+
+ break;
+
+ case MAIN_STATE_EASY:
+
+ /* need local position estimate */
+ if (current_state->condition_local_position_valid) {
+ ret = TRANSITION_CHANGED;
+ }
+
+ break;
+
+ case MAIN_STATE_AUTO:
+
+ /* need global position estimate */
+ if (current_state->condition_global_position_valid) {
+ ret = TRANSITION_CHANGED;
+ }
+
+ break;
+ }
+
+ if (ret == TRANSITION_CHANGED) {
+ current_state->main_state = new_main_state;
+ main_state_changed = true;
+ }
+ }
+
+ return ret;
+}
+
+bool
+check_main_state_changed()
+{
+ if (main_state_changed) {
+ main_state_changed = false;
+ return true;
+
+ } else {
+ return false;
+ }
+}
+
+transition_result_t
+navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode)
+{
+ transition_result_t ret = TRANSITION_DENIED;
+
+ /* only check transition if the new state is actually different from the current one */
+ if (new_navigation_state == status->navigation_state) {
+ ret = TRANSITION_NOT_CHANGED;
+
+ } else {
+
+ switch (new_navigation_state) {
+ case NAVIGATION_STATE_DIRECT:
+ ret = TRANSITION_CHANGED;
+ control_mode->flag_control_rates_enabled = true;
+ control_mode->flag_control_attitude_enabled = false;
+ control_mode->flag_control_velocity_enabled = false;
+ control_mode->flag_control_position_enabled = false;
+ control_mode->flag_control_altitude_enabled = false;
+ control_mode->flag_control_climb_rate_enabled = false;
+ control_mode->flag_control_manual_enabled = true;
+ control_mode->flag_control_auto_enabled = false;
+ break;
+
+ case NAVIGATION_STATE_STABILIZE:
+ ret = TRANSITION_CHANGED;
+ control_mode->flag_control_rates_enabled = true;
+ control_mode->flag_control_attitude_enabled = true;
+ control_mode->flag_control_velocity_enabled = false;
+ control_mode->flag_control_position_enabled = false;
+ control_mode->flag_control_altitude_enabled = false;
+ control_mode->flag_control_climb_rate_enabled = false;
+ control_mode->flag_control_manual_enabled = true;
+ control_mode->flag_control_auto_enabled = false;
+ break;
+
+ case NAVIGATION_STATE_ALTHOLD:
+ ret = TRANSITION_CHANGED;
+ control_mode->flag_control_rates_enabled = true;
+ control_mode->flag_control_attitude_enabled = true;
+ control_mode->flag_control_velocity_enabled = false;
+ control_mode->flag_control_position_enabled = false;
+ control_mode->flag_control_altitude_enabled = true;
+ control_mode->flag_control_climb_rate_enabled = true;
+ control_mode->flag_control_manual_enabled = true;
+ control_mode->flag_control_auto_enabled = false;
+ break;
+
+ case NAVIGATION_STATE_VECTOR:
+ ret = TRANSITION_CHANGED;
+ control_mode->flag_control_rates_enabled = true;
+ control_mode->flag_control_attitude_enabled = true;
+ control_mode->flag_control_velocity_enabled = true;
+ control_mode->flag_control_position_enabled = true;
+ control_mode->flag_control_altitude_enabled = true;
+ control_mode->flag_control_climb_rate_enabled = true;
+ control_mode->flag_control_manual_enabled = true;
+ control_mode->flag_control_auto_enabled = false;
+ break;
+
+ case NAVIGATION_STATE_AUTO_READY:
+ ret = TRANSITION_CHANGED;
+ control_mode->flag_control_rates_enabled = false;
+ control_mode->flag_control_attitude_enabled = false;
+ control_mode->flag_control_velocity_enabled = false;
+ control_mode->flag_control_position_enabled = false;
+ control_mode->flag_control_altitude_enabled = false;
+ control_mode->flag_control_climb_rate_enabled = false;
+ control_mode->flag_control_manual_enabled = false;
+ control_mode->flag_control_auto_enabled = true;
+ break;
+
+ case NAVIGATION_STATE_AUTO_TAKEOFF:
+
+ /* only transitions from AUTO_READY */
+ if (status->navigation_state == NAVIGATION_STATE_AUTO_READY) {
+ ret = TRANSITION_CHANGED;
+ control_mode->flag_control_rates_enabled = true;
+ control_mode->flag_control_attitude_enabled = true;
+ control_mode->flag_control_velocity_enabled = true;
+ control_mode->flag_control_position_enabled = true;
+ control_mode->flag_control_altitude_enabled = true;
+ control_mode->flag_control_climb_rate_enabled = true;
+ control_mode->flag_control_manual_enabled = false;
+ control_mode->flag_control_auto_enabled = true;
+ }
+
+ break;
+
+ case NAVIGATION_STATE_AUTO_LOITER:
+ ret = TRANSITION_CHANGED;
+ control_mode->flag_control_rates_enabled = true;
+ control_mode->flag_control_attitude_enabled = true;
+ control_mode->flag_control_velocity_enabled = true;
+ control_mode->flag_control_position_enabled = true;
+ control_mode->flag_control_altitude_enabled = true;
+ control_mode->flag_control_climb_rate_enabled = true;
+ control_mode->flag_control_manual_enabled = false;
+ control_mode->flag_control_auto_enabled = false;
+ break;
+
+ case NAVIGATION_STATE_AUTO_MISSION:
+ ret = TRANSITION_CHANGED;
+ control_mode->flag_control_rates_enabled = true;
+ control_mode->flag_control_attitude_enabled = true;
+ control_mode->flag_control_velocity_enabled = true;
+ control_mode->flag_control_position_enabled = true;
+ control_mode->flag_control_altitude_enabled = true;
+ control_mode->flag_control_climb_rate_enabled = true;
+ control_mode->flag_control_manual_enabled = false;
+ control_mode->flag_control_auto_enabled = true;
+ break;
+
+ case NAVIGATION_STATE_AUTO_RTL:
+ ret = TRANSITION_CHANGED;
+ control_mode->flag_control_rates_enabled = true;
+ control_mode->flag_control_attitude_enabled = true;
+ control_mode->flag_control_velocity_enabled = true;
+ control_mode->flag_control_position_enabled = true;
+ control_mode->flag_control_altitude_enabled = true;
+ control_mode->flag_control_climb_rate_enabled = true;
+ control_mode->flag_control_manual_enabled = false;
+ control_mode->flag_control_auto_enabled = true;
+ break;
+
+ case NAVIGATION_STATE_AUTO_LAND:
+
+ /* deny transitions from landed state */
+ if (status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
+ ret = TRANSITION_CHANGED;
+ control_mode->flag_control_rates_enabled = true;
+ control_mode->flag_control_attitude_enabled = true;
+ control_mode->flag_control_velocity_enabled = true;
+ control_mode->flag_control_position_enabled = true;
+ control_mode->flag_control_altitude_enabled = true;
+ control_mode->flag_control_climb_rate_enabled = true;
+ control_mode->flag_control_manual_enabled = false;
+ control_mode->flag_control_auto_enabled = true;
+ }
+
+ break;
+
+ default:
+ break;
+ }
+
+ if (ret == TRANSITION_CHANGED) {
+ status->navigation_state = new_navigation_state;
+ control_mode->auto_state = status->navigation_state;
+ navigation_state_changed = true;
+ }
+ }
+
+ return ret;
+}
+
+bool
+check_navigation_state_changed()
+{
+ if (navigation_state_changed) {
+ navigation_state_changed = false;
+ return true;
+
+ } else {
+ return false;
+ }
+}
+
+void
+set_navigation_state_changed()
+{
+ navigation_state_changed = true;
+}
+
+/**
+* Transition from one hil state to another
+*/
+int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd)
+{
+ bool valid_transition = false;
+ int ret = ERROR;
+
+ warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state);
+
+ if (current_status->hil_state == new_state) {
+ warnx("Hil state not changed");
+ valid_transition = true;
+
+ } else {
+
+ switch (new_state) {
+
+ case HIL_STATE_OFF:
+
+ if (current_status->arming_state == ARMING_STATE_INIT
+ || current_status->arming_state == ARMING_STATE_STANDBY) {
+
+ current_control_mode->flag_system_hil_enabled = false;
+ mavlink_log_critical(mavlink_fd, "Switched to OFF hil state");
+ valid_transition = true;
+ }
+
+ break;
+
+ case HIL_STATE_ON:
+
+ if (current_status->arming_state == ARMING_STATE_INIT
+ || current_status->arming_state == ARMING_STATE_STANDBY) {
+
+ current_control_mode->flag_system_hil_enabled = true;
+ mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
+ valid_transition = true;
+ }
+
+ break;
+
+ default:
+ warnx("Unknown hil state");
+ break;
+ }
+ }
+
+ if (valid_transition) {
+ current_status->hil_state = new_state;
+
+ current_status->counter++;
+ current_status->timestamp = hrt_absolute_time();
+ orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
+
+ current_control_mode->timestamp = hrt_absolute_time();
+ orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode);
+
+ ret = OK;
+
+ } else {
+ mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition");
+ }
+
+ return ret;
+}
+
+
+
+// /*
+// * Wrapper functions (to be used in the commander), all functions assume lock on current_status
+// */
+
+// /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR
+// *
+// * START SUBSYSTEM/EMERGENCY FUNCTIONS
+// * */
+
+// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
+// {
+// current_status->onboard_control_sensors_present |= 1 << *subsystem_type;
+// current_status->counter++;
+// current_status->timestamp = hrt_absolute_time();
+// orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
+// }
+
+// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
+// {
+// current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type);
+// current_status->counter++;
+// current_status->timestamp = hrt_absolute_time();
+// orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
+
+// /* if a subsystem was removed something went completely wrong */
+
+// switch (*subsystem_type) {
+// case SUBSYSTEM_TYPE_GYRO:
+// //global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY);
+// state_machine_emergency_always_critical(status_pub, current_status);
+// break;
+
+// case SUBSYSTEM_TYPE_ACC:
+// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY);
+// state_machine_emergency_always_critical(status_pub, current_status);
+// break;
+
+// case SUBSYSTEM_TYPE_MAG:
+// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY);
+// state_machine_emergency_always_critical(status_pub, current_status);
+// break;
+
+// case SUBSYSTEM_TYPE_GPS:
+// {
+// uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV];
+
+// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
+// //global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY);
+// state_machine_emergency(status_pub, current_status);
+// }
+// }
+// break;
+
+// default:
+// break;
+// }
+
+// }
+
+// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
+// {
+// current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type;
+// current_status->counter++;
+// current_status->timestamp = hrt_absolute_time();
+// orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
+// }
+
+// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
+// {
+// current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type);
+// current_status->counter++;
+// current_status->timestamp = hrt_absolute_time();
+// orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
+
+// /* if a subsystem was disabled something went completely wrong */
+
+// switch (*subsystem_type) {
+// case SUBSYSTEM_TYPE_GYRO:
+// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY);
+// state_machine_emergency_always_critical(status_pub, current_status);
+// break;
+
+// case SUBSYSTEM_TYPE_ACC:
+// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY);
+// state_machine_emergency_always_critical(status_pub, current_status);
+// break;
+
+// case SUBSYSTEM_TYPE_MAG:
+// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY);
+// state_machine_emergency_always_critical(status_pub, current_status);
+// break;
+
+// case SUBSYSTEM_TYPE_GPS:
+// {
+// uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]);
+
+// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
+// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY);
+// state_machine_emergency(status_pub, current_status);
+// }
+// }
+// break;
+
+// default:
+// break;
+// }
+
+// }
+
+
+///* END SUBSYSTEM/EMERGENCY FUNCTIONS*/
+//
+//int update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode)
+//{
+// int ret = 1;
+//
+//// /* Switch on HIL if in standby and not already in HIL mode */
+//// if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED)
+//// && !current_status->flag_hil_enabled) {
+//// if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) {
+//// /* Enable HIL on request */
+//// current_status->flag_hil_enabled = true;
+//// ret = OK;
+//// state_machine_publish(status_pub, current_status, mavlink_fd);
+//// publish_armed_status(current_status);
+//// printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n");
+////
+//// } else if (current_status->state_machine != SYSTEM_STATE_STANDBY &&
+//// current_status->flag_fmu_armed) {
+////
+//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!")
+////
+//// } else {
+////
+//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.")
+//// }
+//// }
+//
+// /* switch manual / auto */
+// if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) {
+// update_state_machine_mode_auto(status_pub, current_status, mavlink_fd);
+//
+// } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) {
+// update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd);
+//
+// } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
+// update_state_machine_mode_guided(status_pub, current_status, mavlink_fd);
+//
+// } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
+// update_state_machine_mode_manual(status_pub, current_status, mavlink_fd);
+// }
+//
+// /* vehicle is disarmed, mode requests arming */
+// if (!(current_status->flag_fmu_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
+// /* only arm in standby state */
+// // XXX REMOVE
+// if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
+// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
+// ret = OK;
+// printf("[cmd] arming due to command request\n");
+// }
+// }
+//
+// /* vehicle is armed, mode requests disarming */
+// if (current_status->flag_fmu_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
+// /* only disarm in ground ready */
+// if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
+// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
+// ret = OK;
+// printf("[cmd] disarming due to command request\n");
+// }
+// }
+//
+// /* NEVER actually switch off HIL without reboot */
+// if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
+// warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n");
+// mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL");
+// ret = ERROR;
+// }
+//
+// return ret;
+//}
+
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index 2f2ccc7298e55c159c753cd9f365ab5ec15c528b..1641b6f60b26a4663efe28e6578f04bfd16174c6 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
*
@@ -46,164 +46,34 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/safety.h>
+#include <uORB/topics/vehicle_control_mode.h>
-/**
- * Switch to new state with no checking.
- *
- * do_state_update: this is the functions that all other functions have to call in order to update the state.
- * the function does not question the state change, this must be done before
- * The function performs actions that are connected with the new state (buzzer, reboot, ...)
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- *
- * @return 0 (macro OK) or 1 on error (macro ERROR)
- */
-int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state);
+typedef enum {
+ TRANSITION_DENIED = -1,
+ TRANSITION_NOT_CHANGED = 0,
+ TRANSITION_CHANGED
-/* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR */
-// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
-// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
+} transition_result_t;
-// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
-// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
+transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
+ arming_state_t new_arming_state, struct actuator_armed_s *armed);
-// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
-// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
+bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
+bool check_arming_state_changed();
-/**
- * Handle state machine if got position fix
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
+transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state);
-/**
- * Handle state machine if position fix lost
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
+bool check_main_state_changed();
-/**
- * Handle state machine if user wants to arm
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
+transition_result_t navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode);
-/**
- * Handle state machine if user wants to disarm
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Handle state machine if mode switch is manual
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Handle state machine if mode switch is stabilized
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Handle state machine if mode switch is guided
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Handle state machine if mode switch is auto
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Publish current state information
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-
-/*
- * Functions that handle incoming requests to change the state machine or a parameter (probably from the mavlink app).
- * If the request is obeyed the functions return 0
- *
- */
-
-/**
- * Handles *incoming request* to switch to a specific state, if state change is successful returns 0
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode);
-
-/**
- * Handles *incoming request* to switch to a specific custom state, if state change is successful returns 0
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode);
-
-/**
- * Always switches to critical mode under any circumstances.
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Switches to emergency if required.
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Publish the armed state depending on the current system state
- *
- * @param current_status the current system status
- */
-void publish_armed_status(const struct vehicle_status_s *current_status);
+bool check_navigation_state_changed();
+void set_navigation_state_changed();
+int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd);
#endif /* STATE_MACHINE_HELPER_H_ */
diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp
index 9c0720aa5e87df8462d8538b3917806e2d906db9..46dc1bec25b5c334507c96cb2316436a0e35a719 100644
--- a/src/modules/controllib/uorb/blocks.hpp
+++ b/src/modules/controllib/uorb/blocks.hpp
@@ -58,7 +58,7 @@
#include <poll.h>
extern "C" {
-#include <systemlib/geo/geo.h>
+#include <geo/geo.h>
}
#include "../blocks.hpp"
diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c b/src/modules/fixedwing_att_control/fixedwing_att_control_main.c
index 6c9c137bb12e025f129fc42fce5b50510f7cf0ad..b6b4546c238fb45bc860e9d0613dda88b964a920 100644
--- a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c
+++ b/src/modules/fixedwing_att_control/fixedwing_att_control_main.c
@@ -53,6 +53,7 @@
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
@@ -116,6 +117,8 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
memset(&global_pos, 0, sizeof(global_pos));
struct manual_control_setpoint_s manual_sp;
memset(&manual_sp, 0, sizeof(manual_sp));
+ struct vehicle_control_mode_s control_mode;
+ memset(&control_mode, 0, sizeof(control_mode));
struct vehicle_status_s vstatus;
memset(&vstatus, 0, sizeof(vstatus));
@@ -137,7 +140,8 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
- int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
+ int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
+ int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
/* Setup of loop */
float gyro[3] = {0.0f, 0.0f, 0.0f};
@@ -178,30 +182,16 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
}
orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
- orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
+ orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
+ orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
gyro[0] = att.rollspeed;
gyro[1] = att.pitchspeed;
gyro[2] = att.yawspeed;
- /* control */
-
- if (vstatus.state_machine == SYSTEM_STATE_AUTO ||
- vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
- /* attitude control */
- fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
-
- /* angular rate control */
- fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
-
- /* pass through throttle */
- actuators.control[3] = att_sp.thrust;
-
- /* set flaps to zero */
- actuators.control[4] = 0.0f;
-
- } else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
- if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
+ /* set manual setpoints if required */
+ if (control_mode.flag_control_manual_enabled) {
+ if (control_mode.flag_control_attitude_enabled) {
/* if the RC signal is lost, try to stay level and go slowly back down to ground */
if (vstatus.rc_signal_lost) {
@@ -234,15 +224,6 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
att_sp.timestamp = hrt_absolute_time();
- /* attitude control */
- fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
-
- /* angular rate control */
- fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
-
- /* pass through throttle */
- actuators.control[3] = att_sp.thrust;
-
/* pass through flaps */
if (isfinite(manual_sp.flaps)) {
actuators.control[4] = manual_sp.flaps;
@@ -251,7 +232,7 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
actuators.control[4] = 0.0f;
}
- } else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
+ } else {
/* directly pass through values */
actuators.control[0] = manual_sp.roll;
/* positive pitch means negative actuator -> pull up */
@@ -267,6 +248,22 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
}
}
}
+
+ /* execute attitude control if requested */
+ if (control_mode.flag_control_attitude_enabled) {
+ /* attitude control */
+ fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
+
+ /* angular rate control */
+ fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
+
+ /* pass through throttle */
+ actuators.control[3] = att_sp.thrust;
+
+ /* set flaps to zero */
+ actuators.control[4] = 0.0f;
+
+ }
/* publish rates */
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp
index f655a13bd7d6843a02729dc739387c75b999da58..d65045d6879d537745873b1dcb24c73c21034942 100644
--- a/src/modules/fixedwing_backside/fixedwing.cpp
+++ b/src/modules/fixedwing_backside/fixedwing.cpp
@@ -156,7 +156,7 @@ void BlockMultiModeBacksideAutopilot::update()
_actuators.control[i] = 0.0f;
// only update guidance in auto mode
- if (_status.state_machine == SYSTEM_STATE_AUTO) {
+ if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) { // TODO use vehicle_control_mode here?
// update guidance
_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
}
@@ -166,8 +166,8 @@ void BlockMultiModeBacksideAutopilot::update()
// the setpoint should update to loitering around this position
// handle autopilot modes
- if (_status.state_machine == SYSTEM_STATE_AUTO ||
- _status.state_machine == SYSTEM_STATE_STABILIZED) {
+ if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION ||
+ _status.navigation_state == NAVIGATION_STATE_STABILIZE) { // TODO use vehicle_control_mode here?
// update guidance
_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
@@ -219,89 +219,83 @@ void BlockMultiModeBacksideAutopilot::update()
// This is not a hack, but a design choice.
/* do not limit in HIL */
- if (!_status.flag_hil_enabled) {
+ if (_status.hil_state != HIL_STATE_ON) {
/* limit to value of manual throttle */
_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
_actuators.control[CH_THR] : _manual.throttle;
}
- } else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
-
- if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
- _actuators.control[CH_AIL] = _manual.roll;
- _actuators.control[CH_ELV] = _manual.pitch;
- _actuators.control[CH_RDR] = _manual.yaw;
- _actuators.control[CH_THR] = _manual.throttle;
-
- } else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
-
- // calculate velocity, XXX should be airspeed, but using ground speed for now
- // for the purpose of control we will limit the velocity feedback between
- // the min/max velocity
- float v = _vLimit.update(sqrtf(
- _pos.vx * _pos.vx +
- _pos.vy * _pos.vy +
- _pos.vz * _pos.vz));
-
- // pitch channel -> rate of climb
- // TODO, might want to put a gain on this, otherwise commanding
- // from +1 -> -1 m/s for rate of climb
- //float dThrottle = _cr2Thr.update(
- //_crMax.get()*_manual.pitch - _pos.vz);
-
- // roll channel -> bank angle
- float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax());
- float pCmd = _phi2P.update(phiCmd - _att.roll);
-
- // throttle channel -> velocity
- // negative sign because nose over to increase speed
- float vCmd = _vLimit.update(_manual.throttle *
- (_vLimit.getMax() - _vLimit.getMin()) +
- _vLimit.getMin());
- float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
- float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
-
- // yaw rate cmd
- float rCmd = 0;
-
- // stabilization
- _stabilization.update(pCmd, qCmd, rCmd,
- _att.rollspeed, _att.pitchspeed, _att.yawspeed);
-
- // output
- _actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
- _actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
- _actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
-
- // currently using manual throttle
- // XXX if you enable this watch out, vz might be very noisy
- //_actuators.control[CH_THR] = dThrottle + _trimThr.get();
- _actuators.control[CH_THR] = _manual.throttle;
-
- // XXX limit throttle to manual setting (safety) for now.
- // If it turns out to be confusing, it can be removed later once
- // a first binary release can be targeted.
- // This is not a hack, but a design choice.
-
- /* do not limit in HIL */
- if (!_status.flag_hil_enabled) {
- /* limit to value of manual throttle */
- _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
- _actuators.control[CH_THR] : _manual.throttle;
- }
- }
+ } else if (_status.navigation_state == NAVIGATION_STATE_DIRECT) { // TODO use vehicle_control_mode here?
+ _actuators.control[CH_AIL] = _manual.roll;
+ _actuators.control[CH_ELV] = _manual.pitch;
+ _actuators.control[CH_RDR] = _manual.yaw;
+ _actuators.control[CH_THR] = _manual.throttle;
+ } else if (_status.navigation_state == NAVIGATION_STATE_STABILIZE) { // TODO use vehicle_control_mode here?
+ // calculate velocity, XXX should be airspeed, but using ground speed for now
+ // for the purpose of control we will limit the velocity feedback between
+ // the min/max velocity
+ float v = _vLimit.update(sqrtf(
+ _pos.vx * _pos.vx +
+ _pos.vy * _pos.vy +
+ _pos.vz * _pos.vz));
+
+ // pitch channel -> rate of climb
+ // TODO, might want to put a gain on this, otherwise commanding
+ // from +1 -> -1 m/s for rate of climb
+ //float dThrottle = _cr2Thr.update(
+ //_crMax.get()*_manual.pitch - _pos.vz);
- // body rates controller, disabled for now
- else if (0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) {
+ // roll channel -> bank angle
+ float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax());
+ float pCmd = _phi2P.update(phiCmd - _att.roll);
- _stabilization.update(_manual.roll, _manual.pitch, _manual.yaw,
- _att.rollspeed, _att.pitchspeed, _att.yawspeed);
+ // throttle channel -> velocity
+ // negative sign because nose over to increase speed
+ float vCmd = _vLimit.update(_manual.throttle *
+ (_vLimit.getMax() - _vLimit.getMin()) +
+ _vLimit.getMin());
+ float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
+ float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
- _actuators.control[CH_AIL] = _stabilization.getAileron();
- _actuators.control[CH_ELV] = _stabilization.getElevator();
- _actuators.control[CH_RDR] = _stabilization.getRudder();
- _actuators.control[CH_THR] = _manual.throttle;
+ // yaw rate cmd
+ float rCmd = 0;
+
+ // stabilization
+ _stabilization.update(pCmd, qCmd, rCmd,
+ _att.rollspeed, _att.pitchspeed, _att.yawspeed);
+
+ // output
+ _actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
+ _actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
+ _actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
+
+ // currently using manual throttle
+ // XXX if you enable this watch out, vz might be very noisy
+ //_actuators.control[CH_THR] = dThrottle + _trimThr.get();
+ _actuators.control[CH_THR] = _manual.throttle;
+
+ // XXX limit throttle to manual setting (safety) for now.
+ // If it turns out to be confusing, it can be removed later once
+ // a first binary release can be targeted.
+ // This is not a hack, but a design choice.
+
+ /* do not limit in HIL */
+ if (_status.hil_state != HIL_STATE_ON) {
+ /* limit to value of manual throttle */
+ _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
+ _actuators.control[CH_THR] : _manual.throttle;
}
+ // body rates controller, disabled for now
+ // TODO
+ } else if (0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) { // TODO use vehicle_control_mode here?
+
+ _stabilization.update(_manual.roll, _manual.pitch, _manual.yaw,
+ _att.rollspeed, _att.pitchspeed, _att.yawspeed);
+
+ _actuators.control[CH_AIL] = _stabilization.getAileron();
+ _actuators.control[CH_ELV] = _stabilization.getElevator();
+ _actuators.control[CH_RDR] = _stabilization.getRudder();
+ _actuators.control[CH_THR] = _manual.throttle;
}
// update all publications
diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c
index 1aef739c7b2af045e39e261cd0898115e93b7ffa..d383146f9913d5cf1fec1bd954afb9dac061b73f 100644
--- a/src/modules/gpio_led/gpio_led.c
+++ b/src/modules/gpio_led/gpio_led.c
@@ -51,6 +51,7 @@
#include <systemlib/err.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_armed.h>
#include <poll.h>
#include <drivers/drv_gpio.h>
#include <modules/px4iofirmware/protocol.h>
@@ -62,6 +63,8 @@ struct gpio_led_s {
int pin;
struct vehicle_status_s status;
int vehicle_status_sub;
+ struct actuator_armed_s armed;
+ int actuator_armed_sub;
bool led_state;
int counter;
};
@@ -109,19 +112,19 @@ int gpio_led_main(int argc, char *argv[])
} else if (!strcmp(argv[3], "a1")) {
use_io = true;
- pin = PX4IO_ACC1;
+ pin = PX4IO_P_SETUP_RELAYS_ACC1;
} else if (!strcmp(argv[3], "a2")) {
use_io = true;
- pin = PX4IO_ACC2;
+ pin = PX4IO_P_SETUP_RELAYS_ACC2;
} else if (!strcmp(argv[3], "r1")) {
use_io = true;
- pin = PX4IO_RELAY1;
+ pin = PX4IO_P_SETUP_RELAYS_POWER1;
} else if (!strcmp(argv[3], "r2")) {
use_io = true;
- pin = PX4IO_RELAY2;
+ pin = PX4IO_P_SETUP_RELAYS_POWER2;
} else {
errx(1, "unsupported pin: %s", argv[3]);
@@ -142,7 +145,7 @@ int gpio_led_main(int argc, char *argv[])
char pin_name[24];
if (use_io) {
- if (pin & (PX4IO_ACC1 | PX4IO_ACC2)) {
+ if (pin & (PX4IO_P_SETUP_RELAYS_ACC1 | PX4IO_P_SETUP_RELAYS_ACC2)) {
sprintf(pin_name, "PX4IO ACC%i", (pin >> 3));
} else {
@@ -227,10 +230,15 @@ void gpio_led_cycle(FAR void *arg)
if (status_updated)
orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status);
+ orb_check(priv->vehicle_status_sub, &status_updated);
+
+ if (status_updated)
+ orb_copy(ORB_ID(actuator_armed), priv->actuator_armed_sub, &priv->armed);
+
/* select pattern for current status */
int pattern = 0;
- if (priv->status.flag_system_armed) {
+ if (priv->armed.armed) {
if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
pattern = 0x3f; // ****** solid (armed)
@@ -239,11 +247,10 @@ void gpio_led_cycle(FAR void *arg)
}
} else {
- if (priv->status.state_machine == SYSTEM_STATE_PREFLIGHT) {
+ if (priv->armed.ready_to_arm) {
pattern = 0x00; // ______ off (disarmed, preflight check)
- } else if (priv->status.state_machine == SYSTEM_STATE_STANDBY &&
- priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
+ } else if (priv->armed.ready_to_arm && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
pattern = 0x38; // ***___ slow blink (disarmed, ready)
} else {
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index 919d01561cd10c6dac1665fc7f88a52429260d2c..d7b0fa9c7ea452c1dde3f0448e91634f437f67ca 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -64,6 +64,7 @@
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
#include <mavlink/mavlink_log.h>
+#include <commander/px4_custom_mode.h>
#include "waypoints.h"
#include "orb_topics.h"
@@ -181,102 +182,80 @@ set_hil_on_off(bool hil_enabled)
}
void
-get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
+get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode)
{
/* reset MAVLink mode bitfield */
- *mavlink_mode = 0;
+ *mavlink_base_mode = 0;
+ *mavlink_custom_mode = 0;
- /* set mode flags independent of system state */
+ /**
+ * Set mode flags
+ **/
/* HIL */
- if (v_status.flag_hil_enabled) {
- *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED;
+ if (v_status.hil_state == HIL_STATE_ON) {
+ *mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
}
- /* manual input */
- if (v_status.flag_control_manual_enabled) {
- *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+ /* arming state */
+ if (v_status.arming_state == ARMING_STATE_ARMED
+ || v_status.arming_state == ARMING_STATE_ARMED_ERROR) {
+ *mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
}
- /* attitude or rate control */
- if (v_status.flag_control_attitude_enabled ||
- v_status.flag_control_rates_enabled) {
- *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
- }
-
- /* vector control */
- if (v_status.flag_control_velocity_enabled ||
- v_status.flag_control_position_enabled) {
- *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
- }
-
- /* autonomous mode */
- if (v_status.state_machine == SYSTEM_STATE_AUTO) {
- *mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
- }
-
- /* set arming state */
- if (armed.armed) {
- *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
-
- } else {
- *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
- }
-
- switch (v_status.state_machine) {
- case SYSTEM_STATE_PREFLIGHT:
- if (v_status.flag_preflight_gyro_calibration ||
- v_status.flag_preflight_mag_calibration ||
- v_status.flag_preflight_accel_calibration) {
- *mavlink_state = MAV_STATE_CALIBRATING;
-
- } else {
- *mavlink_state = MAV_STATE_UNINIT;
+ /* 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);
+ 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;
+ 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;
+ 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;
+ 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;
}
-
- break;
-
- case SYSTEM_STATE_STANDBY:
- *mavlink_state = MAV_STATE_STANDBY;
- break;
-
- case SYSTEM_STATE_GROUND_READY:
- *mavlink_state = MAV_STATE_ACTIVE;
- break;
-
- case SYSTEM_STATE_MANUAL:
- *mavlink_state = MAV_STATE_ACTIVE;
- break;
-
- case SYSTEM_STATE_STABILIZED:
- *mavlink_state = MAV_STATE_ACTIVE;
- break;
-
- case SYSTEM_STATE_AUTO:
+ }
+ *mavlink_custom_mode = custom_mode.data;
+
+ /**
+ * Set mavlink state
+ **/
+
+ /* set calibration state */
+ if (v_status.arming_state == ARMING_STATE_INIT
+ || v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE
+ || v_status.arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review
+ *mavlink_state = MAV_STATE_UNINIT;
+ } else if (v_status.arming_state == ARMING_STATE_ARMED) {
*mavlink_state = MAV_STATE_ACTIVE;
- break;
-
- case SYSTEM_STATE_MISSION_ABORT:
- *mavlink_state = MAV_STATE_EMERGENCY;
- break;
-
- case SYSTEM_STATE_EMCY_LANDING:
- *mavlink_state = MAV_STATE_EMERGENCY;
- break;
-
- case SYSTEM_STATE_EMCY_CUTOFF:
- *mavlink_state = MAV_STATE_EMERGENCY;
- break;
-
- case SYSTEM_STATE_GROUND_ERROR:
- *mavlink_state = MAV_STATE_EMERGENCY;
- break;
-
- case SYSTEM_STATE_REBOOT:
+ } else if (v_status.arming_state == ARMING_STATE_ARMED_ERROR) {
+ *mavlink_state = MAV_STATE_CRITICAL;
+ } else if (v_status.arming_state == ARMING_STATE_STANDBY) {
+ *mavlink_state = MAV_STATE_STANDBY;
+ } else if (v_status.arming_state == ARMING_STATE_REBOOT) {
*mavlink_state = MAV_STATE_POWEROFF;
- break;
+ } else {
+ warnx("Unknown mavlink state");
+ *mavlink_state = MAV_STATE_CRITICAL;
}
-
}
@@ -312,6 +291,9 @@ int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_m
orb_set_interval(subs->act_2_sub, min_interval);
orb_set_interval(subs->act_3_sub, min_interval);
orb_set_interval(subs->actuators_sub, min_interval);
+ orb_set_interval(subs->actuators_effective_sub, min_interval);
+ orb_set_interval(subs->spa_sub, min_interval);
+ orb_set_interval(mavlink_subs.rates_setpoint_sub, min_interval);
break;
case MAVLINK_MSG_ID_MANUAL_CONTROL:
@@ -568,6 +550,7 @@ int mavlink_thread_main(int argc, char *argv[])
default:
usage();
+ break;
}
}
@@ -674,23 +657,27 @@ int mavlink_thread_main(int argc, char *argv[])
/* translate the current system state to mavlink state and mode */
uint8_t mavlink_state = 0;
- uint8_t mavlink_mode = 0;
- get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
+ uint8_t mavlink_base_mode = 0;
+ uint32_t mavlink_custom_mode = 0;
+ get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
/* send heartbeat */
- mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state);
+ mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, mavlink_custom_mode, mavlink_state);
/* switch HIL mode if required */
- set_hil_on_off(v_status.flag_hil_enabled);
+ if (v_status.hil_state == HIL_STATE_ON)
+ set_hil_on_off(true);
+ else if (v_status.hil_state == HIL_STATE_OFF)
+ set_hil_on_off(false);
/* send status (values already copied in the section above) */
mavlink_msg_sys_status_send(chan,
v_status.onboard_control_sensors_present,
v_status.onboard_control_sensors_enabled,
v_status.onboard_control_sensors_health,
- v_status.load,
- v_status.voltage_battery * 1000.0f,
- v_status.current_battery * 1000.0f,
+ v_status.load * 1000.0f,
+ v_status.battery_voltage * 1000.0f,
+ v_status.battery_current * 1000.0f,
v_status.battery_remaining,
v_status.drop_rate_comm,
v_status.errors_comm,
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 01bbabd46d695f08fa106270988e5d404745f1e8..af43542da40d9def9193b627ed84e2fc5e31e5ee 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -50,6 +50,10 @@
#include <mqueue.h>
#include <string.h>
#include <drivers/drv_hrt.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_gyro.h>
+#include <drivers/drv_mag.h>
+#include <drivers/drv_baro.h>
#include <time.h>
#include <float.h>
#include <unistd.h>
@@ -67,6 +71,7 @@
#include <systemlib/err.h>
#include <systemlib/airspeed.h>
#include <mavlink/mavlink_log.h>
+#include <commander/px4_custom_mode.h>
__BEGIN_DECLS
@@ -101,6 +106,10 @@ static orb_advert_t pub_hil_global_pos = -1;
static orb_advert_t pub_hil_attitude = -1;
static orb_advert_t pub_hil_gps = -1;
static orb_advert_t pub_hil_sensors = -1;
+static orb_advert_t pub_hil_gyro = -1;
+static orb_advert_t pub_hil_accel = -1;
+static orb_advert_t pub_hil_mag = -1;
+static orb_advert_t pub_hil_baro = -1;
static orb_advert_t pub_hil_airspeed = -1;
static orb_advert_t cmd_pub = -1;
@@ -188,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;
@@ -412,12 +423,12 @@ handle_message(mavlink_message_t *msg)
/* airspeed from differential pressure, ambient pressure and temp */
struct airspeed_s airspeed;
- airspeed.timestamp = hrt_absolute_time();
float ias = calc_indicated_airspeed(imu.diff_pressure);
// XXX need to fix this
float tas = ias;
+ airspeed.timestamp = hrt_absolute_time();
airspeed.indicated_airspeed_m_s = ias;
airspeed.true_airspeed_m_s = tas;
@@ -428,7 +439,67 @@ handle_message(mavlink_message_t *msg)
}
//warnx("SENSOR: IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s);
- /* publish */
+ /* individual sensor publications */
+ struct gyro_report gyro;
+ gyro.x_raw = imu.xgyro / mrad2rad;
+ gyro.y_raw = imu.ygyro / mrad2rad;
+ gyro.z_raw = imu.zgyro / mrad2rad;
+ gyro.x = imu.xgyro;
+ gyro.y = imu.ygyro;
+ gyro.z = imu.zgyro;
+ gyro.temperature = imu.temperature;
+ gyro.timestamp = hrt_absolute_time();
+
+ if (pub_hil_gyro < 0) {
+ pub_hil_gyro = orb_advertise(ORB_ID(sensor_gyro), &gyro);
+ } else {
+ orb_publish(ORB_ID(sensor_gyro), pub_hil_gyro, &gyro);
+ }
+
+ struct accel_report accel;
+ accel.x_raw = imu.xacc / mg2ms2;
+ accel.y_raw = imu.yacc / mg2ms2;
+ accel.z_raw = imu.zacc / mg2ms2;
+ accel.x = imu.xacc;
+ accel.y = imu.yacc;
+ accel.z = imu.zacc;
+ accel.temperature = imu.temperature;
+ accel.timestamp = hrt_absolute_time();
+
+ if (pub_hil_accel < 0) {
+ pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel);
+ } else {
+ orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel);
+ }
+
+ struct mag_report mag;
+ mag.x_raw = imu.xmag / mga2ga;
+ mag.y_raw = imu.ymag / mga2ga;
+ mag.z_raw = imu.zmag / mga2ga;
+ mag.x = imu.xmag;
+ mag.y = imu.ymag;
+ mag.z = imu.zmag;
+ mag.timestamp = hrt_absolute_time();
+
+ if (pub_hil_mag < 0) {
+ pub_hil_mag = orb_advertise(ORB_ID(sensor_mag), &mag);
+ } else {
+ orb_publish(ORB_ID(sensor_mag), pub_hil_mag, &mag);
+ }
+
+ struct baro_report baro;
+ baro.pressure = imu.abs_pressure;
+ baro.altitude = imu.pressure_alt;
+ baro.temperature = imu.temperature;
+ baro.timestamp = hrt_absolute_time();
+
+ if (pub_hil_baro < 0) {
+ pub_hil_baro = orb_advertise(ORB_ID(sensor_baro), &baro);
+ } else {
+ orb_publish(ORB_ID(sensor_baro), pub_hil_baro, &baro);
+ }
+
+ /* publish combined sensor topic */
if (pub_hil_sensors > 0) {
orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
} else {
@@ -552,6 +623,22 @@ handle_message(mavlink_message_t *msg)
} else {
pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
}
+
+ struct accel_report accel;
+ accel.x_raw = hil_state.xacc / 9.81f * 1e3f;
+ accel.y_raw = hil_state.yacc / 9.81f * 1e3f;
+ accel.z_raw = hil_state.zacc / 9.81f * 1e3f;
+ accel.x = hil_state.xacc;
+ accel.y = hil_state.yacc;
+ accel.z = hil_state.zacc;
+ accel.temperature = 25.0f;
+ accel.timestamp = hrt_absolute_time();
+
+ if (pub_hil_accel < 0) {
+ pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel);
+ } else {
+ orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel);
+ }
}
if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {
diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
index bfccb2d389a3c07a8ad3baaf6a09de3dc4ef4cc3..5d3d6a73c1cb99b56facb9155709888423d6cd4a 100644
--- a/src/modules/mavlink/module.mk
+++ b/src/modules/mavlink/module.mk
@@ -39,7 +39,6 @@ MODULE_COMMAND = mavlink
SRCS += mavlink.c \
missionlib.c \
mavlink_parameters.c \
- mavlink_log.c \
mavlink_receiver.cpp \
orb_listener.c \
waypoints.c
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index edb8761b8090c7fb4cf1103448c0f307abe0c107..53d86ec0058fc81d88fd3add82fce87dd49de8d3 100644
--- a/src/modules/mavlink/orb_listener.c
+++ b/src/modules/mavlink/orb_listener.c
@@ -71,7 +71,8 @@ struct vehicle_status_s v_status;
struct rc_channels_s rc;
struct rc_input_values rc_raw;
struct actuator_armed_s armed;
-struct actuator_controls_effective_s actuators_0;
+struct actuator_controls_effective_s actuators_effective_0;
+struct actuator_controls_s actuators_0;
struct vehicle_attitude_s att;
struct mavlink_subscriptions mavlink_subs;
@@ -112,6 +113,7 @@ static void l_attitude_setpoint(const struct listener *l);
static void l_actuator_outputs(const struct listener *l);
static void l_actuator_armed(const struct listener *l);
static void l_manual_control_setpoint(const struct listener *l);
+static void l_vehicle_attitude_controls_effective(const struct listener *l);
static void l_vehicle_attitude_controls(const struct listener *l);
static void l_debug_key_value(const struct listener *l);
static void l_optical_flow(const struct listener *l);
@@ -138,6 +140,7 @@ static const struct listener listeners[] = {
{l_actuator_armed, &mavlink_subs.armed_sub, 0},
{l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0},
{l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0},
+ {l_vehicle_attitude_controls_effective, &mavlink_subs.actuators_effective_sub, 0},
{l_debug_key_value, &mavlink_subs.debug_key_value, 0},
{l_optical_flow, &mavlink_subs.optical_flow, 0},
{l_vehicle_rates_setpoint, &mavlink_subs.rates_setpoint_sub, 0},
@@ -272,19 +275,23 @@ l_vehicle_status(const struct listener *l)
orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
/* enable or disable HIL */
- set_hil_on_off(v_status.flag_hil_enabled);
+ if (v_status.hil_state == HIL_STATE_ON)
+ set_hil_on_off(true);
+ else if (v_status.hil_state == HIL_STATE_OFF)
+ set_hil_on_off(false);
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state = 0;
- uint8_t mavlink_mode = 0;
- get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
+ uint8_t mavlink_base_mode = 0;
+ uint32_t mavlink_custom_mode = 0;
+ get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
/* send heartbeat */
mavlink_msg_heartbeat_send(chan,
mavlink_system.type,
MAV_AUTOPILOT_PX4,
- mavlink_mode,
- v_status.state_machine,
+ mavlink_base_mode,
+ mavlink_custom_mode,
mavlink_state);
}
@@ -334,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,
@@ -470,8 +477,9 @@ l_actuator_outputs(const struct listener *l)
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state = 0;
- uint8_t mavlink_mode = 0;
- get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
+ uint8_t mavlink_base_mode = 0;
+ uint32_t mavlink_custom_mode = 0;
+ get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
/* HIL message as per MAVLink spec */
@@ -488,7 +496,7 @@ l_actuator_outputs(const struct listener *l)
-1,
-1,
-1,
- mavlink_mode,
+ mavlink_base_mode,
0);
} else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) {
@@ -502,7 +510,7 @@ l_actuator_outputs(const struct listener *l)
((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
-1,
-1,
- mavlink_mode,
+ mavlink_base_mode,
0);
} else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) {
@@ -516,7 +524,7 @@ l_actuator_outputs(const struct listener *l)
((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f,
- mavlink_mode,
+ mavlink_base_mode,
0);
} else {
@@ -530,7 +538,7 @@ l_actuator_outputs(const struct listener *l)
(act_outputs.output[5] - 1500.0f) / 500.0f,
(act_outputs.output[6] - 1500.0f) / 500.0f,
(act_outputs.output[7] - 1500.0f) / 500.0f,
- mavlink_mode,
+ mavlink_base_mode,
0);
}
}
@@ -562,28 +570,54 @@ l_manual_control_setpoint(const struct listener *l)
}
void
-l_vehicle_attitude_controls(const struct listener *l)
+l_vehicle_attitude_controls_effective(const struct listener *l)
{
- orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_sub, &actuators_0);
+ orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_effective_sub, &actuators_effective_0);
if (gcs_link) {
/* send, add spaces so that string buffer is at least 10 chars long */
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"eff ctrl0 ",
- actuators_0.control_effective[0]);
+ actuators_effective_0.control_effective[0]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"eff ctrl1 ",
- actuators_0.control_effective[1]);
+ actuators_effective_0.control_effective[1]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"eff ctrl2 ",
- actuators_0.control_effective[2]);
+ actuators_effective_0.control_effective[2]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"eff ctrl3 ",
- actuators_0.control_effective[3]);
+ actuators_effective_0.control_effective[3]);
+ }
+}
+
+void
+l_vehicle_attitude_controls(const struct listener *l)
+{
+ orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators_0);
+
+ if (gcs_link) {
+ /* send, add spaces so that string buffer is at least 10 chars long */
+ mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+ last_sensor_timestamp / 1000,
+ "ctrl0 ",
+ actuators_0.control[0]);
+ mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+ last_sensor_timestamp / 1000,
+ "ctrl1 ",
+ actuators_0.control[1]);
+ mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+ last_sensor_timestamp / 1000,
+ "ctrl2 ",
+ actuators_0.control[2]);
+ mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+ last_sensor_timestamp / 1000,
+ "ctrl3 ",
+ actuators_0.control[3]);
}
}
@@ -632,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);
- float throttle = actuators_0.control_effective[3] * (UINT16_MAX - 1);
- float alt = global_pos.alt;
- float climb = global_pos.vz;
+ 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.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 *
@@ -673,7 +707,7 @@ uorb_receive_thread(void *arg)
/* handle the poll result */
if (poll_ret == 0) {
- mavlink_missionlib_send_gcs_string("[mavlink] No telemetry data for 1 s");
+ /* silent */
} else if (poll_ret < 0) {
mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data");
@@ -768,7 +802,10 @@ uorb_receive_start(void)
orb_set_interval(mavlink_subs.man_control_sp_sub, 100); /* 10Hz updates */
/* --- ACTUATOR CONTROL VALUE --- */
- mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
+ mavlink_subs.actuators_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
+ orb_set_interval(mavlink_subs.actuators_effective_sub, 100); /* 10Hz updates */
+
+ mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
orb_set_interval(mavlink_subs.actuators_sub, 100); /* 10Hz updates */
/* --- DEBUG VALUE OUTPUT --- */
diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h
index 73e278dc647b11d5d4341c04b9d5a768cba96577..e2e6300463ceaa2107143f0a74cb78bb9b6458d3 100644
--- a/src/modules/mavlink/orb_topics.h
+++ b/src/modules/mavlink/orb_topics.h
@@ -55,10 +55,12 @@
#include <uORB/topics/vehicle_global_position_set_triplet.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/debug_key_value.h>
@@ -75,8 +77,10 @@ struct mavlink_subscriptions {
int act_3_sub;
int gps_sub;
int man_control_sp_sub;
- int armed_sub;
+ int safety_sub;
int actuators_sub;
+ int armed_sub;
+ int actuators_effective_sub;
int local_pos_sub;
int spa_sub;
int spl_sub;
diff --git a/src/modules/mavlink/util.h b/src/modules/mavlink/util.h
index a4ff06a883e968c4ffb4cdec67ef0f9293e9c6ce..5e5ee8261ca819bacb3576ca46ae2d8e51e815dd 100644
--- a/src/modules/mavlink/util.h
+++ b/src/modules/mavlink/util.h
@@ -51,4 +51,4 @@ extern mavlink_wpm_storage *wpm;
/**
* Translate the custom state into standard mavlink modes and state.
*/
-extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode);
+extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c
index eea928a170ba0131104f409a3b497b4127925b53..16a7c2d355bbdf8541830ff755e9d3167cc90e2f 100644
--- a/src/modules/mavlink/waypoints.c
+++ b/src/modules/mavlink/waypoints.c
@@ -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;
}
diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c
index 20fb11b2cb5ea17e37aea9cf637b90a044f00235..e713449823099b313b1a889c1333ec7d9903e486 100644
--- a/src/modules/mavlink_onboard/mavlink.c
+++ b/src/modules/mavlink_onboard/mavlink.c
@@ -273,18 +273,18 @@ void mavlink_update_system(void)
}
void
-get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed,
+get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_armed_s *armed,
uint8_t *mavlink_state, uint8_t *mavlink_mode)
{
/* reset MAVLink mode bitfield */
*mavlink_mode = 0;
/* set mode flags independent of system state */
- if (v_status->flag_control_manual_enabled) {
+ if (control_mode->flag_control_manual_enabled) {
*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
}
- if (v_status->flag_hil_enabled) {
+ if (control_mode->flag_system_hil_enabled) {
*mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED;
}
@@ -295,61 +295,67 @@ get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct
*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
}
- switch (v_status->state_machine) {
- case SYSTEM_STATE_PREFLIGHT:
- if (v_status->flag_preflight_gyro_calibration ||
- v_status->flag_preflight_mag_calibration ||
- v_status->flag_preflight_accel_calibration) {
- *mavlink_state = MAV_STATE_CALIBRATING;
- } else {
- *mavlink_state = MAV_STATE_UNINIT;
- }
- break;
-
- case SYSTEM_STATE_STANDBY:
- *mavlink_state = MAV_STATE_STANDBY;
- break;
-
- case SYSTEM_STATE_GROUND_READY:
- *mavlink_state = MAV_STATE_ACTIVE;
- break;
-
- case SYSTEM_STATE_MANUAL:
- *mavlink_state = MAV_STATE_ACTIVE;
- *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
- break;
-
- case SYSTEM_STATE_STABILIZED:
- *mavlink_state = MAV_STATE_ACTIVE;
- *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
- break;
-
- case SYSTEM_STATE_AUTO:
- *mavlink_state = MAV_STATE_ACTIVE;
+ if (control_mode->flag_control_velocity_enabled) {
*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
- break;
-
- case SYSTEM_STATE_MISSION_ABORT:
- *mavlink_state = MAV_STATE_EMERGENCY;
- break;
-
- case SYSTEM_STATE_EMCY_LANDING:
- *mavlink_state = MAV_STATE_EMERGENCY;
- break;
-
- case SYSTEM_STATE_EMCY_CUTOFF:
- *mavlink_state = MAV_STATE_EMERGENCY;
- break;
-
- case SYSTEM_STATE_GROUND_ERROR:
- *mavlink_state = MAV_STATE_EMERGENCY;
- break;
-
- case SYSTEM_STATE_REBOOT:
- *mavlink_state = MAV_STATE_POWEROFF;
- break;
+ } else {
+ *mavlink_mode &= ~MAV_MODE_FLAG_GUIDED_ENABLED;
}
+// switch (v_status->state_machine) {
+// case SYSTEM_STATE_PREFLIGHT:
+// if (v_status->flag_preflight_gyro_calibration ||
+// v_status->flag_preflight_mag_calibration ||
+// v_status->flag_preflight_accel_calibration) {
+// *mavlink_state = MAV_STATE_CALIBRATING;
+// } else {
+// *mavlink_state = MAV_STATE_UNINIT;
+// }
+// break;
+//
+// case SYSTEM_STATE_STANDBY:
+// *mavlink_state = MAV_STATE_STANDBY;
+// break;
+//
+// case SYSTEM_STATE_GROUND_READY:
+// *mavlink_state = MAV_STATE_ACTIVE;
+// break;
+//
+// case SYSTEM_STATE_MANUAL:
+// *mavlink_state = MAV_STATE_ACTIVE;
+// *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+// break;
+//
+// case SYSTEM_STATE_STABILIZED:
+// *mavlink_state = MAV_STATE_ACTIVE;
+// *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
+// break;
+//
+// case SYSTEM_STATE_AUTO:
+// *mavlink_state = MAV_STATE_ACTIVE;
+// *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
+// break;
+//
+// case SYSTEM_STATE_MISSION_ABORT:
+// *mavlink_state = MAV_STATE_EMERGENCY;
+// break;
+//
+// case SYSTEM_STATE_EMCY_LANDING:
+// *mavlink_state = MAV_STATE_EMERGENCY;
+// break;
+//
+// case SYSTEM_STATE_EMCY_CUTOFF:
+// *mavlink_state = MAV_STATE_EMERGENCY;
+// break;
+//
+// case SYSTEM_STATE_GROUND_ERROR:
+// *mavlink_state = MAV_STATE_EMERGENCY;
+// break;
+//
+// case SYSTEM_STATE_REBOOT:
+// *mavlink_state = MAV_STATE_POWEROFF;
+// break;
+// }
+
}
/**
@@ -361,7 +367,9 @@ int mavlink_thread_main(int argc, char *argv[])
char *device_name = "/dev/ttyS1";
baudrate = 57600;
+ /* XXX this is never written? */
struct vehicle_status_s v_status;
+ struct vehicle_control_mode_s control_mode;
struct actuator_armed_s armed;
/* work around some stupidity in task_create's argv handling */
@@ -430,19 +438,19 @@ int mavlink_thread_main(int argc, char *argv[])
/* translate the current system state to mavlink state and mode */
uint8_t mavlink_state = 0;
uint8_t mavlink_mode = 0;
- get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode);
+ get_mavlink_mode_and_state(&control_mode, &armed, &mavlink_state, &mavlink_mode);
/* send heartbeat */
- mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state);
+ mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state);
/* send status (values already copied in the section above) */
mavlink_msg_sys_status_send(chan,
v_status.onboard_control_sensors_present,
v_status.onboard_control_sensors_enabled,
v_status.onboard_control_sensors_health,
- v_status.load,
- v_status.voltage_battery * 1000.0f,
- v_status.current_battery * 1000.0f,
+ v_status.load * 1000.0f,
+ v_status.battery_voltage * 1000.0f,
+ v_status.battery_current * 1000.0f,
v_status.battery_remaining,
v_status.drop_rate_comm,
v_status.errors_comm,
diff --git a/src/modules/mavlink_onboard/mavlink_receiver.c b/src/modules/mavlink_onboard/mavlink_receiver.c
index 68d49c24b60f1e5050cfbfe111d681462213caa2..0236e612642584201d4de2225fdf1c4ec8671b02 100644
--- a/src/modules/mavlink_onboard/mavlink_receiver.c
+++ b/src/modules/mavlink_onboard/mavlink_receiver.c
@@ -327,4 +327,4 @@ receive_start(int uart)
pthread_t thread;
pthread_create(&thread, &receiveloop_attr, receive_thread, &uart);
return thread;
-}
\ No newline at end of file
+}
diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h
index f18f562439d884f8616311588533a2ab78d502f8..1b49c9ce4eb5f846eef9c480343f6851e9482e49 100644
--- a/src/modules/mavlink_onboard/orb_topics.h
+++ b/src/modules/mavlink_onboard/orb_topics.h
@@ -52,9 +52,11 @@
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/debug_key_value.h>
#include <drivers/drv_rc_input.h>
@@ -69,7 +71,7 @@ struct mavlink_subscriptions {
int act_3_sub;
int gps_sub;
int man_control_sp_sub;
- int armed_sub;
+ int safety_sub;
int actuators_sub;
int local_pos_sub;
int spa_sub;
diff --git a/src/modules/mavlink_onboard/util.h b/src/modules/mavlink_onboard/util.h
index 38a4db372b8392b79256e8d26e5ad18fff47c90e..c84b6fd26ef403ab431e298b2fd0c0b7b33fa0e0 100644
--- a/src/modules/mavlink_onboard/util.h
+++ b/src/modules/mavlink_onboard/util.h
@@ -50,5 +50,6 @@ extern volatile bool thread_should_exit;
/**
* Translate the custom state into standard mavlink modes and state.
*/
-extern void get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed,
+extern void
+get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_armed_s *armed,
uint8_t *mavlink_state, uint8_t *mavlink_mode);
diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c
index 99f25cfe9220a8a41eb5f0d1015861154960641d..04582f2a4028170e47cde754bf713e291c038222 100644
--- a/src/modules/multirotor_att_control/multirotor_att_control_main.c
+++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c
@@ -2,6 +2,7 @@
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
+ * Anton Babushkin <anton.babushkin@me.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -38,6 +39,7 @@
* Implementation of multirotor attitude control main loop.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <nuttx/config.h>
@@ -57,12 +59,13 @@
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <drivers/drv_gyro.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/offboard_control_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/parameter_update.h>
@@ -74,23 +77,20 @@
#include "multirotor_attitude_control.h"
#include "multirotor_rate_control.h"
-PARAM_DEFINE_FLOAT(MC_RCLOSS_THR, 0.0f); // This defines the throttle when the RC signal is lost.
-
__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 orb_advert_t actuator_pub;
-
-static struct vehicle_status_s state;
+static const float min_takeoff_throttle = 0.3f;
+static const float yaw_deadzone = 0.01f;
static int
mc_thread_main(int argc, char *argv[])
{
/* declare and safely initialize all structs */
- memset(&state, 0, sizeof(state));
+ struct vehicle_control_mode_s control_mode;
+ memset(&control_mode, 0, sizeof(control_mode));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_attitude_setpoint_s att_sp;
@@ -103,7 +103,8 @@ mc_thread_main(int argc, char *argv[])
memset(&offboard_sp, 0, sizeof(offboard_sp));
struct vehicle_rates_setpoint_s rates_sp;
memset(&rates_sp, 0, sizeof(rates_sp));
-
+ struct vehicle_status_s status;
+ memset(&status, 0, sizeof(status));
struct actuator_controls_s actuators;
memset(&actuators, 0, sizeof(actuators));
@@ -112,9 +113,11 @@ mc_thread_main(int argc, char *argv[])
int param_sub = orb_subscribe(ORB_ID(parameter_update));
int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
- int state_sub = orb_subscribe(ORB_ID(vehicle_status));
+ int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+ int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
+ int status_sub = orb_subscribe(ORB_ID(vehicle_status));
/*
* Do not rate-limit the loop to prevent aliasing
@@ -134,10 +137,9 @@ mc_thread_main(int argc, char *argv[])
actuators.control[i] = 0.0f;
}
- actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
+ orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
orb_advert_t rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
- int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
/* register the perf counter */
perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control_runtime");
@@ -145,23 +147,11 @@ mc_thread_main(int argc, char *argv[])
perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "multirotor_att_control_err");
/* welcome user */
- printf("[multirotor_att_control] starting\n");
+ warnx("starting");
/* store last control mode to detect mode switches */
- bool flag_control_manual_enabled = false;
- bool flag_control_attitude_enabled = false;
- bool flag_system_armed = false;
-
- /* store if yaw position or yaw speed has been changed */
bool control_yaw_position = true;
-
- /* store if we stopped a yaw movement */
- bool first_time_after_yaw_speed_control = true;
-
- /* prepare the handle for the failsafe throttle */
- param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR");
- float failsafe_throttle = 0.0f;
-
+ bool reset_yaw_sp = true;
while (!thread_should_exit) {
@@ -183,7 +173,6 @@ mc_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(parameter_update), param_sub, &update);
/* update parameters */
- // XXX no params here yet
}
/* only run controller if attitude changed */
@@ -193,10 +182,10 @@ mc_thread_main(int argc, char *argv[])
/* get a local copy of system state */
bool updated;
- orb_check(state_sub, &updated);
+ orb_check(control_mode_sub, &updated);
if (updated) {
- orb_copy(ORB_ID(vehicle_status), state_sub, &state);
+ orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
}
/* get a local copy of manual setpoint */
@@ -212,19 +201,32 @@ mc_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(offboard_control_setpoint), setpoint_sub, &offboard_sp);
}
+ /* get a local copy of status */
+ orb_check(status_sub, &updated);
+
+ if (updated) {
+ orb_copy(ORB_ID(vehicle_status), status_sub, &status);
+ }
+
/* 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;
+ }
- /** STEP 1: Define which input is the dominating control input */
- if (state.flag_control_offboard_enabled) {
+ /* define which input is the dominating control input */
+ if (control_mode.flag_control_offboard_enabled) {
/* offboard inputs */
if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) {
rates_sp.roll = offboard_sp.p1;
rates_sp.pitch = offboard_sp.p2;
rates_sp.yaw = offboard_sp.p3;
rates_sp.thrust = offboard_sp.p4;
-// printf("thrust_rate=%8.4f\n",offboard_sp.p4);
rates_sp.timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
@@ -233,108 +235,57 @@ mc_thread_main(int argc, char *argv[])
att_sp.pitch_body = offboard_sp.p2;
att_sp.yaw_body = offboard_sp.p3;
att_sp.thrust = offboard_sp.p4;
-// printf("thrust_att=%8.4f\n",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) {
+ /* manual input */
+ if (control_mode.flag_control_attitude_enabled) {
+ /* control attitude, update attitude setpoint depending on mode */
+ if (att_sp.thrust < 0.1f) {
+ /* no thrust, don't try to control yaw */
+ rates_sp.yaw = 0.0f;
+ control_yaw_position = false;
+
+ if (status.condition_landed) {
+ /* reset yaw setpoint if on ground */
+ reset_yaw_sp = true;
+ }
- } else if (state.flag_control_manual_enabled) {
-
- if (state.flag_control_attitude_enabled) {
-
- /* initialize to current yaw if switching to manual or att control */
- if (state.flag_control_attitude_enabled != flag_control_attitude_enabled ||
- state.flag_control_manual_enabled != flag_control_manual_enabled ||
- state.flag_system_armed != flag_system_armed) {
- 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 (state.rc_signal_lost) {
- /* 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.roll_body = 0.0f;
- att_sp.pitch_body = 0.0f;
-
- /*
- * 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) {
- att_sp.thrust = failsafe_throttle;
+ } else {
+ /* only move yaw setpoint if manual input is != 0 */
+ if (manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) {
+ /* control yaw rate */
+ control_yaw_position = false;
+ rates_sp.yaw = manual.yaw;
+ reset_yaw_sp = true; // has no effect on control, just for beautiful log
} else {
- att_sp.thrust = 0.0f;
+ control_yaw_position = true;
}
+ }
- /* keep current yaw, do not attempt to go to north orientation,
- * 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;
-
+ if (!control_mode.flag_control_velocity_enabled) {
+ /* update attitude setpoint if not in position control mode */
att_sp.roll_body = manual.roll;
att_sp.pitch_body = manual.pitch;
- /* set attitude if arming */
- if (!flag_control_attitude_enabled && state.flag_system_armed) {
- att_sp.yaw_body = att.yaw;
+ if (!control_mode.flag_control_climb_rate_enabled) {
+ /* pass throttle directly if not in altitude control mode */
+ att_sp.thrust = manual.throttle;
}
-
- /* act if stabilization is active or if the (nonsense) direct pass through mode is set */
- if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS ||
- state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
-
- if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) {
- rates_sp.yaw = manual.yaw;
- control_yaw_position = false;
-
- } else {
- /*
- * This mode SHOULD be the default mode, which is:
- * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS
- *
- * However, we fall back to this setting for all other (nonsense)
- * settings as well.
- */
-
- /* only move setpoint if manual input is != 0 */
- if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) {
- rates_sp.yaw = manual.yaw;
- control_yaw_position = false;
- first_time_after_yaw_speed_control = true;
-
- } else {
- if (first_time_after_yaw_speed_control) {
- att_sp.yaw_body = att.yaw;
- first_time_after_yaw_speed_control = false;
- }
-
- control_yaw_position = true;
- }
- }
- }
-
- att_sp.thrust = manual.throttle;
- att_sp.timestamp = hrt_absolute_time();
}
- /* STEP 2: publish the controller output */
- orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
+ /* 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) {
printf("testmode");
@@ -342,65 +293,89 @@ 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 result to the vehicle actuators */
- orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
}
+ att_sp.timestamp = hrt_absolute_time();
+
+ /* publish the attitude setpoint */
+ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
+
} else {
- /* manual rate inputs, from RC control or joystick */
- if (state.flag_control_rates_enabled &&
- state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) {
+ /* manual rate inputs (ACRO), from RC control or joystick */
+ if (control_mode.flag_control_rates_enabled) {
rates_sp.roll = manual.roll;
-
rates_sp.pitch = manual.pitch;
rates_sp.yaw = manual.yaw;
rates_sp.thrust = manual.throttle;
rates_sp.timestamp = hrt_absolute_time();
}
+
+ /* reset yaw setpoint after ACRO */
+ reset_yaw_sp = true;
+ }
+
+ } else {
+ if (!control_mode.flag_control_auto_enabled) {
+ /* no control, try to stay on place */
+ if (!control_mode.flag_control_velocity_enabled) {
+ /* no velocity control, reset attitude setpoint */
+ att_sp.roll_body = 0.0f;
+ att_sp.pitch_body = 0.0f;
+ att_sp.timestamp = hrt_absolute_time();
+ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
+ }
}
+ /* reset yaw setpoint after non-manual control */
+ reset_yaw_sp = true;
}
- /** STEP 3: Identify the controller setup to run and set up the inputs correctly */
- if (state.flag_control_attitude_enabled) {
- multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position);
+ /* check if we should we reset integrals */
+ bool reset_integral = !control_mode.flag_armed || att_sp.thrust < 0.1f; // TODO use landed status instead of throttle
+ /* run attitude controller if needed */
+ if (control_mode.flag_control_attitude_enabled) {
+ multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position, reset_integral);
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
}
/* measure in what intervals the controller runs */
perf_count(mc_interval_perf);
- float gyro[3];
+ /* run rates controller if needed */
+ if (control_mode.flag_control_rates_enabled) {
+ /* get current rate setpoint */
+ bool rates_sp_updated = false;
+ orb_check(rates_sp_sub, &rates_sp_updated);
- /* get current rate setpoint */
- bool rates_sp_valid = false;
- orb_check(rates_sp_sub, &rates_sp_valid);
+ if (rates_sp_updated) {
+ orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp);
+ }
- if (rates_sp_valid) {
- orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp);
+ /* apply controller */
+ float rates[3];
+ rates[0] = att.rollspeed;
+ 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;
+ actuators.control[1] = 0.0f;
+ actuators.control[2] = 0.0f;
+ actuators.control[3] = 0.0f;
}
- /* apply controller */
- gyro[0] = att.rollspeed;
- gyro[1] = att.pitchspeed;
- gyro[2] = att.yawspeed;
-
- multirotor_control_rates(&rates_sp, gyro, &actuators);
+ actuators.timestamp = hrt_absolute_time();
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
- /* update state */
- flag_control_attitude_enabled = state.flag_control_attitude_enabled;
- flag_control_manual_enabled = state.flag_control_manual_enabled;
- flag_system_armed = state.flag_system_armed;
-
perf_end(mc_loop_perf);
} /* end of poll call for attitude updates */
} /* end of poll return value check */
}
- printf("[multirotor att control] stopping, disarming motors.\n");
+ warnx("stopping, disarming motors");
/* kill all outputs */
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
@@ -410,7 +385,7 @@ mc_thread_main(int argc, char *argv[])
close(att_sub);
- close(state_sub);
+ close(control_mode_sub);
close(manual_sub);
close(actuator_pub);
close(att_sp_pub);
@@ -467,11 +442,11 @@ int multirotor_att_control_main(int argc, char *argv[])
thread_should_exit = false;
mc_task = task_spawn_cmd("multirotor_att_control",
- SCHED_DEFAULT,
- SCHED_PRIORITY_MAX - 15,
- 2048,
- mc_thread_main,
- NULL);
+ SCHED_DEFAULT,
+ SCHED_PRIORITY_MAX - 15,
+ 2048,
+ mc_thread_main,
+ NULL);
exit(0);
}
diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c
index 8f19c6a4bc3038955e0e24e84f21021a61d9f2a8..c78232f11c25a1688bde191f03fcd2faad28dc56 100644
--- a/src/modules/multirotor_att_control/multirotor_attitude_control.c
+++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c
@@ -166,7 +166,7 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc
}
void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
- const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position)
+ const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, bool reset_integral)
{
static uint64_t last_run = 0;
static uint64_t last_input = 0;
@@ -210,13 +210,13 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
}
- /* reset integral if on ground */
- if (att_sp->thrust < 0.1f) {
+ /* reset integrals if needed */
+ if (reset_integral) {
pid_reset_integral(&pitch_controller);
pid_reset_integral(&roll_controller);
+ //TODO pid_reset_integral(&yaw_controller);
}
-
/* calculate current control outputs */
/* control pitch (forward) output */
@@ -229,6 +229,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
if (control_yaw_position) {
/* control yaw rate */
+ // TODO use pid lib
/* positive error: rotate to right, negative error, rotate to left (NED frame) */
// yaw_error = _wrap_pi(att_sp->yaw_body - att->yaw);
@@ -246,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++;
}
diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.h b/src/modules/multirotor_att_control/multirotor_attitude_control.h
index e78f45c474fbb92a0660ef45a123bb8435a1f800..431a435f7d7877a08346c70ad44b83055ae03c13 100644
--- a/src/modules/multirotor_att_control/multirotor_attitude_control.h
+++ b/src/modules/multirotor_att_control/multirotor_attitude_control.h
@@ -60,6 +60,6 @@
#include <uORB/topics/actuator_controls.h>
void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
- const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position);
+ const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, bool reset_integral);
#endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */
diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c
index e58d357d58182d0353061410e79e483d32381019..0a336be47dd8e602f9c3de54f65326092ae0956f 100644
--- a/src/modules/multirotor_att_control/multirotor_rate_control.c
+++ b/src/modules/multirotor_att_control/multirotor_rate_control.c
@@ -152,7 +152,7 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru
}
void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
- const float rates[], struct actuator_controls_s *actuators)
+ const float rates[], struct actuator_controls_s *actuators, bool reset_integral)
{
static uint64_t last_run = 0;
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
@@ -193,10 +193,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
}
- /* reset integral if on ground */
- if (rate_sp->thrust < 0.01f) {
+ /* reset integrals if needed */
+ if (reset_integral) {
pid_reset_integral(&pitch_rate_controller);
pid_reset_integral(&roll_rate_controller);
+ // TODO pid_reset_integral(&yaw_rate_controller);
}
/* control pitch (forward) output */
diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.h b/src/modules/multirotor_att_control/multirotor_rate_control.h
index 362b5ed86dfeeb1f08abe2439384a7098e7ebbd8..ca7794c59bdf3dffd8e4259c6ca63a00c3f67331 100644
--- a/src/modules/multirotor_att_control/multirotor_rate_control.h
+++ b/src/modules/multirotor_att_control/multirotor_rate_control.h
@@ -59,6 +59,6 @@
#include <uORB/topics/actuator_controls.h>
void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
- const float rates[], struct actuator_controls_s *actuators);
+ const float rates[], struct actuator_controls_s *actuators, bool reset_integral);
#endif /* MULTIROTOR_RATE_CONTROL_H_ */
diff --git a/src/modules/multirotor_pos_control/module.mk b/src/modules/multirotor_pos_control/module.mk
index d0484774556bc2fd17c0a29251342790d9321b77..bc4b48fb406f296eb4bd064ddfa2cdad9d650ff2 100644
--- a/src/modules/multirotor_pos_control/module.mk
+++ b/src/modules/multirotor_pos_control/module.mk
@@ -38,4 +38,5 @@
MODULE_COMMAND = multirotor_pos_control
SRCS = multirotor_pos_control.c \
- multirotor_pos_control_params.c
+ multirotor_pos_control_params.c \
+ thrust_pid.c
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c
index f39d11438cb589d9a27ee9c111f1b1e46ab33953..8227f76c5e0ed37d5a82928fbd860e869369affd 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c
@@ -1,7 +1,7 @@
/****************************************************************************
*
- * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- * Author: Lorenz Meier <lm@inf.ethz.ch>
+ * Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ * Author: Anton Babushkin <anton.babushkin@me.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -35,13 +35,14 @@
/**
* @file multirotor_pos_control.c
*
- * Skeleton for multirotor position controller
+ * Multirotor position controller
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
+#include <math.h>
#include <stdbool.h>
#include <unistd.h>
#include <fcntl.h>
@@ -52,15 +53,22 @@
#include <sys/prctl.h>
#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>
#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
-#include <uORB/topics/vehicle_vicon_position.h>
+#include <uORB/topics/vehicle_global_position_setpoint.h>
+#include <uORB/topics/vehicle_global_velocity_setpoint.h>
#include <systemlib/systemlib.h>
+#include <systemlib/pid/pid.h>
+#include <mavlink/mavlink_log.h>
#include "multirotor_pos_control_params.h"
+#include "thrust_pid.h"
static bool thread_should_exit = false; /**< Deamon exit flag */
@@ -79,12 +87,16 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]);
*/
static void usage(const char *reason);
-static void
-usage(const char *reason)
+static float scale_control(float ctl, float end, float dz);
+
+static float norm(float x, float y);
+
+static void usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
- fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\n\n");
+
+ fprintf(stderr, "usage: multirotor_pos_control {start|stop|status}\n\n");
exit(1);
}
@@ -92,9 +104,9 @@ usage(const char *reason)
* The deamon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
- *
+ *
* The actual stack size should be set in the call
- * to task_spawn_cmd().
+ * to task_spawn().
*/
int multirotor_pos_control_main(int argc, char *argv[])
{
@@ -104,32 +116,36 @@ int multirotor_pos_control_main(int argc, char *argv[])
if (!strcmp(argv[1], "start")) {
if (thread_running) {
- printf("multirotor pos control already running\n");
+ warnx("already running");
/* this is not an error */
exit(0);
}
+ warnx("start");
thread_should_exit = false;
- deamon_task = task_spawn_cmd("multirotor pos control",
- SCHED_DEFAULT,
- SCHED_PRIORITY_MAX - 60,
- 4096,
- multirotor_pos_control_thread_main,
- (argv) ? (const char **)&argv[2] : (const char **)NULL);
+ deamon_task = task_spawn_cmd("multirotor_pos_control",
+ SCHED_DEFAULT,
+ SCHED_PRIORITY_MAX - 60,
+ 4096,
+ multirotor_pos_control_thread_main,
+ (argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
}
if (!strcmp(argv[1], "stop")) {
+ warnx("stop");
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
- printf("\tmultirotor pos control app is running\n");
+ warnx("app is running");
+
} else {
- printf("\tmultirotor pos control app not started\n");
+ warnx("app not started");
}
+
exit(0);
}
@@ -137,98 +153,510 @@ int multirotor_pos_control_main(int argc, char *argv[])
exit(1);
}
-static int
-multirotor_pos_control_thread_main(int argc, char *argv[])
+static float scale_control(float ctl, float end, float dz)
+{
+ if (ctl > dz) {
+ return (ctl - dz) / (end - dz);
+
+ } else if (ctl < -dz) {
+ return (ctl + dz) / (end - dz);
+
+ } else {
+ return 0.0f;
+ }
+}
+
+static float norm(float x, float y)
+{
+ return sqrtf(x * x + y * y);
+}
+
+static int multirotor_pos_control_thread_main(int argc, char *argv[])
{
/* welcome user */
- printf("[multirotor pos control] Control started, taking over position control\n");
+ warnx("started");
+ static int mavlink_fd;
+ mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+ mavlink_log_info(mavlink_fd, "[mpc] started");
/* structures */
- struct vehicle_status_s state;
+ struct vehicle_control_mode_s control_mode;
+ memset(&control_mode, 0, sizeof(control_mode));
struct vehicle_attitude_s att;
- //struct vehicle_global_position_setpoint_s global_pos_sp;
- struct vehicle_local_position_setpoint_s local_pos_sp;
- struct vehicle_vicon_position_s local_pos;
- struct manual_control_setpoint_s manual;
+ memset(&att, 0, sizeof(att));
struct vehicle_attitude_setpoint_s att_sp;
+ memset(&att_sp, 0, sizeof(att_sp));
+ struct manual_control_setpoint_s manual;
+ memset(&manual, 0, sizeof(manual));
+ struct vehicle_local_position_s local_pos;
+ memset(&local_pos, 0, sizeof(local_pos));
+ struct vehicle_local_position_setpoint_s local_pos_sp;
+ memset(&local_pos_sp, 0, sizeof(local_pos_sp));
+ struct vehicle_global_position_setpoint_s global_pos_sp;
+ memset(&global_pos_sp, 0, sizeof(local_pos_sp));
+ struct vehicle_global_velocity_setpoint_s global_vel_sp;
+ memset(&global_vel_sp, 0, sizeof(global_vel_sp));
/* subscribe to attitude, motor setpoints and system state */
+ int param_sub = orb_subscribe(ORB_ID(parameter_update));
+ int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
- int state_sub = orb_subscribe(ORB_ID(vehicle_status));
+ int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
- int local_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
- //int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
+ int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
+ int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
- /* publish attitude setpoint */
+ /* publish setpoint */
+ orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp);
+ orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp);
orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
+ bool global_pos_sp_reproject = false;
+ bool global_pos_sp_valid = false;
+ bool local_pos_sp_valid = false;
+ bool reset_sp_z = true;
+ bool reset_sp_xy = true;
+ bool reset_int_z = true;
+ bool reset_int_z_manual = false;
+ bool reset_int_xy = true;
+ bool was_armed = false;
+ bool reset_integral = true;
+ bool reset_auto_pos = true;
+
+ hrt_abstime t_prev = 0;
+ 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;
+
+ PID_t xy_pos_pids[2];
+ PID_t xy_vel_pids[2];
+ PID_t z_pos_pid;
+ thrust_pid_t z_vel_pid;
+
thread_running = true;
- int loopcounter = 0;
-
- struct multirotor_position_control_params p;
- struct multirotor_position_control_param_handles h;
- parameters_init(&h);
- parameters_update(&h, &p);
-
-
- while (1) {
- /* get a local copy of the vehicle state */
- orb_copy(ORB_ID(vehicle_status), state_sub, &state);
- /* get a local copy of manual setpoint */
- orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
- /* get a local copy of attitude */
- orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
- /* get a local copy of local position */
- orb_copy(ORB_ID(vehicle_vicon_position), local_pos_sub, &local_pos);
- /* get a local copy of local position setpoint */
- orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp);
-
- if (loopcounter == 500) {
- parameters_update(&h, &p);
- loopcounter = 0;
+ struct multirotor_position_control_params params;
+ struct multirotor_position_control_param_handles params_h;
+ parameters_init(¶ms_h);
+ parameters_update(¶ms_h, ¶ms);
+
+ for (int i = 0; i < 2; i++) {
+ pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f);
+ pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
+ }
+
+ pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f);
+ thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
+
+ int paramcheck_counter = 0;
+
+ while (!thread_should_exit) {
+ /* check parameters at 1 Hz */
+ if (++paramcheck_counter >= 50) {
+ paramcheck_counter = 0;
+ bool param_updated;
+ orb_check(param_sub, ¶m_updated);
+
+ if (param_updated) {
+ parameters_update(¶ms_h, ¶ms);
+
+ for (int i = 0; i < 2; i++) {
+ pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f);
+ /* use integral_limit_out = tilt_max / 2 */
+ float i_limit;
+
+ if (params.xy_vel_i == 0.0) {
+ i_limit = params.tilt_max / params.xy_vel_i / 2.0;
+
+ } else {
+ i_limit = 1.0f; // not used really
+ }
+
+ pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max);
+ }
+
+ pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max);
+ thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min);
+ }
+ }
+
+ bool updated;
+
+ orb_check(control_mode_sub, &updated);
+
+ if (updated) {
+ orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
+ }
+
+ orb_check(global_pos_sp_sub, &updated);
+
+ if (updated) {
+ orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp);
+ global_pos_sp_valid = true;
+ global_pos_sp_reproject = true;
}
- // if (state.state_machine == SYSTEM_STATE_AUTO) {
-
- // XXX IMPLEMENT POSITION CONTROL HERE
-
- float dT = 1.0f / 50.0f;
-
- float x_setpoint = 0.0f;
-
- // XXX enable switching between Vicon and local position estimate
- /* local pos is the Vicon position */
-
- // XXX just an example, lacks rotation around world-body transformation
- att_sp.pitch_body = (local_pos.x - x_setpoint) * p.p;
- att_sp.roll_body = 0.0f;
- att_sp.yaw_body = 0.0f;
- att_sp.thrust = 0.3f;
- att_sp.timestamp = hrt_absolute_time();
-
- /* publish new attitude setpoint */
- orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
- // } else if (state.state_machine == SYSTEM_STATE_STABILIZED) {
- /* set setpoint to current position */
- // XXX select pos reset channel on remote
- /* reset setpoint to current position (position hold) */
- // if (1 == 2) {
- // 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;
- // }
- // }
+ hrt_abstime t = hrt_absolute_time();
+ float dt;
+
+ if (t_prev != 0) {
+ dt = (t - t_prev) * 0.000001f;
+
+ } else {
+ dt = 0.0f;
+ }
+
+ if (control_mode.flag_armed && !was_armed) {
+ /* reset setpoints and integrals on arming */
+ reset_sp_z = true;
+ reset_sp_xy = true;
+ reset_int_z = true;
+ reset_int_xy = true;
+ }
+
+ was_armed = control_mode.flag_armed;
+
+ t_prev = t;
+
+ if (control_mode.flag_control_altitude_enabled || control_mode.flag_control_velocity_enabled || control_mode.flag_control_position_enabled) {
+ orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
+ orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
+ orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
+ orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
+
+ float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f;
+ float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f;
+ float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f };
+
+ if (control_mode.flag_control_manual_enabled) {
+ /* manual control */
+ /* check for reference point updates and correct setpoint */
+ if (local_pos.ref_timestamp != ref_alt_t) {
+ if (ref_alt_t != 0) {
+ /* home alt changed, don't follow large ground level changes in manual flight */
+ local_pos_sp.z += local_pos.ref_alt - ref_alt;
+ }
+
+ ref_alt_t = local_pos.ref_timestamp;
+ ref_alt = local_pos.ref_alt;
+ // TODO also correct XY setpoint
+ }
+
+ /* reset setpoints to current position if needed */
+ if (control_mode.flag_control_altitude_enabled) {
+ if (reset_sp_z) {
+ reset_sp_z = false;
+ local_pos_sp.z = local_pos.z;
+ mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", -local_pos_sp.z);
+ }
+
+ /* move altitude setpoint with throttle stick */
+ float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz);
+
+ if (z_sp_ctl != 0.0f) {
+ sp_move_rate[2] = -z_sp_ctl * params.z_vel_max;
+ local_pos_sp.z += sp_move_rate[2] * dt;
+
+ if (local_pos_sp.z > local_pos.z + z_sp_offs_max) {
+ local_pos_sp.z = local_pos.z + z_sp_offs_max;
+
+ } else if (local_pos_sp.z < local_pos.z - z_sp_offs_max) {
+ local_pos_sp.z = local_pos.z - z_sp_offs_max;
+ }
+ }
+ }
+
+ if (control_mode.flag_control_position_enabled) {
+ if (reset_sp_xy) {
+ reset_sp_xy = false;
+ local_pos_sp.x = local_pos.x;
+ local_pos_sp.y = local_pos.y;
+ pid_reset_integral(&xy_vel_pids[0]);
+ pid_reset_integral(&xy_vel_pids[1]);
+ mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y);
+ }
+
+ /* move position setpoint with roll/pitch stick */
+ float pos_pitch_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz);
+ float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz);
+
+ if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) {
+ /* calculate direction and increment of control in NED frame */
+ float xy_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl);
+ float xy_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * params.xy_vel_max;
+ sp_move_rate[0] = cosf(xy_sp_ctl_dir) * xy_sp_ctl_speed;
+ sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed;
+ local_pos_sp.x += sp_move_rate[0] * dt;
+ local_pos_sp.y += sp_move_rate[1] * dt;
+ /* limit maximum setpoint from position offset and preserve direction
+ * fail safe, should not happen in normal operation */
+ float pos_vec_x = local_pos_sp.x - local_pos.x;
+ float pos_vec_y = local_pos_sp.y - local_pos.y;
+ float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max;
+
+ if (pos_vec_norm > 1.0f) {
+ local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm;
+ local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm;
+ }
+ }
+ }
+
+ 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 if (control_mode.flag_control_auto_enabled) {
+ /* AUTO mode, use global setpoint */
+ 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_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);
+ }
+
+ 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 AUTO mode */
+ reset_sp_xy = true;
+ reset_sp_z = true;
+
+ } else {
+ /* no control, loiter or stay on ground */
+ if (local_pos.landed) {
+ /* landed: move setpoint down */
+ /* in air: hold altitude */
+ if (local_pos_sp.z < 5.0f) {
+ /* set altitude setpoint to 5m under ground,
+ * don't set it too deep to avoid unexpected landing in case of false "landed" signal */
+ local_pos_sp.z = 5.0f;
+ mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", -local_pos_sp.z);
+ }
+
+ reset_sp_z = true;
+
+ } else {
+ /* in air: hold altitude */
+ if (reset_sp_z) {
+ reset_sp_z = false;
+ local_pos_sp.z = local_pos.z;
+ mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", -local_pos_sp.z);
+ }
+ }
+
+ if (control_mode.flag_control_position_enabled) {
+ if (reset_sp_xy) {
+ reset_sp_xy = false;
+ local_pos_sp.x = local_pos.x;
+ local_pos_sp.y = local_pos.y;
+ local_pos_sp.yaw = att.yaw;
+ local_pos_sp_valid = true;
+ att_sp.yaw_body = att.yaw;
+ mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", local_pos_sp.x, local_pos_sp.y);
+ }
+ }
+ }
+
+ /* 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];
+
+ } else {
+ reset_sp_z = true;
+ global_vel_sp.vz = 0.0f;
+ }
+
+ if (control_mode.flag_control_position_enabled) {
+ /* calculate velocity set point in NED frame */
+ global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx - sp_move_rate[0], dt) + sp_move_rate[0];
+ global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy - sp_move_rate[1], dt) + sp_move_rate[1];
+
+ /* limit horizontal speed */
+ float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max;
+
+ if (xy_vel_sp_norm > 1.0f) {
+ global_vel_sp.vx /= xy_vel_sp_norm;
+ global_vel_sp.vy /= xy_vel_sp_norm;
+ }
+
+ } else {
+ reset_sp_xy = true;
+ global_vel_sp.vx = 0.0f;
+ global_vel_sp.vy = 0.0f;
+ }
+
+ /* publish new velocity setpoint */
+ orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp);
+ // TODO subscribe to velocity setpoint if altitude/position control disabled
+
+ if (control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) {
+ /* run velocity controllers, calculate thrust vector with attitude-thrust compensation */
+ float thrust_sp[3] = { 0.0f, 0.0f, 0.0f };
+
+ if (control_mode.flag_control_climb_rate_enabled) {
+ if (reset_int_z) {
+ reset_int_z = false;
+ float i = params.thr_min;
+
+ if (reset_int_z_manual) {
+ i = manual.throttle;
+
+ if (i < params.thr_min) {
+ i = params.thr_min;
+
+ } else if (i > params.thr_max) {
+ i = params.thr_max;
+ }
+ }
+
+ thrust_pid_set_integral(&z_vel_pid, -i);
+ mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", i);
+ }
+
+ thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]);
+ att_sp.thrust = -thrust_sp[2];
+
+ } else {
+ /* reset thrust integral when altitude control enabled */
+ reset_int_z = true;
+ }
+
+ if (control_mode.flag_control_velocity_enabled) {
+ /* calculate thrust set point in NED frame */
+ if (reset_int_xy) {
+ reset_int_xy = false;
+ pid_reset_integral(&xy_vel_pids[0]);
+ pid_reset_integral(&xy_vel_pids[1]);
+ mavlink_log_info(mavlink_fd, "[mpc] reset pos integral");
+ }
+
+ thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt);
+ thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt);
+
+ /* thrust_vector now contains desired acceleration (but not in m/s^2) in NED frame */
+ /* limit horizontal part of thrust */
+ float thrust_xy_dir = atan2f(thrust_sp[1], thrust_sp[0]);
+ /* assuming that vertical component of thrust is g,
+ * horizontal component = g * tan(alpha) */
+ float tilt = atanf(norm(thrust_sp[0], thrust_sp[1]));
+
+ if (tilt > params.tilt_max) {
+ tilt = params.tilt_max;
+ }
+
+ /* convert direction to body frame */
+ thrust_xy_dir -= att.yaw;
+ /* calculate roll and pitch */
+ att_sp.roll_body = sinf(thrust_xy_dir) * tilt;
+ att_sp.pitch_body = -cosf(thrust_xy_dir) * tilt / cosf(att_sp.roll_body);
+
+ } else {
+ reset_int_xy = true;
+ }
+
+ att_sp.timestamp = hrt_absolute_time();
+
+ /* publish new attitude setpoint */
+ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
+ }
+
+ } else {
+ /* position controller disabled, reset setpoints */
+ reset_sp_z = true;
+ reset_sp_xy = true;
+ 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 */
+ reset_int_z_manual = control_mode.flag_armed && control_mode.flag_control_manual_enabled && !control_mode.flag_control_climb_rate_enabled;
/* run at approximately 50 Hz */
usleep(20000);
- loopcounter++;
-
}
- printf("[multirotor pos control] ending now...\n");
+ warnx("stopped");
+ mavlink_log_info(mavlink_fd, "[mpc] stopped");
thread_running = false;
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
index 6b73dc405229c9aa3ec9fef0dfed0da334948b26..9c1ef2edb3928c10f095905019758c9820c95cd1 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
@@ -1,8 +1,7 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
- * Author: Tobias Naegeli <naegelit@student.ethz.ch>
- * Lorenz Meier <lm@inf.ethz.ch>
+ * Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ * Author: Anton Babushkin <anton.babushkin@me.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -34,29 +33,76 @@
****************************************************************************/
/*
- * @file multirotor_position_control_params.c
- *
- * Parameters for EKF filter
+ * @file multirotor_pos_control_params.c
+ *
+ * Parameters for multirotor_pos_control
*/
#include "multirotor_pos_control_params.h"
-/* Extended Kalman Filter covariances */
-
/* controller parameters */
-PARAM_DEFINE_FLOAT(MC_POS_P, 0.2f);
+PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.2f);
+PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.8f);
+PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f);
+PARAM_DEFINE_FLOAT(MPC_Z_D, 0.0f);
+PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f);
+PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.0f);
+PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f);
+PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f);
+PARAM_DEFINE_FLOAT(MPC_XY_P, 0.5f);
+PARAM_DEFINE_FLOAT(MPC_XY_D, 0.0f);
+PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.2f);
+PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.0f);
+PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.0f);
+PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 10.0f);
+PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f);
int parameters_init(struct multirotor_position_control_param_handles *h)
{
- /* PID parameters */
- h->p = param_find("MC_POS_P");
+ h->thr_min = param_find("MPC_THR_MIN");
+ h->thr_max = param_find("MPC_THR_MAX");
+ h->z_p = param_find("MPC_Z_P");
+ h->z_d = param_find("MPC_Z_D");
+ h->z_vel_p = param_find("MPC_Z_VEL_P");
+ h->z_vel_i = param_find("MPC_Z_VEL_I");
+ h->z_vel_d = param_find("MPC_Z_VEL_D");
+ h->z_vel_max = param_find("MPC_Z_VEL_MAX");
+ h->xy_p = param_find("MPC_XY_P");
+ h->xy_d = param_find("MPC_XY_D");
+ h->xy_vel_p = param_find("MPC_XY_VEL_P");
+ h->xy_vel_i = param_find("MPC_XY_VEL_I");
+ h->xy_vel_d = param_find("MPC_XY_VEL_D");
+ h->xy_vel_max = param_find("MPC_XY_VEL_MAX");
+ h->tilt_max = param_find("MPC_TILT_MAX");
+
+ h->rc_scale_pitch = param_find("RC_SCALE_PITCH");
+ h->rc_scale_roll = param_find("RC_SCALE_ROLL");
+ h->rc_scale_yaw = param_find("RC_SCALE_YAW");
return OK;
}
int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p)
{
- param_get(h->p, &(p->p));
+ param_get(h->thr_min, &(p->thr_min));
+ param_get(h->thr_max, &(p->thr_max));
+ param_get(h->z_p, &(p->z_p));
+ param_get(h->z_d, &(p->z_d));
+ param_get(h->z_vel_p, &(p->z_vel_p));
+ param_get(h->z_vel_i, &(p->z_vel_i));
+ param_get(h->z_vel_d, &(p->z_vel_d));
+ param_get(h->z_vel_max, &(p->z_vel_max));
+ param_get(h->xy_p, &(p->xy_p));
+ param_get(h->xy_d, &(p->xy_d));
+ param_get(h->xy_vel_p, &(p->xy_vel_p));
+ param_get(h->xy_vel_i, &(p->xy_vel_i));
+ param_get(h->xy_vel_d, &(p->xy_vel_d));
+ param_get(h->xy_vel_max, &(p->xy_vel_max));
+ param_get(h->tilt_max, &(p->tilt_max));
+
+ param_get(h->rc_scale_pitch, &(p->rc_scale_pitch));
+ param_get(h->rc_scale_roll, &(p->rc_scale_roll));
+ param_get(h->rc_scale_yaw, &(p->rc_scale_yaw));
return OK;
}
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
index 274f6c22a917a9d1c44815b78150f47a7748975d..3ec85a3641a6998f4c30b5ac5709c7eb7a9a1dd6 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
@@ -1,8 +1,7 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
- * Author: Tobias Naegeli <naegelit@student.ethz.ch>
- * Lorenz Meier <lm@inf.ethz.ch>
+ * Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ * Author: Anton Babushkin <anton.babushkin@me.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -34,23 +33,55 @@
****************************************************************************/
/*
- * @file multirotor_position_control_params.h
- *
- * Parameters for position controller
+ * @file multirotor_pos_control_params.h
+ *
+ * Parameters for multirotor_pos_control
*/
#include <systemlib/param/param.h>
struct multirotor_position_control_params {
- float p;
- float i;
- float d;
+ float thr_min;
+ float thr_max;
+ float z_p;
+ float z_d;
+ float z_vel_p;
+ float z_vel_i;
+ float z_vel_d;
+ float z_vel_max;
+ float xy_p;
+ float xy_d;
+ float xy_vel_p;
+ float xy_vel_i;
+ float xy_vel_d;
+ float xy_vel_max;
+ float tilt_max;
+
+ float rc_scale_pitch;
+ float rc_scale_roll;
+ float rc_scale_yaw;
};
struct multirotor_position_control_param_handles {
- param_t p;
- param_t i;
- param_t d;
+ param_t thr_min;
+ param_t thr_max;
+ param_t z_p;
+ param_t z_d;
+ param_t z_vel_p;
+ param_t z_vel_i;
+ param_t z_vel_d;
+ param_t z_vel_max;
+ param_t xy_p;
+ param_t xy_d;
+ param_t xy_vel_p;
+ param_t xy_vel_i;
+ param_t xy_vel_d;
+ param_t xy_vel_max;
+ param_t tilt_max;
+
+ param_t rc_scale_pitch;
+ param_t rc_scale_roll;
+ param_t rc_scale_yaw;
};
/**
diff --git a/src/modules/multirotor_pos_control/position_control.c b/src/modules/multirotor_pos_control/position_control.c
deleted file mode 100644
index 9c53a5bf6c90ca1d78e4f76ee1ad2d6bc5e8d6fa..0000000000000000000000000000000000000000
--- a/src/modules/multirotor_pos_control/position_control.c
+++ /dev/null
@@ -1,235 +0,0 @@
-// /****************************************************************************
-// *
-// * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
-// * Author: @author Lorenz Meier <lm@inf.ethz.ch>
-// * @author Laurens Mackay <mackayl@student.ethz.ch>
-// * @author Tobias Naegeli <naegelit@student.ethz.ch>
-// * @author Martin Rutschmann <rutmarti@student.ethz.ch>
-// *
-// * 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 multirotor_position_control.c
-// * Implementation of the position control for a multirotor VTOL
-// */
-
-// #include <stdio.h>
-// #include <stdlib.h>
-// #include <stdio.h>
-// #include <stdint.h>
-// #include <math.h>
-// #include <stdbool.h>
-// #include <float.h>
-// #include <systemlib/pid/pid.h>
-
-// #include "multirotor_position_control.h"
-
-// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual,
-// const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos,
-// const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp)
-// {
-// static PID_t distance_controller;
-
-// static int read_ret;
-// static global_data_position_t position_estimated;
-
-// static uint16_t counter;
-
-// static bool initialized;
-// static uint16_t pm_counter;
-
-// static float lat_next;
-// static float lon_next;
-
-// static float pitch_current;
-
-// static float thrust_total;
-
-
-// if (initialized == false) {
-
-// pid_init(&distance_controller,
-// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
-// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
-// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
-// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU],
-// PID_MODE_DERIVATIV_CALC, 150);//150
-
-// // pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
-// // pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
-
-// thrust_total = 0.0f;
-
-// /* Position initialization */
-// /* Wait for new position estimate */
-// do {
-// read_ret = read_lock_position(&position_estimated);
-// } while (read_ret != 0);
-
-// lat_next = position_estimated.lat;
-// lon_next = position_estimated.lon;
-
-// /* attitude initialization */
-// global_data_lock(&global_data_attitude->access_conf);
-// pitch_current = global_data_attitude->pitch;
-// global_data_unlock(&global_data_attitude->access_conf);
-
-// initialized = true;
-// }
-
-// /* load new parameters with 10Hz */
-// if (counter % 50 == 0) {
-// if (global_data_trylock(&global_data_parameter_storage->access_conf) == 0) {
-// /* check whether new parameters are available */
-// if (global_data_parameter_storage->counter > pm_counter) {
-// pid_set_parameters(&distance_controller,
-// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
-// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
-// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
-// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]);
-
-// //
-// // pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
-// // pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
-
-// pm_counter = global_data_parameter_storage->counter;
-// printf("Position controller changed pid parameters\n");
-// }
-// }
-
-// global_data_unlock(&global_data_parameter_storage->access_conf);
-// }
-
-
-// /* Wait for new position estimate */
-// do {
-// read_ret = read_lock_position(&position_estimated);
-// } while (read_ret != 0);
-
-// /* Get next waypoint */ //TODO: add local copy
-
-// if (0 == global_data_trylock(&global_data_position_setpoint->access_conf)) {
-// lat_next = global_data_position_setpoint->x;
-// lon_next = global_data_position_setpoint->y;
-// global_data_unlock(&global_data_position_setpoint->access_conf);
-// }
-
-// /* Get distance to waypoint */
-// float distance_to_waypoint = get_distance_to_next_waypoint(position_estimated.lat , position_estimated.lon, lat_next, lon_next);
-// // if(counter % 5 == 0)
-// // printf("distance_to_waypoint: %.4f\n", distance_to_waypoint);
-
-// /* Get bearing to waypoint (direction on earth surface to next waypoint) */
-// float bearing = get_bearing_to_next_waypoint(position_estimated.lat, position_estimated.lon, lat_next, lon_next);
-
-// if (counter % 5 == 0)
-// printf("bearing: %.4f\n", bearing);
-
-// /* Calculate speed in direction of bearing (needed for controller) */
-// float speed_norm = sqrtf(position_estimated.vx * position_estimated.vx + position_estimated.vy * position_estimated.vy);
-// // if(counter % 5 == 0)
-// // printf("speed_norm: %.4f\n", speed_norm);
-// float speed_to_waypoint = 0; //(position_estimated.vx * cosf(bearing) + position_estimated.vy * sinf(bearing))/speed_norm; //FIXME, TODO: re-enable this once we have a full estimate of the speed, then we can do a PID for the distance controller
-
-// /* Control Thrust in bearing direction */
-// float horizontal_thrust = -pid_calculate(&distance_controller, 0, distance_to_waypoint, speed_to_waypoint,
-// CONTROL_PID_POSITION_INTERVAL); //TODO: maybe this "-" sign is an error somewhere else
-
-// // if(counter % 5 == 0)
-// // printf("horizontal thrust: %.4f\n", horizontal_thrust);
-
-// /* Get total thrust (from remote for now) */
-// if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
-// thrust_total = (float)global_data_rc_channels->chan[THROTTLE].scale; //TODO: how should we use the RC_CHANNELS_FUNCTION enum?
-// global_data_unlock(&global_data_rc_channels->access_conf);
-// }
-
-// const float max_gas = 500.0f;
-// thrust_total *= max_gas / 20000.0f; //TODO: check this
-// thrust_total += max_gas / 2.0f;
-
-
-// if (horizontal_thrust > thrust_total) {
-// horizontal_thrust = thrust_total;
-
-// } else if (horizontal_thrust < -thrust_total) {
-// horizontal_thrust = -thrust_total;
-// }
-
-
-
-// //TODO: maybe we want to add a speed controller later...
-
-// /* Calclulate thrust in east and north direction */
-// float thrust_north = cosf(bearing) * horizontal_thrust;
-// float thrust_east = sinf(bearing) * horizontal_thrust;
-
-// if (counter % 10 == 0) {
-// printf("thrust north: %.4f\n", thrust_north);
-// printf("thrust east: %.4f\n", thrust_east);
-// fflush(stdout);
-// }
-
-// /* Get current attitude */
-// if (0 == global_data_trylock(&global_data_attitude->access_conf)) {
-// pitch_current = global_data_attitude->pitch;
-// global_data_unlock(&global_data_attitude->access_conf);
-// }
-
-// /* Get desired pitch & roll */
-// float pitch_desired = 0.0f;
-// float roll_desired = 0.0f;
-
-// if (thrust_total != 0) {
-// float pitch_fraction = -thrust_north / thrust_total;
-// float roll_fraction = thrust_east / (cosf(pitch_current) * thrust_total);
-
-// if (roll_fraction < -1) {
-// roll_fraction = -1;
-
-// } else if (roll_fraction > 1) {
-// roll_fraction = 1;
-// }
-
-// // if(counter % 5 == 0)
-// // {
-// // printf("pitch_fraction: %.4f, roll_fraction: %.4f\n",pitch_fraction, roll_fraction);
-// // fflush(stdout);
-// // }
-
-// pitch_desired = asinf(pitch_fraction);
-// roll_desired = asinf(roll_fraction);
-// }
-
-// att_sp.roll = roll_desired;
-// att_sp.pitch = pitch_desired;
-
-// counter++;
-// }
diff --git a/src/modules/multirotor_pos_control/thrust_pid.c b/src/modules/multirotor_pos_control/thrust_pid.c
new file mode 100644
index 0000000000000000000000000000000000000000..b985630aec4480b877c4c37b67312aafd411e954
--- /dev/null
+++ b/src/modules/multirotor_pos_control/thrust_pid.c
@@ -0,0 +1,189 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ * Author: Anton Babushkin <anton.babushkin@me.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 thrust_pid.c
+ *
+ * Implementation of thrust control PID.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include "thrust_pid.h"
+#include <math.h>
+
+__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min)
+{
+ pid->kp = kp;
+ pid->ki = ki;
+ pid->kd = kd;
+ pid->limit_min = limit_min;
+ pid->limit_max = limit_max;
+ pid->mode = mode;
+ pid->dt_min = dt_min;
+ pid->last_output = 0.0f;
+ pid->sp = 0.0f;
+ pid->error_previous = 0.0f;
+ pid->integral = 0.0f;
+}
+
+__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max)
+{
+ int ret = 0;
+
+ if (isfinite(kp)) {
+ pid->kp = kp;
+
+ } else {
+ ret = 1;
+ }
+
+ if (isfinite(ki)) {
+ pid->ki = ki;
+
+ } else {
+ ret = 1;
+ }
+
+ if (isfinite(kd)) {
+ pid->kd = kd;
+
+ } else {
+ ret = 1;
+ }
+
+ if (isfinite(limit_min)) {
+ pid->limit_min = limit_min;
+
+ } else {
+ ret = 1;
+ }
+
+ if (isfinite(limit_max)) {
+ pid->limit_max = limit_max;
+
+ } else {
+ ret = 1;
+ }
+
+ return ret;
+}
+
+__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22)
+{
+ /* Alternative integral component calculation
+ *
+ * start:
+ * error = setpoint - current_value
+ * integral = integral + (Ki * error * dt)
+ * derivative = (error - previous_error) / dt
+ * previous_error = error
+ * output = (Kp * error) + integral + (Kd * derivative)
+ * wait(dt)
+ * goto start
+ */
+
+ if (!isfinite(sp) || !isfinite(val) || !isfinite(dt)) {
+ return pid->last_output;
+ }
+
+ float i, d;
+ pid->sp = sp;
+
+ // Calculated current error value
+ float error = pid->sp - val;
+
+ // Calculate or measured current error derivative
+ if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC) {
+ d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min);
+ pid->error_previous = error;
+
+ } else if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC_NO_SP) {
+ d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min);
+ pid->error_previous = -val;
+
+ } else {
+ d = 0.0f;
+ }
+
+ if (!isfinite(d)) {
+ d = 0.0f;
+ }
+
+ /* calculate the error integral */
+ i = pid->integral + (pid->ki * error * dt);
+
+ /* attitude-thrust compensation
+ * r22 is (2, 2) componet of rotation matrix for current attitude */
+ float att_comp;
+
+ if (r22 > 0.8f)
+ att_comp = 1.0f / r22;
+ else if (r22 > 0.0f)
+ att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * r22 + 1.0f;
+ else
+ att_comp = 1.0f;
+
+ /* calculate output */
+ float output = ((error * pid->kp) + i + (d * pid->kd)) * att_comp;
+
+ /* check for saturation */
+ if (output < pid->limit_min || output > pid->limit_max) {
+ /* saturated, recalculate output with old integral */
+ output = (error * pid->kp) + pid->integral + (d * pid->kd);
+
+ } else {
+ if (isfinite(i)) {
+ pid->integral = i;
+ }
+ }
+
+ if (isfinite(output)) {
+ if (output > pid->limit_max) {
+ output = pid->limit_max;
+
+ } else if (output < pid->limit_min) {
+ output = pid->limit_min;
+ }
+
+ pid->last_output = output;
+ }
+
+ return pid->last_output;
+}
+
+__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i)
+{
+ pid->integral = i;
+}
diff --git a/src/modules/multirotor_pos_control/thrust_pid.h b/src/modules/multirotor_pos_control/thrust_pid.h
new file mode 100644
index 0000000000000000000000000000000000000000..5e169c1ba5fa12cfbf1c37f2a249e1c371874c27
--- /dev/null
+++ b/src/modules/multirotor_pos_control/thrust_pid.h
@@ -0,0 +1,76 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ * Author: Anton Babushkin <anton.babushkin@me.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 thrust_pid.h
+ *
+ * Definition of thrust control PID interface.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#ifndef THRUST_PID_H_
+#define THRUST_PID_H_
+
+#include <stdint.h>
+
+__BEGIN_DECLS
+
+/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error */
+#define THRUST_PID_MODE_DERIVATIV_CALC 0
+/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored */
+#define THRUST_PID_MODE_DERIVATIV_CALC_NO_SP 1
+
+typedef struct {
+ float kp;
+ float ki;
+ float kd;
+ float sp;
+ float integral;
+ float error_previous;
+ float last_output;
+ float limit_min;
+ float limit_max;
+ float dt_min;
+ uint8_t mode;
+} thrust_pid_t;
+
+__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min);
+__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max);
+__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22);
+__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i);
+
+__END_DECLS
+
+#endif /* THRUST_PID_H_ */
diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c
new file mode 100644
index 0000000000000000000000000000000000000000..13328edb4baa2b2dec3140a24e0d915b822a1098
--- /dev/null
+++ b/src/modules/position_estimator_inav/inertial_filter.c
@@ -0,0 +1,31 @@
+/*
+ * inertial_filter.c
+ *
+ * Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ * Author: Anton Babushkin <rk3dov@gmail.com>
+ */
+
+#include "inertial_filter.h"
+
+void inertial_filter_predict(float dt, float x[3])
+{
+ x[0] += x[1] * dt + x[2] * dt * dt / 2.0f;
+ x[1] += x[2] * dt;
+}
+
+void inertial_filter_correct(float e, float dt, float x[3], int i, float w)
+{
+ float ewdt = w * dt;
+ if (ewdt > 1.0f)
+ ewdt = 1.0f; // prevent over-correcting
+ ewdt *= e;
+ x[i] += ewdt;
+
+ if (i == 0) {
+ x[1] += w * ewdt;
+ x[2] += w * w * ewdt / 3.0;
+
+ } else if (i == 1) {
+ x[2] += w * ewdt;
+ }
+}
diff --git a/src/modules/position_estimator_inav/inertial_filter.h b/src/modules/position_estimator_inav/inertial_filter.h
new file mode 100644
index 0000000000000000000000000000000000000000..761c17097d37a7688d753dbcd89bed96c60fec87
--- /dev/null
+++ b/src/modules/position_estimator_inav/inertial_filter.h
@@ -0,0 +1,13 @@
+/*
+ * inertial_filter.h
+ *
+ * Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ * Author: Anton Babushkin <rk3dov@gmail.com>
+ */
+
+#include <stdbool.h>
+#include <drivers/drv_hrt.h>
+
+void inertial_filter_predict(float dt, float x[3]);
+
+void inertial_filter_correct(float e, float dt, float x[3], int i, float w);
diff --git a/src/modules/position_estimator_inav/module.mk b/src/modules/position_estimator_inav/module.mk
new file mode 100644
index 0000000000000000000000000000000000000000..939d768495460ffb44ef0f84fc5f56cbe65230cb
--- /dev/null
+++ b/src/modules/position_estimator_inav/module.mk
@@ -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.
+#
+############################################################################
+
+#
+# Makefile to build position_estimator_inav
+#
+
+MODULE_COMMAND = position_estimator_inav
+SRCS = position_estimator_inav_main.c \
+ position_estimator_inav_params.c \
+ inertial_filter.c
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
new file mode 100644
index 0000000000000000000000000000000000000000..4a4572b09c0509c26e172bf93c9275b653825475
--- /dev/null
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -0,0 +1,648 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ * Author: Anton Babushkin <rk3dov@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 position_estimator_inav_main.c
+ * Model-identification based position estimator for multirotors
+ */
+
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <float.h>
+#include <string.h>
+#include <nuttx/config.h>
+#include <nuttx/sched.h>
+#include <sys/prctl.h>
+#include <termios.h>
+#include <errno.h>
+#include <limits.h>
+#include <math.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/actuator_controls_effective.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/optical_flow.h>
+#include <mavlink/mavlink_log.h>
+#include <poll.h>
+#include <systemlib/err.h>
+#include <geo/geo.h>
+#include <systemlib/systemlib.h>
+#include <drivers/drv_hrt.h>
+
+#include "position_estimator_inav_params.h"
+#include "inertial_filter.h"
+
+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 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[]);
+
+int position_estimator_inav_thread_main(int argc, char *argv[]);
+
+static void usage(const char *reason);
+
+/**
+ * Print the correct usage.
+ */
+static void usage(const char *reason)
+{
+ if (reason)
+ fprintf(stderr, "%s\n", reason);
+
+ fprintf(stderr,
+ "usage: position_estimator_inav {start|stop|status} [-v]\n\n");
+ exit(1);
+}
+
+/**
+ * The position_estimator_inav_thread only briefly exists to start
+ * the background job. The stack size assigned in the
+ * Makefile does only apply to this management task.
+ *
+ * The actual stack size should be set in the call
+ * to task_create().
+ */
+int position_estimator_inav_main(int argc, char *argv[])
+{
+ if (argc < 1)
+ usage("missing command");
+
+ if (!strcmp(argv[1], "start")) {
+ if (thread_running) {
+ printf("position_estimator_inav already running\n");
+ /* this is not an error */
+ exit(0);
+ }
+
+ verbose_mode = false;
+
+ if (argc > 1)
+ if (!strcmp(argv[2], "-v"))
+ verbose_mode = true;
+
+ thread_should_exit = false;
+ position_estimator_inav_task = task_spawn_cmd("position_estimator_inav",
+ SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096,
+ position_estimator_inav_thread_main,
+ (argv) ? (const char **) &argv[2] : (const char **) NULL);
+ exit(0);
+ }
+
+ if (!strcmp(argv[1], "stop")) {
+ thread_should_exit = true;
+ exit(0);
+ }
+
+ if (!strcmp(argv[1], "status")) {
+ if (thread_running) {
+ printf("\tposition_estimator_inav is running\n");
+
+ } else {
+ printf("\tposition_estimator_inav not started\n");
+ }
+
+ exit(0);
+ }
+
+ usage("unrecognized command");
+ exit(1);
+}
+
+/****************************************************************************
+ * main
+ ****************************************************************************/
+int position_estimator_inav_thread_main(int argc, char *argv[])
+{
+ warnx("started.");
+ int mavlink_fd;
+ mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+ mavlink_log_info(mavlink_fd, "[inav] started");
+
+ /* initialize values */
+ float x_est[3] = { 0.0f, 0.0f, 0.0f };
+ float y_est[3] = { 0.0f, 0.0f, 0.0f };
+ float z_est[3] = { 0.0f, 0.0f, 0.0f };
+
+ int baro_init_cnt = 0;
+ int baro_init_num = 200;
+ float baro_alt0 = 0.0f; /* to determine while start up */
+ 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;
+
+ /* declare and safely initialize all structs */
+ struct actuator_controls_effective_s actuator;
+ memset(&actuator, 0, sizeof(actuator));
+ struct actuator_armed_s armed;
+ memset(&armed, 0, sizeof(armed));
+ struct sensor_combined_s sensor;
+ memset(&sensor, 0, sizeof(sensor));
+ struct vehicle_gps_position_s gps;
+ memset(&gps, 0, sizeof(gps));
+ struct vehicle_attitude_s att;
+ memset(&att, 0, sizeof(att));
+ struct vehicle_local_position_s local_pos;
+ memset(&local_pos, 0, sizeof(local_pos));
+ struct optical_flow_s flow;
+ memset(&flow, 0, sizeof(flow));
+ struct vehicle_global_position_s global_pos;
+ memset(&global_pos, 0, sizeof(global_pos));
+
+ /* subscribe */
+ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
+ int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
+ int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+ int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
+ int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+ int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
+ int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+
+ /* advertise */
+ orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos);
+ orb_advert_t vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
+
+ struct position_estimator_inav_params params;
+ struct position_estimator_inav_param_handles pos_inav_param_handles;
+ /* initialize parameter handles */
+ parameters_init(&pos_inav_param_handles);
+
+ /* first parameters read at start up */
+ struct parameter_update_s param_update;
+ orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_update); /* read from param topic to clear updated flag */
+ /* first parameters update */
+ parameters_update(&pos_inav_param_handles, ¶ms);
+
+ struct pollfd fds_init[1] = {
+ { .fd = sensor_combined_sub, .events = POLLIN },
+ };
+
+ /* wait for initial baro value */
+ bool wait_baro = true;
+
+ thread_running = true;
+
+ while (wait_baro && !thread_should_exit) {
+ int ret = poll(fds_init, 1, 1000);
+
+ if (ret < 0) {
+ /* poll error */
+ errx(1, "subscriptions poll error on init.");
+
+ } else if (ret > 0) {
+ if (fds_init[0].revents & POLLIN) {
+ orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
+
+ if (wait_baro && sensor.baro_counter > baro_counter) {
+ baro_counter = sensor.baro_counter;
+
+ /* mean calculation over several measurements */
+ if (baro_init_cnt < baro_init_num) {
+ baro_alt0 += sensor.baro_alt_meter;
+ baro_init_cnt++;
+
+ } else {
+ wait_baro = false;
+ baro_alt0 /= (float) baro_init_cnt;
+ warnx("init baro: alt = %.3f", baro_alt0);
+ mavlink_log_info(mavlink_fd, "[inav] init baro: alt = %.3f", baro_alt0);
+ local_pos.ref_alt = baro_alt0;
+ local_pos.ref_timestamp = hrt_absolute_time();
+ local_pos.z_valid = true;
+ local_pos.v_z_valid = true;
+ local_pos.global_z = true;
+ }
+ }
+ }
+ }
+ }
+
+ bool ref_xy_inited = false;
+ hrt_abstime ref_xy_init_start = 0;
+ const hrt_abstime ref_xy_init_delay = 5000000; // wait for 5s after 3D fix
+
+ hrt_abstime t_prev = 0;
+
+ uint16_t accel_updates = 0;
+ uint16_t baro_updates = 0;
+ uint16_t gps_updates = 0;
+ uint16_t attitude_updates = 0;
+ uint16_t flow_updates = 0;
+
+ hrt_abstime updates_counter_start = hrt_absolute_time();
+ hrt_abstime pub_last = hrt_absolute_time();
+
+ /* acceleration in NED frame */
+ float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G };
+
+ /* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */
+ float accel_corr[] = { 0.0f, 0.0f, 0.0f }; // N E D
+ float accel_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame
+ float baro_corr = 0.0f; // D
+ float gps_corr[2][2] = {
+ { 0.0f, 0.0f }, // N (pos, vel)
+ { 0.0f, 0.0f }, // E (pos, vel)
+ };
+ float sonar_corr = 0.0f;
+ float sonar_corr_filtered = 0.0f;
+ float flow_corr[] = { 0.0f, 0.0f }; // X, Y
+
+ float sonar_prev = 0.0f;
+ hrt_abstime sonar_time = 0;
+
+ /* main loop */
+ struct pollfd fds[7] = {
+ { .fd = parameter_update_sub, .events = POLLIN },
+ { .fd = actuator_sub, .events = POLLIN },
+ { .fd = armed_sub, .events = POLLIN },
+ { .fd = vehicle_attitude_sub, .events = POLLIN },
+ { .fd = sensor_combined_sub, .events = POLLIN },
+ { .fd = optical_flow_sub, .events = POLLIN },
+ { .fd = vehicle_gps_position_sub, .events = POLLIN }
+ };
+
+ if (!thread_should_exit) {
+ warnx("main loop started.");
+ }
+
+ while (!thread_should_exit) {
+ int ret = poll(fds, 7, 10); // wait maximal this 10 ms = 100 Hz minimum rate
+ hrt_abstime t = hrt_absolute_time();
+
+ if (ret < 0) {
+ /* poll error */
+ warnx("subscriptions poll error.");
+ thread_should_exit = true;
+ continue;
+
+ } else if (ret > 0) {
+ /* parameter update */
+ if (fds[0].revents & POLLIN) {
+ /* read from param to clear updated flag */
+ struct parameter_update_s update;
+ orb_copy(ORB_ID(parameter_update), parameter_update_sub,
+ &update);
+ /* update parameters */
+ parameters_update(&pos_inav_param_handles, ¶ms);
+ }
+
+ /* actuator */
+ if (fds[1].revents & POLLIN) {
+ orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, actuator_sub, &actuator);
+ }
+
+ /* armed */
+ if (fds[2].revents & POLLIN) {
+ orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
+ }
+
+ /* vehicle attitude */
+ if (fds[3].revents & POLLIN) {
+ orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
+ attitude_updates++;
+ }
+
+ /* sensor combined */
+ if (fds[4].revents & POLLIN) {
+ orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
+
+ if (sensor.accelerometer_counter > accel_counter) {
+ if (att.R_valid) {
+ /* correct accel bias, now only for Z */
+ sensor.accelerometer_m_s2[2] -= accel_bias[2];
+
+ /* transform acceleration vector from body frame to NED frame */
+ for (int i = 0; i < 3; i++) {
+ accel_NED[i] = 0.0f;
+
+ for (int j = 0; j < 3; j++) {
+ accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j];
+ }
+ }
+
+ accel_corr[0] = accel_NED[0] - x_est[2];
+ accel_corr[1] = accel_NED[1] - y_est[2];
+ accel_corr[2] = accel_NED[2] + CONSTANTS_ONE_G - z_est[2];
+
+ } else {
+ memset(accel_corr, 0, sizeof(accel_corr));
+ }
+
+ accel_counter = sensor.accelerometer_counter;
+ accel_updates++;
+ }
+
+ if (sensor.baro_counter > baro_counter) {
+ baro_corr = - sensor.baro_alt_meter - z_est[0];
+ baro_counter = sensor.baro_counter;
+ baro_updates++;
+ }
+ }
+
+ /* optical flow */
+ if (fds[5].revents & POLLIN) {
+ orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
+
+ if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && (flow.ground_distance_m != sonar_prev || t - sonar_time < 150000)) {
+ if (flow.ground_distance_m != sonar_prev) {
+ sonar_time = t;
+ sonar_prev = flow.ground_distance_m;
+ sonar_corr = -flow.ground_distance_m - z_est[0];
+ sonar_corr_filtered += (sonar_corr - sonar_corr_filtered) * params.sonar_filt;
+
+ if (fabsf(sonar_corr) > params.sonar_err) {
+ // correction is too large: spike or new ground level?
+ if (fabsf(sonar_corr - sonar_corr_filtered) > params.sonar_err) {
+ // spike detected, ignore
+ sonar_corr = 0.0f;
+
+ } else {
+ // new ground level
+ baro_alt0 += sonar_corr;
+ 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();
+ z_est[0] += sonar_corr;
+ sonar_corr = 0.0f;
+ sonar_corr_filtered = 0.0f;
+ }
+ }
+ }
+
+ } else {
+ sonar_corr = 0.0f;
+ }
+
+ flow_updates++;
+ }
+
+ /* vehicle GPS position */
+ 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 && gps.eph_m < 4.0f) {
+ /* initialize reference position if needed */
+ if (!ref_xy_inited) {
+ if (ref_xy_init_start == 0) {
+ ref_xy_init_start = t;
+
+ } else if (t > ref_xy_init_start + ref_xy_init_delay) {
+ ref_xy_inited = true;
+ /* reference GPS position */
+ double lat = gps.lat * 1e-7;
+ double lon = gps.lon * 1e-7;
+
+ local_pos.ref_lat = gps.lat;
+ local_pos.ref_lon = gps.lon;
+ local_pos.ref_timestamp = t;
+
+ /* initialize projection */
+ map_projection_init(lat, lon);
+ warnx("init GPS: lat = %.10f, lon = %.10f", lat, lon);
+ mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat, lon);
+ }
+ }
+
+ if (ref_xy_inited) {
+ /* project GPS lat lon to plane */
+ float gps_proj[2];
+ map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1]));
+ /* calculate correction for position */
+ gps_corr[0][0] = gps_proj[0] - x_est[0];
+ gps_corr[1][0] = gps_proj[1] - y_est[0];
+
+ /* calculate correction for velocity */
+ if (gps.vel_ned_valid) {
+ gps_corr[0][1] = gps.vel_n_m_s - x_est[1];
+ gps_corr[1][1] = gps.vel_e_m_s - y_est[1];
+
+ } else {
+ gps_corr[0][1] = 0.0f;
+ gps_corr[1][1] = 0.0f;
+ }
+ }
+
+ } else {
+ /* no GPS lock */
+ memset(gps_corr, 0, sizeof(gps_corr));
+ ref_xy_init_start = 0;
+ }
+
+ gps_updates++;
+ }
+ }
+
+ /* end of poll return value check */
+
+ 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;
+
+ /* inertial filter prediction for altitude */
+ inertial_filter_predict(dt, z_est);
+
+ /* inertial filter correction for altitude */
+ baro_alt0 += sonar_corr * params.w_alt_sonar * dt;
+ inertial_filter_correct(baro_corr + baro_alt0, dt, z_est, 0, params.w_alt_baro);
+ inertial_filter_correct(sonar_corr, dt, z_est, 0, params.w_alt_sonar);
+ inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc);
+
+ bool gps_valid = ref_xy_inited && gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout;
+ bool flow_valid = false; // TODO implement opt flow
+
+ /* try to estimate xy even if no absolute position source available,
+ * if using optical flow velocity will be correct in this case */
+ bool can_estimate_xy = gps_valid || flow_valid;
+
+ if (can_estimate_xy) {
+ /* inertial filter prediction for position */
+ inertial_filter_predict(dt, x_est);
+ inertial_filter_predict(dt, y_est);
+
+ /* inertial filter correction for position */
+ inertial_filter_correct(accel_corr[0], dt, x_est, 2, params.w_pos_acc);
+ inertial_filter_correct(accel_corr[1], dt, y_est, 2, params.w_pos_acc);
+
+ if (gps_valid) {
+ inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p);
+ inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p);
+
+ if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_timeout) {
+ inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v);
+ inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v);
+ }
+ }
+ }
+
+ /* detect land */
+ alt_avg += (z_est[0] - alt_avg) * dt / params.land_t;
+ float alt_disp = z_est[0] - alt_avg;
+ alt_disp = alt_disp * alt_disp;
+ float land_disp2 = params.land_disp * params.land_disp;
+ /* get actual thrust output */
+ float thrust = armed.armed ? actuator.control_effective[3] : 0.0f;
+
+ if (landed) {
+ if (alt_disp > land_disp2 && thrust > params.land_thr) {
+ landed = false;
+ landed_time = 0;
+ }
+
+ } else {
+ if (alt_disp < land_disp2 && thrust < params.land_thr) {
+ if (landed_time == 0) {
+ landed_time = t; // land detected first time
+
+ } else {
+ if (t > landed_time + params.land_t * 1000000.0f) {
+ landed = true;
+ landed_time = 0;
+ }
+ }
+
+ } else {
+ landed_time = 0;
+ }
+ }
+
+ if (verbose_mode) {
+ /* print updates rate */
+ if (t > updates_counter_start + updates_counter_len) {
+ float updates_dt = (t - updates_counter_start) * 0.000001f;
+ warnx(
+ "updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s",
+ accel_updates / updates_dt,
+ baro_updates / updates_dt,
+ gps_updates / updates_dt,
+ attitude_updates / updates_dt,
+ flow_updates / updates_dt);
+ updates_counter_start = t;
+ accel_updates = 0;
+ baro_updates = 0;
+ gps_updates = 0;
+ attitude_updates = 0;
+ flow_updates = 0;
+ }
+ }
+
+ if (t > pub_last + pub_interval) {
+ pub_last = t;
+ /* publish local position */
+ local_pos.timestamp = t;
+ local_pos.xy_valid = can_estimate_xy && gps_valid;
+ local_pos.v_xy_valid = can_estimate_xy;
+ local_pos.global_xy = local_pos.xy_valid && gps_valid; // will make sense when local position sources (e.g. vicon) will be implemented
+ local_pos.x = x_est[0];
+ local_pos.vx = x_est[1];
+ local_pos.y = y_est[0];
+ local_pos.vy = y_est[1];
+ 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);
+
+ /* publish global position */
+ global_pos.valid = local_pos.global_xy;
+
+ if (local_pos.global_xy) {
+ double est_lat, est_lon;
+ map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon);
+ global_pos.lat = (int32_t)(est_lat * 1e7);
+ global_pos.lon = (int32_t)(est_lon * 1e7);
+ global_pos.time_gps_usec = gps.time_gps_usec;
+ }
+
+ /* set valid values even if position is not valid */
+ if (local_pos.v_xy_valid) {
+ global_pos.vx = local_pos.vx;
+ global_pos.vy = local_pos.vy;
+ }
+
+ if (local_pos.z_valid) {
+ global_pos.relative_alt = -local_pos.z;
+ }
+
+ if (local_pos.global_z) {
+ global_pos.alt = local_pos.ref_alt - local_pos.z;
+ }
+
+ 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.");
+ mavlink_log_info(mavlink_fd, "[inav] exiting");
+ thread_running = false;
+ return 0;
+}
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c
new file mode 100644
index 0000000000000000000000000000000000000000..4f9ddd009d4271a976204e2618b5b2cbb652dea6
--- /dev/null
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c
@@ -0,0 +1,96 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ * Author: Anton Babushkin <rk3dov@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 position_estimator_inav_params.c
+ *
+ * Parameters for position_estimator_inav
+ */
+
+#include "position_estimator_inav_params.h"
+
+PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 0.5f);
+PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f);
+PARAM_DEFINE_FLOAT(INAV_W_ALT_SONAR, 3.0f);
+PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 1.0f);
+PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 2.0f);
+PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 10.0f);
+PARAM_DEFINE_FLOAT(INAV_W_POS_FLOW, 0.0f);
+PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.0f);
+PARAM_DEFINE_FLOAT(INAV_FLOW_K, 1.0f);
+PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.02f);
+PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f);
+PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f);
+PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f);
+PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.3f);
+
+int parameters_init(struct position_estimator_inav_param_handles *h)
+{
+ h->w_alt_baro = param_find("INAV_W_ALT_BARO");
+ h->w_alt_acc = param_find("INAV_W_ALT_ACC");
+ h->w_alt_sonar = param_find("INAV_W_ALT_SONAR");
+ h->w_pos_gps_p = param_find("INAV_W_POS_GPS_P");
+ h->w_pos_gps_v = param_find("INAV_W_POS_GPS_V");
+ h->w_pos_acc = param_find("INAV_W_POS_ACC");
+ h->w_pos_flow = param_find("INAV_W_POS_FLOW");
+ h->w_acc_bias = param_find("INAV_W_ACC_BIAS");
+ h->flow_k = param_find("INAV_FLOW_K");
+ h->sonar_filt = param_find("INAV_SONAR_FILT");
+ h->sonar_err = param_find("INAV_SONAR_ERR");
+ h->land_t = param_find("INAV_LAND_T");
+ h->land_disp = param_find("INAV_LAND_DISP");
+ h->land_thr = param_find("INAV_LAND_THR");
+
+ return OK;
+}
+
+int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p)
+{
+ param_get(h->w_alt_baro, &(p->w_alt_baro));
+ param_get(h->w_alt_acc, &(p->w_alt_acc));
+ param_get(h->w_alt_sonar, &(p->w_alt_sonar));
+ param_get(h->w_pos_gps_p, &(p->w_pos_gps_p));
+ param_get(h->w_pos_gps_v, &(p->w_pos_gps_v));
+ param_get(h->w_pos_acc, &(p->w_pos_acc));
+ param_get(h->w_pos_flow, &(p->w_pos_flow));
+ param_get(h->w_acc_bias, &(p->w_acc_bias));
+ param_get(h->flow_k, &(p->flow_k));
+ param_get(h->sonar_filt, &(p->sonar_filt));
+ param_get(h->sonar_err, &(p->sonar_err));
+ param_get(h->land_t, &(p->land_t));
+ param_get(h->land_disp, &(p->land_disp));
+ param_get(h->land_thr, &(p->land_thr));
+
+ return OK;
+}
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h
new file mode 100644
index 0000000000000000000000000000000000000000..61570aea7a047ae46fb0a4f9527fd0ecb574ce71
--- /dev/null
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h
@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ * Author: Anton Babushkin <rk3dov@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 position_estimator_inav_params.h
+ *
+ * Parameters for Position Estimator
+ */
+
+#include <systemlib/param/param.h>
+
+struct position_estimator_inav_params {
+ float w_alt_baro;
+ float w_alt_acc;
+ float w_alt_sonar;
+ float w_pos_gps_p;
+ float w_pos_gps_v;
+ float w_pos_acc;
+ float w_pos_flow;
+ float w_acc_bias;
+ float flow_k;
+ float sonar_filt;
+ float sonar_err;
+ float land_t;
+ float land_disp;
+ float land_thr;
+};
+
+struct position_estimator_inav_param_handles {
+ param_t w_alt_baro;
+ param_t w_alt_acc;
+ param_t w_alt_sonar;
+ param_t w_pos_gps_p;
+ param_t w_pos_gps_v;
+ param_t w_pos_acc;
+ param_t w_pos_flow;
+ param_t w_acc_bias;
+ param_t flow_k;
+ param_t sonar_filt;
+ param_t sonar_err;
+ param_t land_t;
+ param_t land_disp;
+ param_t land_thr;
+};
+
+/**
+ * Initialize all parameter handles and values
+ *
+ */
+int parameters_init(struct position_estimator_inav_param_handles *h);
+
+/**
+ * Update all parameters
+ *
+ */
+int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p);
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index fbd82a4c6ddcd07d530a098110384284616922a2..796c6cd9f54725d3a4a4941d71f583469145ff81 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -59,14 +59,14 @@ static perf_counter_t c_gather_ppm;
void
controls_init(void)
{
- /* DSM input */
+ /* DSM input (USART1) */
dsm_init("/dev/ttyS0");
- /* S.bus input */
+ /* S.bus input (USART3) */
sbus_init("/dev/ttyS2");
/* default to a 1:1 input map, all enabled */
- for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) {
+ for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = 0;
@@ -124,7 +124,7 @@ controls_tick() {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
perf_end(c_gather_ppm);
- ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS);
+ ASSERT(r_raw_rc_count <= PX4IO_CONTROL_CHANNELS);
/*
* In some cases we may have received a frame, but input has still
@@ -197,7 +197,7 @@ controls_tick() {
/* and update the scaled/mapped version */
unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
- ASSERT(mapped < MAX_CONTROL_CHANNELS);
+ ASSERT(mapped < PX4IO_CONTROL_CHANNELS);
/* invert channel if pitch - pulling the lever down means pitching up by convention */
if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
@@ -209,7 +209,7 @@ controls_tick() {
}
/* set un-assigned controls to zero */
- for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) {
+ for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
if (!(assigned_channels & (1 << i)))
r_rc_values[i] = 0;
}
@@ -321,8 +321,8 @@ ppm_input(uint16_t *values, uint16_t *num_values)
/* PPM data exists, copy it */
*num_values = ppm_decoded_channels;
- if (*num_values > MAX_CONTROL_CHANNELS)
- *num_values = MAX_CONTROL_CHANNELS;
+ if (*num_values > PX4IO_CONTROL_CHANNELS)
+ *num_values = PX4IO_CONTROL_CHANNELS;
for (unsigned i = 0; i < *num_values; i++)
values[i] = ppm_buffer[i];
diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c
index 745cdfa403ef4170693c8cc2e42c512e478a2753..206e27db5aadaa14524b1c33163972baf22f7689 100644
--- a/src/modules/px4iofirmware/dsm.c
+++ b/src/modules/px4iofirmware/dsm.c
@@ -53,13 +53,13 @@
#define DSM_FRAME_SIZE 16 /**<DSM frame size in bytes*/
#define DSM_FRAME_CHANNELS 7 /**<Max supported DSM channels*/
-static int dsm_fd = -1; /**<File handle to the DSM UART*/
-static hrt_abstime dsm_last_rx_time; /**<Timestamp when we last received*/
-static hrt_abstime dsm_last_frame_time; /**<Timestamp for start of last dsm frame*/
-static uint8_t dsm_frame[DSM_FRAME_SIZE]; /**<DSM dsm frame receive buffer*/
-static unsigned dsm_partial_frame_count; /**<Count of bytes received for current dsm frame*/
-static unsigned dsm_channel_shift; /**<Channel resolution, 0=unknown, 1=10 bit, 2=11 bit*/
-static unsigned dsm_frame_drops; /**<Count of incomplete DSM frames*/
+static int dsm_fd = -1; /**< File handle to the DSM UART */
+static hrt_abstime dsm_last_rx_time; /**< Timestamp when we last received */
+static hrt_abstime dsm_last_frame_time; /**< Timestamp for start of last dsm frame */
+static uint8_t dsm_frame[DSM_FRAME_SIZE]; /**< DSM dsm frame receive buffer */
+static unsigned dsm_partial_frame_count; /**< Count of bytes received for current dsm frame */
+static unsigned dsm_channel_shift; /**< Channel resolution, 0=unknown, 1=10 bit, 2=11 bit */
+static unsigned dsm_frame_drops; /**< Count of incomplete DSM frames */
/**
* Attempt to decode a single channel raw channel datum
@@ -249,6 +249,10 @@ dsm_bind(uint16_t cmd, int pulses)
if (dsm_fd < 0)
return;
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
+ // XXX implement
+ #warning DSM BIND NOT IMPLEMENTED ON PX4IO V2
+#else
switch (cmd) {
case dsm_bind_power_down:
@@ -288,6 +292,7 @@ dsm_bind(uint16_t cmd, int pulses)
break;
}
+#endif
}
/**
diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c
index 4485daa5b1b072ec78c637d254b48f6de1500eab..10aeb5c9fab4e1cdb9a47060700e4fdddb4c9f27 100644
--- a/src/modules/px4iofirmware/i2c.c
+++ b/src/modules/px4iofirmware/i2c.c
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Copyright (C) 2012,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
@@ -69,6 +69,7 @@ static void i2c_rx_setup(void);
static void i2c_tx_setup(void);
static void i2c_rx_complete(void);
static void i2c_tx_complete(void);
+static void i2c_dump(void);
static DMA_HANDLE rx_dma;
static DMA_HANDLE tx_dma;
@@ -92,7 +93,7 @@ enum {
} direction;
void
-i2c_init(void)
+interface_init(void)
{
debug("i2c init");
@@ -148,12 +149,18 @@ i2c_init(void)
#endif
}
+void
+interface_tick()
+{
+}
+
/*
reset the I2C bus
used to recover from lockups
*/
-void i2c_reset(void)
+void
+i2c_reset(void)
{
rCR1 |= I2C_CR1_SWRST;
rCR1 = 0;
@@ -330,7 +337,7 @@ i2c_tx_complete(void)
i2c_tx_setup();
}
-void
+static void
i2c_dump(void)
{
debug("CR1 0x%08x CR2 0x%08x", rCR1, rCR2);
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index a2193b526be0bafd7e914ade72020fdd9f11fdc7..deed258364f5749ba56486e6946e234d46bb50d1 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -59,6 +59,12 @@ extern "C" {
*/
#define FMU_INPUT_DROP_LIMIT_US 200000
+/*
+ * Time that the ESCs need to initialize
+ */
+ #define ESC_INIT_TIME_US 1000000
+ #define ESC_RAMP_TIME_US 2000000
+
/* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */
#define ROLL 0
#define PITCH 1
@@ -68,6 +74,17 @@ extern "C" {
/* current servo arm/disarm state */
static bool mixer_servos_armed = false;
+static bool should_arm = false;
+static bool should_always_enable_pwm = false;
+static uint64_t esc_init_time;
+
+enum esc_state_e {
+ ESC_OFF,
+ ESC_INIT,
+ ESC_RAMP,
+ ESC_ON
+};
+static esc_state_e esc_state;
/* selected control values and count for mixing */
enum mixer_source {
@@ -98,7 +115,7 @@ mixer_tick(void)
if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
isr_debug(1, "AP RX timeout");
}
- r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM);
+ r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK);
r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST;
} else {
@@ -112,12 +129,11 @@ mixer_tick(void)
* Decide which set of controls we're using.
*/
- /* do not mix if mixer is invalid or if RAW_PWM mode is on and FMU is good */
+ /* do not mix if RAW_PWM mode is on and FMU is good */
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) &&
- !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
+ (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
- /* don't actually mix anything - we already have raw PWM values or
- not a valid mixer. */
+ /* don't actually mix anything - we already have raw PWM values */
source = MIX_NONE;
} else {
@@ -132,7 +148,8 @@ mixer_tick(void)
if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
- (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
+ (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
+ !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)) {
/* if allowed, mix from RC inputs directly */
source = MIX_OVERRIDE;
@@ -154,7 +171,7 @@ mixer_tick(void)
if (source == MIX_FAILSAFE) {
/* copy failsafe values to the servo outputs */
- for (unsigned i = 0; i < IO_SERVO_COUNT; i++) {
+ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
r_page_servos[i] = r_page_servo_failsafe[i];
/* safe actuators for FMU feedback */
@@ -164,11 +181,53 @@ mixer_tick(void)
} else if (source != MIX_NONE) {
- float outputs[IO_SERVO_COUNT];
+ float outputs[PX4IO_SERVO_COUNT];
unsigned mixed;
+ uint16_t ramp_promille;
+
+ /* update esc init state, but only if we are truely armed and not just PWM enabled */
+ if (mixer_servos_armed && should_arm) {
+
+ switch (esc_state) {
+
+ /* after arming, some ESCs need an initalization period, count the time from here */
+ case ESC_OFF:
+ esc_init_time = hrt_absolute_time();
+ esc_state = ESC_INIT;
+ break;
+
+ /* after waiting long enough for the ESC initialization, we can start with the ramp to start the ESCs */
+ case ESC_INIT:
+ if (hrt_elapsed_time(&esc_init_time) > ESC_INIT_TIME_US) {
+ esc_state = ESC_RAMP;
+ }
+ break;
+
+ /* then ramp until the min speed is reached */
+ case ESC_RAMP:
+ if (hrt_elapsed_time(&esc_init_time) > (ESC_INIT_TIME_US + ESC_RAMP_TIME_US)) {
+ esc_state = ESC_ON;
+ }
+ break;
+
+ case ESC_ON:
+ default:
+
+ break;
+ }
+ } else {
+ esc_state = ESC_OFF;
+ }
+
+ /* do the calculations during the ramp for all at once */
+ if(esc_state == ESC_RAMP) {
+ ramp_promille = (1000*(hrt_elapsed_time(&esc_init_time)-ESC_INIT_TIME_US))/ESC_RAMP_TIME_US;
+ }
+
+
/* mix */
- mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
+ mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
/* scale to PWM and update the servo outputs as required */
for (unsigned i = 0; i < mixed; i++) {
@@ -176,11 +235,29 @@ mixer_tick(void)
/* save actuator values for FMU readback */
r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
- /* scale to servo output */
- r_page_servos[i] = (outputs[i] * 600.0f) + 1500;
-
+ switch (esc_state) {
+ case ESC_INIT:
+ r_page_servos[i] = (outputs[i] * 600 + 1500);
+ break;
+
+ case ESC_RAMP:
+ r_page_servos[i] = (outputs[i]
+ * (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 - ramp_promille*r_page_servo_control_min[i] - (1000-ramp_promille)*900)/2/1000
+ + (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 + ramp_promille*r_page_servo_control_min[i] + (1000-ramp_promille)*900)/2/1000);
+ break;
+
+ case ESC_ON:
+ r_page_servos[i] = (outputs[i]
+ * (r_page_servo_control_max[i] - r_page_servo_control_min[i])/2
+ + (r_page_servo_control_max[i] + r_page_servo_control_min[i])/2);
+ break;
+
+ case ESC_OFF:
+ default:
+ break;
+ }
}
- for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
+ for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
r_page_servos[i] = 0;
}
@@ -193,30 +270,46 @@ mixer_tick(void)
* XXX correct behaviour for failsafe may require an additional case
* here.
*/
- bool should_arm = (
- /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
- /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
- /* there is valid input via direct PWM or mixer */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) &&
- /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) &&
- /* FMU is available or FMU is not available but override is an option */
- ((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ))
+ should_arm = (
+ /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
+ /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
+ /* and FMU is armed */ && (
+ ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)
+ /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) )
+ /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)
+ /* or failsafe was set manually */ || (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)
+ )
);
- if (should_arm && !mixer_servos_armed) {
+ should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)
+ && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
+ && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK);
+
+ if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) {
/* need to arm, but not armed */
up_pwm_servo_arm(true);
mixer_servos_armed = true;
+ r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED;
+ isr_debug(5, "> PWM enabled");
- } else if (!should_arm && mixer_servos_armed) {
+ } else if ((!should_arm && !should_always_enable_pwm) && mixer_servos_armed) {
/* armed but need to disarm */
up_pwm_servo_arm(false);
mixer_servos_armed = false;
+ r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED);
+ isr_debug(5, "> PWM disabled");
+
}
- if (mixer_servos_armed) {
+ if (mixer_servos_armed && should_arm) {
/* update the servo outputs. */
- for (unsigned i = 0; i < IO_SERVO_COUNT; i++)
+ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
up_pwm_servo_set(i, r_page_servos[i]);
+
+ } else if (mixer_servos_armed && should_always_enable_pwm) {
+ /* set the idle servo outputs. */
+ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
+ up_pwm_servo_set(i, r_page_servo_idle[i]);
}
}
@@ -265,9 +358,8 @@ static unsigned mixer_text_length = 0;
void
mixer_handle_text(const void *buffer, size_t length)
{
- /* do not allow a mixer change while fully armed */
- if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
- /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+ /* do not allow a mixer change while outputs armed */
+ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
return;
}
@@ -344,16 +436,17 @@ mixer_set_failsafe()
* Check if a custom failsafe value has been written,
* or if the mixer is not ok and bail out.
*/
+
if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ||
!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK))
return;
/* set failsafe defaults to the values for all inputs = 0 */
- float outputs[IO_SERVO_COUNT];
+ float outputs[PX4IO_SERVO_COUNT];
unsigned mixed;
/* mix */
- mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
+ mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
/* scale to PWM and update the servo outputs as required */
for (unsigned i = 0; i < mixed; i++) {
@@ -364,7 +457,7 @@ mixer_set_failsafe()
}
/* disable the rest of the outputs */
- for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
+ for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
r_page_servo_failsafe[i] = 0;
}
diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk
index 6379366f434fd89961fe85ee930716c27d06187c..59f470a94ebb5697f5ca73d630381ab855d3da17 100644
--- a/src/modules/px4iofirmware/module.mk
+++ b/src/modules/px4iofirmware/module.mk
@@ -3,17 +3,22 @@
SRCS = adc.c \
controls.c \
dsm.c \
- i2c.c \
px4io.c \
registers.c \
safety.c \
sbus.c \
../systemlib/up_cxxinitialize.c \
- ../systemlib/hx_stream.c \
../systemlib/perf_counter.c \
mixer.cpp \
../systemlib/mixer/mixer.cpp \
../systemlib/mixer/mixer_group.cpp \
../systemlib/mixer/mixer_multirotor.cpp \
../systemlib/mixer/mixer_simple.cpp \
-
\ No newline at end of file
+
+ifeq ($(BOARD),px4io-v1)
+SRCS += i2c.c
+endif
+ifeq ($(BOARD),px4io-v2)
+SRCS += serial.c \
+ ../systemlib/hx_stream.c
+endif
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 88d8cc87c15f7e5182cbd79b000cf26e7fef2f57..f5fa0ba751cd6e3817aa70300263916daf5d37c5 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -36,7 +36,7 @@
/**
* @file protocol.h
*
- * PX4IO I2C interface protocol.
+ * PX4IO interface protocol.
*
* Communication is performed via writes to and reads from 16-bit virtual
* registers organised into pages of 255 registers each.
@@ -45,7 +45,7 @@
* respectively. Subsequent reads and writes increment the offset within
* the page.
*
- * Most pages are readable or writable but not both.
+ * Some pages are read- or write-only.
*
* Note that some pages may permit offset values greater than 255, which
* can only be achieved by long writes. The offset does not wrap.
@@ -62,12 +62,11 @@
* Note that the implementation of readable pages prefers registers within
* readable pages to be densely packed. Page numbers do not need to be
* packed.
+ *
+ * Definitions marked 1 are only valid on PX4IOv1 boards. Likewise,
+ * [2] denotes definitions specific to the PX4IOv2 board.
*/
-#define PX4IO_CONTROL_CHANNELS 8
-#define PX4IO_INPUT_CHANNELS 12
-#define PX4IO_RELAY_CHANNELS 4
-
/* Per C, this is safe for all 2's complement systems */
#define REG_TO_SIGNED(_reg) ((int16_t)(_reg))
#define SIGNED_TO_REG(_signed) ((uint16_t)(_signed))
@@ -75,10 +74,12 @@
#define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f)
#define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f))
+#define PX4IO_PROTOCOL_VERSION 4
+
/* static configuration page */
#define PX4IO_PAGE_CONFIG 0
-#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers TBD */
-#define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers TBD */
+#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* PX4IO_PROTOCOL_VERSION */
+#define PX4IO_P_CONFIG_HARDWARE_VERSION 1 /* magic numbers TBD */
#define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */
#define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */
#define PX4IO_P_CONFIG_CONTROL_COUNT 4 /* hardcoded max control count supported */
@@ -93,7 +94,7 @@
#define PX4IO_P_STATUS_CPULOAD 1
#define PX4IO_P_STATUS_FLAGS 2 /* monitoring flags */
-#define PX4IO_P_STATUS_FLAGS_ARMED (1 << 0) /* arm-ok and locally armed */
+#define PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED (1 << 0) /* arm-ok and locally armed */
#define PX4IO_P_STATUS_FLAGS_OVERRIDE (1 << 1) /* in manual override */
#define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 2) /* RC input is valid */
#define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 3) /* PPM input is valid */
@@ -105,19 +106,24 @@
#define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */
#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */
#define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */
-#define PX4IO_P_STATUS_FLAGS_RC_DSM11 (1 << 12) /* DSM input is 11 bit data */
+#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */
+#define PX4IO_P_STATUS_FLAGS_RC_DSM11 (1 << 13) /* DSM input is 11 bit data */
#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */
-#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* VBatt is very close to regulator dropout */
+#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* [1] VBatt is very close to regulator dropout */
#define PX4IO_P_STATUS_ALARMS_TEMPERATURE (1 << 1) /* board temperature is high */
-#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT (1 << 2) /* servo current limit was exceeded */
-#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT (1 << 3) /* accessory current limit was exceeded */
+#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT (1 << 2) /* [1] servo current limit was exceeded */
+#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT (1 << 3) /* [1] accessory current limit was exceeded */
#define PX4IO_P_STATUS_ALARMS_FMU_LOST (1 << 4) /* timed out waiting for controls from FMU */
#define PX4IO_P_STATUS_ALARMS_RC_LOST (1 << 5) /* timed out waiting for RC input */
#define PX4IO_P_STATUS_ALARMS_PWM_ERROR (1 << 6) /* PWM configuration or output was bad */
+#define PX4IO_P_STATUS_ALARMS_VSERVO_FAULT (1 << 7) /* [2] VServo was out of the valid range (2.5 - 5.5 V) */
-#define PX4IO_P_STATUS_VBATT 4 /* battery voltage in mV */
-#define PX4IO_P_STATUS_IBATT 5 /* battery current (raw ADC) */
+#define PX4IO_P_STATUS_VBATT 4 /* [1] battery voltage in mV */
+#define PX4IO_P_STATUS_IBATT 5 /* [1] battery current (raw ADC) */
+#define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */
+#define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */
+#define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */
/* array of post-mix actuator outputs, -10000..10000 */
#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */
@@ -143,7 +149,7 @@
#define PX4IO_RATE_MAP_BASE 0 /* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */
/* setup page */
-#define PX4IO_PAGE_SETUP 100
+#define PX4IO_PAGE_SETUP 50
#define PX4IO_P_SETUP_FEATURES 0
#define PX4IO_P_SETUP_ARMING 1 /* arming controls */
@@ -152,37 +158,40 @@
#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 2) /* OK to switch to manual override via override RC channel */
#define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM (1 << 3) /* use custom failsafe values, not 0 values of mixer */
#define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 4) /* OK to try in-air restart */
+#define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE (1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */
+#define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED (1 << 6) /* Disable the IO-internal evaluation of the RC */
#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */
#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */
#define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */
-#define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */
-/* px4io relay bit definitions */
-#define PX4IO_RELAY1 (1<<0)
-#define PX4IO_RELAY2 (1<<1)
-#define PX4IO_ACC1 (1<<2)
-#define PX4IO_ACC2 (1<<3)
+#define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */
+#define PX4IO_P_SETUP_RELAYS_POWER1 (1<<0) /* hardware rev [1] power relay 1 */
+#define PX4IO_P_SETUP_RELAYS_POWER2 (1<<1) /* hardware rev [1] power relay 2 */
+#define PX4IO_P_SETUP_RELAYS_ACC1 (1<<2) /* hardware rev [1] accessory power 1 */
+#define PX4IO_P_SETUP_RELAYS_ACC2 (1<<3) /* hardware rev [1] accessory power 2 */
-#define PX4IO_P_SETUP_VBATT_SCALE 6 /* battery voltage correction factor (float) */
-#define PX4IO_P_SETUP_DSM 7 /* DSM bind state */
-enum { /* DSM bind states */
+#define PX4IO_P_SETUP_VBATT_SCALE 6 /* hardware rev [1] battery voltage correction factor (float) */
+#define PX4IO_P_SETUP_VSERVO_SCALE 6 /* hardware rev [2] servo voltage correction factor (float) */
+#define PX4IO_P_SETUP_DSM 7 /* DSM bind state */
+enum { /* DSM bind states */
dsm_bind_power_down = 0,
dsm_bind_power_up,
dsm_bind_set_rx_out,
dsm_bind_send_pulses,
dsm_bind_reinit_uart
};
+ /* 8 */
#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */
/* autopilot control values, -10000..10000 */
-#define PX4IO_PAGE_CONTROLS 101 /* 0..CONFIG_CONTROL_COUNT */
+#define PX4IO_PAGE_CONTROLS 51 /* 0..CONFIG_CONTROL_COUNT */
/* raw text load to the mixer parser - ignores offset */
-#define PX4IO_PAGE_MIXERLOAD 102
+#define PX4IO_PAGE_MIXERLOAD 52
/* R/C channel config */
-#define PX4IO_PAGE_RC_CONFIG 103 /* R/C input configuration */
+#define PX4IO_PAGE_RC_CONFIG 53 /* R/C input configuration */
#define PX4IO_P_RC_CONFIG_MIN 0 /* lowest input value */
#define PX4IO_P_RC_CONFIG_CENTER 1 /* center input value */
#define PX4IO_P_RC_CONFIG_MAX 2 /* highest input value */
@@ -194,10 +203,23 @@ enum { /* DSM bind states */
#define PX4IO_P_RC_CONFIG_STRIDE 6 /* spacing between channel config data */
/* PWM output - overrides mixer */
-#define PX4IO_PAGE_DIRECT_PWM 104 /* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_DIRECT_PWM 54 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/* PWM failsafe values - zero disables the output */
-#define PX4IO_PAGE_FAILSAFE_PWM 105 /* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_FAILSAFE_PWM 55 /* 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* Debug and test page - not used in normal operation */
+#define PX4IO_PAGE_TEST 127
+#define PX4IO_P_TEST_LED 0 /* set the amber LED on/off */
+
+/* PWM minimum values for certain ESCs */
+#define PX4IO_PAGE_CONTROL_MIN_PWM 106 /* 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* PWM maximum values for certain ESCs */
+#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /* 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* PWM idle values that are active, even when SAFETY_SAFE */
+#define PX4IO_PAGE_IDLE_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/**
* As-needed mixer data upload.
@@ -218,3 +240,81 @@ struct px4io_mixdata {
};
#pragma pack(pop)
+/**
+ * Serial protocol encapsulation.
+ */
+
+#define PKT_MAX_REGS 32 // by agreement w/FMU
+
+#pragma pack(push, 1)
+struct IOPacket {
+ uint8_t count_code;
+ uint8_t crc;
+ uint8_t page;
+ uint8_t offset;
+ uint16_t regs[PKT_MAX_REGS];
+};
+#pragma pack(pop)
+
+#define PKT_CODE_READ 0x00 /* FMU->IO read transaction */
+#define PKT_CODE_WRITE 0x40 /* FMU->IO write transaction */
+#define PKT_CODE_SUCCESS 0x00 /* IO->FMU success reply */
+#define PKT_CODE_CORRUPT 0x40 /* IO->FMU bad packet reply */
+#define PKT_CODE_ERROR 0x80 /* IO->FMU register op error reply */
+
+#define PKT_CODE_MASK 0xc0
+#define PKT_COUNT_MASK 0x3f
+
+#define PKT_COUNT(_p) ((_p).count_code & PKT_COUNT_MASK)
+#define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK)
+#define PKT_SIZE(_p) ((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p)))
+
+static const uint8_t crc8_tab[256] __attribute__((unused)) =
+{
+ 0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15,
+ 0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D,
+ 0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65,
+ 0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D,
+ 0xE0, 0xE7, 0xEE, 0xE9, 0xFC, 0xFB, 0xF2, 0xF5,
+ 0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD,
+ 0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85,
+ 0xA8, 0xAF, 0xA6, 0xA1, 0xB4, 0xB3, 0xBA, 0xBD,
+ 0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2,
+ 0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA,
+ 0xB7, 0xB0, 0xB9, 0xBE, 0xAB, 0xAC, 0xA5, 0xA2,
+ 0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A,
+ 0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32,
+ 0x1F, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0D, 0x0A,
+ 0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42,
+ 0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A,
+ 0x89, 0x8E, 0x87, 0x80, 0x95, 0x92, 0x9B, 0x9C,
+ 0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4,
+ 0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC,
+ 0xC1, 0xC6, 0xCF, 0xC8, 0xDD, 0xDA, 0xD3, 0xD4,
+ 0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C,
+ 0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44,
+ 0x19, 0x1E, 0x17, 0x10, 0x05, 0x02, 0x0B, 0x0C,
+ 0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34,
+ 0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B,
+ 0x76, 0x71, 0x78, 0x7F, 0x6A, 0x6D, 0x64, 0x63,
+ 0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B,
+ 0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13,
+ 0xAE, 0xA9, 0xA0, 0xA7, 0xB2, 0xB5, 0xBC, 0xBB,
+ 0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83,
+ 0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB,
+ 0xE6, 0xE1, 0xE8, 0xEF, 0xFA, 0xFD, 0xF4, 0xF3
+};
+
+static uint8_t crc_packet(struct IOPacket *pkt) __attribute__((unused));
+static uint8_t
+crc_packet(struct IOPacket *pkt)
+{
+ uint8_t *end = (uint8_t *)(&pkt->regs[PKT_COUNT(*pkt)]);
+ uint8_t *p = (uint8_t *)pkt;
+ uint8_t c = 0;
+
+ while (p < end)
+ c = crc8_tab[c ^ *(p++)];
+
+ return c;
+}
diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c
index bc8dfc1167814b52043075d3ba66e1f1622a26f4..e70b3fe88045b2fbd16ac42bc0afc4d5acd5644b 100644
--- a/src/modules/px4iofirmware/px4io.c
+++ b/src/modules/px4iofirmware/px4io.c
@@ -64,11 +64,6 @@ struct sys_state_s system_state;
static struct hrt_call serial_dma_call;
-#ifdef CONFIG_STM32_I2C1
-/* store i2c reset count XXX this should be a register, together with other error counters */
-volatile uint32_t i2c_loop_resets = 0;
-#endif
-
/*
* a set of debug buffers to allow us to send debug information from ISRs
*/
@@ -147,8 +142,10 @@ user_start(int argc, char *argv[])
LED_BLUE(false);
LED_SAFETY(false);
- /* turn on servo power */
+ /* turn on servo power (if supported) */
+#ifdef POWER_SERVO
POWER_SERVO(true);
+#endif
/* start the safety switch handler */
safety_init();
@@ -159,10 +156,11 @@ user_start(int argc, char *argv[])
/* initialise the control inputs */
controls_init();
-#ifdef CONFIG_STM32_I2C1
- /* start the i2c handler */
- i2c_init();
-#endif
+ /* start the FMU interface */
+ interface_init();
+
+ /* add a performance counter for the interface */
+ perf_counter_t interface_perf = perf_alloc(PC_ELAPSED, "interface");
/* add a performance counter for mixing */
perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");
@@ -205,6 +203,11 @@ user_start(int argc, char *argv[])
/* track the rate at which the loop is running */
perf_count(loop_perf);
+ /* kick the interface */
+ perf_begin(interface_perf);
+ interface_tick();
+ perf_end(interface_perf);
+
/* kick the mixer */
perf_begin(mixer_perf);
mixer_tick();
@@ -223,12 +226,11 @@ user_start(int argc, char *argv[])
struct mallinfo minfo = mallinfo();
- isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u m=%u",
+ isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u",
(unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG],
(unsigned)r_status_flags,
(unsigned)r_setup_arming,
(unsigned)r_setup_features,
- (unsigned)i2c_loop_resets,
(unsigned)minfo.mxordblk);
last_debug_time = hrt_absolute_time();
}
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 57cffcc23a4fb51c58b05460ef858c5e5bee14e4..dea67043e36b6d17befaac508f4acb9ba675ef94 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -42,15 +42,16 @@
#include <stdbool.h>
#include <stdint.h>
-#include <drivers/boards/px4io/px4io_internal.h>
+#include <board_config.h>
#include "protocol.h"
/*
* Constants and limits.
*/
-#define MAX_CONTROL_CHANNELS 12
-#define IO_SERVO_COUNT 8
+#define PX4IO_SERVO_COUNT 8
+#define PX4IO_CONTROL_CHANNELS 8
+#define PX4IO_INPUT_CHANNELS 12
/*
* Debug logging
@@ -77,6 +78,9 @@ extern volatile uint16_t r_page_setup[]; /* PX4IO_PAGE_SETUP */
extern volatile uint16_t r_page_controls[]; /* PX4IO_PAGE_CONTROLS */
extern uint16_t r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */
extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */
+extern uint16_t r_page_servo_control_min[]; /* PX4IO_PAGE_CONTROL_MIN_PWM */
+extern uint16_t r_page_servo_control_max[]; /* PX4IO_PAGE_CONTROL_MAX_PWM */
+extern uint16_t r_page_servo_idle[]; /* PX4IO_PAGE_IDLE_PWM */
/*
* Register aliases.
@@ -119,32 +123,43 @@ extern struct sys_state_s system_state;
/*
* GPIO handling.
*/
-#define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED1, !(_s))
-#define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s))
-#define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s))
+#define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED1, !(_s))
+#define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s))
+#define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s))
+
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
+
+# define PX4IO_RELAY_CHANNELS 4
+# define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s))
+# define POWER_ACC1(_s) stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s))
+# define POWER_ACC2(_s) stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s))
+# define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s))
+# define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s))
+
+# define OVERCURRENT_ACC (!stm32_gpioread(GPIO_ACC_OC_DETECT))
+# define OVERCURRENT_SERVO (!stm32_gpioread(GPIO_SERVO_OC_DETECT))
+
+# define PX4IO_ADC_CHANNEL_COUNT 2
+# define ADC_VBATT 4
+# define ADC_IN5 5
-#define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s))
-#ifdef GPIO_ACC1_PWR_EN
- #define POWER_ACC1(_s) stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s))
-#endif
-#ifdef GPIO_ACC2_PWR_EN
- #define POWER_ACC2(_s) stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s))
-#endif
-#ifdef GPIO_RELAY1_EN
- #define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s))
#endif
-#ifdef GPIO_RELAY2_EN
- #define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s))
+
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
+
+# define PX4IO_RELAY_CHANNELS 0
+# define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s))
+
+# define VDD_SERVO_FAULT (!stm32_gpioread(GPIO_SERVO_FAULT_DETECT))
+
+# define PX4IO_ADC_CHANNEL_COUNT 2
+# define ADC_VSERVO 4
+# define ADC_RSSI 5
+
#endif
-#define OVERCURRENT_ACC (!stm32_gpioread(GPIO_ACC_OC_DETECT))
-#define OVERCURRENT_SERVO (!stm32_gpioread(GPIO_SERVO_OC_DETECT))
#define BUTTON_SAFETY stm32_gpioread(GPIO_BTN_SAFETY)
-#define ADC_VBATT 4
-#define ADC_IN5 5
-#define ADC_CHANNEL_COUNT 2
-
/*
* Mixer
*/
@@ -156,17 +171,16 @@ extern void mixer_handle_text(const void *buffer, size_t length);
*/
extern void safety_init(void);
-#ifdef CONFIG_STM32_I2C1
/**
* FMU communications
*/
-extern void i2c_init(void);
-#endif
+extern void interface_init(void);
+extern void interface_tick(void);
/**
* Register space
*/
-extern void registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values);
+extern int registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values);
extern int registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values);
/**
@@ -191,10 +205,5 @@ extern bool sbus_input(uint16_t *values, uint16_t *num_values);
/** global debug level for isr_debug() */
extern volatile uint8_t debug_level;
-/* send a debug message to the console */
+/** send a debug message to the console */
extern void isr_debug(uint8_t level, const char *fmt, ...);
-
-#ifdef CONFIG_STM32_I2C1
-void i2c_dump(void);
-void i2c_reset(void);
-#endif
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index a922362b69baaf5ec9396862b66e602e7bb87087..9c95fd1c52812c3c4dd618d84a8948d0e380300b 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -44,6 +44,7 @@
#include <string.h>
#include <drivers/drv_hrt.h>
+#include <drivers/drv_pwm_output.h>
#include "px4io.h"
#include "protocol.h"
@@ -57,14 +58,18 @@ static void pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t alt
* Static configuration parameters.
*/
static const uint16_t r_page_config[] = {
- [PX4IO_P_CONFIG_PROTOCOL_VERSION] = 1, /* XXX hardcoded magic number */
- [PX4IO_P_CONFIG_SOFTWARE_VERSION] = 1, /* XXX hardcoded magic number */
+ [PX4IO_P_CONFIG_PROTOCOL_VERSION] = PX4IO_PROTOCOL_VERSION,
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
+ [PX4IO_P_CONFIG_HARDWARE_VERSION] = 2,
+#else
+ [PX4IO_P_CONFIG_HARDWARE_VERSION] = 1,
+#endif
[PX4IO_P_CONFIG_BOOTLOADER_VERSION] = 3, /* XXX hardcoded magic number */
[PX4IO_P_CONFIG_MAX_TRANSFER] = 64, /* XXX hardcoded magic number */
[PX4IO_P_CONFIG_CONTROL_COUNT] = PX4IO_CONTROL_CHANNELS,
- [PX4IO_P_CONFIG_ACTUATOR_COUNT] = IO_SERVO_COUNT,
- [PX4IO_P_CONFIG_RC_INPUT_COUNT] = MAX_CONTROL_CHANNELS,
- [PX4IO_P_CONFIG_ADC_INPUT_COUNT] = ADC_CHANNEL_COUNT,
+ [PX4IO_P_CONFIG_ACTUATOR_COUNT] = PX4IO_SERVO_COUNT,
+ [PX4IO_P_CONFIG_RC_INPUT_COUNT] = PX4IO_CONTROL_CHANNELS,
+ [PX4IO_P_CONFIG_ADC_INPUT_COUNT] = PX4IO_ADC_CHANNEL_COUNT,
[PX4IO_P_CONFIG_RELAY_COUNT] = PX4IO_RELAY_CHANNELS,
};
@@ -79,7 +84,10 @@ uint16_t r_page_status[] = {
[PX4IO_P_STATUS_FLAGS] = 0,
[PX4IO_P_STATUS_ALARMS] = 0,
[PX4IO_P_STATUS_VBATT] = 0,
- [PX4IO_P_STATUS_IBATT] = 0
+ [PX4IO_P_STATUS_IBATT] = 0,
+ [PX4IO_P_STATUS_VSERVO] = 0,
+ [PX4IO_P_STATUS_VRSSI] = 0,
+ [PX4IO_P_STATUS_PRSSI] = 0
};
/**
@@ -87,14 +95,14 @@ uint16_t r_page_status[] = {
*
* Post-mixed actuator values.
*/
-uint16_t r_page_actuators[IO_SERVO_COUNT];
+uint16_t r_page_actuators[PX4IO_SERVO_COUNT];
/**
* PAGE 3
*
* Servo PWM values
*/
-uint16_t r_page_servos[IO_SERVO_COUNT];
+uint16_t r_page_servos[PX4IO_SERVO_COUNT];
/**
* PAGE 4
@@ -104,7 +112,7 @@ uint16_t r_page_servos[IO_SERVO_COUNT];
uint16_t r_page_raw_rc_input[] =
{
[PX4IO_P_RAW_RC_COUNT] = 0,
- [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + MAX_CONTROL_CHANNELS)] = 0
+ [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0
};
/**
@@ -114,7 +122,7 @@ uint16_t r_page_raw_rc_input[] =
*/
uint16_t r_page_rc_input[] = {
[PX4IO_P_RC_VALID] = 0,
- [PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + MAX_CONTROL_CHANNELS)] = 0
+ [PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0
};
/**
@@ -138,7 +146,11 @@ volatile uint16_t r_page_setup[] =
[PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50,
[PX4IO_P_SETUP_PWM_ALTRATE] = 200,
[PX4IO_P_SETUP_RELAYS] = 0,
+#ifdef ADC_VSERVO
+ [PX4IO_P_SETUP_VSERVO_SCALE] = 10000,
+#else
[PX4IO_P_SETUP_VBATT_SCALE] = 10000,
+#endif
[PX4IO_P_SETUP_SET_DEBUG] = 0,
};
@@ -146,8 +158,11 @@ volatile uint16_t r_page_setup[] =
#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \
PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \
- PX4IO_P_SETUP_ARMING_IO_ARM_OK)
-#define PX4IO_P_SETUP_RATES_VALID ((1 << IO_SERVO_COUNT) - 1)
+ PX4IO_P_SETUP_ARMING_IO_ARM_OK | \
+ PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \
+ PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \
+ PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)
+#define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1)
#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1)
/**
@@ -166,7 +181,7 @@ volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS];
*
* R/C channel input configuration.
*/
-uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE];
+uint16_t r_page_rc_input_config[PX4IO_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE];
/* valid options */
#define PX4IO_P_RC_CONFIG_OPTIONS_VALID (PX4IO_P_RC_CONFIG_OPTIONS_REVERSE | PX4IO_P_RC_CONFIG_OPTIONS_ENABLED)
@@ -182,9 +197,33 @@ uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE
*
* Disable pulses as default.
*/
-uint16_t r_page_servo_failsafe[IO_SERVO_COUNT] = { 0 };
+uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0 };
-void
+/**
+ * PAGE 106
+ *
+ * minimum PWM values when armed
+ *
+ */
+uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 };
+
+/**
+ * PAGE 107
+ *
+ * maximum PWM values when armed
+ *
+ */
+uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { 2100, 2100, 2100, 2100, 2100, 2100, 2100, 2100 };
+
+/**
+ * PAGE 108
+ *
+ * idle PWM values for difficult ESCs
+ *
+ */
+uint16_t r_page_servo_idle[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 };
+
+int
registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
{
@@ -233,7 +272,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
case PX4IO_PAGE_FAILSAFE_PWM:
/* copy channel data */
- while ((offset < IO_SERVO_COUNT) && (num_values > 0)) {
+ while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
/* XXX range-check value? */
r_page_servo_failsafe[offset] = *values;
@@ -247,6 +286,75 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
}
break;
+ case PX4IO_PAGE_CONTROL_MIN_PWM:
+
+ /* copy channel data */
+ while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
+
+ if (*values == 0)
+ /* set to default */
+ r_page_servo_control_min[offset] = 900;
+
+ else if (*values > 1200)
+ r_page_servo_control_min[offset] = 1200;
+ else if (*values < 900)
+ r_page_servo_control_min[offset] = 900;
+ else
+ r_page_servo_control_min[offset] = *values;
+
+ offset++;
+ num_values--;
+ values++;
+ }
+ break;
+
+ case PX4IO_PAGE_CONTROL_MAX_PWM:
+
+ /* copy channel data */
+ while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
+
+ if (*values == 0)
+ /* set to default */
+ r_page_servo_control_max[offset] = 2100;
+
+ else if (*values > 2100)
+ r_page_servo_control_max[offset] = 2100;
+ else if (*values < 1800)
+ r_page_servo_control_max[offset] = 1800;
+ else
+ r_page_servo_control_max[offset] = *values;
+
+ offset++;
+ num_values--;
+ values++;
+ }
+ break;
+
+ case PX4IO_PAGE_IDLE_PWM:
+
+ /* copy channel data */
+ while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
+
+ if (*values == 0)
+ /* set to default */
+ r_page_servo_idle[offset] = 0;
+
+ else if (*values < 900)
+ r_page_servo_idle[offset] = 900;
+ else if (*values > 2100)
+ r_page_servo_idle[offset] = 2100;
+ else
+ r_page_servo_idle[offset] = *values;
+
+ /* flag the failsafe values as custom */
+ r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE;
+
+ offset++;
+ num_values--;
+ values++;
+ }
+ break;
+
/* handle text going to the mixer parser */
case PX4IO_PAGE_MIXERLOAD:
mixer_handle_text(values, num_values * sizeof(*values));
@@ -260,11 +368,13 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
/* iterate individual registers, set each in turn */
while (num_values--) {
if (registers_set_one(page, offset, *values))
- break;
+ return -1;
offset++;
values++;
}
+ break;
}
+ return 0;
}
static int
@@ -317,8 +427,14 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
* so that an in-air reset of FMU can not lead to a
* lockup of the IO arming state.
*/
- if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
- r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
+
+ // XXX do not reset IO's safety state by FMU for now
+ // if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
+ // r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
+ // }
+
+ if (value & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) {
+ r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
}
r_setup_arming = value;
@@ -349,10 +465,22 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
case PX4IO_P_SETUP_RELAYS:
value &= PX4IO_P_SETUP_RELAYS_VALID;
r_setup_relays = value;
- POWER_RELAY1(value & PX4IO_RELAY1 ? 1 : 0);
- POWER_RELAY2(value & PX4IO_RELAY2 ? 1 : 0);
- POWER_ACC1(value & PX4IO_ACC1 ? 1 : 0);
- POWER_ACC2(value & PX4IO_ACC2 ? 1 : 0);
+#ifdef POWER_RELAY1
+ POWER_RELAY1((value & PX4IO_P_SETUP_RELAYS_POWER1) ? 1 : 0);
+#endif
+#ifdef POWER_RELAY2
+ POWER_RELAY2((value & PX4IO_P_SETUP_RELAYS_POWER2) ? 1 : 0);
+#endif
+#ifdef POWER_ACC1
+ POWER_ACC1((value & PX4IO_P_SETUP_RELAYS_ACC1) ? 1 : 0);
+#endif
+#ifdef POWER_ACC2
+ POWER_ACC2((value & PX4IO_P_SETUP_RELAYS_ACC2) ? 1 : 0);
+#endif
+ break;
+
+ case PX4IO_P_SETUP_VBATT_SCALE:
+ r_page_setup[PX4IO_P_SETUP_VBATT_SCALE] = value;
break;
case PX4IO_P_SETUP_SET_DEBUG:
@@ -371,9 +499,12 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
case PX4IO_PAGE_RC_CONFIG: {
- /* do not allow a RC config change while fully armed */
- if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
- /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+ /**
+ * do not allow a RC config change while outputs armed
+ */
+ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
+ (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ||
+ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
break;
}
@@ -381,7 +512,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE;
uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE];
- if (channel >= MAX_CONTROL_CHANNELS)
+ if (channel >= PX4IO_CONTROL_CHANNELS)
return -1;
/* disable the channel until we have a chance to sanity-check it */
@@ -401,6 +532,9 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID;
r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
+ /* clear any existing RC disabled flag */
+ r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED);
+
/* set all options except the enabled option */
conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
@@ -426,7 +560,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) {
count++;
}
- if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) {
+ if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_CONTROL_CHANNELS) {
count++;
}
@@ -446,6 +580,14 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
/* case PX4IO_RC_PAGE_CONFIG */
}
+ case PX4IO_PAGE_TEST:
+ switch (offset) {
+ case PX4IO_P_TEST_LED:
+ LED_AMBER(value & 1);
+ break;
+ }
+ break;
+
default:
return -1;
}
@@ -482,6 +624,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
/* PX4IO_P_STATUS_ALARMS maintained externally */
+#ifdef ADC_VBATT
/* PX4IO_P_STATUS_VBATT */
{
/*
@@ -515,7 +658,8 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
r_page_status[PX4IO_P_STATUS_VBATT] = corrected;
}
}
-
+#endif
+#ifdef ADC_IBATT
/* PX4IO_P_STATUS_IBATT */
{
/*
@@ -525,26 +669,62 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
FMU sort it out, with user selectable
configuration for their sensor
*/
- unsigned counts = adc_measure(ADC_IN5);
+ unsigned counts = adc_measure(ADC_IBATT);
if (counts != 0xffff) {
r_page_status[PX4IO_P_STATUS_IBATT] = counts;
}
}
+#endif
+#ifdef ADC_VSERVO
+ /* PX4IO_P_STATUS_VSERVO */
+ {
+ /*
+ * Coefficients here derived by measurement of the 5-16V
+ * range on one unit:
+ *
+ * XXX pending measurements
+ *
+ * slope = xxx
+ * intercept = xxx
+ *
+ * Intercept corrected for best results @ 5.0V.
+ */
+ unsigned counts = adc_measure(ADC_VSERVO);
+ if (counts != 0xffff) {
+ unsigned mV = (4150 + (counts * 46)) / 10 - 200;
+ unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VSERVO_SCALE]) / 10000;
+
+ r_page_status[PX4IO_P_STATUS_VSERVO] = corrected;
+ }
+ }
+#endif
+ /* XXX PX4IO_P_STATUS_VRSSI */
+ /* XXX PX4IO_P_STATUS_PRSSI */
SELECT_PAGE(r_page_status);
break;
case PX4IO_PAGE_RAW_ADC_INPUT:
memset(r_page_scratch, 0, sizeof(r_page_scratch));
+#ifdef ADC_VBATT
r_page_scratch[0] = adc_measure(ADC_VBATT);
- r_page_scratch[1] = adc_measure(ADC_IN5);
-
+#endif
+#ifdef ADC_IBATT
+ r_page_scratch[1] = adc_measure(ADC_IBATT);
+#endif
+
+#ifdef ADC_VSERVO
+ r_page_scratch[0] = adc_measure(ADC_VSERVO);
+#endif
+#ifdef ADC_RSSI
+ r_page_scratch[1] = adc_measure(ADC_RSSI);
+#endif
SELECT_PAGE(r_page_scratch);
break;
case PX4IO_PAGE_PWM_INFO:
memset(r_page_scratch, 0, sizeof(r_page_scratch));
- for (unsigned i = 0; i < IO_SERVO_COUNT; i++)
+ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
r_page_scratch[PX4IO_RATE_MAP_BASE + i] = up_pwm_servo_get_rate_group(i);
SELECT_PAGE(r_page_scratch);
@@ -587,6 +767,15 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
case PX4IO_PAGE_FAILSAFE_PWM:
SELECT_PAGE(r_page_servo_failsafe);
break;
+ case PX4IO_PAGE_CONTROL_MIN_PWM:
+ SELECT_PAGE(r_page_servo_control_min);
+ break;
+ case PX4IO_PAGE_CONTROL_MAX_PWM:
+ SELECT_PAGE(r_page_servo_control_max);
+ break;
+ case PX4IO_PAGE_IDLE_PWM:
+ SELECT_PAGE(r_page_servo_idle);
+ break;
default:
return -1;
@@ -616,7 +805,7 @@ static void
pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate)
{
for (unsigned pass = 0; pass < 2; pass++) {
- for (unsigned group = 0; group < IO_SERVO_COUNT; group++) {
+ for (unsigned group = 0; group < PX4IO_SERVO_COUNT; group++) {
/* get the channel mask for this rate group */
uint32_t mask = up_pwm_servo_get_rate_group(group);
diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c
index 4dbecc27446390926efc9dfd6ed75b3a8ddc0205..95335f038a5dc23daf7679e0d628edb5bc5652db 100644
--- a/src/modules/px4iofirmware/safety.c
+++ b/src/modules/px4iofirmware/safety.c
@@ -110,7 +110,7 @@ safety_check_button(void *arg)
* state machine, keep ARM_COUNTER_THRESHOLD the same
* length in all cases of the if/else struct below.
*/
- if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
+ if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
(r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) {
if (counter < ARM_COUNTER_THRESHOLD) {
@@ -118,18 +118,18 @@ safety_check_button(void *arg)
} else if (counter == ARM_COUNTER_THRESHOLD) {
/* switch to armed state */
- r_status_flags |= PX4IO_P_STATUS_FLAGS_ARMED;
+ r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
counter++;
}
- } else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+ } else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
if (counter < ARM_COUNTER_THRESHOLD) {
counter++;
} else if (counter == ARM_COUNTER_THRESHOLD) {
/* change to disarmed state and notify the FMU */
- r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
+ r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
counter++;
}
@@ -140,7 +140,7 @@ safety_check_button(void *arg)
/* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */
uint16_t pattern = LED_PATTERN_FMU_REFUSE_TO_ARM;
- if (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) {
+ if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) {
pattern = LED_PATTERN_IO_FMU_ARMED;
diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c
new file mode 100644
index 0000000000000000000000000000000000000000..94d7407dff959dde76aed4f6fba0d72c9228c9e5
--- /dev/null
+++ b/src/modules/px4iofirmware/serial.c
@@ -0,0 +1,352 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012,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 serial.c
+ *
+ * Serial communication for the PX4IO module.
+ */
+
+#include <stdint.h>
+#include <unistd.h>
+#include <termios.h>
+#include <fcntl.h>
+#include <string.h>
+
+#include <nuttx/arch.h>
+#include <arch/board/board.h>
+
+/* XXX might be able to prune these */
+#include <chip.h>
+#include <up_internal.h>
+#include <up_arch.h>
+#include <stm32.h>
+#include <systemlib/perf_counter.h>
+
+//#define DEBUG
+#include "px4io.h"
+
+static perf_counter_t pc_txns;
+static perf_counter_t pc_errors;
+static perf_counter_t pc_ore;
+static perf_counter_t pc_fe;
+static perf_counter_t pc_ne;
+static perf_counter_t pc_idle;
+static perf_counter_t pc_badidle;
+static perf_counter_t pc_regerr;
+static perf_counter_t pc_crcerr;
+
+static void rx_handle_packet(void);
+static void rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg);
+static DMA_HANDLE tx_dma;
+static DMA_HANDLE rx_dma;
+
+static int serial_interrupt(int irq, void *context);
+static void dma_reset(void);
+
+/* if we spend this many ticks idle, reset the DMA */
+static unsigned idle_ticks;
+
+static struct IOPacket dma_packet;
+
+/* serial register accessors */
+#define REG(_x) (*(volatile uint32_t *)(PX4FMU_SERIAL_BASE + _x))
+#define rSR REG(STM32_USART_SR_OFFSET)
+#define rDR REG(STM32_USART_DR_OFFSET)
+#define rBRR REG(STM32_USART_BRR_OFFSET)
+#define rCR1 REG(STM32_USART_CR1_OFFSET)
+#define rCR2 REG(STM32_USART_CR2_OFFSET)
+#define rCR3 REG(STM32_USART_CR3_OFFSET)
+#define rGTPR REG(STM32_USART_GTPR_OFFSET)
+
+void
+interface_init(void)
+{
+ pc_txns = perf_alloc(PC_ELAPSED, "txns");
+ pc_errors = perf_alloc(PC_COUNT, "errors");
+ pc_ore = perf_alloc(PC_COUNT, "overrun");
+ pc_fe = perf_alloc(PC_COUNT, "framing");
+ pc_ne = perf_alloc(PC_COUNT, "noise");
+ pc_idle = perf_alloc(PC_COUNT, "idle");
+ pc_badidle = perf_alloc(PC_COUNT, "badidle");
+ pc_regerr = perf_alloc(PC_COUNT, "regerr");
+ pc_crcerr = perf_alloc(PC_COUNT, "crcerr");
+
+ /* allocate DMA */
+ tx_dma = stm32_dmachannel(PX4FMU_SERIAL_TX_DMA);
+ rx_dma = stm32_dmachannel(PX4FMU_SERIAL_RX_DMA);
+
+ /* configure pins for serial use */
+ stm32_configgpio(PX4FMU_SERIAL_TX_GPIO);
+ stm32_configgpio(PX4FMU_SERIAL_RX_GPIO);
+
+ /* reset and configure the UART */
+ rCR1 = 0;
+ rCR2 = 0;
+ rCR3 = 0;
+
+ /* clear status/errors */
+ (void)rSR;
+ (void)rDR;
+
+ /* configure line speed */
+ uint32_t usartdiv32 = PX4FMU_SERIAL_CLOCK / (PX4FMU_SERIAL_BITRATE / 2);
+ uint32_t mantissa = usartdiv32 >> 5;
+ uint32_t fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1;
+ rBRR = (mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT);
+
+ /* connect our interrupt */
+ irq_attach(PX4FMU_SERIAL_VECTOR, serial_interrupt);
+ up_enable_irq(PX4FMU_SERIAL_VECTOR);
+
+ /* enable UART and error/idle interrupts */
+ rCR3 = USART_CR3_EIE;
+ rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE;
+
+#if 0 /* keep this for signal integrity testing */
+ for (;;) {
+ while (!(rSR & USART_SR_TXE))
+ ;
+ rDR = 0xfa;
+ while (!(rSR & USART_SR_TXE))
+ ;
+ rDR = 0xa0;
+ }
+#endif
+
+ /* configure RX DMA and return to listening state */
+ dma_reset();
+
+ debug("serial init");
+}
+
+void
+interface_tick()
+{
+ /* XXX look for stuck/damaged DMA and reset? */
+ if (idle_ticks++ > 100) {
+ dma_reset();
+ idle_ticks = 0;
+ }
+}
+
+static void
+rx_handle_packet(void)
+{
+ /* check packet CRC */
+ uint8_t crc = dma_packet.crc;
+ dma_packet.crc = 0;
+ if (crc != crc_packet(&dma_packet)) {
+ perf_count(pc_crcerr);
+
+ /* send a CRC error reply */
+ dma_packet.count_code = PKT_CODE_CORRUPT;
+ dma_packet.page = 0xff;
+ dma_packet.offset = 0xff;
+
+ return;
+ }
+
+ if (PKT_CODE(dma_packet) == PKT_CODE_WRITE) {
+
+ /* it's a blind write - pass it on */
+ if (registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], PKT_COUNT(dma_packet))) {
+ perf_count(pc_regerr);
+ dma_packet.count_code = PKT_CODE_ERROR;
+ } else {
+ dma_packet.count_code = PKT_CODE_SUCCESS;
+ }
+ return;
+ }
+
+ if (PKT_CODE(dma_packet) == PKT_CODE_READ) {
+
+ /* it's a read - get register pointer for reply */
+ unsigned count;
+ uint16_t *registers;
+
+ if (registers_get(dma_packet.page, dma_packet.offset, ®isters, &count) < 0) {
+ perf_count(pc_regerr);
+ dma_packet.count_code = PKT_CODE_ERROR;
+ } else {
+ /* constrain reply to requested size */
+ if (count > PKT_MAX_REGS)
+ count = PKT_MAX_REGS;
+ if (count > PKT_COUNT(dma_packet))
+ count = PKT_COUNT(dma_packet);
+
+ /* copy reply registers into DMA buffer */
+ memcpy((void *)&dma_packet.regs[0], registers, count * 2);
+ dma_packet.count_code = count | PKT_CODE_SUCCESS;
+ }
+ return;
+ }
+
+ /* send a bad-packet error reply */
+ dma_packet.count_code = PKT_CODE_CORRUPT;
+ dma_packet.page = 0xff;
+ dma_packet.offset = 0xfe;
+}
+
+static void
+rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg)
+{
+ /*
+ * We are here because DMA completed, or UART reception stopped and
+ * we think we have a packet in the buffer.
+ */
+ perf_begin(pc_txns);
+
+ /* disable UART DMA */
+ rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
+
+ /* reset the idle counter */
+ idle_ticks = 0;
+
+ /* handle the received packet */
+ rx_handle_packet();
+
+ /* re-set DMA for reception first, so we are ready to receive before we start sending */
+ dma_reset();
+
+ /* send the reply to the just-processed request */
+ dma_packet.crc = 0;
+ dma_packet.crc = crc_packet(&dma_packet);
+ stm32_dmasetup(
+ tx_dma,
+ (uint32_t)&rDR,
+ (uint32_t)&dma_packet,
+ PKT_SIZE(dma_packet),
+ DMA_CCR_DIR |
+ DMA_CCR_MINC |
+ DMA_CCR_PSIZE_8BITS |
+ DMA_CCR_MSIZE_8BITS);
+ stm32_dmastart(tx_dma, NULL, NULL, false);
+ rCR3 |= USART_CR3_DMAT;
+
+ perf_end(pc_txns);
+}
+
+static int
+serial_interrupt(int irq, void *context)
+{
+ static bool abort_on_idle = false;
+
+ uint32_t sr = rSR; /* get UART status register */
+ (void)rDR; /* required to clear any of the interrupt status that brought us here */
+
+ if (sr & (USART_SR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */
+ USART_SR_NE | /* noise error - we have lost a byte due to noise */
+ USART_SR_FE)) { /* framing error - start/stop bit lost or line break */
+
+ perf_count(pc_errors);
+ if (sr & USART_SR_ORE)
+ perf_count(pc_ore);
+ if (sr & USART_SR_NE)
+ perf_count(pc_ne);
+ if (sr & USART_SR_FE)
+ perf_count(pc_fe);
+
+ /* send a line break - this will abort transmission/reception on the other end */
+ rCR1 |= USART_CR1_SBK;
+
+ /* when the line goes idle, abort rather than look at the packet */
+ abort_on_idle = true;
+ }
+
+ if (sr & USART_SR_IDLE) {
+
+ /*
+ * If we saw an error, don't bother looking at the packet - it should have
+ * been aborted by the sender and will definitely be bad. Get the DMA reconfigured
+ * ready for their retry.
+ */
+ if (abort_on_idle) {
+
+ abort_on_idle = false;
+ dma_reset();
+ return 0;
+ }
+
+ /*
+ * The sender has stopped sending - this is probably the end of a packet.
+ * Check the received length against the length in the header to see if
+ * we have something that looks like a packet.
+ */
+ unsigned length = sizeof(dma_packet) - stm32_dmaresidual(rx_dma);
+ if ((length < 1) || (length < PKT_SIZE(dma_packet))) {
+
+ /* it was too short - possibly truncated */
+ perf_count(pc_badidle);
+ return 0;
+ }
+
+ /*
+ * Looks like we received a packet. Stop the DMA and go process the
+ * packet.
+ */
+ perf_count(pc_idle);
+ stm32_dmastop(rx_dma);
+ rx_dma_callback(rx_dma, DMA_STATUS_TCIF, NULL);
+ }
+
+ return 0;
+}
+
+static void
+dma_reset(void)
+{
+ rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
+ (void)rSR;
+ (void)rDR;
+ (void)rDR;
+
+ /* kill any pending DMA */
+ stm32_dmastop(tx_dma);
+ stm32_dmastop(rx_dma);
+
+ /* reset the RX side */
+ stm32_dmasetup(
+ rx_dma,
+ (uint32_t)&rDR,
+ (uint32_t)&dma_packet,
+ sizeof(dma_packet),
+ DMA_CCR_MINC |
+ DMA_CCR_PSIZE_8BITS |
+ DMA_CCR_MSIZE_8BITS);
+
+ /* start receive DMA ready for the next packet */
+ stm32_dmastart(rx_dma, rx_dma_callback, NULL, false);
+ rCR3 |= USART_CR3_DMAR;
+}
+
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index ba7cdd91cf39cc40ed00c058f7664042170f696d..2b21bb5a5a704c7375d916ff53f33a056ba6cbc1 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -75,6 +75,7 @@
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_vicon_position.h>
+#include <uORB/topics/vehicle_global_velocity_setpoint.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/differential_pressure.h>
@@ -94,7 +95,6 @@
log_msgs_written++; \
} else { \
log_msgs_skipped++; \
- /*printf("skip\n");*/ \
}
#define LOG_ORB_SUBSCRIBE(_var, _topic) subs.##_var##_sub = orb_subscribe(ORB_ID(##_topic##)); \
@@ -102,9 +102,6 @@
fds[fdsc_count].events = POLLIN; \
fdsc_count++;
-
-//#define SDLOG2_DEBUG
-
static bool main_thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
@@ -233,7 +230,7 @@ int sdlog2_main(int argc, char *argv[])
if (!strcmp(argv[1], "start")) {
if (thread_running) {
- printf("sdlog2 already running\n");
+ warnx("already running");
/* this is not an error */
exit(0);
}
@@ -250,7 +247,7 @@ int sdlog2_main(int argc, char *argv[])
if (!strcmp(argv[1], "stop")) {
if (!thread_running) {
- printf("\tsdlog2 is not started\n");
+ warnx("not started");
}
main_thread_should_exit = true;
@@ -262,7 +259,7 @@ int sdlog2_main(int argc, char *argv[])
sdlog2_status();
} else {
- printf("\tsdlog2 not started\n");
+ warnx("not started\n");
}
exit(0);
@@ -387,11 +384,6 @@ static void *logwriter_thread(void *arg)
/* only get pointer to thread-safe data, do heavy I/O a few lines down */
int available = logbuffer_get_ptr(logbuf, &read_ptr, &is_part);
-#ifdef SDLOG2_DEBUG
- int rp = logbuf->read_ptr;
- int wp = logbuf->write_ptr;
-#endif
-
/* continue */
pthread_mutex_unlock(&logbuffer_mutex);
@@ -407,9 +399,6 @@ static void *logwriter_thread(void *arg)
n = write(log_file, read_ptr, n);
should_wait = (n == available) && !is_part;
-#ifdef SDLOG2_DEBUG
- printf("write %i %i of %i rp=%i wp=%i, is_part=%i, should_wait=%i\n", log_bytes_written, n, available, rp, wp, (int)is_part, (int)should_wait);
-#endif
if (n < 0) {
main_thread_should_exit = true;
@@ -422,14 +411,8 @@ static void *logwriter_thread(void *arg)
} else {
n = 0;
-#ifdef SDLOG2_DEBUG
- printf("no data available, main_thread_should_exit=%i, logwriter_should_exit=%i\n", (int)main_thread_should_exit, (int)logwriter_should_exit);
-#endif
/* exit only with empty buffer */
if (main_thread_should_exit || logwriter_should_exit) {
-#ifdef SDLOG2_DEBUG
- printf("break logwriter thread\n");
-#endif
break;
}
should_wait = true;
@@ -444,10 +427,6 @@ static void *logwriter_thread(void *arg)
fsync(log_file);
close(log_file);
-#ifdef SDLOG2_DEBUG
- printf("logwriter thread exit\n");
-#endif
-
return OK;
}
@@ -604,15 +583,6 @@ int sdlog2_thread_main(int argc, char *argv[])
errx(1, "unable to create logging folder, exiting.");
}
- const char *converter_in = "/etc/logging/conv.zip";
- char* converter_out = malloc(150);
- sprintf(converter_out, "%s/conv.zip", folder_path);
-
- if (file_copy(converter_in, converter_out)) {
- errx(1, "unable to copy conversion scripts, exiting.");
- }
- free(converter_out);
-
/* only print logging path, important to find log file later */
warnx("logging to directory: %s", folder_path);
@@ -623,14 +593,6 @@ int sdlog2_thread_main(int argc, char *argv[])
errx(1, "can't allocate log buffer, exiting.");
}
- /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
- /* number of messages */
- const ssize_t fdsc = 19;
- /* Sanity check variable and index */
- ssize_t fdsc_count = 0;
- /* file descriptors to wait for */
- struct pollfd fds[fdsc];
-
struct vehicle_status_s buf_status;
memset(&buf_status, 0, sizeof(buf_status));
@@ -655,6 +617,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct differential_pressure_s diff_pres;
struct airspeed_s airspeed;
struct esc_status_s esc;
+ struct vehicle_global_velocity_setpoint_s global_vel_sp;
} buf;
memset(&buf, 0, sizeof(buf));
@@ -678,6 +641,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int rc_sub;
int airspeed_sub;
int esc_sub;
+ int global_vel_sp_sub;
} subs;
/* log message buffer: header + body */
@@ -703,6 +667,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_GPOS_s log_GPOS;
struct log_GPSP_s log_GPSP;
struct log_ESC_s log_ESC;
+ struct log_GVSP_s log_GVSP;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
@@ -710,6 +675,14 @@ int sdlog2_thread_main(int argc, char *argv[])
#pragma pack(pop)
memset(&log_msg.body, 0, sizeof(log_msg.body));
+ /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
+ /* number of messages */
+ const ssize_t fdsc = 20;
+ /* Sanity check variable and index */
+ ssize_t fdsc_count = 0;
+ /* file descriptors to wait for */
+ struct pollfd fds[fdsc];
+
/* --- VEHICLE COMMAND --- */
subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
fds[fdsc_count].fd = subs.cmd_sub;
@@ -824,6 +797,12 @@ int sdlog2_thread_main(int argc, char *argv[])
fds[fdsc_count].events = POLLIN;
fdsc_count++;
+ /* --- GLOBAL VELOCITY SETPOINT --- */
+ subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));
+ fds[fdsc_count].fd = subs.global_vel_sp_sub;
+ fds[fdsc_count].events = POLLIN;
+ fdsc_count++;
+
/* WARNING: If you get the error message below,
* then the number of registered messages (fdsc)
* differs from the number of messages in the above list.
@@ -911,13 +890,11 @@ int sdlog2_thread_main(int argc, char *argv[])
if (fds[ifds++].revents & POLLIN) {
// Don't orb_copy, it's already done few lines above
log_msg.msg_type = LOG_STAT_MSG;
- log_msg.body.log_STAT.state = (unsigned char) buf_status.state_machine;
- log_msg.body.log_STAT.flight_mode = (unsigned char) buf_status.flight_mode;
- log_msg.body.log_STAT.manual_control_mode = (unsigned char) buf_status.manual_control_mode;
- log_msg.body.log_STAT.manual_sas_mode = (unsigned char) buf_status.manual_sas_mode;
- log_msg.body.log_STAT.armed = (unsigned char) buf_status.flag_system_armed;
- log_msg.body.log_STAT.battery_voltage = buf_status.voltage_battery;
- log_msg.body.log_STAT.battery_current = buf_status.current_battery;
+ log_msg.body.log_STAT.main_state = (unsigned char) buf_status.main_state;
+ log_msg.body.log_STAT.navigation_state = (unsigned char) buf_status.navigation_state;
+ log_msg.body.log_STAT.arming_state = (unsigned char) buf_status.arming_state;
+ log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage;
+ log_msg.body.log_STAT.battery_current = buf_status.battery_current;
log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
log_msg.body.log_STAT.battery_warning = (unsigned char) buf_status.battery_warning;
LOGBUFFER_WRITE_AND_COUNT(STAT);
@@ -1065,10 +1042,9 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_LPOS.vx = buf.local_pos.vx;
log_msg.body.log_LPOS.vy = buf.local_pos.vy;
log_msg.body.log_LPOS.vz = buf.local_pos.vz;
- log_msg.body.log_LPOS.hdg = buf.local_pos.hdg;
- log_msg.body.log_LPOS.home_lat = buf.local_pos.home_lat;
- log_msg.body.log_LPOS.home_lon = buf.local_pos.home_lon;
- log_msg.body.log_LPOS.home_alt = buf.local_pos.home_alt;
+ log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat;
+ log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon;
+ log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt;
LOGBUFFER_WRITE_AND_COUNT(LPOS);
}
@@ -1175,14 +1151,18 @@ int sdlog2_thread_main(int argc, char *argv[])
}
}
-#ifdef SDLOG2_DEBUG
- printf("fill rp=%i wp=%i count=%i\n", lb.read_ptr, lb.write_ptr, logbuffer_count(&lb));
-#endif
+ /* --- GLOBAL VELOCITY SETPOINT --- */
+ if (fds[ifds++].revents & POLLIN) {
+ orb_copy(ORB_ID(vehicle_global_velocity_setpoint), subs.global_vel_sp_sub, &buf.global_vel_sp);
+ log_msg.msg_type = LOG_GVSP_MSG;
+ log_msg.body.log_GVSP.vx = buf.global_vel_sp.vx;
+ log_msg.body.log_GVSP.vy = buf.global_vel_sp.vy;
+ log_msg.body.log_GVSP.vz = buf.global_vel_sp.vz;
+ LOGBUFFER_WRITE_AND_COUNT(GVSP);
+ }
+
/* signal the other thread new data, but not yet unlock */
if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
-#ifdef SDLOG2_DEBUG
- printf("signal rp=%i wp=%i count=%i\n", lb.read_ptr, lb.write_ptr, logbuffer_count(&lb));
-#endif
/* only request write if several packets can be written at once */
pthread_cond_signal(&logbuffer_cond);
}
@@ -1202,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;
@@ -1265,7 +1247,7 @@ int file_copy(const char *file_old, const char *file_new)
fclose(source);
fclose(target);
- return OK;
+ return ret;
}
void handle_command(struct vehicle_command_s *cmd)
@@ -1297,8 +1279,10 @@ void handle_command(struct vehicle_command_s *cmd)
void handle_status(struct vehicle_status_s *status)
{
- if (status->flag_system_armed != flag_system_armed) {
- flag_system_armed = status->flag_system_armed;
+ // TODO use flag from actuator_armed here?
+ bool armed = status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR;
+ if (armed != flag_system_armed) {
+ flag_system_armed = armed;
if (flag_system_armed) {
sdlog2_start_log();
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 934e4dec89a91f46938ebf6dd33431ccf1a5b4ef..d99892fe2eac90219660c55163ebe639ac198db8 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -106,10 +106,9 @@ struct log_LPOS_s {
float vx;
float vy;
float vz;
- float hdg;
- int32_t home_lat;
- int32_t home_lon;
- float home_alt;
+ int32_t ref_lat;
+ int32_t ref_lon;
+ float ref_alt;
};
/* --- LPSP - LOCAL POSITION SETPOINT --- */
@@ -149,11 +148,9 @@ struct log_ATTC_s {
/* --- STAT - VEHICLE STATE --- */
#define LOG_STAT_MSG 10
struct log_STAT_s {
- uint8_t state;
- uint8_t flight_mode;
- uint8_t manual_control_mode;
- uint8_t manual_sas_mode;
- uint8_t armed;
+ uint8_t main_state;
+ uint8_t navigation_state;
+ uint8_t arming_state;
float battery_voltage;
float battery_current;
float battery_remaining;
@@ -244,6 +241,14 @@ struct log_ESC_s {
uint16_t esc_setpoint_raw;
};
+/* --- GVSP - GLOBAL VELOCITY SETPOINT --- */
+#define LOG_GVSP_MSG 19
+struct log_GVSP_s {
+ float vx;
+ float vy;
+ float vz;
+};
+
#pragma pack(pop)
/* construct list of all message formats */
@@ -254,11 +259,11 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"),
- LOG_FORMAT(LPOS, "fffffffLLf", "X,Y,Z,VX,VY,VZ,Heading,HomeLat,HomeLon,HomeAlt"),
+ LOG_FORMAT(LPOS, "ffffffLLf", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt"),
LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),
LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
- LOG_FORMAT(STAT, "BBBBBfffB", "State,FlightMode,CtlMode,SASMode,Armed,BatV,BatC,BatRem,BatWarn"),
+ LOG_FORMAT(STAT, "BBBfffB", "MainState,NavState,ArmState,BatV,BatC,BatRem,BatWarn"),
LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"),
LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
@@ -266,7 +271,8 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"),
LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"),
- LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,No,Version,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
+ LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
+ LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
};
static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index bd431c9ebd878be14b8f7f0a3ae05fbaef6ab24e..fd2a6318ebddb54a17880622c5c9184909451071 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -68,7 +68,10 @@ PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f);
-PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 1667);
+PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0);
+
+PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
+PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0);
PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
@@ -154,34 +157,33 @@ 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 */
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
+#else
/* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */
PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f));
+#endif
PARAM_DEFINE_INT32(RC_MAP_ROLL, 1);
PARAM_DEFINE_INT32(RC_MAP_PITCH, 2);
PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3);
PARAM_DEFINE_INT32(RC_MAP_YAW, 4);
-PARAM_DEFINE_INT32(RC_MAP_OVER_SW, 5);
-PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 6);
+PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5);
+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_MAN_SW, 0);
-PARAM_DEFINE_INT32(RC_MAP_SAS_SW, 0);
-PARAM_DEFINE_INT32(RC_MAP_RTL_SW, 0);
-PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
+//PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
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);
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 42268b97158284773c0d837fbd367a443bcf749a..e857b1c2f773bc9aac101b42a16d903d54b1d86d 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -50,6 +50,7 @@
#include <stdio.h>
#include <errno.h>
#include <math.h>
+#include <mathlib/mathlib.h>
#include <nuttx/analog/adc.h>
@@ -74,7 +75,7 @@
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/differential_pressure.h>
@@ -137,6 +138,77 @@
#define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg)
+/**
+ * Enum for board and external compass rotations.
+ * This enum maps from board attitude to airframe attitude.
+ */
+enum Rotation {
+ ROTATION_NONE = 0,
+ ROTATION_YAW_45 = 1,
+ ROTATION_YAW_90 = 2,
+ ROTATION_YAW_135 = 3,
+ ROTATION_YAW_180 = 4,
+ ROTATION_YAW_225 = 5,
+ ROTATION_YAW_270 = 6,
+ ROTATION_YAW_315 = 7,
+ ROTATION_ROLL_180 = 8,
+ ROTATION_ROLL_180_YAW_45 = 9,
+ ROTATION_ROLL_180_YAW_90 = 10,
+ ROTATION_ROLL_180_YAW_135 = 11,
+ ROTATION_PITCH_180 = 12,
+ ROTATION_ROLL_180_YAW_225 = 13,
+ ROTATION_ROLL_180_YAW_270 = 14,
+ ROTATION_ROLL_180_YAW_315 = 15,
+ ROTATION_ROLL_90 = 16,
+ ROTATION_ROLL_90_YAW_45 = 17,
+ ROTATION_ROLL_90_YAW_90 = 18,
+ ROTATION_ROLL_90_YAW_135 = 19,
+ ROTATION_ROLL_270 = 20,
+ ROTATION_ROLL_270_YAW_45 = 21,
+ ROTATION_ROLL_270_YAW_90 = 22,
+ ROTATION_ROLL_270_YAW_135 = 23,
+ ROTATION_PITCH_90 = 24,
+ ROTATION_PITCH_270 = 25,
+ ROTATION_MAX
+};
+
+typedef struct
+{
+ uint16_t roll;
+ uint16_t pitch;
+ uint16_t yaw;
+} rot_lookup_t;
+
+const rot_lookup_t rot_lookup[] =
+{
+ { 0, 0, 0 },
+ { 0, 0, 45 },
+ { 0, 0, 90 },
+ { 0, 0, 135 },
+ { 0, 0, 180 },
+ { 0, 0, 225 },
+ { 0, 0, 270 },
+ { 0, 0, 315 },
+ {180, 0, 0 },
+ {180, 0, 45 },
+ {180, 0, 90 },
+ {180, 0, 135 },
+ { 0, 180, 0 },
+ {180, 0, 225 },
+ {180, 0, 270 },
+ {180, 0, 315 },
+ { 90, 0, 0 },
+ { 90, 0, 45 },
+ { 90, 0, 90 },
+ { 90, 0, 135 },
+ {270, 0, 0 },
+ {270, 0, 45 },
+ {270, 0, 90 },
+ {270, 0, 135 },
+ { 0, 90, 0 },
+ { 0, 270, 0 }
+};
+
/**
* Sensor app start / stop handling function
*
@@ -189,9 +261,9 @@ private:
int _mag_sub; /**< raw mag data subscription */
int _rc_sub; /**< raw rc channels data subscription */
int _baro_sub; /**< raw baro data subscription */
- int _airspeed_sub; /**< airspeed subscription */
- int _diff_pres_sub; /**< raw differential pressure subscription */
- int _vstatus_sub; /**< vehicle status subscription */
+ int _airspeed_sub; /**< airspeed subscription */
+ int _diff_pres_sub; /**< raw differential pressure subscription */
+ int _vcontrol_mode_sub; /**< vehicle control mode subscription */
int _params_sub; /**< notification of parameter updates */
int _manual_control_sub; /**< notification of manual control updates */
@@ -210,13 +282,16 @@ private:
struct differential_pressure_s _diff_pres;
struct airspeed_s _airspeed;
+ math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
+ math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */
+ bool _mag_is_external; /**< true if the active mag is on an external board */
+
struct {
float min[_rc_max_chan_count];
float trim[_rc_max_chan_count];
float max[_rc_max_chan_count];
float rev[_rc_max_chan_count];
float dz[_rc_max_chan_count];
- // float ex[_rc_max_chan_count];
float scaling_factor[_rc_max_chan_count];
float gyro_offset[3];
@@ -227,20 +302,20 @@ private:
float accel_scale[3];
float diff_pres_offset_pa;
- int rc_type;
+ int board_rotation;
+ int external_mag_rotation;
int rc_map_roll;
int rc_map_pitch;
int rc_map_yaw;
int rc_map_throttle;
- int rc_map_manual_override_sw;
- int rc_map_auto_mode_sw;
+ int rc_map_mode_sw;
+ int rc_map_return_sw;
+ int rc_map_assisted_sw;
+ int rc_map_mission_sw;
- int rc_map_manual_mode_sw;
- int rc_map_sas_mode_sw;
- int rc_map_rtl_sw;
- int rc_map_offboard_ctrl_mode_sw;
+// int rc_map_offboard_ctrl_mode_sw;
int rc_map_flaps;
@@ -265,10 +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 ex[_rc_max_chan_count];
- param_t rc_type;
-
- param_t rc_demix;
param_t gyro_offset[3];
param_t gyro_scale[3];
@@ -283,13 +354,12 @@ private:
param_t rc_map_yaw;
param_t rc_map_throttle;
- param_t rc_map_manual_override_sw;
- param_t rc_map_auto_mode_sw;
+ param_t rc_map_mode_sw;
+ param_t rc_map_return_sw;
+ param_t rc_map_assisted_sw;
+ param_t rc_map_mission_sw;
- param_t rc_map_manual_mode_sw;
- param_t rc_map_sas_mode_sw;
- param_t rc_map_rtl_sw;
- param_t rc_map_offboard_ctrl_mode_sw;
+// param_t rc_map_offboard_ctrl_mode_sw;
param_t rc_map_flaps;
@@ -306,6 +376,9 @@ private:
param_t battery_voltage_scaling;
+ param_t board_rotation;
+ param_t external_mag_rotation;
+
} _parameter_handles; /**< handles for interesting parameters */
@@ -314,6 +387,11 @@ private:
*/
int parameters_update();
+ /**
+ * Get the rotation matrices
+ */
+ void get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix);
+
/**
* Do accel-related initialisation.
*/
@@ -380,9 +458,9 @@ private:
void diff_pres_poll(struct sensor_combined_s &raw);
/**
- * Check for changes in vehicle status.
+ * Check for changes in vehicle control mode.
*/
- void vehicle_status_poll();
+ void vehicle_control_mode_poll();
/**
* Check for changes in parameters.
@@ -437,7 +515,7 @@ Sensors::Sensors() :
_mag_sub(-1),
_rc_sub(-1),
_baro_sub(-1),
- _vstatus_sub(-1),
+ _vcontrol_mode_sub(-1),
_params_sub(-1),
_manual_control_sub(-1),
@@ -450,7 +528,11 @@ Sensors::Sensors() :
_diff_pres_pub(-1),
/* performance counters */
- _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update"))
+ _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
+
+ _board_rotation(3,3),
+ _external_mag_rotation(3,3),
+ _mag_is_external(false)
{
/* basic r/c parameters */
@@ -479,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");
@@ -488,16 +568,16 @@ Sensors::Sensors() :
_parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE");
/* mandatory mode switches, mapped to channel 5 and 6 per default */
- _parameter_handles.rc_map_manual_override_sw = param_find("RC_MAP_OVER_SW");
- _parameter_handles.rc_map_auto_mode_sw = param_find("RC_MAP_MODE_SW");
+ _parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW");
+ _parameter_handles.rc_map_return_sw = param_find("RC_MAP_RETURN_SW");
_parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS");
/* optional mode switches, not mapped per default */
- _parameter_handles.rc_map_manual_mode_sw = param_find("RC_MAP_MAN_SW");
- _parameter_handles.rc_map_sas_mode_sw = param_find("RC_MAP_SAS_SW");
- _parameter_handles.rc_map_rtl_sw = param_find("RC_MAP_RTL_SW");
- _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW");
+ _parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW");
+ _parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW");
+
+// _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW");
_parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1");
_parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2");
@@ -540,6 +620,10 @@ Sensors::Sensors() :
_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
+ /* rotations */
+ _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
+ _parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT");
+
/* fetch initial parameter values */
parameters_update();
}
@@ -601,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");
@@ -623,54 +702,35 @@ Sensors::parameters_update()
warnx("Failed getting throttle chan index");
}
- if (param_get(_parameter_handles.rc_map_manual_override_sw, &(_parameters.rc_map_manual_override_sw)) != OK) {
- warnx("Failed getting override sw chan index");
- }
-
- if (param_get(_parameter_handles.rc_map_auto_mode_sw, &(_parameters.rc_map_auto_mode_sw)) != OK) {
- warnx("Failed getting auto mode sw chan index");
- }
-
- if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
- warnx("Failed getting flaps chan index");
- }
-
- if (param_get(_parameter_handles.rc_map_manual_mode_sw, &(_parameters.rc_map_manual_mode_sw)) != OK) {
- warnx("Failed getting manual mode sw chan index");
- }
-
- if (param_get(_parameter_handles.rc_map_rtl_sw, &(_parameters.rc_map_rtl_sw)) != OK) {
- warnx("Failed getting rtl sw chan index");
- }
-
- if (param_get(_parameter_handles.rc_map_sas_mode_sw, &(_parameters.rc_map_sas_mode_sw)) != OK) {
- warnx("Failed getting sas mode sw chan index");
+ if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) {
+ warnx("Failed getting mode sw chan index");
}
- if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) {
- warnx("Failed getting offboard control mode sw chan index");
+ if (param_get(_parameter_handles.rc_map_return_sw, &(_parameters.rc_map_return_sw)) != OK) {
+ warnx("Failed getting return 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_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) {
+ warnx("Failed getting assisted sw chan 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_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) {
+ warnx("Failed getting mission sw chan 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_flaps, &(_parameters.rc_map_flaps)) != OK) {
+ warnx("Failed getting flaps chan 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");
- }
+// if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) {
+// warnx("Failed getting offboard control mode sw chan 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));
@@ -682,15 +742,14 @@ Sensors::parameters_update()
_rc.function[PITCH] = _parameters.rc_map_pitch - 1;
_rc.function[YAW] = _parameters.rc_map_yaw - 1;
- _rc.function[OVERRIDE] = _parameters.rc_map_manual_override_sw - 1;
- _rc.function[AUTO_MODE] = _parameters.rc_map_auto_mode_sw - 1;
+ _rc.function[MODE] = _parameters.rc_map_mode_sw - 1;
+ _rc.function[RETURN] = _parameters.rc_map_return_sw - 1;
+ _rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1;
+ _rc.function[MISSION] = _parameters.rc_map_mission_sw - 1;
_rc.function[FLAPS] = _parameters.rc_map_flaps - 1;
- _rc.function[MANUAL_MODE] = _parameters.rc_map_manual_mode_sw - 1;
- _rc.function[RTL] = _parameters.rc_map_rtl_sw - 1;
- _rc.function[SAS_MODE] = _parameters.rc_map_sas_mode_sw - 1;
- _rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_ctrl_mode_sw - 1;
+// _rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_ctrl_mode_sw - 1;
_rc.function[AUX_1] = _parameters.rc_map_aux1 - 1;
_rc.function[AUX_2] = _parameters.rc_map_aux2 - 1;
@@ -731,9 +790,33 @@ Sensors::parameters_update()
warnx("Failed updating voltage scaling param");
}
+ param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation));
+ param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation));
+
+ get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
+ get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation);
+
return OK;
}
+void
+Sensors::get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix)
+{
+ /* first set to zero */
+ rot_matrix->Matrix::zero(3,3);
+
+ float roll = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll;
+ float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch;
+ float yaw = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw;
+
+ math::EulerAngles euler(roll, pitch, yaw);
+
+ math::Dcm R(euler);
+
+ for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
+ (*rot_matrix)(i,j) = R(i, j);
+}
+
void
Sensors::accel_init()
{
@@ -757,7 +840,7 @@ Sensors::accel_init()
/* set the driver to poll at 1000Hz */
ioctl(fd, SENSORIOCSPOLLRATE, 1000);
- #else
+ #elif CONFIG_ARCH_BOARD_PX4FMU_V2
/* set the accel internal sampling rate up to at leat 800Hz */
ioctl(fd, ACCELIOCSSAMPLERATE, 800);
@@ -765,6 +848,9 @@ Sensors::accel_init()
/* set the driver to poll at 800Hz */
ioctl(fd, SENSORIOCSPOLLRATE, 800);
+ #else
+ #error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2
+
#endif
warnx("using system accel");
@@ -799,11 +885,11 @@ Sensors::gyro_init()
#else
- /* set the gyro internal sampling rate up to at leat 800Hz */
- ioctl(fd, GYROIOCSSAMPLERATE, 800);
+ /* set the gyro internal sampling rate up to at least 760Hz */
+ ioctl(fd, GYROIOCSSAMPLERATE, 760);
- /* set the driver to poll at 800Hz */
- ioctl(fd, SENSORIOCSPOLLRATE, 800);
+ /* set the driver to poll at 760Hz */
+ ioctl(fd, SENSORIOCSPOLLRATE, 760);
#endif
@@ -816,6 +902,7 @@ void
Sensors::mag_init()
{
int fd;
+ int ret;
fd = open(MAG_DEVICE_PATH, 0);
@@ -824,11 +911,33 @@ Sensors::mag_init()
errx(1, "FATAL: no magnetometer found");
}
- /* set the mag internal poll rate to at least 150Hz */
- ioctl(fd, MAGIOCSSAMPLERATE, 150);
+ /* try different mag sampling rates */
- /* set the driver to poll at 150Hz */
- ioctl(fd, SENSORIOCSPOLLRATE, 150);
+
+ ret = ioctl(fd, MAGIOCSSAMPLERATE, 150);
+ if (ret == OK) {
+ /* set the pollrate accordingly */
+ ioctl(fd, SENSORIOCSPOLLRATE, 150);
+ } else {
+ ret = ioctl(fd, MAGIOCSSAMPLERATE, 100);
+ /* if the slower sampling rate still fails, something is wrong */
+ if (ret == OK) {
+ /* set the driver to poll also at the slower rate */
+ ioctl(fd, SENSORIOCSPOLLRATE, 100);
+ } else {
+ errx(1, "FATAL: mag sampling rate could not be set");
+ }
+ }
+
+
+
+ ret = ioctl(fd, MAGIOCGEXTERNAL, 0);
+ if (ret < 0)
+ errx(1, "FATAL: unknown if magnetometer is external or onboard");
+ else if (ret == 1)
+ _mag_is_external = true;
+ else
+ _mag_is_external = false;
close(fd);
}
@@ -842,7 +951,7 @@ Sensors::baro_init()
if (fd < 0) {
warn("%s", BARO_DEVICE_PATH);
- warnx("No barometer found, ignoring");
+ errx(1, "FATAL: No barometer found");
}
/* set the driver to poll at 150Hz */
@@ -874,9 +983,12 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
- raw.accelerometer_m_s2[0] = accel_report.x;
- raw.accelerometer_m_s2[1] = accel_report.y;
- raw.accelerometer_m_s2[2] = accel_report.z;
+ math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z};
+ vect = _board_rotation*vect;
+
+ raw.accelerometer_m_s2[0] = vect(0);
+ raw.accelerometer_m_s2[1] = vect(1);
+ raw.accelerometer_m_s2[2] = vect(2);
raw.accelerometer_raw[0] = accel_report.x_raw;
raw.accelerometer_raw[1] = accel_report.y_raw;
@@ -897,9 +1009,12 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
- raw.gyro_rad_s[0] = gyro_report.x;
- raw.gyro_rad_s[1] = gyro_report.y;
- raw.gyro_rad_s[2] = gyro_report.z;
+ math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z};
+ vect = _board_rotation*vect;
+
+ raw.gyro_rad_s[0] = vect(0);
+ raw.gyro_rad_s[1] = vect(1);
+ raw.gyro_rad_s[2] = vect(2);
raw.gyro_raw[0] = gyro_report.x_raw;
raw.gyro_raw[1] = gyro_report.y_raw;
@@ -920,9 +1035,16 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
- raw.magnetometer_ga[0] = mag_report.x;
- raw.magnetometer_ga[1] = mag_report.y;
- raw.magnetometer_ga[2] = mag_report.z;
+ math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z};
+
+ if (_mag_is_external)
+ vect = _external_mag_rotation*vect;
+ else
+ vect = _board_rotation*vect;
+
+ raw.magnetometer_ga[0] = vect(0);
+ raw.magnetometer_ga[1] = vect(1);
+ raw.magnetometer_ga[2] = vect(2);
raw.magnetometer_raw[0] = mag_report.x_raw;
raw.magnetometer_raw[1] = mag_report.y_raw;
@@ -977,21 +1099,21 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
}
void
-Sensors::vehicle_status_poll()
+Sensors::vehicle_control_mode_poll()
{
- struct vehicle_status_s vstatus;
- bool vstatus_updated;
+ struct vehicle_control_mode_s vcontrol_mode;
+ bool vcontrol_mode_updated;
- /* Check HIL state if vehicle status has changed */
- orb_check(_vstatus_sub, &vstatus_updated);
+ /* Check HIL state if vehicle control mode has changed */
+ orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated);
- if (vstatus_updated) {
+ if (vcontrol_mode_updated) {
- orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &vstatus);
+ orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode);
/* switching from non-HIL to HIL mode */
//printf("[sensors] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled);
- if (vstatus.flag_hil_enabled && !_hil_enabled) {
+ if (vcontrol_mode.flag_system_hil_enabled && !_hil_enabled) {
_hil_enabled = true;
_publishing = false;
@@ -1194,10 +1316,11 @@ Sensors::ppm_poll()
manual_control.yaw = NAN;
manual_control.throttle = NAN;
- manual_control.manual_mode_switch = NAN;
- manual_control.manual_sas_switch = NAN;
- manual_control.return_to_launch_switch = NAN;
- manual_control.auto_offboard_input_switch = NAN;
+ manual_control.mode_switch = NAN;
+ manual_control.return_switch = NAN;
+ manual_control.assisted_switch = NAN;
+ manual_control.mission_switch = NAN;
+// manual_control.auto_offboard_input_switch = NAN;
manual_control.flaps = NAN;
manual_control.aux1 = NAN;
@@ -1297,11 +1420,17 @@ Sensors::ppm_poll()
manual_control.yaw *= _parameters.rc_scale_yaw;
}
- /* override switch input */
- manual_control.manual_override_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OVERRIDE]].scaled);
-
/* mode switch input */
- manual_control.auto_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[AUTO_MODE]].scaled);
+ manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled);
+
+ /* land switch input */
+ manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled);
+
+ /* assisted switch input */
+ manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled);
+
+ /* mission switch input */
+ manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled);
/* flaps */
if (_rc.function[FLAPS] >= 0) {
@@ -1313,21 +1442,17 @@ Sensors::ppm_poll()
}
}
- if (_rc.function[MANUAL_MODE] >= 0) {
- manual_control.manual_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MANUAL_MODE]].scaled);
- }
-
- if (_rc.function[SAS_MODE] >= 0) {
- manual_control.manual_sas_switch = limit_minus_one_to_one(_rc.chan[_rc.function[SAS_MODE]].scaled);
+ if (_rc.function[MODE] >= 0) {
+ manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled);
}
- if (_rc.function[RTL] >= 0) {
- manual_control.return_to_launch_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RTL]].scaled);
+ if (_rc.function[MISSION] >= 0) {
+ manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled);
}
- if (_rc.function[OFFBOARD_MODE] >= 0) {
- manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled);
- }
+// if (_rc.function[OFFBOARD_MODE] >= 0) {
+// manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled);
+// }
/* aux functions, only assign if valid mapping is present */
if (_rc.function[AUX_1] >= 0) {
@@ -1400,12 +1525,12 @@ Sensors::task_main()
_rc_sub = orb_subscribe(ORB_ID(input_rc));
_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
- _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
+ _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
/* rate limit vehicle status updates to 5Hz */
- orb_set_interval(_vstatus_sub, 200);
+ orb_set_interval(_vcontrol_mode_sub, 200);
/* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */
orb_set_interval(_gyro_sub, 4);
@@ -1461,7 +1586,7 @@ Sensors::task_main()
perf_begin(_loop_perf);
/* check vehicle status for changes to publication state */
- vehicle_status_poll();
+ vehicle_control_mode_poll();
/* check parameters for updates */
parameter_update_poll();
diff --git a/src/modules/systemlib/airspeed.c b/src/modules/systemlib/airspeed.c
index e01cc4ddaaa2c84d0bdc2174d4b40dfea19d816f..310fbf60ff26c35b502896f60e1096ff84f34271 100644
--- a/src/modules/systemlib/airspeed.c
+++ b/src/modules/systemlib/airspeed.c
@@ -42,7 +42,7 @@
#include <stdio.h>
#include <math.h>
-#include "conversions.h"
+#include <geo/geo.h>
#include "airspeed.h"
@@ -95,17 +95,21 @@ float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_am
float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius)
{
float density = get_air_density(static_pressure, temperature_celsius);
+
if (density < 0.0001f || !isfinite(density)) {
- density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C;
-// printf("[airspeed] Invalid air density, using density at sea level\n");
+ density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C;
}
float pressure_difference = total_pressure - static_pressure;
- if(pressure_difference > 0) {
+ if (pressure_difference > 0) {
return sqrtf((2.0f*(pressure_difference)) / density);
- } else
- {
+ } else {
return -sqrtf((2.0f*fabsf(pressure_difference)) / density);
}
}
+
+float get_air_density(float static_pressure, float temperature_celsius)
+{
+ return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS));
+}
diff --git a/src/modules/systemlib/airspeed.h b/src/modules/systemlib/airspeed.h
index def53f0c1b76b1ab24825ae85d190f8e9be4898a..8dccaab9c57a4174702c152998a8bfc47d0ca137 100644
--- a/src/modules/systemlib/airspeed.h
+++ b/src/modules/systemlib/airspeed.h
@@ -85,6 +85,14 @@
*/
__EXPORT float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius);
+ /**
+ * Calculates air density.
+ *
+ * @param static_pressure ambient pressure in millibar
+ * @param temperature_celcius air / ambient temperature in celcius
+ */
+__EXPORT float get_air_density(float static_pressure, float temperature_celsius);
+
__END_DECLS
#endif
diff --git a/src/modules/systemlib/conversions.c b/src/modules/systemlib/conversions.c
index ac94252c5728ca8afc3ee7315dff66dcec76ce98..9105d83cbefae74bc508b2e9a68538492be083b3 100644
--- a/src/modules/systemlib/conversions.c
+++ b/src/modules/systemlib/conversions.c
@@ -55,100 +55,3 @@ int16_t_from_bytes(uint8_t bytes[])
return u.w;
}
-
-void rot2quat(const float R[9], float Q[4])
-{
- float q0_2;
- float q1_2;
- float q2_2;
- float q3_2;
- int32_t idx;
-
- /* conversion of rotation matrix to quaternion
- * choose the largest component to begin with */
- q0_2 = (((1.0F + R[0]) + R[4]) + R[8]) / 4.0F;
- q1_2 = (((1.0F + R[0]) - R[4]) - R[8]) / 4.0F;
- q2_2 = (((1.0F - R[0]) + R[4]) - R[8]) / 4.0F;
- q3_2 = (((1.0F - R[0]) - R[4]) + R[8]) / 4.0F;
-
- idx = 0;
-
- if (q0_2 < q1_2) {
- q0_2 = q1_2;
-
- idx = 1;
- }
-
- if (q0_2 < q2_2) {
- q0_2 = q2_2;
- idx = 2;
- }
-
- if (q0_2 < q3_2) {
- q0_2 = q3_2;
- idx = 3;
- }
-
- q0_2 = sqrtf(q0_2);
-
- /* solve for the remaining three components */
- if (idx == 0) {
- q1_2 = q0_2;
- q2_2 = (R[5] - R[7]) / 4.0F / q0_2;
- q3_2 = (R[6] - R[2]) / 4.0F / q0_2;
- q0_2 = (R[1] - R[3]) / 4.0F / q0_2;
-
- } else if (idx == 1) {
- q2_2 = q0_2;
- q1_2 = (R[5] - R[7]) / 4.0F / q0_2;
- q3_2 = (R[3] + R[1]) / 4.0F / q0_2;
- q0_2 = (R[6] + R[2]) / 4.0F / q0_2;
-
- } else if (idx == 2) {
- q3_2 = q0_2;
- q1_2 = (R[6] - R[2]) / 4.0F / q0_2;
- q2_2 = (R[3] + R[1]) / 4.0F / q0_2;
- q0_2 = (R[7] + R[5]) / 4.0F / q0_2;
-
- } else {
- q1_2 = (R[1] - R[3]) / 4.0F / q0_2;
- q2_2 = (R[6] + R[2]) / 4.0F / q0_2;
- q3_2 = (R[7] + R[5]) / 4.0F / q0_2;
- }
-
- /* return values */
- Q[0] = q1_2;
- Q[1] = q2_2;
- Q[2] = q3_2;
- Q[3] = q0_2;
-}
-
-void quat2rot(const float Q[4], float R[9])
-{
- float q0_2;
- float q1_2;
- float q2_2;
- float q3_2;
-
- memset(&R[0], 0, 9U * sizeof(float));
-
- q0_2 = Q[0] * Q[0];
- q1_2 = Q[1] * Q[1];
- q2_2 = Q[2] * Q[2];
- q3_2 = Q[3] * Q[3];
-
- R[0] = ((q0_2 + q1_2) - q2_2) - q3_2;
- R[3] = 2.0F * (Q[1] * Q[2] - Q[0] * Q[3]);
- R[6] = 2.0F * (Q[1] * Q[3] + Q[0] * Q[2]);
- R[1] = 2.0F * (Q[1] * Q[2] + Q[0] * Q[3]);
- R[4] = ((q0_2 + q2_2) - q1_2) - q3_2;
- R[7] = 2.0F * (Q[2] * Q[3] - Q[0] * Q[1]);
- R[2] = 2.0F * (Q[1] * Q[3] - Q[0] * Q[2]);
- R[5] = 2.0F * (Q[2] * Q[3] + Q[0] * Q[1]);
- R[8] = ((q0_2 + q3_2) - q1_2) - q2_2;
-}
-
-float get_air_density(float static_pressure, float temperature_celsius)
-{
- return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS));
-}
diff --git a/src/modules/systemlib/conversions.h b/src/modules/systemlib/conversions.h
index 064426f218e758e93bb2495e74b3f752c2051808..dc383e770f9fdc65b2e81093796a59652b6c929c 100644
--- a/src/modules/systemlib/conversions.h
+++ b/src/modules/systemlib/conversions.h
@@ -43,7 +43,6 @@
#define CONVERSIONS_H_
#include <float.h>
#include <stdint.h>
-#include <systemlib/geo/geo.h>
__BEGIN_DECLS
@@ -57,34 +56,6 @@ __BEGIN_DECLS
*/
__EXPORT int16_t int16_t_from_bytes(uint8_t bytes[]);
-/**
- * Converts a 3 x 3 rotation matrix to an unit quaternion.
- *
- * All orientations are expressed in NED frame.
- *
- * @param R rotation matrix to convert
- * @param Q quaternion to write back to
- */
-__EXPORT void rot2quat(const float R[9], float Q[4]);
-
-/**
- * Converts an unit quaternion to a 3 x 3 rotation matrix.
- *
- * All orientations are expressed in NED frame.
- *
- * @param Q quaternion to convert
- * @param R rotation matrix to write back to
- */
-__EXPORT void quat2rot(const float Q[4], float R[9]);
-
-/**
- * Calculates air density.
- *
- * @param static_pressure ambient pressure in millibar
- * @param temperature_celcius air / ambient temperature in celcius
- */
-__EXPORT float get_air_density(float static_pressure, float temperature_celsius);
-
__END_DECLS
#endif /* CONVERSIONS_H_ */
diff --git a/src/modules/mavlink/mavlink_log.c b/src/modules/systemlib/mavlink_log.c
similarity index 81%
rename from src/modules/mavlink/mavlink_log.c
rename to src/modules/systemlib/mavlink_log.c
index 1921958568cf5cd31c0d20672a7ef28b03e8309c..27608bdbfe24005846157e4a14d75df94a1b820c 100644
--- a/src/modules/mavlink/mavlink_log.c
+++ b/src/modules/systemlib/mavlink_log.c
@@ -46,28 +46,25 @@
#include <mavlink/mavlink_log.h>
-static FILE* text_recorder_fd = NULL;
-
-void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size)
+__EXPORT void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size)
{
lb->size = size;
lb->start = 0;
lb->count = 0;
lb->elems = (struct mavlink_logmessage *)calloc(lb->size, sizeof(struct mavlink_logmessage));
- text_recorder_fd = fopen("/fs/microsd/text_recorder.txt", "w");
}
-int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb)
+__EXPORT int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb)
{
return lb->count == (int)lb->size;
}
-int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb)
+__EXPORT int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb)
{
return lb->count == 0;
}
-void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem)
+__EXPORT void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem)
{
int end = (lb->start + lb->count) % lb->size;
memcpy(&(lb->elems[end]), elem, sizeof(struct mavlink_logmessage));
@@ -80,19 +77,13 @@ void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_
}
}
-int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem)
+__EXPORT int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem)
{
if (!mavlink_logbuffer_is_empty(lb)) {
memcpy(elem, &(lb->elems[lb->start]), sizeof(struct mavlink_logmessage));
lb->start = (lb->start + 1) % lb->size;
--lb->count;
- if (text_recorder_fd) {
- fwrite(elem->text, 1, strnlen(elem->text, 50), text_recorder_fd);
- fputc("\n", text_recorder_fd);
- fsync(text_recorder_fd);
- }
-
return 0;
} else {
@@ -100,7 +91,7 @@ int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessa
}
}
-void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...)
+__EXPORT void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...)
{
va_list ap;
va_start(ap, fmt);
diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk
index b470c12271295e5d713e78f2597f0dec88006754..843cda7225c5a718f869ebb11b3a36fc97305087 100644
--- a/src/modules/systemlib/module.mk
+++ b/src/modules/systemlib/module.mk
@@ -45,7 +45,8 @@ SRCS = err.c \
getopt_long.c \
up_cxxinitialize.c \
pid/pid.c \
- geo/geo.c \
systemlib.c \
airspeed.c \
- system_params.c
+ system_params.c \
+ mavlink_log.c \
+ rc_check.c
diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c
new file mode 100644
index 0000000000000000000000000000000000000000..9d47d8000d9ca800182c72aaba1033a2432b77ec
--- /dev/null
+++ b/src/modules/systemlib/rc_check.c
@@ -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, ¶m_min);
+
+ /* trim values */
+ sprintf(nbuf, "RC%d_TRIM", i + 1);
+ _parameter_handles_trim = param_find(nbuf);
+ param_get(_parameter_handles_trim, ¶m_trim);
+
+ /* max values */
+ sprintf(nbuf, "RC%d_MAX", i + 1);
+ _parameter_handles_max = param_find(nbuf);
+ param_get(_parameter_handles_max, ¶m_max);
+
+ /* channel reverse */
+ sprintf(nbuf, "RC%d_REV", i + 1);
+ _parameter_handles_rev = param_find(nbuf);
+ param_get(_parameter_handles_rev, ¶m_rev);
+
+ /* channel deadzone */
+ sprintf(nbuf, "RC%d_DZ", i + 1);
+ _parameter_handles_dz = param_find(nbuf);
+ param_get(_parameter_handles_dz, ¶m_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);
+ }
+ }
+}
diff --git a/src/modules/systemlib/rc_check.h b/src/modules/systemlib/rc_check.h
new file mode 100644
index 0000000000000000000000000000000000000000..e2238d1516fdca77126b02d262c5f59941ad290a
--- /dev/null
+++ b/src/modules/systemlib/rc_check.h
@@ -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
diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c
index 3283aad8a593188341ea47b0ce706ab95b35fea2..57a751e1c3807b8ae8ee176b3ef94739156ab25b 100644
--- a/src/modules/systemlib/systemlib.c
+++ b/src/modules/systemlib/systemlib.c
@@ -43,13 +43,29 @@
#include <fcntl.h>
#include <sched.h>
#include <signal.h>
-#include <sys/stat.h>
#include <unistd.h>
#include <float.h>
#include <string.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+
+#include <stm32_pwr.h>
+
#include "systemlib.h"
+void
+systemreset(bool to_bootloader)
+{
+ if (to_bootloader) {
+ stm32_pwr_enablebkp();
+
+ /* XXX wow, this is evil - write a magic number into backup register zero */
+ *(uint32_t *)0x40002850 = 0xb007b007;
+ }
+ up_systemreset();
+}
+
static void kill_task(FAR struct tcb_s *tcb, FAR void *arg);
void killall()
diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h
index 0194b5e5292fd3e3c37327a946766af9d597cfd8..3728f20672c4b7f07ba81d3eeeffa22ab74992ce 100644
--- a/src/modules/systemlib/systemlib.h
+++ b/src/modules/systemlib/systemlib.h
@@ -42,11 +42,11 @@
#include <float.h>
#include <stdint.h>
-/** Reboots the board */
-extern void up_systemreset(void) noreturn_function;
-
__BEGIN_DECLS
+/** Reboots the board */
+__EXPORT void systemreset(bool to_bootloader) noreturn_function;
+
/** Sends SIGUSR1 to all processes */
__EXPORT void killall(void);
diff --git a/src/modules/test/foo.c b/src/modules/test/foo.c
deleted file mode 100644
index ff6af031f7d4da0b1eeb807b38a2fafbdd98ba0e..0000000000000000000000000000000000000000
--- a/src/modules/test/foo.c
+++ /dev/null
@@ -1,4 +0,0 @@
-int test_main(void)
-{
- return 0;
-}
\ No newline at end of file
diff --git a/src/modules/test/module.mk b/src/modules/test/module.mk
deleted file mode 100644
index abf80eedf7f1f6003aa508dbdedc2316c182d347..0000000000000000000000000000000000000000
--- a/src/modules/test/module.mk
+++ /dev/null
@@ -1,4 +0,0 @@
-
-MODULE_NAME = test
-SRCS = foo.c
-
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index 301cfa255f2bc115209a3779017180023fa28aab..1eb63a79944a4393d04395823ff1afb9df4981c6 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Copyright (C) 2012, 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
@@ -81,6 +81,9 @@ ORB_DEFINE(home_position, struct home_position_s);
#include "topics/vehicle_status.h"
ORB_DEFINE(vehicle_status, struct vehicle_status_s);
+#include "topics/safety.h"
+ORB_DEFINE(safety, struct safety_s);
+
#include "topics/battery_status.h"
ORB_DEFINE(battery_status, struct battery_status_s);
@@ -102,6 +105,9 @@ ORB_DEFINE(rc_channels, struct rc_channels_s);
#include "topics/vehicle_command.h"
ORB_DEFINE(vehicle_command, struct vehicle_command_s);
+#include "topics/vehicle_control_mode.h"
+ORB_DEFINE(vehicle_control_mode, struct vehicle_control_mode_s);
+
#include "topics/vehicle_local_position_setpoint.h"
ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoint_s);
@@ -114,6 +120,9 @@ ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setp
#include "topics/vehicle_global_position_set_triplet.h"
ORB_DEFINE(vehicle_global_position_set_triplet, struct vehicle_global_position_set_triplet_s);
+#include "topics/vehicle_global_velocity_setpoint.h"
+ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s);
+
#include "topics/mission.h"
ORB_DEFINE(mission, struct mission_s);
@@ -123,6 +132,9 @@ ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s);
#include "topics/manual_control_setpoint.h"
ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s);
+#include "topics/vehicle_control_debug.h"
+ORB_DEFINE(vehicle_control_debug, struct vehicle_control_debug_s);
+
#include "topics/offboard_control_setpoint.h"
ORB_DEFINE(offboard_control_setpoint, struct offboard_control_setpoint_s);
@@ -150,6 +162,8 @@ ORB_DEFINE(actuator_controls_0, struct actuator_controls_s);
ORB_DEFINE(actuator_controls_1, struct actuator_controls_s);
ORB_DEFINE(actuator_controls_2, struct actuator_controls_s);
ORB_DEFINE(actuator_controls_3, struct actuator_controls_s);
+
+#include "topics/actuator_armed.h"
ORB_DEFINE(actuator_armed, struct actuator_armed_s);
/* actuator controls, as set by actuators / mixers after limiting */
diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/modules/uORB/topics/actuator_armed.h
new file mode 100644
index 0000000000000000000000000000000000000000..6e944ffee34b55ed8b25a461b13bec1cfa1fe52a
--- /dev/null
+++ b/src/modules/uORB/topics/actuator_armed.h
@@ -0,0 +1,58 @@
+/****************************************************************************
+ *
+ * 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 actuator_armed.h
+ *
+ * Actuator armed topic
+ *
+ */
+
+#ifndef TOPIC_ACTUATOR_ARMED_H
+#define TOPIC_ACTUATOR_ARMED_H
+
+#include <stdint.h>
+#include "../uORB.h"
+
+/** global 'actuator output is live' control. */
+struct actuator_armed_s {
+
+ uint64_t timestamp;
+ bool armed; /**< Set to true if system is armed */
+ bool ready_to_arm; /**< Set to true if system is ready to be armed */
+ bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
+};
+
+ORB_DECLARE(actuator_armed);
+
+#endif
\ No newline at end of file
diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h
index a27095be52b51b34816ea3289440a6ef2b0d78c3..e768ab2f613f57adec2b3d6dd1c633cffdeb9ce7 100644
--- a/src/modules/uORB/topics/actuator_controls.h
+++ b/src/modules/uORB/topics/actuator_controls.h
@@ -52,6 +52,9 @@
#define NUM_ACTUATOR_CONTROLS 8
#define NUM_ACTUATOR_CONTROL_GROUPS 4 /**< for sanity checking */
+/* control sets with pre-defined applications */
+#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0)
+
/**
* @addtogroup topics
* @{
@@ -72,16 +75,4 @@ ORB_DECLARE(actuator_controls_1);
ORB_DECLARE(actuator_controls_2);
ORB_DECLARE(actuator_controls_3);
-/* control sets with pre-defined applications */
-#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0)
-
-/** global 'actuator output is live' control. */
-struct actuator_armed_s {
- bool armed; /**< Set to true if system is armed */
- bool ready_to_arm; /**< Set to true if system is ready to be armed */
- bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
-};
-
-ORB_DECLARE(actuator_armed);
-
-#endif
\ No newline at end of file
+#endif
diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h
index 261a8a4ad3dff1b943e2c0716a80355da8f3487d..eac6d6e986b243c9fe0fe96ef271867bf3738b4c 100644
--- a/src/modules/uORB/topics/manual_control_setpoint.h
+++ b/src/modules/uORB/topics/manual_control_setpoint.h
@@ -56,17 +56,18 @@ struct manual_control_setpoint_s {
float yaw; /**< rudder / yaw rate / yaw */
float throttle; /**< throttle / collective thrust / altitude */
- float manual_override_switch; /**< manual override mode (mandatory) */
- float auto_mode_switch; /**< auto mode switch (mandatory) */
+ float mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */
+ float return_switch; /**< land 2 position switch (mandatory): land, no effect */
+ float assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */
+ float mission_switch; /**< mission 2 position switch (optional): mission, loiter */
/**
* Any of the channels below may not be available and be set to NaN
* to indicate that it does not contain valid data.
*/
- float manual_mode_switch; /**< manual mode (man, sas, alt) switch (optional) */
- float manual_sas_switch; /**< sas mode (rates / attitude) switch (optional) */
- float return_to_launch_switch; /**< return to launch switch (0 = disabled, 1 = enabled) */
- float auto_offboard_input_switch; /**< controller setpoint source (0 = onboard, 1 = offboard) */
+
+ // XXX needed or parameter?
+ //float auto_offboard_input_switch; /**< controller setpoint source (0 = onboard, 1 = offboard) */
float flaps; /**< flap position */
diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h
index e69335b3d8db8f51897d3fecf3197cf49985f44e..5a858014397e5ebe8706676ffba06288b0b93161 100644
--- a/src/modules/uORB/topics/rc_channels.h
+++ b/src/modules/uORB/topics/rc_channels.h
@@ -48,9 +48,12 @@
/**
* The number of RC channel inputs supported.
* Current (Q1/2013) radios support up to 18 channels,
- * leaving at a sane value of 14.
+ * leaving at a sane value of 15.
+ * This number can be greater then number of RC channels,
+ * because single RC channel can be mapped to multiple
+ * functions, e.g. for various mode switches.
*/
-#define RC_CHANNELS_MAX 14
+#define RC_CHANNELS_MAX 15
/**
* This defines the mapping of the RC functions.
@@ -63,18 +66,17 @@ enum RC_CHANNELS_FUNCTION
ROLL = 1,
PITCH = 2,
YAW = 3,
- OVERRIDE = 4,
- AUTO_MODE = 5,
- MANUAL_MODE = 6,
- SAS_MODE = 7,
- RTL = 8,
- OFFBOARD_MODE = 9,
- FLAPS = 10,
- AUX_1 = 11,
- AUX_2 = 12,
- AUX_3 = 13,
- AUX_4 = 14,
- AUX_5 = 15,
+ MODE = 4,
+ RETURN = 5,
+ ASSISTED = 6,
+ MISSION = 7,
+ OFFBOARD_MODE = 8,
+ FLAPS = 9,
+ AUX_1 = 10,
+ AUX_2 = 11,
+ AUX_3 = 12,
+ AUX_4 = 13,
+ AUX_5 = 14,
RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */
};
diff --git a/src/modules/uORB/topics/safety.h b/src/modules/uORB/topics/safety.h
new file mode 100644
index 0000000000000000000000000000000000000000..a5d21cd4ad98502b79cea6b6ca9de498cfb82d22
--- /dev/null
+++ b/src/modules/uORB/topics/safety.h
@@ -0,0 +1,57 @@
+/****************************************************************************
+ *
+ * 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 safety.h
+ *
+ * Safety topic to pass safety state from px4io driver to commander
+ * This concerns only the safety button of the px4io but has nothing to do
+ * with arming/disarming.
+ */
+
+#ifndef TOPIC_SAFETY_H
+#define TOPIC_SAFETY_H
+
+#include <stdint.h>
+#include "../uORB.h"
+
+struct safety_s {
+
+ uint64_t timestamp;
+ bool safety_switch_available; /**< Set to true if a safety switch is connected */
+ bool safety_off; /**< Set to true if safety is off */
+};
+
+ORB_DECLARE(safety);
+
+#endif
\ No newline at end of file
diff --git a/src/modules/uORB/topics/vehicle_control_debug.h b/src/modules/uORB/topics/vehicle_control_debug.h
new file mode 100644
index 0000000000000000000000000000000000000000..6184284a43619615b3f80d903f711ac284488b05
--- /dev/null
+++ b/src/modules/uORB/topics/vehicle_control_debug.h
@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * 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 vehicle_control_debug.h
+ * For debugging purposes to log PID parts of controller
+ */
+
+#ifndef TOPIC_VEHICLE_CONTROL_DEBUG_H_
+#define TOPIC_VEHICLE_CONTROL_DEBUG_H_
+
+#include <stdint.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+struct vehicle_control_debug_s
+{
+ uint64_t timestamp; /**< in microseconds since system start */
+
+ float roll_p; /**< roll P control part */
+ float roll_i; /**< roll I control part */
+ float roll_d; /**< roll D control part */
+
+ float roll_rate_p; /**< roll rate P control part */
+ float roll_rate_i; /**< roll rate I control part */
+ float roll_rate_d; /**< roll rate D control part */
+
+ float pitch_p; /**< pitch P control part */
+ float pitch_i; /**< pitch I control part */
+ float pitch_d; /**< pitch D control part */
+
+ float pitch_rate_p; /**< pitch rate P control part */
+ float pitch_rate_i; /**< pitch rate I control part */
+ float pitch_rate_d; /**< pitch rate D control part */
+
+ float yaw_p; /**< yaw P control part */
+ float yaw_i; /**< yaw I control part */
+ float yaw_d; /**< yaw D control part */
+
+ float yaw_rate_p; /**< yaw rate P control part */
+ float yaw_rate_i; /**< yaw rate I control part */
+ float yaw_rate_d; /**< yaw rate D control part */
+
+}; /**< vehicle_control_debug */
+
+ /**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(vehicle_control_debug);
+
+#endif
diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h
new file mode 100644
index 0000000000000000000000000000000000000000..093c6775d164ff73e21c5db8a69c31f20bd67803
--- /dev/null
+++ b/src/modules/uORB/topics/vehicle_control_mode.h
@@ -0,0 +1,94 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ *
+ * 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 vehicle_control_mode.h
+ * Definition of the vehicle_control_mode uORB topic.
+ *
+ * All control apps should depend their actions based on the flags set here.
+ */
+
+#ifndef VEHICLE_CONTROL_MODE
+#define VEHICLE_CONTROL_MODE
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics @{
+ */
+
+
+/**
+ * state machine / state of vehicle.
+ *
+ * Encodes the complete system state and is set by the commander app.
+ */
+struct vehicle_control_mode_s
+{
+ uint16_t counter; /**< incremented by the writing thread every time new data is stored */
+ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
+
+ bool flag_armed;
+
+ bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */
+
+ // XXX needs yet to be set by state machine helper
+ bool flag_system_hil_enabled;
+
+ bool flag_control_manual_enabled; /**< true if manual input is mixed in */
+ bool flag_control_offboard_enabled; /**< true if offboard control input is on */
+ bool flag_control_rates_enabled; /**< true if rates are stabilized */
+ bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */
+ bool flag_control_velocity_enabled; /**< true if horizontal velocity (implies direction) is controlled */
+ bool flag_control_position_enabled; /**< true if position is controlled */
+ bool flag_control_altitude_enabled; /**< true if altitude is controlled */
+ bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */
+
+ bool flag_control_auto_enabled; // TEMP
+ uint8_t auto_state; // TEMP navigation state for AUTO modes
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(vehicle_control_mode);
+
+#endif
diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h
index f036c72237c16be7b950f34d9dd5c3b960c791ca..0fc0ed5c970fcee8f9fc2a420e4a2fbc82754a97 100644
--- a/src/modules/uORB/topics/vehicle_global_position.h
+++ b/src/modules/uORB/topics/vehicle_global_position.h
@@ -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. */
};
/**
diff --git a/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h b/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h
new file mode 100644
index 0000000000000000000000000000000000000000..73961cdfedfea4cde6bb4797254d9ea467bd7ed6
--- /dev/null
+++ b/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h
@@ -0,0 +1,64 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ * Author: @author Anton Babushkin <anton.babushkin@me.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 vehicle_global_velocity_setpoint.h
+ * Definition of the global velocity setpoint uORB topic.
+ */
+
+#ifndef TOPIC_VEHICLE_GLOBAL_VELOCITY_SETPOINT_H_
+#define TOPIC_VEHICLE_GLOBAL_VELOCITY_SETPOINT_H_
+
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+struct vehicle_global_velocity_setpoint_s
+{
+ float vx; /**< in m/s NED */
+ float vy; /**< in m/s NED */
+ float vz; /**< in m/s NED */
+}; /**< Velocity setpoint in NED frame */
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(vehicle_global_velocity_setpoint);
+
+#endif
diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h
index 76eddeacd2d028c3b651d42b5831c641f5f56960..31a0e632b2415e2fe7ed8b049c137fcda951bf47 100644
--- a/src/modules/uORB/topics/vehicle_local_position.h
+++ b/src/modules/uORB/topics/vehicle_local_position.h
@@ -54,27 +54,29 @@
*/
struct vehicle_local_position_s
{
- uint64_t timestamp; /**< time of this estimate, in microseconds since system start */
- bool valid; /**< true if position satisfies validity criteria of estimator */
-
- float x; /**< X positin in meters in NED earth-fixed frame */
- float y; /**< X positin in meters in NED earth-fixed frame */
- float z; /**< Z positin in meters in NED earth-fixed frame (negative altitude) */
- float absolute_alt; /**< Altitude as defined by pressure / GPS, LOGME */
- float vx; /**< Ground X Speed (Latitude), m/s in NED LOGME */
- float vy; /**< Ground Y Speed (Longitude), m/s in NED LOGME */
- float vz; /**< Ground Z Speed (Altitude), m/s in NED LOGME */
- float hdg; /**< Compass heading in radians -PI..+PI. */
-
- // TODO Add covariances here
-
+ uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */
+ bool xy_valid; /**< true if x and y are valid */
+ bool z_valid; /**< true if z is valid */
+ bool v_xy_valid; /**< true if vy and vy are valid */
+ bool v_z_valid; /**< true if vz is valid */
+ /* Position in local NED frame */
+ float x; /**< X position in meters in NED earth-fixed frame */
+ float y; /**< X position in meters in NED earth-fixed frame */
+ float z; /**< Z position in meters in NED earth-fixed frame (negative altitude) */
+ /* Velocity in NED frame */
+ 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 */
- uint64_t home_timestamp;/**< Time when home position was set */
- int32_t home_lat; /**< Latitude in 1E7 degrees LOGME */
- int32_t home_lon; /**< Longitude in 1E7 degrees LOGME */
- float home_alt; /**< Altitude in meters LOGME */
- float home_hdg; /**< Compass heading in radians -PI..+PI. */
-
+ 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) */
+ uint64_t ref_timestamp; /**< Time when reference position was set */
+ int32_t ref_lat; /**< Reference point latitude in 1E7 degrees */
+ int32_t ref_lon; /**< Reference point longitude in 1E7 degrees */
+ float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */
+ bool landed; /**< true if vehicle is landed */
};
/**
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 94068a9acc469253fb6ba6dc4f6ba2c791cf0785..43218eac4ec8c5503a267b069251171c5f364897 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -54,22 +54,67 @@
#include <stdbool.h>
#include "../uORB.h"
-/* State Machine */
-typedef enum
-{
- SYSTEM_STATE_PREFLIGHT = 0,
- SYSTEM_STATE_STANDBY = 1,
- SYSTEM_STATE_GROUND_READY = 2,
- SYSTEM_STATE_MANUAL = 3,
- SYSTEM_STATE_STABILIZED = 4,
- SYSTEM_STATE_AUTO = 5,
- SYSTEM_STATE_MISSION_ABORT = 6,
- SYSTEM_STATE_EMCY_LANDING = 7,
- SYSTEM_STATE_EMCY_CUTOFF = 8,
- SYSTEM_STATE_GROUND_ERROR = 9,
- SYSTEM_STATE_REBOOT= 10,
-
-} commander_state_machine_t;
+/**
+ * @addtogroup topics @{
+ */
+
+/* main state machine */
+typedef enum {
+ MAIN_STATE_MANUAL = 0,
+ MAIN_STATE_SEATBELT,
+ MAIN_STATE_EASY,
+ MAIN_STATE_AUTO,
+} main_state_t;
+
+/* navigation state machine */
+typedef enum {
+ NAVIGATION_STATE_DIRECT = 0, // true manual control, no any stabilization
+ NAVIGATION_STATE_STABILIZE, // attitude stabilization
+ NAVIGATION_STATE_ALTHOLD, // attitude + altitude stabilization
+ NAVIGATION_STATE_VECTOR, // attitude + altitude + position stabilization
+ NAVIGATION_STATE_AUTO_READY, // AUTO, landed, reeady for takeoff
+ NAVIGATION_STATE_AUTO_TAKEOFF, // detect takeoff using land detector and switch to desired AUTO mode
+ NAVIGATION_STATE_AUTO_LOITER, // pause mission
+ NAVIGATION_STATE_AUTO_MISSION, // fly mission
+ NAVIGATION_STATE_AUTO_RTL, // Return To Launch, when home position switch to LAND
+ NAVIGATION_STATE_AUTO_LAND // land and switch to AUTO_READY when landed (detect using land detector)
+} navigation_state_t;
+
+typedef enum {
+ ARMING_STATE_INIT = 0,
+ ARMING_STATE_STANDBY,
+ ARMING_STATE_ARMED,
+ ARMING_STATE_ARMED_ERROR,
+ ARMING_STATE_STANDBY_ERROR,
+ ARMING_STATE_REBOOT,
+ ARMING_STATE_IN_AIR_RESTORE
+} arming_state_t;
+
+typedef enum {
+ HIL_STATE_OFF = 0,
+ HIL_STATE_ON
+} hil_state_t;
+
+typedef enum {
+ MODE_SWITCH_MANUAL = 0,
+ MODE_SWITCH_ASSISTED,
+ MODE_SWITCH_AUTO
+} mode_switch_pos_t;
+
+typedef enum {
+ ASSISTED_SWITCH_SEATBELT = 0,
+ ASSISTED_SWITCH_EASY
+} assisted_switch_pos_t;
+
+typedef enum {
+ RETURN_SWITCH_NONE = 0,
+ RETURN_SWITCH_RETURN
+} return_switch_pos_t;
+
+typedef enum {
+ MISSION_SWITCH_NONE = 0,
+ MISSION_SWITCH_MISSION
+} mission_switch_pos_t;
enum VEHICLE_MODE_FLAG {
VEHICLE_MODE_FLAG_SAFETY_ARMED = 128,
@@ -82,26 +127,6 @@ enum VEHICLE_MODE_FLAG {
VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1
}; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */
-enum VEHICLE_FLIGHT_MODE {
- VEHICLE_FLIGHT_MODE_MANUAL = 0, /**< direct manual control, exact mode determined by VEHICLE_MANUAL_CONTROL_MODE */
- VEHICLE_FLIGHT_MODE_STAB, /**< attitude or rate stabilization plus velocity or position stabilization */
- VEHICLE_FLIGHT_MODE_HOLD, /**< hold current position (hover or loiter around position when switched) */
- VEHICLE_FLIGHT_MODE_AUTO /**< attitude or rate stabilization plus absolute position control and waypoints */
-};
-
-enum VEHICLE_MANUAL_CONTROL_MODE {
- VEHICLE_MANUAL_CONTROL_MODE_DIRECT = 0, /**< no attitude control, direct stick input mixing (only fixed wing) */
- VEHICLE_MANUAL_CONTROL_MODE_RATES, /**< body rates control mode */
- VEHICLE_MANUAL_CONTROL_MODE_SAS /**< stability augmented system (SAS) mode */
-};
-
-enum VEHICLE_MANUAL_SAS_MODE {
- VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS = 0, /**< roll, pitch and yaw absolute */
- VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE, /**< roll and pitch absolute, yaw rate */
- VEHICLE_MANUAL_SAS_MODE_SIMPLE, /**< simple mode (includes altitude hold) */
- VEHICLE_MANUAL_SAS_MODE_ALTITUDE /**< altitude hold */
-};
-
/**
* Should match 1:1 MAVLink's MAV_TYPE ENUM
*/
@@ -130,7 +155,7 @@ enum VEHICLE_TYPE {
enum VEHICLE_BATTERY_WARNING {
VEHICLE_BATTERY_WARNING_NONE = 0, /**< no battery low voltage warning active */
VEHICLE_BATTERY_WARNING_WARNING, /**< warning of low voltage 1. stage */
- VEHICLE_BATTERY_WARNING_ALERT /**< aleting of low voltage 2. stage */
+ VEHICLE_BATTERY_WARNING_ALERT /**< alerting of low voltage 2. stage */
};
/**
@@ -149,55 +174,54 @@ struct vehicle_status_s
uint16_t counter; /**< incremented by the writing thread everytime new data is stored */
uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
- uint64_t failsave_lowlevel_start_time; /**< time when the lowlevel failsafe flag was set */
- //uint64_t failsave_highlevel_begin; TO BE COMPLETED
- commander_state_machine_t state_machine; /**< current flight state, main state machine */
- enum VEHICLE_FLIGHT_MODE flight_mode; /**< current flight mode, as defined by mode switch */
- enum VEHICLE_MANUAL_CONTROL_MODE manual_control_mode; /**< current attitude control mode, as defined by VEHICLE_ATTITUDE_MODE enum */
- enum VEHICLE_MANUAL_SAS_MODE manual_sas_mode; /**< current stabilization mode */
+ main_state_t main_state; /**< main state machine */
+ navigation_state_t navigation_state; /**< navigation state machine */
+ arming_state_t arming_state; /**< current arming state */
+ hil_state_t hil_state; /**< current hil state */
+
int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */
int32_t system_id; /**< system id, inspired by MAVLink's system ID field */
int32_t component_id; /**< subsystem / component id, inspired by MAVLink's component ID field */
- /* system flags - these represent the state predicates */
-
- bool flag_system_armed; /**< true is motors / actuators are armed */
- bool flag_control_manual_enabled; /**< true if manual input is mixed in */
- bool flag_control_offboard_enabled; /**< true if offboard control input is on */
- bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */
-
- bool flag_control_rates_enabled; /**< true if rates are stabilized */
- bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */
- bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */
- bool flag_control_position_enabled; /**< true if position is controlled */
-
- bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */
- bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */
- bool flag_preflight_accel_calibration;
- bool flag_preflight_airspeed_calibration;
+ bool is_rotary_wing;
+
+ mode_switch_pos_t mode_switch;
+ return_switch_pos_t return_switch;
+ assisted_switch_pos_t assisted_switch;
+ mission_switch_pos_t mission_switch;
+
+ bool condition_battery_voltage_valid;
+ bool condition_system_in_air_restore; /**< true if we can restore in mid air */
+ bool condition_system_sensors_initialized;
+ bool condition_system_returned_to_home;
+ bool condition_auto_mission_available;
+ bool condition_global_position_valid; /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */
+ bool condition_launch_position_valid; /**< indicates a valid launch position */
+ bool condition_home_position_valid; /**< indicates a valid home position (a valid home position is not always a valid launch) */
+ bool condition_local_position_valid;
+ bool condition_local_altitude_valid;
+ bool condition_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */
+ bool condition_landed; /**< true if vehicle is landed, always true if disarmed */
bool rc_signal_found_once;
- bool rc_signal_lost; /**< true if RC reception is terminally lost */
- bool rc_signal_cutting_off; /**< true if RC reception is weak / cutting off */
- uint64_t rc_signal_lost_interval; /**< interval in microseconds since when no RC signal is available */
+ bool rc_signal_lost; /**< true if RC reception lost */
bool offboard_control_signal_found_once;
bool offboard_control_signal_lost;
bool offboard_control_signal_weak;
uint64_t offboard_control_signal_lost_interval; /**< interval in microseconds without an offboard control message */
- bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */
- //bool failsave_highlevel;
-
/* see SYS_STATUS mavlink message for the following */
uint32_t onboard_control_sensors_present;
uint32_t onboard_control_sensors_enabled;
uint32_t onboard_control_sensors_health;
- float load;
- float voltage_battery;
- float current_battery;
+
+ float load; /**< processor load from 0 to 1 */
+ float battery_voltage;
+ float battery_current;
float battery_remaining;
+
enum VEHICLE_BATTERY_WARNING battery_warning; /**< current battery warning mode, as defined by VEHICLE_BATTERY_WARNING enum */
uint16_t drop_rate_comm;
uint16_t errors_comm;
@@ -205,15 +229,6 @@ struct vehicle_status_s
uint16_t errors_count2;
uint16_t errors_count3;
uint16_t errors_count4;
-
- bool flag_global_position_valid; /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */
- bool flag_local_position_valid;
- bool flag_vector_flight_mode_ok; /**< position estimation, battery voltage and other critical subsystems are good for autonomous flight */
- bool flag_auto_flight_mode_ok; /**< conditions of vector flight mode apply plus a valid takeoff position lock has been aquired */
- bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */
- bool flag_valid_launch_position; /**< indicates a valid launch position */
- bool flag_valid_home_position; /**< indicates a valid home position (a valid home position is not always a valid launch) */
- bool flag_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */
};
/**
diff --git a/src/modules/unit_test/module.mk b/src/modules/unit_test/module.mk
new file mode 100644
index 0000000000000000000000000000000000000000..f00b0f5926beaf9293e96f5c9c7216e415e3fa18
--- /dev/null
+++ b/src/modules/unit_test/module.mk
@@ -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
+
diff --git a/src/modules/unit_test/unit_test.cpp b/src/modules/unit_test/unit_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..64ee544a27d432f1bc615a50eedd2971868ae2e3
--- /dev/null
+++ b/src/modules/unit_test/unit_test.cpp
@@ -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());
+ }
+}
diff --git a/src/modules/unit_test/unit_test.h b/src/modules/unit_test/unit_test.h
new file mode 100644
index 0000000000000000000000000000000000000000..3020734f4bb8e6198bcdeec30950fdae67f42ebe
--- /dev/null
+++ b/src/modules/unit_test/unit_test.h
@@ -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_ */
diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c
index 5a02fd6206463fc8589b3ee9f1cfd4acdbc023df..188dafa4ef28ed8176d37a37bdc3f7ed7fd24f0e 100644
--- a/src/systemcmds/config/config.c
+++ b/src/systemcmds/config/config.c
@@ -2,6 +2,7 @@
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
+ * Author: Julian Oes <joes@student.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -35,6 +36,7 @@
/**
* @file config.c
* @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
*
* config tool.
*/
@@ -69,27 +71,15 @@ config_main(int argc, char *argv[])
{
if (argc >= 2) {
if (!strcmp(argv[1], "gyro")) {
- if (argc >= 3) {
- do_gyro(argc - 2, argv + 2);
- } else {
- errx(1, "not enough parameters.");
- }
+ do_gyro(argc - 2, argv + 2);
}
if (!strcmp(argv[1], "accel")) {
- if (argc >= 3) {
- do_accel(argc - 2, argv + 2);
- } else {
- errx(1, "not enough parameters.");
- }
+ do_accel(argc - 2, argv + 2);
}
if (!strcmp(argv[1], "mag")) {
- if (argc >= 3) {
- do_mag(argc - 2, argv + 2);
- } else {
- errx(1, "not enough parameters.");
- }
+ do_mag(argc - 2, argv + 2);
}
}
@@ -100,6 +90,7 @@ static void
do_gyro(int argc, char *argv[])
{
int fd;
+ int ret;
fd = open(GYRO_DEVICE_PATH, 0);
@@ -109,44 +100,45 @@ do_gyro(int argc, char *argv[])
} else {
- if (argc >= 2) {
+ if (argc == 2 && !strcmp(argv[0], "sampling")) {
- char* end;
- int i = strtol(argv[1],&end,10);
+ /* set the gyro internal sampling rate up to at least i Hz */
+ ret = ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[1], NULL, 0));
- if (!strcmp(argv[0], "sampling")) {
+ if (ret)
+ errx(ret,"sampling rate could not be set");
- /* set the accel internal sampling rate up to at leat i Hz */
- ioctl(fd, GYROIOCSSAMPLERATE, i);
+ } else if (argc == 2 && !strcmp(argv[0], "rate")) {
- } else if (!strcmp(argv[0], "rate")) {
+ /* set the driver to poll at i Hz */
+ ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0));
- /* set the driver to poll at i Hz */
- ioctl(fd, SENSORIOCSPOLLRATE, i);
- } else if (!strcmp(argv[0], "range")) {
+ if (ret)
+ errx(ret,"pollrate could not be set");
- /* set the range to i dps */
- ioctl(fd, GYROIOCSRANGE, i);
- }
+ } else if (argc == 2 && !strcmp(argv[0], "range")) {
+
+ /* set the range to i dps */
+ ret = ioctl(fd, GYROIOCSRANGE, strtoul(argv[1], NULL, 0));
- } else if (argc > 0) {
+ if (ret)
+ errx(ret,"range could not be set");
- if(!strcmp(argv[0], "check")) {
- int ret = ioctl(fd, GYROIOCSELFTEST, 0);
+ } else if(argc == 1 && !strcmp(argv[0], "check")) {
+ ret = ioctl(fd, GYROIOCSELFTEST, 0);
- if (ret) {
- warnx("gyro self test FAILED! Check calibration:");
- struct gyro_scale scale;
- ret = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&scale);
- warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
- warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
- } else {
- warnx("gyro calibration and self test OK");
- }
+ if (ret) {
+ warnx("gyro self test FAILED! Check calibration:");
+ struct gyro_scale scale;
+ ret = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&scale);
+ warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
+ warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
+ } else {
+ warnx("gyro calibration and self test OK");
}
} else {
- warnx("no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2000' to set measurement range to 2000 dps\n\t");
+ errx(1, "wrong or no arguments given. Try: \n\n\t'check' for the self test\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2000' to set measurement range to 2000 dps\n\t");
}
int srate = ioctl(fd, GYROIOCGSAMPLERATE, 0);
@@ -165,6 +157,7 @@ static void
do_mag(int argc, char *argv[])
{
int fd;
+ int ret;
fd = open(MAG_DEVICE_PATH, 0);
@@ -174,31 +167,52 @@ do_mag(int argc, char *argv[])
} else {
- if (argc > 0) {
+ if (argc == 2 && !strcmp(argv[0], "sampling")) {
+
+ /* set the mag internal sampling rate up to at least i Hz */
+ ret = ioctl(fd, MAGIOCSSAMPLERATE, strtoul(argv[1], NULL, 0));
+
+ if (ret)
+ errx(ret,"sampling rate could not be set");
+
+ } else if (argc == 2 && !strcmp(argv[0], "rate")) {
- if (!strcmp(argv[0], "check")) {
- int ret = ioctl(fd, MAGIOCSELFTEST, 0);
+ /* set the driver to poll at i Hz */
+ ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0));
- if (ret) {
- warnx("mag self test FAILED! Check calibration.");
- struct mag_scale scale;
- ret = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&scale);
- warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
- warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
- } else {
- warnx("mag calibration and self test OK");
- }
+ if (ret)
+ errx(ret,"pollrate could not be set");
+
+ } else if (argc == 2 && !strcmp(argv[0], "range")) {
+
+ /* set the range to i G */
+ ret = ioctl(fd, MAGIOCSRANGE, strtoul(argv[1], NULL, 0));
+
+ if (ret)
+ errx(ret,"range could not be set");
+
+ } else if(argc == 1 && !strcmp(argv[0], "check")) {
+ ret = ioctl(fd, MAGIOCSELFTEST, 0);
+
+ if (ret) {
+ warnx("mag self test FAILED! Check calibration:");
+ struct mag_scale scale;
+ ret = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&scale);
+ warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
+ warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
+ } else {
+ warnx("mag calibration and self test OK");
}
} else {
- warnx("no arguments given. Try: \n\n\t'check' or 'info'\n\t");
+ errx(1, "wrong or no arguments given. Try: \n\n\t'check' for the self test\n\t");
}
- int srate = -1;//ioctl(fd, MAGIOCGSAMPLERATE, 0);
+ int srate = ioctl(fd, MAGIOCGSAMPLERATE, 0);
int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0);
- int range = -1;//ioctl(fd, MAGIOCGRANGE, 0);
+ int range = ioctl(fd, MAGIOCGRANGE, 0);
- warnx("mag: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d gauss", srate, prate, range);
+ warnx("mag: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", srate, prate, range);
close(fd);
}
@@ -210,6 +224,7 @@ static void
do_accel(int argc, char *argv[])
{
int fd;
+ int ret;
fd = open(ACCEL_DEVICE_PATH, 0);
@@ -219,50 +234,52 @@ do_accel(int argc, char *argv[])
} else {
- if (argc >= 2) {
+ if (argc == 2 && !strcmp(argv[0], "sampling")) {
- char* end;
- int i = strtol(argv[1],&end,10);
+ /* set the accel internal sampling rate up to at least i Hz */
+ ret = ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[1], NULL, 0));
- if (!strcmp(argv[0], "sampling")) {
+ if (ret)
+ errx(ret,"sampling rate could not be set");
- /* set the accel internal sampling rate up to at leat i Hz */
- ioctl(fd, ACCELIOCSSAMPLERATE, i);
+ } else if (argc == 2 && !strcmp(argv[0], "rate")) {
- } else if (!strcmp(argv[0], "rate")) {
+ /* set the driver to poll at i Hz */
+ ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0));
- /* set the driver to poll at i Hz */
- ioctl(fd, SENSORIOCSPOLLRATE, i);
- } else if (!strcmp(argv[0], "range")) {
+ if (ret)
+ errx(ret,"pollrate could not be set");
- /* set the range to i dps */
- ioctl(fd, ACCELIOCSRANGE, i);
- }
- } else if (argc > 0) {
-
- if (!strcmp(argv[0], "check")) {
- int ret = ioctl(fd, ACCELIOCSELFTEST, 0);
-
- if (ret) {
- warnx("accel self test FAILED! Check calibration.");
- struct accel_scale scale;
- ret = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&scale);
- warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
- warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
- } else {
- warnx("accel calibration and self test OK");
- }
+ } else if (argc == 2 && !strcmp(argv[0], "range")) {
+
+ /* set the range to i G */
+ ret = ioctl(fd, ACCELIOCSRANGE, strtoul(argv[1], NULL, 0));
+
+ if (ret)
+ errx(ret,"range could not be set");
+
+ } else if(argc == 1 && !strcmp(argv[0], "check")) {
+ ret = ioctl(fd, ACCELIOCSELFTEST, 0);
+
+ if (ret) {
+ warnx("accel self test FAILED! Check calibration:");
+ struct accel_scale scale;
+ ret = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&scale);
+ warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
+ warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
+ } else {
+ warnx("accel calibration and self test OK");
}
} else {
- warnx("no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2' to set measurement range to 2 G\n\t");
+ errx(1,"no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2' to set measurement range to 4 G\n\t");
}
int srate = ioctl(fd, ACCELIOCGSAMPLERATE, 0);
int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0);
int range = ioctl(fd, ACCELIOCGRANGE, 0);
- warnx("accel: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d m/s", srate, prate, range);
+ warnx("accel: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G", srate, prate, range);
close(fd);
}
diff --git a/src/systemcmds/eeprom/eeprom.c b/src/systemcmds/eeprom/eeprom.c
index 49da51358064376c2d4275cc026bb187af397d62..2aed80e0115516da5ec512b0626ca99d3a839ac6 100644
--- a/src/systemcmds/eeprom/eeprom.c
+++ b/src/systemcmds/eeprom/eeprom.c
@@ -55,7 +55,7 @@
#include <nuttx/fs/nxffs.h>
#include <nuttx/fs/ioctl.h>
-#include <arch/board/board.h>
+#include <board_config.h>
#include "systemlib/systemlib.h"
#include "systemlib/param/param.h"
diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c
new file mode 100644
index 0000000000000000000000000000000000000000..188fa4e371c91e58af5238ece3bd1fcd45ae7a2a
--- /dev/null
+++ b/src/systemcmds/esc_calib/esc_calib.c
@@ -0,0 +1,250 @@
+/****************************************************************************
+ *
+ * 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 esc_calib.c
+ *
+ * Tool for ESC calibration
+ */
+
+#include <nuttx/config.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <sys/mount.h>
+#include <sys/ioctl.h>
+#include <sys/stat.h>
+
+#include <nuttx/i2c.h>
+#include <nuttx/mtd.h>
+#include <nuttx/fs/nxffs.h>
+#include <nuttx/fs/ioctl.h>
+
+#include <arch/board/board.h>
+
+#include "systemlib/systemlib.h"
+#include "systemlib/err.h"
+#include "drivers/drv_pwm_output.h"
+
+static void usage(const char *reason);
+__EXPORT int esc_calib_main(int argc, char *argv[]);
+
+#define MAX_CHANNELS 8
+
+static void
+usage(const char *reason)
+{
+ if (reason != NULL)
+ warnx("%s", reason);
+ errx(1,
+ "usage:\n"
+ "esc_calib [-d <device>] <channels>\n"
+ " <device> PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n"
+ " <channels> Provide channels (e.g.: 1 2 3 4)\n"
+ );
+
+}
+
+int
+esc_calib_main(int argc, char *argv[])
+{
+ const char *dev = PWM_OUTPUT_DEVICE_PATH;
+ char *ep;
+ bool channels_selected[MAX_CHANNELS] = {false};
+ int ch;
+ int ret;
+ char c;
+
+ if (argc < 2)
+ usage(NULL);
+
+ while ((ch = getopt(argc, argv, "d:")) != EOF) {
+ switch (ch) {
+
+ case 'd':
+ dev = optarg;
+ argc--;
+ break;
+
+ default:
+ usage(NULL);
+ }
+ }
+
+ if(argc < 1) {
+ usage("no channels provided");
+ }
+
+ while (--argc) {
+ const char *arg = argv[argc];
+ unsigned channel_number = strtol(arg, &ep, 0);
+
+ if (*ep == '\0') {
+ if (channel_number > MAX_CHANNELS || channel_number <= 0) {
+ err(1, "invalid channel number: %d", channel_number);
+ } else {
+ channels_selected[channel_number-1] = true;
+
+ }
+ }
+ }
+
+ /* Wait for confirmation */
+ int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
+ if (!console)
+ err(1, "failed opening console");
+
+ warnx("ATTENTION, please remove or fix props before starting calibration!\n"
+ "\n"
+ "Also press the arming switch first for safety off\n"
+ "\n"
+ "Do you really want to start calibration: y or n?\n");
+
+ /* wait for user input */
+ while (1) {
+
+ if (read(console, &c, 1) == 1) {
+ if (c == 'y' || c == 'Y') {
+
+ break;
+ } else if (c == 0x03 || c == 0x63 || c == 'q') {
+ warnx("ESC calibration exited");
+ close(console);
+ exit(0);
+ } else if (c == 'n' || c == 'N') {
+ warnx("ESC calibration aborted");
+ close(console);
+ exit(0);
+ } else {
+ warnx("Unknown input, ESC calibration aborted");
+ close(console);
+ exit(0);
+ }
+ }
+ /* rate limit to ~ 20 Hz */
+ usleep(50000);
+ }
+
+ /* open for ioctl only */
+ int fd = open(dev, 0);
+ if (fd < 0)
+ err(1, "can't open %s", dev);
+
+ // XXX arming not necessaire at the moment
+ // /* Then arm */
+ // ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
+ // if (ret != OK)
+ // err(1, "PWM_SERVO_SET_ARM_OK");
+
+ // ret = ioctl(fd, PWM_SERVO_ARM, 0);
+ // if (ret != OK)
+ // err(1, "PWM_SERVO_ARM");
+
+
+
+
+ /* Wait for user confirmation */
+ warnx("Set high PWM\n"
+ "Connect battery now and hit ENTER after the ESCs confirm the first calibration step");
+
+ while (1) {
+
+ /* First set high PWM */
+ for (unsigned i = 0; i<MAX_CHANNELS; i++) {
+ if(channels_selected[i]) {
+ ret = ioctl(fd, PWM_SERVO_SET(i), 2100);
+ if (ret != OK)
+ err(1, "PWM_SERVO_SET(%d)", i);
+ }
+ }
+
+ if (read(console, &c, 1) == 1) {
+ if (c == 13) {
+
+ break;
+ } else if (c == 0x03 || c == 0x63 || c == 'q') {
+ warnx("ESC calibration exited");
+ close(console);
+ exit(0);
+ }
+ }
+ /* rate limit to ~ 20 Hz */
+ usleep(50000);
+ }
+
+ /* we don't need any more user input */
+
+
+ warnx("Set low PWM, hit ENTER when finished");
+
+ while (1) {
+
+ /* Then set low PWM */
+ for (unsigned i = 0; i<MAX_CHANNELS; i++) {
+ if(channels_selected[i]) {
+ ret = ioctl(fd, PWM_SERVO_SET(i), 900);
+ if (ret != OK)
+ err(1, "PWM_SERVO_SET(%d)", i);
+ }
+ }
+
+ if (read(console, &c, 1) == 1) {
+ if (c == 13) {
+
+ break;
+ } else if (c == 0x03 || c == 0x63 || c == 'q') {
+ warnx("ESC calibration exited");
+ close(console);
+ exit(0);
+ }
+ }
+ /* rate limit to ~ 20 Hz */
+ usleep(50000);
+ }
+
+
+ warnx("ESC calibration finished");
+ close(console);
+
+ // XXX disarming not necessaire at the moment
+ /* Now disarm again */
+ // ret = ioctl(fd, PWM_SERVO_DISARM, 0);
+
+
+
+ exit(0);
+}
\ No newline at end of file
diff --git a/src/systemcmds/esc_calib/module.mk b/src/systemcmds/esc_calib/module.mk
new file mode 100644
index 0000000000000000000000000000000000000000..990c5676815f56e0dfcfb4a4ae516000a5b5a9f0
--- /dev/null
+++ b/src/systemcmds/esc_calib/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+# Copyright (c) 2012, 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.
+#
+############################################################################
+
+#
+# Build the esc_calib tool.
+#
+
+MODULE_COMMAND = esc_calib
+SRCS = esc_calib.c
+
+MODULE_STACKSIZE = 4096
diff --git a/src/systemcmds/i2c/i2c.c b/src/systemcmds/i2c/i2c.c
index 4da238039c5b7f2f2dd5a6d8cb7e8fa59dc69c69..34405c4969a46e3456574a3b6decc8d79de5d265 100644
--- a/src/systemcmds/i2c/i2c.c
+++ b/src/systemcmds/i2c/i2c.c
@@ -52,7 +52,7 @@
#include <nuttx/i2c.h>
-#include <arch/board/board.h>
+#include <board_config.h>
#include "systemlib/systemlib.h"
#include "systemlib/err.h"
diff --git a/src/systemcmds/nshterm/module.mk b/src/systemcmds/nshterm/module.mk
new file mode 100644
index 0000000000000000000000000000000000000000..a48588535462aff6416a4c8dbc5869055a4e831a
--- /dev/null
+++ b/src/systemcmds/nshterm/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+# Copyright (c) 2012, 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.
+#
+############################################################################
+
+#
+# Build nshterm utility
+#
+
+MODULE_COMMAND = nshterm
+SRCS = nshterm.c
+
+MODULE_STACKSIZE = 3000
diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c
new file mode 100644
index 0000000000000000000000000000000000000000..458bb2259ee139a65284481bdf23be59d95f28af
--- /dev/null
+++ b/src/systemcmds/nshterm/nshterm.c
@@ -0,0 +1,115 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ * Author: Andrew Tridgell
+ *
+ * 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 nshterm.c
+ * start a nsh terminal on a given port. This can be useful for error
+ * handling in startup scripts to start a nsh shell on /dev/ttyACM0
+ * for diagnostics
+ */
+
+#include <nuttx/config.h>
+#include <termios.h>
+#include <stdbool.h>
+#include <stdio.h>
+#include <stdarg.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <apps/nsh.h>
+#include <fcntl.h>
+#include <systemlib/err.h>
+
+__EXPORT int nshterm_main(int argc, char *argv[]);
+
+int
+nshterm_main(int argc, char *argv[])
+{
+ if (argc < 2) {
+ printf("Usage: nshterm <device>\n");
+ exit(1);
+ }
+ uint8_t retries = 0;
+ int fd = -1;
+ while (retries < 50) {
+ /* the retries are to cope with the behaviour of /dev/ttyACM0 */
+ /* which may not be ready immediately. */
+ fd = open(argv[1], O_RDWR);
+ if (fd != -1) {
+ break;
+ }
+ usleep(100000);
+ retries++;
+ }
+ if (fd == -1) {
+ perror(argv[1]);
+ exit(1);
+ }
+
+ /* set up the serial port with output processing */
+
+ /* Try to set baud rate */
+ struct termios uart_config;
+ int termios_state;
+
+ /* Back up the original uart configuration to restore it after exit */
+ if ((termios_state = tcgetattr(fd, &uart_config)) < 0) {
+ warnx("ERROR get termios config %s: %d\n", argv[1], termios_state);
+ close(fd);
+ return -1;
+ }
+
+ /* Set ONLCR flag (which appends a CR for every LF) */
+ uart_config.c_oflag |= (ONLCR | OPOST/* | OCRNL*/);
+
+ if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
+ warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", argv[1]);
+ close(fd);
+ return -1;
+ }
+
+ /* setup standard file descriptors */
+ close(0);
+ close(1);
+ close(2);
+ dup2(fd, 0);
+ dup2(fd, 1);
+ dup2(fd, 2);
+
+ nsh_consolemain(0, NULL);
+
+ close(fd);
+
+ return OK;
+}
diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c
index d1dd85d479d313fd29138cfe48270579539b1db9..4c19dcffb61b753c900d2db92e64c894cc1cafdd 100644
--- a/src/systemcmds/preflight_check/preflight_check.c
+++ b/src/systemcmds/preflight_check/preflight_check.c
@@ -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, ¶m_min);
-
- /* trim values */
- sprintf(nbuf, "RC%d_TRIM", i + 1);
- _parameter_handles_trim = param_find(nbuf);
- param_get(_parameter_handles_trim, ¶m_trim);
-
- /* max values */
- sprintf(nbuf, "RC%d_MAX", i + 1);
- _parameter_handles_max = param_find(nbuf);
- param_get(_parameter_handles_max, ¶m_max);
-
- /* channel reverse */
- sprintf(nbuf, "RC%d_REV", i + 1);
- _parameter_handles_rev = param_find(nbuf);
- param_get(_parameter_handles_rev, ¶m_rev);
-
- /* channel deadzone */
- sprintf(nbuf, "RC%d_DZ", i + 1);
- _parameter_handles_dz = param_find(nbuf);
- param_get(_parameter_handles_dz, ¶m_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;
@@ -263,7 +170,7 @@ system_eval:
led_toggle(leds, LED_BLUE);
/* display and sound error */
- for (int i = 0; i < 150; i++)
+ for (int i = 0; i < 50; i++)
{
led_toggle(leds, LED_BLUE);
led_toggle(leds, LED_AMBER);
diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c
index e150b5a74b20987e9372ae588c5a107526f60c40..c42a36c7f31a80ca902c20b705cc143fbd19cfe9 100644
--- a/src/systemcmds/pwm/pwm.c
+++ b/src/systemcmds/pwm/pwm.c
@@ -180,7 +180,8 @@ pwm_main(int argc, char *argv[])
}
/* iterate remaining arguments */
- unsigned channel = 0;
+ unsigned nchannels = 0;
+ unsigned channel[8] = {0};
while (argc--) {
const char *arg = argv[0];
argv++;
@@ -204,13 +205,15 @@ pwm_main(int argc, char *argv[])
}
unsigned pwm_value = strtol(arg, &ep, 0);
if (*ep == '\0') {
- ret = ioctl(fd, PWM_SERVO_SET(channel), pwm_value);
- if (ret != OK)
- err(1, "PWM_SERVO_SET(%d)", channel);
- channel++;
+ if (nchannels > sizeof(channel) / sizeof(channel[0]))
+ err(1, "too many pwm values (max %d)", sizeof(channel) / sizeof(channel[0]));
+
+ channel[nchannels] = pwm_value;
+ nchannels++;
+
continue;
}
- usage("unrecognised option");
+ usage("unrecognized option");
}
/* print verbose info */
@@ -250,5 +253,38 @@ pwm_main(int argc, char *argv[])
}
fflush(stdout);
}
+
+ /* perform PWM output */
+ if (nchannels) {
+
+ /* Open console directly to grab CTRL-C signal */
+ int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
+ if (!console)
+ err(1, "failed opening console");
+
+ warnx("Press CTRL-C or 'c' to abort.");
+
+ while (1) {
+ for (int i = 0; i < nchannels; i++) {
+ ret = ioctl(fd, PWM_SERVO_SET(i), channel[i]);
+ if (ret != OK)
+ err(1, "PWM_SERVO_SET(%d)", i);
+ }
+
+ /* abort on user request */
+ char c;
+ if (read(console, &c, 1) == 1) {
+ if (c == 0x03 || c == 0x63 || c == 'q') {
+ warnx("User abort\n");
+ close(console);
+ exit(0);
+ }
+ }
+
+ /* rate limit to ~ 20 Hz */
+ usleep(50000);
+ }
+ }
+
exit(0);
}
\ No newline at end of file
diff --git a/src/systemcmds/reboot/reboot.c b/src/systemcmds/reboot/reboot.c
index 47d3cd94861b66e4b341e69edb4a10a0b00e7bbd..91a2c2eb8d44a071c33a21febda5377f0218be24 100644
--- a/src/systemcmds/reboot/reboot.c
+++ b/src/systemcmds/reboot/reboot.c
@@ -40,14 +40,31 @@
#include <nuttx/config.h>
#include <unistd.h>
#include <stdio.h>
+#include <getopt.h>
#include <systemlib/systemlib.h>
+#include <systemlib/err.h>
__EXPORT int reboot_main(int argc, char *argv[]);
int reboot_main(int argc, char *argv[])
{
- up_systemreset();
+ int ch;
+ bool to_bootloader = false;
+
+ while ((ch = getopt(argc, argv, "b")) != -1) {
+ switch (ch) {
+ case 'b':
+ to_bootloader = true;
+ break;
+ default:
+ errx(1, "usage: reboot [-b]\n"
+ " -b reboot into the bootloader");
+
+ }
+ }
+
+ systemreset(to_bootloader);
}
diff --git a/src/systemcmds/top/top.c b/src/systemcmds/top/top.c
index 0d064a05eb4ca4f1681715d93bf2871c8a207d98..1ca3fc928141d24d5fce13b574195442ec403c87 100644
--- a/src/systemcmds/top/top.c
+++ b/src/systemcmds/top/top.c
@@ -107,9 +107,6 @@ top_main(void)
float interval_time_ms_inv = 0.f;
- /* Open console directly to grab CTRL-C signal */
- int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
-
/* clear screen */
printf("\033[2J");
@@ -256,17 +253,24 @@ top_main(void)
interval_start_time = new_time;
/* Sleep 200 ms waiting for user input five times ~ 1s */
- /* XXX use poll ... */
for (int k = 0; k < 5; k++) {
char c;
- if (read(console, &c, 1) == 1) {
+ struct pollfd fds;
+ int ret;
+ fds.fd = 0; /* stdin */
+ fds.events = POLLIN;
+ ret = poll(&fds, 1, 0);
+
+ if (ret > 0) {
+
+ read(0, &c, 1);
+
switch (c) {
case 0x03: // ctrl-c
case 0x1b: // esc
case 'c':
case 'q':
- close(console);
return OK;
/* not reached */
}
@@ -278,7 +282,5 @@ top_main(void)
new_time = hrt_absolute_time();
}
- close(console);
-
return OK;
}