Merge branch 'next' of ssh://git.openpilot.org/OpenPilot into kfinisterre/WiresharkPlugin

.gitignore

@@ -2,6 +2,7 @@
 /downloads
 /tools
 /build
+/3rdparty
 
 # Exclude temporary and system files
 Thumbs.db
@@ -11,6 +12,7 @@ GRTAGS
 GSYMS
 GTAGS
 core
+*~
 
 # flight
 /flight/*.pnproj

Makefile

@@ -49,6 +49,8 @@ export BUILD_DIR   := $(ROOT_DIR)/build
 export PACKAGE_DIR := $(ROOT_DIR)/build/package
 export DIST_DIR    := $(ROOT_DIR)/build/dist
 
+DIRS = $(DL_DIR) $(TOOLS_DIR) $(BUILD_DIR) $(PACKAGE_DIR) $(DIST_DIR)
+
 # Set up default build configurations (debug | release)
 GCS_BUILD_CONF		:= release
 UAVOGEN_BUILD_CONF	:= release
@@ -81,8 +83,10 @@ $(foreach var, $(SANITIZE_DEPRECATED_VARS), $(eval $(call SANITIZE_VAR,$(var),de
 
 # Make sure this isn't being run as root unless installing (no whoami on Windows, but that is ok here)
 ifeq ($(shell whoami 2>/dev/null),root)
-    ifeq ($(filter install all_clean,$(MAKECMDGOALS)),)
+    ifeq ($(filter install,$(MAKECMDGOALS)),)
-        $(error You should not be running this as root)
+        ifndef FAKEROOTKEY
+            $(error You should not be running this as root)
+        endif
     endif
 endif
 
@@ -109,6 +113,9 @@ endif
 # Include tools installers
 include $(ROOT_DIR)/make/tools.mk
 
+# Include third party builders if available
+-include $(ROOT_DIR)/make/3rdparty/3rdparty.mk
+
 # We almost need to consider autoconf/automake instead of this
 ifeq ($(UNAME), Linux)
     QT_SPEC = linux-g++
@@ -138,20 +145,6 @@ all_clean:
 .PONY: clean
 clean: all_clean
 
-$(DL_DIR):
-	$(MKDIR) -p $@
-
-$(TOOLS_DIR):
-	$(MKDIR) -p $@
-
-$(BUILD_DIR):
-	$(MKDIR) -p $@
-
-$(PACKAGE_DIR):
-	$(MKDIR) -p $@
-
-$(DIST_DIR):
-	$(MKDIR) -p $@
 
 ##############################
 #
@@ -165,13 +158,15 @@ else
     UAVOGEN_SILENT := silent
 endif
 
+UAVOBJGENERATOR_DIR = $(BUILD_DIR)/uavobjgenerator
+DIRS += $(UAVOBJGENERATOR_DIR)
+
 .PHONY: uavobjgenerator
-uavobjgenerator:
+uavobjgenerator: | $(UAVOBJGENERATOR_DIR)
-	$(V1) $(MKDIR) -p $(BUILD_DIR)/$@
+	$(V1) cd $(UAVOBJGENERATOR_DIR) && \
-	$(V1) ( cd $(BUILD_DIR)/$@ && \
+	    $(QMAKE) $(ROOT_DIR)/ground/uavobjgenerator/uavobjgenerator.pro \
-	    $(QMAKE) $(ROOT_DIR)/ground/uavobjgenerator/uavobjgenerator.pro -spec $(QT_SPEC) -r CONFIG+="$(UAVOGEN_BUILD_CONF) $(UAVOGEN_SILENT)" && \
+	    -spec $(QT_SPEC) -r CONFIG+=$(UAVOGEN_BUILD_CONF) CONFIG+=$(UAVOGEN_SILENT) && \
-	    $(MAKE) --no-print-directory -w ; \
+	    $(MAKE) --no-print-directory -w
-	)
 
 UAVOBJ_TARGETS := gcs flight python matlab java wireshark
 
@@ -181,8 +176,7 @@ uavobjects:  $(addprefix uavobjects_, $(UAVOBJ_TARGETS))
 UAVOBJ_XML_DIR := $(ROOT_DIR)/shared/uavobjectdefinition
 UAVOBJ_OUT_DIR := $(BUILD_DIR)/uavobject-synthetics
 
-$(UAVOBJ_OUT_DIR):
+DIRS += $(UAVOBJ_OUT_DIR)
-	$(V1) $(MKDIR) -p $@
 
 uavobjects_%: $(UAVOBJ_OUT_DIR) uavobjgenerator
 	$(V1) ( cd $(UAVOBJ_OUT_DIR) && \
@@ -212,18 +206,19 @@ export OPUAVTALK     := $(ROOT_DIR)/flight/uavtalk
 export OPUAVSYNTHDIR := $(BUILD_DIR)/uavobject-synthetics/flight
 export OPGCSSYNTHDIR := $(BUILD_DIR)/openpilotgcs-synthetics
 
+DIRS += $(OPGCSSYNTHDIR)
+
 # Define supported board lists
-ALL_BOARDS    := coptercontrol oplinkmini revolution osd revoproto simposix discoveryf4bare gpsplatinum
+ALL_BOARDS    := oplinkmini revolution osd revoproto simposix discoveryf4bare gpsplatinum
 
 # Short names of each board (used to display board name in parallel builds)
-coptercontrol_short    := 'cc  '
 oplinkmini_short       := 'oplm'
 revolution_short       := 'revo'
 osd_short              := 'osd '
 revoproto_short        := 'revp'
 simposix_short         := 'posx'
 discoveryf4bare_short  := 'df4b'
-gpsplatinum_short      := 'gps9 '
+gpsplatinum_short      := 'gps9'
 
 # SimPosix only builds on Linux so drop it from the list for
 # all other platforms.
@@ -458,8 +453,9 @@ sim_osx_%: uavobjects_flight
 all_ground: openpilotgcs uploader
 
 # Convenience target for the GCS
-.PHONY: gcs gcs_clean
+.PHONY: gcs gcs_qmake gcs_clean
 gcs: openpilotgcs
+gcs_qmake: openpilotgcs_qmake
 gcs_clean: openpilotgcs_clean
 
 ifeq ($(V), 1)
@@ -468,32 +464,27 @@ else
     GCS_SILENT := silent
 endif
 
-.NOTPARALLEL:
+OPENPILOTGCS_DIR := $(BUILD_DIR)/openpilotgcs_$(GCS_BUILD_CONF)
-.PHONY: openpilotgcs
+DIRS += $(OPENPILOTGCS_DIR)
-openpilotgcs: uavobjects_gcs openpilotgcs_qmake openpilotgcs_make
+
+OPENPILOTGCS_MAKEFILE := $(OPENPILOTGCS_DIR)/Makefile
 
 .PHONY: openpilotgcs_qmake
-openpilotgcs_qmake:
+openpilotgcs_qmake $(OPENPILOTGCS_MAKEFILE): | $(OPENPILOTGCS_DIR)
-ifeq ($(QMAKE_SKIP),)
+	$(V1) cd $(OPENPILOTGCS_DIR) && \
-	$(V1) $(MKDIR) -p $(BUILD_DIR)/openpilotgcs_$(GCS_BUILD_CONF)
+	    $(QMAKE) $(ROOT_DIR)/ground/openpilotgcs/openpilotgcs.pro \
-	$(V1) ( cd $(BUILD_DIR)/openpilotgcs_$(GCS_BUILD_CONF) && \
+	    -spec $(QT_SPEC) -r CONFIG+=$(GCS_BUILD_CONF) CONFIG+=$(GCS_SILENT) $(GCS_QMAKE_OPTS)
-	    $(QMAKE) $(ROOT_DIR)/ground/openpilotgcs/openpilotgcs.pro -spec $(QT_SPEC) -r CONFIG+="$(GCS_BUILD_CONF) $(GCS_SILENT)" $(GCS_QMAKE_OPTS) \
-	)
-else
-	@$(ECHO) "skipping qmake"
-endif
 
-.PHONY: openpilotgcs_make
+.PHONY: openpilotgcs
-openpilotgcs_make:
+openpilotgcs: uavobjects_gcs $(OPENPILOTGCS_MAKEFILE)
-	$(V1) $(MKDIR) -p $(BUILD_DIR)/openpilotgcs_$(GCS_BUILD_CONF)
+	$(V1) $(MAKE) -w -C $(OPENPILOTGCS_DIR)/$(MAKE_DIR);
-	$(V1) ( cd $(BUILD_DIR)/openpilotgcs_$(GCS_BUILD_CONF)/$(MAKE_DIR) && \
-	    $(MAKE) -w ; \
-	)
 
 .PHONY: openpilotgcs_clean
 openpilotgcs_clean:
-	@$(ECHO) " CLEAN      $(call toprel, $(BUILD_DIR)/openpilotgcs_$(GCS_BUILD_CONF))"
+	@$(ECHO) " CLEAN      $(call toprel, $(OPENPILOTGCS_DIR))"
-	$(V1) [ ! -d "$(BUILD_DIR)/openpilotgcs_$(GCS_BUILD_CONF)" ] || $(RM) -r "$(BUILD_DIR)/openpilotgcs_$(GCS_BUILD_CONF)"
+	$(V1) [ ! -d "$(OPENPILOTGCS_DIR)" ] || $(RM) -r "$(OPENPILOTGCS_DIR)"
+
+
 
 ################################
 #
@@ -501,77 +492,27 @@ openpilotgcs_clean:
 #
 ################################
 
-.NOTPARALLEL:
+UPLOADER_DIR := $(BUILD_DIR)/uploader_$(GCS_BUILD_CONF)
-.PHONY: uploader
+DIRS += $(UPLOADER_DIR)
-uploader: uploader_qmake uploader_make
+
+UPLOADER_MAKEFILE := $(UPLOADER_DIR)/Makefile
 
 .PHONY: uploader_qmake
-uploader_qmake:
+uploader_qmake $(UPLOADER_MAKEFILE): | $(UPLOADER_DIR)
-ifeq ($(QMAKE_SKIP),)
+	$(V1) cd $(UPLOADER_DIR) && \
-	$(V1) $(MKDIR) -p $(BUILD_DIR)/uploader_$(GCS_BUILD_CONF)
+	    $(QMAKE) $(ROOT_DIR)/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/upload.pro \
-	$(V1) ( cd $(BUILD_DIR)/uploader_$(GCS_BUILD_CONF) && \
+	    -spec $(QT_SPEC) -r CONFIG+=$(GCS_BUILD_CONF) CONFIG+=$(GCS_SILENT) $(GCS_QMAKE_OPTS)
-	    $(QMAKE) $(ROOT_DIR)/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/upload.pro -spec $(QT_SPEC) -r CONFIG+="$(GCS_BUILD_CONF) $(GCS_SILENT)" $(GCS_QMAKE_OPTS) \
-	) 
-else
-	@$(ECHO) "skipping qmake"
-endif
 
-.PHONY: uploader_make
+.PHONY: uploader
-uploader_make:
+uploader: $(UPLOADER_MAKEFILE)
-	$(V1) $(MKDIR) -p $(BUILD_DIR)/uploader_$(GCS_BUILD_CONF)
+	$(V1) $(MAKE) -w -C $(UPLOADER_DIR)
-	$(V1) ( cd $(BUILD_DIR)/uploader_$(GCS_BUILD_CONF)/$(MAKE_DIR) && \
-	    $(MAKE) -w ; \
-	)
 
 .PHONY: uploader_clean
 uploader_clean:
-	@$(ECHO) " CLEAN      $(call toprel, $(BUILD_DIR)/uploader_$(GCS_BUILD_CONF))"
+	@$(ECHO) " CLEAN      $(call toprel, $(UPLOADER_DIR))"
-	$(V1) [ ! -d "$(BUILD_DIR)/uploader_$(GCS_BUILD_CONF)" ] || $(RM) -r "$(BUILD_DIR)/uploader_$(GCS_BUILD_CONF)"
+	$(V1) [ ! -d "$(UPLOADER_DIR)" ] || $(RM) -r "$(UPLOADER_DIR)"
 
 
-################################
-#
-# Android GCS related components
-#
-################################
-
-# Build the output directory for the Android GCS build
-ANDROIDGCS_OUT_DIR := $(BUILD_DIR)/androidgcs
-$(ANDROIDGCS_OUT_DIR):
-	$(V1) $(MKDIR) -p $@
-
-# Build the asset directory for the android assets
-ANDROIDGCS_ASSETS_DIR := $(ANDROIDGCS_OUT_DIR)/assets
-$(ANDROIDGCS_ASSETS_DIR)/uavos:
-	$(V1) $(MKDIR) -p $@
-
-ifeq ($(V), 1)
-    ANT_QUIET      :=
-    ANDROID_SILENT :=
-else
-    ANT_QUIET      := -q
-    ANDROID_SILENT := -s
-endif
-
-.PHONY: androidgcs
-androidgcs: uavo-collections_java
-	$(V0) @$(ECHO) " ANDROID   $(call toprel, $(ANDROIDGCS_OUT_DIR))"
-	$(V1) $(MKDIR) -p $(ANDROIDGCS_OUT_DIR)
-	$(V1) $(ANDROID) $(ANDROID_SILENT) update project \
-		--target "Google Inc.:Google APIs:$(GOOGLE_API_VERSION)" \
-		--name androidgcs \
-		--path ./androidgcs
-	$(V1) $(ANT) -f ./androidgcs/build.xml \
-		$(ANT_QUIET) \
-		-Dout.dir="../$(call toprel, $(ANDROIDGCS_OUT_DIR)/bin)" \
-		-Dgen.absolute.dir="$(ANDROIDGCS_OUT_DIR)/gen" \
-		$(ANDROIDGCS_BUILD_CONF)
-
-.PHONY: androidgcs_clean
-androidgcs_clean:
-	@$(ECHO) " CLEAN      $(call toprel, $(ANDROIDGCS_OUT_DIR))"
-	$(V1) [ ! -d "$(ANDROIDGCS_OUT_DIR)" ] || $(RM) -r "$(ANDROIDGCS_OUT_DIR)"
-
 # We want to take snapshots of the UAVOs at each point that they change
 # to allow the GCS to be compatible with as many versions as possible.
 # We always include a pseudo collection called "srctree" which represents
@@ -693,8 +634,7 @@ ALL_UNITTESTS := logfs math lednotification
 
 # Build the directory for the unit tests
 UT_OUT_DIR := $(BUILD_DIR)/unit_tests
-$(UT_OUT_DIR):
+DIRS += $(UT_OUT_DIR)
-	$(V1) $(MKDIR) -p $@
 
 .PHONY: all_ut
 all_ut: $(addsuffix _elf, $(addprefix ut_, $(ALL_UNITTESTS)))
@@ -769,14 +709,13 @@ OPFW_CONTENTS := \
 .PHONY: opfw_resource
 opfw_resource: $(OPFW_RESOURCE)
 
-$(OPFW_RESOURCE): $(FW_TARGETS)
+$(OPFW_RESOURCE): $(FW_TARGETS) | $(OPGCSSYNTHDIR)
 	@$(ECHO) Generating OPFW resource file $(call toprel, $@)
-	$(V1) $(MKDIR) -p $(dir $@)
 	$(V1) $(ECHO) $(QUOTE)$(OPFW_CONTENTS)$(QUOTE) > $@
 
 # If opfw_resource or all firmware are requested, GCS should depend on the resource
 ifneq ($(strip $(filter opfw_resource all all_fw all_flight package,$(MAKECMDGOALS))),)
-    $(eval openpilotgcs_qmake: $(OPFW_RESOURCE))
+$(OPENPILOTGCS_MAKEFILE): $(OPFW_RESOURCE)
 endif
 
 # Packaging targets: package
@@ -785,19 +724,15 @@ endif
 #  - calls paltform-specific packaging script
 
 # Define some variables
-export PACKAGE_LBL  := $(shell $(VERSION_INFO) --format=\$${LABEL})
+PACKAGE_LBL       := $(shell $(VERSION_INFO) --format=\$${LABEL})
-export PACKAGE_NAME := OpenPilot
+PACKAGE_NAME      := OpenPilot
-export PACKAGE_SEP  := -
+PACKAGE_SEP       := -
-
+PACKAGE_FULL_NAME := $(PACKAGE_NAME)$(PACKAGE_SEP)$(PACKAGE_LBL)
-.PHONY: package
 
 include $(ROOT_DIR)/package/$(UNAME).mk
 
-package: all_fw all_ground uavobjects_matlab $(PACKAGE_DIR)
+# Source distribution is never dirty because it uses git archive
-ifneq ($(GCS_BUILD_CONF),release)
+DIST_NAME := $(DIST_DIR)/$(subst dirty-,,$(PACKAGE_FULL_NAME)).tar
-	# We can only package release builds
-	$(error Packaging is currently supported for release builds only)
-endif
 
 ##############################
 #
@@ -866,9 +801,8 @@ docs_all_clean:
 ##############################
 
 .PHONY: build-info
-build-info:
+build-info: | $(BUILD_DIR)
 	@$(ECHO) " BUILD-INFO $(call toprel, $(BUILD_DIR)/$@.txt)"
-	$(V1) $(MKDIR) -p $(BUILD_DIR)
 	$(V1) $(VERSION_INFO) \
 		--uavodir=$(ROOT_DIR)/shared/uavobjectdefinition \
 		--template="make/templates/$@.txt" \
@@ -882,20 +816,31 @@ build-info:
 
 DIST_VER_INFO := $(DIST_DIR)/version-info.json
 
-.PHONY: $(DIST_VER_INFO) # Because to many deps to list
+$(DIST_VER_INFO): .git/index | $(DIST_DIR)
-$(DIST_VER_INFO): $(DIST_DIR)
 	$(V1) $(VERSION_INFO) --jsonpath="$(DIST_DIR)"
 
-.PHONY: dist
+
-dist: $(DIST_DIR) $(DIST_VER_INFO)
+$(DIST_NAME).gz: $(DIST_VER_INFO) .git/index | $(DIST_DIR)
-	@$(ECHO) " SOURCE FOR DISTRIBUTION $(call toprel, $(DIST_DIR))"
+	@$(ECHO) " SOURCE FOR DISTRIBUTION $(call toprel, $(DIST_NAME).gz)"
-	$(eval DIST_NAME := $(call toprel, "$(DIST_DIR)/OpenPilot-$(shell git describe).tar"))
+	$(V1) git archive --prefix="$(PACKAGE_NAME)/" -o "$(DIST_NAME)" HEAD
-	$(V1) git archive --prefix="OpenPilot/" -o "$(DIST_NAME)" HEAD
 	$(V1) tar --append --file="$(DIST_NAME)" \
-		--transform='s,.*version-info.json,OpenPilot/version-info.json,' \
+		--transform='s,.*version-info.json,$(PACKAGE_NAME)/version-info.json,' \
 		$(call toprel, "$(DIST_VER_INFO)")
 	$(V1) gzip -f "$(DIST_NAME)"
 
+.PHONY: dist
+dist: $(DIST_NAME).gz
+
+
+##############################
+#
+# Directories
+#
+##############################
+
+$(DIRS):
+	$(V1) $(MKDIR) -p $@
+
 
 ##############################
 #
@@ -930,7 +875,6 @@ help:
 	@$(ECHO) "     openocd_install      - Install the OpenOCD JTAG daemon"
 	@$(ECHO) "     stm32flash_install   - Install the stm32flash tool for unbricking F1-based boards"
 	@$(ECHO) "     dfuutil_install      - Install the dfu-util tool for unbricking F4-based boards"
-	@$(ECHO) "     android_sdk_install  - Install the Android SDK tools"
 	@$(ECHO) "   Install all available tools:"
 	@$(ECHO) "     all_sdk_install      - Install all of above (platform-dependent)"
 	@$(ECHO) "     build_sdk_install    - Install only essential for build tools (platform-dependent)"
@@ -1002,26 +946,19 @@ help:
 	@$(ECHO)
 	@$(ECHO) "   [GCS]"
 	@$(ECHO) "     gcs                  - Build the Ground Control System (GCS) application (debug|release)"
-	@$(ECHO) "                            Skip qmake: QMAKE_SKIP=1"
 	@$(ECHO) "                            Compile specific directory: MAKE_DIR=<dir>"
-	@$(ECHO) "                            Example: make gcs QMAKE_SKIP=1 MAKE_DIR=src/plugins/coreplugin"
+	@$(ECHO) "                            Example: make gcs MAKE_DIR=src/plugins/coreplugin"
+	@$(ECHO) "     gcs_qmake            - Run qmake for the Ground Control System (GCS) application (debug|release)"
 	@$(ECHO) "     gcs_clean            - Remove the Ground Control System (GCS) application (debug|release)"
 	@$(ECHO) "                            Supported build configurations: GCS_BUILD_CONF=debug|release (default is $(GCS_BUILD_CONF))"
 	@$(ECHO)
 	@$(ECHO) "   [Uploader Tool]"
 	@$(ECHO) "     uploader             - Build the serial uploader tool (debug|release)"
-	@$(ECHO) "                            Skip qmake: QMAKE_SKIP=1"
+	@$(ECHO) "     uploader_qmake       - Run qmake for the serial uploader tool (debug|release)"
-	@$(ECHO) "                            Example: make uploader QMAKE_SKIP=1"
 	@$(ECHO) "     uploader_clean       - Remove the serial uploader tool (debug|release)"
 	@$(ECHO) "                            Supported build configurations: GCS_BUILD_CONF=debug|release (default is $(GCS_BUILD_CONF))"
 	@$(ECHO)
 	@$(ECHO)
-	@$(ECHO) "   [AndroidGCS]"
-	@$(ECHO) "     androidgcs           - Build the Android Ground Control System (GCS) application"
-	@$(ECHO) "     androidgcs_install   - Use ADB to install the Android GCS application"
-	@$(ECHO) "     androidgcs_run       - Run the Android GCS application"
-	@$(ECHO) "     androidgcs_clean     - Remove the Android GCS application"
-	@$(ECHO)
 	@$(ECHO) "   [UAVObjects]"
 	@$(ECHO) "     uavobjects           - Generate source files from the UAVObject definition XML files"
 	@$(ECHO) "     uavobjects_test      - Parse xml-files - check for valid, duplicate ObjId's, ..."

WHATSNEW.txt

@@ -1,3 +1,86 @@
+Release Notes - OpenPilot - Version RELEASE-15.05
+Fully autonomous flight is now possible with autotakeoff and landing as flight modes available to path plans.  
+An all new implementation of altitude vario provides improved altitude maintenance and smoother flight.
+A new RateTrainer mode for beginners and aerial photography make it easier to learn this mode my limiting the pitch and roll extents.
+4CH transmitters for multirotor and 2CH for ground is now supported.
+It is now easier to takeoff with an improved axislock on yaw implementation.
+Full speed flight just got faster with new motor constraints that maintain your ability to enact roll without requiring a upper throttle limit.
+Setup on GPS just got easier. See the wiki for details.
+New control protocols and support for due telemtry and OSD programing have been added.
+
+The full list of bugfixes in this release is accessible here:
+
+** Bug
+    * [OP-1691] - PIOS_DELAY_WaitmS() in RFM22B causes jitter
+    * [OP-1756] - Add option to Vehicle Setup Wizard to calibrate all motor outputs at the same time.
+    * [OP-1778] - resolve win_sdk_install.sh issue with https://
+    * [OP-1793] - Fixes for Sensor module
+    * [OP-1794] - AxisLock windup not cleared with low throttle while armed
+    * [OP-1834] - Piro Comp - adverse effect in Atti/Ratti Modes
+    * [OP-1841] - Serial telemetry is not reliable 
+    * [OP-1855] - Limit parsing error in logs when starting GCS
+    * [OP-1858] - PathPlanner AutoTakeoff fixes
+    * [OP-1867] - PathPlanner AutoLand simplification
+    * [OP-1869] - Allow Analog Airspeed scale
+    * [OP-1872] - Vehicle Wiz Tricopter tail servo settings don't save
+
+
+** Improvement
+    * [OP-1289] - Need Revo to send two telemetry streams for OSD and GCS
+    * [OP-1734] - Clarify the need of reversing servo in FW vehicle wizard
+    * [OP-1736] - Make package label something more meaningful than the date.
+    * [OP-1739] - Add GNSS (GPS/GLONASS) selection to UBX autoconf
+    * [OP-1750] - Revo state estimation CPU optimization
+    * [OP-1776] - Performance optimizations for UAVTalk telemetry
+    * [OP-1783] - Fall back PathPlanner flight mode to PH with config warning if no plan
+    * [OP-1791] - Change the description of the stabilization modes 
+    * [OP-1796] - Upgrade GCS to qt 5.4.1
+    * [OP-1797] - Improve GCS workspace layout management reactivity
+    * [OP-1798] - GCS ophid plugin is too verbose
+    * [OP-1802] - Throttle filterstationary fake pos/vel data rate
+    * [OP-1808] - Make the flight mode switch step in transmitter setup wizard to be optional
+    * [OP-1814] - Reset Button for mAH used
+    * [OP-1828] - Changes ADC module to support other pins as optional analog inputs
+    * [OP-1835] - Add motor constraints in place of overhead throttle buffer for enhanced stability and power.
+    * [OP-1837] - Add support for Multiplex SRXL protocol
+    * [OP-1840] - GPS serlal port needed features
+    * [OP-1844] - Create a vagrant environment that contains all the bits for Android development, including Android. 
+    * [OP-1848] - Rewrite AltVario/Hold in C++ for functional improvements
+    * [OP-1852] - Include version number in window title bar.
+    * [OP-1874] - Various improvements to led notifications
+
+** New Feature
+    * [OP-1696] - PathFollower C++ Rewrite: Autonomous Landing, Velocity Roam, RTBL, GroundPathFollower
+    * [OP-1760] - AutoTakeoff
+    * [OP-1769] - Support Ground Vehicles with 2CH receiver and reversible motor
+    * [OP-1781] - Ground Input Channel Configuration
+    * [OP-1803] - Create UAVTalk objects for receiver signal quality
+    * [OP-1818] - Update OP toolchain for compiling osgearth
+    * [OP-1832] - Need method to get default UAV Object settings in Java UAVObjects
+    * [OP-1849] - Support programming/update of minimosd using USB VCP port
+    * [OP-1863] - RateTrainer mode - add maxpitch for beginners and aerial photography
+
+
+
+--- RELEASE-15.02.02 ---
+This release fixes a bug that prevents revo onboard mag to work correctly.
+
+Release Notes - OpenPilot - Version RELEASE-15.02.02
+
+The full list of bugfixes in this release is accessible here:
+https://progress.openpilot.org/issues/?filter=12262
+
+** Bug
+    * [OP-1820] - fix onboard mag orientation
+    * [OP-1821] - Tricopter tail servo wrong speed on wizard
+    * [OP-1827] - Version ID wrong in Windows uninstaller
+    * [OP-1857] - PPM on Flexi does not work on CC/CC3D
+
+** Task
+    * [OP-1831] - due to oneshot higher pid values ki now shows "red" warning in stabilization page 
+
+
+
 --- RELEASE-15.02.01 ---
 This release fixes an in important bug that may prevent failsafe to work correctly using CC3D/CC with a PWM receiver.
 

flight/libraries/inc/CoordinateConversions.h

@@ -112,7 +112,7 @@ static inline void matrix_mult_3x3f(float a[3][3], float b[3][3], float result[3
     result[2][2] = a[0][2] * b[2][0] + a[1][2] * b[2][1] + a[2][2] * b[2][2];
 }
 
-inline void matrix_inline_scale_3f(float a[3][3], float scale)
+static inline void matrix_inline_scale_3f(float a[3][3], float scale)
 {
     a[0][0] *= scale;
     a[0][1] *= scale;
@@ -127,7 +127,7 @@ inline void matrix_inline_scale_3f(float a[3][3], float scale)
     a[2][2] *= scale;
 }
 
-inline void rot_about_axis_x(const float rotation, float R[3][3])
+static inline void rot_about_axis_x(const float rotation, float R[3][3])
 {
     float s = sinf(rotation);
     float c = cosf(rotation);
@@ -145,7 +145,7 @@ inline void rot_about_axis_x(const float rotation, float R[3][3])
     R[2][2] = c;
 }
 
-inline void rot_about_axis_y(const float rotation, float R[3][3])
+static inline void rot_about_axis_y(const float rotation, float R[3][3])
 {
     float s = sinf(rotation);
     float c = cosf(rotation);
@@ -163,7 +163,7 @@ inline void rot_about_axis_y(const float rotation, float R[3][3])
     R[2][2] = c;
 }
 
-inline void rot_about_axis_z(const float rotation, float R[3][3])
+static inline void rot_about_axis_z(const float rotation, float R[3][3])
 {
     float s = sinf(rotation);
     float c = cosf(rotation);

flight/libraries/inc/paths.h

@@ -34,6 +34,6 @@ struct path_status {
     float correction_vector[3];
 };
 
-void path_progress(PathDesiredData *path, float *cur_point, struct path_status *status);
+void path_progress(PathDesiredData *path, float *cur_point, struct path_status *status, bool mode3D);
 
 #endif

flight/libraries/inc/plans.h

@@ -62,27 +62,47 @@ void plan_setup_returnToBase();
  * @brief setup pathplanner/pathfollower for land
  */
 void plan_setup_land();
-
+void plan_setup_AutoTakeoff();
-/**
- * @brief execute land
- */
-void plan_run_land();
 
 /**
  * @brief setup pathplanner/pathfollower for braking
  */
-void plan_setup_assistedcontrol(uint8_t timeout_occurred);
+void plan_setup_assistedcontrol();
 
 #define PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_NORTH 0
 #define PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_EAST  1
 #define PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_DOWN  2
 #define PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT                   3
 
+#define PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_NORTH   0
+#define PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_EAST    1
+#define PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_DOWN    2
+#define PATHDESIRED_MODEPARAMETER_VELOCITY_UNUSED                 3
+
+#define PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_NORTH       0
+#define PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_EAST        1
+#define PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_DOWN        2
+#define PATHDESIRED_MODEPARAMETER_LAND_OPTIONS                    3
+
+#define PATHDESIRED_MODEPARAMETER_LAND_OPTION_NONE                0
+#define PATHDESIRED_MODEPARAMETER_LAND_OPTION_HORIZONTAL_PH       1
+
+#define PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_NEXTCOMMAND        0
+#define PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED1            1
+#define PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED2            2
+#define PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED3            3
+
+#define PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_NORTH               0
+#define PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_EAST                1
+#define PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_DOWN                2
+#define PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE        3
+
 /**
  * @brief setup pathfollower for positionvario
  */
 void plan_setup_CourseLock();
 void plan_setup_PositionRoam();
+void plan_setup_VelocityRoam();
 void plan_setup_HomeLeash();
 void plan_setup_AbsolutePosition();
 
@@ -91,8 +111,10 @@ void plan_setup_AbsolutePosition();
  */
 void plan_run_CourseLock();
 void plan_run_PositionRoam();
+void plan_run_VelocityRoam();
 void plan_run_HomeLeash();
 void plan_run_AbsolutePosition();
+void plan_run_AutoTakeoff();
 
 /**
  * @brief setup pathplanner/pathfollower for AutoCruise

flight/libraries/inc/stopwatch.h

@@ -47,7 +47,7 @@ typedef struct {
 /////////////////////////////////////////////////////////////////////////////
 
 
-inline int32_t STOPWATCH_Init(uint32_t resolution, stopwatch_t *stopwatch)
+static inline int32_t STOPWATCH_Init(uint32_t resolution, stopwatch_t *stopwatch)
 {
     stopwatch->raw = PIOS_DELAY_GetRaw();
     stopwatch->resolution = resolution;
@@ -58,7 +58,7 @@ inline int32_t STOPWATCH_Init(uint32_t resolution, stopwatch_t *stopwatch)
 // ! Resets the stopwatch
 // ! \return < 0 on errors
 /////////////////////////////////////////////////////////////////////////////
-inline int32_t STOPWATCH_Reset(stopwatch_t *stopwatch)
+static inline int32_t STOPWATCH_Reset(stopwatch_t *stopwatch)
 {
     stopwatch->raw = PIOS_DELAY_GetRaw();
     return 0; // no error
@@ -68,7 +68,7 @@ inline int32_t STOPWATCH_Reset(stopwatch_t *stopwatch)
 // ! Returns current value of stopwatch
 // ! \return stopwatch value
 /////////////////////////////////////////////////////////////////////////////
-inline uint32_t STOPWATCH_ValueGet(stopwatch_t *stopwatch)
+static inline uint32_t STOPWATCH_ValueGet(stopwatch_t *stopwatch)
 {
     uint32_t value = PIOS_DELAY_GetuSSince(stopwatch->raw);
 

flight/libraries/insgps13state.c

@@ -33,25 +33,26 @@
 #include <math.h>
 #include <stdint.h>
 #include <pios_math.h>
+#include <mathmisc.h>
 
 // constants/macros/typdefs
 #define NUMX 13 // number of states, X is the state vector
 #define NUMW 9 // number of plant noise inputs, w is disturbance noise vector
 #define NUMV 10 // number of measurements, v is the measurement noise vector
 #define NUMU 6 // number of deterministic inputs, U is the input vector
-
+#pragma GCC optimize "O3"
 // Private functions
 void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
                           float Q[NUMW], float dT, float P[NUMX][NUMX]);
-void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
+static void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
-                  float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
+                         float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
-                  uint16_t SensorsUsed);
+                         uint16_t SensorsUsed);
-void RungeKutta(float X[NUMX], float U[NUMU], float dT);
+static void RungeKutta(float X[NUMX], float U[NUMU], float dT);
-void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]);
+static void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]);
-void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
+static void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
-                 float G[NUMX][NUMW]);
+                        float G[NUMX][NUMW]);
-void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV]);
+static void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV]);
-void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]);
+static void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]);
 
 // Private variables
 
@@ -60,29 +61,29 @@ void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]);
 // LinearizeFG() and LinearizeH():
 //
 // usage F:        usage G:   usage H:
-// 0123456789abc  012345678  0123456789abc
+// -0123456789abc  012345678  0123456789abc
 // 0...X.........  .........  X............
 // 1....X........  .........  .X...........
 // 2.....X.......  .........  ..X..........
 // 3......XXXX...  ...XXX...  ...X.........
 // 4......XXXX...  ...XXX...  ....X........
 // 5......XXXX...  ...XXX...  .....X.......
-// 6.......XXXXXX  XXX......  ......XXXX...
+// 6.....ooXXXXXX  XXX......  ......XXXX...
-// 7......X.XXXXX  XXX......  ......XXXX...
+// 7.....oXoXXXXX  XXX......  ......XXXX...
-// 8......XX.XXXX  XXX......  ......XXXX...
+// 8.....oXXoXXXX  XXX......  ......XXXX...
-// 9......XXX.XXX  XXX......  ..X..........
+// 9.....oXXXoXXX  XXX......  ..X..........
-// a.............  ......X..
+// a.............  ......Xoo
-// b.............  .......X.
+// b.............  ......oXo
-// c.............  ........X
+// c.............  ......ooX
 
-static const int8_t FrowMin[NUMX] = { 3, 4, 5, 6, 6, 6, 7, 6, 6, 6, 13, 13, 13 };
+static int8_t FrowMin[NUMX] = { 3, 4, 5, 6, 6, 6, 5, 5, 5, 5, 13, 13, 13 };
-static const int8_t FrowMax[NUMX] = { 3, 4, 5, 9, 9, 9, 12, 12, 12, 12, -1, -1, -1 };
+static int8_t FrowMax[NUMX] = { 3, 4, 5, 9, 9, 9, 12, 12, 12, 12, -1, -1, -1 };
 
-static const int8_t GrowMin[NUMX] = { 9, 9, 9, 3, 3, 3, 0, 0, 0, 0, 6, 7, 8 };
+static int8_t GrowMin[NUMX] = { 9, 9, 9, 3, 3, 3, 0, 0, 0, 0, 6, 7, 8 };
-static const int8_t GrowMax[NUMX] = { -1, -1, -1, 5, 5, 5, 2, 2, 2, 2, 6, 7, 8 };
+static int8_t GrowMax[NUMX] = { -1, -1, -1, 5, 5, 5, 2, 2, 2, 2, 6, 7, 8 };
 
-static const int8_t HrowMin[NUMV] = { 0, 1, 2, 3, 4, 5, 6, 6, 6, 2 };
+static int8_t HrowMin[NUMV] = { 0, 1, 2, 3, 4, 5, 6, 6, 6, 2 };
-static const int8_t HrowMax[NUMV] = { 0, 1, 2, 3, 4, 5, 9, 9, 9, 2 };
+static int8_t HrowMax[NUMV] = { 0, 1, 2, 3, 4, 5, 9, 9, 9, 2 };
 
 static struct EKFData {
     // linearized system matrices
@@ -182,8 +183,8 @@ void INSGetP(float PDiag[NUMX])
     uint8_t i;
 
     // retrieve diagonal elements (aka state variance)
-    for (i = 0; i < NUMX; i++) {
+    if (PDiag != 0) {
-        if (PDiag != 0) {
+        for (i = 0; i < NUMX; i++) {
             PDiag[i] = ekf.P[i][i];
         }
     }
@@ -289,7 +290,7 @@ void INSSetMagNorth(float B[3])
 void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT)
 {
     float U[6];
-    float qmag;
+    float invqmag;
 
     // rate gyro inputs in units of rad/s
     U[0] = gyro_data[0];
@@ -304,11 +305,11 @@ void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT)
     // EKF prediction step
     LinearizeFG(ekf.X, U, ekf.F, ekf.G);
     RungeKutta(ekf.X, U, dT);
-    qmag      = sqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]);
+    invqmag   = fast_invsqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]);
-    ekf.X[6] /= qmag;
+    ekf.X[6] *= invqmag;
-    ekf.X[7] /= qmag;
+    ekf.X[7] *= invqmag;
-    ekf.X[8] /= qmag;
+    ekf.X[8] *= invqmag;
-    ekf.X[9] /= qmag;
+    ekf.X[9] *= invqmag;
     // CovariancePrediction(ekf.F,ekf.G,ekf.Q,dT,ekf.P);
 
     // Update Nav solution structure
@@ -373,8 +374,8 @@ void VelBaroCorrection(float Vel[3], float BaroAlt)
 void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
                    float BaroAlt, uint16_t SensorsUsed)
 {
-    float Z[10], Y[10];
+    float Z[10] = { 0 };
-    float Bmag, qmag;
+    float Y[10] = { 0 };
 
     // GPS Position in meters and in local NED frame
     Z[0] = Pos[0];
@@ -385,14 +386,15 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
     Z[3] = Vel[0];
     Z[4] = Vel[1];
     Z[5] = Vel[2];
-
     // magnetometer data in any units (use unit vector) and in body frame
-    Bmag =
+
-        sqrtf(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] +
+
-              mag_data[2] * mag_data[2]);
+    if (SensorsUsed & MAG_SENSORS) {
-    Z[6] = mag_data[0] / Bmag;
+        float invBmag = fast_invsqrtf(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] + mag_data[2] * mag_data[2]);
-    Z[7] = mag_data[1] / Bmag;
+        Z[6] = mag_data[0] * invBmag;
-    Z[8] = mag_data[2] / Bmag;
+        Z[7] = mag_data[1] * invBmag;
+        Z[8] = mag_data[2] * invBmag;
+    }
 
     // barometric altimeter in meters and in local NED frame
     Z[9] = BaroAlt;
@@ -401,12 +403,12 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
     LinearizeH(ekf.X, ekf.Be, ekf.H);
     MeasurementEq(ekf.X, ekf.Be, Y);
     SerialUpdate(ekf.H, ekf.R, Z, Y, ekf.P, ekf.X, SensorsUsed);
-    qmag       = sqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]);
-    ekf.X[6]  /= qmag;
-    ekf.X[7]  /= qmag;
-    ekf.X[8]  /= qmag;
-    ekf.X[9]  /= qmag;
 
+    float invqmag = fast_invsqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]);
+    ekf.X[6]  *= invqmag;
+    ekf.X[7]  *= invqmag;
+    ekf.X[8]  *= invqmag;
+    ekf.X[9]  *= invqmag;
     // Update Nav solution structure
     Nav.Pos[0] = ekf.X[0];
     Nav.Pos[1] = ekf.X[1];
@@ -434,60 +436,78 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
 // The first Method is very specific to this implementation
 // ************************************************
 
-__attribute__((optimize("O3")))
 void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
                           float Q[NUMW], float dT, float P[NUMX][NUMX])
 {
     // Pnew = (I+F*T)*P*(I+F*T)' + (T^2)*G*Q*G' = (T^2)[(P/T + F*P)*(I/T + F') + G*Q*G')]
 
-    float dT1  = 1.0f / dT; // multiplication is faster than division on fpu.
+    const float dT1  = 1.0f / dT; // multiplication is faster than division on fpu.
-    float dTsq = dT * dT;
+    const float dTsq = dT * dT;
 
     float Dummy[NUMX][NUMX];
     int8_t i;
+    int8_t k;
 
     for (i = 0; i < NUMX; i++) { // Calculate Dummy = (P/T +F*P)
-        float *Firow   = F[i];
+        float *Firow = F[i];
-        float *Pirow   = P[i];
+        float *Pirow = P[i];
-        float *Dirow   = Dummy[i];
+        float *Dirow = Dummy[i];
-        int8_t Fistart = FrowMin[i];
+        const int8_t Fistart = FrowMin[i];
-        int8_t Fiend   = FrowMax[i];
+        const int8_t Fiend   = FrowMax[i];
         int8_t j;
+
         for (j = 0; j < NUMX; j++) {
             Dirow[j] = Pirow[j] * dT1; // Dummy = P / T ...
-            int8_t k;
+        }
-            for (k = Fistart; k <= Fiend; k++) {
+        for (k = Fistart; k <= Fiend; k++) {
+            for (j = 0; j < NUMX; j++) {
                 Dirow[j] += Firow[k] * P[k][j]; // [] + F * P
             }
         }
     }
     for (i = 0; i < NUMX; i++) { // Calculate Pnew = (T^2) [Dummy/T + Dummy*F' + G*Qw*G']
-        float *Dirow   = Dummy[i];
+        float *Dirow = Dummy[i];
-        float *Girow   = G[i];
+        float *Girow = G[i];
-        float *Pirow   = P[i];
+        float *Pirow = P[i];
-        int8_t Gistart = GrowMin[i];
+        const int8_t Gistart = GrowMin[i];
-        int8_t Giend   = GrowMax[i];
+        const int8_t Giend   = GrowMax[i];
         int8_t j;
-        for (j = i; j < NUMX; j++) { // Use symmetry, ie only find upper triangular
-            float Ptmp = Dirow[j] * dT1; // Pnew = Dummy / T ...
 
-            {
+
-                float *Fjrow   = F[j];
+        for (j = i; j < NUMX; j++) { // Use symmetry, ie only find upper triangular
-                int8_t Fjstart = FrowMin[j];
+            float Ptmp         = Dirow[j] * dT1; // Pnew = Dummy / T ...
-                int8_t Fjend   = FrowMax[j];
+
-                int8_t k;
+            const float *Fjrow = F[j];
-                for (k = Fjstart; k <= Fjend; k++) {
+            int8_t Fjstart     = FrowMin[j];
-                    Ptmp += Dirow[k] * Fjrow[k]; // [] + Dummy*F' ...
+            int8_t Fjend       = FrowMax[j];
-                }
+            k = Fjstart;
+
+            while (k <= Fjend - 3) {
+                Ptmp += Dirow[k] * Fjrow[k]; // [] + Dummy*F' ...
+                Ptmp += Dirow[k + 1] * Fjrow[k + 1];
+                Ptmp += Dirow[k + 2] * Fjrow[k + 2];
+                Ptmp += Dirow[k + 3] * Fjrow[k + 3];
+                k    += 4;
+            }
+            while (k <= Fjend) {
+                Ptmp += Dirow[k] * Fjrow[k];
+                k++;
             }
 
-            {
+            float *Gjrow = G[j];
-                float *Gjrow   = G[j];
+            const int8_t Gjstart = MAX(Gistart, GrowMin[j]);
-                int8_t Gjstart = MAX(Gistart, GrowMin[j]);
+            const int8_t Gjend   = MIN(Giend, GrowMax[j]);
-                int8_t Gjend   = MIN(Giend, GrowMax[j]);
+            k = Gjstart;
-                int8_t k;
+            while (k <= Gjend - 2) {
-                for (k = Gjstart; k <= Gjend; k++) {
+                Ptmp += Q[k] * Girow[k] * Gjrow[k]; // [] + G*Q*G' ...
-                    Ptmp += Q[k] * Girow[k] * Gjrow[k]; // [] + G*Q*G' ...
+                Ptmp += Q[k + 1] * Girow[k + 1] * Gjrow[k + 1];
+                Ptmp += Q[k + 2] * Girow[k + 2] * Gjrow[k + 2];
+                k    += 3;
+            }
+            if (k <= Gjend) {
+                Ptmp += Q[k] * Girow[k] * Gjrow[k];
+                if (k <= Gjend - 1) {
+                    Ptmp += Q[k + 1] * Girow[k + 1] * Gjrow[k + 1];
                 }
             }
 
@@ -511,7 +531,6 @@ void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
 // The SensorsUsed variable is a bitwise mask indicating which sensors
 // should be used in the update.
 // ************************************************
-
 void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
                   float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
                   uint16_t SensorsUsed)
@@ -524,7 +543,10 @@ void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
         if (SensorsUsed & (0x01 << m)) { // use this sensor for update
             for (j = 0; j < NUMX; j++) { // Find Hp = H*P
                 HP[j] = 0;
-                for (k = HrowMin[m]; k <= HrowMax[m]; k++) {
+            }
+
+            for (k = HrowMin[m]; k <= HrowMax[m]; k++) {
+                for (j = 0; j < NUMX; j++) { // Find Hp = H*P
                     HP[j] += H[m][k] * P[k][j];
                 }
             }
@@ -532,14 +554,13 @@ void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
             for (k = HrowMin[m]; k <= HrowMax[m]; k++) {
                 HPHR += HP[k] * H[m][k];
             }
-
+            float invHPHR = 1.0f / HPHR;
             for (k = 0; k < NUMX; k++) {
-                Km[k] = HP[k] / HPHR; // find K = HP/HPHR
+                Km[k] = HP[k] * invHPHR; // find K = HP/HPHR
             }
             for (i = 0; i < NUMX; i++) { // Find P(m)= P(m-1) + K*HP
                 for (j = i; j < NUMX; j++) {
-                    P[i][j] = P[j][i] =
+                    P[i][j] = P[j][i] = P[i][j] - Km[i] * HP[j];
-                                  P[i][j] - Km[i] * HP[j];
                 }
             }
 
@@ -560,8 +581,8 @@ void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
 
 void RungeKutta(float X[NUMX], float U[NUMU], float dT)
 {
-    float dT2 =
+    const float dT2 = dT / 2.0f;
-        dT / 2.0f, K1[NUMX], K2[NUMX], K3[NUMX], K4[NUMX], Xlast[NUMX];
+    float K1[NUMX], K2[NUMX], K3[NUMX], K4[NUMX], Xlast[NUMX];
     uint8_t i;
 
     for (i = 0; i < NUMX; i++) {
@@ -585,7 +606,7 @@ void RungeKutta(float X[NUMX], float U[NUMU], float dT)
     for (i = 0; i < NUMX; i++) {
         X[i] =
             Xlast[i] + dT * (K1[i] + 2.0f * K2[i] + 2.0f * K3[i] +
-                             K4[i]) / 6.0f;
+                             K4[i]) * (1.0f / 6.0f);
     }
 }
 
@@ -608,7 +629,7 @@ void RungeKutta(float X[NUMX], float U[NUMU], float dT)
 // H is output of LinearizeH(), all elements not set should be zero
 // ************************************************
 
-void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX])
+static void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX])
 {
     float ax, ay, az, wx, wy, wz, q0, q1, q2, q3;
 

flight/libraries/math/pid.c

@@ -143,3 +143,95 @@ void pid_configure(struct pid *pid, float p, float i, float d, float iLim)
     pid->d    = d;
     pid->iLim = iLim;
 }
+
+
+/**
+ * Configure the settings for a pid2 structure
+ * @param[out] pid The PID2 structure to configure
+ * @param[in] kp proportional gain
+ * @param[in] ki integral gain.  Time constant Ti = kp/ki
+ * @param[in] kd derivative gain. Time constant Td = kd/kp
+ * @param[in] Tf filtering time = (kd/k)/N, N is in the range of 2 to 20
+ * @param[in] kt tracking gain for anti-windup. Tt = √TiTd and Tt = (Ti + Td)/2
+ * @param[in] dt delta time increment
+ * @param[in] beta setpoint weight on setpoint in P component.  beta=1 error feedback. beta=0 smoothes out response to changes in setpoint
+ * @param[in] u0 initial output for r=y at activation to achieve bumpless transfer
+ * @param[in] va constant for compute of actuator output for check against limits for antiwindup
+ * @param[in] vb multiplier for compute of actuator output for check against limits for anti-windup
+ */
+void pid2_configure(struct pid2 *pid, float kp, float ki, float kd, float Tf, float kt, float dT, float beta, float u0, float va, float vb)
+{
+    pid->reconfigure = true;
+    pid->u0   = u0;
+    pid->va   = va;
+    pid->vb   = vb;
+    pid->kp   = kp;
+    pid->beta = beta; // setpoint weight on proportional term
+
+    pid->bi   = ki * dT;
+    pid->br   = kt * dT / vb;
+
+    pid->ad   = Tf / (Tf + dT);
+    pid->bd   = kd / (Tf + dT);
+}
+
+/**
+ * Achieve a bumpless transfer and trigger initialisation of I term
+ * @param[out] pid The PID structure to configure
+ * @param[in] u0 initial output for r=y at activation to achieve bumpless transfer
+ */
+void pid2_transfer(struct pid2 *pid, float u0)
+{
+    pid->reconfigure = true;
+    pid->u0 = u0;
+}
+
+/**
+ * pid controller with setpoint weighting, anti-windup, with a low-pass filtered derivative on the process variable
+ * See "Feedback Systems" for an explanation
+ * @param[out] pid The PID structure to configure
+ * @param[in] r setpoint
+ * @param[in] y process variable
+ * @param[in] ulow lower limit on actuator
+ * @param[in] uhigh upper limit on actuator
+ */
+float pid2_apply(
+    struct pid2 *pid,
+    const float r,
+    const float y,
+    const float ulow,
+    const float uhigh)
+{
+    // on reconfigure ensure bumpless transfer
+    // http://www.controlguru.com/2008/021008.html
+    if (pid->reconfigure) {
+        pid->reconfigure = false;
+
+        // initialise derivative terms
+        pid->yold = y;
+        pid->D    = 0.0f;
+
+        // t=0, u=u0, y=y0, v=u
+        pid->I    = (pid->u0 - pid->va) / pid->vb - pid->kp * (pid->beta * r - y);
+    }
+
+    // compute proportional part
+    pid->P = pid->kp * (pid->beta * r - y);
+
+    // update derivative part
+    pid->D = pid->ad * pid->D - pid->bd * (y - pid->yold);
+
+    // compute temporary output
+    float v = pid->va + pid->vb * (pid->P + pid->I + pid->D);
+
+    // simulate actuator saturation
+    float u = boundf(v, ulow, uhigh);
+
+    // update integral
+    pid->I    = pid->I + pid->bi * (r - y) + pid->br * (u - v);
+
+    // update old process output
+    pid->yold = y;
+
+    return u;
+}

flight/libraries/math/pid.h

@@ -44,7 +44,25 @@ struct pid {
     float lastDer;
 };
 
-typedef struct pid_scaler {
+// pid2 structure for a PID+setpoint weighting, anti-windup and filtered derivative control
+struct pid2 {
+    float   u0;
+    float   va;
+    float   vb;
+    float   kp;
+    float   bi;
+    float   ad;
+    float   bd;
+    float   br;
+    float   beta;
+    float   yold;
+    float   P;
+    float   I;
+    float   D;
+    uint8_t reconfigure;
+};
+
+typedef struct pid_scaler_s {
     float p;
     float i;
     float d;
@@ -57,4 +75,9 @@ void pid_zero(struct pid *pid);
 void pid_configure(struct pid *pid, float p, float i, float d, float iLim);
 void pid_configure_derivative(float cutoff, float gamma);
 
+// Methods for use with pid2 structure
+void pid2_configure(struct pid2 *pid, float kp, float ki, float kd, float Tf, float kt, float dT, float beta, float u0, float va, float vb);
+void pid2_transfer(struct pid2 *pid, float u0);
+float pid2_apply(struct pid2 *pid, const float r, const float y, float ulow, float uhigh);
+
 #endif /* PID_H */

flight/libraries/paths.c

@@ -44,33 +44,27 @@ static void path_circle(PathDesiredData *path, float *cur_point, struct path_sta
  * @param[in] cur_point Current location
  * @param[out] status Structure containing progress along path and deviation
  */
-void path_progress(PathDesiredData *path, float *cur_point, struct path_status *status)
+void path_progress(PathDesiredData *path, float *cur_point, struct path_status *status, bool mode3D)
 {
     switch (path->Mode) {
-    case PATHDESIRED_MODE_BRAKE: // should never get here...
+    case PATHDESIRED_MODE_BRAKE:
-    case PATHDESIRED_MODE_FLYVECTOR:
+    case PATHDESIRED_MODE_FOLLOWVECTOR:
-        return path_vector(path, cur_point, status, true);
+        return path_vector(path, cur_point, status, mode3D);
 
         break;
-    case PATHDESIRED_MODE_DRIVEVECTOR:
+    case PATHDESIRED_MODE_CIRCLERIGHT:
-        return path_vector(path, cur_point, status, false);
+        return path_circle(path, cur_point, status, true);
 
         break;
-    case PATHDESIRED_MODE_FLYCIRCLERIGHT:
+    case PATHDESIRED_MODE_CIRCLELEFT:
-    case PATHDESIRED_MODE_DRIVECIRCLERIGHT:
+        return path_circle(path, cur_point, status, false);
-        return path_circle(path, cur_point, status, 1);
 
         break;
-    case PATHDESIRED_MODE_FLYCIRCLELEFT:
+    case PATHDESIRED_MODE_GOTOENDPOINT:
-    case PATHDESIRED_MODE_DRIVECIRCLELEFT:
+        return path_endpoint(path, cur_point, status, mode3D);
-        return path_circle(path, cur_point, status, 0);
 
         break;
-    case PATHDESIRED_MODE_FLYENDPOINT:
+    case PATHDESIRED_MODE_LAND:
-        return path_endpoint(path, cur_point, status, true);
-
-        break;
-    case PATHDESIRED_MODE_DRIVEENDPOINT:
     default:
         // use the endpoint as default failsafe if called in unknown modes
         return path_endpoint(path, cur_point, status, false);

flight/libraries/pid/pidcontroldown.cpp

@@ -0,0 +1,372 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup PathFollower PID interface class
+ * @brief PID loop for down control
+ * @{
+ *
+ * @file       pidcontroldown.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Executes PID control for down direction
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+extern "C" {
+#include <openpilot.h>
+
+#include <callbackinfo.h>
+
+#include <math.h>
+#include <pid.h>
+#include <CoordinateConversions.h>
+#include <sin_lookup.h>
+#include <pathdesired.h>
+#include <paths.h>
+#include "plans.h"
+#include <stabilizationdesired.h>
+#include <pidstatus.h>
+#include <vtolselftuningstats.h>
+}
+
+#include "pidcontroldown.h"
+
+#define NEUTRALTHRUST_PH_POSITIONAL_ERROR_LIMIT 0.5f
+#define NEUTRALTHRUST_PH_VEL_DESIRED_LIMIT      0.2f
+#define NEUTRALTHRUST_PH_VEL_STATE_LIMIT        0.2f
+#define NEUTRALTHRUST_PH_VEL_ERROR_LIMIT        0.1f
+
+#define NEUTRALTHRUST_START_DELAY               (2 * 20) // 2 seconds at rate of 20Hz (50ms update rate)
+#define NEUTRALTHRUST_END_COUNT                 (NEUTRALTHRUST_START_DELAY + (4 * 20))  // 4 second sample
+
+
+PIDControlDown::PIDControlDown()
+    : deltaTime(0.0f), mVelocitySetpointTarget(0.0f), mVelocitySetpointCurrent(0.0f), mVelocityState(0.0f), mDownCommand(0.0f),
+    mCallback(NULL), mNeutral(0.5f), mVelocityMax(1.0f), mPositionSetpointTarget(0.0f), mPositionState(0.0f),
+    mMinThrust(0.1f), mMaxThrust(0.6f), mActive(false), mAllowNeutralThrustCalc(true)
+{
+    Deactivate();
+}
+
+PIDControlDown::~PIDControlDown() {}
+
+void PIDControlDown::Initialize(PIDControlDownCallback *callback)
+{
+    mCallback = callback;
+}
+
+void PIDControlDown::SetThrustLimits(float min_thrust, float max_thrust)
+{
+    mMinThrust = min_thrust;
+    mMaxThrust = max_thrust;
+}
+
+void PIDControlDown::Deactivate()
+{
+    mActive = false;
+}
+
+void PIDControlDown::Activate()
+{
+    float currentThrust;
+
+    StabilizationDesiredThrustGet(&currentThrust);
+    pid2_transfer(&PID, currentThrust);
+    mActive = true;
+}
+
+void PIDControlDown::UpdateParameters(float kp, float ki, float kd, float beta, float dT, float velocityMax)
+{
+    // pid_configure(&PID, kp, ki, kd, ilimit);
+    float Ti = kp / ki;
+    float Td = kd / kp;
+    float Tt = (Ti + Td) / 2.0f;
+    float kt = 1.0f / Tt;
+    float N  = 10.0f;
+    float Tf = Td / N;
+
+    if (ki < 1e-6f) {
+        // Avoid Ti being infinite
+        Ti = 1e6f;
+        // Tt antiwindup time constant - we don't need antiwindup with no I term
+        Tt = 1e6f;
+        kt = 0.0f;
+    }
+
+    if (kd < 1e-6f) {
+        // PI Controller or P Controller
+        Tf = Ti / N;
+    }
+
+    if (beta > 1.0f) {
+        beta = 1.0f;
+    } else if (beta < 0.4f) {
+        beta = 0.4f;
+    }
+
+    pid2_configure(&PID, kp, ki, kd, Tf, kt, dT, beta, mNeutral, mNeutral, -1.0f);
+    deltaTime    = dT;
+    mVelocityMax = velocityMax;
+}
+
+
+void PIDControlDown::UpdatePositionalParameters(float kp)
+{
+    pid_configure(&PIDpos, kp, 0.0f, 0.0f, 0.0f);
+}
+void PIDControlDown::UpdatePositionSetpoint(float setpointDown)
+{
+    mPositionSetpointTarget = setpointDown;
+}
+void PIDControlDown::UpdatePositionState(float pvDown)
+{
+    mPositionState = pvDown;
+    setup_neutralThrustCalc();
+}
+// This is a pure position hold position control
+void PIDControlDown::ControlPosition()
+{
+    // Current progress location relative to end
+    float velDown = 0.0f;
+
+    velDown = pid_apply(&PIDpos, mPositionSetpointTarget - mPositionState, deltaTime);
+    UpdateVelocitySetpoint(velDown);
+
+    run_neutralThrustCalc();
+}
+
+
+void PIDControlDown::ControlPositionWithPath(struct path_status *progress)
+{
+    // Current progress location relative to end
+    float velDown = progress->path_vector[2];
+
+    velDown += pid_apply(&PIDpos, progress->correction_vector[2], deltaTime);
+    UpdateVelocitySetpoint(velDown);
+}
+
+
+void PIDControlDown::run_neutralThrustCalc(void)
+{
+    // if auto thrust and we have not run a correction calc check for PH and stability to then run an assessment
+    // note that arming into this flight mode is not allowed, so assumption here is that
+    // we have not landed.  If GPSAssist+Manual/Cruise control thrust mode is used, a user overriding the
+    // altitude maintaining PID will have moved the throttle state to FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL.
+    // In manualcontrol.c the state will stay in manual throttle until the throttle command exceeds the vtol thrust min,
+    // avoiding auto-takeoffs.  Therefore no need to check that here.
+
+    if (neutralThrustEst.have_correction != true) {
+        // Make FSM specific
+        bool stable = (fabsf(mPositionSetpointTarget - mPositionState) < NEUTRALTHRUST_PH_POSITIONAL_ERROR_LIMIT &&
+                       fabsf(mVelocitySetpointCurrent) < NEUTRALTHRUST_PH_VEL_DESIRED_LIMIT &&
+                       fabsf(mVelocityState) < NEUTRALTHRUST_PH_VEL_STATE_LIMIT &&
+                       fabsf(mVelocitySetpointCurrent - mVelocityState) < NEUTRALTHRUST_PH_VEL_ERROR_LIMIT);
+
+        if (stable) {
+            if (neutralThrustEst.start_sampling) {
+                neutralThrustEst.count++;
+
+
+                // delay count for 2 seconds into hold allowing for stablisation
+                if (neutralThrustEst.count > NEUTRALTHRUST_START_DELAY) {
+                    neutralThrustEst.sum += PID.I;
+                    if (PID.I < neutralThrustEst.min) {
+                        neutralThrustEst.min = PID.I;
+                    }
+                    if (PID.I > neutralThrustEst.max) {
+                        neutralThrustEst.max = PID.I;
+                    }
+                }
+
+                if (neutralThrustEst.count >= NEUTRALTHRUST_END_COUNT) {
+                    // 6 seconds have past
+                    // lets take an average
+                    neutralThrustEst.average    = neutralThrustEst.sum / (float)(NEUTRALTHRUST_END_COUNT - NEUTRALTHRUST_START_DELAY);
+                    neutralThrustEst.correction = neutralThrustEst.average;
+
+                    PID.I -= neutralThrustEst.average;
+
+                    neutralThrustEst.start_sampling  = false;
+                    neutralThrustEst.have_correction = true;
+
+                    // Write a new adjustment value
+                    // vtolSelfTuningStats.NeutralThrustOffset  was incremental adjusted above
+                    VtolSelfTuningStatsData vtolSelfTuningStats;
+                    VtolSelfTuningStatsGet(&vtolSelfTuningStats);
+                    // add the average remaining i value to the
+                    vtolSelfTuningStats.NeutralThrustOffset     += neutralThrustEst.correction;
+                    vtolSelfTuningStats.NeutralThrustCorrection  = neutralThrustEst.correction; // the i term thrust correction value applied
+                    vtolSelfTuningStats.NeutralThrustAccumulator = PID.I; // the actual iaccumulator value after correction
+                    vtolSelfTuningStats.NeutralThrustRange = neutralThrustEst.max - neutralThrustEst.min;
+                    VtolSelfTuningStatsSet(&vtolSelfTuningStats);
+                }
+            } else {
+                // start a tick count
+                neutralThrustEst.start_sampling = true;
+                neutralThrustEst.count = 0;
+                neutralThrustEst.sum   = 0.0f;
+                neutralThrustEst.max   = 0.0f;
+                neutralThrustEst.min   = 0.0f;
+            }
+        } else {
+            // reset sampling as we did't get 6 continuous seconds
+            neutralThrustEst.start_sampling = false;
+        }
+    } // else we already have a correction for this PH run
+}
+
+
+void PIDControlDown::setup_neutralThrustCalc(void)
+{
+    // reset neutral thrust assessment.
+    // and do once for each position hold engagement
+    neutralThrustEst.start_sampling  = false;
+    neutralThrustEst.count = 0;
+    neutralThrustEst.sum = 0.0f;
+    neutralThrustEst.have_correction = false;
+    neutralThrustEst.average    = 0.0f;
+    neutralThrustEst.correction = 0.0f;
+    neutralThrustEst.min = 0.0f;
+    neutralThrustEst.max = 0.0f;
+}
+
+
+void PIDControlDown::UpdateNeutralThrust(float neutral)
+{
+    if (mActive) {
+        // adjust neutral and achieve bumpless transfer
+        PID.va = neutral;
+        pid2_transfer(&PID, mDownCommand);
+    }
+    mNeutral = neutral;
+}
+
+void PIDControlDown::UpdateVelocitySetpoint(float setpoint)
+{
+    mVelocitySetpointTarget = setpoint;
+    if (fabsf(mVelocitySetpointTarget) > mVelocityMax) {
+        // maintain sign but set to max
+        mVelocitySetpointTarget *= mVelocityMax / fabsf(mVelocitySetpointTarget);
+    }
+}
+
+void PIDControlDown::RateLimit(float *spDesired, float *spCurrent, float rateLimit)
+{
+    float velocity_delta = *spDesired - *spCurrent;
+
+    if (fabsf(velocity_delta) < 1e-6f) {
+        *spCurrent = *spDesired;
+        return;
+    }
+
+    // Calculate the rate of change
+    float accelerationDesired = velocity_delta / deltaTime;
+
+    if (fabsf(accelerationDesired) > rateLimit) {
+        accelerationDesired *= rateLimit / accelerationDesired;
+    }
+
+    if (fabsf(accelerationDesired) < 0.1f) {
+        *spCurrent = *spDesired;
+    } else {
+        *spCurrent += accelerationDesired * deltaTime;
+    }
+}
+
+void PIDControlDown::UpdateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity)
+{
+    if (startingVelocity >= 0.0f) {
+        *updatedVelocity = startingVelocity - dT * brakeRate;
+        if (*updatedVelocity > currentVelocity) {
+            *updatedVelocity = currentVelocity;
+        }
+        if (*updatedVelocity < 0.0f) {
+            *updatedVelocity = 0.0f;
+        }
+    } else {
+        *updatedVelocity = startingVelocity + dT * brakeRate;
+        if (*updatedVelocity < currentVelocity) {
+            *updatedVelocity = currentVelocity;
+        }
+        if (*updatedVelocity > 0.0f) {
+            *updatedVelocity = 0.0f;
+        }
+    }
+}
+
+void PIDControlDown::UpdateVelocityStateWithBrake(float pvDown, float path_time, float brakeRate)
+{
+    mVelocityState = pvDown;
+
+    float velocitySetpointDesired;
+
+    UpdateBrakeVelocity(mVelocitySetpointTarget, path_time, brakeRate, pvDown, &velocitySetpointDesired);
+
+    // Calculate the rate of change
+    // RateLimit(velocitySetpointDesired[iaxis], mVelocitySetpointCurrent[iaxis], 2.0f );
+
+    mVelocitySetpointCurrent = velocitySetpointDesired;
+}
+
+
+// Update velocity state called per dT. Also update current
+// desired velocity
+void PIDControlDown::UpdateVelocityState(float pv)
+{
+    mVelocityState = pv;
+
+    if (mCallback) {
+        // The FSM controls the actual descent velocity and introduces step changes as required
+        float velocitySetpointDesired = mCallback->BoundVelocityDown(mVelocitySetpointTarget);
+        // RateLimit(velocitySetpointDesired, mVelocitySetpointCurrent, 2.0f );
+        mVelocitySetpointCurrent = velocitySetpointDesired;
+    } else {
+        mVelocitySetpointCurrent = mVelocitySetpointTarget;
+    }
+}
+
+float PIDControlDown::GetVelocityDesired(void)
+{
+    return mVelocitySetpointCurrent;
+}
+
+float PIDControlDown::GetDownCommand(void)
+{
+    PIDStatusData pidStatus;
+    float ulow  = mMinThrust;
+    float uhigh = mMaxThrust;
+
+    if (mCallback) {
+        mCallback->BoundThrust(ulow, uhigh);
+    }
+    float downCommand = pid2_apply(&PID, mVelocitySetpointCurrent, mVelocityState, ulow, uhigh);
+    pidStatus.setpoint = mVelocitySetpointCurrent;
+    pidStatus.actual   = mVelocityState;
+    pidStatus.error    = mVelocitySetpointCurrent - mVelocityState;
+    pidStatus.setpoint = mVelocitySetpointCurrent;
+    pidStatus.ulow     = ulow;
+    pidStatus.uhigh    = uhigh;
+    pidStatus.command  = downCommand;
+    pidStatus.P  = PID.P;
+    pidStatus.I  = PID.I;
+    pidStatus.D  = PID.D;
+    PIDStatusSet(&pidStatus);
+    mDownCommand = downCommand;
+    return mDownCommand;
+}

flight/libraries/pid/pidcontroldown.h

@@ -0,0 +1,108 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup PathFollower PID Control implementation
+ * @brief PID Controller for down direction
+ * @{
+ *
+ * @file       PIDControlDown.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Executes control loop for down direction
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+#ifndef PIDCONTROLDOWN_H
+#define PIDCONTROLDOWN_H
+extern "C" {
+#include <pid.h>
+#include <stabilizationdesired.h>
+}
+#include "pidcontroldowncallback.h"
+
+class PIDControlDown {
+public:
+    PIDControlDown();
+    ~PIDControlDown();
+    void Initialize(PIDControlDownCallback *callback);
+    void SetThrustLimits(float min_thrust, float max_thrust);
+    void Deactivate();
+    void Activate();
+    bool IsActive()
+    {
+        return mActive;
+    }
+    void UpdateParameters(float kp, float ki, float kd, float beta, float dT, float velocityMax);
+    void UpdateNeutralThrust(float neutral);
+    void UpdateVelocitySetpoint(float setpoint);
+    void RateLimit(float *spDesired, float *spCurrent, float rateLimit);
+    void UpdateVelocityState(float pv);
+    float GetVelocityDesired(void);
+    float GetDownCommand(void);
+    void UpdatePositionalParameters(float kp);
+    void UpdatePositionState(float pvDown);
+    void UpdatePositionSetpoint(float setpointDown);
+    void ControlPosition();
+    void ControlPositionWithPath(struct path_status *progress);
+    void UpdateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity);
+    void UpdateVelocityStateWithBrake(float pvDown, float path_time, float brakeRate);
+    void DisableNeutralThrustCalc()
+    {
+        mAllowNeutralThrustCalc = false;
+    }
+    void EnableNeutralThrustCalc()
+    {
+        mAllowNeutralThrustCalc = true;
+    }
+
+private:
+    void setup_neutralThrustCalc();
+    void run_neutralThrustCalc();
+
+    struct pid2 PID;
+    float deltaTime;
+    float mVelocitySetpointTarget;
+    float mVelocitySetpointCurrent;
+    float mVelocityState;
+    float mDownCommand;
+    PIDControlDownCallback *mCallback;
+    float mNeutral;
+    float mVelocityMax;
+    struct pid PIDpos;
+    float mPositionSetpointTarget;
+    float mPositionState;
+    float mMinThrust;
+    float mMaxThrust;
+
+    struct NeutralThrustEstimation {
+        uint32_t count;
+        float    sum;
+        float    average;
+        float    correction;
+        float    min;
+        float    max;
+        bool     start_sampling;
+        bool     have_correction;
+    };
+    struct NeutralThrustEstimation neutralThrustEst;
+    bool mActive;
+    bool mAllowNeutralThrustCalc;
+};
+
+#endif // PIDCONTROLDOWN_H

flight/libraries/pid/pidcontroldowncallback.h

@@ -1,10 +1,15 @@
 /**
  ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup PID Library
+ * @brief Thrust control callback pure virtual
+ * @{
+ *
+ * @file       pidcontroldowncallback.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Interface class for PathFollower FSMs
  *
- * @file       pios_config.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      PiOS configuration header.
- *             Central compile time config for the project.
  * @see        The GNU Public License (GPL) Version 3
  *
  *****************************************************************************/
@@ -23,26 +28,15 @@
  * with this program; if not, write to the Free Software Foundation, Inc.,
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
+#ifndef PIDCONTROLDOWNCALLBACK_H
+#define PIDCONTROLDOWNCALLBACK_H
 
+class PIDControlDownCallback {
+public:
+    // PIDControlDownCalback() {};
+    virtual void BoundThrust(__attribute__((unused)) float &ulow, __attribute__((unused)) float &uhigh) = 0;
+    virtual float BoundVelocityDown(float velocity) = 0;
+    // virtual ~PIDControlDownCalback() = 0;
+};
 
-#ifndef PIOS_CONFIG_POSIX_H
+#endif // PIDCONTROLDOWNCALLBACK_H
-#define PIOS_CONFIG_POSIX_H
-
-
-/* Enable/Disable PiOS Modules */
-#define PIOS_INCLUDE_SYS
-#define PIOS_INCLUDE_DELAY
-#define PIOS_INCLUDE_LED
-#define PIOS_INCLUDE_FREERTOS
-#define PIOS_INCLUDE_CALLBACKSCHEDULER
-#define PIOS_INCLUDE_TASK_MONITOR
-#define PIOS_INCLUDE_COM
-#define PIOS_INCLUDE_UDP
-#define PIOS_INCLUDE_SERVO
-
-
-/* Defaults for Logging */
-#define LOG_FILENAME        "PIOS.LOG"
-#define STARTUP_LOG_ENABLED 1
-
-#endif /* PIOS_CONFIG_POSIX_H */

flight/libraries/plans.c

@@ -40,13 +40,41 @@
 #include <manualcontrolcommand.h>
 #include <attitudestate.h>
 #include <vtolpathfollowersettings.h>
+#include <stabilizationbank.h>
+#include <stabilizationdesired.h>
 #include <sin_lookup.h>
+#include <statusvtolautotakeoff.h>
 
 #define UPDATE_EXPECTED 0.02f
 #define UPDATE_MIN      1.0e-6f
 #define UPDATE_MAX      1.0f
 #define UPDATE_ALPHA    1.0e-2f
 
+
+static float applyExpo(float value, float expo);
+
+
+static float applyExpo(float value, float expo)
+{
+    // note: fastPow makes a small error, therefore result needs to be bound
+    float exp = boundf(fastPow(1.00695f, expo), 0.5f, 2.0f);
+
+    // magic number scales expo
+    // so that
+    // expo=100 yields value**10
+    // expo=0 yields value**1
+    // expo=-100 yields value**(1/10)
+    // (pow(2.0,1/100)~=1.00695)
+    if (value > 0.0f) {
+        return boundf(fastPow(value, exp), 0.0f, 1.0f);
+    } else if (value < -0.0f) {
+        return boundf(-fastPow(-value, exp), -1.0f, 0.0f);
+    } else {
+        return 0.0f;
+    }
+}
+
+
 /**
  * @brief initialize UAVOs and structs used by this library
  */
@@ -61,6 +89,8 @@ void plan_initialize()
     ManualControlCommandInitialize();
     VelocityStateInitialize();
     VtolPathFollowerSettingsInitialize();
+    StabilizationBankInitialize();
+    StabilizationDesiredInitialize();
 }
 
 /**
@@ -85,7 +115,7 @@ void plan_setup_positionHold()
     pathDesired.Start.Down       = positionState.Down;
     pathDesired.StartingVelocity = 0.0f;
     pathDesired.EndingVelocity   = 0.0f;
-    pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
+    pathDesired.Mode = PATHDESIRED_MODE_GOTOENDPOINT;
 
     PathDesiredSet(&pathDesired);
 }
@@ -126,48 +156,209 @@ void plan_setup_returnToBase()
 
     pathDesired.StartingVelocity = 0.0f;
     pathDesired.EndingVelocity   = 0.0f;
-    pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
+
+    FlightModeSettingsReturnToBaseNextCommandOptions ReturnToBaseNextCommand;
+    FlightModeSettingsReturnToBaseNextCommandGet(&ReturnToBaseNextCommand);
+    pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_NEXTCOMMAND] = (float)ReturnToBaseNextCommand;
+    pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED1]     = 0.0f;
+    pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED2]     = 0.0f;
+    pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED3]     = 0.0f;
+    pathDesired.Mode = PATHDESIRED_MODE_GOTOENDPOINT;
 
     PathDesiredSet(&pathDesired);
 }
 
-static PiOSDeltatimeConfig landdT;
+
-void plan_setup_land()
+// Vtol AutoTakeoff invocation from flight mode requires the following sequence:
+// 1. Arming must be done whilst in the AutoTakeOff flight mode
+// 2. If the AutoTakeoff flight mode is selected and already armed, requires disarming first
+// 3. Wait for armed state
+// 4. Once the user increases the throttle position to above 50%, then and only then initiate auto-takeoff.
+// 5. Whilst the throttle is < 50% before takeoff, all stick inputs are being ignored.
+// 6. If during the autotakeoff sequence, at any stage, if the throttle stick position reduces to less than 10%, landing is initiated.
+
+static StatusVtolAutoTakeoffControlStateOptions autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED;
+
+#define AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN 2.0f
+#define AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX 50.0f
+static void plan_setup_AutoTakeoff_helper(PathDesiredData *pathDesired)
 {
-    float descendspeed;
+    PositionStateData positionState;
 
-    plan_setup_positionHold();
+    PositionStateGet(&positionState);
+    float velocity_down;
+    float autotakeoff_height;
 
-    FlightModeSettingsLandingVelocityGet(&descendspeed);
+    FlightModeSettingsAutoTakeOffVelocityGet(&velocity_down);
-    PathDesiredData pathDesired;
+    FlightModeSettingsAutoTakeOffHeightGet(&autotakeoff_height);
-    PathDesiredGet(&pathDesired);
+    autotakeoff_height = fabsf(autotakeoff_height);
-    pathDesired.StartingVelocity = descendspeed;
+    if (autotakeoff_height < AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN) {
-    pathDesired.EndingVelocity   = descendspeed;
+        autotakeoff_height = AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN;
-    PathDesiredSet(&pathDesired);
+    } else if (autotakeoff_height > AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX) {
-    PIOS_DELTATIME_Init(&landdT, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
+        autotakeoff_height = AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX;
-}
-
-/**
- * @brief execute land
- */
-void plan_run_land()
-{
-    float downPos, descendspeed;
-    PathDesiredEndData pathDesiredEnd;
-
-    PositionStateDownGet(&downPos); // current down position
-    PathDesiredEndGet(&pathDesiredEnd); // desired position
-    PathDesiredEndingVelocityGet(&descendspeed);
-
-    // desired position is updated to match the desired descend speed but don't run ahead
-    // too far if the current position can't keep up. This normaly means we have landed.
-    if (pathDesiredEnd.Down - downPos < 10) {
-        pathDesiredEnd.Down += descendspeed * PIOS_DELTATIME_GetAverageSeconds(&landdT);
     }
 
-    PathDesiredEndSet(&pathDesiredEnd);
+
+    pathDesired->Start.North = positionState.North;
+    pathDesired->Start.East  = positionState.East;
+    pathDesired->Start.Down  = positionState.Down;
+    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_NORTH] = 0.0f;
+    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_EAST]  = 0.0f;
+    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_DOWN]  = -velocity_down;
+    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE] = (float)autotakeoffState;
+
+    pathDesired->End.North = positionState.North;
+    pathDesired->End.East  = positionState.East;
+    pathDesired->End.Down  = positionState.Down - autotakeoff_height;
+
+    pathDesired->StartingVelocity = 0.0f;
+    pathDesired->EndingVelocity   = 0.0f;
+    pathDesired->Mode = PATHDESIRED_MODE_AUTOTAKEOFF;
 }
 
+#define AUTOTAKEOFF_INFLIGHT_THROTTLE_CHECK_LIMIT 0.2f
+void plan_setup_AutoTakeoff()
+{
+    autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED;
+    // We only allow takeoff if the state transition of disarmed to armed occurs
+    // whilst in the autotake flight mode
+    FlightStatusData flightStatus;
+    FlightStatusGet(&flightStatus);
+    StabilizationDesiredData stabiDesired;
+    StabilizationDesiredGet(&stabiDesired);
+
+    // Are we inflight?
+    if (flightStatus.Armed && stabiDesired.Thrust > AUTOTAKEOFF_INFLIGHT_THROTTLE_CHECK_LIMIT) {
+        // ok assume already in flight and just enter position hold
+        // if we are not actually inflight this will just be a violent autotakeoff
+        autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD;
+        plan_setup_positionHold();
+    } else {
+        if (flightStatus.Armed) {
+            autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST;
+            // Note that if this mode was invoked unintentionally whilst in flight, effectively
+            // all inputs get ignored and the vtol continues to fly to its previous
+            // stabi command.
+        }
+        PathDesiredData pathDesired;
+        plan_setup_AutoTakeoff_helper(&pathDesired);
+        PathDesiredSet(&pathDesired);
+    }
+}
+
+#define AUTOTAKEOFF_THROTTLE_LIMIT_TO_ALLOW_TAKEOFF_START 0.3f
+#define AUTOTAKEOFF_THROTTLE_ABORT_LIMIT                  0.1f
+void plan_run_AutoTakeoff()
+{
+    StatusVtolAutoTakeoffControlStateOptions priorState = autotakeoffState;
+
+    switch (autotakeoffState) {
+    case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST:
+    {
+        FlightStatusData flightStatus;
+        FlightStatusGet(&flightStatus);
+        if (!flightStatus.Armed) {
+            autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE;
+        }
+    }
+    break;
+    case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED:
+    {
+        FlightStatusData flightStatus;
+        FlightStatusGet(&flightStatus);
+        if (flightStatus.Armed) {
+            autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE;
+        }
+    }
+    break;
+    case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE:
+    {
+        ManualControlCommandData cmd;
+        ManualControlCommandGet(&cmd);
+
+        if (cmd.Throttle > AUTOTAKEOFF_THROTTLE_LIMIT_TO_ALLOW_TAKEOFF_START) {
+            autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE;
+        }
+    }
+    break;
+    case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE:
+    {
+        ManualControlCommandData cmd;
+        ManualControlCommandGet(&cmd);
+
+        if (cmd.Throttle < AUTOTAKEOFF_THROTTLE_ABORT_LIMIT) {
+            autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT;
+            plan_setup_land();
+        }
+    }
+    break;
+
+    case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT:
+    {
+        FlightStatusData flightStatus;
+        FlightStatusGet(&flightStatus);
+        if (!flightStatus.Armed) {
+            autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED;
+        }
+    }
+    break;
+    case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD:
+    // nothing to do. land has been requested. stay here for forever until mode change.
+    default:
+        break;
+    }
+
+    if (autotakeoffState != STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT &&
+        autotakeoffState != STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD) {
+        if (priorState != autotakeoffState) {
+            PathDesiredData pathDesired;
+            plan_setup_AutoTakeoff_helper(&pathDesired);
+            PathDesiredSet(&pathDesired);
+        }
+    }
+}
+
+static void plan_setup_land_helper(PathDesiredData *pathDesired)
+{
+    PositionStateData positionState;
+
+    PositionStateGet(&positionState);
+    float velocity_down;
+
+    FlightModeSettingsLandingVelocityGet(&velocity_down);
+
+    pathDesired->Start.North = positionState.North;
+    pathDesired->Start.East  = positionState.East;
+    pathDesired->Start.Down  = positionState.Down;
+    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_NORTH] = 0.0f;
+    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_EAST]  = 0.0f;
+    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_DOWN]  = velocity_down;
+
+    pathDesired->End.North = positionState.North;
+    pathDesired->End.East  = positionState.East;
+    pathDesired->End.Down  = positionState.Down;
+
+    pathDesired->StartingVelocity = 0.0f;
+    pathDesired->EndingVelocity   = 0.0f;
+    pathDesired->Mode = PATHDESIRED_MODE_LAND;
+    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_OPTIONS] = (float)PATHDESIRED_MODEPARAMETER_LAND_OPTION_HORIZONTAL_PH;
+}
+
+void plan_setup_land()
+{
+    PathDesiredData pathDesired;
+
+    plan_setup_land_helper(&pathDesired);
+    PathDesiredSet(&pathDesired);
+}
+
+static void plan_setup_land_from_velocityroam()
+{
+    plan_setup_land();
+    FlightStatusAssistedControlStateOptions assistedControlFlightMode;
+    assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD;
+    FlightStatusAssistedControlStateSet(&assistedControlFlightMode);
+}
 
 /**
  * @brief positionvario functionality
@@ -197,6 +388,14 @@ void plan_setup_PositionRoam()
     plan_setup_PositionVario();
 }
 
+void plan_setup_VelocityRoam()
+{
+    vario_control_lowpass[0] = 0.0f;
+    vario_control_lowpass[1] = 0.0f;
+    vario_control_lowpass[2] = 0.0f;
+    AttitudeStateYawGet(&vario_course);
+}
+
 void plan_setup_HomeLeash()
 {
     plan_setup_PositionVario();
@@ -340,6 +539,9 @@ static void plan_run_PositionVario(vario_type type)
             pathDesired.Start.North = pathDesired.End.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter
             pathDesired.Start.East  = pathDesired.End.East;
             pathDesired.Start.Down  = pathDesired.End.Down;
+
+            // set mode explicitly
+
             PathDesiredSet(&pathDesired);
         }
     } else {
@@ -381,6 +583,106 @@ static void plan_run_PositionVario(vario_type type)
     }
 }
 
+void plan_run_VelocityRoam()
+{
+    // float alpha;
+    PathDesiredData pathDesired;
+    FlightStatusAssistedControlStateOptions assistedControlFlightMode;
+    FlightStatusFlightModeOptions flightMode;
+
+    PathDesiredGet(&pathDesired);
+    FlightModeSettingsPositionHoldOffsetData offset;
+    FlightModeSettingsPositionHoldOffsetGet(&offset);
+    FlightStatusAssistedControlStateGet(&assistedControlFlightMode);
+    FlightStatusFlightModeGet(&flightMode);
+    StabilizationBankData stabSettings;
+    StabilizationBankGet(&stabSettings);
+
+    ManualControlCommandData cmd;
+    ManualControlCommandGet(&cmd);
+
+    cmd.Roll  = applyExpo(cmd.Roll, stabSettings.StickExpo.Roll);
+    cmd.Pitch = applyExpo(cmd.Pitch, stabSettings.StickExpo.Pitch);
+    cmd.Yaw   = applyExpo(cmd.Yaw, stabSettings.StickExpo.Yaw);
+
+    bool flagRollPitchHasInput = (fabsf(cmd.Roll) > 0.0f || fabsf(cmd.Pitch) > 0.0f);
+
+    if (!flagRollPitchHasInput) {
+        // no movement desired, re-enter positionHold at current start-position
+        if (assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY) {
+            // initiate braking and change assisted control flight mode to braking
+            if (flightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) {
+                // avoid brake then hold sequence to continue descent.
+                plan_setup_land_from_velocityroam();
+            } else {
+                plan_setup_assistedcontrol();
+            }
+        }
+        // otherwise nothing to do in braking/hold modes
+    } else {
+        PositionStateData positionState;
+        PositionStateGet(&positionState);
+
+        // Revert assist control state to primary, which in this case implies
+        // we are in roaming state (a GPS vector assisted velocity roam)
+        assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY;
+
+        // Calculate desired velocity in each direction
+        float angle;
+        AttitudeStateYawGet(&angle);
+        angle = DEG2RAD(angle);
+        float cos_angle  = cosf(angle);
+        float sine_angle = sinf(angle);
+        float rotated[2] = {
+            -cmd.Pitch * cos_angle - cmd.Roll * sine_angle,
+            -cmd.Pitch * sine_angle + cmd.Roll * cos_angle
+        };
+        // flip pitch to have pitch down (away) point north
+        float horizontalVelMax;
+        float verticalVelMax;
+        VtolPathFollowerSettingsHorizontalVelMaxGet(&horizontalVelMax);
+        VtolPathFollowerSettingsVerticalVelMaxGet(&verticalVelMax);
+        float velocity_north = rotated[0] * horizontalVelMax;
+        float velocity_east  = rotated[1] * horizontalVelMax;
+        float velocity_down  = 0.0f;
+
+        if (flightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) {
+            FlightModeSettingsLandingVelocityGet(&velocity_down);
+        }
+
+        float velocity = velocity_north * velocity_north + velocity_east * velocity_east;
+        velocity = sqrtf(velocity);
+
+        // if one stick input (pitch or roll) should we use fly by vector? set arbitrary distance of say 20m after which we
+        // expect new stick input
+        // if two stick input pilot is fighting wind manually and we use fly by velocity
+        // in reality setting velocity desired to zero will fight wind anyway.
+
+        pathDesired.Start.North = positionState.North;
+        pathDesired.Start.East  = positionState.East;
+        pathDesired.Start.Down  = positionState.Down;
+        pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_NORTH] = velocity_north;
+        pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_EAST]  = velocity_east;
+        pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_DOWN]  = velocity_down;
+
+        pathDesired.End.North = positionState.North;
+        pathDesired.End.East  = positionState.East;
+        pathDesired.End.Down  = positionState.Down;
+
+        pathDesired.StartingVelocity = velocity;
+        pathDesired.EndingVelocity   = velocity;
+        pathDesired.Mode = PATHDESIRED_MODE_VELOCITY;
+        if (flightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) {
+            pathDesired.Mode = PATHDESIRED_MODE_LAND;
+            pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_OPTIONS] = (float)PATHDESIRED_MODEPARAMETER_LAND_OPTION_NONE;
+        } else {
+            pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_UNUSED] = 0.0f;
+        }
+        PathDesiredSet(&pathDesired);
+        FlightStatusAssistedControlStateSet(&assistedControlFlightMode);
+    }
+}
+
 void plan_run_CourseLock()
 {
     plan_run_PositionVario(COURSE);
@@ -437,7 +739,7 @@ void plan_setup_AutoCruise()
     pathDesired.Start.Down       = pathDesired.End.Down;
     pathDesired.StartingVelocity = 0.0f;
     pathDesired.EndingVelocity   = 0.0f;
-    pathDesired.Mode             = PATHDESIRED_MODE_FLYENDPOINT;
+    pathDesired.Mode             = PATHDESIRED_MODE_GOTOENDPOINT;
 
     PathDesiredSet(&pathDesired);
 
@@ -522,78 +824,63 @@ void plan_run_AutoCruise()
 #define ASSISTEDCONTROL_TIMETOSTOP_MAXIMUM 9.0f // seconds
 #define ASSISTEDCONTROL_DELAY_TO_BRAKE     1.0f      // seconds
 #define ASSISTEDCONTROL_TIMEOUT_MULTIPLIER 2.0f // actual deceleration rate can be 50% of desired...timeouts need to cater for this
-void plan_setup_assistedcontrol(uint8_t timeout_occurred)
+void plan_setup_assistedcontrol()
 {
     PositionStateData positionState;
 
     PositionStateGet(&positionState);
     PathDesiredData pathDesired;
-    PathDesiredGet(&pathDesired);
 
     FlightStatusAssistedControlStateOptions assistedControlFlightMode;
-    FlightStatusAssistedControlStateGet(&assistedControlFlightMode);
 
-    if (timeout_occurred) {
+    VelocityStateData velocityState;
-        pathDesired.End.North        = positionState.North;
+    VelocityStateGet(&velocityState);
-        pathDesired.End.East         = positionState.East;
+    float brakeRate;
-        pathDesired.End.Down         = positionState.Down;
+    VtolPathFollowerSettingsBrakeRateGet(&brakeRate);
-        pathDesired.Start.North      = positionState.North;
+    if (brakeRate < ASSISTEDCONTROL_BRAKERATE_MINIMUM) {
-        pathDesired.Start.East       = positionState.East;
+        brakeRate = ASSISTEDCONTROL_BRAKERATE_MINIMUM; // set a minimum to avoid a divide by zero potential below
-        pathDesired.Start.Down       = positionState.Down;
-        pathDesired.StartingVelocity = 0.0f;
-        pathDesired.EndingVelocity   = 0.0f;
-        pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
-        assistedControlFlightMode    = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD;
-    } else {
-        VelocityStateData velocityState;
-        VelocityStateGet(&velocityState);
-        float brakeRate;
-        VtolPathFollowerSettingsBrakeRateGet(&brakeRate);
-        if (brakeRate < ASSISTEDCONTROL_BRAKERATE_MINIMUM) {
-            brakeRate = ASSISTEDCONTROL_BRAKERATE_MINIMUM; // set a minimum to avoid a divide by zero potential below
-        }
-        // Calculate the velocity
-        float velocity = velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down;
-        velocity = sqrtf(velocity);
-
-        // Calculate the desired time to zero velocity.
-        float time_to_stopped = ASSISTEDCONTROL_DELAY_TO_BRAKE; // we allow at least 0.5 seconds to rotate to a brake angle.
-        time_to_stopped += velocity / brakeRate;
-
-        // Sanity check the brake rate by ensuring that the time to stop is within a range.
-        if (time_to_stopped < ASSISTEDCONTROL_TIMETOSTOP_MINIMUM) {
-            time_to_stopped = ASSISTEDCONTROL_TIMETOSTOP_MINIMUM;
-        } else if (time_to_stopped > ASSISTEDCONTROL_TIMETOSTOP_MAXIMUM) {
-            time_to_stopped = ASSISTEDCONTROL_TIMETOSTOP_MAXIMUM;
-        }
-
-        // calculate the distance we will travel
-        float north_delta = velocityState.North * ASSISTEDCONTROL_DELAY_TO_BRAKE; // we allow at least 0.5s to rotate to brake angle
-        north_delta += (time_to_stopped - ASSISTEDCONTROL_DELAY_TO_BRAKE) * 0.5f * velocityState.North; // area under the linear deceleration plot
-        float east_delta  = velocityState.East * ASSISTEDCONTROL_DELAY_TO_BRAKE; // we allow at least 0.5s to rotate to brake angle
-        east_delta  += (time_to_stopped - ASSISTEDCONTROL_DELAY_TO_BRAKE) * 0.5f * velocityState.East; // area under the linear deceleration plot
-        float down_delta  = velocityState.Down * ASSISTEDCONTROL_DELAY_TO_BRAKE;
-        down_delta  += (time_to_stopped - ASSISTEDCONTROL_DELAY_TO_BRAKE) * 0.5f * velocityState.Down; // area under the linear deceleration plot
-        float net_delta   = east_delta * east_delta + north_delta * north_delta + down_delta * down_delta;
-        net_delta    = sqrtf(net_delta);
-
-        pathDesired.Start.North = positionState.North;
-        pathDesired.Start.East  = positionState.East;
-        pathDesired.Start.Down  = positionState.Down;
-        pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_NORTH] = velocityState.North;
-        pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_EAST]  = velocityState.East;
-        pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_DOWN]  = velocityState.Down;
-
-        pathDesired.End.North = positionState.North + north_delta;
-        pathDesired.End.East  = positionState.East + east_delta;
-        pathDesired.End.Down  = positionState.Down + down_delta;
-
-        pathDesired.StartingVelocity = velocity;
-        pathDesired.EndingVelocity   = 0.0f;
-        pathDesired.Mode = PATHDESIRED_MODE_BRAKE;
-        pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] = time_to_stopped * ASSISTEDCONTROL_TIMEOUT_MULTIPLIER;
-        assistedControlFlightMode    = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE;
     }
+    // Calculate the velocity
+    float velocity = velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down;
+    velocity = sqrtf(velocity);
+
+    // Calculate the desired time to zero velocity.
+    float time_to_stopped = ASSISTEDCONTROL_DELAY_TO_BRAKE; // we allow at least 0.5 seconds to rotate to a brake angle.
+    time_to_stopped += velocity / brakeRate;
+
+    // Sanity check the brake rate by ensuring that the time to stop is within a range.
+    if (time_to_stopped < ASSISTEDCONTROL_TIMETOSTOP_MINIMUM) {
+        time_to_stopped = ASSISTEDCONTROL_TIMETOSTOP_MINIMUM;
+    } else if (time_to_stopped > ASSISTEDCONTROL_TIMETOSTOP_MAXIMUM) {
+        time_to_stopped = ASSISTEDCONTROL_TIMETOSTOP_MAXIMUM;
+    }
+
+    // calculate the distance we will travel
+    float north_delta = velocityState.North * ASSISTEDCONTROL_DELAY_TO_BRAKE; // we allow at least 0.5s to rotate to brake angle
+    north_delta += (time_to_stopped - ASSISTEDCONTROL_DELAY_TO_BRAKE) * 0.5f * velocityState.North; // area under the linear deceleration plot
+    float east_delta  = velocityState.East * ASSISTEDCONTROL_DELAY_TO_BRAKE; // we allow at least 0.5s to rotate to brake angle
+    east_delta  += (time_to_stopped - ASSISTEDCONTROL_DELAY_TO_BRAKE) * 0.5f * velocityState.East; // area under the linear deceleration plot
+    float down_delta  = velocityState.Down * ASSISTEDCONTROL_DELAY_TO_BRAKE;
+    down_delta  += (time_to_stopped - ASSISTEDCONTROL_DELAY_TO_BRAKE) * 0.5f * velocityState.Down; // area under the linear deceleration plot
+    float net_delta   = east_delta * east_delta + north_delta * north_delta + down_delta * down_delta;
+    net_delta    = sqrtf(net_delta);
+
+    pathDesired.Start.North = positionState.North;
+    pathDesired.Start.East  = positionState.East;
+    pathDesired.Start.Down  = positionState.Down;
+    pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_NORTH] = velocityState.North;
+    pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_EAST]  = velocityState.East;
+    pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_DOWN]  = velocityState.Down;
+
+    pathDesired.End.North = positionState.North + north_delta;
+    pathDesired.End.East  = positionState.East + east_delta;
+    pathDesired.End.Down  = positionState.Down + down_delta;
+
+    pathDesired.StartingVelocity = velocity;
+    pathDesired.EndingVelocity   = 0.0f;
+    pathDesired.Mode = PATHDESIRED_MODE_BRAKE;
+    pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] = time_to_stopped * ASSISTEDCONTROL_TIMEOUT_MULTIPLIER;
+    assistedControlFlightMode    = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE;
     FlightStatusAssistedControlStateSet(&assistedControlFlightMode);
     PathDesiredSet(&pathDesired);
 }

flight/libraries/sanitycheck.c

@@ -74,7 +74,7 @@ int32_t configuration_check()
     // Classify navigation capability
 #ifdef REVOLUTION
     RevoSettingsInitialize();
-    uint8_t revoFusion;
+    RevoSettingsFusionAlgorithmOptions revoFusion;
     RevoSettingsFusionAlgorithmGet(&revoFusion);
     bool navCapableFusion;
     switch (revoFusion) {
@@ -104,8 +104,8 @@ int32_t configuration_check()
     // For each available flight mode position sanity check the available
     // modes
     uint8_t num_modes;
-    uint8_t modes[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM];
+    FlightModeSettingsFlightModePositionOptions modes[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM];
-    uint8_t FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM];
+    StabilizationSettingsFlightModeAssistMapOptions FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM];
     ManualControlSettingsFlightModeNumberGet(&num_modes);
     StabilizationSettingsFlightModeAssistMapGet(FlightModeAssistMap);
     FlightModeSettingsFlightModePositionGet(modes);
@@ -118,7 +118,7 @@ int32_t configuration_check()
             ADDSEVERITY(navCapableFusion);
         }
 
-        switch (modes[i]) {
+        switch ((FlightModeSettingsFlightModePositionOptions)modes[i]) {
         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_MANUAL:
             ADDSEVERITY(!gps_assisted);
             ADDSEVERITY(!multirotor);
@@ -143,24 +143,19 @@ int32_t configuration_check()
             break;
         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_PATHPLANNER:
         {
-            // Revo supports PathPlanner and that must be OK or we are not sane
-            // PathPlan alarm is uninitialized if not running
-            // PathPlan alarm is warning or error if the flightplan is invalid
-            SystemAlarmsAlarmData alarms;
-            SystemAlarmsAlarmGet(&alarms);
-            ADDSEVERITY(alarms.PathPlan == SYSTEMALARMS_ALARM_OK);
             ADDSEVERITY(!gps_assisted);
         }
         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD:
+        case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONROAM:
+        case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND:
+        case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTAKEOFF:
             ADDSEVERITY(!coptercontrol);
             ADDSEVERITY(navCapableFusion);
             break;
 
         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_COURSELOCK:
-        case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONROAM:
         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_HOMELEASH:
         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ABSOLUTEPOSITION:
-        case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND:
         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI:
         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE:
         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOCRUISE:
@@ -214,7 +209,7 @@ int32_t configuration_check()
         }
     }
 
-    uint8_t checks_disabled;
+    FlightModeSettingsDisableSanityChecksOptions checks_disabled;
     FlightModeSettingsDisableSanityChecksGet(&checks_disabled);
     if (checks_disabled == FLIGHTMODESETTINGS_DISABLESANITYCHECKS_TRUE) {
         severity = SYSTEMALARMS_ALARM_WARNING;
@@ -242,22 +237,22 @@ static bool check_stabilization_settings(int index, bool multirotor, bool copter
     // Get the different axis modes for this switch position
     switch (index) {
     case 1:
-        FlightModeSettingsStabilization1SettingsArrayGet(modes);
+        FlightModeSettingsStabilization1SettingsArrayGet((FlightModeSettingsStabilization1SettingsOptions *)modes);
         break;
     case 2:
-        FlightModeSettingsStabilization2SettingsArrayGet(modes);
+        FlightModeSettingsStabilization2SettingsArrayGet((FlightModeSettingsStabilization2SettingsOptions *)modes);
         break;
     case 3:
-        FlightModeSettingsStabilization3SettingsArrayGet(modes);
+        FlightModeSettingsStabilization3SettingsArrayGet((FlightModeSettingsStabilization3SettingsOptions *)modes);
         break;
     case 4:
-        FlightModeSettingsStabilization4SettingsArrayGet(modes);
+        FlightModeSettingsStabilization4SettingsArrayGet((FlightModeSettingsStabilization4SettingsOptions *)modes);
         break;
     case 5:
-        FlightModeSettingsStabilization5SettingsArrayGet(modes);
+        FlightModeSettingsStabilization5SettingsArrayGet((FlightModeSettingsStabilization5SettingsOptions *)modes);
         break;
     case 6:
-        FlightModeSettingsStabilization6SettingsArrayGet(modes);
+        FlightModeSettingsStabilization6SettingsArrayGet((FlightModeSettingsStabilization6SettingsOptions *)modes);
         break;
     default:
         return false;
@@ -331,7 +326,7 @@ static bool check_stabilization_settings(int index, bool multirotor, bool copter
 
 FrameType_t GetCurrentFrameType()
 {
-    uint8_t airframe_type;
+    SystemSettingsAirframeTypeOptions airframe_type;
 
     SystemSettingsAirframeTypeGet(&airframe_type);
     switch ((SystemSettingsAirframeTypeOptions)airframe_type) {

flight/modules/Actuator/actuator.c

@@ -40,11 +40,17 @@
 #include "actuatordesired.h"
 #include "actuatorcommand.h"
 #include "flightstatus.h"
+#include <flightmodesettings.h>
 #include "mixersettings.h"
 #include "mixerstatus.h"
 #include "cameradesired.h"
 #include "manualcontrolcommand.h"
 #include "taskinfo.h"
+#include <systemsettings.h>
+#include <sanitycheck.h>
+#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
+#include <vtolpathfollowersettings.h>
+#endif
 #undef PIOS_INCLUDE_INSTRUMENTATION
 #ifdef PIOS_INCLUDE_INSTRUMENTATION
 #include <pios_instrumentation.h>
@@ -76,27 +82,35 @@ static int8_t counter;
 // Private variables
 static xQueueHandle queue;
 static xTaskHandle taskHandle;
+static FrameType_t frameType = FRAME_TYPE_MULTIROTOR;
+static SystemSettingsThrustControlOptions thrustType = SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE;
 
 static float lastResult[MAX_MIX_ACTUATORS] = { 0 };
 static float filterAccumulator[MAX_MIX_ACTUATORS] = { 0 };
 static uint8_t pinsMode[MAX_MIX_ACTUATORS];
 // used to inform the actuator thread that actuator update rate is changed
-static volatile bool actuator_settings_updated;
+static ActuatorSettingsData actuatorSettings;
+static bool spinWhileArmed;
+
 // used to inform the actuator thread that mixer settings are changed
-static volatile bool mixer_settings_updated;
+static MixerSettingsData mixerSettings;
+static int mixer_settings_count = 2;
 
 // Private functions
 static void actuatorTask(void *parameters);
 static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutral);
-static void setFailsafe(const ActuatorSettingsData *actuatorSettings, const MixerSettingsData *mixerSettings);
+static int16_t scaleMotor(float value, int16_t max, int16_t min, int16_t neutral, float maxMotor, float minMotor, bool armed, bool AlwaysStabilizeWhenArmed, float throttleDesired);
-static float MixerCurve(const float throttle, const float *curve, uint8_t elements);
+static void setFailsafe();
-static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSettingsData *actuatorSettings);
+static float MixerCurveFullRangeProportional(const float input, const float *curve, uint8_t elements, bool multirotor);
-static void actuator_update_rate_if_changed(const ActuatorSettingsData *actuatorSettings, bool force_update);
+static float MixerCurveFullRangeAbsolute(const float input, const float *curve, uint8_t elements, bool multirotor);
+static bool set_channel(uint8_t mixer_channel, uint16_t value);
+static void actuator_update_rate_if_changed(bool force_update);
 static void MixerSettingsUpdatedCb(UAVObjEvent *ev);
 static void ActuatorSettingsUpdatedCb(UAVObjEvent *ev);
+static void SettingsUpdatedCb(UAVObjEvent *ev);
 float ProcessMixer(const int index, const float curve1, const float curve2,
-                   const MixerSettingsData *mixerSettings, ActuatorDesiredData *desired,
+                   ActuatorDesiredData *desired,
-                   const float period);
+                   const float period, bool multirotor);
 
 // this structure is equivalent to the UAVObjects for one mixer.
 typedef struct {
@@ -116,6 +130,9 @@ int32_t ActuatorStart()
 #ifdef PIOS_INCLUDE_WDG
     PIOS_WDG_RegisterFlag(PIOS_WDG_ACTUATOR);
 #endif
+    SettingsUpdatedCb(NULL);
+    MixerSettingsUpdatedCb(NULL);
+    ActuatorSettingsUpdatedCb(NULL);
     return 0;
 }
 
@@ -149,6 +166,13 @@ int32_t ActuatorInitialize()
     MixerStatusInitialize();
 #endif
 
+#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
+    VtolPathFollowerSettingsInitialize();
+    VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
+#endif
+    SystemSettingsInitialize();
+    SystemSettingsConnectCallback(&SettingsUpdatedCb);
+
     return 0;
 }
 MODULE_INITCALL(ActuatorInitialize, ActuatorStart);
@@ -177,8 +201,8 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
     ActuatorCommandData command;
     ActuatorDesiredData desired;
     MixerStatusData mixerStatus;
+    FlightModeSettingsData settings;
     FlightStatusData flightStatus;
-    SystemSettingsThrustControlOptions thrustType;
     float throttleDesired;
     float collectiveDesired;
 
@@ -186,21 +210,17 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
     counter = PIOS_Instrumentation_CreateCounter(0xAC700001);
 #endif
     /* Read initial values of ActuatorSettings */
-    ActuatorSettingsData actuatorSettings;
 
-    actuator_settings_updated = false;
     ActuatorSettingsGet(&actuatorSettings);
 
     /* Read initial values of MixerSettings */
-    MixerSettingsData mixerSettings;
-    mixer_settings_updated = false;
     MixerSettingsGet(&mixerSettings);
 
     /* Force an initial configuration of the actuator update rates */
-    actuator_update_rate_if_changed(&actuatorSettings, true);
+    actuator_update_rate_if_changed(true);
 
     // Go to the neutral (failsafe) values until an ActuatorDesired update is received
-    setFailsafe(&actuatorSettings, &mixerSettings);
+    setFailsafe();
 
     // Main task loop
     lastSysTime = xTaskGetTickCount();
@@ -214,20 +234,10 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
 #ifdef PIOS_INCLUDE_INSTRUMENTATION
         PIOS_Instrumentation_TimeStart(counter);
 #endif
-        /* Process settings updated events even in timeout case so we always act on the latest settings */
-        if (actuator_settings_updated) {
-            actuator_settings_updated = false;
-            ActuatorSettingsGet(&actuatorSettings);
-            actuator_update_rate_if_changed(&actuatorSettings, false);
-        }
-        if (mixer_settings_updated) {
-            mixer_settings_updated = false;
-            MixerSettingsGet(&mixerSettings);
-        }
 
         if (rc != pdTRUE) {
             /* Update of ActuatorDesired timed out.  Go to failsafe */
-            setFailsafe(&actuatorSettings, &mixerSettings);
+            setFailsafe();
             continue;
         }
 
@@ -238,9 +248,9 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
         dTSeconds = dTMilliseconds * 0.001f;
 
         FlightStatusGet(&flightStatus);
+        FlightModeSettingsGet(&settings);
         ActuatorDesiredGet(&desired);
         ActuatorCommandGet(&command);
-        SystemSettingsThrustControlGet(&thrustType);
 
         // read in throttle and collective -demultiplex thrust
         switch (thrustType) {
@@ -258,67 +268,94 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
         }
 
         bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED;
+        bool activeThrottle   = (throttleDesired < -0.001f || throttleDesired > 0.001f); // for ground and reversible motors
+        bool positiveThrottle = (throttleDesired > 0.00f);
+        bool multirotor  = (GetCurrentFrameType() == FRAME_TYPE_MULTIROTOR); // check if frame is a multirotor.
+        bool alwaysArmed = settings.Arming == FLIGHTMODESETTINGS_ARMING_ALWAYSARMED;
+        bool AlwaysStabilizeWhenArmed = settings.AlwaysStabilizeWhenArmed == FLIGHTMODESETTINGS_ALWAYSSTABILIZEWHENARMED_TRUE;
 
+        if (alwaysArmed) {
+            AlwaysStabilizeWhenArmed = false; // Do not allow always stabilize when alwaysArmed is active. This is dangerous.
+        }
         // safety settings
         if (!armed) {
-            throttleDesired = 0;
+            throttleDesired = 0.00f; // this also happens in scaleMotors as a per axis check
         }
-        if (throttleDesired <= 0.00f || !armed) {
+
+        if ((frameType == FRAME_TYPE_GROUND && !activeThrottle) || (frameType != FRAME_TYPE_GROUND && throttleDesired <= 0.00f) || !armed) {
+            // throttleDesired should never be 0 or go below 0.
             // force set all other controls to zero if throttle is cut (previously set in Stabilization)
-            if (actuatorSettings.LowThrottleZeroAxis.Roll == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
+            // todo: can probably remove this
-                desired.Roll = 0;
+            if (!(multirotor && AlwaysStabilizeWhenArmed && armed)) { // we don't do this if this is a multirotor AND AlwaysStabilizeWhenArmed is true and the model is armed
-            }
+                if (actuatorSettings.LowThrottleZeroAxis.Roll == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
-            if (actuatorSettings.LowThrottleZeroAxis.Pitch == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
+                    desired.Roll = 0.00f;
-                desired.Pitch = 0;
+                }
-            }
+                if (actuatorSettings.LowThrottleZeroAxis.Pitch == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
-            if (actuatorSettings.LowThrottleZeroAxis.Yaw == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
+                    desired.Pitch = 0.00f;
-                desired.Yaw = 0;
+                }
+                if (actuatorSettings.LowThrottleZeroAxis.Yaw == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
+                    desired.Yaw = 0.00f;
+                }
             }
         }
 
 #ifdef DIAG_MIXERSTATUS
         MixerStatusGet(&mixerStatus);
 #endif
-        int nMixers     = 0;
+
-        Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type;
+        if ((mixer_settings_count < 2) && !ActuatorCommandReadOnly()) { // Nothing can fly with less than two mixers.
-        for (int ct = 0; ct < MAX_MIX_ACTUATORS; ct++) {
+            setFailsafe();
-            if (mixers[ct].type != MIXERSETTINGS_MIXER1TYPE_DISABLED) {
-                nMixers++;
-            }
-        }
-        if ((nMixers < 2) && !ActuatorCommandReadOnly()) { // Nothing can fly with less than two mixers.
-            setFailsafe(&actuatorSettings, &mixerSettings); // So that channels like PWM buzzer keep working
             continue;
         }
 
         AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR);
 
-        bool activeThrottle   = (throttleDesired < 0.00f || throttleDesired > 0.00f);
+        float curve1 = 0.0f; // curve 1 is the throttle curve applied to all motors.
-        bool positiveThrottle = (throttleDesired > 0.00f);
+        float curve2 = 0.0f;
-        bool spinWhileArmed   = actuatorSettings.MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE;
 
-        float curve1 = MixerCurve(throttleDesired, mixerSettings.ThrottleCurve1, MIXERSETTINGS_THROTTLECURVE1_NUMELEM);
+        // Interpolate curve 1 from throttleDesired as input.
+        // assume reversible motor/mixer initially. We can later reverse this. The difference is simply that -ve throttleDesired values
+        // map differently
+        curve1 = MixerCurveFullRangeProportional(throttleDesired, mixerSettings.ThrottleCurve1, MIXERSETTINGS_THROTTLECURVE1_NUMELEM, multirotor);
 
         // The source for the secondary curve is selectable
-        float curve2 = 0;
         AccessoryDesiredData accessory;
-        switch (mixerSettings.Curve2Source) {
+        uint8_t curve2Source = mixerSettings.Curve2Source;
+        switch (curve2Source) {
         case MIXERSETTINGS_CURVE2SOURCE_THROTTLE:
-            curve2 = MixerCurve(throttleDesired, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
+            // assume reversible motor/mixer initially
+            curve2 = MixerCurveFullRangeProportional(throttleDesired, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
             break;
         case MIXERSETTINGS_CURVE2SOURCE_ROLL:
-            curve2 = MixerCurve(desired.Roll, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
+            // Throttle curve contribution the same for +ve vs -ve roll
+            if (multirotor) {
+                curve2 = MixerCurveFullRangeProportional(desired.Roll, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
+            } else {
+                curve2 = MixerCurveFullRangeAbsolute(desired.Roll, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
+            }
             break;
         case MIXERSETTINGS_CURVE2SOURCE_PITCH:
-            curve2 = MixerCurve(desired.Pitch, mixerSettings.ThrottleCurve2,
+            // Throttle curve contribution the same for +ve vs -ve pitch
-                                MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
+            if (multirotor) {
+                curve2 = MixerCurveFullRangeProportional(desired.Pitch, mixerSettings.ThrottleCurve2,
+                                                         MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
+            } else {
+                curve2 = MixerCurveFullRangeAbsolute(desired.Pitch, mixerSettings.ThrottleCurve2,
+                                                     MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
+            }
             break;
         case MIXERSETTINGS_CURVE2SOURCE_YAW:
-            curve2 = MixerCurve(desired.Yaw, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
+            // Throttle curve contribution the same for +ve vs -ve yaw
+            if (multirotor) {
+                curve2 = MixerCurveFullRangeProportional(desired.Yaw, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
+            } else {
+                curve2 = MixerCurveFullRangeAbsolute(desired.Yaw, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
+            }
             break;
         case MIXERSETTINGS_CURVE2SOURCE_COLLECTIVE:
-            curve2 = MixerCurve(collectiveDesired, mixerSettings.ThrottleCurve2,
+            // assume reversible motor/mixer initially
-                                MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
+            curve2 = MixerCurveFullRangeProportional(collectiveDesired, mixerSettings.ThrottleCurve2,
+                                                     MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
             break;
         case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0:
         case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY1:
@@ -327,14 +364,21 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
         case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY4:
         case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY5:
             if (AccessoryDesiredInstGet(mixerSettings.Curve2Source - MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0, &accessory) == 0) {
-                curve2 = MixerCurve(accessory.AccessoryVal, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
+                // Throttle curve contribution the same for +ve vs -ve accessory....maybe not want we want.
+                curve2 = MixerCurveFullRangeAbsolute(accessory.AccessoryVal, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
             } else {
-                curve2 = 0;
+                curve2 = 0.0f;
             }
             break;
+        default:
+            curve2 = 0.0f;
+            break;
         }
 
-        float *status = (float *)&mixerStatus; // access status objects as an array of floats
+        float *status   = (float *)&mixerStatus; // access status objects as an array of floats
+        Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type;
+        float maxMotor  = -1.0f; // highest motor value. Addition method needs this to be -1.0f, division method needs this to be 1.0f
+        float minMotor  = 1.0f; // lowest motor value Addition method needs this to be 1.0f, division method needs this to be -1.0f
 
         for (int ct = 0; ct < MAX_MIX_ACTUATORS; ct++) {
             // During boot all camera actuators should be completely disabled (PWM pulse = 0).
@@ -343,20 +387,26 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
             // Setting it to 1 by default means "Rescale this channel and enable PWM on its output".
             command.Channel[ct] = 1;
 
-            if (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_DISABLED) {
+            uint8_t mixer_type = mixers[ct].type;
+
+            if (mixer_type == MIXERSETTINGS_MIXER1TYPE_DISABLED) {
                 // Set to minimum if disabled.  This is not the same as saying PWM pulse = 0 us
                 status[ct] = -1;
                 continue;
             }
 
-            if ((mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_SERVO)) {
+            if ((mixer_type == MIXERSETTINGS_MIXER1TYPE_MOTOR)) {
-                status[ct] = ProcessMixer(ct, curve1, curve2, &mixerSettings, &desired, dTSeconds);
+                float nonreversible_curve1 = curve1;
-            } else {
+                float nonreversible_curve2 = curve2;
-                status[ct] = -1;
+                if (nonreversible_curve1 < 0.0f) {
-            }
+                    nonreversible_curve1 = 0.0f;
-
+                }
-            // Motors have additional protection for when to be on
+                if (nonreversible_curve2 < 0.0f) {
-            if (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) {
+                    if (!multirotor) { // allow negative throttle if multirotor. function scaleMotors handles the sanity checks.
+                        nonreversible_curve2 = 0.0f;
+                    }
+                }
+                status[ct] = ProcessMixer(ct, nonreversible_curve1, nonreversible_curve2, &desired, dTSeconds, multirotor);
                 // If not armed or motors aren't meant to spin all the time
                 if (!armed ||
                     (!spinWhileArmed && !positiveThrottle)) {
@@ -367,59 +417,77 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
                 // If armed meant to keep spinning,
                 else if ((spinWhileArmed && !positiveThrottle) ||
                          (status[ct] < 0)) {
-                    status[ct] = 0;
+                    if (!multirotor) {
+                        status[ct] = 0;
+                        // allow throttle values lower than 0 if multirotor.
+                        // Values will be scaled to 0 if they need to be in the scaleMotor function
+                    }
                 }
-            }
+            } else if (mixer_type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) {
-
+                status[ct] = ProcessMixer(ct, curve1, curve2, &desired, dTSeconds, multirotor);
-            // Reversable Motors are like Motors but go to neutral instead of minimum
+                // Reversable Motors are like Motors but go to neutral instead of minimum
-            if (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) {
                 // If not armed or motor is inactive - no "spinwhilearmed" for this engine type
                 if (!armed || !activeThrottle) {
                     filterAccumulator[ct] = 0;
                     lastResult[ct] = 0;
                     status[ct] = 0; // force neutral throttle
                 }
-            }
+            } else if (mixer_type == MIXERSETTINGS_MIXER1TYPE_SERVO) {
+                status[ct] = ProcessMixer(ct, curve1, curve2, &desired, dTSeconds, multirotor);
+            } else {
+                status[ct] = -1;
 
-            // If an accessory channel is selected for direct bypass mode
+                // If an accessory channel is selected for direct bypass mode
-            // In this configuration the accessory channel is scaled and mapped
+                // In this configuration the accessory channel is scaled and mapped
-            // directly to output.  Note: THERE IS NO SAFETY CHECK HERE FOR ARMING
+                // directly to output.  Note: THERE IS NO SAFETY CHECK HERE FOR ARMING
-            // these also will not be updated in failsafe mode.  I'm not sure what
+                // these also will not be updated in failsafe mode.  I'm not sure what
-            // the correct behavior is since it seems domain specific.  I don't love
+                // the correct behavior is since it seems domain specific.  I don't love
-            // this code
+                // this code
-            if ((mixers[ct].type >= MIXERSETTINGS_MIXER1TYPE_ACCESSORY0) &&
+                if ((mixer_type >= MIXERSETTINGS_MIXER1TYPE_ACCESSORY0) &&
-                (mixers[ct].type <= MIXERSETTINGS_MIXER1TYPE_ACCESSORY5)) {
+                    (mixer_type <= MIXERSETTINGS_MIXER1TYPE_ACCESSORY5)) {
-                if (AccessoryDesiredInstGet(mixers[ct].type - MIXERSETTINGS_MIXER1TYPE_ACCESSORY0, &accessory) == 0) {
+                    if (AccessoryDesiredInstGet(mixer_type - MIXERSETTINGS_MIXER1TYPE_ACCESSORY0, &accessory) == 0) {
-                    status[ct] = accessory.AccessoryVal;
+                        status[ct] = accessory.AccessoryVal;
-                } else {
+                    } else {
-                    status[ct] = -1;
+                        status[ct] = -1;
-                }
-            }
-
-            if ((mixers[ct].type >= MIXERSETTINGS_MIXER1TYPE_CAMERAROLLORSERVO1) &&
-                (mixers[ct].type <= MIXERSETTINGS_MIXER1TYPE_CAMERAYAW)) {
-                CameraDesiredData cameraDesired;
-                if (CameraDesiredGet(&cameraDesired) == 0) {
-                    switch (mixers[ct].type) {
-                    case MIXERSETTINGS_MIXER1TYPE_CAMERAROLLORSERVO1:
-                        status[ct] = cameraDesired.RollOrServo1;
-                        break;
-                    case MIXERSETTINGS_MIXER1TYPE_CAMERAPITCHORSERVO2:
-                        status[ct] = cameraDesired.PitchOrServo2;
-                        break;
-                    case MIXERSETTINGS_MIXER1TYPE_CAMERAYAW:
-                        status[ct] = cameraDesired.Yaw;
-                        break;
-                    default:
-                        break;
                     }
-                } else {
-                    status[ct] = -1;
                 }
 
-                // Disable camera actuators for CAMERA_BOOT_DELAY_MS after boot
+                if ((mixer_type >= MIXERSETTINGS_MIXER1TYPE_CAMERAROLLORSERVO1) &&
-                if (thisSysTime < (CAMERA_BOOT_DELAY_MS / portTICK_RATE_MS)) {
+                    (mixer_type <= MIXERSETTINGS_MIXER1TYPE_CAMERAYAW)) {
-                    command.Channel[ct] = 0;
+                    CameraDesiredData cameraDesired;
+                    if (CameraDesiredGet(&cameraDesired) == 0) {
+                        switch (mixer_type) {
+                        case MIXERSETTINGS_MIXER1TYPE_CAMERAROLLORSERVO1:
+                            status[ct] = cameraDesired.RollOrServo1;
+                            break;
+                        case MIXERSETTINGS_MIXER1TYPE_CAMERAPITCHORSERVO2:
+                            status[ct] = cameraDesired.PitchOrServo2;
+                            break;
+                        case MIXERSETTINGS_MIXER1TYPE_CAMERAYAW:
+                            status[ct] = cameraDesired.Yaw;
+                            break;
+                        default:
+                            break;
+                        }
+                    } else {
+                        status[ct] = -1;
+                    }
+
+                    // Disable camera actuators for CAMERA_BOOT_DELAY_MS after boot
+                    if (thisSysTime < (CAMERA_BOOT_DELAY_MS / portTICK_RATE_MS)) {
+                        command.Channel[ct] = 0;
+                    }
+                }
+            }
+
+            // If mixer type is motor we need to find which motor has the highest value and which motor has the lowest value.
+            // For use in function scaleMotor
+            if (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) {
+                if (maxMotor < status[ct]) {
+                    maxMotor = status[ct];
+                }
+                if (minMotor > status[ct]) {
+                    minMotor = status[ct];
                 }
             }
         }
@@ -428,10 +496,22 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
         // will be set except explicitly disabled (which will have PWM pulse = 0).
         for (int i = 0; i < MAX_MIX_ACTUATORS; i++) {
             if (command.Channel[i]) {
-                command.Channel[i] = scaleChannel(status[i],
+                if (mixers[i].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) { // If mixer is for a motor we need to find the highest value of all motors
-                                                  actuatorSettings.ChannelMax[i],
+                    command.Channel[i] = scaleMotor(status[i],
-                                                  actuatorSettings.ChannelMin[i],
+                                                    actuatorSettings.ChannelMax[i],
-                                                  actuatorSettings.ChannelNeutral[i]);
+                                                    actuatorSettings.ChannelMin[i],
+                                                    actuatorSettings.ChannelNeutral[i],
+                                                    maxMotor,
+                                                    minMotor,
+                                                    armed,
+                                                    AlwaysStabilizeWhenArmed,
+                                                    throttleDesired);
+                } else { // else we scale the channel
+                    command.Channel[i] = scaleChannel(status[i],
+                                                      actuatorSettings.ChannelMax[i],
+                                                      actuatorSettings.ChannelMin[i],
+                                                      actuatorSettings.ChannelNeutral[i]);
+                }
             }
         }
 
@@ -454,7 +534,7 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
         bool success = true;
 
         for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) {
-            success &= set_channel(n, command.Channel[n], &actuatorSettings);
+            success &= set_channel(n, command.Channel[n]);
         }
 
         PIOS_Servo_Update();
@@ -475,10 +555,10 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
  * Process mixing for one actuator
  */
 float ProcessMixer(const int index, const float curve1, const float curve2,
-                   const MixerSettingsData *mixerSettings, ActuatorDesiredData *desired, const float period)
+                   ActuatorDesiredData *desired, const float period, bool multirotor)
 {
     static float lastFilteredResult[MAX_MIX_ACTUATORS];
-    const Mixer_t *mixers = (Mixer_t *)&mixerSettings->Mixer1Type; // pointer to array of mixers in UAVObjects
+    const Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type; // pointer to array of mixers in UAVObjects
     const Mixer_t *mixer  = &mixers[index];
 
     float result = ((((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE1]) * curve1) +
@@ -489,24 +569,26 @@ float ProcessMixer(const int index, const float curve1, const float curve2,
 
     // note: no feedforward for reversable motors yet for safety reasons
     if (mixer->type == MIXERSETTINGS_MIXER1TYPE_MOTOR) {
-        if (result < 0.0f) { // idle throttle
+        if (!multirotor) { // we allow negative throttle with a multirotor
-            result = 0.0f;
+            if (result < 0.0f) { // zero throttle
+                result = 0.0f;
+            }
         }
 
         // feed forward
         float accumulator = filterAccumulator[index];
-        accumulator += (result - lastResult[index]) * mixerSettings->FeedForward;
+        accumulator += (result - lastResult[index]) * mixerSettings.FeedForward;
         lastResult[index] = result;
         result += accumulator;
         if (period > 0.0f) {
             if (accumulator > 0.0f) {
-                float invFilter = period / mixerSettings->AccelTime;
+                float invFilter = period / mixerSettings.AccelTime;
                 if (invFilter > 1) {
                     invFilter = 1;
                 }
                 accumulator -= accumulator * invFilter;
             } else {
-                float invFilter = period / mixerSettings->DecelTime;
+                float invFilter = period / mixerSettings.DecelTime;
                 if (invFilter > 1) {
                     invFilter = 1;
                 }
@@ -518,7 +600,7 @@ float ProcessMixer(const int index, const float curve1, const float curve2,
 
         // acceleration limit
         float dt    = result - lastFilteredResult[index];
-        float maxDt = mixerSettings->MaxAccel * period;
+        float maxDt = mixerSettings.MaxAccel * period;
         if (dt > maxDt) { // we are accelerating too hard
             result = lastFilteredResult[index] + maxDt;
         }
@@ -530,30 +612,64 @@ float ProcessMixer(const int index, const float curve1, const float curve2,
 
 
 /**
- * Interpolate a throttle curve. Throttle input should be in the range 0 to 1.
+ * Interpolate a throttle curve
- * Output is in the range 0 to 1.
+ * Full range input (-1 to 1) for yaw, roll, pitch
+ * Output range (-1 to 1) reversible motor/throttle curve
+ *
+ * Input of -1 -> -lookup(1)
+ * Input of 0  ->  lookup(0)
+ * Input of 1  ->  lookup(1)
  */
-static float MixerCurve(const float throttle, const float *curve, uint8_t elements)
+static float MixerCurveFullRangeProportional(const float input, const float *curve, uint8_t elements, bool multirotor)
 {
-    float scale = throttle * (float)(elements - 1);
+    float unsigned_value = MixerCurveFullRangeAbsolute(input, curve, elements, multirotor);
-    int idx1    = scale;
+
+    if (input < 0.0f) {
+        return -unsigned_value;
+    } else {
+        return unsigned_value;
+    }
+}
+
+/**
+ * Interpolate a throttle curve
+ * Full range input (-1 to 1) for yaw, roll, pitch
+ * Output range (0 to 1) non-reversible motor/throttle curve
+ *
+ * Input of -1 -> lookup(1)
+ * Input of 0  -> lookup(0)
+ * Input of 1  -> lookup(1)
+ */
+static float MixerCurveFullRangeAbsolute(const float input, const float *curve, uint8_t elements, bool multirotor)
+{
+    float abs_input = fabsf(input);
+    float scale     = abs_input * (float)(elements - 1);
+    int idx1 = scale;
 
     scale -= (float)idx1; // remainder
     if (curve[0] < -1) {
-        return throttle;
+        return abs_input;
-    }
-    if (idx1 < 0) {
-        idx1  = 0; // clamp to lowest entry in table
-        scale = 0;
     }
     int idx2 = idx1 + 1;
     if (idx2 >= elements) {
         idx2 = elements - 1; // clamp to highest entry in table
         if (idx1 >= elements) {
+            if (multirotor) {
+                // if multirotor frame we can return throttle values higher than 100%.
+                // Since the we don't have elements in the curve higher than 100% we return
+                // the last element multiplied by the throttle float
+                if (input < 2.0f) { // this limits positive throttle to 200% of max value in table (Maybe this is too much allowance)
+                    return curve[idx2] * input;
+                } else {
+                    return curve[idx2] * 2.0f; // return 200% of max value in table
+                }
+            }
             idx1 = elements - 1;
         }
     }
-    return curve[idx1] * (1.0f - scale) + curve[idx2] * scale;
+
+    float unsigned_value = curve[idx1] * (1.0f - scale) + curve[idx2] * scale;
+    return unsigned_value;
 }
 
 
@@ -590,24 +706,92 @@ static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutr
     return valueScaled;
 }
 
+/**
+ * Constrain motor values to keep any one motor value from going too far out of range of another motor
+ */
+static int16_t scaleMotor(float value, int16_t max, int16_t min, int16_t neutral, float maxMotor, float minMotor, bool armed, bool AlwaysStabilizeWhenArmed, float throttleDesired)
+{
+    int16_t valueScaled;
+    int16_t maxMotorScaled;
+    int16_t minMotorScaled;
+    int16_t diff;
+
+    // Scale
+    valueScaled    = (int16_t)(value * ((float)(max - neutral))) + neutral;
+    maxMotorScaled = (int16_t)(maxMotor * ((float)(max - neutral))) + neutral;
+    minMotorScaled = (int16_t)(minMotor * ((float)(max - neutral))) + neutral;
+
+
+    if (max > min) {
+        diff = max - maxMotorScaled; // difference between max allowed and actual max motor
+        if (diff < 0) { // if the difference is smaller than 0 we add it to the scaled value
+            valueScaled += diff;
+        }
+        diff = neutral - minMotorScaled; // difference between min allowed and actual min motor
+        if (diff > 0) { // if the difference is larger than 0 we add it to the scaled value
+            valueScaled += diff;
+        }
+        // todo: make this flow easier to understand
+        if (valueScaled > max) {
+            valueScaled = max; // clamp to max value only after scaling is done.
+        }
+
+        if ((valueScaled < neutral) && (spinWhileArmed) && AlwaysStabilizeWhenArmed) {
+            valueScaled = neutral; // clamp to neutral value only after scaling is done.
+        } else if ((valueScaled < neutral) && (!spinWhileArmed) && AlwaysStabilizeWhenArmed) {
+            valueScaled = neutral; // clamp to neutral value only after scaling is done. //throttle goes to min if throttledesired is equal to or less than 0 below
+        } else if (valueScaled < neutral) {
+            valueScaled = min; // clamp to min value only after scaling is done.
+        }
+    } else {
+        // not sure what to do about reversed polarity right now. Why would anyone do this?
+        if (valueScaled < max) {
+            valueScaled = max; // clamp to max value only after scaling is done.
+        }
+        if (valueScaled > min) {
+            valueScaled = min; // clamp to min value only after scaling is done.
+        }
+    }
+
+    // I've added the bool AlwaysStabilizeWhenArmed to this function. Right now we command the motors at min or a range between neutral and max.
+    // NEVER should a motor be command at between min and neutral. I don't like the idea of stabilization ever commanding a motor to min, but we give people the option
+    // This prevents motors startup sync issues causing possible ESC failures.
+
+    // safety checks
+    if (!armed) {
+        // if not armed return min EVERYTIME!
+        valueScaled = min;
+    } else if (!AlwaysStabilizeWhenArmed && (throttleDesired <= 0.0f) && spinWhileArmed) {
+        // all motors idle is AlwaysStabilizeWhenArmed is false, throttle is less than or equal to neutral and spin while armed
+        // stabilize when armed?
+        valueScaled = neutral;
+    } else if (!spinWhileArmed && (throttleDesired <= 0.0f)) {
+        // soft disarm
+        valueScaled = min;
+    }
+
+    return valueScaled;
+}
+
 /**
  * Set actuator output to the neutral values (failsafe)
  */
-static void setFailsafe(const ActuatorSettingsData *actuatorSettings, const MixerSettingsData *mixerSettings)
+static void setFailsafe()
 {
     /* grab only the parts that we are going to use */
     int16_t Channel[ACTUATORCOMMAND_CHANNEL_NUMELEM];
 
     ActuatorCommandChannelGet(Channel);
 
-    const Mixer_t *mixers = (Mixer_t *)&mixerSettings->Mixer1Type; // pointer to array of mixers in UAVObjects
+    const Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type; // pointer to array of mixers in UAVObjects
 
     // Reset ActuatorCommand to safe values
     for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) {
         if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) {
-            Channel[n] = actuatorSettings->ChannelMin[n];
+            Channel[n] = actuatorSettings.ChannelMin[n];
         } else if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_SERVO || mixers[n].type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) {
-            Channel[n] = actuatorSettings->ChannelNeutral[n];
+            // reversible motors need calibration wizard that allows channel neutral to be the 0 velocity point
+            Channel[n] = actuatorSettings.ChannelNeutral[n];
         } else {
             Channel[n] = 0;
         }
@@ -618,7 +802,7 @@ static void setFailsafe(const ActuatorSettingsData *actuatorSettings, const Mixe
 
     // Update servo outputs
     for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) {
-        set_channel(n, Channel[n], actuatorSettings);
+        set_channel(n, Channel[n]);
     }
     // Send the updated command
     PIOS_Servo_Update();
@@ -713,39 +897,39 @@ static inline bool buzzerState(buzzertype type)
 
 
 #if defined(ARCH_POSIX) || defined(ARCH_WIN32)
-static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSettingsData *actuatorSettings)
+static bool set_channel(uint8_t mixer_channel, uint16_t value)
 {
     return true;
 }
 #else
-static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSettingsData *actuatorSettings)
+static bool set_channel(uint8_t mixer_channel, uint16_t value)
 {
-    switch (actuatorSettings->ChannelType[mixer_channel]) {
+    switch (actuatorSettings.ChannelType[mixer_channel]) {
     case ACTUATORSETTINGS_CHANNELTYPE_PWMALARMBUZZER:
-        PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel],
+        PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel],
-                       buzzerState(BUZZ_BUZZER) ? actuatorSettings->ChannelMax[mixer_channel] : actuatorSettings->ChannelMin[mixer_channel]);
+                       buzzerState(BUZZ_BUZZER) ? actuatorSettings.ChannelMax[mixer_channel] : actuatorSettings.ChannelMin[mixer_channel]);
         return true;
 
     case ACTUATORSETTINGS_CHANNELTYPE_ARMINGLED:
-        PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel],
+        PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel],
-                       buzzerState(BUZZ_ARMING) ? actuatorSettings->ChannelMax[mixer_channel] : actuatorSettings->ChannelMin[mixer_channel]);
+                       buzzerState(BUZZ_ARMING) ? actuatorSettings.ChannelMax[mixer_channel] : actuatorSettings.ChannelMin[mixer_channel]);
         return true;
 
     case ACTUATORSETTINGS_CHANNELTYPE_INFOLED:
-        PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel],
+        PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel],
-                       buzzerState(BUZZ_INFO) ? actuatorSettings->ChannelMax[mixer_channel] : actuatorSettings->ChannelMin[mixer_channel]);
+                       buzzerState(BUZZ_INFO) ? actuatorSettings.ChannelMax[mixer_channel] : actuatorSettings.ChannelMin[mixer_channel]);
         return true;
 
     case ACTUATORSETTINGS_CHANNELTYPE_PWM:
     {
-        uint8_t mode = pinsMode[actuatorSettings->ChannelAddr[mixer_channel]];
+        uint8_t mode = pinsMode[actuatorSettings.ChannelAddr[mixer_channel]];
         switch (mode) {
         case ACTUATORSETTINGS_BANKMODE_ONESHOT125:
             // Remap 1000-2000 range to 125-250
-            PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], value / ACTUATOR_ONESHOT125_PULSE_SCALE);
+            PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel], value / ACTUATOR_ONESHOT125_PULSE_SCALE);
             break;
         default:
-            PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], value);
+            PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel], value);
             break;
         }
         return true;
@@ -770,12 +954,12 @@ static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSet
 /**
  * @brief Update the servo update rate
  */
-static void actuator_update_rate_if_changed(const ActuatorSettingsData *actuatorSettings, bool force_update)
+static void actuator_update_rate_if_changed(bool force_update)
 {
     static uint16_t prevBankUpdateFreq[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM];
     static uint8_t prevBankMode[ACTUATORSETTINGS_BANKMODE_NUMELEM];
-    bool updateMode = force_update || (memcmp(prevBankMode, actuatorSettings->BankMode, sizeof(prevBankMode)) != 0);
+    bool updateMode = force_update || (memcmp(prevBankMode, actuatorSettings.BankMode, sizeof(prevBankMode)) != 0);
-    bool updateFreq = force_update || (memcmp(prevBankUpdateFreq, actuatorSettings->BankUpdateFreq, sizeof(prevBankUpdateFreq)) != 0);
+    bool updateFreq = force_update || (memcmp(prevBankUpdateFreq, actuatorSettings.BankUpdateFreq, sizeof(prevBankUpdateFreq)) != 0);
 
     // check if any setting is changed
     if (updateMode || updateFreq) {
@@ -784,15 +968,15 @@ static void actuator_update_rate_if_changed(const ActuatorSettingsData *actuator
         uint16_t freq[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM];
         uint32_t clock[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM] = { 0 };
         for (uint8_t i = 0; i < ACTUATORSETTINGS_BANKMODE_NUMELEM; i++) {
-            if (force_update || (actuatorSettings->BankMode[i] != prevBankMode[i])) {
+            if (force_update || (actuatorSettings.BankMode[i] != prevBankMode[i])) {
                 PIOS_Servo_SetBankMode(i,
-                                       actuatorSettings->BankMode[i] ==
+                                       actuatorSettings.BankMode[i] ==
                                        ACTUATORSETTINGS_BANKMODE_PWM ?
                                        PIOS_SERVO_BANK_MODE_PWM :
                                        PIOS_SERVO_BANK_MODE_SINGLE_PULSE
                                        );
             }
-            switch (actuatorSettings->BankMode[i]) {
+            switch (actuatorSettings.BankMode[i]) {
             case ACTUATORSETTINGS_BANKMODE_ONESHOT125:
                 freq[i]  = 100; // Value must be small enough so CCr isn't update until the PIOS_Servo_Update is triggered
                 clock[i] = ACTUATOR_ONESHOT125_CLOCK; // Setup an 2MHz timer clock
@@ -802,37 +986,73 @@ static void actuator_update_rate_if_changed(const ActuatorSettingsData *actuator
                 clock[i] = ACTUATOR_PWM_CLOCK;
                 break;
             default: // PWM
-                freq[i]  = actuatorSettings->BankUpdateFreq[i];
+                freq[i]  = actuatorSettings.BankUpdateFreq[i];
                 clock[i] = ACTUATOR_PWM_CLOCK;
                 break;
             }
         }
 
         memcpy(prevBankMode,
-               actuatorSettings->BankMode,
+               actuatorSettings.BankMode,
                sizeof(prevBankMode));
 
         PIOS_Servo_SetHz(freq, clock, ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM);
 
         memcpy(prevBankUpdateFreq,
-               actuatorSettings->BankUpdateFreq,
+               actuatorSettings.BankUpdateFreq,
                sizeof(prevBankUpdateFreq));
         // retrieve mode from related bank
         for (uint8_t i = 0; i < MAX_MIX_ACTUATORS; i++) {
             uint8_t bank = PIOS_Servo_GetPinBank(i);
-            pinsMode[i] = actuatorSettings->BankMode[bank];
+            pinsMode[i] = actuatorSettings.BankMode[bank];
         }
     }
 }
 
 static void ActuatorSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 {
-    actuator_settings_updated = true;
+    ActuatorSettingsGet(&actuatorSettings);
+    spinWhileArmed = actuatorSettings.MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE;
+    if (frameType == FRAME_TYPE_GROUND) {
+        spinWhileArmed = false;
+    }
+    actuator_update_rate_if_changed(false);
 }
 
 static void MixerSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 {
-    mixer_settings_updated = true;
+    MixerSettingsGet(&mixerSettings);
+    mixer_settings_count = 0;
+    Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type;
+    for (int ct = 0; ct < MAX_MIX_ACTUATORS; ct++) {
+        if (mixers[ct].type != MIXERSETTINGS_MIXER1TYPE_DISABLED) {
+            mixer_settings_count++;
+        }
+    }
+}
+static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+{
+    frameType = GetCurrentFrameType();
+#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
+    uint8_t TreatCustomCraftAs;
+    VtolPathFollowerSettingsTreatCustomCraftAsGet(&TreatCustomCraftAs);
+
+    if (frameType == FRAME_TYPE_CUSTOM) {
+        switch (TreatCustomCraftAs) {
+        case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING:
+            frameType = FRAME_TYPE_FIXED_WING;
+            break;
+        case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL:
+            frameType = FRAME_TYPE_MULTIROTOR;
+            break;
+        case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_GROUND:
+            frameType = FRAME_TYPE_GROUND;
+            break;
+        }
+    }
+#endif
+
+    SystemSettingsThrustControlGet(&thrustType);
 }
 
 /**

flight/modules/Airspeed/airspeed.c

@@ -98,7 +98,7 @@ int32_t AirspeedInitialize()
 #else
 
     HwSettingsInitialize();
-    uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
+    HwSettingsOptionalModulesOptions optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
     HwSettingsOptionalModulesArrayGet(optionalModules);
 
 
@@ -110,7 +110,7 @@ int32_t AirspeedInitialize()
     }
 #endif
 
-    uint8_t adcRouting[HWSETTINGS_ADCROUTING_NUMELEM];
+    HwSettingsADCRoutingOptions adcRouting[HWSETTINGS_ADCROUTING_NUMELEM];
     HwSettingsADCRoutingArrayGet(adcRouting);
 
     // Determine if the barometric airspeed sensor is routed to an ADC pin
@@ -169,6 +169,8 @@ static void airspeedTask(__attribute__((unused)) void *parameters)
                     imu_airspeedInitialize(&airspeedSettings);
                 }
                 break;
+            default:
+                break;
             }
         }
         switch (airspeedSettings.AirspeedSensorType) {

flight/modules/Airspeed/baro_airspeed_mpxv.c

@@ -46,7 +46,7 @@
 
 #define CALIBRATION_IDLE_MS                   2000   // Time to wait before calibrating, in [ms]
 #define CALIBRATION_COUNT_MS                  2000   // Time to spend calibrating, in [ms]
-#define ANALOG_BARO_AIRSPEED_TIME_CONSTANT_MS 100 // low pass filter
+#define ANALOG_BARO_AIRSPEED_TIME_CONSTANT_MS 256 // low pass filter
 
 // Private types
 
@@ -104,9 +104,10 @@ void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsDa
         }
     }
     sensor.zeroPoint = airspeedSettings->ZeroPoint;
+    sensor.scale     = airspeedSettings->Scale;
 
-    // Filter CAS
+    // Filter CAS : airspeedSettings->SamplePeriod value is 0 - 255 range
-    float alpha = airspeedSettings->SamplePeriod / (airspeedSettings->SamplePeriod + ANALOG_BARO_AIRSPEED_TIME_CONSTANT_MS); // Low pass filter.
+    float alpha = 1 - (airspeedSettings->SamplePeriod / ANALOG_BARO_AIRSPEED_TIME_CONSTANT_MS); // Low pass filter.
 
     airspeedSensor->CalibratedAirspeed = PIOS_MPXV_CalcAirspeed(&sensor, airspeedSensor->SensorValue) * (alpha) + airspeedSensor->CalibratedAirspeed * (1.0f - alpha);
     airspeedSensor->SensorConnected    = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;

flight/modules/Battery/battery.c

@@ -138,6 +138,13 @@ static void onTimer(__attribute__((unused)) UAVObjEvent *ev)
     const float dT = SAMPLE_PERIOD_MS / 1000.0f;
     float energyRemaining;
 
+    // Reset ConsumedEnergy counter
+    if (batterySettings.ResetConsumedEnergy) {
+        flightBatteryData.ConsumedEnergy    = 0;
+        batterySettings.ResetConsumedEnergy = false;
+        FlightBatterySettingsSet(&batterySettings);
+    }
+
     // calculate the battery parameters
     if (voltageADCPin >= 0) {
         flightBatteryData.Voltage = (PIOS_ADC_PinGetVolt(voltageADCPin) - batterySettings.SensorCalibrations.VoltageZero) * batterySettings.SensorCalibrations.VoltageFactor; // in Volts

flight/modules/ComUsbBridge/ComUsbBridge.c

@@ -43,6 +43,7 @@
 static void com2UsbBridgeTask(void *parameters);
 static void usb2ComBridgeTask(void *parameters);
 static void updateSettings(UAVObjEvent *ev);
+static void usb2ComBridgeSetCtrlLine(uint32_t com_id, uint32_t mask, uint32_t state);
 
 // ****************
 // Private constants
@@ -95,8 +96,16 @@ static int32_t comUsbBridgeStart(void)
 static int32_t comUsbBridgeInitialize(void)
 {
     // TODO: Get from settings object
-    usart_port     = PIOS_COM_BRIDGE;
+    usart_port = PIOS_COM_BRIDGE;
-    vcp_port       = PIOS_COM_VCP;
+    vcp_port   = PIOS_COM_VCP;
+
+    // Register the call back handler for USB control line changes to simply
+    // pass these onto any handler registered on the USART
+    if (vcp_port) {
+        PIOS_COM_RegisterCtrlLineCallback(vcp_port,
+                                          usb2ComBridgeSetCtrlLine,
+                                          usart_port);
+    }
 
 #ifdef MODULE_COMUSBBRIDGE_BUILTIN
     bridge_enabled = true;
@@ -168,6 +177,14 @@ static void usb2ComBridgeTask(__attribute__((unused)) void *parameters)
     }
 }
 
+/* This routine is registered with the USB driver and will be called in the
+ * event of a control line state change. It will then call down to the USART
+ * driver to drive the required control line state.
+ */
+static void usb2ComBridgeSetCtrlLine(uint32_t com_id, uint32_t mask, uint32_t state)
+{
+    PIOS_COM_SetCtrlLine(com_id, mask, state);
+}
 
 static void updateSettings(__attribute__((unused)) UAVObjEvent *ev)
 {

flight/modules/GPS/GPS.c

@@ -50,6 +50,7 @@
 #include "UBX.h"
 #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
 #include "inc/ubx_autoconfig.h"
+// #include "../../libraries/inc/fifo_buffer.h"
 #endif
 
 #include <pios_instrumentation_helper.h>
@@ -60,7 +61,7 @@ PERF_DEFINE_COUNTER(counterParse);
 // Private functions
 
 static void gpsTask(void *parameters);
-static void updateHwSettings();
+static void updateHwSettings(UAVObjEvent *ev);
 
 #ifdef PIOS_GPS_SETS_HOMELOCATION
 static void setHomeLocation(GPSPositionSensorData *gpsData);
@@ -111,6 +112,8 @@ void updateGpsSettings(UAVObjEvent *ev);
 // ****************
 // Private variables
 
+static GPSSettingsData gpsSettings;
+
 static uint32_t gpsPort;
 static bool gpsEnabled = false;
 
@@ -150,15 +153,15 @@ int32_t GPSStart(void)
  * \return -1 if initialisation failed
  * \return 0 on success
  */
+
 int32_t GPSInitialize(void)
 {
     gpsPort = PIOS_COM_GPS;
-    uint8_t gpsProtocol;
 
+    HwSettingsInitialize();
 #ifdef MODULE_GPS_BUILTIN
     gpsEnabled = true;
 #else
-    HwSettingsInitialize();
     HwSettingsOptionalModulesData optionalModules;
 
     HwSettingsOptionalModulesGet(&optionalModules);
@@ -188,8 +191,12 @@ int32_t GPSInitialize(void)
     AuxMagSettingsUpdatedCb(NULL);
     AuxMagSettingsConnectCallback(AuxMagSettingsUpdatedCb);
 #endif
-    updateHwSettings();
+    GPSSettingsInitialize();
-#else
+    // updateHwSettings() uses gpsSettings
+    GPSSettingsGet(&gpsSettings);
+    // must updateHwSettings() before updateGpsSettings() so baud rate is set before GPS serial code starts running
+    updateHwSettings(0);
+#else /* if defined(REVOLUTION) */
     if (gpsPort && gpsEnabled) {
         GPSPositionSensorInitialize();
         GPSVelocitySensorInitialize();
@@ -200,27 +207,29 @@ int32_t GPSInitialize(void)
 #if defined(PIOS_GPS_SETS_HOMELOCATION)
         HomeLocationInitialize();
 #endif
-        updateHwSettings();
+        GPSSettingsInitialize();
+        // updateHwSettings() uses gpsSettings
+        GPSSettingsGet(&gpsSettings);
+        // must updateHwSettings() before updateGpsSettings() so baud rate is set before GPS serial code starts running
+        updateHwSettings(0);
     }
 #endif /* if defined(REVOLUTION) */
 
     if (gpsPort && gpsEnabled) {
-        GPSSettingsInitialize();
+#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) && defined(PIOS_INCLUDE_GPS_UBX_PARSER)
-        GPSSettingsDataProtocolGet(&gpsProtocol);
+        gps_rx_buffer = pios_malloc((sizeof(struct UBXPacket) > NMEA_MAX_PACKET_LENGTH) ? sizeof(struct UBXPacket) : NMEA_MAX_PACKET_LENGTH);
-        switch (gpsProtocol) {
+#elif defined(PIOS_INCLUDE_GPS_UBX_PARSER)
-#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
+        gps_rx_buffer = pios_malloc(sizeof(struct UBXPacket));
-        case GPSSETTINGS_DATAPROTOCOL_NMEA:
+#elif defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
-            gps_rx_buffer = pios_malloc(NMEA_MAX_PACKET_LENGTH);
+        gps_rx_buffer = pios_malloc(NMEA_MAX_PACKET_LENGTH);
-            break;
+#else
+        gps_rx_buffer = NULL;
 #endif
-        case GPSSETTINGS_DATAPROTOCOL_UBX:
+#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER)
-            gps_rx_buffer = pios_malloc(sizeof(struct UBXPacket));
-            break;
-        default:
-            gps_rx_buffer = NULL;
-        }
         PIOS_Assert(gps_rx_buffer);
+#endif
 #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
+        HwSettingsConnectCallback(updateHwSettings); // allow changing baud rate even after startup
         GPSSettingsConnectCallback(updateGpsSettings);
 #endif
         return 0;
@@ -245,15 +254,13 @@ static void gpsTask(__attribute__((unused)) void *parameters)
     portTickType homelocationSetDelay = 0;
 #endif
     GPSPositionSensorData gpspositionsensor;
-    GPSSettingsData gpsSettings;
-
-    GPSSettingsGet(&gpsSettings);
 
     timeOfLastUpdateMs  = timeNowMs;
     timeOfLastCommandMs = timeNowMs;
 
     GPSPositionSensorGet(&gpspositionsensor);
 #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
+    // this should be done in the task because it calls out to actually start the GPS serial reads
     updateGpsSettings(0);
 #endif
 
@@ -270,18 +277,41 @@ static void gpsTask(__attribute__((unused)) void *parameters)
         if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX) {
             char *buffer   = 0;
             uint16_t count = 0;
-            uint8_t status;
+
-            GPSPositionSensorStatusGet(&status);
+            ubx_autoconfig_run(&buffer, &count);
-            ubx_autoconfig_run(&buffer, &count, status != GPSPOSITIONSENSOR_STATUS_NOGPS);
             // Something to send?
             if (count) {
+                // clear ack/nak
+                ubxLastAck.clsID = 0x00;
+                ubxLastAck.msgID = 0x00;
+                ubxLastNak.clsID = 0x00;
+                ubxLastNak.msgID = 0x00;
+
                 PIOS_COM_SendBuffer(gpsPort, (uint8_t *)buffer, count);
             }
         }
-#endif
+
-        // This blocks the task until there is something on the buffer
+        // need to do this whether there is received data or not, or the status (e.g. gcs) is not always correct
+        int32_t ac_status = ubx_autoconfig_get_status();
+        static uint8_t lastStatus = GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DISABLED
+                                    + GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_RUNNING
+                                    + GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DONE
+                                    + GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR;
+        gpspositionsensor.AutoConfigStatus =
+            ac_status == UBX_AUTOCONFIG_STATUS_DISABLED ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DISABLED :
+            ac_status == UBX_AUTOCONFIG_STATUS_DONE ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DONE :
+            ac_status == UBX_AUTOCONFIG_STATUS_ERROR ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR :
+            GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_RUNNING;
+        if (gpspositionsensor.AutoConfigStatus != lastStatus) {
+            GPSPositionSensorAutoConfigStatusSet(&gpspositionsensor.AutoConfigStatus);
+            lastStatus = gpspositionsensor.AutoConfigStatus;
+        }
+#endif /* if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) */
+
+        // This blocks the task until there is something on the buffer (or 100ms? passes)
         uint16_t cnt;
-        while ((cnt = PIOS_COM_ReceiveBuffer(gpsPort, c, GPS_READ_BUFFER, xDelay)) > 0) {
+        cnt = PIOS_COM_ReceiveBuffer(gpsPort, c, GPS_READ_BUFFER, xDelay);
+        if (cnt > 0) {
             PERF_TIMED_SECTION_START(counterParse);
             PERF_TRACK_VALUE(counterBytesIn, cnt);
             PERF_MEASURE_PERIOD(counterRate);
@@ -294,24 +324,8 @@ static void gpsTask(__attribute__((unused)) void *parameters)
 #endif
 #if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
             case GPSSETTINGS_DATAPROTOCOL_UBX:
-            {
-#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
-                int32_t ac_status = ubx_autoconfig_get_status();
-                static uint8_t lastStatus = 0;
-
-                gpspositionsensor.AutoConfigStatus =
-                    ac_status == UBX_AUTOCONFIG_STATUS_DISABLED ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DISABLED :
-                    ac_status == UBX_AUTOCONFIG_STATUS_DONE ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DONE :
-                    ac_status == UBX_AUTOCONFIG_STATUS_ERROR ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR :
-                    GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_RUNNING;
-                if (gpspositionsensor.AutoConfigStatus != lastStatus) {
-                    GPSPositionSensorAutoConfigStatusSet(&gpspositionsensor.AutoConfigStatus);
-                    lastStatus = gpspositionsensor.AutoConfigStatus;
-                }
-#endif
                 res = parse_ubx_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
-            }
+                break;
-            break;
 #endif
             default:
                 res = NO_PARSER; // this should not happen
@@ -332,7 +346,7 @@ static void gpsTask(__attribute__((unused)) void *parameters)
             (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX && gpspositionsensor.AutoConfigStatus == GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR)) {
             // we have not received any valid GPS sentences for a while.
             // either the GPS is not plugged in or a hardware problem or the GPS has locked up.
-            uint8_t status = GPSPOSITIONSENSOR_STATUS_NOGPS;
+            GPSPositionSensorStatusOptions status = GPSPOSITIONSENSOR_STATUS_NOGPS;
             GPSPositionSensorStatusSet(&status);
             AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR);
         } else {
@@ -429,40 +443,136 @@ static void setHomeLocation(GPSPositionSensorData *gpsData)
  * like protocol, etc. Thus the HwSettings object which contains the
  * GPS port speed is used for now.
  */
-static void updateHwSettings()
-{
-    if (gpsPort) {
-        // Retrieve settings
-        uint8_t speed;
-        HwSettingsGPSSpeedGet(&speed);
 
-        // Set port speed
+// must updateHwSettings() before updateGpsSettings() so baud rate is set before GPS serial code starts running
-        switch (speed) {
+static void updateHwSettings(UAVObjEvent __attribute__((unused)) *ev)
-        case HWSETTINGS_GPSSPEED_2400:
+{
-            PIOS_COM_ChangeBaud(gpsPort, 2400);
+    // with these changes, no one (not even OP board makers) should ever need u-center (the complex UBlox configuration program)
-            break;
+    // no booting of Revo should be required during any setup or testing, just Send the settings you want to play with
-        case HWSETTINGS_GPSSPEED_4800:
+    // with autoconfig enabled, just change the baud rate in HwSettings and it will change the GPS internal baud and then the Revo port
-            PIOS_COM_ChangeBaud(gpsPort, 4800);
+    // with autoconfig disabled, it will only change the Revo port, so you can try to find the current GPS baud rate if you don't know it
-            break;
+    // GPSPositionSensor.SensorType and .UbxAutoConfigStatus now give correct values at all times, so use .SensorType to prove it is
-        case HWSETTINGS_GPSSPEED_9600:
+    // connected, even to an incorrectly configured (e.g. factory default) GPS
-            PIOS_COM_ChangeBaud(gpsPort, 9600);
+
-            break;
+    // Use cases:
-        case HWSETTINGS_GPSSPEED_19200:
+    // - the user can plug in a default GPS, use autoconfig-store once and then always use autoconfig-disabled
-            PIOS_COM_ChangeBaud(gpsPort, 19200);
+    // - the user can plug in a default GPS that can't store settings (defaults to 9600 baud) and always use autoconfig-nostore
-            break;
+    // - - this is one reason for coding this: cheap eBay GPS's that loose their settings sometimes
-        case HWSETTINGS_GPSSPEED_38400:
+    // - the user can plug in a default GPS that _can_ store settings and always use autoconfig-nostore with that too
-            PIOS_COM_ChangeBaud(gpsPort, 38400);
+    // - - if settings change in newer releases of OP, this will make sure to use the correct settings with whatever code you are running
-            break;
+    // - the user can plug in a correctly configured GPS and use autoconfig-disabled
-        case HWSETTINGS_GPSSPEED_57600:
+    // - the user can recover an old or incorrectly configured GPS (internal GPS baud rate or other GPS settings wrong)
-            PIOS_COM_ChangeBaud(gpsPort, 57600);
+    // - - disable UBX GPS autoconfig
-            break;
+    // - - set HwSettings GPS baud rate to the current GPS internal baud rate to get it to connect at current (especially non-9600) baud rate
-        case HWSETTINGS_GPSSPEED_115200:
+    // - - - you can tell the baud rate is correct when GPSPositionSensor.SensorType is known (not "Unknown") (e.g. UBX6)
-            PIOS_COM_ChangeBaud(gpsPort, 115200);
+    // - - - the SensorType and AutoConfigStatus may fail at 9600 and will fail at 4800 or lower if the GPS is configured to send OP data
-            break;
+    // - - - (way more than default) at 4800 baud or slower
-        case HWSETTINGS_GPSSPEED_230400:
+    // - - enable autoconfig-nostore GPSSettings to set correct messages and enable further magic baud rate settings
-            PIOS_COM_ChangeBaud(gpsPort, 230400);
+    // - - change the baud rate to what you want in HwSettings (it will change the baud rate in the GPS and then in the Revo port)
-            break;
+    // - - wait for the baud rate change to complete, GPSPositionSensor.AutoConfigStatus will say "DONE"
+    // - - enable autoconfig-store and it will save the new baud rate and the correct message configuration
+    // - - wait for the store to complete, GPSPositionSensor.AutoConfigStatus will say "DONE"
+    // - - for the new baud rate, the user should either choose 9600 for use with autoconfig-nostore or can choose 57600 for use with autoconfig-disabled
+    // - the user (Dave :)) can configure a whole bunch of default GPS's with no intervention by using autoconfig-store as a saved Revo setting
+    // - - just plug the default (9600 baud) GPS in to an unpowered Revo, power the Revo/GPS through the servo connector, wait some time, unplug
+    // - - or with this code, OP could and even should just ship GPS's with default settings
+
+    // if we add an "initial baud rate" instead of assuming 9600 at boot up for autoconfig-nostore/store
+    // - the user can use the same GPS with both an older release of OP (e.g. GPS settings at 38400) and the current release (autoconfig-nostore)
+    // - the 57600 could be fixed instead of the 9600 as part of autoconfig-store/nostore and the HwSettings.GPSSpeed be the initial baud rate?
+
+    // About using UBlox GPS's with default settings (9600 baud and NEMA data):
+    // - the default uBlox settings (different than OP settings) are NEMA and 9600 baud
+    // - - and that is OK and you use autoconfig-nostore
+    // - - I personally feel that this is the best way to set it up because if OP dev changes GPS settings,
+    // - - you get them automatically changed to match whatever version of OP code you are running
+    // - - but 9600 is only OK for this autoconfig startup
+    // - by my testing, the 9600 initial to 57600 final baud startup takes:
+    // - - 0:10 if the GPS has been configured to send OP data at 9600
+    // - - 0:06 if the GPS has default data settings (NEMA) at 9600
+    // - - reminder that even 0:10 isn't that bad.  You need to wait for the GPS to acquire satellites,
+
+    // Some things you want to think about if you want to play around with this:
+    // - don't play with low baud rates, with OP data settings (lots of data) it can take a long time to auto-configure
+    // - - at 2400 it could take a long time to autoconfig or error out
+    // - - at 9600 or lower the autoconfig is skipped and only the baud rate is changed
+    // - if you autoconfigure an OP configuration at 9600 baud or lower
+    // - - it will actually make a factory default configuration (not an OP configuration) at that baud rate
+    // - remember that there are two baud rates (inside the GPS and the Revo port) and you can get them out of sync
+    // - - rebooting the Revo from the Ground Control Station (without powering the GPS down too)
+    // - - can leave the baud rates out of sync if you are using autoconfig
+    // - - just power off and on both the Revo and the GPS
+    // - my OP GPS #2 will NOT save 115200 baud or higher, but it will use all bauds, even 230400
+    // - - so you could use autoconfig.nostore with this high baud rate, but not autoconfig.store (once) followed by autoconfig.disabled
+    // - - I haven't tested other GPS's in regard to this
+
+    // since 9600 baud and lower are not usable, and are best left at NEMA, I could have coded it to do a factory reset
+    // - if set to 9600 baud (or lower)
+
+    if (gpsPort) {
+        HwSettingsGPSSpeedOptions speed;
+
+#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
+        // use 9600 baud during startup if autoconfig is enabled
+        // that is the flag that the code should assumes a factory default baud rate
+        if (ev == NULL && gpsSettings.UbxAutoConfig != GPSSETTINGS_UBXAUTOCONFIG_DISABLED) {
+            speed = HWSETTINGS_GPSSPEED_9600;
+        } else
+#endif
+        {
+            // Retrieve settings
+            HwSettingsGPSSpeedGet(&speed);
         }
+
+        // must always set the baud here, even if it looks like it is the same
+        // e.g. startup sets 9600 here, ubx_autoconfig sets 57600 there, then the user selects a change back to 9600 here
+
+        // on    startup (ev == NULL) we must set the Revo port baud rate
+        // after startup (ev != NULL) we must set the Revo port baud rate only if autoconfig is disabled
+        // always we must set the Revo port baud rate if not using UBX
+#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
+        if (ev == NULL || gpsSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_DISABLED || gpsSettings.DataProtocol != GPSSETTINGS_DATAPROTOCOL_UBX)
+#endif
+        {
+            // Set Revo port speed
+            switch (speed) {
+            case HWSETTINGS_GPSSPEED_2400:
+                PIOS_COM_ChangeBaud(gpsPort, 2400);
+                break;
+            case HWSETTINGS_GPSSPEED_4800:
+                PIOS_COM_ChangeBaud(gpsPort, 4800);
+                break;
+            case HWSETTINGS_GPSSPEED_9600:
+                PIOS_COM_ChangeBaud(gpsPort, 9600);
+                break;
+            case HWSETTINGS_GPSSPEED_19200:
+                PIOS_COM_ChangeBaud(gpsPort, 19200);
+                break;
+            case HWSETTINGS_GPSSPEED_38400:
+                PIOS_COM_ChangeBaud(gpsPort, 38400);
+                break;
+            case HWSETTINGS_GPSSPEED_57600:
+                PIOS_COM_ChangeBaud(gpsPort, 57600);
+                break;
+            case HWSETTINGS_GPSSPEED_115200:
+                PIOS_COM_ChangeBaud(gpsPort, 115200);
+                break;
+            case HWSETTINGS_GPSSPEED_230400:
+                PIOS_COM_ChangeBaud(gpsPort, 230400);
+                break;
+            }
+#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
+            // even changing the baud rate will make it re-verify the sensor type
+            // that way the user can just try some baud rates and it when the sensor type is valid, the baud rate is correct
+            ubx_reset_sensor_type();
+#endif
+        }
+#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
+        else {
+            // it will never do this during startup because ev == NULL
+            ubx_autoconfig_set(NULL);
+        }
+#endif
     }
 }
 
@@ -474,31 +584,26 @@ void AuxMagSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 
 void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev)
 {
-    uint8_t ubxAutoConfig;
-    uint8_t ubxDynamicModel;
-    uint8_t ubxSbasMode;
     ubx_autoconfig_settings_t newconfig;
-    uint8_t ubxSbasSats;
 
-    GPSSettingsUbxRateGet(&newconfig.navRate);
+    GPSSettingsGet(&gpsSettings);
 
-    GPSSettingsUbxAutoConfigGet(&ubxAutoConfig);
+    newconfig.navRate = gpsSettings.UbxRate;
-    newconfig.autoconfigEnabled = ubxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_DISABLED ? false : true;
-    newconfig.storeSettings     = ubxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGUREANDSTORE;
 
-    GPSSettingsUbxDynamicModelGet(&ubxDynamicModel);
+    newconfig.autoconfigEnabled = gpsSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_DISABLED ? false : true;
-    newconfig.dynamicModel = ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PORTABLE ? UBX_DYNMODEL_PORTABLE :
+    newconfig.storeSettings     = gpsSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGUREANDSTORE;
-                             ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_STATIONARY ? UBX_DYNMODEL_STATIONARY :
+
-                             ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PEDESTRIAN ? UBX_DYNMODEL_PEDESTRIAN :
+    newconfig.dynamicModel = gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PORTABLE ? UBX_DYNMODEL_PORTABLE :
-                             ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AUTOMOTIVE ? UBX_DYNMODEL_AUTOMOTIVE :
+                             gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_STATIONARY ? UBX_DYNMODEL_STATIONARY :
-                             ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_SEA ? UBX_DYNMODEL_SEA :
+                             gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PEDESTRIAN ? UBX_DYNMODEL_PEDESTRIAN :
-                             ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE1G ? UBX_DYNMODEL_AIRBORNE1G :
+                             gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AUTOMOTIVE ? UBX_DYNMODEL_AUTOMOTIVE :
-                             ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE2G ? UBX_DYNMODEL_AIRBORNE2G :
+                             gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_SEA ? UBX_DYNMODEL_SEA :
-                             ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE4G ? UBX_DYNMODEL_AIRBORNE4G :
+                             gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE1G ? UBX_DYNMODEL_AIRBORNE1G :
+                             gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE2G ? UBX_DYNMODEL_AIRBORNE2G :
+                             gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE4G ? UBX_DYNMODEL_AIRBORNE4G :
                              UBX_DYNMODEL_AIRBORNE1G;
 
-    GPSSettingsUbxSBASModeGet(&ubxSbasMode);
+    switch ((GPSSettingsUbxSBASModeOptions)gpsSettings.UbxSBASMode) {
-    switch ((GPSSettingsUbxSBASModeOptions)ubxSbasMode) {
     case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTION:
     case GPSSETTINGS_UBXSBASMODE_CORRECTION:
     case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTIONINTEGRITY:
@@ -509,7 +614,7 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev)
         newconfig.SBASCorrection = false;
     }
 
-    switch ((GPSSettingsUbxSBASModeOptions)ubxSbasMode) {
+    switch ((GPSSettingsUbxSBASModeOptions)gpsSettings.UbxSBASMode) {
     case GPSSETTINGS_UBXSBASMODE_RANGING:
     case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTION:
     case GPSSETTINGS_UBXSBASMODE_RANGINGINTEGRITY:
@@ -520,7 +625,7 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev)
         newconfig.SBASRanging = false;
     }
 
-    switch ((GPSSettingsUbxSBASModeOptions)ubxSbasMode) {
+    switch ((GPSSettingsUbxSBASModeOptions)gpsSettings.UbxSBASMode) {
     case GPSSETTINGS_UBXSBASMODE_INTEGRITY:
     case GPSSETTINGS_UBXSBASMODE_RANGINGINTEGRITY:
     case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTIONINTEGRITY:
@@ -531,17 +636,48 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev)
         newconfig.SBASIntegrity = false;
     }
 
-    GPSSettingsUbxSBASChannelsUsedGet(&newconfig.SBASChannelsUsed);
+    newconfig.SBASChannelsUsed = gpsSettings.UbxSBASChannelsUsed;
 
-    GPSSettingsUbxSBASSatsGet(&ubxSbasSats);
+    newconfig.SBASSats = gpsSettings.UbxSBASSats == GPSSETTINGS_UBXSBASSATS_WAAS ? UBX_SBAS_SATS_WAAS :
+                         gpsSettings.UbxSBASSats == GPSSETTINGS_UBXSBASSATS_EGNOS ? UBX_SBAS_SATS_EGNOS :
+                         gpsSettings.UbxSBASSats == GPSSETTINGS_UBXSBASSATS_MSAS ? UBX_SBAS_SATS_MSAS :
+                         gpsSettings.UbxSBASSats == GPSSETTINGS_UBXSBASSATS_GAGAN ? UBX_SBAS_SATS_GAGAN :
+                         gpsSettings.UbxSBASSats == GPSSETTINGS_UBXSBASSATS_SDCM ? UBX_SBAS_SATS_SDCM : UBX_SBAS_SATS_AUTOSCAN;
 
-    newconfig.SBASSats = ubxSbasSats == GPSSETTINGS_UBXSBASSATS_WAAS ? UBX_SBAS_SATS_WAAS :
+    switch (gpsSettings.UbxGNSSMode) {
-                         ubxSbasSats == GPSSETTINGS_UBXSBASSATS_EGNOS ? UBX_SBAS_SATS_EGNOS :
+    case GPSSETTINGS_UBXGNSSMODE_GPSGLONASS:
-                         ubxSbasSats == GPSSETTINGS_UBXSBASSATS_MSAS ? UBX_SBAS_SATS_MSAS :
+        newconfig.enableGPS     = true;
-                         ubxSbasSats == GPSSETTINGS_UBXSBASSATS_GAGAN ? UBX_SBAS_SATS_GAGAN :
+        newconfig.enableGLONASS = true;
-                         ubxSbasSats == GPSSETTINGS_UBXSBASSATS_SDCM ? UBX_SBAS_SATS_SDCM : UBX_SBAS_SATS_AUTOSCAN;
+        newconfig.enableBeiDou  = false;
+        break;
+    case GPSSETTINGS_UBXGNSSMODE_GLONASS:
+        newconfig.enableGPS     = false;
+        newconfig.enableGLONASS = true;
+        newconfig.enableBeiDou  = false;
+        break;
+    case GPSSETTINGS_UBXGNSSMODE_GPS:
+        newconfig.enableGPS     = true;
+        newconfig.enableGLONASS = false;
+        newconfig.enableBeiDou  = false;
+        break;
+    case GPSSETTINGS_UBXGNSSMODE_GPSBEIDOU:
+        newconfig.enableGPS     = true;
+        newconfig.enableGLONASS = false;
+        newconfig.enableBeiDou  = true;
+        break;
+    case GPSSETTINGS_UBXGNSSMODE_GLONASSBEIDOU:
+        newconfig.enableGPS     = false;
+        newconfig.enableGLONASS = true;
+        newconfig.enableBeiDou  = true;
+        break;
+    default:
+        newconfig.enableGPS     = false;
+        newconfig.enableGLONASS = false;
+        newconfig.enableBeiDou  = false;
+        break;
+    }
 
-    ubx_autoconfig_set(newconfig);
+    ubx_autoconfig_set(&newconfig);
 }
 #endif /* if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) */
 /**

flight/modules/GPS/UBX.c

@@ -43,7 +43,7 @@
 
 static bool useMag = false;
 #endif
-static GPSPositionSensorSensorTypeOptions sensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN;
+GPSPositionSensorSensorTypeOptions sensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN;
 
 static bool usePvt = false;
 static uint32_t lastPvtTime = 0;
@@ -107,7 +107,7 @@ struct UBX_ACK_NAK ubxLastNak;
 #define UBX_PVT_TIMEOUT (1000)
 // parse incoming character stream for messages in UBX binary format
 
-int parse_ubx_stream(uint8_t *rx, uint8_t len, char *gps_rx_buffer, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *gpsRxStats)
+int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *gpsRxStats)
 {
     int ret = PARSER_INCOMPLETE; // message not (yet) complete
     enum proto_states {
@@ -124,8 +124,8 @@ int parse_ubx_stream(uint8_t *rx, uint8_t len, char *gps_rx_buffer, GPSPositionS
     };
     uint8_t c;
     static enum proto_states proto_state = START;
-    static uint8_t rx_count = 0;
+    static uint16_t rx_count = 0;
-    struct UBXPacket *ubx   = (struct UBXPacket *)gps_rx_buffer;
+    struct UBXPacket *ubx    = (struct UBXPacket *)gps_rx_buffer;
 
     for (int i = 0; i < len; i++) {
         c = rx[i];
@@ -413,8 +413,10 @@ static void parse_ubx_nav_svinfo(struct UBXPacket *ubx, __attribute__((unused))
     struct UBX_NAV_SVINFO *svinfo = &ubx->payload.nav_svinfo;
 
     svdata.SatsInView = 0;
+
+    // First, use slots for SVs actually being received
     for (chan = 0; chan < svinfo->numCh; chan++) {
-        if (svdata.SatsInView < GPSSATELLITES_PRN_NUMELEM) {
+        if (svdata.SatsInView < GPSSATELLITES_PRN_NUMELEM && svinfo->sv[chan].cno > 0) {
             svdata.Azimuth[svdata.SatsInView]   = svinfo->sv[chan].azim;
             svdata.Elevation[svdata.SatsInView] = svinfo->sv[chan].elev;
             svdata.PRN[svdata.SatsInView] = svinfo->sv[chan].svid;
@@ -422,6 +424,18 @@ static void parse_ubx_nav_svinfo(struct UBXPacket *ubx, __attribute__((unused))
             svdata.SatsInView++;
         }
     }
+
+    // Now try to add the rest
+    for (chan = 0; chan < svinfo->numCh; chan++) {
+        if (svdata.SatsInView < GPSSATELLITES_PRN_NUMELEM && 0 == svinfo->sv[chan].cno) {
+            svdata.Azimuth[svdata.SatsInView]   = svinfo->sv[chan].azim;
+            svdata.Elevation[svdata.SatsInView] = svinfo->sv[chan].elev;
+            svdata.PRN[svdata.SatsInView] = svinfo->sv[chan].svid;
+            svdata.SNR[svdata.SatsInView] = svinfo->sv[chan].cno;
+            svdata.SatsInView++;
+        }
+    }
+
     // fill remaining slots (if any)
     for (chan = svdata.SatsInView; chan < GPSSATELLITES_PRN_NUMELEM; chan++) {
         svdata.Azimuth[chan]   = 0;
@@ -452,9 +466,11 @@ static void parse_ubx_mon_ver(struct UBXPacket *ubx, __attribute__((unused)) GPS
     struct UBX_MON_VER *mon_ver = &ubx->payload.mon_ver;
 
     ubxHwVersion = atoi(mon_ver->hwVersion);
-
     sensorType   = (ubxHwVersion >= 80000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX8 :
                    ((ubxHwVersion >= 70000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX7 : GPSPOSITIONSENSOR_SENSORTYPE_UBX);
+    // send sensor type right now because on UBX NEMA we don't get a full set of messages
+    // and we want to be able to see sensor type even on UBX NEMA GPS's
+    GPSPositionSensorSensorTypeSet((uint8_t *)&sensorType);
 }
 
 static void parse_ubx_op_sys(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition)

flight/modules/GPS/inc/UBX.h

@@ -102,6 +102,8 @@ typedef enum {
     UBX_ID_CFG_MSG  = 0x01,
     UBX_ID_CFG_CFG  = 0x09,
     UBX_ID_CFG_SBAS = 0x16,
+    UBX_ID_CFG_GNSS = 0x3E,
+    UBX_ID_CFG_PRT  = 0x00
 } ubx_class_cfg_id;
 
 typedef enum {
@@ -128,6 +130,39 @@ typedef enum {
     UBX_ID_RXM_SFRB = 0x11,
     UBX_ID_RXM_SVSI = 0x20,
 } ubx_class_rxm_id;
+
+typedef enum {
+    UBX_GNSS_ID_GPS     = 0,
+    UBX_GNSS_ID_SBAS    = 1,
+    UBX_GNSS_ID_GALILEO = 2,
+    UBX_GNSS_ID_BEIDOU  = 3,
+    UBX_GNSS_ID_IMES    = 4,
+    UBX_GNSS_ID_QZSS    = 5,
+    UBX_GNSS_ID_GLONASS = 6,
+    UBX_GNSS_ID_MAX
+} ubx_config_gnss_id_t;
+
+// Enumeration options for field UBXDynamicModel
+typedef enum {
+    UBX_DYNMODEL_PORTABLE   = 0,
+    UBX_DYNMODEL_STATIONARY = 2,
+    UBX_DYNMODEL_PEDESTRIAN = 3,
+    UBX_DYNMODEL_AUTOMOTIVE = 4,
+    UBX_DYNMODEL_SEA = 5,
+    UBX_DYNMODEL_AIRBORNE1G = 6,
+    UBX_DYNMODEL_AIRBORNE2G = 7,
+    UBX_DYNMODEL_AIRBORNE4G = 8
+} ubx_config_dynamicmodel_t;
+
+typedef enum {
+    UBX_SBAS_SATS_AUTOSCAN = 0,
+    UBX_SBAS_SATS_WAAS     = 1,
+    UBX_SBAS_SATS_EGNOS    = 2,
+    UBX_SBAS_SATS_MSAS     = 3,
+    UBX_SBAS_SATS_GAGAN    = 4,
+    UBX_SBAS_SATS_SDCM     = 5
+} ubx_config_sats_t;
+
 // private structures
 
 // Geodetic Position Solution
@@ -139,10 +174,9 @@ struct UBX_NAV_POSLLH {
     int32_t  hMSL;   // Height above mean sea level (mm)
     uint32_t hAcc; // Horizontal Accuracy Estimate (mm)
     uint32_t vAcc; // Vertical Accuracy Estimate (mm)
-};
+} __attribute__((packed));
 
 // Receiver Navigation Status
-
 #define STATUS_GPSFIX_NOFIX    0x00
 #define STATUS_GPSFIX_DRONLY   0x01
 #define STATUS_GPSFIX_2DFIX    0x02
@@ -163,7 +197,7 @@ struct UBX_NAV_STATUS {
     uint8_t  flags2;  // Additional navigation output information
     uint32_t ttff; // Time to first fix (ms)
     uint32_t msss; // Milliseconds since startup/reset (ms)
-};
+} __attribute__((packed));
 
 // Dilution of precision
 struct UBX_NAV_DOP {
@@ -175,10 +209,9 @@ struct UBX_NAV_DOP {
     uint16_t hDOP; // Horizontal DOP
     uint16_t nDOP; // Northing DOP
     uint16_t eDOP; // Easting DOP
-};
+} __attribute__((packed));
 
 // Navigation solution
-
 struct UBX_NAV_SOL {
     uint32_t iTOW; // GPS Millisecond Time of Week (ms)
     int32_t  fTOW;       // fractional nanoseconds (ns)
@@ -197,41 +230,9 @@ struct UBX_NAV_SOL {
     uint8_t  reserved1;  // Reserved
     uint8_t  numSV;      // Number of SVs used in Nav Solution
     uint32_t reserved2; // Reserved
-};
+} __attribute__((packed));
-
-// North/East/Down velocity
-
-struct UBX_NAV_VELNED {
-    uint32_t iTOW; // ms GPS Millisecond Time of Week
-    int32_t  velN;     // cm/s NED north velocity
-    int32_t  velE;     // cm/s NED east velocity
-    int32_t  velD;     // cm/s NED down velocity
-    uint32_t speed; // cm/s Speed (3-D)
-    uint32_t gSpeed; // cm/s Ground Speed (2-D)
-    int32_t  heading;  // 1e-5 *deg Heading of motion 2-D
-    uint32_t sAcc; // cm/s Speed Accuracy Estimate
-    uint32_t cAcc; // 1e-5 *deg Course / Heading Accuracy Estimate
-};
-
-// UTC Time Solution
-
-#define TIMEUTC_VALIDTOW (1 << 0)
-#define TIMEUTC_VALIDWKN (1 << 1)
-#define TIMEUTC_VALIDUTC (1 << 2)
-
-struct UBX_NAV_TIMEUTC {
-    uint32_t iTOW; // GPS Millisecond Time of Week (ms)
-    uint32_t tAcc; // Time Accuracy Estimate (ns)
-    int32_t  nano;   // Nanoseconds of second
-    uint16_t year;
-    uint8_t  month;
-    uint8_t  day;
-    uint8_t  hour;
-    uint8_t  min;
-    uint8_t  sec;
-    uint8_t  valid;  // Validity Flags
-};
 
+// PVT Navigation Position Velocity Time Solution
 #define PVT_VALID_VALIDDATE            0x01
 #define PVT_VALID_VALIDTIME            0x02
 #define PVT_VALID_FULLYRESOLVED        0x04
@@ -251,7 +252,6 @@ struct UBX_NAV_TIMEUTC {
 #define PVT_FLAGS_PSMSTATE_PO_TRACKING (4 << 2)
 #define PVT_FLAGS_PSMSTATE_INACTIVE    (5 << 2)
 
-// PVT Navigation Position Velocity Time Solution
 struct UBX_NAV_PVT {
     uint32_t iTOW;
     uint16_t year;
@@ -285,10 +285,39 @@ struct UBX_NAV_PVT {
     uint32_t reserved3;
 } __attribute__((packed));
 
+// North/East/Down velocity
+struct UBX_NAV_VELNED {
+    uint32_t iTOW; // ms GPS Millisecond Time of Week
+    int32_t  velN;     // cm/s NED north velocity
+    int32_t  velE;     // cm/s NED east velocity
+    int32_t  velD;     // cm/s NED down velocity
+    uint32_t speed; // cm/s Speed (3-D)
+    uint32_t gSpeed; // cm/s Ground Speed (2-D)
+    int32_t  heading;  // 1e-5 *deg Heading of motion 2-D
+    uint32_t sAcc; // cm/s Speed Accuracy Estimate
+    uint32_t cAcc; // 1e-5 *deg Course / Heading Accuracy Estimate
+} __attribute__((packed));
+
+// UTC Time Solution
+#define TIMEUTC_VALIDTOW (1 << 0)
+#define TIMEUTC_VALIDWKN (1 << 1)
+#define TIMEUTC_VALIDUTC (1 << 2)
+
+struct UBX_NAV_TIMEUTC {
+    uint32_t iTOW; // GPS Millisecond Time of Week (ms)
+    uint32_t tAcc; // Time Accuracy Estimate (ns)
+    int32_t  nano;   // Nanoseconds of second
+    uint16_t year;
+    uint8_t  month;
+    uint8_t  day;
+    uint8_t  hour;
+    uint8_t  min;
+    uint8_t  sec;
+    uint8_t  valid;  // Validity Flags
+} __attribute__((packed));
+
 // Space Vehicle (SV) Information
-
 // Single SV information block
-
 #define SVUSED     (1 << 0) // This SV is used for navigation
 #define DIFFCORR   (1 << 1) // Differential correction available
 #define ORBITAVAIL (1 << 2) // Orbit information available
@@ -307,10 +336,10 @@ struct UBX_NAV_SVINFO_SV {
     int8_t  elev;     // Elevation (integer degrees)
     int16_t azim; // Azimuth	(integer degrees)
     int32_t prRes; // Pseudo range residual (cm)
-};
+} __attribute__((packed));
 
 // SV information message
-#define MAX_SVS 16
+#define MAX_SVS 32
 
 struct UBX_NAV_SVINFO {
     uint32_t iTOW; // GPS Millisecond Time of Week (ms)
@@ -318,19 +347,180 @@ struct UBX_NAV_SVINFO {
     uint8_t  globalFlags;  //
     uint16_t reserved2; // Reserved
     struct UBX_NAV_SVINFO_SV sv[MAX_SVS]; // Repeated 'numCh' times
-};
+} __attribute__((packed));
 
 // ACK message class
-
 struct UBX_ACK_ACK {
     uint8_t clsID; // ClassID
     uint8_t msgID; // MessageID
-};
+} __attribute__((packed));
 
 struct UBX_ACK_NAK {
     uint8_t clsID; // ClassID
     uint8_t msgID; // MessageID
-};
+} __attribute__((packed));
+
+// Communication port information
+// default cfg_prt packet at 9600,N,8,1 (from u-center)
+// 0000  B5 62 06 00 14 00-01 00
+// 0008  00 00 D0 08 00 00 80 25
+// 0010  00 00 07 00 03 00 00 00
+// 0018  00 00-A2 B5
+#define UBX_CFG_PRT_PORTID_DDC       0
+#define UBX_CFG_PRT_PORTID_UART1     1
+#define UBX_CFG_PRT_PORTID_UART2     2
+#define UBX_CFG_PRT_PORTID_USB       3
+#define UBX_CFG_PRT_PORTID_SPI       4
+#define UBX_CFG_PRT_MODE_DATABITS5   0x00
+#define UBX_CFG_PRT_MODE_DATABITS6   0x40
+#define UBX_CFG_PRT_MODE_DATABITS7   0x80
+#define UBX_CFG_PRT_MODE_DATABITS8   0xC0
+#define UBX_CFG_PRT_MODE_EVENPARITY  0x000
+#define UBX_CFG_PRT_MODE_ODDPARITY   0x200
+#define UBX_CFG_PRT_MODE_NOPARITY    0x800
+#define UBX_CFG_PRT_MODE_STOPBITS1_0 0x0000
+#define UBX_CFG_PRT_MODE_STOPBITS1_5 0x1000
+#define UBX_CFG_PRT_MODE_STOPBITS2_0 0x2000
+#define UBX_CFG_PRT_MODE_STOPBITS0_5 0x3000
+#define UBX_CFG_PRT_MODE_RESERVED    0x10
+
+#define UBX_CFG_PRT_MODE_DEFAULT     (UBX_CFG_PRT_MODE_DATABITS8 | UBX_CFG_PRT_MODE_NOPARITY | UBX_CFG_PRT_MODE_STOPBITS1_0 | UBX_CFG_PRT_MODE_RESERVED)
+
+struct UBX_CFG_PRT {
+    uint8_t  portID;       // 1 or 2 for UART ports
+    uint8_t  res0;         // reserved
+    uint16_t res1; // reserved
+    uint32_t mode; // bit masks for databits, stopbits, parity, and non-zero reserved
+    uint32_t baudRate; // bits per second, 9600 means 9600
+    uint16_t inProtoMask; // bit 0 on = UBX, bit 1 on = NEMA
+    uint16_t outProtoMask; // bit 0 on = UBX, bit 1 on = NEMA
+    uint16_t flags; // reserved
+    uint16_t pad; // reserved
+} __attribute__((packed));
+
+struct UBX_CFG_MSG {
+    uint8_t msgClass;
+    uint8_t msgID;
+    uint8_t rate;
+} __attribute__((packed));
+
+struct UBX_CFG_RATE {
+    uint16_t measRate;
+    uint16_t navRate;
+    uint16_t timeRef;
+} __attribute__((packed));
+
+// Mask for "all supported devices": battery backed RAM, Flash, EEPROM, SPI Flash
+#define UBX_CFG_CFG_DEVICE_BBR        0x01
+#define UBX_CFG_CFG_DEVICE_FLASH      0x02
+#define UBX_CFG_CFG_DEVICE_EEPROM     0x04
+#define UBX_CFG_CFG_DEVICE_SPIFLASH   0x10
+#define UBX_CFG_CFG_DEVICE_ALL        (UBX_CFG_CFG_DEVICE_BBR | UBX_CFG_CFG_DEVICE_FLASH | UBX_CFG_CFG_DEVICE_EEPROM | UBX_CFG_CFG_DEVICE_SPIFLASH)
+#define UBX_CFG_CFG_SETTINGS_NONE     0x000
+#define UBX_CFG_CFG_SETTINGS_IOPORT   0x001
+#define UBX_CFG_CFG_SETTINGS_MSGCONF  0x002
+#define UBX_CFG_CFG_SETTINGS_INFMSG   0x004
+#define UBX_CFG_CFG_SETTINGS_NAVCONF  0x008
+#define UBX_CFG_CFG_SETTINGS_TPCONF   0x010
+#define UBX_CFG_CFG_SETTINGS_SFDRCONF 0x100
+#define UBX_CFG_CFG_SETTINGS_RINVCONF 0x200
+#define UBX_CFG_CFG_SETTINGS_ANTCONF  0x400
+
+#define UBX_CFG_CFG_SETTINGS_ALL \
+    (UBX_CFG_CFG_SETTINGS_IOPORT | \
+     UBX_CFG_CFG_SETTINGS_MSGCONF | \
+     UBX_CFG_CFG_SETTINGS_INFMSG | \
+     UBX_CFG_CFG_SETTINGS_NAVCONF | \
+     UBX_CFG_CFG_SETTINGS_TPCONF | \
+     UBX_CFG_CFG_SETTINGS_SFDRCONF | \
+     UBX_CFG_CFG_SETTINGS_RINVCONF | \
+     UBX_CFG_CFG_SETTINGS_ANTCONF)
+
+// Sent messages for configuration support
+struct UBX_CFG_CFG {
+    uint32_t clearMask;
+    uint32_t saveMask;
+    uint32_t loadMask;
+    uint8_t  deviceMask;
+} __attribute__((packed));
+
+#define UBX_CFG_SBAS_MODE_ENABLED    0x01
+#define UBX_CFG_SBAS_MODE_TEST       0x02
+#define UBX_CFG_SBAS_USAGE_RANGE     0x01
+#define UBX_CFG_SBAS_USAGE_DIFFCORR  0x02
+#define UBX_CFG_SBAS_USAGE_INTEGRITY 0x04
+
+// SBAS used satellite PNR bitmask (120-151)
+// -------------------------------------1---------1---------1---------1
+// -------------------------------------5---------4---------3---------2
+// ------------------------------------10987654321098765432109876543210
+// WAAS 122, 133, 134, 135, 138---------|---------|---------|---------|
+#define UBX_CFG_SBAS_SCANMODE1_WAAS  0b00000000000001001110000000000100
+// EGNOS 120, 124, 126, 131-------------|---------|---------|---------|
+#define UBX_CFG_SBAS_SCANMODE1_EGNOS 0b00000000000000000000100001010001
+// MSAS 129, 137------------------------|---------|---------|---------|
+#define UBX_CFG_SBAS_SCANMODE1_MSAS  0b00000000000000100000001000000000
+// GAGAN 127, 128-----------------------|---------|---------|---------|
+#define UBX_CFG_SBAS_SCANMODE1_GAGAN 0b00000000000000000000000110000000
+// SDCM 125, 140, 141-------------------|---------|---------|---------|
+#define UBX_CFG_SBAS_SCANMODE1_SDCM  0b00000000001100000000000000100000
+
+#define UBX_CFG_SBAS_SCANMODE2       0x00
+struct UBX_CFG_SBAS {
+    uint8_t  mode;
+    uint8_t  usage;
+    uint8_t  maxSBAS;
+    uint8_t  scanmode2;
+    uint32_t scanmode1;
+} __attribute__((packed));
+
+#define UBX_CFG_GNSS_FLAGS_ENABLED      0x000001
+#define UBX_CFG_GNSS_FLAGS_GPS_L1CA     0x010000
+#define UBX_CFG_GNSS_FLAGS_SBAS_L1CA    0x010000
+#define UBX_CFG_GNSS_FLAGS_BEIDOU_B1I   0x010000
+#define UBX_CFG_GNSS_FLAGS_QZSS_L1CA    0x010000
+#define UBX_CFG_GNSS_FLAGS_QZSS_L1SAIF  0x040000
+#define UBX_CFG_GNSS_FLAGS_GLONASS_L1OF 0x010000
+
+#define UBX_CFG_GNSS_NUMCH_VER7         22
+#define UBX_CFG_GNSS_NUMCH_VER8         32
+
+struct UBX_CFG_GNSS_CFGBLOCK {
+    uint8_t  gnssId;
+    uint8_t  resTrkCh;
+    uint8_t  maxTrkCh;
+    uint8_t  resvd;
+    uint32_t flags;
+} __attribute__((packed));
+
+struct UBX_CFG_GNSS {
+    uint8_t msgVer;
+    uint8_t numTrkChHw;
+    uint8_t numTrkChUse;
+    uint8_t numConfigBlocks;
+    struct UBX_CFG_GNSS_CFGBLOCK cfgBlocks[UBX_GNSS_ID_MAX];
+} __attribute__((packed));
+
+struct UBX_CFG_NAV5 {
+    uint16_t mask;
+    uint8_t  dynModel;
+    uint8_t  fixMode;
+    int32_t  fixedAlt;
+    uint32_t fixedAltVar;
+    int8_t   minElev;
+    uint8_t  drLimit;
+    uint16_t pDop;
+    uint16_t tDop;
+    uint16_t pAcc;
+    uint16_t tAcc;
+    uint8_t  staticHoldThresh;
+    uint8_t  dgpsTimeOut;
+    uint8_t  cnoThreshNumSVs;
+    uint8_t  cnoThresh;
+    uint16_t reserved2;
+    uint32_t reserved3;
+    uint32_t reserved4;
+} __attribute__((packed));
 
 // MON message Class
 #define UBX_MON_MAX_EXT 5
@@ -340,8 +530,7 @@ struct UBX_MON_VER {
 #if UBX_MON_MAX_EXT > 0
     char extension[UBX_MON_MAX_EXT][30];
 #endif
-};
+} __attribute__((packed));
-
 
 // OP custom messages
 struct UBX_OP_SYSINFO {
@@ -359,7 +548,7 @@ struct UBX_OP_MAG {
     int16_t  y;
     int16_t  z;
     uint16_t Status;
-};
+} __attribute__((packed));
 
 typedef union {
     uint8_t payload[0];
@@ -373,6 +562,7 @@ typedef union {
     struct UBX_NAV_PVT     nav_pvt;
     struct UBX_NAV_TIMEUTC nav_timeutc;
     struct UBX_NAV_SVINFO  nav_svinfo;
+    struct UBX_CFG_PRT     cfg_prt;
     // Ack Class
     struct UBX_ACK_ACK     ack_ack;
     struct UBX_ACK_NAK     ack_nak;
@@ -389,22 +579,47 @@ struct UBXHeader {
     uint16_t len;
     uint8_t  ck_a;
     uint8_t  ck_b;
-};
+} __attribute__((packed));
 
 struct UBXPacket {
     struct UBXHeader header;
     UBXPayload payload;
-};
+} __attribute__((packed));
+
+struct UBXSENTHEADER {
+    uint8_t  prolog[2];
+    uint8_t  class;
+    uint8_t  id;
+    uint16_t len;
+} __attribute__((packed));
+
+union UBXSENTPACKET {
+    uint8_t buffer[0];
+    struct {
+        struct UBXSENTHEADER header;
+        union {
+            struct UBX_CFG_CFG  cfg_cfg;
+            struct UBX_CFG_MSG  cfg_msg;
+            struct UBX_CFG_NAV5 cfg_nav5;
+            struct UBX_CFG_PRT  cfg_prt;
+            struct UBX_CFG_RATE cfg_rate;
+            struct UBX_CFG_SBAS cfg_sbas;
+            struct UBX_CFG_GNSS cfg_gnss;
+        } payload;
+        uint8_t resvd[2]; // added space for checksum bytes
+    } message;
+} __attribute__((packed));
 
 // Used by AutoConfig code
 extern int32_t ubxHwVersion;
+extern GPSPositionSensorSensorTypeOptions sensorType;
 extern struct UBX_ACK_ACK ubxLastAck;
 extern struct UBX_ACK_NAK ubxLastNak;
 
 bool checksum_ubx_message(struct UBXPacket *);
 uint32_t parse_ubx_message(struct UBXPacket *, GPSPositionSensorData *);
 
-int parse_ubx_stream(uint8_t *rx, uint8_t len, char *, GPSPositionSensorData *, struct GPS_RX_STATS *);
+int parse_ubx_stream(uint8_t *rx, uint16_t len, char *, GPSPositionSensorData *, struct GPS_RX_STATS *);
 void load_mag_settings();
 
 #endif /* UBX_H */

flight/modules/GPS/inc/ubx_autoconfig.h

@@ -33,20 +33,48 @@
 
 // defines
 // TODO: NEO8 max rate is for Rom version, flash is limited to 10Hz, need to handle that.
-#define UBX_MAX_RATE_VER8       18
+#define UBX_MAX_RATE_VER8 18
-#define UBX_MAX_RATE_VER7       10
+#define UBX_MAX_RATE_VER7 10
-#define UBX_MAX_RATE            5
+#define UBX_MAX_RATE      5
+
+// reset to factory (and 9600 baud), and baud changes are not acked
+// it could be 31 (full PIOS buffer) + 60 (gnss) + overhead bytes at 240 bytes per second
+// = .42 seconds for the longest sentence to be fully transmitted
+// and then it may have to do something like erase flash before replying...
+
+// at low baud rates and high data rates the ubx gps simply must drop some outgoing data
+// and when a lot of data is being dropped, the MON VER reply often gets dropped
+// on the other hand, uBlox documents that some versions discard data that is over a second old
+// implying that it could be over 1 second before a reply is received
+// later versions dropped this and drop data when the send buffer is full and that could be even longer
+// rather than have long timeouts, we will let timeouts * retries handle that if it happens
 
-// time to wait before reinitializing the fsm due to disconnection
-#define UBX_CONNECTION_TIMEOUT  (2000 * 1000)
-// times between retries in case an error does occurs
-#define UBX_ERROR_RETRY_TIMEOUT (1000 * 1000)
 // timeout for ack reception
-#define UBX_REPLY_TIMEOUT       (500 * 1000)
+#define UBX_REPLY_TIMEOUT             (500 * 1000)
+// timeout for a settings save, in case it has to erase flash
+#define UBX_REPLY_TO_SAVE_TIMEOUT     (3000 * 1000)
 // max retries in case of timeout
-#define UBX_MAX_RETRIES         5
+#define UBX_MAX_RETRIES               5
-// pause between each configuration step
+// pause between each verifiably correct configuration step
-#define UBX_STEP_WAIT_TIME      (10 * 1000)
+#define UBX_VERIFIED_STEP_WAIT_TIME   (50 * 1000)
+// pause between each unverifiably correct configuration step
+#define UBX_UNVERIFIED_STEP_WAIT_TIME (500 * 1000)
+
+#define UBX_CFG_CFG_OP_STORE_SETTINGS \
+    (UBX_CFG_CFG_SETTINGS_IOPORT | \
+     UBX_CFG_CFG_SETTINGS_MSGCONF | \
+     UBX_CFG_CFG_SETTINGS_NAVCONF)
+#define UBX_CFG_CFG_OP_CLEAR_SETTINGS ((~UBX_CFG_CFG_OP_STORE_SETTINGS) & UBX_CFG_CFG_SETTINGS_ALL)
+// don't clear rinv as that is just text as configured by the owner
+#define UBX_CFG_CFG_OP_RESET_SETTINGS \
+    (UBX_CFG_CFG_SETTINGS_IOPORT | \
+     UBX_CFG_CFG_SETTINGS_MSGCONF | \
+     UBX_CFG_CFG_SETTINGS_INFMSG | \
+     UBX_CFG_CFG_SETTINGS_NAVCONF | \
+     UBX_CFG_CFG_SETTINGS_TPCONF | \
+     UBX_CFG_CFG_SETTINGS_SFDRCONF | \
+     UBX_CFG_CFG_SETTINGS_ANTCONF)
+
 // types
 typedef enum {
     UBX_AUTOCONFIG_STATUS_DISABLED = 0,
@@ -54,26 +82,6 @@ typedef enum {
     UBX_AUTOCONFIG_STATUS_DONE,
     UBX_AUTOCONFIG_STATUS_ERROR
 } ubx_autoconfig_status_t;
-// Enumeration options for field UBXDynamicModel
-typedef enum {
-    UBX_DYNMODEL_PORTABLE   = 0,
-    UBX_DYNMODEL_STATIONARY = 2,
-    UBX_DYNMODEL_PEDESTRIAN = 3,
-    UBX_DYNMODEL_AUTOMOTIVE = 4,
-    UBX_DYNMODEL_SEA = 5,
-    UBX_DYNMODEL_AIRBORNE1G = 6,
-    UBX_DYNMODEL_AIRBORNE2G = 7,
-    UBX_DYNMODEL_AIRBORNE4G = 8
-} ubx_config_dynamicmodel_t;
-
-typedef enum {
-    UBX_SBAS_SATS_AUTOSCAN = 0,
-    UBX_SBAS_SATS_WAAS     = 1,
-    UBX_SBAS_SATS_EGNOS    = 2,
-    UBX_SBAS_SATS_MSAS     = 3,
-    UBX_SBAS_SATS_GAGAN    = 4,
-    UBX_SBAS_SATS_SDCM     = 5
-} ubx_config_sats_t;
 
 #define UBX_
 typedef struct {
@@ -88,105 +96,27 @@ typedef struct {
 
     int8_t  navRate;
     ubx_config_dynamicmodel_t dynamicModel;
+
+    bool    enableGPS;
+    bool    enableGLONASS;
+    bool    enableBeiDou;
 } ubx_autoconfig_settings_t;
 
-// Mask for "all supported devices": battery backed RAM, Flash, EEPROM, SPI Flash
-#define UBX_CFG_CFG_ALL_DEVICES_MASK (0x01 | 0x02 | 0x04 | 0x10)
-
 // Sent messages for configuration support
-typedef struct {
+typedef struct UBX_CFG_CFG ubx_cfg_cfg_t;
-    uint32_t clearMask;
+typedef struct UBX_CFG_NAV5 ubx_cfg_nav5_t;
-    uint32_t saveMask;
+typedef struct UBX_CFG_RATE ubx_cfg_rate_t;
-    uint32_t loadMask;
+typedef struct UBX_CFG_MSG ubx_cfg_msg_t;
-    uint8_t  deviceMask;
+typedef struct UBX_CFG_PRT ubx_cfg_prt_t;
-} __attribute__((packed)) ubx_cfg_cfg_t;
+typedef struct UBX_CFG_SBAS ubx_cfg_sbas_t;
+typedef struct UBX_CFG_GNSS_CFGBLOCK ubx_cfg_gnss_cfgblock_t;
+typedef struct UBX_CFG_GNSS ubx_cfg_gnss_t;
+typedef struct UBXSENTHEADER UBXSentHeader_t;
+typedef union  UBXSENTPACKET UBXSentPacket_t;
 
-typedef struct {
+void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send);
-    uint16_t mask;
+void ubx_autoconfig_set(ubx_autoconfig_settings_t *config);
-    uint8_t  dynModel;
+void ubx_reset_sensor_type();
-    uint8_t  fixMode;
-    int32_t  fixedAlt;
-    uint32_t fixedAltVar;
-    int8_t   minElev;
-    uint8_t  drLimit;
-    uint16_t pDop;
-    uint16_t tDop;
-    uint16_t pAcc;
-    uint16_t tAcc;
-    uint8_t  staticHoldThresh;
-    uint8_t  dgpsTimeOut;
-    uint8_t  cnoThreshNumSVs;
-    uint8_t  cnoThresh;
-    uint16_t reserved2;
-    uint32_t reserved3;
-    uint32_t reserved4;
-} __attribute__((packed)) ubx_cfg_nav5_t;
 
-typedef struct {
-    uint16_t measRate;
-    uint16_t navRate;
-    uint16_t timeRef;
-} __attribute__((packed)) ubx_cfg_rate_t;
-
-typedef struct {
-    uint8_t msgClass;
-    uint8_t msgID;
-    uint8_t rate;
-} __attribute__((packed)) ubx_cfg_msg_t;
-
-#define UBX_CFG_SBAS_MODE_ENABLED    0x01
-#define UBX_CFG_SBAS_MODE_TEST       0x02
-#define UBX_CFG_SBAS_USAGE_RANGE     0x01
-#define UBX_CFG_SBAS_USAGE_DIFFCORR  0x02
-#define UBX_CFG_SBAS_USAGE_INTEGRITY 0x04
-
-// SBAS used satellite PNR bitmask (120-151)
-// -------------------------------------1---------1---------1---------1
-// -------------------------------------5---------4---------3---------2
-// ------------------------------------10987654321098765432109876543210
-// WAAS 122, 133, 134, 135, 138---------|---------|---------|---------|
-#define UBX_CFG_SBAS_SCANMODE1_WAAS  0b00000000000001001110000000000100
-// EGNOS 120, 124, 126, 131-------------|---------|---------|---------|
-#define UBX_CFG_SBAS_SCANMODE1_EGNOS 0b00000000000000000000100001010001
-// MSAS 129, 137------------------------|---------|---------|---------|
-#define UBX_CFG_SBAS_SCANMODE1_MSAS  0b00000000000000100000001000000000
-// GAGAN 127, 128-----------------------|---------|---------|---------|
-#define UBX_CFG_SBAS_SCANMODE1_GAGAN 0b00000000000000000000000110000000
-// SDCM 125, 140, 141-------------------|---------|---------|---------|
-#define UBX_CFG_SBAS_SCANMODE1_SDCM  0b00000000001100000000000000100000
-
-#define UBX_CFG_SBAS_SCANMODE2       0x00
-typedef struct {
-    uint8_t  mode;
-    uint8_t  usage;
-    uint8_t  maxSBAS;
-    uint8_t  scanmode2;
-    uint32_t scanmode1;
-} __attribute__((packed)) ubx_cfg_sbas_t;
-
-typedef struct {
-    uint8_t  prolog[2];
-    uint8_t  class;
-    uint8_t  id;
-    uint16_t len;
-} __attribute__((packed)) UBXSentHeader_t;
-
-typedef union {
-    uint8_t buffer[0];
-    struct {
-        UBXSentHeader_t header;
-        union {
-            ubx_cfg_cfg_t  cfg_cfg;
-            ubx_cfg_msg_t  cfg_msg;
-            ubx_cfg_nav5_t cfg_nav5;
-            ubx_cfg_rate_t cfg_rate;
-            ubx_cfg_sbas_t cfg_sbas;
-        } payload;
-        uint8_t resvd[2]; // added space for checksum bytes
-    } message;
-} __attribute__((packed)) UBXSentPacket_t;
-
-void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send, bool gps_connected);
-void ubx_autoconfig_set(ubx_autoconfig_settings_t config);
 int32_t ubx_autoconfig_get_status();
 #endif /* UBX_AUTOCONFIG_H_ */

flight/modules/GPS/ubx_autoconfig.c

@@ -27,29 +27,48 @@
  * with this program; if not, write to the Free Software Foundation, Inc.,
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
+#include <openpilot.h>
+#include "hwsettings.h"
+
 #include "inc/ubx_autoconfig.h"
 #include <pios_mem.h>
+
 // private type definitions
+
 typedef enum {
     INIT_STEP_DISABLED = 0,
     INIT_STEP_START,
-    INIT_STEP_ASK_VER,
+    INIT_STEP_RESET_GPS,
-    INIT_STEP_WAIT_VER,
+    INIT_STEP_REVO_9600_BAUD,
+    INIT_STEP_GPS_BAUD,
+    INIT_STEP_REVO_BAUD,
     INIT_STEP_ENABLE_SENTENCES,
     INIT_STEP_ENABLE_SENTENCES_WAIT_ACK,
     INIT_STEP_CONFIGURE,
     INIT_STEP_CONFIGURE_WAIT_ACK,
+    INIT_STEP_SAVE,
+    INIT_STEP_SAVE_WAIT_ACK,
     INIT_STEP_DONE,
-    INIT_STEP_ERROR,
+    INIT_STEP_ERROR
 } initSteps_t;
 
 typedef struct {
-    initSteps_t        currentStep; // Current configuration "fsm" status
+    initSteps_t currentStep; // Current configuration "fsm" status
-    uint32_t           lastStepTimestampRaw; // timestamp of last operation
+    initSteps_t currentStepSave; // Current configuration "fsm" status
-    uint32_t           lastConnectedRaw; // timestamp of last time gps was connected
+    uint32_t    lastStepTimestampRaw; // timestamp of last operation
-    UBXSentPacket_t    working_packet; // outbound "buffer"
+    uint32_t    lastConnectedRaw; // timestamp of last time gps was connected
-    ubx_autoconfig_settings_t currentSettings;
+    struct {
-    int8_t lastConfigSent; // index of last configuration string sent
+        UBXSentPacket_t working_packet; // outbound "buffer"
+        // bufferPaddingForPiosBugAt2400Baud must exist for baud rate change to work at 2400 or 4800
+        // failure mode otherwise:
+        // - send message with baud rate change
+        // - wait 1 second (even at 2400, the baud rate change command should clear even an initially full 31 byte PIOS buffer much more quickly)
+        // - change Revo port baud rate
+        // sometimes fails (much worse for lowest baud rates)
+        uint8_t bufferPaddingForPiosBugAt2400Baud[2]; // must be at least 2 for 2400 to work, probably 1 for 4800 and 0 for 9600+
+    } __attribute__((packed));
+    volatile ubx_autoconfig_settings_t currentSettings;
+    int8_t  lastConfigSent;          // index of last configuration string sent
     struct UBX_ACK_ACK requiredAck; // Class and id of the message we are waiting for an ACK from GPS
     uint8_t retryCount;
 } status_t;
@@ -114,15 +133,24 @@ ubx_cfg_msg_t msg_config_ubx7[] = {
 };
 
 // private defines
+
 #define LAST_CONFIG_SENT_START     (-1)
 #define LAST_CONFIG_SENT_COMPLETED (-2)
+// always reset the stored GPS configuration, even when doing autoconfig.nostore
+// that is required to do a 100% correct configuration
+// but is unexpected because it changes the stored configuration when doing autoconfig.nostore
+// note that a reset is always done with autoconfig.store
+// #define ALWAYS_RESET
 
 // private variables
 
 // enable the autoconfiguration system
-static bool enabled;
+static volatile bool enabled = false;
+static volatile bool current_step_touched = false;
+// both the pointer and what it points to are volatile.  Yuk.
+static volatile status_t *volatile status = 0;
+static uint8_t hwsettings_baud;
 
-static status_t *status = 0;
 
 static void append_checksum(UBXSentPacket_t *packet)
 {
@@ -139,29 +167,152 @@ static void append_checksum(UBXSentPacket_t *packet)
     packet->buffer[len]     = ck_a;
     packet->buffer[len + 1] = ck_b;
 }
+
+
 /**
  * prepare a packet to be sent, fill the header and appends the checksum.
  * return the total packet lenght comprising header and checksum
  */
 static uint16_t prepare_packet(UBXSentPacket_t *packet, uint8_t classID, uint8_t messageID, uint16_t len)
 {
+    memset((uint8_t *)status->working_packet.buffer + len + sizeof(UBXSentHeader_t) + 2, 0, sizeof(status->bufferPaddingForPiosBugAt2400Baud));
     packet->message.header.prolog[0] = UBX_SYNC1;
     packet->message.header.prolog[1] = UBX_SYNC2;
     packet->message.header.class     = classID;
     packet->message.header.id  = messageID;
     packet->message.header.len = len;
     append_checksum(packet);
-    return packet->message.header.len + sizeof(UBXSentHeader_t) + 2; // header + payload + checksum
+
+    status->requiredAck.clsID  = classID;
+    status->requiredAck.msgID  = messageID;
+
+    return len + sizeof(UBXSentHeader_t) + 2 + sizeof(status->bufferPaddingForPiosBugAt2400Baud); // payload + header + checksum + extra bytes
 }
 
+
 static void build_request(UBXSentPacket_t *packet, uint8_t classID, uint8_t messageID, uint16_t *bytes_to_send)
 {
     *bytes_to_send = prepare_packet(packet, classID, messageID, 0);
 }
 
-void config_rate(uint16_t *bytes_to_send)
+
+static void set_current_step_if_untouched(initSteps_t new_steps)
 {
-    memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_rate_t));
+    // assume this one byte initSteps_t is atomic
+    // take care of some but not all concurrency issues
+
+    if (!current_step_touched) {
+        status->currentStep = new_steps;
+    }
+    if (current_step_touched) {
+        status->currentStep = status->currentStepSave;
+    }
+}
+
+
+void ubx_reset_sensor_type()
+{
+    ubxHwVersion = -1;
+    sensorType   = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN;
+    GPSPositionSensorSensorTypeSet((uint8_t *)&sensorType);
+}
+
+
+static void config_reset(uint16_t *bytes_to_send)
+{
+    memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_cfg_t));
+    // mask LSB=ioPort|msgConf|infMsg|navConf|rxmConf|||||rinvConf|antConf|....|= MSB
+    // ioPort=1, msgConf=2, infMsg=4, navConf=8, tpConf=0x10, sfdrConf=0x100, rinvConf=0x200, antConf=0x400
+    // first: reset (permanent settings to default) all but rinv = e.g. owner name
+    status->working_packet.message.payload.cfg_cfg.clearMask  = UBX_CFG_CFG_OP_RESET_SETTINGS;
+    // then: don't store any current settings to permanent
+    status->working_packet.message.payload.cfg_cfg.saveMask   = UBX_CFG_CFG_SETTINGS_NONE;
+    // lastly: load (immediately start to use) all but rinv = e.g. owner name
+    status->working_packet.message.payload.cfg_cfg.loadMask   = UBX_CFG_CFG_OP_RESET_SETTINGS;
+    // all devices
+    status->working_packet.message.payload.cfg_cfg.deviceMask = UBX_CFG_CFG_DEVICE_ALL;
+
+    *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_CFG, sizeof(ubx_cfg_cfg_t));
+}
+
+
+// set the GPS baud rate to the user specified baud rate
+// because we may have started up with 9600 baud (for a GPS with no permanent settings)
+static void config_gps_baud(uint16_t *bytes_to_send)
+{
+    memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_prt_t));
+    status->working_packet.message.payload.cfg_prt.mode   = UBX_CFG_PRT_MODE_DEFAULT; // 8databits, 1stopbit, noparity, and non-zero reserved
+    status->working_packet.message.payload.cfg_prt.portID = 1; // 1 = UART1, 2 = UART2
+    status->working_packet.message.payload.cfg_prt.inProtoMask  = 1; // 1 = UBX only (bit 0)
+    status->working_packet.message.payload.cfg_prt.outProtoMask = 1; // 1 = UBX only (bit 0)
+    // Ask GPS to change it's speed
+    switch (hwsettings_baud) {
+    case HWSETTINGS_GPSSPEED_2400:
+        status->working_packet.message.payload.cfg_prt.baudRate = 2400;
+        break;
+    case HWSETTINGS_GPSSPEED_4800:
+        status->working_packet.message.payload.cfg_prt.baudRate = 4800;
+        break;
+    case HWSETTINGS_GPSSPEED_9600:
+        status->working_packet.message.payload.cfg_prt.baudRate = 9600;
+        break;
+    case HWSETTINGS_GPSSPEED_19200:
+        status->working_packet.message.payload.cfg_prt.baudRate = 19200;
+        break;
+    case HWSETTINGS_GPSSPEED_38400:
+        status->working_packet.message.payload.cfg_prt.baudRate = 38400;
+        break;
+    case HWSETTINGS_GPSSPEED_57600:
+        status->working_packet.message.payload.cfg_prt.baudRate = 57600;
+        break;
+    case HWSETTINGS_GPSSPEED_115200:
+        status->working_packet.message.payload.cfg_prt.baudRate = 115200;
+        break;
+    case HWSETTINGS_GPSSPEED_230400:
+        status->working_packet.message.payload.cfg_prt.baudRate = 230400;
+        break;
+    }
+
+    *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_PRT, sizeof(ubx_cfg_prt_t));
+}
+
+
+// having already set the GPS's baud rate with a serial command, set the local Revo port baud rate
+static void config_baud(uint8_t baud)
+{
+    // Set Revo port hwsettings_baud
+    switch (baud) {
+    case HWSETTINGS_GPSSPEED_2400:
+        PIOS_COM_ChangeBaud(PIOS_COM_GPS, 2400);
+        break;
+    case HWSETTINGS_GPSSPEED_4800:
+        PIOS_COM_ChangeBaud(PIOS_COM_GPS, 4800);
+        break;
+    case HWSETTINGS_GPSSPEED_9600:
+        PIOS_COM_ChangeBaud(PIOS_COM_GPS, 9600);
+        break;
+    case HWSETTINGS_GPSSPEED_19200:
+        PIOS_COM_ChangeBaud(PIOS_COM_GPS, 19200);
+        break;
+    case HWSETTINGS_GPSSPEED_38400:
+        PIOS_COM_ChangeBaud(PIOS_COM_GPS, 38400);
+        break;
+    case HWSETTINGS_GPSSPEED_57600:
+        PIOS_COM_ChangeBaud(PIOS_COM_GPS, 57600);
+        break;
+    case HWSETTINGS_GPSSPEED_115200:
+        PIOS_COM_ChangeBaud(PIOS_COM_GPS, 115200);
+        break;
+    case HWSETTINGS_GPSSPEED_230400:
+        PIOS_COM_ChangeBaud(PIOS_COM_GPS, 230400);
+        break;
+    }
+}
+
+
+static void config_rate(uint16_t *bytes_to_send)
+{
+    memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_rate_t));
     // if rate is less than 1 uses the highest rate for current hardware
     uint16_t rate = status->currentSettings.navRate > 0 ? status->currentSettings.navRate : 99;
     if (ubxHwVersion < UBX_HW_VERSION_7 && rate > UBX_MAX_RATE) {
@@ -172,36 +323,31 @@ void config_rate(uint16_t *bytes_to_send)
         rate = UBX_MAX_RATE_VER8;
     }
     uint16_t period = 1000 / rate;
-
     status->working_packet.message.payload.cfg_rate.measRate = period;
     status->working_packet.message.payload.cfg_rate.navRate  = 1; // must be set to 1
     status->working_packet.message.payload.cfg_rate.timeRef  = 1; // 0 = UTC Time, 1 = GPS Time
-    *bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_RATE, sizeof(ubx_cfg_rate_t));
+
-    status->requiredAck.clsID = UBX_CLASS_CFG;
+    *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_RATE, sizeof(ubx_cfg_rate_t));
-    status->requiredAck.msgID = UBX_ID_CFG_RATE;
 }
 
-void config_nav(uint16_t *bytes_to_send)
-{
-    memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_nav5_t));
 
+static void config_nav(uint16_t *bytes_to_send)
+{
+    memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_nav5_t));
     status->working_packet.message.payload.cfg_nav5.dynModel = status->currentSettings.dynamicModel;
     status->working_packet.message.payload.cfg_nav5.fixMode  = 2; // 1=2D only, 2=3D only, 3=Auto 2D/3D
     // mask LSB=dyn|minEl|posFixMode|drLim|posMask|statisticHoldMask|dgpsMask|......|reservedBit0 = MSB
-
     status->working_packet.message.payload.cfg_nav5.mask     = 0x01 + 0x04; // Dyn Model | posFixMode configuration
-    *bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_NAV5, sizeof(ubx_cfg_nav5_t));
+
-    status->requiredAck.clsID = UBX_CLASS_CFG;
+    *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_NAV5, sizeof(ubx_cfg_nav5_t));
-    status->requiredAck.msgID = UBX_ID_CFG_NAV5;
 }
 
-void config_sbas(uint16_t *bytes_to_send)
+
+static void config_sbas(uint16_t *bytes_to_send)
 {
-    memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_sbas_t));
+    memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_sbas_t));
-
+    status->working_packet.message.payload.cfg_sbas.maxSBAS =
-    status->working_packet.message.payload.cfg_sbas.maxSBAS = status->currentSettings.SBASChannelsUsed < 4 ?
+        status->currentSettings.SBASChannelsUsed < 4 ? status->currentSettings.SBASChannelsUsed : 3;
-                                                              status->currentSettings.SBASChannelsUsed : 3;
-
     status->working_packet.message.payload.cfg_sbas.usage   =
         (status->currentSettings.SBASCorrection ? UBX_CFG_SBAS_USAGE_DIFFCORR : 0) |
         (status->currentSettings.SBASIntegrity ? UBX_CFG_SBAS_USAGE_INTEGRITY : 0) |
@@ -209,58 +355,119 @@ void config_sbas(uint16_t *bytes_to_send)
     // If sbas is used for anything then set mode as enabled
     status->working_packet.message.payload.cfg_sbas.mode =
         status->working_packet.message.payload.cfg_sbas.usage != 0 ? UBX_CFG_SBAS_MODE_ENABLED : 0;
-
     status->working_packet.message.payload.cfg_sbas.scanmode1 =
         status->currentSettings.SBASSats == UBX_SBAS_SATS_WAAS ? UBX_CFG_SBAS_SCANMODE1_WAAS :
         status->currentSettings.SBASSats == UBX_SBAS_SATS_EGNOS ? UBX_CFG_SBAS_SCANMODE1_EGNOS :
         status->currentSettings.SBASSats == UBX_SBAS_SATS_MSAS ? UBX_CFG_SBAS_SCANMODE1_MSAS :
         status->currentSettings.SBASSats == UBX_SBAS_SATS_GAGAN ? UBX_CFG_SBAS_SCANMODE1_GAGAN :
         status->currentSettings.SBASSats == UBX_SBAS_SATS_SDCM ? UBX_CFG_SBAS_SCANMODE1_SDCM : UBX_SBAS_SATS_AUTOSCAN;
+    status->working_packet.message.payload.cfg_sbas.scanmode2 =
+        UBX_CFG_SBAS_SCANMODE2;
 
-    status->working_packet.message.payload.cfg_sbas.scanmode2 = UBX_CFG_SBAS_SCANMODE2;
+    *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_SBAS, sizeof(ubx_cfg_sbas_t));
-
-    *bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_SBAS, sizeof(ubx_cfg_sbas_t));
-    status->requiredAck.clsID = UBX_CLASS_CFG;
-    status->requiredAck.msgID = UBX_ID_CFG_SBAS;
 }
 
-void config_save(uint16_t *bytes_to_send)
+
+static void config_gnss(uint16_t *bytes_to_send)
 {
-    memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_cfg_t));
+    memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_gnss_t));
-    // mask LSB=ioPort|msgConf|infMsg|navConf|rxmConf|||||rinvConf|antConf|....|= MSB
+    status->working_packet.message.payload.cfg_gnss.numConfigBlocks = UBX_GNSS_ID_MAX;
-    status->working_packet.message.payload.cfg_cfg.saveMask   = 0x02 | 0x08; // msgConf + navConf
+    status->working_packet.message.payload.cfg_gnss.numTrkChHw = (ubxHwVersion > UBX_HW_VERSION_7) ? UBX_CFG_GNSS_NUMCH_VER8 : UBX_CFG_GNSS_NUMCH_VER7;
-    status->working_packet.message.payload.cfg_cfg.deviceMask = UBX_CFG_CFG_ALL_DEVICES_MASK;
+    status->working_packet.message.payload.cfg_gnss.numTrkChUse     = status->working_packet.message.payload.cfg_gnss.numTrkChHw;
-    *bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_CFG, sizeof(ubx_cfg_cfg_t));
+
-    status->requiredAck.clsID = UBX_CLASS_CFG;
+    for (int32_t i = 0; i < UBX_GNSS_ID_MAX; i++) {
-    status->requiredAck.msgID = UBX_ID_CFG_CFG;
+        status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].gnssId = i;
+        switch (i) {
+        case UBX_GNSS_ID_GPS:
+            if (status->currentSettings.enableGPS) {
+                status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].flags    = UBX_CFG_GNSS_FLAGS_ENABLED | UBX_CFG_GNSS_FLAGS_GPS_L1CA;
+                status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].maxTrkCh = 16;
+                status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].resTrkCh = 8;
+            }
+            break;
+        case UBX_GNSS_ID_QZSS:
+            if (status->currentSettings.enableGPS) {
+                status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].flags    = UBX_CFG_GNSS_FLAGS_ENABLED | UBX_CFG_GNSS_FLAGS_QZSS_L1CA;
+                status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].maxTrkCh = 3;
+                status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].resTrkCh = 0;
+            }
+            break;
+        case UBX_GNSS_ID_SBAS:
+            if (status->currentSettings.SBASCorrection || status->currentSettings.SBASIntegrity || status->currentSettings.SBASRanging) {
+                status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].flags    = UBX_CFG_GNSS_FLAGS_ENABLED | UBX_CFG_GNSS_FLAGS_SBAS_L1CA;
+                status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].maxTrkCh = status->currentSettings.SBASChannelsUsed < 4 ? status->currentSettings.SBASChannelsUsed : 3;
+                status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].resTrkCh = 1;
+            }
+            break;
+        case UBX_GNSS_ID_GLONASS:
+            if (status->currentSettings.enableGLONASS) {
+                status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].flags    = UBX_CFG_GNSS_FLAGS_ENABLED | UBX_CFG_GNSS_FLAGS_GLONASS_L1OF;
+                status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].maxTrkCh = 14;
+                status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].resTrkCh = 8;
+            }
+            break;
+        case UBX_GNSS_ID_BEIDOU:
+            if (status->currentSettings.enableBeiDou) {
+                status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].flags    = UBX_CFG_GNSS_FLAGS_ENABLED | UBX_CFG_GNSS_FLAGS_BEIDOU_B1I;
+                status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].maxTrkCh = 14;
+                status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].resTrkCh = 8;
+            }
+            break;
+        default:
+            break;
+        }
+    }
+
+    *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_GNSS, sizeof(ubx_cfg_gnss_t));
 }
+
+
+static void config_save(uint16_t *bytes_to_send)
+{
+    memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_cfg_t));
+    // mask LSB=ioPort|msgConf|infMsg|navConf|rxmConf|||||rinvConf|antConf|....|= MSB
+    // ioPort=1, msgConf=2, infMsg=4, navConf=8, tpConf=0x10, sfdrConf=0x100, rinvConf=0x200, antConf=0x400
+    status->working_packet.message.payload.cfg_cfg.saveMask   = UBX_CFG_CFG_OP_STORE_SETTINGS; // a list of settings we just set
+    status->working_packet.message.payload.cfg_cfg.clearMask  = UBX_CFG_CFG_OP_CLEAR_SETTINGS; // everything else gets factory default
+    status->working_packet.message.payload.cfg_cfg.deviceMask = UBX_CFG_CFG_DEVICE_ALL;
+
+    *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_CFG, sizeof(ubx_cfg_cfg_t));
+}
+
+
 static void configure(uint16_t *bytes_to_send)
 {
     switch (status->lastConfigSent) {
     case LAST_CONFIG_SENT_START:
+        // increase message rates to 5 fixes per second
         config_rate(bytes_to_send);
         break;
+
     case LAST_CONFIG_SENT_START + 1:
         config_nav(bytes_to_send);
         break;
+
     case LAST_CONFIG_SENT_START + 2:
-        config_sbas(bytes_to_send);
+        if (status->currentSettings.enableGLONASS || status->currentSettings.enableGPS) {
-        if (!status->currentSettings.storeSettings) {
+            config_gnss(bytes_to_send);
-            // skips saving
+            break;
-            status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED;
+        } else {
+            // Skip and fall through to next step
+            status->lastConfigSent++;
         }
-        break;
+    // in the else case we must fall through because we must send something each time because successful send is tested externally
+
     case LAST_CONFIG_SENT_START + 3:
-        config_save(bytes_to_send);
+        config_sbas(bytes_to_send);
-        status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED;
+        break;
-        return;
 
     default:
         status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED;
-        return;
+        break;
     }
 }
 
+
 static void enable_sentences(__attribute__((unused)) uint16_t *bytes_to_send)
 {
     int8_t msg = status->lastConfigSent + 1;
@@ -271,76 +478,303 @@ static void enable_sentences(__attribute__((unused)) uint16_t *bytes_to_send)
 
     if (msg >= 0 && msg < msg_count) {
         status->working_packet.message.payload.cfg_msg = msg_config[msg];
-
+        *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_MSG, sizeof(ubx_cfg_msg_t));
-        *bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_MSG, sizeof(ubx_cfg_msg_t));
-        status->requiredAck.clsID = UBX_CLASS_CFG;
-        status->requiredAck.msgID = UBX_ID_CFG_MSG;
     } else {
         status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED;
     }
 }
 
-void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send, bool gps_connected)
+
+// End User Documentation
+
+// There are two baud rates of interest
+// The baud rate the GPS is talking at
+// The baud rate Revo is talking at
+// These two must match for the GPS to work
+// You only have direct control of the Revo baud rate
+// The two baud rates must be the same for the Revo to send a command to the GPS
+// to tell the GPS to change it's baud rate
+// So you start out by changing Revo's baud rate to match the GPS's
+// and then enable UbxAutoConfig to tell Revo to change the GPS baud every time, just before it changes the Revo baud
+// That is the basis of these instructions
+
+// There are microprocessors and they each have internal settings
+// Revo
+// GPS
+// and each of these settings can be temporary or permanent
+
+// To change a Revo setting
+// Use the System tab in the GCS for all the following
+// Example: in Settings->GPSSettings click on the VALUE for UbxAutoConfig and change it to Disabled
+// Click on UbxAutoConfig itself and the line will turn green and blue
+// To change this setting permanently, press the  red  up arrow (Save) at the top of the screen
+// Permanently means that it uses this setting, even if you reboot Revo, e.g. power off and on
+// To change this setting temporarily, press the green up arrow (Send) at the top of the screen
+// Temporarily means that it overrides the permanent setting, but it goes back to the permanent setting when you reboot Revo, e.g. power off and on
+
+// To change an internal GPS setting you use the OP GCS System tab to tell Revo to make the GPS changes
+// This only works correctly after you have matching baud rates so Revo and GPS can talk together
+// "Settings->GPSSettings->UbxAutoConfig = Configure"         sets the internal GPS setting temporarily
+// "Settings->GPSSettings->UbxAutoConfig = ConfigureAndStore" sets the internal GPS setting permanently
+
+// You want to wind up with a set of permanent settings that work together
+// There are two different sets of permanent settings that work together
+// GPS at 9600 baud and factory defaults
+// Revo configured to start out at 9600 baud, but then completely configure the GPS and switch both to 57600 baud
+// (takes 6 seconds at boot up while you are waiting for it to acquire satellites anyway)
+// This is the preferred way so that if we change the settings in the future, the new release will automatically use the correct settings
+// GPS at 57600 baud with all the settings for the current release stored in the GPS
+// Revo configured to disable UbxAutoConfig since all the GPS settings are permanently stored correctly
+// May require reconfiguring in a future release
+
+// Changable settings of interest
+// AutoConfig mode
+// Settings->GPSSettings->UbxAutoConfig (Disabled, Configure, ConfigureAndStore, default=Configure)
+// Disabled means that changes to the GPS baud setting only affect the Revo port
+// It doesn't try to change the GPS's internal baud rate setting
+// Configure means change the GPS's internal baud setting temporarily (GPS settings revert to the permanent values when GPS is powered off/on)
+// ConfigureAndStore means change the GPS's internal baud setting permanently (even after the GPS is powered off/on)
+// GPS baud rate
+// Settings->HwSettings->GPSSpeed
+// If the baud rates are the same and an AutoConfig mode is enabled this will change both the GPS baud rate and the Revo baud rate
+// If the baud rates are not the same and an AutoConfig mode is enabled it will fail
+// If AutoConfig mode is disabled this will only change the Revo baud rate
+
+// View only settings of interest
+// Detected GPS type
+// Data Objects -> GPSPositionSensor -> SensorType (Unknown, NMEA, UBX, UBX7, UBX8)
+// When it says something other than Unknown, the GPS and Revo baud rates are synced and talking
+// Real time progress of the GPS detection process
+// Data Objects -> GPSPositionSensor -> AutoConfigStatus (DISABLED, RUNNING, DONE, ERROR)
+
+// Syncing the baud rates means that the GPS's internal baud rate setting is the same as the Revo port setting
+// This is necessary for the GPS to work with Revo
+// To sync to and find out an unknown GPS baud rate (or sync to and use a known GPS baud rate)
+// Temporarily change the AutoConfig mode to Disabled
+// Temporarily change the GPS baud rate to a value you think it might be (or go up the list)
+// See if that baud rate is correct (Data Objects->GPSPositionSensor->SensorType will be something besides Unknown)
+// Repeat, changing the GPS baud rate, until found
+
+// Some very important facts:
+// For 9600 baud or lower, the autoconfig will configure it to factory default settings
+// For 19200 baud or higher, the autoconfig will configure it to OP required settings
+// If autoconfig is enabled permanently in Revo, it will assume that the GPS is configured to power up at 9600 baud
+// 57600 baud is recommended for the current release
+// That can be achieved either by
+// autoconfiging the GPS from a permanent 9600 baud (and factory settings) to a temporary 57600 (with OP settings) on each power up
+// or by configuring the GPS with a permanent 57600 (with OP settings) and then permanently disabling autoconfig
+// Some previous releases used 38400 and had some other settings differences
+
+// The user should either:
+// Permanently configure their GPS to 9600 baud factory settings and tell the Revo configuration to load volatile settings at each startup by:
+// (Recommended method because new versions could require new settings and this handles future changes automatically)
+// Syncing the baud rates
+// Setting it to autoconfig.nostore and waiting for it to complete
+// Setting HwSettings.GPSSpeed to 9600 and waiting for it to complete
+// Setting it to autoconfig.store and waiting for it to complete (this tells the GPS to store the 9600 permanently)
+// Permanently setting it to autoconfig.nostore and waiting for it to complete
+// Permanently setting HwSettings.GPSSpeed to 57600 and waiting for it to complete
+// Permanently configure their GPS to 57600 baud, including OpenPilot settings and telling the Revo configuration to just set the baud to 57600 at each startup by:
+// (Less recommended method because new versions could require new settings so you would have to do this again)
+// Syncing the baud rates
+// Setting it to autoconfig.nostore and waiting for it to complete
+// Permanently setting HwSettings.GPSSpeed to 57600 and waiting for it to complete
+// Setting it to autoconfig.store
+// Permanently setting it to autoconfig.disabled
+
+// The algorithm is:
+// If autoconfig is enabled at all
+// It will assume that the GPS boot up baud rate is 9600 and the user wants that changed to HwSettings.GPSSpeed
+// and that change can be either volatile (must be done each boot up) or non-volatile (stored in GPS's non-volatile settings storage)
+// according to whether CONFIGURE is used or CONFIGUREANDSTORE is used
+// The only user who should need CONFIGUREANDSTORE stored permanently in Revo is Dave, who configures many OP GPS's before shipping
+// plug a factory default GPS in to a Revo, power up, wait for it to configure and permanently store in the GPS, power down, ship
+// If autoconfig is not enabled
+// it will use HwSettings.GPSSpeed for the baud rate and not do any configuration changes
+// If GPSSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGUREANDSTORE it will
+// 1 Reset the permanent configuration back to factory default
+// 2 Disable NEMA message settings
+// 3 Add some volatile UBX settings to the copies of the non-volatile ones that are currently running
+// 4 Save the current volatile settings to non-volatile storage
+// If GPSSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGURE it will
+// 2 Disable NEMA message settings
+// 3 Add some volatile UBX settings to the copies of the non-volatile ones that are currently running
+// If the requested baud rate is 9600 or less it skips the step (3) of adding some volatile UBX settings
+
+// Talking points to point out:
+// U-center is no longer needed for any use case with this code
+// 9600 is factory default for GPS's
+// Some GPS can't even permanently store settings and must start at 9600 baud?
+// I have a GPS that sometimes looses settings and reverts to 9600 and this is a fix for that too :)
+// This code handles a GPS configured either way (9600 with factory default settings or e.g. 57600 with OP settings)
+// Autoconfig.nostore at each boot for 9600, autoconfig.disabled for the 57600 with OP settings (or custom settings and baud)
+// This code can permanently configure a GPS to be e.g. 9600 with factory default settings or 57600 with OP settings
+// GPS's with 9600 baud and factory default settings would be a good default for future OP releases
+// Changing the GPS internal settings multiple times in the future is handled automatically
+// This code is written to do a configure from 9600 to 57600
+// (actually 9600 to whatever is stored in HwSettings.GPSSpeed)
+// if autoconfig is enabled at boot up
+// When autoconfiging to 9600 baud or lower, the autoconfig will configure it to factory default settings, not OP settings
+// That is because 9600 baud drops many of the OP messages and because 9600 baud is factory default
+// For 19200 baud or higher, the autoconfig will configure it to OP required settings
+// If autoconfig is enabled permanently in Revo, it will assume that the GPS is configured to power up at 9600 baud
+// This is good for factory default GPS's
+// This is good in case we change some settings in a future release
+
+
+void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
 {
     *bytes_to_send = 0;
     *buffer = (char *)status->working_packet.buffer;
-    if (!status || !enabled || status->currentStep == INIT_STEP_DISABLED) {
+    current_step_touched = false;
-        return; // autoconfig not enabled
+
-    }
+    // autoconfig struct not yet allocated
-    if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_STEP_WAIT_TIME) {
+    if (!status) {
         return;
     }
-    // when gps is disconnected it will replay the autoconfig sequence.
+    // smallest delay between each step
-    if (!gps_connected) {
+    if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_VERIFIED_STEP_WAIT_TIME) {
-        if (status->currentStep == INIT_STEP_DONE) {
+        return;
-            // if some operation is in progress and it is not running into issues it maybe that
+    }
-            // the disconnection is only due to wrong message rates, so reinit only when done.
+    // get UBX version whether autoconfig is enabled or not
-            // errors caused by disconnection are handled by error retry logic
+    // this allows the user to try some baud rates and visibly see when it works
-            if (PIOS_DELAY_DiffuS(status->lastConnectedRaw) > UBX_CONNECTION_TIMEOUT) {
+    // ubxHwVersion is a global set externally by the caller of this function
-                status->currentStep = INIT_STEP_START;
+    if (ubxHwVersion <= 0) {
-                return;
+        // at low baud rates and high data rates the ubx gps simply must drop some outgoing data
-            }
+        // this isn't really an error
+        // and when a lot of data is being dropped, the MON VER reply often gets dropped
+        // on the other hand, uBlox documents that some versions discard data that is over 1 second old
+        // implying a 1 second send buffer and that it could be over 1 second before a reply is received
+        // later uBlox versions dropped this 1 second constraint and drop data when the send buffer is full
+        // and that could be even longer than 1 second
+        // send this more quickly and it will get a reply more quickly if a fixed percentage of replies are being dropped
+
+        // wait for the normal reply timeout before sending it over and over
+        if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_REPLY_TIMEOUT) {
+            return;
         }
-    } else {
+        build_request((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_MON, UBX_ID_MON_VER, bytes_to_send);
-        // reset connection timer
+        // keep timeouts running properly, we (will have) just sent a packet that generates a reply
-        status->lastConnectedRaw = PIOS_DELAY_GetRaw();
+        status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
+        return;
+    }
+    if (!enabled) {
+        // keep resetting the timeouts here if we are not actually going to run the configure code
+        // not really necessary, but it keeps the timer from wrapping every 50 seconds
+        status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
+        return; // autoconfig not enabled
+    }
+
+    // replaying constantly could wear the settings memory out
+    // don't allow constant reconfiging when offline
+    // don't even allow program bugs that could constantly toggle between connected and disconnected to cause configuring
+    if (status->currentStep == INIT_STEP_DONE || status->currentStep == INIT_STEP_ERROR) {
+        return;
     }
 
     switch (status->currentStep) {
-    case INIT_STEP_ERROR:
-        if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) > UBX_ERROR_RETRY_TIMEOUT) {
-            status->currentStep = INIT_STEP_START;
-            status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
-        }
-        return;
-
-    case INIT_STEP_DISABLED:
-    case INIT_STEP_DONE:
-        return;
-
     case INIT_STEP_START:
-    case INIT_STEP_ASK_VER:
+        // we should look for the GPS version again
-        // clear ack
+        ubx_reset_sensor_type();
-        ubxLastAck.clsID = 0x00;
+        // do not fall through to next state
-        ubxLastAck.msgID = 0x00;
+        // or it might try to get the sensor type when the baud rate is half changed
-
+        set_current_step_if_untouched(INIT_STEP_RESET_GPS);
-        status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
+        // allow it to get the sensor type immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
-        build_request(&status->working_packet, UBX_CLASS_MON, UBX_ID_MON_VER, bytes_to_send);
-        status->currentStep = INIT_STEP_WAIT_VER;
         break;
 
-    case INIT_STEP_WAIT_VER:
+    case INIT_STEP_RESET_GPS:
-        if (ubxHwVersion > 0) {
+        // make sure we don't change the baud rate too soon and garble the packet being sent
-            status->lastConfigSent = LAST_CONFIG_SENT_START;
+        // even after pios says the buffer is empty, the serial port buffer still has data in it
-            status->currentStep    = INIT_STEP_ENABLE_SENTENCES;
+        // and changing the baud will screw it up
+        // when the GPS is configured to send a lot of data, but has a low baud rate
+        // it has way too many messages to send and has to drop most of them
+
+        // Retrieve desired GPS baud rate once for use throughout this module
+        HwSettingsGPSSpeedGet(&hwsettings_baud);
+#if !defined(ALWAYS_RESET)
+        // ALWAYS_RESET is undefined because it causes stored settings to change even with autoconfig.nostore
+        // but with it off, some settings may be enabled that should really be disabled (but aren't) after autoconfig.nostore
+        // if user requests a low baud rate then we just reset and leave it set to NEMA
+        // because low baud and high OP data rate doesn't play nice
+        // if user requests that settings be saved, we will reset here too
+        // that makes sure that all strange settings are reset to factory default
+        // else these strange settings may persist because we don't reset all settings by table
+        if (status->currentSettings.storeSettings)
+#endif
+        {
+            // reset all GPS parameters to factory default (configure low rate NEMA for low baud rates)
+            // this is not usable by OP code for either baud rate or types of messages sent
+            // but it starts up very quickly for use with autoconfig-nostore (which sets a high baud and enables all the necessary messages)
+            config_reset(bytes_to_send);
             status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
+        }
+        // else allow it enter the next state immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
+        set_current_step_if_untouched(INIT_STEP_REVO_9600_BAUD);
+        break;
+
+    case INIT_STEP_REVO_9600_BAUD:
+#if !defined(ALWAYS_RESET)
+        // if user requests a low baud rate then we just reset and leave it set to NEMA
+        // because low baud and high OP data rate doesn't play nice
+        // if user requests that settings be saved, we will reset here too
+        // that makes sure that all strange settings are reset to factory default
+        // else these strange settings may persist because we don't reset all settings by hand
+        if (status->currentSettings.storeSettings)
+#endif
+        {
+            // wait for previous step
+            if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_UNVERIFIED_STEP_WAIT_TIME) {
+                return;
+            }
+            // set the Revo GPS port to 9600 baud to match the reset to factory default that has already been done
+            config_baud(HWSETTINGS_GPSSPEED_9600);
+        }
+    // at most, we just set Revo baud and that doesn't send any data
+    // fall through to next state
+    // we can do that if we choose because we haven't sent any data in this state
+    // set_current_step_if_untouched(INIT_STEP_GPS_BAUD);
+    // allow it enter the next state immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
+    // break;
+
+    case INIT_STEP_GPS_BAUD:
+        // https://www.u-blox.com/images/downloads/Product_Docs/u-bloxM8_ReceiverDescriptionProtocolSpec_%28UBX-13003221%29_Public.pdf
+        // It is possible to change the current communications port settings using a UBX-CFG-CFG message. This could
+        // affect baud rate and other transmission parameters. Because there may be messages queued for transmission
+        // there may be uncertainty about which protocol applies to such messages. In addition a message currently in
+        // transmission may be corrupted by a protocol change. Host data reception parameters may have to be changed to
+        // be able to receive future messages, including the acknowledge message associated with the UBX-CFG-CFG message.
+
+        // so the message that changes the baud rate will send it's acknowledgement back at the new baud rate; this is not good.
+        // if your message was corrupted, you didn't change the baud rate and you have to guess; try pinging at both baud rates.
+        // also, you would have to change the baud rate instantly after the last byte of the sentence was sent,
+        // and you would have to poll the port in real time for that, and there may be messages ahead of the baud rate change.
+        //
+        // so we ignore the ack from this.  it has proven to be reliable (with the addition of two dummy bytes after the packet)
+
+        // set the GPS internal baud rate to the user configured value
+        config_gps_baud(bytes_to_send);
+        set_current_step_if_untouched(INIT_STEP_REVO_BAUD);
+        status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
+        break;
+
+    case INIT_STEP_REVO_BAUD:
+        // wait for previous step
+        if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_UNVERIFIED_STEP_WAIT_TIME) {
             return;
         }
-
+        // set the Revo GPS port baud rate to the (same) user configured value
-        if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) > UBX_REPLY_TIMEOUT) {
+        config_baud(hwsettings_baud);
-            status->currentStep = INIT_STEP_ASK_VER;
+        status->lastConfigSent = LAST_CONFIG_SENT_START;
+        status->retryCount     = 0;
+        // skip enabling UBX sentences for low baud rates
+        // low baud rates are not usable, and higher data rates just makes it harder for this code to change the configuration
+        if (hwsettings_baud <= HWSETTINGS_GPSSPEED_9600) {
+            set_current_step_if_untouched(INIT_STEP_SAVE);
+        } else {
+            set_current_step_if_untouched(INIT_STEP_ENABLE_SENTENCES);
         }
-        return;
+        // allow it enter the next state immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
+        break;
 
     case INIT_STEP_ENABLE_SENTENCES:
     case INIT_STEP_CONFIGURE:
@@ -352,64 +786,136 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send, bool gps_connec
             enable_sentences(bytes_to_send);
         }
 
-        status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
+        // for some branches, allow it enter the next state immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
         if (status->lastConfigSent == LAST_CONFIG_SENT_COMPLETED) {
-            status->lastConfigSent = LAST_CONFIG_SENT_START;
+            if (step_configure) {
-            status->currentStep    = step_configure ? INIT_STEP_DONE : INIT_STEP_CONFIGURE;
+                // zero retries for the next state that needs it (INIT_STEP_SAVE)
+                status->retryCount = 0;
+                set_current_step_if_untouched(INIT_STEP_SAVE);
+            } else {
+                // finished enabling sentences, now configure() needs to start at the beginning
+                status->lastConfigSent = LAST_CONFIG_SENT_START;
+                set_current_step_if_untouched(INIT_STEP_CONFIGURE);
+            }
         } else {
-            status->currentStep = step_configure ? INIT_STEP_CONFIGURE_WAIT_ACK : INIT_STEP_ENABLE_SENTENCES_WAIT_ACK;
+            set_current_step_if_untouched(step_configure ? INIT_STEP_CONFIGURE_WAIT_ACK : INIT_STEP_ENABLE_SENTENCES_WAIT_ACK);
+            status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
         }
-        return;
+        break;
     }
+
     case INIT_STEP_ENABLE_SENTENCES_WAIT_ACK:
     case INIT_STEP_CONFIGURE_WAIT_ACK: // Wait for an ack from GPS
     {
         bool step_configure = (status->currentStep == INIT_STEP_CONFIGURE_WAIT_ACK);
         if (ubxLastAck.clsID == status->requiredAck.clsID && ubxLastAck.msgID == status->requiredAck.msgID) {
-            // clear ack
-            ubxLastAck.clsID   = 0x00;
-            ubxLastAck.msgID   = 0x00;
-
             // Continue with next configuration option
+            // start retries over for the next setting to be sent
             status->retryCount = 0;
             status->lastConfigSent++;
-        } else if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) > UBX_REPLY_TIMEOUT) {
+        } else if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_REPLY_TIMEOUT &&
-            // timeout, resend the message or abort
+                   (ubxLastNak.clsID != status->requiredAck.clsID || ubxLastNak.msgID != status->requiredAck.msgID)) {
+            // allow timeouts to count up by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
+            break;
+        } else {
+            // timeout or NAK, resend the message or abort
             status->retryCount++;
             if (status->retryCount > UBX_MAX_RETRIES) {
-                status->currentStep = INIT_STEP_ERROR;
+                set_current_step_if_untouched(INIT_STEP_ERROR);
                 status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
-                return;
+                break;
             }
         }
-        // no abort condition, continue or retries.,
+        // success or failure here, retries are handled elsewhere
         if (step_configure) {
-            status->currentStep = INIT_STEP_CONFIGURE;
+            set_current_step_if_untouched(INIT_STEP_CONFIGURE);
         } else {
-            status->currentStep = INIT_STEP_ENABLE_SENTENCES;
+            set_current_step_if_untouched(INIT_STEP_ENABLE_SENTENCES);
         }
-        return;
+        break;
     }
+
+    case INIT_STEP_SAVE:
+        if (status->currentSettings.storeSettings) {
+            config_save(bytes_to_send);
+            set_current_step_if_untouched(INIT_STEP_SAVE_WAIT_ACK);
+            status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
+        } else {
+            set_current_step_if_untouched(INIT_STEP_DONE);
+            // allow it enter INIT_STEP_DONE immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
+        }
+        break;
+
+    // we could remove this state
+    // if we retry, it writes to settings storage a few more times
+    // and it is probably the ack that was dropped, with the save actually performed correctly
+    case INIT_STEP_SAVE_WAIT_ACK:
+        if (ubxLastAck.clsID == status->requiredAck.clsID && ubxLastAck.msgID == status->requiredAck.msgID) {
+            // Continue with next configuration option
+            set_current_step_if_untouched(INIT_STEP_DONE);
+            // note that we increase the reply timeout in case the GPS must do a flash erase
+        } else if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_REPLY_TO_SAVE_TIMEOUT &&
+                   (ubxLastNak.clsID != status->requiredAck.clsID || ubxLastNak.msgID != status->requiredAck.msgID)) {
+            // allow timeouts to count up by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
+            break;
+        } else {
+            // timeout or NAK, resend the message or abort
+            status->retryCount++;
+            if (status->retryCount > UBX_MAX_RETRIES / 2) {
+                // give up on the retries
+                set_current_step_if_untouched(INIT_STEP_ERROR);
+                status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
+            } else {
+                // retry a few times
+                set_current_step_if_untouched(INIT_STEP_SAVE);
+            }
+        }
+        break;
+
+    case INIT_STEP_ERROR:
+        // on error we should get the GPS version immediately
+        ubx_reset_sensor_type();
+    // fall through
+    case INIT_STEP_DISABLED:
+    case INIT_STEP_DONE:
+        break;
     }
 }
 
-void ubx_autoconfig_set(ubx_autoconfig_settings_t config)
+
+void ubx_autoconfig_set(ubx_autoconfig_settings_t *config)
 {
+    initSteps_t new_step;
+
     enabled = false;
-    if (config.autoconfigEnabled) {
+
-        if (!status) {
+    if (!status) {
-            status = (status_t *)pios_malloc(sizeof(status_t));
+        status = (status_t *)pios_malloc(sizeof(status_t));
-            PIOS_Assert(status);
+        PIOS_Assert(status);
-            memset(status, 0, sizeof(status_t));
+        memset((status_t *)status, 0, sizeof(status_t));
-            status->currentStep = INIT_STEP_DISABLED;
+    }
-        }
+
-        status->currentSettings = config;
+    // if caller used NULL, just use current settings to restart autoconfig process
-        status->currentStep     = INIT_STEP_START;
+    if (config != NULL) {
-        enabled = true;
+        status->currentSettings = *config;
+    }
+    if (status->currentSettings.autoconfigEnabled) {
+        new_step = INIT_STEP_START;
     } else {
-        if (status) {
+        new_step = INIT_STEP_DISABLED;
-            status->currentStep = INIT_STEP_DISABLED;
+    }
-        }
+
+    // assume this one byte initSteps_t is atomic
+    // take care of some but not all concurrency issues
+
+    status->currentStep     = new_step;
+    status->currentStepSave = new_step;
+    current_step_touched    = true;
+    status->currentStep     = new_step;
+    status->currentStepSave = new_step;
+
+    if (status->currentSettings.autoconfigEnabled) {
+        enabled = true;
     }
 }
 

flight/modules/Logging/Logging.c

@@ -134,7 +134,7 @@ static void ControlUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
         }
         DebugLogEntrySet(entry);
     } else if (control.Operation == DEBUGLOGCONTROL_OPERATION_FORMATFLASH) {
-        uint8_t armed;
+        FlightStatusArmedOptions armed;
         FlightStatusArmedGet(&armed);
         if (armed == FLIGHTSTATUS_ARMED_DISARMED) {
             PIOS_DEBUGLOG_Format();

flight/modules/ManualControl/armhandler.c

@@ -35,9 +35,13 @@
 #include <flightstatus.h>
 #include <flightmodesettings.h>
 #include <stabilizationdesired.h>
+#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
+#include <statusvtolland.h>
+#endif
 
 // Private constants
-#define ARMED_THRESHOLD 0.50f
+#define ARMED_THRESHOLD     0.50f
+#define GROUND_LOW_THROTTLE 0.01f
 
 // Private types
 typedef enum {
@@ -62,7 +66,7 @@ static bool forcedDisArm(void);
  * @input: ManualControlCommand, AccessoryDesired
  * @output: FlightStatus.Arming
  */
-void armHandler(bool newinit)
+void armHandler(bool newinit, FrameType_t frameType)
 {
     static ArmState_t armState;
 
@@ -82,7 +86,12 @@ void armHandler(bool newinit)
 
     bool lowThrottle = cmd.Throttle < 0;
 
-    bool armSwitch   = false;
+    if (frameType == FRAME_TYPE_GROUND) {
+        // Deadbanding applied in receiver.c typically at 2% but we don't assume its enabled.
+        lowThrottle = fabsf(cmd.Throttle) < GROUND_LOW_THROTTLE;
+    }
+
+    bool armSwitch = false;
 
     switch (settings.Arming) {
     case FLIGHTMODESETTINGS_ARMING_ACCESSORY0:
@@ -173,6 +182,8 @@ void armHandler(bool newinit)
     case FLIGHTMODESETTINGS_ARMING_ACCESSORY2:
         armingInputLevel = -1.0f * acc.AccessoryVal;
         break;
+    default:
+        break;
     }
 
     bool manualArm    = false;
@@ -304,6 +315,12 @@ static bool okToArm(void)
         return true;
 
         break;
+    case FLIGHTSTATUS_FLIGHTMODE_LAND:
+        return false;
+
+    case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
+    case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
+        return true;
 
     default:
         return false;
@@ -328,6 +345,19 @@ static bool forcedDisArm(void)
     if (alarms.Receiver == SYSTEMALARMS_ALARM_CRITICAL) {
         return true;
     }
+
+#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
+    // check landing state if active
+    FlightStatusData flightStatus;
+    FlightStatusGet(&flightStatus);
+    if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) {
+        StatusVtolLandData statusland;
+        StatusVtolLandGet(&statusland);
+        if (statusland.State == STATUSVTOLLAND_STATE_DISARMED) {
+            return true;
+        }
+    }
+#endif
     return false;
 }
 

flight/modules/ManualControl/inc/manualcontrol.h

@@ -32,6 +32,7 @@
 
 #include <openpilot.h>
 #include <flightstatus.h>
+#include <sanitycheck.h>
 
 typedef void (*handlerFunc)(bool newinit);
 
@@ -45,7 +46,7 @@ typedef struct controlHandlerStruct {
  * @input: ManualControlCommand, AccessoryDesired
  * @output: FlightStatus.Arming
  */
-void armHandler(bool newinit);
+void armHandler(bool newinit, FrameType_t frameType);
 
 /**
  * @brief Handler to control Manual flightmode - input directly steers actuators

flight/modules/ManualControl/manualcontrol.c

@@ -59,8 +59,8 @@
 #define CBTASK_PRIORITY                                CALLBACK_TASK_FLIGHTCONTROL
 
 #define ASSISTEDCONTROL_NEUTRALTHROTTLERANGE_FACTOR    0.2f
-#define ASSISTEDCONTROL_BRAKETHRUST_DEADBAND_FACTOR_LO 0.92f
+#define ASSISTEDCONTROL_BRAKETHRUST_DEADBAND_FACTOR_LO 0.96f
-#define ASSISTEDCONTROL_BRAKETHRUST_DEADBAND_FACTOR_HI 1.08f
+#define ASSISTEDCONTROL_BRAKETHRUST_DEADBAND_FACTOR_HI 1.04f
 
 
 // defined handlers
@@ -108,6 +108,7 @@ static float thrustHi = 0.0f;
 #endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
 // Private variables
 static DelayedCallbackInfo *callbackHandle;
+static FrameType_t frameType = FRAME_TYPE_MULTIROTOR;
 
 // Private functions
 static void configurationUpdatedCb(UAVObjEvent *ev);
@@ -116,6 +117,7 @@ static void manualControlTask(void);
 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
 static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, FlightModeSettingsData *modeSettings);
 #endif
+static void SettingsUpdatedCb(UAVObjEvent *ev);
 
 #define assumptions (assumptions1 && assumptions2 && assumptions3 && assumptions4 && assumptions5 && assumptions6 && assumptions7 && assumptions_flightmode)
 
@@ -135,11 +137,14 @@ int32_t ManualControlStart()
     // clear alarms
     AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);
 
+    SettingsUpdatedCb(NULL);
+
     // Make sure unarmed on power up
-    armHandler(true);
+    armHandler(true, frameType);
 
 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
     takeOffLocationHandlerInit();
+
 #endif
     // Start main task
     PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
@@ -164,6 +169,8 @@ int32_t ManualControlInitialize()
 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
     VtolSelfTuningStatsInitialize();
     VtolPathFollowerSettingsInitialize();
+    VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
+    SystemSettingsConnectCallback(&SettingsUpdatedCb);
 #endif
     callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&manualControlTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_MANUALCONTROL, STACK_SIZE_BYTES);
 
@@ -171,13 +178,37 @@ int32_t ManualControlInitialize()
 }
 MODULE_INITCALL(ManualControlInitialize, ManualControlStart);
 
+static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+{
+    frameType = GetCurrentFrameType();
+#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
+    VtolPathFollowerSettingsTreatCustomCraftAsOptions TreatCustomCraftAs;
+    VtolPathFollowerSettingsTreatCustomCraftAsGet(&TreatCustomCraftAs);
+
+
+    if (frameType == FRAME_TYPE_CUSTOM) {
+        switch (TreatCustomCraftAs) {
+        case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING:
+            frameType = FRAME_TYPE_FIXED_WING;
+            break;
+        case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL:
+            frameType = FRAME_TYPE_MULTIROTOR;
+            break;
+        case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_GROUND:
+            frameType = FRAME_TYPE_GROUND;
+            break;
+        }
+    }
+#endif
+}
+
 /**
  * Module task
  */
 static void manualControlTask(void)
 {
     // Process Arming
-    armHandler(false);
+    armHandler(false, frameType);
 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
     takeOffLocationHandler();
 #endif
@@ -211,7 +242,7 @@ static void manualControlTask(void)
         // set assist mode to none to avoid an assisted flight mode position
         // carrying over and impacting a newly selected non-assisted flight mode pos
         newFlightModeAssist      = FLIGHTSTATUS_FLIGHTMODEASSIST_NONE;
-        // The following are eqivalent to none effectively. Code should always
+        // The following are equivalent to none effectively. Code should always
         // check the flightmodeassist state.
         newAssistedControlState  = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY;
         newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
@@ -233,6 +264,16 @@ static void manualControlTask(void)
 
 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
         newFlightModeAssist = isAssistedFlightMode(position, newMode, &modeSettings);
+
+        if (newFlightModeAssist != flightStatus.FlightModeAssist) {
+            // On change of assist mode reinitialise control state.  This is required
+            // for the scenario where a flight position change reuses a flight mode
+            // but adds assistedcontrol.
+            newAssistedControlState  = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY;
+            newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
+        }
+
+
         if (newFlightModeAssist) {
             // assess roll/pitch state
             bool flagRollPitchHasInput = (fabsf(cmd.Roll) > 0.0f || fabsf(cmd.Pitch) > 0.0f);
@@ -270,7 +311,7 @@ static void manualControlTask(void)
                     // retain thrust cmd for later comparison with actual in braking
                     thrustAtBrakeStart = cmd.Thrust;
 
-                    // calculate hi and low value of +-8% as a mini-deadband
+                    // calculate hi and low value of +-4% as a mini-deadband
                     // for use in auto-override in brake sequence
                     thrustLo = ASSISTEDCONTROL_BRAKETHRUST_DEADBAND_FACTOR_LO * thrustAtBrakeStart;
                     thrustHi = ASSISTEDCONTROL_BRAKETHRUST_DEADBAND_FACTOR_HI * thrustAtBrakeStart;
@@ -366,9 +407,16 @@ static void manualControlTask(void)
         break;
 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
 
+    // During development the assistedcontrol implementation is optional and set
+    // set in stabi settings.  Once if we decide to always have this on, it can
+    // can be directly set here...i.e. set the flight mode assist as required.
     case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
+    case FLIGHTSTATUS_FLIGHTMODE_POSITIONROAM:
+    case FLIGHTSTATUS_FLIGHTMODE_LAND:
+    case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
         newFlightModeAssist = isAssistedFlightMode(position, newMode, &modeSettings);
         if (newFlightModeAssist) {
+            // Set the default thrust state
             switch (newFlightModeAssist) {
             case FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST:
                 newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
@@ -381,28 +429,14 @@ static void manualControlTask(void)
                 newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL; // Effectively None
                 break;
             }
-
-            switch (newAssistedControlState) {
-            case FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE:
-                newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE;
-                break;
-            case FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY:
-                newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE;
-                break;
-            case FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD:
-                newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD;
-                break;
-            }
         }
         handler = &handler_PATHFOLLOWER;
         break;
 
     case FLIGHTSTATUS_FLIGHTMODE_COURSELOCK:
-    case FLIGHTSTATUS_FLIGHTMODE_POSITIONROAM:
     case FLIGHTSTATUS_FLIGHTMODE_HOMELEASH:
     case FLIGHTSTATUS_FLIGHTMODE_ABSOLUTEPOSITION:
     case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
-    case FLIGHTSTATUS_FLIGHTMODE_LAND:
     case FLIGHTSTATUS_FLIGHTMODE_POI:
     case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE:
         handler = &handler_PATHFOLLOWER;
@@ -462,7 +496,7 @@ static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, Flight
 {
     uint8_t flightModeAssistOption = STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NONE;
     uint8_t isAssistedFlag = FLIGHTSTATUS_FLIGHTMODEASSIST_NONE;
-    uint8_t FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM];
+    StabilizationSettingsFlightModeAssistMapOptions FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM];
 
     StabilizationSettingsFlightModeAssistMapGet(FlightModeAssistMap);
     if (position < STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM) {
@@ -497,11 +531,16 @@ static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, Flight
             thrustMode = modeSettings->Stabilization6Settings.Thrust;
             break;
         case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
-            // we hard code the "GPS Assisted" PostionHold to use alt-vario which
+        case FLIGHTSTATUS_FLIGHTMODE_POSITIONROAM:
+            // we hard code the "GPS Assisted" PostionHold/Roam to use alt-vario which
             // is a more appropriate throttle mode.  "GPSAssist" adds braking
             // and a better throttle management to the standard Position Hold.
             thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO;
             break;
+        case FLIGHTSTATUS_FLIGHTMODE_LAND:
+        case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
+            thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL;
+            break;
 
             // other modes will use cruisecontrol as default
         }

flight/modules/ManualControl/pathfollowerhandler.c

@@ -55,9 +55,11 @@ void pathFollowerHandler(bool newinit)
         plan_initialize();
     }
 
-    uint8_t flightMode;
+    FlightStatusFlightModeOptions flightMode;
-    uint8_t assistedControlFlightMode;
+    FlightStatusAssistedControlStateOptions assistedControlFlightMode;
+    FlightStatusFlightModeAssistOptions flightModeAssist;
     FlightStatusFlightModeGet(&flightMode);
+    FlightStatusFlightModeAssistGet(&flightModeAssist);
     FlightStatusAssistedControlStateGet(&assistedControlFlightMode);
 
     if (newinit) {
@@ -67,9 +69,10 @@ void pathFollowerHandler(bool newinit)
             plan_setup_returnToBase();
             break;
         case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
-            if (assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE) {
+            if ((flightModeAssist != FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) &&
-                // Just initiated braking after returning from stabi control
+                (assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY)) {
-                plan_setup_assistedcontrol(false);
+                // Switch from primary (just entered this PH flight mode) into brake
+                plan_setup_assistedcontrol();
             } else {
                 plan_setup_positionHold();
             }
@@ -78,7 +81,11 @@ void pathFollowerHandler(bool newinit)
             plan_setup_CourseLock();
             break;
         case FLIGHTSTATUS_FLIGHTMODE_POSITIONROAM:
-            plan_setup_PositionRoam();
+            if (flightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) {
+                plan_setup_PositionRoam();
+            } else {
+                plan_setup_VelocityRoam();
+            }
             break;
         case FLIGHTSTATUS_FLIGHTMODE_HOMELEASH:
             plan_setup_HomeLeash();
@@ -88,7 +95,14 @@ void pathFollowerHandler(bool newinit)
             break;
 
         case FLIGHTSTATUS_FLIGHTMODE_LAND:
-            plan_setup_land();
+            if (flightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) {
+                plan_setup_land();
+            } else {
+                plan_setup_VelocityRoam();
+            }
+            break;
+        case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
+            plan_setup_AutoTakeoff();
             break;
         case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE:
             plan_setup_AutoCruise();
@@ -102,7 +116,7 @@ void pathFollowerHandler(bool newinit)
         case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
             if (assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE) {
                 // Just initiated braking after returning from stabi control
-                plan_setup_assistedcontrol(false);
+                plan_setup_assistedcontrol();
             }
             break;
 
@@ -117,7 +131,11 @@ void pathFollowerHandler(bool newinit)
         plan_run_CourseLock();
         break;
     case FLIGHTSTATUS_FLIGHTMODE_POSITIONROAM:
-        plan_run_PositionRoam();
+        if (flightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) {
+            plan_run_PositionRoam();
+        } else {
+            plan_run_VelocityRoam();
+        }
         break;
     case FLIGHTSTATUS_FLIGHTMODE_HOMELEASH:
         plan_run_HomeLeash();
@@ -126,7 +144,12 @@ void pathFollowerHandler(bool newinit)
         plan_run_AbsolutePosition();
         break;
     case FLIGHTSTATUS_FLIGHTMODE_LAND:
-        plan_run_land();
+        if (flightModeAssist != FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) {
+            plan_run_VelocityRoam();
+        }
+        break;
+    case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
+        plan_run_AutoTakeoff();
         break;
     case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE:
         plan_run_AutoCruise();

flight/modules/ManualControl/stabilizedhandler.c

@@ -96,33 +96,34 @@ void stabilizedHandler(bool newinit)
 
     switch (flightStatus.FlightMode) {
     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
-        stab_settings = FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings);
+        stab_settings = (uint8_t *)FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings);
         break;
     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
-        stab_settings = FlightModeSettingsStabilization2SettingsToArray(settings.Stabilization2Settings);
+        stab_settings = (uint8_t *)FlightModeSettingsStabilization2SettingsToArray(settings.Stabilization2Settings);
         break;
     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
-        stab_settings = FlightModeSettingsStabilization3SettingsToArray(settings.Stabilization3Settings);
+        stab_settings = (uint8_t *)FlightModeSettingsStabilization3SettingsToArray(settings.Stabilization3Settings);
         break;
     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
-        stab_settings = FlightModeSettingsStabilization4SettingsToArray(settings.Stabilization4Settings);
+        stab_settings = (uint8_t *)FlightModeSettingsStabilization4SettingsToArray(settings.Stabilization4Settings);
         break;
     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
-        stab_settings = FlightModeSettingsStabilization5SettingsToArray(settings.Stabilization5Settings);
+        stab_settings = (uint8_t *)FlightModeSettingsStabilization5SettingsToArray(settings.Stabilization5Settings);
         break;
     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
-        stab_settings = FlightModeSettingsStabilization6SettingsToArray(settings.Stabilization6Settings);
+        stab_settings = (uint8_t *)FlightModeSettingsStabilization6SettingsToArray(settings.Stabilization6Settings);
         break;
     default:
         // Major error, this should not occur because only enter this block when one of these is true
         AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
-        stab_settings = FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings);
+        stab_settings = (uint8_t *)FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings);
         return;
     }
 
     stabilization.Roll =
         (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Roll :
         (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Roll * stabSettings.ManualRate.Roll :
+        (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATETRAINER) ? cmd.Roll * stabSettings.ManualRate.Roll :
         (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Roll * stabSettings.RollMax :
         (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Roll * stabSettings.RollMax :
         (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Roll * stabSettings.ManualRate.Roll :
@@ -134,6 +135,7 @@ void stabilizedHandler(bool newinit)
     stabilization.Pitch =
         (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Pitch :
         (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
+        (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATETRAINER) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
         (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Pitch * stabSettings.PitchMax :
         (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Pitch * stabSettings.PitchMax :
         (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
@@ -155,6 +157,7 @@ void stabilizedHandler(bool newinit)
         stabilization.Yaw =
             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Yaw :
             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
+            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATETRAINER) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Yaw * stabSettings.YawMax :
             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Yaw * stabSettings.YawMax :
             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Yaw * stabSettings.ManualRate.Yaw :

flight/modules/ManualControl/takeofflocationhandler.c

@@ -42,8 +42,8 @@ void takeOffLocationHandlerInit()
 {
     TakeOffLocationInitialize();
     // check whether there is a preset/valid takeoff location
-    uint8_t mode;
+    TakeOffLocationModeOptions mode;
-    uint8_t status;
+    TakeOffLocationStatusOptions status;
     TakeOffLocationModeGet(&mode);
     TakeOffLocationStatusGet(&status);
     // preset with invalid location will actually behave like FirstTakeoff
@@ -61,8 +61,8 @@ void takeOffLocationHandlerInit()
  */
 void takeOffLocationHandler()
 {
-    uint8_t armed;
+    FlightStatusArmedOptions armed;
-    uint8_t status;
+    TakeOffLocationStatusOptions status;
 
     FlightStatusArmedGet(&armed);
 
@@ -77,7 +77,7 @@ void takeOffLocationHandler()
     case FLIGHTSTATUS_ARMED_ARMING:
     case FLIGHTSTATUS_ARMED_ARMED:
         if (!locationSet || status != TAKEOFFLOCATION_STATUS_VALID) {
-            uint8_t mode;
+            TakeOffLocationModeOptions mode;
             TakeOffLocationModeGet(&mode);
 
             if ((mode != TAKEOFFLOCATION_MODE_PRESET) || (status == TAKEOFFLOCATION_STATUS_INVALID)) {
@@ -91,7 +91,7 @@ void takeOffLocationHandler()
     case FLIGHTSTATUS_ARMED_DISARMED:
         // unset if location is to be acquired at each arming
         if (locationSet) {
-            uint8_t mode;
+            TakeOffLocationModeOptions mode;
             TakeOffLocationModeGet(&mode);
             if (mode == TAKEOFFLOCATION_MODE_ARMINGLOCATION) {
                 locationSet = false;

flight/modules/PathFollower/fixedwingflycontroller.cpp

@@ -0,0 +1,618 @@
+/*
+ ******************************************************************************
+ *
+ * @file       FixedWingFlyController.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Fixed wing fly controller implementation
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+extern "C" {
+#include <openpilot.h>
+
+#include <callbackinfo.h>
+
+#include <math.h>
+#include <pid.h>
+#include <CoordinateConversions.h>
+#include <sin_lookup.h>
+#include <pathdesired.h>
+#include <paths.h>
+#include "plans.h"
+#include <sanitycheck.h>
+
+#include <homelocation.h>
+#include <accelstate.h>
+#include <fixedwingpathfollowersettings.h>
+#include <fixedwingpathfollowerstatus.h>
+#include <flightstatus.h>
+#include <flightmodesettings.h>
+#include <pathstatus.h>
+#include <positionstate.h>
+#include <velocitystate.h>
+#include <velocitydesired.h>
+#include <stabilizationdesired.h>
+#include <airspeedstate.h>
+#include <attitudestate.h>
+#include <takeofflocation.h>
+#include <poilocation.h>
+#include <manualcontrolcommand.h>
+#include <systemsettings.h>
+#include <stabilizationbank.h>
+#include <stabilizationdesired.h>
+#include <vtolselftuningstats.h>
+#include <pathsummary.h>
+}
+
+// C++ includes
+#include "fixedwingflycontroller.h"
+
+// Private constants
+
+// pointer to a singleton instance
+FixedWingFlyController *FixedWingFlyController::p_inst = 0;
+
+FixedWingFlyController::FixedWingFlyController()
+    : fixedWingSettings(NULL), mActive(false), mMode(0), indicatedAirspeedStateBias(0.0f)
+{}
+
+// Called when mode first engaged
+void FixedWingFlyController::Activate(void)
+{
+    if (!mActive) {
+        mActive = true;
+        SettingsUpdated();
+        resetGlobals();
+        mMode   = pathDesired->Mode;
+    }
+}
+
+uint8_t FixedWingFlyController::IsActive(void)
+{
+    return mActive;
+}
+
+uint8_t FixedWingFlyController::Mode(void)
+{
+    return mMode;
+}
+
+// Objective updated in pathdesired
+void FixedWingFlyController::ObjectiveUpdated(void)
+{}
+
+void FixedWingFlyController::Deactivate(void)
+{
+    if (mActive) {
+        mActive = false;
+        resetGlobals();
+    }
+}
+
+
+void FixedWingFlyController::SettingsUpdated(void)
+{
+    // fixed wing PID only
+    pid_configure(&PIDposH[0], fixedWingSettings->HorizontalPosP, 0.0f, 0.0f, 0.0f);
+    pid_configure(&PIDposH[1], fixedWingSettings->HorizontalPosP, 0.0f, 0.0f, 0.0f);
+    pid_configure(&PIDposV, fixedWingSettings->VerticalPosP, 0.0f, 0.0f, 0.0f);
+
+    pid_configure(&PIDcourse, fixedWingSettings->CoursePI.Kp, fixedWingSettings->CoursePI.Ki, 0.0f, fixedWingSettings->CoursePI.ILimit);
+    pid_configure(&PIDspeed, fixedWingSettings->SpeedPI.Kp, fixedWingSettings->SpeedPI.Ki, 0.0f, fixedWingSettings->SpeedPI.ILimit);
+    pid_configure(&PIDpower, fixedWingSettings->PowerPI.Kp, fixedWingSettings->PowerPI.Ki, 0.0f, fixedWingSettings->PowerPI.ILimit);
+}
+
+/**
+ * Initialise the module, called on startup
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t FixedWingFlyController::Initialize(FixedWingPathFollowerSettingsData *ptr_fixedWingSettings)
+{
+    PIOS_Assert(ptr_fixedWingSettings);
+
+    fixedWingSettings = ptr_fixedWingSettings;
+
+    resetGlobals();
+
+    return 0;
+}
+
+/**
+ * reset integrals
+ */
+void FixedWingFlyController::resetGlobals()
+{
+    pid_zero(&PIDposH[0]);
+    pid_zero(&PIDposH[1]);
+    pid_zero(&PIDposV);
+    pid_zero(&PIDcourse);
+    pid_zero(&PIDspeed);
+    pid_zero(&PIDpower);
+    pathStatus->path_time = 0.0f;
+}
+
+void FixedWingFlyController::UpdateAutoPilot()
+{
+    uint8_t result = updateAutoPilotFixedWing();
+
+    if (result) {
+        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
+    } else {
+        pathStatus->Status = PATHSTATUS_STATUS_CRITICAL;
+        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
+    }
+
+    PathStatusSet(pathStatus);
+}
+
+/**
+ * fixed wing autopilot:
+ * straight forward:
+ * 1. update path velocity for limited motion crafts
+ * 2. update attitude according to default fixed wing pathfollower algorithm
+ */
+uint8_t FixedWingFlyController::updateAutoPilotFixedWing()
+{
+    updatePathVelocity(fixedWingSettings->CourseFeedForward, true);
+    return updateFixedDesiredAttitude();
+}
+
+/**
+ * Compute desired velocity from the current position and path
+ */
+void FixedWingFlyController::updatePathVelocity(float kFF, bool limited)
+{
+    PositionStateData positionState;
+
+    PositionStateGet(&positionState);
+    VelocityStateData velocityState;
+    VelocityStateGet(&velocityState);
+    VelocityDesiredData velocityDesired;
+
+    const float dT = fixedWingSettings->UpdatePeriod / 1000.0f;
+
+    // look ahead kFF seconds
+    float cur[3]   = { positionState.North + (velocityState.North * kFF),
+                       positionState.East + (velocityState.East * kFF),
+                       positionState.Down + (velocityState.Down * kFF) };
+    struct path_status progress;
+    path_progress(pathDesired, cur, &progress, true);
+
+    // calculate velocity - can be zero if waypoints are too close
+    velocityDesired.North = progress.path_vector[0];
+    velocityDesired.East  = progress.path_vector[1];
+    velocityDesired.Down  = progress.path_vector[2];
+
+    if (limited &&
+        // if a plane is crossing its desired flightpath facing the wrong way (away from flight direction)
+        // it would turn towards the flightpath to get on its desired course. This however would reverse the correction vector
+        // once it crosses the flightpath again, which would make it again turn towards the flightpath (but away from its desired heading)
+        // leading to an S-shape snake course the wrong way
+        // this only happens especially if HorizontalPosP is too high, as otherwise the angle between velocity desired and path_direction won't
+        // turn steep unless there is enough space complete the turn before crossing the flightpath
+        // in this case the plane effectively needs to be turned around
+        // indicators:
+        // difference between correction_direction and velocitystate >90 degrees and
+        // difference between path_direction and velocitystate >90 degrees  ( 4th sector, facing away from everything )
+        // fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore)
+        // calculating angles < 90 degrees through dot products
+        (vector_lengthf(progress.path_vector, 2) > 1e-6f) &&
+        ((progress.path_vector[0] * velocityState.North + progress.path_vector[1] * velocityState.East) < 0.0f) &&
+        ((progress.correction_vector[0] * velocityState.North + progress.correction_vector[1] * velocityState.East) < 0.0f)) {
+        ;
+    } else {
+        // calculate correction
+        velocityDesired.North += pid_apply(&PIDposH[0], progress.correction_vector[0], dT);
+        velocityDesired.East  += pid_apply(&PIDposH[1], progress.correction_vector[1], dT);
+    }
+    velocityDesired.Down += pid_apply(&PIDposV, progress.correction_vector[2], dT);
+
+    // update pathstatus
+    pathStatus->error     = progress.error;
+    pathStatus->fractional_progress  = progress.fractional_progress;
+    pathStatus->path_direction_north = progress.path_vector[0];
+    pathStatus->path_direction_east  = progress.path_vector[1];
+    pathStatus->path_direction_down  = progress.path_vector[2];
+
+    pathStatus->correction_direction_north = progress.correction_vector[0];
+    pathStatus->correction_direction_east  = progress.correction_vector[1];
+    pathStatus->correction_direction_down  = progress.correction_vector[2];
+
+
+    VelocityDesiredSet(&velocityDesired);
+}
+
+
+/**
+ * Compute desired attitude from the desired velocity for fixed wing craft
+ */
+uint8_t FixedWingFlyController::updateFixedDesiredAttitude()
+{
+    uint8_t result = 1;
+
+    const float dT = fixedWingSettings->UpdatePeriod / 1000.0f;
+
+    VelocityDesiredData velocityDesired;
+    VelocityStateData velocityState;
+    StabilizationDesiredData stabDesired;
+    AttitudeStateData attitudeState;
+    FixedWingPathFollowerStatusData fixedWingPathFollowerStatus;
+    AirspeedStateData airspeedState;
+    SystemSettingsData systemSettings;
+
+    float groundspeedProjection;
+    float indicatedAirspeedState;
+    float indicatedAirspeedDesired;
+    float airspeedError;
+
+    float pitchCommand;
+
+    float descentspeedDesired;
+    float descentspeedError;
+    float powerCommand;
+
+    float airspeedVector[2];
+    float fluidMovement[2];
+    float courseComponent[2];
+    float courseError;
+    float courseCommand;
+
+    FixedWingPathFollowerStatusGet(&fixedWingPathFollowerStatus);
+
+    VelocityStateGet(&velocityState);
+    StabilizationDesiredGet(&stabDesired);
+    VelocityDesiredGet(&velocityDesired);
+    AttitudeStateGet(&attitudeState);
+    AirspeedStateGet(&airspeedState);
+    SystemSettingsGet(&systemSettings);
+
+    /**
+     * Compute speed error and course
+     */
+    // missing sensors for airspeed-direction we have to assume within
+    // reasonable error that measured airspeed is actually the airspeed
+    // component in forward pointing direction
+    // airspeedVector is normalized
+    airspeedVector[0]     = cos_lookup_deg(attitudeState.Yaw);
+    airspeedVector[1]     = sin_lookup_deg(attitudeState.Yaw);
+
+    // current ground speed projected in forward direction
+    groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1];
+
+    // note that airspeedStateBias is ( calibratedAirspeed - groundspeedProjection ) at the time of measurement,
+    // but thanks to accelerometers,  groundspeedProjection reacts faster to changes in direction
+    // than airspeed and gps sensors alone
+    indicatedAirspeedState = groundspeedProjection + indicatedAirspeedStateBias;
+
+    // fluidMovement is a vector describing the aproximate movement vector of
+    // the surrounding fluid in 2d space (aka wind vector)
+    fluidMovement[0] = velocityState.North - (indicatedAirspeedState * airspeedVector[0]);
+    fluidMovement[1] = velocityState.East - (indicatedAirspeedState * airspeedVector[1]);
+
+    // calculate the movement vector we need to fly to reach velocityDesired -
+    // taking fluidMovement into account
+    courseComponent[0] = velocityDesired.North - fluidMovement[0];
+    courseComponent[1] = velocityDesired.East - fluidMovement[1];
+
+    indicatedAirspeedDesired = boundf(sqrtf(courseComponent[0] * courseComponent[0] + courseComponent[1] * courseComponent[1]),
+                                      fixedWingSettings->HorizontalVelMin,
+                                      fixedWingSettings->HorizontalVelMax);
+
+    // if we could fly at arbitrary speeds, we'd just have to move towards the
+    // courseComponent vector as previously calculated and we'd be fine
+    // unfortunately however we are bound by min and max air speed limits, so
+    // we need to recalculate the correct course to meet at least the
+    // velocityDesired vector direction at our current speed
+    // this overwrites courseComponent
+    bool valid = correctCourse(courseComponent, (float *)&velocityDesired.North, fluidMovement, indicatedAirspeedDesired);
+
+    // Error condition: wind speed too high, we can't go where we want anymore
+    fixedWingPathFollowerStatus.Errors.Wind = 0;
+    if ((!valid) &&
+        fixedWingSettings->Safetymargins.Wind > 0.5f) { // alarm switched on
+        fixedWingPathFollowerStatus.Errors.Wind = 1;
+        result = 0;
+    }
+
+    // Airspeed error
+    airspeedError = indicatedAirspeedDesired - indicatedAirspeedState;
+
+    // Vertical speed error
+    descentspeedDesired = boundf(
+        velocityDesired.Down,
+        -fixedWingSettings->VerticalVelMax,
+        fixedWingSettings->VerticalVelMax);
+    descentspeedError = descentspeedDesired - velocityState.Down;
+
+    // Error condition: plane too slow or too fast
+    fixedWingPathFollowerStatus.Errors.Highspeed = 0;
+    fixedWingPathFollowerStatus.Errors.Lowspeed  = 0;
+    if (indicatedAirspeedState > systemSettings.AirSpeedMax * fixedWingSettings->Safetymargins.Overspeed) {
+        fixedWingPathFollowerStatus.Errors.Overspeed = 1;
+        result = 0;
+    }
+    if (indicatedAirspeedState > fixedWingSettings->HorizontalVelMax * fixedWingSettings->Safetymargins.Highspeed) {
+        fixedWingPathFollowerStatus.Errors.Highspeed = 1;
+        result = 0;
+    }
+    if (indicatedAirspeedState < fixedWingSettings->HorizontalVelMin * fixedWingSettings->Safetymargins.Lowspeed) {
+        fixedWingPathFollowerStatus.Errors.Lowspeed = 1;
+        result = 0;
+    }
+    if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedWingSettings->Safetymargins.Stallspeed) {
+        fixedWingPathFollowerStatus.Errors.Stallspeed = 1;
+        result = 0;
+    }
+
+    /**
+     * Compute desired thrust command
+     */
+
+    // Compute the cross feed from vertical speed to pitch, with saturation
+    float speedErrorToPowerCommandComponent = boundf(
+        (airspeedError / fixedWingSettings->HorizontalVelMin) * fixedWingSettings->AirspeedToPowerCrossFeed.Kp,
+        -fixedWingSettings->AirspeedToPowerCrossFeed.Max,
+        fixedWingSettings->AirspeedToPowerCrossFeed.Max
+        );
+
+    // Compute final thrust response
+    powerCommand = pid_apply(&PIDpower, -descentspeedError, dT) +
+                   speedErrorToPowerCommandComponent;
+
+    // Output internal state to telemetry
+    fixedWingPathFollowerStatus.Error.Power    = descentspeedError;
+    fixedWingPathFollowerStatus.ErrorInt.Power = PIDpower.iAccumulator;
+    fixedWingPathFollowerStatus.Command.Power  = powerCommand;
+
+    // set thrust
+    stabDesired.Thrust = boundf(fixedWingSettings->ThrustLimit.Neutral + powerCommand,
+                                fixedWingSettings->ThrustLimit.Min,
+                                fixedWingSettings->ThrustLimit.Max);
+
+    // Error condition: plane cannot hold altitude at current speed.
+    fixedWingPathFollowerStatus.Errors.Lowpower = 0;
+    if (fixedWingSettings->ThrustLimit.Neutral + powerCommand >= fixedWingSettings->ThrustLimit.Max && // thrust at maximum
+        velocityState.Down > 0.0f && // we ARE going down
+        descentspeedDesired < 0.0f && // we WANT to go up
+        airspeedError > 0.0f && // we are too slow already
+        fixedWingSettings->Safetymargins.Lowpower > 0.5f) { // alarm switched on
+        fixedWingPathFollowerStatus.Errors.Lowpower = 1;
+        result = 0;
+    }
+    // Error condition: plane keeps climbing despite minimum thrust (opposite of above)
+    fixedWingPathFollowerStatus.Errors.Highpower = 0;
+    if (fixedWingSettings->ThrustLimit.Neutral + powerCommand <= fixedWingSettings->ThrustLimit.Min && // thrust at minimum
+        velocityState.Down < 0.0f && // we ARE going up
+        descentspeedDesired > 0.0f && // we WANT to go down
+        airspeedError < 0.0f && // we are too fast already
+        fixedWingSettings->Safetymargins.Highpower > 0.5f) { // alarm switched on
+        fixedWingPathFollowerStatus.Errors.Highpower = 1;
+        result = 0;
+    }
+
+    /**
+     * Compute desired pitch command
+     */
+    // Compute the cross feed from vertical speed to pitch, with saturation
+    float verticalSpeedToPitchCommandComponent = boundf(-descentspeedError * fixedWingSettings->VerticalToPitchCrossFeed.Kp,
+                                                        -fixedWingSettings->VerticalToPitchCrossFeed.Max,
+                                                        fixedWingSettings->VerticalToPitchCrossFeed.Max
+                                                        );
+
+    // Compute the pitch command as err*Kp + errInt*Ki + X_feed.
+    pitchCommand = -pid_apply(&PIDspeed, airspeedError, dT) + verticalSpeedToPitchCommandComponent;
+
+    fixedWingPathFollowerStatus.Error.Speed    = airspeedError;
+    fixedWingPathFollowerStatus.ErrorInt.Speed = PIDspeed.iAccumulator;
+    fixedWingPathFollowerStatus.Command.Speed  = pitchCommand;
+
+    stabDesired.Pitch = boundf(fixedWingSettings->PitchLimit.Neutral + pitchCommand,
+                               fixedWingSettings->PitchLimit.Min,
+                               fixedWingSettings->PitchLimit.Max);
+
+    // Error condition: high speed dive
+    fixedWingPathFollowerStatus.Errors.Pitchcontrol = 0;
+    if (fixedWingSettings->PitchLimit.Neutral + pitchCommand >= fixedWingSettings->PitchLimit.Max && // pitch demand is full up
+        velocityState.Down > 0.0f && // we ARE going down
+        descentspeedDesired < 0.0f && // we WANT to go up
+        airspeedError < 0.0f && // we are too fast already
+        fixedWingSettings->Safetymargins.Pitchcontrol > 0.5f) { // alarm switched on
+        fixedWingPathFollowerStatus.Errors.Pitchcontrol = 1;
+        result = 0;
+    }
+
+    /**
+     * Compute desired roll command
+     */
+    courseError = RAD2DEG(atan2f(courseComponent[1], courseComponent[0])) - attitudeState.Yaw;
+
+    if (courseError < -180.0f) {
+        courseError += 360.0f;
+    }
+    if (courseError > 180.0f) {
+        courseError -= 360.0f;
+    }
+
+    // overlap calculation. Theres a dead zone behind the craft where the
+    // counter-yawing of some craft while rolling could render a desired right
+    // turn into a desired left turn. Making the turn direction based on
+    // current roll angle keeps the plane committed to a direction once chosen
+    if (courseError < -180.0f + (fixedWingSettings->ReverseCourseOverlap * 0.5f)
+        && attitudeState.Roll > 0.0f) {
+        courseError += 360.0f;
+    }
+    if (courseError > 180.0f - (fixedWingSettings->ReverseCourseOverlap * 0.5f)
+        && attitudeState.Roll < 0.0f) {
+        courseError -= 360.0f;
+    }
+
+    courseCommand = pid_apply(&PIDcourse, courseError, dT);
+
+    fixedWingPathFollowerStatus.Error.Course    = courseError;
+    fixedWingPathFollowerStatus.ErrorInt.Course = PIDcourse.iAccumulator;
+    fixedWingPathFollowerStatus.Command.Course  = courseCommand;
+
+    stabDesired.Roll = boundf(fixedWingSettings->RollLimit.Neutral +
+                              courseCommand,
+                              fixedWingSettings->RollLimit.Min,
+                              fixedWingSettings->RollLimit.Max);
+
+    // TODO: find a check to determine loss of directional control. Likely needs some check of derivative
+
+
+    /**
+     * Compute desired yaw command
+     */
+    // TODO implement raw control mode for yaw and base on Accels.Y
+    stabDesired.Yaw = 0.0f;
+
+
+    stabDesired.StabilizationMode.Roll   = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.StabilizationMode.Pitch  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.StabilizationMode.Yaw    = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
+    stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
+
+    StabilizationDesiredSet(&stabDesired);
+
+    FixedWingPathFollowerStatusSet(&fixedWingPathFollowerStatus);
+
+    return result;
+}
+
+
+/**
+ * Function to calculate course vector C based on airspeed s, fluid movement F
+ * and desired movement vector V
+ * parameters in: V,F,s
+ * parameters out: C
+ * returns true if a valid solution could be found for V,F,s, false if not
+ * C will be set to a best effort attempt either way
+ */
+bool FixedWingFlyController::correctCourse(float *C, float *V, float *F, float s)
+{
+    // Approach:
+    // Let Sc be a circle around origin marking possible movement vectors
+    // of the craft with airspeed s (all possible options for C)
+    // Let Vl be a line through the origin along movement vector V where fr any
+    // point v on line Vl v = k * (V / |V|) = k' * V
+    // Let Wl be a line parallel to Vl where for any point v on line Vl exists
+    // a point w on WL with w = v - F
+    // Then any intersection between circle Sc and line Wl represents course
+    // vector which would result in a movement vector
+    // V' = k * ( V / |V|) = k' * V
+    // If there is no intersection point, S is insufficient to compensate
+    // for F and we can only try to fly in direction of V (thus having wind drift
+    // but at least making progress orthogonal to wind)
+
+    s = fabsf(s);
+    float f = vector_lengthf(F, 2);
+
+    // normalize Cn=V/|V|, |V| must be >0
+    float v = vector_lengthf(V, 2);
+    if (v < 1e-6f) {
+        // if |V|=0, we aren't supposed to move, turn into the wind
+        // (this allows hovering)
+        C[0] = -F[0];
+        C[1] = -F[1];
+        // if desired airspeed matches fluidmovement a hover is actually
+        // intended so return true
+        return fabsf(f - s) < 1e-3f;
+    }
+    float Vn[2] = { V[0] / v, V[1] / v };
+
+    // project F on V
+    float fp    = F[0] * Vn[0] + F[1] * Vn[1];
+
+    // find component Fo of F that is orthogonal to V
+    // (which is exactly the distance between Vl and Wl)
+    float Fo[2] = { F[0] - (fp * Vn[0]), F[1] - (fp * Vn[1]) };
+    float fo2   = Fo[0] * Fo[0] + Fo[1] * Fo[1];
+
+    // find k where k * Vn = C - Fo
+    // |C|=s is the hypothenuse in any rectangular triangle formed by k * Vn and Fo
+    // so k^2 + fo^2 = s^2 (since |Vn|=1)
+    float k2 = s * s - fo2;
+    if (k2 <= -1e-3f) {
+        // there is no solution, we will be drifted off either way
+        // fallback: fly stupidly in direction of V and hope for the best
+        C[0] = V[0];
+        C[1] = V[1];
+        return false;
+    } else if (k2 <= 1e-3f) {
+        // there is exactly one solution: -Fo
+        C[0] = -Fo[0];
+        C[1] = -Fo[1];
+        return true;
+    }
+    // we have two possible solutions k positive and k negative as there are
+    // two intersection points between Wl and Sc
+    // which one is better? two criteria:
+    // 1. we MUST move in the right direction, if any k leads to -v its invalid
+    // 2. we should minimize the speed error
+    float k     = sqrt(k2);
+    float C1[2] = { -k * Vn[0] - Fo[0], -k * Vn[1] - Fo[1] };
+    float C2[2] = { k *Vn[0] - Fo[0], k * Vn[1] - Fo[1] };
+    // project C+F on Vn to find signed resulting movement vector length
+    float vp1   = (C1[0] + F[0]) * Vn[0] + (C1[1] + F[1]) * Vn[1];
+    float vp2   = (C2[0] + F[0]) * Vn[0] + (C2[1] + F[1]) * Vn[1];
+    if (vp1 >= 0.0f && fabsf(v - vp1) < fabsf(v - vp2)) {
+        // in this case the angle between course and resulting movement vector
+        // is greater than 90 degrees - so we actually fly backwards
+        C[0] = C1[0];
+        C[1] = C1[1];
+        return true;
+    }
+    C[0] = C2[0];
+    C[1] = C2[1];
+    if (vp2 >= 0.0f) {
+        // in this case the angle between course and movement vector is less than
+        // 90 degrees, but we do move in the right direction
+        return true;
+    } else {
+        // in this case we actually get driven in the opposite direction of V
+        // with both solutions for C
+        // this might be reached in headwind stronger than maximum allowed
+        // airspeed.
+        return false;
+    }
+}
+
+
+void FixedWingFlyController::AirspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+{
+    AirspeedStateData airspeedState;
+    VelocityStateData velocityState;
+
+    AirspeedStateGet(&airspeedState);
+    VelocityStateGet(&velocityState);
+    float airspeedVector[2];
+    float yaw;
+    AttitudeStateYawGet(&yaw);
+    airspeedVector[0] = cos_lookup_deg(yaw);
+    airspeedVector[1] = sin_lookup_deg(yaw);
+    // vector projection of groundspeed on airspeed vector to handle both forward and backwards movement
+    float groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1];
+
+    indicatedAirspeedStateBias = airspeedState.CalibratedAirspeed - groundspeedProjection;
+    // note - we do fly by Indicated Airspeed (== calibrated airspeed) however
+    // since airspeed is updated less often than groundspeed, we use sudden
+    // changes to groundspeed to offset the airspeed by the same measurement.
+    // This has a side effect that in the absence of any airspeed updates, the
+    // pathfollower will fly using groundspeed.
+}

flight/modules/PathFollower/grounddrivecontroller.cpp

@@ -0,0 +1,303 @@
+/*
+ ******************************************************************************
+ *
+ * @file       grounddrivecontroller.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Ground drive controller
+ *		the required PathDesired LAND mode.
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+extern "C" {
+#include <openpilot.h>
+
+#include <callbackinfo.h>
+
+#include <math.h>
+#include <pid.h>
+#include <CoordinateConversions.h>
+#include <sin_lookup.h>
+#include <pathdesired.h>
+#include <paths.h>
+#include "plans.h"
+#include <sanitycheck.h>
+
+#include <homelocation.h>
+#include <accelstate.h>
+#include <groundpathfollowersettings.h>
+#include <flightstatus.h>
+#include <flightmodesettings.h>
+#include <pathstatus.h>
+#include <positionstate.h>
+#include <velocitystate.h>
+#include <velocitydesired.h>
+#include <stabilizationdesired.h>
+#include <airspeedstate.h>
+#include <attitudestate.h>
+#include <takeofflocation.h>
+#include <poilocation.h>
+#include <manualcontrolcommand.h>
+#include <systemsettings.h>
+#include <stabilizationbank.h>
+#include <stabilizationdesired.h>
+#include <pathsummary.h>
+#include <statusgrounddrive.h>
+}
+
+// C++ includes
+#include "grounddrivecontroller.h"
+
+// Private constants
+
+// pointer to a singleton instance
+GroundDriveController *GroundDriveController::p_inst = 0;
+
+GroundDriveController::GroundDriveController()
+    : groundSettings(0), mActive(false), mMode(0)
+{}
+
+// Called when mode first engaged
+void GroundDriveController::Activate(void)
+{
+    if (!mActive) {
+        mActive = true;
+        SettingsUpdated();
+        controlNE.Activate();
+        mMode   = pathDesired->Mode;
+    }
+}
+
+uint8_t GroundDriveController::IsActive(void)
+{
+    return mActive;
+}
+
+uint8_t GroundDriveController::Mode(void)
+{
+    return mMode;
+}
+
+// Objective updated in pathdesired
+void GroundDriveController::ObjectiveUpdated(void)
+{}
+
+void GroundDriveController::Deactivate(void)
+{
+    if (mActive) {
+        mActive = false;
+        controlNE.Deactivate();
+    }
+}
+
+
+void GroundDriveController::SettingsUpdated(void)
+{
+    const float dT = groundSettings->UpdatePeriod / 1000.0f;
+
+    controlNE.UpdatePositionalParameters(groundSettings->HorizontalPosP);
+    controlNE.UpdateParameters(groundSettings->SpeedPI.Kp,
+                               groundSettings->SpeedPI.Ki,
+                               groundSettings->SpeedPI.Kd,
+                               groundSettings->SpeedPI.Beta,
+                               dT,
+                               groundSettings->HorizontalVelMax);
+
+
+    // max/min are NE command values equivalent to thrust but must be symmetrical as this is NE not forward/reverse.
+    controlNE.UpdateCommandParameters(-groundSettings->ThrustLimit.Max, groundSettings->ThrustLimit.Max, groundSettings->VelocityFeedForward);
+}
+
+/**
+ * Initialise the module, called on startup
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t GroundDriveController::Initialize(GroundPathFollowerSettingsData *ptr_groundSettings)
+{
+    PIOS_Assert(ptr_groundSettings);
+
+    groundSettings = ptr_groundSettings;
+
+    return 0;
+}
+
+void GroundDriveController::UpdateAutoPilot()
+{
+    uint8_t result = updateAutoPilotGround();
+
+    if (result) {
+        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
+    } else {
+        pathStatus->Status = PATHSTATUS_STATUS_CRITICAL;
+        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
+    }
+
+    PathStatusSet(pathStatus);
+}
+
+/**
+ * fixed wing autopilot:
+ * straight forward:
+ * 1. update path velocity for limited motion crafts
+ * 2. update attitude according to default fixed wing pathfollower algorithm
+ */
+uint8_t GroundDriveController::updateAutoPilotGround()
+{
+    updatePathVelocity(groundSettings->CourseFeedForward);
+    return updateGroundDesiredAttitude();
+}
+
+/**
+ * Compute desired velocity from the current position and path
+ */
+void GroundDriveController::updatePathVelocity(float kFF)
+{
+    PositionStateData positionState;
+
+    PositionStateGet(&positionState);
+    VelocityStateData velocityState;
+    VelocityStateGet(&velocityState);
+    VelocityDesiredData velocityDesired;
+    controlNE.UpdateVelocityState(velocityState.North, velocityState.East);
+
+    // look ahead kFF seconds
+    float cur[3] = { positionState.North + (velocityState.North * kFF),
+                     positionState.East + (velocityState.East * kFF),
+                     positionState.Down + (velocityState.Down * kFF) };
+    struct path_status progress;
+    path_progress(pathDesired, cur, &progress, false);
+
+    // GOTOENDPOINT: correction_vector is distance array to endpoint, path_vector is velocity vector
+    // FOLLOWVECTOR:  correct_vector is distance to vector path, path_vector is the desired velocity vector
+
+    // Calculate the desired velocity from the lateral vector path errors (correct_vector) and the desired velocity vector (path_vector)
+    controlNE.ControlPositionWithPath(&progress);
+    float north, east;
+    controlNE.GetVelocityDesired(&north, &east);
+    velocityDesired.North = north;
+    velocityDesired.East  = east;
+    velocityDesired.Down  = 0.0f;
+
+    // update pathstatus
+    pathStatus->error     = progress.error;
+    pathStatus->fractional_progress  = progress.fractional_progress;
+    // FOLLOWVECTOR: desired velocity vector
+    pathStatus->path_direction_north = progress.path_vector[0];
+    pathStatus->path_direction_east  = progress.path_vector[1];
+    pathStatus->path_direction_down  = progress.path_vector[2];
+
+    // FOLLOWVECTOR: correction distance to vector path
+    pathStatus->correction_direction_north = progress.correction_vector[0];
+    pathStatus->correction_direction_east  = progress.correction_vector[1];
+    pathStatus->correction_direction_down  = progress.correction_vector[2];
+
+    VelocityDesiredSet(&velocityDesired);
+}
+
+/**
+ * Compute desired attitude for ground vehicles
+ */
+uint8_t GroundDriveController::updateGroundDesiredAttitude()
+{
+    StatusGroundDriveData statusGround;
+    VelocityStateData velocityState;
+
+    VelocityStateGet(&velocityState);
+    AttitudeStateData attitudeState;
+    AttitudeStateGet(&attitudeState);
+    statusGround.State.Yaw = attitudeState.Yaw;
+    statusGround.State.Velocity = sqrtf(velocityState.North * velocityState.North + velocityState.East * velocityState.East);
+
+    StabilizationDesiredData stabDesired;
+    StabilizationDesiredGet(&stabDesired);
+
+    // estimate a north/east command value to control the velocity error.
+    // ThrustLimits.Max(+-) limits the range.  Think of this as a command unit vector
+    // of the ultimate direction to reduce lateral error and achieve the target direction (desired angle).
+    float northCommand, eastCommand;
+    controlNE.GetNECommand(&northCommand, &eastCommand);
+
+    // Get current vehicle orientation
+    float angle_radians  = DEG2RAD(attitudeState.Yaw); // (+-pi)
+    float cos_angle      = cosf(angle_radians);
+    float sine_angle     = sinf(angle_radians);
+
+    float courseCommand  = 0.0f;
+    float speedCommand   = 0.0f;
+    float lateralCommand = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -groundSettings->ThrustLimit.Max, groundSettings->ThrustLimit.Max);
+    float forwardCommand = boundf(northCommand * cos_angle + eastCommand * sine_angle, -groundSettings->ThrustLimit.Max, groundSettings->ThrustLimit.Max);
+    // +ve facing correct direction, lateral command should just correct angle,
+    if (forwardCommand > 0.0f) {
+        // if +ve forward command, -+ lateralCommand drives steering to manage lateral error and angular error
+
+        courseCommand = boundf(lateralCommand, -1.0f, 1.0f);
+        speedCommand  = boundf(forwardCommand, groundSettings->ThrustLimit.SlowForward, groundSettings->ThrustLimit.Max);
+
+        // reinstate max thrust
+        controlNE.UpdateCommandParameters(-groundSettings->ThrustLimit.Max, groundSettings->ThrustLimit.Max, groundSettings->VelocityFeedForward);
+
+        statusGround.ControlState = STATUSGROUNDDRIVE_CONTROLSTATE_ONTRACK;
+    } else {
+        // -ve facing opposite direction, lateral command irrelevant, need to turn to change direction and do so slowly.
+
+        // Reduce steering angle based on current velocity
+        float steeringReductionFactor = 1.0f;
+        if (stabDesired.Thrust > 0.3f) {
+            steeringReductionFactor = (groundSettings->HorizontalVelMax - statusGround.State.Velocity) / groundSettings->HorizontalVelMax;
+            steeringReductionFactor = boundf(steeringReductionFactor, 0.05f, 1.0f);
+        }
+
+        // should we turn left or right?
+        if (lateralCommand >= 0.1f) {
+            courseCommand = 1.0f * steeringReductionFactor;
+            statusGround.ControlState = STATUSGROUNDDRIVE_CONTROLSTATE_TURNAROUNDRIGHT;
+        } else {
+            courseCommand = -1.0f * steeringReductionFactor;
+            statusGround.ControlState = STATUSGROUNDDRIVE_CONTROLSTATE_TURNAROUNDLEFT;
+        }
+
+        // Impose limits to slow down.
+        controlNE.UpdateCommandParameters(-groundSettings->ThrustLimit.SlowForward, groundSettings->ThrustLimit.SlowForward, 0.0f);
+
+        speedCommand = groundSettings->ThrustLimit.SlowForward;
+    }
+
+    stabDesired.Roll   = 0.0f;
+    stabDesired.Pitch  = 0.0f;
+    stabDesired.Yaw    = courseCommand;
+
+    // Mix yaw into thrust limit TODO
+    stabDesired.Thrust = boundf(speedCommand, groundSettings->ThrustLimit.Min, groundSettings->ThrustLimit.Max);
+
+    stabDesired.StabilizationMode.Roll   = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
+    stabDesired.StabilizationMode.Pitch  = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
+    stabDesired.StabilizationMode.Yaw    = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
+    stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
+    StabilizationDesiredSet(&stabDesired);
+
+    statusGround.NECommand.North       = northCommand;
+    statusGround.NECommand.East        = eastCommand;
+    statusGround.State.Thrust          = stabDesired.Thrust;
+    statusGround.BodyCommand.Forward   = forwardCommand;
+    statusGround.BodyCommand.Right     = lateralCommand;
+    statusGround.ControlCommand.Course = courseCommand;
+    statusGround.ControlCommand.Speed  = speedCommand;
+    StatusGroundDriveSet(&statusGround);
+
+    return 1;
+}

flight/modules/PathFollower/inc/fixedwingflycontroller.h

@@ -0,0 +1,80 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup FixedWing CONTROL interface class
+ * @brief CONTROL interface class for pathfollower fixed wing fly controller
+ * @{
+ *
+ * @file       FixedWingCONTROL.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Executes CONTROL for fixed wing fly objectives
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+#ifndef FIXEDWINGFLYCONTROLLER_H
+#define FIXEDWINGFLYCONTROLLER_H
+#include "pathfollowercontrol.h"
+
+class FixedWingFlyController : public PathFollowerControl {
+private:
+    static FixedWingFlyController *p_inst;
+    FixedWingFlyController();
+
+
+public:
+    static FixedWingFlyController *instance()
+    {
+        if (!p_inst) {
+            p_inst = new FixedWingFlyController();
+        }
+        return p_inst;
+    }
+
+    int32_t Initialize(FixedWingPathFollowerSettingsData *fixedWingSettings);
+    void Activate(void);
+    void Deactivate(void);
+    void SettingsUpdated(void);
+    void UpdateAutoPilot(void);
+    void ObjectiveUpdated(void);
+    uint8_t IsActive(void);
+    uint8_t Mode(void);
+    void AirspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent * ev);
+
+private:
+    void resetGlobals();
+    uint8_t updateAutoPilotFixedWing();
+    void updatePathVelocity(float kFF, bool limited);
+    uint8_t updateFixedDesiredAttitude();
+    bool correctCourse(float *C, float *V, float *F, float s);
+
+    FixedWingPathFollowerSettingsData *fixedWingSettings;
+    uint8_t mActive;
+    uint8_t mMode;
+
+    struct pid PIDposH[2];
+    struct pid PIDposV;
+    struct pid PIDcourse;
+    struct pid PIDspeed;
+    struct pid PIDpower;
+    // correct speed by measured airspeed
+    float indicatedAirspeedStateBias;
+};
+
+#endif // FIXEDWINGFLYCONTROLLER_H

flight/modules/PathFollower/inc/grounddrivecontroller.h

@@ -0,0 +1,71 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup Ground CONTROL interface class
+ * @brief CONTROL interface class for ground drive controller
+ * @{
+ *
+ * @file       grounddrivecontroller.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Class definition for ground drive controller implementation
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+#ifndef GROUNDDRIVECONTROLLER_H
+#define GROUNDDRIVECONTROLLER_H
+#include "pathfollowercontrol.h"
+#include "pidcontrolne.h"
+
+class GroundDriveController : public PathFollowerControl {
+private:
+    static GroundDriveController *p_inst;
+    GroundDriveController();
+
+
+public:
+    static GroundDriveController *instance()
+    {
+        if (!p_inst) {
+            p_inst = new GroundDriveController();
+        }
+        return p_inst;
+    }
+
+    int32_t Initialize(GroundPathFollowerSettingsData *groundSettings);
+    void Activate(void);
+    void Deactivate(void);
+    void SettingsUpdated(void);
+    void UpdateAutoPilot(void);
+    void ObjectiveUpdated(void);
+    uint8_t IsActive(void);
+    uint8_t Mode(void);
+
+private:
+    uint8_t updateAutoPilotGround();
+    void updatePathVelocity(float kFF);
+    uint8_t updateGroundDesiredAttitude();
+
+    GroundPathFollowerSettingsData *groundSettings;
+    uint8_t mActive;
+    uint8_t mMode;
+    PIDControlNE controlNE;
+};
+
+#endif // GROUNDDRIVECONTROLLER_H

flight/modules/PathFollower/inc/pathfollowercontrol.h

@@ -1,14 +1,15 @@
 /**
  ******************************************************************************
- * @addtogroup CopterControlBL CopterControl BootLoader
+ * @addtogroup OpenPilotModules OpenPilot Modules
- * @brief These files contain the code to the CopterControl Bootloader.
  * @{
- * @file       pios_config.h
+ * @addtogroup PathFollower CONTROL interface class
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief CONTROL interface class for pathfollower goal implementations
- * @brief      PiOS configuration header.
+ * @{
- *             Central compile time config for the project.
+ *
- *             In particular, pios_config.h is where you define which PiOS libraries
+ * @file       pathfollowercontrol.h
- *             and features are included in the firmware.
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Interface class for controllers
+ *
  * @see        The GNU Public License (GPL) Version 3
  *
  *****************************************************************************/
@@ -27,27 +28,23 @@
  * with this program; if not, write to the Free Software Foundation, Inc.,
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
+#ifndef PATHFOLLOWERCONTROL_H
+#define PATHFOLLOWERCONTROL_H
+class PathFollowerControl {
+public:
+    virtual void Activate(void)   = 0;
+    virtual void Deactivate(void) = 0;
+    virtual void SettingsUpdated(void)  = 0;
+    virtual void UpdateAutoPilot(void)  = 0;
+    virtual void ObjectiveUpdated(void) = 0;
+    virtual uint8_t Mode(void) = 0;
+    static int32_t Initialize(PathDesiredData *ptr_pathDesired,
+                              FlightStatusData *ptr_flightStatus,
+                              PathStatusData *ptr_pathStatus);
+protected:
+    static PathDesiredData *pathDesired;
+    static FlightStatusData *flightStatus;
+    static PathStatusData *pathStatus;
+};
 
-#ifndef PIOS_CONFIG_H
+#endif // PATHFOLLOWERCONTROL_H
-#define PIOS_CONFIG_H
-
-/* Enable/Disable PiOS modules */
-#define PIOS_INCLUDE_DELAY
-#define PIOS_INCLUDE_SYS
-#define PIOS_INCLUDE_IRQ
-#define PIOS_INCLUDE_GPIO
-#define PIOS_INCLUDE_USB
-#define PIOS_INCLUDE_USB_HID
-#define PIOS_INCLUDE_LED
-#define PIOS_INCLUDE_IAP
-#define PIOS_INCLUDE_COM
-#define PIOS_INCLUDE_COM_MSG
-#define PIOS_INCLUDE_BL_HELPER
-#define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT
-
-#endif /* PIOS_CONFIG_H */
-
-/**
- * @}
- * @}
- */

flight/modules/PathFollower/inc/pathfollowerfsm.h

@@ -0,0 +1,81 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup PathFollower FSM interface class
+ * @brief FSM interface class for pathfollower goal fsm implementations
+ * @{
+ *
+ * @file       pathfollowerfsm.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Interface class for PathFollower FSMs
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+#ifndef PATHFOLLOWERFSM_H
+#define PATHFOLLOWERFSM_H
+
+extern "C" {
+#include <stabilizationdesired.h>
+}
+#include <pidcontroldowncallback.h>
+
+typedef enum {
+    PFFSM_STATE_INACTIVE = 0, // Inactive state is the initialised state on startup
+    PFFSM_STATE_ACTIVE,
+    PFFSM_STATE_DISARMED,
+    PFFSM_STATE_ABORT // Abort on error
+} PathFollowerFSMState_T;
+
+class PathFollowerFSM : public PIDControlDownCallback {
+public:
+    // PathFollowerFSM() {};
+    virtual void Inactive(void) {}
+    virtual void Activate(void) {}
+    virtual void Update(void) {}
+    virtual void SettingsUpdated(void) {}
+    virtual void BoundThrust(__attribute__((unused)) float &ulow, __attribute__((unused)) float &uhigh) {}
+    virtual PathFollowerFSMState_T GetCurrentState(void)
+    {
+        return PFFSM_STATE_INACTIVE;
+    }
+    virtual void ConstrainStabiDesired(__attribute__((unused)) StabilizationDesiredData *stabDesired) {}
+    virtual float BoundVelocityDown(float velocity)
+    {
+        return velocity;
+    }
+    virtual void CheckPidScaler(__attribute__((unused)) pid_scaler *scaler) {}
+    virtual void GetYaw(bool &yaw_attitude, float &yaw_direction)
+    {
+        yaw_attitude = false; yaw_direction = 0.0f;
+    }
+    virtual void Abort(void) {}
+    virtual uint8_t ManualThrust(void)
+    {
+        return false;
+    }
+    virtual uint8_t PositionHoldState(void)
+    {
+        return false;
+    }
+
+    // virtual ~PathFollowerFSM() = 0;
+};
+
+#endif // PATHFOLLOWERFSM_H

flight/modules/PathFollower/inc/pidcontrolne.h

@@ -0,0 +1,80 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup PathFollower CONTROL interface class
+ * @brief PID Controler for NE Class definition
+ * @{
+ *
+ * @file       pidcontrolne.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Executes pid control loop for NE
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+#ifndef PIDCONTROLNE_H
+#define PIDCONTROLNE_H
+extern "C" {
+#include <pid.h>
+}
+#include "pathfollowerfsm.h"
+
+class PIDControlNE {
+public:
+    PIDControlNE();
+    ~PIDControlNE();
+    void Initialize();
+    void Deactivate();
+    void Activate();
+    void UpdateParameters(float kp, float ki, float kd, __attribute__((unused)) float ilimit, float dT, float velocityMax);
+    void UpdatePositionalParameters(float kp);
+    void UpdatePositionState(float pvNorth, float pvEast);
+    void UpdatePositionSetpoint(float setpointNorth, float setpointEast);
+    void UpdateVelocitySetpoint(float setpointNorth, float setpointEast);
+    // void RateLimit(float *spDesired, float *spCurrent, float rateLimit);
+    void UpdateVelocityState(float pvNorth, float pvEast);
+    void UpdateCommandParameters(float minCommand, float maxCommand, float velocityFeedforward);
+    void ControlPosition();
+    void ControlPositionWithPath(struct path_status *progress);
+    void GetNECommand(float *northCommand, float *eastCommand);
+    void GetVelocityDesired(float *north, float *east);
+    void UpdateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity);
+    void UpdateVelocityStateWithBrake(float pvNorth, float pvEast, float path_time, float brakeRate);
+
+private:
+    struct pid2 PIDvel[2]; // North, East
+    float mVelocitySetpointTarget[2];
+    float mVelocityState[2];
+    struct pid PIDposH[2];
+    float mPositionSetpointTarget[2];
+    float mPositionState[2];
+
+
+    float deltaTime;
+    float mVelocitySetpointCurrent[2];
+    float mNECommand;
+    float mNeutral;
+    float mVelocityMax;
+    float mMinCommand;
+    float mMaxCommand;
+    float mVelocityFeedforward;
+    uint8_t mActive;
+};
+
+#endif // PIDCONTROLNE_H

flight/modules/PathFollower/inc/vtolautotakeoffcontroller.h

@@ -0,0 +1,76 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup PathFollower CONTROL interface class
+ * @brief vtol land controller class
+ * @{
+ *
+ * @file       vtollandcontroller.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Executes CONTROL for landing sequence
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+#ifndef VTOLAUTOTAKEOFFCONTROLLER_H
+#define VTOLAUTOTAKEOFFCONTROLLER_H
+#include "pathfollowercontrol.h"
+#include "pidcontroldown.h"
+#include "pidcontrolne.h"
+// forward decl
+class VtolAutoTakeoffFSM;
+class VtolAutoTakeoffController : public PathFollowerControl {
+private:
+    static VtolAutoTakeoffController *p_inst;
+    VtolAutoTakeoffController();
+
+
+public:
+    static VtolAutoTakeoffController *instance()
+    {
+        if (!p_inst) {
+            p_inst = new VtolAutoTakeoffController();
+        }
+        return p_inst;
+    }
+
+    int32_t Initialize(VtolPathFollowerSettingsData *vtolPathFollowerSettings);
+
+
+    void Activate(void);
+    void Deactivate(void);
+    void SettingsUpdated(void);
+    void UpdateAutoPilot(void);
+    void ObjectiveUpdated(void);
+    uint8_t IsActive(void);
+    uint8_t Mode(void);
+
+private:
+    void UpdateVelocityDesired(void);
+    int8_t UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction);
+    void setArmedIfChanged(FlightStatusArmedOptions val);
+
+    VtolAutoTakeoffFSM *fsm;
+    VtolPathFollowerSettingsData *vtolPathFollowerSettings;
+    PIDControlDown controlDown;
+    PIDControlNE controlNE;
+    uint8_t mActive;
+};
+
+#endif // VTOLAUTOTAKEOFFCONTROLLER_H

flight/modules/PathFollower/inc/vtolautotakeofffsm.h

@@ -0,0 +1,158 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup PathFollower FSM
+ * @brief Executes landing sequence via an FSM
+ * @{
+ *
+ * @file       vtollandfsm.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Executes FSM for landing sequence
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+#ifndef VTOLAUTOTAKEOFFFSM_H
+#define VTOLAUTOTAKEOFFFSM_H
+
+extern "C" {
+#include "statusvtolautotakeoff.h"
+}
+#include "pathfollowerfsm.h"
+
+// AutoTakeoffing state machine
+typedef enum {
+    AUTOTAKEOFF_STATE_INACTIVE = 0, // Inactive state is the initialised state on startup
+    AUTOTAKEOFF_STATE_CHECKSTATE, // Initial condition checks
+    AUTOTAKEOFF_STATE_SLOWSTART, // Slow start motors
+    AUTOTAKEOFF_STATE_THRUSTUP, // Ramp motors up to neutral thrust
+    AUTOTAKEOFF_STATE_TAKEOFF, // Ascend to target velocity
+    AUTOTAKEOFF_STATE_HOLD, // Hold position as completion of the sequence
+    AUTOTAKEOFF_STATE_THRUSTDOWN, // Thrust down sequence
+    AUTOTAKEOFF_STATE_THRUSTOFF, // Thrust is now off
+    AUTOTAKEOFF_STATE_DISARMED, // Disarmed
+    AUTOTAKEOFF_STATE_ABORT, // Abort on error triggerig fallback to hold
+    AUTOTAKEOFF_STATE_SIZE
+} PathFollowerFSM_AutoTakeoffState_T;
+
+class VtolAutoTakeoffFSM : public PathFollowerFSM {
+private:
+    static VtolAutoTakeoffFSM *p_inst;
+    VtolAutoTakeoffFSM();
+
+public:
+    static VtolAutoTakeoffFSM *instance()
+    {
+        if (!p_inst) {
+            p_inst = new VtolAutoTakeoffFSM();
+        }
+        return p_inst;
+    }
+    int32_t Initialize(VtolPathFollowerSettingsData *vtolPathFollowerSettings,
+                       PathDesiredData *pathDesired,
+                       FlightStatusData *flightStatus);
+    void Inactive(void);
+    void Activate(void);
+    void Update(void);
+    void BoundThrust(float &ulow, float &uhigh);
+    PathFollowerFSMState_T GetCurrentState(void);
+    void ConstrainStabiDesired(StabilizationDesiredData *stabDesired);
+    void Abort(void);
+    uint8_t PositionHoldState(void);
+    void setControlState(StatusVtolAutoTakeoffControlStateOptions controlState);
+
+protected:
+
+    // FSM instance data type
+    typedef struct {
+        StatusVtolAutoTakeoffData fsmAutoTakeoffStatus;
+        StatusVtolAutoTakeoffStateOptions currentState;
+        TakeOffLocationData takeOffLocation;
+        uint32_t stateRunCount;
+        uint32_t stateTimeoutCount;
+        float    sum1;
+        float    sum2;
+        float    expectedAutoTakeoffPositionNorth;
+        float    expectedAutoTakeoffPositionEast;
+        float    thrustLimit;
+        float    boundThrustMin;
+        float    boundThrustMax;
+        uint8_t  observationCount;
+        uint8_t  observation2Count;
+        uint8_t  flZeroStabiHorizontal;
+        uint8_t  flConstrainThrust;
+        uint8_t  flLowAltitude;
+        uint8_t  flAltitudeHold;
+    } VtolAutoTakeoffFSMData_T;
+
+    // FSM state structure
+    typedef struct {
+        void(VtolAutoTakeoffFSM::*setup) (void); // Called to initialise the state
+        void(VtolAutoTakeoffFSM::*run) (uint8_t); // Run the event detection code for a state
+    } PathFollowerFSM_AutoTakeoffStateHandler_T;
+
+    // Private variables
+    VtolAutoTakeoffFSMData_T *mAutoTakeoffData;
+    VtolPathFollowerSettingsData *vtolPathFollowerSettings;
+    PathDesiredData *pathDesired;
+    FlightStatusData *flightStatus;
+
+    void setup_inactive(void);
+
+    void setup_checkstate(void);
+
+    void setup_slowstart(void);
+    void run_slowstart(uint8_t);
+
+    void setup_takeoff(void);
+    void run_takeoff(uint8_t);
+
+    void setup_hold(void);
+    void run_hold(uint8_t);
+
+    void setup_thrustup(void);
+    void run_thrustup(uint8_t);
+
+    void setup_thrustdown(void);
+    void run_thrustdown(uint8_t);
+
+    void setup_thrustoff(void);
+    void run_thrustoff(uint8_t);
+
+    void setup_disarmed(void);
+    void run_disarmed(uint8_t);
+
+    void setup_abort(void);
+    void run_abort(uint8_t);
+
+    void initFSM(void);
+    void setState(StatusVtolAutoTakeoffStateOptions newState, StatusVtolAutoTakeoffStateExitReasonOptions reason);
+    int32_t runState();
+    int32_t runAlways();
+
+    void updateVtolAutoTakeoffFSMStatus();
+    void assessAltitude(void);
+
+    void setStateTimeout(int32_t count);
+    void fallback_to_hold(void);
+
+    static PathFollowerFSM_AutoTakeoffStateHandler_T sAutoTakeoffStateTable[AUTOTAKEOFF_STATE_SIZE];
+};
+
+#endif // VTOLAUTOTAKEOFFFSM_H

flight/modules/PathFollower/inc/vtolbrakecontroller.h

@@ -0,0 +1,76 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup PathFollower CONTROL interface class
+ * @brief CONTROL interface class for brake controller
+ * @{
+ *
+ * @file       vtolbrakecontroller.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Executes brake controller for vtol
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+#ifndef VTOLBRAKECONTROLLER_H
+#define VTOLBRAKECONTROLLER_H
+#include "pathfollowercontrol.h"
+#include "pidcontroldown.h"
+#include "pidcontrolne.h"
+// forward decl
+class PathFollowerFSM;
+class VtolBrakeController : public PathFollowerControl {
+private:
+    static VtolBrakeController *p_inst;
+    VtolBrakeController();
+
+
+public:
+    static VtolBrakeController *instance()
+    {
+        if (!p_inst) {
+            p_inst = new VtolBrakeController();
+        }
+        return p_inst;
+    }
+
+    int32_t Initialize(VtolPathFollowerSettingsData *vtolPathFollowerSettings);
+    void Activate(void);
+    void Deactivate(void);
+    void SettingsUpdated(void);
+    void UpdateAutoPilot(void);
+    void ObjectiveUpdated(void);
+    uint8_t IsActive(void);
+    uint8_t Mode(void);
+
+private:
+    void UpdateVelocityDesired(void);
+    int8_t UpdateStabilizationDesired(void);
+    void updateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity);
+
+    PathFollowerFSM *fsm;
+    VtolPathFollowerSettingsData *vtolPathFollowerSettings;
+    PIDControlDown controlDown;
+    PIDControlNE controlNE;
+    uint8_t mActive;
+    uint8_t mHoldActive;
+    uint8_t mManualThrust;
+};
+
+#endif // VTOLBRAKECONTROLLER_H

flight/modules/PathFollower/inc/vtolbrakefsm.h

@@ -0,0 +1,111 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup PathFollower FSM Brake
+ * @brief Executes brake seqeuence
+ * @{
+ *
+ * @file       vtolbrakfsm.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Executes brake sequence fsm
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+#ifndef VTOLBRAKEFSM_H
+#define VTOLBRAKEFSM_H
+
+#include "pathfollowerfsm.h"
+
+// Brakeing state machine
+typedef enum {
+    BRAKE_STATE_INACTIVE = 0, // Inactive state is the initialised state on startup
+    BRAKE_STATE_BRAKE, // Initiate altitude hold before starting descent
+    BRAKE_STATE_HOLD, // Waiting for attainment of landing descent rate
+    BRAKE_STATE_SIZE
+} PathFollowerFSM_BrakeState_T;
+
+typedef enum {
+    FSMBRAKESTATUS_STATEEXITREASON_NONE = 0
+} VtolBrakeFSMStatusStateExitReasonOptions;
+
+class VtolBrakeFSM : public PathFollowerFSM {
+private:
+    static VtolBrakeFSM *p_inst;
+    VtolBrakeFSM();
+
+public:
+    static VtolBrakeFSM *instance()
+    {
+        if (!p_inst) {
+            p_inst = new VtolBrakeFSM();
+        }
+        return p_inst;
+    }
+    int32_t Initialize(VtolPathFollowerSettingsData *vtolPathFollowerSettings,
+                       PathDesiredData *pathDesired,
+                       FlightStatusData *flightStatus,
+                       PathStatusData *ptr_pathStatus);
+    void Inactive(void);
+    void Activate(void);
+    void Update(void);
+    PathFollowerFSMState_T GetCurrentState(void);
+    void Abort(void);
+    uint8_t PositionHoldState(void);
+
+protected:
+
+    // FSM instance data type
+    typedef struct {
+        PathFollowerFSM_BrakeState_T currentState;
+        uint32_t stateRunCount;
+        uint32_t stateTimeoutCount;
+        float    sum1;
+        float    sum2;
+        uint8_t  observationCount;
+        uint8_t  observation2Count;
+    } VtolBrakeFSMData_T;
+
+    // FSM state structure
+    typedef struct {
+        void(VtolBrakeFSM::*setup) (void); // Called to initialise the state
+        void(VtolBrakeFSM::*run) (uint8_t); // Run the event detection code for a state
+    } PathFollowerFSM_BrakeStateHandler_T;
+
+    // Private variables
+    VtolBrakeFSMData_T *mBrakeData;
+    VtolPathFollowerSettingsData *vtolPathFollowerSettings;
+    PathDesiredData *pathDesired;
+    PathStatusData *pathStatus;
+    FlightStatusData *flightStatus;
+
+    void setup_brake(void);
+    void run_brake(uint8_t);
+    void initFSM(void);
+    void setState(PathFollowerFSM_BrakeState_T newState, VtolBrakeFSMStatusStateExitReasonOptions reason);
+    int32_t runState();
+    // void updateVtolBrakeFSMStatus();
+
+    void setStateTimeout(int32_t count);
+    void fallback_to_hold(void);
+
+    static PathFollowerFSM_BrakeStateHandler_T sBrakeStateTable[BRAKE_STATE_SIZE];
+};
+
+#endif // VTOLBRAKEFSM_H

flight/modules/PathFollower/inc/vtolflycontroller.h

@@ -0,0 +1,84 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup PathFollower CONTROL interface class
+ * @brief vtol fly controller class definition
+ * @{
+ *
+ * @file       vtolflycontroller.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Executes fly control for vtols
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+#ifndef VTOLFLYCONTROLLER_H
+#define VTOLFLYCONTROLLER_H
+#include "pathfollowercontrol.h"
+#include "pidcontrolne.h"
+#include "pidcontroldown.h"
+
+class VtolFlyController : public PathFollowerControl {
+private:
+    static VtolFlyController *p_inst;
+    VtolFlyController();
+
+
+public:
+    static VtolFlyController *instance()
+    {
+        if (!p_inst) {
+            p_inst = new VtolFlyController();
+        }
+        return p_inst;
+    }
+
+    int32_t Initialize(VtolPathFollowerSettingsData *vtolPathFollowerSettings);
+
+
+    void Activate(void);
+    void Deactivate(void);
+    void SettingsUpdated(void);
+    void UpdateAutoPilot(void);
+    void ObjectiveUpdated(void);
+    uint8_t IsActive(void);
+    uint8_t Mode(void);
+
+private:
+    void UpdateVelocityDesired(void);
+    int8_t UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction);
+    void UpdateDesiredAttitudeEmergencyFallback();
+    void fallback_to_hold(void);
+    float updateTailInBearing();
+    float updateCourseBearing();
+    float updatePathBearing();
+    float updatePOIBearing();
+    uint8_t RunAutoPilot();
+
+    VtolPathFollowerSettingsData *vtolPathFollowerSettings;
+    PIDControlNE controlNE;
+    PIDControlDown controlDown;
+    uint8_t mActive;
+    uint8_t mManualThrust;
+    uint8_t mMode;
+    float vtolEmergencyFallback;
+    bool vtolEmergencyFallbackSwitch;
+};
+
+#endif // VTOLFLYCONTROLLER_H

flight/modules/PathFollower/inc/vtollandcontroller.h

@@ -0,0 +1,76 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup PathFollower CONTROL interface class
+ * @brief vtol land controller class
+ * @{
+ *
+ * @file       vtollandcontroller.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Executes CONTROL for landing sequence
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+#ifndef VTOLLANDCONTROLLER_H
+#define VTOLLANDCONTROLLER_H
+#include "pathfollowercontrol.h"
+#include "pidcontroldown.h"
+#include "pidcontrolne.h"
+// forward decl
+class PathFollowerFSM;
+class VtolLandController : public PathFollowerControl {
+private:
+    static VtolLandController *p_inst;
+    VtolLandController();
+
+
+public:
+    static VtolLandController *instance()
+    {
+        if (!p_inst) {
+            p_inst = new VtolLandController();
+        }
+        return p_inst;
+    }
+
+    int32_t Initialize(VtolPathFollowerSettingsData *vtolPathFollowerSettings);
+
+
+    void Activate(void);
+    void Deactivate(void);
+    void SettingsUpdated(void);
+    void UpdateAutoPilot(void);
+    void ObjectiveUpdated(void);
+    uint8_t IsActive(void);
+    uint8_t Mode(void);
+
+private:
+    void UpdateVelocityDesired(void);
+    int8_t UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction);
+    void setArmedIfChanged(FlightStatusArmedOptions val);
+
+    PathFollowerFSM *fsm;
+    VtolPathFollowerSettingsData *vtolPathFollowerSettings;
+    PIDControlDown controlDown;
+    PIDControlNE controlNE;
+    uint8_t mActive;
+};
+
+#endif // VTOLLANDCONTROLLER_H

flight/modules/PathFollower/inc/vtollandfsm.h

@@ -0,0 +1,148 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup PathFollower FSM
+ * @brief Executes landing sequence via an FSM
+ * @{
+ *
+ * @file       vtollandfsm.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Executes FSM for landing sequence
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+#ifndef VTOLLANDFSM_H
+#define VTOLLANDFSM_H
+
+extern "C" {
+#include "statusvtolland.h"
+}
+#include "pathfollowerfsm.h"
+
+// Landing state machine
+typedef enum {
+    LAND_STATE_INACTIVE = 0, // Inactive state is the initialised state on startup
+    LAND_STATE_INIT_ALTHOLD, // Initiate altitude hold before starting descent
+    LAND_STATE_WTG_FOR_DESCENTRATE, // Waiting for attainment of landing descent rate
+    LAND_STATE_AT_DESCENTRATE, // Landing descent rate achieved
+    LAND_STATE_WTG_FOR_GROUNDEFFECT, // Waiting for group effect to be detected
+    LAND_STATE_GROUNDEFFECT, // Ground effect detected
+    LAND_STATE_THRUSTDOWN, // Thrust down sequence
+    LAND_STATE_THRUSTOFF, // Thrust is now off
+    LAND_STATE_DISARMED, // Disarmed
+    LAND_STATE_ABORT, // Abort on error triggerig fallback to hold
+    LAND_STATE_SIZE
+} PathFollowerFSM_LandState_T;
+
+class VtolLandFSM : public PathFollowerFSM {
+private:
+    static VtolLandFSM *p_inst;
+    VtolLandFSM();
+
+public:
+    static VtolLandFSM *instance()
+    {
+        if (!p_inst) {
+            p_inst = new VtolLandFSM();
+        }
+        return p_inst;
+    }
+    int32_t Initialize(VtolPathFollowerSettingsData *vtolPathFollowerSettings,
+                       PathDesiredData *pathDesired,
+                       FlightStatusData *flightStatus);
+    void Inactive(void);
+    void Activate(void);
+    void Update(void);
+    void BoundThrust(float &ulow, float &uhigh);
+    PathFollowerFSMState_T GetCurrentState(void);
+    void ConstrainStabiDesired(StabilizationDesiredData *stabDesired);
+    float BoundVelocityDown(float);
+    void CheckPidScaler(pid_scaler *scaler);
+    void Abort(void);
+
+protected:
+
+    // FSM instance data type
+    typedef struct {
+        StatusVtolLandData  fsmLandStatus;
+        StatusVtolLandStateOptions currentState;
+        TakeOffLocationData takeOffLocation;
+        uint32_t stateRunCount;
+        uint32_t stateTimeoutCount;
+        float    sum1;
+        float    sum2;
+        float    expectedLandPositionNorth;
+        float    expectedLandPositionEast;
+        float    thrustLimit;
+        float    boundThrustMin;
+        float    boundThrustMax;
+        uint8_t  observationCount;
+        uint8_t  observation2Count;
+        uint8_t  flZeroStabiHorizontal;
+        uint8_t  flConstrainThrust;
+        uint8_t  flLowAltitude;
+        uint8_t  flAltitudeHold;
+    } VtolLandFSMData_T;
+
+    // FSM state structure
+    typedef struct {
+        void(VtolLandFSM::*setup) (void); // Called to initialise the state
+        void(VtolLandFSM::*run) (uint8_t); // Run the event detection code for a state
+    } PathFollowerFSM_LandStateHandler_T;
+
+    // Private variables
+    VtolLandFSMData_T *mLandData;
+    VtolPathFollowerSettingsData *vtolPathFollowerSettings;
+    PathDesiredData *pathDesired;
+    FlightStatusData *flightStatus;
+
+    void setup_inactive(void);
+    void setup_init_althold(void);
+    void setup_wtg_for_descentrate(void);
+    void setup_at_descentrate(void);
+    void setup_wtg_for_groundeffect(void);
+    void run_init_althold(uint8_t);
+    void run_wtg_for_descentrate(uint8_t);
+    void run_at_descentrate(uint8_t);
+    void run_wtg_for_groundeffect(uint8_t);
+    void setup_groundeffect(void);
+    void run_groundeffect(uint8_t);
+    void setup_thrustdown(void);
+    void run_thrustdown(uint8_t);
+    void setup_thrustoff(void);
+    void run_thrustoff(uint8_t);
+    void setup_disarmed(void);
+    void run_disarmed(uint8_t);
+    void setup_abort(void);
+    void run_abort(uint8_t);
+    void initFSM(void);
+    void setState(StatusVtolLandStateOptions newState, StatusVtolLandStateExitReasonOptions reason);
+    int32_t runState();
+    int32_t runAlways();
+    void updateVtolLandFSMStatus();
+    void assessAltitude(void);
+
+    void setStateTimeout(int32_t count);
+    void fallback_to_hold(void);
+
+    static PathFollowerFSM_LandStateHandler_T sLandStateTable[LAND_STATE_SIZE];
+};
+
+#endif // VTOLLANDFSM_H

flight/modules/PathFollower/inc/vtolvelocitycontroller.h

@@ -0,0 +1,72 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup PathFollower Controller
+ * @brief Controller for Vtol velocity roam
+ * @{
+ *
+ * @file       vtolvelocitycontroller.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Executes velocity roam control
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+#ifndef PATHFOLLOWERCONTROLVELOCITYROAM_H
+#define PATHFOLLOWERCONTROLVELOCITYROAM_H
+#include "pathfollowercontrol.h"
+#include "pidcontrolne.h"
+
+class VtolVelocityController : public PathFollowerControl {
+private:
+    static VtolVelocityController *p_inst;
+    VtolVelocityController();
+
+
+public:
+    static VtolVelocityController *instance()
+    {
+        if (!p_inst) {
+            p_inst = new VtolVelocityController();
+        }
+        return p_inst;
+    }
+
+    int32_t Initialize(VtolPathFollowerSettingsData *vtolPathFollowerSettings);
+
+
+    void Activate(void);
+    void Deactivate(void);
+    void SettingsUpdated(void);
+    void UpdateAutoPilot(void);
+    void ObjectiveUpdated(void);
+    uint8_t IsActive(void);
+    uint8_t Mode(void);
+
+private:
+    void UpdateVelocityDesired(void);
+    int8_t UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction);
+    void fallback_to_hold(void);
+
+    VtolPathFollowerSettingsData *vtolPathFollowerSettings;
+    PIDControlNE controlNE;
+    uint8_t mActive;
+};
+
+#endif // ifndef PATHFOLLOWERCONTROLVELOCITYROAM_H

flight/modules/PathFollower/pathfollower.c

@@ -1,1544 +0,0 @@
-/**
- ******************************************************************************
- *
- * @file       pathfollower.c
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      This module compared @ref PositionActuatl to @ref ActiveWaypoint
- * and sets @ref AttitudeDesired.  It only does this when the FlightMode field
- * of @ref ManualControlCommand is Auto.
- *
- * @see        The GNU Public License (GPL) Version 3
- *
- *****************************************************************************/
-/*
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
- * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
- * for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- */
-
-/**
- * Input object: PathDesired
- * Input object: PositionState
- * Input object: ManualControlCommand
- * Output object: StabilizationDesired
- *
- * This module acts as "autopilot" - it controls the setpoints of stabilization
- * based on current flight situation and desired flight path (PathDesired) as
- * directed by flightmode selection or pathplanner
- * This is a periodic delayed callback module
- *
- * Modules have no API, all communication to other modules is done through UAVObjects.
- * However modules may use the API exposed by shared libraries.
- * See the OpenPilot wiki for more details.
- * http://www.openpilot.org/OpenPilot_Application_Architecture
- *
- */
-
-#include <openpilot.h>
-
-#include <callbackinfo.h>
-
-#include <math.h>
-#include <pid.h>
-#include <CoordinateConversions.h>
-#include <sin_lookup.h>
-#include <pathdesired.h>
-#include <paths.h>
-#include "plans.h"
-#include <sanitycheck.h>
-
-
-#include <fixedwingpathfollowersettings.h>
-#include <fixedwingpathfollowerstatus.h>
-#include <vtolpathfollowersettings.h>
-#include <flightstatus.h>
-#include <flightmodesettings.h>
-#include <pathstatus.h>
-#include <positionstate.h>
-#include <velocitystate.h>
-#include <velocitydesired.h>
-#include <stabilizationdesired.h>
-#include <airspeedstate.h>
-#include <attitudestate.h>
-#include <takeofflocation.h>
-#include <poilocation.h>
-#include <manualcontrolcommand.h>
-#include <systemsettings.h>
-#include <stabilizationbank.h>
-#include <vtolselftuningstats.h>
-#include <pathsummary.h>
-
-
-// Private constants
-
-#define CALLBACK_PRIORITY                           CALLBACK_PRIORITY_LOW
-#define CBTASK_PRIORITY                             CALLBACK_TASK_FLIGHTCONTROL
-
-#define PF_IDLE_UPDATE_RATE_MS                      100
-
-#define STACK_SIZE_BYTES                            2048
-
-#define DEADBAND_HIGH                               0.10f
-#define DEADBAND_LOW                                -0.10f
-
-#define BRAKE_FRACTIONALPROGRESS_STARTVELOCITYCHECK 0.95f
-#define BRAKE_EXIT_VELOCITY_LIMIT                   0.2f
-
-#define BRAKE_RATE_MINIMUM                          0.2f
-
-#define NEUTRALTHRUST_PH_POSITIONAL_ERROR_LIMIT     0.5f
-#define NEUTRALTHRUST_PH_VEL_DESIRED_LIMIT          0.2f
-#define NEUTRALTHRUST_PH_VEL_STATE_LIMIT            0.2f
-#define NEUTRALTHRUST_PH_VEL_ERROR_LIMIT            0.1f
-
-#define NEUTRALTHRUST_START_DELAY                   (2 * 20) // 2 seconds at rate of 20Hz (50ms update rate)
-#define NEUTRALTHRUST_END_COUNT                     (NEUTRALTHRUST_START_DELAY + (4 * 20))  // 4 second sample
-
-// Private types
-
-struct Globals {
-    struct pid PIDposH[2];
-    struct pid PIDposV;
-    struct pid PIDvel[3]; // North, East, Down
-    struct pid BrakePIDvel[2]; // North, East
-    struct pid PIDcourse;
-    struct pid PIDspeed;
-    struct pid PIDpower;
-    float poiRadius;
-    float vtolEmergencyFallback;
-    bool  vtolEmergencyFallbackSwitch;
-};
-
-struct NeutralThrustEstimation {
-    uint32_t count;
-    float    sum;
-    float    average;
-    float    correction;
-    float    algo_erro_check;
-    float    min;
-    float    max;
-    bool     start_sampling;
-    bool     have_correction;
-};
-static struct NeutralThrustEstimation neutralThrustEst;
-
-
-// Private variables
-static DelayedCallbackInfo *pathFollowerCBInfo;
-static uint32_t updatePeriod = PF_IDLE_UPDATE_RATE_MS;
-static struct Globals global;
-static PathStatusData pathStatus;
-static PathDesiredData pathDesired;
-static FixedWingPathFollowerSettingsData fixedWingPathFollowerSettings;
-static VtolPathFollowerSettingsData vtolPathFollowerSettings;
-static FlightStatusData flightStatus;
-static PathSummaryData pathSummary;
-
-// correct speed by measured airspeed
-static float indicatedAirspeedStateBias = 0.0f;
-
-
-// Private functions
-static void pathFollowerTask(void);
-static void resetGlobals();
-static void SettingsUpdatedCb(UAVObjEvent *ev);
-static uint8_t updateAutoPilotByFrameType();
-static uint8_t updateAutoPilotFixedWing();
-static uint8_t updateAutoPilotVtol();
-static float updateTailInBearing();
-static float updateCourseBearing();
-static float updatePathBearing();
-static float updatePOIBearing();
-static void processPOI();
-static void updateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity);
-static void updatePathVelocity(float kFF, bool limited);
-static uint8_t updateFixedDesiredAttitude();
-static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction);
-static void updateFixedAttitude();
-static void updateVtolDesiredAttitudeEmergencyFallback();
-static void airspeedStateUpdatedCb(UAVObjEvent *ev);
-static bool correctCourse(float *C, float *V, float *F, float s);
-
-/**
- * Initialise the module, called on startup
- * \returns 0 on success or -1 if initialisation failed
- */
-int32_t PathFollowerStart()
-{
-    // Start main task
-    PathStatusGet(&pathStatus);
-    SettingsUpdatedCb(NULL);
-    PIOS_CALLBACKSCHEDULER_Dispatch(pathFollowerCBInfo);
-
-    return 0;
-}
-
-/**
- * Initialise the module, called on startup
- * \returns 0 on success or -1 if initialisation failed
- */
-int32_t PathFollowerInitialize()
-{
-    // initialize objects
-    FixedWingPathFollowerSettingsInitialize();
-    FixedWingPathFollowerStatusInitialize();
-    VtolPathFollowerSettingsInitialize();
-    FlightStatusInitialize();
-    FlightModeSettingsInitialize();
-    PathStatusInitialize();
-    PathSummaryInitialize();
-    PathDesiredInitialize();
-    PositionStateInitialize();
-    VelocityStateInitialize();
-    VelocityDesiredInitialize();
-    StabilizationDesiredInitialize();
-    AirspeedStateInitialize();
-    AttitudeStateInitialize();
-    TakeOffLocationInitialize();
-    PoiLocationInitialize();
-    ManualControlCommandInitialize();
-    SystemSettingsInitialize();
-    StabilizationBankInitialize();
-    VtolSelfTuningStatsInitialize();
-
-
-    // reset integrals
-    resetGlobals();
-
-    // Create object queue
-    pathFollowerCBInfo = PIOS_CALLBACKSCHEDULER_Create(&pathFollowerTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_PATHFOLLOWER, STACK_SIZE_BYTES);
-    FixedWingPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
-    VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
-    PathDesiredConnectCallback(SettingsUpdatedCb);
-    AirspeedStateConnectCallback(airspeedStateUpdatedCb);
-
-    return 0;
-}
-MODULE_INITCALL(PathFollowerInitialize, PathFollowerStart);
-
-
-/**
- * Module thread, should not return.
- */
-static void pathFollowerTask(void)
-{
-    FlightStatusGet(&flightStatus);
-
-    if (flightStatus.ControlChain.PathFollower != FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
-        resetGlobals();
-        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_UNINITIALISED);
-        PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, PF_IDLE_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
-        return;
-    }
-
-    if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POI) { // TODO Hack from vtolpathfollower, move into manualcontrol!
-        processPOI();
-    }
-
-    int16_t old_uid = pathStatus.UID;
-    pathStatus.UID    = pathDesired.UID;
-    pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
-    if (pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] > 0.0f) {
-        if (old_uid != pathStatus.UID) {
-            pathStatus.path_time = 0.0f;
-        } else {
-            pathStatus.path_time += updatePeriod / 1000.0f;
-        }
-    }
-
-    switch (pathDesired.Mode) {
-    case PATHDESIRED_MODE_FLYENDPOINT:
-    case PATHDESIRED_MODE_FLYVECTOR:
-    case PATHDESIRED_MODE_BRAKE:
-    case PATHDESIRED_MODE_FLYCIRCLERIGHT:
-    case PATHDESIRED_MODE_FLYCIRCLELEFT:
-    {
-        uint8_t result = updateAutoPilotByFrameType();
-        if (result) {
-            AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
-        } else {
-            pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
-            AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
-        }
-    }
-    break;
-    case PATHDESIRED_MODE_FIXEDATTITUDE:
-        updateFixedAttitude(pathDesired.ModeParameters);
-        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
-        break;
-    case PATHDESIRED_MODE_DISARMALARM:
-        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
-        break;
-    default:
-        pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
-        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
-        break;
-    }
-    PathStatusSet(&pathStatus);
-
-
-    PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, updatePeriod, CALLBACK_UPDATEMODE_SOONER);
-}
-
-
-static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
-{
-    FixedWingPathFollowerSettingsGet(&fixedWingPathFollowerSettings);
-
-    pid_configure(&global.PIDcourse, fixedWingPathFollowerSettings.CoursePI.Kp, fixedWingPathFollowerSettings.CoursePI.Ki, 0.0f, fixedWingPathFollowerSettings.CoursePI.ILimit);
-    pid_configure(&global.PIDspeed, fixedWingPathFollowerSettings.SpeedPI.Kp, fixedWingPathFollowerSettings.SpeedPI.Ki, 0.0f, fixedWingPathFollowerSettings.SpeedPI.ILimit);
-    pid_configure(&global.PIDpower, fixedWingPathFollowerSettings.PowerPI.Kp, fixedWingPathFollowerSettings.PowerPI.Ki, 0.0f, fixedWingPathFollowerSettings.PowerPI.ILimit);
-
-
-    VtolPathFollowerSettingsGet(&vtolPathFollowerSettings);
-
-    pid_configure(&global.PIDvel[0], vtolPathFollowerSettings.HorizontalVelPID.Kp, vtolPathFollowerSettings.HorizontalVelPID.Ki, vtolPathFollowerSettings.HorizontalVelPID.Kd, vtolPathFollowerSettings.HorizontalVelPID.ILimit);
-    pid_configure(&global.PIDvel[1], vtolPathFollowerSettings.HorizontalVelPID.Kp, vtolPathFollowerSettings.HorizontalVelPID.Ki, vtolPathFollowerSettings.HorizontalVelPID.Kd, vtolPathFollowerSettings.HorizontalVelPID.ILimit);
-    pid_configure(&global.PIDvel[2], vtolPathFollowerSettings.VerticalVelPID.Kp, vtolPathFollowerSettings.VerticalVelPID.Ki, vtolPathFollowerSettings.VerticalVelPID.Kd, vtolPathFollowerSettings.VerticalVelPID.ILimit);
-
-    pid_configure(&global.BrakePIDvel[0], vtolPathFollowerSettings.BrakeHorizontalVelPID.Kp, vtolPathFollowerSettings.BrakeHorizontalVelPID.Ki, vtolPathFollowerSettings.BrakeHorizontalVelPID.Kd, vtolPathFollowerSettings.BrakeHorizontalVelPID.ILimit);
-    pid_configure(&global.BrakePIDvel[1], vtolPathFollowerSettings.BrakeHorizontalVelPID.Kp, vtolPathFollowerSettings.BrakeHorizontalVelPID.Ki, vtolPathFollowerSettings.BrakeHorizontalVelPID.Kd, vtolPathFollowerSettings.BrakeHorizontalVelPID.ILimit);
-
-    PathDesiredGet(&pathDesired);
-}
-
-
-static void airspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
-{
-    AirspeedStateData airspeedState;
-    VelocityStateData velocityState;
-
-    AirspeedStateGet(&airspeedState);
-    VelocityStateGet(&velocityState);
-    float airspeedVector[2];
-    float yaw;
-    AttitudeStateYawGet(&yaw);
-    airspeedVector[0] = cos_lookup_deg(yaw);
-    airspeedVector[1] = sin_lookup_deg(yaw);
-    // vector projection of groundspeed on airspeed vector to handle both forward and backwards movement
-    float groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1];
-
-    indicatedAirspeedStateBias = airspeedState.CalibratedAirspeed - groundspeedProjection;
-    // note - we do fly by Indicated Airspeed (== calibrated airspeed) however
-    // since airspeed is updated less often than groundspeed, we use sudden
-    // changes to groundspeed to offset the airspeed by the same measurement.
-    // This has a side effect that in the absence of any airspeed updates, the
-    // pathfollower will fly using groundspeed.
-}
-
-
-/**
- * reset integrals
- */
-static void resetGlobals()
-{
-    pid_zero(&global.PIDposH[0]);
-    pid_zero(&global.PIDposH[1]);
-    pid_zero(&global.PIDposV);
-    pid_zero(&global.PIDvel[0]);
-    pid_zero(&global.PIDvel[1]);
-    pid_zero(&global.PIDvel[2]);
-    pid_zero(&global.BrakePIDvel[0]);
-    pid_zero(&global.BrakePIDvel[1]);
-    pid_zero(&global.PIDcourse);
-    pid_zero(&global.PIDspeed);
-    pid_zero(&global.PIDpower);
-    global.poiRadius = 0.0f;
-    global.vtolEmergencyFallback = 0;
-    global.vtolEmergencyFallbackSwitch = false;
-
-    // reset neutral thrust assessment. We restart this process
-    // and do once for each position hold engagement
-    neutralThrustEst.start_sampling  = false;
-    neutralThrustEst.count = 0;
-    neutralThrustEst.sum = 0.0f;
-    neutralThrustEst.have_correction = false;
-    neutralThrustEst.average    = 0.0f;
-    neutralThrustEst.correction = 0.0f;
-    neutralThrustEst.min = 0.0f;
-    neutralThrustEst.max = 0.0f;
-
-    pathStatus.path_time = 0.0f;
-}
-
-static uint8_t updateAutoPilotByFrameType()
-{
-    FrameType_t frameType = GetCurrentFrameType();
-
-    if (frameType == FRAME_TYPE_CUSTOM || frameType == FRAME_TYPE_GROUND) {
-        switch (vtolPathFollowerSettings.TreatCustomCraftAs) {
-        case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING:
-            frameType = FRAME_TYPE_FIXED_WING;
-            break;
-        case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL:
-            frameType = FRAME_TYPE_MULTIROTOR;
-            break;
-        }
-    }
-    switch (frameType) {
-    case FRAME_TYPE_MULTIROTOR:
-    case FRAME_TYPE_HELI:
-        updatePeriod = vtolPathFollowerSettings.UpdatePeriod;
-        return updateAutoPilotVtol();
-
-        break;
-    case FRAME_TYPE_FIXED_WING:
-    default:
-        updatePeriod = fixedWingPathFollowerSettings.UpdatePeriod;
-        return updateAutoPilotFixedWing();
-
-        break;
-    }
-}
-
-/**
- * fixed wing autopilot:
- * straight forward:
- * 1. update path velocity for limited motion crafts
- * 2. update attitude according to default fixed wing pathfollower algorithm
- */
-static uint8_t updateAutoPilotFixedWing()
-{
-    pid_configure(&global.PIDposH[0], fixedWingPathFollowerSettings.HorizontalPosP, 0.0f, 0.0f, 0.0f);
-    pid_configure(&global.PIDposH[1], fixedWingPathFollowerSettings.HorizontalPosP, 0.0f, 0.0f, 0.0f);
-    pid_configure(&global.PIDposV, fixedWingPathFollowerSettings.VerticalPosP, 0.0f, 0.0f, 0.0f);
-    updatePathVelocity(fixedWingPathFollowerSettings.CourseFeedForward, true);
-    return updateFixedDesiredAttitude();
-}
-
-/**
- * vtol autopilot
- * use hover capable algorithm with unlimeted movement calculation. if that fails (flyaway situation due to compass failure)
- * fall back to emergency fallback autopilot to keep minimum amount of flight control
- */
-static uint8_t updateAutoPilotVtol()
-{
-    enum { RETURN_0 = 0, RETURN_1 = 1, RETURN_RESULT } returnmode;
-    enum { FOLLOWER_REGULAR, FOLLOWER_FALLBACK } followermode;
-    uint8_t result = 0;
-
-    // decide on behaviour based on settings and system state
-    if (global.vtolEmergencyFallbackSwitch) {
-        returnmode   = RETURN_0;
-        followermode = FOLLOWER_FALLBACK;
-    } else {
-        if (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_ALWAYS) {
-            returnmode   = RETURN_1;
-            followermode = FOLLOWER_FALLBACK;
-        } else {
-            returnmode   = RETURN_RESULT;
-            followermode = FOLLOWER_REGULAR;
-        }
-    }
-
-    // vertical positon control PID loops works the same in both regular and fallback modes, setup according to settings
-    pid_configure(&global.PIDposV, vtolPathFollowerSettings.VerticalPosP, 0.0f, 0.0f, 0.0f);
-
-    switch (followermode) {
-    case FOLLOWER_REGULAR:
-    {
-        // horizontal position control PID loop works according to settings in regular mode, allowing integral terms
-        pid_configure(&global.PIDposH[0], vtolPathFollowerSettings.HorizontalPosP, 0.0f, 0.0f, 0.0f);
-        pid_configure(&global.PIDposH[1], vtolPathFollowerSettings.HorizontalPosP, 0.0f, 0.0f, 0.0f);
-        updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, false);
-
-        // yaw behaviour is configurable in vtolpathfollower, select yaw control algorithm
-        bool yaw_attitude = true;
-        float yaw = 0.0f;
-
-        if (pathDesired.Mode == PATHDESIRED_MODE_BRAKE) {
-            yaw_attitude = false;
-        } else {
-            switch (vtolPathFollowerSettings.YawControl) {
-            case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MANUAL:
-                yaw_attitude = false;
-                break;
-            case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_TAILIN:
-                yaw = updateTailInBearing();
-                break;
-            case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MOVEMENTDIRECTION:
-                yaw = updateCourseBearing();
-                break;
-            case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_PATHDIRECTION:
-                yaw = updatePathBearing();
-                break;
-            case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_POI:
-                yaw = updatePOIBearing();
-                break;
-            }
-        }
-        result = updateVtolDesiredAttitude(yaw_attitude, yaw);
-
-        if (!result) {
-            if (pathDesired.Mode == PATHDESIRED_MODE_BRAKE) {
-                plan_setup_assistedcontrol(true); // revert braking to position hold, user can always stick override
-            } else if (vtolPathFollowerSettings.FlyawayEmergencyFallback != VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DISABLED) {
-                // switch to emergency follower if follower indicates problems
-                global.vtolEmergencyFallbackSwitch = true;
-            }
-        }
-    }
-    break;
-    case FOLLOWER_FALLBACK:
-    {
-        // fallback loop only cares about intended horizontal flight direction, simplify control behaviour accordingly
-        pid_configure(&global.PIDposH[0], 1.0f, 0.0f, 0.0f, 0.0f);
-        pid_configure(&global.PIDposH[1], 1.0f, 0.0f, 0.0f, 0.0f);
-        updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, true);
-
-        // emergency follower has no return value
-        updateVtolDesiredAttitudeEmergencyFallback();
-    }
-    break;
-    }
-
-
-    // Brake mode end condition checks
-    if (pathDesired.Mode == PATHDESIRED_MODE_BRAKE) {
-        bool exit_brake = false;
-        VelocityStateData velocityState;
-        if (pathStatus.path_time > pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT]) { // enter hold on timeout
-            pathSummary.brake_exit_reason = PATHSUMMARY_BRAKE_EXIT_REASON_TIMEOUT;
-            exit_brake = true;
-        } else if (pathStatus.fractional_progress > BRAKE_FRACTIONALPROGRESS_STARTVELOCITYCHECK) {
-            VelocityStateGet(&velocityState);
-            if (fabsf(velocityState.East) < BRAKE_EXIT_VELOCITY_LIMIT && fabsf(velocityState.North) < BRAKE_EXIT_VELOCITY_LIMIT) {
-                pathSummary.brake_exit_reason = PATHSUMMARY_BRAKE_EXIT_REASON_PATHCOMPLETED;
-                exit_brake = true;
-            }
-        }
-
-        if (exit_brake) {
-            // Calculate the distance error between the originally desired
-            // stopping point and the actual brake-exit point.
-
-            PositionStateData p;
-            PositionStateGet(&p);
-            float north_offset = pathDesired.End.North - p.North;
-            float east_offset  = pathDesired.End.East - p.East;
-            float down_offset  = pathDesired.End.Down - p.Down;
-            pathSummary.brake_distance_offset = sqrtf(north_offset * north_offset + east_offset * east_offset + down_offset * down_offset);
-            pathSummary.time_remaining = pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] - pathStatus.path_time;
-            pathSummary.fractional_progress   = pathStatus.fractional_progress;
-            float cur_velocity = velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down;
-            cur_velocity = sqrtf(cur_velocity);
-            pathSummary.decelrate = (pathDesired.StartingVelocity - cur_velocity) / pathStatus.path_time;
-            pathSummary.brakeRateActualDesiredRatio = pathSummary.decelrate / vtolPathFollowerSettings.BrakeRate;
-            pathSummary.velocityIntoHold = cur_velocity;
-            pathSummary.UID = pathStatus.UID;
-            PathSummarySet(&pathSummary);
-
-            plan_setup_assistedcontrol(true); // braking timeout true
-            // having re-entered hold allow reassessment of neutral thrust
-            neutralThrustEst.have_correction = false;
-        }
-    }
-
-    switch (returnmode) {
-    case RETURN_RESULT:
-        return result;
-
-    default:
-        // returns either 0 or 1 according to enum definition above
-        return returnmode;
-    }
-}
-
-
-/**
- * Compute bearing of current takeoff location
- */
-static float updateTailInBearing()
-{
-    PositionStateData p;
-
-    PositionStateGet(&p);
-    TakeOffLocationData t;
-    TakeOffLocationGet(&t);
-    // atan2f always returns in between + and - 180 degrees
-    return RAD2DEG(atan2f(p.East - t.East, p.North - t.North));
-}
-
-/**
- * Compute bearing of current movement direction
- */
-static float updateCourseBearing()
-{
-    VelocityStateData v;
-
-    VelocityStateGet(&v);
-    // atan2f always returns in between + and - 180 degrees
-    return RAD2DEG(atan2f(v.East, v.North));
-}
-
-/**
- * Compute bearing of current path direction
- */
-static float updatePathBearing()
-{
-    PositionStateData positionState;
-
-    PositionStateGet(&positionState);
-
-    float cur[3] = { positionState.North,
-                     positionState.East,
-                     positionState.Down };
-    struct path_status progress;
-
-    path_progress(&pathDesired, cur, &progress);
-
-    // atan2f always returns in between + and - 180 degrees
-    return RAD2DEG(atan2f(progress.path_vector[1], progress.path_vector[0]));
-}
-
-/**
- * Compute bearing between current position and POI
- */
-static float updatePOIBearing()
-{
-    PoiLocationData poi;
-
-    PoiLocationGet(&poi);
-    PositionStateData positionState;
-    PositionStateGet(&positionState);
-
-    const float dT = updatePeriod / 1000.0f;
-    float dLoc[3];
-    float yaw = 0;
-    /*float elevation = 0;*/
-
-    dLoc[0] = positionState.North - poi.North;
-    dLoc[1] = positionState.East - poi.East;
-    dLoc[2] = positionState.Down - poi.Down;
-
-    if (dLoc[1] < 0) {
-        yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) + 180.0f;
-    } else {
-        yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) - 180.0f;
-    }
-    ManualControlCommandData manualControlData;
-    ManualControlCommandGet(&manualControlData);
-
-    float pathAngle = 0;
-    if (manualControlData.Roll > DEADBAND_HIGH) {
-        pathAngle = -(manualControlData.Roll - DEADBAND_HIGH) * dT * 300.0f;
-    } else if (manualControlData.Roll < DEADBAND_LOW) {
-        pathAngle = -(manualControlData.Roll - DEADBAND_LOW) * dT * 300.0f;
-    }
-
-    return yaw + (pathAngle / 2.0f);
-}
-
-/**
- * process POI control logic TODO: this should most likely go into manualcontrol!!!!
- * TODO: the whole process of POI handling likely needs cleanup and rethinking, might be broken since manualcontrol was refactored currently
- **/
-static void processPOI()
-{
-    const float dT = updatePeriod / 1000.0f;
-
-
-    PositionStateData positionState;
-
-    PositionStateGet(&positionState);
-    // TODO put commented out camera feature code back in place either
-    // permanently or optionally or remove it
-    // CameraDesiredData cameraDesired;
-    // CameraDesiredGet(&cameraDesired);
-    StabilizationDesiredData stabDesired;
-    StabilizationDesiredGet(&stabDesired);
-    PoiLocationData poi;
-    PoiLocationGet(&poi);
-
-    float dLoc[3];
-    float yaw = 0;
-    // TODO camera feature
-    /*float elevation = 0;*/
-
-    dLoc[0] = positionState.North - poi.North;
-    dLoc[1] = positionState.East - poi.East;
-    dLoc[2] = positionState.Down - poi.Down;
-
-    if (dLoc[1] < 0) {
-        yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) + 180.0f;
-    } else {
-        yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) - 180.0f;
-    }
-
-    // distance
-    float distance = sqrtf(powf(dLoc[0], 2.0f) + powf(dLoc[1], 2.0f));
-
-    ManualControlCommandData manualControlData;
-    ManualControlCommandGet(&manualControlData);
-
-    float changeRadius = 0;
-    // Move closer or further, radially
-    if (manualControlData.Pitch > DEADBAND_HIGH) {
-        changeRadius = (manualControlData.Pitch - DEADBAND_HIGH) * dT * 100.0f;
-    } else if (manualControlData.Pitch < DEADBAND_LOW) {
-        changeRadius = (manualControlData.Pitch - DEADBAND_LOW) * dT * 100.0f;
-    }
-
-    // move along circular path
-    float pathAngle = 0;
-    if (manualControlData.Roll > DEADBAND_HIGH) {
-        pathAngle = -(manualControlData.Roll - DEADBAND_HIGH) * dT * 300.0f;
-    } else if (manualControlData.Roll < DEADBAND_LOW) {
-        pathAngle = -(manualControlData.Roll - DEADBAND_LOW) * dT * 300.0f;
-    } else if (manualControlData.Roll >= DEADBAND_LOW && manualControlData.Roll <= DEADBAND_HIGH) {
-        // change radius only when not circling
-        global.poiRadius = distance + changeRadius;
-    }
-
-    // don't try to move any closer
-    if (global.poiRadius >= 3.0f || changeRadius > 0) {
-        if (fabsf(pathAngle) > 0.0f || fabsf(changeRadius) > 0.0f) {
-            pathDesired.End.North = poi.North + (global.poiRadius * cosf(DEG2RAD(pathAngle + yaw - 180.0f)));
-            pathDesired.End.East  = poi.East + (global.poiRadius * sinf(DEG2RAD(pathAngle + yaw - 180.0f)));
-            pathDesired.StartingVelocity = 1.0f;
-            pathDesired.EndingVelocity   = 0.0f;
-            pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
-            PathDesiredSet(&pathDesired);
-        }
-    }
-    // not above
-    if (distance >= 3.0f) {
-        // TODO camera feature
-        // You can feed this into camerastabilization
-        /*elevation = RAD2DEG(atan2f(dLoc[2],distance));*/
-
-        // cameraDesired.Yaw=yaw;
-        // cameraDesired.PitchOrServo2=elevation;
-
-        // CameraDesiredSet(&cameraDesired);
-    }
-}
-
-static void updateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity)
-{
-    if (startingVelocity >= 0.0f) {
-        *updatedVelocity = startingVelocity - dT * brakeRate;
-        if (*updatedVelocity > currentVelocity) {
-            *updatedVelocity = currentVelocity;
-        }
-        if (*updatedVelocity < 0.0f) {
-            *updatedVelocity = 0.0f;
-        }
-    } else {
-        *updatedVelocity = startingVelocity + dT * brakeRate;
-        if (*updatedVelocity < currentVelocity) {
-            *updatedVelocity = currentVelocity;
-        }
-        if (*updatedVelocity > 0.0f) {
-            *updatedVelocity = 0.0f;
-        }
-    }
-}
-
-
-/**
- * Compute desired velocity from the current position and path
- */
-static void updatePathVelocity(float kFF, bool limited)
-{
-    PositionStateData positionState;
-
-    PositionStateGet(&positionState);
-    VelocityStateData velocityState;
-    VelocityStateGet(&velocityState);
-    VelocityDesiredData velocityDesired;
-
-    const float dT = updatePeriod / 1000.0f;
-
-    if (pathDesired.Mode == PATHDESIRED_MODE_BRAKE) {
-        float brakeRate = vtolPathFollowerSettings.BrakeRate;
-        if (brakeRate < BRAKE_RATE_MINIMUM) {
-            brakeRate = BRAKE_RATE_MINIMUM; // set a minimum to avoid a divide by zero potential below
-        }
-        updateBrakeVelocity(pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_NORTH], pathStatus.path_time, brakeRate, velocityState.North, &velocityDesired.North);
-        updateBrakeVelocity(pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_EAST], pathStatus.path_time, brakeRate, velocityState.East, &velocityDesired.East);
-        updateBrakeVelocity(pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_DOWN], pathStatus.path_time, brakeRate, velocityState.Down, &velocityDesired.Down);
-
-        float cur_velocity     = velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down;
-        cur_velocity     = sqrtf(cur_velocity);
-        float desired_velocity = velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East + velocityDesired.Down * velocityDesired.Down;
-        desired_velocity = sqrtf(desired_velocity);
-
-        // update pathstatus
-        pathStatus.error = cur_velocity - desired_velocity;
-        pathStatus.fractional_progress = 1.0f;
-        if (pathDesired.StartingVelocity > 0.0f) {
-            pathStatus.fractional_progress = (pathDesired.StartingVelocity - cur_velocity) / pathDesired.StartingVelocity;
-
-            // sometimes current velocity can exceed starting velocity due to initial acceleration
-            if (pathStatus.fractional_progress < 0.0f) {
-                pathStatus.fractional_progress = 0.0f;
-            }
-        }
-        pathStatus.path_direction_north = velocityDesired.North;
-        pathStatus.path_direction_east  = velocityDesired.East;
-        pathStatus.path_direction_down  = velocityDesired.Down;
-
-        pathStatus.correction_direction_north = velocityDesired.North - velocityState.North;
-        pathStatus.correction_direction_east  = velocityDesired.East - velocityState.East;
-        pathStatus.correction_direction_down  = velocityDesired.Down - velocityState.Down;
-    } else {
-        // look ahead kFF seconds
-        float cur[3] = { positionState.North + (velocityState.North * kFF),
-                         positionState.East + (velocityState.East * kFF),
-                         positionState.Down + (velocityState.Down * kFF) };
-        struct path_status progress;
-        path_progress(&pathDesired, cur, &progress);
-
-        // calculate velocity - can be zero if waypoints are too close
-        velocityDesired.North = progress.path_vector[0];
-        velocityDesired.East  = progress.path_vector[1];
-        velocityDesired.Down  = progress.path_vector[2];
-
-        if (limited &&
-            // if a plane is crossing its desired flightpath facing the wrong way (away from flight direction)
-            // it would turn towards the flightpath to get on its desired course. This however would reverse the correction vector
-            // once it crosses the flightpath again, which would make it again turn towards the flightpath (but away from its desired heading)
-            // leading to an S-shape snake course the wrong way
-            // this only happens especially if HorizontalPosP is too high, as otherwise the angle between velocity desired and path_direction won't
-            // turn steep unless there is enough space complete the turn before crossing the flightpath
-            // in this case the plane effectively needs to be turned around
-            // indicators:
-            // difference between correction_direction and velocitystate >90 degrees and
-            // difference between path_direction and velocitystate >90 degrees  ( 4th sector, facing away from everything )
-            // fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore)
-            // calculating angles < 90 degrees through dot products
-            (vector_lengthf(progress.path_vector, 2) > 1e-6f) &&
-            ((progress.path_vector[0] * velocityState.North + progress.path_vector[1] * velocityState.East) < 0.0f) &&
-            ((progress.correction_vector[0] * velocityState.North + progress.correction_vector[1] * velocityState.East) < 0.0f)) {
-            ;
-        } else {
-            // calculate correction
-            velocityDesired.North += pid_apply(&global.PIDposH[0], progress.correction_vector[0], dT);
-            velocityDesired.East  += pid_apply(&global.PIDposH[1], progress.correction_vector[1], dT);
-        }
-        velocityDesired.Down += pid_apply(&global.PIDposV, progress.correction_vector[2], dT);
-
-        // update pathstatus
-        pathStatus.error      = progress.error;
-        pathStatus.fractional_progress  = progress.fractional_progress;
-        pathStatus.path_direction_north = progress.path_vector[0];
-        pathStatus.path_direction_east  = progress.path_vector[1];
-        pathStatus.path_direction_down  = progress.path_vector[2];
-
-        pathStatus.correction_direction_north = progress.correction_vector[0];
-        pathStatus.correction_direction_east  = progress.correction_vector[1];
-        pathStatus.correction_direction_down  = progress.correction_vector[2];
-    }
-
-
-    VelocityDesiredSet(&velocityDesired);
-}
-
-
-/**
- * Compute desired attitude from the desired velocity for fixed wing craft
- */
-static uint8_t updateFixedDesiredAttitude()
-{
-    uint8_t result = 1;
-
-    const float dT = updatePeriod / 1000.0f; // Convert from [ms] to [s]
-
-    VelocityDesiredData velocityDesired;
-    VelocityStateData velocityState;
-    StabilizationDesiredData stabDesired;
-    AttitudeStateData attitudeState;
-    FixedWingPathFollowerStatusData fixedWingPathFollowerStatus;
-    AirspeedStateData airspeedState;
-    SystemSettingsData systemSettings;
-
-    float groundspeedProjection;
-    float indicatedAirspeedState;
-    float indicatedAirspeedDesired;
-    float airspeedError;
-
-    float pitchCommand;
-
-    float descentspeedDesired;
-    float descentspeedError;
-    float powerCommand;
-
-    float airspeedVector[2];
-    float fluidMovement[2];
-    float courseComponent[2];
-    float courseError;
-    float courseCommand;
-
-    FixedWingPathFollowerStatusGet(&fixedWingPathFollowerStatus);
-
-    VelocityStateGet(&velocityState);
-    StabilizationDesiredGet(&stabDesired);
-    VelocityDesiredGet(&velocityDesired);
-    AttitudeStateGet(&attitudeState);
-    AirspeedStateGet(&airspeedState);
-    SystemSettingsGet(&systemSettings);
-
-    /**
-     * Compute speed error and course
-     */
-    // missing sensors for airspeed-direction we have to assume within
-    // reasonable error that measured airspeed is actually the airspeed
-    // component in forward pointing direction
-    // airspeedVector is normalized
-    airspeedVector[0]     = cos_lookup_deg(attitudeState.Yaw);
-    airspeedVector[1]     = sin_lookup_deg(attitudeState.Yaw);
-
-    // current ground speed projected in forward direction
-    groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1];
-
-    // note that airspeedStateBias is ( calibratedAirspeed - groundspeedProjection ) at the time of measurement,
-    // but thanks to accelerometers,  groundspeedProjection reacts faster to changes in direction
-    // than airspeed and gps sensors alone
-    indicatedAirspeedState = groundspeedProjection + indicatedAirspeedStateBias;
-
-    // fluidMovement is a vector describing the aproximate movement vector of
-    // the surrounding fluid in 2d space (aka wind vector)
-    fluidMovement[0] = velocityState.North - (indicatedAirspeedState * airspeedVector[0]);
-    fluidMovement[1] = velocityState.East - (indicatedAirspeedState * airspeedVector[1]);
-
-    // calculate the movement vector we need to fly to reach velocityDesired -
-    // taking fluidMovement into account
-    courseComponent[0] = velocityDesired.North - fluidMovement[0];
-    courseComponent[1] = velocityDesired.East - fluidMovement[1];
-
-    indicatedAirspeedDesired = boundf(sqrtf(courseComponent[0] * courseComponent[0] + courseComponent[1] * courseComponent[1]),
-                                      fixedWingPathFollowerSettings.HorizontalVelMin,
-                                      fixedWingPathFollowerSettings.HorizontalVelMax);
-
-    // if we could fly at arbitrary speeds, we'd just have to move towards the
-    // courseComponent vector as previously calculated and we'd be fine
-    // unfortunately however we are bound by min and max air speed limits, so
-    // we need to recalculate the correct course to meet at least the
-    // velocityDesired vector direction at our current speed
-    // this overwrites courseComponent
-    bool valid = correctCourse(courseComponent, (float *)&velocityDesired.North, fluidMovement, indicatedAirspeedDesired);
-
-    // Error condition: wind speed too high, we can't go where we want anymore
-    fixedWingPathFollowerStatus.Errors.Wind = 0;
-    if ((!valid) &&
-        fixedWingPathFollowerSettings.Safetymargins.Wind > 0.5f) { // alarm switched on
-        fixedWingPathFollowerStatus.Errors.Wind = 1;
-        result = 0;
-    }
-
-    // Airspeed error
-    airspeedError = indicatedAirspeedDesired - indicatedAirspeedState;
-
-    // Vertical speed error
-    descentspeedDesired = boundf(
-        velocityDesired.Down,
-        -fixedWingPathFollowerSettings.VerticalVelMax,
-        fixedWingPathFollowerSettings.VerticalVelMax);
-    descentspeedError = descentspeedDesired - velocityState.Down;
-
-    // Error condition: plane too slow or too fast
-    fixedWingPathFollowerStatus.Errors.Highspeed = 0;
-    fixedWingPathFollowerStatus.Errors.Lowspeed  = 0;
-    if (indicatedAirspeedState > systemSettings.AirSpeedMax * fixedWingPathFollowerSettings.Safetymargins.Overspeed) {
-        fixedWingPathFollowerStatus.Errors.Overspeed = 1;
-        result = 0;
-    }
-    if (indicatedAirspeedState > fixedWingPathFollowerSettings.HorizontalVelMax * fixedWingPathFollowerSettings.Safetymargins.Highspeed) {
-        fixedWingPathFollowerStatus.Errors.Highspeed = 1;
-        result = 0;
-    }
-    if (indicatedAirspeedState < fixedWingPathFollowerSettings.HorizontalVelMin * fixedWingPathFollowerSettings.Safetymargins.Lowspeed) {
-        fixedWingPathFollowerStatus.Errors.Lowspeed = 1;
-        result = 0;
-    }
-    if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedWingPathFollowerSettings.Safetymargins.Stallspeed) {
-        fixedWingPathFollowerStatus.Errors.Stallspeed = 1;
-        result = 0;
-    }
-
-    /**
-     * Compute desired thrust command
-     */
-
-    // Compute the cross feed from vertical speed to pitch, with saturation
-    float speedErrorToPowerCommandComponent = boundf(
-        (airspeedError / fixedWingPathFollowerSettings.HorizontalVelMin) * fixedWingPathFollowerSettings.AirspeedToPowerCrossFeed.Kp,
-        -fixedWingPathFollowerSettings.AirspeedToPowerCrossFeed.Max,
-        fixedWingPathFollowerSettings.AirspeedToPowerCrossFeed.Max
-        );
-
-    // Compute final thrust response
-    powerCommand = pid_apply(&global.PIDpower, -descentspeedError, dT) +
-                   speedErrorToPowerCommandComponent;
-
-    // Output internal state to telemetry
-    fixedWingPathFollowerStatus.Error.Power    = descentspeedError;
-    fixedWingPathFollowerStatus.ErrorInt.Power = global.PIDpower.iAccumulator;
-    fixedWingPathFollowerStatus.Command.Power  = powerCommand;
-
-    // set thrust
-    stabDesired.Thrust = boundf(fixedWingPathFollowerSettings.ThrustLimit.Neutral + powerCommand,
-                                fixedWingPathFollowerSettings.ThrustLimit.Min,
-                                fixedWingPathFollowerSettings.ThrustLimit.Max);
-
-    // Error condition: plane cannot hold altitude at current speed.
-    fixedWingPathFollowerStatus.Errors.Lowpower = 0;
-    if (fixedWingPathFollowerSettings.ThrustLimit.Neutral + powerCommand >= fixedWingPathFollowerSettings.ThrustLimit.Max && // thrust at maximum
-        velocityState.Down > 0.0f && // we ARE going down
-        descentspeedDesired < 0.0f && // we WANT to go up
-        airspeedError > 0.0f && // we are too slow already
-        fixedWingPathFollowerSettings.Safetymargins.Lowpower > 0.5f) { // alarm switched on
-        fixedWingPathFollowerStatus.Errors.Lowpower = 1;
-        result = 0;
-    }
-    // Error condition: plane keeps climbing despite minimum thrust (opposite of above)
-    fixedWingPathFollowerStatus.Errors.Highpower = 0;
-    if (fixedWingPathFollowerSettings.ThrustLimit.Neutral + powerCommand <= fixedWingPathFollowerSettings.ThrustLimit.Min && // thrust at minimum
-        velocityState.Down < 0.0f && // we ARE going up
-        descentspeedDesired > 0.0f && // we WANT to go down
-        airspeedError < 0.0f && // we are too fast already
-        fixedWingPathFollowerSettings.Safetymargins.Highpower > 0.5f) { // alarm switched on
-        fixedWingPathFollowerStatus.Errors.Highpower = 1;
-        result = 0;
-    }
-
-    /**
-     * Compute desired pitch command
-     */
-    // Compute the cross feed from vertical speed to pitch, with saturation
-    float verticalSpeedToPitchCommandComponent = boundf(-descentspeedError * fixedWingPathFollowerSettings.VerticalToPitchCrossFeed.Kp,
-                                                        -fixedWingPathFollowerSettings.VerticalToPitchCrossFeed.Max,
-                                                        fixedWingPathFollowerSettings.VerticalToPitchCrossFeed.Max
-                                                        );
-
-    // Compute the pitch command as err*Kp + errInt*Ki + X_feed.
-    pitchCommand = -pid_apply(&global.PIDspeed, airspeedError, dT) + verticalSpeedToPitchCommandComponent;
-
-    fixedWingPathFollowerStatus.Error.Speed    = airspeedError;
-    fixedWingPathFollowerStatus.ErrorInt.Speed = global.PIDspeed.iAccumulator;
-    fixedWingPathFollowerStatus.Command.Speed  = pitchCommand;
-
-    stabDesired.Pitch = boundf(fixedWingPathFollowerSettings.PitchLimit.Neutral + pitchCommand,
-                               fixedWingPathFollowerSettings.PitchLimit.Min,
-                               fixedWingPathFollowerSettings.PitchLimit.Max);
-
-    // Error condition: high speed dive
-    fixedWingPathFollowerStatus.Errors.Pitchcontrol = 0;
-    if (fixedWingPathFollowerSettings.PitchLimit.Neutral + pitchCommand >= fixedWingPathFollowerSettings.PitchLimit.Max && // pitch demand is full up
-        velocityState.Down > 0.0f && // we ARE going down
-        descentspeedDesired < 0.0f && // we WANT to go up
-        airspeedError < 0.0f && // we are too fast already
-        fixedWingPathFollowerSettings.Safetymargins.Pitchcontrol > 0.5f) { // alarm switched on
-        fixedWingPathFollowerStatus.Errors.Pitchcontrol = 1;
-        result = 0;
-    }
-
-    /**
-     * Compute desired roll command
-     */
-    courseError = RAD2DEG(atan2f(courseComponent[1], courseComponent[0])) - attitudeState.Yaw;
-
-    if (courseError < -180.0f) {
-        courseError += 360.0f;
-    }
-    if (courseError > 180.0f) {
-        courseError -= 360.0f;
-    }
-
-    // overlap calculation. Theres a dead zone behind the craft where the
-    // counter-yawing of some craft while rolling could render a desired right
-    // turn into a desired left turn. Making the turn direction based on
-    // current roll angle keeps the plane committed to a direction once chosen
-    if (courseError < -180.0f + (fixedWingPathFollowerSettings.ReverseCourseOverlap * 0.5f)
-        && attitudeState.Roll > 0.0f) {
-        courseError += 360.0f;
-    }
-    if (courseError > 180.0f - (fixedWingPathFollowerSettings.ReverseCourseOverlap * 0.5f)
-        && attitudeState.Roll < 0.0f) {
-        courseError -= 360.0f;
-    }
-
-    courseCommand = pid_apply(&global.PIDcourse, courseError, dT);
-
-    fixedWingPathFollowerStatus.Error.Course    = courseError;
-    fixedWingPathFollowerStatus.ErrorInt.Course = global.PIDcourse.iAccumulator;
-    fixedWingPathFollowerStatus.Command.Course  = courseCommand;
-
-    stabDesired.Roll = boundf(fixedWingPathFollowerSettings.RollLimit.Neutral +
-                              courseCommand,
-                              fixedWingPathFollowerSettings.RollLimit.Min,
-                              fixedWingPathFollowerSettings.RollLimit.Max);
-
-    // TODO: find a check to determine loss of directional control. Likely needs some check of derivative
-
-
-    /**
-     * Compute desired yaw command
-     */
-    // TODO implement raw control mode for yaw and base on Accels.Y
-    stabDesired.Yaw = 0.0f;
-
-
-    stabDesired.StabilizationMode.Roll   = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
-    stabDesired.StabilizationMode.Pitch  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
-    stabDesired.StabilizationMode.Yaw    = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
-    stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
-
-    StabilizationDesiredSet(&stabDesired);
-
-    FixedWingPathFollowerStatusSet(&fixedWingPathFollowerStatus);
-
-    return result;
-}
-
-
-/**
- * Function to calculate course vector C based on airspeed s, fluid movement F
- * and desired movement vector V
- * parameters in: V,F,s
- * parameters out: C
- * returns true if a valid solution could be found for V,F,s, false if not
- * C will be set to a best effort attempt either way
- */
-static bool correctCourse(float *C, float *V, float *F, float s)
-{
-    // Approach:
-    // Let Sc be a circle around origin marking possible movement vectors
-    // of the craft with airspeed s (all possible options for C)
-    // Let Vl be a line through the origin along movement vector V where fr any
-    // point v on line Vl v = k * (V / |V|) = k' * V
-    // Let Wl be a line parallel to Vl where for any point v on line Vl exists
-    // a point w on WL with w = v - F
-    // Then any intersection between circle Sc and line Wl represents course
-    // vector which would result in a movement vector
-    // V' = k * ( V / |V|) = k' * V
-    // If there is no intersection point, S is insufficient to compensate
-    // for F and we can only try to fly in direction of V (thus having wind drift
-    // but at least making progress orthogonal to wind)
-
-    s = fabsf(s);
-    float f = vector_lengthf(F, 2);
-
-    // normalize Cn=V/|V|, |V| must be >0
-    float v = vector_lengthf(V, 2);
-    if (v < 1e-6f) {
-        // if |V|=0, we aren't supposed to move, turn into the wind
-        // (this allows hovering)
-        C[0] = -F[0];
-        C[1] = -F[1];
-        // if desired airspeed matches fluidmovement a hover is actually
-        // intended so return true
-        return fabsf(f - s) < 1e-3f;
-    }
-    float Vn[2] = { V[0] / v, V[1] / v };
-
-    // project F on V
-    float fp    = F[0] * Vn[0] + F[1] * Vn[1];
-
-    // find component Fo of F that is orthogonal to V
-    // (which is exactly the distance between Vl and Wl)
-    float Fo[2] = { F[0] - (fp * Vn[0]), F[1] - (fp * Vn[1]) };
-    float fo2   = Fo[0] * Fo[0] + Fo[1] * Fo[1];
-
-    // find k where k * Vn = C - Fo
-    // |C|=s is the hypothenuse in any rectangular triangle formed by k * Vn and Fo
-    // so k^2 + fo^2 = s^2 (since |Vn|=1)
-    float k2 = s * s - fo2;
-    if (k2 <= -1e-3f) {
-        // there is no solution, we will be drifted off either way
-        // fallback: fly stupidly in direction of V and hope for the best
-        C[0] = V[0];
-        C[1] = V[1];
-        return false;
-    } else if (k2 <= 1e-3f) {
-        // there is exactly one solution: -Fo
-        C[0] = -Fo[0];
-        C[1] = -Fo[1];
-        return true;
-    }
-    // we have two possible solutions k positive and k negative as there are
-    // two intersection points between Wl and Sc
-    // which one is better? two criteria:
-    // 1. we MUST move in the right direction, if any k leads to -v its invalid
-    // 2. we should minimize the speed error
-    float k     = sqrt(k2);
-    float C1[2] = { -k * Vn[0] - Fo[0], -k * Vn[1] - Fo[1] };
-    float C2[2] = { k *Vn[0] - Fo[0], k * Vn[1] - Fo[1] };
-    // project C+F on Vn to find signed resulting movement vector length
-    float vp1   = (C1[0] + F[0]) * Vn[0] + (C1[1] + F[1]) * Vn[1];
-    float vp2   = (C2[0] + F[0]) * Vn[0] + (C2[1] + F[1]) * Vn[1];
-    if (vp1 >= 0.0f && fabsf(v - vp1) < fabsf(v - vp2)) {
-        // in this case the angle between course and resulting movement vector
-        // is greater than 90 degrees - so we actually fly backwards
-        C[0] = C1[0];
-        C[1] = C1[1];
-        return true;
-    }
-    C[0] = C2[0];
-    C[1] = C2[1];
-    if (vp2 >= 0.0f) {
-        // in this case the angle between course and movement vector is less than
-        // 90 degrees, but we do move in the right direction
-        return true;
-    } else {
-        // in this case we actually get driven in the opposite direction of V
-        // with both solutions for C
-        // this might be reached in headwind stronger than maximum allowed
-        // airspeed.
-        return false;
-    }
-}
-
-/**
- * Compute desired attitude from the desired velocity
- *
- * Takes in @ref NedState which has the acceleration in the
- * NED frame as the feedback term and then compares the
- * @ref VelocityState against the @ref VelocityDesired
- */
-static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction)
-{
-    const float dT     = updatePeriod / 1000.0f;
-    uint8_t result     = 1;
-    bool manual_thrust = false;
-
-    VelocityDesiredData velocityDesired;
-    VelocityStateData velocityState;
-    StabilizationDesiredData stabDesired;
-    AttitudeStateData attitudeState;
-    StabilizationBankData stabSettings;
-    SystemSettingsData systemSettings;
-    VtolSelfTuningStatsData vtolSelfTuningStats;
-
-    float northError;
-    float northCommand;
-
-    float eastError;
-    float eastCommand;
-
-    float downError;
-    float downCommand;
-
-    SystemSettingsGet(&systemSettings);
-    VelocityStateGet(&velocityState);
-    VelocityDesiredGet(&velocityDesired);
-    StabilizationDesiredGet(&stabDesired);
-    VelocityDesiredGet(&velocityDesired);
-    AttitudeStateGet(&attitudeState);
-    StabilizationBankGet(&stabSettings);
-    VtolSelfTuningStatsGet(&vtolSelfTuningStats);
-
-
-    if (pathDesired.Mode != PATHDESIRED_MODE_BRAKE) {
-        // scale velocity if it is above configured maximum
-        // for braking, we can not help it if initial velocity was greater
-        float velH = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East);
-        if (velH > vtolPathFollowerSettings.HorizontalVelMax) {
-            velocityDesired.North *= vtolPathFollowerSettings.HorizontalVelMax / velH;
-            velocityDesired.East  *= vtolPathFollowerSettings.HorizontalVelMax / velH;
-        }
-        if (fabsf(velocityDesired.Down) > vtolPathFollowerSettings.VerticalVelMax) {
-            velocityDesired.Down *= vtolPathFollowerSettings.VerticalVelMax / fabsf(velocityDesired.Down);
-        }
-    }
-
-    // calculate the velocity errors between desired and actual
-    northError = velocityDesired.North - velocityState.North;
-    eastError  = velocityDesired.East - velocityState.East;
-    downError  = velocityDesired.Down - velocityState.Down;
-
-    // Must flip this sign
-    downError  = -downError;
-
-    // Compute desired commands
-    if (pathDesired.Mode != PATHDESIRED_MODE_BRAKE) {
-        northCommand = pid_apply(&global.PIDvel[0], northError, dT) + velocityDesired.North * vtolPathFollowerSettings.VelocityFeedforward;
-        eastCommand  = pid_apply(&global.PIDvel[1], eastError, dT) + velocityDesired.East * vtolPathFollowerSettings.VelocityFeedforward;
-    } else {
-        northCommand = pid_apply(&global.BrakePIDvel[0], northError, dT) + velocityDesired.North * vtolPathFollowerSettings.BrakeVelocityFeedforward;
-        eastCommand  = pid_apply(&global.BrakePIDvel[1], eastError, dT) + velocityDesired.East * vtolPathFollowerSettings.BrakeVelocityFeedforward;
-    }
-    downCommand = pid_apply(&global.PIDvel[2], downError, dT);
-
-
-    if ((vtolPathFollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL &&
-         flightStatus.FlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) ||
-        (flightStatus.FlightModeAssist && flightStatus.AssistedThrottleState == FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL)) {
-        manual_thrust = true;
-    }
-
-    // if auto thrust and we have not run a correction calc check for PH and stability to then run an assessment
-    // note that arming into this flight mode is not allowed, so assumption here is that
-    // we have not landed.  If GPSAssist+Manual/Cruise control thrust mode is used, a user overriding the
-    // altitude maintaining PID will have moved the throttle state to FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL.
-    // In manualcontrol.c the state will stay in manual throttle until the throttle command exceeds the vtol thrust min,
-    // avoiding auto-takeoffs.  Therefore no need to check that here.
-    if (!manual_thrust && neutralThrustEst.have_correction != true) {
-        // Assess if position hold state running.  This can be normal position hold or
-        // another mode with assist-hold active.
-        bool ph_active =
-            ((flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD &&
-              flightStatus.FlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) ||
-             (flightStatus.FlightModeAssist != FLIGHTSTATUS_FLIGHTMODEASSIST_NONE &&
-              flightStatus.AssistedControlState == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD));
-
-
-        bool stable = (fabsf(pathStatus.correction_direction_down) < NEUTRALTHRUST_PH_POSITIONAL_ERROR_LIMIT &&
-                       fabsf(velocityDesired.Down) < NEUTRALTHRUST_PH_VEL_DESIRED_LIMIT &&
-                       fabsf(velocityState.Down) < NEUTRALTHRUST_PH_VEL_STATE_LIMIT &&
-                       fabsf(downError) < NEUTRALTHRUST_PH_VEL_ERROR_LIMIT);
-
-        if (ph_active && stable) {
-            if (neutralThrustEst.start_sampling) {
-                neutralThrustEst.count++;
-
-
-                // delay count for 2 seconds into hold allowing for stablisation
-                if (neutralThrustEst.count > NEUTRALTHRUST_START_DELAY) {
-                    neutralThrustEst.sum += global.PIDvel[2].iAccumulator;
-                    if (global.PIDvel[2].iAccumulator < neutralThrustEst.min) {
-                        neutralThrustEst.min = global.PIDvel[2].iAccumulator;
-                    }
-                    if (global.PIDvel[2].iAccumulator > neutralThrustEst.max) {
-                        neutralThrustEst.max = global.PIDvel[2].iAccumulator;
-                    }
-                }
-
-                if (neutralThrustEst.count >= NEUTRALTHRUST_END_COUNT) {
-                    // 6 seconds have past
-                    // lets take an average
-                    neutralThrustEst.average         = neutralThrustEst.sum / (float)(NEUTRALTHRUST_END_COUNT - NEUTRALTHRUST_START_DELAY);
-                    neutralThrustEst.correction      = neutralThrustEst.average / 1000.0f;
-
-                    global.PIDvel[2].iAccumulator   -= neutralThrustEst.average;
-
-                    neutralThrustEst.start_sampling  = false;
-                    neutralThrustEst.have_correction = true;
-
-                    // Write a new adjustment value
-                    // vtolSelfTuningStats.NeutralThrustOffset  was incremental adjusted above
-                    VtolSelfTuningStatsData new_vtolSelfTuningStats;
-                    // add the average remaining i value to the
-                    new_vtolSelfTuningStats.NeutralThrustOffset      = vtolSelfTuningStats.NeutralThrustOffset + neutralThrustEst.correction;
-                    new_vtolSelfTuningStats.NeutralThrustCorrection  = neutralThrustEst.correction; // the i term thrust correction value applied
-                    new_vtolSelfTuningStats.NeutralThrustAccumulator = global.PIDvel[2].iAccumulator; // the actual iaccumulator value after correction
-                    new_vtolSelfTuningStats.NeutralThrustRange = neutralThrustEst.max - neutralThrustEst.min;
-                    VtolSelfTuningStatsSet(&new_vtolSelfTuningStats);
-                }
-            } else {
-                // start a tick count
-                neutralThrustEst.start_sampling = true;
-                neutralThrustEst.count = 0;
-                neutralThrustEst.sum   = 0.0f;
-                neutralThrustEst.max   = 0.0f;
-                neutralThrustEst.min   = 0.0f;
-            }
-        } else {
-            // reset sampling as we did't get 6 continuous seconds
-            neutralThrustEst.start_sampling = false;
-        }
-    } // else we already have a correction for this PH run
-
-    // Generally in braking the downError will be an increased altitude.  We really will rely on cruisecontrol to backoff.
-    stabDesired.Thrust = boundf(vtolSelfTuningStats.NeutralThrustOffset + downCommand + vtolPathFollowerSettings.ThrustLimits.Neutral, vtolPathFollowerSettings.ThrustLimits.Min, vtolPathFollowerSettings.ThrustLimits.Max);
-
-
-    // DEBUG HACK: allow user to skew compass on purpose to see if emergency failsafe kicks in
-    if (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DEBUGTEST) {
-        attitudeState.Yaw += 120.0f;
-        if (attitudeState.Yaw > 180.0f) {
-            attitudeState.Yaw -= 360.0f;
-        }
-    }
-
-
-    if ( // emergency flyaway detection
-        ( // integral already at its limit
-            vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[0].iAccumulator) < 1e-6f ||
-            vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[1].iAccumulator) < 1e-6f
-        ) &&
-        // angle between desired and actual velocity >90 degrees (by dot product)
-        (velocityDesired.North * velocityState.North + velocityDesired.East * velocityState.East < 0.0f) &&
-        // quad is moving at significant speed (during flyaway it would keep speeding up)
-        squaref(velocityState.North) + squaref(velocityState.East) > 1.0f
-        ) {
-        global.vtolEmergencyFallback += dT;
-        if (global.vtolEmergencyFallback >= vtolPathFollowerSettings.FlyawayEmergencyFallbackTriggerTime) {
-            // after emergency timeout, trigger alarm - everything else is handled by callers
-            // (switch to emergency algorithm, switch to emergency waypoint in pathplanner, alarms, ...)
-            result = 0;
-        }
-    } else {
-        global.vtolEmergencyFallback = 0.0f;
-    }
-
-    // Project the north and east command signals into the pitch and roll based on yaw.  For this to behave well the
-    // craft should move similarly for 5 deg roll versus 5 deg pitch
-
-    float maxPitch = vtolPathFollowerSettings.MaxRollPitch;
-    if (pathDesired.Mode == PATHDESIRED_MODE_BRAKE) {
-        maxPitch = vtolPathFollowerSettings.BrakeMaxPitch;
-    }
-
-    stabDesired.Pitch = boundf(-northCommand * cosf(DEG2RAD(attitudeState.Yaw)) +
-                               -eastCommand * sinf(DEG2RAD(attitudeState.Yaw)),
-                               -maxPitch, maxPitch);
-    stabDesired.Roll  = boundf(-northCommand * sinf(DEG2RAD(attitudeState.Yaw)) +
-                               eastCommand * cosf(DEG2RAD(attitudeState.Yaw)),
-                               -maxPitch, maxPitch);
-
-    ManualControlCommandData manualControl;
-    ManualControlCommandGet(&manualControl);
-
-
-    stabDesired.StabilizationMode.Roll  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
-    stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
-    if (yaw_attitude) {
-        stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
-        stabDesired.Yaw = yaw_direction;
-    } else {
-        stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
-        stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
-    }
-
-    // default thrust mode to cruise control
-    stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
-
-    // when flight mode assist is active but in primary-thrust mode, the thrust mode must be set to the same as per the primary mode.
-    if (flightStatus.FlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST) {
-        FlightModeSettingsData settings;
-        FlightModeSettingsGet(&settings);
-        FlightModeSettingsStabilization1SettingsOptions thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL;
-
-        switch (flightStatus.FlightMode) {
-        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
-            thrustMode = settings.Stabilization1Settings.Thrust;
-            break;
-        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
-            thrustMode = settings.Stabilization2Settings.Thrust;
-            break;
-        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
-            thrustMode = settings.Stabilization3Settings.Thrust;
-            break;
-        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
-            thrustMode = settings.Stabilization4Settings.Thrust;
-            break;
-        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
-            thrustMode = settings.Stabilization5Settings.Thrust;
-            break;
-        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
-            thrustMode = settings.Stabilization6Settings.Thrust;
-            break;
-        case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
-            // we hard code the "GPS Assisted" PostionHold to use alt-vario which
-            // is a more appropriate throttle mode.  "GPSAssist" adds braking
-            // and a better throttle management to the standard Position Hold.
-            thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO;
-            break;
-        }
-        stabDesired.StabilizationMode.Thrust = thrustMode;
-        stabDesired.Thrust = manualControl.Thrust;
-    } else if (manual_thrust) {
-        stabDesired.Thrust = manualControl.Thrust;
-    }
-    // else thrust is set by the PID controller
-
-    StabilizationDesiredSet(&stabDesired);
-
-    return result;
-}
-
-/**
- * Compute desired attitude for vtols - emergency fallback
- */
-static void updateVtolDesiredAttitudeEmergencyFallback()
-{
-    const float dT = updatePeriod / 1000.0f;
-
-    VelocityDesiredData velocityDesired;
-    VelocityStateData velocityState;
-    StabilizationDesiredData stabDesired;
-
-    float courseError;
-    float courseCommand;
-
-    float downError;
-    float downCommand;
-
-    VelocityStateGet(&velocityState);
-    VelocityDesiredGet(&velocityDesired);
-
-    ManualControlCommandData manualControlData;
-    ManualControlCommandGet(&manualControlData);
-
-    courseError = RAD2DEG(atan2f(velocityDesired.East, velocityDesired.North) - atan2f(velocityState.East, velocityState.North));
-
-    if (courseError < -180.0f) {
-        courseError += 360.0f;
-    }
-    if (courseError > 180.0f) {
-        courseError -= 360.0f;
-    }
-
-
-    courseCommand   = (courseError * vtolPathFollowerSettings.EmergencyFallbackYawRate.kP);
-
-    stabDesired.Yaw = boundf(courseCommand, -vtolPathFollowerSettings.EmergencyFallbackYawRate.Max, vtolPathFollowerSettings.EmergencyFallbackYawRate.Max);
-
-    // Compute desired down command
-    downError   = velocityDesired.Down - velocityState.Down;
-    // Must flip this sign
-    downError   = -downError;
-    downCommand = pid_apply(&global.PIDvel[2], downError, dT);
-
-    stabDesired.Thrust = boundf(downCommand + vtolPathFollowerSettings.ThrustLimits.Neutral, vtolPathFollowerSettings.ThrustLimits.Min, vtolPathFollowerSettings.ThrustLimits.Max);
-
-
-    stabDesired.Roll   = vtolPathFollowerSettings.EmergencyFallbackAttitude.Roll;
-    stabDesired.Pitch  = vtolPathFollowerSettings.EmergencyFallbackAttitude.Pitch;
-
-    if (vtolPathFollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL) {
-        // For now override thrust with manual control.  Disable at your risk, quad goes to China.
-        ManualControlCommandData manualControl;
-        ManualControlCommandGet(&manualControl);
-        stabDesired.Thrust = manualControl.Thrust;
-    }
-
-    stabDesired.StabilizationMode.Roll   = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
-    stabDesired.StabilizationMode.Pitch  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
-    stabDesired.StabilizationMode.Yaw    = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
-    stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
-    StabilizationDesiredSet(&stabDesired);
-}
-
-
-/**
- * Compute desired attitude from a fixed preset
- *
- */
-static void updateFixedAttitude(float *attitude)
-{
-    StabilizationDesiredData stabDesired;
-
-    StabilizationDesiredGet(&stabDesired);
-    stabDesired.Roll   = attitude[0];
-    stabDesired.Pitch  = attitude[1];
-    stabDesired.Yaw    = attitude[2];
-    stabDesired.Thrust = attitude[3];
-    stabDesired.StabilizationMode.Roll   = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
-    stabDesired.StabilizationMode.Pitch  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
-    stabDesired.StabilizationMode.Yaw    = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
-    stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
-    StabilizationDesiredSet(&stabDesired);
-}

flight/modules/PathFollower/pathfollower.cpp

@@ -0,0 +1,470 @@
+/**
+ ******************************************************************************
+ *
+ * @file       pathfollower.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      This module compared @ref PositionActuatl to @ref ActiveWaypoint
+ * and sets @ref AttitudeDesired.  It only does this when the FlightMode field
+ * of @ref ManualControlCommand is Auto.
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+/**
+ * Input object: PathDesired
+ * Input object: PositionState
+ * Input object: ManualControlCommand
+ * Output object: StabilizationDesired
+ *
+ * This module acts as "autopilot" - it controls the setpoints of stabilization
+ * based on current flight situation and desired flight path (PathDesired) as
+ * directed by flightmode selection or pathplanner
+ * This is a periodic delayed callback module
+ *
+ * Modules have no API, all communication to other modules is done through UAVObjects.
+ * However modules may use the API exposed by shared libraries.
+ * See the OpenPilot wiki for more details.
+ * http://www.openpilot.org/OpenPilot_Application_Architecture
+ *
+ */
+
+extern "C" {
+#include <openpilot.h>
+
+#include <callbackinfo.h>
+
+#include <math.h>
+#include <pid.h>
+#include <CoordinateConversions.h>
+#include <sin_lookup.h>
+#include <pathdesired.h>
+#include <paths.h>
+#include "plans.h"
+#include <sanitycheck.h>
+
+#include <groundpathfollowersettings.h>
+#include <fixedwingpathfollowersettings.h>
+#include <fixedwingpathfollowerstatus.h>
+#include <vtolpathfollowersettings.h>
+#include <flightstatus.h>
+#include <flightmodesettings.h>
+#include <pathstatus.h>
+#include <positionstate.h>
+#include <velocitystate.h>
+#include <velocitydesired.h>
+#include <stabilizationdesired.h>
+#include <airspeedstate.h>
+#include <attitudestate.h>
+#include <takeofflocation.h>
+#include <poilocation.h>
+#include <manualcontrolcommand.h>
+#include <systemsettings.h>
+#include <stabilizationbank.h>
+#include <vtolselftuningstats.h>
+#include <pathsummary.h>
+#include <pidstatus.h>
+#include <homelocation.h>
+#include <accelstate.h>
+#include <statusvtolautotakeoff.h>
+#include <statusvtolland.h>
+#include <statusgrounddrive.h>
+}
+
+#include "pathfollowercontrol.h"
+#include "vtollandcontroller.h"
+#include "vtolautotakeoffcontroller.h"
+#include "vtolvelocitycontroller.h"
+#include "vtolbrakecontroller.h"
+#include "vtolflycontroller.h"
+#include "fixedwingflycontroller.h"
+#include "grounddrivecontroller.h"
+
+// Private constants
+
+#define CALLBACK_PRIORITY      CALLBACK_PRIORITY_LOW
+#define CBTASK_PRIORITY        CALLBACK_TASK_FLIGHTCONTROL
+
+#define PF_IDLE_UPDATE_RATE_MS 100
+
+#define STACK_SIZE_BYTES       2048
+
+
+// Private types
+
+// Private variables
+static DelayedCallbackInfo *pathFollowerCBInfo;
+static uint32_t updatePeriod = PF_IDLE_UPDATE_RATE_MS;
+static FrameType_t frameType = FRAME_TYPE_MULTIROTOR;
+static PathStatusData pathStatus;
+static PathDesiredData pathDesired;
+static FixedWingPathFollowerSettingsData fixedWingPathFollowerSettings;
+static GroundPathFollowerSettingsData groundPathFollowerSettings;
+static VtolPathFollowerSettingsData vtolPathFollowerSettings;
+static FlightStatusData flightStatus;
+static PathFollowerControl *activeController = 0;
+
+// Private functions
+static void pathFollowerTask(void);
+static void SettingsUpdatedCb(UAVObjEvent *ev);
+static void airspeedStateUpdatedCb(UAVObjEvent *ev);
+static void pathFollowerObjectiveUpdatedCb(UAVObjEvent *ev);
+static void flightStatusUpdatedCb(UAVObjEvent *ev);
+static void updateFixedAttitude(float *attitude);
+static void pathFollowerInitializeControllersForFrameType();
+
+/**
+ * Initialise the module, called on startup
+ * \returns 0 on success or -1 if initialisation failed
+ */
+extern "C" int32_t PathFollowerStart()
+{
+    // Start main task
+    PathStatusGet(&pathStatus);
+    SettingsUpdatedCb(NULL);
+    PIOS_CALLBACKSCHEDULER_Dispatch(pathFollowerCBInfo);
+
+    return 0;
+}
+
+
+/**
+ * Initialise the module, called on startup
+ * \returns 0 on success or -1 if initialisation failed
+ */
+extern "C" int32_t PathFollowerInitialize()
+{
+    // initialize objects
+    GroundPathFollowerSettingsInitialize();
+    FixedWingPathFollowerSettingsInitialize();
+    FixedWingPathFollowerStatusInitialize();
+    VtolPathFollowerSettingsInitialize();
+    FlightStatusInitialize();
+    FlightModeSettingsInitialize();
+    PathStatusInitialize();
+    PathSummaryInitialize();
+    PathDesiredInitialize();
+    PositionStateInitialize();
+    VelocityStateInitialize();
+    VelocityDesiredInitialize();
+    StabilizationDesiredInitialize();
+    AirspeedStateInitialize();
+    AttitudeStateInitialize();
+    TakeOffLocationInitialize();
+    PoiLocationInitialize();
+    ManualControlCommandInitialize();
+    SystemSettingsInitialize();
+    StabilizationBankInitialize();
+    VtolSelfTuningStatsInitialize();
+    PIDStatusInitialize();
+    StatusVtolLandInitialize();
+    StatusGroundDriveInitialize();
+    StatusVtolAutoTakeoffInitialize();
+
+    // VtolLandFSM additional objects
+    HomeLocationInitialize();
+    AccelStateInitialize();
+
+    // Init references to controllers
+    PathFollowerControl::Initialize(&pathDesired, &flightStatus, &pathStatus);
+
+    // Create object queue
+    pathFollowerCBInfo = PIOS_CALLBACKSCHEDULER_Create(&pathFollowerTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_PATHFOLLOWER, STACK_SIZE_BYTES);
+    FixedWingPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
+    GroundPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
+    VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
+    PathDesiredConnectCallback(&pathFollowerObjectiveUpdatedCb);
+    FlightStatusConnectCallback(&flightStatusUpdatedCb);
+    SystemSettingsConnectCallback(&SettingsUpdatedCb);
+    AirspeedStateConnectCallback(&airspeedStateUpdatedCb);
+    VtolSelfTuningStatsConnectCallback(&SettingsUpdatedCb);
+
+    return 0;
+}
+MODULE_INITCALL(PathFollowerInitialize, PathFollowerStart);
+
+void pathFollowerInitializeControllersForFrameType()
+{
+    static uint8_t multirotor_initialised = 0;
+    static uint8_t fixedwing_initialised  = 0;
+    static uint8_t ground_initialised     = 0;
+
+    switch (frameType) {
+    case FRAME_TYPE_MULTIROTOR:
+    case FRAME_TYPE_HELI:
+        if (!multirotor_initialised) {
+            VtolLandController::instance()->Initialize(&vtolPathFollowerSettings);
+            VtolAutoTakeoffController::instance()->Initialize(&vtolPathFollowerSettings);
+            VtolVelocityController::instance()->Initialize(&vtolPathFollowerSettings);
+            VtolFlyController::instance()->Initialize(&vtolPathFollowerSettings);
+            VtolBrakeController::instance()->Initialize(&vtolPathFollowerSettings);
+            multirotor_initialised = 1;
+        }
+        break;
+
+    case FRAME_TYPE_FIXED_WING:
+        if (!fixedwing_initialised) {
+            FixedWingFlyController::instance()->Initialize(&fixedWingPathFollowerSettings);
+            fixedwing_initialised = 1;
+        }
+        break;
+
+    case FRAME_TYPE_GROUND:
+        if (!ground_initialised) {
+            GroundDriveController::instance()->Initialize(&groundPathFollowerSettings);
+            ground_initialised = 1;
+        }
+        break;
+
+    default:
+        break;
+    }
+}
+
+static void pathFollowerSetActiveController(void)
+{
+    // Init controllers for the frame type
+    if (activeController == 0) {
+        // Initialise
+        pathFollowerInitializeControllersForFrameType();
+
+        switch (frameType) {
+        case FRAME_TYPE_MULTIROTOR:
+        case FRAME_TYPE_HELI:
+
+            switch (pathDesired.Mode) {
+            case PATHDESIRED_MODE_BRAKE: // brake then hold sequence controller
+                activeController = VtolBrakeController::instance();
+                activeController->Activate();
+                break;
+            case PATHDESIRED_MODE_VELOCITY: // velocity roam controller
+                activeController = VtolVelocityController::instance();
+                activeController->Activate();
+                break;
+            case PATHDESIRED_MODE_GOTOENDPOINT:
+            case PATHDESIRED_MODE_FOLLOWVECTOR:
+            case PATHDESIRED_MODE_CIRCLERIGHT:
+            case PATHDESIRED_MODE_CIRCLELEFT:
+                activeController = VtolFlyController::instance();
+                activeController->Activate();
+                break;
+            case PATHDESIRED_MODE_LAND: // land with optional velocity roam option
+                activeController = VtolLandController::instance();
+                activeController->Activate();
+                break;
+            case PATHDESIRED_MODE_AUTOTAKEOFF:
+                activeController = VtolAutoTakeoffController::instance();
+                activeController->Activate();
+                break;
+            default:
+                activeController = 0;
+                break;
+            }
+            break;
+
+        case FRAME_TYPE_FIXED_WING:
+
+            switch (pathDesired.Mode) {
+            case PATHDESIRED_MODE_GOTOENDPOINT:
+            case PATHDESIRED_MODE_FOLLOWVECTOR:
+            case PATHDESIRED_MODE_CIRCLERIGHT:
+            case PATHDESIRED_MODE_CIRCLELEFT:
+                activeController = FixedWingFlyController::instance();
+                activeController->Activate();
+                break;
+            default:
+                activeController = 0;
+                break;
+            }
+            break;
+
+        case FRAME_TYPE_GROUND:
+
+            switch (pathDesired.Mode) {
+            case PATHDESIRED_MODE_GOTOENDPOINT:
+            case PATHDESIRED_MODE_FOLLOWVECTOR:
+            case PATHDESIRED_MODE_CIRCLERIGHT:
+            case PATHDESIRED_MODE_CIRCLELEFT:
+                activeController = GroundDriveController::instance();
+                activeController->Activate();
+                break;
+            default:
+                activeController = 0;
+                break;
+            }
+            break;
+
+        default:
+            activeController = 0;
+            break;
+        }
+    }
+}
+
+/**
+ * Module thread, should not return.
+ */
+static void pathFollowerTask(void)
+{
+    FlightStatusGet(&flightStatus);
+
+    if (flightStatus.ControlChain.PathFollower != FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
+        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_UNINITIALISED);
+        PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, PF_IDLE_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
+        return;
+    }
+
+    pathStatus.UID    = pathDesired.UID;
+    pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
+
+    pathFollowerSetActiveController();
+
+    if (activeController) {
+        activeController->UpdateAutoPilot();
+        PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, updatePeriod, CALLBACK_UPDATEMODE_SOONER);
+        return;
+    }
+
+    switch (pathDesired.Mode) {
+    case PATHDESIRED_MODE_FIXEDATTITUDE:
+        updateFixedAttitude(pathDesired.ModeParameters);
+        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
+        break;
+    case PATHDESIRED_MODE_DISARMALARM:
+        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
+        break;
+    default:
+        pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
+        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
+        break;
+    }
+    PathStatusSet(&pathStatus);
+
+    PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, updatePeriod, CALLBACK_UPDATEMODE_SOONER);
+}
+
+
+static void pathFollowerObjectiveUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+{
+    PathDesiredGet(&pathDesired);
+
+    if (activeController && pathDesired.Mode != activeController->Mode()) {
+        activeController->Deactivate();
+        activeController = 0;
+    }
+
+    pathFollowerSetActiveController();
+
+    if (activeController) {
+        activeController->ObjectiveUpdated();
+    }
+}
+
+// we use this to deactivate active controllers as the pathdesired
+// objective never gets updated with switching to a non-pathfollower flight mode
+static void flightStatusUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+{
+    if (!activeController) {
+        return;
+    }
+
+    FlightStatusControlChainData controlChain;
+    FlightStatusControlChainGet(&controlChain);
+
+    if (controlChain.PathFollower != FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
+        activeController->Deactivate();
+        activeController = 0;
+    }
+}
+
+
+static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+{
+    VtolPathFollowerSettingsGet(&vtolPathFollowerSettings);
+
+    FrameType_t previous_frameType = frameType;
+    frameType = GetCurrentFrameType();
+
+    if (frameType == FRAME_TYPE_CUSTOM) {
+        switch (vtolPathFollowerSettings.TreatCustomCraftAs) {
+        case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING:
+            frameType = FRAME_TYPE_FIXED_WING;
+            break;
+        case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL:
+            frameType = FRAME_TYPE_MULTIROTOR;
+            break;
+        case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_GROUND:
+            frameType = FRAME_TYPE_GROUND;
+            break;
+        }
+    }
+
+    // If frame type changes, initialise the frame type controllers
+    if (frameType != previous_frameType) {
+        pathFollowerInitializeControllersForFrameType();
+    }
+
+    switch (frameType) {
+    case FRAME_TYPE_MULTIROTOR:
+    case FRAME_TYPE_HELI:
+        updatePeriod = vtolPathFollowerSettings.UpdatePeriod;
+        break;
+    case FRAME_TYPE_FIXED_WING:
+        FixedWingPathFollowerSettingsGet(&fixedWingPathFollowerSettings);
+        updatePeriod = fixedWingPathFollowerSettings.UpdatePeriod;
+        break;
+    case FRAME_TYPE_GROUND:
+        GroundPathFollowerSettingsGet(&groundPathFollowerSettings);
+        updatePeriod = groundPathFollowerSettings.UpdatePeriod;
+        break;
+    default:
+        updatePeriod = vtolPathFollowerSettings.UpdatePeriod;
+        break;
+    }
+
+    if (activeController) {
+        activeController->SettingsUpdated();
+    }
+}
+
+
+static void airspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+{
+    FixedWingFlyController::instance()->AirspeedStateUpdatedCb(ev);
+}
+
+
+/**
+ * Compute desired attitude from a fixed preset
+ *
+ */
+static void updateFixedAttitude(float *attitude)
+{
+    StabilizationDesiredData stabDesired;
+
+    StabilizationDesiredGet(&stabDesired);
+    stabDesired.Roll   = attitude[0];
+    stabDesired.Pitch  = attitude[1];
+    stabDesired.Yaw    = attitude[2];
+    stabDesired.Thrust = attitude[3];
+    stabDesired.StabilizationMode.Roll   = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.StabilizationMode.Pitch  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.StabilizationMode.Yaw    = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
+    stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
+    StabilizationDesiredSet(&stabDesired);
+}

flight/modules/PathFollower/pathfollowercontrol.cpp

@@ -1,14 +1,9 @@
-/**
+/*
  ******************************************************************************
- * @addtogroup PIOS PIOS Core hardware abstraction layer
- * @{
- * @addtogroup PIOS_USB_BOARD Board specific USB definitions
- * @brief Board specific USB definitions
- * @{
  *
- * @file       pios_usb_board_data.h
+ * @file       PathFollowerControl.c
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
- * @brief      Board specific USB definitions
+ * @brief      Controller interface class implementation
  * @see        The GNU Public License (GPL) Version 3
  *
  *****************************************************************************/
@@ -28,20 +23,30 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 
-#ifndef PIOS_USB_BOARD_DATA_H
+extern "C" {
-#define PIOS_USB_BOARD_DATA_H
+#include <openpilot.h>
+#include <flightstatus.h>
+#include <pathstatus.h>
+#include <pathdesired.h>
+}
 
-// Note : changing below length will require changes to the USB buffer setup
+// C++ includes
-#define PIOS_USB_BOARD_CDC_DATA_LENGTH 64
+#include "pathfollowercontrol.h"
-#define PIOS_USB_BOARD_CDC_MGMT_LENGTH 32
-#define PIOS_USB_BOARD_HID_DATA_LENGTH 64
 
-#define PIOS_USB_BOARD_EP_NUM          4
+PathDesiredData *PathFollowerControl::pathDesired   = 0;
+FlightStatusData *PathFollowerControl::flightStatus = 0;
+PathStatusData *PathFollowerControl::pathStatus     = 0;
 
-#include <pios_usb_defs.h> /* USB_* macros */
+int32_t PathFollowerControl::Initialize(PathDesiredData *ptr_pathDesired,
+                                        FlightStatusData *ptr_flightStatus,
+                                        PathStatusData *ptr_pathStatus)
+{
+    PIOS_Assert(ptr_pathDesired);
+    PIOS_Assert(ptr_flightStatus);
+    PIOS_Assert(ptr_pathStatus);
 
-#define PIOS_USB_BOARD_PRODUCT_ID      USB_PRODUCT_ID_COPTERCONTROL
+    pathDesired  = ptr_pathDesired;
-#define PIOS_USB_BOARD_DEVICE_VER      USB_OP_DEVICE_VER(USB_OP_BOARD_ID_COPTERCONTROL, USB_OP_BOARD_MODE_FW)
+    flightStatus = ptr_flightStatus;
-#define PIOS_USB_BOARD_SN_SUFFIX       "+FW"
+    pathStatus   = ptr_pathStatus;
-
+    return 0;
-#endif /* PIOS_USB_BOARD_DATA_H */
+}

flight/modules/PathFollower/pidcontrolne.cpp

@@ -0,0 +1,244 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup PathFollower CONTROL interface class
+ * @brief PID controller for NE
+ * @{
+ *
+ * @file       PIDControlNE.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Executes PID control loops for NE directions
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+extern "C" {
+#include <openpilot.h>
+
+#include <callbackinfo.h>
+
+#include <math.h>
+#include <pid.h>
+#include <CoordinateConversions.h>
+#include <sin_lookup.h>
+#include <pathdesired.h>
+#include <paths.h>
+#include "plans.h"
+#include <pidstatus.h>
+}
+#include "pathfollowerfsm.h"
+#include "pidcontrolne.h"
+
+PIDControlNE::PIDControlNE()
+    : deltaTime(0), mNECommand(0), mNeutral(0), mVelocityMax(0), mMinCommand(0), mMaxCommand(0), mVelocityFeedforward(0), mActive(false)
+{}
+
+PIDControlNE::~PIDControlNE() {}
+
+void PIDControlNE::Initialize()
+{}
+
+void PIDControlNE::Deactivate()
+{
+    mActive = false;
+}
+
+void PIDControlNE::Activate()
+{
+    mActive = true;
+}
+
+void PIDControlNE::UpdateParameters(float kp, float ki, float kd, float beta, float dT, float velocityMax)
+{
+    // pid_configure(&PID, kp, ki, kd, ilimit);
+    float Ti = kp / ki;
+    float Td = kd / kp;
+    float Tt = (Ti + Td) / 2.0f;
+    float kt = 1.0f / Tt;
+    float u0 = 0.0f;
+    float N  = 10.0f;
+    float Tf = Td / N;
+
+    if (ki < 1e-6f) {
+        // Avoid Ti being infinite
+        Ti = 1e6f;
+        // Tt antiwindup time constant - we don't need antiwindup with no I term
+        Tt = 1e6f;
+        kt = 0.0f;
+    }
+
+    if (kd < 1e-6f) {
+        // PI Controller
+        Tf = Ti / N;
+    }
+
+    if (beta > 1.0f) {
+        beta = 1.0f;
+    } else if (beta < 0.4f) {
+        beta = 0.4f;
+    }
+
+    pid2_configure(&PIDvel[0], kp, ki, kd, Tf, kt, dT, beta, u0, 0.0f, 1.0f);
+    pid2_configure(&PIDvel[1], kp, ki, kd, Tf, kt, dT, beta, u0, 0.0f, 1.0f);
+    deltaTime    = dT;
+    mVelocityMax = velocityMax;
+}
+
+void PIDControlNE::UpdatePositionalParameters(float kp)
+{
+    pid_configure(&PIDposH[0], kp, 0.0f, 0.0f, 0.0f);
+    pid_configure(&PIDposH[1], kp, 0.0f, 0.0f, 0.0f);
+}
+void PIDControlNE::UpdatePositionSetpoint(float setpointNorth, float setpointEast)
+{
+    mPositionSetpointTarget[0] = setpointNorth;
+    mPositionSetpointTarget[1] = setpointEast;
+}
+void PIDControlNE::UpdatePositionState(float pvNorth, float pvEast)
+{
+    mPositionState[0] = pvNorth;
+    mPositionState[1] = pvEast;
+}
+// This is a pure position hold position control
+void PIDControlNE::ControlPosition()
+{
+    // Current progress location relative to end
+    float velNorth = 0.0f;
+    float velEast  = 0.0f;
+
+    velNorth = pid_apply(&PIDposH[0], mPositionSetpointTarget[0] - mPositionState[0], deltaTime);
+    velEast  = pid_apply(&PIDposH[1], mPositionSetpointTarget[1] - mPositionState[1], deltaTime);
+    UpdateVelocitySetpoint(velNorth, velEast);
+}
+
+void PIDControlNE::ControlPositionWithPath(struct path_status *progress)
+{
+    // Current progress location relative to end
+    float velNorth = progress->path_vector[0];
+    float velEast  = progress->path_vector[1];
+
+    velNorth += pid_apply(&PIDposH[0], progress->correction_vector[0], deltaTime);
+    velEast  += pid_apply(&PIDposH[1], progress->correction_vector[1], deltaTime);
+    UpdateVelocitySetpoint(velNorth, velEast);
+}
+
+void PIDControlNE::UpdateVelocitySetpoint(float setpointNorth, float setpointEast)
+{
+    // scale velocity if it is above configured maximum
+    // for braking, we can not help it if initial velocity was greater
+    float velH = sqrtf(setpointNorth * setpointNorth + setpointEast * setpointEast);
+
+    if (velH > mVelocityMax) {
+        setpointNorth *= mVelocityMax / velH;
+        setpointEast  *= mVelocityMax / velH;
+    }
+
+    mVelocitySetpointTarget[0] = setpointNorth;
+    mVelocitySetpointTarget[1] = setpointEast;
+}
+
+
+void PIDControlNE::UpdateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity)
+{
+    if (startingVelocity >= 0.0f) {
+        *updatedVelocity = startingVelocity - dT * brakeRate;
+        if (*updatedVelocity > currentVelocity) {
+            *updatedVelocity = currentVelocity;
+        }
+        if (*updatedVelocity < 0.0f) {
+            *updatedVelocity = 0.0f;
+        }
+    } else {
+        *updatedVelocity = startingVelocity + dT * brakeRate;
+        if (*updatedVelocity < currentVelocity) {
+            *updatedVelocity = currentVelocity;
+        }
+        if (*updatedVelocity > 0.0f) {
+            *updatedVelocity = 0.0f;
+        }
+    }
+}
+
+void PIDControlNE::UpdateVelocityStateWithBrake(float pvNorth, float pvEast, float path_time, float brakeRate)
+{
+    mVelocityState[0] = pvNorth;
+    mVelocityState[1] = pvEast;
+
+    float velocitySetpointDesired[2];
+
+    UpdateBrakeVelocity(mVelocitySetpointTarget[0], path_time, brakeRate, pvNorth, &velocitySetpointDesired[0]);
+    UpdateBrakeVelocity(mVelocitySetpointTarget[1], path_time, brakeRate, pvEast, &velocitySetpointDesired[1]);
+
+    // If rate of change limits required, add here
+    for (int iaxis = 0; iaxis < 2; iaxis++) {
+        mVelocitySetpointCurrent[iaxis] = velocitySetpointDesired[iaxis];
+    }
+}
+
+void PIDControlNE::UpdateVelocityState(float pvNorth, float pvEast)
+{
+    mVelocityState[0] = pvNorth;
+    mVelocityState[1] = pvEast;
+
+    // The FSM controls the actual descent velocity and introduces step changes as required
+    float velocitySetpointDesired[2];
+    velocitySetpointDesired[0] = mVelocitySetpointTarget[0];
+    velocitySetpointDesired[1] = mVelocitySetpointTarget[1];
+
+    // If rate of change limits required, add here
+    for (int iaxis = 0; iaxis < 2; iaxis++) {
+        mVelocitySetpointCurrent[iaxis] = velocitySetpointDesired[iaxis];
+    }
+}
+
+
+void PIDControlNE::UpdateCommandParameters(float minCommand, float maxCommand, float velocityFeedforward)
+{
+    mMinCommand = minCommand;
+    mMaxCommand = maxCommand;
+    mVelocityFeedforward = velocityFeedforward;
+}
+
+
+void PIDControlNE::GetNECommand(float *northCommand, float *eastCommand)
+{
+    PIDvel[0].va  = mVelocitySetpointCurrent[0] * mVelocityFeedforward;
+    *northCommand = pid2_apply(&(PIDvel[0]), mVelocitySetpointCurrent[0], mVelocityState[0], mMinCommand, mMaxCommand);
+    PIDvel[1].va  = mVelocitySetpointCurrent[1] * mVelocityFeedforward;
+    *eastCommand  = pid2_apply(&(PIDvel[1]), mVelocitySetpointCurrent[1], mVelocityState[1], mMinCommand, mMaxCommand);
+
+    PIDStatusData pidStatus;
+    pidStatus.setpoint = mVelocitySetpointCurrent[0];
+    pidStatus.actual   = mVelocityState[0];
+    pidStatus.error    = mVelocitySetpointCurrent[0] - mVelocityState[0];
+    pidStatus.setpoint = mVelocitySetpointCurrent[0];
+    pidStatus.ulow     = mMinCommand;
+    pidStatus.uhigh    = mMaxCommand;
+    pidStatus.command  = *northCommand;
+    pidStatus.P = PIDvel[0].P;
+    pidStatus.I = PIDvel[0].I;
+    pidStatus.D = PIDvel[0].D;
+    PIDStatusSet(&pidStatus);
+}
+
+void PIDControlNE::GetVelocityDesired(float *north, float *east)
+{
+    *north = mVelocitySetpointCurrent[0];
+    *east  = mVelocitySetpointCurrent[1];
+}

flight/modules/PathFollower/vtolautotakeoffcontroller.cpp

@@ -0,0 +1,287 @@
+/*
+ ******************************************************************************
+ *
+ * @file       vtollandcontroller.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Vtol landing controller loop
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+extern "C" {
+#include <openpilot.h>
+
+#include <callbackinfo.h>
+
+#include <math.h>
+#include <pid.h>
+#include <CoordinateConversions.h>
+#include <sin_lookup.h>
+#include <pathdesired.h>
+#include <paths.h>
+#include "plans.h"
+#include <sanitycheck.h>
+
+#include <homelocation.h>
+#include <accelstate.h>
+#include <vtolpathfollowersettings.h>
+#include <flightstatus.h>
+#include <flightmodesettings.h>
+#include <pathstatus.h>
+#include <positionstate.h>
+#include <velocitystate.h>
+#include <velocitydesired.h>
+#include <stabilizationdesired.h>
+#include <airspeedstate.h>
+#include <attitudestate.h>
+#include <takeofflocation.h>
+#include <poilocation.h>
+#include <manualcontrolcommand.h>
+#include <systemsettings.h>
+#include <stabilizationbank.h>
+#include <stabilizationdesired.h>
+#include <vtolselftuningstats.h>
+#include <pathsummary.h>
+}
+
+// C++ includes
+#include "vtolautotakeoffcontroller.h"
+#include "pathfollowerfsm.h"
+#include "vtolautotakeofffsm.h"
+#include "pidcontroldown.h"
+
+// Private constants
+
+// pointer to a singleton instance
+VtolAutoTakeoffController *VtolAutoTakeoffController::p_inst = 0;
+
+VtolAutoTakeoffController::VtolAutoTakeoffController()
+    : fsm(0), vtolPathFollowerSettings(0), mActive(false)
+{}
+
+// Called when mode first engaged
+void VtolAutoTakeoffController::Activate(void)
+{
+    if (!mActive) {
+        mActive = true;
+        SettingsUpdated();
+        fsm->Activate();
+        controlDown.Activate();
+        controlNE.Activate();
+    }
+}
+
+uint8_t VtolAutoTakeoffController::IsActive(void)
+{
+    return mActive;
+}
+
+uint8_t VtolAutoTakeoffController::Mode(void)
+{
+    return PATHDESIRED_MODE_AUTOTAKEOFF;
+}
+
+// Objective updated in pathdesired, e.g. same flight mode but new target velocity
+void VtolAutoTakeoffController::ObjectiveUpdated(void)
+{
+    // Set the objective's target velocity
+
+    controlDown.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_DOWN]);
+    controlNE.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_NORTH],
+                                     pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_EAST]);
+    controlNE.UpdatePositionSetpoint(pathDesired->End.North, pathDesired->End.East);
+    controlDown.UpdatePositionSetpoint(pathDesired->End.Down);
+    fsm->setControlState((StatusVtolAutoTakeoffControlStateOptions)pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE]);
+}
+void VtolAutoTakeoffController::Deactivate(void)
+{
+    if (mActive) {
+        mActive = false;
+        fsm->Inactive();
+        controlDown.Deactivate();
+        controlNE.Deactivate();
+    }
+}
+
+// AutoTakeoff Uses different vertical velocity PID.
+void VtolAutoTakeoffController::SettingsUpdated(void)
+{
+    const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f;
+
+    controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp,
+                               vtolPathFollowerSettings->HorizontalVelPID.Ki,
+                               vtolPathFollowerSettings->HorizontalVelPID.Kd,
+                               vtolPathFollowerSettings->HorizontalVelPID.Beta,
+                               dT,
+                               vtolPathFollowerSettings->HorizontalVelMax);
+
+
+    controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP);
+    controlNE.UpdateCommandParameters(-vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->VelocityFeedforward);
+
+    controlDown.UpdateParameters(vtolPathFollowerSettings->AutoTakeoffVerticalVelPID.Kp,
+                                 vtolPathFollowerSettings->AutoTakeoffVerticalVelPID.Ki,
+                                 vtolPathFollowerSettings->AutoTakeoffVerticalVelPID.Kd,
+                                 vtolPathFollowerSettings->AutoTakeoffVerticalVelPID.Beta,
+                                 dT,
+                                 vtolPathFollowerSettings->VerticalVelMax);
+    controlDown.UpdatePositionalParameters(vtolPathFollowerSettings->VerticalPosP);
+    VtolSelfTuningStatsData vtolSelfTuningStats;
+    VtolSelfTuningStatsGet(&vtolSelfTuningStats);
+    controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + vtolPathFollowerSettings->ThrustLimits.Neutral);
+    controlDown.SetThrustLimits(vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max);
+    fsm->SettingsUpdated();
+}
+
+// AutoTakeoff Uses a different FSM to its parent
+int32_t VtolAutoTakeoffController::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings)
+{
+    PIOS_Assert(ptr_vtolPathFollowerSettings);
+    vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
+
+    if (fsm == 0) {
+        fsm = VtolAutoTakeoffFSM::instance();
+        VtolAutoTakeoffFSM::instance()->Initialize(vtolPathFollowerSettings, pathDesired, flightStatus);
+        controlDown.Initialize(fsm);
+    }
+    return 0;
+}
+
+
+void VtolAutoTakeoffController::UpdateVelocityDesired()
+{
+    VelocityStateData velocityState;
+
+    VelocityStateGet(&velocityState);
+    VelocityDesiredData velocityDesired;
+    PositionStateData positionState;
+    PositionStateGet(&positionState);
+
+    if (fsm->PositionHoldState()) {
+        controlDown.UpdatePositionState(positionState.Down);
+        controlDown.ControlPosition();
+    }
+
+    controlDown.UpdateVelocityState(velocityState.Down);
+    controlNE.UpdateVelocityState(velocityState.North, velocityState.East);
+
+    controlNE.UpdatePositionState(positionState.North, positionState.East);
+    controlNE.ControlPosition();
+
+    velocityDesired.Down  = controlDown.GetVelocityDesired();
+    float north, east;
+    controlNE.GetVelocityDesired(&north, &east);
+    velocityDesired.North = north;
+    velocityDesired.East  = east;
+
+    // update pathstatus
+    pathStatus->error     = 0.0f;
+    pathStatus->fractional_progress = 0.0f;
+    if (fsm->PositionHoldState()) {
+        pathStatus->fractional_progress = 1.0f;
+        // note if the takeoff waypoint and the launch position are significantly different
+        // the above check might need to expand to assessment of north and east.
+    }
+    pathStatus->path_direction_north = velocityDesired.North;
+    pathStatus->path_direction_east  = velocityDesired.East;
+    pathStatus->path_direction_down  = velocityDesired.Down;
+
+    pathStatus->correction_direction_north = velocityDesired.North - velocityState.North;
+    pathStatus->correction_direction_east  = velocityDesired.East - velocityState.East;
+    pathStatus->correction_direction_down  = velocityDesired.Down - velocityState.Down;
+
+    VelocityDesiredSet(&velocityDesired);
+}
+
+int8_t VtolAutoTakeoffController::UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction)
+{
+    uint8_t result = 1;
+    StabilizationDesiredData stabDesired;
+    AttitudeStateData attitudeState;
+    StabilizationBankData stabSettings;
+    float northCommand;
+    float eastCommand;
+
+    StabilizationDesiredGet(&stabDesired);
+    AttitudeStateGet(&attitudeState);
+    StabilizationBankGet(&stabSettings);
+
+    controlNE.GetNECommand(&northCommand, &eastCommand);
+    stabDesired.Thrust = controlDown.GetDownCommand();
+
+    float angle_radians = DEG2RAD(attitudeState.Yaw);
+    float cos_angle     = cosf(angle_radians);
+    float sine_angle    = sinf(angle_radians);
+    float maxPitch = vtolPathFollowerSettings->MaxRollPitch;
+    stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.Pitch = boundf(-northCommand * cos_angle - eastCommand * sine_angle, -maxPitch, maxPitch);
+    stabDesired.StabilizationMode.Roll  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.Roll = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -maxPitch, maxPitch);
+
+    ManualControlCommandData manualControl;
+    ManualControlCommandGet(&manualControl);
+
+    if (yaw_attitude) {
+        stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+        stabDesired.Yaw = yaw_direction;
+    } else {
+        stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
+        stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
+    }
+
+    // default thrust mode to cruise control
+    stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
+
+    fsm->ConstrainStabiDesired(&stabDesired); // excludes thrust
+    StabilizationDesiredSet(&stabDesired);
+
+    return result;
+}
+
+void VtolAutoTakeoffController::UpdateAutoPilot()
+{
+    fsm->Update();
+
+    UpdateVelocityDesired();
+
+    // yaw behaviour is configurable in vtolpathfollower, select yaw control algorithm
+    bool yaw_attitude = true;
+    float yaw = 0.0f;
+
+    fsm->GetYaw(yaw_attitude, yaw);
+
+    int8_t result = UpdateStabilizationDesired(yaw_attitude, yaw);
+
+    if (!result) {
+        fsm->Abort();
+    }
+
+    if (fsm->GetCurrentState() == PFFSM_STATE_DISARMED) {
+        setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
+    }
+
+    PathStatusSet(pathStatus);
+}
+
+void VtolAutoTakeoffController::setArmedIfChanged(FlightStatusArmedOptions val)
+{
+    if (flightStatus->Armed != val) {
+        flightStatus->Armed = val;
+        FlightStatusSet(flightStatus);
+    }
+}

flight/modules/PathFollower/vtolautotakeofffsm.cpp

@@ -0,0 +1,590 @@
+/*
+ ******************************************************************************
+ *
+ * @file       vtolautotakeofffsm.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      This autotakeoff state machine is a helper state machine to the
+ *             VtolAutoTakeoffController.
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+extern "C" {
+#include <openpilot.h>
+
+#include <callbackinfo.h>
+
+#include <math.h>
+#include <pid.h>
+#include <pathdesired.h>
+#include <paths.h>
+#include "plans.h"
+#include <sanitycheck.h>
+
+#include <homelocation.h>
+#include <accelstate.h>
+#include <vtolpathfollowersettings.h>
+#include <flightstatus.h>
+#include <flightmodesettings.h>
+#include <pathstatus.h>
+#include <positionstate.h>
+#include <velocitystate.h>
+#include <velocitydesired.h>
+#include <stabilizationdesired.h>
+#include <attitudestate.h>
+#include <takeofflocation.h>
+#include <manualcontrolcommand.h>
+#include <systemsettings.h>
+#include <stabilizationbank.h>
+#include <stabilizationdesired.h>
+#include <vtolselftuningstats.h>
+#include <statusvtolautotakeoff.h>
+#include <pathsummary.h>
+}
+
+// C++ includes
+#include <vtolautotakeofffsm.h>
+
+
+// Private constants
+#define TIMER_COUNT_PER_SECOND         (1000 / vtolPathFollowerSettings->UpdatePeriod)
+#define TIMEOUT_SLOWSTART              (2 * TIMER_COUNT_PER_SECOND)
+#define TIMEOUT_THRUSTUP               (1 * TIMER_COUNT_PER_SECOND)
+#define TIMEOUT_THRUSTDOWN             (2 * TIMER_COUNT_PER_SECOND)
+#define AUTOTAKEOFFING_SLOWDOWN_HEIGHT -5.0f
+
+VtolAutoTakeoffFSM::PathFollowerFSM_AutoTakeoffStateHandler_T VtolAutoTakeoffFSM::sAutoTakeoffStateTable[AUTOTAKEOFF_STATE_SIZE] = {
+    [AUTOTAKEOFF_STATE_INACTIVE]   = { .setup = &VtolAutoTakeoffFSM::setup_inactive,   .run = 0                                   },
+    [AUTOTAKEOFF_STATE_CHECKSTATE] = { .setup = &VtolAutoTakeoffFSM::setup_checkstate, .run = 0                                   },
+    [AUTOTAKEOFF_STATE_SLOWSTART]  = { .setup = &VtolAutoTakeoffFSM::setup_slowstart,  .run = &VtolAutoTakeoffFSM::run_slowstart  },
+    [AUTOTAKEOFF_STATE_THRUSTUP]   = { .setup = &VtolAutoTakeoffFSM::setup_thrustup,   .run = &VtolAutoTakeoffFSM::run_thrustup   },
+    [AUTOTAKEOFF_STATE_TAKEOFF]    = { .setup = &VtolAutoTakeoffFSM::setup_takeoff,    .run = &VtolAutoTakeoffFSM::run_takeoff    },
+    [AUTOTAKEOFF_STATE_HOLD] =       { .setup = &VtolAutoTakeoffFSM::setup_hold,       .run = &VtolAutoTakeoffFSM::run_hold       },
+    [AUTOTAKEOFF_STATE_THRUSTDOWN] = { .setup = &VtolAutoTakeoffFSM::setup_thrustdown, .run = &VtolAutoTakeoffFSM::run_thrustdown },
+    [AUTOTAKEOFF_STATE_THRUSTOFF]  = { .setup = &VtolAutoTakeoffFSM::setup_thrustoff,  .run = &VtolAutoTakeoffFSM::run_thrustoff  },
+    [AUTOTAKEOFF_STATE_DISARMED]   = { .setup = &VtolAutoTakeoffFSM::setup_disarmed,   .run = &VtolAutoTakeoffFSM::run_disarmed   },
+    [AUTOTAKEOFF_STATE_ABORT] =      { .setup = &VtolAutoTakeoffFSM::setup_abort,      .run = &VtolAutoTakeoffFSM::run_abort      }
+};
+
+// pointer to a singleton instance
+VtolAutoTakeoffFSM *VtolAutoTakeoffFSM::p_inst = 0;
+
+
+VtolAutoTakeoffFSM::VtolAutoTakeoffFSM()
+    : mAutoTakeoffData(0), vtolPathFollowerSettings(0), pathDesired(0), flightStatus(0)
+{}
+
+// Private types
+
+// Private functions
+// Public API methods
+/**
+ * Initialise the module, called on startup
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t VtolAutoTakeoffFSM::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings,
+                                       PathDesiredData *ptr_pathDesired,
+                                       FlightStatusData *ptr_flightStatus)
+{
+    PIOS_Assert(ptr_vtolPathFollowerSettings);
+    PIOS_Assert(ptr_pathDesired);
+    PIOS_Assert(ptr_flightStatus);
+
+    if (mAutoTakeoffData == 0) {
+        mAutoTakeoffData = (VtolAutoTakeoffFSMData_T *)pios_malloc(sizeof(VtolAutoTakeoffFSMData_T));
+        PIOS_Assert(mAutoTakeoffData);
+    }
+    memset(mAutoTakeoffData, 0, sizeof(VtolAutoTakeoffFSMData_T));
+    vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
+    pathDesired  = ptr_pathDesired;
+    flightStatus = ptr_flightStatus;
+    initFSM();
+
+    return 0;
+}
+
+void VtolAutoTakeoffFSM::Inactive(void)
+{
+    memset(mAutoTakeoffData, 0, sizeof(VtolAutoTakeoffFSMData_T));
+    initFSM();
+}
+
+// Initialise the FSM
+void VtolAutoTakeoffFSM::initFSM(void)
+{
+    if (vtolPathFollowerSettings != 0) {
+        setState(STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
+    } else {
+        mAutoTakeoffData->currentState = STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE;
+    }
+}
+
+void VtolAutoTakeoffFSM::Activate()
+{
+    memset(mAutoTakeoffData, 0, sizeof(VtolAutoTakeoffFSMData_T));
+    mAutoTakeoffData->currentState   = STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE;
+    mAutoTakeoffData->flLowAltitude  = true;
+    mAutoTakeoffData->flAltitudeHold = false;
+    mAutoTakeoffData->boundThrustMin = 0.0f;
+    mAutoTakeoffData->boundThrustMax = 0.0f;
+    mAutoTakeoffData->flZeroStabiHorizontal = true; // turn off positional controllers
+    TakeOffLocationGet(&(mAutoTakeoffData->takeOffLocation));
+    mAutoTakeoffData->fsmAutoTakeoffStatus.AltitudeAtState[STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE] = 0.0f;
+    mAutoTakeoffData->fsmAutoTakeoffStatus.ControlState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED;
+    assessAltitude();
+
+    // Check if we are already flying.  This can happen in pathplanner mode
+    // going into a second loop of the waypoints.
+    StabilizationDesiredData stabDesired;
+    StabilizationDesiredGet(&stabDesired);
+    if (stabDesired.Thrust > vtolPathFollowerSettings->ThrustLimits.Min) {
+        setState(STATUSVTOLAUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
+        return;
+    }
+
+    // initially inactive and wait for a change in controlstate.
+    setState(STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
+}
+
+void VtolAutoTakeoffFSM::Abort(void)
+{
+    setState(STATUSVTOLAUTOTAKEOFF_STATE_ABORT, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
+}
+
+PathFollowerFSMState_T VtolAutoTakeoffFSM::GetCurrentState(void)
+{
+    switch (mAutoTakeoffData->currentState) {
+    case STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE:
+        return PFFSM_STATE_INACTIVE;
+
+        break;
+    case STATUSVTOLAUTOTAKEOFF_STATE_ABORT:
+        return PFFSM_STATE_ABORT;
+
+        break;
+    case STATUSVTOLAUTOTAKEOFF_STATE_DISARMED:
+        return PFFSM_STATE_DISARMED;
+
+        break;
+    default:
+        return PFFSM_STATE_ACTIVE;
+
+        break;
+    }
+}
+
+void VtolAutoTakeoffFSM::Update()
+{
+    runState();
+    if (GetCurrentState() != PFFSM_STATE_INACTIVE) {
+        runAlways();
+    }
+}
+
+int32_t VtolAutoTakeoffFSM::runState(void)
+{
+    uint8_t flTimeout = false;
+
+    mAutoTakeoffData->stateRunCount++;
+
+    if (mAutoTakeoffData->stateTimeoutCount > 0 && mAutoTakeoffData->stateRunCount > mAutoTakeoffData->stateTimeoutCount) {
+        flTimeout = true;
+    }
+
+    // If the current state has a static function, call it
+    if (sAutoTakeoffStateTable[mAutoTakeoffData->currentState].run) {
+        (this->*sAutoTakeoffStateTable[mAutoTakeoffData->currentState].run)(flTimeout);
+    }
+    return 0;
+}
+
+int32_t VtolAutoTakeoffFSM::runAlways(void)
+{
+    void assessAltitude(void);
+
+    return 0;
+}
+
+// PathFollower implements the PID scheme and has a objective
+// set by a PathDesired object.  Based on the mode, pathfollower
+// uses FSM's as helper functions that manage state and event detection.
+// PathFollower calls into FSM methods to alter its commands.
+
+void VtolAutoTakeoffFSM::BoundThrust(float &ulow, float &uhigh)
+{
+    ulow  = mAutoTakeoffData->boundThrustMin;
+    uhigh = mAutoTakeoffData->boundThrustMax;
+
+
+    if (mAutoTakeoffData->flConstrainThrust) {
+        uhigh = mAutoTakeoffData->thrustLimit;
+    }
+}
+
+void VtolAutoTakeoffFSM::ConstrainStabiDesired(StabilizationDesiredData *stabDesired)
+{
+    if (mAutoTakeoffData->flZeroStabiHorizontal && stabDesired) {
+        stabDesired->Pitch = 0.0f;
+        stabDesired->Roll  = 0.0f;
+        stabDesired->Yaw   = 0.0f;
+    }
+}
+
+// Set the new state and perform setup for subsequent state run calls
+// This is called by state run functions on event detection that drive
+// state transitions.
+void VtolAutoTakeoffFSM::setState(StatusVtolAutoTakeoffStateOptions newState, StatusVtolAutoTakeoffStateExitReasonOptions reason)
+{
+    mAutoTakeoffData->fsmAutoTakeoffStatus.StateExitReason[mAutoTakeoffData->currentState] = reason;
+
+    if (mAutoTakeoffData->currentState == newState) {
+        return;
+    }
+    mAutoTakeoffData->currentState = newState;
+
+    if (newState != STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE) {
+        PositionStateData positionState;
+        PositionStateGet(&positionState);
+        float takeOffDown = 0.0f;
+        if (mAutoTakeoffData->takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) {
+            takeOffDown = mAutoTakeoffData->takeOffLocation.Down;
+        }
+        mAutoTakeoffData->fsmAutoTakeoffStatus.AltitudeAtState[newState] = positionState.Down - takeOffDown;
+        assessAltitude();
+    }
+
+    // Restart state timer counter
+    mAutoTakeoffData->stateRunCount     = 0;
+
+    // Reset state timeout to disabled/zero
+    mAutoTakeoffData->stateTimeoutCount = 0;
+
+    if (sAutoTakeoffStateTable[mAutoTakeoffData->currentState].setup) {
+        (this->*sAutoTakeoffStateTable[mAutoTakeoffData->currentState].setup)();
+    }
+
+    updateVtolAutoTakeoffFSMStatus();
+}
+
+
+// Timeout utility function for use by state init implementations
+void VtolAutoTakeoffFSM::setStateTimeout(int32_t count)
+{
+    mAutoTakeoffData->stateTimeoutCount = count;
+}
+
+void VtolAutoTakeoffFSM::updateVtolAutoTakeoffFSMStatus()
+{
+    mAutoTakeoffData->fsmAutoTakeoffStatus.State = mAutoTakeoffData->currentState;
+    if (mAutoTakeoffData->flLowAltitude) {
+        mAutoTakeoffData->fsmAutoTakeoffStatus.AltitudeState = STATUSVTOLAUTOTAKEOFF_ALTITUDESTATE_LOW;
+    } else {
+        mAutoTakeoffData->fsmAutoTakeoffStatus.AltitudeState = STATUSVTOLAUTOTAKEOFF_ALTITUDESTATE_HIGH;
+    }
+    StatusVtolAutoTakeoffSet(&mAutoTakeoffData->fsmAutoTakeoffStatus);
+}
+
+
+void VtolAutoTakeoffFSM::assessAltitude(void)
+{
+    float positionDown;
+
+    PositionStateDownGet(&positionDown);
+    float takeOffDown = 0.0f;
+    if (mAutoTakeoffData->takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) {
+        takeOffDown = mAutoTakeoffData->takeOffLocation.Down;
+    }
+    float positionDownRelativeToTakeoff = positionDown - takeOffDown;
+    if (positionDownRelativeToTakeoff < AUTOTAKEOFFING_SLOWDOWN_HEIGHT) {
+        mAutoTakeoffData->flLowAltitude = false;
+    } else {
+        mAutoTakeoffData->flLowAltitude = true;
+    }
+}
+
+// Action the required state from plans.c
+void VtolAutoTakeoffFSM::setControlState(StatusVtolAutoTakeoffControlStateOptions controlState)
+{
+    mAutoTakeoffData->fsmAutoTakeoffStatus.ControlState = controlState;
+
+    switch (controlState) {
+    case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED:
+        setState(STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
+        break;
+    case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE:
+        setState(STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
+        break;
+    case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST:
+        setState(STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
+        break;
+    case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE:
+        setState(STATUSVTOLAUTOTAKEOFF_STATE_CHECKSTATE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
+        break;
+    case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD:
+        setState(STATUSVTOLAUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
+        break;
+    case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT:
+        setState(STATUSVTOLAUTOTAKEOFF_STATE_ABORT, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
+        break;
+    }
+}
+
+
+// State: INACTIVE
+void VtolAutoTakeoffFSM::setup_inactive(void)
+{
+    // Re-initialise local variables
+    mAutoTakeoffData->flZeroStabiHorizontal = false;
+    mAutoTakeoffData->flConstrainThrust     = false;
+}
+
+// State: CHECKSTATE
+void VtolAutoTakeoffFSM::setup_checkstate(void)
+{
+    // Assumptions that do not need to be checked if flight mode is AUTOTAKEOFF
+    // 1. Already armed
+    // 2. Not in flight. This was checked in plans.c
+    // 3. User has placed throttle position to more than 50% to allow autotakeoff
+
+    // If pathplanner, we need additional checks
+    // E.g. if inflight, this mode is just positon hol
+    StabilizationDesiredData stabDesired;
+
+    StabilizationDesiredGet(&stabDesired);
+    if (stabDesired.Thrust > vtolPathFollowerSettings->ThrustLimits.Min) {
+        setState(STATUSVTOLAUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
+        return;
+    }
+
+
+    // Start from a enforced thrust off condition
+    mAutoTakeoffData->flConstrainThrust = false;
+    mAutoTakeoffData->boundThrustMin    = -0.1f;
+    mAutoTakeoffData->boundThrustMax    = 0.0f;
+
+    setState(STATUSVTOLAUTOTAKEOFF_STATE_SLOWSTART, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_TIMEOUT);
+}
+
+// STATE: SLOWSTART spools up motors to vtol min over 2 seconds for effect.
+// PID loops may be cumulating I terms but that problem needs to be solved
+#define SLOWSTART_INITIAL_THRUST 0.05f // assumed to be less than vtol min
+void VtolAutoTakeoffFSM::setup_slowstart(void)
+{
+    setStateTimeout(TIMEOUT_SLOWSTART);
+    mAutoTakeoffData->flZeroStabiHorizontal = true; // turn off positional controllers
+    StabilizationDesiredData stabDesired;
+    StabilizationDesiredGet(&stabDesired);
+    float vtol_thrust_min = vtolPathFollowerSettings->ThrustLimits.Min;
+    if (vtol_thrust_min < SLOWSTART_INITIAL_THRUST) {
+        vtol_thrust_min = SLOWSTART_INITIAL_THRUST;
+    }
+    mAutoTakeoffData->sum1 = (vtol_thrust_min - SLOWSTART_INITIAL_THRUST) / (float)TIMEOUT_SLOWSTART;
+    mAutoTakeoffData->sum2 = vtol_thrust_min;
+    mAutoTakeoffData->boundThrustMin = SLOWSTART_INITIAL_THRUST;
+    mAutoTakeoffData->boundThrustMax = SLOWSTART_INITIAL_THRUST;
+    PositionStateData positionState;
+    PositionStateGet(&positionState);
+    mAutoTakeoffData->expectedAutoTakeoffPositionNorth = positionState.North;
+    mAutoTakeoffData->expectedAutoTakeoffPositionEast  = positionState.East;
+}
+
+void VtolAutoTakeoffFSM::run_slowstart(__attribute__((unused)) uint8_t flTimeout)
+{
+    // increase thrust setpoint step by step
+    if (mAutoTakeoffData->boundThrustMin < mAutoTakeoffData->sum2) {
+        mAutoTakeoffData->boundThrustMin += mAutoTakeoffData->sum1;
+    }
+    mAutoTakeoffData->boundThrustMax += mAutoTakeoffData->sum1;
+    if (mAutoTakeoffData->boundThrustMax > mAutoTakeoffData->sum2) {
+        mAutoTakeoffData->boundThrustMax = mAutoTakeoffData->sum2;
+    }
+
+    if (flTimeout) {
+        setState(STATUSVTOLAUTOTAKEOFF_STATE_THRUSTUP, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_TIMEOUT);
+    }
+}
+
+// STATE: THRUSTUP spools up motors to vtol min over 5 seconds for effect.
+// PID loops may be cumulating I terms but that problem needs to be solved
+#define THRUSTUP_FINAL_THRUST_AS_RATIO_OF_VTOLMAX 0.8f
+void VtolAutoTakeoffFSM::setup_thrustup(void)
+{
+    setStateTimeout(TIMEOUT_THRUSTUP);
+    mAutoTakeoffData->flZeroStabiHorizontal = false;
+    StabilizationDesiredData stabDesired;
+    StabilizationDesiredGet(&stabDesired);
+    mAutoTakeoffData->sum2 = THRUSTUP_FINAL_THRUST_AS_RATIO_OF_VTOLMAX * vtolPathFollowerSettings->ThrustLimits.Max;
+    mAutoTakeoffData->sum1 = (mAutoTakeoffData->sum2 - mAutoTakeoffData->boundThrustMax) / (float)TIMEOUT_THRUSTUP;
+    mAutoTakeoffData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
+}
+
+void VtolAutoTakeoffFSM::run_thrustup(__attribute__((unused)) uint8_t flTimeout)
+{
+    // increase thrust setpoint step by step
+    mAutoTakeoffData->boundThrustMax += mAutoTakeoffData->sum1;
+    if (mAutoTakeoffData->boundThrustMax > mAutoTakeoffData->sum2) {
+        mAutoTakeoffData->boundThrustMax = mAutoTakeoffData->sum2;
+    }
+
+    if (flTimeout) {
+        setState(STATUSVTOLAUTOTAKEOFF_STATE_TAKEOFF, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_TIMEOUT);
+    }
+}
+
+
+// STATE: TAKEOFF
+void VtolAutoTakeoffFSM::setup_takeoff(void)
+{
+    mAutoTakeoffData->flZeroStabiHorizontal = false;
+}
+void VtolAutoTakeoffFSM::run_takeoff(__attribute__((unused)) uint8_t flTimeout)
+{
+    StabilizationDesiredData stabDesired;
+
+    StabilizationDesiredGet(&stabDesired);
+    if (stabDesired.Thrust < 0.0f) {
+        setState(STATUSVTOLAUTOTAKEOFF_STATE_THRUSTOFF, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_ZEROTHRUST);
+        return;
+    }
+
+    // detect broad sideways drift.
+    PositionStateData positionState;
+    PositionStateGet(&positionState);
+    float north_error   = mAutoTakeoffData->expectedAutoTakeoffPositionNorth - positionState.North;
+    float east_error    = mAutoTakeoffData->expectedAutoTakeoffPositionEast - positionState.East;
+    float down_error    = pathDesired->End.Down - positionState.Down;
+    float positionError = sqrtf(north_error * north_error + east_error * east_error);
+    if (positionError > 3.0f) {
+        setState(STATUSVTOLAUTOTAKEOFF_STATE_THRUSTDOWN, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_POSITIONERROR);
+        return;
+    }
+    if (fabsf(down_error) < 0.5f) {
+        setState(STATUSVTOLAUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_ARRIVEDATALT);
+        return;
+    }
+}
+
+// STATE: HOLD
+void VtolAutoTakeoffFSM::setup_hold(void)
+{
+    mAutoTakeoffData->flZeroStabiHorizontal = false;
+    mAutoTakeoffData->flAltitudeHold = true;
+}
+void VtolAutoTakeoffFSM::run_hold(__attribute__((unused)) uint8_t flTimeout)
+{}
+
+uint8_t VtolAutoTakeoffFSM::PositionHoldState(void)
+{
+    return mAutoTakeoffData->flAltitudeHold;
+}
+
+// STATE: THRUSTDOWN
+void VtolAutoTakeoffFSM::setup_thrustdown(void)
+{
+    setStateTimeout(TIMEOUT_THRUSTDOWN);
+    mAutoTakeoffData->flZeroStabiHorizontal = true;
+    mAutoTakeoffData->flConstrainThrust     = true;
+    StabilizationDesiredData stabDesired;
+    StabilizationDesiredGet(&stabDesired);
+    mAutoTakeoffData->thrustLimit    = stabDesired.Thrust;
+    mAutoTakeoffData->sum1 = stabDesired.Thrust / (float)TIMEOUT_THRUSTDOWN;
+    mAutoTakeoffData->boundThrustMin = -0.1f;
+    mAutoTakeoffData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Neutral;
+}
+
+void VtolAutoTakeoffFSM::run_thrustdown(__attribute__((unused)) uint8_t flTimeout)
+{
+    // reduce thrust setpoint step by step
+    mAutoTakeoffData->thrustLimit -= mAutoTakeoffData->sum1;
+
+    StabilizationDesiredData stabDesired;
+    StabilizationDesiredGet(&stabDesired);
+    if (stabDesired.Thrust < 0.0f || mAutoTakeoffData->thrustLimit < 0.0f) {
+        setState(STATUSVTOLAUTOTAKEOFF_STATE_THRUSTOFF, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_ZEROTHRUST);
+    }
+
+    if (flTimeout) {
+        setState(STATUSVTOLAUTOTAKEOFF_STATE_THRUSTOFF, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_TIMEOUT);
+    }
+}
+
+// STATE: THRUSTOFF
+void VtolAutoTakeoffFSM::setup_thrustoff(void)
+{
+    mAutoTakeoffData->thrustLimit       = -1.0f;
+    mAutoTakeoffData->flConstrainThrust = true;
+    mAutoTakeoffData->boundThrustMin    = -0.1f;
+    mAutoTakeoffData->boundThrustMax    = 0.0f;
+}
+
+void VtolAutoTakeoffFSM::run_thrustoff(__attribute__((unused)) uint8_t flTimeout)
+{
+    setState(STATUSVTOLAUTOTAKEOFF_STATE_DISARMED, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
+}
+
+// STATE: DISARMED
+void VtolAutoTakeoffFSM::setup_disarmed(void)
+{
+    // nothing to do
+    mAutoTakeoffData->flConstrainThrust     = false;
+    mAutoTakeoffData->flZeroStabiHorizontal = false;
+    mAutoTakeoffData->observationCount = 0;
+    mAutoTakeoffData->boundThrustMin   = -0.1f;
+    mAutoTakeoffData->boundThrustMax   = 0.0f;
+}
+
+void VtolAutoTakeoffFSM::run_disarmed(__attribute__((unused)) uint8_t flTimeout)
+{
+#ifdef DEBUG_GROUNDIMPACT
+    if (mAutoTakeoffData->observationCount++ > 100) {
+        setState(AUTOTAKEOFF_STATE_WTG_FOR_GROUNDEFFECT, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
+    }
+#endif
+}
+
+void VtolAutoTakeoffFSM::fallback_to_hold(void)
+{
+    PositionStateData positionState;
+
+    PositionStateGet(&positionState);
+    pathDesired->End.North        = positionState.North;
+    pathDesired->End.East         = positionState.East;
+    pathDesired->End.Down         = positionState.Down;
+    pathDesired->Start.North      = positionState.North;
+    pathDesired->Start.East       = positionState.East;
+    pathDesired->Start.Down       = positionState.Down;
+    pathDesired->StartingVelocity = 0.0f;
+    pathDesired->EndingVelocity   = 0.0f;
+    pathDesired->Mode = PATHDESIRED_MODE_GOTOENDPOINT;
+
+    PathDesiredSet(pathDesired);
+}
+
+// abort repeatedly overwrites pathfollower's objective on a landing abort and
+// continues to do so until a flight mode change.
+void VtolAutoTakeoffFSM::setup_abort(void)
+{
+    mAutoTakeoffData->boundThrustMin        = vtolPathFollowerSettings->ThrustLimits.Min;
+    mAutoTakeoffData->boundThrustMax        = vtolPathFollowerSettings->ThrustLimits.Max;
+    mAutoTakeoffData->flConstrainThrust     = false;
+    mAutoTakeoffData->flZeroStabiHorizontal = false;
+    fallback_to_hold();
+}
+
+void VtolAutoTakeoffFSM::run_abort(__attribute__((unused)) uint8_t flTimeout)
+{}

flight/modules/PathFollower/vtolbrakecontroller.cpp

@@ -0,0 +1,372 @@
+/*
+ ******************************************************************************
+ *
+ * @file       vtolbrakecontroller.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      This landing state machine is a helper state machine to the
+ *              pathfollower task/thread to implement detailed landing controls.
+ *		This is to be called only from the pathfollower task.
+ *		Note that initiation of the land occurs in the manual control
+ *		command thread calling plans.c plan_setup_land which writes
+ *		the required PathDesired LAND mode.
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+extern "C" {
+#include <openpilot.h>
+
+#include <callbackinfo.h>
+
+#include <math.h>
+#include <pid.h>
+#include <CoordinateConversions.h>
+#include <sin_lookup.h>
+#include <pathdesired.h>
+#include <paths.h>
+#include "plans.h"
+#include <sanitycheck.h>
+
+#include <accelstate.h>
+#include <vtolpathfollowersettings.h>
+#include <flightstatus.h>
+#include <flightmodesettings.h>
+#include <pathstatus.h>
+#include <positionstate.h>
+#include <velocitystate.h>
+#include <velocitydesired.h>
+#include <stabilizationdesired.h>
+#include <attitudestate.h>
+#include <takeofflocation.h>
+#include <manualcontrolcommand.h>
+#include <systemsettings.h>
+#include <stabilizationbank.h>
+#include <stabilizationdesired.h>
+#include <vtolselftuningstats.h>
+#include <pathsummary.h>
+}
+
+// C++ includes
+#include "vtolbrakecontroller.h"
+#include "pathfollowerfsm.h"
+#include "vtolbrakefsm.h"
+#include "pidcontroldown.h"
+
+// Private constants
+#define BRAKE_RATE_MINIMUM 0.2f
+
+// pointer to a singleton instance
+VtolBrakeController *VtolBrakeController::p_inst = 0;
+
+VtolBrakeController::VtolBrakeController()
+    : fsm(0), vtolPathFollowerSettings(0), mActive(false), mHoldActive(false), mManualThrust(false)
+{}
+
+// Called when mode first engaged
+void VtolBrakeController::Activate(void)
+{
+    if (!mActive) {
+        mActive       = true;
+        mHoldActive   = false;
+        mManualThrust = false;
+        SettingsUpdated();
+        fsm->Activate();
+        controlDown.Activate();
+        controlNE.Activate();
+    }
+}
+
+uint8_t VtolBrakeController::IsActive(void)
+{
+    return mActive;
+}
+
+uint8_t VtolBrakeController::Mode(void)
+{
+    return PATHDESIRED_MODE_BRAKE;
+}
+
+// Objective updated in pathdesired
+void VtolBrakeController::ObjectiveUpdated(void)
+{
+    // Set the objective's target velocity
+    controlDown.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_DOWN]);
+    controlNE.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_NORTH],
+                                     pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_EAST]);
+    if (pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] > 0.0f) {
+        pathStatus->path_time = 0.0f;
+    }
+}
+void VtolBrakeController::Deactivate(void)
+{
+    if (mActive) {
+        mActive       = false;
+        mHoldActive   = false;
+        mManualThrust = false;
+        fsm->Inactive();
+        controlDown.Deactivate();
+        controlNE.Deactivate();
+    }
+}
+
+
+void VtolBrakeController::SettingsUpdated(void)
+{
+    const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f;
+
+    controlNE.UpdateParameters(vtolPathFollowerSettings->BrakeHorizontalVelPID.Kp,
+                               vtolPathFollowerSettings->BrakeHorizontalVelPID.Ki,
+                               vtolPathFollowerSettings->BrakeHorizontalVelPID.Kd,
+                               vtolPathFollowerSettings->BrakeHorizontalVelPID.Beta,
+                               dT,
+                               10.0f * vtolPathFollowerSettings->HorizontalVelMax); // avoid constraining initial fast entry into brake
+    controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP);
+    controlNE.UpdateCommandParameters(-vtolPathFollowerSettings->BrakeMaxPitch, vtolPathFollowerSettings->BrakeMaxPitch, vtolPathFollowerSettings->BrakeVelocityFeedforward);
+
+    controlDown.UpdateParameters(vtolPathFollowerSettings->LandVerticalVelPID.Kp,
+                                 vtolPathFollowerSettings->LandVerticalVelPID.Ki,
+                                 vtolPathFollowerSettings->LandVerticalVelPID.Kd,
+                                 vtolPathFollowerSettings->LandVerticalVelPID.Beta,
+                                 dT,
+                                 10.0f * vtolPathFollowerSettings->VerticalVelMax); // avoid constraining initial fast entry into brake
+    controlDown.UpdatePositionalParameters(vtolPathFollowerSettings->VerticalPosP);
+    controlDown.SetThrustLimits(vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max);
+
+    VtolSelfTuningStatsData vtolSelfTuningStats;
+    VtolSelfTuningStatsGet(&vtolSelfTuningStats);
+    controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + vtolPathFollowerSettings->ThrustLimits.Neutral);
+    fsm->SettingsUpdated();
+}
+
+/**
+ * Initialise the module, called on startup
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t VtolBrakeController::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings)
+{
+    PIOS_Assert(ptr_vtolPathFollowerSettings);
+    vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
+    if (fsm == 0) {
+        VtolBrakeFSM::instance()->Initialize(vtolPathFollowerSettings, pathDesired, flightStatus, pathStatus);
+        fsm = (PathFollowerFSM *)VtolBrakeFSM::instance();
+        controlDown.Initialize(fsm);
+    }
+    return 0;
+}
+
+
+void VtolBrakeController::UpdateVelocityDesired()
+{
+    VelocityStateData velocityState;
+
+    VelocityStateGet(&velocityState);
+    VelocityDesiredData velocityDesired;
+
+    float brakeRate = vtolPathFollowerSettings->BrakeRate;
+    if (brakeRate < BRAKE_RATE_MINIMUM) {
+        brakeRate = BRAKE_RATE_MINIMUM; // set a minimum to avoid a divide by zero potential below
+    }
+
+    if (fsm->PositionHoldState()) {
+        PositionStateData positionState;
+        PositionStateGet(&positionState);
+
+        // On first engagement set the position setpoint
+        if (!mHoldActive) {
+            mHoldActive = true;
+            controlNE.UpdatePositionSetpoint(positionState.North, positionState.East);
+            if (!mManualThrust) {
+                controlDown.UpdatePositionSetpoint(positionState.Down);
+            }
+
+
+            FlightStatusFlightModeAssistOptions flightModeAssist;
+            FlightStatusFlightModeAssistGet(&flightModeAssist);
+            if (flightModeAssist != FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) {
+                // Notify manualcommand via setting hold state in flightstatus assistedcontrolstate
+                FlightStatusAssistedControlStateOptions assistedControlFlightMode;
+                FlightStatusAssistedControlStateGet(&assistedControlFlightMode);
+                // sanity check that we are in brake state according to flight status, which means
+                // we are being used for gpsassist
+                if (assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE) {
+                    assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD;
+                    FlightStatusAssistedControlStateSet(&assistedControlFlightMode);
+                }
+            }
+        }
+
+        // Update position state and control position to create inputs to velocity control
+        controlNE.UpdatePositionState(positionState.North, positionState.East);
+        controlNE.ControlPosition();
+        if (!mManualThrust) {
+            controlDown.UpdatePositionState(positionState.Down);
+            controlDown.ControlPosition();
+        }
+
+        controlNE.UpdateVelocityState(velocityState.North, velocityState.East);
+        if (!mManualThrust) {
+            controlDown.UpdateVelocityState(velocityState.Down);
+        }
+    } else {
+        controlNE.UpdateVelocityStateWithBrake(velocityState.North, velocityState.East, pathStatus->path_time, brakeRate);
+        if (!mManualThrust) {
+            controlDown.UpdateVelocityStateWithBrake(velocityState.Down, pathStatus->path_time, brakeRate);
+        }
+    }
+
+    if (!mManualThrust) {
+        velocityDesired.Down = controlDown.GetVelocityDesired();
+    } else { velocityDesired.Down = 0.0f; }
+
+    float north, east;
+    controlNE.GetVelocityDesired(&north, &east);
+    velocityDesired.North = north;
+    velocityDesired.East  = east;
+
+    VelocityDesiredSet(&velocityDesired);
+
+    // update pathstatus
+    float cur_velocity     = velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down;
+    cur_velocity      = sqrtf(cur_velocity);
+    float desired_velocity = velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East + velocityDesired.Down * velocityDesired.Down;
+    desired_velocity  = sqrtf(desired_velocity);
+    pathStatus->error = cur_velocity - desired_velocity;
+    pathStatus->fractional_progress = 1.0f;
+    if (pathDesired->StartingVelocity > 0.0f) {
+        pathStatus->fractional_progress = (pathDesired->StartingVelocity - cur_velocity) / pathDesired->StartingVelocity;
+
+        // sometimes current velocity can exceed starting velocity due to initial acceleration
+        if (pathStatus->fractional_progress < 0.0f) {
+            pathStatus->fractional_progress = 0.0f;
+        }
+    }
+    pathStatus->path_direction_north = velocityDesired.North;
+    pathStatus->path_direction_east  = velocityDesired.East;
+    pathStatus->path_direction_down  = velocityDesired.Down;
+
+    pathStatus->correction_direction_north = velocityDesired.North - velocityState.North;
+    pathStatus->correction_direction_east  = velocityDesired.East - velocityState.East;
+}
+
+int8_t VtolBrakeController::UpdateStabilizationDesired(void)
+{
+    uint8_t result = 1;
+    StabilizationDesiredData stabDesired;
+    AttitudeStateData attitudeState;
+    StabilizationBankData stabSettings;
+    float northCommand;
+    float eastCommand;
+
+    StabilizationDesiredGet(&stabDesired);
+    AttitudeStateGet(&attitudeState);
+    StabilizationBankGet(&stabSettings);
+
+    controlNE.GetNECommand(&northCommand, &eastCommand);
+
+    float angle_radians = DEG2RAD(attitudeState.Yaw);
+    float cos_angle     = cosf(angle_radians);
+    float sine_angle    = sinf(angle_radians);
+    float maxPitch = vtolPathFollowerSettings->BrakeMaxPitch;
+    stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.Pitch = boundf(-northCommand * cos_angle - eastCommand * sine_angle, -maxPitch, maxPitch); // this should be in the controller
+    stabDesired.StabilizationMode.Roll  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.Roll = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -maxPitch, maxPitch);
+
+    ManualControlCommandData manualControl;
+    ManualControlCommandGet(&manualControl);
+
+    stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
+    stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
+
+    // default thrust mode to cruise control
+    stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
+
+    // when flight mode assist is active but in primary-thrust mode, the thrust mode must be set to the same as per the primary mode.
+    if (flightStatus->FlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST) {
+        FlightModeSettingsData settings;
+        FlightModeSettingsGet(&settings);
+        uint8_t thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL;
+
+        switch (flightStatus->FlightMode) {
+        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
+            thrustMode = settings.Stabilization1Settings.Thrust;
+            break;
+        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
+            thrustMode = settings.Stabilization2Settings.Thrust;
+            break;
+        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
+            thrustMode = settings.Stabilization3Settings.Thrust;
+            break;
+        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
+            thrustMode = settings.Stabilization4Settings.Thrust;
+            break;
+        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
+            thrustMode = settings.Stabilization5Settings.Thrust;
+            break;
+        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
+            thrustMode = settings.Stabilization6Settings.Thrust;
+            break;
+        case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
+            // we hard code the "GPS Assisted" PostionHold/Roam to use alt-vario which
+            // is a more appropriate throttle mode.  "GPSAssist" adds braking
+            // and a better throttle management to the standard Position Hold.
+            thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO;
+            break;
+        default:
+            break;
+        }
+        stabDesired.StabilizationMode.Thrust = (StabilizationDesiredStabilizationModeOptions)thrustMode;
+    }
+
+    // set the thrust value
+    if (mManualThrust) {
+        stabDesired.Thrust = manualControl.Thrust;
+    } else {
+        stabDesired.Thrust = controlDown.GetDownCommand();
+    }
+
+    StabilizationDesiredSet(&stabDesired);
+
+    return result;
+}
+
+
+void VtolBrakeController::UpdateAutoPilot()
+{
+    if (pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] > 0.0f) {
+        pathStatus->path_time += vtolPathFollowerSettings->UpdatePeriod / 1000.0f;
+    }
+
+    if (flightStatus->FlightModeAssist && flightStatus->AssistedThrottleState == FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL) {
+        mManualThrust = true;
+    } else {
+        mManualThrust = false;
+    }
+
+    fsm->Update(); // This will run above end condition checks
+
+    UpdateVelocityDesired();
+
+    int8_t result = UpdateStabilizationDesired();
+
+    if (!result) {
+        fsm->Abort(); // enter PH
+    }
+
+    PathStatusSet(pathStatus);
+}

flight/modules/PathFollower/vtolbrakefsm.cpp

@@ -0,0 +1,261 @@
+/*
+ ******************************************************************************
+ *
+ * @file       vtolbrakefsm.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Vtol brake finate state machine to regulate behaviour of the
+ *              brake controller.
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+extern "C" {
+#include <openpilot.h>
+
+#include <callbackinfo.h>
+
+#include <math.h>
+#include <pid.h>
+#include <CoordinateConversions.h>
+#include <sin_lookup.h>
+#include <pathdesired.h>
+#include <paths.h>
+#include "plans.h"
+#include <sanitycheck.h>
+
+#include <vtolpathfollowersettings.h>
+#include <flightstatus.h>
+#include <flightmodesettings.h>
+#include <pathstatus.h>
+#include <positionstate.h>
+#include <velocitystate.h>
+#include <velocitydesired.h>
+#include <stabilizationdesired.h>
+#include <attitudestate.h>
+#include <manualcontrolcommand.h>
+#include <systemsettings.h>
+#include <stabilizationbank.h>
+#include <stabilizationdesired.h>
+#include <vtolselftuningstats.h>
+#include <pathsummary.h>
+}
+
+// C++ includes
+#include <vtolbrakefsm.h>
+
+
+// Private constants
+#define TIMER_COUNT_PER_SECOND                      (1000 / vtolPathFollowerSettings->UpdatePeriod)
+#define BRAKE_FRACTIONALPROGRESS_STARTVELOCITYCHECK 0.95f
+#define BRAKE_EXIT_VELOCITY_LIMIT                   0.2f
+
+VtolBrakeFSM::PathFollowerFSM_BrakeStateHandler_T VtolBrakeFSM::sBrakeStateTable[BRAKE_STATE_SIZE] = {
+    [BRAKE_STATE_INACTIVE] = { .setup = 0,                          .run = 0                        },
+    [BRAKE_STATE_BRAKE]    = { .setup = &VtolBrakeFSM::setup_brake, .run = &VtolBrakeFSM::run_brake },
+    [BRAKE_STATE_HOLD]     = { .setup = 0,                          .run = 0                        }
+};
+
+// pointer to a singleton instance
+VtolBrakeFSM *VtolBrakeFSM::p_inst = 0;
+
+
+VtolBrakeFSM::VtolBrakeFSM()
+    : mBrakeData(0), vtolPathFollowerSettings(0), pathDesired(0), flightStatus(0)
+{}
+
+
+// Private types
+
+// Private functions
+// Public API methods
+/**
+ * Initialise the module, called on startup
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t VtolBrakeFSM::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings,
+                                 PathDesiredData *ptr_pathDesired,
+                                 FlightStatusData *ptr_flightStatus,
+                                 PathStatusData *ptr_pathStatus)
+{
+    PIOS_Assert(ptr_vtolPathFollowerSettings);
+    PIOS_Assert(ptr_pathDesired);
+    PIOS_Assert(ptr_flightStatus);
+
+    // allow for Initialize being called more than once.
+    if (!mBrakeData) {
+        mBrakeData = (VtolBrakeFSMData_T *)pios_malloc(sizeof(VtolBrakeFSMData_T));
+        PIOS_Assert(mBrakeData);
+    }
+    memset(mBrakeData, 0, sizeof(VtolBrakeFSMData_T));
+    vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
+    pathDesired  = ptr_pathDesired;
+    flightStatus = ptr_flightStatus;
+    pathStatus   = ptr_pathStatus;
+    initFSM();
+
+    return 0;
+}
+
+void VtolBrakeFSM::Inactive(void)
+{
+    memset(mBrakeData, 0, sizeof(VtolBrakeFSMData_T));
+    initFSM();
+}
+
+// Initialise the FSM
+void VtolBrakeFSM::initFSM(void)
+{
+    mBrakeData->currentState = BRAKE_STATE_INACTIVE;
+}
+
+void VtolBrakeFSM::Activate()
+{
+    memset(mBrakeData, 0, sizeof(VtolBrakeFSMData_T));
+    mBrakeData->currentState = BRAKE_STATE_INACTIVE;
+    setState(BRAKE_STATE_BRAKE, FSMBRAKESTATUS_STATEEXITREASON_NONE);
+}
+
+void VtolBrakeFSM::Abort(void)
+{
+    setState(BRAKE_STATE_HOLD, FSMBRAKESTATUS_STATEEXITREASON_NONE);
+}
+
+PathFollowerFSMState_T VtolBrakeFSM::GetCurrentState(void)
+{
+    switch (mBrakeData->currentState) {
+    case BRAKE_STATE_INACTIVE:
+        return PFFSM_STATE_INACTIVE;
+
+        break;
+    default:
+        return PFFSM_STATE_ACTIVE;
+
+        break;
+    }
+}
+
+void VtolBrakeFSM::Update()
+{
+    runState();
+}
+
+int32_t VtolBrakeFSM::runState(void)
+{
+    uint8_t flTimeout = false;
+
+    mBrakeData->stateRunCount++;
+
+    if (mBrakeData->stateTimeoutCount > 0 && mBrakeData->stateRunCount > mBrakeData->stateTimeoutCount) {
+        flTimeout = true;
+    }
+
+    // If the current state has a static function, call it
+    if (sBrakeStateTable[mBrakeData->currentState].run) {
+        (this->*sBrakeStateTable[mBrakeData->currentState].run)(flTimeout);
+    }
+    return 0;
+}
+
+// Set the new state and perform setup for subsequent state run calls
+// This is called by state run functions on event detection that drive
+// state transitions.
+void VtolBrakeFSM::setState(PathFollowerFSM_BrakeState_T newState, __attribute__((unused)) VtolBrakeFSMStatusStateExitReasonOptions reason)
+{
+    // mBrakeData->fsmBrakeStatus.StateExitReason[mBrakeData->currentState] = reason;
+
+    if (mBrakeData->currentState == newState) {
+        return;
+    }
+    mBrakeData->currentState      = newState;
+
+    // Restart state timer counter
+    mBrakeData->stateRunCount     = 0;
+
+    // Reset state timeout to disabled/zero
+    mBrakeData->stateTimeoutCount = 0;
+
+    if (sBrakeStateTable[mBrakeData->currentState].setup) {
+        (this->*sBrakeStateTable[mBrakeData->currentState].setup)();
+    }
+}
+
+
+// Timeout utility function for use by state init implementations
+void VtolBrakeFSM::setStateTimeout(int32_t count)
+{
+    mBrakeData->stateTimeoutCount = count;
+}
+
+// FSM Setup and Run method implementation
+
+// State: WAITING FOR DESCENT RATE
+void VtolBrakeFSM::setup_brake(void)
+{
+    setStateTimeout(TIMER_COUNT_PER_SECOND * pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT]);
+    mBrakeData->observationCount  = 0;
+    mBrakeData->observation2Count = 0;
+}
+
+
+void VtolBrakeFSM::run_brake(uint8_t flTimeout)
+{
+    // Brake mode end condition checks
+    bool exit_brake = false;
+    VelocityStateData velocityState;
+    PathSummaryData pathSummary;
+
+    if (flTimeout) {
+        pathSummary.brake_exit_reason = PATHSUMMARY_BRAKE_EXIT_REASON_TIMEOUT;
+        exit_brake = true;
+    } else if (pathStatus->fractional_progress > BRAKE_FRACTIONALPROGRESS_STARTVELOCITYCHECK) {
+        VelocityStateGet(&velocityState);
+        if (fabsf(velocityState.East) < BRAKE_EXIT_VELOCITY_LIMIT && fabsf(velocityState.North) < BRAKE_EXIT_VELOCITY_LIMIT) {
+            pathSummary.brake_exit_reason = PATHSUMMARY_BRAKE_EXIT_REASON_PATHCOMPLETED;
+            exit_brake = true;
+        }
+    }
+
+    if (exit_brake) {
+        // Calculate the distance error between the originally desired
+        // stopping point and the actual brake-exit point.
+
+        PositionStateData p;
+        PositionStateGet(&p);
+        float north_offset = pathDesired->End.North - p.North;
+        float east_offset  = pathDesired->End.East - p.East;
+        float down_offset  = pathDesired->End.Down - p.Down;
+        pathSummary.brake_distance_offset = sqrtf(north_offset * north_offset + east_offset * east_offset + down_offset * down_offset);
+        pathSummary.time_remaining = pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] - pathStatus->path_time;
+        pathSummary.fractional_progress   = pathStatus->fractional_progress;
+        float cur_velocity = velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down;
+        cur_velocity = sqrtf(cur_velocity);
+        pathSummary.decelrate = (pathDesired->StartingVelocity - cur_velocity) / pathStatus->path_time;
+        pathSummary.brakeRateActualDesiredRatio = pathSummary.decelrate / vtolPathFollowerSettings->BrakeRate;
+        pathSummary.velocityIntoHold = cur_velocity;
+        pathSummary.Mode = PATHSUMMARY_MODE_BRAKE;
+        pathSummary.UID  = pathStatus->UID;
+        PathSummarySet(&pathSummary);
+
+        setState(BRAKE_STATE_HOLD, FSMBRAKESTATUS_STATEEXITREASON_NONE);
+    }
+}
+
+uint8_t VtolBrakeFSM::PositionHoldState(void)
+{
+    return mBrakeData->currentState == BRAKE_STATE_HOLD;
+}

flight/modules/PathFollower/vtolflycontroller.cpp

@@ -0,0 +1,550 @@
+/*
+ ******************************************************************************
+ *
+ * @file       vtolflycontroller.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Class implements the fly controller for vtols
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+extern "C" {
+#include <openpilot.h>
+
+#include <callbackinfo.h>
+
+#include <math.h>
+#include <pid.h>
+#include <CoordinateConversions.h>
+#include <sin_lookup.h>
+#include <pathdesired.h>
+#include <paths.h>
+#include "plans.h"
+#include <sanitycheck.h>
+
+#include <accelstate.h>
+#include <vtolpathfollowersettings.h>
+#include <flightstatus.h>
+#include <flightmodesettings.h>
+#include <pathstatus.h>
+#include <positionstate.h>
+#include <velocitystate.h>
+#include <velocitydesired.h>
+#include <stabilizationdesired.h>
+#include <attitudestate.h>
+#include <takeofflocation.h>
+#include <poilocation.h>
+#include <manualcontrolcommand.h>
+#include <systemsettings.h>
+#include <stabilizationbank.h>
+#include <stabilizationdesired.h>
+#include <vtolselftuningstats.h>
+#include <pathsummary.h>
+}
+
+// C++ includes
+#include "vtolflycontroller.h"
+#include "pathfollowerfsm.h"
+#include "pidcontroldown.h"
+#include "pidcontrolne.h"
+
+// Private constants
+#define DEADBAND_HIGH                                        0.10f
+#define DEADBAND_LOW                                         -0.10f
+#define RTB_LAND_FRACTIONAL_PROGRESS_START_CHECKS            0.95f
+#define RTB_LAND_NE_DISTANCE_REQUIRED_TO_START_LAND_SEQUENCE 2.0f
+
+// pointer to a singleton instance
+VtolFlyController *VtolFlyController::p_inst = 0;
+
+VtolFlyController::VtolFlyController()
+    : vtolPathFollowerSettings(NULL), mActive(false), mManualThrust(false), mMode(0), vtolEmergencyFallback(0.0f), vtolEmergencyFallbackSwitch(false)
+{}
+
+// Called when mode first engaged
+void VtolFlyController::Activate(void)
+{
+    if (!mActive) {
+        mActive = true;
+        mManualThrust = false;
+        SettingsUpdated();
+        controlDown.Activate();
+        controlNE.Activate();
+        mMode = pathDesired->Mode;
+
+        vtolEmergencyFallback = 0.0f;
+        vtolEmergencyFallbackSwitch = false;
+    }
+}
+
+uint8_t VtolFlyController::IsActive(void)
+{
+    return mActive;
+}
+
+uint8_t VtolFlyController::Mode(void)
+{
+    return mMode;
+}
+
+// Objective updated in pathdesired
+void VtolFlyController::ObjectiveUpdated(void)
+{}
+
+
+void VtolFlyController::Deactivate(void)
+{
+    if (mActive) {
+        mActive = false;
+        mManualThrust = false;
+        controlDown.Deactivate();
+        controlNE.Deactivate();
+        vtolEmergencyFallback = 0.0f;
+        vtolEmergencyFallbackSwitch = false;
+    }
+}
+
+
+void VtolFlyController::SettingsUpdated(void)
+{
+    const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f;
+
+    controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp,
+                               vtolPathFollowerSettings->HorizontalVelPID.Ki,
+                               vtolPathFollowerSettings->HorizontalVelPID.Kd,
+                               vtolPathFollowerSettings->HorizontalVelPID.Beta,
+                               dT,
+                               vtolPathFollowerSettings->HorizontalVelMax);
+    controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP);
+    controlNE.UpdateCommandParameters(-vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->VelocityFeedforward);
+
+    controlDown.UpdateParameters(vtolPathFollowerSettings->VerticalVelPID.Kp,
+                                 vtolPathFollowerSettings->VerticalVelPID.Ki,
+                                 vtolPathFollowerSettings->VerticalVelPID.Kd,
+                                 vtolPathFollowerSettings->VerticalVelPID.Beta,
+                                 dT,
+                                 vtolPathFollowerSettings->VerticalVelMax);
+    controlDown.UpdatePositionalParameters(vtolPathFollowerSettings->VerticalPosP);
+
+    VtolSelfTuningStatsData vtolSelfTuningStats;
+    VtolSelfTuningStatsGet(&vtolSelfTuningStats);
+    controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + vtolPathFollowerSettings->ThrustLimits.Neutral);
+    controlDown.SetThrustLimits(vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max);
+
+    // disable neutral thrust calcs which should only be done in a hold mode.
+    controlDown.DisableNeutralThrustCalc();
+}
+
+/**
+ * Initialise the module, called on startup
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t VtolFlyController::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings)
+{
+    PIOS_Assert(ptr_vtolPathFollowerSettings);
+
+    vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
+
+    return 0;
+}
+
+
+/**
+ * Compute desired velocity from the current position and path
+ */
+void VtolFlyController::UpdateVelocityDesired()
+{
+    PositionStateData positionState;
+
+    PositionStateGet(&positionState);
+
+    VelocityStateData velocityState;
+    VelocityStateGet(&velocityState);
+    controlNE.UpdateVelocityState(velocityState.North, velocityState.East);
+    controlDown.UpdateVelocityState(velocityState.Down);
+
+    VelocityDesiredData velocityDesired;
+
+    // look ahead kFF seconds
+    float cur[3] = { positionState.North + (velocityState.North * vtolPathFollowerSettings->CourseFeedForward),
+                     positionState.East + (velocityState.East * vtolPathFollowerSettings->CourseFeedForward),
+                     positionState.Down + (velocityState.Down * vtolPathFollowerSettings->CourseFeedForward) };
+    struct path_status progress;
+    path_progress(pathDesired, cur, &progress, true);
+
+    controlNE.ControlPositionWithPath(&progress);
+    if (!mManualThrust) {
+        controlDown.ControlPositionWithPath(&progress);
+    }
+
+    float north, east;
+    controlNE.GetVelocityDesired(&north, &east);
+    velocityDesired.North = north;
+    velocityDesired.East  = east;
+    if (!mManualThrust) {
+        velocityDesired.Down = controlDown.GetVelocityDesired();
+    } else { velocityDesired.Down = 0.0f; }
+
+    // update pathstatus
+    pathStatus->error = progress.error;
+    pathStatus->fractional_progress  = progress.fractional_progress;
+    pathStatus->path_direction_north = progress.path_vector[0];
+    pathStatus->path_direction_east  = progress.path_vector[1];
+    pathStatus->path_direction_down  = progress.path_vector[2];
+
+    pathStatus->correction_direction_north = progress.correction_vector[0];
+    pathStatus->correction_direction_east  = progress.correction_vector[1];
+    pathStatus->correction_direction_down  = progress.correction_vector[2];
+
+    VelocityDesiredSet(&velocityDesired);
+}
+
+
+int8_t VtolFlyController::UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction)
+{
+    uint8_t result = 1;
+    StabilizationDesiredData stabDesired;
+    AttitudeStateData attitudeState;
+    StabilizationBankData stabSettings;
+    float northCommand;
+    float eastCommand;
+
+    StabilizationDesiredGet(&stabDesired);
+    AttitudeStateGet(&attitudeState);
+    StabilizationBankGet(&stabSettings);
+
+    controlNE.GetNECommand(&northCommand, &eastCommand);
+
+    float angle_radians = DEG2RAD(attitudeState.Yaw);
+    float cos_angle     = cosf(angle_radians);
+    float sine_angle    = sinf(angle_radians);
+    float maxPitch = vtolPathFollowerSettings->MaxRollPitch;
+    stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.Pitch = boundf(-northCommand * cos_angle - eastCommand * sine_angle, -maxPitch, maxPitch); // this should be in the controller
+    stabDesired.StabilizationMode.Roll  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.Roll = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -maxPitch, maxPitch);
+
+    ManualControlCommandData manualControl;
+    ManualControlCommandGet(&manualControl);
+
+    // TODO The below need to be rewritten because the PID implementation has changed.
+#if 0
+    // DEBUG HACK: allow user to skew compass on purpose to see if emergency failsafe kicks in
+    if (vtolPathFollowerSettings->FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DEBUGTEST) {
+        attitudeState.Yaw += 120.0f;
+        if (attitudeState.Yaw > 180.0f) {
+            attitudeState.Yaw -= 360.0f;
+        }
+    }
+
+
+    if ( // emergency flyaway detection
+        ( // integral already at its limit
+            vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[0].iAccumulator) < 1e-6f ||
+            vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[1].iAccumulator) < 1e-6f
+        ) &&
+        // angle between desired and actual velocity >90 degrees (by dot product)
+        (velocityDesired.North * velocityState.North + velocityDesired.East * velocityState.East < 0.0f) &&
+        // quad is moving at significant speed (during flyaway it would keep speeding up)
+        squaref(velocityState.North) + squaref(velocityState.East) > 1.0f
+        ) {
+        vtolEmergencyFallback += dT;
+        if (vtolEmergencyFallback >= vtolPathFollowerSettings->FlyawayEmergencyFallbackTriggerTime) {
+            // after emergency timeout, trigger alarm - everything else is handled by callers
+            // (switch to emergency algorithm, switch to emergency waypoint in pathplanner, alarms, ...)
+            result = 0;
+        }
+    } else {
+        vtolEmergencyFallback = 0.0f;
+    }
+#endif // if 0
+
+    if (yaw_attitude) {
+        stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+        stabDesired.Yaw = yaw_direction;
+    } else {
+        stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
+        stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
+    }
+
+    // default thrust mode to cruise control
+    stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
+
+    if (mManualThrust) {
+        stabDesired.Thrust = manualControl.Thrust;
+    } else {
+        stabDesired.Thrust = controlDown.GetDownCommand();
+    }
+
+    StabilizationDesiredSet(&stabDesired);
+
+    return result;
+}
+
+/**
+ * Compute desired attitude for vtols - emergency fallback
+ */
+void VtolFlyController::UpdateDesiredAttitudeEmergencyFallback()
+{
+    VelocityDesiredData velocityDesired;
+    VelocityStateData velocityState;
+    StabilizationDesiredData stabDesired;
+
+    float courseError;
+    float courseCommand;
+
+    VelocityStateGet(&velocityState);
+    VelocityDesiredGet(&velocityDesired);
+
+    ManualControlCommandData manualControlData;
+    ManualControlCommandGet(&manualControlData);
+
+    courseError = RAD2DEG(atan2f(velocityDesired.East, velocityDesired.North) - atan2f(velocityState.East, velocityState.North));
+
+    if (courseError < -180.0f) {
+        courseError += 360.0f;
+    }
+    if (courseError > 180.0f) {
+        courseError -= 360.0f;
+    }
+
+
+    courseCommand   = (courseError * vtolPathFollowerSettings->EmergencyFallbackYawRate.kP);
+    stabDesired.Yaw = boundf(courseCommand, -vtolPathFollowerSettings->EmergencyFallbackYawRate.Max, vtolPathFollowerSettings->EmergencyFallbackYawRate.Max);
+
+    controlDown.UpdateVelocitySetpoint(velocityDesired.Down);
+    controlDown.UpdateVelocityState(velocityState.Down);
+    stabDesired.Thrust = controlDown.GetDownCommand();
+
+
+    stabDesired.Roll   = vtolPathFollowerSettings->EmergencyFallbackAttitude.Roll;
+    stabDesired.Pitch  = vtolPathFollowerSettings->EmergencyFallbackAttitude.Pitch;
+
+    if (vtolPathFollowerSettings->ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL) {
+        stabDesired.Thrust = manualControlData.Thrust;
+    }
+
+    stabDesired.StabilizationMode.Roll   = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.StabilizationMode.Pitch  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.StabilizationMode.Yaw    = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
+    stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
+    StabilizationDesiredSet(&stabDesired);
+}
+
+
+void VtolFlyController::UpdateAutoPilot()
+{
+    if (vtolPathFollowerSettings->ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL) {
+        mManualThrust = true;
+    }
+
+    uint8_t result = RunAutoPilot();
+
+    if (result) {
+        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
+    } else {
+        pathStatus->Status = PATHSTATUS_STATUS_CRITICAL;
+        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
+    }
+
+    PathStatusSet(pathStatus);
+
+    // If rtbl, detect arrival at the endpoint and then triggers a change
+    // to the pathDesired to initiate a Landing sequence. This is the simpliest approach. plans.c
+    // can't manage this.  And pathplanner whilst similar does not manage this as it is not a
+    // waypoint traversal and is not aware of flight modes other than path plan.
+    if ((uint8_t)pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_NEXTCOMMAND] == FLIGHTMODESETTINGS_RETURNTOBASENEXTCOMMAND_LAND) {
+        if (pathStatus->fractional_progress > RTB_LAND_FRACTIONAL_PROGRESS_START_CHECKS) {
+            if (fabsf(pathStatus->correction_direction_north) < RTB_LAND_NE_DISTANCE_REQUIRED_TO_START_LAND_SEQUENCE && fabsf(pathStatus->correction_direction_east) < RTB_LAND_NE_DISTANCE_REQUIRED_TO_START_LAND_SEQUENCE) {
+                plan_setup_land();
+            }
+        }
+    }
+}
+/**
+ * vtol autopilot
+ * use hover capable algorithm with unlimeted movement calculation. if that fails (flyaway situation due to compass failure)
+ * fall back to emergency fallback autopilot to keep minimum amount of flight control
+ */
+uint8_t VtolFlyController::RunAutoPilot()
+{
+    enum { RETURN_0 = 0, RETURN_1 = 1, RETURN_RESULT } returnmode;
+    enum { FOLLOWER_REGULAR, FOLLOWER_FALLBACK } followermode;
+    uint8_t result = 0;
+
+    // decide on behaviour based on settings and system state
+    if (vtolEmergencyFallbackSwitch) {
+        returnmode   = RETURN_0;
+        followermode = FOLLOWER_FALLBACK;
+    } else {
+        if (vtolPathFollowerSettings->FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_ALWAYS) {
+            returnmode   = RETURN_1;
+            followermode = FOLLOWER_FALLBACK;
+        } else {
+            returnmode   = RETURN_RESULT;
+            followermode = FOLLOWER_REGULAR;
+        }
+    }
+
+    switch (followermode) {
+    case FOLLOWER_REGULAR:
+    {
+        // horizontal position control PID loop works according to settings in regular mode, allowing integral terms
+        UpdateVelocityDesired();
+
+        // yaw behaviour is configurable in vtolpathfollower, select yaw control algorithm
+        bool yaw_attitude = true;
+        float yaw = 0.0f;
+
+        switch (vtolPathFollowerSettings->YawControl) {
+        case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MANUAL:
+            yaw_attitude = false;
+            break;
+        case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_TAILIN:
+            yaw = updateTailInBearing();
+            break;
+        case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MOVEMENTDIRECTION:
+            yaw = updateCourseBearing();
+            break;
+        case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_PATHDIRECTION:
+            yaw = updatePathBearing();
+            break;
+        case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_POI:
+            yaw = updatePOIBearing();
+            break;
+        }
+
+        result = UpdateStabilizationDesired(yaw_attitude, yaw);
+
+        if (!result) {
+            if (vtolPathFollowerSettings->FlyawayEmergencyFallback != VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DISABLED) {
+                // switch to emergency follower if follower indicates problems
+                vtolEmergencyFallbackSwitch = true;
+            }
+        }
+    }
+    break;
+    case FOLLOWER_FALLBACK:
+    {
+        // fallback loop only cares about intended horizontal flight direction, simplify control behaviour accordingly
+        controlNE.UpdatePositionalParameters(1.0f);
+        UpdateVelocityDesired();
+
+        // emergency follower has no return value
+        UpdateDesiredAttitudeEmergencyFallback();
+    }
+    break;
+    }
+
+    switch (returnmode) {
+    case RETURN_RESULT:
+        return result;
+
+    default:
+        // returns either 0 or 1 according to enum definition above
+        return returnmode;
+    }
+}
+
+
+/**
+ * Compute bearing of current takeoff location
+ */
+float VtolFlyController::updateTailInBearing()
+{
+    PositionStateData p;
+
+    PositionStateGet(&p);
+    TakeOffLocationData t;
+    TakeOffLocationGet(&t);
+    // atan2f always returns in between + and - 180 degrees
+    return RAD2DEG(atan2f(p.East - t.East, p.North - t.North));
+}
+
+
+/**
+ * Compute bearing of current movement direction
+ */
+float VtolFlyController::updateCourseBearing()
+{
+    VelocityStateData v;
+
+    VelocityStateGet(&v);
+    // atan2f always returns in between + and - 180 degrees
+    return RAD2DEG(atan2f(v.East, v.North));
+}
+
+
+/**
+ * Compute bearing of current path direction
+ */
+float VtolFlyController::updatePathBearing()
+{
+    PositionStateData positionState;
+
+    PositionStateGet(&positionState);
+
+    float cur[3] = { positionState.North,
+                     positionState.East,
+                     positionState.Down };
+    struct path_status progress;
+
+    path_progress(pathDesired, cur, &progress, true);
+
+    // atan2f always returns in between + and - 180 degrees
+    return RAD2DEG(atan2f(progress.path_vector[1], progress.path_vector[0]));
+}
+
+
+/**
+ * Compute bearing between current position and POI
+ */
+float VtolFlyController::updatePOIBearing()
+{
+    PoiLocationData poi;
+
+    PoiLocationGet(&poi);
+    PositionStateData positionState;
+    PositionStateGet(&positionState);
+
+    const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f;
+    float dLoc[3];
+    float yaw = 0;
+    /*float elevation = 0;*/
+
+    dLoc[0] = positionState.North - poi.North;
+    dLoc[1] = positionState.East - poi.East;
+    dLoc[2] = positionState.Down - poi.Down;
+
+    if (dLoc[1] < 0) {
+        yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) + 180.0f;
+    } else {
+        yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) - 180.0f;
+    }
+    ManualControlCommandData manualControlData;
+    ManualControlCommandGet(&manualControlData);
+
+    float pathAngle = 0;
+    if (manualControlData.Roll > DEADBAND_HIGH) {
+        pathAngle = -(manualControlData.Roll - DEADBAND_HIGH) * dT * 300.0f;
+    } else if (manualControlData.Roll < DEADBAND_LOW) {
+        pathAngle = -(manualControlData.Roll - DEADBAND_LOW) * dT * 300.0f;
+    }
+
+    return yaw + (pathAngle / 2.0f);
+}

flight/modules/PathFollower/vtollandcontroller.cpp

@@ -0,0 +1,275 @@
+/*
+ ******************************************************************************
+ *
+ * @file       vtollandcontroller.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Vtol landing controller loop
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+extern "C" {
+#include <openpilot.h>
+
+#include <callbackinfo.h>
+
+#include <math.h>
+#include <pid.h>
+#include <CoordinateConversions.h>
+#include <sin_lookup.h>
+#include <pathdesired.h>
+#include <paths.h>
+#include "plans.h"
+#include <sanitycheck.h>
+
+#include <accelstate.h>
+#include <vtolpathfollowersettings.h>
+#include <flightstatus.h>
+#include <flightmodesettings.h>
+#include <pathstatus.h>
+#include <positionstate.h>
+#include <velocitystate.h>
+#include <velocitydesired.h>
+#include <stabilizationdesired.h>
+#include <attitudestate.h>
+#include <takeofflocation.h>
+#include <manualcontrolcommand.h>
+#include <systemsettings.h>
+#include <stabilizationbank.h>
+#include <stabilizationdesired.h>
+#include <vtolselftuningstats.h>
+#include <pathsummary.h>
+}
+
+// C++ includes
+#include "vtollandcontroller.h"
+#include "pathfollowerfsm.h"
+#include "vtollandfsm.h"
+#include "pidcontroldown.h"
+
+// Private constants
+
+// pointer to a singleton instance
+VtolLandController *VtolLandController::p_inst = 0;
+
+VtolLandController::VtolLandController()
+    : fsm(NULL), vtolPathFollowerSettings(NULL), mActive(false)
+{}
+
+// Called when mode first engaged
+void VtolLandController::Activate(void)
+{
+    if (!mActive) {
+        mActive = true;
+        SettingsUpdated();
+        fsm->Activate();
+        controlDown.Activate();
+        controlNE.Activate();
+    }
+}
+
+uint8_t VtolLandController::IsActive(void)
+{
+    return mActive;
+}
+
+uint8_t VtolLandController::Mode(void)
+{
+    return PATHDESIRED_MODE_LAND;
+}
+
+// Objective updated in pathdesired, e.g. same flight mode but new target velocity
+void VtolLandController::ObjectiveUpdated(void)
+{
+    // Set the objective's target velocity
+    controlDown.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_DOWN]);
+    controlNE.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_NORTH],
+                                     pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_EAST]);
+    controlNE.UpdatePositionSetpoint(pathDesired->End.North, pathDesired->End.East);
+}
+void VtolLandController::Deactivate(void)
+{
+    if (mActive) {
+        mActive = false;
+        fsm->Inactive();
+        controlDown.Deactivate();
+        controlNE.Deactivate();
+    }
+}
+
+
+void VtolLandController::SettingsUpdated(void)
+{
+    const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f;
+
+    controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp,
+                               vtolPathFollowerSettings->HorizontalVelPID.Ki,
+                               vtolPathFollowerSettings->HorizontalVelPID.Kd,
+                               vtolPathFollowerSettings->HorizontalVelPID.Beta,
+                               dT,
+                               vtolPathFollowerSettings->HorizontalVelMax);
+
+
+    controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP);
+    controlNE.UpdateCommandParameters(-vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->VelocityFeedforward);
+
+    controlDown.UpdateParameters(vtolPathFollowerSettings->LandVerticalVelPID.Kp,
+                                 vtolPathFollowerSettings->LandVerticalVelPID.Ki,
+                                 vtolPathFollowerSettings->LandVerticalVelPID.Kd,
+                                 vtolPathFollowerSettings->LandVerticalVelPID.Beta,
+                                 dT,
+                                 vtolPathFollowerSettings->VerticalVelMax);
+
+    // The following is not currently used in the landing control.
+    controlDown.UpdatePositionalParameters(vtolPathFollowerSettings->VerticalPosP);
+
+    VtolSelfTuningStatsData vtolSelfTuningStats;
+    VtolSelfTuningStatsGet(&vtolSelfTuningStats);
+    controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + vtolPathFollowerSettings->ThrustLimits.Neutral);
+    // initialise limits on thrust but note the FSM can override.
+    controlDown.SetThrustLimits(vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max);
+    fsm->SettingsUpdated();
+}
+
+/**
+ * Initialise the module, called on startup
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t VtolLandController::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings)
+{
+    PIOS_Assert(ptr_vtolPathFollowerSettings);
+    if (fsm == 0) {
+        fsm = (PathFollowerFSM *)VtolLandFSM::instance();
+        VtolLandFSM::instance()->Initialize(ptr_vtolPathFollowerSettings, pathDesired, flightStatus);
+        vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
+        controlDown.Initialize(fsm);
+    }
+
+    return 0;
+}
+
+
+void VtolLandController::UpdateVelocityDesired()
+{
+    VelocityStateData velocityState;
+
+    VelocityStateGet(&velocityState);
+    VelocityDesiredData velocityDesired;
+
+    controlDown.UpdateVelocityState(velocityState.Down);
+    controlNE.UpdateVelocityState(velocityState.North, velocityState.East);
+
+    // Implement optional horizontal position hold.
+    if ((((uint8_t)pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_OPTIONS]) == PATHDESIRED_MODEPARAMETER_LAND_OPTION_HORIZONTAL_PH) ||
+        (flightStatus->ControlChain.PathPlanner == FLIGHTSTATUS_CONTROLCHAIN_TRUE)) {
+        // landing flight mode has stored original horizontal position in pathdesired
+        PositionStateData positionState;
+        PositionStateGet(&positionState);
+        controlNE.UpdatePositionState(positionState.North, positionState.East);
+        controlNE.ControlPosition();
+    }
+
+    velocityDesired.Down  = controlDown.GetVelocityDesired();
+    float north, east;
+    controlNE.GetVelocityDesired(&north, &east);
+    velocityDesired.North = north;
+    velocityDesired.East  = east;
+
+    // update pathstatus
+    pathStatus->error     = 0.0f;
+    pathStatus->fractional_progress = 0.0f;
+    if (fsm->GetCurrentState() == PFFSM_STATE_DISARMED) {
+        pathStatus->fractional_progress = 1.0f;
+    }
+    pathStatus->path_direction_north = velocityDesired.North;
+    pathStatus->path_direction_east  = velocityDesired.East;
+    pathStatus->path_direction_down  = velocityDesired.Down;
+
+    pathStatus->correction_direction_north = velocityDesired.North - velocityState.North;
+    pathStatus->correction_direction_east  = velocityDesired.East - velocityState.East;
+    pathStatus->correction_direction_down  = velocityDesired.Down - velocityState.Down;
+
+
+    VelocityDesiredSet(&velocityDesired);
+}
+
+int8_t VtolLandController::UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction)
+{
+    uint8_t result = 1;
+    StabilizationDesiredData stabDesired;
+    AttitudeStateData attitudeState;
+    StabilizationBankData stabSettings;
+    float northCommand;
+    float eastCommand;
+
+    StabilizationDesiredGet(&stabDesired);
+    AttitudeStateGet(&attitudeState);
+    StabilizationBankGet(&stabSettings);
+
+    controlNE.GetNECommand(&northCommand, &eastCommand);
+    stabDesired.Thrust = controlDown.GetDownCommand();
+
+    float angle_radians = DEG2RAD(attitudeState.Yaw);
+    float cos_angle     = cosf(angle_radians);
+    float sine_angle    = sinf(angle_radians);
+    float maxPitch = vtolPathFollowerSettings->MaxRollPitch;
+    stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.Pitch = boundf(-northCommand * cos_angle - eastCommand * sine_angle, -maxPitch, maxPitch);
+    stabDesired.StabilizationMode.Roll  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.Roll = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -maxPitch, maxPitch);
+
+    ManualControlCommandData manualControl;
+    ManualControlCommandGet(&manualControl);
+
+    if (yaw_attitude) {
+        stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+        stabDesired.Yaw = yaw_direction;
+    } else {
+        stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
+        stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
+    }
+
+    // default thrust mode to cruise control
+    stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
+
+    fsm->ConstrainStabiDesired(&stabDesired); // excludes thrust
+    StabilizationDesiredSet(&stabDesired);
+
+    return result;
+}
+
+void VtolLandController::UpdateAutoPilot()
+{
+    fsm->Update();
+
+    UpdateVelocityDesired();
+
+    // yaw behaviour is configurable in vtolpathfollower, select yaw control algorithm
+    bool yaw_attitude = false;
+    float yaw = 0.0f;
+
+    fsm->GetYaw(yaw_attitude, yaw);
+
+    int8_t result = UpdateStabilizationDesired(yaw_attitude, yaw);
+
+    if (!result) {
+        fsm->Abort();
+    }
+
+    PathStatusSet(pathStatus);
+}

flight/modules/PathFollower/vtollandfsm.cpp

@@ -0,0 +1,701 @@
+/*
+ ******************************************************************************
+ *
+ * @file       vtollandfsm.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      This landing state machine is a helper state machine to the
+ *             VtolLandController.
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+extern "C" {
+#include <openpilot.h>
+
+#include <callbackinfo.h>
+
+#include <math.h>
+#include <pid.h>
+#include <CoordinateConversions.h>
+#include <sin_lookup.h>
+#include <pathdesired.h>
+#include <paths.h>
+#include "plans.h"
+#include <sanitycheck.h>
+
+#include <homelocation.h>
+#include <accelstate.h>
+#include <vtolpathfollowersettings.h>
+#include <flightstatus.h>
+#include <flightmodesettings.h>
+#include <pathstatus.h>
+#include <positionstate.h>
+#include <velocitystate.h>
+#include <velocitydesired.h>
+#include <stabilizationdesired.h>
+#include <airspeedstate.h>
+#include <attitudestate.h>
+#include <takeofflocation.h>
+#include <poilocation.h>
+#include <manualcontrolcommand.h>
+#include <systemsettings.h>
+#include <stabilizationbank.h>
+#include <stabilizationdesired.h>
+#include <vtolselftuningstats.h>
+#include <statusvtolland.h>
+#include <pathsummary.h>
+}
+
+// C++ includes
+#include <vtollandfsm.h>
+
+
+// Private constants
+#define TIMER_COUNT_PER_SECOND            (1000 / vtolPathFollowerSettings->UpdatePeriod)
+#define MIN_LANDRATE                      0.1f
+#define MAX_LANDRATE                      0.6f
+#define LOW_ALT_DESCENT_REDUCTION_FACTOR  0.7f  // TODO Need to make the transition smooth
+#define LANDRATE_LOWLIMIT_FACTOR          0.5f
+#define LANDRATE_HILIMIT_FACTOR           1.5f
+#define TIMEOUT_INIT_ALTHOLD              (3 * TIMER_COUNT_PER_SECOND)
+#define TIMEOUT_WTG_FOR_DESCENTRATE       (10 * TIMER_COUNT_PER_SECOND)
+#define WTG_FOR_DESCENTRATE_COUNT_LIMIT   10
+#define TIMEOUT_AT_DESCENTRATE            10
+#define TIMEOUT_GROUNDEFFECT              (1 * TIMER_COUNT_PER_SECOND)
+#define TIMEOUT_THRUSTDOWN                (2 * TIMER_COUNT_PER_SECOND)
+#define LANDING_PID_SCALAR_P              2.0f
+#define LANDING_PID_SCALAR_I              10.0f
+#define LANDING_SLOWDOWN_HEIGHT           -5.0f
+#define BOUNCE_VELOCITY_TRIGGER_LIMIT     -0.3f
+#define BOUNCE_ACCELERATION_TRIGGER_LIMIT -9.0f // -6.0 found to be too sensitive
+#define BOUNCE_TRIGGER_COUNT              4
+#define GROUNDEFFECT_SLOWDOWN_FACTOR      0.3f
+#define GROUNDEFFECT_SLOWDOWN_COUNT       4
+
+VtolLandFSM::PathFollowerFSM_LandStateHandler_T VtolLandFSM::sLandStateTable[LAND_STATE_SIZE] = {
+    [LAND_STATE_INACTIVE]       =       { .setup = &VtolLandFSM::setup_inactive,             .run = 0                                      },
+    [LAND_STATE_INIT_ALTHOLD]   =       { .setup = &VtolLandFSM::setup_init_althold,         .run = &VtolLandFSM::run_init_althold         },
+    [LAND_STATE_WTG_FOR_DESCENTRATE] =  { .setup = &VtolLandFSM::setup_wtg_for_descentrate,  .run = &VtolLandFSM::run_wtg_for_descentrate  },
+    [LAND_STATE_AT_DESCENTRATE] =       { .setup = &VtolLandFSM::setup_at_descentrate,       .run = &VtolLandFSM::run_at_descentrate       },
+    [LAND_STATE_WTG_FOR_GROUNDEFFECT] = { .setup = &VtolLandFSM::setup_wtg_for_groundeffect, .run = &VtolLandFSM::run_wtg_for_groundeffect },
+    [LAND_STATE_GROUNDEFFECT]   =       { .setup = &VtolLandFSM::setup_groundeffect,         .run = &VtolLandFSM::run_groundeffect         },
+    [LAND_STATE_THRUSTDOWN]     =       { .setup = &VtolLandFSM::setup_thrustdown,           .run = &VtolLandFSM::run_thrustdown           },
+    [LAND_STATE_THRUSTOFF]      =       { .setup = &VtolLandFSM::setup_thrustoff,            .run = &VtolLandFSM::run_thrustoff            },
+    [LAND_STATE_DISARMED]       =       { .setup = &VtolLandFSM::setup_disarmed,             .run = &VtolLandFSM::run_disarmed             },
+    [LAND_STATE_ABORT] =                { .setup = &VtolLandFSM::setup_abort,                .run = &VtolLandFSM::run_abort                }
+};
+
+// pointer to a singleton instance
+VtolLandFSM *VtolLandFSM::p_inst = 0;
+
+
+VtolLandFSM::VtolLandFSM()
+    : mLandData(0), vtolPathFollowerSettings(0), pathDesired(0), flightStatus(0)
+{}
+
+// Private types
+
+// Private functions
+// Public API methods
+/**
+ * Initialise the module, called on startup
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t VtolLandFSM::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings,
+                                PathDesiredData *ptr_pathDesired,
+                                FlightStatusData *ptr_flightStatus)
+{
+    PIOS_Assert(ptr_vtolPathFollowerSettings);
+    PIOS_Assert(ptr_pathDesired);
+    PIOS_Assert(ptr_flightStatus);
+
+    if (mLandData == 0) {
+        mLandData = (VtolLandFSMData_T *)pios_malloc(sizeof(VtolLandFSMData_T));
+        PIOS_Assert(mLandData);
+    }
+    memset(mLandData, 0, sizeof(VtolLandFSMData_T));
+    vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
+    pathDesired  = ptr_pathDesired;
+    flightStatus = ptr_flightStatus;
+    initFSM();
+
+    return 0;
+}
+
+void VtolLandFSM::Inactive(void)
+{
+    memset(mLandData, 0, sizeof(VtolLandFSMData_T));
+    initFSM();
+}
+
+// Initialise the FSM
+void VtolLandFSM::initFSM(void)
+{
+    if (vtolPathFollowerSettings != 0) {
+        setState(STATUSVTOLLAND_STATE_INACTIVE, STATUSVTOLLAND_STATEEXITREASON_NONE);
+    } else {
+        mLandData->currentState = STATUSVTOLLAND_STATE_INACTIVE;
+    }
+}
+
+void VtolLandFSM::Activate()
+{
+    memset(mLandData, 0, sizeof(VtolLandFSMData_T));
+    mLandData->currentState   = STATUSVTOLLAND_STATE_INACTIVE;
+    mLandData->flLowAltitude  = false;
+    mLandData->flAltitudeHold = false;
+    mLandData->fsmLandStatus.averageDescentRate      = MIN_LANDRATE;
+    mLandData->fsmLandStatus.averageDescentThrust    = vtolPathFollowerSettings->ThrustLimits.Neutral;
+    mLandData->fsmLandStatus.calculatedNeutralThrust = vtolPathFollowerSettings->ThrustLimits.Neutral;
+    mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
+    mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
+    TakeOffLocationGet(&(mLandData->takeOffLocation));
+    mLandData->fsmLandStatus.AltitudeAtState[STATUSVTOLLAND_STATE_INACTIVE] = 0.0f;
+    assessAltitude();
+
+    if (pathDesired->Mode == PATHDESIRED_MODE_LAND) {
+#ifndef DEBUG_GROUNDIMPACT
+        setState(STATUSVTOLLAND_STATE_INITALTHOLD, STATUSVTOLLAND_STATEEXITREASON_NONE);
+#else
+        setState(STATUSVTOLLAND_STATE_WTGFORGROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_NONE);
+#endif
+    } else {
+        // move to error state and callback to position hold
+        setState(STATUSVTOLLAND_STATE_ABORT, STATUSVTOLLAND_STATEEXITREASON_NONE);
+    }
+}
+
+void VtolLandFSM::Abort(void)
+{
+    setState(STATUSVTOLLAND_STATE_ABORT, STATUSVTOLLAND_STATEEXITREASON_NONE);
+}
+
+PathFollowerFSMState_T VtolLandFSM::GetCurrentState(void)
+{
+    switch (mLandData->currentState) {
+    case STATUSVTOLLAND_STATE_INACTIVE:
+        return PFFSM_STATE_INACTIVE;
+
+        break;
+    case STATUSVTOLLAND_STATE_ABORT:
+        return PFFSM_STATE_ABORT;
+
+        break;
+    case STATUSVTOLLAND_STATE_DISARMED:
+        return PFFSM_STATE_DISARMED;
+
+        break;
+    default:
+        return PFFSM_STATE_ACTIVE;
+
+        break;
+    }
+}
+
+void VtolLandFSM::Update()
+{
+    runState();
+    if (GetCurrentState() != PFFSM_STATE_INACTIVE) {
+        runAlways();
+    }
+}
+
+int32_t VtolLandFSM::runState(void)
+{
+    uint8_t flTimeout = false;
+
+    mLandData->stateRunCount++;
+
+    if (mLandData->stateTimeoutCount > 0 && mLandData->stateRunCount > mLandData->stateTimeoutCount) {
+        flTimeout = true;
+    }
+
+    // If the current state has a static function, call it
+    if (sLandStateTable[mLandData->currentState].run) {
+        (this->*sLandStateTable[mLandData->currentState].run)(flTimeout);
+    }
+    return 0;
+}
+
+int32_t VtolLandFSM::runAlways(void)
+{
+    void assessAltitude(void);
+
+    return 0;
+}
+
+// PathFollower implements the PID scheme and has a objective
+// set by a PathDesired object.  Based on the mode, pathfollower
+// uses FSM's as helper functions that manage state and event detection.
+// PathFollower calls into FSM methods to alter its commands.
+
+void VtolLandFSM::BoundThrust(float &ulow, float &uhigh)
+{
+    ulow  = mLandData->boundThrustMin;
+    uhigh = mLandData->boundThrustMax;
+
+
+    if (mLandData->flConstrainThrust) {
+        uhigh = mLandData->thrustLimit;
+    }
+}
+
+void VtolLandFSM::ConstrainStabiDesired(StabilizationDesiredData *stabDesired)
+{
+    if (mLandData->flZeroStabiHorizontal && stabDesired) {
+        stabDesired->Pitch = 0.0f;
+        stabDesired->Roll  = 0.0f;
+        stabDesired->Yaw   = 0.0f;
+    }
+}
+
+void VtolLandFSM::CheckPidScaler(pid_scaler *local_scaler)
+{
+    if (mLandData->flLowAltitude) {
+        local_scaler->p = LANDING_PID_SCALAR_P;
+        local_scaler->i = LANDING_PID_SCALAR_I;
+    }
+}
+
+
+// Set the new state and perform setup for subsequent state run calls
+// This is called by state run functions on event detection that drive
+// state transitions.
+void VtolLandFSM::setState(StatusVtolLandStateOptions newState, StatusVtolLandStateExitReasonOptions reason)
+{
+    mLandData->fsmLandStatus.StateExitReason[mLandData->currentState] = reason;
+
+    if (mLandData->currentState == newState) {
+        return;
+    }
+    mLandData->currentState = newState;
+
+    if (newState != STATUSVTOLLAND_STATE_INACTIVE) {
+        PositionStateData positionState;
+        PositionStateGet(&positionState);
+        float takeOffDown = 0.0f;
+        if (mLandData->takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) {
+            takeOffDown = mLandData->takeOffLocation.Down;
+        }
+        mLandData->fsmLandStatus.AltitudeAtState[newState] = positionState.Down - takeOffDown;
+        assessAltitude();
+    }
+
+    // Restart state timer counter
+    mLandData->stateRunCount     = 0;
+
+    // Reset state timeout to disabled/zero
+    mLandData->stateTimeoutCount = 0;
+
+    if (sLandStateTable[mLandData->currentState].setup) {
+        (this->*sLandStateTable[mLandData->currentState].setup)();
+    }
+
+    updateVtolLandFSMStatus();
+}
+
+
+// Timeout utility function for use by state init implementations
+void VtolLandFSM::setStateTimeout(int32_t count)
+{
+    mLandData->stateTimeoutCount = count;
+}
+
+void VtolLandFSM::updateVtolLandFSMStatus()
+{
+    mLandData->fsmLandStatus.State = mLandData->currentState;
+    if (mLandData->flLowAltitude) {
+        mLandData->fsmLandStatus.AltitudeState = STATUSVTOLLAND_ALTITUDESTATE_LOW;
+    } else {
+        mLandData->fsmLandStatus.AltitudeState = STATUSVTOLLAND_ALTITUDESTATE_HIGH;
+    }
+    StatusVtolLandSet(&mLandData->fsmLandStatus);
+}
+
+
+float VtolLandFSM::BoundVelocityDown(float velocity_down)
+{
+    velocity_down = boundf(velocity_down, MIN_LANDRATE, MAX_LANDRATE);
+    if (mLandData->flLowAltitude) {
+        velocity_down *= LOW_ALT_DESCENT_REDUCTION_FACTOR;
+    }
+    mLandData->fsmLandStatus.targetDescentRate = velocity_down;
+
+    if (mLandData->flAltitudeHold) {
+        return 0.0f;
+    } else {
+        return velocity_down;
+    }
+}
+
+void VtolLandFSM::assessAltitude(void)
+{
+    float positionDown;
+
+    PositionStateDownGet(&positionDown);
+    float takeOffDown = 0.0f;
+    if (mLandData->takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) {
+        takeOffDown = mLandData->takeOffLocation.Down;
+    }
+    float positionDownRelativeToTakeoff = positionDown - takeOffDown;
+    if (positionDownRelativeToTakeoff < LANDING_SLOWDOWN_HEIGHT) {
+        mLandData->flLowAltitude = false;
+    } else {
+        mLandData->flLowAltitude = true;
+    }
+}
+
+
+// FSM Setup and Run method implementation
+
+// State: INACTIVE
+void VtolLandFSM::setup_inactive(void)
+{
+    // Re-initialise local variables
+    mLandData->flZeroStabiHorizontal = false;
+    mLandData->flConstrainThrust     = false;
+}
+
+// State: INIT ALTHOLD
+void VtolLandFSM::setup_init_althold(void)
+{
+    setStateTimeout(TIMEOUT_INIT_ALTHOLD);
+    // get target descent velocity
+    mLandData->flZeroStabiHorizontal = false;
+    mLandData->fsmLandStatus.targetDescentRate = BoundVelocityDown(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_DOWN]);
+    mLandData->flConstrainThrust     = false;
+    mLandData->flAltitudeHold = true;
+    mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
+    mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
+}
+
+void VtolLandFSM::run_init_althold(uint8_t flTimeout)
+{
+    if (flTimeout) {
+        mLandData->flAltitudeHold = false;
+        setState(STATUSVTOLLAND_STATE_WTGFORDESCENTRATE, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT);
+    }
+}
+
+
+// State: WAITING FOR DESCENT RATE
+void VtolLandFSM::setup_wtg_for_descentrate(void)
+{
+    setStateTimeout(TIMEOUT_WTG_FOR_DESCENTRATE);
+    // get target descent velocity
+    mLandData->flZeroStabiHorizontal = false;
+    mLandData->observationCount = 0;
+    mLandData->observation2Count     = 0;
+    mLandData->flConstrainThrust     = false;
+    mLandData->flAltitudeHold = false;
+    mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
+    mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
+}
+
+void VtolLandFSM::run_wtg_for_descentrate(uint8_t flTimeout)
+{
+    // Look at current actual thrust...are we already shutdown??
+    VelocityStateData velocityState;
+
+    VelocityStateGet(&velocityState);
+    StabilizationDesiredData stabDesired;
+    StabilizationDesiredGet(&stabDesired);
+
+    // We don't expect PID to get exactly the target descent rate, so have a lower
+    // water mark but need to see 5 observations to be confident that we have semi-stable
+    // descent achieved
+
+    // we need to see velocity down within a range of control before we proceed, without which we
+    // really don't have confidence to allow later states to run.
+    if (velocityState.Down > (LANDRATE_LOWLIMIT_FACTOR * mLandData->fsmLandStatus.targetDescentRate) &&
+        velocityState.Down < (LANDRATE_HILIMIT_FACTOR * mLandData->fsmLandStatus.targetDescentRate)) {
+        if (mLandData->observationCount++ > WTG_FOR_DESCENTRATE_COUNT_LIMIT) {
+            setState(STATUSVTOLLAND_STATE_ATDESCENTRATE, STATUSVTOLLAND_STATEEXITREASON_DESCENTRATEOK);
+            return;
+        }
+    }
+
+    if (flTimeout) {
+        setState(STATUSVTOLLAND_STATE_ABORT, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT);
+    }
+}
+
+
+// State: AT DESCENT RATE
+void VtolLandFSM::setup_at_descentrate(void)
+{
+    setStateTimeout(TIMEOUT_AT_DESCENTRATE);
+    mLandData->flZeroStabiHorizontal = false;
+    mLandData->observationCount  = 0;
+    mLandData->sum1 = 0.0f;
+    mLandData->sum2 = 0.0f;
+    mLandData->flConstrainThrust = false;
+    mLandData->fsmLandStatus.averageDescentRate = MIN_LANDRATE;
+    mLandData->fsmLandStatus.averageDescentThrust = vtolPathFollowerSettings->ThrustLimits.Neutral;
+    mLandData->boundThrustMin    = vtolPathFollowerSettings->ThrustLimits.Min;
+    mLandData->boundThrustMax    = vtolPathFollowerSettings->ThrustLimits.Max;
+}
+
+void VtolLandFSM::run_at_descentrate(uint8_t flTimeout)
+{
+    VelocityStateData velocityState;
+
+    VelocityStateGet(&velocityState);
+
+    StabilizationDesiredData stabDesired;
+    StabilizationDesiredGet(&stabDesired);
+
+    mLandData->sum1 += velocityState.Down;
+    mLandData->sum2 += stabDesired.Thrust;
+    mLandData->observationCount++;
+    if (flTimeout) {
+        mLandData->fsmLandStatus.averageDescentRate   = boundf((mLandData->sum1 / (float)(mLandData->observationCount)), 0.5f * MIN_LANDRATE, 1.5f * MAX_LANDRATE);
+        mLandData->fsmLandStatus.averageDescentThrust = boundf((mLandData->sum2 / (float)(mLandData->observationCount)), vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max);
+
+        // We need to calculate a neutral limit to use later to constrain upper thrust range during states where we are close to the ground
+        // As our battery gets flat, ThrustLimits.Neutral needs to constrain us too much and we get too fast a descent rate. We can
+        // detect this by the fact that the descent rate will exceed the target and the required thrust will exceed the neutral value
+        mLandData->fsmLandStatus.calculatedNeutralThrust = mLandData->fsmLandStatus.averageDescentRate / mLandData->fsmLandStatus.targetDescentRate * mLandData->fsmLandStatus.averageDescentThrust;
+        mLandData->fsmLandStatus.calculatedNeutralThrust = boundf(mLandData->fsmLandStatus.calculatedNeutralThrust, vtolPathFollowerSettings->ThrustLimits.Neutral, vtolPathFollowerSettings->ThrustLimits.Max);
+
+
+        setState(STATUSVTOLLAND_STATE_WTGFORGROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_DESCENTRATEOK);
+    }
+}
+
+
+// State: WAITING FOR GROUND EFFECT
+void VtolLandFSM::setup_wtg_for_groundeffect(void)
+{
+    // No timeout
+    mLandData->flZeroStabiHorizontal = false;
+    mLandData->observationCount = 0;
+    mLandData->observation2Count     = 0;
+    mLandData->sum1 = 0.0f;
+    mLandData->sum2 = 0.0f;
+    mLandData->flConstrainThrust     = false;
+    mLandData->fsmLandStatus.WtgForGroundEffect.BounceVelocity = 0.0f;
+    mLandData->fsmLandStatus.WtgForGroundEffect.BounceAccel    = 0.0f;
+    mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
+    mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
+}
+
+void VtolLandFSM::run_wtg_for_groundeffect(__attribute__((unused)) uint8_t flTimeout)
+{
+    // detect material downrating in thrust for 1 second.
+    VelocityStateData velocityState;
+
+    VelocityStateGet(&velocityState);
+    AccelStateData accelState;
+    AccelStateGet(&accelState);
+
+    // +ve 9.8 expected
+    float g_e;
+    HomeLocationg_eGet(&g_e);
+
+    StabilizationDesiredData stabDesired;
+    StabilizationDesiredGet(&stabDesired);
+
+    // detect bounce
+    uint8_t flBounce = (velocityState.Down < BOUNCE_VELOCITY_TRIGGER_LIMIT);
+    if (flBounce) {
+        mLandData->fsmLandStatus.WtgForGroundEffect.BounceVelocity = velocityState.Down;
+    } else {
+        mLandData->fsmLandStatus.WtgForGroundEffect.BounceVelocity = 0.0f;
+    }
+
+    // invert sign of accel to the standard convention of down is +ve and subtract the gravity to get
+    // a relative acceleration term.
+    float bounceAccel     = -accelState.z - g_e;
+    uint8_t flBounceAccel = (bounceAccel < BOUNCE_ACCELERATION_TRIGGER_LIMIT);
+    if (flBounceAccel) {
+        mLandData->fsmLandStatus.WtgForGroundEffect.BounceAccel = bounceAccel;
+    } else {
+        mLandData->fsmLandStatus.WtgForGroundEffect.BounceAccel = 0.0f;
+    }
+
+    if (flBounce) { // || flBounceAccel) { // accel trigger can occur due to vibration and is too sensitive
+        mLandData->observation2Count++;
+        if (mLandData->observation2Count > BOUNCE_TRIGGER_COUNT) {
+            setState(STATUSVTOLLAND_STATE_GROUNDEFFECT, (flBounce ? STATUSVTOLLAND_STATEEXITREASON_BOUNCEVELOCITY : STATUSVTOLLAND_STATEEXITREASON_BOUNCEACCEL));
+            return;
+        }
+    } else {
+        mLandData->observation2Count = 0;
+    }
+
+    // detect low descent rate
+    uint8_t flDescentRateLow = (velocityState.Down < (GROUNDEFFECT_SLOWDOWN_FACTOR * mLandData->fsmLandStatus.averageDescentRate));
+    if (flDescentRateLow) {
+        mLandData->boundThrustMax = mLandData->fsmLandStatus.calculatedNeutralThrust;
+        mLandData->observationCount++;
+        if (mLandData->observationCount > GROUNDEFFECT_SLOWDOWN_COUNT) {
+#ifndef DEBUG_GROUNDIMPACT
+            setState(STATUSVTOLLAND_STATE_GROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_LOWDESCENTRATE);
+#endif
+            return;
+        }
+    } else {
+        mLandData->observationCount = 0;
+    }
+
+    updateVtolLandFSMStatus();
+}
+
+// STATE: GROUNDEFFET
+void VtolLandFSM::setup_groundeffect(void)
+{
+    setStateTimeout(TIMEOUT_GROUNDEFFECT);
+    mLandData->flZeroStabiHorizontal     = false;
+    PositionStateData positionState;
+    PositionStateGet(&positionState);
+    mLandData->expectedLandPositionNorth = positionState.North;
+    mLandData->expectedLandPositionEast  = positionState.East;
+    mLandData->flConstrainThrust = false;
+
+    // now that we have ground effect limit max thrust to neutral
+    mLandData->boundThrustMin    = -0.1f;
+    mLandData->boundThrustMax    = mLandData->fsmLandStatus.calculatedNeutralThrust;
+}
+void VtolLandFSM::run_groundeffect(__attribute__((unused)) uint8_t flTimeout)
+{
+    StabilizationDesiredData stabDesired;
+
+    StabilizationDesiredGet(&stabDesired);
+    if (stabDesired.Thrust < 0.0f) {
+        setState(STATUSVTOLLAND_STATE_THRUSTOFF, STATUSVTOLLAND_STATEEXITREASON_ZEROTHRUST);
+        return;
+    }
+
+    // Stay in this state until we get a low altitude flag.
+    if (mLandData->flLowAltitude == false) {
+        // worst case scenario is that we land and the pid brings thrust down to zero.
+        return;
+    }
+
+    // detect broad sideways drift.  If for some reason we have a hard landing that the bounce detection misses, this will kick in
+    PositionStateData positionState;
+    PositionStateGet(&positionState);
+    float north_error   = mLandData->expectedLandPositionNorth - positionState.North;
+    float east_error    = mLandData->expectedLandPositionEast - positionState.East;
+    float positionError = sqrtf(north_error * north_error + east_error * east_error);
+    if (positionError > 1.5f) {
+        setState(STATUSVTOLLAND_STATE_THRUSTDOWN, STATUSVTOLLAND_STATEEXITREASON_POSITIONERROR);
+        return;
+    }
+
+    if (flTimeout) {
+        setState(STATUSVTOLLAND_STATE_THRUSTDOWN, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT);
+    }
+}
+
+// STATE: THRUSTDOWN
+void VtolLandFSM::setup_thrustdown(void)
+{
+    setStateTimeout(TIMEOUT_THRUSTDOWN);
+    mLandData->flZeroStabiHorizontal = true;
+    mLandData->flConstrainThrust     = true;
+    StabilizationDesiredData stabDesired;
+    StabilizationDesiredGet(&stabDesired);
+    mLandData->thrustLimit    = stabDesired.Thrust;
+    mLandData->sum1 = stabDesired.Thrust / (float)TIMEOUT_THRUSTDOWN;
+    mLandData->boundThrustMin = -0.1f;
+    mLandData->boundThrustMax = mLandData->fsmLandStatus.calculatedNeutralThrust;
+}
+
+void VtolLandFSM::run_thrustdown(__attribute__((unused)) uint8_t flTimeout)
+{
+    // reduce thrust setpoint step by step
+    mLandData->thrustLimit -= mLandData->sum1;
+
+    StabilizationDesiredData stabDesired;
+    StabilizationDesiredGet(&stabDesired);
+    if (stabDesired.Thrust < 0.0f || mLandData->thrustLimit < 0.0f) {
+        setState(STATUSVTOLLAND_STATE_THRUSTOFF, STATUSVTOLLAND_STATEEXITREASON_ZEROTHRUST);
+    }
+
+    if (flTimeout) {
+        setState(STATUSVTOLLAND_STATE_THRUSTOFF, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT);
+    }
+}
+
+// STATE: THRUSTOFF
+void VtolLandFSM::setup_thrustoff(void)
+{
+    mLandData->thrustLimit       = -1.0f;
+    mLandData->flConstrainThrust = true;
+    mLandData->boundThrustMin    = -0.1f;
+    mLandData->boundThrustMax    = 0.0f;
+}
+
+void VtolLandFSM::run_thrustoff(__attribute__((unused)) uint8_t flTimeout)
+{
+    setState(STATUSVTOLLAND_STATE_DISARMED, STATUSVTOLLAND_STATEEXITREASON_NONE);
+}
+
+// STATE: DISARMED
+void VtolLandFSM::setup_disarmed(void)
+{
+    // nothing to do
+    mLandData->flConstrainThrust     = false;
+    mLandData->flZeroStabiHorizontal = false;
+    mLandData->observationCount = 0;
+    mLandData->boundThrustMin   = -0.1f;
+    mLandData->boundThrustMax   = 0.0f;
+}
+
+void VtolLandFSM::run_disarmed(__attribute__((unused)) uint8_t flTimeout)
+{
+#ifdef DEBUG_GROUNDIMPACT
+    if (mLandData->observationCount++ > 100) {
+        setState(STATUSVTOLLAND_STATE_WTGFORGROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_NONE);
+    }
+#endif
+}
+
+void VtolLandFSM::fallback_to_hold(void)
+{
+    PositionStateData positionState;
+
+    PositionStateGet(&positionState);
+    pathDesired->End.North        = positionState.North;
+    pathDesired->End.East         = positionState.East;
+    pathDesired->End.Down         = positionState.Down;
+    pathDesired->Start.North      = positionState.North;
+    pathDesired->Start.East       = positionState.East;
+    pathDesired->Start.Down       = positionState.Down;
+    pathDesired->StartingVelocity = 0.0f;
+    pathDesired->EndingVelocity   = 0.0f;
+    pathDesired->Mode = PATHDESIRED_MODE_GOTOENDPOINT;
+
+    PathDesiredSet(pathDesired);
+}
+
+// abort repeatedly overwrites pathfollower's objective on a landing abort and
+// continues to do so until a flight mode change.
+void VtolLandFSM::setup_abort(void)
+{
+    mLandData->boundThrustMin        = vtolPathFollowerSettings->ThrustLimits.Min;
+    mLandData->boundThrustMax        = vtolPathFollowerSettings->ThrustLimits.Max;
+    mLandData->flConstrainThrust     = false;
+    mLandData->flZeroStabiHorizontal = false;
+    fallback_to_hold();
+}
+
+void VtolLandFSM::run_abort(__attribute__((unused)) uint8_t flTimeout)
+{}

flight/modules/PathFollower/vtolvelocitycontroller.cpp

@@ -0,0 +1,227 @@
+/*
+ ******************************************************************************
+ *
+ * @file       VtolVelocityController.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Velocity roam controller for vtols
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+extern "C" {
+#include <openpilot.h>
+
+#include <callbackinfo.h>
+
+#include <math.h>
+#include <pid.h>
+#include <CoordinateConversions.h>
+#include <sin_lookup.h>
+#include <pathdesired.h>
+#include <paths.h>
+#include "plans.h"
+#include <sanitycheck.h>
+
+#include <vtolpathfollowersettings.h>
+#include <flightmodesettings.h>
+#include <pathstatus.h>
+#include <positionstate.h>
+#include <velocitystate.h>
+#include <velocitydesired.h>
+#include <stabilizationdesired.h>
+#include <attitudestate.h>
+#include <flightstatus.h>
+#include <manualcontrolcommand.h>
+#include <systemsettings.h>
+#include <stabilizationbank.h>
+#include <stabilizationdesired.h>
+#include <pathsummary.h>
+}
+
+// C++ includes
+#include "vtolvelocitycontroller.h"
+#include "pidcontrolne.h"
+
+// Private constants
+
+// pointer to a singleton instance
+VtolVelocityController *VtolVelocityController::p_inst = 0;
+
+VtolVelocityController::VtolVelocityController()
+    : vtolPathFollowerSettings(0), mActive(false)
+{}
+
+// Called when mode first engaged
+void VtolVelocityController::Activate(void)
+{
+    if (!mActive) {
+        mActive = true;
+        SettingsUpdated();
+        controlNE.Activate();
+    }
+}
+
+uint8_t VtolVelocityController::IsActive(void)
+{
+    return mActive;
+}
+
+uint8_t VtolVelocityController::Mode(void)
+{
+    return PATHDESIRED_MODE_VELOCITY;
+}
+
+void VtolVelocityController::ObjectiveUpdated(void)
+{
+    controlNE.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_NORTH],
+                                     pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_EAST]);
+}
+
+void VtolVelocityController::Deactivate(void)
+{
+    if (mActive) {
+        mActive = false;
+        controlNE.Deactivate();
+    }
+}
+
+void VtolVelocityController::SettingsUpdated(void)
+{
+    const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f;
+
+    controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp,
+                               vtolPathFollowerSettings->HorizontalVelPID.Ki,
+                               vtolPathFollowerSettings->HorizontalVelPID.Kd,
+                               vtolPathFollowerSettings->HorizontalVelPID.Beta,
+                               dT,
+                               vtolPathFollowerSettings->HorizontalVelMax);
+
+
+    controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP);
+    controlNE.UpdateCommandParameters(-vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->VelocityFeedforward);
+}
+
+int32_t VtolVelocityController::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings)
+{
+    PIOS_Assert(ptr_vtolPathFollowerSettings);
+
+    vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
+    return 0;
+}
+
+
+void VtolVelocityController::UpdateVelocityDesired()
+{
+    VelocityStateData velocityState;
+
+    VelocityStateGet(&velocityState);
+    VelocityDesiredData velocityDesired;
+
+    controlNE.UpdateVelocityState(velocityState.North, velocityState.East);
+
+    velocityDesired.Down  = 0.0f;
+    float north, east;
+    controlNE.GetVelocityDesired(&north, &east);
+    velocityDesired.North = north;
+    velocityDesired.East  = east;
+
+    // update pathstatus
+    pathStatus->error     = 0.0f;
+    pathStatus->fractional_progress  = 0.0f;
+    pathStatus->path_direction_north = velocityDesired.North;
+    pathStatus->path_direction_east  = velocityDesired.East;
+    pathStatus->path_direction_down  = velocityDesired.Down;
+
+    pathStatus->correction_direction_north = velocityDesired.North - velocityState.North;
+    pathStatus->correction_direction_east  = velocityDesired.East - velocityState.East;
+    pathStatus->correction_direction_down  = 0.0f;
+
+    VelocityDesiredSet(&velocityDesired);
+}
+
+int8_t VtolVelocityController::UpdateStabilizationDesired(__attribute__((unused)) bool yaw_attitude, __attribute__((unused)) float yaw_direction)
+{
+    uint8_t result = 1;
+    StabilizationDesiredData stabDesired;
+    AttitudeStateData attitudeState;
+    StabilizationBankData stabSettings;
+    float northCommand;
+    float eastCommand;
+
+    StabilizationDesiredGet(&stabDesired);
+    AttitudeStateGet(&attitudeState);
+    StabilizationBankGet(&stabSettings);
+
+    controlNE.GetNECommand(&northCommand, &eastCommand);
+
+    float angle_radians = DEG2RAD(attitudeState.Yaw);
+    float cos_angle     = cosf(angle_radians);
+    float sine_angle    = sinf(angle_radians);
+    float maxPitch = vtolPathFollowerSettings->MaxRollPitch;
+    stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.Pitch = boundf(-northCommand * cos_angle - eastCommand * sine_angle, -maxPitch, maxPitch);
+    stabDesired.StabilizationMode.Roll  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.Roll = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -maxPitch, maxPitch);
+
+    ManualControlCommandData manualControl;
+    ManualControlCommandGet(&manualControl);
+
+    stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
+    stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
+
+    // default thrust mode to altvario
+    stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO;
+
+    StabilizationDesiredSet(&stabDesired);
+
+    return result;
+}
+
+void VtolVelocityController::UpdateAutoPilot()
+{
+    UpdateVelocityDesired();
+
+    bool yaw_attitude = false;
+    float yaw = 0.0f;
+
+    int8_t result     = UpdateStabilizationDesired(yaw_attitude, yaw);
+
+    if (!result) {
+        fallback_to_hold();
+    }
+
+    PathStatusSet(pathStatus);
+}
+
+void VtolVelocityController::fallback_to_hold(void)
+{
+    PositionStateData positionState;
+
+    PositionStateGet(&positionState);
+    pathDesired->End.North        = positionState.North;
+    pathDesired->End.East         = positionState.East;
+    pathDesired->End.Down         = positionState.Down;
+    pathDesired->Start.North      = positionState.North;
+    pathDesired->Start.East       = positionState.East;
+    pathDesired->Start.Down       = positionState.Down;
+    pathDesired->StartingVelocity = 0.0f;
+    pathDesired->EndingVelocity   = 0.0f;
+    pathDesired->Mode = PATHDESIRED_MODE_GOTOENDPOINT;
+
+    PathDesiredSet(pathDesired);
+}

flight/modules/PathPlanner/pathplanner.c

@@ -43,8 +43,14 @@
 #include "waypoint.h"
 #include "waypointactive.h"
 #include "flightmodesettings.h"
+#include <systemsettings.h>
 #include "paths.h"
 #include "plans.h"
+#include <sanitycheck.h>
+#include <vtolpathfollowersettings.h>
+#include <statusvtolautotakeoff.h>
+#include <statusvtolland.h>
+#include <manualcontrolcommand.h>
 
 // Private constants
 #define STACK_SIZE_BYTES            1024
@@ -73,6 +79,10 @@ static uint8_t conditionAboveSpeed();
 static uint8_t conditionPointingTowardsNext();
 static uint8_t conditionPythonScript();
 static uint8_t conditionImmediate();
+static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
+static void planner_setup_pathdesired_land(PathDesiredData *pathDesired);
+static void planner_setup_pathdesired_takeoff(PathDesiredData *pathDesired);
+static void planner_setup_pathdesired(PathDesiredData *pathDesired, bool overwrite_start_position);
 
 
 // Private variables
@@ -82,7 +92,10 @@ static WaypointActiveData waypointActive;
 static WaypointData waypoint;
 static PathActionData pathAction;
 static bool pathplanner_active = false;
+static FrameType_t frameType;
+static bool mode3D;
 
+extern FrameType_t GetCurrentFrameType();
 
 /**
  * Module initialization
@@ -95,6 +108,9 @@ int32_t PathPlannerStart()
     WaypointActiveConnectCallback(commandUpdated);
     PathActionConnectCallback(commandUpdated);
     PathStatusConnectCallback(statusUpdated);
+    SettingsUpdatedCb(NULL);
+    SystemSettingsConnectCallback(&SettingsUpdatedCb);
+    VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
 
     // Start main task callback
     PIOS_CALLBACKSCHEDULER_Dispatch(pathPlannerHandle);
@@ -116,6 +132,8 @@ int32_t PathPlannerInitialize()
     VelocityStateInitialize();
     WaypointInitialize();
     WaypointActiveInitialize();
+    StatusVtolAutoTakeoffInitialize();
+    StatusVtolLandInitialize();
 
     pathPlannerHandle = PIOS_CALLBACKSCHEDULER_Create(&pathPlannerTask, CALLBACK_PRIORITY_REGULAR, TASK_PRIORITY, CALLBACKINFO_RUNNING_PATHPLANNER0, STACK_SIZE_BYTES);
     pathDesiredUpdaterHandle = PIOS_CALLBACKSCHEDULER_Create(&updatePathDesired, CALLBACK_PRIORITY_CRITICAL, TASK_PRIORITY, CALLBACKINFO_RUNNING_PATHPLANNER1, STACK_SIZE_BYTES);
@@ -125,6 +143,40 @@ int32_t PathPlannerInitialize()
 
 MODULE_INITCALL(PathPlannerInitialize, PathPlannerStart);
 
+
+static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+{
+    VtolPathFollowerSettingsTreatCustomCraftAsOptions TreatCustomCraftAs;
+
+    VtolPathFollowerSettingsTreatCustomCraftAsGet(&TreatCustomCraftAs);
+
+    frameType = GetCurrentFrameType();
+
+    if (frameType == FRAME_TYPE_CUSTOM) {
+        switch (TreatCustomCraftAs) {
+        case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING:
+            frameType = FRAME_TYPE_FIXED_WING;
+            break;
+        case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL:
+            frameType = FRAME_TYPE_MULTIROTOR;
+            break;
+        case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_GROUND:
+            frameType = FRAME_TYPE_GROUND;
+            break;
+        }
+    }
+
+    switch (frameType) {
+    case FRAME_TYPE_GROUND:
+        mode3D = false;
+        break;
+    default:
+        mode3D = true;
+    }
+}
+
+#define AUTOTAKEOFF_THROTTLE_LIMIT_TO_ALLOW_TAKEOFF_START 0.3f
+
 /**
  * Module task
  */
@@ -160,27 +212,33 @@ static void pathPlannerTask()
 
     static uint8_t failsafeRTHset = 0;
     if (!validPathPlan) {
-        // At this point the craft is in PathPlanner mode, so pilot has no manual control capability.
-        // Failsafe: behave as if in return to base mode
-        // what to do in this case is debatable, it might be better to just
-        // make a forced disarming but rth has a higher chance of survival when
-        // in flight.
         pathplanner_active = false;
 
         if (!failsafeRTHset) {
             failsafeRTHset = 1;
-            // copy pasta: same calculation as in manualcontrol, set return to home coordinates
             plan_setup_positionHold();
         }
         AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_CRITICAL);
 
         return;
     }
+
     failsafeRTHset = 0;
     AlarmsClear(SYSTEMALARMS_ALARM_PATHPLAN);
 
     WaypointActiveGet(&waypointActive);
 
+    // with the introduction of takeoff, we allow for arming
+    // whilst in pathplanner mode.  Previously it was just an assumption that
+    // a user never armed in pathplanner mode.  This check allows a user to select
+    // pathplanner, to upload waypoints, and then arm in pathplanner.
+    if (!flightStatus.Armed) {
+        return;
+    }
+
+
+    // the transition from pathplanner to another flightmode back to pathplanner
+    // triggers a reset back to 0 index in the waypoint list
     if (pathplanner_active == false) {
         pathplanner_active   = true;
 
@@ -207,6 +265,22 @@ static void pathPlannerTask()
         return;
     }
 
+    // check start conditions
+    // autotakeoff requires midpoint thrust if we are in a pending takeoff situation
+    if (pathAction.Mode == PATHACTION_MODE_AUTOTAKEOFF) {
+        pathAction.EndCondition = PATHACTION_ENDCONDITION_LEGREMAINING;
+        if ((uint8_t)pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE] == STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE) {
+            ManualControlCommandData cmd;
+            ManualControlCommandGet(&cmd);
+            if (cmd.Throttle > AUTOTAKEOFF_THROTTLE_LIMIT_TO_ALLOW_TAKEOFF_START) {
+                pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE] = (float)STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE;
+                PathDesiredSet(&pathDesired);
+            }
+            return;
+        }
+    }
+
+
     // check if condition has been met
     endCondition = pathConditionCheck();
     // decide what to do
@@ -245,6 +319,40 @@ static void pathPlannerTask()
     }
 }
 
+// callback function when waypoints changed in any way, update pathDesired
+void updatePathDesired()
+{
+    // only ever touch pathDesired if pathplanner is enabled
+    if (!pathplanner_active) {
+        return;
+    }
+
+    // find out current waypoint
+    WaypointActiveGet(&waypointActive);
+    WaypointInstGet(waypointActive.Index, &waypoint);
+
+    // Capture if current mode is takeoff
+    bool autotakeoff = (pathAction.Mode == PATHACTION_MODE_AUTOTAKEOFF);
+
+    PathActionInstGet(waypoint.Action, &pathAction);
+
+    PathDesiredData pathDesired;
+    switch (pathAction.Mode) {
+    case PATHACTION_MODE_AUTOTAKEOFF:
+        planner_setup_pathdesired_takeoff(&pathDesired);
+        break;
+    case PATHACTION_MODE_LAND:
+        planner_setup_pathdesired_land(&pathDesired);
+        break;
+    default:
+        planner_setup_pathdesired(&pathDesired, autotakeoff);
+        break;
+    }
+
+    PathDesiredSet(&pathDesired);
+}
+
+
 // safety checks for path plan integrity
 static uint8_t checkPathPlan()
 {
@@ -329,35 +437,22 @@ void statusUpdated(__attribute__((unused)) UAVObjEvent *ev)
     PIOS_CALLBACKSCHEDULER_Dispatch(pathPlannerHandle);
 }
 
-
+// Standard setup of a pathDesired command from the waypoint path plan
-// callback function when waypoints changed in any way, update pathDesired
+static void planner_setup_pathdesired(PathDesiredData *pathDesired, bool overwrite_start_position)
-void updatePathDesired()
 {
-    // only ever touch pathDesired if pathplanner is enabled
+    pathDesired->End.North = waypoint.Position.North;
-    if (!pathplanner_active) {
+    pathDesired->End.East  = waypoint.Position.East;
-        return;
+    pathDesired->End.Down  = waypoint.Position.Down;
-    }
+    pathDesired->EndingVelocity    = waypoint.Velocity;
+    pathDesired->Mode = pathAction.Mode;
+    pathDesired->ModeParameters[0] = pathAction.ModeParameters[0];
+    pathDesired->ModeParameters[1] = pathAction.ModeParameters[1];
+    pathDesired->ModeParameters[2] = pathAction.ModeParameters[2];
+    pathDesired->ModeParameters[3] = pathAction.ModeParameters[3];
+    pathDesired->UID = waypointActive.Index;
 
-    PathDesiredData pathDesired;
 
-    // find out current waypoint
+    if (waypointActive.Index == 0 || overwrite_start_position) {
-    WaypointActiveGet(&waypointActive);
-
-    WaypointInstGet(waypointActive.Index, &waypoint);
-    PathActionInstGet(waypoint.Action, &pathAction);
-
-    pathDesired.End.North = waypoint.Position.North;
-    pathDesired.End.East  = waypoint.Position.East;
-    pathDesired.End.Down  = waypoint.Position.Down;
-    pathDesired.EndingVelocity    = waypoint.Velocity;
-    pathDesired.Mode = pathAction.Mode;
-    pathDesired.ModeParameters[0] = pathAction.ModeParameters[0];
-    pathDesired.ModeParameters[1] = pathAction.ModeParameters[1];
-    pathDesired.ModeParameters[2] = pathAction.ModeParameters[2];
-    pathDesired.ModeParameters[3] = pathAction.ModeParameters[3];
-    pathDesired.UID = waypointActive.Index;
-
-    if (waypointActive.Index == 0) {
         PositionStateData positionState;
         PositionStateGet(&positionState);
         // First waypoint has itself as start point (used to be home position but that proved dangerous when looping)
@@ -365,23 +460,90 @@ void updatePathDesired()
         /*pathDesired.Start[PATHDESIRED_START_NORTH] =  waypoint.Position[WAYPOINT_POSITION_NORTH];
            pathDesired.Start[PATHDESIRED_START_EAST] =  waypoint.Position[WAYPOINT_POSITION_EAST];
            pathDesired.Start[PATHDESIRED_START_DOWN] =  waypoint.Position[WAYPOINT_POSITION_DOWN];*/
-        pathDesired.Start.North = positionState.North;
+        // note takeoff relies on the start being the current location as it merely ascends and using
-        pathDesired.Start.East  = positionState.East;
+        // the start as assumption current NE location
-        pathDesired.Start.Down  = positionState.Down;
+        pathDesired->Start.North = positionState.North;
-        pathDesired.StartingVelocity = pathDesired.EndingVelocity;
+        pathDesired->Start.East  = positionState.East;
+        pathDesired->Start.Down  = positionState.Down;
+        pathDesired->StartingVelocity = pathDesired->EndingVelocity;
     } else {
         // Get previous waypoint as start point
         WaypointData waypointPrev;
         WaypointInstGet(waypointActive.Index - 1, &waypointPrev);
 
-        pathDesired.Start.North = waypointPrev.Position.North;
+        pathDesired->Start.North = waypointPrev.Position.North;
-        pathDesired.Start.East  = waypointPrev.Position.East;
+        pathDesired->Start.East  = waypointPrev.Position.East;
-        pathDesired.Start.Down  = waypointPrev.Position.Down;
+        pathDesired->Start.Down  = waypointPrev.Position.Down;
-        pathDesired.StartingVelocity = waypointPrev.Velocity;
+        pathDesired->StartingVelocity = waypointPrev.Velocity;
     }
-    PathDesiredSet(&pathDesired);
 }
 
+#define AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN 2.0f
+#define AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX 50.0f
+static void planner_setup_pathdesired_takeoff(PathDesiredData *pathDesired)
+{
+    PositionStateData positionState;
+
+    PositionStateGet(&positionState);
+    float velocity_down;
+    float autotakeoff_height;
+    FlightModeSettingsAutoTakeOffVelocityGet(&velocity_down);
+    FlightModeSettingsAutoTakeOffHeightGet(&autotakeoff_height);
+    autotakeoff_height = fabsf(autotakeoff_height);
+    if (autotakeoff_height < AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN) {
+        autotakeoff_height = AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN;
+    } else if (autotakeoff_height > AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX) {
+        autotakeoff_height = AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX;
+    }
+
+
+    pathDesired->Start.North = positionState.North;
+    pathDesired->Start.East  = positionState.East;
+    pathDesired->Start.Down  = positionState.Down;
+    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_NORTH] = 0.0f;
+    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_EAST]  = 0.0f;
+    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_DOWN]  = -velocity_down;
+    // initially halt takeoff.
+    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE] = (float)STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE;
+
+    pathDesired->End.North = positionState.North;
+    pathDesired->End.East  = positionState.East;
+    pathDesired->End.Down  = positionState.Down - autotakeoff_height;
+
+    pathDesired->StartingVelocity = 0.0f;
+    pathDesired->EndingVelocity   = 0.0f;
+    pathDesired->Mode = PATHDESIRED_MODE_AUTOTAKEOFF;
+
+    pathDesired->UID  = waypointActive.Index;
+}
+
+static void planner_setup_pathdesired_land(PathDesiredData *pathDesired)
+{
+    PositionStateData positionState;
+
+    PositionStateGet(&positionState);
+    float velocity_down;
+
+    FlightModeSettingsLandingVelocityGet(&velocity_down);
+
+    pathDesired->Start.North = positionState.North;
+    pathDesired->Start.East  = positionState.East;
+    pathDesired->Start.Down  = positionState.Down;
+    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_NORTH] = 0.0f;
+    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_EAST]  = 0.0f;
+    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_DOWN]  = velocity_down;
+
+    pathDesired->End.North = positionState.North;
+    pathDesired->End.East  = positionState.East;
+    pathDesired->End.Down  = positionState.Down;
+
+    pathDesired->StartingVelocity = 0.0f;
+    pathDesired->EndingVelocity   = 0.0f;
+    pathDesired->Mode = PATHDESIRED_MODE_LAND;
+    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_OPTIONS] = (float)PATHDESIRED_MODEPARAMETER_LAND_OPTION_HORIZONTAL_PH;
+}
+
+
 // helper function to go to a specific waypoint
 static void setWaypoint(uint16_t num)
 {
@@ -515,18 +677,11 @@ static uint8_t conditionDistanceToTarget()
  */
 static uint8_t conditionLegRemaining()
 {
-    PathDesiredData pathDesired;
+    PathStatusData pathStatus;
-    PositionStateData positionState;
 
-    PathDesiredGet(&pathDesired);
+    PathStatusGet(&pathStatus);
-    PositionStateGet(&positionState);
 
-    float cur[3] = { positionState.North, positionState.East, positionState.Down };
+    if (pathStatus.fractional_progress >= (1.0f - pathAction.ConditionParameters[0])) {
-    struct path_status progress;
-
-    path_progress(&pathDesired,
-                  cur, &progress);
-    if (progress.fractional_progress >= 1.0f - pathAction.ConditionParameters[0]) {
         return true;
     }
     return false;
@@ -549,7 +704,7 @@ static uint8_t conditionBelowError()
     struct path_status progress;
 
     path_progress(&pathDesired,
-                  cur, &progress);
+                  cur, &progress, mode3D);
     if (progress.error <= pathAction.ConditionParameters[0]) {
         return true;
     }

flight/modules/RadioComBridge/RadioComBridge.c

@@ -108,8 +108,8 @@ static void radioRxTask(void *parameters);
 static void PPMInputTask(void *parameters);
 static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length);
 static int32_t RadioSendHandler(uint8_t *buf, int32_t length);
-static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte);
+static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t *rxbuffer, uint8_t count);
-static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte);
+static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t *rxbuffer, uint8_t count);
 static void objectPersistenceUpdatedCb(UAVObjEvent *objEv);
 static void registerObject(UAVObjHandle obj);
 
@@ -403,14 +403,12 @@ static void radioRxTask(__attribute__((unused)) void *parameters)
         PIOS_WDG_UpdateFlag(PIOS_WDG_RADIORX);
 #endif
         if (PIOS_COM_RADIO) {
-            uint8_t serial_data[1];
+            uint8_t serial_data[16];
             uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(PIOS_COM_RADIO, serial_data, sizeof(serial_data), MAX_PORT_DELAY);
             if (bytes_to_process > 0) {
                 if (data->parseUAVTalk) {
                     // Pass the data through the UAVTalk parser.
-                    for (uint8_t i = 0; i < bytes_to_process; i++) {
+                    ProcessRadioStream(data->radioUAVTalkCon, data->telemUAVTalkCon, serial_data, bytes_to_process);
-                        ProcessRadioStream(data->radioUAVTalkCon, data->telemUAVTalkCon, serial_data[i]);
-                    }
                 } else if (PIOS_COM_TELEMETRY) {
                     // Send the data straight to the telemetry port.
                     // Following call can fail with -2 error code (buffer full) or -3 error code (could not acquire send mutex)
@@ -448,12 +446,10 @@ static void telemetryRxTask(__attribute__((unused)) void *parameters)
         }
 #endif /* PIOS_INCLUDE_USB */
         if (inputPort) {
-            uint8_t serial_data[1];
+            uint8_t serial_data[16];
             uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(inputPort, serial_data, sizeof(serial_data), MAX_PORT_DELAY);
             if (bytes_to_process > 0) {
-                for (uint8_t i = 0; i < bytes_to_process; i++) {
+                ProcessTelemetryStream(data->telemUAVTalkCon, data->radioUAVTalkCon, serial_data, bytes_to_process);
-                    ProcessTelemetryStream(data->telemUAVTalkCon, data->radioUAVTalkCon, serial_data[i]);
-                }
             }
         } else {
             vTaskDelay(5);
@@ -597,42 +593,45 @@ static int32_t RadioSendHandler(uint8_t *buf, int32_t length)
  * @param[in] outConnectionHandle  The UAVTalk connection handle on the radio port.
  * @param[in] rxbyte  The received byte.
  */
-static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte)
+static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t *rxbuffer, uint8_t length)
 {
-    // Keep reading until we receive a completed packet.
+    uint8_t position = 0;
-    UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(inConnectionHandle, rxbyte);
 
-    if (state == UAVTALK_STATE_COMPLETE) {
+    // Keep reading until we receive a completed packet.
-        // We only want to unpack certain telemetry objects
+    while (position < length) {
-        uint32_t objId = UAVTalkGetPacketObjId(inConnectionHandle);
+        UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(inConnectionHandle, rxbuffer, length, &position);
-        switch (objId) {
+        if (state == UAVTALK_STATE_COMPLETE) {
-        case OPLINKSTATUS_OBJID:
+            // We only want to unpack certain telemetry objects
-        case OPLINKSETTINGS_OBJID:
+            uint32_t objId = UAVTalkGetPacketObjId(inConnectionHandle);
-        case OPLINKRECEIVER_OBJID:
+            switch (objId) {
-        case MetaObjectId(OPLINKSTATUS_OBJID):
+            case OPLINKSTATUS_OBJID:
-        case MetaObjectId(OPLINKSETTINGS_OBJID):
+            case OPLINKSETTINGS_OBJID:
-        case MetaObjectId(OPLINKRECEIVER_OBJID):
+            case OPLINKRECEIVER_OBJID:
-            UAVTalkReceiveObject(inConnectionHandle);
+            case MetaObjectId(OPLINKSTATUS_OBJID):
-            break;
+            case MetaObjectId(OPLINKSETTINGS_OBJID):
-        case OBJECTPERSISTENCE_OBJID:
+            case MetaObjectId(OPLINKRECEIVER_OBJID):
-        case MetaObjectId(OBJECTPERSISTENCE_OBJID):
+                UAVTalkReceiveObject(inConnectionHandle);
-            // receive object locally
+                break;
-            // some objects will send back a response to telemetry
+            case OBJECTPERSISTENCE_OBJID:
-            // FIXME:
+            case MetaObjectId(OBJECTPERSISTENCE_OBJID):
-            // OPLM will ack or nack all objects requests and acked object sends
+                // receive object locally
-            // Receiver will probably also ack / nack the same messages
+                // some objects will send back a response to telemetry
-            // This has some consequences like :
+                // FIXME:
-            // Second ack/nack will not match an open transaction or will apply to wrong transaction
+                // OPLM will ack or nack all objects requests and acked object sends
-            // Question : how does GCS handle receiving the same object twice
+                // Receiver will probably also ack / nack the same messages
-            // The OBJECTPERSISTENCE logic can be broken too if for example OPLM nacks and then REVO acks...
+                // This has some consequences like :
-            UAVTalkReceiveObject(inConnectionHandle);
+                // Second ack/nack will not match an open transaction or will apply to wrong transaction
-            // relay packet to remote modem
+                // Question : how does GCS handle receiving the same object twice
-            UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
+                // The OBJECTPERSISTENCE logic can be broken too if for example OPLM nacks and then REVO acks...
-            break;
+                UAVTalkReceiveObject(inConnectionHandle);
-        default:
+                // relay packet to remote modem
-            // all other packets are relayed to the remote modem
+                UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
-            UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
+                break;
-            break;
+            default:
+                // all other packets are relayed to the remote modem
+                UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
+                break;
+            }
         }
     }
 }
@@ -642,39 +641,43 @@ static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalk
  *
  * @param[in] inConnectionHandle  The UAVTalk connection handle on the radio port.
  * @param[in] outConnectionHandle  The UAVTalk connection handle on the telemetry port.
- * @param[in] rxbyte  The received byte.
+ * @param[in] rxbuffer  The received buffer.
+ * @param[in] length  buffer length
  */
-static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte)
+static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t *rxbuffer, uint8_t length)
 {
-    // Keep reading until we receive a completed packet.
+    uint8_t position = 0;
-    UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(inConnectionHandle, rxbyte);
 
-    if (state == UAVTALK_STATE_COMPLETE) {
+    // Keep reading until we receive a completed packet.
-        // We only want to unpack certain objects from the remote modem
+    while (position < length) {
-        // Similarly we only want to relay certain objects to the telemetry port
+        UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(inConnectionHandle, rxbuffer, length, &position);
-        uint32_t objId = UAVTalkGetPacketObjId(inConnectionHandle);
+        if (state == UAVTALK_STATE_COMPLETE) {
-        switch (objId) {
+            // We only want to unpack certain objects from the remote modem
-        case OPLINKSTATUS_OBJID:
+            // Similarly we only want to relay certain objects to the telemetry port
-        case OPLINKSETTINGS_OBJID:
+            uint32_t objId = UAVTalkGetPacketObjId(inConnectionHandle);
-        case MetaObjectId(OPLINKSTATUS_OBJID):
+            switch (objId) {
-        case MetaObjectId(OPLINKSETTINGS_OBJID):
+            case OPLINKSTATUS_OBJID:
-            // Ignore object...
+            case OPLINKSETTINGS_OBJID:
-            // These objects are shadowed by the modem and are not transmitted to the telemetry port
+            case MetaObjectId(OPLINKSTATUS_OBJID):
-            // - OPLINKSTATUS_OBJID : ground station will receive the OPLM link status instead
+            case MetaObjectId(OPLINKSETTINGS_OBJID):
-            // - OPLINKSETTINGS_OBJID : ground station will read and write the OPLM settings instead
+                // Ignore object...
-            break;
+                // These objects are shadowed by the modem and are not transmitted to the telemetry port
-        case OPLINKRECEIVER_OBJID:
+                // - OPLINKSTATUS_OBJID : ground station will receive the OPLM link status instead
-        case MetaObjectId(OPLINKRECEIVER_OBJID):
+                // - OPLINKSETTINGS_OBJID : ground station will read and write the OPLM settings instead
-            // Receive object locally
+                break;
-            // These objects are received by the modem and are not transmitted to the telemetry port
+            case OPLINKRECEIVER_OBJID:
-            // - OPLINKRECEIVER_OBJID : not sure why
+            case MetaObjectId(OPLINKRECEIVER_OBJID):
-            // some objects will send back a response to the remote modem
+                // Receive object locally
-            UAVTalkReceiveObject(inConnectionHandle);
+                // These objects are received by the modem and are not transmitted to the telemetry port
-            break;
+                // - OPLINKRECEIVER_OBJID : not sure why
-        default:
+                // some objects will send back a response to the remote modem
-            // all other packets are relayed to the telemetry port
+                UAVTalkReceiveObject(inConnectionHandle);
-            UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
+                break;
-            break;
+            default:
+                // all other packets are relayed to the telemetry port
+                UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
+                break;
+            }
         }
     }
 }

flight/modules/Receiver/receiver.c

@@ -37,14 +37,17 @@
 #include <manualcontrolsettings.h>
 #include <manualcontrolcommand.h>
 #include <receiveractivity.h>
+#include <receiverstatus.h>
 #include <flightstatus.h>
 #include <flighttelemetrystats.h>
 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
 #include <stabilizationsettings.h>
+#include <vtolpathfollowersettings.h>
 #endif
 #include <flightmodesettings.h>
 #include <systemsettings.h>
 #include <taskinfo.h>
+#include <sanitycheck.h>
 
 
 #if defined(PIOS_INCLUDE_USB_RCTX)
@@ -72,6 +75,7 @@
 // Private variables
 static xTaskHandle taskHandle;
 static portTickType lastSysTime;
+static FrameType_t frameType = FRAME_TYPE_MULTIROTOR;
 
 #ifdef USE_INPUT_LPF
 static portTickType lastSysTimeLPF;
@@ -84,6 +88,7 @@ static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutr
 static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time);
 static bool validInputRange(int16_t min, int16_t max, uint16_t value);
 static void applyDeadband(float *value, float deadband);
+static void SettingsUpdatedCb(UAVObjEvent *ev);
 
 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
 static uint8_t isAssistedFlightMode(uint8_t position);
@@ -98,12 +103,17 @@ static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel
 struct rcvr_activity_fsm {
     ManualControlSettingsChannelGroupsOptions group;
     uint16_t prev[RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP];
-    uint8_t sample_count;
+    uint8_t  sample_count;
+    uint8_t  quality;
 };
 static struct rcvr_activity_fsm activity_fsm;
 
 static void resetRcvrActivity(struct rcvr_activity_fsm *fsm);
 static bool updateRcvrActivity(struct rcvr_activity_fsm *fsm);
+static void resetRcvrStatus(struct rcvr_activity_fsm *fsm);
+static bool updateRcvrStatus(
+    struct rcvr_activity_fsm *fsm,
+    ManualControlSettingsChannelGroupsOptions group);
 
 #define assumptions \
     ( \
@@ -124,6 +134,7 @@ int32_t ReceiverStart()
 #ifdef PIOS_INCLUDE_WDG
     PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL);
 #endif
+    SettingsUpdatedCb(NULL);
 
     return 0;
 }
@@ -138,16 +149,47 @@ int32_t ReceiverInitialize()
     AccessoryDesiredInitialize();
     ManualControlCommandInitialize();
     ReceiverActivityInitialize();
+    ReceiverStatusInitialize();
     ManualControlSettingsInitialize();
 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
     StabilizationSettingsInitialize();
+    VtolPathFollowerSettingsInitialize();
+    VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
 #endif
+    SystemSettingsInitialize();
+    SystemSettingsConnectCallback(&SettingsUpdatedCb);
 
 
     return 0;
 }
 MODULE_INITCALL(ReceiverInitialize, ReceiverStart);
 
+static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+{
+    frameType = GetCurrentFrameType();
+
+#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
+    uint8_t TreatCustomCraftAs;
+    VtolPathFollowerSettingsTreatCustomCraftAsGet(&TreatCustomCraftAs);
+
+
+    if (frameType == FRAME_TYPE_CUSTOM) {
+        switch (TreatCustomCraftAs) {
+        case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING:
+            frameType = FRAME_TYPE_FIXED_WING;
+            break;
+        case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL:
+            frameType = FRAME_TYPE_MULTIROTOR;
+            break;
+        case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_GROUND:
+            frameType = FRAME_TYPE_GROUND;
+            break;
+        }
+    }
+#endif
+}
+
+
 /**
  * Module task
  */
@@ -173,6 +215,7 @@ static void receiverTask(__attribute__((unused)) void *parameters)
     /* Initialize the RcvrActivty FSM */
     portTickType lastActivityTime = xTaskGetTickCount();
     resetRcvrActivity(&activity_fsm);
+    resetRcvrStatus(&activity_fsm);
 
     // Main task loop
     lastSysTime = xTaskGetTickCount();
@@ -197,9 +240,13 @@ static void receiverTask(__attribute__((unused)) void *parameters)
                 /* Reset the aging timer because activity was detected */
                 lastActivityTime = lastSysTime;
             }
+            /* Read signal quality from the group used for the throttle */
+            (void)updateRcvrStatus(&activity_fsm,
+                                   settings.ChannelGroups.Throttle);
         }
         if (timeDifferenceMs(lastActivityTime, lastSysTime) > 5000) {
             resetRcvrActivity(&activity_fsm);
+            resetRcvrStatus(&activity_fsm);
             lastActivityTime = lastSysTime;
         }
 
@@ -243,22 +290,20 @@ static void receiverTask(__attribute__((unused)) void *parameters)
             }
         }
 
-        // Check settings, if error raise alarm
+        /* Read signal quality from the group used for the throttle */
-        if (settings.ChannelGroups.Roll >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
+        (void)updateRcvrStatus(&activity_fsm,
-            || settings.ChannelGroups.Pitch >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
+                               settings.ChannelGroups.Throttle);
-            || settings.ChannelGroups.Yaw >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
+
+        // Sanity Check Throttle and Yaw
+        if (settings.ChannelGroups.Yaw >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
             || settings.ChannelGroups.Throttle >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
             ||
             // Check all channel mappings are valid
-            cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_INVALID
+            cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_INVALID
-            || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_INVALID
-            || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_INVALID
             || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t)PIOS_RCVR_INVALID
             ||
             // Check the driver exists
-            cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_NODRIVER
+            cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_NODRIVER
-            || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_NODRIVER
-            || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_NODRIVER
             || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t)PIOS_RCVR_NODRIVER
             ||
             // Check collective if required
@@ -271,7 +316,7 @@ static void receiverTask(__attribute__((unused)) void *parameters)
             settings.FlightModeNumber < 1 || settings.FlightModeNumber > FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM
             ||
             // Similar checks for FlightMode channel but only if more than one flight mode has been set. Otherwise don't care
-            ((settings.FlightModeNumber > 1)
+            ((settings.FlightModeNumber > 1) && (frameType != FRAME_TYPE_GROUND)
              && (settings.ChannelGroups.FlightMode >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
                  || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_INVALID
                  || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_NODRIVER))) {
@@ -282,15 +327,39 @@ static void receiverTask(__attribute__((unused)) void *parameters)
             continue;
         }
 
+
+        if (frameType != FRAME_TYPE_GROUND) {
+            // Sanity Check Pitch and Roll
+            if (settings.ChannelGroups.Roll >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
+                || settings.ChannelGroups.Pitch >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
+                ||
+                // Check all channel mappings are valid
+                cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_INVALID
+                || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_INVALID
+                ||
+                // Check the driver exists
+                cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_NODRIVER
+                || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_NODRIVER) {
+                AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_CRITICAL);
+                cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
+                ManualControlCommandSet(&cmd);
+
+                continue;
+            }
+        }
+
         // decide if we have valid manual input or not
         valid_input_detected &= validInputRange(settings.ChannelMin.Throttle,
                                                 settings.ChannelMax.Throttle, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE])
-                                && validInputRange(settings.ChannelMin.Roll,
-                                                   settings.ChannelMax.Roll, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL])
                                 && validInputRange(settings.ChannelMin.Yaw,
-                                                   settings.ChannelMax.Yaw, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW])
+                                                   settings.ChannelMax.Yaw, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]);
-                                && validInputRange(settings.ChannelMin.Pitch,
+
-                                                   settings.ChannelMax.Pitch, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]);
+        if (frameType != FRAME_TYPE_GROUND) {
+            valid_input_detected &= validInputRange(settings.ChannelMin.Roll,
+                                                    settings.ChannelMax.Roll, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL])
+                                    && validInputRange(settings.ChannelMin.Pitch,
+                                                       settings.ChannelMax.Pitch, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]);
+        }
 
         if (settings.ChannelGroups.Collective != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
             valid_input_detected &= validInputRange(settings.ChannelMin.Collective,
@@ -321,10 +390,14 @@ static void receiverTask(__attribute__((unused)) void *parameters)
         }
 
         if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) {
-            cmd.Throttle   = settings.FailsafeChannel.Throttle;
+            if (frameType != FRAME_TYPE_GROUND) {
-            cmd.Roll       = settings.FailsafeChannel.Roll;
+                cmd.Throttle = settings.FailsafeChannel.Throttle;
-            cmd.Pitch      = settings.FailsafeChannel.Pitch;
+            } else {
-            cmd.Yaw = settings.FailsafeChannel.Yaw;
+                cmd.Throttle = 0.0f;
+            }
+            cmd.Roll  = settings.FailsafeChannel.Roll;
+            cmd.Pitch = settings.FailsafeChannel.Pitch;
+            cmd.Yaw   = settings.FailsafeChannel.Yaw;
             cmd.Collective = settings.FailsafeChannel.Collective;
             switch (thrustType) {
             case SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE:
@@ -401,6 +474,9 @@ static void receiverTask(__attribute__((unused)) void *parameters)
                 applyDeadband(&cmd.Roll, deadband_checked);
                 applyDeadband(&cmd.Pitch, deadband_checked);
                 applyDeadband(&cmd.Yaw, deadband_checked);
+                if (frameType == FRAME_TYPE_GROUND) { // assumes reversible motors
+                    applyDeadband(&cmd.Throttle, deadband_checked);
+                }
             }
 #ifdef USE_INPUT_LPF
             // Apply Low Pass Filter to input channels, time delta between calls in ms
@@ -508,6 +584,12 @@ static void resetRcvrActivity(struct rcvr_activity_fsm *fsm)
     fsm->sample_count = 0;
 }
 
+static void resetRcvrStatus(struct rcvr_activity_fsm *fsm)
+{
+    /* Reset the state */
+    fsm->quality = 0;
+}
+
 static void updateRcvrActivitySample(uint32_t rcvr_id, uint16_t samples[], uint8_t max_channels)
 {
     for (uint8_t channel = 1; channel <= max_channels; channel++) {
@@ -552,6 +634,9 @@ static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm
             case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS:
                 group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS;
                 break;
+            case MANUALCONTROLSETTINGS_CHANNELGROUPS_SRXL:
+                group = RECEIVERACTIVITY_ACTIVEGROUP_SRXL;
+                break;
             case MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS:
                 group = RECEIVERACTIVITY_ACTIVEGROUP_GCS;
                 break;
@@ -578,6 +663,7 @@ static bool updateRcvrActivity(struct rcvr_activity_fsm *fsm)
     if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
         /* We're out of range, reset things */
         resetRcvrActivity(fsm);
+        resetRcvrStatus(fsm);
     }
 
     extern uint32_t pios_rcvr_group_map[];
@@ -623,6 +709,32 @@ group_completed:
     return activity_updated;
 }
 
+/* Read signal quality from the specified group */
+static bool updateRcvrStatus(
+    struct rcvr_activity_fsm *fsm,
+    ManualControlSettingsChannelGroupsOptions group)
+{
+    extern uint32_t pios_rcvr_group_map[];
+    bool activity_updated = false;
+    int8_t quality;
+
+    quality = PIOS_RCVR_GetQuality(pios_rcvr_group_map[group]);
+
+    /* If no driver is detected or any other error then return */
+    if (quality < 0) {
+        return activity_updated;
+    }
+
+    /* Compare with previous sample */
+    if (quality != fsm->quality) {
+        fsm->quality     = quality;
+        ReceiverStatusQualitySet(&fsm->quality);
+        activity_updated = true;
+    }
+
+    return activity_updated;
+}
+
 /**
  * Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range.
  */

flight/modules/Sensors/sensors.c

@@ -411,9 +411,9 @@ static void handleAccel(float *samples, float temperature)
     AccelSensorData accelSensorData;
 
     updateAccelTempBias(temperature);
-    float accels_out[3] = { samples[0] * agcal.accel_scale.X - agcal.accel_bias.X - accel_temp_bias[0],
+    float accels_out[3] = { (samples[0] - agcal.accel_bias.X) * agcal.accel_scale.X - accel_temp_bias[0],
-                            samples[1] * agcal.accel_scale.Y - agcal.accel_bias.Y - accel_temp_bias[1],
+                            (samples[1] - agcal.accel_bias.Y) * agcal.accel_scale.Y - accel_temp_bias[1],
-                            samples[2] * agcal.accel_scale.Z - agcal.accel_bias.Z - accel_temp_bias[2] };
+                            (samples[2] - agcal.accel_bias.Z) * agcal.accel_scale.Z - accel_temp_bias[2] };
 
     rot_mult(R, accels_out, samples);
     accelSensorData.x = samples[0];
@@ -444,8 +444,8 @@ static void handleGyro(float *samples, float temperature)
 static void handleMag(float *samples, float temperature)
 {
     MagSensorData mag;
-    float mags[3] = { (float)samples[1] - mag_bias[0],
+    float mags[3] = { (float)samples[0] - mag_bias[0],
-                      (float)samples[0] - mag_bias[1],
+                      (float)samples[1] - mag_bias[1],
                       (float)samples[2] - mag_bias[2] };
 
     rot_mult(mag_transform, mags, samples);
@@ -485,7 +485,10 @@ static void updateAccelTempBias(float temperature)
     if ((accel_temp_calibrated) && !accel_temp_calibration_count) {
         accel_temp_calibration_count = TEMP_CALIB_INTERVAL;
         if (accel_temp_calibrated) {
-            float ctemp = boundf(accel_temperature, agcal.temp_calibrated_extent.max, agcal.temp_calibrated_extent.min);
+            float ctemp = boundf(accel_temperature,
+                                 agcal.temp_calibrated_extent.max,
+                                 agcal.temp_calibrated_extent.min);
+
             accel_temp_bias[0] = agcal.accel_temp_coeff.X * ctemp;
             accel_temp_bias[1] = agcal.accel_temp_coeff.Y * ctemp;
             accel_temp_bias[2] = agcal.accel_temp_coeff.Z * ctemp;
@@ -588,8 +591,12 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
 
     RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection);
     RevoSettingsBaroTempCorrectionExtentGet(&baroCorrectionExtent);
-    baro_temp_correction_enabled = !(baroCorrectionExtent.max - baroCorrectionExtent.min < 0.1f ||
+    baro_temp_correction_enabled =
-                                     (baroCorrection.a < 1e-9f && baroCorrection.b < 1e-9f && baroCorrection.c < 1e-9f && baroCorrection.d < 1e-9f));
+        (baroCorrectionExtent.max - baroCorrectionExtent.min > 0.1f &&
+         (fabsf(baroCorrection.a) > 1e-9f ||
+          fabsf(baroCorrection.b) > 1e-9f ||
+          fabsf(baroCorrection.c) > 1e-9f ||
+          fabsf(baroCorrection.d) > 1e-9f));
 }
 /**
  * @}

flight/modules/Stabilization/altitudeloop.cpp

@@ -1,7 +1,7 @@
 /**
  ******************************************************************************
  *
- * @file       altitudeloop.c
+ * @file       altitudeloop.cpp
  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
  * @brief      This module compared @ref PositionActuatl to @ref ActiveWaypoint
  * and sets @ref AttitudeDesired.  It only does this when the FlightMode field
@@ -26,6 +26,7 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 
+extern "C" {
 #include <openpilot.h>
 
 #include <callbackinfo.h>
@@ -37,6 +38,12 @@
 #include <altitudeholdstatus.h>
 #include <velocitystate.h>
 #include <positionstate.h>
+#include <vtolselftuningstats.h>
+#include <stabilization.h>
+}
+
+#include <pidcontroldown.h>
+
 // Private constants
 
 
@@ -50,78 +57,134 @@
 #define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW
 #define CBTASK_PRIORITY   CALLBACK_TASK_FLIGHTCONTROL
 
-#define STACK_SIZE_BYTES  512
 // Private types
 
 // Private variables
 static DelayedCallbackInfo *altitudeHoldCBInfo;
+static PIDControlDown controlDown;
 static AltitudeHoldSettingsData altitudeHoldSettings;
-static struct pid pid0, pid1;
 static ThrustModeType thrustMode;
-static PiOSDeltatimeConfig timeval;
+static float thrustDemand = 0.0f;
-static float thrustSetpoint = 0.0f;
-static float thrustDemand   = 0.0f;
-static float startThrust    = 0.5f;
 
 
 // Private functions
-static void altitudeHoldTask(void);
 static void SettingsUpdatedCb(UAVObjEvent *ev);
+static void altitudeHoldTask(void);
 static void VelocityStateUpdatedCb(UAVObjEvent *ev);
 
 /**
  * Setup mode and setpoint
+ *
+ * reinit: true when althold/vario mode selected over a previous alternate thrust mode
  */
 float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit)
 {
     static bool newaltitude = true;
 
-    if (reinit) {
+    if (reinit || !controlDown.IsActive()) {
-        startThrust = setpoint;
+        controlDown.Activate();
-        pid_zero(&pid0);
-        pid_zero(&pid1);
         newaltitude = true;
+        // calculate a thrustDemand on reinit only
+        altitudeHoldTask();
     }
 
     const float DEADBAND      = 0.20f;
     const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2;
     const float DEADBAND_LOW  = 1.0f / 2 - DEADBAND / 2;
 
-    // this is the max speed in m/s at the extents of thrust
-    float thrustRate;
-    uint8_t thrustExp;
-
-    AltitudeHoldSettingsThrustExpGet(&thrustExp);
-    AltitudeHoldSettingsThrustRateGet(&thrustRate);
-
-    PositionStateData posState;
-    PositionStateGet(&posState);
 
     if (altitudeHoldSettings.CutThrustWhenZero && setpoint <= 0) {
         // Cut thrust if desired
-        thrustSetpoint = 0.0f;
+        controlDown.UpdateVelocitySetpoint(0.0f);
-        thrustDemand   = 0.0f;
+        thrustDemand = 0.0f;
-        thrustMode     = DIRECT;
+        thrustMode   = DIRECT;
-        newaltitude    = true;
+        newaltitude  = true;
+        return thrustDemand;
     } else if (mode == ALTITUDEVARIO && setpoint > DEADBAND_HIGH) {
         // being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1
         // then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255
-        thrustSetpoint = -((thrustExp * powf((setpoint - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - thrustExp) * (setpoint - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * thrustRate);
+        controlDown.UpdateVelocitySetpoint(-((altitudeHoldSettings.ThrustExp * powf((setpoint - DEADBAND_HIGH) / (DEADBAND_LOW), 3.0f) + (255.0f - altitudeHoldSettings.ThrustExp) * (setpoint - DEADBAND_HIGH) / DEADBAND_LOW) / 255.0f * altitudeHoldSettings.ThrustRate));
-        thrustMode     = ALTITUDEVARIO;
+        thrustMode  = ALTITUDEVARIO;
-        newaltitude    = true;
+        newaltitude = true;
     } else if (mode == ALTITUDEVARIO && setpoint < DEADBAND_LOW) {
-        thrustSetpoint = -(-(thrustExp * powf((DEADBAND_LOW - (setpoint < 0 ? 0 : setpoint)) / DEADBAND_LOW, 3) + (255 - thrustExp) * (DEADBAND_LOW - setpoint) / DEADBAND_LOW) / 255 * thrustRate);
+        controlDown.UpdateVelocitySetpoint(-(-(altitudeHoldSettings.ThrustExp * powf((DEADBAND_LOW - (setpoint < 0 ? 0 : setpoint)) / DEADBAND_LOW, 3.0f) + (255.0f - altitudeHoldSettings.ThrustExp) * (DEADBAND_LOW - setpoint) / DEADBAND_LOW) / 255.0f * altitudeHoldSettings.ThrustRate));
-        thrustMode     = ALTITUDEVARIO;
+        thrustMode  = ALTITUDEVARIO;
-        newaltitude    = true;
+        newaltitude = true;
     } else if (newaltitude == true) {
-        thrustSetpoint = posState.Down;
+        controlDown.UpdateVelocitySetpoint(0.0f);
-        thrustMode     = ALTITUDEHOLD;
+        PositionStateData posState;
-        newaltitude    = false;
+        PositionStateGet(&posState);
+        controlDown.UpdatePositionSetpoint(posState.Down);
+        thrustMode  = ALTITUDEHOLD;
+        newaltitude = false;
     }
 
+    thrustDemand = boundf(thrustDemand, altitudeHoldSettings.ThrustLimits.Min, altitudeHoldSettings.ThrustLimits.Max);
+
     return thrustDemand;
 }
 
+/**
+ * Disable the alt hold task loop velocity controller to save cpu cycles
+ */
+void stabilizationDisableAltitudeHold(void)
+{
+    controlDown.Deactivate();
+}
+
+
+/**
+ * Module thread, should not return.
+ */
+static void altitudeHoldTask(void)
+{
+    if (!controlDown.IsActive()) {
+        return;
+    }
+
+    AltitudeHoldStatusData altitudeHoldStatus;
+    AltitudeHoldStatusGet(&altitudeHoldStatus);
+
+    float velocityStateDown;
+    VelocityStateDownGet(&velocityStateDown);
+    controlDown.UpdateVelocityState(velocityStateDown);
+
+    float local_thrustDemand = 0.0f;
+
+    switch (thrustMode) {
+    case ALTITUDEHOLD:
+    {
+        float positionStateDown;
+        PositionStateDownGet(&positionStateDown);
+        controlDown.UpdatePositionState(positionStateDown);
+        controlDown.ControlPosition();
+        altitudeHoldStatus.VelocityDesired = controlDown.GetVelocityDesired();
+        altitudeHoldStatus.State = ALTITUDEHOLDSTATUS_STATE_ALTITUDEHOLD;
+        local_thrustDemand = controlDown.GetDownCommand();
+    }
+    break;
+
+    case ALTITUDEVARIO:
+        altitudeHoldStatus.VelocityDesired = controlDown.GetVelocityDesired();
+        altitudeHoldStatus.State = ALTITUDEHOLDSTATUS_STATE_ALTITUDEVARIO;
+        local_thrustDemand = controlDown.GetDownCommand();
+        break;
+
+    case DIRECT:
+        altitudeHoldStatus.VelocityDesired = 0.0f;
+        altitudeHoldStatus.State = ALTITUDEHOLDSTATUS_STATE_DIRECT;
+        break;
+    }
+
+    AltitudeHoldStatusSet(&altitudeHoldStatus);
+    thrustDemand = local_thrustDemand;
+}
+
+static void VelocityStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+{
+    PIOS_CALLBACKSCHEDULER_Dispatch(altitudeHoldCBInfo);
+}
+
 /**
  * Initialise the module, called on startup
  */
@@ -131,82 +194,39 @@ void stabilizationAltitudeloopInit()
     AltitudeHoldStatusInitialize();
     PositionStateInitialize();
     VelocityStateInitialize();
+    VtolSelfTuningStatsInitialize();
 
-    PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
+    AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb);
-    // Create object queue
+    VtolSelfTuningStatsConnectCallback(&SettingsUpdatedCb);
+    SettingsUpdatedCb(NULL);
 
     altitudeHoldCBInfo = PIOS_CALLBACKSCHEDULER_Create(&altitudeHoldTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_ALTITUDEHOLD, STACK_SIZE_BYTES);
-    AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb);
     VelocityStateConnectCallback(&VelocityStateUpdatedCb);
-
-    // Start main task
-    SettingsUpdatedCb(NULL);
 }
 
 
-/**
- * Module thread, should not return.
- */
-static void altitudeHoldTask(void)
-{
-    AltitudeHoldStatusData altitudeHoldStatus;
-
-    AltitudeHoldStatusGet(&altitudeHoldStatus);
-
-    // do the actual control loop(s)
-    float positionStateDown;
-    PositionStateDownGet(&positionStateDown);
-    float velocityStateDown;
-    VelocityStateDownGet(&velocityStateDown);
-
-    float dT;
-    dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
-    switch (thrustMode) {
-    case ALTITUDEHOLD:
-    {
-        // altitude control loop
-        // No scaling.
-        const pid_scaler scaler = { .p = 1.0f, .i = 1.0f, .d = 1.0f };
-        altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, &scaler, thrustSetpoint, positionStateDown, dT);
-    }
-    break;
-    case ALTITUDEVARIO:
-        altitudeHoldStatus.VelocityDesired = thrustSetpoint;
-        break;
-    default:
-        altitudeHoldStatus.VelocityDesired = 0;
-        break;
-    }
-
-    AltitudeHoldStatusSet(&altitudeHoldStatus);
-
-    switch (thrustMode) {
-    case DIRECT:
-        thrustDemand = thrustSetpoint;
-        break;
-    default:
-    {
-        // velocity control loop
-        // No scaling.
-        const pid_scaler scaler = { .p = 1.0f, .i = 1.0f, .d = 1.0f };
-        thrustDemand = startThrust - pid_apply_setpoint(&pid1, &scaler, altitudeHoldStatus.VelocityDesired, velocityStateDown, dT);
-    }
-    break;
-    }
-}
-
 static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 {
     AltitudeHoldSettingsGet(&altitudeHoldSettings);
-    pid_configure(&pid0, altitudeHoldSettings.AltitudePI.Kp, altitudeHoldSettings.AltitudePI.Ki, 0, altitudeHoldSettings.AltitudePI.Ilimit);
-    pid_zero(&pid0);
-    pid_configure(&pid1, altitudeHoldSettings.VelocityPI.Kp, altitudeHoldSettings.VelocityPI.Ki, 0, altitudeHoldSettings.VelocityPI.Ilimit);
-    pid_zero(&pid1);
-}
 
-static void VelocityStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+    controlDown.UpdateParameters(altitudeHoldSettings.VerticalVelPID.Kp,
-{
+                                 altitudeHoldSettings.VerticalVelPID.Ki,
-    PIOS_CALLBACKSCHEDULER_Dispatch(altitudeHoldCBInfo);
+                                 altitudeHoldSettings.VerticalVelPID.Kd,
+                                 altitudeHoldSettings.VerticalVelPID.Beta,
+                                 (float)(UPDATE_EXPECTED),
+                                 altitudeHoldSettings.ThrustRate);
+
+    controlDown.UpdatePositionalParameters(altitudeHoldSettings.VerticalPosP);
+
+    VtolSelfTuningStatsData vtolSelfTuningStats;
+    VtolSelfTuningStatsGet(&vtolSelfTuningStats);
+    controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + altitudeHoldSettings.ThrustLimits.Neutral);
+
+    // initialise limits on thrust but note the FSM can override.
+    controlDown.SetThrustLimits(altitudeHoldSettings.ThrustLimits.Min, altitudeHoldSettings.ThrustLimits.Max);
+
+    // disable neutral thrust calcs which should only be done in a hold mode.
+    controlDown.DisableNeutralThrustCalc();
 }
 
 

flight/modules/Stabilization/inc/altitudeloop.h

@@ -37,5 +37,6 @@ typedef enum { ALTITUDEHOLD = 0, ALTITUDEVARIO = 1, DIRECT = 2 } ThrustModeType;
 
 void stabilizationAltitudeloopInit();
 float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit);
+void stabilizationDisableAltitudeHold(void);
 
 #endif /* ALTITUDELOOP_H */

flight/modules/Stabilization/innerloop.c

@@ -49,7 +49,7 @@
 #include <stabilization.h>
 #include <virtualflybar.h>
 #include <cruisecontrol.h>
-
+#include <sanitycheck.h>
 // Private constants
 
 #define CALLBACK_PRIORITY CALLBACK_PRIORITY_CRITICAL
@@ -66,6 +66,7 @@ static float axis_lock_accum[3] = { 0, 0, 0 };
 static uint8_t previous_mode[AXES] = { 255, 255, 255, 255 };
 static PiOSDeltatimeConfig timeval;
 static float speedScaleFactor = 1.0f;
+static bool frame_is_multirotor;
 
 // Private functions
 static void stabilizationInnerloopTask();
@@ -95,6 +96,8 @@ void stabilizationInnerloopInit()
 
     // schedule dead calls every FAILSAFE_TIMEOUT_MS to have the watchdog cleared
     PIOS_CALLBACKSCHEDULER_Schedule(callbackHandle, FAILSAFE_TIMEOUT_MS, CALLBACK_UPDATEMODE_LATER);
+
+    frame_is_multirotor = (GetCurrentFrameType() == FRAME_TYPE_MULTIROTOR);
 }
 
 static float get_pid_scale_source_value()
@@ -239,8 +242,13 @@ static void stabilizationInnerloopTask()
     float *actuatorDesiredAxis = &actuator.Roll;
     int t;
     float dT;
+    bool multirotor = (GetCurrentFrameType() == FRAME_TYPE_MULTIROTOR); // check if frame is a multirotor
     dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
 
+    StabilizationStatusOuterLoopData outerLoop;
+    StabilizationStatusOuterLoopGet(&outerLoop);
+    bool allowPiroComp = true;
+
     for (t = 0; t < AXES; t++) {
         bool reinit = (StabilizationStatusInnerLoopToArray(enabled)[t] != previous_mode[t]);
         previous_mode[t] = StabilizationStatusInnerLoopToArray(enabled)[t];
@@ -248,6 +256,15 @@ static void stabilizationInnerloopTask()
         if (t < STABILIZATIONSTATUS_INNERLOOP_THRUST) {
             if (reinit) {
                 stabSettings.innerPids[t].iAccumulator = 0;
+                if (frame_is_multirotor) {
+                    // Multirotors should dump axis lock accumulators when unarmed or throttle is low.
+                    // Fixed wing or ground vehicles can fly/drive with low throttle.
+                    axis_lock_accum[t] = 0;
+                }
+            }
+            // Any self leveling on roll or pitch must prevent pirouette compensation
+            if (t < STABILIZATIONSTATUS_INNERLOOP_YAW && StabilizationStatusOuterLoopToArray(outerLoop)[t] != STABILIZATIONSTATUS_OUTERLOOP_DIRECT) {
+                allowPiroComp = false;
             }
             switch (StabilizationStatusInnerLoopToArray(enabled)[t]) {
             case STABILIZATIONSTATUS_INNERLOOP_VIRTUALFLYBAR:
@@ -308,7 +325,12 @@ static void stabilizationInnerloopTask()
             }
         }
 
-        actuatorDesiredAxis[t] = boundf(actuatorDesiredAxis[t], -1.0f, 1.0f);
+        if (!multirotor) {
+            // we only need to clamp the desired axis to a sane range if the frame is not a multirotor type
+            // we don't want to do any clamping until after the motors are calculated and scaled.
+            // need to figure out what to do with a tricopter tail servo.
+            actuatorDesiredAxis[t] = boundf(actuatorDesiredAxis[t], -1.0f, 1.0f);
+        }
     }
 
     actuator.UpdateTime = dT * 1000;
@@ -322,7 +344,7 @@ static void stabilizationInnerloopTask()
         }
     }
 
-    if (stabSettings.stabBank.EnablePiroComp == STABILIZATIONBANK_ENABLEPIROCOMP_TRUE && stabSettings.innerPids[0].iLim > 1e-3f && stabSettings.innerPids[1].iLim > 1e-3f) {
+    if (allowPiroComp && stabSettings.stabBank.EnablePiroComp == STABILIZATIONBANK_ENABLEPIROCOMP_TRUE && stabSettings.innerPids[0].iLim > 1e-3f && stabSettings.innerPids[1].iLim > 1e-3f) {
         // attempted piro compensation - rotate pitch and yaw integrals (experimental)
         float angleYaw = DEG2RAD(gyro_filtered[2] * dT);
         float sinYaw   = sinf(angleYaw);
@@ -334,7 +356,7 @@ static void stabilizationInnerloopTask()
     }
 
     {
-        uint8_t armed;
+        FlightStatusArmedOptions armed;
         FlightStatusArmedGet(&armed);
         float throttleDesired;
         ManualControlCommandThrottleGet(&throttleDesired);

flight/modules/Stabilization/outerloop.c

@@ -103,7 +103,33 @@ static void stabilizationOuterloopTask()
     float *stabilizationDesiredAxis = &stabilizationDesired.Roll;
     float *rateDesiredAxis = &rateDesired.Roll;
     int t;
-    float dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
+    float dT    = PIOS_DELTATIME_GetAverageSeconds(&timeval);
+
+    bool reinit = (StabilizationStatusOuterLoopToArray(enabled)[STABILIZATIONSTATUS_OUTERLOOP_THRUST] != previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST]);
+    previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = StabilizationStatusOuterLoopToArray(enabled)[STABILIZATIONSTATUS_OUTERLOOP_THRUST];
+#ifdef REVOLUTION
+    if (reinit && (previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST] == STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE ||
+                   previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST] == STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO)) {
+        // disable the altvario velocity control loop
+        stabilizationDisableAltitudeHold();
+    }
+#endif /* REVOLUTION */
+    switch (StabilizationStatusOuterLoopToArray(enabled)[STABILIZATIONSTATUS_OUTERLOOP_THRUST]) {
+#ifdef REVOLUTION
+    case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE:
+        rateDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = stabilizationAltitudeHold(stabilizationDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST], ALTITUDEHOLD, reinit);
+        break;
+    case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO:
+        rateDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = stabilizationAltitudeHold(stabilizationDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST], ALTITUDEVARIO, reinit);
+        break;
+#endif /* REVOLUTION */
+    case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
+    case STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS:
+    default:
+        rateDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = stabilizationDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST];
+        break;
+    }
+
 
     float local_error[3];
     {
@@ -120,6 +146,7 @@ static void stabilizationOuterloopTask()
             case STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING:
                 rpy_desired[t] = stabilizationDesiredAxis[t];
                 break;
+            case STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS:
             case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
             default:
                 rpy_desired[t] = ((float *)&attitudeState.Roll)[t];
@@ -148,123 +175,142 @@ static void stabilizationOuterloopTask()
         }
 #endif /* if defined(PIOS_QUATERNION_STABILIZATION) */
     }
-    for (t = 0; t < AXES; t++) {
+
-        bool reinit = (StabilizationStatusOuterLoopToArray(enabled)[t] != previous_mode[t]);
+
+    for (t = STABILIZATIONSTATUS_OUTERLOOP_ROLL; t < STABILIZATIONSTATUS_OUTERLOOP_THRUST; t++) {
+        reinit = (StabilizationStatusOuterLoopToArray(enabled)[t] != previous_mode[t]);
         previous_mode[t] = StabilizationStatusOuterLoopToArray(enabled)[t];
 
-        if (t < STABILIZATIONSTATUS_OUTERLOOP_THRUST) {
+        if (reinit) {
-            if (reinit) {
+            stabSettings.outerPids[t].iAccumulator = 0;
-                stabSettings.outerPids[t].iAccumulator = 0;
+        }
+        switch (StabilizationStatusOuterLoopToArray(enabled)[t]) {
+        case STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE:
+            rateDesiredAxis[t] = pid_apply(&stabSettings.outerPids[t], local_error[t], dT);
+            break;
+        case STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE:
+        {
+            float stickinput[3];
+            stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f);
+            stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f);
+            stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f);
+            float rateDesiredAxisRate = stickinput[t] * StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t];
+            // limit corrective rate to maximum rates to not give it overly large impact over manual rate when joined together
+            rateDesiredAxis[t] = boundf(pid_apply(&stabSettings.outerPids[t], local_error[t], dT),
+                                        -StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t],
+                                        StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t]
+                                        );
+            // Compute the weighted average rate desired
+            // Using max() rather than sqrt() for cpu speed;
+            // - this makes the stick region into a square;
+            // - this is a feature!
+            // - hold a roll angle and add just pitch without the stick sensitivity changing
+            float magnitude = fabsf(stickinput[t]);
+            if (t < 2) {
+                magnitude = fmaxf(fabsf(stickinput[0]), fabsf(stickinput[1]));
             }
-            switch (StabilizationStatusOuterLoopToArray(enabled)[t]) {
+
-            case STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE:
+            // modify magnitude to move the Att to Rate transition to the place
-                rateDesiredAxis[t] = pid_apply(&stabSettings.outerPids[t], local_error[t], dT);
+            // specified by the user
-                break;
+            // we are looking for where the stick angle == transition angle
-            case STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE:
+            // and the Att rate equals the Rate rate
-            {
+            // that's where Rate x (1-StickAngle) [Attitude pulling down max X Ratt proportion]
-                float stickinput[3];
+            // == Rate x StickAngle [Rate pulling up according to stick angle]
-                stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f);
+            // * StickAngle [X Ratt proportion]
-                stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f);
+            // so 1-x == x*x or x*x+x-1=0 where xE(0,1)
-                stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f);
+            // (-1+-sqrt(1+4))/2 = (-1+sqrt(5))/2
-                float rateDesiredAxisRate = stickinput[t] * StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t];
+            // and quadratic formula says that is 0.618033989f
-                // limit corrective rate to maximum rates to not give it overly large impact over manual rate when joined together
+            // I tested 14.01 and came up with .61 without even remembering this number
-                rateDesiredAxis[t] = boundf(pid_apply(&stabSettings.outerPids[t], local_error[t], dT),
+            // I thought that moving the P,I, and maxangle terms around would change this value
-                                            -StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t],
+            // and that I would have to take these into account, but varying
-                                            StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t]
+            // all P's and I's by factors of 1/2 to 2 didn't change it noticeably
-                                            );
+            // and varying maxangle from 4 to 120 didn't either.
-                // Compute the weighted average rate desired
+            // so for now I'm not taking these into account
-                // Using max() rather than sqrt() for cpu speed;
+            // while working with this, it occurred to me that Attitude mode,
-                // - this makes the stick region into a square;
+            // set up with maxangle=190 would be similar to Ratt, and it is.
-                // - this is a feature!
+#define STICK_VALUE_AT_MODE_TRANSITION 0.618033989f
-                // - hold a roll angle and add just pitch without the stick sensitivity changing
+
-                float magnitude = fabsf(stickinput[t]);
+            // the following assumes the transition would otherwise be at 0.618033989f
-                if (t < 2) {
+            // and THAT assumes that Att ramps up to max roll rate
-                    magnitude = fmaxf(fabsf(stickinput[0]), fabsf(stickinput[1]));
+            // when a small number of degrees off of where it should be
+
+            // if below the transition angle (still in attitude mode)
+            // '<=' instead of '<' keeps rattitude_mode_transition_stick_position==1.0 from causing DZ
+            if (magnitude <= stabSettings.rattitude_mode_transition_stick_position) {
+                magnitude *= STICK_VALUE_AT_MODE_TRANSITION / stabSettings.rattitude_mode_transition_stick_position;
+            } else {
+                magnitude = (magnitude - stabSettings.rattitude_mode_transition_stick_position)
+                            * (1.0f - STICK_VALUE_AT_MODE_TRANSITION)
+                            / (1.0f - stabSettings.rattitude_mode_transition_stick_position)
+                            + STICK_VALUE_AT_MODE_TRANSITION;
+            }
+            rateDesiredAxis[t] = (1.0f - magnitude) * rateDesiredAxis[t] + magnitude * rateDesiredAxisRate;
+        }
+        break;
+        case STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING:
+            // FIXME: local_error[] is rate - attitude for Weak Leveling
+            // The only ramifications are:
+            // Weak Leveling Kp is off by a factor of 3 to 12 and may need a different default in GCS
+            // Changing Rate mode max rate currently requires a change to Kp
+            // That would be changed to Attitude mode max angle affecting Kp
+            // Also does not take dT into account
+        {
+            float stickinput[3];
+            stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f);
+            stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f);
+            stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f);
+            float rate_input    = stickinput[t] * StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t];
+            float weak_leveling = local_error[t] * stabSettings.settings.WeakLevelingKp;
+            weak_leveling = boundf(weak_leveling, -stabSettings.settings.MaxWeakLevelingRate, stabSettings.settings.MaxWeakLevelingRate);
+
+            // Compute desired rate as input biased towards leveling
+            rateDesiredAxis[t] = rate_input + weak_leveling;
+        }
+        break;
+        case STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS:
+            rateDesiredAxis[t] = stabilizationDesiredAxis[t]; // default for all axes
+            // now test limits for pitch and/or roll
+            if (t == 1) { // pitch
+                if (attitudeState.Pitch < -stabSettings.stabBank.PitchMax) {
+                    // attitude exceeds pitch max.
+                    // zero rate desired if also -ve
+                    if (rateDesiredAxis[t] < 0.0f) {
+                        rateDesiredAxis[t] = 0.0f;
+                    }
+                } else if (attitudeState.Pitch > stabSettings.stabBank.PitchMax) {
+                    // attitude exceeds pitch max
+                    // zero rate desired if also +ve
+                    if (rateDesiredAxis[t] > 0.0f) {
+                        rateDesiredAxis[t] = 0.0f;
+                    }
                 }
-
+            } else if (t == 0) { // roll
-                // modify magnitude to move the Att to Rate transition to the place
+                if (attitudeState.Roll < -stabSettings.stabBank.RollMax) {
-                // specified by the user
+                    // attitude exceeds roll max.
-                // we are looking for where the stick angle == transition angle
+                    // zero rate desired if also -ve
-                // and the Att rate equals the Rate rate
+                    if (rateDesiredAxis[t] < 0.0f) {
-                // that's where Rate x (1-StickAngle) [Attitude pulling down max X Ratt proportion]
+                        rateDesiredAxis[t] = 0.0f;
-                // == Rate x StickAngle [Rate pulling up according to stick angle]
+                    }
-                // * StickAngle [X Ratt proportion]
+                } else if (attitudeState.Roll > stabSettings.stabBank.RollMax) {
-                // so 1-x == x*x or x*x+x-1=0 where xE(0,1)
+                    // attitude exceeds roll max
-                // (-1+-sqrt(1+4))/2 = (-1+sqrt(5))/2
+                    // zero rate desired if also +ve
-                // and quadratic formula says that is 0.618033989f
+                    if (rateDesiredAxis[t] > 0.0f) {
-                // I tested 14.01 and came up with .61 without even remembering this number
+                        rateDesiredAxis[t] = 0.0f;
-                // I thought that moving the P,I, and maxangle terms around would change this value
+                    }
-                // and that I would have to take these into account, but varying
-                // all P's and I's by factors of 1/2 to 2 didn't change it noticeably
-                // and varying maxangle from 4 to 120 didn't either.
-                // so for now I'm not taking these into account
-                // while working with this, it occurred to me that Attitude mode,
-                // set up with maxangle=190 would be similar to Ratt, and it is.
-                #define STICK_VALUE_AT_MODE_TRANSITION 0.618033989f
-
-                // the following assumes the transition would otherwise be at 0.618033989f
-                // and THAT assumes that Att ramps up to max roll rate
-                // when a small number of degrees off of where it should be
-
-                // if below the transition angle (still in attitude mode)
-                // '<=' instead of '<' keeps rattitude_mode_transition_stick_position==1.0 from causing DZ
-                if (magnitude <= stabSettings.rattitude_mode_transition_stick_position) {
-                    magnitude *= STICK_VALUE_AT_MODE_TRANSITION / stabSettings.rattitude_mode_transition_stick_position;
-                } else {
-                    magnitude = (magnitude - stabSettings.rattitude_mode_transition_stick_position)
-                                * (1.0f - STICK_VALUE_AT_MODE_TRANSITION)
-                                / (1.0f - stabSettings.rattitude_mode_transition_stick_position)
-                                + STICK_VALUE_AT_MODE_TRANSITION;
                 }
-                rateDesiredAxis[t] = (1.0f - magnitude) * rateDesiredAxis[t] + magnitude * rateDesiredAxisRate;
             }
             break;
-            case STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING:
-                // FIXME: local_error[] is rate - attitude for Weak Leveling
-                // The only ramifications are:
-                // Weak Leveling Kp is off by a factor of 3 to 12 and may need a different default in GCS
-                // Changing Rate mode max rate currently requires a change to Kp
-                // That would be changed to Attitude mode max angle affecting Kp
-                // Also does not take dT into account
-            {
-                float stickinput[3];
-                stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f);
-                stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f);
-                stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f);
-                float rate_input    = stickinput[t] * StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t];
-                float weak_leveling = local_error[t] * stabSettings.settings.WeakLevelingKp;
-                weak_leveling = boundf(weak_leveling, -stabSettings.settings.MaxWeakLevelingRate, stabSettings.settings.MaxWeakLevelingRate);
 
-                // Compute desired rate as input biased towards leveling
+        case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
-                rateDesiredAxis[t] = rate_input + weak_leveling;
+        default:
-            }
+            rateDesiredAxis[t] = stabilizationDesiredAxis[t];
             break;
-            case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
-            default:
-                rateDesiredAxis[t] = stabilizationDesiredAxis[t];
-                break;
-            }
-        } else {
-            switch (StabilizationStatusOuterLoopToArray(enabled)[t]) {
-#ifdef REVOLUTION
-            case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE:
-                rateDesiredAxis[t] = stabilizationAltitudeHold(stabilizationDesiredAxis[t], ALTITUDEHOLD, reinit);
-                break;
-            case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO:
-                rateDesiredAxis[t] = stabilizationAltitudeHold(stabilizationDesiredAxis[t], ALTITUDEVARIO, reinit);
-                break;
-#endif /* REVOLUTION */
-            case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
-            default:
-                rateDesiredAxis[t] = stabilizationDesiredAxis[t];
-                break;
-            }
         }
     }
 
     RateDesiredSet(&rateDesired);
     {
-        uint8_t armed;
+        FlightStatusArmedOptions armed;
         FlightStatusArmedGet(&armed);
         float throttleDesired;
         ManualControlCommandThrottleGet(&throttleDesired);
@@ -277,7 +323,7 @@ static void stabilizationOuterloopTask()
         }
     }
 
-    // update cruisecontrol based on attitude
+// update cruisecontrol based on attitude
     cruisecontrol_compute_factor(&attitudeState, rateDesired.Thrust);
     stabSettings.monitor.rateupdates = 0;
 }

flight/modules/Stabilization/stabilization.c

@@ -137,6 +137,10 @@ static void StabilizationDesiredUpdatedCb(__attribute__((unused)) UAVObjEvent *e
             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
             break;
+        case STABILIZATIONDESIRED_STABILIZATIONMODE_RATETRAINER:
+            StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS;
+            StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
+            break;
         case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE;
             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;

flight/modules/StateEstimation/filterstationary.c

@@ -30,13 +30,17 @@
  */
 
 #include "inc/stateestimation.h"
-
+// Fake pos rate in uS
+#define FILTERSTATIONARY_FAKEPOSRATE 100000
 // Private constants
 
-#define STACK_REQUIRED 64
+#define STACK_REQUIRED               64
 
 // Private types
-
+// Private types
+struct data {
+    uint32_t lastUpdate;
+};
 // Private variables
 
 // Private functions
@@ -49,27 +53,34 @@ int32_t filterStationaryInitialize(stateFilter *handle)
 {
     handle->init      = &init;
     handle->filter    = &filter;
-    handle->localdata = NULL;
+    handle->localdata = pios_malloc(sizeof(struct data));
     return STACK_REQUIRED;
 }
 
-static int32_t init(__attribute__((unused)) stateFilter *self)
+static int32_t init(stateFilter *self)
 {
+    struct data *this = (struct data *)self->localdata;
+
+    this->lastUpdate = PIOS_DELAY_GetRaw();
     return 0;
 }
 
-static filterResult filter(__attribute__((unused)) stateFilter *self, stateEstimation *state)
+static filterResult filter(stateFilter *self, stateEstimation *state)
 {
-    state->pos[0]   = 0.0f;
+    struct data *this = (struct data *)self->localdata;
-    state->pos[1]   = 0.0f;
-    state->pos[2]   = 0.0f;
-    state->updated |= SENSORUPDATES_pos;
 
-    state->vel[0]   = 0.0f;
+    if (PIOS_DELAY_DiffuS(this->lastUpdate) > FILTERSTATIONARY_FAKEPOSRATE) {
-    state->vel[1]   = 0.0f;
+        this->lastUpdate = PIOS_DELAY_GetRaw();
-    state->vel[2]   = 0.0f;
+        state->pos[0]    = 0.0f;
-    state->updated |= SENSORUPDATES_vel;
+        state->pos[1]    = 0.0f;
+        state->pos[2]    = 0.0f;
+        state->updated  |= SENSORUPDATES_pos;
 
+        state->vel[0]    = 0.0f;
+        state->vel[1]    = 0.0f;
+        state->vel[2]    = 0.0f;
+        state->updated  |= SENSORUPDATES_vel;
+    }
     return FILTERRESULT_OK;
 }
 

flight/modules/StateEstimation/stateestimation.c

@@ -345,7 +345,6 @@ MODULE_INITCALL(StateEstimationInitialize, StateEstimationStart);
  */
 static void StateEstimationCb(void)
 {
-    static enum { RUNSTATE_LOAD = 0, RUNSTATE_FILTER = 1, RUNSTATE_SAVE = 2 } runState = RUNSTATE_LOAD;
     static filterResult alarm     = FILTERRESULT_OK;
     static filterResult lastAlarm = FILTERRESULT_UNINITIALISED;
     static uint16_t alarmcounter  = 0;
@@ -361,193 +360,171 @@ static void StateEstimationCb(void)
         return;
     }
 
-    switch (runState) {
+    alarm = FILTERRESULT_OK;
-    case RUNSTATE_LOAD:
 
-        alarm = FILTERRESULT_OK;
+    // set alarm to warning if called through timeout
-
+    if (updatedSensors == 0) {
-        // set alarm to warning if called through timeout
+        if (PIOS_DELAY_DiffuS(last_time) > 1000 * TIMEOUT_MS) {
-        if (updatedSensors == 0) {
+            alarm = FILTERRESULT_WARNING;
-            if (PIOS_DELAY_DiffuS(last_time) > 1000 * TIMEOUT_MS) {
-                alarm = FILTERRESULT_WARNING;
-            }
-        } else {
-            last_time = PIOS_DELAY_GetRaw();
         }
+    } else {
+        last_time = PIOS_DELAY_GetRaw();
+    }
 
-        // check if a new filter chain should be initialized
+    // check if a new filter chain should be initialized
-        if (fusionAlgorithm != revoSettings.FusionAlgorithm) {
+    if (fusionAlgorithm != revoSettings.FusionAlgorithm) {
-            FlightStatusData fs;
+        FlightStatusData fs;
-            FlightStatusGet(&fs);
+        FlightStatusGet(&fs);
-            if (fs.Armed == FLIGHTSTATUS_ARMED_DISARMED || fusionAlgorithm == FILTER_INIT_FORCE) {
+        if (fs.Armed == FLIGHTSTATUS_ARMED_DISARMED || fusionAlgorithm == FILTER_INIT_FORCE) {
-                const filterPipeline *newFilterChain;
+            const filterPipeline *newFilterChain;
-                switch (revoSettings.FusionAlgorithm) {
+            switch ((RevoSettingsFusionAlgorithmOptions)revoSettings.FusionAlgorithm) {
-                case REVOSETTINGS_FUSIONALGORITHM_BASICCOMPLEMENTARY:
+            case REVOSETTINGS_FUSIONALGORITHM_BASICCOMPLEMENTARY:
-                    newFilterChain = cfQueue;
+                newFilterChain = cfQueue;
-                    break;
-                case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAG:
-                    newFilterChain = cfmiQueue;
-                    break;
-                case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAGGPSOUTDOOR:
-                    newFilterChain = cfmQueue;
-                    break;
-                case REVOSETTINGS_FUSIONALGORITHM_INS13INDOOR:
-                    newFilterChain = ekf13iQueue;
-                    break;
-                case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13:
-                    newFilterChain = ekf13Queue;
-                    break;
-                default:
-                    newFilterChain = NULL;
-                }
-                // initialize filters in chain
-                current = newFilterChain;
-                bool error = 0;
-                while (current != NULL) {
-                    int32_t result = current->filter->init((stateFilter *)current->filter);
-                    if (result != 0) {
-                        error = 1;
-                        break;
-                    }
-                    current = current->next;
-                }
-                if (error) {
-                    AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
-                    return;
-                } else {
-                    // set new fusion algortithm
-                    filterChain     = newFilterChain;
-                    fusionAlgorithm = revoSettings.FusionAlgorithm;
-                }
-            }
-        }
-
-        // read updated sensor UAVObjects and set initial state
-        states.updated = updatedSensors;
-        updatedSensors = 0;
-
-        // fetch sensors, check values, and load into state struct
-        FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GyroSensor, gyro, x, y, z);
-        if (IS_SET(states.updated, SENSORUPDATES_gyro)) {
-            gyroRaw[0] = states.gyro[0];
-            gyroRaw[1] = states.gyro[1];
-            gyroRaw[2] = states.gyro[2];
-        }
-        FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(AccelSensor, accel, x, y, z);
-        FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(MagSensor, boardMag, x, y, z);
-        FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(AuxMagSensor, auxMag, x, y, z);
-        FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GPSVelocitySensor, vel, North, East, Down);
-        FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(BaroSensor, baro, Altitude, true);
-        FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_2_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(AirspeedSensor, airspeed, CalibratedAirspeed, TrueAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE);
-
-        // GPS position data (LLA) is not fetched here since it does not contain floats. The filter must do all checks itself
-
-        // at this point sensor state is stored in "states" with some rudimentary filtering applied
-
-        // apply all filters in the current filter chain
-        current  = filterChain;
-
-        // we are not done, re-dispatch self execution
-        runState = RUNSTATE_FILTER;
-        PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback);
-        break;
-
-    case RUNSTATE_FILTER:
-
-        if (current != NULL) {
-            filterResult result = current->filter->filter((stateFilter *)current->filter, &states);
-            if (result > alarm) {
-                alarm = result;
-            }
-            current = current->next;
-        }
-
-        // we are not done, re-dispatch self execution
-        if (!current) {
-            runState = RUNSTATE_SAVE;
-        }
-        PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback);
-        break;
-
-    case RUNSTATE_SAVE:
-
-        // the final output of filters is saved in state variables
-        // EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(GyroState, gyro, x, y, z) // replaced by performance shortcut
-        if (IS_SET(states.updated, SENSORUPDATES_gyro)) {
-            gyroDelta[0] = states.gyro[0] - gyroRaw[0];
-            gyroDelta[1] = states.gyro[1] - gyroRaw[1];
-            gyroDelta[2] = states.gyro[2] - gyroRaw[2];
-        }
-        EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(AccelState, accel, x, y, z);
-        if (IS_SET(states.updated, SENSORUPDATES_mag)) {
-            MagStateData s;
-
-            MagStateGet(&s);
-            s.x = states.mag[0];
-            s.y = states.mag[1];
-            s.z = states.mag[2];
-            switch (states.magStatus) {
-            case MAGSTATUS_OK:
-                s.Source = MAGSTATE_SOURCE_ONBOARD;
                 break;
-            case MAGSTATUS_AUX:
+            case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAG:
-                s.Source = MAGSTATE_SOURCE_AUX;
+                newFilterChain = cfmiQueue;
+                break;
+            case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAGGPSOUTDOOR:
+                newFilterChain = cfmQueue;
+                break;
+            case REVOSETTINGS_FUSIONALGORITHM_INS13INDOOR:
+                newFilterChain = ekf13iQueue;
+                break;
+            case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13:
+                newFilterChain = ekf13Queue;
                 break;
             default:
-                s.Source = MAGSTATE_SOURCE_INVALID;
+                newFilterChain = NULL;
+            }
+            // initialize filters in chain
+            current = newFilterChain;
+            bool error = 0;
+            while (current != NULL) {
+                int32_t result = current->filter->init((stateFilter *)current->filter);
+                if (result != 0) {
+                    error = 1;
+                    break;
+                }
+                current = current->next;
+            }
+            if (error) {
+                AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
+                return;
+            } else {
+                // set new fusion algortithm
+                filterChain     = newFilterChain;
+                fusionAlgorithm = revoSettings.FusionAlgorithm;
             }
-            MagStateSet(&s);
         }
+    }
 
-        EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(PositionState, pos, North, East, Down);
+    // read updated sensor UAVObjects and set initial state
-        EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(VelocityState, vel, North, East, Down);
+    states.updated = updatedSensors;
-        EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_2_DIMENSIONS(AirspeedState, airspeed, CalibratedAirspeed, TrueAirspeed);
+    updatedSensors = 0;
-        // attitude nees manual conversion from quaternion to euler
+
-        if (IS_SET(states.updated, SENSORUPDATES_attitude)) { \
+    // fetch sensors, check values, and load into state struct
-            AttitudeStateData s;
+    FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GyroSensor, gyro, x, y, z);
-            AttitudeStateGet(&s);
+    if (IS_SET(states.updated, SENSORUPDATES_gyro)) {
-            s.q1 = states.attitude[0];
+        gyroRaw[0] = states.gyro[0];
-            s.q2 = states.attitude[1];
+        gyroRaw[1] = states.gyro[1];
-            s.q3 = states.attitude[2];
+        gyroRaw[2] = states.gyro[2];
-            s.q4 = states.attitude[3];
+    }
-            Quaternion2RPY(&s.q1, &s.Roll);
+    FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(AccelSensor, accel, x, y, z);
-            AttitudeStateSet(&s);
+    FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(MagSensor, boardMag, x, y, z);
+    FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(AuxMagSensor, auxMag, x, y, z);
+    FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GPSVelocitySensor, vel, North, East, Down);
+    FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(BaroSensor, baro, Altitude, true);
+    FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_2_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(AirspeedSensor, airspeed, CalibratedAirspeed, TrueAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE);
+
+    // GPS position data (LLA) is not fetched here since it does not contain floats. The filter must do all checks itself
+
+    // at this point sensor state is stored in "states" with some rudimentary filtering applied
+
+    // apply all filters in the current filter chain
+    current = filterChain;
+
+    // we are not done, re-dispatch self execution
+
+    while (current) {
+        filterResult result = current->filter->filter((stateFilter *)current->filter, &states);
+        if (result > alarm) {
+            alarm = result;
         }
+        current = current->next;
+    }
 
-        // throttle alarms, raise alarm flags immediately
+    // the final output of filters is saved in state variables
-        // but require system to run for a while before decreasing
+    // EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(GyroState, gyro, x, y, z) // replaced by performance shortcut
-        // to prevent alarm flapping
+    if (IS_SET(states.updated, SENSORUPDATES_gyro)) {
-        if (alarm >= lastAlarm) {
+        gyroDelta[0] = states.gyro[0] - gyroRaw[0];
+        gyroDelta[1] = states.gyro[1] - gyroRaw[1];
+        gyroDelta[2] = states.gyro[2] - gyroRaw[2];
+    }
+    EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(AccelState, accel, x, y, z);
+    if (IS_SET(states.updated, SENSORUPDATES_mag)) {
+        MagStateData s;
+
+        MagStateGet(&s);
+        s.x = states.mag[0];
+        s.y = states.mag[1];
+        s.z = states.mag[2];
+        switch (states.magStatus) {
+        case MAGSTATUS_OK:
+            s.Source = MAGSTATE_SOURCE_ONBOARD;
+            break;
+        case MAGSTATUS_AUX:
+            s.Source = MAGSTATE_SOURCE_AUX;
+            break;
+        default:
+            s.Source = MAGSTATE_SOURCE_INVALID;
+        }
+        MagStateSet(&s);
+    }
+
+    EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(PositionState, pos, North, East, Down);
+    EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(VelocityState, vel, North, East, Down);
+    EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_2_DIMENSIONS(AirspeedState, airspeed, CalibratedAirspeed, TrueAirspeed);
+    // attitude nees manual conversion from quaternion to euler
+    if (IS_SET(states.updated, SENSORUPDATES_attitude)) { \
+        AttitudeStateData s;
+        AttitudeStateGet(&s);
+        s.q1 = states.attitude[0];
+        s.q2 = states.attitude[1];
+        s.q3 = states.attitude[2];
+        s.q4 = states.attitude[3];
+        Quaternion2RPY(&s.q1, &s.Roll);
+        AttitudeStateSet(&s);
+    }
+    // throttle alarms, raise alarm flags immediately
+    // but require system to run for a while before decreasing
+    // to prevent alarm flapping
+    if (alarm >= lastAlarm) {
+        lastAlarm    = alarm;
+        alarmcounter = 0;
+    } else {
+        if (alarmcounter < 100) {
+            alarmcounter++;
+        } else {
             lastAlarm    = alarm;
             alarmcounter = 0;
-        } else {
-            if (alarmcounter < 100) {
-                alarmcounter++;
-            } else {
-                lastAlarm    = alarm;
-                alarmcounter = 0;
-            }
         }
+    }
 
-        // clear alarms if everything is alright, then schedule callback execution after timeout
+    // clear alarms if everything is alright, then schedule callback execution after timeout
-        if (lastAlarm == FILTERRESULT_WARNING) {
+    if (lastAlarm == FILTERRESULT_WARNING) {
-            AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING);
+        AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING);
-        } else if (lastAlarm == FILTERRESULT_CRITICAL) {
+    } else if (lastAlarm == FILTERRESULT_CRITICAL) {
-            AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
+        AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
-        } else if (lastAlarm >= FILTERRESULT_ERROR) {
+    } else if (lastAlarm >= FILTERRESULT_ERROR) {
-            AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
+        AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
-        } else {
+    } else {
-            AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
+        AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
-        }
+    }
 
-        // we are done, re-schedule next self execution
+    if (updatedSensors) {
-        runState = RUNSTATE_LOAD;
+        PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback);
-        if (updatedSensors) {
+    } else {
-            PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback);
+        PIOS_CALLBACKSCHEDULER_Schedule(stateEstimationCallback, TIMEOUT_MS, CALLBACK_UPDATEMODE_SOONER);
-        } else {
-            PIOS_CALLBACKSCHEDULER_Schedule(stateEstimationCallback, TIMEOUT_MS, CALLBACK_UPDATEMODE_SOONER);
-        }
-        break;
     }
 }
 

flight/modules/System/systemmod.c

@@ -101,7 +101,9 @@ static enum { STACKOVERFLOW_NONE = 0, STACKOVERFLOW_WARNING = 1, STACKOVERFLOW_C
 static bool mallocFailed;
 static HwSettingsData bootHwSettings;
 static FrameType_t bootFrameType;
+#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32)
 static struct PIOS_FLASHFS_Stats fsStats;
+#endif
 
 // Private functions
 static void objectUpdatedCb(UAVObjEvent *ev);

flight/modules/Telemetry/telemetry.c

@@ -9,7 +9,7 @@
  * @{
  *
  * @file       telemetry.c
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
  * @brief      Telemetry module, handles telemetry and UAVObject updates
  * @see        The GNU Public License (GPL) Version 3
  *
@@ -30,6 +30,34 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 
+/* Telemetry uses four tasks. Two are created for the main telemetry
+ * stream called "TelTx" and "TelRx". Two are created to handle the OPLink
+ * radio connection, called "RadioTx" and "RadioRx", the latter being
+ * overridden by USB if connected.
+ *
+ * The following code uses a "local" prefix to refer to the telemetry channel
+ * associated with a port on the FC and a "radio" prefix to refer to the
+ * telemetry channel associated with the OPLink/USB.
+ *
+ * The "local" telemetry port to use is defined by PIOS_COM_TELEM_RF in
+ * PIOS_Board_Init().
+ *
+ * A UAVTalk connection instance, telemUavTalkCon, is associated with the
+ * "local" channel and another, radioUavTalkCon, with the "radio" channel.
+ * Associated with each instance is a transmit routine which will send data
+ * to the appropriate port.
+ *
+ * Data is passed on the telemetry channels using queues. If
+ * PIOS_TELEM_PRIORITY_QUEUE is defined then two queues are created, one normal
+ * priority and the other high priority.
+ *
+ * The "Tx" tasks read events first from the priority queue and then from
+ * the normal queue, passing each event to processObjEvent() which ultimately
+ * passes each event to the UAVTalk library which results in the appropriate
+ * transmit routine being called to send the data back to the recipient on
+ * the "local" or "radio" link.
+ */
+
 #include <openpilot.h>
 
 #include "telemetry.h"
@@ -53,62 +81,74 @@
 
 #ifdef PIOS_TELEM_RADIO_RX_STACK_SIZE
 #define STACK_SIZE_RADIO_RX_BYTES PIOS_TELEM_RADIO_RX_STACK_SIZE
+#define STACK_SIZE_RADIO_TX_BYTES PIOS_TELEM_RADIO_TX_STACK_SIZE
 #else
 #define STACK_SIZE_RADIO_RX_BYTES STACK_SIZE_RX_BYTES
+#define STACK_SIZE_RADIO_TX_BYTES STACK_SIZE_TX_BYTES
 #endif
 #define TASK_PRIORITY_RX          (tskIDLE_PRIORITY + 2)
 #define TASK_PRIORITY_TX          (tskIDLE_PRIORITY + 2)
 #define TASK_PRIORITY_RADRX       (tskIDLE_PRIORITY + 2)
+#define TASK_PRIORITY_RADTX       (tskIDLE_PRIORITY + 2)
 #define REQ_TIMEOUT_MS            250
 #define MAX_RETRIES               2
 #define STATS_UPDATE_PERIOD_MS    4000
 #define CONNECTION_TIMEOUT_MS     8000
 
 // Private types
+typedef struct {
+    // Determine port on which to communicate telemetry information
+    uint32_t (*getPort)();
+    // Main telemetry queue
+    xQueueHandle queue;
+#ifdef PIOS_TELEM_PRIORITY_QUEUE
+    // Priority telemetry queue
+    xQueueHandle priorityQueue;
+#endif /* PIOS_TELEM_PRIORITY_QUEUE */
+    // Transmit/receive task handles
+    xTaskHandle  txTaskHandle;
+    xTaskHandle  rxTaskHandle;
+    // Telemetry stream
+    UAVTalkConnection uavTalkCon;
+} channelContext;
 
-// Private variables
+// Main telemetry channel
-static uint32_t telemetryPort;
+static channelContext localChannel;
-#ifdef PIOS_INCLUDE_RFM22B
+static int32_t transmitLocalData(uint8_t *data, int32_t length);
-static uint32_t radioPort;
+static void registerLocalObject(UAVObjHandle obj);
-#endif
+static uint32_t localPort();
-static xQueueHandle queue;
 
-#if defined(PIOS_TELEM_PRIORITY_QUEUE)
+// OPLink telemetry channel
-static xQueueHandle priorityQueue;
+static channelContext radioChannel;
-#else
+static int32_t transmitRadioData(uint8_t *data, int32_t length);
-#define priorityQueue queue
+static void registerRadioObject(UAVObjHandle obj);
-#endif
+static uint32_t radioPort();
 
-static xTaskHandle telemetryTxTaskHandle;
+// Telemetry stats
-static xTaskHandle telemetryRxTaskHandle;
-#ifdef PIOS_INCLUDE_RFM22B
-static xTaskHandle radioRxTaskHandle;
-#endif
 static uint32_t txErrors;
 static uint32_t txRetries;
 static uint32_t timeOfLastObjectUpdate;
-static UAVTalkConnection uavTalkCon;
-#ifdef PIOS_INCLUDE_RFM22B
-static UAVTalkConnection radioUavTalkCon;
-#endif
 
-// Private functions
 static void telemetryTxTask(void *parameters);
 static void telemetryRxTask(void *parameters);
-#ifdef PIOS_INCLUDE_RFM22B
+static void updateObject(
-static void radioRxTask(void *parameters);
+    channelContext *channel,
-static int32_t transmitRadioData(uint8_t *data, int32_t length);
+    UAVObjHandle obj,
-#endif
+    int32_t eventType);
-static int32_t transmitData(uint8_t *data, int32_t length);
+static void processObjEvent(
-static void registerObject(UAVObjHandle obj);
+    channelContext *channel,
-static void updateObject(UAVObjHandle obj, int32_t eventType);
+    UAVObjEvent *ev);
-static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs);
+static int32_t setUpdatePeriod(
-static int32_t setLoggingPeriod(UAVObjHandle obj, int32_t updatePeriodMs);
+    channelContext *channel,
-static void processObjEvent(UAVObjEvent *ev);
+    UAVObjHandle obj,
+    int32_t updatePeriodMs);
+static int32_t setLoggingPeriod(
+    channelContext *channel,
+    UAVObjHandle obj,
+    int32_t updatePeriodMs);
 static void updateTelemetryStats();
 static void gcsTelemetryStatsUpdated();
-static void updateSettings();
+static void updateSettings(channelContext *channel);
-static uint32_t getComPort(bool input);
 
 /**
  * Initialise the telemetry module
@@ -117,26 +157,94 @@ static uint32_t getComPort(bool input);
  */
 int32_t TelemetryStart(void)
 {
-    // Process all registered objects and connect queue for updates
+    // Only start the local telemetry tasks if needed
-    UAVObjIterate(&registerObject);
+    if (localPort()) {
+        UAVObjIterate(&registerLocalObject);
+
+        // Listen to objects of interest
+#ifdef PIOS_TELEM_PRIORITY_QUEUE
+        GCSTelemetryStatsConnectQueue(localChannel.priorityQueue);
+#else /* PIOS_TELEM_PRIORITY_QUEUE */
+        GCSTelemetryStatsConnectQueue(localChannel.queue);
+#endif /* PIOS_TELEM_PRIORITY_QUEUE */
+        // Start telemetry tasks
+        xTaskCreate(telemetryTxTask,
+                    "TelTx",
+                    STACK_SIZE_TX_BYTES / 4,
+                    &localChannel,
+                    TASK_PRIORITY_TX,
+                    &localChannel.txTaskHandle);
+        PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYTX,
+                                       localChannel.txTaskHandle);
+        xTaskCreate(telemetryRxTask,
+                    "TelRx",
+                    STACK_SIZE_RX_BYTES / 4,
+                    &localChannel,
+                    TASK_PRIORITY_RX,
+                    &localChannel.rxTaskHandle);
+        PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYRX,
+                                       localChannel.rxTaskHandle);
+    }
+
+    // Start the telemetry tasks associated with Radio/USB
+    UAVObjIterate(&registerRadioObject);
 
     // Listen to objects of interest
-    GCSTelemetryStatsConnectQueue(priorityQueue);
+#ifdef PIOS_TELEM_PRIORITY_QUEUE
+    GCSTelemetryStatsConnectQueue(radioChannel.priorityQueue);
+#else /* PIOS_TELEM_PRIORITY_QUEUE */
+    GCSTelemetryStatsConnectQueue(radioChannel.queue);
+#endif /* PIOS_TELEM_PRIORITY_QUEUE */
 
-    // Start telemetry tasks
+    xTaskCreate(telemetryTxTask,
-    xTaskCreate(telemetryTxTask, "TelTx", STACK_SIZE_TX_BYTES / 4, NULL, TASK_PRIORITY_TX, &telemetryTxTaskHandle);
+                "RadioTx",
-    PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYTX, telemetryTxTaskHandle);
+                STACK_SIZE_RADIO_TX_BYTES / 4,
-    xTaskCreate(telemetryRxTask, "TelRx", STACK_SIZE_RX_BYTES / 4, NULL, TASK_PRIORITY_RX, &telemetryRxTaskHandle);
+                &radioChannel,
-    PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYRX, telemetryRxTaskHandle);
+                TASK_PRIORITY_RADTX,
-
+                &radioChannel.txTaskHandle);
-#ifdef PIOS_INCLUDE_RFM22B
+    PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_RADIOTX,
-    xTaskCreate(radioRxTask, "RadioRx", STACK_SIZE_RADIO_RX_BYTES / 4, NULL, TASK_PRIORITY_RADRX, &radioRxTaskHandle);
+                                   radioChannel.txTaskHandle);
-    PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_RADIORX, radioRxTaskHandle);
+    xTaskCreate(telemetryRxTask,
-#endif
+                "RadioRx",
+                STACK_SIZE_RADIO_RX_BYTES / 4,
+                &radioChannel,
+                TASK_PRIORITY_RADRX,
+                &radioChannel.rxTaskHandle);
+    PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_RADIORX,
+                                   radioChannel.rxTaskHandle);
 
     return 0;
 }
 
+/* Intialise a telemetry channel */
+void TelemetryInitializeChannel(channelContext *channel)
+{
+    // Create object queues
+    channel->queue = xQueueCreate(MAX_QUEUE_SIZE,
+                                  sizeof(UAVObjEvent));
+#if defined(PIOS_TELEM_PRIORITY_QUEUE)
+    channel->priorityQueue = xQueueCreate(MAX_QUEUE_SIZE,
+                                          sizeof(UAVObjEvent));
+#endif /* PIOS_TELEM_PRIORITY_QUEUE */
+
+    // Initialise UAVTalk
+    channel->uavTalkCon = UAVTalkInitialize(&transmitLocalData);
+
+    // Create periodic event that will be used to update the telemetry stats
+    UAVObjEvent ev;
+    memset(&ev, 0, sizeof(UAVObjEvent));
+
+#ifdef PIOS_TELEM_PRIORITY_QUEUE
+    EventPeriodicQueueCreate(&ev,
+                             channel->priorityQueue,
+                             STATS_UPDATE_PERIOD_MS);
+#else /* PIOS_TELEM_PRIORITY_QUEUE */
+    EventPeriodicQueueCreate(&ev,
+                             channel->queue,
+                             STATS_UPDATE_PERIOD_MS);
+#endif /* PIOS_TELEM_PRIORITY_QUEUE */
+}
+
 /**
  * Initialise the telemetry module
  * \return -1 if initialisation failed
@@ -144,39 +252,37 @@ int32_t TelemetryStart(void)
  */
 int32_t TelemetryInitialize(void)
 {
+    HwSettingsInitialize();
+
     FlightTelemetryStatsInitialize();
     GCSTelemetryStatsInitialize();
 
     // Initialize vars
     timeOfLastObjectUpdate = 0;
 
-    // Create object queues
+    // Reset link stats
-    queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
-#if defined(PIOS_TELEM_PRIORITY_QUEUE)
-    priorityQueue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
-#endif
-
-    // Update telemetry settings
-    telemetryPort = PIOS_COM_TELEM_RF;
-#ifdef PIOS_INCLUDE_RFM22B
-    radioPort     = PIOS_COM_RF;
-#endif
-    HwSettingsInitialize();
-    updateSettings();
-
-    // Initialise UAVTalk
-    uavTalkCon = UAVTalkInitialize(&transmitData);
-#ifdef PIOS_INCLUDE_RFM22B
-    radioUavTalkCon = UAVTalkInitialize(&transmitRadioData);
-#endif
-
-    // Create periodic event that will be used to update the telemetry stats
-    // FIXME STATS_UPDATE_PERIOD_MS is 4000ms while FlighTelemetryStats update period is 5000ms...
     txErrors  = 0;
     txRetries = 0;
-    UAVObjEvent ev;
+
-    memset(&ev, 0, sizeof(UAVObjEvent));
+    // Set channel port handlers
-    EventPeriodicQueueCreate(&ev, priorityQueue, STATS_UPDATE_PERIOD_MS);
+    localChannel.getPort = localPort;
+    radioChannel.getPort = radioPort;
+
+    // Set the local telemetry baud rate
+    updateSettings(&localChannel);
+
+    // Only initialise local channel if telemetry port enabled
+    if (localPort()) {
+        // Initialise channel
+        TelemetryInitializeChannel(&localChannel);
+        // Initialise UAVTalk
+        localChannel.uavTalkCon = UAVTalkInitialize(&transmitLocalData);
+    }
+
+    // Initialise channel
+    TelemetryInitializeChannel(&radioChannel);
+    // Initialise UAVTalk
+    radioChannel.uavTalkCon = UAVTalkInitialize(&transmitRadioData);
 
     return 0;
 }
@@ -188,22 +294,51 @@ MODULE_INITCALL(TelemetryInitialize, TelemetryStart);
  * telemetry settings.
  * \param[in] obj Object to connect
  */
-static void registerObject(UAVObjHandle obj)
+static void registerLocalObject(UAVObjHandle obj)
 {
     if (UAVObjIsMetaobject(obj)) {
         // Only connect change notifications for meta objects.  No periodic updates
-        UAVObjConnectQueue(obj, priorityQueue, EV_MASK_ALL_UPDATES);
+#ifdef PIOS_TELEM_PRIORITY_QUEUE
+        UAVObjConnectQueue(obj, localChannel.priorityQueue, EV_MASK_ALL_UPDATES);
+#else /* PIOS_TELEM_PRIORITY_QUEUE */
+        UAVObjConnectQueue(obj, localChannel.queue, EV_MASK_ALL_UPDATES);
+#endif /* PIOS_TELEM_PRIORITY_QUEUE */
     } else {
         // Setup object for periodic updates
-        updateObject(obj, EV_NONE);
+        updateObject(
+            &localChannel,
+            obj,
+            EV_NONE);
+    }
+}
+
+static void registerRadioObject(UAVObjHandle obj)
+{
+    if (UAVObjIsMetaobject(obj)) {
+        // Only connect change notifications for meta objects.  No periodic updates
+#ifdef PIOS_TELEM_PRIORITY_QUEUE
+        UAVObjConnectQueue(obj, radioChannel.priorityQueue, EV_MASK_ALL_UPDATES);
+#else /* PIOS_TELEM_PRIORITY_QUEUE */
+        UAVObjConnectQueue(obj, radioChannel.queue, EV_MASK_ALL_UPDATES);
+#endif /* PIOS_TELEM_PRIORITY_QUEUE */
+    } else {
+        // Setup object for periodic updates
+        updateObject(
+            &radioChannel,
+            obj,
+            EV_NONE);
     }
 }
 
 /**
  * Update object's queue connections and timer, depending on object's settings
+ * \param[in] telemetry channel context
  * \param[in] obj Object to updates
  */
-static void updateObject(UAVObjHandle obj, int32_t eventType)
+static void updateObject(
+    channelContext *channel,
+    UAVObjHandle obj,
+    int32_t eventType)
 {
     UAVObjMetadata metadata;
     UAVObjUpdateMode updateMode, loggingMode;
@@ -226,13 +361,15 @@ static void updateObject(UAVObjHandle obj, int32_t eventType)
     switch (updateMode) {
     case UPDATEMODE_PERIODIC:
         // Set update period
-        setUpdatePeriod(obj, metadata.telemetryUpdatePeriod);
+        setUpdatePeriod(channel,
+                        obj,
+                        metadata.telemetryUpdatePeriod);
         // Connect queue
         eventMask |= EV_UPDATED_PERIODIC | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
         break;
     case UPDATEMODE_ONCHANGE:
         // Set update period
-        setUpdatePeriod(obj, 0);
+        setUpdatePeriod(channel, obj, 0);
         // Connect queue
         eventMask |= EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
         break;
@@ -242,7 +379,9 @@ static void updateObject(UAVObjHandle obj, int32_t eventType)
             eventMask |= EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
             // Set update period on initialization and metadata change
             if (eventType == EV_NONE) {
-                setUpdatePeriod(obj, metadata.telemetryUpdatePeriod);
+                setUpdatePeriod(channel,
+                                obj,
+                                metadata.telemetryUpdatePeriod);
             }
         } else {
             // Otherwise, we just received an object update, so switch to periodic for the timeout period to prevent more updates
@@ -251,7 +390,7 @@ static void updateObject(UAVObjHandle obj, int32_t eventType)
         break;
     case UPDATEMODE_MANUAL:
         // Set update period
-        setUpdatePeriod(obj, 0);
+        setUpdatePeriod(channel, obj, 0);
         // Connect queue
         eventMask |= EV_UPDATED_MANUAL | EV_UPDATE_REQ;
         break;
@@ -259,13 +398,13 @@ static void updateObject(UAVObjHandle obj, int32_t eventType)
     switch (loggingMode) {
     case UPDATEMODE_PERIODIC:
         // Set update period
-        setLoggingPeriod(obj, metadata.loggingUpdatePeriod);
+        setLoggingPeriod(channel, obj, metadata.loggingUpdatePeriod);
         // Connect queue
         eventMask |= EV_LOGGING_PERIODIC | EV_LOGGING_MANUAL;
         break;
     case UPDATEMODE_ONCHANGE:
         // Set update period
-        setLoggingPeriod(obj, 0);
+        setLoggingPeriod(channel, obj, 0);
         // Connect queue
         eventMask |= EV_UPDATED | EV_LOGGING_MANUAL;
         break;
@@ -275,7 +414,9 @@ static void updateObject(UAVObjHandle obj, int32_t eventType)
             eventMask |= EV_UPDATED | EV_LOGGING_MANUAL;
             // Set update period on initialization and metadata change
             if (eventType == EV_NONE) {
-                setLoggingPeriod(obj, metadata.loggingUpdatePeriod);
+                setLoggingPeriod(channel,
+                                 obj,
+                                 metadata.loggingUpdatePeriod);
             }
         } else {
             // Otherwise, we just received an object update, so switch to periodic for the timeout period to prevent more updates
@@ -284,23 +425,28 @@ static void updateObject(UAVObjHandle obj, int32_t eventType)
         break;
     case UPDATEMODE_MANUAL:
         // Set update period
-        setLoggingPeriod(obj, 0);
+        setLoggingPeriod(channel, obj, 0);
         // Connect queue
         eventMask |= EV_LOGGING_MANUAL;
         break;
     }
+
     // note that all setting objects have implicitly IsPriority=true
+#ifdef PIOS_TELEM_PRIORITY_QUEUE
     if (UAVObjIsPriority(obj)) {
-        UAVObjConnectQueue(obj, priorityQueue, eventMask);
+        UAVObjConnectQueue(obj, channel->priorityQueue, eventMask);
-    } else {
+    } else
-        UAVObjConnectQueue(obj, queue, eventMask);
+#endif /* PIOS_TELEM_PRIORITY_QUEUE */
-    }
+    UAVObjConnectQueue(obj, channel->queue, eventMask);
 }
 
+
 /**
  * Processes queue events
  */
-static void processObjEvent(UAVObjEvent *ev)
+static void processObjEvent(
+    channelContext *channel,
+    UAVObjEvent *ev)
 {
     UAVObjMetadata metadata;
     UAVObjUpdateMode updateMode;
@@ -325,7 +471,10 @@ static void processObjEvent(UAVObjEvent *ev)
             // Send update to GCS (with retries)
             while (retries < MAX_RETRIES && success == -1) {
                 // call blocks until ack is received or timeout
-                success = UAVTalkSendObject(uavTalkCon, ev->obj, ev->instId, UAVObjGetTelemetryAcked(&metadata), REQ_TIMEOUT_MS);
+                success = UAVTalkSendObject(channel->uavTalkCon,
+                                            ev->obj,
+                                            ev->instId,
+                                            UAVObjGetTelemetryAcked(&metadata), REQ_TIMEOUT_MS);
                 if (success == -1) {
                     ++retries;
                 }
@@ -339,7 +488,10 @@ static void processObjEvent(UAVObjEvent *ev)
             // Request object update from GCS (with retries)
             while (retries < MAX_RETRIES && success == -1) {
                 // call blocks until update is received or timeout
-                success = UAVTalkSendObjectRequest(uavTalkCon, ev->obj, ev->instId, REQ_TIMEOUT_MS);
+                success = UAVTalkSendObjectRequest(channel->uavTalkCon,
+                                                   ev->obj,
+                                                   ev->instId,
+                                                   REQ_TIMEOUT_MS);
                 if (success == -1) {
                     ++retries;
                 }
@@ -353,11 +505,17 @@ static void processObjEvent(UAVObjEvent *ev)
         // If this is a metaobject then make necessary telemetry updates
         if (UAVObjIsMetaobject(ev->obj)) {
             // linked object will be the actual object the metadata are for
-            updateObject(UAVObjGetLinkedObj(ev->obj), EV_NONE);
+            updateObject(
+                channel,
+                UAVObjGetLinkedObj(ev->obj),
+                EV_NONE);
         } else {
             if (updateMode == UPDATEMODE_THROTTLED) {
                 // If this is UPDATEMODE_THROTTLED, the event mask changes on every event.
-                updateObject(ev->obj, ev->event);
+                updateObject(
+                    channel,
+                    ev->obj,
+                    ev->event);
             }
         }
     }
@@ -378,7 +536,10 @@ static void processObjEvent(UAVObjEvent *ev)
         }
         if (updateMode == UPDATEMODE_THROTTLED) {
             // If this is UPDATEMODE_THROTTLED, the event mask changes on every event.
-            updateObject(ev->obj, ev->event);
+            updateObject(
+                channel,
+                ev->obj,
+                ev->event);
         }
     }
 }
@@ -386,37 +547,43 @@ static void processObjEvent(UAVObjEvent *ev)
 /**
  * Telemetry transmit task, regular priority
  */
-static void telemetryTxTask(__attribute__((unused)) void *parameters)
+static void telemetryTxTask(void *parameters)
 {
+    channelContext *channel = (channelContext *)parameters;
     UAVObjEvent ev;
 
+    /* Check for a bad context */
+    if (!channel) {
+        return;
+    }
+
     // Loop forever
     while (1) {
         /**
          * Tries to empty the high priority queue before handling any standard priority item
          */
-#if defined(PIOS_TELEM_PRIORITY_QUEUE)
+#ifdef PIOS_TELEM_PRIORITY_QUEUE
         // empty priority queue, non-blocking
-        while (xQueueReceive(priorityQueue, &ev, 0) == pdTRUE) {
+        while (xQueueReceive(channel->priorityQueue, &ev, 0) == pdTRUE) {
             // Process event
-            processObjEvent(&ev);
+            processObjEvent(channel, &ev);
         }
         // check regular queue and process update - non-blocking
-        if (xQueueReceive(queue, &ev, 0) == pdTRUE) {
+        if (xQueueReceive(channel->queue, &ev, 0) == pdTRUE) {
             // Process event
-            processObjEvent(&ev);
+            processObjEvent(channel, &ev);
             // if both queues are empty, wait on priority queue for updates (1 tick) then repeat cycle
-        } else if (xQueueReceive(priorityQueue, &ev, 1) == pdTRUE) {
+        } else if (xQueueReceive(channel->priorityQueue, &ev, 1) == pdTRUE) {
             // Process event
-            processObjEvent(&ev);
+            processObjEvent(channel, &ev);
         }
 #else
         // wait on queue for updates (1 tick) then repeat cycle
-        if (xQueueReceive(queue, &ev, 1) == pdTRUE) {
+        if (xQueueReceive(channel->queue, &ev, 1) == pdTRUE) {
             // Process event
-            processObjEvent(&ev);
+            processObjEvent(channel, &ev);
         }
-#endif /* if defined(PIOS_TELEM_PRIORITY_QUEUE) */
+#endif /* PIOS_TELEM_PRIORITY_QUEUE */
     }
 }
 
@@ -424,22 +591,27 @@ static void telemetryTxTask(__attribute__((unused)) void *parameters)
 /**
  * Telemetry receive task. Processes queue events and periodic updates.
  */
-static void telemetryRxTask(__attribute__((unused)) void *parameters)
+static void telemetryRxTask(void *parameters)
 {
+    channelContext *channel = (channelContext *)parameters;
+
+    /* Check for a bad context */
+    if (!channel) {
+        return;
+    }
+
     // Task loop
     while (1) {
-        uint32_t inputPort = getComPort(true);
+        uint32_t inputPort = channel->getPort();
 
         if (inputPort) {
             // Block until data are available
-            uint8_t serial_data[1];
+            uint8_t serial_data[16];
             uint16_t bytes_to_process;
 
             bytes_to_process = PIOS_COM_ReceiveBuffer(inputPort, serial_data, sizeof(serial_data), 500);
             if (bytes_to_process > 0) {
-                for (uint8_t i = 0; i < bytes_to_process; i++) {
+                UAVTalkProcessInputStream(channel->uavTalkCon, serial_data, bytes_to_process);
-                    UAVTalkProcessInputStream(uavTalkCon, serial_data[i]);
-                }
             }
         } else {
             vTaskDelay(5);
@@ -447,30 +619,58 @@ static void telemetryRxTask(__attribute__((unused)) void *parameters)
     }
 }
 
-#ifdef PIOS_INCLUDE_RFM22B
+
 /**
- * Radio telemetry receive task. Processes queue events and periodic updates.
+ * Determine the port to be used for communication on the  telemetry channel
+ * \return com port number
  */
-static void radioRxTask(__attribute__((unused)) void *parameters)
+static uint32_t localPort()
 {
-    // Task loop
+    return PIOS_COM_TELEM_RF;
-    while (1) {
-        if (radioPort) {
-            // Block until data are available
-            uint8_t serial_data[1];
-            uint16_t bytes_to_process;
-
-            bytes_to_process = PIOS_COM_ReceiveBuffer(radioPort, serial_data, sizeof(serial_data), 500);
-            if (bytes_to_process > 0) {
-                for (uint8_t i = 0; i < bytes_to_process; i++) {
-                    UAVTalkProcessInputStream(radioUavTalkCon, serial_data[i]);
-                }
-            }
-        } else {
-            vTaskDelay(5);
-        }
-    }
 }
+
+
+/**
+ * Determine the port to be used for communication on the radio channel
+ * \return com port number
+ */
+static uint32_t radioPort()
+{
+#ifdef PIOS_INCLUDE_RFM22B
+    uint32_t port = PIOS_COM_RF;
+#else /* PIOS_INCLUDE_RFM22B */
+    uint32_t port = 0;
+#endif /* PIOS_INCLUDE_RFM22B */
+#ifdef PIOS_INCLUDE_USB
+    // if USB is connected, USB takes precedence for telemetry
+    if (PIOS_COM_Available(PIOS_COM_TELEM_USB)) {
+        port = PIOS_COM_TELEM_USB;
+    }
+#endif /* PIOS_INCLUDE_USB */
+
+    return port;
+}
+
+
+/**
+ * Transmit data buffer to the modem or USB port.
+ * \param[in] data Data buffer to send
+ * \param[in] length Length of buffer
+ * \return -1 on failure
+ * \return number of bytes transmitted on success
+ */
+static int32_t transmitLocalData(uint8_t *data, int32_t length)
+{
+    uint32_t outputPort = localChannel.getPort();
+
+    if (outputPort) {
+        return PIOS_COM_SendBuffer(outputPort, data, length);
+    }
+
+    return -1;
+}
+
+
 /**
  * Transmit data buffer to the radioport.
  * \param[in] data Data buffer to send
@@ -480,24 +680,7 @@ static void radioRxTask(__attribute__((unused)) void *parameters)
  */
 static int32_t transmitRadioData(uint8_t *data, int32_t length)
 {
-    if (radioPort) {
+    uint32_t outputPort = radioChannel.getPort();
-        return PIOS_COM_SendBuffer(radioPort, data, length);
-    }
-
-    return -1;
-}
-#endif /* PIOS_INCLUDE_RFM22B */
-
-/**
- * Transmit data buffer to the modem or USB port.
- * \param[in] data Data buffer to send
- * \param[in] length Length of buffer
- * \return -1 on failure
- * \return number of bytes transmitted on success
- */
-static int32_t transmitData(uint8_t *data, int32_t length)
-{
-    uint32_t outputPort = getComPort(false);
 
     if (outputPort) {
         return PIOS_COM_SendBuffer(outputPort, data, length);
@@ -508,12 +691,16 @@ static int32_t transmitData(uint8_t *data, int32_t length)
 
 /**
  * Set update period of object (it must be already setup for periodic updates)
+ * \param[in] telemetry channel context
  * \param[in] obj The object to update
  * \param[in] updatePeriodMs The update period in ms, if zero then periodic updates are disabled
  * \return 0 Success
  * \return -1 Failure
  */
-static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs)
+static int32_t setUpdatePeriod(
+    channelContext *channel,
+    UAVObjHandle obj,
+    int32_t updatePeriodMs)
 {
     UAVObjEvent ev;
     int32_t ret;
@@ -524,7 +711,12 @@ static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs)
     ev.event  = EV_UPDATED_PERIODIC;
     ev.lowPriority = true;
 
-    xQueueHandle targetQueue = UAVObjIsPriority(obj) ? priorityQueue : queue;
+#ifdef PIOS_TELEM_PRIORITY_QUEUE
+    xQueueHandle targetQueue = UAVObjIsPriority(obj) ? channel->priorityQueue :
+                               channel->queue;
+#else /* PIOS_TELEM_PRIORITY_QUEUE */
+    xQueueHandle targetQueue = channel->queue;
+#endif /* PIOS_TELEM_PRIORITY_QUEUE */
 
     ret = EventPeriodicQueueUpdate(&ev, targetQueue, updatePeriodMs);
     if (ret == -1) {
@@ -535,12 +727,16 @@ static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs)
 
 /**
  * Set logging update period of object (it must be already setup for periodic updates)
+ * \param[in] telemetry channel context
  * \param[in] obj The object to update
  * \param[in] updatePeriodMs The update period in ms, if zero then periodic updates are disabled
  * \return 0 Success
  * \return -1 Failure
  */
-static int32_t setLoggingPeriod(UAVObjHandle obj, int32_t updatePeriodMs)
+static int32_t setLoggingPeriod(
+    channelContext *channel,
+    UAVObjHandle obj,
+    int32_t updatePeriodMs)
 {
     UAVObjEvent ev;
     int32_t ret;
@@ -551,7 +747,12 @@ static int32_t setLoggingPeriod(UAVObjHandle obj, int32_t updatePeriodMs)
     ev.event  = EV_LOGGING_PERIODIC;
     ev.lowPriority = true;
 
-    xQueueHandle targetQueue = UAVObjIsPriority(obj) ? priorityQueue : queue;
+#ifdef PIOS_TELEM_PRIORITY_QUEUE
+    xQueueHandle targetQueue = UAVObjIsPriority(obj) ? channel->priorityQueue :
+                               channel->queue;
+#else /* PIOS_TELEM_PRIORITY_QUEUE */
+    xQueueHandle targetQueue = channel->queue;
+#endif /* PIOS_TELEM_PRIORITY_QUEUE */
 
     ret = EventPeriodicQueueUpdate(&ev, targetQueue, updatePeriodMs);
     if (ret == -1) {
@@ -590,10 +791,8 @@ static void updateTelemetryStats()
     uint32_t timeNow;
 
     // Get stats
-    UAVTalkGetStats(uavTalkCon, &utalkStats, true);
+    UAVTalkGetStats(localChannel.uavTalkCon, &utalkStats, true);
-#ifdef PIOS_INCLUDE_RFM22B
+    UAVTalkAddStats(radioChannel.uavTalkCon, &utalkStats, true);
-    UAVTalkAddStats(radioUavTalkCon, &utalkStats, true);
-#endif
 
     // Get object data
     FlightTelemetryStatsGet(&flightStats);
@@ -683,68 +882,42 @@ static void updateTelemetryStats()
  * settings, etc. Thus the HwSettings object which contains the
  * telemetry port speed is used for now.
  */
-static void updateSettings()
+static void updateSettings(channelContext *channel)
 {
-    if (telemetryPort) {
+    uint32_t port = channel->getPort();
+
+    if (port) {
         // Retrieve settings
-        uint8_t speed;
+        HwSettingsTelemetrySpeedOptions speed;
         HwSettingsTelemetrySpeedGet(&speed);
 
         // Set port speed
         switch (speed) {
         case HWSETTINGS_TELEMETRYSPEED_2400:
-            PIOS_COM_ChangeBaud(telemetryPort, 2400);
+            PIOS_COM_ChangeBaud(port, 2400);
             break;
         case HWSETTINGS_TELEMETRYSPEED_4800:
-            PIOS_COM_ChangeBaud(telemetryPort, 4800);
+            PIOS_COM_ChangeBaud(port, 4800);
             break;
         case HWSETTINGS_TELEMETRYSPEED_9600:
-            PIOS_COM_ChangeBaud(telemetryPort, 9600);
+            PIOS_COM_ChangeBaud(port, 9600);
             break;
         case HWSETTINGS_TELEMETRYSPEED_19200:
-            PIOS_COM_ChangeBaud(telemetryPort, 19200);
+            PIOS_COM_ChangeBaud(port, 19200);
             break;
         case HWSETTINGS_TELEMETRYSPEED_38400:
-            PIOS_COM_ChangeBaud(telemetryPort, 38400);
+            PIOS_COM_ChangeBaud(port, 38400);
             break;
         case HWSETTINGS_TELEMETRYSPEED_57600:
-            PIOS_COM_ChangeBaud(telemetryPort, 57600);
+            PIOS_COM_ChangeBaud(port, 57600);
             break;
         case HWSETTINGS_TELEMETRYSPEED_115200:
-            PIOS_COM_ChangeBaud(telemetryPort, 115200);
+            PIOS_COM_ChangeBaud(port, 115200);
             break;
         }
     }
 }
 
-/**
- * Determine input/output com port as highest priority available
- * @param[in] input Returns the approproate input com port if true, else the appropriate output com port
- */
-#ifdef PIOS_INCLUDE_RFM22B
-static uint32_t getComPort(bool input)
-#else
-static uint32_t getComPort(__attribute__((unused)) bool input)
-#endif
-{
-#if defined(PIOS_INCLUDE_USB)
-    // if USB is connected, USB takes precedence for telemetry
-    if (PIOS_COM_Available(PIOS_COM_TELEM_USB)) {
-        return PIOS_COM_TELEM_USB;
-    } else
-#endif /* PIOS_INCLUDE_USB */
-#ifdef PIOS_INCLUDE_RFM22B
-    // PIOS_COM_RF input is handled by a separate RX thread and therefore must be ignored
-    if (input && telemetryPort == PIOS_COM_RF) {
-        return 0;
-    } else
-#endif /* PIOS_INCLUDE_RFM22B */
-    if (PIOS_COM_Available(telemetryPort)) {
-        return telemetryPort;
-    } else {
-        return 0;
-    }
-}
 
 /**
  * @}

flight/modules/TxPID/txpid.c

@@ -55,6 +55,9 @@
 #include "accessorydesired.h"
 #include "manualcontrolcommand.h"
 #include "stabilizationsettings.h"
+#ifdef REVOLUTION
+#include "altitudeholdsettings.h"
+#endif
 #include "stabilizationbank.h"
 #include "stabilizationsettingsbank1.h"
 #include "stabilizationsettingsbank2.h"
@@ -95,6 +98,10 @@ int32_t TxPIDInitialize(void)
     bool txPIDEnabled;
     HwSettingsOptionalModulesData optionalModules;
 
+#ifdef REVOLUTION
+    AltitudeHoldSettingsInitialize();
+#endif
+
     HwSettingsInitialize();
     HwSettingsOptionalModulesGet(&optionalModules);
 
@@ -188,10 +195,17 @@ static void updatePIDs(UAVObjEvent *ev)
     }
     StabilizationSettingsData stab;
     StabilizationSettingsGet(&stab);
+#ifdef REVOLUTION
+    AltitudeHoldSettingsData altitude;
+    AltitudeHoldSettingsGet(&altitude);
+#endif
     AccessoryDesiredData accessory;
 
-    uint8_t needsUpdateBank = 0;
+    uint8_t needsUpdateBank     = 0;
-    uint8_t needsUpdateStab = 0;
+    uint8_t needsUpdateStab     = 0;
+#ifdef REVOLUTION
+    uint8_t needsUpdateAltitude = 0;
+#endif
 
     // Loop through every enabled instance
     for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) {
@@ -351,6 +365,23 @@ static void updatePIDs(UAVObjEvent *ev)
             case TXPIDSETTINGS_PIDS_ACROPLUSFACTOR:
                 needsUpdateBank |= update(&bank.AcroInsanityFactor, value);
                 break;
+#ifdef REVOLUTION
+            case TXPIDSETTINGS_PIDS_ALTITUDEPOSKP:
+                needsUpdateAltitude |= update(&altitude.VerticalPosP, value);
+                break;
+            case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYKP:
+                needsUpdateAltitude |= update(&altitude.VerticalVelPID.Kp, value);
+                break;
+            case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYKI:
+                needsUpdateAltitude |= update(&altitude.VerticalVelPID.Ki, value);
+                break;
+            case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYKD:
+                needsUpdateAltitude |= update(&altitude.VerticalVelPID.Kd, value);
+                break;
+            case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYBETA:
+                needsUpdateAltitude |= update(&altitude.VerticalVelPID.Beta, value);
+                break;
+#endif
             default:
                 PIOS_Assert(0);
             }
@@ -359,6 +390,11 @@ static void updatePIDs(UAVObjEvent *ev)
     if (needsUpdateStab) {
         StabilizationSettingsSet(&stab);
     }
+#ifdef REVOLUTION
+    if (needsUpdateAltitude) {
+        AltitudeHoldSettingsSet(&altitude);
+    }
+#endif
     if (needsUpdateBank) {
         switch (inst.BankNumber) {
         case 0:

flight/modules/gpsp/gps9maghandler.c

@@ -49,9 +49,8 @@ void handleMag()
 
     if (PIOS_HMC5x83_ReadMag(onboard_mag, mag) == 0) {
         MagUbxPkt magPkt;
-        // swap axis so that if side with connector is aligned to revo side with connectors, mags data are aligned
+        magPkt.fragments.data.X = mag[0];
-        magPkt.fragments.data.X = -mag[1];
+        magPkt.fragments.data.Y = mag[1];
-        magPkt.fragments.data.Y = mag[0];
         magPkt.fragments.data.Z = mag[2];
         magPkt.fragments.data.status = 1;
         ubx_buildPacket(&magPkt.packet, UBX_OP_CUST_CLASS, UBX_OP_MAG, sizeof(MagData));

flight/pios/common/libraries/msheap/msheap.c

@@ -194,13 +194,18 @@ msheap_init(heap_handle_t *heap, void *base, void *limit)
 }
 
 void *
-msheap_alloc(heap_handle_t *heap, uint32_t size)
+msheap_alloc(heap_handle_t *heap, void *ptr, uint32_t size)
 {
     marker_t    cursor;
     marker_t    best;
+    uint32_t    copy_data = 0;
+    uint16_t    old_size = 0;
 
     ASSERT(3, msheap_check(heap));
 
+    if (size == 0)
+        return 0;
+
     /* convert the passed-in size to the number of marker-size units we need to allocate */
     size += marker_size;
     size = round_up(size, marker_size);
@@ -210,6 +215,39 @@ msheap_alloc(heap_handle_t *heap, uint32_t size)
     if (size > heap->heap_free)
         return 0;
 
+    /* realloc */
+    if (ptr != 0) {
+
+        best = (marker_t)ptr - 1;
+        ASSERT(0, region_check(heap, best));
+        ASSERT(3, msheap_check(heap));
+
+#ifdef HEAP_REALLOC_FREE_UNUSED_AREA
+
+        if (best->next.size == size)
+            goto done;
+
+        if (best->next.size > size) {
+            /* this region is free, mark it accordingly */
+            best->next.free = 1;
+            (best + best->next.size)->prev.free = 1;
+
+            traceFREE( ptr, best->next.size );
+
+            /* account for space we are freeing */
+            heap->heap_free += best->next.size;
+
+            goto split;
+        }
+#else
+        if (best->next.size >= size)
+            goto done;
+#endif
+        old_size = best->next.size;
+        msheap_free(heap, ptr);
+        copy_data = 1;
+    }
+
     /* simple single-pass best-fit search */
 restart:
     cursor = heap->free_hint;
@@ -242,13 +280,28 @@ restart:
         /* no space */
         return 0;
     }
+#ifdef HEAP_REALLOC_FREE_UNUSED_AREA
+split:
+#endif
 
     /* split the free region to make space */
     split_region(heap, best, size);
 
     /* update free space counter */
     heap->heap_free -= size;
+
+done:
     traceMALLOC( (void *)(best + 1), size );
+
+    /* Copy data that might be reused */
+    if (copy_data && ptr) {
+        size = old_size;
+        size = size - 1;
+        size *= marker_size;
+        for(uint32_t i=0 ; i < size; i++)
+            ((uint8_t *)(best + 1))[i] = ((uint8_t *)ptr)[i];
+    }
+
     /* and return a pointer to the allocated region */
     return (void *)(best + 1);
 }

flight/pios/common/libraries/msheap/msheap.h

@@ -116,7 +116,7 @@ extern void msheap_init(heap_handle_t *heap, void *base, void *limit);
  *
  * @param   size        The number of bytes required (more may be allocated).
  */
-extern void *msheap_alloc(heap_handle_t *heap, uint32_t size);
+extern void *msheap_alloc(heap_handle_t *heap, void *ptr, uint32_t size);
 
 /**
  * Free memory back to the heap.

flight/pios/common/libraries/msheap/pios_msheap.c

@@ -74,15 +74,15 @@ heap_handle_t fast_heap;
 extern void vApplicationMallocFailedHook(void) __attribute__((weak));
 
 void *
-pios_general_malloc(size_t s, bool use_fast_heap)
+pios_general_malloc(void *ptr, size_t s, bool use_fast_heap)
 {
 	void *p;
 
 	vPortEnterCritical();
 	if(use_fast_heap){
-		p = msheap_alloc(&fast_heap, s);
+		p = msheap_alloc(&fast_heap, ptr, s);
 	} else {
-		p = msheap_alloc(&sram_heap, s);
+		p = msheap_alloc(&sram_heap, ptr, s);
 	}
 	vPortExitCritical();
 
@@ -95,13 +95,13 @@ pios_general_malloc(size_t s, bool use_fast_heap)
 void *
 pvPortMalloc(size_t s)
 {
-	return pios_general_malloc(s, true);
+	return pios_general_malloc(NULL, s, true);
 }
 
 void *
 pvPortMallocStack(size_t s)
 {
-	return pios_general_malloc(s, false);
+	return pios_general_malloc(NULL, s, false);
 }
 
 void
@@ -154,7 +154,7 @@ malloc(size_t size)
 		heap_init_done = 1;
 	}
 
-	return msheap_alloc(sram_heap, size);
+	return msheap_alloc(NULL, sram_heap, size);
 }
 
 void

flight/pios/common/pios_com.c

@@ -283,6 +283,56 @@ int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud)
     return 0;
 }
 
+/**
+ * Set control lines associated with the port
+ * \param[in] port COM port
+ * \param[in] mask Lines to change
+ * \param[in] state New state for lines
+ * \return -1 if port not available
+ * \return 0 on success
+ */
+int32_t PIOS_COM_SetCtrlLine(uint32_t com_id, uint32_t mask, uint32_t state)
+{
+    struct pios_com_dev *com_dev = (struct pios_com_dev *)com_id;
+
+    if (!PIOS_COM_validate(com_dev)) {
+        /* Undefined COM port for this board (see pios_board.c) */
+        return -1;
+    }
+
+    /* Invoke the driver function if it exists */
+    if (com_dev->driver->set_ctrl_line) {
+        com_dev->driver->set_ctrl_line(com_dev->lower_id, mask, state);
+    }
+
+    return 0;
+}
+
+/**
+ * Set control lines associated with the port
+ * \param[in] port COM port
+ * \param[in] ctrl_line_cb Callback function
+ * \param[in] context context to pass to the callback function
+ * \return -1 if port not available
+ * \return 0 on success
+ */
+int32_t PIOS_COM_RegisterCtrlLineCallback(uint32_t com_id, pios_com_callback_ctrl_line ctrl_line_cb, uint32_t context)
+{
+    struct pios_com_dev *com_dev = (struct pios_com_dev *)com_id;
+
+    if (!PIOS_COM_validate(com_dev)) {
+        /* Undefined COM port for this board (see pios_board.c) */
+        return -1;
+    }
+
+    /* Invoke the driver function if it exists */
+    if (com_dev->driver->bind_ctrl_line_cb) {
+        com_dev->driver->bind_ctrl_line_cb(com_dev->lower_id, ctrl_line_cb, context);
+    }
+
+    return 0;
+}
+
 
 static int32_t PIOS_COM_SendBufferNonBlockingInternal(struct pios_com_dev *com_dev, const uint8_t *buffer, uint16_t len)
 {

flight/pios/common/pios_hmc5x83.c

@@ -224,7 +224,7 @@ int32_t PIOS_HMC5x83_ReadMag(pios_hmc5x83_dev_t handler, int16_t out[3])
 
     dev->data_ready = false;
     uint8_t buffer[6];
-    int32_t temp;
+    int16_t temp[3];
     int32_t sensitivity;
 
     if (dev->cfg->Driver->Read(handler, PIOS_HMC5x83_DATAOUT_XMSB_REG, buffer, 6) != 0) {
@@ -259,16 +259,54 @@ int32_t PIOS_HMC5x83_ReadMag(pios_hmc5x83_dev_t handler, int16_t out[3])
     default:
         PIOS_Assert(0);
     }
-
     for (int i = 0; i < 3; i++) {
-        temp   = ((int16_t)((uint16_t)buffer[2 * i] << 8)
+        int16_t v = ((int16_t)((uint16_t)buffer[2 * i] << 8)
-                  + buffer[2 * i + 1]) * 1000 / sensitivity;
+                     + buffer[2 * i + 1]) * 1000 / sensitivity;
-        out[i] = temp;
+        temp[i] = v;
+    }
+
+    switch (dev->cfg->Orientation) {
+    case PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP:
+        out[0] = temp[2];
+        out[1] = temp[0];
+        out[2] = -temp[1];
+        break;
+    case PIOS_HMC5X83_ORIENTATION_SOUTH_EAST_UP:
+        out[0] = -temp[0];
+        out[1] = temp[2];
+        out[2] = -temp[1];
+        break;
+    case PIOS_HMC5X83_ORIENTATION_WEST_SOUTH_UP:
+        out[0] = -temp[2];
+        out[1] = -temp[0];
+        out[2] = -temp[1];
+        break;
+    case PIOS_HMC5X83_ORIENTATION_NORTH_WEST_UP:
+        out[0] = temp[0];
+        out[1] = -temp[2];
+        out[2] = -temp[1];
+        break;
+    case PIOS_HMC5X83_ORIENTATION_EAST_SOUTH_DOWN:
+        out[0] = temp[2];
+        out[1] = -temp[0];
+        out[2] = temp[1];
+        break;
+    case PIOS_HMC5X83_ORIENTATION_SOUTH_WEST_DOWN:
+        out[0] = -temp[0];
+        out[1] = -temp[2];
+        out[2] = temp[1];
+        break;
+    case PIOS_HMC5X83_ORIENTATION_WEST_NORTH_DOWN:
+        out[0] = -temp[2];
+        out[1] = temp[0];
+        out[2] = temp[1];
+        break;
+    case PIOS_HMC5X83_ORIENTATION_NORTH_EAST_DOWN:
+        out[0] = temp[0];
+        out[1] = temp[2];
+        out[2] = temp[1];
+        break;
     }
-    // Data reads out as X,Z,Y
-    temp   = out[2];
-    out[2] = out[1];
-    out[1] = temp;
 
     // This should not be necessary but for some reason it is coming out of continuous conversion mode
     dev->cfg->Driver->Write(handler, PIOS_HMC5x83_MODE_REG, PIOS_HMC5x83_MODE_CONTINUOUS);

flight/pios/common/pios_mem.c

@@ -29,17 +29,32 @@
 
 #ifdef PIOS_TARGET_PROVIDES_FAST_HEAP
 // relies on pios_general_malloc to perform the allocation (i.e. pios_msheap.c)
-extern void *pios_general_malloc(size_t size, bool fastheap);
+extern void *pios_general_malloc(void *ptr, size_t size, bool fastheap);
 
 void *pios_fastheapmalloc(size_t size)
 {
-    return pios_general_malloc(size, true);
+    return pios_general_malloc(NULL, size, true);
 }
 
 
 void *pios_malloc(size_t size)
 {
-    return pios_general_malloc(size, false);
+    return pios_general_malloc(NULL, size, false);
+}
+
+void *pios_realloc(__attribute__((unused)) void *ptr, __attribute__((unused)) size_t size)
+{
+#ifdef PIOS_INCLUDE_REALLOC
+    return pios_general_malloc(ptr, size, false);
+
+#else
+    // Not allowed to use realloc without the proper define PIOS_INCLUDE_REALLOC set
+    while (1) {
+        ;
+    }
+    return NULL;
+
+#endif
 }
 
 void pios_free(void *p)
@@ -47,7 +62,7 @@ void pios_free(void *p)
     vPortFree(p);
 }
 
-#else
+#else /* ifdef PIOS_TARGET_PROVIDES_FAST_HEAP */
 // demand to pvPortMalloc implementation
 void *pios_fastheapmalloc(size_t size)
 {
@@ -60,6 +75,21 @@ void *pios_malloc(size_t size)
     return pvPortMalloc(size);
 }
 
+void *pios_realloc(__attribute__((unused)) void *ptr, __attribute__((unused)) size_t size)
+{
+#ifdef PIOS_INCLUDE_REALLOC
+    return pvPortMalloc(size);
+
+#else
+    // Not allowed to use realloc without the proper define PIOS_INCLUDE_REALLOC set
+    while (1) {
+        ;
+    }
+    return NULL;
+
+#endif
+}
+
 void pios_free(void *p)
 {
     vPortFree(p);

flight/pios/common/pios_mpxv.c

@@ -68,8 +68,8 @@ uint16_t PIOS_MPXV_Calibrate(PIOS_MPXV_descriptor *desc, uint16_t measurement)
  */
 float PIOS_MPXV_CalcAirspeed(PIOS_MPXV_descriptor *desc, uint16_t measurement)
 {
-    // Calculate dynamic pressure, as per docs
+    // Calculate dynamic pressure, as per docs - Apply scale factor (voltage divider)
-    float Qc = 3.3f / 4096.0f * (float)(measurement - desc->zeroPoint);
+    float Qc = 3.3f / 4096.0f * (float)((measurement - desc->zeroPoint) * desc->scale);
 
     // Saturate Qc on the lower bound, in order to make sure we don't have negative airspeeds. No need
     // to saturate on the upper bound, we'll handle that later with calibratedAirspeed.

flight/pios/common/pios_ms5611.c

@@ -42,7 +42,9 @@
 #ifndef PIOS_MS5611_SLOW_TEMP_RATE
 #define PIOS_MS5611_SLOW_TEMP_RATE 1
 #endif
-
+// Running moving average smoothing factor
+#define PIOS_MS5611_TEMP_SMOOTHING 10
+//
 /* Local Types */
 typedef struct {
     uint16_t C[6];
@@ -72,6 +74,7 @@ static uint32_t RawTemperature;
 static uint32_t RawPressure;
 static int64_t Pressure;
 static int64_t Temperature;
+static int64_t FilteredTemperature = INT32_MIN;
 static int32_t lastConversionStart;
 
 static uint32_t conversionDelayMs;
@@ -247,6 +250,12 @@ int32_t PIOS_MS5611_ReadADC(void)
         // Actual temperature (-40…85°C with 0.01°C resolution)
         // TEMP = 20°C + dT * TEMPSENS = 2000 + dT * C6 / 2^23
         Temperature = 2000l + ((deltaTemp * CalibData.C[5]) / POW2(23));
+        if (FilteredTemperature != INT32_MIN) {
+            FilteredTemperature = (FilteredTemperature * (PIOS_MS5611_TEMP_SMOOTHING - 1)
+                                   + Temperature) / PIOS_MS5611_TEMP_SMOOTHING;
+        } else {
+            FilteredTemperature = Temperature;
+        }
     } else {
         int64_t Offset;
         int64_t Sens;
@@ -259,21 +268,21 @@ int32_t PIOS_MS5611_ReadADC(void)
             return -2;
         }
         // check if temperature is less than 20°C
-        if (Temperature < 2000) {
+        if (FilteredTemperature < 2000) {
             // Apply compensation
             // T2 = dT^2 / 2^31
             // OFF2 = 5 ⋅ (TEMP – 2000)^2/2
             // SENS2 = 5 ⋅ (TEMP – 2000)^2/2^2
 
-            int64_t tcorr = (Temperature - 2000) * (Temperature - 2000);
+            int64_t tcorr = (FilteredTemperature - 2000) * (FilteredTemperature - 2000);
             Offset2 = (5 * tcorr) / 2;
             Sens2   = (5 * tcorr) / 4;
             compensation_t2 = (deltaTemp * deltaTemp) >> 31;
             // Apply the "Very low temperature compensation" when temp is less than -15°C
-            if (Temperature < -1500) {
+            if (FilteredTemperature < -1500) {
                 // OFF2 = OFF2 + 7 ⋅ (TEMP + 1500)^2
                 // SENS2 = SENS2 + 11 ⋅ (TEMP + 1500)^2 / 2
-                int64_t tcorr2 = (Temperature + 1500) * (Temperature + 1500);
+                int64_t tcorr2 = (FilteredTemperature + 1500) * (FilteredTemperature + 1500);
                 Offset2 += 7 * tcorr2;
                 Sens2   += (11 * tcorr2) / 2;
             }
@@ -302,7 +311,7 @@ int32_t PIOS_MS5611_ReadADC(void)
 static float PIOS_MS5611_GetTemperature(void)
 {
     // Apply the second order low and very low temperature compensation offset
-    return ((float)(Temperature - compensation_t2)) / 100.0f;
+    return ((float)(FilteredTemperature - compensation_t2)) / 100.0f;
 }
 
 /**

flight/pios/common/pios_oplinkrcvr.c

@@ -34,6 +34,7 @@
 
 #include <uavobjectmanager.h>
 #include <oplinkreceiver.h>
+#include <oplinkstatus.h>
 #include <pios_oplinkrcvr_priv.h>
 
 static OPLinkReceiverData oplinkreceiverdata;
@@ -41,9 +42,11 @@ static OPLinkReceiverData oplinkreceiverdata;
 /* Provide a RCVR driver */
 static int32_t PIOS_OPLinkRCVR_Get(uint32_t rcvr_id, uint8_t channel);
 static void PIOS_oplinkrcvr_Supervisor(uint32_t ppm_id);
+static uint8_t PIOS_OPLinkRCVR_Quality_Get(uint32_t oplinkrcvr_id);
 
 const struct pios_rcvr_driver pios_oplinkrcvr_rcvr_driver = {
-    .read = PIOS_OPLinkRCVR_Get,
+    .read        = PIOS_OPLinkRCVR_Get,
+    .get_quality = PIOS_OPLinkRCVR_Quality_Get
 };
 
 /* Local Variables */
@@ -186,6 +189,16 @@ static void PIOS_oplinkrcvr_Supervisor(uint32_t oplinkrcvr_id)
     oplinkrcvr_dev->Fresh = false;
 }
 
+static uint8_t PIOS_OPLinkRCVR_Quality_Get(__attribute__((unused)) uint32_t oplinkrcvr_id)
+{
+    uint8_t oplink_quality;
+
+    OPLinkStatusLinkQualityGet(&oplink_quality);
+
+    /* link_status is in the range 0-128, so scale to a % */
+    return oplink_quality * 100 / 128;
+}
+
 #endif /* PIOS_INCLUDE_OPLINKRCVR */
 
 /**

flight/pios/common/pios_rcvr.c

@@ -113,6 +113,34 @@ int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel)
     return rcvr_dev->driver->read(rcvr_dev->lower_id, channel);
 }
 
+/**
+ * @brief Reads input quality from the appropriate driver
+ * @param[in] rcvr_id driver to read from
+ * @returns received signal quality expressed as a %
+ *  @retval PIOS_RCVR_NODRIVER driver was not initialized
+ */
+uint8_t PIOS_RCVR_GetQuality(uint32_t rcvr_id)
+{
+    if (rcvr_id == 0) {
+        return PIOS_RCVR_NODRIVER;
+    }
+
+    struct pios_rcvr_dev *rcvr_dev = (struct pios_rcvr_dev *)rcvr_id;
+
+    if (!PIOS_RCVR_validate(rcvr_dev)) {
+        /* Undefined RCVR port for this board (see pios_board.c) */
+        /* As no receiver is available assume min */
+        return 0;
+    }
+
+    if (!rcvr_dev->driver->get_quality) {
+        /* If no quality is available assume max */
+        return 100;
+    }
+
+    return rcvr_dev->driver->get_quality(rcvr_dev->lower_id);
+}
+
 /**
  * @brief Get a semaphore that signals when a new sample is available.
  * @param[in] rcvr_id driver to read from

flight/pios/common/pios_rfm22b.c

@@ -1376,7 +1376,7 @@ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
         }
 
         // Wait 1ms if not.
-        PIOS_DELAY_WaitmS(1);
+        vTaskDelay(1 + (1 / (portTICK_RATE_MS + 1)));
     }
 
     // ****************
@@ -1481,6 +1481,15 @@ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
     // x-nibbles rx preamble detection
     rfm22_write(rfm22b_dev, RFM22_preamble_detection_ctrl1, RX_PREAMBLE_NIBBLES << 3);
 
+    // Release the bus
+    rfm22_releaseBus(rfm22b_dev);
+
+    // Yield the CPU.
+    vTaskDelay(1 + (1 / (portTICK_RATE_MS + 1)));
+
+    // Claim the SPI bus.
+    rfm22_claimBus(rfm22b_dev);
+
     // header control - using a 4 by header with broadcast of 0xffffffff
     rfm22_write(rfm22b_dev, RFM22_header_control1,
                 RFM22_header_cntl1_bcen_0 |
@@ -1530,6 +1539,9 @@ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
     // Release the bus
     rfm22_releaseBus(rfm22b_dev);
 
+    // Yield the CPU.
+    vTaskDelay(1 + (1 / (portTICK_RATE_MS + 1)));
+
     // Initialize the frequency and datarate to te default.
     rfm22_setNominalCarrierFrequency(rfm22b_dev, 0);
     pios_rfm22_setDatarate(rfm22b_dev);

flight/pios/common/pios_sbus.c

@@ -32,6 +32,10 @@
 
 #ifdef PIOS_INCLUDE_SBUS
 
+// Define to report number of frames since last dropped instead of weighted ave
+#undef SBUS_GOOD_FRAME_COUNT
+
+#include <uavobjectmanager.h>
 #include "pios_sbus_priv.h"
 
 /* Forward Declarations */
@@ -42,11 +46,12 @@ static uint16_t PIOS_SBus_RxInCallback(uint32_t context,
                                        uint16_t *headroom,
                                        bool *need_yield);
 static void PIOS_SBus_Supervisor(uint32_t sbus_id);
-
+static uint8_t PIOS_SBus_Quality_Get(uint32_t rcvr_id);
 
 /* Local Variables */
 const struct pios_rcvr_driver pios_sbus_rcvr_driver = {
-    .read = PIOS_SBus_Get,
+    .read        = PIOS_SBus_Get,
+    .get_quality = PIOS_SBus_Quality_Get
 };
 
 enum pios_sbus_dev_magic {
@@ -60,8 +65,17 @@ struct pios_sbus_state {
     uint8_t  failsafe_timer;
     uint8_t  frame_found;
     uint8_t  byte_count;
+    float    quality;
+#ifdef SBUS_GOOD_FRAME_COUNT
+    uint8_t  frame_count;
+#endif /* SBUS_GOOD_FRAME_COUNT */
 };
 
+/* With an S.Bus frame rate of 7ms (130Hz) averaging over 26 samples
+ * gives about a 200ms response.
+ */
+#define SBUS_FL_WEIGHTED_AVE 26
+
 struct pios_sbus_dev {
     enum pios_sbus_dev_magic   magic;
     const struct pios_sbus_cfg *cfg;
@@ -120,6 +134,10 @@ static void PIOS_SBus_ResetState(struct pios_sbus_state *state)
     state->receive_timer  = 0;
     state->failsafe_timer = 0;
     state->frame_found    = 0;
+    state->quality = 0.0f;
+#ifdef SBUS_GOOD_FRAME_COUNT
+    state->frame_count    = 0;
+#endif /* SBUS_GOOD_FRAME_COUNT */
     PIOS_SBus_ResetChannels(state);
 }
 
@@ -251,18 +269,42 @@ static void PIOS_SBus_UpdateState(struct pios_sbus_state *state, uint8_t b)
         state->byte_count++;
     } else {
         if (b == SBUS_EOF_BYTE || (b & SBUS_R7008SB_EOF_COUNTER_MASK) == 0) {
+#ifndef SBUS_GOOD_FRAME_COUNT
+            /* Quality trend is towards 0% by default*/
+            uint8_t quality_trend = 0;
+#endif /* SBUS_GOOD_FRAME_COUNT */
+
             /* full frame received */
             uint8_t flags = state->received_data[SBUS_FRAME_LENGTH - 3];
             if (flags & SBUS_FLAG_FL) {
                 /* frame lost, do not update */
-            } else if (flags & SBUS_FLAG_FS) {
+#ifdef SBUS_GOOD_FRAME_COUNT
-                /* failsafe flag active */
+                state->quality     = state->frame_count;
-                PIOS_SBus_ResetChannels(state);
+                state->frame_count = 0;
+#endif /* SBUS_GOOD_FRAME_COUNT */
             } else {
-                /* data looking good */
+#ifdef SBUS_GOOD_FRAME_COUNT
-                PIOS_SBus_UnrollChannels(state);
+                if (++state->frame_count == 255) {
-                state->failsafe_timer = 0;
+                    state->quality = state->frame_count--;
+                }
+#else /* SBUS_GOOD_FRAME_COUNT */
+                /* Quality trend is towards 100% */
+                quality_trend = 100;
+#endif /* SBUS_GOOD_FRAME_COUNT */
+                if (flags & SBUS_FLAG_FS) {
+                    /* failsafe flag active */
+                    PIOS_SBus_ResetChannels(state);
+                } else {
+                    /* data looking good */
+                    PIOS_SBus_UnrollChannels(state);
+                    state->failsafe_timer = 0;
+                }
             }
+#ifndef SBUS_GOOD_FRAME_COUNT
+            /* Present quality as a weighted average of good frames */
+            state->quality = ((state->quality * (SBUS_FL_WEIGHTED_AVE - 1)) +
+                              quality_trend) / SBUS_FL_WEIGHTED_AVE;
+#endif /* SBUS_GOOD_FRAME_COUNT */
         } else {
             /* discard whole frame */
         }
@@ -341,6 +383,19 @@ static void PIOS_SBus_Supervisor(uint32_t sbus_id)
     }
 }
 
+static uint8_t PIOS_SBus_Quality_Get(uint32_t sbus_id)
+{
+    struct pios_sbus_dev *sbus_dev = (struct pios_sbus_dev *)sbus_id;
+
+    bool valid = PIOS_SBus_Validate(sbus_dev);
+
+    PIOS_Assert(valid);
+
+    struct pios_sbus_state *state = &(sbus_dev->state);
+
+    return (uint8_t)(state->quality + 0.5f);
+}
+
 #endif /* PIOS_INCLUDE_SBUS */
 
 /**

flight/pios/common/pios_srxl.c

@@ -0,0 +1,365 @@
+/**
+ ******************************************************************************
+ * @addtogroup PIOS PIOS Core hardware abstraction layer
+ * @{
+ * @addtogroup PIOS_SRXL Multiplex SRXL receiver functions
+ * @brief Code to read Multiplex SRXL receiver serial stream
+ * @{
+ *
+ * @file       pios_srxl.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Code to read Multiplex SRXL receiver serial stream
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include "pios.h"
+
+#ifdef PIOS_INCLUDE_SRXL
+
+#include "pios_srxl_priv.h"
+
+// #define PIOS_INSTRUMENT_MODULE
+#include <pios_instrumentation_helper.h>
+
+PERF_DEFINE_COUNTER(crcFailureCount);
+PERF_DEFINE_COUNTER(failsafeCount);
+PERF_DEFINE_COUNTER(successfulCount);
+PERF_DEFINE_COUNTER(messageUnrollTimer);
+PERF_DEFINE_COUNTER(messageReceiveRate);
+PERF_DEFINE_COUNTER(receivedBytesCount);
+PERF_DEFINE_COUNTER(frameStartCount);
+PERF_DEFINE_COUNTER(frameAbortCount);
+PERF_DEFINE_COUNTER(completeMessageCount);
+
+/* Forward Declarations */
+static int32_t PIOS_SRXL_Get(uint32_t rcvr_id, uint8_t channel);
+static uint16_t PIOS_SRXL_RxInCallback(uint32_t context,
+                                       uint8_t *buf,
+                                       uint16_t buf_len,
+                                       uint16_t *headroom,
+                                       bool *need_yield);
+static void PIOS_SRXL_Supervisor(uint32_t srxl_id);
+
+
+/* Local Variables */
+const struct pios_rcvr_driver pios_srxl_rcvr_driver = {
+    .read = PIOS_SRXL_Get,
+};
+
+enum pios_srxl_dev_magic {
+    PIOS_SRXL_DEV_MAGIC = 0x55545970,
+};
+
+struct pios_srxl_state {
+    uint16_t channel_data[PIOS_SRXL_NUM_INPUTS];
+    uint8_t  received_data[SRXL_FRAME_LENGTH];
+    uint8_t  receive_timer;
+    uint8_t  failsafe_timer;
+    uint8_t  frame_found;
+    uint8_t  byte_count;
+    uint8_t  data_bytes;
+};
+
+struct pios_srxl_dev {
+    enum pios_srxl_dev_magic magic;
+    struct pios_srxl_state   state;
+};
+
+/* Allocate S.Bus device descriptor */
+#if defined(PIOS_INCLUDE_FREERTOS)
+static struct pios_srxl_dev *PIOS_SRXL_Alloc(void)
+{
+    struct pios_srxl_dev *srxl_dev;
+
+    srxl_dev = (struct pios_srxl_dev *)pios_malloc(sizeof(*srxl_dev));
+    if (!srxl_dev) {
+        return NULL;
+    }
+
+    srxl_dev->magic = PIOS_SRXL_DEV_MAGIC;
+    return srxl_dev;
+}
+#else
+static struct pios_srxl_dev pios_srxl_devs[PIOS_SRXL_MAX_DEVS];
+static uint8_t pios_srxl_num_devs;
+static struct pios_srxl_dev *PIOS_SRXL_Alloc(void)
+{
+    struct pios_srxl_dev *srxl_dev;
+
+    if (pios_srxl_num_devs >= PIOS_SRXL_MAX_DEVS) {
+        return NULL;
+    }
+
+    srxl_dev = &pios_srxl_devs[pios_srxl_num_devs++];
+    srxl_dev->magic = PIOS_SRXL_DEV_MAGIC;
+
+    return srxl_dev;
+}
+#endif /* if defined(PIOS_INCLUDE_FREERTOS) */
+
+/* Validate SRXL device descriptor */
+static bool PIOS_SRXL_Validate(struct pios_srxl_dev *srxl_dev)
+{
+    return srxl_dev->magic == PIOS_SRXL_DEV_MAGIC;
+}
+
+/* Reset channels in case of lost signal or explicit failsafe receiver flag */
+static void PIOS_SRXL_ResetChannels(struct pios_srxl_state *state)
+{
+    for (int i = 0; i < PIOS_SRXL_NUM_INPUTS; i++) {
+        state->channel_data[i] = PIOS_RCVR_TIMEOUT;
+    }
+}
+
+/* Reset SRXL receiver state */
+static void PIOS_SRXL_ResetState(struct pios_srxl_state *state)
+{
+    state->receive_timer  = 0;
+    state->failsafe_timer = 0;
+    state->frame_found    = 0;
+    state->data_bytes     = 0;
+    PIOS_SRXL_ResetChannels(state);
+}
+
+/* Initialize SRXL receiver interface */
+int32_t PIOS_SRXL_Init(uint32_t *srxl_id,
+                       const struct pios_com_driver *driver,
+                       uint32_t lower_id)
+{
+    PIOS_DEBUG_Assert(srxl_id);
+    PIOS_DEBUG_Assert(driver);
+
+    struct pios_srxl_dev *srxl_dev;
+
+    srxl_dev = (struct pios_srxl_dev *)PIOS_SRXL_Alloc();
+    if (!srxl_dev) {
+        goto out_fail;
+    }
+
+    PIOS_SRXL_ResetState(&(srxl_dev->state));
+
+    *srxl_id = (uint32_t)srxl_dev;
+
+    /* Set comm driver callback */
+    (driver->bind_rx_cb)(lower_id, PIOS_SRXL_RxInCallback, *srxl_id);
+
+    if (!PIOS_RTC_RegisterTickCallback(PIOS_SRXL_Supervisor, *srxl_id)) {
+        PIOS_DEBUG_Assert(0);
+    }
+
+    PERF_INIT_COUNTER(crcFailureCount, 0x5551);
+    PERF_INIT_COUNTER(failsafeCount, 0x5552);
+    PERF_INIT_COUNTER(successfulCount, 0x5553);
+    PERF_INIT_COUNTER(messageUnrollTimer, 0x5554);
+    PERF_INIT_COUNTER(messageReceiveRate, 0x5555);
+    PERF_INIT_COUNTER(receivedBytesCount, 0x5556);
+    PERF_INIT_COUNTER(frameStartCount, 0x5557);
+    PERF_INIT_COUNTER(frameAbortCount, 0x5558);
+    PERF_INIT_COUNTER(completeMessageCount, 0x5559);
+
+    return 0;
+
+out_fail:
+    return -1;
+}
+
+/**
+ * Get the value of an input channel
+ * \param[in] channel Number of the channel desired (zero based)
+ * \output PIOS_RCVR_INVALID channel not available
+ * \output PIOS_RCVR_TIMEOUT failsafe condition or missing receiver
+ * \output >=0 channel value
+ */
+static int32_t PIOS_SRXL_Get(uint32_t rcvr_id, uint8_t channel)
+{
+    struct pios_srxl_dev *srxl_dev = (struct pios_srxl_dev *)rcvr_id;
+
+    if (!PIOS_SRXL_Validate(srxl_dev)) {
+        return PIOS_RCVR_INVALID;
+    }
+
+    /* return error if channel is not available */
+    if (channel >= PIOS_SRXL_NUM_INPUTS) {
+        return PIOS_RCVR_INVALID;
+    }
+
+    return srxl_dev->state.channel_data[channel];
+}
+
+static void PIOS_SRXL_UnrollChannels(struct pios_srxl_state *state)
+{
+    PERF_TIMED_SECTION_START(messageUnrollTimer);
+    uint8_t *received_data = state->received_data;
+    uint8_t channel;
+    uint16_t channel_value;
+    for (channel = 0; channel < (state->data_bytes / 2); channel++) {
+        channel_value = ((uint16_t)received_data[SRXL_HEADER_LENGTH + (channel * 2)]) << 8;
+        channel_value = channel_value + ((uint16_t)received_data[SRXL_HEADER_LENGTH + (channel * 2) + 1]);
+        state->channel_data[channel] = (800 + ((channel_value * 1400) >> 12));
+    }
+    PERF_TIMED_SECTION_END(messageUnrollTimer);
+}
+
+static bool PIOS_SRXL_Validate_Checksum(struct pios_srxl_state *state)
+{
+    // Check the CRC16 checksum. The provided checksum is immediately after the channel data.
+    // All data including start byte and version byte is included in crc calculation.
+    uint8_t i    = 0;
+    uint16_t crc = 0;
+
+    for (i = 0; i < SRXL_HEADER_LENGTH + state->data_bytes; i++) {
+        crc = crc ^ (int16_t)state->received_data[i] << 8;
+        uint8_t j = 0;
+        for (j = 0; j < 8; j++) {
+            if (crc & 0x8000) {
+                crc = crc << 1 ^ 0x1021;
+            } else {
+                crc = crc << 1;
+            }
+        }
+    }
+    uint16_t checksum = (((uint16_t)(state->received_data[i] << 8) |
+                          (uint16_t)state->received_data[i + 1]));
+    return crc == checksum;
+}
+
+/* Update decoder state processing input byte from the SRXL stream */
+static void PIOS_SRXL_UpdateState(struct pios_srxl_state *state, uint8_t b)
+{
+    /* should not process any data until new frame is found */
+    if (!state->frame_found) {
+        return;
+    }
+
+    if (state->byte_count < (SRXL_HEADER_LENGTH + state->data_bytes + SRXL_CHECKSUM_LENGTH)) {
+        if (state->byte_count == 0) {
+            // Set up the length of the channel data according to version received
+            PERF_INCREMENT_VALUE(frameStartCount);
+            if (b == SRXL_V1_HEADER) {
+                state->data_bytes = SRXL_V1_CHANNEL_DATA_BYTES;
+            } else if (b == SRXL_V2_HEADER) {
+                state->data_bytes = SRXL_V2_CHANNEL_DATA_BYTES;
+            } else {
+                /* discard the whole frame if the 1st byte is not correct */
+                state->frame_found = 0;
+                PERF_INCREMENT_VALUE(frameAbortCount);
+                return;
+            }
+        }
+        /* store next byte */
+        state->received_data[state->byte_count] = b;
+        state->byte_count++;
+        if (state->byte_count == (SRXL_HEADER_LENGTH + state->data_bytes + SRXL_CHECKSUM_LENGTH)) {
+            PERF_INCREMENT_VALUE(completeMessageCount);
+            // We have a complete message, lets decode it
+            if (PIOS_SRXL_Validate_Checksum(state)) {
+                /* data looking good */
+                PIOS_SRXL_UnrollChannels(state);
+                state->failsafe_timer = 0;
+                PERF_INCREMENT_VALUE(successfulCount);
+            } else {
+                /* discard whole frame */
+                PERF_INCREMENT_VALUE(crcFailureCount);
+            }
+            /* prepare for the next frame */
+            state->frame_found = 0;
+            PERF_MEASURE_PERIOD(messageReceiveRate);
+        }
+    }
+}
+
+/* Comm byte received callback */
+static uint16_t PIOS_SRXL_RxInCallback(uint32_t context,
+                                       uint8_t *buf,
+                                       uint16_t buf_len,
+                                       uint16_t *headroom,
+                                       bool *need_yield)
+{
+    struct pios_srxl_dev *srxl_dev = (struct pios_srxl_dev *)context;
+
+    bool valid = PIOS_SRXL_Validate(srxl_dev);
+
+    PIOS_Assert(valid);
+
+    struct pios_srxl_state *state = &(srxl_dev->state);
+
+    /* process byte(s) and clear receive timer */
+    for (uint8_t i = 0; i < buf_len; i++) {
+        PIOS_SRXL_UpdateState(state, buf[i]);
+        state->receive_timer = 0;
+        PERF_INCREMENT_VALUE(receivedBytesCount);
+    }
+
+    /* Always signal that we can accept another byte */
+    if (headroom) {
+        *headroom = SRXL_FRAME_LENGTH;
+    }
+
+    /* We never need a yield */
+    *need_yield = false;
+
+    /* Always indicate that all bytes were consumed */
+    return buf_len;
+}
+
+/**
+ * Input data supervisor is called periodically and provides
+ * two functions: frame syncing and failsafe triggering.
+ *
+ * Multiplex SRXL frames come at 14ms (FastResponse ON) or 21ms (FastResponse OFF)
+ * rate at 115200bps.
+ * RTC timer is running at 625Hz (1.6ms). So with divider 2 it gives
+ * 3.2ms pause between frames which is good for both SRXL frame rates.
+ *
+ * Data receive function must clear the receive_timer to confirm new
+ * data reception. If no new data received in 100ms, we must call the
+ * failsafe function which clears all channels.
+ */
+static void PIOS_SRXL_Supervisor(uint32_t srxl_id)
+{
+    struct pios_srxl_dev *srxl_dev = (struct pios_srxl_dev *)srxl_id;
+
+    bool valid = PIOS_SRXL_Validate(srxl_dev);
+
+    PIOS_Assert(valid);
+
+    struct pios_srxl_state *state = &(srxl_dev->state);
+
+    /* waiting for new frame if no bytes were received in 6.4ms */
+
+    if (++state->receive_timer > 4) {
+        state->frame_found   = 1;
+        state->byte_count    = 0;
+        state->receive_timer = 0;
+    }
+
+    /* activate failsafe if no frames have arrived in 102.4ms */
+    if (++state->failsafe_timer > 64) {
+        PIOS_SRXL_ResetChannels(state);
+        state->failsafe_timer = 0;
+        PERF_INCREMENT_VALUE(failsafeCount);
+    }
+}
+
+#endif /* PIOS_INCLUDE_SRXL */
+
+/**
+ * @}
+ * @}
+ */

flight/pios/inc/pios_adc.h

@@ -38,6 +38,7 @@ typedef void (*ADCCallback)(float *data);
 
 /* Public Functions */
 void PIOS_ADC_Config(uint32_t oversampling);
+void PIOS_ADC_PinSetup(uint32_t pin);
 int32_t PIOS_ADC_PinGet(uint32_t pin);
 float PIOS_ADC_PinGetVolt(uint32_t pin);
 int16_t *PIOS_ADC_GetRawBuffer(void);

flight/pios/inc/pios_com.h

@@ -8,6 +8,7 @@
  *
  * @file       pios_com.h
  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ *             Parts by Thorsten Klose (tk@midibox.org)
  * @brief      COM layer functions header
  * @see        The GNU Public License (GPL) Version 3
  *
@@ -35,19 +36,29 @@
 #include <stdbool.h> /* bool */
 
 typedef uint16_t (*pios_com_callback)(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *task_woken);
+typedef void (*pios_com_callback_ctrl_line)(uint32_t context, uint32_t mask, uint32_t state);
 
 struct pios_com_driver {
     void (*init)(uint32_t id);
     void (*set_baud)(uint32_t id, uint32_t baud);
+    void (*set_ctrl_line)(uint32_t id, uint32_t mask, uint32_t state);
     void (*tx_start)(uint32_t id, uint16_t tx_bytes_avail);
     void (*rx_start)(uint32_t id, uint16_t rx_bytes_avail);
     void (*bind_rx_cb)(uint32_t id, pios_com_callback rx_in_cb, uint32_t context);
     void (*bind_tx_cb)(uint32_t id, pios_com_callback tx_out_cb, uint32_t context);
+    void (*bind_ctrl_line_cb)(uint32_t id, pios_com_callback_ctrl_line ctrl_line_cb, uint32_t context);
     bool (*available)(uint32_t id);
 };
 
+/* Control line definitions */
+#define COM_CTRL_LINE_DTR_MASK 0x01
+#define COM_CTRL_LINE_RTS_MASK 0x02
+
 /* Public Functions */
+extern int32_t PIOS_COM_Init(uint32_t *com_id, const struct pios_com_driver *driver, uint32_t lower_id, uint8_t *rx_buffer, uint16_t rx_buffer_len, uint8_t *tx_buffer, uint16_t tx_buffer_len);
 extern int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud);
+extern int32_t PIOS_COM_SetCtrlLine(uint32_t com_id, uint32_t mask, uint32_t state);
+extern int32_t PIOS_COM_RegisterCtrlLineCallback(uint32_t usart_id, pios_com_callback_ctrl_line ctrl_line_cb, uint32_t context);
 extern int32_t PIOS_COM_SendCharNonBlocking(uint32_t com_id, char c);
 extern int32_t PIOS_COM_SendChar(uint32_t com_id, char c);
 extern int32_t PIOS_COM_SendBufferNonBlocking(uint32_t com_id, const uint8_t *buffer, uint16_t len);

flight/pios/inc/pios_hmc5x83.h

@@ -109,6 +109,18 @@ extern const struct pios_hmc5x83_io_driver PIOS_HMC5x83_SPI_DRIVER;
 #ifdef PIOS_INCLUDE_I2C
 extern const struct pios_hmc5x83_io_driver PIOS_HMC5x83_I2C_DRIVER;
 #endif
+// xyz axis orientation
+enum PIOS_HMC5X83_ORIENTATION {
+    PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP,
+    PIOS_HMC5X83_ORIENTATION_SOUTH_EAST_UP,
+    PIOS_HMC5X83_ORIENTATION_WEST_SOUTH_UP,
+    PIOS_HMC5X83_ORIENTATION_NORTH_WEST_UP,
+    PIOS_HMC5X83_ORIENTATION_EAST_SOUTH_DOWN,
+    PIOS_HMC5X83_ORIENTATION_SOUTH_WEST_DOWN,
+    PIOS_HMC5X83_ORIENTATION_WEST_NORTH_DOWN,
+    PIOS_HMC5X83_ORIENTATION_NORTH_EAST_DOWN,
+};
+
 
 struct pios_hmc5x83_cfg {
 #ifdef PIOS_HMC5X83_HAS_GPIOS
@@ -119,6 +131,7 @@ struct pios_hmc5x83_cfg {
     uint8_t Gain; // Gain Configuration, select the full scale --> here below the relative define (See datasheet page 11 for more details) */
     uint8_t Mode;
     bool    TempCompensation; // enable temperature sensor on HMC5983 for temperature gain compensation
+    enum PIOS_HMC5X83_ORIENTATION Orientation;
     const struct pios_hmc5x83_io_driver *Driver;
 };
 

flight/pios/inc/pios_instrumentation.h

@@ -47,7 +47,7 @@ extern int8_t pios_instrumentation_last_used_counter;
  * @param counter_handle handle of the counter to update @see PIOS_Instrumentation_SearchCounter @see PIOS_Instrumentation_CreateCounter
  * @param newValue the updated value.
  */
-inline void PIOS_Instrumentation_updateCounter(pios_counter_t counter_handle, int32_t newValue)
+static inline void PIOS_Instrumentation_updateCounter(pios_counter_t counter_handle, int32_t newValue)
 {
     PIOS_Assert(pios_instrumentation_perf_counters && counter_handle);
     vPortEnterCritical();
@@ -69,7 +69,7 @@ inline void PIOS_Instrumentation_updateCounter(pios_counter_t counter_handle, in
  * Used to determine the time duration of a code block, mark the begin of the block. @see PIOS_Instrumentation_TimeEnd
  * @param counter_handle handle of the counter @see PIOS_Instrumentation_SearchCounter @see PIOS_Instrumentation_CreateCounter
  */
-inline void PIOS_Instrumentation_TimeStart(pios_counter_t counter_handle)
+static inline void PIOS_Instrumentation_TimeStart(pios_counter_t counter_handle)
 {
     PIOS_Assert(pios_instrumentation_perf_counters && counter_handle);
     vPortEnterCritical();
@@ -83,7 +83,7 @@ inline void PIOS_Instrumentation_TimeStart(pios_counter_t counter_handle)
  * Used to determine the time duration of a code block, mark the end of the block. @see PIOS_Instrumentation_TimeStart
  * @param counter_handle handle of the counter @see PIOS_Instrumentation_SearchCounter @see PIOS_Instrumentation_CreateCounter
  */
-inline void PIOS_Instrumentation_TimeEnd(pios_counter_t counter_handle)
+static inline void PIOS_Instrumentation_TimeEnd(pios_counter_t counter_handle)
 {
     PIOS_Assert(pios_instrumentation_perf_counters && counter_handle);
     vPortEnterCritical();
@@ -106,7 +106,7 @@ inline void PIOS_Instrumentation_TimeEnd(pios_counter_t counter_handle)
  * Used to determine the mean period between each call to the function
  * @param counter_handle handle of the counter @see PIOS_Instrumentation_SearchCounter @see PIOS_Instrumentation_CreateCounter
  */
-inline void PIOS_Instrumentation_TrackPeriod(pios_counter_t counter_handle)
+static inline void PIOS_Instrumentation_TrackPeriod(pios_counter_t counter_handle)
 {
     PIOS_Assert(pios_instrumentation_perf_counters && counter_handle);
     pios_perf_counter_t *counter = (pios_perf_counter_t *)counter_handle;
@@ -127,6 +127,29 @@ inline void PIOS_Instrumentation_TrackPeriod(pios_counter_t counter_handle)
     counter->lastUpdateTS = PIOS_DELAY_GetRaw();
 }
 
+/**
+ * Increment a counter with a value
+ * @param counter_handle handle of the counter to update @see PIOS_Instrumentation_SearchCounter @see PIOS_Instrumentation_CreateCounter
+ * @param increment the value to increment counter with.
+ */
+static inline void PIOS_Instrumentation_incrementCounter(pios_counter_t counter_handle, int32_t increment)
+{
+    PIOS_Assert(pios_instrumentation_perf_counters && counter_handle);
+    vPortEnterCritical();
+    pios_perf_counter_t *counter = (pios_perf_counter_t *)counter_handle;
+    counter->value += increment;
+    counter->max--;
+    if (counter->value > counter->max) {
+        counter->max = counter->value;
+    }
+    counter->min++;
+    if (counter->value < counter->min) {
+        counter->min = counter->value;
+    }
+    counter->lastUpdateTS = PIOS_DELAY_GetRaw();
+    vPortExitCritical();
+}
+
 /**
  * Initialize the Instrumentation infrastructure
  * @param maxCounters maximum number of allowed counters

flight/pios/inc/pios_instrumentation_helper.h

@@ -98,6 +98,8 @@
 #define PERF_TIMED_SECTION_END(x)   PIOS_Instrumentation_TimeEnd(x)
 #define PERF_MEASURE_PERIOD(x)      PIOS_Instrumentation_TrackPeriod(x)
 #define PERF_TRACK_VALUE(x, y)      PIOS_Instrumentation_updateCounter(x, y)
+#define PERF_INCREMENT_VALUE(x)     PIOS_Instrumentation_incrementCounter(x, 1)
+#define PERF_DECREMENT_VALUE(x)     PIOS_Instrumentation_incrementCounter(x, -1)
 
 #else
 
@@ -107,5 +109,7 @@
 #define PERF_TIMED_SECTION_END(x)
 #define PERF_MEASURE_PERIOD(x)
 #define PERF_TRACK_VALUE(x, y)
+#define PERF_INCREMENT_VALUE(x)
+#define PERF_DECREMENT_VALUE(x)
 #endif /* PIOS_INCLUDE_INSTRUMENTATION */
 #endif /* PIOS_INSTRUMENTATION_HELPER_H */

flight/pios/inc/pios_mem.h

@@ -32,6 +32,8 @@ void *pios_fastheapmalloc(size_t size);
 
 void *pios_malloc(size_t size);
 
+void *pios_realloc(void *ptr, size_t size);
+
 void pios_free(void *p);
 
 #endif /* PIOS_MEM_H */

flight/pios/inc/pios_mpxv.h

@@ -38,6 +38,7 @@ typedef struct {
     uint16_t calibrationCount;
     uint32_t calibrationSum;
     uint16_t zeroPoint;
+    float    scale;
     float    maxSpeed;
 } PIOS_MPXV_descriptor;
 

flight/pios/inc/pios_posix.h

@@ -28,7 +28,9 @@
 
 #include <stdint.h>
 
+#ifndef __cplusplus
 typedef enum { FALSE = 0, TRUE = !FALSE } bool;
+#endif
 
 #ifndef false
         #define false FALSE

flight/pios/inc/pios_rcvr.h

@@ -35,10 +35,12 @@ struct pios_rcvr_driver {
     void    (*init)(uint32_t id);
     int32_t (*read)(uint32_t id, uint8_t channel);
     xSemaphoreHandle (*get_semaphore)(uint32_t id, uint8_t channel);
+    uint8_t (*get_quality)(uint32_t id);
 };
 
 /* Public Functions */
 extern int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel);
+extern uint8_t PIOS_RCVR_GetQuality(uint32_t rcvr_id);
 extern xSemaphoreHandle PIOS_RCVR_GetSemaphore(uint32_t rcvr_id, uint8_t channel);
 
 /*! Define error codes for PIOS_RCVR_Get */

flight/pios/inc/pios_srxl.h

@@ -1,12 +1,14 @@
 /**
  ******************************************************************************
- * @addtogroup OpenPilotSystem OpenPilot System
+ * @addtogroup PIOS PIOS Core hardware abstraction layer
  * @{
- * @addtogroup OpenPilotCore OpenPilot Core
+ * @addtogroup PIOS_SRXL Multiplex SRXL receiver functions
+ * @brief Code to read Multiplex SRXL receiver serial stream
  * @{
- * @file       openpilot.h
+ *
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @file       pios_srxl.h
- * @brief      Main OpenPilot header.
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Code to read Multiplex SRXL receiver serial stream
  * @see        The GNU Public License (GPL) Version 3
  *
  *****************************************************************************/
@@ -26,25 +28,14 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 
-#ifndef OPENPILOT_H
+#ifndef PIOS_SRXL_H
-#define OPENPILOT_H
+#define PIOS_SRXL_H
 
-/* PIOS Includes */
+/* Global Types */
-#include <pios.h>
 
-/* OpenPilot Libraries */
+/* Public Functions */
-#include <utlist.h>
-#include <uavobjectmanager.h>
-#include <eventdispatcher.h>
-#include <uavtalk.h>
 
-#include "alarms.h"
+#endif /* PIOS_SRXL_H */
-#include <mathmisc.h>
-
-/* Global Functions */
-void OpenPilotInit(void);
-
-#endif /* OPENPILOT_H */
 
 /**
  * @}

flight/pios/inc/pios_srxl_priv.h

@@ -0,0 +1,95 @@
+/**
+ ******************************************************************************
+ * @addtogroup PIOS PIOS Core hardware abstraction layer
+ * @{
+ * @addtogroup PIOS_SRXL Multiplex SRXL receiver functions
+ * @brief Code to read Multiplex SRXL receiver serial stream
+ * @{
+ *
+ * @file       pios_srxl_priv.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Code to read Multiplex SRXL receiver serial stream
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#ifndef PIOS_SRXL_PRIV_H
+#define PIOS_SRXL_PRIV_H
+
+#include <pios.h>
+#include <pios_stm32.h>
+#include <pios_usart_priv.h>
+
+/*
+ * Multiplex SRXL serial port settings:
+ *  115200bps inverted serial stream, 8 bits, no parity, 1 stop bits
+ *  frame period is 14ms (FastResponse ON) or 21ms (FastResponse OFF)
+ *
+ * Frame structure:
+ *  1 byte  - start and version
+ *      0xa1 (v1 12-channels)
+ *      0xa2 (v2 16-channels)
+ * 24/32 bytes - channel data (4 + 12 bit/channel, 12/16 channels, MSB first)
+ *      16 bits per channel. 4 first reserved/not used. 12 bits channel data in
+ *          4095 steps, 0x000(800µs) - 0x800(1500µs) - 0xfff(2200µs)
+ *  2 bytes checksum (calculated over all bytes including start and version)
+ *
+ * Checksum calculation:
+ * u16 CRC16(u16 crc, u8 value) {
+ *     u8 i;
+ *     crc = crc ^ (s16)value << 8;
+ *     for(i = 0; i < 8; i++) {
+ *         if(crc & 0x8000) {
+ *             crc = crc << 1 ^ 0x1021;
+ *         } else {
+ *             crc = crc << 1;
+ *         }
+ *     }
+ *     return crc;
+ * }
+ */
+
+#define SRXL_V1_HEADER             0xa1
+#define SRXL_V2_HEADER             0xa2
+
+#define SRXL_HEADER_LENGTH         1
+#define SRXL_CHECKSUM_LENGTH       2
+#define SRXL_V1_CHANNEL_DATA_BYTES (12 * 2)
+#define SRXL_V2_CHANNEL_DATA_BYTES (16 * 2)
+#define SRXL_FRAME_LENGTH          (SRXL_HEADER_LENGTH + SRXL_V2_CHANNEL_DATA_BYTES + SRXL_CHECKSUM_LENGTH)
+
+/*
+ * Multiplex SRXL protocol provides 16 proportional channels.
+ * Do not change unless driver code is updated accordingly.
+ */
+#if (PIOS_SRXL_NUM_INPUTS != 16)
+#error "Multiplex SRXL protocol provides 16 proportional channels."
+#endif
+
+extern const struct pios_rcvr_driver pios_srxl_rcvr_driver;
+
+extern int32_t PIOS_SRXL_Init(uint32_t *srxl_id,
+                              const struct pios_com_driver *driver,
+                              uint32_t lower_id);
+
+#endif /* PIOS_SRXL_PRIV_H */
+
+/**
+ * @}
+ * @}
+ */

flight/pios/inc/pios_usart_priv.h

@@ -44,6 +44,7 @@ struct pios_usart_cfg {
     USART_InitTypeDef init;
     struct stm32_gpio rx;
     struct stm32_gpio tx;
+    struct stm32_gpio dtr;
     struct stm32_irq  irq;
 };