Merge branch 'next' into corvuscorax/OP-1161_magnetometer-alarm

Conflicts:
	shared/uavobjectdefinition/revosettings.xml
	shared/uavobjectdefinition/systemalarms.xml

.commit-template

@@ -0,0 +1,23 @@
+
+
+# *************** OpenPilot commits guidelines ***************
+# Each commit needs to have a message like the following sample:
+#  OP-1150 UI for thermal calibration: Connect State machine to UI
+#
+# It needs to begin with a reference to one or more Jira tickets followed by a short description.
+# If needed add a longer description in the following lines, after an empty line.
+#
+# Before  committing, ensure your code is properly formatted using:
+#  	make uncrustify_all 
+# You can format flight or ground code only using respectively 
+# uncrustify_flight or uncrustify_ground
+#
+# To automatically create a review, append the following smart commit messages:
+#   +review OPReview
+#
+# To append the commit to an existing review, use the following smart commit message:
+#   +review OPReview-NNN
+# For example "+review OPReview-609"  
+#
+# *NOTE* leave an empty line between the commit message and "smart commit command"
+# Smart commits commands need to starts immediately at first column

CREDITS.txt

@@ -5,7 +5,9 @@ Pedro Assuncao
 Fredrik Arvidsson
 Werner Backes
 Jose Barros
+Mikael Blomqvist
 Pete  Boehl
+Glenn Campigli
 David Carlson
 James Cotton
 Steve Doll
@@ -13,7 +15,9 @@ Piotr Esden-Tempski
 Richard Flay
 Peter Farnworth
 Ed Faulkner
+Andrew Finegan
 Darren Furniss
+Cliff Geerdes
 Frederic Goddeeris
 Daniel Godin
 Anthony Gomez
@@ -24,6 +28,7 @@ Peter Gunnarsson
 Dean Hall
 Joe Hlebasko
 Andy Honecker
+Patrick Huebner
 Ryan Hunt
 Mark James
 Ricky King
@@ -34,6 +39,7 @@ Alan Krum
 Edouard Lafargue
 Mike Labranche
 Fredrik Larsson
+Richard von Lehe
 Pablo Lema
 David Llama
 Matt Lipski
@@ -65,6 +71,7 @@ Troy Schultz
 Dr. Erhard Siegl
 Dusty Anne Smith
 Mike Smith
+Bertrand Songis
 Alex Sowa
 Pete Stapley
 Vova Starikh

Makefile

@@ -450,11 +450,25 @@ else
     GCS_SILENT := silent
 endif
 
+.NOTPARALLEL:
 .PHONY: openpilotgcs
-openpilotgcs: uavobjects_gcs
+openpilotgcs: uavobjects_gcs openpilotgcs_qmake openpilotgcs_make
-	$(V1) $(MKDIR) -p $(BUILD_DIR)/$@_$(GCS_BUILD_CONF)
+
-	$(V1) ( cd $(BUILD_DIR)/$@_$(GCS_BUILD_CONF) && \
+.PHONY: openpilotgcs_qmake
-	    $(QMAKE) $(ROOT_DIR)/ground/openpilotgcs/openpilotgcs.pro -spec $(QT_SPEC) -r CONFIG+="$(GCS_BUILD_CONF) $(GCS_SILENT)" $(GCS_QMAKE_OPTS) && \
+openpilotgcs_qmake:
+ifeq ($(QMAKE_SKIP),)
+	$(V1) $(MKDIR) -p $(BUILD_DIR)/openpilotgcs_$(GCS_BUILD_CONF)
+	$(V1) ( cd $(BUILD_DIR)/openpilotgcs_$(GCS_BUILD_CONF) && \
+	    $(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
+openpilotgcs_make:
+	$(V1) $(MKDIR) -p $(BUILD_DIR)/openpilotgcs_$(GCS_BUILD_CONF)
+	$(V1) ( cd $(BUILD_DIR)/openpilotgcs_$(GCS_BUILD_CONF)/$(MAKE_DIR) && \
 	    $(MAKE) -w ; \
 	)
 
@@ -709,7 +723,7 @@ $(OPFW_RESOURCE): $(FW_TARGETS)
 
 # If opfw_resource or all firmware are requested, GCS should depend on the resource
 ifneq ($(strip $(filter opfw_resource all all_fw all_flight,$(MAKECMDGOALS))),)
-    $(eval openpilotgcs: $(OPFW_RESOURCE))
+    $(eval openpilotgcs_qmake: $(OPFW_RESOURCE))
 endif
 
 # Packaging targets: package, clean_package
@@ -859,12 +873,13 @@ help:
 	@$(ECHO)
 	@$(ECHO) "   Here is a summary of the available targets:"
 	@$(ECHO)
+	@$(ECHO) "   [Source tree preparation]"
+	@$(ECHO) "     prepare              - Install GIT commit message template"
 	@$(ECHO) "   [Tool Installers]"
 	@$(ECHO) "     arm_sdk_install      - Install the GNU ARM gcc toolchain"
 	@$(ECHO) "     qt_sdk_install       - Install the QT development tools"
-	@$(ECHO) "     mingw_install        - Install the MinGW toolchain (Windows only)"
-	@$(ECHO) "     python_install       - Install the Python interpreter (Windows only)"
 	@$(ECHO) "     nsis_install         - Install the NSIS Unicode (Windows only)"
+	@$(ECHO) "     sdl_install          - Install the SDL library (Windows only)"
 	@$(ECHO) "     openssl_install      - Install the OpenSSL libraries (Windows only)"
 	@$(ECHO) "     uncrustify_install   - Install the Uncrustify source code beautifier"
 	@$(ECHO) "     doxygen_install      - Install the Doxygen documentation generator"
@@ -945,6 +960,9 @@ 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) "     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)

WHATSNEW.txt

@@ -1,3 +1,132 @@
+--- RELEASE-14.01 --- Cruising Ratt ---
+This is the first 2014 software release. 
+This version still supports the  CopterControl and CC3D.
+It includes some major "under the hood" changes like migration
+to Qt5.1 and QtQuick2 widgets, an overhaul of UAVTalk to improve
+Telemetry and OPLink reliability.
+Some additions in this release:
+- "Rattitude" flight mode;
+- Altitude Hold Reimplementation;
+- Multiple PID banks;
+- "Cruise Control" 
+
+the full list of features, improvements and bufixes shipping
+in this release is accessible here:
+
+http://progress.openpilot.org/issues/?filter=11260
+
+** Improvement
+    * [OP-771] - Change Wizard wording for better usability
+    * [OP-791] - Integrate About Authors, OpenPilot GCS, Plugins dialogs into a single dialog window
+    * [OP-803] - Gadgets get their configuration set twice when restoring workspaces during GCS startup
+    * [OP-835] - Upgrade GCS to use Qt 5.1.0
+    * [OP-883] - Make system and flight targets cleanup, pass 01
+    * [OP-913] - Poor UAVObject data structure alignment on flight side causes performance degradation
+    * [OP-951] - Add -Wshadow to flight CFLAGS and fix compilation breakage that results
+    * [OP-966] - Scope Plugin Cleanup
+    * [OP-984] - Provide multi PID banks, these should be assignable per flight mode. 
+    * [OP-996] - Add GCS option to remember the last selected workspace
+    * [OP-1022] - Additional improvements for altitude hold
+    * [OP-1036] - Improvements to Fixed Wing PathFollower and Nav
+    * [OP-1059] - Typo (2x) in OpenPilot Setup Wizard - Output Calibration Window
+    * [OP-1063] - Multirotor Configuration
+    * [OP-1071] - Make map "emergency" lines less strong and dashed
+    * [OP-1079] - Update to FreeRTOS v7.5.2
+    * [OP-1082] - Add a ticker on the Welcome page showing Jira activity alongside the 'Project News'
+    * [OP-1083] - Fix minor English spelling errors in stabilization tooltips
+    * [OP-1085] - Upgrade GCS to use Qt 5.1.1
+    * [OP-1094] - Turn on Progress for large SDK downloads / remove for MD5 files
+    * [OP-1104] - Create BL version 6 to support larger firmware
+    * [OP-1105] - If firmware .info blob is missing, test string is too long
+    * [OP-1107] - Convert About dialog to QTQuick 2.0 and cleanup code.
+    * [OP-1110] - Move Welcome screen to QtQuick 2
+    * [OP-1111] - Move About to QtQuick2
+    * [OP-1112] - Update contributors in GCS
+    * [OP-1113] - Convert new PFD design to QtQuik2
+    * [OP-1117] - Implement Horizon mode
+    * [OP-1120] - Waypoint upload to board should be transacted
+    * [OP-1133] - UAVTalk - expose send/request all instances of multi instance uav objects + related uavtalk fixes
+    * [OP-1137] - Make Configuration Checkbox checked by default during uninstall
+    * [OP-1141] - Add a further bias correction to barometer to better handle thermal variations
+    * [OP-1143] - Missing Linux udev rules for Revolution boards 
+    * [OP-1153] - Provide a mean to instrument SystemMod stack utilization
+    * [OP-1154] - Config Option to Automatically Increase Copter Throttle per 1/cos(bank_angle)
+    * [OP-1158] - Add flight plan consistency checks
+    * [OP-1160] - Some dev Env improvements,  git hooks for messages, make prepare etc.
+
+** Task
+    * [OP-775] - Add ARM DSP library to OP codebase
+    * [OP-813] - Manage merge of translation work to French 
+    * [OP-839] - Disable pyMyte dependency until really used
+    * [OP-901] - Update STM32 StdPeriphLib to current
+    * [OP-1087] - Update Qt used from Makefile to 5.1.1 for Windows and Mac
+    * [OP-1109] - Created share Qt5 QtQuick2 port branch
+    * [OP-1115] - Remove old artwork from the Artwork folder in Git
+    * [OP-1119] - Write GCS plugin to access and display on board logs through uavtalk and export .opl files from logged uavobjects
+    * [OP-1058] - UAVO:Implement a structured named accessors for multielement fields (Flight side)
+
+** Bug
+    * [OP-844] - Fix header comments in altitudehold.c
+    * [OP-845] - Fix reading serial number from USB device on mac platform
+    * [OP-846] - make qt_sdk_install fails
+    * [OP-865] - PWM output 6 does not work on RM
+    * [OP-887] - Provide some standard method of calibrating CPU speed and load measurement for boards
+    * [OP-924] - PPM output does not have failsafe
+    * [OP-934] - Incorrect timeout handling in rfm22b receiver
+    * [OP-971] - Add UI to set AccellTau with revo board
+    * [OP-1004] - UAVObjectBrowser, buttons don't work when scientific display is turned on
+    * [OP-1014] - Com port connections are not working on OPLink
+    * [OP-1018] - Zero point initialization in ETASv3 Airspeed sensor buggy
+    * [OP-1027] - Segfault in UAVObjectBrowser when "Request"ing a UAVObjectCategory
+    * [OP-1042] - Revo firmware version isn't read correctly through OPLink
+    * [OP-1046] - Waypoint upload incomplete, no visual confirmation of failed uploads in uavobjectbrowser and waypoint editor
+    * [OP-1048] - Attitude is not working with AccelTau > 0
+    * [OP-1049] - CC3D attitude estimation failure after multiple settings changes and reboots
+    * [OP-1067] - Invalid value for "LinkState"
+    * [OP-1076] - CF Attitude filter in next randomly re-initializes on arming.
+    * [OP-1078] - GCS segfaults if you close it after playing a log file
+    * [OP-1080] - Unreliable detection of board through OPLink
+    * [OP-1095] - GCS crashing on macosx 10.9 upon connection of oplink mini
+    * [OP-1098] - CDC driver fails installation in Windows 8 or 8.1
+    * [OP-1099] - Hidden icons in Configuration tab
+    * [OP-1101] - Tools.mk has a few tabs and they need to be converted to spaces
+    * [OP-1102] - OP GCS registers some file types is should not
+    * [OP-1103] - GCS can not be compiled on OSX 10.8 after update to Qt5.1.1
+    * [OP-1108] - Minor bugs found while reading the code
+    * [OP-1114] - QGLWidget prohibits QListWidgetItem, set AA_DontCreateNativeWidgetSiblings as work around
+    * [OP-1118] - QComboBox in UAVObjectBrowser does not stay in focus on Mac OSX
+    * [OP-1121] - GCS will not exit if the Waypoint editor/PathPlanner dialog is open
+    * [OP-1123] - GCS assertion failure when loading a waypoint file
+    * [OP-1125] - UAVTalk - acking/nacking multi instance uavobjects is broken (when sending individual instances)
+    * [OP-1132] - LIBEAY32.dll missing from installer
+    * [OP-1139] - Add higher order correction to MS5611 driver for low and very low temperature compensation 
+    * [OP-1142] - No yaw in Horizon mode
+    * [OP-1145] - OPLM to GCS link not reliable
+    * [OP-1148] - Futaba R7008SB S.Bus protocol not supported
+    * [OP-1151] - PFD display - inverted flight
+    * [OP-1152] - Check Stack usage for CopterControl & CC3D
+    * [OP-1155] - Fix OSX Packaging for GCS
+    * [OP-1157] - sin_lookup_deg() returns garbage for negative angles
+    * [OP-1166] - GCS misses yaw neutral setting on sync from initial connection
+    * [OP-1167] - New flight mode switch position UAVO to work better with SITL, HITL
+    * [OP-1168] - GCS Reload Board Data button doesn't work
+    * [OP-1169] - GCS UAVO object titles off by one
+    * [OP-1176] - Cruise Control checkboxes use wrong Default button
+    * [OP-1177] - AltHold - Need a setting to allow disabling of bank angle throttle compensation in AH
+    * [OP-1178] - After re-factoring of ConfigTaskWidget code the OPLink config page does not work reliably.
+    * [OP-1179] - About box not working in Linux64 build (but probably the same is for Linux32)
+    * [OP-1180] - GCS AltHold Tab - Reload button and update in real time
+    * [OP-1181] - on radio configuration the pitch slider has maxed out on its own three times randomly
+    * [OP-1182] - Telemetry monitor widget is too small on Mac
+    * [OP-1183] - UAVBrowser displays hex string as decimal
+    * [OP-1184] - Scope gadget - Stack monitor configurations need a cleanup
+    * [OP-1188] - Optimize Stabilization Module stack size usage 
+    * [OP-1191] - Revo OPLink bug in GCS
+    * [OP-1192] - Even though Throttle is off there is motor movement in some situations.
+    * [OP-1211] - dT calculation in Stabilization and other modules unsafe
+    * [OP-1218] - PIOS_COM is not thread safe
+    * [OP-1228] - GCS Quits unexpectedly
+
 --- RELEASE-13.06.04 ---
 This maintenance release includes the following fixes missing in (previously not released to public) RELEASE-13.06.03.
 - Fixed issues with Google Maps;
@@ -6,7 +135,8 @@ This maintenance release includes the following fixes missing in (previously not
 JIRA issues addressed in this maintenance release:
 OP-1044, OP-1070, OP-1072
 Use the following link for a comprehensive list of issues addressed by this release
-http://progress.openpilot.org/browse/OP-1070
+
+http://progress.openpilot.org/issues/?filter=11060
 
 --- RELEASE-13.06.03 ---
 
@@ -296,6 +426,7 @@ GCS code changes:
 - added OPLinkMini configuration page;
 - hardware options are now dynamically enabled/disabled to allow supported configurations only;
 - new artwork for all boards everywhere;
+
 - optimised some 3D models;
 - new About dialog showing version info and contributors list;
 - fixed badly broken HiTL options dialog;

flight/libraries/alarms.c

@@ -32,11 +32,17 @@
 #include "inc/alarms.h"
 
 // Private constants
+#ifndef PIOS_ALARM_GRACETIME
+// alarm cannot be turned off for at least 1000 milliseconds
+// to prevent event system overload through flapping alarms
+        #define PIOS_ALARM_GRACETIME 1000
+#endif // PIOS_ALARM_GRACETIME
 
 // Private types
 
 // Private variables
 static xSemaphoreHandle lock;
+static volatile uint16_t lastAlarmChange[SYSTEMALARMS_ALARM_NUMELEM] = { 0 }; // this deliberately overflows every 2^16 milliseconds to save memory
 
 // Private functions
 static int32_t hasSeverity(SystemAlarmsAlarmOptions severity);
@@ -63,7 +69,7 @@ int32_t AlarmsInitialize(void)
  */
 int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity)
 {
-    SystemAlarmsData alarms;
+    SystemAlarmsAlarmData alarms;
 
     // Check that this is a valid alarm
     if (alarm >= SYSTEMALARMS_ALARM_NUMELEM) {
@@ -74,10 +80,14 @@ int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity
     xSemaphoreTakeRecursive(lock, portMAX_DELAY);
 
     // Read alarm and update its severity only if it was changed
-    SystemAlarmsGet(&alarms);
+    SystemAlarmsAlarmGet(&alarms);
-    if (cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] != severity) {
+    uint16_t flightTime = (uint16_t)xTaskGetTickCount() * (uint16_t)portTICK_RATE_MS; // this deliberately overflows every 2^16 milliseconds to save memory
-        cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] = severity;
+    if ((flightTime - lastAlarmChange[alarm] > PIOS_ALARM_GRACETIME &&
-        SystemAlarmsSet(&alarms);
+         cast_struct_to_array(alarms, alarms.Actuator)[alarm] != severity)
+        || cast_struct_to_array(alarms, alarms.Actuator)[alarm] < severity) {
+        cast_struct_to_array(alarms, alarms.Actuator)[alarm] = severity;
+        lastAlarmChange[alarm] = flightTime;
+        SystemAlarmsAlarmSet(&alarms);
     }
 
     // Release lock
@@ -110,10 +120,14 @@ int32_t ExtendedAlarmsSet(SystemAlarmsAlarmElem alarm,
 
     // Read alarm and update its severity only if it was changed
     SystemAlarmsGet(&alarms);
-    if (cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] != severity) {
+    uint16_t flightTime = (uint16_t)xTaskGetTickCount() * (uint16_t)portTICK_RATE_MS; // this deliberately overflows every 2^16 milliseconds to save memory
+    if ((flightTime - lastAlarmChange[alarm] > PIOS_ALARM_GRACETIME &&
+         cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] != severity)
+        || cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] < severity) {
         cast_struct_to_array(alarms.ExtendedAlarmStatus, alarms.ExtendedAlarmStatus.BootFault)[alarm]    = status;
         cast_struct_to_array(alarms.ExtendedAlarmSubStatus, alarms.ExtendedAlarmStatus.BootFault)[alarm] = subStatus;
         cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] = severity;
+        lastAlarmChange[alarm] = flightTime;
         SystemAlarmsSet(&alarms);
     }
 
@@ -129,7 +143,7 @@ int32_t ExtendedAlarmsSet(SystemAlarmsAlarmElem alarm,
  */
 SystemAlarmsAlarmOptions AlarmsGet(SystemAlarmsAlarmElem alarm)
 {
-    SystemAlarmsData alarms;
+    SystemAlarmsAlarmData alarms;
 
     // Check that this is a valid alarm
     if (alarm >= SYSTEMALARMS_ALARM_NUMELEM) {
@@ -137,8 +151,8 @@ SystemAlarmsAlarmOptions AlarmsGet(SystemAlarmsAlarmElem alarm)
     }
 
     // Read alarm
-    SystemAlarmsGet(&alarms);
+    SystemAlarmsAlarmGet(&alarms);
-    return cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm];
+    return cast_struct_to_array(alarms, alarms.Actuator)[alarm];
 }
 
 /**
@@ -220,17 +234,17 @@ int32_t AlarmsHasCritical()
  */
 static int32_t hasSeverity(SystemAlarmsAlarmOptions severity)
 {
-    SystemAlarmsData alarms;
+    SystemAlarmsAlarmData alarms;
 
     // Lock
     xSemaphoreTakeRecursive(lock, portMAX_DELAY);
 
     // Read alarms
-    SystemAlarmsGet(&alarms);
+    SystemAlarmsAlarmGet(&alarms);
 
     // Go through alarms and check if any are of the given severity or higher
     for (uint32_t n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n) {
-        if (cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[n] >= severity) {
+        if (cast_struct_to_array(alarms, alarms.Actuator)[n] >= severity) {
             xSemaphoreGiveRecursive(lock);
             return 1;
         }

flight/libraries/math/sin_lookup.c

@@ -105,7 +105,14 @@ float sin_lookup_deg(float angle)
         return 0;
     }
 
-    int i_ang = ((int32_t)angle) % 360;
+    // <bug, was> int i_ang = ((int32_t)angle) % 360;
+    // 1073741760 is a multiple of 360 that is close to 0x3fffffff
+    // so angle can be a very large number of positive or negative rotations
+    // this is the fastest fix (no tests, one integer addition)
+    // and has the largest range since float mantissas are 23-4 bit
+    // we could halve the lookup table size
+    // we could interpolate for greater accuracy
+    int i_ang = ((int32_t)(angle + 0.5f) + (int32_t)1073741760) % 360;
     if (i_ang >= 180) { // for 180 to 360 deg
         return -sin_table[i_ang - 180];
     } else { // for 0 to 179 deg

flight/libraries/sanitycheck.c

@@ -34,7 +34,9 @@
 
 // UAVOs
 #include <manualcontrolsettings.h>
+#include <flightmodesettings.h>
 #include <systemsettings.h>
+#include <systemalarms.h>
 #include <taskinfo.h>
 
 /****************************
@@ -84,47 +86,47 @@ int32_t configuration_check()
     // For each available flight mode position sanity check the available
     // modes
     uint8_t num_modes;
-    uint8_t modes[MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_NUMELEM];
+    uint8_t modes[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM];
     ManualControlSettingsFlightModeNumberGet(&num_modes);
-    ManualControlSettingsFlightModePositionGet(modes);
+    FlightModeSettingsFlightModePositionGet(modes);
 
     for (uint32_t i = 0; i < num_modes; i++) {
         switch (modes[i]) {
-        case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL:
+        case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_MANUAL:
             if (multirotor) {
                 severity = SYSTEMALARMS_ALARM_ERROR;
             }
             break;
-        case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1:
+        case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED1:
             severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(1, multirotor) : severity;
             break;
-        case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2:
+        case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED2:
             severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(2, multirotor) : severity;
             break;
-        case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3:
+        case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED3:
             severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(3, multirotor) : severity;
             break;
-        case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_AUTOTUNE:
+        case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE:
             if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_AUTOTUNE)) {
                 severity = SYSTEMALARMS_ALARM_ERROR;
             }
             break;
-        case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD:
+        case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD:
-        case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEVARIO:
+        case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ALTITUDEVARIO:
             if (coptercontrol) {
                 severity = SYSTEMALARMS_ALARM_ERROR;
-            } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_ALTITUDEHOLD)) { // Revo supports altitude hold
-                severity = SYSTEMALARMS_ALARM_ERROR;
             }
+            // TODO: put check equivalent to TASK_MONITOR_IsRunning
+            // here as soon as available for delayed callbacks
             break;
-        case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL:
+        case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL:
             if (coptercontrol) {
                 severity = SYSTEMALARMS_ALARM_ERROR;
             } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { // Revo supports altitude hold
                 severity = SYSTEMALARMS_ALARM_ERROR;
             }
             break;
-        case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD:
+        case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD:
             if (coptercontrol) {
                 severity = SYSTEMALARMS_ALARM_ERROR;
             } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
@@ -132,7 +134,7 @@ int32_t configuration_check()
                 severity = SYSTEMALARMS_ALARM_ERROR;
             }
             break;
-        case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_LAND:
+        case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND:
             if (coptercontrol) {
                 severity = SYSTEMALARMS_ALARM_ERROR;
             } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
@@ -140,7 +142,7 @@ int32_t configuration_check()
                 severity = SYSTEMALARMS_ALARM_ERROR;
             }
             break;
-        case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POI:
+        case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI:
             if (coptercontrol) {
                 severity = SYSTEMALARMS_ALARM_ERROR;
             } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
@@ -148,15 +150,21 @@ int32_t configuration_check()
                 severity = SYSTEMALARMS_ALARM_ERROR;
             }
             break;
-        case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_PATHPLANNER:
+        case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_PATHPLANNER:
             if (coptercontrol) {
                 severity = SYSTEMALARMS_ALARM_ERROR;
-            } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
+            } else {
-                // Revo supports PathPlanner
+                // Revo supports PathPlanner and that must be OK or we are not sane
-                severity = SYSTEMALARMS_ALARM_ERROR;
+                // PathPlan alarm is uninitialized if not running
+                // PathPlan alarm is warning or error if the flightplan is invalid
+                SystemAlarmsAlarmData alarms;
+                SystemAlarmsAlarmGet(&alarms);
+                if (alarms.PathPlan != SYSTEMALARMS_ALARM_OK) {
+                    severity = SYSTEMALARMS_ALARM_ERROR;
+                }
             }
             break;
-        case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE:
+        case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE:
             if (coptercontrol) {
                 severity = SYSTEMALARMS_ALARM_ERROR;
             } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
@@ -194,23 +202,23 @@ int32_t configuration_check()
 static int32_t check_stabilization_settings(int index, bool multirotor)
 {
     // Make sure the modes have identical sizes
-    if (MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM != MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_NUMELEM ||
+    if (FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM != FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_NUMELEM ||
-        MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM != MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NUMELEM) {
+        FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM != FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_NUMELEM) {
         return SYSTEMALARMS_ALARM_ERROR;
     }
 
-    uint8_t modes[MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM];
+    uint8_t modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM];
 
     // Get the different axis modes for this switch position
     switch (index) {
     case 1:
-        ManualControlSettingsStabilization1SettingsArrayGet(modes);
+        FlightModeSettingsStabilization1SettingsArrayGet(modes);
         break;
     case 2:
-        ManualControlSettingsStabilization2SettingsArrayGet(modes);
+        FlightModeSettingsStabilization2SettingsArrayGet(modes);
         break;
     case 3:
-        ManualControlSettingsStabilization3SettingsArrayGet(modes);
+        FlightModeSettingsStabilization3SettingsArrayGet(modes);
         break;
     default:
         return SYSTEMALARMS_ALARM_ERROR;
@@ -219,14 +227,14 @@ static int32_t check_stabilization_settings(int index, bool multirotor)
     // For multirotors verify that nothing is set to "none"
     if (multirotor) {
         for (uint32_t i = 0; i < NELEMENTS(modes); i++) {
-            if (modes[i] == MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE) {
+            if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NONE) {
                 return SYSTEMALARMS_ALARM_ERROR;
             }
         }
     }
 
     // Warning: This assumes that certain conditions in the XML file are met.  That
-    // MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE has the same numeric value for each channel
+    // FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NONE has the same numeric value for each channel
     // and is the same for STABILIZATIONDESIRED_STABILIZATIONMODE_NONE
 
     return SYSTEMALARMS_ALARM_OK;

flight/modules/Actuator/actuator.c

@@ -55,7 +55,7 @@
 #define STACK_SIZE_BYTES     1312
 #endif
 
-#define TASK_PRIORITY        (tskIDLE_PRIORITY + 4)
+#define TASK_PRIORITY        (tskIDLE_PRIORITY + 4) // device driver
 #define FAILSAFE_TIMEOUT_MS  100
 #define MAX_MIX_ACTUATORS    ACTUATORCOMMAND_CHANNEL_NUMELEM
 
@@ -129,6 +129,9 @@ int32_t ActuatorInitialize()
     queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
     ActuatorDesiredConnectQueue(queue);
 
+    // Register AccessoryDesired (Secondary input to this module)
+    AccessoryDesiredInitialize();
+
     // Primary output of this module
     ActuatorCommandInitialize();
 
@@ -166,6 +169,9 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
     ActuatorDesiredData desired;
     MixerStatusData mixerStatus;
     FlightStatusData flightStatus;
+    SystemSettingsThrustControlOptions thrustType;
+    float throttleDesired;
+    float collectiveDesired;
 
     /* Read initial values of ActuatorSettings */
     ActuatorSettingsData actuatorSettings;
@@ -220,6 +226,41 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
         FlightStatusGet(&flightStatus);
         ActuatorDesiredGet(&desired);
         ActuatorCommandGet(&command);
+        SystemSettingsThrustControlGet(&thrustType);
+
+        // read in throttle and collective -demultiplex thrust
+        switch (thrustType) {
+        case SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE:
+            throttleDesired = desired.Thrust;
+            ManualControlCommandCollectiveGet(&collectiveDesired);
+            break;
+        case SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE:
+            ManualControlCommandThrottleGet(&throttleDesired);
+            collectiveDesired = desired.Thrust;
+            break;
+        default:
+            ManualControlCommandThrottleGet(&throttleDesired);
+            ManualControlCommandCollectiveGet(&collectiveDesired);
+        }
+
+        bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED;
+
+        // safety settings
+        if (!armed) {
+            throttleDesired = 0;
+        }
+        if (throttleDesired <= 0.00f || !armed) {
+            // force set all other controls to zero if throttle is cut (previously set in Stabilization)
+            if (actuatorSettings.LowThrottleZeroAxis.Roll == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
+                desired.Roll = 0;
+            }
+            if (actuatorSettings.LowThrottleZeroAxis.Pitch == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
+                desired.Pitch = 0;
+            }
+            if (actuatorSettings.LowThrottleZeroAxis.Yaw == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
+                desired.Yaw = 0;
+            }
+        }
 
 #ifdef DIAG_MIXERSTATUS
         MixerStatusGet(&mixerStatus);
@@ -238,18 +279,18 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
 
         AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR);
 
-        bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED;
+        bool activeThrottle   = (throttleDesired < 0.00f || throttleDesired > 0.00f);
-        bool positiveThrottle = desired.Throttle >= 0.00f;
+        bool positiveThrottle = (throttleDesired > 0.00f);
         bool spinWhileArmed   = actuatorSettings.MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE;
 
-        float curve1 = MixerCurve(desired.Throttle, mixerSettings.ThrottleCurve1, MIXERSETTINGS_THROTTLECURVE1_NUMELEM);
+        float curve1 = MixerCurve(throttleDesired, mixerSettings.ThrottleCurve1, MIXERSETTINGS_THROTTLECURVE1_NUMELEM);
 
         // The source for the secondary curve is selectable
         float curve2 = 0;
         AccessoryDesiredData accessory;
         switch (mixerSettings.Curve2Source) {
         case MIXERSETTINGS_CURVE2SOURCE_THROTTLE:
-            curve2 = MixerCurve(desired.Throttle, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
+            curve2 = MixerCurve(throttleDesired, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
             break;
         case MIXERSETTINGS_CURVE2SOURCE_ROLL:
             curve2 = MixerCurve(desired.Roll, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
@@ -262,8 +303,7 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
             curve2 = MixerCurve(desired.Yaw, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
             break;
         case MIXERSETTINGS_CURVE2SOURCE_COLLECTIVE:
-            ManualControlCommandCollectiveGet(&curve2);
+            curve2 = MixerCurve(collectiveDesired, mixerSettings.ThrottleCurve2,
-            curve2 = MixerCurve(curve2, mixerSettings.ThrottleCurve2,
                                 MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
             break;
         case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0:
@@ -295,7 +335,7 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
                 continue;
             }
 
-            if ((mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_SERVO)) {
+            if ((mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_SERVO)) {
                 status[ct] = ProcessMixer(ct, curve1, curve2, &mixerSettings, &desired, dTSeconds);
             } else {
                 status[ct] = -1;
@@ -317,6 +357,16 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
                 }
             }
 
+            // 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
+                }
+            }
+
             // If an accessory channel is selected for direct bypass mode
             // In this configuration the accessory channel is scaled and mapped
             // directly to output.  Note: THERE IS NO SAFETY CHECK HERE FOR ARMING
@@ -419,6 +469,7 @@ float ProcessMixer(const int index, const float curve1, const float curve2,
                    (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_PITCH] / 128.0f) * desired->Pitch) +
                    (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_YAW] / 128.0f) * desired->Yaw);
 
+    // note: no feedforward for reversable motors yet for safety reasons
     if (mixer->type == MIXERSETTINGS_MIXER1TYPE_MOTOR) {
         if (result < 0.0f) { // idle throttle
             result = 0.0f;
@@ -537,7 +588,7 @@ static void setFailsafe(const ActuatorSettingsData *actuatorSettings, const Mixe
     for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) {
         if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) {
             Channel[n] = actuatorSettings->ChannelMin[n];
-        } else if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_SERVO) {
+        } else if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_SERVO || mixers[n].type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) {
             Channel[n] = actuatorSettings->ChannelNeutral[n];
         } else {
             Channel[n] = 0;

flight/modules/Airspeed/airspeed.c

@@ -41,9 +41,11 @@
 #include "hwsettings.h"
 #include "airspeedsettings.h"
 #include "airspeedsensor.h" // object that will be updated by the module
+#include "baro_airspeed_ms4525do.h"
 #include "baro_airspeed_etasv3.h"
 #include "baro_airspeed_mpxv.h"
 #include "gps_airspeed.h"
+#include "airspeedalarm.h"
 #include "taskinfo.h"
 
 // Private constants
@@ -59,7 +61,7 @@
 static xTaskHandle taskHandle;
 static bool airspeedEnabled  = false;
 static AirspeedSettingsData airspeedSettings;
-
+static AirspeedSettingsAirspeedSensorTypeOptions lastAirspeedSensorType = 0;
 static int8_t airspeedADCPin = -1;
 
 
@@ -118,6 +120,8 @@ int32_t AirspeedInitialize()
         }
     }
 
+    lastAirspeedSensorType = airspeedSettings.AirspeedSensorType;
+
     AirspeedSensorInitialize();
     AirspeedSettingsInitialize();
 
@@ -144,6 +148,7 @@ static void airspeedTask(__attribute__((unused)) void *parameters)
 
     airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
 
+
     // Main task loop
     portTickType lastSysTime = xTaskGetTickCount();
     while (1) {
@@ -152,6 +157,12 @@ static void airspeedTask(__attribute__((unused)) void *parameters)
         // Update the airspeed object
         AirspeedSensorGet(&airspeedData);
 
+        // if sensor type changed reset Airspeed alarm
+        if (airspeedSettings.AirspeedSensorType != lastAirspeedSensorType) {
+            AirspeedAlarm(SYSTEMALARMS_ALARM_DEFAULT);
+            lastAirspeedSensorType = airspeedSettings.AirspeedSensorType;
+        }
+
         switch (airspeedSettings.AirspeedSensorType) {
 #if defined(PIOS_INCLUDE_MPXV)
         case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002:
@@ -165,12 +176,20 @@ static void airspeedTask(__attribute__((unused)) void *parameters)
             // Eagletree Airspeed v3
             baro_airspeedGetETASV3(&airspeedData, &airspeedSettings);
             break;
+#endif
+#if defined(PIOS_INCLUDE_MS4525DO)
+        case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_PIXHAWKAIRSPEEDMS4525DO:
+            // PixHawk Airpeed based on MS4525DO
+            baro_airspeedGetMS4525DO(&airspeedData, &airspeedSettings);
+            break;
 #endif
         case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GROUNDSPEEDBASEDWINDESTIMATION:
             gps_airspeedGet(&airspeedData, &airspeedSettings);
             break;
+        case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_NONE:
         default:
             airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
+            break;
         }
 
         // Set the UAVO

flight/modules/Airspeed/airspeedalarm.c

@@ -0,0 +1,65 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup AirspeedModule Airspeed Module
+ * @brief Handle locally airspeed alarms issue changes to PIOS only when necessary
+ * @{
+ *
+ * @file       airspeedalarm.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ * @brief      Airspeed module
+ *
+ * @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
+ */
+
+/**
+ * Output object: none
+ *
+ * Handle locally airspeed alarms issue changes to PIOS only when necessary
+ *
+ */
+
+#include "airspeedalarm.h"
+
+// local variable
+
+static SystemAlarmsAlarmOptions severitySet = SYSTEMALARMS_ALARM_UNINITIALISED;
+
+// functions
+
+/**
+ * Handle airspeed alarms and isuue an Alarm to PIOS only if necessary
+ */
+bool AirspeedAlarm(SystemAlarmsAlarmOptions severity)
+{
+    if (severity == severitySet) {
+        return false;
+    }
+
+    severitySet = severity;
+
+    return AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, severity) == 0;
+}
+
+
+/**
+ * @}
+ * @}
+ */

flight/modules/Airspeed/baro_airspeed_etasv3.c

@@ -40,6 +40,7 @@
 #include "hwsettings.h"
 #include "airspeedsettings.h"
 #include "airspeedsensor.h" // object that will be updated by the module
+#include "airspeedalarm.h"
 
 #if defined(PIOS_INCLUDE_ETASV3)
 
@@ -64,11 +65,13 @@ void baro_airspeedGetETASV3(AirspeedSensorData *airspeedSensor, AirspeedSettings
     if (airspeedSensor->SensorValue == (uint16_t)-1) {
         airspeedSensor->SensorConnected    = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
         airspeedSensor->CalibratedAirspeed = 0;
+        AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR);
         return;
     }
 
     // only calibrate if no stored calibration is available
     if (!airspeedSettings->ZeroPoint) {
+        AirspeedAlarm(SYSTEMALARMS_ALARM_WARNING);
         // Calibrate sensor by averaging zero point value
         if (calibrationCount <= CALIBRATION_IDLE_MS / airspeedSettings->SamplePeriod) {
             calibrationCount++;
@@ -93,6 +96,7 @@ void baro_airspeedGetETASV3(AirspeedSensorData *airspeedSensor, AirspeedSettings
     // Compute airspeed
     airspeedSensor->CalibratedAirspeed = airspeedSettings->Scale * sqrtf((float)abs(airspeedSensor->SensorValue - airspeedSettings->ZeroPoint));
     airspeedSensor->SensorConnected    = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
+    AirspeedAlarm(SYSTEMALARMS_ALARM_OK);
 }
 
 

flight/modules/Airspeed/baro_airspeed_mpxv.c

@@ -40,6 +40,7 @@
 #include "hwsettings.h"
 #include "airspeedsettings.h"
 #include "airspeedsensor.h" // object that will be updated by the module
+#include "airspeedalarm.h"
 
 #if defined(PIOS_INCLUDE_MPXV)
 
@@ -63,7 +64,7 @@ void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsDa
     // Ensure that the ADC pin is properly configured
     if (airspeedADCPin < 0) {
         airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
-
+        AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR);
         return;
     }
     if (sensor.type == PIOS_MPXV_UNKNOWN) {
@@ -76,6 +77,7 @@ void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsDa
             break;
         default:
             airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
+            AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR);
             return;
         }
     }
@@ -83,6 +85,7 @@ void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsDa
     airspeedSensor->SensorValue = PIOS_MPXV_Measure(&sensor);
 
     if (!airspeedSettings->ZeroPoint) {
+        AirspeedAlarm(SYSTEMALARMS_ALARM_WARNING);
         // Calibrate sensor by averaging zero point value
         if (calibrationCount < CALIBRATION_IDLE_MS / airspeedSettings->SamplePeriod) { // First let sensor warm up and stabilize.
             calibrationCount++;
@@ -107,6 +110,7 @@ void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsDa
 
     airspeedSensor->CalibratedAirspeed = PIOS_MPXV_CalcAirspeed(&sensor, airspeedSensor->SensorValue) * (alpha) + airspeedSensor->CalibratedAirspeed * (1.0f - alpha);
     airspeedSensor->SensorConnected    = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
+    AirspeedAlarm(SYSTEMALARMS_ALARM_OK);
 }
 
 #endif /* if defined(PIOS_INCLUDE_MPXV) */

flight/modules/Airspeed/baro_airspeed_ms4525do.c

@@ -0,0 +1,140 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup AirspeedModule Airspeed Module
+ * @brief Communicate with airspeed sensors and return values
+ * @{
+ *
+ * @file       baro_airspeed_ms4525do.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ * @brief      Airspeed module, handles temperature and pressure readings from MS4525DO
+ *
+ * @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
+ */
+
+/**
+ * Output object: BaroAirspeed
+ *
+ * This module will periodically update the value of the BaroAirspeed object.
+ *
+ */
+
+#include "openpilot.h"
+#include "hwsettings.h"
+#include "airspeedsettings.h"
+#include "airspeedsensor.h" // object that will be updated by the module
+#include "airspeedalarm.h"
+#include "taskinfo.h"
+
+#if defined(PIOS_INCLUDE_MS4525DO)
+
+#define CALIBRATION_IDLE_MS  0              // Time to wait before calibrating, in [ms]
+#define CALIBRATION_COUNT_MS 4000 // Time to spend calibrating, in [ms]
+#define FILTER_SHIFT         5                      // Barry Dorr filter parameter k
+
+#define P0                   101325.0f           // standard pressure
+#define CCEXPONENT           0.2857142857f       // exponent of compressibility correction 2/7
+#define CASFACTOR            760.8802669f        // sqrt(5) * speed of sound at standard
+#define TASFACTOR            0.05891022589f      // 1/sqrt(T0)
+
+// Private types
+
+// Private functions definitions
+
+// Private variables
+static uint16_t calibrationCount = 0;
+static uint32_t filter_reg = 0; // Barry Dorr filter register
+
+void baro_airspeedGetMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings)
+{
+    uint16_t values[2];
+
+    // Read sensor
+    int8_t retVal = PIOS_MS4525DO_Read(values);
+
+    // check for errors
+    if (retVal != 0) {
+        airspeedSensor->SensorValue            = -1;
+        airspeedSensor->SensorValueTemperature = -1;
+        airspeedSensor->SensorConnected        = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
+        airspeedSensor->CalibratedAirspeed     = 0;
+        AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR);
+        return;
+    }
+
+    airspeedSensor->SensorValue = values[0];
+    airspeedSensor->SensorValueTemperature = values[1];
+
+    // only calibrate if no stored calibration is available
+    if (!airspeedSettings->ZeroPoint) {
+        // Calibrate sensor by averaging zero point value
+        if (calibrationCount <= CALIBRATION_IDLE_MS / airspeedSettings->SamplePeriod) {
+            calibrationCount++;
+            filter_reg = (airspeedSensor->SensorValue << FILTER_SHIFT);
+            AirspeedAlarm(SYSTEMALARMS_ALARM_WARNING);
+            return;
+        } else if (calibrationCount <= (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS) / airspeedSettings->SamplePeriod) {
+            calibrationCount++;
+            // update filter register
+            filter_reg = filter_reg - (filter_reg >> FILTER_SHIFT) + airspeedSensor->SensorValue;
+
+            if (calibrationCount > (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS) / airspeedSettings->SamplePeriod) {
+                // Scale output for unity gain.
+                airspeedSettings->ZeroPoint = (uint16_t)(filter_reg >> FILTER_SHIFT);
+
+                AirspeedSettingsZeroPointSet(&airspeedSettings->ZeroPoint);
+                calibrationCount = 0;
+            }
+            AirspeedAlarm(SYSTEMALARMS_ALARM_WARNING);
+            return;
+        }
+    }
+
+    /*  Compute airspeed
+        assume sensor is A Type and has a range of 1 psi, i.e. Pmin=-1.0 psi and Pmax=1.0 psi
+        Datasheet pressure: output = 0.8 * 16383 / (Pmax-Pmin) * (P - Pmin) + 0.1 * 16383
+        Inversion: P = (10*output - 81915)/65532 in psi
+        1 psi = 6894,757293168 Pa
+        P = (10*output - 81915)*0.1052120688 in Pa
+        Datasheet temperature: output = (T+50)*2047 / 200
+        Inversion: T = (200*out - 102350)/2047 in C
+        T = (200*out - 102350)/2047 + 273.15 in K
+     */
+    const float dP = (10 * (int32_t)(airspeedSensor->SensorValue - airspeedSettings->ZeroPoint)) * 0.1052120688f;
+    const float T  = (float)(200 * (int32_t)airspeedSensor->SensorValueTemperature - 102350) / 2047 + 273.15f;
+
+    airspeedSensor->DifferentialPressure = dP;
+    airspeedSensor->Temperature = T;
+    // CAS  = Csound * sqrt( 5 *( (dP/P0 +1)^(2/7) - 1) )
+    // TAS  = Csound * sqrt( 5 T/T0 *( (dP/P0 +1)^(2/7) - 1) )
+    // where Csound = 340.276 m/s at standard condition T0=288.15 K and P0 = 101315 Pa
+    airspeedSensor->CalibratedAirspeed = airspeedSettings->Scale * CASFACTOR * sqrtf(powf(fabsf(dP) / P0 + 1.0f, CCEXPONENT) - 1.0f);
+    airspeedSensor->TrueAirspeed = airspeedSensor->CalibratedAirspeed * TASFACTOR * sqrtf(T);
+    airspeedSensor->SensorConnected    = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
+    // everything is fine so set ALARM_OK
+    AirspeedAlarm(SYSTEMALARMS_ALARM_OK);
+}
+
+#endif /* if defined(PIOS_INCLUDE_MS4525DO) */
+
+/**
+ * @}
+ * @}
+ */

flight/modules/Airspeed/gps_airspeed.c

@@ -37,6 +37,7 @@
 #include "airspeedsettings.h"
 #include "gps_airspeed.h"
 #include "CoordinateConversions.h"
+#include "airspeedalarm.h"
 #include <pios_math.h>
 
 
@@ -127,6 +128,7 @@ void gps_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *air
         if (gpsVelData.North * gpsVelData.North + gpsVelData.East * gpsVelData.East + gpsVelData.Down * gpsVelData.Down < 1.0f) {
             airspeedData->CalibratedAirspeed = 0;
             airspeedData->SensorConnected    = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
+            AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR);
             return; // do not calculate if gps velocity is insufficient...
         }
 
@@ -141,11 +143,13 @@ void gps_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *air
         if (!IS_REAL(airspeedData->CalibratedAirspeed)) {
             airspeedData->CalibratedAirspeed = 0;
             airspeedData->SensorConnected    = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
+            AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR);
         } else {
             // need a low pass filter to filter out spikes in non coordinated maneuvers
             airspeedData->CalibratedAirspeed = (1.0f - airspeedSettings->GroundSpeedBasedEstimationLowPassAlpha) * gps->oldAirspeed + airspeedSettings->GroundSpeedBasedEstimationLowPassAlpha * airspeed;
             gps->oldAirspeed = airspeedData->CalibratedAirspeed;
             airspeedData->SensorConnected    = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
+            AirspeedAlarm(SYSTEMALARMS_ALARM_OK);
         }
 
         // Save old variables for next pass

flight/modules/Airspeed/inc/airspeedalarm.h

@@ -2,13 +2,13 @@
  ******************************************************************************
  * @addtogroup OpenPilotModules OpenPilot Modules
  * @{
- * @addtogroup FlightPlan Flight Plan Module
+ * @addtogroup AirspeedModule Airspeed Module
- * @brief Executes flight plan scripts in Python
+ * @brief Handle locally airspeed alarms issue changes to PIOS only when necessary
  * @{
  *
- * @file       flightplan.c
+ * @file       airspeedalarm.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
- * @brief      Executes flight plan scripts in Python
+ * @brief      Airspeed module, reads temperature and pressure from MS4525DO
  *
  * @see        The GNU Public License (GPL) Version 3
  *
@@ -28,9 +28,18 @@
  * with this program; if not, write to the Free Software Foundation, Inc.,
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
-#ifndef FLIGHTPLAN_H
+#ifndef AIRSPEEDALARM_H
-#define FLIGHTPLAN_H
+#define AIRSPEEDALARM_H
 
-int32_t FlightPlanInitialize();
+#include <openpilot.h>
+#include "alarms.h"
 
-#endif // FLIGHTPLAN_H
+
+bool AirspeedAlarm(SystemAlarmsAlarmOptions severity);
+
+#endif // AIRSPEEDALARM_H
+
+/**
+ * @}
+ * @}
+ */

flight/modules/Airspeed/inc/baro_airspeed_ms4525do.h

@@ -0,0 +1,43 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup AirspeedModule Airspeed Module
+ * @brief Calculate airspeed from measurements of differential pressure from a MS4525DO (PixHawk airspeed sensor)
+ * @{
+ *
+ * @file       baro_airspeed_mas4525do.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ * @brief      Airspeed module, reads temperature and pressure from MS4525DO
+ *
+ * @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 BARO_AIRSPEED_MS4525DO_H
+#define BARO_AIRSPEED_MS4525DO_H
+#if defined(PIOS_INCLUDE_MS4525DO)
+
+void baro_airspeedGetMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings);
+
+#endif /* PIOS_INCLUDE_MS4525DO */
+#endif // BARO_AIRSPEED_MS4525DO_H
+
+/**
+ * @}
+ * @}
+ */

flight/modules/Altitude/revolution/altitude.c

@@ -40,22 +40,26 @@
 
 #include "altitude.h"
 #include "barosensor.h" // object that will be updated by the module
+#include "revosettings.h"
 #if defined(PIOS_INCLUDE_HCSR04)
 #include "sonaraltitude.h" // object that will be updated by the module
 #endif
 #include "taskinfo.h"
 
 // Private constants
-#define STACK_SIZE_BYTES 500
+#define STACK_SIZE_BYTES 550
 #define TASK_PRIORITY    (tskIDLE_PRIORITY + 1)
 
 // Private types
 
 // Private variables
 static xTaskHandle taskHandle;
-
+static RevoSettingsBaroTempCorrectionPolynomialData baroCorrection;
+static RevoSettingsBaroTempCorrectionExtentData baroCorrectionExtent;
+static volatile bool tempCorrectionEnabled;
 // Private functions
 static void altitudeTask(void *parameters);
+static void SettingsUpdatedCb(UAVObjEvent *ev);
 
 /**
  * Initialise the module, called on startup
@@ -77,6 +81,9 @@ int32_t AltitudeStart()
 int32_t AltitudeInitialize()
 {
     BaroSensorInitialize();
+    RevoSettingsInitialize();
+    RevoSettingsConnectCallback(&SettingsUpdatedCb);
+    SettingsUpdatedCb(NULL);
 #if defined(PIOS_INCLUDE_HCSR04)
     SonarAltitudeInitialize();
 #endif
@@ -103,6 +110,8 @@ static void altitudeTask(__attribute__((unused)) void *parameters)
 // Undef for normal operation
 // #define PIOS_MS5611_SLOW_TEMP_RATE 20
 
+    RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection);
+
 #ifdef PIOS_MS5611_SLOW_TEMP_RATE
     uint8_t temp_press_interleave_count = 1;
 #endif
@@ -154,12 +163,17 @@ static void altitudeTask(__attribute__((unused)) void *parameters)
         vTaskDelay(PIOS_MS5611_GetDelay());
         PIOS_MS5611_ReadADC();
 
-
         temp  = PIOS_MS5611_GetTemperature();
         press = PIOS_MS5611_GetPressure();
 
-
+        if (tempCorrectionEnabled) {
-        float altitude = 44330.0f * (1.0f - powf(press / MS5611_P0, (1.0f / 5.255f)));
+            // pressure bias = A + B*t + C*t^2 + D * t^3
+            // in case the temperature is outside of the calibrated range, uses the nearest extremes
+            float ctemp = temp > baroCorrectionExtent.max ? baroCorrectionExtent.max :
+                          (temp < baroCorrectionExtent.min ? baroCorrectionExtent.min : temp);
+            press -= baroCorrection.a + ((baroCorrection.d * ctemp + baroCorrection.c) * ctemp + baroCorrection.b) * ctemp;
+        }
+        float altitude = 44330.0f * (1.0f - powf((press) / MS5611_P0, (1.0f / 5.255f)));
 
         if (!isnan(altitude)) {
             data.Altitude    = altitude;
@@ -171,6 +185,13 @@ static void altitudeTask(__attribute__((unused)) void *parameters)
     }
 }
 
+static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+{
+    RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection);
+    RevoSettingsBaroTempCorrectionExtentGet(&baroCorrectionExtent);
+    tempCorrectionEnabled = !(baroCorrectionExtent.max - baroCorrectionExtent.min < 0.1f ||
+                              (baroCorrection.a < 1e-9f && baroCorrection.b < 1e-9f && baroCorrection.c < 1e-9f && baroCorrection.d < 1e-9f));
+}
 /**
  * @}
  * @}

flight/modules/AltitudeHold/altitudehold.c

@@ -45,34 +45,37 @@
 
 #include <openpilot.h>
 
+#include <callbackinfo.h>
+
 #include <math.h>
+#include <pid.h>
 #include <CoordinateConversions.h>
-#include <altholdsmoothed.h>
-#include <attitudestate.h>
 #include <altitudeholdsettings.h>
 #include <altitudeholddesired.h> // object that will be updated by the module
-#include <barosensor.h>
+#include <altitudeholdstatus.h>
-#include <positionstate.h>
 #include <flightstatus.h>
 #include <stabilizationdesired.h>
 #include <accelstate.h>
-#include <taskinfo.h>
 #include <pios_constants.h>
+#include <velocitystate.h>
+#include <positionstate.h>
 // Private constants
-#define MAX_QUEUE_SIZE   2
+
-#define STACK_SIZE_BYTES 1024
+#define CALLBACK_PRIORITY      CALLBACK_PRIORITY_LOW
-#define TASK_PRIORITY    (tskIDLE_PRIORITY + 1)
+#define CBTASK_PRIORITY        CALLBACK_TASK_FLIGHTCONTROL
-#define ACCEL_DOWNSAMPLE 4
+
-#define TIMEOUT_TRESHOLD 200000
+#define STACK_SIZE_BYTES       1024
+#define DESIRED_UPDATE_RATE_MS 100 // milliseconds
 // Private types
 
 // Private variables
-static xTaskHandle altitudeHoldTaskHandle;
+static DelayedCallbackInfo *altitudeHoldCBInfo;
-static xQueueHandle queue;
 static AltitudeHoldSettingsData altitudeHoldSettings;
+static struct pid pid0, pid1;
+
 
 // Private functions
-static void altitudeHoldTask(void *parameters);
+static void altitudeHoldTask(void);
 static void SettingsUpdatedCb(UAVObjEvent *ev);
 
 /**
@@ -82,8 +85,8 @@ static void SettingsUpdatedCb(UAVObjEvent *ev);
 int32_t AltitudeHoldStart()
 {
     // Start main task
-    xTaskCreate(altitudeHoldTask, (signed char *)"AltitudeHold", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &altitudeHoldTaskHandle);
+    SettingsUpdatedCb(NULL);
-    PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ALTITUDEHOLD, altitudeHoldTaskHandle);
+    PIOS_CALLBACKSCHEDULER_Dispatch(altitudeHoldCBInfo);
 
     return 0;
 }
@@ -96,319 +99,107 @@ int32_t AltitudeHoldInitialize()
 {
     AltitudeHoldSettingsInitialize();
     AltitudeHoldDesiredInitialize();
-    AltHoldSmoothedInitialize();
+    AltitudeHoldStatusInitialize();
 
     // Create object queue
-    queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
 
+    altitudeHoldCBInfo = PIOS_CALLBACKSCHEDULER_Create(&altitudeHoldTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_ALTITUDEHOLD, STACK_SIZE_BYTES);
     AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb);
 
     return 0;
 }
 MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart);
 
-float tau;
-float throttleIntegral;
-float velocity;
-float decay;
-float velocity_decay;
-bool running = false;
-float error;
-float switchThrottle;
-float smoothed_altitude;
-float starting_altitude;
 
 /**
  * Module thread, should not return.
  */
-static void altitudeHoldTask(__attribute__((unused)) void *parameters)
+static void altitudeHoldTask(void)
 {
-    AltitudeHoldDesiredData altitudeHoldDesired;
+    static float startThrust = 0.5f;
-    StabilizationDesiredData stabilizationDesired;
 
-    portTickType thisTime, lastUpdateTime;
+    // make sure we run only when we are supposed to run
-    UAVObjEvent ev;
+    FlightStatusData flightStatus;
 
-    // Force update of the settings
+    FlightStatusGet(&flightStatus);
-    SettingsUpdatedCb(&ev);
+    switch (flightStatus.FlightMode) {
-    // Failsafe handling
+    case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
-    uint32_t lastAltitudeHoldDesiredUpdate = 0;
+    case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO:
-    bool enterFailSafe = false;
+        break;
-    // Listen for updates.
+    default:
-    AltitudeHoldDesiredConnectQueue(queue);
+        pid_zero(&pid0);
-    BaroSensorConnectQueue(queue);
+        pid_zero(&pid1);
-    FlightStatusConnectQueue(queue);
+        StabilizationDesiredThrustGet(&startThrust);
-    AccelStateConnectQueue(queue);
+        PIOS_CALLBACKSCHEDULER_Schedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
-    bool altitudeHoldFlightMode = false;
+        return;
-    BaroSensorAltitudeGet(&smoothed_altitude);
-    running = false;
-    enum init_state { WAITING_BARO, WAITIING_INIT, INITED } init = WAITING_BARO;
 
-    uint8_t flightMode;
+        break;
-    FlightStatusFlightModeGet(&flightMode);
-    // initialize enable flag
-    altitudeHoldFlightMode = flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD || flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO;
-    // Main task loop
-    bool baro_updated = false;
-    while (1) {
-        enterFailSafe = PIOS_DELAY_DiffuS(lastAltitudeHoldDesiredUpdate) > TIMEOUT_TRESHOLD;
-        // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
-        if (xQueueReceive(queue, &ev, 100 / portTICK_RATE_MS) != pdTRUE) {
-            if (!running) {
-                throttleIntegral = 0;
-            }
-
-            // Todo: Add alarm if it should be running
-            continue;
-        } else if (ev.obj == BaroSensorHandle()) {
-            baro_updated = true;
-
-            init = (init == WAITING_BARO) ? WAITIING_INIT : init;
-        } else if (ev.obj == FlightStatusHandle()) {
-            FlightStatusFlightModeGet(&flightMode);
-            altitudeHoldFlightMode = flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD || flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO;
-            if (altitudeHoldFlightMode && !running) {
-                // Copy the current throttle as a starting point for integral
-                StabilizationDesiredThrottleGet(&throttleIntegral);
-                switchThrottle = throttleIntegral;
-                error    = 0;
-                velocity = 0;
-                running  = true;
-
-                AltHoldSmoothedData altHold;
-                AltHoldSmoothedGet(&altHold);
-                starting_altitude = altHold.Altitude;
-            } else if (!altitudeHoldFlightMode) {
-                running = false;
-                lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw();
-            }
-        } else if (ev.obj == AccelStateHandle()) {
-            static uint32_t timeval;
-
-            static float z[4] = { 0, 0, 0, 0 };
-            float z_new[4];
-            float P[4][4], K[4][2], x[2];
-            float G[4] = { 1.0e-15f, 1.0e-15f, 1.0e-3f, 1.0e-7f };
-            static float V[4][4] = {
-                { 10.0f, 0.0f,   0.0f,   0.0f    },
-                { 0.0f,  100.0f, 0.0f,   0.0f    },
-                { 0.0f,  0.0f,   100.0f, 0.0f    },
-                { 0.0f,  0.0f,   0.0f,   1000.0f }
-            };
-            static uint32_t accel_downsample_count = 0;
-            static float accels_accum[3] = { 0.0f, 0.0f, 0.0f };
-            float dT;
-            static float S[2] = { 1.0f, 10.0f };
-
-            /* Somehow this always assigns to zero.  Compiler bug? Race condition? */
-            S[0] = altitudeHoldSettings.PressureNoise;
-            S[1] = altitudeHoldSettings.AccelNoise;
-            G[2] = altitudeHoldSettings.AccelDrift;
-
-            AccelStateData accelState;
-            AccelStateGet(&accelState);
-            AttitudeStateData attitudeState;
-            AttitudeStateGet(&attitudeState);
-            BaroSensorData baro;
-            BaroSensorGet(&baro);
-
-            /* Downsample accels to stop this calculation consuming too much CPU */
-            accels_accum[0] += accelState.x;
-            accels_accum[1] += accelState.y;
-            accels_accum[2] += accelState.z;
-            accel_downsample_count++;
-
-            if (accel_downsample_count < ACCEL_DOWNSAMPLE) {
-                continue;
-            }
-
-            accel_downsample_count = 0;
-            accelState.x    = accels_accum[0] / ACCEL_DOWNSAMPLE;
-            accelState.y    = accels_accum[1] / ACCEL_DOWNSAMPLE;
-            accelState.z    = accels_accum[2] / ACCEL_DOWNSAMPLE;
-            accels_accum[0] = accels_accum[1] = accels_accum[2] = 0;
-
-            thisTime = xTaskGetTickCount();
-
-            if (init == WAITIING_INIT) {
-                z[0] = baro.Altitude;
-                z[1] = 0;
-                z[2] = accelState.z;
-                z[3] = 0;
-                init = INITED;
-            } else if (init == WAITING_BARO) {
-                continue;
-            }
-
-            x[0] = baro.Altitude;
-            // rotate avg accels into earth frame and store it
-            if (1) {
-                float q[4], Rbe[3][3];
-                q[0] = attitudeState.q1;
-                q[1] = attitudeState.q2;
-                q[2] = attitudeState.q3;
-                q[3] = attitudeState.q4;
-                Quaternion2R(q, Rbe);
-                x[1] = -(Rbe[0][2] * accelState.x + Rbe[1][2] * accelState.y + Rbe[2][2] * accelState.z + 9.81f);
-            } else {
-                x[1] = -accelState.z + 9.81f;
-            }
-
-            dT = PIOS_DELAY_DiffuS(timeval) / 1.0e6f;
-            timeval = PIOS_DELAY_GetRaw();
-
-            P[0][0] = dT * (V[0][1] + dT * V[1][1]) + V[0][0] + G[0] + dT * V[1][0];
-            P[0][1] = dT * (V[0][2] + dT * V[1][2]) + V[0][1] + dT * V[1][1];
-            P[0][2] = V[0][2] + dT * V[1][2];
-            P[0][3] = V[0][3] + dT * V[1][3];
-            P[1][0] = dT * (V[1][1] + dT * V[2][1]) + V[1][0] + dT * V[2][0];
-            P[1][1] = dT * (V[1][2] + dT * V[2][2]) + V[1][1] + G[1] + dT * V[2][1];
-            P[1][2] = V[1][2] + dT * V[2][2];
-            P[1][3] = V[1][3] + dT * V[2][3];
-            P[2][0] = V[2][0] + dT * V[2][1];
-            P[2][1] = V[2][1] + dT * V[2][2];
-            P[2][2] = V[2][2] + G[2];
-            P[2][3] = V[2][3];
-            P[3][0] = V[3][0] + dT * V[3][1];
-            P[3][1] = V[3][1] + dT * V[3][2];
-            P[3][2] = V[3][2];
-            P[3][3] = V[3][3] + G[3];
-
-            if (baro_updated) {
-                K[0][0]  = -(V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]) + 1.0f;
-                K[0][1]  = ((V[0][2] + V[0][3]) * S[0] + dT * (V[1][2] + V[1][3]) * S[0]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]);
-                K[1][0]  = (V[1][0] * G[2] + V[1][0] * G[3] + V[1][0] * S[1] + V[1][0] * V[2][2] - V[2][0] * V[1][2] + V[1][0] * V[2][3] + V[1][0] * V[3][2] - V[2][0] * V[1][3] - V[1][2] * V[3][0] + V[1][0] * V[3][3] - V[3][0] * V[1][3] + (dT * dT) * V[2][1] * V[3][2] - (dT * dT) * V[2][2] * V[3][1] + (dT * dT) * V[2][1] * V[3][3] - (dT * dT) * V[3][1] * V[2][3] + dT * V[1][1] * G[2] + dT * V[2][0] * G[2] + dT * V[1][1] * G[3] + dT * V[2][0] * G[3] + dT * V[1][1] * S[1] + dT * V[2][0] * S[1] + (dT * dT) * V[2][1] * G[2] + (dT * dT) * V[2][1] * G[3] + (dT * dT) * V[2][1] * S[1] + dT * V[1][1] * V[2][2] - dT * V[1][2] * V[2][1] + dT * V[1][1] * V[2][3] + dT * V[1][1] * V[3][2] + dT * V[2][0] * V[3][2] - dT * V[1][2] * V[3][1] - dT * V[2][1] * V[1][3] - dT * V[3][0] * V[2][2] + dT * V[1][1] * V[3][3] + dT * V[2][0] * V[3][3] - dT * V[3][0] * V[2][3] - dT * V[1][3] * V[3][1]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]);
-                K[1][1]  = (V[1][2] * G[0] + V[1][3] * G[0] + V[1][2] * S[0] + V[1][3] * S[0] + V[0][0] * V[1][2] - V[1][0] * V[0][2] + V[0][0] * V[1][3] - V[1][0] * V[0][3] + (dT * dT) * V[0][1] * V[2][2] + (dT * dT) * V[1][0] * V[2][2] - (dT * dT) * V[0][2] * V[2][1] - (dT * dT) * V[2][0] * V[1][2] + (dT * dT) * V[0][1] * V[2][3] + (dT * dT) * V[1][0] * V[2][3] - (dT * dT) * V[2][0] * V[1][3] - (dT * dT) * V[0][3] * V[2][1] + (dT * dT * dT) * V[1][1] * V[2][2] - (dT * dT * dT) * V[1][2] * V[2][1] + (dT * dT * dT) * V[1][1] * V[2][3] - (dT * dT * dT) * V[2][1] * V[1][3] + dT * V[2][2] * G[0] + dT * V[2][3] * G[0] + dT * V[2][2] * S[0] + dT * V[2][3] * S[0] + dT * V[0][0] * V[2][2] + dT * V[0][1] * V[1][2] - dT * V[0][2] * V[1][1] - dT * V[0][2] * V[2][0] + dT * V[0][0] * V[2][3] + dT * V[0][1] * V[1][3] - dT * V[1][1] * V[0][3] - dT * V[2][0] * V[0][3]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]);
-                K[2][0]  = (V[2][0] * G[3] - V[3][0] * G[2] + V[2][0] * S[1] + V[2][0] * V[3][2] - V[3][0] * V[2][2] + V[2][0] * V[3][3] - V[3][0] * V[2][3] + dT * V[2][1] * G[3] - dT * V[3][1] * G[2] + dT * V[2][1] * S[1] + dT * V[2][1] * V[3][2] - dT * V[2][2] * V[3][1] + dT * V[2][1] * V[3][3] - dT * V[3][1] * V[2][3]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]);
-                K[2][1]  = (V[0][0] * G[2] + V[2][2] * G[0] + V[2][3] * G[0] + V[2][2] * S[0] + V[2][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] - V[2][0] * V[0][3] + G[0] * G[2] + G[2] * S[0] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] - (dT * dT) * V[2][1] * V[1][3] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + (dT * dT) * V[1][1] * G[2] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[1][0] * V[2][3] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]);
-                K[3][0]  = (-V[2][0] * G[3] + V[3][0] * G[2] + V[3][0] * S[1] - V[2][0] * V[3][2] + V[3][0] * V[2][2] - V[2][0] * V[3][3] + V[3][0] * V[2][3] - dT * V[2][1] * G[3] + dT * V[3][1] * G[2] + dT * V[3][1] * S[1] - dT * V[2][1] * V[3][2] + dT * V[2][2] * V[3][1] - dT * V[2][1] * V[3][3] + dT * V[3][1] * V[2][3]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]);
-                K[3][1]  = (V[0][0] * G[3] + V[3][2] * G[0] + V[3][3] * G[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[3][2] - V[0][2] * V[3][0] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[3] + G[3] * S[0] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + (dT * dT) * V[1][1] * G[3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]);
-
-                z_new[0] = -K[0][0] * (dT * z[1] - x[0] + z[0]) + dT * z[1] - K[0][1] * (-x[1] + z[2] + z[3]) + z[0];
-                z_new[1] = -K[1][0] * (dT * z[1] - x[0] + z[0]) + dT * z[2] - K[1][1] * (-x[1] + z[2] + z[3]) + z[1];
-                z_new[2] = -K[2][0] * (dT * z[1] - x[0] + z[0]) - K[2][1] * (-x[1] + z[2] + z[3]) + z[2];
-                z_new[3] = -K[3][0] * (dT * z[1] - x[0] + z[0]) - K[3][1] * (-x[1] + z[2] + z[3]) + z[3];
-
-                memcpy(z, z_new, sizeof(z_new));
-
-                V[0][0] = -K[0][1] * P[2][0] - K[0][1] * P[3][0] - P[0][0] * (K[0][0] - 1.0f);
-                V[0][1] = -K[0][1] * P[2][1] - K[0][1] * P[3][2] - P[0][1] * (K[0][0] - 1.0f);
-                V[0][2] = -K[0][1] * P[2][2] - K[0][1] * P[3][2] - P[0][2] * (K[0][0] - 1.0f);
-                V[0][3] = -K[0][1] * P[2][3] - K[0][1] * P[3][3] - P[0][3] * (K[0][0] - 1.0f);
-                V[1][0] = P[1][0] - K[1][0] * P[0][0] - K[1][1] * P[2][0] - K[1][1] * P[3][0];
-                V[1][1] = P[1][1] - K[1][0] * P[0][1] - K[1][1] * P[2][1] - K[1][1] * P[3][2];
-                V[1][2] = P[1][2] - K[1][0] * P[0][2] - K[1][1] * P[2][2] - K[1][1] * P[3][2];
-                V[1][3] = P[1][3] - K[1][0] * P[0][3] - K[1][1] * P[2][3] - K[1][1] * P[3][3];
-                V[2][0] = -K[2][0] * P[0][0] - K[2][1] * P[3][0] - P[2][0] * (K[2][1] - 1.0f);
-                V[2][1] = -K[2][0] * P[0][1] - K[2][1] * P[3][2] - P[2][1] * (K[2][1] - 1.0f);
-                V[2][2] = -K[2][0] * P[0][2] - K[2][1] * P[3][2] - P[2][2] * (K[2][1] - 1.0f);
-                V[2][3] = -K[2][0] * P[0][3] - K[2][1] * P[3][3] - P[2][3] * (K[2][1] - 1.0f);
-                V[3][0] = -K[3][0] * P[0][0] - K[3][1] * P[2][0] - P[3][0] * (K[3][1] - 1.0f);
-                V[3][1] = -K[3][0] * P[0][1] - K[3][1] * P[2][1] - P[3][2] * (K[3][1] - 1.0f);
-                V[3][2] = -K[3][0] * P[0][2] - K[3][1] * P[2][2] - P[3][2] * (K[3][1] - 1.0f);
-                V[3][3] = -K[3][0] * P[0][3] - K[3][1] * P[2][3] - P[3][3] * (K[3][1] - 1.0f);
-
-
-                baro_updated = false;
-            } else {
-                K[0][0]  = (V[0][2] + V[0][3] + dT * V[1][2] + dT * V[1][3]) / (V[2][2] + V[2][3] + V[3][2] + V[3][3] + G[2] + G[3] + S[1]);
-                K[1][0]  = (V[1][2] + V[1][3] + dT * V[2][2] + dT * V[2][3]) / (V[2][2] + V[2][3] + V[3][2] + V[3][3] + G[2] + G[3] + S[1]);
-                K[2][0]  = (V[2][2] + V[2][3] + G[2]) / (V[2][2] + V[2][3] + V[3][2] + V[3][3] + G[2] + G[3] + S[1]);
-                K[3][0]  = (V[3][2] + V[3][3] + G[3]) / (V[2][2] + V[2][3] + V[3][2] + V[3][3] + G[2] + G[3] + S[1]);
-
-
-                z_new[0] = dT * z[1] - K[0][0] * (-x[1] + z[2] + z[3]) + z[0];
-                z_new[1] = dT * z[2] - K[1][0] * (-x[1] + z[2] + z[3]) + z[1];
-                z_new[2] = -K[2][0] * (-x[1] + z[2] + z[3]) + z[2];
-                z_new[3] = -K[3][0] * (-x[1] + z[2] + z[3]) + z[3];
-
-                memcpy(z, z_new, sizeof(z_new));
-
-                V[0][0] = P[0][0] - K[0][0] * P[2][0] - K[0][0] * P[3][0];
-                V[0][1] = P[0][1] - K[0][0] * P[2][1] - K[0][0] * P[3][2];
-                V[0][2] = P[0][2] - K[0][0] * P[2][2] - K[0][0] * P[3][2];
-                V[0][3] = P[0][3] - K[0][0] * P[2][3] - K[0][0] * P[3][3];
-                V[1][0] = P[1][0] - K[1][0] * P[2][0] - K[1][0] * P[3][0];
-                V[1][1] = P[1][1] - K[1][0] * P[2][1] - K[1][0] * P[3][2];
-                V[1][2] = P[1][2] - K[1][0] * P[2][2] - K[1][0] * P[3][2];
-                V[1][3] = P[1][3] - K[1][0] * P[2][3] - K[1][0] * P[3][3];
-                V[2][0] = -K[2][0] * P[3][0] - P[2][0] * (K[2][0] - 1.0f);
-                V[2][1] = -K[2][0] * P[3][2] - P[2][1] * (K[2][0] - 1.0f);
-                V[2][2] = -K[2][0] * P[3][2] - P[2][2] * (K[2][0] - 1.0f);
-                V[2][3] = -K[2][0] * P[3][3] - P[2][3] * (K[2][0] - 1.0f);
-                V[3][0] = -K[3][0] * P[2][0] - P[3][0] * (K[3][0] - 1.0f);
-                V[3][1] = -K[3][0] * P[2][1] - P[3][2] * (K[3][0] - 1.0f);
-                V[3][2] = -K[3][0] * P[2][2] - P[3][2] * (K[3][0] - 1.0f);
-                V[3][3] = -K[3][0] * P[2][3] - P[3][3] * (K[3][0] - 1.0f);
-            }
-
-            AltHoldSmoothedData altHold;
-            AltHoldSmoothedGet(&altHold);
-            altHold.Altitude = z[0];
-            altHold.Velocity = z[1];
-            altHold.Accel    = z[2];
-            AltHoldSmoothedSet(&altHold);
-
-            AltHoldSmoothedGet(&altHold);
-
-            // Verify that we are in altitude hold mode
-            uint8_t armed;
-            FlightStatusArmedGet(&armed);
-            if (!altitudeHoldFlightMode || armed != FLIGHTSTATUS_ARMED_ARMED) {
-                running = false;
-            }
-
-            if (!running) {
-                lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw();
-                continue;
-            }
-
-            // Compute the altitude error
-            error = (starting_altitude + altitudeHoldDesired.Altitude) - altHold.Altitude;
-
-            // Compute integral off altitude error
-            throttleIntegral += error * altitudeHoldSettings.Ki * dT;
-
-            // Only update stabilizationDesired less frequently
-            if ((thisTime - lastUpdateTime) < 20) {
-                continue;
-            }
-
-            lastUpdateTime = thisTime;
-
-            // Instead of explicit limit on integral you output limit feedback
-            StabilizationDesiredGet(&stabilizationDesired);
-            if (!enterFailSafe) {
-                stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp + throttleIntegral -
-                                                altHold.Velocity * altitudeHoldSettings.Kd - altHold.Accel * altitudeHoldSettings.Ka;
-                if (stabilizationDesired.Throttle > 1) {
-                    throttleIntegral -= (stabilizationDesired.Throttle - 1);
-                    stabilizationDesired.Throttle = 1;
-                } else if (stabilizationDesired.Throttle < 0) {
-                    throttleIntegral -= stabilizationDesired.Throttle;
-                    stabilizationDesired.Throttle = 0;
-                }
-            } else {
-                // shutdown motors
-                stabilizationDesired.Throttle = -1;
-            }
-            stabilizationDesired.StabilizationMode.Roll  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
-            stabilizationDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
-            stabilizationDesired.StabilizationMode.Yaw   = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
-            stabilizationDesired.Roll  = altitudeHoldDesired.Roll;
-            stabilizationDesired.Pitch = altitudeHoldDesired.Pitch;
-            stabilizationDesired.Yaw   = altitudeHoldDesired.Yaw;
-
-            StabilizationDesiredSet(&stabilizationDesired);
-        } else if (ev.obj == AltitudeHoldDesiredHandle()) {
-            // reset the failsafe timer
-            lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw();
-            AltitudeHoldDesiredGet(&altitudeHoldDesired);
-        }
     }
+
+    AltitudeHoldStatusData altitudeHoldStatus;
+    AltitudeHoldStatusGet(&altitudeHoldStatus);
+
+    // do the actual control loop(s)
+    AltitudeHoldDesiredData altitudeHoldDesired;
+    AltitudeHoldDesiredGet(&altitudeHoldDesired);
+    float positionStateDown;
+    PositionStateDownGet(&positionStateDown);
+    float velocityStateDown;
+    VelocityStateDownGet(&velocityStateDown);
+
+    switch (altitudeHoldDesired.ControlMode) {
+    case ALTITUDEHOLDDESIRED_CONTROLMODE_ALTITUDE:
+        // altitude control loop
+        altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, 1.0f, altitudeHoldDesired.SetPoint, positionStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS);
+        break;
+    case ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY:
+        altitudeHoldStatus.VelocityDesired = altitudeHoldDesired.SetPoint;
+        break;
+    default:
+        altitudeHoldStatus.VelocityDesired = 0;
+        break;
+    }
+
+    AltitudeHoldStatusSet(&altitudeHoldStatus);
+
+    float thrust;
+    switch (altitudeHoldDesired.ControlMode) {
+    case ALTITUDEHOLDDESIRED_CONTROLMODE_THRUST:
+        thrust = altitudeHoldDesired.SetPoint;
+        break;
+    default:
+        // velocity control loop
+        thrust = startThrust - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS);
+
+        if (thrust >= 1.0f) {
+            thrust = 1.0f;
+        }
+        if (thrust <= 0.0f) {
+            thrust = 0.0f;
+        }
+        break;
+    }
+
+    StabilizationDesiredData stab;
+    StabilizationDesiredGet(&stab);
+    stab.Roll   = altitudeHoldDesired.Roll;
+    stab.Pitch  = altitudeHoldDesired.Pitch;
+    stab.Yaw    = altitudeHoldDesired.Yaw;
+    stab.Thrust = thrust;
+    stab.StabilizationMode.Roll  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stab.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stab.StabilizationMode.Yaw   = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
+
+    StabilizationDesiredSet(&stab);
+
+    PIOS_CALLBACKSCHEDULER_Schedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
 }
 
 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);
 }

flight/modules/Attitude/attitude.c

@@ -57,6 +57,7 @@
 #include "accelstate.h"
 #include "attitudestate.h"
 #include "attitudesettings.h"
+#include "accelgyrosettings.h"
 #include "flightstatus.h"
 #include "manualcontrolcommand.h"
 #include "taskinfo.h"
@@ -70,12 +71,17 @@
 
 #define SENSOR_PERIOD    4
 #define UPDATE_RATE      25.0f
-#define GYRO_NEUTRAL     1665
+
+#define UPDATE_EXPECTED  (1.0f / 666.0f)
+#define UPDATE_MIN       1.0e-6f
+#define UPDATE_MAX       1.0f
+#define UPDATE_ALPHA     1.0e-2f
 
 // Private types
 
 // Private variables
 static xTaskHandle taskHandle;
+static PiOSDeltatimeConfig dtconfig;
 
 // Private functions
 static void AttitudeTask(void *parameters);
@@ -96,24 +102,42 @@ static float accels_filtered[3];
 static float grot_filtered[3];
 static float yawBiasRate = 0;
 static float rollPitchBiasRate = 0.0f;
-static float gyroGain    = 0.42f;
+static AccelGyroSettingsaccel_biasData accel_bias;
-static int16_t accelbias[3];
 static float q[4] = { 1, 0, 0, 0 };
 static float R[3][3];
 static int8_t rotate = 0;
 static bool zero_during_arming = false;
 static bool bias_correct_gyro  = true;
 
+// static float gyros_passed[3];
+
+// temp coefficient to calculate gyro bias
+static bool apply_gyro_temp  = false;
+static bool apply_accel_temp = false;
+static AccelGyroSettingsgyro_temp_coeffData gyro_temp_coeff;;
+static AccelGyroSettingsaccel_temp_coeffData accel_temp_coeff;
+static AccelGyroSettingstemp_calibrated_extentData temp_calibrated_extent;
+
+// Accel and Gyro scaling (this is the product of sensor scale and adjustement in AccelGyroSettings
+static AccelGyroSettingsgyro_scaleData gyro_scale;
+static AccelGyroSettingsaccel_scaleData accel_scale;
+
+
 // For running trim flights
 static volatile bool trim_requested   = false;
 static volatile int32_t trim_accels[3];
 static volatile int32_t trim_samples;
 int32_t const MAX_TRIM_FLIGHT_SAMPLES = 65535;
 
-#define GRAV        9.81f
+#define GRAV                       9.81f
-#define ACCEL_SCALE (GRAV * 0.004f)
+#define STD_CC_ACCEL_SCALE         (GRAV * 0.004f)
 /* 0.004f is gravity / LSB */
+#define STD_CC_ANALOG_GYRO_NEUTRAL 1665
+#define STD_CC_ANALOG_GYRO_GAIN    0.42f
 
+// Used to detect CC vs CC3D
+static const struct pios_board_info *bdinfo = &pios_board_info_blob;
+#define BOARDISCC3D                (bdinfo->board_rev == 0x02)
 /**
  * Initialise the module, called on startup
  * \returns 0 on success or -1 if initialisation failed
@@ -136,6 +160,7 @@ int32_t AttitudeInitialize(void)
 {
     AttitudeStateInitialize();
     AttitudeSettingsInitialize();
+    AccelGyroSettingsInitialize();
     AccelStateInitialize();
     GyroStateInitialize();
 
@@ -166,7 +191,7 @@ int32_t AttitudeInitialize(void)
     trim_requested = false;
 
     AttitudeSettingsConnectCallback(&settingsUpdatedCb);
-
+    AccelGyroSettingsConnectCallback(&settingsUpdatedCb);
     return 0;
 }
 
@@ -190,9 +215,7 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
         PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
     }
 
-    const struct pios_board_info *bdinfo = &pios_board_info_blob;
+    bool cc3d = BOARDISCC3D;
-
-    bool cc3d = bdinfo->board_rev == 0x02;
 
     if (cc3d) {
 #if defined(PIOS_INCLUDE_MPU6000)
@@ -214,6 +237,8 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
     // Force settings update to make sure rotation loaded
     settingsUpdatedCb(AttitudeSettingsHandle());
 
+    PIOS_DELTATIME_Init(&dtconfig, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
+
     // Main task loop
     while (1) {
         FlightStatusData flightStatus;
@@ -272,8 +297,6 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
     }
 }
 
-float gyros_passed[3];
-
 /**
  * Get an update from the sensors
  * @param[in] attitudeRaw Populate the UAVO instead of saving right here
@@ -301,9 +324,9 @@ static int32_t updateSensors(AccelStateData *accelState, GyroStateData *gyros)
     }
 
     // First sample is temperature
-    gyros->x = -(gyro[1] - GYRO_NEUTRAL) * gyroGain;
+    gyros->x = -(gyro[1] - STD_CC_ANALOG_GYRO_NEUTRAL) * gyro_scale.X;
-    gyros->y = (gyro[2] - GYRO_NEUTRAL) * gyroGain;
+    gyros->y = (gyro[2] - STD_CC_ANALOG_GYRO_NEUTRAL) * gyro_scale.Y;
-    gyros->z = -(gyro[3] - GYRO_NEUTRAL) * gyroGain;
+    gyros->z = -(gyro[3] - STD_CC_ANALOG_GYRO_NEUTRAL) * gyro_scale.Z;
 
     int32_t x = 0;
     int32_t y = 0;
@@ -317,9 +340,10 @@ static int32_t updateSensors(AccelStateData *accelState, GyroStateData *gyros)
         y += -accel_data.y;
         z += -accel_data.z;
     } while ((i < 32) && (samples_remaining > 0));
-    // gyros->temperature = samples_remaining;
 
-    float accel[3] = { (float)x / i, (float)y / i, (float)z / i };
+    float accel[3] = { accel_scale.X * (float)x / i,
+                       accel_scale.Y * (float)y / i,
+                       accel_scale.Z * (float)z / i };
 
     if (rotate) {
         // TODO: rotate sensors too so stabilization is well behaved
@@ -357,9 +381,9 @@ static int32_t updateSensors(AccelStateData *accelState, GyroStateData *gyros)
     }
 
     // Scale accels and correct bias
-    accelState->x = (accelState->x - accelbias[0]) * ACCEL_SCALE;
+    accelState->x -= accel_bias.X;
-    accelState->y = (accelState->y - accelbias[1]) * ACCEL_SCALE;
+    accelState->y -= accel_bias.Y;
-    accelState->z = (accelState->z - accelbias[2]) * ACCEL_SCALE;
+    accelState->z -= accel_bias.Z;
 
     if (bias_correct_gyro) {
         // Applying integral component here so it can be seen on the gyros and correct bias
@@ -387,7 +411,7 @@ static int32_t updateSensors(AccelStateData *accelState, GyroStateData *gyros)
  * @param[in] attitudeRaw Populate the UAVO instead of saving right here
  * @return 0 if successfull, -1 if not
  */
-struct pios_mpu6000_data mpu6000_data;
+static struct pios_mpu6000_data mpu6000_data;
 static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *gyrosData)
 {
     float accels[3], gyros[3];
@@ -403,15 +427,30 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *
     if (GyroStateReadOnly() || AccelStateReadOnly()) {
         return 0;
     }
+    gyros[0]  = mpu6000_data.gyro_x * gyro_scale.X;
+    gyros[1]  = mpu6000_data.gyro_y * gyro_scale.Y;
+    gyros[2]  = mpu6000_data.gyro_z * gyro_scale.Z;
 
-    gyros[0]  = mpu6000_data.gyro_x * PIOS_MPU6000_GetScale();
+    accels[0] = mpu6000_data.accel_x * accel_scale.X;
-    gyros[1]  = mpu6000_data.gyro_y * PIOS_MPU6000_GetScale();
+    accels[1] = mpu6000_data.accel_y * accel_scale.Y;
-    gyros[2]  = mpu6000_data.gyro_z * PIOS_MPU6000_GetScale();
+    accels[2] = mpu6000_data.accel_z * accel_scale.Z;
 
-    accels[0] = mpu6000_data.accel_x * PIOS_MPU6000_GetAccelScale();
+    float ctemp = mpu6000_data.temperature > temp_calibrated_extent.max ? temp_calibrated_extent.max :
-    accels[1] = mpu6000_data.accel_y * PIOS_MPU6000_GetAccelScale();
+                  (mpu6000_data.temperature < temp_calibrated_extent.min ? temp_calibrated_extent.min
-    accels[2] = mpu6000_data.accel_z * PIOS_MPU6000_GetAccelScale();
+                   : mpu6000_data.temperature);
 
+
+    if (apply_gyro_temp) {
+        gyros[0] -= (gyro_temp_coeff.X + gyro_temp_coeff.X2 * ctemp) * ctemp;
+        gyros[1] -= (gyro_temp_coeff.Y + gyro_temp_coeff.Y2 * ctemp) * ctemp;
+        gyros[2] -= (gyro_temp_coeff.Z + gyro_temp_coeff.Z2 * ctemp) * ctemp;
+    }
+
+    if (apply_accel_temp) {
+        accels[0] -= accel_temp_coeff.X * ctemp;
+        accels[1] -= accel_temp_coeff.Y * ctemp;
+        accels[2] -= accel_temp_coeff.Z * ctemp;
+    }
     // gyrosData->temperature  = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f;
     // accelsData->temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f;
 #endif /* if defined(PIOS_INCLUDE_MPU6000) */
@@ -429,9 +468,9 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *
         gyros[2]  = vec_out[2];
     }
 
-    accelStateData->x = accels[0] - accelbias[0] * ACCEL_SCALE; // Applying arbitrary scale here to match CC v1
+    accelStateData->x = accels[0] - accel_bias.X;
-    accelStateData->y = accels[1] - accelbias[1] * ACCEL_SCALE;
+    accelStateData->y = accels[1] - accel_bias.Y;
-    accelStateData->z = accels[2] - accelbias[2] * ACCEL_SCALE;
+    accelStateData->z = accels[2] - accel_bias.Z;
 
     gyrosData->x = gyros[0];
     gyrosData->y = gyros[1];
@@ -473,12 +512,7 @@ static inline void apply_accel_filter(const float *raw, float *filtered)
 
 static void updateAttitude(AccelStateData *accelStateData, GyroStateData *gyrosData)
 {
-    float dT;
+    float dT      = PIOS_DELTATIME_GetAverageSeconds(&dtconfig);
-    portTickType thisSysTime = xTaskGetTickCount();
-    static portTickType lastSysTime = 0;
-
-    dT = (thisSysTime == lastSysTime) ? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f;
-    lastSysTime = thisSysTime;
 
     // Bad practice to assume structure order, but saves memory
     float *gyros  = &gyrosData->x;
@@ -587,14 +621,14 @@ static void updateAttitude(AccelStateData *accelStateData, GyroStateData *gyrosD
 static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
 {
     AttitudeSettingsData attitudeSettings;
+    AccelGyroSettingsData accelGyroSettings;
 
     AttitudeSettingsGet(&attitudeSettings);
-
+    AccelGyroSettingsGet(&accelGyroSettings);
 
     accelKp     = attitudeSettings.AccelKp;
     accelKi     = attitudeSettings.AccelKi;
     yawBiasRate = attitudeSettings.YawBiasRate;
-    gyroGain    = attitudeSettings.GyroGain;
 
     // Calculate accel filter alpha, in the same way as for gyro data in stabilization module.
     const float fakeDt = 0.0025f;
@@ -609,13 +643,54 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
     zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE;
     bias_correct_gyro  = attitudeSettings.BiasCorrectGyro == ATTITUDESETTINGS_BIASCORRECTGYRO_TRUE;
 
-    accelbias[0] = attitudeSettings.AccelBias.X;
+    memcpy(&gyro_temp_coeff, &accelGyroSettings.gyro_temp_coeff, sizeof(AccelGyroSettingsgyro_temp_coeffData));
-    accelbias[1] = attitudeSettings.AccelBias.Y;
+    memcpy(&accel_temp_coeff, &accelGyroSettings.accel_temp_coeff, sizeof(AccelGyroSettingsaccel_temp_coeffData));
-    accelbias[2] = attitudeSettings.AccelBias.Z;
 
-    gyro_correct_int[0] = attitudeSettings.GyroBias.X / 100.0f;
+
-    gyro_correct_int[1] = attitudeSettings.GyroBias.Y / 100.0f;
+    apply_gyro_temp = (fabsf(gyro_temp_coeff.X) > 1e-6f ||
-    gyro_correct_int[2] = attitudeSettings.GyroBias.Z / 100.0f;
+                       fabsf(gyro_temp_coeff.Y) > 1e-6f ||
+                       fabsf(gyro_temp_coeff.Z) > 1e-6f ||
+                       fabsf(gyro_temp_coeff.X2) > 1e-6f ||
+                       fabsf(gyro_temp_coeff.Y2) > 1e-6f ||
+                       fabsf(gyro_temp_coeff.Z2) > 1e-6f);
+
+    apply_accel_temp = (fabsf(accel_temp_coeff.X) > 1e-6f ||
+                        fabsf(accel_temp_coeff.Y) > 1e-6f ||
+                        fabsf(accel_temp_coeff.Z) > 1e-6f);
+
+    gyro_correct_int[0] = accelGyroSettings.gyro_bias.X;
+    gyro_correct_int[1] = accelGyroSettings.gyro_bias.Y;
+    gyro_correct_int[2] = accelGyroSettings.gyro_bias.Z;
+
+    temp_calibrated_extent.min = accelGyroSettings.temp_calibrated_extent.min;
+    temp_calibrated_extent.max = accelGyroSettings.temp_calibrated_extent.max;
+
+    if (BOARDISCC3D) {
+        accel_bias.X  = accelGyroSettings.accel_bias.X;
+        accel_bias.Y  = accelGyroSettings.accel_bias.Y;
+        accel_bias.Z  = accelGyroSettings.accel_bias.Z;
+
+        gyro_scale.X  = accelGyroSettings.gyro_scale.X * PIOS_MPU6000_GetScale();
+        gyro_scale.Y  = accelGyroSettings.gyro_scale.Y * PIOS_MPU6000_GetScale();
+        gyro_scale.Z  = accelGyroSettings.gyro_scale.Z * PIOS_MPU6000_GetScale();
+
+        accel_scale.X = accelGyroSettings.accel_scale.X * PIOS_MPU6000_GetAccelScale();
+        accel_scale.Y = accelGyroSettings.accel_scale.Y * PIOS_MPU6000_GetAccelScale();
+        accel_scale.Z = accelGyroSettings.accel_scale.Z * PIOS_MPU6000_GetAccelScale();
+    } else {
+        // Original CC with analog gyros and ADXL accel
+        accel_bias.X  = accelGyroSettings.accel_bias.X;
+        accel_bias.Y  = accelGyroSettings.accel_bias.Y;
+        accel_bias.Z  = accelGyroSettings.accel_bias.Z;
+
+        gyro_scale.X  = accelGyroSettings.gyro_scale.X * STD_CC_ANALOG_GYRO_GAIN;
+        gyro_scale.Y  = accelGyroSettings.gyro_scale.Y * STD_CC_ANALOG_GYRO_GAIN;
+        gyro_scale.Z  = accelGyroSettings.gyro_scale.Z * STD_CC_ANALOG_GYRO_GAIN;
+
+        accel_scale.X = accelGyroSettings.accel_scale.X * STD_CC_ACCEL_SCALE;
+        accel_scale.Y = accelGyroSettings.accel_scale.Y * STD_CC_ACCEL_SCALE;
+        accel_scale.Z = accelGyroSettings.accel_scale.Z * STD_CC_ACCEL_SCALE;
+    }
 
     // Indicates not to expend cycles on rotation
     if (attitudeSettings.BoardRotation.Pitch == 0 && attitudeSettings.BoardRotation.Roll == 0 &&
@@ -643,11 +718,11 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
         trim_requested = true;
     } else if (attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_LOAD) {
         trim_requested = false;
-        attitudeSettings.AccelBias.X = trim_accels[0] / trim_samples;
+        accelGyroSettings.accel_scale.X = trim_accels[0] / trim_samples;
-        attitudeSettings.AccelBias.Y = trim_accels[1] / trim_samples;
+        accelGyroSettings.accel_scale.Y = trim_accels[1] / trim_samples;
         // Z should average -grav
-        attitudeSettings.AccelBias.Z = trim_accels[2] / trim_samples + GRAV / ACCEL_SCALE;
+        accelGyroSettings.accel_scale.Z = trim_accels[2] / trim_samples + GRAV;
-        attitudeSettings.TrimFlight  = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL;
+        attitudeSettings.TrimFlight     = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL;
         AttitudeSettingsSet(&attitudeSettings);
     } else {
         trim_requested = false;

flight/modules/Attitude/revolution/attitude.c

@@ -278,7 +278,7 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
         case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY:
             ret_val = updateAttitudeComplementary(first_run);
             break;
-        case REVOSETTINGS_FUSIONALGORITHM_INS13OUTDOOR:
+        case REVOSETTINGS_FUSIONALGORITHM_INS13GPSOUTDOOR:
             ret_val = updateAttitudeINSGPS(first_run, true);
             break;
         case REVOSETTINGS_FUSIONALGORITHM_INS13INDOOR:
@@ -650,7 +650,7 @@ static int32_t updateAttitudeComplementary(bool first_run)
         if (airspeedSensor.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE) {
             // we have airspeed available
             airspeed.CalibratedAirspeed = airspeedSensor.CalibratedAirspeed;
-            airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS(homeLocation.Altitude - positionState.Down);
+            airspeed.TrueAirspeed = (airspeedSensor.TrueAirspeed < 0.f) ? airspeed.CalibratedAirspeed *IAS2TAS(homeLocation.Altitude - positionState.Down) : airspeedSensor.TrueAirspeed;
             AirspeedStateSet(&airspeed);
         }
     }
@@ -1072,13 +1072,23 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
         vel[2]   = gpsVelData.Down;
     }
 
+    // Copy the position into the UAVO
+    PositionStateData positionState;
+    PositionStateGet(&positionState);
+    positionState.North = Nav.Pos[0];
+    positionState.East  = Nav.Pos[1];
+    positionState.Down  = Nav.Pos[2];
+    PositionStateSet(&positionState);
+
+    // airspeed correction needs current positionState
     if (airspeed_updated) {
         // we have airspeed available
         AirspeedStateData airspeed;
         AirspeedStateGet(&airspeed);
 
         airspeed.CalibratedAirspeed = airspeedData.CalibratedAirspeed;
-        airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS(homeLocation.Altitude - Nav.Pos[2]);
+        airspeed.TrueAirspeed = (airspeedData.TrueAirspeed < 0.f) ? airspeed.CalibratedAirspeed *IAS2TAS(homeLocation.Altitude - positionState.Down) : airspeedData.TrueAirspeed;
+
         AirspeedStateSet(&airspeed);
 
         if (!gps_vel_updated && !gps_updated) {
@@ -1107,14 +1117,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
         INSCorrection(&magData.x, NED, vel, (baroData.Altitude + baroOffset), sensors);
     }
 
-    // Copy the position and velocity into the UAVO
+    // Copy the velocity into the UAVO
-    PositionStateData positionState;
-    PositionStateGet(&positionState);
-    positionState.North = Nav.Pos[0];
-    positionState.East  = Nav.Pos[1];
-    positionState.Down  = Nav.Pos[2];
-    PositionStateSet(&positionState);
-
     VelocityStateData velocityState;
     VelocityStateGet(&velocityState);
     velocityState.North = Nav.Vel[0];

flight/modules/Autotune/autotune.c

@@ -182,8 +182,8 @@ static void AutotuneTask(__attribute__((unused)) void *parameters)
         }
 
         stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
-        stabDesired.Yaw = manualControl.Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW];
+        stabDesired.Yaw    = manualControl.Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW];
-        stabDesired.Throttle = manualControl.Throttle;
+        stabDesired.Thrust = manualControl.Thrust;
 
         switch (state) {
         case AT_INIT:
@@ -191,7 +191,7 @@ static void AutotuneTask(__attribute__((unused)) void *parameters)
             lastUpdateTime = xTaskGetTickCount();
 
             // Only start when armed and flying
-            if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && stabDesired.Throttle > 0) {
+            if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && stabDesired.Thrust > 0) {
                 state = AT_START;
             }
             break;
@@ -236,7 +236,7 @@ static void AutotuneTask(__attribute__((unused)) void *parameters)
         case AT_FINISHED:
 
             // Wait until disarmed and landed before updating the settings
-            if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED && stabDesired.Throttle <= 0) {
+            if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED && stabDesired.Thrust <= 0) {
                 state = AT_SET;
             }
 

flight/modules/Battery/battery.c

@@ -47,6 +47,7 @@
 
 #include "openpilot.h"
 
+#include "flightstatus.h"
 #include "flightbatterystate.h"
 #include "flightbatterysettings.h"
 #include "hwsettings.h"
@@ -69,6 +70,7 @@ static int8_t currentADCPin = -1; // ADC pin for current
 
 // Private functions
 static void onTimer(UAVObjEvent *ev);
+static int8_t GetNbCells(const FlightBatterySettingsData *batterySettings, FlightBatteryStateData *flightBatteryData);
 
 /**
  * Initialise the module, called on startup
@@ -113,6 +115,8 @@ int32_t BatteryInitialize(void)
         FlightBatteryStateInitialize();
         FlightBatterySettingsInitialize();
 
+        // FlightBatterySettingsConnectCallback(FlightBatterySettingsUpdatedCb);
+
         static UAVObjEvent ev;
 
         memset(&ev, 0, sizeof(UAVObjEvent));
@@ -125,10 +129,11 @@ int32_t BatteryInitialize(void)
 MODULE_INITCALL(BatteryInitialize, 0);
 static void onTimer(__attribute__((unused)) UAVObjEvent *ev)
 {
-    static FlightBatteryStateData flightBatteryData;
     static FlightBatterySettingsData batterySettings;
+    static FlightBatteryStateData flightBatteryData;
 
     FlightBatterySettingsGet(&batterySettings);
+    FlightBatteryStateGet(&flightBatteryData);
 
     const float dT = SAMPLE_PERIOD_MS / 1000.0f;
     float energyRemaining;
@@ -140,7 +145,11 @@ static void onTimer(__attribute__((unused)) UAVObjEvent *ev)
         flightBatteryData.Voltage = 0; // Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC
     }
 
-    if (currentADCPin >= 0) {
+    // voltage available: get the number of cells if possible, desired and not armed
+    GetNbCells(&batterySettings, &flightBatteryData);
+
+    // ad a plausibility check: zero voltage => zero current
+    if (currentADCPin >= 0 && flightBatteryData.Voltage > 0.f) {
         flightBatteryData.Current = (PIOS_ADC_PinGetVolt(currentADCPin) - batterySettings.SensorCalibrations.CurrentZero) * batterySettings.SensorCalibrations.CurrentFactor; // in Amps
         if (flightBatteryData.Current > flightBatteryData.PeakCurrent) {
             flightBatteryData.PeakCurrent = flightBatteryData.Current; // in Amps
@@ -149,7 +158,11 @@ static void onTimer(__attribute__((unused)) UAVObjEvent *ev)
         flightBatteryData.Current = -0; // Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC
     }
 
-    flightBatteryData.ConsumedEnergy += (flightBatteryData.Current * dT * 1000.0f / 3600.0f); // in mAh
+    // For safety reasons consider only positive currents in energy comsumption, i.e. no charging up.
+    // necesary when sensor are not perfectly calibrated
+    if (flightBatteryData.Current > 0) {
+        flightBatteryData.ConsumedEnergy += (flightBatteryData.Current * dT * 1000.0f / 3600.0f); // in mAh
+    }
 
     // Apply a 2 second rise time low-pass filter to average the current
     float alpha = 1.0f - dT / (dT + 2.0f);
@@ -184,9 +197,9 @@ static void onTimer(__attribute__((unused)) UAVObjEvent *ev)
 
         // FIXME: should make the battery voltage detection dependent on battery type.
         /*Not so sure. Some users will want to run their batteries harder than others, so it should be the user's choice. [KDS]*/
-        if (flightBatteryData.Voltage < batterySettings.CellVoltageThresholds.Alarm * batterySettings.NbCells) {
+        if (flightBatteryData.Voltage < batterySettings.CellVoltageThresholds.Alarm * flightBatteryData.NbCells) {
             AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_CRITICAL);
-        } else if (flightBatteryData.Voltage < batterySettings.CellVoltageThresholds.Warning * batterySettings.NbCells) {
+        } else if (flightBatteryData.Voltage < batterySettings.CellVoltageThresholds.Warning * flightBatteryData.NbCells) {
             AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_WARNING);
         } else {
             AlarmsClear(SYSTEMALARMS_ALARM_BATTERY);
@@ -196,6 +209,71 @@ static void onTimer(__attribute__((unused)) UAVObjEvent *ev)
     FlightBatteryStateSet(&flightBatteryData);
 }
 
+
+static int8_t GetNbCells(const FlightBatterySettingsData *batterySettings, FlightBatteryStateData *flightBatteryData)
+{
+    // get flight status to check for armed
+    uint8_t armed = 0;
+
+    FlightStatusArmedGet(&armed);
+
+
+    // check only if not armed
+    if (armed == FLIGHTSTATUS_ARMED_ARMED) {
+        return -2;
+    }
+
+    // prescribed number of cells?
+    if (batterySettings->NbCells != 0) {
+        flightBatteryData->NbCells = batterySettings->NbCells;
+        flightBatteryData->NbCellsAutodetected = 0;
+        return -3;
+    }
+
+    // plausibility check
+    if (flightBatteryData->Voltage <= 0.5f) {
+        // cannot detect number of cells
+        flightBatteryData->NbCellsAutodetected = 0;
+        return -1;
+    }
+
+    float voltageMin = 0.f, voltageMax = 0.f;
+
+    // Cell type specific values
+    // TODO: could be implemented as constant arrays indexed by cellType
+    // or could be part of the UAVObject definition
+    switch (batterySettings->Type) {
+    case FLIGHTBATTERYSETTINGS_TYPE_LIPO:
+    case FLIGHTBATTERYSETTINGS_TYPE_LICO:
+        voltageMin = 3.6f;
+        voltageMax = 4.2f;
+        break;
+    case FLIGHTBATTERYSETTINGS_TYPE_A123:
+        voltageMin = 2.01f;
+        voltageMax = 3.59f;
+        break;
+    case FLIGHTBATTERYSETTINGS_TYPE_LIFESO4:
+    default:
+        flightBatteryData->NbCellsAutodetected = 0;
+        return -1;
+    }
+
+    // uniquely measurable under any condition iff n * voltageMax < (n+1) * voltageMin
+    // or n < voltageMin / (voltageMax-voltageMin)
+    // weaken condition by setting n <= voltageMin / (voltageMax-voltageMin) and
+    // checking for v <= voltageMin * voltageMax / (voltageMax-voltageMin)
+    if (flightBatteryData->Voltage > voltageMin * voltageMax / (voltageMax - voltageMin)) {
+        flightBatteryData->NbCellsAutodetected = 0;
+        return -1;
+    }
+
+
+    flightBatteryData->NbCells = (int8_t)(flightBatteryData->Voltage / voltageMin);
+    flightBatteryData->NbCellsAutodetected = 1;
+
+    return flightBatteryData->NbCells;
+}
+
 /**
  * @}
  */

flight/modules/CallbackTest/callbacktest.c

@@ -65,13 +65,13 @@ int32_t CallbackTestInitialize()
 {
     mutex     = xSemaphoreCreateRecursiveMutex();
 
-    cbinfo[0] = DelayedCallbackCreate(&DelayedCb0, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, STACK_SIZE);
+    cbinfo[0] = PIOS_CALLBACKSCHEDULER_Create(&DelayedCb0, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, -1, STACK_SIZE);
-    cbinfo[1] = DelayedCallbackCreate(&DelayedCb1, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, STACK_SIZE);
+    cbinfo[1] = PIOS_CALLBACKSCHEDULER_Create(&DelayedCb1, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, -1, STACK_SIZE);
-    cbinfo[2] = DelayedCallbackCreate(&DelayedCb2, CALLBACK_PRIORITY_CRITICAL, tskIDLE_PRIORITY + 2, STACK_SIZE);
+    cbinfo[2] = PIOS_CALLBACKSCHEDULER_Create(&DelayedCb2, CALLBACK_PRIORITY_CRITICAL, tskIDLE_PRIORITY + 2, -1, STACK_SIZE);
-    cbinfo[3] = DelayedCallbackCreate(&DelayedCb3, CALLBACK_PRIORITY_CRITICAL, tskIDLE_PRIORITY + 2, STACK_SIZE);
+    cbinfo[3] = PIOS_CALLBACKSCHEDULER_Create(&DelayedCb3, CALLBACK_PRIORITY_CRITICAL, tskIDLE_PRIORITY + 2, -1, STACK_SIZE);
-    cbinfo[4] = DelayedCallbackCreate(&DelayedCb4, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, STACK_SIZE);
+    cbinfo[4] = PIOS_CALLBACKSCHEDULER_Create(&DelayedCb4, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, -1, STACK_SIZE);
-    cbinfo[5] = DelayedCallbackCreate(&DelayedCb5, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, STACK_SIZE);
+    cbinfo[5] = PIOS_CALLBACKSCHEDULER_Create(&DelayedCb5, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, -1, STACK_SIZE);
-    cbinfo[6] = DelayedCallbackCreate(&DelayedCb6, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 20, STACK_SIZE);
+    cbinfo[6] = PIOS_CALLBACKSCHEDULER_Create(&DelayedCb6, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 20, -1, STACK_SIZE);
 
 
     return 0;
@@ -79,22 +79,22 @@ int32_t CallbackTestInitialize()
 int32_t CallbackTestStart()
 {
     xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
-    DelayedCallbackDispatch(cbinfo[3]);
+    PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[3]);
-    DelayedCallbackDispatch(cbinfo[2]);
+    PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[2]);
-    DelayedCallbackDispatch(cbinfo[1]);
+    PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[1]);
-    DelayedCallbackDispatch(cbinfo[0]);
+    PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[0]);
     // different callback priorities within a taskpriority
-    DelayedCallbackSchedule(cbinfo[4], 30000, CALLBACK_UPDATEMODE_NONE);
+    PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[4], 30000, CALLBACK_UPDATEMODE_NONE);
-    DelayedCallbackSchedule(cbinfo[4], 5000, CALLBACK_UPDATEMODE_OVERRIDE);
+    PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[4], 5000, CALLBACK_UPDATEMODE_OVERRIDE);
-    DelayedCallbackSchedule(cbinfo[4], 4000, CALLBACK_UPDATEMODE_SOONER);
+    PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[4], 4000, CALLBACK_UPDATEMODE_SOONER);
-    DelayedCallbackSchedule(cbinfo[4], 10000, CALLBACK_UPDATEMODE_SOONER);
+    PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[4], 10000, CALLBACK_UPDATEMODE_SOONER);
-    DelayedCallbackSchedule(cbinfo[4], 1000, CALLBACK_UPDATEMODE_LATER);
+    PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[4], 1000, CALLBACK_UPDATEMODE_LATER);
-    DelayedCallbackSchedule(cbinfo[4], 4800, CALLBACK_UPDATEMODE_LATER);
+    PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[4], 4800, CALLBACK_UPDATEMODE_LATER);
-    DelayedCallbackSchedule(cbinfo[4], 48000, CALLBACK_UPDATEMODE_NONE);
+    PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[4], 48000, CALLBACK_UPDATEMODE_NONE);
     // should be at 4.8 seconds after this, allowing for exactly 9 prints of the following
-    DelayedCallbackSchedule(cbinfo[5], 500, CALLBACK_UPDATEMODE_NONE);
+    PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[5], 500, CALLBACK_UPDATEMODE_NONE);
     // delayed counter with 500 ms
-    DelayedCallbackDispatch(cbinfo[6]);
+    PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[6]);
     // high task prio
     xSemaphoreGiveRecursive(mutex);
     return 0;
@@ -104,28 +104,28 @@ static void DelayedCb0()
 {
     DEBUGPRINT("delayed counter low prio 0 updated: %i\n", counter[0]);
     if (++counter[0] < 10) {
-        DelayedCallbackDispatch(cbinfo[0]);
+        PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[0]);
     }
 }
 static void DelayedCb1()
 {
     DEBUGPRINT("delayed counter low prio 1 updated: %i\n", counter[1]);
     if (++counter[1] < 10) {
-        DelayedCallbackDispatch(cbinfo[1]);
+        PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[1]);
     }
 }
 static void DelayedCb2()
 {
     DEBUGPRINT("delayed counter high prio 2 updated: %i\n", counter[2]);
     if (++counter[2] < 10) {
-        DelayedCallbackDispatch(cbinfo[2]);
+        PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[2]);
     }
 }
 static void DelayedCb3()
 {
     DEBUGPRINT("delayed counter high prio 3 updated: %i\n", counter[3]);
     if (++counter[3] < 10) {
-        DelayedCallbackDispatch(cbinfo[3]);
+        PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[3]);
     }
 }
 static void DelayedCb4()
@@ -137,7 +137,7 @@ static void DelayedCb5()
 {
     DEBUGPRINT("delayed scheduled counter 5 updated: %i\n", counter[5]);
     if (++counter[5] < 10) {
-        DelayedCallbackSchedule(cbinfo[5], 500, CALLBACK_UPDATEMODE_NONE);
+        PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[5], 500, CALLBACK_UPDATEMODE_NONE);
     }
     // it will likely only reach 8 due to cb4 aborting the run
 }
@@ -145,6 +145,6 @@ static void DelayedCb6()
 {
     DEBUGPRINT("delayed counter 6 (high task prio) updated: %i\n", counter[6]);
     if (++counter[6] < 10) {
-        DelayedCallbackDispatch(cbinfo[6]);
+        PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[6]);
     }
 }

flight/modules/CameraStab/camerastab.c

@@ -128,6 +128,7 @@ int32_t CameraStabInitialize(void)
             .obj    = AttitudeStateHandle(),
             .instId = 0,
             .event  = 0,
+            .lowPriority = false,
         };
         EventPeriodicCallbackCreate(&ev, attitudeUpdated, SAMPLE_PERIOD_MS / portTICK_RATE_MS);
 

flight/modules/Example/examplemodcallback.c

@@ -45,6 +45,7 @@
  */
 
 #include "openpilot.h"
+#include "callbackinfo.h" // object needed for callback id macro CALLBACKINFO_RUNNING_<MODULENAME>
 #include "exampleobject1.h" // object the module will listen for updates (input)
 #include "exampleobject2.h" // object the module will update (output)
 #include "examplesettings.h" // object holding module settings (input)
@@ -71,7 +72,7 @@ int32_t ExampleModCallbackInitialize()
     // Listen for ExampleObject1 updates, connect a callback function
     ExampleObject1ConnectCallback(&ObjectUpdatedCb);
 
-    cbinfo = DelayedCallbackCreate(&DelayedCb, CALLBACK_PRIORITY, CBTASK_PRIORITY, STACK_SIZE);
+    cbinfo = PIOS_CALLBACKSCHEDULER_Create(&DelayedCb, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_EXAMPLE, STACK_SIZE);
 
     return 0;
 }
@@ -85,11 +86,11 @@ int32_t ExampleModCallbackInitialize()
  */
 static void ObjectUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 {
-    DelayedCallbackDispatch(cbinfo);
+    PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo);
 }
 
 /**
- * This function is called by the DelayedCallbackScheduler when its execution
+ * This function is called by the PIOS_CALLBACKSCHEDULER_Scheduler when its execution
  * has been requested.  Callbacks scheduled for execution are executed in the
  * same thread in a round robin fashion. The Dispatch function to reschedule
  * execution can be called from within the Callback itself, in which case the
@@ -135,5 +136,5 @@ ExampleObject2Set(&data2);
 
 // call the module again 10 seconds later,
 // even if the exampleobject has not been updated
-DelayedCallbackSchedule(cbinfo, 10 * 1000, CALLBACK_UPDATEMODE_NONE);
+PIOS_CALLBACKSCHEDULER_Schedule(cbinfo, 10 * 1000, CALLBACK_UPDATEMODE_NONE);
 }

flight/modules/FirmwareIAP/firmwareiap.c

@@ -100,6 +100,7 @@ int32_t FirmwareIAPInitialize()
     PIOS_BL_HELPER_FLASH_Read_Description(data.Description, FIRMWAREIAPOBJ_DESCRIPTION_NUMELEM);
     PIOS_SYS_SerialNumberGetBinary(data.CPUSerial);
     data.BoardRevision = bdinfo->board_rev;
+    data.BootloaderRevision = bdinfo->bl_rev;
     data.ArmReset = 0;
     data.crc = 0;
     FirmwareIAPObjSet(&data);
@@ -147,6 +148,7 @@ static void FirmwareIAPCallback(UAVObjEvent *ev)
             PIOS_BL_HELPER_FLASH_Read_Description(data.Description, FIRMWAREIAPOBJ_DESCRIPTION_NUMELEM);
             PIOS_SYS_SerialNumberGetBinary(data.CPUSerial);
             data.BoardRevision = bdinfo->board_rev;
+            data.BootloaderRevision = bdinfo->bl_rev;
             data.crc = PIOS_BL_HELPER_CRC_Memory_Calc();
             FirmwareIAPObjSet(&data);
         }

flight/modules/FixedWingPathFollower/fixedwingpathfollower.c

@@ -49,7 +49,6 @@
 #include "attitudestate.h"
 #include "pathdesired.h" // object that will be updated by the module
 #include "positionstate.h"
-#include "manualcontrol.h"
 #include "flightstatus.h"
 #include "pathstatus.h"
 #include "airspeedstate.h"
@@ -182,52 +181,50 @@ static void pathfollowerTask(__attribute__((unused)) void *parameters)
 
         uint8_t result;
         // Check the combinations of flightmode and pathdesired mode
-        switch (flightStatus.FlightMode) {
+        if (flightStatus.ControlChain.PathFollower == FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
-        case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
+            if (flightStatus.ControlChain.PathPlanner == FLIGHTSTATUS_CONTROLCHAIN_FALSE) {
-        case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
+                if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
-            if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
+                    updatePathVelocity();
-                updatePathVelocity();
+                    result = updateFixedDesiredAttitude();
-                result = updateFixedDesiredAttitude();
+                    if (result) {
-                if (result) {
+                        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
-                    AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
+                    } else {
+                        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
+                    }
                 } else {
-                    AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
+                    AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
                 }
             } else {
-                AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
+                pathStatus.UID    = pathDesired.UID;
-            }
+                pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
-            break;
+                switch (pathDesired.Mode) {
-        case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
+                case PATHDESIRED_MODE_FLYENDPOINT:
-            pathStatus.UID    = pathDesired.UID;
+                case PATHDESIRED_MODE_FLYVECTOR:
-            pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
+                case PATHDESIRED_MODE_FLYCIRCLERIGHT:
-            switch (pathDesired.Mode) {
+                case PATHDESIRED_MODE_FLYCIRCLELEFT:
-            case PATHDESIRED_MODE_FLYENDPOINT:
+                    updatePathVelocity();
-            case PATHDESIRED_MODE_FLYVECTOR:
+                    result = updateFixedDesiredAttitude();
-            case PATHDESIRED_MODE_FLYCIRCLERIGHT:
+                    if (result) {
-            case PATHDESIRED_MODE_FLYCIRCLELEFT:
+                        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
-                updatePathVelocity();
+                    } else {
-                result = updateFixedDesiredAttitude();
+                        pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
-                if (result) {
+                        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
+                    }
+                    break;
+                case PATHDESIRED_MODE_FIXEDATTITUDE:
+                    updateFixedAttitude(pathDesired.ModeParameters);
                     AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
-                } else {
+                    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_WARNING);
+                    AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
+                    break;
                 }
-                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;
             }
-            break;
+        } else {
-        default:
             // Be cleaner and get rid of global variables
             northVelIntegral = 0;
             eastVelIntegral  = 0;
@@ -235,8 +232,6 @@ static void pathfollowerTask(__attribute__((unused)) void *parameters)
             courseIntegral   = 0;
             speedIntegral    = 0;
             powerIntegral    = 0;
-
-            break;
         }
         PathStatusSet(&pathStatus);
     }
@@ -357,10 +352,10 @@ static void updateFixedAttitude(float *attitude)
     StabilizationDesiredData stabDesired;
 
     StabilizationDesiredGet(&stabDesired);
-    stabDesired.Roll     = attitude[0];
+    stabDesired.Roll   = attitude[0];
-    stabDesired.Pitch    = attitude[1];
+    stabDesired.Pitch  = attitude[1];
-    stabDesired.Yaw      = attitude[2];
+    stabDesired.Yaw    = attitude[2];
-    stabDesired.Throttle = attitude[3];
+    stabDesired.Thrust = attitude[3];
     stabDesired.StabilizationMode.Roll  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
     stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
     stabDesired.StabilizationMode.Yaw   = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
@@ -420,7 +415,7 @@ static uint8_t updateFixedDesiredAttitude()
 
 
     /**
-     * Compute speed error (required for throttle and pitch)
+     * Compute speed error (required for thrust and pitch)
      */
 
     // Current ground speed
@@ -474,9 +469,9 @@ static uint8_t updateFixedDesiredAttitude()
     }
 
     /**
-     * Compute desired throttle command
+     * Compute desired thrust command
      */
-    // compute saturated integral error throttle response. Make integral leaky for better performance. Approximately 30s time constant.
+    // compute saturated integral error thrust response. Make integral leaky for better performance. Approximately 30s time constant.
     if (fixedwingpathfollowerSettings.PowerPI.Ki > 0) {
         powerIntegral = bound(powerIntegral + -descentspeedError * dT,
                               -fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki,
@@ -491,7 +486,7 @@ static uint8_t updateFixedDesiredAttitude()
         fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Max
         );
 
-    // Compute final throttle response
+    // Compute final thrust response
     powerCommand = -descentspeedError * fixedwingpathfollowerSettings.PowerPI.Kp +
                    powerIntegral * fixedwingpathfollowerSettings.PowerPI.Ki +
                    speedErrorToPowerCommandComponent;
@@ -501,14 +496,14 @@ static uint8_t updateFixedDesiredAttitude()
     fixedwingpathfollowerStatus.ErrorInt.Power = powerIntegral;
     fixedwingpathfollowerStatus.Command.Power  = powerCommand;
 
-    // set throttle
+    // set thrust
-    stabDesired.Throttle = bound(fixedwingpathfollowerSettings.ThrottleLimit.Neutral + powerCommand,
+    stabDesired.Thrust = bound(fixedwingpathfollowerSettings.ThrustLimit.Neutral + powerCommand,
-                                 fixedwingpathfollowerSettings.ThrottleLimit.Min,
+                               fixedwingpathfollowerSettings.ThrustLimit.Min,
-                                 fixedwingpathfollowerSettings.ThrottleLimit.Max);
+                               fixedwingpathfollowerSettings.ThrustLimit.Max);
 
     // Error condition: plane cannot hold altitude at current speed.
     fixedwingpathfollowerStatus.Errors.Lowpower = 0;
-    if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit.Max && // throttle at maximum
+    if (fixedwingpathfollowerSettings.ThrustLimit.Neutral + powerCommand >= fixedwingpathfollowerSettings.ThrustLimit.Max && // thrust at maximum
         velocityState.Down > 0 && // we ARE going down
         descentspeedDesired < 0 && // we WANT to go up
         airspeedError > 0 && // we are too slow already
@@ -516,9 +511,9 @@ static uint8_t updateFixedDesiredAttitude()
         fixedwingpathfollowerStatus.Errors.Lowpower = 1;
         result = 0;
     }
-    // Error condition: plane keeps climbing despite minimum throttle (opposite of above)
+    // Error condition: plane keeps climbing despite minimum thrust (opposite of above)
     fixedwingpathfollowerStatus.Errors.Highpower = 0;
-    if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit.Min && // throttle at minimum
+    if (fixedwingpathfollowerSettings.ThrustLimit.Neutral + powerCommand <= fixedwingpathfollowerSettings.ThrustLimit.Min && // thrust at minimum
         velocityState.Down < 0 && // we ARE going up
         descentspeedDesired > 0 && // we WANT to go down
         airspeedError < 0 && // we are too fast already
@@ -560,7 +555,7 @@ static uint8_t updateFixedDesiredAttitude()
 
     // Error condition: high speed dive
     fixedwingpathfollowerStatus.Errors.Pitchcontrol = 0;
-    if (pitchCommand >= fixedwingpathfollowerSettings.PitchLimit.Max && // pitch demand is full up
+    if (fixedwingpathfollowerSettings.PitchLimit.Neutral + pitchCommand >= fixedwingpathfollowerSettings.PitchLimit.Max && // pitch demand is full up
         velocityState.Down > 0 && // we ARE going down
         descentspeedDesired < 0 && // we WANT to go up
         airspeedError < 0 && // we are too fast already

flight/modules/FlightPlan/flightplan.c

@@ -31,7 +31,6 @@
 
 #include <openpilot.h>
 
-#include "flightplan.h"
 #include "flightplanstatus.h"
 #include "flightplancontrol.h"
 #include "flightplansettings.h"

flight/modules/GPS/UBX.c

@@ -254,22 +254,25 @@ void parse_ubx_nav_velned(struct UBX_NAV_VELNED *velned, GPSPositionSensorData *
 #if !defined(PIOS_GPS_MINIMAL)
 void parse_ubx_nav_timeutc(struct UBX_NAV_TIMEUTC *timeutc)
 {
-    if (!(timeutc->valid & TIMEUTC_VALIDUTC)) {
+    // Test if time is valid
+    if ((timeutc->valid & TIMEUTC_VALIDTOW) && (timeutc->valid & TIMEUTC_VALIDWKN)) {
+        // Time is valid, set GpsTime
+        GPSTimeData GpsTime;
+
+        GpsTime.Year   = timeutc->year;
+        GpsTime.Month  = timeutc->month;
+        GpsTime.Day    = timeutc->day;
+        GpsTime.Hour   = timeutc->hour;
+        GpsTime.Minute = timeutc->min;
+        GpsTime.Second = timeutc->sec;
+
+        GPSTimeSet(&GpsTime);
+    } else {
+        // Time is not valid, nothing to do
         return;
     }
-
-    GPSTimeData GpsTime;
-
-    GpsTime.Year   = timeutc->year;
-    GpsTime.Month  = timeutc->month;
-    GpsTime.Day    = timeutc->day;
-    GpsTime.Hour   = timeutc->hour;
-    GpsTime.Minute = timeutc->min;
-    GpsTime.Second = timeutc->sec;
-
-    GPSTimeSet(&GpsTime);
 }
-#endif
+#endif /* if !defined(PIOS_GPS_MINIMAL) */
 
 #if !defined(PIOS_GPS_MINIMAL)
 void parse_ubx_nav_svinfo(struct UBX_NAV_SVINFO *svinfo)

flight/modules/Logging/Logging.c

@@ -73,7 +73,12 @@ int32_t LoggingStart(void)
     FlightStatusConnectCallback(FlightStatusUpdatedCb);
     SettingsUpdatedCb(DebugLogSettingsHandle());
 
-    UAVObjEvent ev = { .obj = DebugLogSettingsHandle(), .instId = 0, .event = EV_UPDATED_PERIODIC };
+    UAVObjEvent ev = {
+        .obj    = DebugLogSettingsHandle(),
+        .instId = 0,
+        .event  = EV_UPDATED_PERIODIC,
+        .lowPriority = true,
+    };
     EventPeriodicCallbackCreate(&ev, StatusUpdatedCb, 1000);
     // invoke a periodic dispatcher callback - the event struct is a dummy, it could be filled with anything!
     StatusUpdatedCb(&ev);

flight/modules/ManualControl/altitudehandler.c

@@ -0,0 +1,133 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup ManualControl
+ * @brief Interpretes the control input in ManualControlCommand
+ * @{
+ *
+ * @file       altitudehandler.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ *
+ * @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 "inc/manualcontrol.h"
+#include <manualcontrolcommand.h>
+#include <stabilizationbank.h>
+#include <altitudeholddesired.h>
+#include <altitudeholdsettings.h>
+#include <positionstate.h>
+
+#if defined(REVOLUTION)
+// Private constants
+
+// Private types
+
+// Private functions
+
+/**
+ * @brief Handler to control deprecated flight modes controlled by AltitudeHold module
+ * @input: ManualControlCommand
+ * @output: AltitudeHoldDesired
+ */
+void altitudeHandler(bool newinit)
+{
+    const float DEADBAND      = 0.20f;
+    const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2;
+    const float DEADBAND_LOW  = 1.0f / 2 - DEADBAND / 2;
+
+    if (newinit) {
+        StabilizationBankInitialize();
+        AltitudeHoldDesiredInitialize();
+        AltitudeHoldSettingsInitialize();
+        PositionStateInitialize();
+    }
+
+
+    // this is the max speed in m/s at the extents of thrust
+    float thrustRate;
+    uint8_t thrustExp;
+
+    static uint8_t flightMode;
+    static bool newaltitude = true;
+
+    ManualControlCommandData cmd;
+    ManualControlCommandGet(&cmd);
+
+    FlightStatusFlightModeGet(&flightMode);
+
+    AltitudeHoldDesiredData altitudeHoldDesiredData;
+    AltitudeHoldDesiredGet(&altitudeHoldDesiredData);
+
+    AltitudeHoldSettingsThrustExpGet(&thrustExp);
+    AltitudeHoldSettingsThrustRateGet(&thrustRate);
+
+    StabilizationBankData stabSettings;
+    StabilizationBankGet(&stabSettings);
+
+    PositionStateData posState;
+    PositionStateGet(&posState);
+
+    altitudeHoldDesiredData.Roll  = cmd.Roll * stabSettings.RollMax;
+    altitudeHoldDesiredData.Pitch = cmd.Pitch * stabSettings.PitchMax;
+    altitudeHoldDesiredData.Yaw   = cmd.Yaw * stabSettings.ManualRate.Yaw;
+
+    if (newinit) {
+        newaltitude = true;
+    }
+
+    uint8_t cutOff;
+    AltitudeHoldSettingsCutThrustWhenZeroGet(&cutOff);
+    if (cutOff && cmd.Thrust < 0) {
+        // Cut thrust if desired
+        altitudeHoldDesiredData.SetPoint    = cmd.Thrust;
+        altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_THRUST;
+        newaltitude = true;
+    } else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd.Thrust > 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
+        altitudeHoldDesiredData.SetPoint    = -((thrustExp * powf((cmd.Thrust - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - thrustExp) * (cmd.Thrust - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * thrustRate);
+        altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY;
+        newaltitude = true;
+    } else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd.Thrust < DEADBAND_LOW) {
+        altitudeHoldDesiredData.SetPoint    = -(-(thrustExp * powf((DEADBAND_LOW - (cmd.Thrust < 0 ? 0 : cmd.Thrust)) / DEADBAND_LOW, 3) + (255 - thrustExp) * (DEADBAND_LOW - cmd.Thrust) / DEADBAND_LOW) / 255 * thrustRate);
+        altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY;
+        newaltitude = true;
+    } else if (newaltitude == true) {
+        altitudeHoldDesiredData.SetPoint    = posState.Down;
+        altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_ALTITUDE;
+        newaltitude = false;
+    }
+
+    AltitudeHoldDesiredSet(&altitudeHoldDesiredData);
+}
+
+#else /* if defined(REVOLUTION) */
+void altitudeHandler(__attribute__((unused)) bool newinit)
+{
+    AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); // should not be called
+}
+#endif // REVOLUTION
+
+
+/**
+ * @}
+ * @}
+ */

flight/modules/ManualControl/armhandler.c

@@ -0,0 +1,326 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup ManualControl
+ * @brief Interpretes the control input in ManualControlCommand
+ * @{
+ *
+ * @file       armhandler.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ *
+ * @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 "inc/manualcontrol.h"
+#include <pios_struct_helper.h>
+#include <sanitycheck.h>
+#include <manualcontrolcommand.h>
+#include <accessorydesired.h>
+#include <flightstatus.h>
+#include <flightmodesettings.h>
+
+// Private constants
+#define ARMED_THRESHOLD 0.50f
+
+// Private types
+typedef enum {
+    ARM_STATE_DISARMED,
+    ARM_STATE_ARMING_MANUAL,
+    ARM_STATE_ARMED,
+    ARM_STATE_DISARMING_MANUAL,
+    ARM_STATE_DISARMING_TIMEOUT
+} ArmState_t;
+
+// Private variables
+
+// Private functions
+static void setArmedIfChanged(uint8_t val);
+static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time);
+static bool okToArm(void);
+static bool forcedDisArm(void);
+
+
+/**
+ * @brief Handler to interprete Command inputs in regards to arming/disarming
+ * @input: ManualControlCommand, AccessoryDesired
+ * @output: FlightStatus.Arming
+ */
+void armHandler(bool newinit)
+{
+    static ArmState_t armState;
+
+    if (newinit) {
+        AccessoryDesiredInitialize();
+        setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
+        armState = ARM_STATE_DISARMED;
+    }
+
+    portTickType sysTime = xTaskGetTickCount();
+
+    FlightModeSettingsData settings;
+    FlightModeSettingsGet(&settings);
+    ManualControlCommandData cmd;
+    ManualControlCommandGet(&cmd);
+    AccessoryDesiredData acc;
+
+    bool lowThrottle = cmd.Throttle < 0;
+
+    bool armSwitch   = false;
+
+    switch (settings.Arming) {
+    case FLIGHTMODESETTINGS_ARMING_ACCESSORY0:
+        AccessoryDesiredInstGet(0, &acc);
+        armSwitch = true;
+        break;
+    case FLIGHTMODESETTINGS_ARMING_ACCESSORY1:
+        AccessoryDesiredInstGet(1, &acc);
+        armSwitch = true;
+        break;
+    case FLIGHTMODESETTINGS_ARMING_ACCESSORY2:
+        AccessoryDesiredInstGet(2, &acc);
+        armSwitch = true;
+        break;
+    default:
+        break;
+    }
+
+    // immediate disarm on switch
+    if (armSwitch && acc.AccessoryVal <= -ARMED_THRESHOLD) {
+        lowThrottle = true;
+    }
+
+    if (forcedDisArm()) {
+        // PathPlanner forces explicit disarming due to error condition (crash, impact, fire, ...)
+        setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
+        return;
+    }
+
+    if (settings.Arming == FLIGHTMODESETTINGS_ARMING_ALWAYSDISARMED) {
+        // In this configuration we always disarm
+        setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
+        return;
+    }
+
+
+    // The throttle is not low, in case we where arming or disarming, abort
+    if (!lowThrottle) {
+        switch (armState) {
+        case ARM_STATE_DISARMING_MANUAL:
+        case ARM_STATE_DISARMING_TIMEOUT:
+            armState = ARM_STATE_ARMED;
+            break;
+        case ARM_STATE_ARMING_MANUAL:
+            armState = ARM_STATE_DISARMED;
+            break;
+        default:
+            // Nothing needs to be done in the other states
+            break;
+        }
+        return;
+    }
+
+    // The rest of these cases throttle is low
+    if (settings.Arming == FLIGHTMODESETTINGS_ARMING_ALWAYSARMED) {
+        // In this configuration, we go into armed state as soon as the throttle is low, never disarm
+        setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
+        return;
+    }
+
+    // When the configuration is not "Always armed" and no "Always disarmed",
+    // the state will not be changed when the throttle is not low
+    static portTickType armedDisarmStart;
+    float armingInputLevel = 0;
+
+    // Calc channel see assumptions7
+    switch (settings.Arming) {
+    case FLIGHTMODESETTINGS_ARMING_ROLLLEFT:
+        armingInputLevel = 1.0f * cmd.Roll;
+        break;
+    case FLIGHTMODESETTINGS_ARMING_ROLLRIGHT:
+        armingInputLevel = -1.0f * cmd.Roll;
+        break;
+    case FLIGHTMODESETTINGS_ARMING_PITCHFORWARD:
+        armingInputLevel = 1.0f * cmd.Pitch;
+        break;
+    case FLIGHTMODESETTINGS_ARMING_PITCHAFT:
+        armingInputLevel = -1.0f * cmd.Pitch;
+        break;
+    case FLIGHTMODESETTINGS_ARMING_YAWLEFT:
+        armingInputLevel = 1.0f * cmd.Yaw;
+        break;
+    case FLIGHTMODESETTINGS_ARMING_YAWRIGHT:
+        armingInputLevel = -1.0f * cmd.Yaw;
+        break;
+    case FLIGHTMODESETTINGS_ARMING_ACCESSORY0:
+    case FLIGHTMODESETTINGS_ARMING_ACCESSORY1:
+    case FLIGHTMODESETTINGS_ARMING_ACCESSORY2:
+        armingInputLevel = -1.0f * acc.AccessoryVal;
+        break;
+    }
+
+    bool manualArm    = false;
+    bool manualDisarm = false;
+
+    if (armingInputLevel <= -ARMED_THRESHOLD) {
+        manualArm = true;
+    } else if (armingInputLevel >= +ARMED_THRESHOLD) {
+        manualDisarm = true;
+    }
+
+    switch (armState) {
+    case ARM_STATE_DISARMED:
+        setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
+
+        // only allow arming if it's OK too
+        if (manualArm && okToArm()) {
+            armedDisarmStart = sysTime;
+            armState = ARM_STATE_ARMING_MANUAL;
+        }
+        break;
+
+    case ARM_STATE_ARMING_MANUAL:
+        setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING);
+
+        if (manualArm && (timeDifferenceMs(armedDisarmStart, sysTime) > settings.ArmingSequenceTime)) {
+            armState = ARM_STATE_ARMED;
+        } else if (!manualArm) {
+            armState = ARM_STATE_DISARMED;
+        }
+        break;
+
+    case ARM_STATE_ARMED:
+        // When we get here, the throttle is low,
+        // we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled
+        armedDisarmStart = sysTime;
+        armState = ARM_STATE_DISARMING_TIMEOUT;
+        setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
+        break;
+
+    case ARM_STATE_DISARMING_TIMEOUT:
+        // We get here when armed while throttle low, even when the arming timeout is not enabled
+        if ((settings.ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, sysTime) > settings.ArmedTimeout)) {
+            armState = ARM_STATE_DISARMED;
+        }
+
+        // Switch to disarming due to manual control when needed
+        if (manualDisarm) {
+            armedDisarmStart = sysTime;
+            armState = ARM_STATE_DISARMING_MANUAL;
+        }
+        break;
+
+    case ARM_STATE_DISARMING_MANUAL:
+        // arming switch disarms immediately,
+        if (manualDisarm && (timeDifferenceMs(armedDisarmStart, sysTime) > settings.DisarmingSequenceTime)) {
+            armState = ARM_STATE_DISARMED;
+        } else if (!manualDisarm) {
+            armState = ARM_STATE_ARMED;
+        }
+        break;
+    } // End Switch
+}
+
+static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time)
+{
+    return (end_time - start_time) * portTICK_RATE_MS;
+}
+
+/**
+ * @brief Determine if the aircraft is safe to arm
+ * @returns True if safe to arm, false otherwise
+ */
+static bool okToArm(void)
+{
+    // update checks
+    configuration_check();
+
+    // read alarms
+    SystemAlarmsData alarms;
+
+    SystemAlarmsGet(&alarms);
+
+    // Check each alarm
+    for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) {
+        if (cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[i] >= SYSTEMALARMS_ALARM_ERROR) { // found an alarm thats set
+            if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) {
+                continue;
+            }
+
+            return false;
+        }
+    }
+
+    uint8_t flightMode;
+    FlightStatusFlightModeGet(&flightMode);
+    switch (flightMode) {
+    case FLIGHTSTATUS_FLIGHTMODE_MANUAL:
+    case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
+    case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
+    case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
+        return true;
+
+        break;
+
+    default:
+        return false;
+
+        break;
+    }
+}
+/**
+ * @brief Determine if the aircraft is forced to disarm by an explicit alarm
+ * @returns True if safe to arm, false otherwise
+ */
+static bool forcedDisArm(void)
+{
+    // read alarms
+    SystemAlarmsAlarmData alarms;
+
+    SystemAlarmsAlarmGet(&alarms);
+
+    if (alarms.Guidance == SYSTEMALARMS_ALARM_CRITICAL) {
+        return true;
+    }
+    if (alarms.Receiver == SYSTEMALARMS_ALARM_CRITICAL) {
+        return true;
+    }
+    return false;
+}
+
+/**
+ * @brief Update the flightStatus object only if value changed.  Reduces callbacks
+ * @param[in] val The new value
+ */
+static void setArmedIfChanged(uint8_t val)
+{
+    FlightStatusData flightStatus;
+
+    FlightStatusGet(&flightStatus);
+
+    if (flightStatus.Armed != val) {
+        flightStatus.Armed = val;
+        FlightStatusSet(&flightStatus);
+    }
+}
+
+/**
+ * @}
+ * @}
+ */

flight/modules/ManualControl/inc/manualcontrol.h

@@ -6,7 +6,7 @@
  * @{
  *
  * @file       manualcontrol.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
  * @brief      ManualControl module. Handles safety R/C link and flight mode.
  *
  * @see        The GNU Public License (GPL) Version 3
@@ -30,30 +30,57 @@
 #ifndef MANUALCONTROL_H
 #define MANUALCONTROL_H
 
-#include "manualcontrolcommand.h"
+#include <openpilot.h>
+#include <flightstatus.h>
 
-typedef enum { FLIGHTMODE_UNDEFINED = 0, FLIGHTMODE_MANUAL = 1, FLIGHTMODE_STABILIZED = 2, FLIGHTMODE_GUIDANCE = 3, FLIGHTMODE_TUNING = 4 } flightmode_path;
+typedef void (*handlerFunc)(bool newinit);
 
-#define PARSE_FLIGHT_MODE(x) \
+typedef struct controlHandlerStruct {
-    ( \
+    FlightStatusControlChainData controlChain;
-        (x == FLIGHTSTATUS_FLIGHTMODE_MANUAL) ? FLIGHTMODE_MANUAL : \
+    handlerFunc handler;
-        (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) ? FLIGHTMODE_STABILIZED : \
+} controlHandler;
-        (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) ? FLIGHTMODE_STABILIZED : \
-        (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) ? FLIGHTMODE_STABILIZED : \
-        (x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) ? FLIGHTMODE_GUIDANCE : \
-        (x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO) ? FLIGHTMODE_GUIDANCE : \
-        (x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) ? FLIGHTMODE_GUIDANCE : \
-        (x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) ? FLIGHTMODE_GUIDANCE : \
-        (x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) ? FLIGHTMODE_GUIDANCE : \
-        (x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) ? FLIGHTMODE_GUIDANCE : \
-        (x == FLIGHTSTATUS_FLIGHTMODE_LAND) ? FLIGHTMODE_GUIDANCE : \
-        (x == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) ? FLIGHTMODE_TUNING : \
-        (x == FLIGHTSTATUS_FLIGHTMODE_POI) ? FLIGHTMODE_GUIDANCE : \
-        FLIGHTMODE_UNDEFINED \
-    )
 
-int32_t ManualControlInitialize();
+/**
+ * @brief Handler to interprete Command inputs in regards to arming/disarming (called in all flight modes)
+ * @input: ManualControlCommand, AccessoryDesired
+ * @output: FlightStatus.Arming
+ */
+void armHandler(bool newinit);
 
+/**
+ * @brief Handler to control Manual flightmode - input directly steers actuators
+ * @input: ManualControlCommand
+ * @output: ActuatorDesired
+ */
+void manualHandler(bool newinit);
+
+/**
+ * @brief Handler to control Stabilized flightmodes. FlightControl is governed by "Stabilization"
+ * @input: ManualControlCommand
+ * @output: StabilizationDesired
+ */
+void stabilizedHandler(bool newinit);
+
+/**
+ * @brief Handler to control deprecated flight modes controlled by AltitudeHold module
+ * @input: ManualControlCommand
+ * @output: AltitudeHoldDesired
+ */
+void altitudeHandler(bool newinit);
+
+/**
+ * @brief Handler to control Guided flightmodes. FlightControl is governed by PathFollower, control via PathDesired
+ * @input: NONE: fully automated mode -- TODO recursively call handler for advanced stick commands
+ * @output: PathDesired
+ */
+void pathFollowerHandler(bool newinit);
+
+/**
+ * @brief Handler to control Navigated flightmodes. FlightControl is governed by PathFollower, controlled indirectly via PathPlanner
+ * @input: NONE: fully automated mode -- TODO recursively call handler for advanced stick commands to affect navigation
+ * @output: NONE
+ */
+void pathPlannerHandler(bool newinit);
 
 /*
  * These are assumptions we make in the flight code about the order of settings and their consistency between
@@ -61,54 +88,46 @@ int32_t ManualControlInitialize();
  */
 #define assumptions1 \
     ( \
-        ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
+        ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
-        ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
+        ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
-        ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
+        ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
-        ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
+        ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
-        ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)      \
+        ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)      \
     )
 
 #define assumptions3 \
     ( \
-        ((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
+        ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
-        ((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
+        ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
-        ((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
+        ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
-        ((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
+        ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
-        ((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)     \
+        ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)     \
     )
 
 #define assumptions5 \
     ( \
-        ((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
+        ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
-        ((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
+        ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
-        ((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
+        ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
-        ((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
+        ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
-        ((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)     \
+        ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)     \
     )
 
 #define assumptions_flightmode \
     ( \
-        ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL == (int)FLIGHTSTATUS_FLIGHTMODE_MANUAL) && \
+        ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_MANUAL == (int)FLIGHTSTATUS_FLIGHTMODE_MANUAL) && \
-        ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) && \
+        ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED1 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) && \
-        ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) && \
+        ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED2 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) && \
-        ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) && \
+        ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED3 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) && \
-        ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD == (int)FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) && \
+        ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD == (int)FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) && \
-        ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEVARIO == (int)FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO) && \
+        ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ALTITUDEVARIO == (int)FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO) && \
-        ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int)FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) && \
+        ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int)FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) && \
-        ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int)FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) && \
+        ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int)FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) && \
-        ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_PATHPLANNER == (int)FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) && \
+        ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_PATHPLANNER == (int)FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) && \
-        ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE == (int)FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) && \
+        ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE == (int)FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) && \
-        ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_LAND == (int)FLIGHTSTATUS_FLIGHTMODE_LAND) && \
+        ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND == (int)FLIGHTSTATUS_FLIGHTMODE_LAND) && \
-        ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_AUTOTUNE == (int)FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) && \
+        ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE == (int)FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) && \
-        ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POI == (int)FLIGHTSTATUS_FLIGHTMODE_POI) \
+        ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI == (int)FLIGHTSTATUS_FLIGHTMODE_POI) \
     )
 
-#define assumptions_channelcount \
-    ( \
-        ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM) && \
-        ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELNUMBER_NUMELEM) && \
-        ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELMIN_NUMELEM) && \
-        ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELMAX_NUMELEM) && \
-        ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELNEUTRAL_NUMELEM))
-
 #endif // MANUALCONTROL_H

flight/modules/ManualControl/manualcontrol.c

@@ -11,7 +11,7 @@
  * AttitudeDesired object (stabilized mode)
  *
  * @file       manualcontrol.c
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
  * @brief      ManualControl module. Handles safety R/C link and flight mode.
  *
  * @see        The GNU Public License (GPL) Version 3
@@ -33,29 +33,16 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 
-#include <openpilot.h>
+#include "inc/manualcontrol.h"
-#include <pios_struct_helper.h>
+#include <sanitycheck.h>
-#include "accessorydesired.h"
+#include <manualcontrolsettings.h>
-#include "actuatordesired.h"
+#include <manualcontrolcommand.h>
-#include "altitudeholddesired.h"
+#include <flightmodesettings.h>
-#include "flighttelemetrystats.h"
+#include <flightstatus.h>
-#include "flightstatus.h"
+#include <systemsettings.h>
-#include "sanitycheck.h"
+#include <stabilizationdesired.h>
-#include "manualcontrol.h"
+#include <callbackinfo.h>
-#include "manualcontrolsettings.h"
-#include "manualcontrolcommand.h"
-#include "positionstate.h"
-#include "pathdesired.h"
-#include "stabilizationsettings.h"
-#include "stabilizationdesired.h"
-#include "receiveractivity.h"
-#include "systemsettings.h"
-#include <altitudeholdsettings.h>
-#include <taskinfo.h>
 
-#if defined(PIOS_INCLUDE_USB_RCTX)
-#include "pios_usb_rctx.h"
-#endif /* PIOS_INCLUDE_USB_RCTX */
 
 // Private constants
 #if defined(PIOS_MANUAL_STACK_SIZE)
@@ -64,79 +51,101 @@
 #define STACK_SIZE_BYTES  1152
 #endif
 
-#define TASK_PRIORITY     (tskIDLE_PRIORITY + 4)
+#define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR
-#define UPDATE_PERIOD_MS  20
+#define CBTASK_PRIORITY   CALLBACK_TASK_FLIGHTCONTROL
-#define THROTTLE_FAILSAFE -0.1f
-#define ARMED_THRESHOLD   0.50f
-// safe band to allow a bit of calibration error or trim offset (in microseconds)
-#define CONNECTION_OFFSET 250
 
-// Private types
-typedef enum {
-    ARM_STATE_DISARMED,
-    ARM_STATE_ARMING_MANUAL,
-    ARM_STATE_ARMED,
-    ARM_STATE_DISARMING_MANUAL,
-    ARM_STATE_DISARMING_TIMEOUT
-} ArmState_t;
 
+// defined handlers
+
+static const controlHandler handler_MANUAL = {
+    .controlChain      = {
+        .Stabilization = false,
+        .PathFollower  = false,
+        .PathPlanner   = false,
+    },
+    .handler           = &manualHandler,
+};
+static const controlHandler handler_STABILIZED = {
+    .controlChain      = {
+        .Stabilization = true,
+        .PathFollower  = false,
+        .PathPlanner   = false,
+    },
+    .handler           = &stabilizedHandler,
+};
+
+
+static const controlHandler handler_AUTOTUNE = {
+    .controlChain      = {
+        .Stabilization = false,
+        .PathFollower  = false,
+        .PathPlanner   = false,
+    },
+    .handler           = NULL,
+};
+
+#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
+// TODO: move the altitude handling into stabi
+static const controlHandler handler_ALTITUDE = {
+    .controlChain      = {
+        .Stabilization = true,
+        .PathFollower  = false,
+        .PathPlanner   = false,
+    },
+    .handler           = &altitudeHandler,
+};
+
+static const controlHandler handler_PATHFOLLOWER = {
+    .controlChain      = {
+        .Stabilization = true,
+        .PathFollower  = true,
+        .PathPlanner   = false,
+    },
+    .handler           = &pathFollowerHandler,
+};
+
+static const controlHandler handler_PATHPLANNER = {
+    .controlChain      = {
+        .Stabilization = true,
+        .PathFollower  = true,
+        .PathPlanner   = true,
+    },
+    .handler           = &pathPlannerHandler,
+};
+
+#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
 // Private variables
-static xTaskHandle taskHandle;
+static DelayedCallbackInfo *callbackHandle;
-static ArmState_t armState;
-static portTickType lastSysTime;
-
-#ifdef USE_INPUT_LPF
-static portTickType lastSysTimeLPF;
-static float inputFiltered[MANUALCONTROLSETTINGS_RESPONSETIME_NUMELEM];
-#endif
 
 // Private functions
-static void updateActuatorDesired(ManualControlCommandData *cmd);
-static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualControlSettingsData *settings);
-static void updateLandDesired(ManualControlCommandData *cmd, bool changed);
-static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed);
-static void updatePathDesired(ManualControlCommandData *cmd, bool changed, bool home);
-static void processFlightMode(ManualControlSettingsData *settings, float flightMode);
-static void processArm(ManualControlCommandData *cmd, ManualControlSettingsData *settings, int8_t armSwitch);
-static void setArmedIfChanged(uint8_t val);
 static void configurationUpdatedCb(UAVObjEvent *ev);
+static void commandUpdatedCb(UAVObjEvent *ev);
 
-static void manualControlTask(void *parameters);
+static void manualControlTask(void);
-static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral);
-static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time);
-static bool okToArm(void);
-static bool validInputRange(int16_t min, int16_t max, uint16_t value);
-static void applyDeadband(float *value, float deadband);
 
-#ifdef USE_INPUT_LPF
+#define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions_flightmode)
-static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT);
-#endif
-
-#define RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP 12
-#define RCVR_ACTIVITY_MONITOR_MIN_RANGE          10
-struct rcvr_activity_fsm {
-    ManualControlSettingsChannelGroupsOptions group;
-    uint16_t prev[RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP];
-    uint8_t sample_count;
-};
-static struct rcvr_activity_fsm activity_fsm;
-
-static void resetRcvrActivity(struct rcvr_activity_fsm *fsm);
-static bool updateRcvrActivity(struct rcvr_activity_fsm *fsm);
-
-#define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions_flightmode && assumptions_channelcount)
 
 /**
  * Module starting
  */
 int32_t ManualControlStart()
 {
+    // Run this initially to make sure the configuration is checked
+    configuration_check();
+
+    // Whenever the configuration changes, make sure it is safe to fly
+    SystemSettingsConnectCallback(configurationUpdatedCb);
+    ManualControlSettingsConnectCallback(configurationUpdatedCb);
+    ManualControlCommandConnectCallback(commandUpdatedCb);
+
+    // clear alarms
+    AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);
+
+    // Make sure unarmed on power up
+    armHandler(true);
+
     // Start main task
-    xTaskCreate(manualControlTask, (signed char *)"ManualControl", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
+    PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
-    PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_MANUALCONTROL, taskHandle);
-#ifdef PIOS_INCLUDE_WDG
-    PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL);
-#endif
 
     return 0;
 }
@@ -147,16 +156,15 @@ int32_t ManualControlStart()
 int32_t ManualControlInitialize()
 {
     /* Check the assumptions about uavobject enum's are correct */
-    if (!assumptions) {
+    PIOS_STATIC_ASSERT(assumptions);
-        return -1;
-    }
 
-    AccessoryDesiredInitialize();
     ManualControlCommandInitialize();
     FlightStatusInitialize();
-    StabilizationDesiredInitialize();
-    ReceiverActivityInitialize();
     ManualControlSettingsInitialize();
+    FlightModeSettingsInitialize();
+    SystemSettingsInitialize();
+
+    callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&manualControlTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_MANUALCONTROL, STACK_SIZE_BYTES);
 
     return 0;
 }
@@ -165,1105 +173,77 @@ MODULE_INITCALL(ManualControlInitialize, ManualControlStart);
 /**
  * Module task
  */
-static void manualControlTask(__attribute__((unused)) void *parameters)
+static void manualControlTask(void)
 {
-    ManualControlSettingsData settings;
+    // Process Arming
+    armHandler(false);
+
+    // Process flight mode
+    FlightStatusData flightStatus;
+
+    FlightStatusGet(&flightStatus);
     ManualControlCommandData cmd;
-    FlightStatusData flightStatus;
-    float flightMode = 0;
-
-    uint8_t disconnected_count = 0;
-    uint8_t connected_count    = 0;
-
-    // For now manual instantiate extra instances of Accessory Desired.  In future should be done dynamically
-    // this includes not even registering it if not used
-    AccessoryDesiredCreateInstance();
-    AccessoryDesiredCreateInstance();
-
-    // Run this initially to make sure the configuration is checked
-    configuration_check();
-
-    // Whenever the configuration changes, make sure it is safe to fly
-    SystemSettingsConnectCallback(configurationUpdatedCb);
-    ManualControlSettingsConnectCallback(configurationUpdatedCb);
-
-    // Whenever the configuration changes, make sure it is safe to fly
-
-    // Make sure unarmed on power up
     ManualControlCommandGet(&cmd);
-    FlightStatusGet(&flightStatus);
-    flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED;
-    armState = ARM_STATE_DISARMED;
 
-    /* Initialize the RcvrActivty FSM */
+    FlightModeSettingsData modeSettings;
-    portTickType lastActivityTime = xTaskGetTickCount();
+    FlightModeSettingsGet(&modeSettings);
-    resetRcvrActivity(&activity_fsm);
 
-    // Main task loop
+    uint8_t position = cmd.FlightModeSwitchPosition;
-    lastSysTime = xTaskGetTickCount();
+    uint8_t newMode  = flightStatus.FlightMode;
-
+    if (position < FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM) {
-    float scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM] = { 0 };
+        newMode = modeSettings.FlightModePosition[position];
-
-    while (1) {
-        // Wait until next update
-        vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS);
-#ifdef PIOS_INCLUDE_WDG
-        PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL);
-#endif
-
-        // Read settings
-        ManualControlSettingsGet(&settings);
-
-        /* Update channel activity monitor */
-        if (flightStatus.Armed == ARM_STATE_DISARMED) {
-            if (updateRcvrActivity(&activity_fsm)) {
-                /* Reset the aging timer because activity was detected */
-                lastActivityTime = lastSysTime;
-            }
-        }
-        if (timeDifferenceMs(lastActivityTime, lastSysTime) > 5000) {
-            resetRcvrActivity(&activity_fsm);
-            lastActivityTime = lastSysTime;
-        }
-
-        if (ManualControlCommandReadOnly()) {
-            FlightTelemetryStatsData flightTelemStats;
-            FlightTelemetryStatsGet(&flightTelemStats);
-            if (flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
-                /* trying to fly via GCS and lost connection.  fall back to transmitter */
-                UAVObjMetadata metadata;
-                ManualControlCommandGetMetadata(&metadata);
-                UAVObjSetAccess(&metadata, ACCESS_READWRITE);
-                ManualControlCommandSetMetadata(&metadata);
-            }
-        }
-
-        if (!ManualControlCommandReadOnly()) {
-            bool valid_input_detected = true;
-
-            // Read channel values in us
-            for (uint8_t n = 0; n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) {
-                extern uint32_t pios_rcvr_group_map[];
-
-                if (cast_struct_to_array(settings.ChannelGroups, settings.ChannelGroups.Roll)[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
-                    cmd.Channel[n] = PIOS_RCVR_INVALID;
-                } else {
-                    cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[
-                                                        cast_struct_to_array(settings.ChannelGroups, settings.ChannelGroups.Pitch)[n]],
-                                                    cast_struct_to_array(settings.ChannelNumber, settings.ChannelNumber.Pitch)[n]);
-                }
-
-                // If a channel has timed out this is not valid data and we shouldn't update anything
-                // until we decide to go to failsafe
-                if (cmd.Channel[n] == (uint16_t)PIOS_RCVR_TIMEOUT) {
-                    valid_input_detected = false;
-                } else {
-                    scaledChannel[n] = scaleChannel(cmd.Channel[n],
-                                                    cast_struct_to_array(settings.ChannelMax, settings.ChannelMax.Pitch)[n],
-                                                    cast_struct_to_array(settings.ChannelMin, settings.ChannelMin.Pitch)[n],
-                                                    cast_struct_to_array(settings.ChannelNeutral, settings.ChannelNeutral.Pitch)[n]);
-                }
-            }
-
-            // Check settings, if error raise alarm
-            if (settings.ChannelGroups.Roll >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
-                || settings.ChannelGroups.Pitch >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
-                || 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_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_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 the FlightModeNumber is valid
-                settings.FlightModeNumber < 1 || settings.FlightModeNumber > MANUALCONTROLSETTINGS_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.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))) {
-                AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
-                cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
-                ManualControlCommandSet(&cmd);
-
-                // Need to do this here since we don't process armed status.  Since this shouldn't happen in flight (changed config)
-                // immediately disarm
-                setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
-
-                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])
-                                    && validInputRange(settings.ChannelMin.Pitch,
-                                                       settings.ChannelMax.Pitch, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]);
-
-            // Implement hysteresis loop on connection status
-            if (valid_input_detected && (++connected_count > 10)) {
-                cmd.Connected      = MANUALCONTROLCOMMAND_CONNECTED_TRUE;
-                connected_count    = 0;
-                disconnected_count = 0;
-            } else if (!valid_input_detected && (++disconnected_count > 10)) {
-                cmd.Connected      = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
-                connected_count    = 0;
-                disconnected_count = 0;
-            }
-
-            int8_t armSwitch = 0;
-            if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) {
-                cmd.Throttle   = -1;      // Shut down engine with no control
-                cmd.Roll       = 0;
-                cmd.Yaw = 0;
-                cmd.Pitch      = 0;
-                cmd.Collective = 0;
-                if (settings.FailsafeBehavior != MANUALCONTROLSETTINGS_FAILSAFEBEHAVIOR_NONE) {
-                    FlightStatusGet(&flightStatus);
-
-                    flightStatus.FlightMode = settings.FlightModePosition[settings.FailsafeBehavior - 1];
-                    FlightStatusSet(&flightStatus);
-                }
-                AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
-
-                AccessoryDesiredData accessory;
-                // Set Accessory 0
-                if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
-                    accessory.AccessoryVal = 0;
-                    if (AccessoryDesiredInstSet(0, &accessory) != 0) {
-                        AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
-                    }
-                }
-                // Set Accessory 1
-                if (settings.ChannelGroups.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
-                    accessory.AccessoryVal = 0;
-                    if (AccessoryDesiredInstSet(1, &accessory) != 0) {
-                        AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
-                    }
-                }
-                // Set Accessory 2
-                if (settings.ChannelGroups.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
-                    accessory.AccessoryVal = 0;
-                    if (AccessoryDesiredInstSet(2, &accessory) != 0) {
-                        AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
-                    }
-                }
-            } else if (valid_input_detected) {
-                AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);
-
-                // Scale channels to -1 -> +1 range
-                cmd.Roll     = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL];
-                cmd.Pitch    = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH];
-                cmd.Yaw      = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW];
-                cmd.Throttle = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE];
-                flightMode   = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE];
-
-                // Apply deadband for Roll/Pitch/Yaw stick inputs
-                if (settings.Deadband > 0.0f) {
-                    applyDeadband(&cmd.Roll, settings.Deadband);
-                    applyDeadband(&cmd.Pitch, settings.Deadband);
-                    applyDeadband(&cmd.Yaw, settings.Deadband);
-                }
-#ifdef USE_INPUT_LPF
-                // Apply Low Pass Filter to input channels, time delta between calls in ms
-                portTickType thisSysTime = xTaskGetTickCount();
-                float dT = (thisSysTime > lastSysTimeLPF) ?
-                           (float)((thisSysTime - lastSysTimeLPF) * portTICK_RATE_MS) :
-                           (float)UPDATE_PERIOD_MS;
-                lastSysTimeLPF = thisSysTime;
-
-                applyLPF(&cmd.Roll, MANUALCONTROLSETTINGS_RESPONSETIME_ROLL, &settings, dT);
-                applyLPF(&cmd.Pitch, MANUALCONTROLSETTINGS_RESPONSETIME_PITCH, &settings, dT);
-                applyLPF(&cmd.Yaw, MANUALCONTROLSETTINGS_RESPONSETIME_YAW, &settings, dT);
-#endif // USE_INPUT_LPF
-                if (cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_INVALID
-                    && cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_NODRIVER
-                    && cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_TIMEOUT) {
-                    cmd.Collective = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE];
-                }
-
-                AccessoryDesiredData accessory;
-                // Set Accessory 0
-                if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
-                    accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0];
-#ifdef USE_INPUT_LPF
-                    applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT);
-#endif
-                    if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ACCESSORY0) {
-                        if (accessory.AccessoryVal > ARMED_THRESHOLD) {
-                            armSwitch = 1;
-                        } else if (accessory.AccessoryVal < -ARMED_THRESHOLD) {
-                            armSwitch = -1;
-                        }
-                    }
-                    if (AccessoryDesiredInstSet(0, &accessory) != 0) {
-                        AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
-                    }
-                }
-                // Set Accessory 1
-                if (settings.ChannelGroups.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
-                    accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1];
-#ifdef USE_INPUT_LPF
-                    applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT);
-#endif
-                    if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ACCESSORY1) {
-                        if (accessory.AccessoryVal > ARMED_THRESHOLD) {
-                            armSwitch = 1;
-                        } else if (accessory.AccessoryVal < -ARMED_THRESHOLD) {
-                            armSwitch = -1;
-                        }
-                    }
-                    if (AccessoryDesiredInstSet(1, &accessory) != 0) {
-                        AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
-                    }
-                }
-                // Set Accessory 2
-                if (settings.ChannelGroups.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
-                    accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2];
-#ifdef USE_INPUT_LPF
-                    applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT);
-#endif
-                    if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ACCESSORY2) {
-                        if (accessory.AccessoryVal > ARMED_THRESHOLD) {
-                            armSwitch = 1;
-                        } else if (accessory.AccessoryVal < -ARMED_THRESHOLD) {
-                            armSwitch = -1;
-                        }
-                    }
-
-                    if (AccessoryDesiredInstSet(2, &accessory) != 0) {
-                        AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
-                    }
-                }
-
-                processFlightMode(&settings, flightMode);
-            }
-
-            // Process arming outside conditional so system will disarm when disconnected
-            processArm(&cmd, &settings, armSwitch);
-
-            // Update cmd object
-            ManualControlCommandSet(&cmd);
-#if defined(PIOS_INCLUDE_USB_RCTX)
-            if (pios_usb_rctx_id) {
-                PIOS_USB_RCTX_Update(pios_usb_rctx_id,
-                                     cmd.Channel,
-                                     cast_struct_to_array(settings.ChannelMin, settings.ChannelMin.Roll),
-                                     cast_struct_to_array(settings.ChannelMax, settings.ChannelMax.Roll),
-                                     NELEMENTS(cmd.Channel));
-            }
-#endif /* PIOS_INCLUDE_USB_RCTX */
-        } else {
-            ManualControlCommandGet(&cmd); /* Under GCS control */
-        }
-
-        FlightStatusGet(&flightStatus);
-
-        // Depending on the mode update the Stabilization or Actuator objects
-        static uint8_t lastFlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL;
-        switch (PARSE_FLIGHT_MODE(flightStatus.FlightMode)) {
-        case FLIGHTMODE_UNDEFINED:
-            // This reflects a bug in the code architecture!
-            AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
-            break;
-        case FLIGHTMODE_MANUAL:
-            updateActuatorDesired(&cmd);
-            break;
-        case FLIGHTMODE_STABILIZED:
-            updateStabilizationDesired(&cmd, &settings);
-            break;
-        case FLIGHTMODE_TUNING:
-            // Tuning takes settings directly from manualcontrolcommand.  No need to
-            // call anything else.  This just avoids errors.
-            break;
-        case FLIGHTMODE_GUIDANCE:
-            switch (flightStatus.FlightMode) {
-            case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
-            case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO:
-                altitudeHoldDesired(&cmd, lastFlightMode != flightStatus.FlightMode);
-                break;
-            case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
-            case FLIGHTSTATUS_FLIGHTMODE_POI:
-                updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, false);
-                break;
-            case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
-                updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, true);
-                break;
-            case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
-                // No need to call anything.  This just avoids errors.
-                break;
-            case FLIGHTSTATUS_FLIGHTMODE_LAND:
-                updateLandDesired(&cmd, lastFlightMode != flightStatus.FlightMode);
-                break;
-            default:
-                AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
-            }
-            break;
-        }
-        lastFlightMode = flightStatus.FlightMode;
-    }
-}
-
-static void resetRcvrActivity(struct rcvr_activity_fsm *fsm)
-{
-    ReceiverActivityData data;
-    bool updated = false;
-
-    /* Clear all channel activity flags */
-    ReceiverActivityGet(&data);
-    if (data.ActiveGroup != RECEIVERACTIVITY_ACTIVEGROUP_NONE && data.ActiveChannel != 255) {
-        data.ActiveGroup   = RECEIVERACTIVITY_ACTIVEGROUP_NONE;
-        data.ActiveChannel = 255;
-        updated = true;
-    }
-    if (updated) {
-        ReceiverActivitySet(&data);
     }
 
-    /* Reset the FSM state */
+    // Depending on the mode update the Stabilization or Actuator objects
-    fsm->group = 0;
+    const controlHandler *handler = &handler_MANUAL;
-    fsm->sample_count = 0;
+    switch (newMode) {
-}
-
-static void updateRcvrActivitySample(uint32_t rcvr_id, uint16_t samples[], uint8_t max_channels)
-{
-    for (uint8_t channel = 1; channel <= max_channels; channel++) {
-        // Subtract 1 because channels are 1 indexed
-        samples[channel - 1] = PIOS_RCVR_Read(rcvr_id, channel);
-    }
-}
-
-static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm *fsm)
-{
-    bool activity_updated = false;
-
-    /* Compare the current value to the previous sampled value */
-    for (uint8_t channel = 1; channel <= RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP; channel++) {
-        uint16_t delta;
-        uint16_t prev = fsm->prev[channel - 1]; // Subtract 1 because channels are 1 indexed
-        uint16_t curr = PIOS_RCVR_Read(rcvr_id, channel);
-        if (curr > prev) {
-            delta = curr - prev;
-        } else {
-            delta = prev - curr;
-        }
-
-        if (delta > RCVR_ACTIVITY_MONITOR_MIN_RANGE) {
-            /* Mark this channel as active */
-            ReceiverActivityActiveGroupOptions group;
-
-            /* Don't assume manualcontrolsettings and receiveractivity are in the same order. */
-            switch (fsm->group) {
-            case MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM:
-                group = RECEIVERACTIVITY_ACTIVEGROUP_PWM;
-                break;
-            case MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM:
-                group = RECEIVERACTIVITY_ACTIVEGROUP_PPM;
-                break;
-            case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT:
-                group = RECEIVERACTIVITY_ACTIVEGROUP_DSMMAINPORT;
-                break;
-            case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT:
-                group = RECEIVERACTIVITY_ACTIVEGROUP_DSMFLEXIPORT;
-                break;
-            case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS:
-                group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS;
-                break;
-            case MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS:
-                group = RECEIVERACTIVITY_ACTIVEGROUP_GCS;
-                break;
-            case MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK:
-                group = RECEIVERACTIVITY_ACTIVEGROUP_OPLINK;
-                break;
-            default:
-                PIOS_Assert(0);
-                break;
-            }
-
-            ReceiverActivityActiveGroupSet((uint8_t *)&group);
-            ReceiverActivityActiveChannelSet(&channel);
-            activity_updated = true;
-        }
-    }
-    return activity_updated;
-}
-
-static bool updateRcvrActivity(struct rcvr_activity_fsm *fsm)
-{
-    bool activity_updated = false;
-
-    if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
-        /* We're out of range, reset things */
-        resetRcvrActivity(fsm);
-    }
-
-    extern uint32_t pios_rcvr_group_map[];
-    if (!pios_rcvr_group_map[fsm->group]) {
-        /* Unbound group, skip it */
-        goto group_completed;
-    }
-
-    if (fsm->sample_count == 0) {
-        /* Take a sample of each channel in this group */
-        updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], fsm->prev, NELEMENTS(fsm->prev));
-        fsm->sample_count++;
-        return false;
-    }
-
-    /* Compare with previous sample */
-    activity_updated = updateRcvrActivityCompare(pios_rcvr_group_map[fsm->group], fsm);
-
-group_completed:
-    /* Reset the sample counter */
-    fsm->sample_count = 0;
-
-    /* Find the next active group, but limit search so we can't loop forever here */
-    for (uint8_t i = 0; i < MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE; i++) {
-        /* Move to the next group */
-        fsm->group++;
-        if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
-            /* Wrap back to the first group */
-            fsm->group = 0;
-        }
-        if (pios_rcvr_group_map[fsm->group]) {
-            /*
-             * Found an active group, take a sample here to avoid an
-             * extra 20ms delay in the main thread so we can speed up
-             * this algorithm.
-             */
-            updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], fsm->prev, NELEMENTS(fsm->prev));
-            fsm->sample_count++;
-            break;
-        }
-    }
-
-    return activity_updated;
-}
-
-static void updateActuatorDesired(ManualControlCommandData *cmd)
-{
-    ActuatorDesiredData actuator;
-
-    ActuatorDesiredGet(&actuator);
-    actuator.Roll     = cmd->Roll;
-    actuator.Pitch    = cmd->Pitch;
-    actuator.Yaw      = cmd->Yaw;
-    actuator.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
-    ActuatorDesiredSet(&actuator);
-}
-
-static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualControlSettingsData *settings)
-{
-    StabilizationDesiredData stabilization;
-
-    StabilizationDesiredGet(&stabilization);
-
-    StabilizationSettingsData stabSettings;
-    StabilizationSettingsGet(&stabSettings);
-
-    uint8_t *stab_settings;
-    FlightStatusData flightStatus;
-    FlightStatusGet(&flightStatus);
-    switch (flightStatus.FlightMode) {
-    case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
-        stab_settings = cast_struct_to_array(settings->Stabilization1Settings, settings->Stabilization1Settings.Roll);
-        break;
-    case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
-        stab_settings = cast_struct_to_array(settings->Stabilization2Settings, settings->Stabilization2Settings.Roll);
-        break;
-    case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
-        stab_settings = cast_struct_to_array(settings->Stabilization3Settings, settings->Stabilization3Settings.Roll);
-        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);
-        return;
-    }
-
-    stabilization.Roll =
-        (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll :
-        (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate.Roll :
-        (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Roll * stabSettings.ManualRate.Roll :
-        (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax :
-        (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate.Roll :
-        (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll :
-        (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Roll :
-        (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Roll * stabSettings.ManualRate.Roll :
-        (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax :
-        0; // this is an invalid mode
-
-    stabilization.Pitch =
-        (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch :
-        (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate.Pitch :
-        (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Pitch * stabSettings.ManualRate.Pitch :
-        (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
-        (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.ManualRate.Pitch :
-        (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch :
-        (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Pitch :
-        (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Pitch * stabSettings.ManualRate.Pitch :
-        (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
-        0; // this is an invalid mode
-
-    // TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order
-    stabilization.StabilizationMode.Roll  = stab_settings[0];
-    stabilization.StabilizationMode.Pitch = stab_settings[1];
-    // Other axes (yaw) cannot be Rattitude, so use Rate
-    // Should really do this for Attitude mode as well?
-    if (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) {
-        stabilization.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
-        stabilization.Yaw = cmd->Yaw * stabSettings.ManualRate.Yaw;
-    } else {
-        stabilization.StabilizationMode.Yaw = stab_settings[2];
-        stabilization.Yaw =
-            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw :
-            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw :
-            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Yaw * stabSettings.ManualRate.Yaw :
-            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax :
-            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate.Yaw :
-            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw :
-            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Yaw :
-            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw :
-            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax :
-            0; // this is an invalid mode
-    }
-
-    stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
-    StabilizationDesiredSet(&stabilization);
-}
-
-#if defined(REVOLUTION)
-// TODO: Need compile flag to exclude this from copter control
-/**
- * @brief Update the position desired to current location when
- * enabled and allow the waypoint to be moved by transmitter
- */
-static void updatePathDesired(__attribute__((unused)) ManualControlCommandData *cmd, bool changed, bool home)
-{
-    /*
-       static portTickType lastSysTime;
-       portTickType thisSysTime = xTaskGetTickCount();
-       dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f);
-       lastSysTime = thisSysTime;
-     */
-
-    if (home && changed) {
-        // Simple Return To Base mode - keep altitude the same, fly to home position
-        PositionStateData positionState;
-        PositionStateGet(&positionState);
-        ManualControlSettingsData settings;
-        ManualControlSettingsGet(&settings);
-
-        PathDesiredData pathDesired;
-        PathDesiredGet(&pathDesired);
-        pathDesired.Start.North      = 0;
-        pathDesired.Start.East       = 0;
-        pathDesired.Start.Down       = positionState.Down - settings.ReturnToHomeAltitudeOffset;
-        pathDesired.End.North        = 0;
-        pathDesired.End.East         = 0;
-        pathDesired.End.Down         = positionState.Down - settings.ReturnToHomeAltitudeOffset;
-        pathDesired.StartingVelocity = 1;
-        pathDesired.EndingVelocity   = 0;
-        pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
-        PathDesiredSet(&pathDesired);
-    } else if (changed) {
-        // After not being in this mode for a while init at current height
-        PositionStateData positionState;
-        PositionStateGet(&positionState);
-
-        PathDesiredData pathDesired;
-        PathDesiredGet(&pathDesired);
-        pathDesired.Start.North      = positionState.North;
-        pathDesired.Start.East       = positionState.East;
-        pathDesired.Start.Down       = positionState.Down;
-        pathDesired.End.North        = positionState.North;
-        pathDesired.End.East         = positionState.East;
-        pathDesired.End.Down         = positionState.Down;
-        pathDesired.StartingVelocity = 1;
-        pathDesired.EndingVelocity   = 0;
-        pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
-        PathDesiredSet(&pathDesired);
-        /* Disable this section, until such time as proper discussion can be had about how to implement it for all types of crafts.
-           } else {
-           PathDesiredData pathDesired;
-           PathDesiredGet(&pathDesired);
-           pathDesired.End[PATHDESIRED_END_NORTH] += dT * -cmd->Pitch;
-           pathDesired.End[PATHDESIRED_END_EAST] += dT * cmd->Roll;
-           pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
-           PathDesiredSet(&pathDesired);
-         */
-    }
-}
-
-static void updateLandDesired(__attribute__((unused)) ManualControlCommandData *cmd, bool changed)
-{
-    /*
-       static portTickType lastSysTime;
-       portTickType thisSysTime;
-       float dT;
-
-       thisSysTime = xTaskGetTickCount();
-        dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f);
-       lastSysTime = thisSysTime;
-     */
-
-    PositionStateData positionState;
-
-    PositionStateGet(&positionState);
-
-    PathDesiredData pathDesired;
-    PathDesiredGet(&pathDesired);
-    if (changed) {
-        // After not being in this mode for a while init at current height
-        pathDesired.Start.North      = positionState.North;
-        pathDesired.Start.East       = positionState.East;
-        pathDesired.Start.Down       = positionState.Down;
-        pathDesired.End.North        = positionState.North;
-        pathDesired.End.East         = positionState.East;
-        pathDesired.End.Down         = positionState.Down;
-        pathDesired.StartingVelocity = 1;
-        pathDesired.EndingVelocity   = 0;
-        pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
-    }
-    pathDesired.End.Down = positionState.Down + 5;
-    PathDesiredSet(&pathDesired);
-}
-
-/**
- * @brief Update the altitude desired to current altitude when
- * enabled and enable altitude mode for stabilization
- * @todo: Need compile flag to exclude this from copter control
- */
-static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed)
-{
-    const float DEADBAND      = 0.25f;
-    const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2;
-    const float DEADBAND_LOW  = 1.0f / 2 - DEADBAND / 2;
-
-    // Stop updating AltitudeHoldDesired triggering a failsafe condition.
-    if (cmd->Throttle < 0) {
-        return;
-    }
-
-    // this is the max speed in m/s at the extents of throttle
-    uint8_t throttleRate;
-    uint8_t throttleExp;
-
-    static uint8_t flightMode;
-    static portTickType lastSysTimeAH;
-    static bool zeroed = false;
-    portTickType thisSysTime;
-    float dT;
-
-    FlightStatusFlightModeGet(&flightMode);
-
-    AltitudeHoldDesiredData altitudeHoldDesiredData;
-    AltitudeHoldDesiredGet(&altitudeHoldDesiredData);
-
-    if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) {
-        throttleExp  = 128;
-        throttleRate = 0;
-    } else {
-        AltitudeHoldSettingsThrottleExpGet(&throttleExp);
-        AltitudeHoldSettingsThrottleRateGet(&throttleRate);
-    }
-
-    StabilizationSettingsData stabSettings;
-    StabilizationSettingsGet(&stabSettings);
-
-    thisSysTime   = xTaskGetTickCount();
-    dT = ((thisSysTime == lastSysTimeAH) ? 0.001f : (thisSysTime - lastSysTimeAH) * portTICK_RATE_MS * 0.001f);
-    lastSysTimeAH = thisSysTime;
-
-    altitudeHoldDesiredData.Roll  = cmd->Roll * stabSettings.RollMax;
-    altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax;
-    altitudeHoldDesiredData.Yaw   = cmd->Yaw * stabSettings.ManualRate.Yaw;
-
-    float currentDown;
-    PositionStateDownGet(&currentDown);
-    if (changed) {
-        // After not being in this mode for a while init at current height
-        altitudeHoldDesiredData.Altitude = 0;
-        zeroed = false;
-    } else if (cmd->Throttle > DEADBAND_HIGH && zeroed) {
-        // 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 + x*(256−k)) / 256
-        altitudeHoldDesiredData.Altitude += (throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (256 - throttleExp)) / 256 * throttleRate * dT;
-    } else if (cmd->Throttle < DEADBAND_LOW && zeroed) {
-        altitudeHoldDesiredData.Altitude -= (throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (256 - throttleExp)) / 256 * throttleRate * dT;
-    } else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH && (throttleRate != 0)) {
-        // Require the stick to enter the dead band before they can move height
-        // Vario is not "engaged" when throttleRate == 0
-        zeroed = true;
-    }
-
-    AltitudeHoldDesiredSet(&altitudeHoldDesiredData);
-}
-#else /* if defined(REVOLUTION) */
-
-// TODO: These functions should never be accessible on CC.  Any configuration that
-// could allow them to be called should already throw an error to prevent this happening
-// in flight
-static void updatePathDesired(__attribute__((unused)) ManualControlCommandData *cmd,
-                              __attribute__((unused)) bool changed,
-                              __attribute__((unused)) bool home)
-{
-    AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
-}
-
-static void updateLandDesired(__attribute__((unused)) ManualControlCommandData *cmd,
-                              __attribute__((unused)) bool changed)
-{
-    AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
-}
-
-static void altitudeHoldDesired(__attribute__((unused)) ManualControlCommandData *cmd,
-                                __attribute__((unused)) bool changed)
-{
-    AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
-}
-#endif /* if defined(REVOLUTION) */
-/**
- * Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range.
- */
-static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral)
-{
-    float valueScaled;
-
-    // Scale
-    if ((max > min && value >= neutral) || (min > max && value <= neutral)) {
-        if (max != neutral) {
-            valueScaled = (float)(value - neutral) / (float)(max - neutral);
-        } else {
-            valueScaled = 0;
-        }
-    } else {
-        if (min != neutral) {
-            valueScaled = (float)(value - neutral) / (float)(neutral - min);
-        } else {
-            valueScaled = 0;
-        }
-    }
-
-    // Bound
-    if (valueScaled > 1.0f) {
-        valueScaled = 1.0f;
-    } else if (valueScaled < -1.0f) {
-        valueScaled = -1.0f;
-    }
-
-    return valueScaled;
-}
-
-static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time)
-{
-    return (end_time - start_time) * portTICK_RATE_MS;
-}
-
-/**
- * @brief Determine if the aircraft is safe to arm
- * @returns True if safe to arm, false otherwise
- */
-static bool okToArm(void)
-{
-    // update checks
-    configuration_check();
-
-    // read alarms
-    SystemAlarmsData alarms;
-
-    SystemAlarmsGet(&alarms);
-
-    // Check each alarm
-    for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) {
-        if (cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[i] >= SYSTEMALARMS_ALARM_ERROR) { // found an alarm thats set
-            if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) {
-                continue;
-            }
-
-            return false;
-        }
-    }
-
-    uint8_t flightMode;
-    FlightStatusFlightModeGet(&flightMode);
-    switch (flightMode) {
     case FLIGHTSTATUS_FLIGHTMODE_MANUAL:
+        handler = &handler_MANUAL;
+        break;
     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
-        return true;
+        handler = &handler_STABILIZED;
-
-    default:
-        return false;
-    }
-}
-/**
- * @brief Determine if the aircraft is forced to disarm by an explicit alarm
- * @returns True if safe to arm, false otherwise
- */
-static bool forcedDisArm(void)
-{
-    // read alarms
-    SystemAlarmsData alarms;
-
-    SystemAlarmsGet(&alarms);
-
-    if (alarms.Alarm.Guidance == SYSTEMALARMS_ALARM_CRITICAL) {
-        return true;
-    }
-    return false;
-}
-
-/**
- * @brief Update the flightStatus object only if value changed.  Reduces callbacks
- * @param[in] val The new value
- */
-static void setArmedIfChanged(uint8_t val)
-{
-    FlightStatusData flightStatus;
-
-    FlightStatusGet(&flightStatus);
-
-    if (flightStatus.Armed != val) {
-        flightStatus.Armed = val;
-        FlightStatusSet(&flightStatus);
-    }
-}
-
-/**
- * @brief Process the inputs and determine whether to arm or not
- * @param[out] cmd The structure to set the armed in
- * @param[in] settings Settings indicating the necessary position
- */
-static void processArm(ManualControlCommandData *cmd, ManualControlSettingsData *settings, int8_t armSwitch)
-{
-    bool lowThrottle = cmd->Throttle <= 0;
-
-    /**
-     * do NOT check throttle if disarming via switch, must be instant
-     */
-    switch (settings->Arming) {
-    case MANUALCONTROLSETTINGS_ARMING_ACCESSORY0:
-    case MANUALCONTROLSETTINGS_ARMING_ACCESSORY1:
-    case MANUALCONTROLSETTINGS_ARMING_ACCESSORY2:
-        if (armSwitch < 0) {
-            lowThrottle = true;
-        }
         break;
-    default:
+#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
+    case FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL:
+    case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
+    case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
+    case FLIGHTSTATUS_FLIGHTMODE_LAND:
+    case FLIGHTSTATUS_FLIGHTMODE_POI:
+        handler = &handler_PATHFOLLOWER;
         break;
+    case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
+        handler = &handler_PATHPLANNER;
+        break;
+    case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
+    case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO:
+        handler = &handler_ALTITUDE;
+        break;
+#endif
+    case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE:
+        handler = &handler_AUTOTUNE;
+        break;
+        // There is no default, so if a flightmode is forgotten the compiler can throw a warning!
     }
 
-    if (forcedDisArm()) {
+    bool newinit = false;
-        // PathPlanner forces explicit disarming due to error condition (crash, impact, fire, ...)
-        setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
-        return;
-    }
 
-    if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) {
+    // FlightMode needs to be set correctly on first run (otherwise ControlChain is invalid)
-        // In this configuration we always disarm
+    static bool firstRun = true;
-        setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
-    } else {
-        // Not really needed since this function not called when disconnected
-        if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) {
-            lowThrottle = true;
-        }
 
-        // The throttle is not low, in case we where arming or disarming, abort
+    if (flightStatus.FlightMode != newMode || firstRun) {
-        if (!lowThrottle) {
+        firstRun = false;
-            switch (armState) {
+        flightStatus.ControlChain = handler->controlChain;
-            case ARM_STATE_DISARMING_MANUAL:
+        flightStatus.FlightMode   = newMode;
-            case ARM_STATE_DISARMING_TIMEOUT:
-                armState = ARM_STATE_ARMED;
-                break;
-            case ARM_STATE_ARMING_MANUAL:
-                armState = ARM_STATE_DISARMED;
-                break;
-            default:
-                // Nothing needs to be done in the other states
-                break;
-            }
-            return;
-        }
-
-        // The rest of these cases throttle is low
-        if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) {
-            // In this configuration, we go into armed state as soon as the throttle is low, never disarm
-            setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
-            return;
-        }
-
-        // When the configuration is not "Always armed" and no "Always disarmed",
-        // the state will not be changed when the throttle is not low
-        static portTickType armedDisarmStart;
-        float armingInputLevel = 0;
-
-        // Calc channel see assumptions7
-        switch (settings->Arming) {
-        case MANUALCONTROLSETTINGS_ARMING_ROLLLEFT:
-            armingInputLevel = 1.0f * cmd->Roll;
-            break;
-        case MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT:
-            armingInputLevel = -1.0f * cmd->Roll;
-            break;
-        case MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD:
-            armingInputLevel = 1.0f * cmd->Pitch;
-            break;
-        case MANUALCONTROLSETTINGS_ARMING_PITCHAFT:
-            armingInputLevel = -1.0f * cmd->Pitch;
-            break;
-        case MANUALCONTROLSETTINGS_ARMING_YAWLEFT:
-            armingInputLevel = 1.0f * cmd->Yaw;
-            break;
-        case MANUALCONTROLSETTINGS_ARMING_YAWRIGHT:
-            armingInputLevel = -1.0f * cmd->Yaw;
-            break;
-        case MANUALCONTROLSETTINGS_ARMING_ACCESSORY0:
-        case MANUALCONTROLSETTINGS_ARMING_ACCESSORY1:
-        case MANUALCONTROLSETTINGS_ARMING_ACCESSORY2:
-            armingInputLevel = -1.0f * (float)armSwitch;
-            break;
-        }
-
-        bool manualArm    = false;
-        bool manualDisarm = false;
-
-        if (armingInputLevel <= -ARMED_THRESHOLD) {
-            manualArm = true;
-        } else if (armingInputLevel >= +ARMED_THRESHOLD) {
-            manualDisarm = true;
-        }
-
-        switch (armState) {
-        case ARM_STATE_DISARMED:
-            setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
-
-            // only allow arming if it's OK too
-            if (manualArm && okToArm()) {
-                armedDisarmStart = lastSysTime;
-                armState = ARM_STATE_ARMING_MANUAL;
-            }
-            break;
-
-        case ARM_STATE_ARMING_MANUAL:
-            setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING);
-
-            if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->ArmingSequenceTime)) {
-                armState = ARM_STATE_ARMED;
-            } else if (!manualArm) {
-                armState = ARM_STATE_DISARMED;
-            }
-            break;
-
-        case ARM_STATE_ARMED:
-            // When we get here, the throttle is low,
-            // we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled
-            armedDisarmStart = lastSysTime;
-            armState = ARM_STATE_DISARMING_TIMEOUT;
-            setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
-            break;
-
-        case ARM_STATE_DISARMING_TIMEOUT:
-            // We get here when armed while throttle low, even when the arming timeout is not enabled
-            if ((settings->ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->ArmedTimeout)) {
-                armState = ARM_STATE_DISARMED;
-            }
-
-            // Switch to disarming due to manual control when needed
-            if (manualDisarm) {
-                armedDisarmStart = lastSysTime;
-                armState = ARM_STATE_DISARMING_MANUAL;
-            }
-            break;
-
-        case ARM_STATE_DISARMING_MANUAL:
-            if (manualDisarm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->DisarmingSequenceTime)) {
-                armState = ARM_STATE_DISARMED;
-            } else if (!manualDisarm) {
-                armState = ARM_STATE_ARMED;
-            }
-            break;
-        } // End Switch
-    }
-}
-
-/**
- * @brief Determine which of N positions the flight mode switch is in and set flight mode accordingly
- * @param[out] cmd Pointer to the command structure to set the flight mode in
- * @param[in] settings The settings which indicate which position is which mode
- * @param[in] flightMode the value of the switch position
- */
-static void processFlightMode(ManualControlSettingsData *settings, float flightMode)
-{
-    FlightStatusData flightStatus;
-
-    FlightStatusGet(&flightStatus);
-
-    // Convert flightMode value into the switch position in the range [0..N-1]
-    uint8_t pos = ((int16_t)(flightMode * 256.0f) + 256) * settings->FlightModeNumber >> 9;
-    if (pos >= settings->FlightModeNumber) {
-        pos = settings->FlightModeNumber - 1;
-    }
-
-    uint8_t newMode = settings->FlightModePosition[pos];
-
-    if (flightStatus.FlightMode != newMode) {
-        flightStatus.FlightMode = newMode;
         FlightStatusSet(&flightStatus);
+        newinit = true;
+    }
+    if (handler->handler) {
+        handler->handler(newinit);
     }
 }
 
-/**
- * @brief Determine if the manual input value is within acceptable limits
- * @returns return TRUE if so, otherwise return FALSE
- */
-bool validInputRange(int16_t min, int16_t max, uint16_t value)
-{
-    if (min > max) {
-        int16_t tmp = min;
-        min = max;
-        max = tmp;
-    }
-    return value >= min - CONNECTION_OFFSET && value <= max + CONNECTION_OFFSET;
-}
-
-/**
- * @brief Apply deadband to Roll/Pitch/Yaw channels
- */
-static void applyDeadband(float *value, float deadband)
-{
-    if (fabsf(*value) < deadband) {
-        *value = 0.0f;
-    } else if (*value > 0.0f) {
-        *value -= deadband;
-    } else {
-        *value += deadband;
-    }
-}
-
-#ifdef USE_INPUT_LPF
-/**
- * @brief Apply Low Pass Filter to Throttle/Roll/Pitch/Yaw or Accessory channel
- */
-static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT)
-{
-    if (cast_struct_to_array(settings->ResponseTime, settings->ResponseTime.Roll)[channel]) {
-        float rt = (float)cast_struct_to_array(settings->ResponseTime, settings->ResponseTime.Roll)[channel];
-        inputFiltered[channel] = ((rt * inputFiltered[channel]) + (dT * (*value))) / (rt + dT);
-        *value = inputFiltered[channel];
-    }
-}
-#endif // USE_INPUT_LPF
 /**
  * Called whenever a critical configuration component changes
  */
@@ -1272,6 +252,14 @@ static void configurationUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
     configuration_check();
 }
 
+/**
+ * Called whenever a critical configuration component changes
+ */
+static void commandUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+{
+    PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
+}
+
 /**
  * @}
  * @}

flight/modules/ManualControl/manualhandler.c

@@ -0,0 +1,71 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup ManualControl
+ * @brief Interpretes the control input in ManualControlCommand
+ * @{
+ *
+ * @file       manualhandler.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ *
+ * @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 "inc/manualcontrol.h"
+#include <manualcontrolcommand.h>
+#include <actuatordesired.h>
+
+// Private constants
+
+// Private types
+
+// Private functions
+
+
+/**
+ * @brief Handler to control Manual flightmode - input directly steers actuators
+ * @input: ManualControlCommand
+ * @output: ActuatorDesired
+ */
+void manualHandler(bool newinit)
+{
+    if (newinit) {
+        ActuatorDesiredInitialize();
+    }
+
+    ManualControlCommandData cmd;
+    ManualControlCommandGet(&cmd);
+
+    ActuatorDesiredData actuator;
+    ActuatorDesiredGet(&actuator);
+
+    actuator.Roll   = cmd.Roll;
+    actuator.Pitch  = cmd.Pitch;
+    actuator.Yaw    = cmd.Yaw;
+    actuator.Thrust = cmd.Thrust;
+
+    ActuatorDesiredSet(&actuator);
+}
+
+
+/**
+ * @}
+ * @}
+ */

flight/modules/ManualControl/pathfollowerhandler.c

@@ -0,0 +1,131 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup ManualControl
+ * @brief Interpretes the control input in ManualControlCommand
+ * @{
+ *
+ * @file       pathfollowerhandler.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ *
+ * @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 "inc/manualcontrol.h"
+#include <pathdesired.h>
+#include <manualcontrolcommand.h>
+#include <flightstatus.h>
+#include <positionstate.h>
+#include <flightmodesettings.h>
+
+
+#if defined(REVOLUTION)
+// Private constants
+
+// Private types
+
+// Private functions
+
+/**
+ * @brief Handler to control Guided flightmodes. FlightControl is governed by PathFollower, control via PathDesired
+ * @input: NONE: fully automated mode -- TODO recursively call handler for advanced stick commands
+ * @output: PathDesired
+ */
+void pathFollowerHandler(bool newinit)
+{
+    if (newinit) {
+        PathDesiredInitialize();
+        PositionStateInitialize();
+    }
+
+    FlightStatusData flightStatus;
+    FlightStatusGet(&flightStatus);
+
+    if (newinit) {
+        // After not being in this mode for a while init at current height
+        PositionStateData positionState;
+        PositionStateGet(&positionState);
+        FlightModeSettingsData settings;
+        FlightModeSettingsGet(&settings);
+        PathDesiredData pathDesired;
+        PathDesiredGet(&pathDesired);
+        switch (flightStatus.FlightMode) {
+        case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
+            // Simple Return To Base mode - keep altitude the same, fly to home position
+
+
+            pathDesired.Start.North      = 0;
+            pathDesired.Start.East       = 0;
+            pathDesired.Start.Down       = positionState.Down - settings.ReturnToHomeAltitudeOffset;
+            pathDesired.End.North        = 0;
+            pathDesired.End.East         = 0;
+            pathDesired.End.Down         = positionState.Down - settings.ReturnToHomeAltitudeOffset;
+            pathDesired.StartingVelocity = 1;
+            pathDesired.EndingVelocity   = 0;
+            pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
+            break;
+        default:
+
+            pathDesired.Start.North      = positionState.North;
+            pathDesired.Start.East       = positionState.East;
+            pathDesired.Start.Down       = positionState.Down;
+            pathDesired.End.North        = positionState.North;
+            pathDesired.End.East         = positionState.East;
+            pathDesired.End.Down         = positionState.Down;
+            pathDesired.StartingVelocity = 1;
+            pathDesired.EndingVelocity   = 0;
+            pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
+            /* Disable this section, until such time as proper discussion can be had about how to implement it for all types of crafts.
+               } else {
+               PathDesiredData pathDesired;
+               PathDesiredGet(&pathDesired);
+               pathDesired.End[PATHDESIRED_END_NORTH] += dT * -cmd->Pitch;
+               pathDesired.End[PATHDESIRED_END_EAST] += dT * cmd->Roll;
+               pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
+               PathDesiredSet(&pathDesired);
+             */
+            break;
+        }
+        PathDesiredSet(&pathDesired);
+    }
+
+    // special handling of autoland - behaves like positon hold but with slow altitude decrease
+    if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) {
+        PositionStateData positionState;
+        PositionStateGet(&positionState);
+        PathDesiredData pathDesired;
+        PathDesiredGet(&pathDesired);
+        pathDesired.End.Down = positionState.Down + 5;
+        PathDesiredSet(&pathDesired);
+    }
+}
+
+#else /* if defined(REVOLUTION) */
+void pathFollowerHandler(__attribute__((unused)) bool newinit)
+{
+    AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); // should not be called
+}
+#endif // REVOLUTION
+
+
+/**
+ * @}
+ * @}
+ */

flight/modules/ManualControl/pathplannerhandler.c

@@ -0,0 +1,64 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup ManualControl
+ * @brief Interpretes the control input in ManualControlCommand
+ * @{
+ *
+ * @file       pathplannerhandler.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ *
+ * @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 "inc/manualcontrol.h"
+
+// Private constants
+
+// Private types
+
+// Private functions
+
+
+/**
+ * @brief Handler to control Navigated flightmodes. FlightControl is governed by PathFollower, controlled indirectly via PathPlanner
+ * @input: NONE: fully automated mode -- TODO recursively call handler for advanced stick commands to affect navigation
+ * @output: NONE
+ */
+void pathPlannerHandler(__attribute__((unused)) bool newinit)
+{
+    /**
+     *
+     * TODO: In fully autonomous mode, commands to the navigation facility
+     * can be encoded through standard input cmd channels, as the pathplanner
+     * pathfollower generally ignores them
+     *
+     * this should be provided by a separate handler invoked here, as the
+     * handler for pathFollower should likely invoke them as well!!!
+     * (inputs also ignored)
+     *
+     */
+}
+
+
+/**
+ * @}
+ * @}
+ */

flight/modules/ManualControl/stabilizedhandler.c

@@ -0,0 +1,143 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup ManualControl
+ * @brief Interpretes the control input in ManualControlCommand
+ * @{
+ *
+ * @file       stabilizedhandler.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ *
+ * @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 "inc/manualcontrol.h"
+#include <pios_struct_helper.h>
+#include <manualcontrolcommand.h>
+#include <stabilizationdesired.h>
+#include <flightmodesettings.h>
+#include <stabilizationbank.h>
+
+// Private constants
+
+// Private types
+
+// Private functions
+
+
+/**
+ * @brief Handler to control Stabilized flightmodes. FlightControl is governed by "Stabilization"
+ * @input: ManualControlCommand
+ * @output: StabilizationDesired
+ */
+void stabilizedHandler(bool newinit)
+{
+    if (newinit) {
+        StabilizationDesiredInitialize();
+        StabilizationBankInitialize();
+    }
+    ManualControlCommandData cmd;
+    ManualControlCommandGet(&cmd);
+
+    FlightModeSettingsData settings;
+    FlightModeSettingsGet(&settings);
+
+    StabilizationDesiredData stabilization;
+    StabilizationDesiredGet(&stabilization);
+
+    StabilizationBankData stabSettings;
+    StabilizationBankGet(&stabSettings);
+
+    uint8_t *stab_settings;
+    FlightStatusData flightStatus;
+    FlightStatusGet(&flightStatus);
+    switch (flightStatus.FlightMode) {
+    case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
+        stab_settings = cast_struct_to_array(settings.Stabilization1Settings, settings.Stabilization1Settings.Roll);
+        break;
+    case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
+        stab_settings = cast_struct_to_array(settings.Stabilization2Settings, settings.Stabilization2Settings.Roll);
+        break;
+    case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
+        stab_settings = cast_struct_to_array(settings.Stabilization3Settings, settings.Stabilization3Settings.Roll);
+        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 = cast_struct_to_array(settings.Stabilization1Settings, settings.Stabilization1Settings.Roll);
+        return;
+    }
+
+    stabilization.Roll =
+        (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd.Roll :
+        (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Roll * stabSettings.ManualRate.Roll :
+        (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Roll * stabSettings.ManualRate.Roll :
+        (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Roll * stabSettings.RollMax :
+        (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Roll * stabSettings.ManualRate.Roll :
+        (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Roll :
+        (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Roll :
+        (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd.Roll * stabSettings.ManualRate.Roll :
+        (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd.Roll * stabSettings.RollMax :
+        0; // this is an invalid mode
+
+    stabilization.Pitch =
+        (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd.Pitch :
+        (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
+        (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
+        (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Pitch * stabSettings.PitchMax :
+        (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
+        (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Pitch :
+        (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Pitch :
+        (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
+        (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd.Pitch * stabSettings.PitchMax :
+        0; // this is an invalid mode
+
+    // TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order
+    stabilization.StabilizationMode.Roll  = stab_settings[0];
+    stabilization.StabilizationMode.Pitch = stab_settings[1];
+    // Other axes (yaw) cannot be Rattitude, so use Rate
+    // Should really do this for Attitude mode as well?
+    if (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) {
+        stabilization.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
+        stabilization.Yaw = cmd.Yaw * stabSettings.ManualRate.Yaw;
+    } else {
+        stabilization.StabilizationMode.Yaw = stab_settings[2];
+        stabilization.Yaw =
+            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd.Yaw :
+            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
+            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
+            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Yaw * stabSettings.YawMax :
+            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
+            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Yaw :
+            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Yaw :
+            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
+            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd.Yaw * stabSettings.YawMax :
+            0; // this is an invalid mode
+    }
+
+    stabilization.Thrust = cmd.Thrust;
+    StabilizationDesiredSet(&stabilization);
+}
+
+
+/**
+ * @}
+ * @}
+ */

flight/modules/OPLink/oplinkmod.c

@@ -63,8 +63,6 @@
 // Private types
 
 // Private variables
-static uint32_t idleCounter;
-static uint32_t idleCounterClear;
 static xTaskHandle systemTaskHandle;
 static bool stackOverflow;
 static bool mallocFailed;
@@ -119,7 +117,7 @@ static void systemTask(__attribute__((unused)) void *parameters)
     MODULE_TASKCREATE_ALL;
 
     /* start the delayed callback scheduler */
-    CallbackSchedulerStart();
+    PIOS_CALLBACKSCHEDULER_Start();
 
     if (mallocFailed) {
         /* We failed to malloc during task creation,
@@ -130,8 +128,6 @@ static void systemTask(__attribute__((unused)) void *parameters)
     }
 
     // Initialize vars
-    idleCounter = 0;
-    idleCounterClear = 0;
     lastSysTime = xTaskGetTickCount();
 
     // Main system loop
@@ -205,15 +201,7 @@ static void systemTask(__attribute__((unused)) void *parameters)
  * Called by the RTOS when the CPU is idle, used to measure the CPU idle time.
  */
 void vApplicationIdleHook(void)
-{
+{}
-    // Called when the scheduler has no tasks to run
-    if (idleCounterClear == 0) {
-        ++idleCounter;
-    } else {
-        idleCounter = 0;
-        idleCounterClear = 0;
-    }
-}
 
 /**
  * Called by the RTOS when a stack overflow is detected.

flight/modules/PathPlanner/pathplanner.c

@@ -31,6 +31,8 @@
 
 #include "openpilot.h"
 
+#include "callbackinfo.h"
+#include "pathplan.h"
 #include "flightstatus.h"
 #include "airspeedstate.h"
 #include "pathaction.h"
@@ -40,23 +42,26 @@
 #include "velocitystate.h"
 #include "waypoint.h"
 #include "waypointactive.h"
-#include "taskinfo.h"
+#include "flightmodesettings.h"
 #include <pios_struct_helper.h>
 #include "paths.h"
 
 // Private constants
 #define STACK_SIZE_BYTES            1024
-#define TASK_PRIORITY               (tskIDLE_PRIORITY + 1)
+#define TASK_PRIORITY               CALLBACK_TASK_NAVIGATION
 #define MAX_QUEUE_SIZE              2
-#define PATH_PLANNER_UPDATE_RATE_MS 20
+#define PATH_PLANNER_UPDATE_RATE_MS 100 // can be slow, since we listen to status updates as well
 
 // Private types
 
 // Private functions
-static void pathPlannerTask(void *parameters);
+static void pathPlannerTask();
-static void updatePathDesired(UAVObjEvent *ev);
+static void commandUpdated(UAVObjEvent *ev);
+static void statusUpdated(UAVObjEvent *ev);
+static void updatePathDesired();
 static void setWaypoint(uint16_t num);
 
+static uint8_t checkPathPlan();
 static uint8_t pathConditionCheck();
 static uint8_t conditionNone();
 static uint8_t conditionTimeOut();
@@ -71,7 +76,8 @@ static uint8_t conditionImmediate();
 
 
 // Private variables
-static xTaskHandle taskHandle;
+static DelayedCallbackInfo *pathPlannerHandle;
+static DelayedCallbackInfo *pathDesiredUpdaterHandle;
 static WaypointActiveData waypointActive;
 static WaypointData waypoint;
 static PathActionData pathAction;
@@ -83,11 +89,14 @@ static bool pathplanner_active = false;
  */
 int32_t PathPlannerStart()
 {
-    taskHandle = NULL;
+    // when the active waypoint changes, update pathDesired
+    WaypointConnectCallback(commandUpdated);
+    WaypointActiveConnectCallback(commandUpdated);
+    PathActionConnectCallback(commandUpdated);
+    PathStatusConnectCallback(statusUpdated);
 
-    // Start VM thread
+    // Start main task callback
-    xTaskCreate(pathPlannerTask, (signed char *)"PathPlanner", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
+    PIOS_CALLBACKSCHEDULER_Dispatch(pathPlannerHandle);
-    PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_PATHPLANNER, taskHandle);
 
     return 0;
 }
@@ -97,8 +106,7 @@ int32_t PathPlannerStart()
  */
 int32_t PathPlannerInitialize()
 {
-    taskHandle = NULL;
+    PathPlanInitialize();
-
     PathActionInitialize();
     PathStatusInitialize();
     PathDesiredInitialize();
@@ -108,6 +116,9 @@ int32_t PathPlannerInitialize()
     WaypointInitialize();
     WaypointActiveInitialize();
 
+    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);
+
     return 0;
 }
 
@@ -116,129 +127,250 @@ MODULE_INITCALL(PathPlannerInitialize, PathPlannerStart);
 /**
  * Module task
  */
-static void pathPlannerTask(__attribute__((unused)) void *parameters)
+static void pathPlannerTask()
 {
-    // when the active waypoint changes, update pathDesired
+    PIOS_CALLBACKSCHEDULER_Schedule(pathPlannerHandle, PATH_PLANNER_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
-    WaypointConnectCallback(updatePathDesired);
+
-    WaypointActiveConnectCallback(updatePathDesired);
+    bool endCondition = false;
-    PathActionConnectCallback(updatePathDesired);
+
+    // check path plan validity early to raise alarm
+    // even if not in guided mode
+    uint8_t validPathPlan = checkPathPlan();
 
     FlightStatusData flightStatus;
+    FlightStatusGet(&flightStatus);
+    if (flightStatus.ControlChain.PathPlanner != FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
+        pathplanner_active = false;
+        if (!validPathPlan) {
+            // unverified path plans are only a warning while we are not in pathplanner mode
+            // so it does not prevent arming. However manualcontrols safety check
+            // shall test for this warning when pathplan is on the flight mode selector
+            // thus a valid flight plan is a prerequirement for arming
+            AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_WARNING);
+        } else {
+            AlarmsClear(SYSTEMALARMS_ALARM_PATHPLAN);
+        }
+
+        return;
+    }
+
     PathDesiredData pathDesired;
+    PathDesiredGet(&pathDesired);
+
+    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
+            PositionStateData positionState;
+            PositionStateGet(&positionState);
+            FlightModeSettingsData settings;
+            FlightModeSettingsGet(&settings);
+
+            pathDesired.Start.North      = 0;
+            pathDesired.Start.East       = 0;
+            pathDesired.Start.Down       = positionState.Down - settings.ReturnToHomeAltitudeOffset;
+            pathDesired.End.North        = 0;
+            pathDesired.End.East         = 0;
+            pathDesired.End.Down         = positionState.Down - settings.ReturnToHomeAltitudeOffset;
+            pathDesired.StartingVelocity = 1;
+            pathDesired.EndingVelocity   = 0;
+            pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
+            PathDesiredSet(&pathDesired);
+        }
+        AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_ERROR);
+
+        return;
+    }
+    failsafeRTHset = 0;
+    AlarmsClear(SYSTEMALARMS_ALARM_PATHPLAN);
+
+    WaypointActiveGet(&waypointActive);
+
+    if (pathplanner_active == false) {
+        pathplanner_active   = true;
+
+        // This triggers callback to update variable
+        waypointActive.Index = 0;
+        WaypointActiveSet(&waypointActive);
+
+        return;
+    }
+
+    WaypointInstGet(waypointActive.Index, &waypoint);
+    PathActionInstGet(waypoint.Action, &pathAction);
     PathStatusData pathStatus;
+    PathStatusGet(&pathStatus);
 
-    // Main thread loop
+    // delay next step until path follower has acknowledged the path mode
-    bool endCondition = false;
+    if (pathStatus.UID != pathDesired.UID) {
-    while (1) {
+        return;
-        vTaskDelay(PATH_PLANNER_UPDATE_RATE_MS);
+    }
 
-        FlightStatusGet(&flightStatus);
+    // negative destinations DISABLE this feature
-        if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) {
+    if (pathStatus.Status == PATHSTATUS_STATUS_CRITICAL && waypointActive.Index != pathAction.ErrorDestination && pathAction.ErrorDestination >= 0) {
-            pathplanner_active = false;
+        setWaypoint(pathAction.ErrorDestination);
-            continue;
+        return;
+    }
+
+    // check if condition has been met
+    endCondition = pathConditionCheck();
+    // decide what to do
+    switch (pathAction.Command) {
+    case PATHACTION_COMMAND_ONNOTCONDITIONNEXTWAYPOINT:
+        endCondition = !endCondition;
+    case PATHACTION_COMMAND_ONCONDITIONNEXTWAYPOINT:
+        if (endCondition) {
+            setWaypoint(waypointActive.Index + 1);
         }
-
+        break;
-        WaypointActiveGet(&waypointActive);
+    case PATHACTION_COMMAND_ONNOTCONDITIONJUMPWAYPOINT:
-
+        endCondition = !endCondition;
-        if (pathplanner_active == false) {
+    case PATHACTION_COMMAND_ONCONDITIONJUMPWAYPOINT:
-            pathplanner_active   = true;
+        if (endCondition) {
-
+            if (pathAction.JumpDestination < 0) {
-            // This triggers callback to update variable
+                // waypoint ids <0 code relative jumps
-            waypointActive.Index = 0;
+                setWaypoint(waypointActive.Index - pathAction.JumpDestination);
-            WaypointActiveSet(&waypointActive);
-
-            continue;
-        }
-
-        WaypointInstGet(waypointActive.Index, &waypoint);
-        PathActionInstGet(waypoint.Action, &pathAction);
-        PathStatusGet(&pathStatus);
-        PathDesiredGet(&pathDesired);
-
-        // delay next step until path follower has acknowledged the path mode
-        if (pathStatus.UID != pathDesired.UID) {
-            continue;
-        }
-
-        // negative destinations DISABLE this feature
-        if (pathStatus.Status == PATHSTATUS_STATUS_CRITICAL && waypointActive.Index != pathAction.ErrorDestination && pathAction.ErrorDestination >= 0) {
-            setWaypoint(pathAction.ErrorDestination);
-            continue;
-        }
-
-        // check if condition has been met
-        endCondition = pathConditionCheck();
-
-        // decide what to do
-        switch (pathAction.Command) {
-        case PATHACTION_COMMAND_ONNOTCONDITIONNEXTWAYPOINT:
-            endCondition = !endCondition;
-        case PATHACTION_COMMAND_ONCONDITIONNEXTWAYPOINT:
-            if (endCondition) {
-                setWaypoint(waypointActive.Index + 1);
-            }
-            break;
-        case PATHACTION_COMMAND_ONNOTCONDITIONJUMPWAYPOINT:
-            endCondition = !endCondition;
-        case PATHACTION_COMMAND_ONCONDITIONJUMPWAYPOINT:
-            if (endCondition) {
-                if (pathAction.JumpDestination < 0) {
-                    // waypoint ids <0 code relative jumps
-                    setWaypoint(waypointActive.Index - pathAction.JumpDestination);
-                } else {
-                    setWaypoint(pathAction.JumpDestination);
-                }
-            }
-            break;
-        case PATHACTION_COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT:
-            if (endCondition) {
-                if (pathAction.JumpDestination < 0) {
-                    // waypoint ids <0 code relative jumps
-                    setWaypoint(waypointActive.Index - pathAction.JumpDestination);
-                } else {
-                    setWaypoint(pathAction.JumpDestination);
-                }
             } else {
-                setWaypoint(waypointActive.Index + 1);
+                setWaypoint(pathAction.JumpDestination);
             }
-            break;
         }
+        break;
+    case PATHACTION_COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT:
+        if (endCondition) {
+            if (pathAction.JumpDestination < 0) {
+                // waypoint ids <0 code relative jumps
+                setWaypoint(waypointActive.Index - pathAction.JumpDestination);
+            } else {
+                setWaypoint(pathAction.JumpDestination);
+            }
+        } else {
+            setWaypoint(waypointActive.Index + 1);
+        }
+        break;
     }
 }
 
+// safety checks for path plan integrity
+static uint8_t checkPathPlan()
+{
+    uint16_t i;
+    uint16_t waypointCount;
+    uint16_t actionCount;
+    uint8_t pathCrc;
+    PathPlanData pathPlan;
+
+    // WaypointData waypoint; // using global instead (?)
+    // PathActionData action; // using global instead (?)
+
+    PathPlanGet(&pathPlan);
+
+    waypointCount = pathPlan.WaypointCount;
+    if (waypointCount == 0) {
+        // an empty path plan is invalid
+        return false;
+    }
+    actionCount = pathPlan.PathActionCount;
+
+    // check count consistency
+    if (waypointCount > UAVObjGetNumInstances(WaypointHandle())) {
+        // PIOS_DEBUGLOG_Printf("PathPlan : waypoint count error!");
+        return false;
+    }
+    if (actionCount > UAVObjGetNumInstances(PathActionHandle())) {
+        // PIOS_DEBUGLOG_Printf("PathPlan : path action count error!");
+        return false;
+    }
+
+    // check CRC
+    pathCrc = 0;
+    for (i = 0; i < waypointCount; i++) {
+        pathCrc = UAVObjUpdateCRC(WaypointHandle(), i, pathCrc);
+    }
+    for (i = 0; i < actionCount; i++) {
+        pathCrc = UAVObjUpdateCRC(PathActionHandle(), i, pathCrc);
+    }
+    if (pathCrc != pathPlan.Crc) {
+        // failed crc check
+        // PIOS_DEBUGLOG_Printf("PathPlan : bad CRC (%d / %d)!", pathCrc, pathPlan.Crc);
+        return false;
+    }
+
+    // waypoint consistency
+    for (i = 0; i < waypointCount; i++) {
+        WaypointInstGet(i, &waypoint);
+        if (waypoint.Action >= actionCount) {
+            // path action id is out of range
+            return false;
+        }
+    }
+
+    // path action consistency
+    for (i = 0; i < actionCount; i++) {
+        PathActionInstGet(i, &pathAction);
+        if (pathAction.ErrorDestination >= waypointCount) {
+            // waypoint id is out of range
+            return false;
+        }
+        if (pathAction.JumpDestination >= waypointCount) {
+            // waypoint id is out of range
+            return false;
+        }
+    }
+
+    // path plan passed checks
+
+    return true;
+}
+
+// callback function when status changed, issue execution of state machine
+void commandUpdated(__attribute__((unused)) UAVObjEvent *ev)
+{
+    PIOS_CALLBACKSCHEDULER_Dispatch(pathDesiredUpdaterHandle);
+}
+
 // callback function when waypoints changed in any way, update pathDesired
-void updatePathDesired(__attribute__((unused)) UAVObjEvent *ev)
+void statusUpdated(__attribute__((unused)) UAVObjEvent *ev)
+{
+    PIOS_CALLBACKSCHEDULER_Dispatch(pathPlannerHandle);
+}
+
+
+// callback function when waypoints changed in any way, update pathDesired
+void updatePathDesired()
 {
     // only ever touch pathDesired if pathplanner is enabled
     if (!pathplanner_active) {
         return;
     }
 
-    // use local variables, dont use stack since this is huge and a callback,
+    PathDesiredData pathDesired;
-    // dont use the globals because we cant use mutexes here
-    static WaypointActiveData waypointActiveData;
-    static PathActionData pathActionData;
-    static WaypointData waypointData;
-    static PathDesiredData pathDesired;
 
     // find out current waypoint
-    WaypointActiveGet(&waypointActiveData);
+    WaypointActiveGet(&waypointActive);
 
-    WaypointInstGet(waypointActiveData.Index, &waypointData);
+    WaypointInstGet(waypointActive.Index, &waypoint);
-    PathActionInstGet(waypointData.Action, &pathActionData);
+    PathActionInstGet(waypoint.Action, &pathAction);
 
-    pathDesired.End.North = waypointData.Position.North;
+    pathDesired.End.North = waypoint.Position.North;
-    pathDesired.End.East  = waypointData.Position.East;
+    pathDesired.End.East  = waypoint.Position.East;
-    pathDesired.End.Down  = waypointData.Position.Down;
+    pathDesired.End.Down  = waypoint.Position.Down;
-    pathDesired.EndingVelocity    = waypointData.Velocity;
+    pathDesired.EndingVelocity    = waypoint.Velocity;
-    pathDesired.Mode = pathActionData.Mode;
+    pathDesired.Mode = pathAction.Mode;
-    pathDesired.ModeParameters[0] = pathActionData.ModeParameters[0];
+    pathDesired.ModeParameters[0] = pathAction.ModeParameters[0];
-    pathDesired.ModeParameters[1] = pathActionData.ModeParameters[1];
+    pathDesired.ModeParameters[1] = pathAction.ModeParameters[1];
-    pathDesired.ModeParameters[2] = pathActionData.ModeParameters[2];
+    pathDesired.ModeParameters[2] = pathAction.ModeParameters[2];
-    pathDesired.ModeParameters[3] = pathActionData.ModeParameters[3];
+    pathDesired.ModeParameters[3] = pathAction.ModeParameters[3];
-    pathDesired.UID = waypointActiveData.Index;
+    pathDesired.UID = waypointActive.Index;
 
-    if (waypointActiveData.Index == 0) {
+    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)
@@ -266,8 +398,13 @@ void updatePathDesired(__attribute__((unused)) UAVObjEvent *ev)
 // helper function to go to a specific waypoint
 static void setWaypoint(uint16_t num)
 {
-    // path plans wrap around
+    PathPlanData pathPlan;
-    if (num >= UAVObjGetNumInstances(WaypointHandle())) {
+
+    PathPlanGet(&pathPlan);
+
+    // here it is assumed that the path plan has been validated (waypoint count is consistent)
+    if (num >= pathPlan.WaypointCount) {
+        // path plans wrap around
         num = 0;
     }
 
@@ -275,10 +412,10 @@ static void setWaypoint(uint16_t num)
     WaypointActiveSet(&waypointActive);
 }
 
-// execute the apropriate condition and report result
+// execute the appropriate condition and report result
 static uint8_t pathConditionCheck()
 {
-    // i thought about a lookup table, but a switch is saver considering there could be invalid EndCondition ID's
+    // i thought about a lookup table, but a switch is safer considering there could be invalid EndCondition ID's
     switch (pathAction.EndCondition) {
     case PATHACTION_ENDCONDITION_NONE:
         return conditionNone();

flight/modules/RadioComBridge/RadioComBridge.c

@@ -1,5 +1,6 @@
 /**
  ******************************************************************************
+
  * @addtogroup OpenPilotModules OpenPilot Modules
  * @{
  * @addtogroup RadioComBridgeModule Com Port to Radio Bridge Module
@@ -36,6 +37,7 @@
 #include <objectpersistence.h>
 #include <oplinksettings.h>
 #include <oplinkreceiver.h>
+#include <radiocombridgestats.h>
 #include <uavtalk_priv.h>
 #include <pios_rfm22b.h>
 #include <ecc.h>
@@ -57,6 +59,7 @@
 #define SERIAL_RX_BUF_LEN 100
 #define PPM_INPUT_TIMEOUT 100
 
+
 // ****************
 // Private types
 
@@ -81,10 +84,11 @@ typedef struct {
     uint8_t  serialRxBuf[SERIAL_RX_BUF_LEN];
 
     // Error statistics.
-    uint32_t comTxErrors;
+    uint32_t telemetryTxRetries;
-    uint32_t comTxRetries;
+    uint32_t radioTxRetries;
-    uint32_t UAVTalkErrors;
+
-    uint32_t droppedPackets;
+    // Is this modem the coordinator
+    bool     isCoordinator;
 
     // Should we parse UAVTalk?
     bool     parseUAVTalk;
@@ -107,6 +111,7 @@ static int32_t RadioSendHandler(uint8_t *buf, int32_t length);
 static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte);
 static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte);
 static void objectPersistenceUpdatedCb(UAVObjEvent *objEv);
+static void registerObject(UAVObjHandle obj);
 
 // ****************
 // Private variables
@@ -124,12 +129,14 @@ static int32_t RadioComBridgeStart(void)
         // Get the settings.
         OPLinkSettingsData oplinkSettings;
         OPLinkSettingsGet(&oplinkSettings);
-        bool is_coordinator = (oplinkSettings.Coordinator == OPLINKSETTINGS_COORDINATOR_TRUE);
+
+        // Check if this is the coordinator modem
+        data->isCoordinator = (oplinkSettings.Coordinator == OPLINKSETTINGS_COORDINATOR_TRUE);
 
         // We will not parse/send UAVTalk if any ports are configured as Serial (except for over the USB HID port).
-        data->parseUAVTalk = ((oplinkSettings.MainPort != OPLINKSETTINGS_MAINPORT_SERIAL) &&
+        data->parseUAVTalk  = ((oplinkSettings.MainPort != OPLINKSETTINGS_MAINPORT_SERIAL) &&
-                              (oplinkSettings.FlexiPort != OPLINKSETTINGS_FLEXIPORT_SERIAL) &&
+                               (oplinkSettings.FlexiPort != OPLINKSETTINGS_FLEXIPORT_SERIAL) &&
-                              (oplinkSettings.VCPPort != OPLINKSETTINGS_VCPPORT_SERIAL));
+                               (oplinkSettings.VCPPort != OPLINKSETTINGS_VCPPORT_SERIAL));
 
         // Set the maximum radio RF power.
         switch (oplinkSettings.MaxRFPower) {
@@ -165,12 +172,16 @@ static int32_t RadioComBridgeStart(void)
         // Configure our UAVObjects for updates.
         UAVObjConnectQueue(UAVObjGetByID(OPLINKSTATUS_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ);
         UAVObjConnectQueue(UAVObjGetByID(OBJECTPERSISTENCE_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL);
-        if (is_coordinator) {
+        if (data->isCoordinator) {
             UAVObjConnectQueue(UAVObjGetByID(OPLINKRECEIVER_OBJID), data->radioEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ);
         } else {
             UAVObjConnectQueue(UAVObjGetByID(OPLINKRECEIVER_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ);
         }
 
+        if (data->isCoordinator) {
+            registerObject(RadioComBridgeStatsHandle());
+        }
+
         // Configure the UAVObject callbacks
         ObjectPersistenceConnectCallback(&objectPersistenceUpdatedCb);
 
@@ -223,27 +234,94 @@ static int32_t RadioComBridgeInitialize(void)
     OPLinkStatusInitialize();
     ObjectPersistenceInitialize();
     OPLinkReceiverInitialize();
+    RadioComBridgeStatsInitialize();
 
     // Initialise UAVTalk
-    data->telemUAVTalkCon   = UAVTalkInitialize(&UAVTalkSendHandler);
+    data->telemUAVTalkCon    = UAVTalkInitialize(&UAVTalkSendHandler);
-    data->radioUAVTalkCon   = UAVTalkInitialize(&RadioSendHandler);
+    data->radioUAVTalkCon    = UAVTalkInitialize(&RadioSendHandler);
 
     // Initialize the queues.
-    data->uavtalkEventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent));
+    data->uavtalkEventQueue  = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent));
-    data->radioEventQueue   = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent));
+    data->radioEventQueue    = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent));
 
     // Initialize the statistics.
-    data->comTxErrors   = 0;
+    data->telemetryTxRetries = 0;
-    data->comTxRetries  = 0;
+    data->radioTxRetries     = 0;
-    data->UAVTalkErrors = 0;
+
-    data->parseUAVTalk  = true;
+    data->parseUAVTalk = true;
-    data->comSpeed = OPLINKSETTINGS_COMSPEED_9600;
+    data->comSpeed     = OPLINKSETTINGS_COMSPEED_9600;
-    PIOS_COM_RADIO = PIOS_COM_RFM22B;
+    PIOS_COM_RADIO     = PIOS_COM_RFM22B;
 
     return 0;
 }
 MODULE_INITCALL(RadioComBridgeInitialize, RadioComBridgeStart);
 
+
+// TODO this code (badly) duplicates code from telemetry.c
+// This method should be used only for periodically updated objects.
+// The register method defined in telemetry.c should be factored out in a shared location so it can be
+// used from here...
+static void registerObject(UAVObjHandle obj)
+{
+    // Setup object for periodic updates
+    UAVObjEvent ev = {
+        .obj    = obj,
+        .instId = UAVOBJ_ALL_INSTANCES,
+        .event  = EV_UPDATED_PERIODIC,
+        .lowPriority = true,
+    };
+
+    // Get metadata
+    UAVObjMetadata metadata;
+
+    UAVObjGetMetadata(obj, &metadata);
+
+    EventPeriodicQueueCreate(&ev, data->uavtalkEventQueue, metadata.telemetryUpdatePeriod);
+    UAVObjConnectQueue(obj, data->uavtalkEventQueue, EV_UPDATED_PERIODIC | EV_UPDATED_MANUAL | EV_UPDATE_REQ);
+}
+
+/**
+ * Update telemetry statistics
+ */
+static void updateRadioComBridgeStats()
+{
+    UAVTalkStats telemetryUAVTalkStats;
+    UAVTalkStats radioUAVTalkStats;
+    RadioComBridgeStatsData radioComBridgeStats;
+
+    // Get telemetry stats
+    UAVTalkGetStats(data->telemUAVTalkCon, &telemetryUAVTalkStats, true);
+
+    // Get radio stats
+    UAVTalkGetStats(data->radioUAVTalkCon, &radioUAVTalkStats, true);
+
+    // Get stats object data
+    RadioComBridgeStatsGet(&radioComBridgeStats);
+
+    radioComBridgeStats.TelemetryTxRetries     = data->telemetryTxRetries;
+    radioComBridgeStats.RadioTxRetries         = data->radioTxRetries;
+
+    // Update stats object
+    radioComBridgeStats.TelemetryTxBytes      += telemetryUAVTalkStats.txBytes;
+    radioComBridgeStats.TelemetryTxFailures   += telemetryUAVTalkStats.txErrors;
+
+    radioComBridgeStats.TelemetryRxBytes      += telemetryUAVTalkStats.rxBytes;
+    radioComBridgeStats.TelemetryRxFailures   += telemetryUAVTalkStats.rxErrors;
+    radioComBridgeStats.TelemetryRxSyncErrors += telemetryUAVTalkStats.rxSyncErrors;
+    radioComBridgeStats.TelemetryRxCrcErrors  += telemetryUAVTalkStats.rxCrcErrors;
+
+    radioComBridgeStats.RadioTxBytes += radioUAVTalkStats.txBytes;
+    radioComBridgeStats.RadioTxFailures       += radioUAVTalkStats.txErrors;
+
+    radioComBridgeStats.RadioRxBytes += radioUAVTalkStats.rxBytes;
+    radioComBridgeStats.RadioRxFailures       += radioUAVTalkStats.rxErrors;
+    radioComBridgeStats.RadioRxSyncErrors     += radioUAVTalkStats.rxSyncErrors;
+    radioComBridgeStats.RadioRxCrcErrors      += radioUAVTalkStats.rxCrcErrors;
+
+    // Update stats object data
+    RadioComBridgeStatsSet(&radioComBridgeStats);
+}
+
 /**
  * @brief Telemetry transmit task, regular priority
  *
@@ -260,18 +338,20 @@ static void telemetryTxTask(__attribute__((unused)) void *parameters)
 #endif
         // Wait for queue message
         if (xQueueReceive(data->uavtalkEventQueue, &ev, MAX_PORT_DELAY) == pdTRUE) {
-            if ((ev.event == EV_UPDATED) || (ev.event == EV_UPDATE_REQ)) {
+            if (ev.obj == RadioComBridgeStatsHandle()) {
-                // Send update (with retries)
+                updateRadioComBridgeStats();
-                uint32_t retries = 0;
-                int32_t success  = -1;
-                while (retries < MAX_RETRIES && success == -1) {
-                    success = UAVTalkSendObject(data->telemUAVTalkCon, ev.obj, 0, 0, RETRY_TIMEOUT_MS) == 0;
-                    if (!success) {
-                        ++retries;
-                    }
-                }
-                data->comTxRetries += retries;
             }
+            // Send update (with retries)
+            int32_t ret = -1;
+            uint32_t retries = 0;
+            while (retries <= MAX_RETRIES && ret == -1) {
+                ret = UAVTalkSendObject(data->telemUAVTalkCon, ev.obj, ev.instId, 0, RETRY_TIMEOUT_MS);
+                if (ret == -1) {
+                    ++retries;
+                }
+            }
+            // Update stats
+            data->telemetryTxRetries += retries;
         }
     }
 }
@@ -296,15 +376,15 @@ static void radioTxTask(__attribute__((unused)) void *parameters)
         if (xQueueReceive(data->radioEventQueue, &ev, MAX_PORT_DELAY) == pdTRUE) {
             if ((ev.event == EV_UPDATED) || (ev.event == EV_UPDATE_REQ)) {
                 // Send update (with retries)
+                int32_t ret = -1;
                 uint32_t retries = 0;
-                int32_t success  = -1;
+                while (retries <= MAX_RETRIES && ret == -1) {
-                while (retries < MAX_RETRIES && success == -1) {
+                    ret = UAVTalkSendObject(data->radioUAVTalkCon, ev.obj, ev.instId, 0, RETRY_TIMEOUT_MS);
-                    success = UAVTalkSendObject(data->radioUAVTalkCon, ev.obj, 0, 0, RETRY_TIMEOUT_MS) == 0;
+                    if (ret == -1) {
-                    if (!success) {
                         ++retries;
                     }
                 }
-                data->comTxRetries += retries;
+                data->radioTxRetries += retries;
             }
         }
     }
@@ -333,7 +413,13 @@ static void radioRxTask(__attribute__((unused)) void *parameters)
                     }
                 } else if (PIOS_COM_TELEMETRY) {
                     // Send the data straight to the telemetry port.
-                    PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEMETRY, serial_data, bytes_to_process);
+                    // Following call can fail with -2 error code (buffer full) or -3 error code (could not acquire send mutex)
+                    // It is the caller responsibility to retry in such cases...
+                    int32_t ret   = -2;
+                    uint8_t count = 5;
+                    while (count-- > 0 && ret < -1) {
+                        ret = PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEMETRY, serial_data, bytes_to_process);
+                    }
                 }
             }
         } else {
@@ -425,9 +511,15 @@ static void serialRxTask(__attribute__((unused)) void *parameters)
             // Receive some data.
             uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(inputPort, data->serialRxBuf, sizeof(data->serialRxBuf), MAX_PORT_DELAY);
 
-            // Send the data over the radio link.
             if (bytes_to_process > 0) {
-                PIOS_COM_SendBufferNonBlocking(PIOS_COM_RADIO, data->serialRxBuf, bytes_to_process);
+                // Send the data over the radio link.
+                // Following call can fail with -2 error code (buffer full) or -3 error code (could not acquire send mutex)
+                // It is the caller responsibility to retry in such cases...
+                int32_t ret   = -2;
+                uint8_t count = 5;
+                while (count-- > 0 && ret < -1) {
+                    ret = PIOS_COM_SendBufferNonBlocking(PIOS_COM_RADIO, data->serialRxBuf, bytes_to_process);
+                }
             }
         } else {
             vTaskDelay(5);
@@ -445,6 +537,7 @@ static void serialRxTask(__attribute__((unused)) void *parameters)
  */
 static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length)
 {
+    int32_t ret;
     uint32_t outputPort = data->parseUAVTalk ? PIOS_COM_TELEMETRY : 0;
 
 #if defined(PIOS_INCLUDE_USB)
@@ -454,10 +547,17 @@ static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length)
     }
 #endif /* PIOS_INCLUDE_USB */
     if (outputPort) {
-        return PIOS_COM_SendBufferNonBlocking(outputPort, buf, length);
+        // Following call can fail with -2 error code (buffer full) or -3 error code (could not acquire send mutex)
+        // It is the caller responsibility to retry in such cases...
+        ret = -2;
+        uint8_t count = 5;
+        while (count-- > 0 && ret < -1) {
+            ret = PIOS_COM_SendBufferNonBlocking(outputPort, buf, length);
+        }
     } else {
-        return -1;
+        ret = -1;
     }
+    return ret;
 }
 
 /**
@@ -477,10 +577,16 @@ static int32_t RadioSendHandler(uint8_t *buf, int32_t length)
 
     // Don't send any data unless the radio port is available.
     if (outputPort && PIOS_COM_Available(outputPort)) {
-        return PIOS_COM_SendBufferNonBlocking(outputPort, buf, length);
+        // Following call can fail with -2 error code (buffer full) or -3 error code (could not acquire send mutex)
+        // It is the caller responsibility to retry in such cases...
+        int32_t ret   = -2;
+        uint8_t count = 5;
+        while (count-- > 0 && ret < -1) {
+            ret = PIOS_COM_SendBufferNonBlocking(outputPort, buf, length);
+        }
+        return ret;
     } else {
-        // For some reason, if this function returns failure, it prevents saving settings.
+        return -1;
-        return length;
     }
 }
 
@@ -494,12 +600,40 @@ static int32_t RadioSendHandler(uint8_t *buf, int32_t length)
 static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte)
 {
     // Keep reading until we receive a completed packet.
-    UAVTalkRxState state = UAVTalkProcessInputStream(inConnectionHandle, rxbyte);
+    UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(inConnectionHandle, rxbyte);
 
-    if (state == UAVTALK_STATE_ERROR) {
+    if (state == UAVTALK_STATE_COMPLETE) {
-        data->UAVTalkErrors++;
+        // We only want to unpack certain telemetry objects
-    } else if (state == UAVTALK_STATE_COMPLETE) {
+        uint32_t objId = UAVTalkGetPacketObjId(inConnectionHandle);
-        UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
+        switch (objId) {
+        case OPLINKSTATUS_OBJID:
+        case OPLINKSETTINGS_OBJID:
+        case OPLINKRECEIVER_OBJID:
+        case MetaObjectId(OPLINKSTATUS_OBJID):
+        case MetaObjectId(OPLINKSETTINGS_OBJID):
+        case MetaObjectId(OPLINKRECEIVER_OBJID):
+            UAVTalkReceiveObject(inConnectionHandle);
+            break;
+        case OBJECTPERSISTENCE_OBJID:
+        case MetaObjectId(OBJECTPERSISTENCE_OBJID):
+            // receive object locally
+            // some objects will send back a response to telemetry
+            // FIXME:
+            // OPLM will ack or nack all objects requests and acked object sends
+            // Receiver will probably also ack / nack the same messages
+            // This has some consequences like :
+            // Second ack/nack will not match an open transaction or will apply to wrong transaction
+            // Question : how does GCS handle receiving the same object twice
+            // The OBJECTPERSISTENCE logic can be broken too if for example OPLM nacks and then REVO acks...
+            UAVTalkReceiveObject(inConnectionHandle);
+            // relay packet to remote modem
+            UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
+            break;
+        default:
+            // all other packets are relayed to the remote modem
+            UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
+            break;
+        }
     }
 }
 
@@ -515,19 +649,30 @@ static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConn
     // Keep reading until we receive a completed packet.
     UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(inConnectionHandle, rxbyte);
 
-    if (state == UAVTALK_STATE_ERROR) {
+    if (state == UAVTALK_STATE_COMPLETE) {
-        data->UAVTalkErrors++;
+        // We only want to unpack certain objects from the remote modem
-    } else if (state == UAVTALK_STATE_COMPLETE) {
+        // Similarly we only want to relay certain objects to the telemetry port
-        // We only want to unpack certain objects from the remote modem.
         uint32_t objId = UAVTalkGetPacketObjId(inConnectionHandle);
         switch (objId) {
         case OPLINKSTATUS_OBJID:
         case OPLINKSETTINGS_OBJID:
+        case MetaObjectId(OPLINKSTATUS_OBJID):
+        case MetaObjectId(OPLINKSETTINGS_OBJID):
+            // Ignore object...
+            // These objects are shadowed by the modem and are not transmitted to the telemetry port
+            // - OPLINKSTATUS_OBJID : ground station will receive the OPLM link status instead
+            // - OPLINKSETTINGS_OBJID : ground station will read and write the OPLM settings instead
             break;
         case OPLINKRECEIVER_OBJID:
+        case MetaObjectId(OPLINKRECEIVER_OBJID):
+            // Receive object locally
+            // These objects are received by the modem and are not transmitted to the telemetry port
+            // - OPLINKRECEIVER_OBJID : not sure why
+            // some objects will send back a response to the remote modem
             UAVTalkReceiveObject(inConnectionHandle);
             break;
         default:
+            // all other packets are relayed to the telemetry port
             UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
             break;
         }
@@ -545,7 +690,7 @@ static void objectPersistenceUpdatedCb(__attribute__((unused)) UAVObjEvent *objE
 
     ObjectPersistenceGet(&obj_per);
 
-    // Is this concerning or setting object?
+    // Is this concerning our setting object?
     if (obj_per.ObjectID == OPLINKSETTINGS_OBJID) {
         // Is this a save, load, or delete?
         bool success = false;

flight/modules/Receiver/receiver.c

@@ -0,0 +1,673 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup ReceiverModule Manual Control Module
+ * @brief Provide manual control or allow it alter flight mode.
+ * @{
+ *
+ * Reads in the ManualControlCommand from receiver then
+ * pass it to ManualControl
+ *
+ * @file       receiver.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ * @brief      Receiver module. Handles safety R/C link and flight 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
+ */
+
+#include <openpilot.h>
+#include <pios_struct_helper.h>
+#include <accessorydesired.h>
+#include <manualcontrolsettings.h>
+#include <manualcontrolcommand.h>
+#include <receiveractivity.h>
+#include <flightstatus.h>
+#include <flighttelemetrystats.h>
+#include <flightmodesettings.h>
+#include <systemsettings.h>
+#include <taskinfo.h>
+
+#if defined(PIOS_INCLUDE_USB_RCTX)
+#include "pios_usb_rctx.h"
+#endif /* PIOS_INCLUDE_USB_RCTX */
+
+// Private constants
+#if defined(PIOS_RECEIVER_STACK_SIZE)
+#define STACK_SIZE_BYTES  PIOS_RECEIVER_STACK_SIZE
+#else
+#define STACK_SIZE_BYTES  1152
+#endif
+
+#define TASK_PRIORITY     (tskIDLE_PRIORITY + 3) // 3 = flight control
+#define UPDATE_PERIOD_MS  20
+#define THROTTLE_FAILSAFE -0.1f
+#define ARMED_THRESHOLD   0.50f
+// safe band to allow a bit of calibration error or trim offset (in microseconds)
+#define CONNECTION_OFFSET 250
+
+// Private types
+
+// Private variables
+static xTaskHandle taskHandle;
+static portTickType lastSysTime;
+
+#ifdef USE_INPUT_LPF
+static portTickType lastSysTimeLPF;
+static float inputFiltered[MANUALCONTROLSETTINGS_RESPONSETIME_NUMELEM];
+#endif
+
+// Private functions
+static void receiverTask(void *parameters);
+static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral);
+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);
+
+#ifdef USE_INPUT_LPF
+static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT);
+#endif
+
+#define RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP 12
+#define RCVR_ACTIVITY_MONITOR_MIN_RANGE          10
+struct rcvr_activity_fsm {
+    ManualControlSettingsChannelGroupsOptions group;
+    uint16_t prev[RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP];
+    uint8_t sample_count;
+};
+static struct rcvr_activity_fsm activity_fsm;
+
+static void resetRcvrActivity(struct rcvr_activity_fsm *fsm);
+static bool updateRcvrActivity(struct rcvr_activity_fsm *fsm);
+
+#define assumptions \
+    ( \
+        ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM) && \
+        ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELNUMBER_NUMELEM) && \
+        ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELMIN_NUMELEM) && \
+        ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELMAX_NUMELEM) && \
+        ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELNEUTRAL_NUMELEM))
+
+/**
+ * Module starting
+ */
+int32_t ReceiverStart()
+{
+    // Start main task
+    xTaskCreate(receiverTask, (signed char *)"Receiver", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
+    PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_RECEIVER, taskHandle);
+#ifdef PIOS_INCLUDE_WDG
+    PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL);
+#endif
+
+    return 0;
+}
+
+/**
+ * Module initialization
+ */
+int32_t ReceiverInitialize()
+{
+    /* Check the assumptions about uavobject enum's are correct */
+    PIOS_STATIC_ASSERT(assumptions);
+
+    ManualControlCommandInitialize();
+    ReceiverActivityInitialize();
+    ManualControlSettingsInitialize();
+
+    return 0;
+}
+MODULE_INITCALL(ReceiverInitialize, ReceiverStart);
+
+/**
+ * Module task
+ */
+static void receiverTask(__attribute__((unused)) void *parameters)
+{
+    ManualControlSettingsData settings;
+    ManualControlCommandData cmd;
+    FlightStatusData flightStatus;
+
+    uint8_t disconnected_count = 0;
+    uint8_t connected_count    = 0;
+
+    // For now manual instantiate extra instances of Accessory Desired.  In future should be done dynamically
+    // this includes not even registering it if not used
+    AccessoryDesiredCreateInstance();
+    AccessoryDesiredCreateInstance();
+
+    // Whenever the configuration changes, make sure it is safe to fly
+
+    ManualControlCommandGet(&cmd);
+    FlightStatusGet(&flightStatus);
+
+    /* Initialize the RcvrActivty FSM */
+    portTickType lastActivityTime = xTaskGetTickCount();
+    resetRcvrActivity(&activity_fsm);
+
+    // Main task loop
+    lastSysTime = xTaskGetTickCount();
+
+    float scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM] = { 0 };
+    SystemSettingsThrustControlOptions thrustType;
+
+    while (1) {
+        // Wait until next update
+        vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS);
+#ifdef PIOS_INCLUDE_WDG
+        PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL);
+#endif
+
+        // Read settings
+        ManualControlSettingsGet(&settings);
+        SystemSettingsThrustControlGet(&thrustType);
+
+        /* Update channel activity monitor */
+        if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) {
+            if (updateRcvrActivity(&activity_fsm)) {
+                /* Reset the aging timer because activity was detected */
+                lastActivityTime = lastSysTime;
+            }
+        }
+        if (timeDifferenceMs(lastActivityTime, lastSysTime) > 5000) {
+            resetRcvrActivity(&activity_fsm);
+            lastActivityTime = lastSysTime;
+        }
+
+        if (ManualControlCommandReadOnly()) {
+            FlightTelemetryStatsData flightTelemStats;
+            FlightTelemetryStatsGet(&flightTelemStats);
+            if (flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
+                /* trying to fly via GCS and lost connection.  fall back to transmitter */
+                UAVObjMetadata metadata;
+                ManualControlCommandGetMetadata(&metadata);
+                UAVObjSetAccess(&metadata, ACCESS_READWRITE);
+                ManualControlCommandSetMetadata(&metadata);
+            }
+        }
+
+        bool valid_input_detected = true;
+
+        // Read channel values in us
+        for (uint8_t n = 0; n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) {
+            extern uint32_t pios_rcvr_group_map[];
+
+            if (cast_struct_to_array(settings.ChannelGroups, settings.ChannelGroups.Roll)[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
+                cmd.Channel[n] = PIOS_RCVR_INVALID;
+            } else {
+                cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[
+                                                    cast_struct_to_array(settings.ChannelGroups, settings.ChannelGroups.Pitch)[n]],
+                                                cast_struct_to_array(settings.ChannelNumber, settings.ChannelNumber.Pitch)[n]);
+            }
+
+            // If a channel has timed out this is not valid data and we shouldn't update anything
+            // until we decide to go to failsafe
+            if (cmd.Channel[n] == (uint16_t)PIOS_RCVR_TIMEOUT) {
+                valid_input_detected = false;
+            } else {
+                scaledChannel[n] = scaleChannel(cmd.Channel[n],
+                                                cast_struct_to_array(settings.ChannelMax, settings.ChannelMax.Pitch)[n],
+                                                cast_struct_to_array(settings.ChannelMin, settings.ChannelMin.Pitch)[n],
+                                                cast_struct_to_array(settings.ChannelNeutral, settings.ChannelNeutral.Pitch)[n]);
+            }
+        }
+
+        // Check settings, if error raise alarm
+        if (settings.ChannelGroups.Roll >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
+            || settings.ChannelGroups.Pitch >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
+            || 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_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_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
+            (thrustType == SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE && (
+                 settings.ChannelGroups.Collective >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
+                 || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] == (uint16_t)PIOS_RCVR_INVALID
+                 || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] == (uint16_t)PIOS_RCVR_NODRIVER))
+            ||
+            // Check the FlightModeNumber is valid
+            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.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))) {
+            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])
+                                && 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,
+                                                    settings.ChannelMax.Collective, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE]);
+        }
+        if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
+            valid_input_detected &= validInputRange(settings.ChannelMin.Accessory0,
+                                                    settings.ChannelMax.Accessory0, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0]);
+        }
+        if (settings.ChannelGroups.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
+            valid_input_detected &= validInputRange(settings.ChannelMin.Accessory1,
+                                                    settings.ChannelMax.Accessory1, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1]);
+        }
+        if (settings.ChannelGroups.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
+            valid_input_detected &= validInputRange(settings.ChannelMin.Accessory2,
+                                                    settings.ChannelMax.Accessory2, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2]);
+        }
+
+        // Implement hysteresis loop on connection status
+        if (valid_input_detected && (++connected_count > 10)) {
+            cmd.Connected      = MANUALCONTROLCOMMAND_CONNECTED_TRUE;
+            connected_count    = 0;
+            disconnected_count = 0;
+        } else if (!valid_input_detected && (++disconnected_count > 10)) {
+            cmd.Connected      = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
+            connected_count    = 0;
+            disconnected_count = 0;
+        }
+
+        if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) {
+            cmd.Throttle   = settings.FailsafeChannel.Throttle;
+            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:
+                cmd.Thrust = cmd.Throttle;
+                break;
+            case SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE:
+                cmd.Thrust = cmd.Collective;
+                break;
+            default:
+                break;
+            }
+            if (settings.FailsafeFlightModeSwitchPosition >= 0 && settings.FailsafeFlightModeSwitchPosition < settings.FlightModeNumber) {
+                cmd.FlightModeSwitchPosition = (uint8_t)settings.FailsafeFlightModeSwitchPosition;
+            }
+            AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING);
+
+            AccessoryDesiredData accessory;
+            // Set Accessory 0
+            if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
+                accessory.AccessoryVal = settings.FailsafeChannel.Accessory0;
+                if (AccessoryDesiredInstSet(0, &accessory) != 0) {
+                    AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING);
+                }
+            }
+            // Set Accessory 1
+            if (settings.ChannelGroups.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
+                accessory.AccessoryVal = settings.FailsafeChannel.Accessory1;
+                if (AccessoryDesiredInstSet(1, &accessory) != 0) {
+                    AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING);
+                }
+            }
+            // Set Accessory 2
+            if (settings.ChannelGroups.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
+                accessory.AccessoryVal = settings.FailsafeChannel.Accessory2;
+                if (AccessoryDesiredInstSet(2, &accessory) != 0) {
+                    AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING);
+                }
+            }
+        } else if (valid_input_detected) {
+            AlarmsClear(SYSTEMALARMS_ALARM_RECEIVER);
+
+            // Scale channels to -1 -> +1 range
+            cmd.Roll     = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL];
+            cmd.Pitch    = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH];
+            cmd.Yaw      = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW];
+            cmd.Throttle = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE];
+            // Convert flightMode value into the switch position in the range [0..N-1]
+            cmd.FlightModeSwitchPosition = ((int16_t)(scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] * 256.0f) + 256) * settings.FlightModeNumber >> 9;
+            if (cmd.FlightModeSwitchPosition >= settings.FlightModeNumber) {
+                cmd.FlightModeSwitchPosition = settings.FlightModeNumber - 1;
+            }
+
+            // Apply deadband for Roll/Pitch/Yaw stick inputs
+            if (settings.Deadband > 0.0f) {
+                applyDeadband(&cmd.Roll, settings.Deadband);
+                applyDeadband(&cmd.Pitch, settings.Deadband);
+                applyDeadband(&cmd.Yaw, settings.Deadband);
+            }
+#ifdef USE_INPUT_LPF
+            // Apply Low Pass Filter to input channels, time delta between calls in ms
+            portTickType thisSysTime = xTaskGetTickCount();
+            float dT = (thisSysTime > lastSysTimeLPF) ?
+                       (float)((thisSysTime - lastSysTimeLPF) * portTICK_RATE_MS) :
+                       (float)UPDATE_PERIOD_MS;
+            lastSysTimeLPF = thisSysTime;
+
+            applyLPF(&cmd.Roll, MANUALCONTROLSETTINGS_RESPONSETIME_ROLL, &settings, dT);
+            applyLPF(&cmd.Pitch, MANUALCONTROLSETTINGS_RESPONSETIME_PITCH, &settings, dT);
+            applyLPF(&cmd.Yaw, MANUALCONTROLSETTINGS_RESPONSETIME_YAW, &settings, dT);
+#endif // USE_INPUT_LPF
+            if (cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_INVALID
+                && cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_NODRIVER
+                && cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_TIMEOUT) {
+                cmd.Collective = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE];
+                if (settings.Deadband > 0.0f) {
+                    applyDeadband(&cmd.Collective, settings.Deadband);
+                }
+#ifdef USE_INPUT_LPF
+                applyLPF(&cmd.Collective, MANUALCONTROLSETTINGS_RESPONSETIME_COLLECTIVE, &settings, dT);
+#endif // USE_INPUT_LPF
+            }
+
+            switch (thrustType) {
+            case SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE:
+                cmd.Thrust = cmd.Throttle;
+                break;
+            case SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE:
+                cmd.Thrust = cmd.Collective;
+                break;
+            default:
+                break;
+            }
+
+            AccessoryDesiredData accessory;
+            // Set Accessory 0
+            if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
+                accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0];
+#ifdef USE_INPUT_LPF
+                applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT);
+#endif
+                if (AccessoryDesiredInstSet(0, &accessory) != 0) {
+                    AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING);
+                }
+            }
+            // Set Accessory 1
+            if (settings.ChannelGroups.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
+                accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1];
+#ifdef USE_INPUT_LPF
+                applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT);
+#endif
+                if (AccessoryDesiredInstSet(1, &accessory) != 0) {
+                    AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING);
+                }
+            }
+            // Set Accessory 2
+            if (settings.ChannelGroups.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
+                accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2];
+#ifdef USE_INPUT_LPF
+                applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT);
+#endif
+
+                if (AccessoryDesiredInstSet(2, &accessory) != 0) {
+                    AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING);
+                }
+            }
+        }
+
+        // Update cmd object
+        ManualControlCommandSet(&cmd);
+#if defined(PIOS_INCLUDE_USB_RCTX)
+        if (pios_usb_rctx_id) {
+            PIOS_USB_RCTX_Update(pios_usb_rctx_id,
+                                 cmd.Channel,
+                                 cast_struct_to_array(settings.ChannelMin, settings.ChannelMin.Roll),
+                                 cast_struct_to_array(settings.ChannelMax, settings.ChannelMax.Roll),
+                                 NELEMENTS(cmd.Channel));
+        }
+#endif /* PIOS_INCLUDE_USB_RCTX */
+    }
+}
+
+static void resetRcvrActivity(struct rcvr_activity_fsm *fsm)
+{
+    ReceiverActivityData data;
+    bool updated = false;
+
+    /* Clear all channel activity flags */
+    ReceiverActivityGet(&data);
+    if (data.ActiveGroup != RECEIVERACTIVITY_ACTIVEGROUP_NONE && data.ActiveChannel != 255) {
+        data.ActiveGroup   = RECEIVERACTIVITY_ACTIVEGROUP_NONE;
+        data.ActiveChannel = 255;
+        updated = true;
+    }
+    if (updated) {
+        ReceiverActivitySet(&data);
+    }
+
+    /* Reset the FSM state */
+    fsm->group = 0;
+    fsm->sample_count = 0;
+}
+
+static void updateRcvrActivitySample(uint32_t rcvr_id, uint16_t samples[], uint8_t max_channels)
+{
+    for (uint8_t channel = 1; channel <= max_channels; channel++) {
+        // Subtract 1 because channels are 1 indexed
+        samples[channel - 1] = PIOS_RCVR_Read(rcvr_id, channel);
+    }
+}
+
+static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm *fsm)
+{
+    bool activity_updated = false;
+
+    /* Compare the current value to the previous sampled value */
+    for (uint8_t channel = 1; channel <= RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP; channel++) {
+        uint16_t delta;
+        uint16_t prev = fsm->prev[channel - 1]; // Subtract 1 because channels are 1 indexed
+        uint16_t curr = PIOS_RCVR_Read(rcvr_id, channel);
+        if (curr > prev) {
+            delta = curr - prev;
+        } else {
+            delta = prev - curr;
+        }
+
+        if (delta > RCVR_ACTIVITY_MONITOR_MIN_RANGE) {
+            /* Mark this channel as active */
+            ReceiverActivityActiveGroupOptions group;
+
+            /* Don't assume manualcontrolsettings and receiveractivity are in the same order. */
+            switch (fsm->group) {
+            case MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM:
+                group = RECEIVERACTIVITY_ACTIVEGROUP_PWM;
+                break;
+            case MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM:
+                group = RECEIVERACTIVITY_ACTIVEGROUP_PPM;
+                break;
+            case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT:
+                group = RECEIVERACTIVITY_ACTIVEGROUP_DSMMAINPORT;
+                break;
+            case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT:
+                group = RECEIVERACTIVITY_ACTIVEGROUP_DSMFLEXIPORT;
+                break;
+            case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS:
+                group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS;
+                break;
+            case MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS:
+                group = RECEIVERACTIVITY_ACTIVEGROUP_GCS;
+                break;
+            case MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK:
+                group = RECEIVERACTIVITY_ACTIVEGROUP_OPLINK;
+                break;
+            default:
+                PIOS_Assert(0);
+                break;
+            }
+
+            ReceiverActivityActiveGroupSet((uint8_t *)&group);
+            ReceiverActivityActiveChannelSet(&channel);
+            activity_updated = true;
+        }
+    }
+    return activity_updated;
+}
+
+static bool updateRcvrActivity(struct rcvr_activity_fsm *fsm)
+{
+    bool activity_updated = false;
+
+    if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
+        /* We're out of range, reset things */
+        resetRcvrActivity(fsm);
+    }
+
+    extern uint32_t pios_rcvr_group_map[];
+    if (!pios_rcvr_group_map[fsm->group]) {
+        /* Unbound group, skip it */
+        goto group_completed;
+    }
+
+    if (fsm->sample_count == 0) {
+        /* Take a sample of each channel in this group */
+        updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], fsm->prev, NELEMENTS(fsm->prev));
+        fsm->sample_count++;
+        return false;
+    }
+
+    /* Compare with previous sample */
+    activity_updated = updateRcvrActivityCompare(pios_rcvr_group_map[fsm->group], fsm);
+
+group_completed:
+    /* Reset the sample counter */
+    fsm->sample_count = 0;
+
+    /* Find the next active group, but limit search so we can't loop forever here */
+    for (uint8_t i = 0; i < MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE; i++) {
+        /* Move to the next group */
+        fsm->group++;
+        if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
+            /* Wrap back to the first group */
+            fsm->group = 0;
+        }
+        if (pios_rcvr_group_map[fsm->group]) {
+            /*
+             * Found an active group, take a sample here to avoid an
+             * extra 20ms delay in the main thread so we can speed up
+             * this algorithm.
+             */
+            updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], fsm->prev, NELEMENTS(fsm->prev));
+            fsm->sample_count++;
+            break;
+        }
+    }
+
+    return activity_updated;
+}
+
+/**
+ * Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range.
+ */
+static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral)
+{
+    float valueScaled;
+
+    // Scale
+    if ((max > min && value >= neutral) || (min > max && value <= neutral)) {
+        if (max != neutral) {
+            valueScaled = (float)(value - neutral) / (float)(max - neutral);
+        } else {
+            valueScaled = 0;
+        }
+    } else {
+        if (min != neutral) {
+            valueScaled = (float)(value - neutral) / (float)(neutral - min);
+        } else {
+            valueScaled = 0;
+        }
+    }
+
+    // Bound
+    if (valueScaled > 1.0f) {
+        valueScaled = 1.0f;
+    } else if (valueScaled < -1.0f) {
+        valueScaled = -1.0f;
+    }
+
+    return valueScaled;
+}
+
+static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time)
+{
+    return (end_time - start_time) * portTICK_RATE_MS;
+}
+
+
+/**
+ * @brief Determine if the manual input value is within acceptable limits
+ * @returns return TRUE if so, otherwise return FALSE
+ */
+bool validInputRange(int16_t min, int16_t max, uint16_t value)
+{
+    if (min > max) {
+        int16_t tmp = min;
+        min = max;
+        max = tmp;
+    }
+    return value >= min - CONNECTION_OFFSET && value <= max + CONNECTION_OFFSET;
+}
+
+/**
+ * @brief Apply deadband to Roll/Pitch/Yaw channels
+ */
+static void applyDeadband(float *value, float deadband)
+{
+    if (fabsf(*value) < deadband) {
+        *value = 0.0f;
+    } else if (*value > 0.0f) {
+        *value -= deadband;
+    } else {
+        *value += deadband;
+    }
+}
+
+#ifdef USE_INPUT_LPF
+/**
+ * @brief Apply Low Pass Filter to Throttle/Roll/Pitch/Yaw or Accessory channel
+ */
+static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT)
+{
+    if (cast_struct_to_array(settings->ResponseTime, settings->ResponseTime.Roll)[channel]) {
+        float rt = (float)cast_struct_to_array(settings->ResponseTime, settings->ResponseTime.Roll)[channel];
+        inputFiltered[channel] = ((rt * inputFiltered[channel]) + (dT * (*value))) / (rt + dT);
+        *value = inputFiltered[channel];
+    }
+}
+#endif // USE_INPUT_LPF
+
+/**
+ * @}
+ * @}
+ */

flight/modules/Sensors/sensors.c

@@ -55,6 +55,7 @@
 #include <attitudestate.h>
 #include <attitudesettings.h>
 #include <revocalibration.h>
+#include <accelgyrosettings.h>
 #include <flightstatus.h>
 #include <taskinfo.h>
 
@@ -78,16 +79,15 @@ static void settingsUpdatedCb(UAVObjEvent *objEv);
 // Private variables
 static xTaskHandle sensorsTaskHandle;
 RevoCalibrationData cal;
+AccelGyroSettingsData agcal;
 
 // These values are initialized by settings but can be updated by the attitude algorithm
 
 static float mag_bias[3] = { 0, 0, 0 };
 static float mag_scale[3] = { 0, 0, 0 };
-static float accel_bias[3] = { 0, 0, 0 };
+// temp coefficient to calculate gyro bias
-static float accel_scale[3] = { 0, 0, 0 };
+static volatile bool gyro_temp_calibrated  = false;
-static float gyro_staticbias[3] = { 0, 0, 0 };
+static volatile bool accel_temp_calibrated = false;
-static float gyro_scale[3] = { 0, 0, 0 };
-
 static float R[3][3] = {
     { 0 }
 };
@@ -113,12 +113,13 @@ int32_t SensorsInitialize(void)
     MagSensorInitialize();
     RevoCalibrationInitialize();
     AttitudeSettingsInitialize();
+    AccelGyroSettingsInitialize();
 
     rotate = 0;
 
     RevoCalibrationConnectCallback(&settingsUpdatedCb);
     AttitudeSettingsConnectCallback(&settingsUpdatedCb);
-
+    AccelGyroSettingsConnectCallback(&settingsUpdatedCb);
     return 0;
 }
 
@@ -341,12 +342,24 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
         }
 
         // Scale the accels
-        float accels[3]     = { (float)accel_accum[0] / accel_samples,
+        float accels[3] = { (float)accel_accum[0] / accel_samples,
-                                (float)accel_accum[1] / accel_samples,
+                            (float)accel_accum[1] / accel_samples,
-                                (float)accel_accum[2] / accel_samples };
+                            (float)accel_accum[2] / accel_samples };
-        float accels_out[3] = { accels[0] * accel_scaling * accel_scale[0] - accel_bias[0],
+
-                                accels[1] * accel_scaling * accel_scale[1] - accel_bias[1],
+
-                                accels[2] * accel_scaling * accel_scale[2] - accel_bias[2] };
+        float accels_out[3] = { accels[0] * accel_scaling * agcal.accel_scale.X - agcal.accel_bias.X,
+                                accels[1] * accel_scaling * agcal.accel_scale.Y - agcal.accel_bias.Y,
+                                accels[2] * accel_scaling * agcal.accel_scale.Z - agcal.accel_bias.Z };
+
+        if (accel_temp_calibrated) {
+            float ctemp = accelSensorData.temperature > agcal.temp_calibrated_extent.max ? agcal.temp_calibrated_extent.max :
+                          (accelSensorData.temperature < agcal.temp_calibrated_extent.min ? agcal.temp_calibrated_extent.min
+                           : accelSensorData.temperature);
+            accels_out[0] -= agcal.accel_temp_coeff.X * ctemp;
+            accels_out[1] -= agcal.accel_temp_coeff.Y * ctemp;
+            accels_out[2] -= agcal.accel_temp_coeff.Z * ctemp;
+        }
+
         if (rotate) {
             rot_mult(R, accels_out, accels);
             accelSensorData.x = accels[0];
@@ -360,12 +373,23 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
         AccelSensorSet(&accelSensorData);
 
         // Scale the gyros
-        float gyros[3]     = { (float)gyro_accum[0] / gyro_samples,
+        float gyros[3] = { (float)gyro_accum[0] / gyro_samples,
-                               (float)gyro_accum[1] / gyro_samples,
+                           (float)gyro_accum[1] / gyro_samples,
-                               (float)gyro_accum[2] / gyro_samples };
+                           (float)gyro_accum[2] / gyro_samples };
-        float gyros_out[3] = { gyros[0] * gyro_scaling * gyro_scale[0] - gyro_staticbias[0],
+
-                               gyros[1] * gyro_scaling * gyro_scale[1] - gyro_staticbias[1],
+        float gyros_out[3] = { gyros[0] * gyro_scaling * agcal.gyro_scale.X - agcal.gyro_bias.X,
-                               gyros[2] * gyro_scaling * gyro_scale[2] - gyro_staticbias[2] };
+                               gyros[1] * gyro_scaling * agcal.gyro_scale.Y - agcal.gyro_bias.Y,
+                               gyros[2] * gyro_scaling * agcal.gyro_scale.Z - agcal.gyro_bias.Z };
+
+        if (gyro_temp_calibrated) {
+            float ctemp = gyroSensorData.temperature > agcal.temp_calibrated_extent.max ? agcal.temp_calibrated_extent.max :
+                          (gyroSensorData.temperature < agcal.temp_calibrated_extent.min ? agcal.temp_calibrated_extent.min
+                           : gyroSensorData.temperature);
+            gyros_out[0] -= (agcal.gyro_temp_coeff.X + agcal.gyro_temp_coeff.X2 * ctemp) * ctemp;
+            gyros_out[1] -= (agcal.gyro_temp_coeff.Y + agcal.gyro_temp_coeff.Y2 * ctemp) * ctemp;
+            gyros_out[2] -= (agcal.gyro_temp_coeff.Z + agcal.gyro_temp_coeff.Z2 * ctemp) * ctemp;
+        }
+
         if (rotate) {
             rot_mult(R, gyros_out, gyros);
             gyroSensorData.x = gyros[0];
@@ -421,25 +445,20 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
 static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
 {
     RevoCalibrationGet(&cal);
+    AccelGyroSettingsGet(&agcal);
+    mag_bias[0]  = cal.mag_bias.X;
+    mag_bias[1]  = cal.mag_bias.Y;
+    mag_bias[2]  = cal.mag_bias.Z;
+    mag_scale[0] = cal.mag_scale.X;
+    mag_scale[1] = cal.mag_scale.Y;
+    mag_scale[2] = cal.mag_scale.Z;
 
-    mag_bias[0]        = cal.mag_bias.X;
+    accel_temp_calibrated = (agcal.temp_calibrated_extent.max - agcal.temp_calibrated_extent.min > .1f) &&
-    mag_bias[1]        = cal.mag_bias.Y;
+                            (fabsf(agcal.accel_temp_coeff.X) > 1e-9f || fabsf(agcal.accel_temp_coeff.Y) > 1e-9f || fabsf(agcal.accel_temp_coeff.Z) > 1e-9f);
-    mag_bias[2]        = cal.mag_bias.Z;
+
-    mag_scale[0]       = cal.mag_scale.X;
+    gyro_temp_calibrated  = (agcal.temp_calibrated_extent.max - agcal.temp_calibrated_extent.min > .1f) &&
-    mag_scale[1]       = cal.mag_scale.Y;
+                            (fabsf(agcal.gyro_temp_coeff.X) > 1e-9f || fabsf(agcal.gyro_temp_coeff.Y) > 1e-9f ||
-    mag_scale[2]       = cal.mag_scale.Z;
+                            fabsf(agcal.gyro_temp_coeff.Z) > 1e-9f || fabsf(agcal.gyro_temp_coeff.Z2) > 1e-9f);
-    accel_bias[0]      = cal.accel_bias.X;
-    accel_bias[1]      = cal.accel_bias.Y;
-    accel_bias[2]      = cal.accel_bias.Z;
-    accel_scale[0]     = cal.accel_scale.X;
-    accel_scale[1]     = cal.accel_scale.Y;
-    accel_scale[2]     = cal.accel_scale.Z;
-    gyro_staticbias[0] = cal.gyro_bias.X;
-    gyro_staticbias[1] = cal.gyro_bias.Y;
-    gyro_staticbias[2] = cal.gyro_bias.Z;
-    gyro_scale[0]      = cal.gyro_scale.X;
-    gyro_scale[1]      = cal.gyro_scale.Y;
-    gyro_scale[2]      = cal.gyro_scale.Z;
 
 
     AttitudeSettingsData attitudeSettings;

flight/modules/Stabilization/stabilization.c

@@ -35,6 +35,10 @@
 #include <pios_struct_helper.h>
 #include "stabilization.h"
 #include "stabilizationsettings.h"
+#include "stabilizationbank.h"
+#include "stabilizationsettingsbank1.h"
+#include "stabilizationsettingsbank2.h"
+#include "stabilizationsettingsbank3.h"
 #include "actuatordesired.h"
 #include "ratedesired.h"
 #include "relaytuning.h"
@@ -44,7 +48,9 @@
 #include "airspeedstate.h"
 #include "gyrostate.h"
 #include "flightstatus.h"
-#include "manualcontrol.h" // Just to get a macro
+#include "manualcontrolsettings.h"
+#include "manualcontrolcommand.h"
+#include "flightmodesettings.h"
 #include "taskinfo.h"
 
 // Math libraries
@@ -60,22 +66,27 @@
 #include "relay_tuning.h"
 
 // Private constants
+#define UPDATE_EXPECTED     (1.0f / 666.0f)
+#define UPDATE_MIN          1.0e-6f
+#define UPDATE_MAX          1.0f
+#define UPDATE_ALPHA        1.0e-2f
+
 #define MAX_QUEUE_SIZE      1
 
 #if defined(PIOS_STABILIZATION_STACK_SIZE)
 #define STACK_SIZE_BYTES    PIOS_STABILIZATION_STACK_SIZE
 #else
-#define STACK_SIZE_BYTES    724
+#define STACK_SIZE_BYTES    860
 #endif
 
-#define TASK_PRIORITY       (tskIDLE_PRIORITY + 4)
+#define TASK_PRIORITY       (tskIDLE_PRIORITY + 3) // FLIGHT CONTROL priority
 #define FAILSAFE_TIMEOUT_MS 30
 
 // The PID_RATE_ROLL set is used by Rate mode and the rate portion of Attitude mode
 // The PID_RATE set is used by the attitude portion of Attitude mode
-// The PID_RATEA_ROLL set is used by Rattitude mode because it needs to maintain
+enum { PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_MAX };
-// - two independant rate PIDs because it does rate and attitude simultaneously
+enum { RATE_P, RATE_I, RATE_D, RATE_LIMIT, RATE_OFFSET };
-enum { PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_RATEA_ROLL, PID_RATEA_PITCH, PID_RATEA_YAW, PID_MAX };
+enum { ATT_P, ATT_I, ATT_LIMIT, ATT_OFFSET };
 
 // Private variables
 static xTaskHandle taskHandle;
@@ -88,18 +99,28 @@ uint8_t max_axislock_rate = 0;
 float weak_leveling_kp    = 0;
 uint8_t weak_leveling_max = 0;
 bool lowThrottleZeroIntegral;
-bool lowThrottleZeroAxis[MAX_AXES];
+float vbar_decay    = 0.991f;
-float vbar_decay = 0.991f;
 struct pid pids[PID_MAX];
-static uint8_t rattitude_anti_windup;
+
+int cur_flight_mode = -1;
+
+static float rattitude_mode_transition_stick_position;
+static float cruise_control_min_thrust;
+static float cruise_control_max_thrust;
+static uint8_t cruise_control_max_angle;
+static float cruise_control_max_power_factor;
+static float cruise_control_power_trim;
+static int8_t cruise_control_inverted_power_switch;
+static float cruise_control_neutral_thrust;
+static uint8_t cruise_control_flight_mode_switch_pos_enable[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM];
 
 // Private functions
 static void stabilizationTask(void *parameters);
 static float bound(float val, float range);
 static void ZeroPids(void);
 static void SettingsUpdatedCb(UAVObjEvent *ev);
-static float stab_log2f(float x);
+static void BankUpdatedCb(UAVObjEvent *ev);
-static float stab_powf(float x, uint8_t p);
+static void SettingsBankUpdatedCb(UAVObjEvent *ev);
 
 /**
  * Module initialization
@@ -117,6 +138,13 @@ int32_t StabilizationStart()
     StabilizationSettingsConnectCallback(SettingsUpdatedCb);
     SettingsUpdatedCb(StabilizationSettingsHandle());
 
+    StabilizationBankConnectCallback(BankUpdatedCb);
+
+    StabilizationSettingsBank1ConnectCallback(SettingsBankUpdatedCb);
+    StabilizationSettingsBank2ConnectCallback(SettingsBankUpdatedCb);
+    StabilizationSettingsBank3ConnectCallback(SettingsBankUpdatedCb);
+
+
     // Start main task
     xTaskCreate(stabilizationTask, (signed char *)"Stabilization", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
     PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_STABILIZATION, taskHandle);
@@ -132,7 +160,15 @@ int32_t StabilizationStart()
 int32_t StabilizationInitialize()
 {
     // Initialize variables
+    ManualControlCommandInitialize();
+    ManualControlSettingsInitialize();
+    FlightStatusInitialize();
+    StabilizationDesiredInitialize();
     StabilizationSettingsInitialize();
+    StabilizationBankInitialize();
+    StabilizationSettingsBank1Initialize();
+    StabilizationSettingsBank2Initialize();
+    StabilizationSettingsBank3Initialize();
     ActuatorDesiredInitialize();
 #ifdef DIAG_RATEDESIRED
     RateDesiredInitialize();
@@ -156,15 +192,19 @@ MODULE_INITCALL(StabilizationInitialize, StabilizationStart);
 static void stabilizationTask(__attribute__((unused)) void *parameters)
 {
     UAVObjEvent ev;
+    PiOSDeltatimeConfig timeval;
 
-    uint32_t timeval = PIOS_DELAY_GetRaw();
+    PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
 
     ActuatorDesiredData actuatorDesired;
     StabilizationDesiredData stabDesired;
+    float throttleDesired;
     RateDesiredData rateDesired;
     AttitudeStateData attitudeState;
     GyroStateData gyroStateData;
     FlightStatusData flightStatus;
+    StabilizationBankData stabBank;
+
 
 #ifdef REVOLUTION
     AirspeedStateData airspeedState;
@@ -187,16 +227,24 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
             continue;
         }
 
-        dT = PIOS_DELAY_DiffuS(timeval) * 1.0e-6f;
+        dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
-        timeval = PIOS_DELAY_GetRaw();
-
         FlightStatusGet(&flightStatus);
         StabilizationDesiredGet(&stabDesired);
+        ManualControlCommandThrottleGet(&throttleDesired);
         AttitudeStateGet(&attitudeState);
         GyroStateGet(&gyroStateData);
+        StabilizationBankGet(&stabBank);
 #ifdef DIAG_RATEDESIRED
         RateDesiredGet(&rateDesired);
 #endif
+        uint8_t flight_mode_switch_position;
+        ManualControlCommandFlightModeSwitchPositionGet(&flight_mode_switch_position);
+
+        if (cur_flight_mode != flight_mode_switch_position) {
+            cur_flight_mode = flight_mode_switch_position;
+            SettingsBankUpdatedCb(NULL);
+        }
+
 #ifdef REVOLUTION
         float speedScaleFactor;
         // Scale PID coefficients based on current airspeed estimation - needed for fixed wing planes
@@ -294,7 +342,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
 
                 // Store to rate desired variable for storing to UAVO
                 rateDesiredAxis[i] =
-                    bound(stabDesiredAxis[i], cast_struct_to_array(settings.ManualRate, settings.ManualRate.Roll)[i]);
+                    bound(stabDesiredAxis[i], cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i]);
 
                 // Compute the inner loop
                 actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
@@ -311,7 +359,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
                 // Compute the outer loop
                 rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT);
                 rateDesiredAxis[i] = bound(rateDesiredAxis[i],
-                                           cast_struct_to_array(settings.MaximumRate, settings.MaximumRate.Roll)[i]);
+                                           cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[i]);
 
                 // Compute the inner loop
                 actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
@@ -329,14 +377,13 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
                 if (reinit) {
                     pids[PID_ROLL + i].iAccumulator = 0;
                     pids[PID_RATE_ROLL + i].iAccumulator = 0;
-                    pids[PID_RATEA_ROLL + i].iAccumulator = 0;
                 }
 
                 // Compute what Rate mode would give for this stick angle's rate
                 // Save Rate's rate in a temp for later merging with Attitude's rate
                 float rateDesiredAxisRate;
                 rateDesiredAxisRate = bound(stabDesiredAxis[i], 1.0f)
-                                      * cast_struct_to_array(settings.ManualRate, settings.ManualRate.Roll)[i];
+                                      * cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i];
 
                 // Compute what Attitude mode would give for this stick angle's rate
 
@@ -345,15 +392,15 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
                 // - subtract off the actual angle to get the angle error
                 // This is what local_error[] holds for Attitude mode
                 float attitude_error = stabDesiredAxis[i]
-                                       * cast_struct_to_array(settings.RollMax, settings.RollMax)[i]
+                                       * cast_struct_to_array(stabBank.RollMax, stabBank.RollMax)[i]
                                        - cast_struct_to_array(attitudeState.Roll, attitudeState.Roll)[i];
 
                 // Compute the outer loop just like Attitude mode does
                 float rateDesiredAxisAttitude;
                 rateDesiredAxisAttitude = pid_apply(&pids[PID_ROLL + i], attitude_error, dT);
                 rateDesiredAxisAttitude = bound(rateDesiredAxisAttitude,
-                                                cast_struct_to_array(settings.MaximumRate,
+                                                cast_struct_to_array(stabBank.ManualRate,
-                                                                     settings.MaximumRate.Roll)[i]);
+                                                                     stabBank.ManualRate.Roll)[i]);
 
                 // Compute the weighted average rate desired
                 // Using max() rather than sqrt() for cpu speed;
@@ -363,81 +410,54 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
                 // magnitude = sqrt(stabDesired.Roll*stabDesired.Roll + stabDesired.Pitch*stabDesired.Pitch);
                 float magnitude;
                 magnitude = fmaxf(fabsf(stabDesired.Roll), fabsf(stabDesired.Pitch));
+
+                // modify magnitude to move the Att to Rate transition to the place
+                // specified by the user
+                // we are looking for where the stick angle == transition angle
+                // and the Att rate equals the Rate rate
+                // that's where Rate x (1-StickAngle) [Attitude pulling down max X Ratt proportion]
+                // == Rate x StickAngle [Rate pulling up according to stick angle]
+                // * StickAngle [X Ratt proportion]
+                // so 1-x == x*x or x*x+x-1=0 where xE(0,1)
+                // (-1+-sqrt(1+4))/2 = (-1+sqrt(5))/2
+                // and quadratic formula says that is 0.618033989f
+                // I tested 14.01 and came up with .61 without even remembering this number
+                // 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 <= rattitude_mode_transition_stick_position) {
+                    magnitude *= STICK_VALUE_AT_MODE_TRANSITION / rattitude_mode_transition_stick_position;
+                } else {
+                    magnitude = (magnitude - rattitude_mode_transition_stick_position)
+                                * (1.0f - STICK_VALUE_AT_MODE_TRANSITION)
+                                / (1.0f - rattitude_mode_transition_stick_position)
+                                + STICK_VALUE_AT_MODE_TRANSITION;
+                }
                 rateDesiredAxis[i] = (1.0f - magnitude) * rateDesiredAxisAttitude
                                      + magnitude * rateDesiredAxisRate;
 
-                // Compute the inner loop for both Rate mode and Attitude mode
+                // Compute the inner loop for the averaged rate
-                // actuatorDesiredAxis[i] is the weighted average of the two PIDs from the two rates
+                // actuatorDesiredAxis[i] is the weighted average
-                actuatorDesiredAxis[i] =
+                actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor,
-                    (1.0f - magnitude) * pid_apply_setpoint(&pids[PID_RATEA_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT)
+                                                            rateDesiredAxis[i], gyro_filtered[i], dT);
-                    + magnitude * pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
                 actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
 
-                // settings.RattitudeAntiWindup controls the iAccumulator zeroing
-                // - so both iAccs don't wind up too far;
-                // - nor do both iAccs get held too close to zero at mid stick
-
-                // I suspect that there would be windup without it
-                // - since rate and attitude fight each other here
-                // - rate trying to pull it over the top and attitude trying to pull it back down
-
-                // Wind-up increases linearly with cycles for a fixed error.
-                // We must never increase the iAcc or we risk oscillation.
-
-                // Use the powf() function to make two anti-windup curves
-                // - one for zeroing rate close to center stick
-                // - the other for zeroing attitude close to max stick
-
-                // the bigger the dT      the more anti windup needed
-                // the bigger the PID[].i the more anti windup needed
-                // more anti windup is achieved with a lower powf() power
-                // a doubling of e.g. PID[].i should cause the runtime anti windup factor
-                // to get twice as far away from 1.0 (towards zero)
-                // e.g. from .90 to .80
-
-                // magic numbers
-                // these generate the inverted parabola like curves that go through [0,1] and [1,0]
-                // the higher the power, the closer the curve is to a straight line
-                // from [0,1] to [1,1] to [1,0] and the less anti windup is applied
-
-                // the UAVO RattitudeAntiWindup varies from 0 to 31
-                // 0 turns off anti windup
-                // 1 gives very little anti-windup because the power given to powf() is 31
-                // 31 gives a lot of anti-windup because the power given to powf() is 1
-                // the 32.1 is what does this
-                // the 7.966 and 17.668 cancel the default PID value and dT given to log2f()
-                // if these are non-default, tweaking is thus done so the user doesn't have to readjust
-                // the default value of 10 for UAVO RattitudeAntiWindup gives a power of 22
-                // these calculations are for magnitude = 0.5, so 22 corresponds to the number of bits
-                // used in the mantissa of the float
-                // i.e. 1.0-(0.5^22) almost underflows
-
-                // This may only be useful for aircraft with large Ki values and limits
-                if (dT > 0.0f && rattitude_anti_windup > 0.0f) {
-                    float factor;
-
-                    // At magnitudes close to one, the Attitude accumulators gets zeroed
-                    if (pids[PID_ROLL + i].i > 0.0f) {
-                        factor = 1.0f - stab_powf(magnitude, ((uint8_t)(32.1f - 7.966f - stab_log2f(dT * pids[PID_ROLL + i].i))) - rattitude_anti_windup);
-                        pids[PID_ROLL + i].iAccumulator *= factor;
-                    }
-                    if (pids[PID_RATEA_ROLL + i].i > 0.0f) {
-                        factor = 1.0f - stab_powf(magnitude, ((uint8_t)(32.1f - 17.668f - stab_log2f(dT * pids[PID_RATEA_ROLL + i].i))) - rattitude_anti_windup);
-                        pids[PID_RATEA_ROLL + i].iAccumulator *= factor;
-                    }
-
-                    // At magnitudes close to zero, the Rate accumulator gets zeroed
-                    if (pids[PID_RATE_ROLL + i].i > 0.0f) {
-                        factor = 1.0f - stab_powf(1.0f - magnitude, ((uint8_t)(32.1f - 17.668f - stab_log2f(dT * pids[PID_RATE_ROLL + i].i))) - rattitude_anti_windup);
-                        pids[PID_RATE_ROLL + i].iAccumulator *= factor;
-                    }
-                }
-
                 break;
             }
 
             case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR:
-
                 // Store for debugging output
                 rateDesiredAxis[i] = stabDesiredAxis[i];
 
@@ -486,7 +506,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
                 }
 
                 rateDesiredAxis[i]     = bound(rateDesiredAxis[i],
-                                               cast_struct_to_array(settings.ManualRate, settings.ManualRate.Roll)[i]);
+                                               cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i]);
 
                 actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
                 actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
@@ -496,7 +516,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
             case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE:
                 // Store to rate desired variable for storing to UAVO
                 rateDesiredAxis[i] = bound(stabDesiredAxis[i],
-                                           cast_struct_to_array(settings.ManualRate, settings.ManualRate.Roll)[i]);
+                                           cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i]);
 
                 // Run the relay controller which also estimates the oscillation parameters
                 stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit);
@@ -512,7 +532,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
                 // Compute the outer loop like attitude mode
                 rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT);
                 rateDesiredAxis[i] = bound(rateDesiredAxis[i],
-                                           cast_struct_to_array(settings.MaximumRate, settings.MaximumRate.Roll)[i]);
+                                           cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[i]);
 
                 // Run the relay controller which also estimates the oscillation parameters
                 stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit);
@@ -539,24 +559,66 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
 
         // Save dT
         actuatorDesired.UpdateTime = dT * 1000;
-        actuatorDesired.Throttle   = stabDesired.Throttle;
+        actuatorDesired.Thrust     = stabDesired.Thrust;
 
-        // Suppress desired output while disarmed or throttle low, for configured axis
+        // modify thrust according to 1/cos(bank angle)
-        if (flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED || stabDesired.Throttle < 0) {
+        // to maintain same altitdue with changing bank angle
-            if (lowThrottleZeroAxis[ROLL]) {
+        // but without manually adjusting thrust
-                actuatorDesired.Roll = 0.0f;
+        // do it here and all the various flight modes (e.g. Altitude Hold) can use it
+        if (flight_mode_switch_position < FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM
+            && cruise_control_flight_mode_switch_pos_enable[flight_mode_switch_position] != (uint8_t)0
+            && cruise_control_max_power_factor > 0.0001f) {
+            static uint8_t toggle;
+            static float factor;
+            float angle;
+            // get attitude state and calculate angle
+            // do it every 8th iteration to save CPU
+            if ((toggle++ & 7) == 0) {
+                // spherical right triangle
+                // 0 <= acosf() <= Pi
+                angle = RAD2DEG(acosf(cos_lookup_deg(attitudeState.Roll) * cos_lookup_deg(attitudeState.Pitch)));
+                // if past the cutoff angle (60 to 180 (180 means never))
+                if (angle > cruise_control_max_angle) {
+                    // -1 reversed collective, 0 zero power, or 1 normal power
+                    // these are all unboosted
+                    factor = cruise_control_inverted_power_switch;
+                } else {
+                    // avoid singularity
+                    if (angle > 89.999f && angle < 90.001f) {
+                        factor = cruise_control_max_power_factor;
+                    } else {
+                        factor = 1.0f / fabsf(cos_lookup_deg(angle));
+                        if (factor > cruise_control_max_power_factor) {
+                            factor = cruise_control_max_power_factor;
+                        }
+                    }
+                    // factor in the power trim, no effect at 1.0, linear effect increases with factor
+                    factor = (factor - 1.0f) * cruise_control_power_trim + 1.0f;
+                    // if inverted and they want negative boost
+                    if (angle > 90.0f && cruise_control_inverted_power_switch == (int8_t)-1) {
+                        factor = -factor;
+                        // as long as thrust is getting reversed
+                        // we may as well do pitch and yaw for a complete "invert switch"
+                        actuatorDesired.Pitch = -actuatorDesired.Pitch;
+                        actuatorDesired.Yaw   = -actuatorDesired.Yaw;
+                    }
+                }
             }
 
-            if (lowThrottleZeroAxis[PITCH]) {
+            // also don't adjust thrust if <= 0, leaves neg alone and zero thrust stops motors
-                actuatorDesired.Pitch = 0.0f;
+            if (actuatorDesired.Thrust > cruise_control_min_thrust) {
-            }
+                // quad    example factor of 2 at hover power of 40%: (0.4 - 0.0) * 2.0 + 0.0 = 0.8
-
+                // CP heli example factor of 2 at hover stick of 60%: (0.6 - 0.5) * 2.0 + 0.5 = 0.7
-            if (lowThrottleZeroAxis[YAW]) {
+                actuatorDesired.Thrust = (actuatorDesired.Thrust - cruise_control_neutral_thrust) * factor + cruise_control_neutral_thrust;
-                actuatorDesired.Yaw = 0.0f;
+                if (actuatorDesired.Thrust > cruise_control_max_thrust) {
+                    actuatorDesired.Thrust = cruise_control_max_thrust;
+                } else if (actuatorDesired.Thrust < cruise_control_min_thrust) {
+                    actuatorDesired.Thrust = cruise_control_min_thrust;
+                }
             }
         }
 
-        if (PARSE_FLIGHT_MODE(flightStatus.FlightMode) != FLIGHTMODE_MANUAL) {
+        if (flightStatus.ControlChain.Stabilization == FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
             ActuatorDesiredSet(&actuatorDesired);
         } else {
             // Force all axes to reinitialize when engaged
@@ -566,7 +628,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
         }
 
         if (flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED ||
-            (lowThrottleZeroIntegral && stabDesired.Throttle < 0)) {
+            (lowThrottleZeroIntegral && throttleDesired < 0)) {
             // Force all axes to reinitialize when engaged
             for (uint8_t i = 0; i < MAX_AXES; i++) {
                 previous_mode[i] = 255;
@@ -606,46 +668,122 @@ static void ZeroPids(void)
 static float bound(float val, float range)
 {
     if (val < -range) {
-        val = -range;
+        return -range;
     } else if (val > range) {
-        val = range;
+        return range;
     }
     return val;
 }
 
 
-// x small (0.0 < x < .01) so interpolation of fractional part is reasonable
+static void SettingsBankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
-static float stab_log2f(float x)
 {
-    union {
+    if (cur_flight_mode < 0 || cur_flight_mode >= FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM) {
-        volatile float    f;
+        return;
-        volatile uint32_t i;
+    }
-        volatile unsigned char c[4];
+    if ((ev) && ((settings.FlightModeMap[cur_flight_mode] == 0 && ev->obj != StabilizationSettingsBank1Handle()) ||
-    } __attribute__((packed)) u1, u2;
+                 (settings.FlightModeMap[cur_flight_mode] == 1 && ev->obj != StabilizationSettingsBank2Handle()) ||
-
+                 (settings.FlightModeMap[cur_flight_mode] == 2 && ev->obj != StabilizationSettingsBank3Handle()) ||
-    u2.f   = u1.f = x;
+                 settings.FlightModeMap[cur_flight_mode] > 2)) {
-    u1.i <<= 1;
+        return;
-    u2.i  &= 0xff800000;
-
-    // get and unbias the exponent, add in a linear interpolation of the fractional part
-    return (float)(u1.c[3] - 127) + (x / u2.f) - 1.0f;
-}
-
-
-// 0<=x<=1, 0<=p<=31
-static float stab_powf(float x, uint8_t p)
-{
-    float retval = 1.0f;
-
-    while (p) {
-        if (p & 1) {
-            retval *= x;
-        }
-        x  *= x;
-        p >>= 1;
     }
 
-    return retval;
+    StabilizationBankData bank;
+
+    switch (settings.FlightModeMap[cur_flight_mode]) {
+    case 0:
+        StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *)&bank);
+        break;
+
+    case 1:
+        StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *)&bank);
+        break;
+
+    case 2:
+        StabilizationSettingsBank3Get((StabilizationSettingsBank3Data *)&bank);
+        break;
+    }
+    StabilizationBankSet(&bank);
+}
+
+static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+{
+    StabilizationBankData bank;
+
+    StabilizationBankGet(&bank);
+
+// this code will be needed if any other modules alter stabilizationbank
+/*
+    StabilizationBankData curBank;
+    if(flight_mode < 0) return;
+
+    switch(cast_struct_to_array(settings.FlightModeMap, settings.FlightModeMap.Stabilized1)[flight_mode])
+    {
+    case 0:
+        StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *) &curBank);
+        if(memcmp(&curBank, &bank, sizeof(StabilizationBankDataPacked)) != 0)
+        {
+            StabilizationSettingsBank1Set((StabilizationSettingsBank1Data *) &bank);
+        }
+        break;
+
+    case 1:
+        StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *) &curBank);
+        if(memcmp(&curBank, &bank, sizeof(StabilizationBankDataPacked)) != 0)
+        {
+            StabilizationSettingsBank2Set((StabilizationSettingsBank2Data *) &bank);
+        }
+        break;
+
+    case 2:
+        StabilizationSettingsBank3Get((StabilizationSettingsBank3Data *) &curBank);
+        if(memcmp(&curBank, &bank, sizeof(StabilizationBankDataPacked)) != 0)
+        {
+            StabilizationSettingsBank3Set((StabilizationSettingsBank3Data *) &bank);
+        }
+        break;
+
+    default:
+        return; //invalid bank number
+    }
+ */
+
+
+    // Set the roll rate PID constants
+    pid_configure(&pids[PID_RATE_ROLL], bank.RollRatePID.Kp,
+                  bank.RollRatePID.Ki,
+                  bank.RollRatePID.Kd,
+                  bank.RollRatePID.ILimit);
+
+    // Set the pitch rate PID constants
+    pid_configure(&pids[PID_RATE_PITCH], bank.PitchRatePID.Kp,
+                  bank.PitchRatePID.Ki,
+                  bank.PitchRatePID.Kd,
+                  bank.PitchRatePID.ILimit);
+
+    // Set the yaw rate PID constants
+    pid_configure(&pids[PID_RATE_YAW], bank.YawRatePID.Kp,
+                  bank.YawRatePID.Ki,
+                  bank.YawRatePID.Kd,
+                  bank.YawRatePID.ILimit);
+
+    // Set the roll attitude PI constants
+    pid_configure(&pids[PID_ROLL], bank.RollPI.Kp,
+                  bank.RollPI.Ki,
+                  0,
+                  bank.RollPI.ILimit);
+
+    // Set the pitch attitude PI constants
+    pid_configure(&pids[PID_PITCH], bank.PitchPI.Kp,
+                  bank.PitchPI.Ki,
+                  0,
+                  bank.PitchPI.ILimit);
+
+    // Set the yaw attitude PI constants
+    pid_configure(&pids[PID_YAW], bank.YawPI.Kp,
+                  bank.YawPI.Ki,
+                  0,
+                  bank.YawPI.ILimit);
 }
 
 
@@ -653,69 +791,6 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 {
     StabilizationSettingsGet(&settings);
 
-    // Set the roll rate PID constants
-    pid_configure(&pids[PID_RATE_ROLL],
-                  settings.RollRatePID.Kp,
-                  settings.RollRatePID.Ki,
-                  settings.RollRatePID.Kd,
-                  settings.RollRatePID.ILimit);
-
-    // Set the pitch rate PID constants
-    pid_configure(&pids[PID_RATE_PITCH],
-                  settings.PitchRatePID.Kp,
-                  settings.PitchRatePID.Ki,
-                  settings.PitchRatePID.Kd,
-                  settings.PitchRatePID.ILimit);
-
-    // Set the yaw rate PID constants
-    pid_configure(&pids[PID_RATE_YAW],
-                  settings.YawRatePID.Kp,
-                  settings.YawRatePID.Ki,
-                  settings.YawRatePID.Kd,
-                  settings.YawRatePID.ILimit);
-
-    // Set the roll attitude PI constants
-    pid_configure(&pids[PID_ROLL],
-                  settings.RollPI.Kp,
-                  settings.RollPI.Ki,
-                  0,
-                  settings.RollPI.ILimit);
-
-    // Set the pitch attitude PI constants
-    pid_configure(&pids[PID_PITCH],
-                  settings.PitchPI.Kp,
-                  settings.PitchPI.Ki,
-                  0,
-                  settings.PitchPI.ILimit);
-
-    // Set the yaw attitude PI constants
-    pid_configure(&pids[PID_YAW],
-                  settings.YawPI.Kp,
-                  settings.YawPI.Ki,
-                  0,
-                  settings.YawPI.ILimit);
-
-    // Set the Rattitude roll rate PID constants
-    pid_configure(&pids[PID_RATEA_ROLL],
-                  settings.RollRatePID.Kp,
-                  settings.RollRatePID.Ki,
-                  settings.RollRatePID.Kd,
-                  settings.RollRatePID.ILimit);
-
-    // Set the Rattitude pitch rate PID constants
-    pid_configure(&pids[PID_RATEA_PITCH],
-                  settings.PitchRatePID.Kp,
-                  settings.PitchRatePID.Ki,
-                  settings.PitchRatePID.Kd,
-                  settings.PitchRatePID.ILimit);
-
-    // Set the Rattitude yaw rate PID constants
-    pid_configure(&pids[PID_RATEA_YAW],
-                  settings.YawRatePID.Kp,
-                  settings.YawRatePID.Ki,
-                  settings.YawRatePID.Kd,
-                  settings.YawRatePID.ILimit);
-
     // Set up the derivative term
     pid_configure_derivative(settings.DerivativeCutoff, settings.DerivativeGamma);
 
@@ -727,13 +802,8 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
     weak_leveling_kp  = settings.WeakLevelingKp;
     weak_leveling_max = settings.MaxWeakLevelingRate;
 
-    // Whether to zero the PID integrals while throttle is low
+    // Whether to zero the PID integrals while thrust is low
-    lowThrottleZeroIntegral    = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE;
+    lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE;
-
-    // Whether to suppress (zero) the StabilizationDesired output for each axis while disarmed or throttle is low
-    lowThrottleZeroAxis[ROLL]  = settings.LowThrottleZeroAxis.Roll == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE;
-    lowThrottleZeroAxis[PITCH] = settings.LowThrottleZeroAxis.Pitch == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE;
-    lowThrottleZeroAxis[YAW]   = settings.LowThrottleZeroAxis.Yaw == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE;
 
     // The dT has some jitter iteration to iteration that we don't want to
     // make thie result unpredictable.  Still, it's nicer to specify the constant
@@ -750,10 +820,29 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
     // Compute time constant for vbar decay term based on a tau
     vbar_decay = expf(-fakeDt / settings.VbarTau);
 
-    // Rattitude flight mode anti-windup factor
+    // force flight mode update
-    rattitude_anti_windup = settings.RattitudeAntiWindup;
+    cur_flight_mode = -1;
-}
 
+    // Rattitude stick angle where the attitude to rate transition happens
+    if (settings.RattitudeModeTransition < (uint8_t)10) {
+        rattitude_mode_transition_stick_position = 10.0f / 100.0f;
+    } else {
+        rattitude_mode_transition_stick_position = (float)settings.RattitudeModeTransition / 100.0f;
+    }
+
+    cruise_control_min_thrust       = (float)settings.CruiseControlMinThrust / 100.0f;
+    cruise_control_max_thrust       = (float)settings.CruiseControlMaxThrust / 100.0f;
+    cruise_control_max_angle        = settings.CruiseControlMaxAngle;
+    cruise_control_max_power_factor = settings.CruiseControlMaxPowerFactor;
+    cruise_control_power_trim       = settings.CruiseControlPowerTrim / 100.0f;
+    cruise_control_inverted_power_switch = settings.CruiseControlInvertedPowerSwitch;
+    cruise_control_neutral_thrust   = (float)settings.CruiseControlNeutralThrust / 100.0f;
+
+    memcpy(
+        cruise_control_flight_mode_switch_pos_enable,
+        settings.CruiseControlFlightModeSwitchPosEnable,
+        sizeof(cruise_control_flight_mode_switch_pos_enable));
+}
 
 /**
  * @}

flight/modules/StateEstimation/filterair.c

@@ -78,7 +78,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
         this->altitude = state->baro[0];
     }
     // calculate true airspeed estimation
-    if (IS_SET(state->updated, SENSORUPDATES_airspeed)) {
+    if (IS_SET(state->updated, SENSORUPDATES_airspeed) && (state->airspeed[1] < 0.f)) {
         state->airspeed[1] = state->airspeed[0] * IAS2TAS(this->altitude);
     }
 

flight/modules/StateEstimation/filteraltitude.c

@@ -38,22 +38,32 @@
 
 // Private constants
 
-#define STACK_REQUIRED 128
+// duration of accel bias initialization phase
+#define INITIALIZATION_DURATION_MS 5000
 
-#define DT_ALPHA       1e-3f
+#define STACK_REQUIRED             128
+
+#define DT_ALPHA                   1e-2f
+#define DT_MIN                     1e-6f
+#define DT_MAX                     1.0f
+#define DT_AVERAGE                 1e-3f
+static volatile bool reloadSettings;
 
 // Private types
 struct data {
-    float   state[4]; // state = altitude,velocity,accel_offset,accel
+    float altitudeState; // state = altitude,velocity,accel_offset,accel
-    float   pos[3]; // position updates from other filters
+    float velocityState;
-    float   vel[3]; // position updates from other filters
+    float accelBiasState;
-    float   dTA;
+    float accelState;
-    float   dTA2;
+    float pos[3]; // position updates from other filters
-    int32_t lastTime;
+    float vel[3]; // position updates from other filters
-    float   accelLast;
+
-    float   baroLast;
+    PiOSDeltatimeConfig dt1config;
-    int32_t baroLastTime;
+    PiOSDeltatimeConfig dt2config;
-    bool    first_run;
+    float accelLast;
+    float baroLast;
+    bool  first_run;
+    portTickType initTimer;
     AltitudeFilterSettingsData settings;
 };
 
@@ -63,6 +73,7 @@ struct data {
 
 static int32_t init(stateFilter *self);
 static int32_t filter(stateFilter *self, stateEstimation *state);
+static void settingsUpdatedCb(UAVObjEvent *ev);
 
 
 int32_t filterAltitudeInitialize(stateFilter *handle)
@@ -72,6 +83,8 @@ int32_t filterAltitudeInitialize(stateFilter *handle)
     handle->localdata = pvPortMalloc(sizeof(struct data));
     AttitudeStateInitialize();
     AltitudeFilterSettingsInitialize();
+    AltitudeFilterSettingsConnectCallback(&settingsUpdatedCb);
+    reloadSettings = true;
     return STACK_REQUIRED;
 }
 
@@ -79,22 +92,21 @@ static int32_t init(stateFilter *self)
 {
     struct data *this = (struct data *)self->localdata;
 
-    this->state[0]  = 0.0f;
+    this->altitudeState  = 0.0f;
-    this->state[1]  = 0.0f;
+    this->velocityState  = 0.0f;
-    this->state[2]  = 0.0f;
+    this->accelBiasState = 0.0f;
-    this->state[3]  = 0.0f;
+    this->accelState     = 0.0f;
     this->pos[0]    = 0.0f;
     this->pos[1]    = 0.0f;
     this->pos[2]    = 0.0f;
     this->vel[0]    = 0.0f;
     this->vel[1]    = 0.0f;
     this->vel[2]    = 0.0f;
-    this->dTA = -1.0f;
+    PIOS_DELTATIME_Init(&this->dt1config, DT_AVERAGE, DT_MIN, DT_MAX, DT_ALPHA);
-    this->dTA2      = -1.0f;
+    PIOS_DELTATIME_Init(&this->dt2config, DT_AVERAGE, DT_MIN, DT_MAX, DT_ALPHA);
     this->baroLast  = 0.0f;
     this->accelLast = 0.0f;
     this->first_run = 1;
-    AltitudeFilterSettingsGet(&this->settings);
     return 0;
 }
 
@@ -102,14 +114,16 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
 {
     struct data *this = (struct data *)self->localdata;
 
+    if (reloadSettings) {
+        reloadSettings = false;
+        AltitudeFilterSettingsGet(&this->settings);
+    }
+
     if (this->first_run) {
         // Initialize to current altitude reading at initial location
-        if (IS_SET(state->updated, SENSORUPDATES_accel)) {
-            this->lastTime = PIOS_DELAY_GetRaw();
-        }
         if (IS_SET(state->updated, SENSORUPDATES_baro)) {
-            this->first_run    = 0;
+            this->first_run = 0;
-            this->baroLastTime = PIOS_DELAY_GetRaw();
+            this->initTimer = xTaskGetTickCount();
         }
     } else {
         // save existing position and velocity updates so GPS will still work
@@ -117,13 +131,13 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
             this->pos[0]  = state->pos[0];
             this->pos[1]  = state->pos[1];
             this->pos[2]  = state->pos[2];
-            state->pos[2] = -this->state[0];
+            state->pos[2] = -this->altitudeState;
         }
         if (IS_SET(state->updated, SENSORUPDATES_vel)) {
             this->vel[0]  = state->vel[0];
             this->vel[1]  = state->vel[1];
             this->vel[2]  = state->vel[2];
-            state->vel[2] = -this->state[1];
+            state->vel[2] = -this->velocityState;
         }
         if (IS_SET(state->updated, SENSORUPDATES_accel)) {
             // rotate accels into global coordinate frame
@@ -134,68 +148,54 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
             float current = -(Rbe[0][2] * state->accel[0] + Rbe[1][2] * state->accel[1] + Rbe[2][2] * state->accel[2] + 9.81f);
 
             // low pass filter accelerometers
-            this->state[3] = (1.0f - this->settings.AccelLowPassKp) * this->state[3] + this->settings.AccelLowPassKp * current;
+            this->accelState = (1.0f - this->settings.AccelLowPassKp) * this->accelState + this->settings.AccelLowPassKp * current;
-
+            if (((xTaskGetTickCount() - this->initTimer) / portTICK_RATE_MS) < INITIALIZATION_DURATION_MS) {
-            // correct accel offset (low pass zeroing)
+                // allow the offset to reach quickly the target value in case of small AccelDriftKi
-            this->state[2] = (1.0f - this->settings.AccelDriftKi) * this->state[2] + this->settings.AccelDriftKi * this->state[3];
+                this->accelBiasState = (1.0f - this->settings.InitializationAccelDriftKi) * this->accelBiasState + this->settings.InitializationAccelDriftKi * this->accelState;
-
+            } else {
+                // correct accel offset (low pass zeroing)
+                this->accelBiasState = (1.0f - this->settings.AccelDriftKi) * this->accelBiasState + this->settings.AccelDriftKi * this->accelState;
+            }
             // correct velocity and position state (integration)
             // low pass for average dT, compensate timing jitter from scheduler
-            float dT = PIOS_DELAY_DiffuS(this->lastTime) / 1.0e6f;
+            //
-            this->lastTime = PIOS_DELAY_GetRaw();
+            float dT = PIOS_DELTATIME_GetAverageSeconds(&this->dt1config);
-            if (dT < 0.001f) {
+            float speedLast = this->velocityState;
-                dT = 0.001f;
-            }
-            if (this->dTA < 0) {
-                this->dTA = dT;
-            } else {
-                this->dTA = this->dTA * (1.0f - DT_ALPHA) + dT * DT_ALPHA;
-            }
-            float speedLast = this->state[1];
 
-            this->state[1] += 0.5f * (this->accelLast + (this->state[3] - this->state[2])) * this->dTA;
+            this->velocityState += 0.5f * (this->accelLast + (this->accelState - this->accelBiasState)) * dT;
-            this->accelLast = this->state[3] - this->state[2];
+            this->accelLast      = this->accelState - this->accelBiasState;
 
-            this->state[0] += 0.5f * (speedLast + this->state[1]) * this->dTA;
+            this->altitudeState += 0.5f * (speedLast + this->velocityState) * dT;
 
 
             state->pos[0]   = this->pos[0];
             state->pos[1]   = this->pos[1];
-            state->pos[2]   = -this->state[0];
+            state->pos[2]   = -this->altitudeState;
             state->updated |= SENSORUPDATES_pos;
 
             state->vel[0]   = this->vel[0];
             state->vel[1]   = this->vel[1];
-            state->vel[2]   = -this->state[1];
+            state->vel[2]   = -this->velocityState;
             state->updated |= SENSORUPDATES_vel;
         }
         if (IS_SET(state->updated, SENSORUPDATES_baro)) {
             // correct the altitude state (simple low pass)
-            this->state[0] = (1.0f - this->settings.BaroKp) * this->state[0] + this->settings.BaroKp * state->baro[0];
+            this->altitudeState = (1.0f - this->settings.BaroKp) * this->altitudeState + this->settings.BaroKp * state->baro[0];
 
             // correct the velocity state (low pass differentiation)
             // low pass for average dT, compensate timing jitter from scheduler
-            float dT = PIOS_DELAY_DiffuS(this->baroLastTime) / 1.0e6f;
+            float dT = PIOS_DELTATIME_GetAverageSeconds(&this->dt2config);
-            this->baroLastTime = PIOS_DELAY_GetRaw();
+            this->velocityState = (1.0f - (this->settings.BaroKp * this->settings.BaroKp)) * this->velocityState + (this->settings.BaroKp * this->settings.BaroKp) * (state->baro[0] - this->baroLast) / dT;
-            if (dT < 0.001f) {
-                dT = 0.001f;
-            }
-            if (this->dTA2 < 0) {
-                this->dTA2 = dT;
-            } else {
-                this->dTA2 = this->dTA2 * (1.0f - DT_ALPHA) + dT * DT_ALPHA;
-            }
-            this->state[1]  = (1.0f - (this->settings.BaroKp * this->settings.BaroKp)) * this->state[1] + (this->settings.BaroKp * this->settings.BaroKp) * (state->baro[0] - this->baroLast) / this->dTA2;
             this->baroLast  = state->baro[0];
 
             state->pos[0]   = this->pos[0];
             state->pos[1]   = this->pos[1];
-            state->pos[2]   = -this->state[0];
+            state->pos[2]   = -this->altitudeState;
             state->updated |= SENSORUPDATES_pos;
 
             state->vel[0]   = this->vel[0];
             state->vel[1]   = this->vel[1];
-            state->vel[2]   = -this->state[1];
+            state->vel[2]   = -this->velocityState;
             state->updated |= SENSORUPDATES_vel;
         }
     }
@@ -203,6 +203,12 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
     return 0;
 }
 
+void settingsUpdatedCb(UAVObjEvent *ev)
+{
+    if (ev->obj == AltitudeFilterSettingsHandle()) {
+        reloadSettings = true;
+    }
+}
 
 /**
  * @}

flight/modules/StateEstimation/filterbaro.c

@@ -36,38 +36,68 @@
 
 // Private constants
 
-#define STACK_REQUIRED 64
+#define STACK_REQUIRED 128
+#define INIT_CYCLES    100
 
 // Private types
 struct data {
     float   baroOffset;
     float   baroGPSOffsetCorrectionAlpha;
     float   baroAlt;
+    float   gpsAlt;
     int16_t first_run;
+    bool    useGPS;
 };
 
 // Private variables
 
 // Private functions
 
-static int32_t init(stateFilter *self);
+static int32_t initwithgps(stateFilter *self);
+static int32_t initwithoutgps(stateFilter *self);
+static int32_t maininit(stateFilter *self);
 static int32_t filter(stateFilter *self, stateEstimation *state);
 
 
 int32_t filterBaroInitialize(stateFilter *handle)
 {
-    handle->init      = &init;
+    handle->init      = &initwithgps;
     handle->filter    = &filter;
     handle->localdata = pvPortMalloc(sizeof(struct data));
     return STACK_REQUIRED;
 }
 
-static int32_t init(stateFilter *self)
+int32_t filterBaroiInitialize(stateFilter *handle)
+{
+    handle->init      = &initwithoutgps;
+    handle->filter    = &filter;
+    handle->localdata = pvPortMalloc(sizeof(struct data));
+    return STACK_REQUIRED;
+}
+
+static int32_t initwithgps(stateFilter *self)
+{
+    struct data *this = (struct data *)self->localdata;
+
+    this->useGPS = 1;
+    return maininit(self);
+}
+
+static int32_t initwithoutgps(stateFilter *self)
+{
+    struct data *this = (struct data *)self->localdata;
+
+    this->useGPS = 0;
+    return maininit(self);
+}
+
+static int32_t maininit(stateFilter *self)
 {
     struct data *this = (struct data *)self->localdata;
 
     this->baroOffset = 0.0f;
-    this->first_run  = 100;
+    this->gpsAlt     = 0.0f;
+    this->first_run  = INIT_CYCLES;
 
     RevoSettingsInitialize();
     RevoSettingsBaroGPSOffsetCorrectionAlphaGet(&this->baroGPSOffsetCorrectionAlpha);
@@ -80,17 +110,29 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
     struct data *this = (struct data *)self->localdata;
 
     if (this->first_run) {
+        // Make sure initial location is initialized properly before continuing
+        if (this->useGPS && IS_SET(state->updated, SENSORUPDATES_pos)) {
+            if (this->first_run == INIT_CYCLES) {
+                this->gpsAlt = state->pos[2];
+                this->first_run--;
+            }
+        }
         // Initialize to current altitude reading at initial location
         if (IS_SET(state->updated, SENSORUPDATES_baro)) {
-            this->baroOffset = (100.f - this->first_run) / 100.f * this->baroOffset + (this->first_run / 100.f) * state->baro[0];
+            if (this->first_run < INIT_CYCLES || !this->useGPS) {
-            this->baroAlt    = this->baroOffset;
+                this->baroOffset = (((float)(INIT_CYCLES)-this->first_run) / (float)(INIT_CYCLES)) * this->baroOffset + (this->first_run / (float)(INIT_CYCLES)) * (state->baro[0] + this->gpsAlt);
-            this->first_run--;
+                this->baroAlt    = state->baro[0];
+                this->first_run--;
+            }
             UNSET_MASK(state->updated, SENSORUPDATES_baro);
         }
+        // make sure we raise an error until properly initialized - would not be good if people arm and
+        // use altitudehold without initialized barometer filter
+        return 2;
     } else {
         // Track barometric altitude offset with a low pass filter
         // based on GPS altitude if available
-        if (IS_SET(state->updated, SENSORUPDATES_pos)) {
+        if (this->useGPS && IS_SET(state->updated, SENSORUPDATES_pos)) {
             this->baroOffset = this->baroOffset * this->baroGPSOffsetCorrectionAlpha +
                                (1.0f - this->baroGPSOffsetCorrectionAlpha) * (this->baroAlt + state->pos[2]);
         }
@@ -99,9 +141,8 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
             this->baroAlt   = state->baro[0];
             state->baro[0] -= this->baroOffset;
         }
+        return 0;
     }
-
-    return 0;
 }
 
 

flight/modules/StateEstimation/filterekf.c

@@ -46,6 +46,8 @@
 
 #define STACK_REQUIRED 2048
 #define DT_ALPHA       1e-3f
+#define DT_MIN         1e-6f
+#define DT_MAX         1.0f
 #define DT_INIT        (1.0f / 666.0f) // initialize with 666 Hz (default sensor update rate on revo)
 
 #define IMPORT_SENSOR_IF_UPDATED(shortname, num) \
@@ -67,10 +69,9 @@ struct data {
 
     stateEstimation work;
 
-    uint32_t ins_last_time;
+    bool inited;
-    bool     inited;
 
-    float    dTa;
+    PiOSDeltatimeConfig dtconfig;
 };
 
 // Private variables
@@ -155,11 +156,10 @@ static int32_t maininit(stateFilter *self)
 {
     struct data *this = (struct data *)self->localdata;
 
-    this->inited        = false;
+    this->inited       = false;
-    this->init_stage    = 0;
+    this->init_stage   = 0;
-    this->work.updated  = 0;
+    this->work.updated = 0;
-    this->ins_last_time = PIOS_DELAY_GetRaw();
+    PIOS_DELTATIME_Init(&this->dtconfig, DT_INIT, DT_MIN, DT_MAX, DT_ALPHA);
-    this->dTa = DT_INIT;
 
     EKFConfigurationGet(&this->ekfConfiguration);
     int t;
@@ -225,17 +225,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
         return 0;
     }
 
-    dT = PIOS_DELAY_DiffuS(this->ins_last_time) / 1.0e6f;
+    dT = PIOS_DELTATIME_GetAverageSeconds(&this->dtconfig);
-    this->ins_last_time = PIOS_DELAY_GetRaw();
-
-    // This should only happen at start up or at mode switches
-    if (dT > 0.01f) {
-        dT = 0.01f;
-    } else if (dT <= 0.001f) {
-        dT = 0.001f;
-    }
-
-    this->dTa = this->dTa * (1.0f - DT_ALPHA) + dT * DT_ALPHA; // low pass for average dT, compensate timing jitter from scheduler
 
     if (!this->inited && IS_SET(this->work.updated, SENSORUPDATES_mag) && IS_SET(this->work.updated, SENSORUPDATES_baro) && IS_SET(this->work.updated, SENSORUPDATES_pos)) {
         // Don't initialize until all sensors are read
@@ -301,7 +291,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
             // Run prediction a bit before any corrections
 
             float gyros[3] = { DEG2RAD(this->work.gyro[0]), DEG2RAD(this->work.gyro[1]), DEG2RAD(this->work.gyro[2]) };
-            INSStatePrediction(gyros, this->work.accel, this->dTa);
+            INSStatePrediction(gyros, this->work.accel, dT);
 
             // Copy the attitude into the state
             // NOTE: updating gyr correctly is valid, because this code is reached only when SENSORUPDATES_gyro is already true
@@ -336,7 +326,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
     float gyros[3] = { DEG2RAD(this->work.gyro[0]), DEG2RAD(this->work.gyro[1]), DEG2RAD(this->work.gyro[2]) };
 
     // Advance the state estimate
-    INSStatePrediction(gyros, this->work.accel, this->dTa);
+    INSStatePrediction(gyros, this->work.accel, dT);
 
     // Copy the attitude into the state
     // NOTE: updating gyr correctly is valid, because this code is reached only when SENSORUPDATES_gyro is already true
@@ -356,7 +346,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
     state->updated |= SENSORUPDATES_attitude | SENSORUPDATES_pos | SENSORUPDATES_vel;
 
     // Advance the covariance estimate
-    INSCovariancePrediction(this->dTa);
+    INSCovariancePrediction(dT);
 
     if (IS_SET(this->work.updated, SENSORUPDATES_mag)) {
         SystemAlarmsAlarmData alarms;

flight/modules/StateEstimation/filtermag.c

@@ -152,7 +152,7 @@ static void checkMagValidity(struct data *this, float mag[3])
             }
         } else {
             if (this->errorcount > ALARM_THRESHOLD) {
-                AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_ERROR);
+                AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_CRITICAL);
             } else {
                 this->errorcount++;
             }

flight/modules/StateEstimation/inc/stateestimation.h

@@ -64,6 +64,7 @@ typedef struct stateFilterStruct {
 
 
 int32_t filterMagInitialize(stateFilter *handle);
+int32_t filterBaroiInitialize(stateFilter *handle);
 int32_t filterBaroInitialize(stateFilter *handle);
 int32_t filterAltitudeInitialize(stateFilter *handle);
 int32_t filterAirInitialize(stateFilter *handle);

flight/modules/StateEstimation/stateestimation.c

@@ -31,6 +31,8 @@
 
 #include "inc/stateestimation.h"
 
+#include <callbackinfo.h>
+
 #include <gyrosensor.h>
 #include <accelsensor.h>
 #include <magsensor.h>
@@ -38,6 +40,7 @@
 #include <airspeedsensor.h>
 #include <gpspositionsensor.h>
 #include <gpsvelocitysensor.h>
+#include <homelocation.h>
 
 #include <gyrostate.h>
 #include <accelstate.h>
@@ -53,10 +56,14 @@
 #include "CoordinateConversions.h"
 
 // Private constants
-#define STACK_SIZE_BYTES  256
+#define STACK_SIZE_BYTES        256
-#define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR
+#define CALLBACK_PRIORITY       CALLBACK_PRIORITY_REGULAR
-#define TASK_PRIORITY     CALLBACK_TASK_FLIGHTCONTROL
+#define TASK_PRIORITY           CALLBACK_TASK_FLIGHTCONTROL
-#define TIMEOUT_MS        10
+#define TIMEOUT_MS              10
+
+// Private filter init const
+#define FILTER_INIT_FORCE       -1
+#define FILTER_INIT_IF_POSSIBLE -2
 
 // local macros, ONLY to be used in the middle of StateEstimationCb in section RUNSTATE_LOAD after the update of states updated!
 #define FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(sensorname, shortname, a1, a2, a3) \
@@ -72,6 +79,7 @@
             UNSET_MASK(states.updated, SENSORUPDATES_##shortname); \
         } \
     }
+
 #define FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(sensorname, shortname, a1, EXTRACHECK) \
     if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \
         sensorname##Data s; \
@@ -84,6 +92,19 @@
         } \
     }
 
+#define FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_2_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(sensorname, shortname, a1, a2, EXTRACHECK) \
+    if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \
+        sensorname##Data s; \
+        sensorname##Get(&s); \
+        if (IS_REAL(s.a1) && IS_REAL(s.a2) && EXTRACHECK) { \
+            states.shortname[0] = s.a1; \
+            states.shortname[1] = s.a2; \
+        } \
+        else { \
+            UNSET_MASK(states.updated, SENSORUPDATES_##shortname); \
+        } \
+    }
+
 // local macros, ONLY to be used in the middle of StateEstimationCb in section RUNSTATE_SAVE before the check of alarms!
 #define EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(statename, shortname, a1, a2, a3) \
     if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \
@@ -94,6 +115,7 @@
         s.a3 = states.shortname[2]; \
         statename##Set(&s); \
     }
+
 #define EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_2_DIMENSIONS(statename, shortname, a1, a2) \
     if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \
         statename##Data s; \
@@ -103,8 +125,8 @@
         statename##Set(&s); \
     }
 
-// Private types
 
+// Private types
 struct filterPipelineStruct;
 
 typedef const struct filterPipelineStruct {
@@ -117,12 +139,13 @@ static DelayedCallbackInfo *stateEstimationCallback;
 
 static volatile RevoSettingsData revoSettings;
 static volatile sensorUpdates updatedSensors;
-static int32_t fusionAlgorithm     = -1;
+static volatile int32_t fusionAlgorithm  = -1;
-static filterPipeline *filterChain = NULL;
+static const filterPipeline *filterChain = NULL;
 
 // different filters available to state estimation
 static stateFilter magFilter;
 static stateFilter baroFilter;
+static stateFilter baroiFilter;
 static stateFilter altitudeFilter;
 static stateFilter airFilter;
 static stateFilter stationaryFilter;
@@ -133,20 +156,33 @@ static stateFilter ekf13iFilter;
 static stateFilter ekf13Filter;
 
 // preconfigured filter chains selectable via revoSettings.FusionAlgorithm
-static filterPipeline *cfQueue = &(filterPipeline) {
+static const filterPipeline *cfQueue = &(filterPipeline) {
     .filter = &magFilter,
     .next   = &(filterPipeline) {
         .filter = &airFilter,
         .next   = &(filterPipeline) {
-            .filter = &llaFilter,
+            .filter = &baroiFilter,
             .next   = &(filterPipeline) {
-                .filter = &baroFilter,
+                .filter = &altitudeFilter,
                 .next   = &(filterPipeline) {
-                    .filter = &altitudeFilter,
+                    .filter = &cfFilter,
-                    .next   = &(filterPipeline) {
+                    .next   = NULL,
-                        .filter = &cfFilter,
+                }
-                        .next   = NULL,
+            }
-                    }
+        }
+    }
+};
+static const filterPipeline *cfmiQueue = &(filterPipeline) {
+    .filter = &magFilter,
+    .next   = &(filterPipeline) {
+        .filter = &airFilter,
+        .next   = &(filterPipeline) {
+            .filter = &baroiFilter,
+            .next   = &(filterPipeline) {
+                .filter = &altitudeFilter,
+                .next   = &(filterPipeline) {
+                    .filter = &cfmFilter,
+                    .next   = NULL,
                 }
             }
         }
@@ -176,15 +212,12 @@ static const filterPipeline *ekf13iQueue = &(filterPipeline) {
     .next   = &(filterPipeline) {
         .filter = &airFilter,
         .next   = &(filterPipeline) {
-            .filter = &llaFilter,
+            .filter = &baroiFilter,
             .next   = &(filterPipeline) {
-                .filter = &baroFilter,
+                .filter = &stationaryFilter,
                 .next   = &(filterPipeline) {
-                    .filter = &stationaryFilter,
+                    .filter = &ekf13iFilter,
-                    .next   = &(filterPipeline) {
+                    .next   = NULL,
-                        .filter = &ekf13iFilter,
-                        .next   = NULL,
-                    }
                 }
             }
         }
@@ -211,6 +244,7 @@ static const filterPipeline *ekf13Queue = &(filterPipeline) {
 
 static void settingsUpdatedCb(UAVObjEvent *objEv);
 static void sensorUpdatedCb(UAVObjEvent *objEv);
+static void homeLocationUpdatedCb(UAVObjEvent *objEv);
 static void StateEstimationCb(void);
 
 static inline int32_t maxint32_t(int32_t a, int32_t b)
@@ -236,6 +270,8 @@ int32_t StateEstimationInitialize(void)
     GPSVelocitySensorInitialize();
     GPSPositionSensorInitialize();
 
+    HomeLocationInitialize();
+
     GyroStateInitialize();
     AccelStateInitialize();
     MagStateInitialize();
@@ -245,6 +281,8 @@ int32_t StateEstimationInitialize(void)
 
     RevoSettingsConnectCallback(&settingsUpdatedCb);
 
+    HomeLocationConnectCallback(&homeLocationUpdatedCb);
+
     GyroSensorConnectCallback(&sensorUpdatedCb);
     AccelSensorConnectCallback(&sensorUpdatedCb);
     MagSensorConnectCallback(&sensorUpdatedCb);
@@ -256,6 +294,7 @@ int32_t StateEstimationInitialize(void)
     uint32_t stack_required = STACK_SIZE_BYTES;
     // Initialize Filters
     stack_required = maxint32_t(stack_required, filterMagInitialize(&magFilter));
+    stack_required = maxint32_t(stack_required, filterBaroiInitialize(&baroiFilter));
     stack_required = maxint32_t(stack_required, filterBaroInitialize(&baroFilter));
     stack_required = maxint32_t(stack_required, filterAltitudeInitialize(&altitudeFilter));
     stack_required = maxint32_t(stack_required, filterAirInitialize(&airFilter));
@@ -266,7 +305,7 @@ int32_t StateEstimationInitialize(void)
     stack_required = maxint32_t(stack_required, filterEKF13iInitialize(&ekf13iFilter));
     stack_required = maxint32_t(stack_required, filterEKF13Initialize(&ekf13Filter));
 
-    stateEstimationCallback = DelayedCallbackCreate(&StateEstimationCb, CALLBACK_PRIORITY, TASK_PRIORITY, stack_required);
+    stateEstimationCallback = PIOS_CALLBACKSCHEDULER_Create(&StateEstimationCb, CALLBACK_PRIORITY, TASK_PRIORITY, CALLBACKINFO_RUNNING_STATEESTIMATION, stack_required);
 
     return 0;
 }
@@ -297,7 +336,7 @@ static void StateEstimationCb(void)
     static int8_t alarm     = 0;
     static int8_t lastAlarm = -1;
     static uint16_t alarmcounter = 0;
-    static filterPipeline *current;
+    static const filterPipeline *current;
     static stateEstimation states;
     static uint32_t last_time;
     static uint16_t bootDelay = 64;
@@ -305,7 +344,7 @@ static void StateEstimationCb(void)
     // after system startup, first few sensor readings might be messed up, delay until everything has settled
     if (bootDelay) {
         bootDelay--;
-        DelayedCallbackSchedule(stateEstimationCallback, TIMEOUT_MS, CALLBACK_UPDATEMODE_SOONER);
+        PIOS_CALLBACKSCHEDULER_Schedule(stateEstimationCallback, TIMEOUT_MS, CALLBACK_UPDATEMODE_SOONER);
         return;
     }
 
@@ -327,26 +366,29 @@ static void StateEstimationCb(void)
         if (fusionAlgorithm != revoSettings.FusionAlgorithm) {
             FlightStatusData fs;
             FlightStatusGet(&fs);
-            if (fs.Armed == FLIGHTSTATUS_ARMED_DISARMED || fusionAlgorithm == -1) {
+            if (fs.Armed == FLIGHTSTATUS_ARMED_DISARMED || fusionAlgorithm == FILTER_INIT_FORCE) {
                 const filterPipeline *newFilterChain;
                 switch (revoSettings.FusionAlgorithm) {
                 case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY:
                     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_INS13OUTDOOR:
+                case REVOSETTINGS_FUSIONALGORITHM_INS13GPSOUTDOOR:
                     newFilterChain = ekf13Queue;
                     break;
                 default:
                     newFilterChain = NULL;
                 }
                 // initialize filters in chain
-                current = (filterPipeline *)newFilterChain;
+                current = newFilterChain;
                 bool error = 0;
                 while (current != NULL) {
                     int32_t result = current->filter->init((stateFilter *)current->filter);
@@ -361,7 +403,7 @@ static void StateEstimationCb(void)
                     return;
                 } else {
                     // set new fusion algortithm
-                    filterChain     = (filterPipeline *)newFilterChain;
+                    filterChain     = newFilterChain;
                     fusionAlgorithm = revoSettings.FusionAlgorithm;
                 }
             }
@@ -377,18 +419,18 @@ static void StateEstimationCb(void)
         FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(MagSensor, mag, 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_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(AirspeedSensor, airspeed, CalibratedAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_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);
-        states.airspeed[1] = 0.0f; // sensor does not provide true airspeed, needs to be calculated by filter, set to zero for now
+
         // 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  = (filterPipeline *)filterChain;
+        current  = filterChain;
 
         // we are not done, re-dispatch self execution
         runState = RUNSTATE_FILTER;
-        DelayedCallbackDispatch(stateEstimationCallback);
+        PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback);
         break;
 
     case RUNSTATE_FILTER:
@@ -405,7 +447,7 @@ static void StateEstimationCb(void)
         if (!current) {
             runState = RUNSTATE_SAVE;
         }
-        DelayedCallbackDispatch(stateEstimationCallback);
+        PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback);
         break;
 
     case RUNSTATE_SAVE:
@@ -458,9 +500,9 @@ static void StateEstimationCb(void)
         // we are done, re-schedule next self execution
         runState = RUNSTATE_LOAD;
         if (updatedSensors) {
-            DelayedCallbackDispatch(stateEstimationCallback);
+            PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback);
         } else {
-            DelayedCallbackSchedule(stateEstimationCallback, TIMEOUT_MS, CALLBACK_UPDATEMODE_SOONER);
+            PIOS_CALLBACKSCHEDULER_Schedule(stateEstimationCallback, TIMEOUT_MS, CALLBACK_UPDATEMODE_SOONER);
         }
         break;
     }
@@ -475,6 +517,17 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
     RevoSettingsGet((RevoSettingsData *)&revoSettings);
 }
 
+/**
+ * Callback for eventdispatcher when HomeLocation has been updated
+ */
+static void homeLocationUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+{
+    // Ask for a filter init (necessary for LLA filter)
+    // Only possible if disarmed
+    fusionAlgorithm = FILTER_INIT_IF_POSSIBLE;
+}
+
+
 /**
  * Callback for eventdispatcher when any sensor UAVObject has been updated
  * updates the list of "recently updated UAVObjects" and dispatches the state estimator callback
@@ -513,7 +566,7 @@ static void sensorUpdatedCb(UAVObjEvent *ev)
         updatedSensors |= SENSORUPDATES_airspeed;
     }
 
-    DelayedCallbackDispatch(stateEstimationCallback);
+    PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback);
 }
 
 

flight/modules/System/systemmod.c

@@ -52,7 +52,7 @@
 #include <i2cstats.h>
 #include <taskinfo.h>
 #include <watchdogstatus.h>
-#include <taskinfo.h>
+#include <callbackinfo.h>
 #include <hwsettings.h>
 #include <pios_flashfs.h>
 #if defined(PIOS_INCLUDE_RFM22B)
@@ -92,11 +92,9 @@
 // Private types
 
 // Private variables
-static uint32_t idleCounter;
-static uint32_t idleCounterClear;
 static xTaskHandle systemTaskHandle;
 static xQueueHandle objectPersistenceQueue;
-static bool stackOverflow;
+static enum { STACKOVERFLOW_NONE = 0, STACKOVERFLOW_WARNING = 1, STACKOVERFLOW_CRITICAL = 3 } stackOverflow;
 static bool mallocFailed;
 static HwSettingsData bootHwSettings;
 static struct PIOS_FLASHFS_Stats fsStats;
@@ -105,6 +103,7 @@ static void objectUpdatedCb(UAVObjEvent *ev);
 static void hwSettingsUpdatedCb(UAVObjEvent *ev);
 #ifdef DIAG_TASKS
 static void taskMonitorForEachCallback(uint16_t task_id, const struct pios_task_info *task_info, void *context);
+static void callbackSchedulerForEachCallback(int16_t callback_id, const struct pios_callback_info *callback_info, void *context);
 #endif
 static void updateStats();
 static void updateSystemAlarms();
@@ -124,13 +123,14 @@ extern uintptr_t pios_user_fs_id;
 int32_t SystemModStart(void)
 {
     // Initialize vars
-    stackOverflow = false;
+    stackOverflow = STACKOVERFLOW_NONE;
     mallocFailed  = false;
     // Create system task
     xTaskCreate(systemTask, (signed char *)"System", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &systemTaskHandle);
     // Register task
     PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_SYSTEM, systemTaskHandle);
 
+
     return 0;
 }
 
@@ -147,6 +147,7 @@ int32_t SystemModInitialize(void)
     ObjectPersistenceInitialize();
 #ifdef DIAG_TASKS
     TaskInfoInitialize();
+    CallbackInfoInitialize();
 #endif
 #ifdef DIAG_I2C_WDG_STATS
     I2CStatsInitialize();
@@ -175,7 +176,7 @@ static void systemTask(__attribute__((unused)) void *parameters)
     MODULE_TASKCREATE_ALL;
 
     /* start the delayed callback scheduler */
-    CallbackSchedulerStart();
+    PIOS_CALLBACKSCHEDULER_Start();
 
     if (mallocFailed) {
         /* We failed to malloc during task creation,
@@ -184,15 +185,12 @@ static void systemTask(__attribute__((unused)) void *parameters)
          */
         PIOS_SYS_Reset();
     }
-
 #if defined(PIOS_INCLUDE_IAP)
     /* Record a successful boot */
     PIOS_IAP_WriteBootCount(0);
 #endif
 
     // Initialize vars
-    idleCounter = 0;
-    idleCounterClear = 0;
 
     // Listen for SettingPersistance object updates, connect a callback function
     ObjectPersistenceConnectQueue(objectPersistenceQueue);
@@ -204,14 +202,16 @@ static void systemTask(__attribute__((unused)) void *parameters)
 
 #ifdef DIAG_TASKS
     TaskInfoData taskInfoData;
+    CallbackInfoData callbackInfoData;
 #endif
 
     // Main system loop
     while (1) {
         // Update the system statistics
-        updateStats();
-        cycleCount = cycleCount > 0 ? cycleCount - 1 : 7;
 
+        cycleCount = cycleCount > 0 ? cycleCount - 1 : 7;
+// if(cycleCount == 1){
+        updateStats();
         // Update the system alarms
         updateSystemAlarms();
 #ifdef DIAG_I2C_WDG_STATS
@@ -223,8 +223,13 @@ static void systemTask(__attribute__((unused)) void *parameters)
         // Update the task status object
         PIOS_TASK_MONITOR_ForEachTask(taskMonitorForEachCallback, &taskInfoData);
         TaskInfoSet(&taskInfoData);
+        // Update the callback status object
+// if(FALSE){
+        PIOS_CALLBACKSCHEDULER_ForEachCallback(callbackSchedulerForEachCallback, &callbackInfoData);
+        CallbackInfoSet(&callbackInfoData);
+// }
 #endif
-
+// }
         // Flash the heartbeat LED
 #if defined(PIOS_LED_HEARTBEAT)
         uint8_t armingStatus;
@@ -440,7 +445,26 @@ static void taskMonitorForEachCallback(uint16_t task_id, const struct pios_task_
     ((uint16_t *)&taskData->StackRemaining)[task_id] = task_info->stack_remaining;
     ((uint8_t *)&taskData->RunningTime)[task_id]     = task_info->running_time_percentage;
 }
-#endif
+
+static void callbackSchedulerForEachCallback(int16_t callback_id, const struct pios_callback_info *callback_info, void *context)
+{
+    CallbackInfoData *callbackData = (CallbackInfoData *)context;
+
+    if (callback_id < 0) {
+        return;
+    }
+    // delayed callback scheduler reports callback stack overflows as remaininng: -1
+    if (callback_info->stack_remaining < 0 && stackOverflow == STACKOVERFLOW_NONE) {
+        stackOverflow = STACKOVERFLOW_WARNING;
+    }
+    // By convention, there is a direct mapping between (not negative) callback scheduler callback_id's and members
+    // of the CallbackInfoXXXXElem enums
+    PIOS_DEBUG_Assert(callback_id < CALLBACKINFO_RUNNING_NUMELEM);
+    ((uint8_t *)&callbackData->Running)[callback_id] = callback_info->is_running;
+    ((uint32_t *)&callbackData->RunningTime)[callback_id]   = callback_info->running_time_count;
+    ((int16_t *)&callbackData->StackRemaining)[callback_id] = callback_info->stack_remaining;
+}
+#endif /* ifdef DIAG_TASKS */
 
 /**
  * Called periodically to update the I2C statistics
@@ -516,26 +540,23 @@ static uint16_t GetFreeIrqStackSize(void)
  */
 static void updateStats()
 {
-    static portTickType lastTickCount = 0;
     SystemStatsData stats;
 
     // Get stats and update
     SystemStatsGet(&stats);
-    stats.FlightTime    = xTaskGetTickCount() * portTICK_RATE_MS;
+    stats.FlightTime = xTaskGetTickCount() * portTICK_RATE_MS;
 #if defined(ARCH_POSIX) || defined(ARCH_WIN32)
     // POSIX port of FreeRTOS doesn't have xPortGetFreeHeapSize()
+    stats.SystemModStackRemaining = 128;
     stats.HeapRemaining = 10240;
 #else
     stats.HeapRemaining = xPortGetFreeHeapSize();
+    stats.SystemModStackRemaining = uxTaskGetStackHighWaterMark(NULL) * 4;
 #endif
 
     // Get Irq stack status
     stats.IRQStackRemaining = GetFreeIrqStackSize();
 
-    // When idleCounterClear was not reset by the idle-task, it means the idle-task did not run
-    if (idleCounterClear) {
-        idleCounter = 0;
-    }
 #if !defined(ARCH_POSIX) && !defined(ARCH_WIN32)
     if (pios_uavo_settings_fs_id) {
         PIOS_FLASHFS_GetStats(pios_uavo_settings_fs_id, &fsStats);
@@ -548,16 +569,11 @@ static void updateStats()
         stats.UsrSlotsActive = fsStats.num_active_slots;
     }
 #endif
-    portTickType now = xTaskGetTickCount();
+    stats.CPULoad = 100 - PIOS_TASK_MONITOR_GetIdlePercentage();
-    if (now > lastTickCount) {
+
-        uint32_t dT = (xTaskGetTickCount() - lastTickCount) * portTICK_RATE_MS; // in ms
-        stats.CPULoad = 100 - (uint8_t)roundf(100.0f * ((float)idleCounter / ((float)dT / 1000.0f)) / (float)IDLE_COUNTS_PER_SEC_AT_NO_LOAD);
-    } // else: TickCount has wrapped, do not calc now
-    lastTickCount    = now;
-    idleCounterClear = 1;
 #if defined(PIOS_INCLUDE_ADC) && defined(PIOS_ADC_USE_TEMP_SENSOR)
     float temp_voltage = PIOS_ADC_PinGetVolt(PIOS_ADC_TEMPERATURE_PIN);
-    stats.CPUTemp    = PIOS_CONVERT_VOLT_TO_CPU_TEMP(temp_voltage);;
+    stats.CPUTemp = PIOS_CONVERT_VOLT_TO_CPU_TEMP(temp_voltage);;
 #endif
     SystemStatsSet(&stats);
 }
@@ -600,10 +616,15 @@ static void updateSystemAlarms()
     }
 
     // Check for stack overflow
-    if (stackOverflow) {
+    switch (stackOverflow) {
-        AlarmsSet(SYSTEMALARMS_ALARM_STACKOVERFLOW, SYSTEMALARMS_ALARM_CRITICAL);
+    case STACKOVERFLOW_NONE:
-    } else {
         AlarmsClear(SYSTEMALARMS_ALARM_STACKOVERFLOW);
+        break;
+    case STACKOVERFLOW_WARNING:
+        AlarmsSet(SYSTEMALARMS_ALARM_STACKOVERFLOW, SYSTEMALARMS_ALARM_WARNING);
+        break;
+    default:
+        AlarmsSet(SYSTEMALARMS_ALARM_STACKOVERFLOW, SYSTEMALARMS_ALARM_CRITICAL);
     }
 
     // Check for event errors
@@ -631,15 +652,7 @@ static void updateSystemAlarms()
  * Called by the RTOS when the CPU is idle, used to measure the CPU idle time.
  */
 void vApplicationIdleHook(void)
-{
+{}
-    // Called when the scheduler has no tasks to run
-    if (idleCounterClear == 0) {
-        ++idleCounter;
-    } else {
-        idleCounter = 0;
-        idleCounterClear = 0;
-    }
-}
 
 /**
  * Called by the RTOS when a stack overflow is detected.
@@ -648,7 +661,7 @@ void vApplicationIdleHook(void)
 void vApplicationStackOverflowHook(__attribute__((unused)) xTaskHandle *pxTask,
                                    __attribute__((unused)) signed portCHAR *pcTaskName)
 {
-    stackOverflow = true;
+    stackOverflow = STACKOVERFLOW_CRITICAL;
 #if DEBUG_STACK_OVERFLOW
     static volatile bool wait_here = true;
     while (wait_here) {

flight/modules/Telemetry/telemetry.c

@@ -40,27 +40,41 @@
 #include "taskinfo.h"
 
 // Private constants
-#define MAX_QUEUE_SIZE         TELEM_QUEUE_SIZE
+#define MAX_QUEUE_SIZE            TELEM_QUEUE_SIZE
-#define STACK_SIZE_BYTES       PIOS_TELEM_STACK_SIZE
+// Three different stack size parameter are accepted for Telemetry(RX PIOS_TELEM_RX_STACK_SIZE)
-#define TASK_PRIORITY_RX       (tskIDLE_PRIORITY + 2)
+// Tx(PIOS_TELEM_TX_STACK_SIZE) and Radio RX(PIOS_TELEM_RADIO_RX_STACK_SIZE)
-#define TASK_PRIORITY_TX       (tskIDLE_PRIORITY + 2)
+#ifdef PIOS_TELEM_RX_STACK_SIZE
-#define TASK_PRIORITY_RADRX    (tskIDLE_PRIORITY + 2)
+#define STACK_SIZE_RX_BYTES       PIOS_TELEM_RX_STACK_SIZE
-#define TASK_PRIORITY_TXPRI    (tskIDLE_PRIORITY + 2)
+#define STACK_SIZE_TX_BYTES       PIOS_TELEM_TX_STACK_SIZE
-#define REQ_TIMEOUT_MS         250
+#else
-#define MAX_RETRIES            2
+#define STACK_SIZE_RX_BYTES       PIOS_TELEM_STACK_SIZE
-#define STATS_UPDATE_PERIOD_MS 4000
+#define STACK_SIZE_TX_BYTES       PIOS_TELEM_STACK_SIZE
-#define CONNECTION_TIMEOUT_MS  8000
+#endif
+
+#ifdef PIOS_TELEM_RADIO_RX_STACK_SIZE
+#define STACK_SIZE_RADIO_RX_BYTES PIOS_TELEM_RADIO_RX_STACK_SIZE
+#else
+#define STACK_SIZE_RADIO_RX_BYTES STACK_SIZE_RX_BYTES
+#endif
+#define TASK_PRIORITY_RX          (tskIDLE_PRIORITY + 2)
+#define TASK_PRIORITY_TX          (tskIDLE_PRIORITY + 2)
+#define TASK_PRIORITY_RADRX       (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
 
 // Private variables
 static uint32_t telemetryPort;
+#ifdef PIOS_INCLUDE_RFM22B
+static uint32_t radioPort;
+#endif
 static xQueueHandle queue;
 
 #if defined(PIOS_TELEM_PRIORITY_QUEUE)
 static xQueueHandle priorityQueue;
-static xTaskHandle telemetryTxPriTaskHandle;
-static void telemetryTxPriTask(void *parameters);
 #else
 #define priorityQueue queue
 #endif
@@ -83,6 +97,7 @@ static void telemetryTxTask(void *parameters);
 static void telemetryRxTask(void *parameters);
 #ifdef PIOS_INCLUDE_RFM22B
 static void radioRxTask(void *parameters);
+static int32_t transmitRadioData(uint8_t *data, int32_t length);
 #endif
 static int32_t transmitData(uint8_t *data, int32_t length);
 static void registerObject(UAVObjHandle obj);
@@ -109,21 +124,16 @@ int32_t TelemetryStart(void)
     GCSTelemetryStatsConnectQueue(priorityQueue);
 
     // Start telemetry tasks
-    xTaskCreate(telemetryTxTask, (signed char *)"TelTx", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY_TX, &telemetryTxTaskHandle);
+    xTaskCreate(telemetryTxTask, (signed char *)"TelTx", STACK_SIZE_TX_BYTES / 4, NULL, TASK_PRIORITY_TX, &telemetryTxTaskHandle);
     PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYTX, telemetryTxTaskHandle);
-    xTaskCreate(telemetryRxTask, (signed char *)"TelRx", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY_RX, &telemetryRxTaskHandle);
+    xTaskCreate(telemetryRxTask, (signed char *)"TelRx", STACK_SIZE_RX_BYTES / 4, NULL, TASK_PRIORITY_RX, &telemetryRxTaskHandle);
     PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYRX, telemetryRxTaskHandle);
 
 #ifdef PIOS_INCLUDE_RFM22B
-    xTaskCreate(radioRxTask, (signed char *)"RadioRx", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY_RADRX, &radioRxTaskHandle);
+    xTaskCreate(radioRxTask, (signed char *)"RadioRx", STACK_SIZE_RADIO_RX_BYTES / 4, NULL, TASK_PRIORITY_RADRX, &radioRxTaskHandle);
     PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_RADIORX, radioRxTaskHandle);
 #endif
 
-#if defined(PIOS_TELEM_PRIORITY_QUEUE)
-    xTaskCreate(telemetryTxPriTask, (signed char *)"TelPriTx", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY_TXPRI, &telemetryTxPriTaskHandle);
-    PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYTXPRI, telemetryTxPriTaskHandle);
-#endif
-
     return 0;
 }
 
@@ -148,16 +158,20 @@ int32_t TelemetryInitialize(void)
 
     // 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(&transmitData);
+    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;
@@ -177,22 +191,10 @@ MODULE_INITCALL(TelemetryInitialize, TelemetryStart);
 static void registerObject(UAVObjHandle obj)
 {
     if (UAVObjIsMetaobject(obj)) {
-        /* Only connect change notifications for meta objects.  No periodic updates */
+        // Only connect change notifications for meta objects.  No periodic updates
         UAVObjConnectQueue(obj, priorityQueue, EV_MASK_ALL_UPDATES);
-        return;
     } else {
         // Setup object for periodic updates
-        UAVObjEvent ev = {
-            .obj    = obj,
-            .instId = UAVOBJ_ALL_INSTANCES,
-            .event  = EV_UPDATED_PERIODIC,
-        };
-        EventPeriodicQueueCreate(&ev, queue, 0);
-        ev.event = EV_LOGGING_PERIODIC;
-        EventPeriodicQueueCreate(&ev, queue, 0);
-
-
-        // Setup object for telemetry updates
         updateObject(obj, EV_NONE);
     }
 }
@@ -208,9 +210,8 @@ static void updateObject(UAVObjHandle obj, int32_t eventType)
     int32_t eventMask;
 
     if (UAVObjIsMetaobject(obj)) {
-        /* This function updates the periodic updates for the object.
+        // This function updates the periodic updates for the object.
-         * Meta Objects cannot have periodic updates.
+        // Meta Objects cannot have periodic updates.
-         */
         PIOS_Assert(false);
         return;
     }
@@ -288,7 +289,12 @@ static void updateObject(UAVObjHandle obj, int32_t eventType)
         eventMask |= EV_LOGGING_MANUAL;
         break;
     }
-    UAVObjConnectQueue(obj, priorityQueue, eventMask);
+    // note that all setting objects have implicitly IsPriority=true
+    if (UAVObjIsPriority(obj)) {
+        UAVObjConnectQueue(obj, priorityQueue, eventMask);
+    } else {
+        UAVObjConnectQueue(obj, queue, eventMask);
+    }
 }
 
 /**
@@ -298,7 +304,6 @@ static void processObjEvent(UAVObjEvent *ev)
 {
     UAVObjMetadata metadata;
     UAVObjUpdateMode updateMode;
-    FlightTelemetryStatsData flightStats;
     int32_t retries;
     int32_t success;
 
@@ -307,7 +312,6 @@ static void processObjEvent(UAVObjEvent *ev)
     } else if (ev->obj == GCSTelemetryStatsHandle()) {
         gcsTelemetryStatsUpdated();
     } else {
-        FlightTelemetryStatsGet(&flightStats);
         // Get object metadata
         UAVObjGetMetadata(ev->obj, &metadata);
         updateMode = UAVObjGetTelemetryUpdateMode(&metadata);
@@ -315,32 +319,41 @@ static void processObjEvent(UAVObjEvent *ev)
         // Act on event
         retries    = 0;
         success    = -1;
-        if ((ev->event == EV_UPDATED && (updateMode == UPDATEMODE_ONCHANGE || updateMode == UPDATEMODE_THROTTLED)) || ev->event == EV_UPDATED_MANUAL || ((ev->event == EV_UPDATED_PERIODIC) && (updateMode != UPDATEMODE_THROTTLED))) {
+        if ((ev->event == EV_UPDATED && (updateMode == UPDATEMODE_ONCHANGE || updateMode == UPDATEMODE_THROTTLED))
+            || ev->event == EV_UPDATED_MANUAL
+            || (ev->event == EV_UPDATED_PERIODIC && updateMode != UPDATEMODE_THROTTLED)) {
             // Send update to GCS (with retries)
             while (retries < MAX_RETRIES && success == -1) {
-                success = UAVTalkSendObject(uavTalkCon, ev->obj, ev->instId, UAVObjGetTelemetryAcked(&metadata), REQ_TIMEOUT_MS); // call blocks until ack is received or timeout
+                // call blocks until ack is received or timeout
-                ++retries;
+                success = UAVTalkSendObject(uavTalkCon, ev->obj, ev->instId, UAVObjGetTelemetryAcked(&metadata), REQ_TIMEOUT_MS);
+                if (success == -1) {
+                    ++retries;
+                }
             }
             // Update stats
-            txRetries += (retries - 1);
+            txRetries += retries;
             if (success == -1) {
                 ++txErrors;
             }
         } else if (ev->event == EV_UPDATE_REQ) {
             // Request object update from GCS (with retries)
             while (retries < MAX_RETRIES && success == -1) {
-                success = UAVTalkSendObjectRequest(uavTalkCon, ev->obj, ev->instId, REQ_TIMEOUT_MS); // call blocks until update is received or timeout
+                // call blocks until update is received or timeout
-                ++retries;
+                success = UAVTalkSendObjectRequest(uavTalkCon, ev->obj, ev->instId, REQ_TIMEOUT_MS);
+                if (success == -1) {
+                    ++retries;
+                }
             }
             // Update stats
-            txRetries += (retries - 1);
+            txRetries += retries;
             if (success == -1) {
                 ++txErrors;
             }
         }
         // If this is a metaobject then make necessary telemetry updates
         if (UAVObjIsMetaobject(ev->obj)) {
-            updateObject(UAVObjGetLinkedObj(ev->obj), EV_NONE); // linked object will be the actual object the metadata are for
+            // linked object will be the actual object the metadata are for
+            updateObject(UAVObjGetLinkedObj(ev->obj), EV_NONE);
         } else {
             if (updateMode == UPDATEMODE_THROTTLED) {
                 // If this is UPDATEMODE_THROTTLED, the event mask changes on every event.
@@ -351,7 +364,9 @@ static void processObjEvent(UAVObjEvent *ev)
     // Log UAVObject if necessary
     if (ev->obj) {
         updateMode = UAVObjGetLoggingUpdateMode(&metadata);
-        if ((ev->event == EV_UPDATED && (updateMode == UPDATEMODE_ONCHANGE || updateMode == UPDATEMODE_THROTTLED)) || ev->event == EV_LOGGING_MANUAL || ((ev->event == EV_LOGGING_PERIODIC) && (updateMode != UPDATEMODE_THROTTLED))) {
+        if ((ev->event == EV_UPDATED && (updateMode == UPDATEMODE_ONCHANGE || updateMode == UPDATEMODE_THROTTLED))
+            || ev->event == EV_LOGGING_MANUAL
+            || (ev->event == EV_LOGGING_PERIODIC && updateMode != UPDATEMODE_THROTTLED)) {
             if (ev->instId == UAVOBJ_ALL_INSTANCES) {
                 success = UAVObjGetNumInstances(ev->obj);
                 for (retries = 0; retries < success; retries++) {
@@ -377,32 +392,34 @@ static void telemetryTxTask(__attribute__((unused)) void *parameters)
 
     // Loop forever
     while (1) {
-        // Wait for queue message
+        /**
-        if (xQueueReceive(queue, &ev, portMAX_DELAY) == pdTRUE) {
+         * Tries to empty the high priority queue before handling any standard priority item
-            // Process event
+         */
-            processObjEvent(&ev);
-        }
-    }
-}
-
-/**
- * Telemetry transmit task, high priority
- */
 #if defined(PIOS_TELEM_PRIORITY_QUEUE)
-static void telemetryTxPriTask(__attribute__((unused)) void *parameters)
+        // empty priority queue, non-blocking
-{
+        while (xQueueReceive(priorityQueue, &ev, 0) == pdTRUE) {
-    UAVObjEvent ev;
-
-    // Loop forever
-    while (1) {
-        // Wait for queue message
-        if (xQueueReceive(priorityQueue, &ev, portMAX_DELAY) == pdTRUE) {
             // Process event
             processObjEvent(&ev);
         }
+        // check regular queue and process update - non-blocking
+        if (xQueueReceive(queue, &ev, 0) == pdTRUE) {
+            // Process event
+            processObjEvent(&ev);
+            // if both queues are empty, wait on priority queue for updates (1 tick) then repeat cycle
+        } else if (xQueueReceive(priorityQueue, &ev, 1) == pdTRUE) {
+            // Process event
+            processObjEvent(&ev);
+        }
+#else
+        // wait on queue for updates (1 tick) then repeat cycle
+        if (xQueueReceive(queue, &ev, 1) == pdTRUE) {
+            // Process event
+            processObjEvent(&ev);
+        }
+#endif /* if defined(PIOS_TELEM_PRIORITY_QUEUE) */
     }
 }
-#endif
+
 
 /**
  * Telemetry receive task. Processes queue events and periodic updates.
@@ -438,12 +455,12 @@ static void radioRxTask(__attribute__((unused)) void *parameters)
 {
     // Task loop
     while (1) {
-        if (telemetryPort) {
+        if (radioPort) {
             // Block until data are available
             uint8_t serial_data[1];
             uint16_t bytes_to_process;
 
-            bytes_to_process = PIOS_COM_ReceiveBuffer(telemetryPort, serial_data, sizeof(serial_data), 500);
+            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]);
@@ -454,6 +471,21 @@ static void radioRxTask(__attribute__((unused)) void *parameters)
         }
     }
 }
+/**
+ * Transmit data buffer to the radioport.
+ * \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 transmitRadioData(uint8_t *data, int32_t length)
+{
+    if (radioPort) {
+        return PIOS_COM_SendBuffer(radioPort, data, length);
+    }
+
+    return -1;
+}
 #endif /* PIOS_INCLUDE_RFM22B */
 
 /**
@@ -484,12 +516,21 @@ static int32_t transmitData(uint8_t *data, int32_t length)
 static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs)
 {
     UAVObjEvent ev;
+    int32_t ret;
 
-    // Add object for periodic updates
+    // Add or update object for periodic updates
     ev.obj    = obj;
     ev.instId = UAVOBJ_ALL_INSTANCES;
     ev.event  = EV_UPDATED_PERIODIC;
-    return EventPeriodicQueueUpdate(&ev, queue, updatePeriodMs);
+    ev.lowPriority = true;
+
+    xQueueHandle targetQueue = UAVObjIsPriority(obj) ? priorityQueue : queue;
+
+    ret = EventPeriodicQueueUpdate(&ev, targetQueue, updatePeriodMs);
+    if (ret == -1) {
+        ret = EventPeriodicQueueCreate(&ev, targetQueue, updatePeriodMs);
+    }
+    return ret;
 }
 
 /**
@@ -502,12 +543,21 @@ static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs)
 static int32_t setLoggingPeriod(UAVObjHandle obj, int32_t updatePeriodMs)
 {
     UAVObjEvent ev;
+    int32_t ret;
 
-    // Add object for periodic updates
+    // Add or update object for periodic updates
     ev.obj    = obj;
     ev.instId = UAVOBJ_ALL_INSTANCES;
     ev.event  = EV_LOGGING_PERIODIC;
-    return EventPeriodicQueueUpdate(&ev, queue, updatePeriodMs);
+    ev.lowPriority = true;
+
+    xQueueHandle targetQueue = UAVObjIsPriority(obj) ? priorityQueue : queue;
+
+    ret = EventPeriodicQueueUpdate(&ev, targetQueue, updatePeriodMs);
+    if (ret == -1) {
+        ret = EventPeriodicQueueCreate(&ev, targetQueue, updatePeriodMs);
+    }
+    return ret;
 }
 
 /**
@@ -540,12 +590,10 @@ static void updateTelemetryStats()
     uint32_t timeNow;
 
     // Get stats
-    UAVTalkGetStats(uavTalkCon, &utalkStats);
+    UAVTalkGetStats(uavTalkCon, &utalkStats, true);
 #ifdef PIOS_INCLUDE_RFM22B
-    UAVTalkAddStats(radioUavTalkCon, &utalkStats);
+    UAVTalkAddStats(radioUavTalkCon, &utalkStats, true);
-    UAVTalkResetStats(radioUavTalkCon);
 #endif
-    UAVTalkResetStats(uavTalkCon);
 
     // Get object data
     FlightTelemetryStatsGet(&flightStats);
@@ -553,25 +601,33 @@ static void updateTelemetryStats()
 
     // Update stats object
     if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
-        flightStats.RxDataRate  = (float)utalkStats.rxBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f);
+        flightStats.TxDataRate    = (float)utalkStats.txBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f);
-        flightStats.TxDataRate  = (float)utalkStats.txBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f);
+        flightStats.TxBytes      += utalkStats.txBytes;
-        flightStats.RxFailures += utalkStats.rxErrors;
+        flightStats.TxFailures   += txErrors;
-        flightStats.TxFailures += txErrors;
+        flightStats.TxRetries    += txRetries;
-        flightStats.TxRetries  += txRetries;
+
-        txErrors = 0;
+        flightStats.RxDataRate    = (float)utalkStats.rxBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f);
-        txRetries = 0;
+        flightStats.RxBytes      += utalkStats.rxBytes;
+        flightStats.RxFailures   += utalkStats.rxErrors;
+        flightStats.RxSyncErrors += utalkStats.rxSyncErrors;
+        flightStats.RxCrcErrors  += utalkStats.rxCrcErrors;
     } else {
-        flightStats.RxDataRate = 0;
+        flightStats.TxDataRate   = 0;
-        flightStats.TxDataRate = 0;
+        flightStats.TxBytes      = 0;
-        flightStats.RxFailures = 0;
+        flightStats.TxFailures   = 0;
-        flightStats.TxFailures = 0;
+        flightStats.TxRetries    = 0;
-        flightStats.TxRetries  = 0;
+
-        txErrors = 0;
+        flightStats.RxDataRate   = 0;
-        txRetries = 0;
+        flightStats.RxBytes      = 0;
+        flightStats.RxFailures   = 0;
+        flightStats.RxSyncErrors = 0;
+        flightStats.RxCrcErrors  = 0;
     }
+    txErrors  = 0;
+    txRetries = 0;
 
     // Check for connection timeout
-    timeNow = xTaskGetTickCount() * portTICK_RATE_MS;
+    timeNow   = xTaskGetTickCount() * portTICK_RATE_MS;
     if (utalkStats.rxObjects > 0) {
         timeOfLastObjectUpdate = timeNow;
     }
@@ -666,17 +722,25 @@ static void updateSettings()
  * 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
-#ifndef PIOS_INCLUDE_RFM22B
+static uint32_t getComPort(__attribute__((unused)) bool input)
-    input = false;
 #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 */
-    if (!input && PIOS_COM_Available(telemetryPort)) {
+#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

@@ -56,6 +56,10 @@
 #include "accessorydesired.h"
 #include "manualcontrolcommand.h"
 #include "stabilizationsettings.h"
+#include "stabilizationbank.h"
+#include "stabilizationsettingsbank1.h"
+#include "stabilizationsettingsbank2.h"
+#include "stabilizationsettingsbank3.h"
 #include "flightstatus.h"
 #include "hwsettings.h"
 
@@ -107,6 +111,7 @@ int32_t TxPIDInitialize(void)
             .obj    = AccessoryDesiredHandle(),
             .instId = 0,
             .event  = 0,
+            .lowPriority = false,
         };
         EventPeriodicCallbackCreate(&ev, updatePIDs, SAMPLE_PERIOD_MS / portTICK_RATE_MS);
 
@@ -163,11 +168,29 @@ static void updatePIDs(UAVObjEvent *ev)
         return;
     }
 
+    StabilizationBankData bank;
+    switch (inst.BankNumber) {
+    case 0:
+        StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *)&bank);
+        break;
+
+    case 1:
+        StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *)&bank);
+        break;
+
+    case 2:
+        StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *)&bank);
+        break;
+
+    default:
+        return;
+    }
     StabilizationSettingsData stab;
     StabilizationSettingsGet(&stab);
     AccessoryDesiredData accessory;
 
-    uint8_t needsUpdate = 0;
+    uint8_t needsUpdateBank = 0;
+    uint8_t needsUpdateStab = 0;
 
     // Loop through every enabled instance
     for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) {
@@ -192,107 +215,125 @@ static void updatePIDs(UAVObjEvent *ev)
 
             switch (cast_struct_to_array(inst.PIDs, inst.PIDs.Instance1)[i]) {
             case TXPIDSETTINGS_PIDS_ROLLRATEKP:
-                needsUpdate |= update(&stab.RollRatePID.Kp, value);
+                needsUpdateBank |= update(&bank.RollRatePID.Kp, value);
                 break;
             case TXPIDSETTINGS_PIDS_ROLLRATEKI:
-                needsUpdate |= update(&stab.RollRatePID.Ki, value);
+                needsUpdateBank |= update(&bank.RollRatePID.Ki, value);
                 break;
             case TXPIDSETTINGS_PIDS_ROLLRATEKD:
-                needsUpdate |= update(&stab.RollRatePID.Kd, value);
+                needsUpdateBank |= update(&bank.RollRatePID.Kd, value);
                 break;
             case TXPIDSETTINGS_PIDS_ROLLRATEILIMIT:
-                needsUpdate |= update(&stab.RollRatePID.ILimit, value);
+                needsUpdateBank |= update(&bank.RollRatePID.ILimit, value);
                 break;
             case TXPIDSETTINGS_PIDS_ROLLATTITUDEKP:
-                needsUpdate |= update(&stab.RollPI.Kp, value);
+                needsUpdateBank |= update(&bank.RollPI.Kp, value);
                 break;
             case TXPIDSETTINGS_PIDS_ROLLATTITUDEKI:
-                needsUpdate |= update(&stab.RollPI.Ki, value);
+                needsUpdateBank |= update(&bank.RollPI.Ki, value);
                 break;
             case TXPIDSETTINGS_PIDS_ROLLATTITUDEILIMIT:
-                needsUpdate |= update(&stab.RollPI.ILimit, value);
+                needsUpdateBank |= update(&bank.RollPI.ILimit, value);
                 break;
             case TXPIDSETTINGS_PIDS_PITCHRATEKP:
-                needsUpdate |= update(&stab.PitchRatePID.Kp, value);
+                needsUpdateBank |= update(&bank.PitchRatePID.Kp, value);
                 break;
             case TXPIDSETTINGS_PIDS_PITCHRATEKI:
-                needsUpdate |= update(&stab.PitchRatePID.Ki, value);
+                needsUpdateBank |= update(&bank.PitchRatePID.Ki, value);
                 break;
             case TXPIDSETTINGS_PIDS_PITCHRATEKD:
-                needsUpdate |= update(&stab.PitchRatePID.Kd, value);
+                needsUpdateBank |= update(&bank.PitchRatePID.Kd, value);
                 break;
             case TXPIDSETTINGS_PIDS_PITCHRATEILIMIT:
-                needsUpdate |= update(&stab.PitchRatePID.ILimit, value);
+                needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value);
                 break;
             case TXPIDSETTINGS_PIDS_PITCHATTITUDEKP:
-                needsUpdate |= update(&stab.PitchPI.Kp, value);
+                needsUpdateBank |= update(&bank.PitchPI.Kp, value);
                 break;
             case TXPIDSETTINGS_PIDS_PITCHATTITUDEKI:
-                needsUpdate |= update(&stab.PitchPI.Ki, value);
+                needsUpdateBank |= update(&bank.PitchPI.Ki, value);
                 break;
             case TXPIDSETTINGS_PIDS_PITCHATTITUDEILIMIT:
-                needsUpdate |= update(&stab.PitchPI.ILimit, value);
+                needsUpdateBank |= update(&bank.PitchPI.ILimit, value);
                 break;
             case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKP:
-                needsUpdate |= update(&stab.RollRatePID.Kp, value);
+                needsUpdateBank |= update(&bank.RollRatePID.Kp, value);
-                needsUpdate |= update(&stab.PitchRatePID.Kp, value);
+                needsUpdateBank |= update(&bank.PitchRatePID.Kp, value);
                 break;
             case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKI:
-                needsUpdate |= update(&stab.RollRatePID.Ki, value);
+                needsUpdateBank |= update(&bank.RollRatePID.Ki, value);
-                needsUpdate |= update(&stab.PitchRatePID.Ki, value);
+                needsUpdateBank |= update(&bank.PitchRatePID.Ki, value);
                 break;
             case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKD:
-                needsUpdate |= update(&stab.RollRatePID.Kd, value);
+                needsUpdateBank |= update(&bank.RollRatePID.Kd, value);
-                needsUpdate |= update(&stab.PitchRatePID.Kd, value);
+                needsUpdateBank |= update(&bank.PitchRatePID.Kd, value);
                 break;
             case TXPIDSETTINGS_PIDS_ROLLPITCHRATEILIMIT:
-                needsUpdate |= update(&stab.RollRatePID.ILimit, value);
+                needsUpdateBank |= update(&bank.RollRatePID.ILimit, value);
-                needsUpdate |= update(&stab.PitchRatePID.ILimit, value);
+                needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value);
                 break;
             case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKP:
-                needsUpdate |= update(&stab.RollPI.Kp, value);
+                needsUpdateBank |= update(&bank.RollPI.Kp, value);
-                needsUpdate |= update(&stab.PitchPI.Kp, value);
+                needsUpdateBank |= update(&bank.PitchPI.Kp, value);
                 break;
             case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKI:
-                needsUpdate |= update(&stab.RollPI.Ki, value);
+                needsUpdateBank |= update(&bank.RollPI.Ki, value);
-                needsUpdate |= update(&stab.PitchPI.Ki, value);
+                needsUpdateBank |= update(&bank.PitchPI.Ki, value);
                 break;
             case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEILIMIT:
-                needsUpdate |= update(&stab.RollPI.ILimit, value);
+                needsUpdateBank |= update(&bank.RollPI.ILimit, value);
-                needsUpdate |= update(&stab.PitchPI.ILimit, value);
+                needsUpdateBank |= update(&bank.PitchPI.ILimit, value);
                 break;
             case TXPIDSETTINGS_PIDS_YAWRATEKP:
-                needsUpdate |= update(&stab.YawRatePID.Kp, value);
+                needsUpdateBank |= update(&bank.YawRatePID.Kp, value);
                 break;
             case TXPIDSETTINGS_PIDS_YAWRATEKI:
-                needsUpdate |= update(&stab.YawRatePID.Ki, value);
+                needsUpdateBank |= update(&bank.YawRatePID.Ki, value);
                 break;
             case TXPIDSETTINGS_PIDS_YAWRATEKD:
-                needsUpdate |= update(&stab.YawRatePID.Kd, value);
+                needsUpdateBank |= update(&bank.YawRatePID.Kd, value);
                 break;
             case TXPIDSETTINGS_PIDS_YAWRATEILIMIT:
-                needsUpdate |= update(&stab.YawRatePID.ILimit, value);
+                needsUpdateBank |= update(&bank.YawRatePID.ILimit, value);
                 break;
             case TXPIDSETTINGS_PIDS_YAWATTITUDEKP:
-                needsUpdate |= update(&stab.YawPI.Kp, value);
+                needsUpdateBank |= update(&bank.YawPI.Kp, value);
                 break;
             case TXPIDSETTINGS_PIDS_YAWATTITUDEKI:
-                needsUpdate |= update(&stab.YawPI.Ki, value);
+                needsUpdateBank |= update(&bank.YawPI.Ki, value);
                 break;
             case TXPIDSETTINGS_PIDS_YAWATTITUDEILIMIT:
-                needsUpdate |= update(&stab.YawPI.ILimit, value);
+                needsUpdateBank |= update(&bank.YawPI.ILimit, value);
                 break;
             case TXPIDSETTINGS_PIDS_GYROTAU:
-                needsUpdate |= update(&stab.GyroTau, value);
+                needsUpdateStab |= update(&stab.GyroTau, value);
                 break;
             default:
                 PIOS_Assert(0);
             }
         }
     }
-    if (needsUpdate) {
+    if (needsUpdateStab) {
         StabilizationSettingsSet(&stab);
     }
+    if (needsUpdateBank) {
+        switch (inst.BankNumber) {
+        case 0:
+            StabilizationSettingsBank1Set((StabilizationSettingsBank1Data *)&bank);
+            break;
+
+        case 1:
+            StabilizationSettingsBank2Set((StabilizationSettingsBank2Data *)&bank);
+            break;
+
+        case 2:
+            StabilizationSettingsBank3Set((StabilizationSettingsBank3Data *)&bank);
+            break;
+
+        default:
+            return;
+        }
+    }
 }
 
 /**

flight/modules/VtolPathFollower/vtolpathfollower.c

@@ -55,7 +55,7 @@
 #include "hwsettings.h"
 #include "pathdesired.h" // object that will be updated by the module
 #include "positionstate.h"
-#include "manualcontrol.h"
+#include "manualcontrolcommand.h"
 #include "flightstatus.h"
 #include "pathstatus.h"
 #include "gpsvelocitysensor.h"
@@ -65,6 +65,7 @@
 #include "nedaccel.h"
 #include "stabilizationdesired.h"
 #include "stabilizationsettings.h"
+#include "stabilizationbank.h"
 #include "systemsettings.h"
 #include "velocitydesired.h"
 #include "velocitystate.h"
@@ -157,7 +158,7 @@ static float northPosIntegral = 0;
 static float eastPosIntegral  = 0;
 static float downPosIntegral  = 0;
 
-static float throttleOffset   = 0;
+static float thrustOffset     = 0;
 /**
  * Module thread, should not return.
  */
@@ -205,55 +206,53 @@ static void vtolPathFollowerTask(__attribute__((unused)) void *parameters)
         PathDesiredGet(&pathDesired);
 
         // Check the combinations of flightmode and pathdesired mode
-        switch (flightStatus.FlightMode) {
+        if (flightStatus.ControlChain.PathFollower == FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
-        case FLIGHTSTATUS_FLIGHTMODE_LAND:
+            if (flightStatus.ControlChain.PathPlanner == FLIGHTSTATUS_CONTROLCHAIN_FALSE) {
-        case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
+                if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POI) {
-        case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
+                    if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
-            if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
+                        updateEndpointVelocity();
-                updateEndpointVelocity();
+                        updateVtolDesiredAttitude(true);
-                updateVtolDesiredAttitude(false);
+                        updatePOIBearing();
-                AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
+                    } else {
+                        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
+                    }
+                } else {
+                    if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
+                        updateEndpointVelocity();
+                        updateVtolDesiredAttitude(false);
+                        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
+                    } else {
+                        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
+                    }
+                }
             } else {
-                AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
+                pathStatus.UID    = pathDesired.UID;
+                pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
+                switch (pathDesired.Mode) {
+                // TODO: Make updateVtolDesiredAttitude and velocity report success and update PATHSTATUS_STATUS accordingly
+                case PATHDESIRED_MODE_FLYENDPOINT:
+                case PATHDESIRED_MODE_FLYVECTOR:
+                case PATHDESIRED_MODE_FLYCIRCLERIGHT:
+                case PATHDESIRED_MODE_FLYCIRCLELEFT:
+                    updatePathVelocity();
+                    updateVtolDesiredAttitude(false);
+                    AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
+                    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);
             }
-            break;
+        } else {
-        case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
-            pathStatus.UID    = pathDesired.UID;
-            pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
-            switch (pathDesired.Mode) {
-            // TODO: Make updateVtolDesiredAttitude and velocity report success and update PATHSTATUS_STATUS accordingly
-            case PATHDESIRED_MODE_FLYENDPOINT:
-            case PATHDESIRED_MODE_FLYVECTOR:
-            case PATHDESIRED_MODE_FLYCIRCLERIGHT:
-            case PATHDESIRED_MODE_FLYCIRCLELEFT:
-                updatePathVelocity();
-                updateVtolDesiredAttitude(false);
-                AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
-                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);
-            break;
-        case FLIGHTSTATUS_FLIGHTMODE_POI:
-            if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
-                updateEndpointVelocity();
-                updateVtolDesiredAttitude(true);
-                updatePOIBearing();
-            } else {
-                AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
-            }
-            break;
-        default:
             // Be cleaner and get rid of global variables
             northVelIntegral = 0;
             eastVelIntegral  = 0;
@@ -262,12 +261,10 @@ static void vtolPathFollowerTask(__attribute__((unused)) void *parameters)
             eastPosIntegral  = 0;
             downPosIntegral  = 0;
 
-            // Track throttle before engaging this mode.  Cheap system ident
+            // Track thrust before engaging this mode.  Cheap system ident
             StabilizationDesiredData stabDesired;
             StabilizationDesiredGet(&stabDesired);
-            throttleOffset = stabDesired.Throttle;
+            thrustOffset = stabDesired.Thrust;
-
-            break;
         }
 
         AlarmsClear(SYSTEMALARMS_ALARM_GUIDANCE);
@@ -473,48 +470,14 @@ void updateEndpointVelocity()
     float eastCommand;
     float downCommand;
 
-    float northPos = 0, eastPos = 0, downPos = 0;
-    switch (vtolpathfollowerSettings.PositionSource) {
-    case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_EKF:
-        northPos = positionState.North;
-        eastPos  = positionState.East;
-        downPos  = positionState.Down;
-        break;
-    case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_GPSPOS:
-    {
-        // this used to work with the NEDposition UAVObject
-        // however this UAVObject has been removed
-        GPSPositionSensorData gpsPosition;
-        GPSPositionSensorGet(&gpsPosition);
-        HomeLocationData homeLocation;
-        HomeLocationGet(&homeLocation);
-        float lat    = DEG2RAD(homeLocation.Latitude / 10.0e6f);
-        float alt    = homeLocation.Altitude;
-        float T[3]   = { alt + 6.378137E6f,
-                         cosf(lat) * (alt + 6.378137E6f),
-                         -1.0f };
-        float NED[3] = { T[0] * (DEG2RAD((gpsPosition.Latitude - homeLocation.Latitude) / 10.0e6f)),
-                         T[1] * (DEG2RAD((gpsPosition.Longitude - homeLocation.Longitude) / 10.0e6f)),
-                         T[2] * ((gpsPosition.Altitude + gpsPosition.GeoidSeparation - homeLocation.Altitude)) };
-
-        northPos = NED[0];
-        eastPos  = NED[1];
-        downPos  = NED[2];
-    }
-    break;
-    default:
-        PIOS_Assert(0);
-        break;
-    }
-
     // Compute desired north command
-    northError = pathDesired.End.North - northPos;
+    northError = pathDesired.End.North - positionState.North;
     northPosIntegral = bound(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki,
                              -vtolpathfollowerSettings.HorizontalPosPI.ILimit,
                              vtolpathfollowerSettings.HorizontalPosPI.ILimit);
     northCommand     = (northError * vtolpathfollowerSettings.HorizontalPosPI.Kp + northPosIntegral);
 
-    eastError = pathDesired.End.East - eastPos;
+    eastError = pathDesired.End.East - positionState.East;
     eastPosIntegral  = bound(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki,
                              -vtolpathfollowerSettings.HorizontalPosPI.ILimit,
                              vtolpathfollowerSettings.HorizontalPosPI.ILimit);
@@ -530,7 +493,7 @@ void updateEndpointVelocity()
     velocityDesired.North = northCommand * scale;
     velocityDesired.East  = eastCommand * scale;
 
-    downError = pathDesired.End.Down - downPos;
+    downError = pathDesired.End.Down - positionState.Down;
     downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki,
                             -vtolpathfollowerSettings.VerticalPosPI.ILimit,
                             vtolpathfollowerSettings.VerticalPosPI.ILimit);
@@ -549,10 +512,10 @@ static void updateFixedAttitude(float *attitude)
     StabilizationDesiredData stabDesired;
 
     StabilizationDesiredGet(&stabDesired);
-    stabDesired.Roll     = attitude[0];
+    stabDesired.Roll   = attitude[0];
-    stabDesired.Pitch    = attitude[1];
+    stabDesired.Pitch  = attitude[1];
-    stabDesired.Yaw      = attitude[2];
+    stabDesired.Yaw    = attitude[2];
-    stabDesired.Throttle = attitude[3];
+    stabDesired.Thrust = attitude[3];
     stabDesired.StabilizationMode.Roll  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
     stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
     stabDesired.StabilizationMode.Yaw   = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
@@ -575,7 +538,7 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
     StabilizationDesiredData stabDesired;
     AttitudeStateData attitudeState;
     NedAccelData nedAccel;
-    StabilizationSettingsData stabSettings;
+    StabilizationBankData stabSettings;
     SystemSettingsData systemSettings;
 
     float northError;
@@ -593,17 +556,17 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
     StabilizationDesiredGet(&stabDesired);
     VelocityDesiredGet(&velocityDesired);
     AttitudeStateGet(&attitudeState);
-    StabilizationSettingsGet(&stabSettings);
+    StabilizationBankGet(&stabSettings);
     NedAccelGet(&nedAccel);
 
     float northVel = 0, eastVel = 0, downVel = 0;
     switch (vtolpathfollowerSettings.VelocitySource) {
-    case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_EKF:
+    case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_STATE_ESTIMATION:
         northVel = velocityState.North;
         eastVel  = velocityState.East;
         downVel  = velocityState.Down;
         break;
-    case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_NEDVEL:
+    case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPS_VELNED:
     {
         GPSVelocitySensorData gpsVelocity;
         GPSVelocitySensorGet(&gpsVelocity);
@@ -612,7 +575,7 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
         downVel  = gpsVelocity.Down;
     }
     break;
-    case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPSPOS:
+    case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPS_GROUNDSPEED:
     {
         GPSPositionSensorData gpsPosition;
         GPSPositionSensorGet(&gpsPosition);
@@ -652,13 +615,13 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
     downError = velocityDesired.Down - downVel;
     // Must flip this sign
     downError = -downError;
-    downVelIntegral = bound(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID.Ki,
+    downVelIntegral    = bound(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID.Ki,
-                            -vtolpathfollowerSettings.VerticalVelPID.ILimit,
+                               -vtolpathfollowerSettings.VerticalVelPID.ILimit,
-                            vtolpathfollowerSettings.VerticalVelPID.ILimit);
+                               vtolpathfollowerSettings.VerticalVelPID.ILimit);
-    downCommand     = (downError * vtolpathfollowerSettings.VerticalVelPID.Kp + downVelIntegral
+    downCommand        = (downError * vtolpathfollowerSettings.VerticalVelPID.Kp + downVelIntegral
-                       - nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID.Kd);
+                          - nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID.Kd);
 
-    stabDesired.Throttle = bound(downCommand + throttleOffset, 0, 1);
+    stabDesired.Thrust = bound(downCommand + thrustOffset, 0, 1);
 
     // 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
@@ -669,11 +632,11 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
                               eastCommand * cosf(DEG2RAD(attitudeState.Yaw)),
                               -vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
 
-    if (vtolpathfollowerSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) {
+    if (vtolpathfollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_FALSE) {
-        // For now override throttle with manual control.  Disable at your risk, quad goes to China.
+        // For now override thrust with manual control.  Disable at your risk, quad goes to China.
         ManualControlCommandData manualControl;
         ManualControlCommandGet(&manualControl);
-        stabDesired.Throttle = manualControl.Throttle;
+        stabDesired.Thrust = manualControl.Thrust;
     }
 
     stabDesired.StabilizationMode.Roll  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;

flight/pios/common/pios_callbackscheduler.c

@@ -1,7 +1,7 @@
 /**
  ******************************************************************************
  *
- * @file       callbackscheduler.c
+ * @file       pios_callbackscheduler.c
  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
  * @brief      Scheduler to run callback functions from a shared context with given priorities.
  *
@@ -24,13 +24,18 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 
-#include <openpilot.h>
+#include <pios.h>
+#ifdef PIOS_INCLUDE_CALLBACKSCHEDULER
 
+#include <utlist.h>
+#include <uavobjectmanager.h>
 #include <taskinfo.h>
 
 // Private constants
-#define STACK_SIZE 128
+#define STACK_SAFETYCOUNT 16
-#define MAX_SLEEP  1000
+#define STACK_SIZE        (190 + STACK_SAFETYSIZE)
+#define STACK_SAFETYSIZE  8
+#define MAX_SLEEP         1000
 
 // Private types
 /**
@@ -52,8 +57,15 @@ struct DelayedCallbackTaskStruct {
  */
 struct DelayedCallbackInfoStruct {
     DelayedCallback   cb;
+    int16_t callbackID;
     bool volatile     waiting;
     uint32_t volatile scheduletime;
+    uint32_t stackSize;
+    int32_t  stackFree;
+    int32_t  stackNotFree;
+    uint16_t stackSafetyCount;
+    uint16_t currentSafetyCount;
+    uint32_t runCount;
     struct DelayedCallbackTaskStruct *task;
     struct DelayedCallbackInfoStruct *next;
 };
@@ -73,7 +85,7 @@ static int32_t runNextCallback(struct DelayedCallbackTaskStruct *task, DelayedCa
  * must be called before any other functions are called
  * \return Success (0), failure (-1)
  */
-int32_t CallbackSchedulerInitialize()
+int32_t PIOS_CALLBACKSCHEDULER_Initialize()
 {
     // Initialize variables
     schedulerTasks   = NULL;
@@ -99,7 +111,7 @@ int32_t CallbackSchedulerInitialize()
  * they can be marked for later execution by executing the dispatch function.
  * \return Success (0), failure (-1)
  */
-int32_t CallbackSchedulerStart()
+int32_t PIOS_CALLBACKSCHEDULER_Start()
 {
     xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
 
@@ -113,7 +125,7 @@ int32_t CallbackSchedulerStart()
         xTaskCreate(
             CallbackSchedulerTask,
             cursor->name,
-            cursor->stackSize / 4,
+            1 + (cursor->stackSize / 4),
             cursor,
             cursor->priorityTask,
             &cursor->callbackSchedulerTaskHandle
@@ -143,7 +155,7 @@ int32_t CallbackSchedulerStart()
  * UPDATEMODE_OVERRIDE: The callback will be rescheduled in any case, effectively overriding any previous schedule. (sooner+later=override)
  * \return 0: not scheduled, previous schedule takes precedence, 1: new schedule, 2: previous schedule overridden
  */
-int32_t DelayedCallbackSchedule(
+int32_t PIOS_CALLBACKSCHEDULER_Schedule(
     DelayedCallbackInfo *cbinfo,
     int32_t milliseconds,
     DelayedCallbackUpdateMode updatemode)
@@ -191,7 +203,7 @@ int32_t DelayedCallbackSchedule(
  * \param[in] cbinfo the callback handle
  * \return Success (-1), failure (0)
  */
-int32_t DelayedCallbackDispatch(DelayedCallbackInfo *cbinfo)
+int32_t PIOS_CALLBACKSCHEDULER_Dispatch(DelayedCallbackInfo *cbinfo)
 {
     PIOS_Assert(cbinfo);
 
@@ -215,7 +227,7 @@ int32_t DelayedCallbackDispatch(DelayedCallbackInfo *cbinfo)
  * Check the demo task for your port to find the syntax required.
  * \return Success (-1), failure (0)
  */
-int32_t DelayedCallbackDispatchFromISR(DelayedCallbackInfo *cbinfo, long *pxHigherPriorityTaskWoken)
+int32_t PIOS_CALLBACKSCHEDULER_DispatchFromISR(DelayedCallbackInfo *cbinfo, long *pxHigherPriorityTaskWoken)
 {
     PIOS_Assert(cbinfo);
 
@@ -237,14 +249,18 @@ int32_t DelayedCallbackDispatchFromISR(DelayedCallbackInfo *cbinfo, long *pxHigh
  * \param[in] stacksize The stack requirements of the callback when called by the scheduler.
  * \return CallbackInfo Pointer on success, NULL if failed.
  */
-DelayedCallbackInfo *DelayedCallbackCreate(
+DelayedCallbackInfo *PIOS_CALLBACKSCHEDULER_Create(
     DelayedCallback cb,
     DelayedCallbackPriority priority,
     DelayedCallbackPriorityTask priorityTask,
+    int16_t callbackID,
     uint32_t stacksize)
 {
     xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
 
+    // add callback schedulers own stack requirements
+    stacksize += STACK_SIZE;
+
     // find appropriate scheduler task matching priorityTask
     struct DelayedCallbackTaskStruct *task = NULL;
     int t = 0;
@@ -271,7 +287,7 @@ DelayedCallbackInfo *DelayedCallbackCreate(
         task->name[0]      = 'C';
         task->name[1]      = 'a' + t;
         task->name[2]      = 0;
-        task->stackSize    = ((STACK_SIZE > stacksize) ? STACK_SIZE : stacksize);
+        task->stackSize    = stacksize;
         task->priorityTask = priorityTask;
         task->next = NULL;
 
@@ -285,13 +301,13 @@ DelayedCallbackInfo *DelayedCallbackCreate(
         // add to list of scheduler tasks
         LL_APPEND(schedulerTasks, task);
 
-        // Previously registered tasks are spawned when CallbackSchedulerStart() is called.
+        // Previously registered tasks are spawned when PIOS_CALLBACKSCHEDULER_Start() is called.
         // Tasks registered afterwards need to spawn upon creation.
         if (schedulerStarted) {
             xTaskCreate(
                 CallbackSchedulerTask,
                 task->name,
-                task->stackSize / 4,
+                1 + (task->stackSize / 4),
                 task,
                 task->priorityTask,
                 &task->callbackSchedulerTaskHandle
@@ -318,11 +334,18 @@ DelayedCallbackInfo *DelayedCallbackCreate(
         xSemaphoreGiveRecursive(mutex);
         return NULL; // error - not enough memory
     }
-    info->next    = NULL;
+    info->next               = NULL;
-    info->waiting = false;
+    info->waiting            = false;
-    info->scheduletime = 0;
+    info->scheduletime       = 0;
-    info->task    = task;
+    info->task               = task;
     info->cb = cb;
+    info->callbackID         = callbackID;
+    info->runCount           = 0;
+    info->stackSize          = stacksize - STACK_SIZE;
+    info->stackNotFree       = info->stackSize;
+    info->stackFree          = 0;
+    info->stackSafetyCount   = STACK_SAFETYCOUNT;
+    info->currentSafetyCount = 0;
 
     // add to scheduling queue
     LL_APPEND(task->callbackQueue[priority], info);
@@ -332,6 +355,108 @@ DelayedCallbackInfo *DelayedCallbackCreate(
     return info;
 }
 
+/**
+ * Iterator. Iterates over all callbacks and all scheduler tasks and retrieves information
+ *
+ * @param[in] callback  Callback function to receive the data - will be called in same task context as the callerThe id of the task the task_info refers to.
+ * @param     context   Context information optionally provided to the callback.
+ */
+void PIOS_CALLBACKSCHEDULER_ForEachCallback(CallbackSchedulerCallbackInfoCallback callback, void *context)
+{
+    if (!callback) {
+        return;
+    }
+
+    struct pios_callback_info info;
+
+    struct DelayedCallbackTaskStruct *task = NULL;
+    LL_FOREACH(schedulerTasks, task) {
+        int prio;
+
+        for (prio = 0; prio < (CALLBACK_PRIORITY_LOW + 1); prio++) {
+            struct DelayedCallbackInfoStruct *cbinfo;
+            LL_FOREACH(task->callbackQueue[prio], cbinfo) {
+                xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+                info.is_running = true;
+                info.stack_remaining    = cbinfo->stackNotFree;
+                info.running_time_count = cbinfo->runCount;
+                xSemaphoreGiveRecursive(mutex);
+                callback(cbinfo->callbackID, &info, context);
+            }
+        }
+    }
+}
+
+/**
+ * Stack magic, find how much stack is being used without affecting performance
+ */
+static void markStack(DelayedCallbackInfo *current)
+{
+    register int8_t t;
+    register int32_t halfWayMark;
+    volatile unsigned char *marker;
+
+    if (current->stackNotFree < 0) {
+        return;
+    }
+
+    // end of stack watermark
+    marker = (unsigned char *)(((size_t)&marker) - (size_t)current->stackSize);
+    for (t = -STACK_SAFETYSIZE; t < 0; t++) {
+        *(marker + t) = '#';
+    }
+    // shifted watermarks
+    halfWayMark = current->stackFree + (current->stackNotFree - current->stackFree) / 2;
+    marker = (unsigned char *)((size_t)marker + halfWayMark);
+    for (t = -STACK_SAFETYSIZE; t < 0; t++) {
+        *(marker + t) = '#';
+    }
+}
+static void checkStack(DelayedCallbackInfo *current)
+{
+    register int8_t t;
+    register int32_t halfWayMark;
+    volatile unsigned char *marker;
+
+    if (current->stackNotFree < 0) {
+        return;
+    }
+
+    // end of stack watermark
+    marker = (unsigned char *)(((size_t)&marker) - (size_t)current->stackSize);
+    for (t = -STACK_SAFETYSIZE; t < 0; t++) {
+        if (*(marker + t) != '#') {
+            current->stackNotFree = -1; // stack overflow, disable all further checks
+            return;
+        }
+    }
+    // shifted watermarks
+    halfWayMark = current->stackFree + (current->stackNotFree - current->stackFree) / 2;
+    marker = (unsigned char *)((size_t)marker + halfWayMark);
+    for (t = -STACK_SAFETYSIZE; t < 0; t++) {
+        if (*(marker + t) != '#') {
+            current->stackNotFree = halfWayMark; // tainted mark, this place is definitely used stack
+            current->currentSafetyCount = 0;
+            if (current->stackNotFree <= current->stackFree) {
+                current->stackFree = 0; // if it was supposed to be free, restart search between here and bottom of stack
+            }
+            return;
+        }
+    }
+
+    if (current->currentSafetyCount < 0xffff) {
+        current->currentSafetyCount++; // mark has not been tainted, increase safety counter
+    }
+    if (current->currentSafetyCount >= current->stackSafetyCount) { // if the safety counter is above the limit, then
+        if (halfWayMark == current->stackFree) { // check if search already converged, if so increase the limit to find very rare stack usage incidents
+            current->stackSafetyCount = current->currentSafetyCount;
+        } else {
+            current->stackFree = halfWayMark; // otherwise just mark this position as free stack to narrow search
+            current->currentSafetyCount = 0;
+        }
+    }
+}
+
 /**
  * Scheduler subtask
  * \param[in] task The scheduler task in question
@@ -384,7 +509,16 @@ static int32_t runNextCallback(struct DelayedCallbackTaskStruct *task, DelayedCa
                 current->scheduletime = 0; // any schedules are reset
                 current->waiting = false; // the flag is reset just before execution.
                 xSemaphoreGiveRecursive(mutex);
+
+                /* callback gets invoked here - check stack sizes */
+                markStack(current);
+
                 current->cb(); // call the callback
+
+                checkStack(current);
+
+                current->runCount++;
+
                 return 0;
             }
             xSemaphoreGiveRecursive(mutex);
@@ -411,3 +545,5 @@ static void CallbackSchedulerTask(void *task)
         }
     }
 }
+
+#endif // ifdef PIOS_INCLUDE_CALLBACKSCHEDULER

flight/pios/common/pios_com.c

@@ -51,6 +51,7 @@ struct pios_com_dev {
 #if defined(PIOS_INCLUDE_FREERTOS)
     xSemaphoreHandle tx_sem;
     xSemaphoreHandle rx_sem;
+    xSemaphoreHandle sendbuffer_sem;
 #endif
 
     bool has_rx;
@@ -155,6 +156,9 @@ int32_t PIOS_COM_Init(uint32_t *com_id, const struct pios_com_driver *driver, ui
 #endif /* PIOS_INCLUDE_FREERTOS */
         (com_dev->driver->bind_tx_cb)(lower_id, PIOS_COM_TxOutCallback, (uint32_t)com_dev);
     }
+#if defined(PIOS_INCLUDE_FREERTOS)
+    com_dev->sendbuffer_sem = xSemaphoreCreateMutex();
+#endif /* PIOS_INCLUDE_FREERTOS */
 
     *com_id = (uint32_t)com_dev;
     return 0;
@@ -267,27 +271,11 @@ int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud)
     return 0;
 }
 
-/**
+
- * Sends a package over given port
+static int32_t PIOS_COM_SendBufferNonBlockingInternal(struct pios_com_dev *com_dev, const uint8_t *buffer, uint16_t len)
- * \param[in] port COM port
- * \param[in] buffer character buffer
- * \param[in] len buffer length
- * \return -1 if port not available
- * \return -2 if non-blocking mode activated: buffer is full
- *            caller should retry until buffer is free again
- * \return number of bytes transmitted on success
- */
-int32_t PIOS_COM_SendBufferNonBlocking(uint32_t com_id, const uint8_t *buffer, uint16_t len)
 {
-    struct pios_com_dev *com_dev = (struct pios_com_dev *)com_id;
+    PIOS_Assert(com_dev);
-
-    if (!PIOS_COM_validate(com_dev)) {
-        /* Undefined COM port for this board (see pios_board.c) */
-        return -1;
-    }
-
     PIOS_Assert(com_dev->has_tx);
-
     if (com_dev->driver->available && !com_dev->driver->available(com_dev->lower_id)) {
         /*
          * Underlying device is down/unconnected.
@@ -314,10 +302,42 @@ int32_t PIOS_COM_SendBufferNonBlocking(uint32_t com_id, const uint8_t *buffer, u
                                       fifoBuf_getUsed(&com_dev->tx));
         }
     }
-
     return bytes_into_fifo;
 }
 
+/**
+ * Sends a package over given port
+ * \param[in] port COM port
+ * \param[in] buffer character buffer
+ * \param[in] len buffer length
+ * \return -1 if port not available
+ * \return -2 if non-blocking mode activated: buffer is full
+ *            caller should retry until buffer is free again
+ * \return -3 another thread is already sending, caller should
+ *            retry until com is available again
+ * \return number of bytes transmitted on success
+ */
+int32_t PIOS_COM_SendBufferNonBlocking(uint32_t com_id, const uint8_t *buffer, uint16_t len)
+{
+    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;
+    }
+#if defined(PIOS_INCLUDE_FREERTOS)
+    if (xSemaphoreTake(com_dev->sendbuffer_sem, 0) != pdTRUE) {
+        return -3;
+    }
+#endif /* PIOS_INCLUDE_FREERTOS */
+    int32_t ret = PIOS_COM_SendBufferNonBlockingInternal(com_dev, buffer, len);
+#if defined(PIOS_INCLUDE_FREERTOS)
+    xSemaphoreGive(com_dev->sendbuffer_sem);
+#endif /* PIOS_INCLUDE_FREERTOS */
+    return ret;
+}
+
+
 /**
  * Sends a package over given port
  * (blocking function)
@@ -325,6 +345,8 @@ int32_t PIOS_COM_SendBufferNonBlocking(uint32_t com_id, const uint8_t *buffer, u
  * \param[in] buffer character buffer
  * \param[in] len buffer length
  * \return -1 if port not available
+ * \return -2 if mutex can't be taken;
+ * \return -3 if data cannot be sent in the max allotted time of 5000msec
  * \return number of bytes transmitted on success
  */
 int32_t PIOS_COM_SendBuffer(uint32_t com_id, const uint8_t *buffer, uint16_t len)
@@ -335,9 +357,12 @@ int32_t PIOS_COM_SendBuffer(uint32_t com_id, const uint8_t *buffer, uint16_t len
         /* Undefined COM port for this board (see pios_board.c) */
         return -1;
     }
-
     PIOS_Assert(com_dev->has_tx);
-
+#if defined(PIOS_INCLUDE_FREERTOS)
+    if (xSemaphoreTake(com_dev->sendbuffer_sem, 5) != pdTRUE) {
+        return -2;
+    }
+#endif /* PIOS_INCLUDE_FREERTOS */
     uint32_t max_frag_len  = fifoBuf_getSize(&com_dev->tx);
     uint32_t bytes_to_send = len;
     while (bytes_to_send) {
@@ -348,13 +373,16 @@ int32_t PIOS_COM_SendBuffer(uint32_t com_id, const uint8_t *buffer, uint16_t len
         } else {
             frag_size = bytes_to_send;
         }
-        int32_t rc = PIOS_COM_SendBufferNonBlocking(com_id, buffer, frag_size);
+        int32_t rc = PIOS_COM_SendBufferNonBlockingInternal(com_dev, buffer, frag_size);
         if (rc >= 0) {
             bytes_to_send -= rc;
             buffer += rc;
         } else {
             switch (rc) {
             case -1:
+#if defined(PIOS_INCLUDE_FREERTOS)
+                xSemaphoreGive(com_dev->sendbuffer_sem);
+#endif /* PIOS_INCLUDE_FREERTOS */
                 /* Device is invalid, this will never work */
                 return -1;
 
@@ -367,17 +395,23 @@ int32_t PIOS_COM_SendBuffer(uint32_t com_id, const uint8_t *buffer, uint16_t len
                 }
 #if defined(PIOS_INCLUDE_FREERTOS)
                 if (xSemaphoreTake(com_dev->tx_sem, 5000) != pdTRUE) {
+                    xSemaphoreGive(com_dev->sendbuffer_sem);
                     return -3;
                 }
 #endif
                 continue;
             default:
                 /* Unhandled return code */
+#if defined(PIOS_INCLUDE_FREERTOS)
+                xSemaphoreGive(com_dev->sendbuffer_sem);
+#endif /* PIOS_INCLUDE_FREERTOS */
                 return rc;
             }
         }
     }
-
+#if defined(PIOS_INCLUDE_FREERTOS)
+    xSemaphoreGive(com_dev->sendbuffer_sem);
+#endif /* PIOS_INCLUDE_FREERTOS */
     return len;
 }
 

flight/pios/common/pios_crc.c

@@ -64,6 +64,17 @@ static const uint16_t CRC_Table16[] = { // HDLC polynomial
     0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78
 };
 
+/**
+ * Generated by pycrc v0.7.5, http://www.tty1.net/pycrc/
+ * using the configuration:
+ *    Width        = 8
+ *    Poly         = 0x07
+ *    XorIn        = 0x00
+ *    ReflectIn    = False
+ *    XorOut       = 0x00
+ *    ReflectOut   = False
+ *    Algorithm    = table-driven
+ */
 static const uint32_t CRC_Table32[] = {
     0x00000000, 0x04c11db7, 0x09823b6e, 0x0d4326d9, 0x130476dc, 0x17c56b6b, 0x1a864db2, 0x1e475005,
     0x2608edb8, 0x22c9f00f, 0x2f8ad6d6, 0x2b4bcb61, 0x350c9b64, 0x31cd86d3, 0x3c8ea00a, 0x384fbdbd,
@@ -101,17 +112,6 @@ static const uint32_t CRC_Table32[] = {
 
 /**
  * Update the crc value with new data.
- *
- * Generated by pycrc v0.7.5, http://www.tty1.net/pycrc/
- * using the configuration:
- *    Width        = 8
- *    Poly         = 0x07
- *    XorIn        = 0x00
- *    ReflectIn    = False
- *    XorOut       = 0x00
- *    ReflectOut   = False
- *    Algorithm    = table-driven
- *
  * \param crc      The current crc value.
  * \param data     Pointer to a buffer of \a data_len bytes.
  * \param length   Number of bytes in the \a data buffer.

flight/pios/common/pios_deltatime.c

@@ -0,0 +1,64 @@
+/**
+ ******************************************************************************
+ * @addtogroup PIOS PIOS Core hardware abstraction layer
+ * @{
+ * @addtogroup PIOS_DELTATIME time measurement Functions
+ * @brief PiOS Delay functionality
+ * @{
+ *
+ * @file       pios_deltatime.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Settings functions header
+ * @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>
+
+
+void PIOS_DELTATIME_Init(PiOSDeltatimeConfig *config, float average, float min, float max, float alpha)
+{
+    PIOS_Assert(config);
+    config->average = average;
+    config->min     = min;
+    config->max     = max;
+    config->alpha   = alpha;
+    config->last    = PIOS_DELAY_GetRaw();
+};
+
+
+float PIOS_DELTATIME_GetAverageSeconds(PiOSDeltatimeConfig *config)
+{
+    PIOS_Assert(config);
+    float dT = PIOS_DELAY_DiffuS(config->last) * 1.0e-6f;
+    config->last = PIOS_DELAY_GetRaw();
+    if (dT < config->min) {
+        dT = config->min;
+    }
+    if (dT > config->max) {
+        dT = config->max;
+    }
+    config->average = config->average * (1.0f - config->alpha) + dT * config->alpha;
+    return config->average;
+}
+
+
+/**
+ * @}
+ * @}
+ */

flight/pios/common/pios_ms4525do.c

@@ -0,0 +1,84 @@
+/**
+ ******************************************************************************
+ * @addtogroup PIOS PIOS Core hardware abstraction layer
+ * @{
+ * @addtogroup PIOS_MS4525DO MS4525DO Functions
+ * @brief Hardware functions to deal with the PixHawk Airspeed Sensor based on MS4525DO
+ * @{
+ *
+ * @file       pios_ms4525do.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ * @brief      PixHawk MS4525DO Airspeed Sensor Driver
+ * @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_MS4525DO
+
+/* Local Defs and Variables */
+
+
+static int8_t PIOS_MS4525DO_ReadI2C(uint8_t *buffer, uint8_t len)
+{
+    const struct pios_i2c_txn txn_list[] = {
+        {
+            .info = __func__,
+            .addr = MS4525DO_I2C_ADDR,
+            .rw   = PIOS_I2C_TXN_READ,
+            .len  = len,
+            .buf  = buffer,
+        }
+    };
+
+    return PIOS_I2C_Transfer(PIOS_I2C_MS4525DO_ADAPTER, txn_list, NELEMENTS(txn_list));
+}
+
+
+// values has to ba an arrray with two elements
+// values stay untouched on error
+int8_t PIOS_MS4525DO_Read(uint16_t *values)
+{
+    uint8_t data[4];
+    int8_t retVal  = PIOS_MS4525DO_ReadI2C(data, sizeof(data));
+
+    uint8_t status = data[0] & 0xC0;
+
+    if (status == 0x80) {
+        /* stale data */
+        return -5;
+    } else if (status == 0xC0) {
+        /* device probably broken */
+        return -6;
+    }
+
+    /* differential pressure */
+    values[0]  = (data[0] << 8);
+    values[0] += data[1];
+
+    /* temperature */
+    values[1]  = (data[2] << 8);
+    values[1] += data[3];
+    values[1]  = (values[1] >> 5);
+
+    return retVal;
+}
+
+#endif /* PIOS_INCLUDE_MS4525DO */

flight/pios/common/pios_ms5611.c

@@ -32,6 +32,7 @@
 
 #ifdef PIOS_INCLUDE_MS5611
 
+#define POW2(x) (1 << x)
 
 // TODO: Clean this up.  Getting around old constant.
 #define PIOS_MS5611_OVERSAMPLING oversampling
@@ -52,6 +53,9 @@ static int32_t lastConversionStart;
 static int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len);
 static int32_t PIOS_MS5611_WriteCommand(uint8_t command);
 
+// Second order temperature compensation. Temperature offset
+static int64_t compensation_t2;
+
 // Move into proper driver structure with cfg stored
 static uint32_t oversampling;
 static const struct pios_ms5611_cfg *dev_cfg;
@@ -74,6 +78,9 @@ void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device)
 
     uint8_t data[2];
 
+    // reset temperature compensation values
+    compensation_t2 = 0;
+
     /* Calibration parameters */
     for (int i = 0; i < 6; i++) {
         PIOS_MS5611_Read(MS5611_CALIB_ADDR + i * 2, data, 2);
@@ -160,7 +167,6 @@ uint32_t PIOS_MS5611_GetDelayUs()
 
 /**
  * Read the ADC conversion value (once ADC conversion has completed)
- * \param[in] PresOrTemp BMP085_PRES_ADDR or BMP085_TEMP_ADDR
  * \return 0 if successfully read the ADC, -1 if failed
  */
 int32_t PIOS_MS5611_ReadADC(void)
@@ -184,24 +190,57 @@ int32_t PIOS_MS5611_ReadADC(void)
         }
 
         RawTemperature = (Data[0] << 16) | (Data[1] << 8) | Data[2];
-
+        // Difference between actual and reference temperature
-        deltaTemp = ((int32_t)RawTemperature) - (CalibData.C[4] << 8);
+        // dT = D2 - TREF = D2 - C5 * 2^8
-        Temperature    = 2000l + ((deltaTemp * CalibData.C[5]) >> 23);
+        deltaTemp   = ((int32_t)RawTemperature) - (CalibData.C[4] * POW2(8));
+        // 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));
     } else {
         int64_t Offset;
         int64_t Sens;
+        // used for second order temperature compensation
+        int64_t Offset2 = 0;
+        int64_t Sens2   = 0;
 
         /* Read the pressure conversion */
         if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) {
             return -1;
         }
+        // check if temperature is less than 20°C
+        if (Temperature < 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);
+            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) {
+                // OFF2 = OFF2 + 7 ⋅ (TEMP + 1500)^2
+                // SENS2 = SENS2 + 11 ⋅ (TEMP + 1500)^2 / 2
+                int64_t tcorr2 = (Temperature + 1500) * (Temperature + 1500);
+                Offset2 += 7 * tcorr2;
+                Sens2   += (11 * tcorr2) / 2;
+            }
+        } else {
+            compensation_t2 = 0;
+            Offset2 = 0;
+            Sens2 = 0;
+        }
         RawPressure = ((Data[0] << 16) | (Data[1] << 8) | Data[2]);
-
+        // Offset at actual temperature
-        Offset   = (((int64_t)CalibData.C[1]) << 16) + ((((int64_t)CalibData.C[3]) * deltaTemp) >> 7);
+        // OFF = OFFT1 + TCO * dT = C2 * 2^16 + (C4 * dT) / 2^7
-        Sens     = ((int64_t)CalibData.C[0]) << 15;
+        Offset   = ((int64_t)CalibData.C[1]) * POW2(16) + (((int64_t)CalibData.C[3]) * deltaTemp) / POW2(7) - Offset2;
-        Sens     = Sens + ((((int64_t)CalibData.C[2]) * deltaTemp) >> 8);
+        // Sensitivity at actual temperature
-
+        // SENS = SENST1 + TCS * dT = C1 * 2^15 + (C3 * dT) / 2^8
-        Pressure = (((((int64_t)RawPressure) * Sens) >> 21) - Offset) >> 15;
+        Sens     = ((int64_t)CalibData.C[0]) * POW2(15) + (((int64_t)CalibData.C[2]) * deltaTemp) / POW2(8) - Sens2;
+        // Temperature compensated pressure (10…1200mbar with 0.01mbar resolution)
+        // P = D1 * SENS - OFF = (D1 * SENS / 2^21 - OFF) / 2^15
+        Pressure = (((((int64_t)RawPressure) * Sens) / POW2(21)) - Offset) / POW2(15);
     }
     return 0;
 }
@@ -211,7 +250,8 @@ int32_t PIOS_MS5611_ReadADC(void)
  */
 float PIOS_MS5611_GetTemperature(void)
 {
-    return ((float)Temperature) / 100.0f;
+    // Apply the second order low and very low temperature compensation offset
+    return ((float)(Temperature - compensation_t2)) / 100.0f;
 }
 
 /**

flight/pios/common/pios_rfm22b.c

@@ -64,7 +64,7 @@
 
 /* Local Defines */
 #define STACK_SIZE_BYTES                 200
-#define TASK_PRIORITY                    (tskIDLE_PRIORITY + 2)
+#define TASK_PRIORITY                    (tskIDLE_PRIORITY + 4) // flight control relevant device driver (ppm link)
 #define ISR_TIMEOUT                      1 // ms
 #define EVENT_QUEUE_SIZE                 5
 #define RFM22B_DEFAULT_RX_DATARATE       RFM22_datarate_9600
@@ -139,7 +139,11 @@ static const uint8_t FULL_PREAMBLE[FIFO_SIZE] = {
     PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE,
     PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE
 }; // 64 bytes
-static const uint8_t HEADER[(TX_PREAMBLE_NIBBLES + 1) / 2 + 2] = { PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, SYNC_BYTE_1, SYNC_BYTE_2 };
+
+static const uint8_t HEADER[(TX_PREAMBLE_NIBBLES + 1) / 2 + 2] = {
+    PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, SYNC_BYTE_1, SYNC_BYTE_2
+};
+
 static const uint8_t OUT_FF[64] = {
     0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
     0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
@@ -150,8 +154,21 @@ static const uint8_t OUT_FF[64] = {
     0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
     0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF
 };
+
 // The randomized channel list.
-static const uint8_t channel_list[] = { 68, 34, 2, 184, 166, 94, 204, 18, 47, 118, 239, 176, 5, 213, 218, 186, 104, 160, 199, 209, 231, 197, 92, 191, 88, 129, 40, 19, 93, 200, 156, 14, 247, 182, 193, 194, 208, 210, 248, 76, 244, 48, 179, 105, 25, 74, 155, 203, 39, 97, 195, 81, 83, 180, 134, 172, 235, 132, 198, 119, 207, 154, 0, 61, 140, 171, 245, 26, 95, 3, 22, 62, 169, 55, 127, 144, 45, 33, 170, 91, 158, 167, 63, 201, 41, 21, 190, 51, 103, 49, 189, 205, 240, 89, 181, 149, 6, 157, 249, 230, 115, 72, 163, 17, 29, 99, 28, 117, 219, 73, 78, 53, 69, 216, 161, 124, 110, 242, 214, 145, 13, 11, 220, 113, 138, 58, 54, 162, 237, 37, 152, 187, 232, 77, 126, 85, 38, 238, 173, 23, 188, 100, 131, 226, 31, 9, 114, 106, 221, 42, 233, 139, 4, 241, 96, 211, 8, 98, 121, 147, 24, 217, 27, 87, 122, 125, 135, 148, 178, 71, 206, 57, 141, 35, 30, 246, 159, 16, 32, 15, 229, 20, 12, 223, 150, 101, 79, 56, 102, 111, 174, 236, 137, 143, 52, 225, 64, 224, 112, 168, 243, 130, 108, 202, 123, 146, 228, 75, 46, 153, 7, 192, 175, 151, 222, 59, 82, 90, 1, 65, 109, 44, 165, 84, 43, 36, 128, 196, 67, 80, 136, 86, 70, 234, 66, 185, 10, 164, 177, 116, 50, 107, 183, 215, 212, 60, 227, 133, 120, 142 };
+static const uint8_t channel_list[] = {
+    68,  34,  2,   184, 166, 94,  204, 18,  47,  118, 239, 176, 5,   213, 218, 186, 104, 160, 199, 209, 231, 197, 92,
+    191, 88,  129, 40,  19,  93,  200, 156, 14,  247, 182, 193, 194, 208, 210, 248, 76,  244, 48,  179, 105, 25,  74,
+    155, 203, 39,  97,  195, 81,  83,  180, 134, 172, 235, 132, 198, 119, 207, 154, 0,   61,  140, 171, 245, 26,  95,
+    3,   22,  62,  169, 55,  127, 144, 45,  33,  170, 91,  158, 167, 63,  201, 41,  21,  190, 51,  103, 49,  189, 205,
+    240, 89,  181, 149, 6,   157, 249, 230, 115, 72,  163, 17,  29,  99,  28,  117, 219, 73,  78,  53,  69,  216, 161,
+    124, 110, 242, 214, 145, 13,  11,  220, 113, 138, 58,  54,  162, 237, 37,  152, 187, 232, 77,  126, 85,  38,  238,
+    173, 23,  188, 100, 131, 226, 31,  9,   114, 106, 221, 42,  233, 139, 4,   241, 96,  211, 8,   98,  121, 147, 24,
+    217, 27,  87,  122, 125, 135, 148, 178, 71,  206, 57,  141, 35,  30,  246, 159, 16,  32,  15,  229, 20,  12,  223,
+    150, 101, 79,  56,  102, 111, 174, 236, 137, 143, 52,  225, 64,  224, 112, 168, 243, 130, 108, 202, 123, 146, 228,
+    75,  46,  153, 7,   192, 175, 151, 222, 59,  82,  90,  1,   65,  109, 44,  165, 84,  43,  36,  128, 196, 67,  80,
+    136, 86,  70,  234, 66,  185, 10,  164, 177, 116, 50,  107, 183, 215, 212, 60,  227, 133, 120, 14
+};
 
 /* Local function forwared declarations */
 static void pios_rfm22_task(void *parameters);
@@ -1221,7 +1238,7 @@ static void pios_rfm22_inject_event(struct pios_rfm22b_dev *rfm22b_dev, enum pio
             // Something went fairly seriously wrong
             rfm22b_dev->errors++;
         }
-        portEND_SWITCHING_ISR((pxHigherPriorityTaskWoken2 == pdTRUE) || (pxHigherPriorityTaskWoken2 == pdTRUE));
+        portEND_SWITCHING_ISR((pxHigherPriorityTaskWoken1 == pdTRUE) || (pxHigherPriorityTaskWoken2 == pdTRUE));
     } else {
         // Store the event.
         if (xQueueSend(rfm22b_dev->eventQueue, &event, portMAX_DELAY) != pdTRUE) {

flight/pios/common/pios_sbus.c

@@ -250,7 +250,7 @@ static void PIOS_SBus_UpdateState(struct pios_sbus_state *state, uint8_t b)
         state->received_data[state->byte_count - 1] = b;
         state->byte_count++;
     } else {
-        if (b == SBUS_EOF_BYTE) {
+        if (b == SBUS_EOF_BYTE || (b % SBUS_R7008SB_EOF_COUNTER_MASK) == SBUS_R7008SB_EOF_BYTE) {
             /* full frame received */
             uint8_t flags = state->received_data[SBUS_FRAME_LENGTH - 3];
             if (flags & SBUS_FLAG_FL) {

flight/pios/common/pios_task_monitor.c

@@ -33,6 +33,7 @@
 static xSemaphoreHandle mLock;
 static xTaskHandle *mTaskHandles;
 static uint32_t mLastMonitorTime;
+static uint32_t mLastIdleMonitorTime;
 static uint16_t mMaxTasks;
 
 /**
@@ -53,9 +54,11 @@ int32_t PIOS_TASK_MONITOR_Initialize(uint16_t max_tasks)
 
     mMaxTasks = max_tasks;
 #if (configGENERATE_RUN_TIME_STATS == 1)
-    mLastMonitorTime = portGET_RUN_TIME_COUNTER_VALUE();
+    mLastMonitorTime     = portGET_RUN_TIME_COUNTER_VALUE();
+    mLastIdleMonitorTime = portGET_RUN_TIME_COUNTER_VALUE();
 #else
-    mLastMonitorTime = 0;
+    mLastMonitorTime     = 0;
+    mLastIdleMonitorTime = 0;
 #endif
     return 0;
 }
@@ -146,4 +149,31 @@ void PIOS_TASK_MONITOR_ForEachTask(TaskMonitorTaskInfoCallback callback, void *c
     xSemaphoreGiveRecursive(mLock);
 }
 
+uint8_t PIOS_TASK_MONITOR_GetIdlePercentage()
+{
+#if defined(ARCH_POSIX) || defined(ARCH_WIN32)
+    return 50;
+
+#elif (configGENERATE_RUN_TIME_STATS == 1)
+    xSemaphoreTakeRecursive(mLock, portMAX_DELAY);
+
+    uint32_t currentTime = portGET_RUN_TIME_COUNTER_VALUE();
+
+    /* avoid divide-by-zero if the interval is too small */
+    uint32_t deltaTime   = ((currentTime - mLastIdleMonitorTime) / 100) ? : 1;
+    mLastIdleMonitorTime = currentTime;
+    uint8_t running_time_percentage = 0;
+
+    /* Generate idle time percentage stats */
+    running_time_percentage = uxTaskGetRunTime(xTaskGetIdleTaskHandle()) / deltaTime;
+    xSemaphoreGiveRecursive(mLock);
+    return running_time_percentage;
+
+#else
+    return 0;
+
+#endif
+}
+
+
 #endif // PIOS_INCLUDE_TASK_MONITOR

flight/pios/inc/pios_callbackscheduler.h

@@ -1,9 +1,9 @@
 /**
  ******************************************************************************
  *
- * @file       callbackscheduler.h
+ * @file       pios_callbackscheduler.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
- * @brief      Include files of the uavobjectlist library
+ * @brief      Include files of the PIOS_CALLBACKSCHEDULER
  * @see        The GNU Public License (GPL) Version 3
  *
  *****************************************************************************/
@@ -23,8 +23,8 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 
-#ifndef CALLBACKSCHEDULER_H
+#ifndef PIOS_CALLBACKSCHEDULER_H
-#define CALLBACKSCHEDULER_H
+#define PIOS_CALLBACKSCHEDULER_H
 
 // Public types
 typedef enum {
@@ -48,7 +48,7 @@ typedef enum {
 // ...ABcABdABxABcABdAByABcABdABxABcABdABy...
 // However if only the 3 callbacks, A, c and x want to execute, you will get:
 // ...AcAxAcAxAcAxAcAxAcAxAcAxAcAxAcAxAcAx...
-// And if onlz A and y need execution it will be:
+// And if only A and y need execution it will be:
 // ...AyAyAyAyAyAyAyAyAyAyAyAyAyAyAyAyAyAy...
 // despite their different priority they would get treated equally in this case.
 //
@@ -113,7 +113,7 @@ typedef struct DelayedCallbackInfoStruct DelayedCallbackInfo;
  * must be called before any other functions are called
  * \return Success (0), failure (-1)
  */
-int32_t CallbackSchedulerInitialize();
+int32_t PIOS_CALLBACKSCHEDULER_Initialize();
 
 /**
  * Start all scheduler tasks
@@ -125,7 +125,7 @@ int32_t CallbackSchedulerInitialize();
  * they can be marked for later execution by executing the dispatch function.
  * \return Success (0), failure (-1)
  */
-int32_t CallbackSchedulerStart();
+int32_t PIOS_CALLBACKSCHEDULER_Start();
 
 /**
  * Register a new callback to be called by a delayed callback scheduler task.
@@ -135,12 +135,14 @@ int32_t CallbackSchedulerStart();
  * \param[in] priority Priority of the callback compared to other callbacks scheduled by the same delayed callback scheduler task.
  * \param[in] priorityTask Task priority of the scheduler task. One scheduler task will be spawned for each distinct value specified, further callbacks created  with the same priorityTask will all be handled by the same delayed callback scheduler task and scheduled according to their individual callback priorities
  * \param[in] stacksize The stack requirements of the callback when called by the scheduler.
+ * \param[in] callbackID - CallbackInfoRunningElem from CallbackInfo UAVObject, unique identified to collect stats for the callback, -1 to ignore!
  * \return CallbackInfo Pointer on success, NULL if failed.
  */
-DelayedCallbackInfo *DelayedCallbackCreate(
+DelayedCallbackInfo *PIOS_CALLBACKSCHEDULER_Create(
     DelayedCallback cb,
     DelayedCallbackPriority priority,
     DelayedCallbackPriorityTask priorityTask,
+    int16_t callbackID,
     uint32_t stacksize);
 
 /**
@@ -155,7 +157,7 @@ DelayedCallbackInfo *DelayedCallbackCreate(
  * UPDATEMODE_OVERRIDE: The callback will be rescheduled in any case, effectively overriding any previous schedule. (sooner+later=override)
  * \return 0: not scheduled, previous schedule takes precedence, 1: new schedule, 2: previous schedule overridden
  */
-int32_t DelayedCallbackSchedule(
+int32_t PIOS_CALLBACKSCHEDULER_Schedule(
     DelayedCallbackInfo *cbinfo,
     int32_t milliseconds,
     DelayedCallbackUpdateMode updatemode);
@@ -166,7 +168,7 @@ int32_t DelayedCallbackSchedule(
  * \param[in] *cbinfo the callback handle
  * \return Success (-1), failure (0)
  */
-int32_t DelayedCallbackDispatch(DelayedCallbackInfo *cbinfo);
+int32_t PIOS_CALLBACKSCHEDULER_Dispatch(DelayedCallbackInfo *cbinfo);
 
 /**
  * Dispatch an event by invoking the supplied callback. The function
@@ -182,6 +184,36 @@ int32_t DelayedCallbackDispatch(DelayedCallbackInfo *cbinfo);
  * Check the demo task for your port to find the syntax required.
  * \return Success (-1), failure (0)
  */
-int32_t DelayedCallbackDispatchFromISR(DelayedCallbackInfo *cbinfo, long *pxHigherPriorityTaskWoken);
+int32_t PIOS_CALLBACKSCHEDULER_DispatchFromISR(DelayedCallbackInfo *cbinfo, long *pxHigherPriorityTaskWoken);
 
-#endif // CALLBACKSCHEDULER_H
+/**
+ * Information about a running callback that has been registered
+ * via a call to PIOS_CALLBACKSCHEDULER_Create().
+ */
+struct pios_callback_info {
+    /** Remaining task stack in bytes -1 for detected stack overflow. */
+    int32_t  stack_remaining;
+    /** Flag indicating whether or not the task is running. */
+    bool     is_running;
+    /** Count of executions of the callback since system start */
+    uint32_t running_time_count;
+};
+
+/**
+ * Iterator callback, called for each monitored callback by PIOS_CALLBACKSCHEDULER_ForEachCallback().
+ *
+ * @param task_id   The id of the task the task_info refers to.
+ * @param task_info Information about the task identified by task_id.
+ * @param context   Context information optionally provided by the caller to PIOS_TASK_MONITOR_TasksIterate()
+ */
+typedef void (*CallbackSchedulerCallbackInfoCallback)(int16_t task_id, const struct pios_callback_info *callback_info, void *context);
+
+/**
+ * Iterator. Iterates over all callbacks and all scheduler tasks and retrieves information
+ *
+ * @param[in] callback  Callback function to receive the data - will be called in same task context as the callerThe id of the task the task_info refers to.
+ * @param     context   Context information optionally provided to the callback.
+ */
+void PIOS_CALLBACKSCHEDULER_ForEachCallback(CallbackSchedulerCallbackInfoCallback callback, void *context);
+
+#endif // PIOS_CALLBACKSCHEDULER_H

flight/pios/inc/pios_deltatime.h

@@ -0,0 +1,53 @@
+/**
+ ******************************************************************************
+ * @addtogroup PIOS PIOS Core hardware abstraction layer
+ * @{
+ * @addtogroup PIOS_DELTATIME time measurement Functions
+ * @brief PiOS Delay functionality
+ * @{
+ *
+ * @file       pios_deltatime.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Settings functions header
+ * @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_DELTATIME_H
+#define PIOS_DELTATIME_H
+
+struct PiOSDeltatimeConfigStruct {
+    uint32_t last;
+    float    average;
+    float    min;
+    float    max;
+    float    alpha;
+};
+typedef struct PiOSDeltatimeConfigStruct PiOSDeltatimeConfig;
+
+/* Public Functions */
+void PIOS_DELTATIME_Init(PiOSDeltatimeConfig *config, float average, float min, float max, float alpha);
+
+float PIOS_DELTATIME_GetAverageSeconds(PiOSDeltatimeConfig *config);
+
+#endif /* PIOS_DELTATIME_H */
+
+/**
+ * @}
+ * @}
+ */

flight/pios/inc/pios_i2c_priv.h

@@ -92,8 +92,12 @@ struct pios_i2c_adapter {
     uint8_t busy;
 #endif
 
-    bool    bus_error;
+    /* variables for transfer timeouts */
-    bool    nack;
+    uint32_t transfer_delay_uS; // approx time to transfer one byte, calculated later basen on setting use here time based on 100 kbits/s
+    uint32_t transfer_timeout_ticks; // take something tha makes sense for small transaction, calculated later based upon transmission desired
+
+    bool     bus_error;
+    bool     nack;
 
     volatile enum i2c_adapter_state curr_state;
     const struct pios_i2c_txn *first_txn;

flight/pios/inc/pios_ms4525do.h

@@ -0,0 +1,45 @@
+/**
+ ******************************************************************************
+ * @addtogroup PIOS PIOS Core hardware abstraction layer
+ * @{
+ * @addtogroup PIOS_MS4525DO MS4525DO Functions
+ * @brief Hardware functions to deal with the PixHawk Airspeed Sensor based on MS4525DO
+ * @{
+ *
+ * @file       pios_ms4525do.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ * @brief      PixHawk MS4525DO Airspeed Sensor Driver
+ * @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_MS4525DO_H
+#define PIOS_MS4525DO_H
+
+// Interface Type I chip
+#define MS4525DO_I2C_ADDR 0x28
+// Interface Type J chip
+/* #define MS4525DO_I2C_ADDR 0x36 */
+// Interface Type K chip
+/* #define MS4525DO_I2C_ADDR 0x46 */
+
+
+extern int8_t PIOS_MS4525DO_Request(void);
+extern int8_t PIOS_MS4525DO_Read(uint16_t *values);
+
+#endif /* PIOS_MS4525DO_H */

flight/pios/inc/pios_sbus_priv.h

@@ -50,14 +50,24 @@
  *                   0x08 - failsafe flag,
  *                   0xf0 - reserved
  *  1 byte  - 0x00 (end of frame byte)
+ *
+ * The R7008SB receiver has four different end of frame bytes, which rotates in order:
+ *    00000100
+ *    00010100
+ *    00100100
+ *    00110100
  */
-#define SBUS_FRAME_LENGTH (1 + 22 + 1 + 1)
+
-#define SBUS_SOF_BYTE     0x0f
+#define SBUS_FRAME_LENGTH             (1 + 22 + 1 + 1)
-#define SBUS_EOF_BYTE     0x00
+#define SBUS_SOF_BYTE                 0x0f
-#define SBUS_FLAG_DC1     0x01
+#define SBUS_EOF_BYTE                 0x00
-#define SBUS_FLAG_DC2     0x02
+#define SBUS_FLAG_DC1                 0x01
-#define SBUS_FLAG_FL      0x04
+#define SBUS_FLAG_DC2                 0x02
-#define SBUS_FLAG_FS      0x08
+#define SBUS_FLAG_FL                  0x04
+#define SBUS_FLAG_FS                  0x08
+
+#define SBUS_R7008SB_EOF_COUNTER_MASK 0xCF
+#define SBUS_R7008SB_EOF_BYTE         0x04
 
 /*
  * S.Bus protocol provides 16 proportional and 2 discrete channels.

flight/pios/inc/pios_task_monitor.h

@@ -104,4 +104,9 @@ typedef void (*TaskMonitorTaskInfoCallback)(uint16_t task_id, const struct pios_
  */
 extern void PIOS_TASK_MONITOR_ForEachTask(TaskMonitorTaskInfoCallback callback, void *context);
 
+/**
+ * Return the idle task running time percentage.
+ */
+extern uint8_t PIOS_TASK_MONITOR_GetIdlePercentage();
+
 #endif // PIOS_TASK_MONITOR_H

flight/pios/pios.h

@@ -93,6 +93,14 @@
 #include <pios_task_monitor.h>
 #endif
 
+/* PIOS CallbackScheduler */
+#ifdef PIOS_INCLUDE_CALLBACKSCHEDULER
+#ifndef PIOS_INCLUDE_FREERTOS
+#error PiOS CallbackScheduler requires PIOS_INCLUDE_FREERTOS to be defined
+#endif
+#include <pios_callbackscheduler.h>
+#endif
+
 /* PIOS bootloader helper */
 #ifdef PIOS_INCLUDE_BL_HELPER
 /* #define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT */
@@ -102,6 +110,7 @@
 /* PIOS system functions */
 #ifdef PIOS_INCLUDE_DELAY
 #include <pios_delay.h>
+#include <pios_deltatime.h>
 #endif
 
 #ifdef PIOS_INCLUDE_INITCALL
@@ -220,6 +229,12 @@
 #include <pios_etasv3.h>
 #endif
 
+#ifdef PIOS_INCLUDE_MS4525DO
+/* PixHawk Airspeed Sensor based on MS4525DO */
+#include <pios_ms4525do.h>
+#endif
+
+
 #ifdef PIOS_INCLUDE_HCSR04
 /* HC-SR04 Ultrasonic Sensor */
 #include <pios_hcsr04.h>

flight/pios/pios_sim_posix.h

@@ -47,6 +47,14 @@
 #include <pios_task_monitor.h>
 #endif
 
+/* PIOS CallbackScheduler */
+#ifdef PIOS_INCLUDE_CALLBACKSCHEDULER
+#ifndef PIOS_INCLUDE_FREERTOS
+#error PiOS CallbackScheduler requires PIOS_INCLUDE_FREERTOS to be defined
+#endif
+#include <pios_callbackscheduler.h>
+#endif
+
 /* C Lib Includes */
 #include <stdio.h>
 #include <stdlib.h>
@@ -79,6 +87,7 @@ extern void PIOS_LED_Init(void);
 #include <pios_wdg.h>
 #include <pios_debug.h>
 #include <pios_debuglog.h>
+#include <pios_deltatime.h>
 #include <pios_crc.h>
 #include <pios_rcvr.h>
 #include <pios_flash.h>

flight/pios/stm32f10x/inc/usb_conf.h

@@ -29,25 +29,24 @@
 /* --------------   Buffer Description Table  -----------------*/
 /*-------------------------------------------------------------*/
 /* buffer table base address */
-/* buffer table base address */
 #define BTABLE_ADDRESS (0x00)
 
-/* EP0  */
+/* EP0 (Control) */
 /* rx/tx buffer base address */
 #define ENDP0_RXADDR   (0x20)
 #define ENDP0_TXADDR   (0x40)
 
-/* EP1  */
+/* EP1 (HID) */
 /* rx/tx buffer base address */
 #define ENDP1_TXADDR   (0x60)
-#define ENDP1_RXADDR   (0x80)
+#define ENDP1_RXADDR   (0xA0)
 
-/* EP2 */
+/* EP2 (CDC Call Control) */
 /* rx/tx buffer base address */
 #define ENDP2_TXADDR   (0x100)
 #define ENDP2_RXADDR   (0x140)
 
-/* EP3 */
+/* EP3 (CDC Data) */
 /* rx/tx buffer base address */
 #define ENDP3_TXADDR   (0x180)
 #define ENDP3_RXADDR   (0x1C0)

flight/pios/stm32f10x/pios_i2c.c

@@ -404,8 +404,17 @@ static void go_starting(struct pios_i2c_adapter *i2c_adapter)
     PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn);
     PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn);
 
-    i2c_adapter->active_byte = &(i2c_adapter->active_txn->buf[0]);
+    // check for an empty read/write
-    i2c_adapter->last_byte   = &(i2c_adapter->active_txn->buf[i2c_adapter->active_txn->len - 1]);
+    if (i2c_adapter->active_txn->buf != NULL && i2c_adapter->active_txn->len != 0) {
+        // Data available
+        i2c_adapter->active_byte = &(i2c_adapter->active_txn->buf[0]);
+        i2c_adapter->last_byte   = &(i2c_adapter->active_txn->buf[i2c_adapter->active_txn->len - 1]);
+    } else {
+        // No Data available => Empty read/write
+        i2c_adapter->last_byte   = NULL;
+        i2c_adapter->active_byte = i2c_adapter->last_byte + 1;
+    }
+
 
     I2C_GenerateSTART(i2c_adapter->cfg->regs, ENABLE);
     if (i2c_adapter->active_txn->rw == PIOS_I2C_TXN_READ) {
@@ -924,9 +933,9 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[],
 {
     struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id;
 
-    bool valid = PIOS_I2C_validate(i2c_adapter);
+    if (!PIOS_I2C_validate(i2c_adapter)) {
-
+        return -1;
-    PIOS_Assert(valid)
+    }
 
     PIOS_DEBUG_Assert(txn_list);
     PIOS_DEBUG_Assert(num_txns);
@@ -968,6 +977,14 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[],
     semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE);
 #endif
 
+    // Estimate bytes of transmission. Per txns: 1 adress byte + length
+    i2c_adapter->transfer_timeout_ticks = num_txns;
+    for (uint32_t i = 0; i < num_txns; i++) {
+        i2c_adapter->transfer_timeout_ticks += txn_list[i].len;
+    }
+    // timeout if it takes eight times the expected time
+    i2c_adapter->transfer_timeout_ticks <<= 3;
+
     i2c_adapter->bus_error = false;
     i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_START);
 
@@ -983,7 +1000,15 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[],
 
     /* Spin waiting for the transfer to finish */
     while (!i2c_adapter_fsm_terminated(i2c_adapter)) {
-        ;
+        /* sleep 9 clock ticks (1 byte), because FSM can't be faster than one byte
+           FIXME: clock streching could make problems, but citing NPX: alsmost no slave device implements clock stretching
+           three times the expected time should cover clock delay */
+        PIOS_DELAY_WaituS(i2c_adapter->transfer_delay_uS);
+
+        i2c_adapter->transfer_timeout_ticks--;
+        if (i2c_adapter->transfer_timeout_ticks == 0) {
+            break;
+        }
     }
 
     if (i2c_adapter_wait_for_stopped(i2c_adapter)) {
@@ -1010,9 +1035,9 @@ void PIOS_I2C_EV_IRQ_Handler(uint32_t i2c_id)
 {
     struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id;
 
-    bool valid = PIOS_I2C_validate(i2c_adapter);
+    if (!PIOS_I2C_validate(i2c_adapter)) {
-
+        return;
-    PIOS_Assert(valid)
+    }
 
     uint32_t event = I2C_GetLastEvent(i2c_adapter->cfg->regs);
 
@@ -1136,9 +1161,10 @@ void PIOS_I2C_ER_IRQ_Handler(uint32_t i2c_id)
 {
     struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id;
 
-    bool valid = PIOS_I2C_validate(i2c_adapter);
+    if (!PIOS_I2C_validate(i2c_adapter)) {
+        return;
+    }
 
-    PIOS_Assert(valid)
 
 #if defined(PIOS_I2C_DIAGNOSTICS)
     uint32_t event = I2C_GetLastEvent(i2c_adapter->cfg->regs);

flight/pios/stm32f4xx/inc/usb_conf.h

@@ -32,8 +32,6 @@
 /* Includes ------------------------------------------------------------------*/
 #include "stm32f4xx.h"
 
-#include "usb_conf.h"
-
 /** @addtogroup USB_OTG_DRIVER
  * @{
  */

flight/pios/stm32f4xx/pios_i2c.c

@@ -94,26 +94,19 @@ static void go_bus_error(struct pios_i2c_adapter *i2c_adapter);
 static void go_stopping(struct pios_i2c_adapter *i2c_adapter);
 static void go_stopped(struct pios_i2c_adapter *i2c_adapter);
 static void go_starting(struct pios_i2c_adapter *i2c_adapter);
-static void go_r_any_txn_addr(struct pios_i2c_adapter *i2c_adapter);
-static void go_r_more_txn_pre_one(struct pios_i2c_adapter *i2c_adapter);
-static void go_r_any_txn_pre_first(struct pios_i2c_adapter *i2c_adapter);
-static void go_r_any_txn_pre_middle(struct pios_i2c_adapter *i2c_adapter);
-static void go_r_more_txn_pre_last(struct pios_i2c_adapter *i2c_adapter);
-static void go_r_any_txn_post_last(struct pios_i2c_adapter *i2c_adapter);
 
 static void go_r_any_txn_addr(struct pios_i2c_adapter *i2c_adapter);
+static void go_r_more_txn_pre_one(struct pios_i2c_adapter *i2c_adapter);
 static void go_r_last_txn_pre_one(struct pios_i2c_adapter *i2c_adapter);
 static void go_r_any_txn_pre_first(struct pios_i2c_adapter *i2c_adapter);
 static void go_r_any_txn_pre_middle(struct pios_i2c_adapter *i2c_adapter);
 static void go_r_last_txn_pre_last(struct pios_i2c_adapter *i2c_adapter);
+static void go_r_more_txn_pre_last(struct pios_i2c_adapter *i2c_adapter);
 static void go_r_any_txn_post_last(struct pios_i2c_adapter *i2c_adapter);
 
 static void go_w_any_txn_addr(struct pios_i2c_adapter *i2c_adapter);
 static void go_w_any_txn_middle(struct pios_i2c_adapter *i2c_adapter);
 static void go_w_more_txn_last(struct pios_i2c_adapter *i2c_adapter);
-
-static void go_w_any_txn_addr(struct pios_i2c_adapter *i2c_adapter);
-static void go_w_any_txn_middle(struct pios_i2c_adapter *i2c_adapter);
 static void go_w_last_txn_last(struct pios_i2c_adapter *i2c_adapter);
 
 static void go_nack(struct pios_i2c_adapter *i2c_adapter);
@@ -414,8 +407,16 @@ static void go_starting(struct pios_i2c_adapter *i2c_adapter)
     PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn);
     PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn);
 
-    i2c_adapter->active_byte = &(i2c_adapter->active_txn->buf[0]);
+    // check for an empty read/write
-    i2c_adapter->last_byte   = &(i2c_adapter->active_txn->buf[i2c_adapter->active_txn->len - 1]);
+    if (i2c_adapter->active_txn->buf != NULL && i2c_adapter->active_txn->len != 0) {
+        // Data available
+        i2c_adapter->active_byte = &(i2c_adapter->active_txn->buf[0]);
+        i2c_adapter->last_byte   = &(i2c_adapter->active_txn->buf[i2c_adapter->active_txn->len - 1]);
+    } else {
+        // No Data available => Empty read/write
+        i2c_adapter->last_byte   = NULL;
+        i2c_adapter->active_byte = i2c_adapter->last_byte + 1;
+    }
 
     I2C_GenerateSTART(i2c_adapter->cfg->regs, ENABLE);
     if (i2c_adapter->active_txn->rw == PIOS_I2C_TXN_READ) {
@@ -753,6 +754,11 @@ static void i2c_adapter_reset_bus(struct pios_i2c_adapter *i2c_adapter)
     /* Initialize the I2C block */
     I2C_Init(i2c_adapter->cfg->regs, (I2C_InitTypeDef *)&(i2c_adapter->cfg->init));
 
+    // for delays during transfer timeouts
+    // one tick correspond to transmission of 1 byte i.e. 9 clock ticks
+    i2c_adapter->transfer_delay_uS = 9000000 / (((I2C_InitTypeDef *)&(i2c_adapter->cfg->init))->I2C_ClockSpeed);
+
+
     if (i2c_adapter->cfg->regs->SR2 & I2C_FLAG_BUSY) {
         /* Reset the I2C block */
         I2C_SoftwareResetCmd(i2c_adapter->cfg->regs, ENABLE);
@@ -760,7 +766,6 @@ static void i2c_adapter_reset_bus(struct pios_i2c_adapter *i2c_adapter)
     }
 }
 
-#include <pios_i2c_priv.h>
 
 /* Return true if the FSM is in a terminal state */
 static bool i2c_adapter_fsm_terminated(struct pios_i2c_adapter *i2c_adapter)
@@ -788,9 +793,18 @@ static bool i2c_adapter_callback_handler(struct pios_i2c_adapter *i2c_adapter)
     xSemaphoreGive(i2c_adapter->sem_ready);
 #endif /* USE_FREERTOS */
 
+    /* transfer_timeout_ticks is set by PIOS_I2C_Transfer_Callback */
     /* Spin waiting for the transfer to finish */
     while (!i2c_adapter_fsm_terminated(i2c_adapter)) {
-        ;
+        // sleep 1 byte, as FSM can't be faster
+        // FIXME: clock stretching could make problems, but citing NPX: alsmost no slave device implements clock stretching
+        // three times the expected time should cover clock delay
+        PIOS_DELAY_WaituS(i2c_adapter->transfer_delay_uS);
+
+        i2c_adapter->transfer_timeout_ticks--;
+        if (i2c_adapter->transfer_timeout_ticks == 0) {
+            break;
+        }
     }
 
     if (i2c_adapter_wait_for_stopped(i2c_adapter)) {
@@ -956,6 +970,7 @@ int32_t PIOS_I2C_Init(uint32_t *i2c_id, const struct pios_i2c_adapter_cfg *cfg)
     NVIC_Init((NVIC_InitTypeDef *)&(i2c_adapter->cfg->event.init));
     NVIC_Init((NVIC_InitTypeDef *)&(i2c_adapter->cfg->error.init));
 
+
     /* No error */
     return 0;
 
@@ -967,9 +982,9 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[],
 {
     struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id;
 
-    bool valid = PIOS_I2C_validate(i2c_adapter);
+    if (!PIOS_I2C_validate(i2c_adapter)) {
-
+        return -1;
-    PIOS_Assert(valid)
+    }
 
     PIOS_DEBUG_Assert(txn_list);
     PIOS_DEBUG_Assert(num_txns);
@@ -1001,12 +1016,20 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[],
 
 #ifdef USE_FREERTOS
     /* Make sure the done/ready semaphore is consumed before we start */
-    semaphore_success     &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE);
+    semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE);
 #endif
 
+    // Estimate bytes of transmission. Per txns: 1 adress byte + length
+    i2c_adapter->transfer_timeout_ticks = num_txns;
+    for (uint32_t i = 0; i < num_txns; i++) {
+        i2c_adapter->transfer_timeout_ticks += txn_list[i].len;
+    }
+    // timeout if it takes eight times the expected time
+    i2c_adapter->transfer_timeout_ticks <<= 3;
+
     i2c_adapter->callback  = NULL;
     i2c_adapter->bus_error = false;
-    i2c_adapter->nack      = false;
+    i2c_adapter->nack = false;
     i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_START);
 
     /* Wait for the transfer to complete */
@@ -1017,7 +1040,15 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[],
 
     /* Spin waiting for the transfer to finish */
     while (!i2c_adapter_fsm_terminated(i2c_adapter)) {
-        ;
+        /* sleep 9 clock ticks (1 byte), because FSM can't be faster than one byte
+           FIXME: clock stretching could make problems, but citing NPX: alsmost no slave device implements clock stretching
+           three times the expected time should cover clock delay */
+        PIOS_DELAY_WaituS(i2c_adapter->transfer_delay_uS);
+
+        i2c_adapter->transfer_timeout_ticks--;
+        if (i2c_adapter->transfer_timeout_ticks == 0) {
+            break;
+        }
     }
 
     if (i2c_adapter_wait_for_stopped(i2c_adapter)) {
@@ -1048,9 +1079,10 @@ int32_t PIOS_I2C_Transfer_Callback(uint32_t i2c_id, const struct pios_i2c_txn tx
 {
     struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id;
 
-    bool valid = PIOS_I2C_validate(i2c_adapter);
+    if (!PIOS_I2C_validate(i2c_adapter)) {
+        return -1;
+    }
 
-    PIOS_Assert(valid)
     PIOS_Assert(callback);
 
     PIOS_DEBUG_Assert(txn_list);
@@ -1080,9 +1112,18 @@ int32_t PIOS_I2C_Transfer_Callback(uint32_t i2c_id, const struct pios_i2c_txn tx
 
 #ifdef USE_FREERTOS
     /* Make sure the done/ready semaphore is consumed before we start */
-    semaphore_success     &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE);
+    semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE);
 #endif
 
+    // used in the i2c_adapter_callback_handler function
+    // Estimate bytes of transmission. Per txns: 1 adress byte + length
+    i2c_adapter->transfer_timeout_ticks = num_txns;
+    for (uint32_t i = 0; i < num_txns; i++) {
+        i2c_adapter->transfer_timeout_ticks += txn_list[i].len;
+    }
+    // timeout if it takes eight times the expected time
+    i2c_adapter->transfer_timeout_ticks <<= 3;
+
     i2c_adapter->callback  = callback;
     i2c_adapter->bus_error = false;
     i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_START);
@@ -1094,9 +1135,9 @@ void PIOS_I2C_EV_IRQ_Handler(uint32_t i2c_id)
 {
     struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id;
 
-    bool valid = PIOS_I2C_validate(i2c_adapter);
+    if (!PIOS_I2C_validate(i2c_adapter)) {
-
+        return;
-    PIOS_Assert(valid)
+    }
 
     uint32_t event = I2C_GetLastEvent(i2c_adapter->cfg->regs);
 
@@ -1204,9 +1245,9 @@ void PIOS_I2C_EV_IRQ_Handler(uint32_t i2c_id)
         /* Ignore this event and wait for TRANSMITTED in case we can't keep up */
         goto skip_event;
         break;
-    case 0x30084: /* Occurs between byte tranmistted and master mode selected */
+    case 0x30084: /* BUSY + MSL + TXE + BFT Occurs between byte transmitted and master mode selected */
-    case 0x30000: /* Need to throw away this spurious event */
+    case 0x30000: /* BUSY + MSL Need to throw away this spurious event */
-    case 0x30403 & EVENT_MASK: /* Detected this after got a NACK, probably stop bit */
+    case 0x30403 & EVENT_MASK: /* BUSY + MSL + SB + ADDR Detected this after got a NACK, probably stop bit */
         goto skip_event;
         break;
     default:
@@ -1227,16 +1268,15 @@ void PIOS_I2C_ER_IRQ_Handler(uint32_t i2c_id)
 {
     struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id;
 
-    bool valid = PIOS_I2C_validate(i2c_adapter);
+    if (!PIOS_I2C_validate(i2c_adapter)) {
+        return;
+    }
 
-    PIOS_Assert(valid)
-
-#if defined(PIOS_I2C_DIAGNOSTICS)
     uint32_t event = I2C_GetLastEvent(i2c_adapter->cfg->regs);
 
+#if defined(PIOS_I2C_DIAGNOSTICS)
     i2c_erirq_history[i2c_erirq_history_pointer] = event;
     i2c_erirq_history_pointer = (i2c_erirq_history_pointer + 1) % 5;
-
 #endif
 
     if (event & I2C_FLAG_AF) {

flight/pios/stm32f4xx/pios_wdg.c

@@ -73,7 +73,7 @@ uint16_t PIOS_WDG_Init()
         delay = 0x0fff;
     }
 #if defined(PIOS_INCLUDE_WDG)
-    DBGMCU_Config(DBGMCU_IWDG_STOP, ENABLE); // make the watchdog stop counting in debug mode
+    DBGMCU_APB1PeriphConfig(DBGMCU_IWDG_STOP, ENABLE); // OP-1272 : write in APB1 register
     IWDG_WriteAccessCmd(IWDG_WriteAccess_Enable);
     IWDG_SetPrescaler(IWDG_Prescaler_16);
     IWDG_SetReload(delay);

flight/targets/boards/coptercontrol/bootloader/inc/pios_usb_board_data.h

@@ -31,6 +31,7 @@
 #ifndef PIOS_USB_BOARD_DATA_H
 #define PIOS_USB_BOARD_DATA_H
 
+// Note : changing below length will require changes to the USB buffer setup
 #define PIOS_USB_BOARD_HID_DATA_LENGTH 64
 
 #define PIOS_USB_BOARD_EP_NUM          2

flight/targets/boards/coptercontrol/firmware/Makefile

@@ -34,6 +34,7 @@ ERASE_FLASH ?= NO
 MODULES += Attitude
 MODULES += Stabilization
 MODULES += Actuator
+MODULES += Receiver
 MODULES += ManualControl
 MODULES += FirmwareIAP
 MODULES += Telemetry
@@ -70,7 +71,6 @@ ifndef TESTAPP
     SRC += $(OPUAVTALK)/uavtalk.c
     SRC += $(OPUAVOBJ)/uavobjectmanager.c
     SRC += $(OPUAVOBJ)/eventdispatcher.c
-    SRC += $(OPUAVOBJ)/callbackscheduler.c
 
     ## UAVObjects
     SRC += $(OPUAVSYNTHDIR)/accessorydesired.c
@@ -84,16 +84,22 @@ ifndef TESTAPP
     SRC += $(OPUAVSYNTHDIR)/systemsettings.c
     SRC += $(OPUAVSYNTHDIR)/stabilizationdesired.c
     SRC += $(OPUAVSYNTHDIR)/stabilizationsettings.c
+    SRC += $(OPUAVSYNTHDIR)/stabilizationsettingsbank1.c
+    SRC += $(OPUAVSYNTHDIR)/stabilizationsettingsbank2.c
+    SRC += $(OPUAVSYNTHDIR)/stabilizationsettingsbank3.c
+    SRC += $(OPUAVSYNTHDIR)/stabilizationbank.c
     SRC += $(OPUAVSYNTHDIR)/actuatorcommand.c
     SRC += $(OPUAVSYNTHDIR)/actuatordesired.c
     SRC += $(OPUAVSYNTHDIR)/actuatorsettings.c
     SRC += $(OPUAVSYNTHDIR)/accelstate.c
+    SRC += $(OPUAVSYNTHDIR)/accelgyrosettings.c
     SRC += $(OPUAVSYNTHDIR)/gyrostate.c
     SRC += $(OPUAVSYNTHDIR)/attitudestate.c
     SRC += $(OPUAVSYNTHDIR)/manualcontrolcommand.c
     SRC += $(OPUAVSYNTHDIR)/i2cstats.c
     SRC += $(OPUAVSYNTHDIR)/watchdogstatus.c
     SRC += $(OPUAVSYNTHDIR)/manualcontrolsettings.c
+    SRC += $(OPUAVSYNTHDIR)/flightmodesettings.c
     SRC += $(OPUAVSYNTHDIR)/mixersettings.c
     SRC += $(OPUAVSYNTHDIR)/firmwareiapobj.c
     SRC += $(OPUAVSYNTHDIR)/attitudesettings.c
@@ -108,6 +114,7 @@ ifndef TESTAPP
     SRC += $(OPUAVSYNTHDIR)/relaytuningsettings.c
     SRC += $(OPUAVSYNTHDIR)/relaytuning.c
     SRC += $(OPUAVSYNTHDIR)/taskinfo.c
+    SRC += $(OPUAVSYNTHDIR)/callbackinfo.c
     SRC += $(OPUAVSYNTHDIR)/mixerstatus.c
     SRC += $(OPUAVSYNTHDIR)/ratedesired.c
     SRC += $(OPUAVSYNTHDIR)/barosensor.c

flight/targets/boards/coptercontrol/firmware/inc/FreeRTOSConfig.h

@@ -23,7 +23,7 @@
 /* Notes: We use 5 task priorities */
 
 #define configUSE_PREEMPTION                         1
-#define configUSE_IDLE_HOOK                          1
+#define configUSE_IDLE_HOOK                          0
 #define configUSE_TICK_HOOK                          0
 #define configUSE_MALLOC_FAILED_HOOK                 1
 #define configCPU_CLOCK_HZ                           ((unsigned long)72000000)
@@ -31,7 +31,7 @@
 #define configMAX_PRIORITIES                         ((unsigned portBASE_TYPE)5)
 #define configMINIMAL_STACK_SIZE                     ((unsigned short)48)
 #define configTOTAL_HEAP_SIZE                        ((size_t)(53 * 256))
-#define configMAX_TASK_NAME_LEN                      (16)
+#define configMAX_TASK_NAME_LEN                      (6)
 #define configUSE_TRACE_FACILITY                     0
 #define configUSE_16_BIT_TICKS                       0
 #define configIDLE_SHOULD_YIELD                      0
@@ -39,7 +39,7 @@
 #define configUSE_RECURSIVE_MUTEXES                  1
 #define configUSE_COUNTING_SEMAPHORES                0
 #define configUSE_ALTERNATIVE_API                    0
-#define configQUEUE_REGISTRY_SIZE                    10
+#define configQUEUE_REGISTRY_SIZE                    0
 
 /* Co-routine definitions. */
 #define configUSE_CO_ROUTINES                        0
@@ -52,10 +52,10 @@
 #define INCLUDE_uxTaskPriorityGet                    1
 #define INCLUDE_vTaskDelete                          1
 #define INCLUDE_vTaskCleanUpResources                0
-#define INCLUDE_vTaskSuspend                         1
+#define INCLUDE_vTaskSuspend                         0
 #define INCLUDE_vTaskDelayUntil                      1
 #define INCLUDE_vTaskDelay                           1
-#define INCLUDE_xTaskGetSchedulerState               1
+#define INCLUDE_xTaskGetSchedulerState               0
 #define INCLUDE_xTaskGetCurrentTaskHandle            1
 #define INCLUDE_uxTaskGetStackHighWaterMark          1
 
@@ -75,11 +75,9 @@
 #endif
 
 /* Enable run time stats collection */
-#ifdef DIAG_TASKS
-#define configCHECK_FOR_STACK_OVERFLOW 2
-
 #define configGENERATE_RUN_TIME_STATS  1
 #define INCLUDE_uxTaskGetRunTime       1
+#define INCLUDE_xTaskGetIdleTaskHandle 1
 #define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() \
     do { \
         (*(unsigned long *)0xe000edfc) |= (1 << 24); /* DEMCR |= DEMCR_TRCENA */ \
@@ -87,6 +85,9 @@
     } \
     while (0)
 #define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004) /* DWT_CYCCNT */
+
+#ifdef DIAG_TASKS
+#define configCHECK_FOR_STACK_OVERFLOW 2
 #else
 #define configCHECK_FOR_STACK_OVERFLOW 1
 #endif

flight/targets/boards/coptercontrol/firmware/inc/openpilot.h

@@ -36,7 +36,6 @@
 #include <utlist.h>
 #include <uavobjectmanager.h>
 #include <eventdispatcher.h>
-#include <callbackscheduler.h>
 #include <uavtalk.h>
 
 #include "alarms.h"

flight/targets/boards/coptercontrol/firmware/inc/pios_config.h

@@ -43,6 +43,10 @@
 /* PIOS FreeRTOS support */
 #define PIOS_INCLUDE_FREERTOS
 
+
+/* PIOS CallbackScheduler support */
+#define PIOS_INCLUDE_CALLBACKSCHEDULER
+
 /* PIOS bootloader helper */
 #define PIOS_INCLUDE_BL_HELPER
 /* #define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT */
@@ -140,7 +144,7 @@
 
 /* Stabilization options */
 /* #define PIOS_QUATERNION_STABILIZATION */
-
+#define PIOS_EXCLUDE_ADVANCED_FEATURES
 /* Performance counters */
 #define IDLE_COUNTS_PER_SEC_AT_NO_LOAD  1995998
 
@@ -153,12 +157,19 @@
 #define CPULOAD_LIMIT_CRITICAL          95
 
 /* Task stack sizes */
-#define PIOS_ACTUATOR_STACK_SIZE        1020
+#define PIOS_ACTUATOR_STACK_SIZE        820
-#define PIOS_MANUAL_STACK_SIZE          800
+#define PIOS_MANUAL_STACK_SIZE          635
+#define PIOS_RECEIVER_STACK_SIZE        620
+#define PIOS_STABILIZATION_STACK_SIZE   780
+
+#ifdef DIAG_TASKS
+#define PIOS_SYSTEM_STACK_SIZE          740
+#else
 #define PIOS_SYSTEM_STACK_SIZE          660
-#define PIOS_STABILIZATION_STACK_SIZE   524
+#endif
-#define PIOS_TELEM_STACK_SIZE           800
+#define PIOS_TELEM_RX_STACK_SIZE        410
-#define PIOS_EVENTDISPATCHER_STACK_SIZE 130
+#define PIOS_TELEM_TX_STACK_SIZE        560
+#define PIOS_EVENTDISPATCHER_STACK_SIZE 95
 
 /* This can't be too high to stop eventdispatcher thread overflowing */
 #define PIOS_EVENTDISAPTCHER_QUEUE      10

flight/targets/boards/coptercontrol/firmware/inc/pios_config_posix.h

@@ -34,6 +34,7 @@
 #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

flight/targets/boards/coptercontrol/firmware/inc/pios_usb_board_data.h

@@ -31,6 +31,7 @@
 #ifndef PIOS_USB_BOARD_DATA_H
 #define PIOS_USB_BOARD_DATA_H
 
+// Note : changing below length will require changes to the USB buffer setup
 #define PIOS_USB_BOARD_CDC_DATA_LENGTH 64
 #define PIOS_USB_BOARD_CDC_MGMT_LENGTH 32
 #define PIOS_USB_BOARD_HID_DATA_LENGTH 64

flight/targets/boards/coptercontrol/firmware/pios_board.c

@@ -198,7 +198,7 @@ void PIOS_Board_Init(void)
     }
 
     /* Initialize the delayed callback library */
-    CallbackSchedulerInitialize();
+    PIOS_CALLBACKSCHEDULER_Initialize();
 
     /* Initialize UAVObject libraries */
     EventDispatcherInitialize();

flight/targets/boards/coptercontrol/firmware/pios_board_posix.c

@@ -39,7 +39,7 @@ void PIOS_Board_Init(void)
     PIOS_DELAY_Init();
 
     /* Initialize the delayed callback library */
-    CallbackSchedulerInitialize();
+    PIOS_CALLBACKSCHEDULER_Initialize();
 
     /* Initialize UAVObject libraries */
     EventDispatcherInitialize();

flight/targets/boards/oplinkmini/bootloader/inc/pios_usb_board_data.h

@@ -31,6 +31,7 @@
 #ifndef PIOS_USB_BOARD_DATA_H
 #define PIOS_USB_BOARD_DATA_H
 
+// Note : changing below length will require changes to the USB buffer setup
 #define PIOS_USB_BOARD_HID_DATA_LENGTH 64
 
 #define PIOS_USB_BOARD_EP_NUM          2

flight/targets/boards/oplinkmini/firmware/Makefile

@@ -46,13 +46,13 @@ ifndef TESTAPP
     SRC += $(OPUAVTALK)/uavtalk.c
     SRC += $(OPUAVOBJ)/uavobjectmanager.c
     SRC += $(OPUAVOBJ)/eventdispatcher.c
-    SRC += $(OPUAVOBJ)/callbackscheduler.c
 
     ## UAVObjects
     SRC += $(OPUAVSYNTHDIR)/oplinkstatus.c
     SRC += $(OPUAVSYNTHDIR)/oplinksettings.c
     SRC += $(OPUAVSYNTHDIR)/objectpersistence.c
     SRC += $(OPUAVSYNTHDIR)/oplinkreceiver.c
+    SRC += $(OPUAVSYNTHDIR)/radiocombridgestats.c
 else
     ## Test Code
     SRC += $(OPTESTS)/test_common.c

flight/targets/boards/oplinkmini/firmware/inc/FreeRTOSConfig.h

@@ -75,11 +75,9 @@
 #endif
 
 /* Enable run time stats collection */
-#ifdef DIAG_TASKS
-#define configCHECK_FOR_STACK_OVERFLOW 2
-
 #define configGENERATE_RUN_TIME_STATS  1
 #define INCLUDE_uxTaskGetRunTime       1
+#define INCLUDE_xTaskGetIdleTaskHandle 1
 #define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() \
     do { \
         (*(unsigned long *)0xe000edfc) |= (1 << 24); /* DEMCR |= DEMCR_TRCENA */ \
@@ -87,6 +85,9 @@
     } \
     while (0)
 #define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004) /* DWT_CYCCNT */
+
+#ifdef DIAG_TASKS
+#define configCHECK_FOR_STACK_OVERFLOW 2
 #else
 #define configCHECK_FOR_STACK_OVERFLOW 1
 #endif

flight/targets/boards/oplinkmini/firmware/inc/openpilot.h

@@ -37,7 +37,6 @@
 #include <utlist.h>
 #include <uavobjectmanager.h>
 #include <eventdispatcher.h>
-#include <callbackscheduler.h>
 #include <uavtalk.h>
 
 #include "alarms.h"

flight/targets/boards/oplinkmini/firmware/inc/pios_config.h

@@ -43,6 +43,9 @@
 /* PIOS FreeRTOS support */
 #define PIOS_INCLUDE_FREERTOS
 
+/* PIOS CallbackScheduler support */
+#define PIOS_INCLUDE_CALLBACKSCHEDULER
+
 /* PIOS bootloader helper */
 #define PIOS_INCLUDE_BL_HELPER
 /* #define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT */

flight/targets/boards/oplinkmini/firmware/inc/pios_config_posix.h

@@ -34,6 +34,7 @@
 #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

flight/targets/boards/oplinkmini/firmware/inc/pios_usb_board_data.h

@@ -31,6 +31,7 @@
 #ifndef PIOS_USB_BOARD_DATA_H
 #define PIOS_USB_BOARD_DATA_H
 
+// Note : changing below length will require changes to the USB buffer setup
 #define PIOS_USB_BOARD_CDC_DATA_LENGTH 64
 #define PIOS_USB_BOARD_CDC_MGMT_LENGTH 32
 #define PIOS_USB_BOARD_HID_DATA_LENGTH 64

flight/targets/boards/oplinkmini/firmware/pios_board.c

@@ -106,7 +106,7 @@ void PIOS_Board_Init(void)
     }
 
     /* Initialize the delayed callback library */
-    CallbackSchedulerInitialize();
+    PIOS_CALLBACKSCHEDULER_Initialize();
 
     /* Initialize UAVObject libraries */
     EventDispatcherInitialize();

flight/targets/boards/oplinkmini/firmware/pios_board_posix.c

@@ -39,7 +39,7 @@ void PIOS_Board_Init(void)
     PIOS_DELAY_Init();
 
     /* Initialize the delayed callback library */
-    CallbackSchedulerInitialize();
+    PIOS_CALLBACKSCHEDULER_Initialize();
 
     /* Initialize UAVObject libraries */
     EventDispatcherInitialize();