Merge remote-tracking branch 'origin/rel-15.02'

CREDITS.txt

@@ -111,6 +111,7 @@ Kevin Vertucio
 Richard Von Lehe
 Alex Vrubel
 Mike Walters
+Sam Wang
 Brian Webb
 Justin Welander
 Mat Wellington

Makefile

@@ -135,6 +135,9 @@ all_clean:
 	@$(ECHO) " CLEAN      $(call toprel, $(BUILD_DIR))"
 	$(V1) [ ! -d "$(BUILD_DIR)" ] || $(RM) -rf "$(BUILD_DIR)"
 
+.PONY: clean
+clean: all_clean
+
 $(DL_DIR):
 	$(MKDIR) -p $@
 
@@ -144,6 +147,12 @@ $(TOOLS_DIR):
 $(BUILD_DIR):
 	$(MKDIR) -p $@
 
+$(PACKAGE_DIR):
+	$(MKDIR) -p $@
+
+$(DIST_DIR):
+	$(MKDIR) -p $@
+
 ##############################
 #
 # UAVObjects
@@ -243,8 +252,8 @@ EF_TARGETS := $(addprefix ef_, $(EF_BOARDS))
 # When building any of the "all_*" targets, tell all sub makefiles to display
 # additional details on each line of output to describe which build and target
 # that each line applies to. The same applies also to all, opfw_resource,
-# package and clean_package targets
+# package targets
-ifneq ($(strip $(filter all_% all opfw_resource package clean_package,$(MAKECMDGOALS))),)
+ifneq ($(strip $(filter all_% all opfw_resource package,$(MAKECMDGOALS))),)
     export ENABLE_MSG_EXTRA := yes
 endif
 
@@ -766,51 +775,29 @@ $(OPFW_RESOURCE): $(FW_TARGETS)
 	$(V1) $(ECHO) $(QUOTE)$(OPFW_CONTENTS)$(QUOTE) > $@
 
 # If opfw_resource or all firmware are requested, GCS should depend on the resource
-ifneq ($(strip $(filter opfw_resource all all_fw all_flight,$(MAKECMDGOALS))),)
+ifneq ($(strip $(filter opfw_resource all all_fw all_flight package,$(MAKECMDGOALS))),)
     $(eval openpilotgcs_qmake: $(OPFW_RESOURCE))
 endif
 
-# Packaging targets: package, clean_package
+# Packaging targets: package
-#  - removes build directory (clean_package only)
 #  - builds all firmware, opfw_resource, gcs
 #  - copies firmware into a package directory
 #  - calls paltform-specific packaging script
 
-# Do some checks and define some values if package is requested
+# Define some variables
-ifneq ($(strip $(filter package clean_package,$(MAKECMDGOALS))),)
+export PACKAGE_LBL  := $(shell $(VERSION_INFO) --format=\$${LABEL})
-    # Define some variables
+export PACKAGE_NAME := OpenPilot
-    export PACKAGE_LBL  := $(shell $(VERSION_INFO) --format=\$${LABEL})
+export PACKAGE_SEP  := -
-    export PACKAGE_NAME := OpenPilot
-    export PACKAGE_SEP  := -
-
-    # We can only package release builds
-    ifneq ($(GCS_BUILD_CONF),release)
-        $(error Packaging is currently supported for release builds only)
-    endif
-
-    # Packaged GCS should depend on opfw_resource
-    ifneq ($(strip $(filter package clean_package,$(MAKECMDGOALS))),)
-        $(eval openpilotgcs_qmake: $(OPFW_RESOURCE))
-    endif
-
-    # Clean the build directory if clean_package is requested
-    ifneq ($(strip $(filter clean_package,$(MAKECMDGOALS))),)
-        $(info Cleaning build directory before packaging...)
-        ifneq ($(shell $(MAKE) all_clean >/dev/null 2>&1 && $(ECHO) "clean"), clean)
-            $(error Cannot clean build directory)
-        endif
-
-        .PHONY: clean_package
-        clean_package: package
-    endif
-endif
 
 .PHONY: package
-package: all_fw all_ground uavobjects_matlab
+
-	@$(ECHO) "Packaging for $(UNAME) $(ARCH) into $(call toprel, $(PACKAGE_DIR)) directory"
+include $(ROOT_DIR)/package/$(UNAME).mk
-	$(V1) [ ! -d "$(PACKAGE_DIR)" ] || $(RM) -rf "$(PACKAGE_DIR)"
+
-	$(V1) $(MKDIR) -p "$(PACKAGE_DIR)"
+package: all_fw all_ground uavobjects_matlab $(PACKAGE_DIR)
-	$(MAKE) --no-print-directory -C $(ROOT_DIR)/package --file=$(UNAME).mk $@
+ifneq ($(GCS_BUILD_CONF),release)
+	# We can only package release builds
+	$(error Packaging is currently supported for release builds only)
+endif
 
 ##############################
 #
@@ -893,60 +880,23 @@ build-info:
 #
 ##############################
 
+DIST_VER_INFO := $(DIST_DIR)/version-info.json
+
+.PHONY: $(DIST_VER_INFO) # Because to many deps to list
+$(DIST_VER_INFO): $(DIST_DIR)
+	$(V1) $(VERSION_INFO) --jsonpath="$(DIST_DIR)"
+
 .PHONY: dist
-dist:
+dist: $(DIST_DIR) $(DIST_VER_INFO)
 	@$(ECHO) " SOURCE FOR DISTRIBUTION $(call toprel, $(DIST_DIR))"
-	$(V1) $(MKDIR) -p "$(DIST_DIR)"
-	$(V1) $(VERSION_INFO) \
-		--jsonpath="$(DIST_DIR)"
 	$(eval DIST_NAME := $(call toprel, "$(DIST_DIR)/OpenPilot-$(shell git describe).tar"))
 	$(V1) git archive --prefix="OpenPilot/" -o "$(DIST_NAME)" HEAD
 	$(V1) tar --append --file="$(DIST_NAME)" \
 		--transform='s,.*version-info.json,OpenPilot/version-info.json,' \
-		$(call toprel, "$(DIST_DIR)/version-info.json")
+		$(call toprel, "$(DIST_VER_INFO)")
 	$(V1) gzip -f "$(DIST_NAME)"
 
 
-
-##############################
-#
-# Install OpenPilot
-#
-##############################
-prefix  := /usr/local
-bindir  := $(prefix)/bin
-libdir  := $(prefix)/lib
-datadir := $(prefix)/share
-
-INSTALL = cp -a --no-preserve=ownership
-LN = ln
-LN_S = ln -s
-
-ifeq ($(MAKECMDGOALS), install)
-        ifneq ($(UNAME), Linux)
-            $(error install only supported for Linux)
-        endif
-endif
-
-
-.PHONY: install
-install:
-	@$(ECHO) " INSTALLING GCS TO $(DESTDIR)/)"
-	$(V1) $(MKDIR) -p $(DESTDIR)$(bindir)
-	$(V1) $(MKDIR) -p $(DESTDIR)$(libdir)
-	$(V1) $(MKDIR) -p $(DESTDIR)$(datadir)
-	$(V1) $(MKDIR) -p $(DESTDIR)$(datadir)/applications
-	$(V1) $(MKDIR) -p $(DESTDIR)$(datadir)/pixmaps
-	$(V1) $(MKDIR) -p $(DESTDIR)$(udevdir)
-	$(V1) $(INSTALL) $(BUILD_DIR)/openpilotgcs_$(GCS_BUILD_CONF)/bin/openpilotgcs $(DESTDIR)$(bindir)
-	$(V1) $(INSTALL) $(BUILD_DIR)/openpilotgcs_$(GCS_BUILD_CONF)/bin/udp_test $(DESTDIR)$(bindir)
-	$(V1) $(INSTALL) $(BUILD_DIR)/openpilotgcs_$(GCS_BUILD_CONF)/lib/openpilotgcs $(DESTDIR)$(libdir)
-	$(V1) $(INSTALL) $(BUILD_DIR)/openpilotgcs_$(GCS_BUILD_CONF)/share/openpilotgcs $(DESTDIR)$(datadir)
-	$(V1) $(INSTALL) $(ROOT_DIR)/package/linux/openpilot.desktop $(DESTDIR)$(datadir)/applications
-	$(V1) $(INSTALL) $(ROOT_DIR)/package/linux/openpilot.png $(DESTDIR)$(datadir)/pixmaps
-	$(V1) rm $(DESTDIR)/$(datadir)/openpilotgcs/translations/Makefile
-
-
 ##############################
 #
 # Help message, the default Makefile goal
@@ -1079,7 +1029,6 @@ help:
 	@$(ECHO) "                            Supported groups are ($(UAVOBJ_TARGETS))"
 	@$(ECHO)
 	@$(ECHO) "   [Packaging]"
-	@$(ECHO) "     clean_package        - Clean, build and package the OpenPilot platform-dependent package"
 	@$(ECHO) "     package              - Build and package the OpenPilot platform-dependent package (no clean)"
 	@$(ECHO) "     opfw_resource        - Generate resources to embed firmware binaries into the GCS"
 	@$(ECHO) "     dist                 - Generate source archive for distribution"

WHATSNEW.txt

@@ -1,3 +1,74 @@
+--- RELEASE-15.02 RC7 
+This release introduces major flight performance improvements, enhancements as well as bug fixes. Many enhancements have been made to reducing dead-time of the communication between the flight controller and ESCs. In our testing, we have found this to be not only the best flight performance so far in the OpenPilot project but the best flight performance of any project we have tested against. This is a recommended upgrade for everyone and the more skilled of a pilot you are, the more you will love this release.
+
+A key improvement that helped achieve this was the addition of the PWMSync code path, this is now enabled by default. Some restrictions applies to CC3D/CC as it needs a compatible input method to enable PWMSync. Compatible input methods are PPM, S.Bus, DSM and OPLink. This release also introduces support for OneShot125 capable ESCs, such as the KISS ESCs and all ESCs supported in BLHeli V13 and above.  Note that OneShot125 support has the same restrictions as PWMSync for CC and CC3D. 
+
+Note for CC3D/CC: To support PWMSync/OneShot125 PPM input pin has changed from earlier releases to the last pin on the input IO port, please refer to the connection diagram in the setup wizard to see the required new pin layout for PPM. 
+
+Other enhancements include key parts of the GCS translated to Chinese and further OPLink reliability additions allowing us to confidently say it can be used as a primary control link as well as for telemetry.
+
+The full list of features, improvements and bugfixes in this release is accessible here:
+https://progress.openpilot.org/issues/?filter=12161
+
+Release Notes - OpenPilot - Version RELEASE-15.02
+
+** Bug
+    * [OP-969] - Input Configuration Wizard has scrollbars showing up and next/previous buttons are pushed down out of sight
+    * [OP-1034] - CCPM Config Widget crashes GCS if required boxes aren't set i.e. Channel set to None
+    * [OP-1236] - Icons on Welcome tab - moves to the left
+    * [OP-1466] - Gcs crashes on Helicopter config tab
+    * [OP-1522] - Improve Robustness of OPLink radio
+    * [OP-1601] - Still not enough ram on CC for gps to be usable
+    * [OP-1644] - Radio Setup Wiz problem with 6 flight modes
+    * [OP-1670] - cruise control conflict with flight modes (rate & acro +)
+    * [OP-1680] - OPLink control limited to 1000-1896
+    * [OP-1682] - Overflow issue with pwm rx and CC3D 
+    * [OP-1686] - Slave OPLM should receive PPM in PPM_only mode
+    * [OP-1706] - Output Reverse checkboxes tick them selves when min equals max
+    * [OP-1718] - FW vehicle setup wizard may strip servo gears
+    * [OP-1722] - libusb include is incorrect.
+    * [OP-1728] - FW servos ignore set neutral
+    * [OP-1733] - version-info.py should check for version-info.json not git repo.
+    * [OP-1735] - Build fails with a tilde (~) in path.
+    * [OP-1737] - Min/max will be greyed even if motor output test is canceled
+    * [OP-1741] - Repo path is incorrect in version_info.pro
+    * [OP-1743] - cc3d fails to connect once "next " is loaded 2-23-15
+    * [OP-1744] - Vehicle config wizard produces a bad configuration
+    * [OP-1754] - Vehicle Wizard bad config when setting PPM in and RapidESC out
+    * [OP-1755] - Add additional path for cloudconfigs
+    * [OP-1758] - Upgrade hidapi for all OSs (except windows) to solve mac issue:Fix incorrect device list after device removal 
+    * [OP-1761] - Wizard bad config when PPM in, RapidESC out and hexa frame with CC/CC3D/Atom
+    * [OP-1764] - System should sanity check RC Input Channel value ranges and raise alarm accordingly
+    * [OP-1768] - PWM Sync and OneShot125 wizard output level and warning are incorrect
+
+
+** Improvement
+    * [OP-1519] - Auto Reboot of board when required by Wizard
+    * [OP-1576] - Remove tx resent accounting from OPlink
+    * [OP-1635] - Remove Quad H from wizard to avoid confusion with Quad X
+    * [OP-1650] - Reduce telemetry to improve OPLink
+    * [OP-1658] - Sensor driver API and overhaul of sensor module
+    * [OP-1683] - Support synchronous (OneShot) and OneShot125 output mode
+    * [OP-1685] - Support OneShot/OneShot125 for CC* targets
+    * [OP-1694] - Make package make rule non-nested
+    * [OP-1698] - Add easy to set channel passtrough GCS functionality
+    * [OP-1704] - Add support for sanity check custom hooks
+    * [OP-1759] - Hide CC3D non-supported options (GPSAssist)
+
+** New Feature
+    * [OP-1723] - RCCar Forward/reverse support
+
+
+** Task
+    * [OP-1721] - C++ enable flight controller and upgrade ARM tools
+    * [OP-1738] - change default flight modes and thrust settings
+    * [OP-1747] - 15.02 rc1 motor end points do not reflect oneshot125 / pwmsync
+
+** Sub task
+    * [OP-1748] - Chinese translation for 15.02
+    * [OP-1752] - Add Alarm sub status to SystemHealth
+
+
 --- RELEASE-15.01 --- Look Ma, No hands ---
 This release mainly focuses on a new feature, GPSAssist which is a new form of assisted control for multirotors.  
 Assisted Control provides assistance functions on top of existing flight modes.  GPSAssist is the 

flight/libraries/inc/sanitycheck.h

@@ -32,6 +32,9 @@
 
 #include <systemalarms.h>
 
+#include <stdint.h>
+#include <utlist.h>
+
 typedef enum {
     FRAME_TYPE_MULTIROTOR,
     FRAME_TYPE_HELI,
@@ -40,6 +43,8 @@ typedef enum {
     FRAME_TYPE_CUSTOM,
 } FrameType_t;
 
+typedef SystemAlarmsExtendedAlarmStatusOptions (SANITYCHECK_CustomHook_function)();
+
 #define SANITYCHECK_STATUS_ERROR_NONE         SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE
 #define SANITYCHECK_STATUS_ERROR_FLIGHTMODE   SYSTEMALARMS_EXTENDEDALARMSTATUS_FLIGHTMODE
 
@@ -55,4 +60,16 @@ extern int32_t configuration_check();
 
 extern FrameType_t GetCurrentFrameType();
 
+/**
+ * Attach a custom hook to the sanity check process
+ * @param hook a custom hook function
+ */
+extern void SANITYCHECK_AttachHook(SANITYCHECK_CustomHook_function *hook);
+
+/**
+ * Detach a custom hook to the sanity check process
+ * @param hook a custom hook function
+ */
+extern void SANITYCHECK_DetachHook(SANITYCHECK_CustomHook_function *hook);
+
 #endif /* SANITYCHECK_H */

flight/libraries/inc/sha1.h

@@ -0,0 +1,37 @@
+/*	$NetBSD: sha1.h,v 1.14 2009/11/06 20:31:19 joerg Exp $	*/
+
+/*
+ * SHA-1 in C
+ * By Steve Reid <steve@edmweb.com>
+ * 100% Public Domain
+ */
+
+#ifndef _SYS_SHA1_H_
+#define _SYS_SHA1_H_
+
+#include <sys/cdefs.h>
+#include <sys/types.h>
+
+#define SHA1_DIGEST_LENGTH        20
+#define SHA1_DIGEST_STRING_LENGTH 41
+
+typedef struct {
+    uint32_t state[5];
+    uint32_t count[2];
+    uint8_t  buffer[64];
+} SHA1_CTX;
+
+__BEGIN_DECLS
+void SHA1Transform(uint32_t[5], const uint8_t[64]);
+void    SHA1Init(SHA1_CTX *);
+void    SHA1Update(SHA1_CTX *, const uint8_t *, unsigned int);
+void SHA1Final(uint8_t[SHA1_DIGEST_LENGTH], SHA1_CTX *);
+#ifndef _KERNEL
+char *SHA1End(SHA1_CTX *, char *);
+char *SHA1FileChunk(const char *, char *, off_t, off_t);
+char *SHA1File(const char *, char *);
+char *SHA1Data(const uint8_t *, size_t, char *);
+#endif /* _KERNEL */
+__END_DECLS
+
+#endif /* _SYS_SHA1_H_ */

flight/libraries/math/vectors.h

@@ -1,14 +1,13 @@
 /**
  ******************************************************************************
- * @addtogroup OpenPilotModules OpenPilot Modules
+ * @addtogroup OpenPilot Math Utilities
  * @{
- * @addtogroup AltitudeModule Altitude Module
+ * @addtogroup Reuseable vector data type and functions
- * @brief Communicate with BMP085 and update @ref AltitudeActual "AltitudeActual UAV Object"
  * @{
  *
- * @file       altitude.h
+ * @file       vectors.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
- * @brief      Altitude module, reads temperature and pressure from BMP085
+ * @brief      Reuseable vector data type and functions
  *
  * @see        The GNU Public License (GPL) Version 3
  *
@@ -28,14 +27,37 @@
  * with this program; if not, write to the Free Software Foundation, Inc.,
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
-#ifndef ALTITUDE_H
-#define ALTITUDE_H
 
-int32_t AltitudeInitialize();
+#ifndef VECTORS_H_
+#define VECTORS_H_
 
-#endif // ALTITUDE_H
+#include <math.h>
+#include <stdint.h>
 
-/**
+#define DECLAREVECTOR3(suffix, datatype) \
- * @}
+    typedef struct Vector3##suffix##_t { \
- * @}
+        datatype x; \
- */
+        datatype y; \
+        datatype z; \
+    } Vector3##suffix
+
+#define DECLAREVECTOR2(suffix, datatype) \
+    typedef struct Vector2##suffix##_t { \
+        datatype x; \
+        datatype y; \
+    } Vector2##suffix
+
+
+DECLAREVECTOR3(i16, int16_t);
+DECLAREVECTOR3(i32, int32_t);
+DECLAREVECTOR3(u16, uint16_t);
+DECLAREVECTOR3(u32, uint32_t);
+DECLAREVECTOR3(f, float);
+
+DECLAREVECTOR2(i16, int16_t);
+DECLAREVECTOR2(i32, int32_t);
+DECLAREVECTOR2(u16, uint16_t);
+DECLAREVECTOR2(u32, uint32_t);
+DECLAREVECTOR2(f, float);
+
+#endif /* VECTORS_H_ */

flight/libraries/mini_cpp.cpp

@@ -0,0 +1,89 @@
+/**
+ ******************************************************************************
+ * @addtogroup PIOS PIOS Core hardware abstraction layer
+ * @{
+ *
+ * @file       mini_cpp.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ * @brief      CPP support methods
+ * @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>
+
+// _init is called by __libc_init_array during invocation of static constructors
+// __libc_init_array calls _init, which is defined in crti.o. _init calls functions that are in .init section.
+// If you don't have meaningful stuff in .init section, just define an empty _init function.
+extern "C" int _init(void)
+{
+    return 0;
+}
+
+
+// operator new
+void *operator new(size_t size) throw()
+{
+    return pios_malloc(size);
+}
+
+// operator delete
+void operator delete(void *p) throw()
+{
+    pios_free(p);
+}
+
+// The function __aeabi_atexit() handles the static destructors.  This can be empty
+// because we have no operating system to return to, hence the static destructors
+// will never be called.
+extern "C" int __aeabi_atexit(__attribute__((unused)) void *object, __attribute__((unused)) void (*destructor)(void *), __attribute__((unused)) void *dso_handle)
+{
+    return 0;
+}
+
+// see https://answers.launchpad.net/gcc-arm-embedded/+question/221105
+// and https://answers.launchpad.net/gcc-arm-embedded/+question/224709
+__extension__ typedef int __guard __attribute__((mode(__DI__)));
+extern "C" int __cxa_atexit(void (*f)(void *), void *p, void *d);
+extern "C" int __cxa_guard_acquire(__guard *);
+extern "C" void __cxa_guard_release(__guard *);
+extern "C" void __cxa_guard_abort(__guard *);
+extern "C" void __cxa_pure_virtual(void);
+
+int __cxa_guard_acquire(__attribute__((unused)) __guard *g)
+{
+    return !*(char *)(g);
+};
+void __cxa_guard_release(__attribute__((unused)) __guard *g)
+{
+    *(char *)g = 1;
+};
+void __cxa_guard_abort(__attribute__((unused)) __guard *) {};
+void __cxa_pure_virtual(void)
+{
+    while (1) {
+        ;
+    }
+}
+int __cxa_atexit(__attribute__((unused)) void (*f)(void *), __attribute__((unused)) void *p, __attribute__((unused)) void *d)
+{
+    return 0;
+};
+/**
+ * @}
+ */

flight/libraries/sanitycheck.c

@@ -43,12 +43,21 @@
 #include <taskinfo.h>
 
 // a number of useful macros
-#define ADDSEVERITY(check) severity = (severity != SYSTEMALARMS_ALARM_OK ? severity : ((check) ? SYSTEMALARMS_ALARM_OK : SYSTEMALARMS_ALARM_CRITICAL))
+#define ADDSEVERITY(check)                                  severity = (severity != SYSTEMALARMS_ALARM_OK ? severity : ((check) ? SYSTEMALARMS_ALARM_OK : SYSTEMALARMS_ALARM_CRITICAL))
+#define ADDEXTENDEDALARMSTATUS(error_code, error_substatus) if ((severity != SYSTEMALARMS_ALARM_OK) && (alarmstatus == SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE)) { alarmstatus = (error_code); alarmsubstatus = (error_substatus); }
 
+// private types
+typedef struct SANITYCHECK_CustomHookInstance {
+    SANITYCHECK_CustomHook_function *hook;
+    struct SANITYCHECK_CustomHookInstance *next;
+    bool enabled;
+} SANITYCHECK_CustomHookInstance;
 
 // ! Check a stabilization mode switch position for safety
 static bool check_stabilization_settings(int index, bool multirotor, bool coptercontrol, bool gpsassisted);
 
+SANITYCHECK_CustomHookInstance *hooks = 0;
+
 /**
  * Run a preflight check over the hardware configuration
  * and currently active modules
@@ -170,6 +179,41 @@ int32_t configuration_check()
         }
     }
 
+
+    // Check throttle/collective channel range for valid configuration of input for critical control
+    SystemSettingsThrustControlOptions thrustType;
+    SystemSettingsThrustControlGet(&thrustType);
+    ManualControlSettingsChannelMinData channelMin;
+    ManualControlSettingsChannelMaxData channelMax;
+    ManualControlSettingsChannelMinGet(&channelMin);
+    ManualControlSettingsChannelMaxGet(&channelMax);
+    switch (thrustType) {
+    case SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE:
+        ADDSEVERITY(fabsf(channelMax.Throttle - channelMin.Throttle) > 300.0f);
+        ADDEXTENDEDALARMSTATUS(SYSTEMALARMS_EXTENDEDALARMSTATUS_BADTHROTTLEORCOLLECTIVEINPUTRANGE, 0);
+        break;
+    case SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE:
+        ADDSEVERITY(fabsf(channelMax.Collective - channelMin.Collective) > 300.0f);
+        ADDEXTENDEDALARMSTATUS(SYSTEMALARMS_EXTENDEDALARMSTATUS_BADTHROTTLEORCOLLECTIVEINPUTRANGE, 0);
+        break;
+    default:
+        break;
+    }
+
+    // query sanity check hooks
+    if (severity < SYSTEMALARMS_ALARM_CRITICAL) {
+        SANITYCHECK_CustomHookInstance *instance = NULL;
+        LL_FOREACH(hooks, instance) {
+            if (instance->enabled) {
+                alarmstatus = instance->hook();
+                if (alarmstatus != SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE) {
+                    severity = SYSTEMALARMS_ALARM_CRITICAL;
+                    break;
+                }
+            }
+        }
+    }
+
     uint8_t checks_disabled;
     FlightModeSettingsDisableSanityChecksGet(&checks_disabled);
     if (checks_disabled == FLIGHTMODESETTINGS_DISABLESANITYCHECKS_TRUE) {
@@ -265,11 +309,23 @@ static bool check_stabilization_settings(int index, bool multirotor, bool copter
         return false;
     }
 
+
+    // if cruise control, ensure rate or acro are not set
+    if (modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL) {
+        for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_YAW; i++) {
+            if ((modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RATE ||
+                 modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ACRO)) {
+                return false;
+            }
+        }
+    }
+
     // Warning: This assumes that certain conditions in the XML file are met.  That
     // FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL has the same numeric value for each channel
     // and is the same for STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL
     // (this is checked at compile time by static constraint manualcontrol.h)
 
+
     return true;
 }
 
@@ -281,7 +337,6 @@ FrameType_t GetCurrentFrameType()
     switch ((SystemSettingsAirframeTypeOptions)airframe_type) {
     case SYSTEMSETTINGS_AIRFRAMETYPE_QUADX:
     case SYSTEMSETTINGS_AIRFRAMETYPE_QUADP:
-    case SYSTEMSETTINGS_AIRFRAMETYPE_QUADH:
     case SYSTEMSETTINGS_AIRFRAMETYPE_HEXA:
     case SYSTEMSETTINGS_AIRFRAMETYPE_OCTO:
     case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOX:
@@ -314,3 +369,39 @@ FrameType_t GetCurrentFrameType()
     // anyway it should not reach here
     return FRAME_TYPE_CUSTOM;
 }
+
+void SANITYCHECK_AttachHook(SANITYCHECK_CustomHook_function *hook)
+{
+    PIOS_Assert(hook);
+    SANITYCHECK_CustomHookInstance *instance = NULL;
+
+    // Check whether there is an existing instance and enable it
+    LL_FOREACH(hooks, instance) {
+        if (instance->hook == hook) {
+            instance->enabled = true;
+            return;
+        }
+    }
+
+    // No existing instance found, attach this new one
+    instance = (SANITYCHECK_CustomHookInstance *)pios_malloc(sizeof(SANITYCHECK_CustomHookInstance));
+    PIOS_Assert(instance);
+    instance->hook    = hook;
+    instance->next    = NULL;
+    instance->enabled = true;
+    LL_APPEND(hooks, instance);
+}
+
+void SANITYCHECK_DetachHook(SANITYCHECK_CustomHook_function *hook)
+{
+    if (!hooks) {
+        return;
+    }
+    SANITYCHECK_CustomHookInstance *instance = NULL;
+    LL_FOREACH(hooks, instance) {
+        if (instance->hook == hook) {
+            instance->enabled = false;
+            return;
+        }
+    }
+}

flight/libraries/sha1.c

@@ -0,0 +1,279 @@
+/*	$NetBSD: sha1.c,v 1.6 2009/11/06 20:31:18 joerg Exp $	*/
+/*	$OpenBSD: sha1.c,v 1.9 1997/07/23 21:12:32 kstailey Exp $	*/
+
+/*
+ * SHA-1 in C
+ * By Steve Reid <steve@edmweb.com>
+ * 100% Public Domain
+ *
+ * Test Vectors (from FIPS PUB 180-1)
+ * "abc"
+ *   A9993E36 4706816A BA3E2571 7850C26C 9CD0D89D
+ * "abcdbcdecdefdefgefghfghighijhijkijkljklmklmnlmnomnopnopq"
+ *   84983E44 1C3BD26E BAAE4AA1 F95129E5 E54670F1
+ * A million repetitions of "a"
+ *   34AA973C D4C4DAA4 F61EEB2B DBAD2731 6534016F
+ */
+
+#define SHA1HANDSOFF /* Copies data before messing with it. */
+
+#include <sys/cdefs.h>
+
+#if defined(_KERNEL) || defined(_STANDALONE)
+__KERNEL_RCSID(0, "$NetBSD: sha1.c,v 1.6 2009/11/06 20:31:18 joerg Exp $");
+
+#include <lib/libkern/libkern.h>
+
+#else
+
+#if defined(LIBC_SCCS) && !defined(lint)
+__RCSID("$NetBSD: sha1.c,v 1.6 2009/11/06 20:31:18 joerg Exp $");
+#endif /* LIBC_SCCS and not lint */
+
+// #include "namespace.h"
+#include <assert.h>
+#include <string.h>
+
+#endif
+
+#include <stdint.h>
+#include <sha1.h>
+
+
+#if HAVE_NBTOOL_CONFIG_H
+#include "nbtool_config.h"
+#endif
+
+#if !HAVE_SHA1_H
+
+#define rol(value, bits) (((value) << (bits)) | ((value) >> (32 - (bits))))
+
+/*
+ * blk0() and blk() perform the initial expand.
+ * I got the idea of expanding during the round function from SSLeay
+ */
+#if BYTE_ORDER == LITTLE_ENDIAN
+# define blk0(i) \
+    (block->l[i] = (rol(block->l[i], 24) & 0xFF00FF00) \
+                   | (rol(block->l[i], 8) & 0x00FF00FF))
+#else
+# define blk0(i)             block->l[i]
+#endif
+#define blk(i) \
+    (block->l[i & 15] = rol(block->l[(i + 13) & 15] ^ block->l[(i + 8) & 15] \
+                            ^ block->l[(i + 2) & 15] ^ block->l[i & 15], 1))
+
+/*
+ * (R0+R1), R2, R3, R4 are the different operations (rounds) used in SHA1
+ */
+#define R0(v, w, x, y, z, i) z += ((w & (x ^ y)) ^ y) + blk0(i) + 0x5A827999 + rol(v, 5); w = rol(w, 30);
+#define R1(v, w, x, y, z, i) z += ((w & (x ^ y)) ^ y) + blk(i) + 0x5A827999 + rol(v, 5); w = rol(w, 30);
+#define R2(v, w, x, y, z, i) z += (w ^ x ^ y) + blk(i) + 0x6ED9EBA1 + rol(v, 5); w = rol(w, 30);
+#define R3(v, w, x, y, z, i) z += (((w | x) & y) | (w & x)) + blk(i) + 0x8F1BBCDC + rol(v, 5); w = rol(w, 30);
+#define R4(v, w, x, y, z, i) z += (w ^ x ^ y) + blk(i) + 0xCA62C1D6 + rol(v, 5); w = rol(w, 30);
+
+
+#if !defined(_KERNEL) && !defined(_STANDALONE)
+#if defined(__weak_alias)
+__weak_alias(SHA1Transform, _SHA1Transform)
+__weak_alias(SHA1Init, _SHA1Init)
+__weak_alias(SHA1Update, _SHA1Update)
+__weak_alias(SHA1Final, _SHA1Final)
+#endif
+#endif
+
+typedef union {
+    uint8_t  c[64];
+    uint32_t l[16];
+} CHAR64LONG16;
+
+/* old sparc64 gcc could not compile this */
+#undef SPARC64_GCC_WORKAROUND
+#if defined(__sparc64__) && defined(__GNUC__) && __GNUC__ < 3
+#define SPARC64_GCC_WORKAROUND
+#endif
+
+#ifdef SPARC64_GCC_WORKAROUND
+void do_R01(uint32_t *a, uint32_t *b, uint32_t *c, uint32_t *d, uint32_t *e, CHAR64LONG16 *);
+void do_R2(uint32_t *a, uint32_t *b, uint32_t *c, uint32_t *d, uint32_t *e, CHAR64LONG16 *);
+void do_R3(uint32_t *a, uint32_t *b, uint32_t *c, uint32_t *d, uint32_t *e, CHAR64LONG16 *);
+void do_R4(uint32_t *a, uint32_t *b, uint32_t *c, uint32_t *d, uint32_t *e, CHAR64LONG16 *);
+
+#define nR0(v, w, x, y, z, i) R0(*v, *w, *x, *y, *z, i)
+#define nR1(v, w, x, y, z, i) R1(*v, *w, *x, *y, *z, i)
+#define nR2(v, w, x, y, z, i) R2(*v, *w, *x, *y, *z, i)
+#define nR3(v, w, x, y, z, i) R3(*v, *w, *x, *y, *z, i)
+#define nR4(v, w, x, y, z, i) R4(*v, *w, *x, *y, *z, i)
+
+void do_R01(uint32_t *a, uint32_t *b, uint32_t *c, uint32_t *d, uint32_t *e, CHAR64LONG16 *block)
+{
+    nR0(a, b, c, d, e, 0); nR0(e, a, b, c, d, 1); nR0(d, e, a, b, c, 2); nR0(c, d, e, a, b, 3);
+    nR0(b, c, d, e, a, 4); nR0(a, b, c, d, e, 5); nR0(e, a, b, c, d, 6); nR0(d, e, a, b, c, 7);
+    nR0(c, d, e, a, b, 8); nR0(b, c, d, e, a, 9); nR0(a, b, c, d, e, 10); nR0(e, a, b, c, d, 11);
+    nR0(d, e, a, b, c, 12); nR0(c, d, e, a, b, 13); nR0(b, c, d, e, a, 14); nR0(a, b, c, d, e, 15);
+    nR1(e, a, b, c, d, 16); nR1(d, e, a, b, c, 17); nR1(c, d, e, a, b, 18); nR1(b, c, d, e, a, 19);
+}
+
+void do_R2(uint32_t *a, uint32_t *b, uint32_t *c, uint32_t *d, uint32_t *e, CHAR64LONG16 *block)
+{
+    nR2(a, b, c, d, e, 20); nR2(e, a, b, c, d, 21); nR2(d, e, a, b, c, 22); nR2(c, d, e, a, b, 23);
+    nR2(b, c, d, e, a, 24); nR2(a, b, c, d, e, 25); nR2(e, a, b, c, d, 26); nR2(d, e, a, b, c, 27);
+    nR2(c, d, e, a, b, 28); nR2(b, c, d, e, a, 29); nR2(a, b, c, d, e, 30); nR2(e, a, b, c, d, 31);
+    nR2(d, e, a, b, c, 32); nR2(c, d, e, a, b, 33); nR2(b, c, d, e, a, 34); nR2(a, b, c, d, e, 35);
+    nR2(e, a, b, c, d, 36); nR2(d, e, a, b, c, 37); nR2(c, d, e, a, b, 38); nR2(b, c, d, e, a, 39);
+}
+
+void do_R3(uint32_t *a, uint32_t *b, uint32_t *c, uint32_t *d, uint32_t *e, CHAR64LONG16 *block)
+{
+    nR3(a, b, c, d, e, 40); nR3(e, a, b, c, d, 41); nR3(d, e, a, b, c, 42); nR3(c, d, e, a, b, 43);
+    nR3(b, c, d, e, a, 44); nR3(a, b, c, d, e, 45); nR3(e, a, b, c, d, 46); nR3(d, e, a, b, c, 47);
+    nR3(c, d, e, a, b, 48); nR3(b, c, d, e, a, 49); nR3(a, b, c, d, e, 50); nR3(e, a, b, c, d, 51);
+    nR3(d, e, a, b, c, 52); nR3(c, d, e, a, b, 53); nR3(b, c, d, e, a, 54); nR3(a, b, c, d, e, 55);
+    nR3(e, a, b, c, d, 56); nR3(d, e, a, b, c, 57); nR3(c, d, e, a, b, 58); nR3(b, c, d, e, a, 59);
+}
+
+void do_R4(uint32_t *a, uint32_t *b, uint32_t *c, uint32_t *d, uint32_t *e, CHAR64LONG16 *block)
+{
+    nR4(a, b, c, d, e, 60); nR4(e, a, b, c, d, 61); nR4(d, e, a, b, c, 62); nR4(c, d, e, a, b, 63);
+    nR4(b, c, d, e, a, 64); nR4(a, b, c, d, e, 65); nR4(e, a, b, c, d, 66); nR4(d, e, a, b, c, 67);
+    nR4(c, d, e, a, b, 68); nR4(b, c, d, e, a, 69); nR4(a, b, c, d, e, 70); nR4(e, a, b, c, d, 71);
+    nR4(d, e, a, b, c, 72); nR4(c, d, e, a, b, 73); nR4(b, c, d, e, a, 74); nR4(a, b, c, d, e, 75);
+    nR4(e, a, b, c, d, 76); nR4(d, e, a, b, c, 77); nR4(c, d, e, a, b, 78); nR4(b, c, d, e, a, 79);
+}
+#endif /* ifdef SPARC64_GCC_WORKAROUND */
+
+/*
+ * Hash a single 512-bit block. This is the core of the algorithm.
+ */
+void SHA1Transform(uint32_t state[5], const uint8_t buffer[64])
+{
+    uint32_t a, b, c, d, e;
+    CHAR64LONG16 *block;
+
+#ifdef SHA1HANDSOFF
+    CHAR64LONG16 workspace;
+#endif
+
+#ifdef SHA1HANDSOFF
+    block = &workspace;
+    (void)memcpy(block, buffer, 64);
+#else
+    block = (CHAR64LONG16 *)(void *)buffer;
+#endif
+
+    /* Copy context->state[] to working vars */
+    a = state[0];
+    b = state[1];
+    c = state[2];
+    d = state[3];
+    e = state[4];
+
+#ifdef SPARC64_GCC_WORKAROUND
+    do_R01(&a, &b, &c, &d, &e, block);
+    do_R2(&a, &b, &c, &d, &e, block);
+    do_R3(&a, &b, &c, &d, &e, block);
+    do_R4(&a, &b, &c, &d, &e, block);
+#else
+    /* 4 rounds of 20 operations each. Loop unrolled. */
+    R0(a, b, c, d, e, 0); R0(e, a, b, c, d, 1); R0(d, e, a, b, c, 2); R0(c, d, e, a, b, 3);
+    R0(b, c, d, e, a, 4); R0(a, b, c, d, e, 5); R0(e, a, b, c, d, 6); R0(d, e, a, b, c, 7);
+    R0(c, d, e, a, b, 8); R0(b, c, d, e, a, 9); R0(a, b, c, d, e, 10); R0(e, a, b, c, d, 11);
+    R0(d, e, a, b, c, 12); R0(c, d, e, a, b, 13); R0(b, c, d, e, a, 14); R0(a, b, c, d, e, 15);
+    R1(e, a, b, c, d, 16); R1(d, e, a, b, c, 17); R1(c, d, e, a, b, 18); R1(b, c, d, e, a, 19);
+    R2(a, b, c, d, e, 20); R2(e, a, b, c, d, 21); R2(d, e, a, b, c, 22); R2(c, d, e, a, b, 23);
+    R2(b, c, d, e, a, 24); R2(a, b, c, d, e, 25); R2(e, a, b, c, d, 26); R2(d, e, a, b, c, 27);
+    R2(c, d, e, a, b, 28); R2(b, c, d, e, a, 29); R2(a, b, c, d, e, 30); R2(e, a, b, c, d, 31);
+    R2(d, e, a, b, c, 32); R2(c, d, e, a, b, 33); R2(b, c, d, e, a, 34); R2(a, b, c, d, e, 35);
+    R2(e, a, b, c, d, 36); R2(d, e, a, b, c, 37); R2(c, d, e, a, b, 38); R2(b, c, d, e, a, 39);
+    R3(a, b, c, d, e, 40); R3(e, a, b, c, d, 41); R3(d, e, a, b, c, 42); R3(c, d, e, a, b, 43);
+    R3(b, c, d, e, a, 44); R3(a, b, c, d, e, 45); R3(e, a, b, c, d, 46); R3(d, e, a, b, c, 47);
+    R3(c, d, e, a, b, 48); R3(b, c, d, e, a, 49); R3(a, b, c, d, e, 50); R3(e, a, b, c, d, 51);
+    R3(d, e, a, b, c, 52); R3(c, d, e, a, b, 53); R3(b, c, d, e, a, 54); R3(a, b, c, d, e, 55);
+    R3(e, a, b, c, d, 56); R3(d, e, a, b, c, 57); R3(c, d, e, a, b, 58); R3(b, c, d, e, a, 59);
+    R4(a, b, c, d, e, 60); R4(e, a, b, c, d, 61); R4(d, e, a, b, c, 62); R4(c, d, e, a, b, 63);
+    R4(b, c, d, e, a, 64); R4(a, b, c, d, e, 65); R4(e, a, b, c, d, 66); R4(d, e, a, b, c, 67);
+    R4(c, d, e, a, b, 68); R4(b, c, d, e, a, 69); R4(a, b, c, d, e, 70); R4(e, a, b, c, d, 71);
+    R4(d, e, a, b, c, 72); R4(c, d, e, a, b, 73); R4(b, c, d, e, a, 74); R4(a, b, c, d, e, 75);
+    R4(e, a, b, c, d, 76); R4(d, e, a, b, c, 77); R4(c, d, e, a, b, 78); R4(b, c, d, e, a, 79);
+#endif /* ifdef SPARC64_GCC_WORKAROUND */
+
+    /* Add the working vars back into context.state[] */
+    state[0] += a;
+    state[1] += b;
+    state[2] += c;
+    state[3] += d;
+    state[4] += e;
+
+    /* Wipe variables */
+    a = b = c = d = e = 0;
+}
+
+
+/*
+ * SHA1Init - Initialize new context
+ */
+void SHA1Init(SHA1_CTX *context)
+{
+    /* SHA1 initialization constants */
+    context->state[0] = 0x67452301;
+    context->state[1] = 0xEFCDAB89;
+    context->state[2] = 0x98BADCFE;
+    context->state[3] = 0x10325476;
+    context->state[4] = 0xC3D2E1F0;
+    context->count[0] = context->count[1] = 0;
+}
+
+
+/*
+ * Run your data through this.
+ */
+void SHA1Update(SHA1_CTX *context, const uint8_t *data, unsigned int len)
+{
+    unsigned int i, j;
+
+    j = context->count[0];
+    if ((context->count[0] += len << 3) < j) {
+        context->count[1] += (len >> 29) + 1;
+    }
+    j = (j >> 3) & 63;
+    if ((j + len) > 63) {
+        (void)memcpy(&context->buffer[j], data, (i = 64 - j));
+        SHA1Transform(context->state, context->buffer);
+        for (; i + 63 < len; i += 64) {
+            SHA1Transform(context->state, &data[i]);
+        }
+        j = 0;
+    } else {
+        i = 0;
+    }
+    (void)memcpy(&context->buffer[j], &data[i], len - i);
+}
+
+
+/*
+ * Add padding and return the message digest.
+ */
+void SHA1Final(uint8_t digest[20], SHA1_CTX *context)
+{
+    unsigned int i;
+    uint8_t finalcount[8];
+
+    for (i = 0; i < 8; i++) {
+        finalcount[i] = (uint8_t)((context->count[(i >= 4 ? 0 : 1)]
+                                   >> ((3 - (i & 3)) * 8)) & 255); /* Endian independent */
+    }
+    SHA1Update(context, (const uint8_t *)"\200", 1);
+    while ((context->count[0] & 504) != 448) {
+        SHA1Update(context, (const uint8_t *)"\0", 1);
+    }
+    SHA1Update(context, finalcount, 8); /* Should cause a SHA1Transform() */
+
+    if (digest) {
+        for (i = 0; i < 20; i++) {
+            digest[i] = (uint8_t)
+                        ((context->state[i >> 2] >> ((3 - (i & 3)) * 8)) & 255);
+        }
+    }
+}
+
+#endif /* HAVE_SHA1_H */

flight/modules/Actuator/actuator.c

@@ -53,20 +53,23 @@ static int8_t counter;
 #endif
 
 // Private constants
-#define MAX_QUEUE_SIZE       2
+#define MAX_QUEUE_SIZE                  2
 
 #if defined(PIOS_ACTUATOR_STACK_SIZE)
-#define STACK_SIZE_BYTES     PIOS_ACTUATOR_STACK_SIZE
+#define STACK_SIZE_BYTES                PIOS_ACTUATOR_STACK_SIZE
 #else
-#define STACK_SIZE_BYTES     1312
+#define STACK_SIZE_BYTES                1312
 #endif
 
-#define TASK_PRIORITY        (tskIDLE_PRIORITY + 4) // device driver
+#define TASK_PRIORITY                   (tskIDLE_PRIORITY + 4) // device driver
-#define FAILSAFE_TIMEOUT_MS  100
+#define FAILSAFE_TIMEOUT_MS             100
-#define MAX_MIX_ACTUATORS    ACTUATORCOMMAND_CHANNEL_NUMELEM
+#define MAX_MIX_ACTUATORS               ACTUATORCOMMAND_CHANNEL_NUMELEM
 
-#define CAMERA_BOOT_DELAY_MS 7000
+#define CAMERA_BOOT_DELAY_MS            7000
 
+#define ACTUATOR_ONESHOT125_CLOCK       2000000
+#define ACTUATOR_ONESHOT125_PULSE_SCALE 4
+#define ACTUATOR_PWM_CLOCK              1000000
 // Private types
 
 
@@ -74,8 +77,9 @@ static int8_t counter;
 static xQueueHandle queue;
 static xTaskHandle taskHandle;
 
-static float lastResult[MAX_MIX_ACTUATORS] = { 0, 0, 0, 0, 0, 0, 0, 0 };
+static float lastResult[MAX_MIX_ACTUATORS] = { 0 };
-static float filterAccumulator[MAX_MIX_ACTUATORS] = { 0, 0, 0, 0, 0, 0, 0, 0 };
+static float filterAccumulator[MAX_MIX_ACTUATORS] = { 0 };
+static uint8_t pinsMode[MAX_MIX_ACTUATORS];
 // used to inform the actuator thread that actuator update rate is changed
 static volatile bool actuator_settings_updated;
 // used to inform the actuator thread that mixer settings are changed
@@ -436,7 +440,6 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
         if (command.UpdateTime > command.MaxUpdateTime) {
             command.MaxUpdateTime = command.UpdateTime;
         }
-
         // Update output object
         ActuatorCommandSet(&command);
         // Update in case read only (eg. during servo configuration)
@@ -454,6 +457,8 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
             success &= set_channel(n, command.Channel[n], &actuatorSettings);
         }
 
+        PIOS_Servo_Update();
+
         if (!success) {
             command.NumFailedUpdates++;
             ActuatorCommandSet(&command);
@@ -476,11 +481,11 @@ float ProcessMixer(const int index, const float curve1, const float curve2,
     const Mixer_t *mixers = (Mixer_t *)&mixerSettings->Mixer1Type; // pointer to array of mixers in UAVObjects
     const Mixer_t *mixer  = &mixers[index];
 
-    float result = (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE1] / 128.0f) * curve1) +
+    float result = ((((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE1]) * curve1) +
-                   (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE2] / 128.0f) * curve2) +
+                    (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE2]) * curve2) +
-                   (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_ROLL] / 128.0f) * desired->Roll) +
+                    (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_ROLL]) * desired->Roll) +
-                   (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_PITCH] / 128.0f) * desired->Pitch) +
+                    (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_PITCH]) * desired->Pitch) +
-                   (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_YAW] / 128.0f) * desired->Yaw);
+                    (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_YAW]) * desired->Yaw)) / 128.0f;
 
     // note: no feedforward for reversable motors yet for safety reasons
     if (mixer->type == MIXERSETTINGS_MIXER1TYPE_MOTOR) {
@@ -615,6 +620,8 @@ static void setFailsafe(const ActuatorSettingsData *actuatorSettings, const Mixe
     for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) {
         set_channel(n, Channel[n], actuatorSettings);
     }
+    // Send the updated command
+    PIOS_Servo_Update();
 
     // Update output object's parts that we changed
     ActuatorCommandChannelSet(Channel);
@@ -730,8 +737,19 @@ static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSet
         return true;
 
     case ACTUATORSETTINGS_CHANNELTYPE_PWM:
-        PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], value);
+    {
+        uint8_t mode = pinsMode[actuatorSettings->ChannelAddr[mixer_channel]];
+        switch (mode) {
+        case ACTUATORSETTINGS_BANKMODE_ONESHOT125:
+            // Remap 1000-2000 range to 125-250
+            PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], value / ACTUATOR_ONESHOT125_PULSE_SCALE);
+            break;
+        default:
+            PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], value);
+            break;
+        }
         return true;
+    }
 
 #if defined(PIOS_INCLUDE_I2C_ESC)
     case ACTUATORSETTINGS_CHANNELTYPE_MK:
@@ -754,18 +772,56 @@ static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSet
  */
 static void actuator_update_rate_if_changed(const ActuatorSettingsData *actuatorSettings, bool force_update)
 {
-    static uint16_t prevChannelUpdateFreq[ACTUATORSETTINGS_CHANNELUPDATEFREQ_NUMELEM];
+    static uint16_t prevBankUpdateFreq[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM];
+    static uint8_t prevBankMode[ACTUATORSETTINGS_BANKMODE_NUMELEM];
+    bool updateMode = force_update || (memcmp(prevBankMode, actuatorSettings->BankMode, sizeof(prevBankMode)) != 0);
+    bool updateFreq = force_update || (memcmp(prevBankUpdateFreq, actuatorSettings->BankUpdateFreq, sizeof(prevBankUpdateFreq)) != 0);
 
-    // check if the any rate setting is changed
+    // check if any setting is changed
-    if (force_update ||
+    if (updateMode || updateFreq) {
-        memcmp(prevChannelUpdateFreq,
-               actuatorSettings->ChannelUpdateFreq,
-               sizeof(prevChannelUpdateFreq)) != 0) {
         /* Something has changed, apply the settings to HW */
-        memcpy(prevChannelUpdateFreq,
+
-               actuatorSettings->ChannelUpdateFreq,
+        uint16_t freq[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM];
-               sizeof(prevChannelUpdateFreq));
+        uint32_t clock[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM] = { 0 };
-        PIOS_Servo_SetHz(actuatorSettings->ChannelUpdateFreq, ACTUATORSETTINGS_CHANNELUPDATEFREQ_NUMELEM);
+        for (uint8_t i = 0; i < ACTUATORSETTINGS_BANKMODE_NUMELEM; i++) {
+            if (force_update || (actuatorSettings->BankMode[i] != prevBankMode[i])) {
+                PIOS_Servo_SetBankMode(i,
+                                       actuatorSettings->BankMode[i] ==
+                                       ACTUATORSETTINGS_BANKMODE_PWM ?
+                                       PIOS_SERVO_BANK_MODE_PWM :
+                                       PIOS_SERVO_BANK_MODE_SINGLE_PULSE
+                                       );
+            }
+            switch (actuatorSettings->BankMode[i]) {
+            case ACTUATORSETTINGS_BANKMODE_ONESHOT125:
+                freq[i]  = 100; // Value must be small enough so CCr isn't update until the PIOS_Servo_Update is triggered
+                clock[i] = ACTUATOR_ONESHOT125_CLOCK; // Setup an 2MHz timer clock
+                break;
+            case ACTUATORSETTINGS_BANKMODE_PWMSYNC:
+                freq[i]  = 100;
+                clock[i] = ACTUATOR_PWM_CLOCK;
+                break;
+            default: // PWM
+                freq[i]  = actuatorSettings->BankUpdateFreq[i];
+                clock[i] = ACTUATOR_PWM_CLOCK;
+                break;
+            }
+        }
+
+        memcpy(prevBankMode,
+               actuatorSettings->BankMode,
+               sizeof(prevBankMode));
+
+        PIOS_Servo_SetHz(freq, clock, ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM);
+
+        memcpy(prevBankUpdateFreq,
+               actuatorSettings->BankUpdateFreq,
+               sizeof(prevBankUpdateFreq));
+        // retrieve mode from related bank
+        for (uint8_t i = 0; i < MAX_MIX_ACTUATORS; i++) {
+            uint8_t bank = PIOS_Servo_GetPinBank(i);
+            pinsMode[i] = actuatorSettings->BankMode[bank];
+        }
     }
 }
 

flight/modules/Altitude/revolution/altitude.c

@@ -1,221 +0,0 @@
-/**
- ******************************************************************************
- * @addtogroup OpenPilotModules OpenPilot Modules
- * @{
- * @addtogroup AltitudeModule Altitude Module
- * @brief Communicate with BMP085 and update @ref BaroSensor "BaroSensor UAV Object"
- * @{
- *
- * @file       altitude.c
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      Altitude module, handles temperature and pressure readings from BMP085
- *
- * @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: BaroSensor
- *
- * This module will periodically update the value of the BaroSensor object.
- *
- */
-
-#include <openpilot.h>
-
-#include "altitude.h"
-#include "barosensor.h" // object that will be updated by the module
-#include "revosettings.h"
-#include <mathmisc.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    550
-#define TASK_PRIORITY       (tskIDLE_PRIORITY + 1)
-
-// Interval in number of sample to recalculate temp bias
-#define TEMP_CALIB_INTERVAL 10
-
-// LPF
-#define TEMP_DT             (1.0f / 120.0f)
-#define TEMP_LPF_FC         5.0f
-static const float temp_alpha = TEMP_DT / (TEMP_DT + 1.0f / (2.0f * M_PI_F * TEMP_LPF_FC));
-
-// Private types
-
-// Private variables
-static xTaskHandle taskHandle;
-static RevoSettingsBaroTempCorrectionPolynomialData baroCorrection;
-static RevoSettingsBaroTempCorrectionExtentData baroCorrectionExtent;
-static volatile bool tempCorrectionEnabled;
-
-static float baro_temp_bias   = 0;
-static float baro_temperature = NAN;
-static uint8_t temp_calibration_count = 0;
-
-// Private functions
-static void altitudeTask(void *parameters);
-static void SettingsUpdatedCb(UAVObjEvent *ev);
-
-/**
- * Initialise the module, called on startup
- * \returns 0 on success or -1 if initialisation failed
- */
-int32_t AltitudeStart()
-{
-    // Start main task
-    xTaskCreate(altitudeTask, "Altitude", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
-    PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ALTITUDE, taskHandle);
-
-    return 0;
-}
-
-/**
- * Initialise the module, called on startup
- * \returns 0 on success or -1 if initialisation failed
- */
-int32_t AltitudeInitialize()
-{
-    BaroSensorInitialize();
-    RevoSettingsInitialize();
-    RevoSettingsConnectCallback(&SettingsUpdatedCb);
-    SettingsUpdatedCb(NULL);
-#if defined(PIOS_INCLUDE_HCSR04)
-    SonarAltitudeInitialize();
-#endif
-    return 0;
-}
-MODULE_INITCALL(AltitudeInitialize, AltitudeStart);
-/**
- * Module thread, should not return.
- */
-static void altitudeTask(__attribute__((unused)) void *parameters)
-{
-    BaroSensorData data;
-
-#if defined(PIOS_INCLUDE_HCSR04)
-    SonarAltitudeData sonardata;
-    int32_t value = 0, timeout = 10, sample_rate = 0;
-    float coeff   = 0.25, height_out = 0, height_in = 0;
-    PIOS_HCSR04_Trigger();
-#endif
-
-    // TODO: Check the pressure sensor and set a warning if it fails test
-
-// Option to change the interleave between Temp and Pressure conversions
-// 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
-    // Main task loop
-    while (1) {
-#if defined(PIOS_INCLUDE_HCSR04)
-        // Compute the current altitude
-        // depends on baro samplerate
-        if (!(sample_rate--)) {
-            if (PIOS_HCSR04_Completed()) {
-                value = PIOS_HCSR04_Get();
-                // from 3.4cm to 5.1m
-                if ((value > 100) && (value < 15000)) {
-                    height_in  = value * 0.00034f / 2.0f;
-                    height_out = (height_out * (1 - coeff)) + (height_in * coeff);
-                    sonardata.Altitude = height_out; // m/us
-                }
-
-                // Update the SonarAltitude UAVObject
-                SonarAltitudeSet(&sonardata);
-                timeout = 10;
-                PIOS_HCSR04_Trigger();
-            }
-            if (!(timeout--)) {
-                // retrigger
-                timeout = 10;
-                PIOS_HCSR04_Trigger();
-            }
-            sample_rate = 25;
-        }
-#endif /* if defined(PIOS_INCLUDE_HCSR04) */
-        float temp, press;
-#ifdef PIOS_MS5611_SLOW_TEMP_RATE
-        temp_press_interleave_count--;
-        if (temp_press_interleave_count == 0) {
-#endif
-        // Update the temperature data
-        PIOS_MS5611_StartADC(TemperatureConv);
-        vTaskDelay(PIOS_MS5611_GetDelay());
-        PIOS_MS5611_ReadADC();
-
-#ifdef PIOS_MS5611_SLOW_TEMP_RATE
-        temp_press_interleave_count = PIOS_MS5611_SLOW_TEMP_RATE;
-    }
-#endif
-
-        // Update the pressure data
-        PIOS_MS5611_StartADC(PressureConv);
-        vTaskDelay(PIOS_MS5611_GetDelay());
-        PIOS_MS5611_ReadADC();
-
-        temp  = PIOS_MS5611_GetTemperature();
-        press = PIOS_MS5611_GetPressure();
-
-        if (isnan(baro_temperature)) {
-            baro_temperature = temp;
-        }
-
-        baro_temperature = temp_alpha * (temp - baro_temperature) + baro_temperature;
-
-        if (tempCorrectionEnabled && !temp_calibration_count) {
-            temp_calibration_count = TEMP_CALIB_INTERVAL;
-            // 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 = boundf(baro_temperature, baroCorrectionExtent.max, baroCorrectionExtent.min);
-            baro_temp_bias = baroCorrection.a + ((baroCorrection.d * ctemp + baroCorrection.c) * ctemp + baroCorrection.b) * ctemp;
-        }
-
-        press -= baro_temp_bias;
-
-        float altitude = 44330.0f * (1.0f - powf((press) / MS5611_P0, (1.0f / 5.255f)));
-
-        if (!isnan(altitude)) {
-            data.Altitude    = altitude;
-            data.Temperature = temp;
-            data.Pressure    = press;
-            // Update the BasoSensor UAVObject
-            BaroSensorSet(&data);
-        }
-    }
-}
-
-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/Attitude/attitude.c

@@ -62,6 +62,10 @@
 #include "manualcontrolcommand.h"
 #include "taskinfo.h"
 
+#include <pios_sensors.h>
+#include <pios_adxl345.h>
+#include <pios_mpu6000.h>
+
 #include "CoordinateConversions.h"
 #include <pios_notify.h>
 #include <mathmisc.h>
@@ -160,9 +164,11 @@ int32_t const MAX_TRIM_FLIGHT_SAMPLES = 65535;
 #define STD_CC_ANALOG_GYRO_NEUTRAL 1665
 #define STD_CC_ANALOG_GYRO_GAIN    0.42f
 
+static struct PIOS_SENSORS_3Axis_SensorsWithTemp *mpu6000_data = NULL;
+
 // Used to detect CC vs CC3D
 static const struct pios_board_info *bdinfo = &pios_board_info_blob;
-#define BOARDISCC3D                (bdinfo->board_rev == 0x02)
+#define BOARDISCC3D (bdinfo->board_rev == 0x02)
 /**
  * Initialise the module, called on startup
  * \returns 0 on success or -1 if initialisation failed
@@ -172,8 +178,9 @@ int32_t AttitudeStart(void)
     // Start main task
     xTaskCreate(AttitudeTask, "Attitude", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
     PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ATTITUDE, taskHandle);
+#ifdef PIOS_INCLUDE_WDG
     PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE);
-
+#endif
     return 0;
 }
 
@@ -238,14 +245,18 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
 
     if (cc3d) {
 #if defined(PIOS_INCLUDE_MPU6000)
-        gyro_test = PIOS_MPU6000_Test();
+
+        gyro_test    = PIOS_MPU6000_Driver.test(0);
+        mpu6000_data = pios_malloc(sizeof(PIOS_SENSORS_3Axis_SensorsWithTemp) + sizeof(Vector3i16) * 2);
 #endif
     } else {
 #if defined(PIOS_INCLUDE_ADXL345)
         // Set critical error and wait until the accel is producing data
         while (PIOS_ADXL345_FifoElements() == 0) {
             AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
+#ifdef PIOS_INCLUDE_WDG
             PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
+#endif
         }
         accel_test = PIOS_ADXL345_Test();
 #endif
@@ -302,9 +313,9 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
             }
             init = 1;
         }
-
+#ifdef PIOS_INCLUDE_WDG
         PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
-
+#endif
         AccelStateData accelState;
         GyroStateData gyros;
         int32_t retval = 0;
@@ -455,7 +466,6 @@ 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
  */
-static struct pios_mpu6000_data mpu6000_data;
 static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *gyrosData)
 {
     float accels[3] = { 0 };
@@ -465,22 +475,22 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *
 
 #if defined(PIOS_INCLUDE_MPU6000)
 
-    xQueueHandle queue = PIOS_MPU6000_GetQueue();
+    xQueueHandle queue = PIOS_MPU6000_Driver.get_queue(0);
-    BaseType_t ret     = xQueueReceive(queue, (void *)&mpu6000_data, sensor_period_ms);
+    BaseType_t ret     = xQueueReceive(queue, (void *)mpu6000_data, sensor_period_ms);
     while (ret == pdTRUE) {
-        gyros[0]  += mpu6000_data.gyro_x;
+        gyros[0]  += mpu6000_data->sample[1].x;
-        gyros[1]  += mpu6000_data.gyro_y;
+        gyros[1]  += mpu6000_data->sample[1].y;
-        gyros[2]  += mpu6000_data.gyro_z;
+        gyros[2]  += mpu6000_data->sample[1].z;
 
-        accels[0] += mpu6000_data.accel_x;
+        accels[0] += mpu6000_data->sample[0].x;
-        accels[1] += mpu6000_data.accel_y;
+        accels[1] += mpu6000_data->sample[0].y;
-        accels[2] += mpu6000_data.accel_z;
+        accels[2] += mpu6000_data->sample[0].z;
 
-        temp += mpu6000_data.temperature;
+        temp += mpu6000_data->temperature;
 
         count++;
         // check if further samples are already in queue
-        ret = xQueueReceive(queue, (void *)&mpu6000_data, 0);
+        ret = xQueueReceive(queue, (void *)mpu6000_data, 0);
     }
     PERF_TRACK_VALUE(counterAccelSamples, count);
 
@@ -751,17 +761,19 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
     temp_calibrated_extent.max = accelGyroSettings.temp_calibrated_extent.max;
 
     if (BOARDISCC3D) {
+        float scales[2];
+        PIOS_MPU6000_Driver.get_scale(scales, 2, 0);
         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.X  = accelGyroSettings.gyro_scale.X * scales[1];
-        gyro_scale.Y  = accelGyroSettings.gyro_scale.Y * PIOS_MPU6000_GetScale();
+        gyro_scale.Y  = accelGyroSettings.gyro_scale.Y * scales[1];
-        gyro_scale.Z  = accelGyroSettings.gyro_scale.Z * PIOS_MPU6000_GetScale();
+        gyro_scale.Z  = accelGyroSettings.gyro_scale.Z * scales[1];
 
-        accel_scale.X = accelGyroSettings.accel_scale.X * PIOS_MPU6000_GetAccelScale();
+        accel_scale.X = accelGyroSettings.accel_scale.X * scales[0];
-        accel_scale.Y = accelGyroSettings.accel_scale.Y * PIOS_MPU6000_GetAccelScale();
+        accel_scale.Y = accelGyroSettings.accel_scale.Y * scales[0];
-        accel_scale.Z = accelGyroSettings.accel_scale.Z * PIOS_MPU6000_GetAccelScale();
+        accel_scale.Z = accelGyroSettings.accel_scale.Z * scales[0];
     } else {
         // Original CC with analog gyros and ADXL accel
         accel_bias.X  = accelGyroSettings.accel_bias.X;

flight/modules/GPS/GPS.c

@@ -91,7 +91,12 @@ void updateGpsSettings(UAVObjEvent *ev);
 #else
 #if defined(PIOS_GPS_MINIMAL)
         #define GPS_READ_BUFFER    32
-        #define STACK_SIZE_BYTES   500
+
+#ifdef PIOS_INCLUDE_GPS_NMEA_PARSER
+        #define STACK_SIZE_BYTES   580 // NMEA
+#else
+        #define STACK_SIZE_BYTES   440 // UBX
+#endif // PIOS_INCLUDE_GPS_NMEA_PARSER
 #else
         #define STACK_SIZE_BYTES   650
 #endif // PIOS_GPS_MINIMAL
@@ -203,9 +208,11 @@ int32_t GPSInitialize(void)
         GPSSettingsInitialize();
         GPSSettingsDataProtocolGet(&gpsProtocol);
         switch (gpsProtocol) {
+#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
         case GPSSETTINGS_DATAPROTOCOL_NMEA:
             gps_rx_buffer = pios_malloc(NMEA_MAX_PACKET_LENGTH);
             break;
+#endif
         case GPSSETTINGS_DATAPROTOCOL_UBX:
             gps_rx_buffer = pios_malloc(sizeof(struct UBXPacket));
             break;
@@ -459,12 +466,12 @@ static void updateHwSettings()
     }
 }
 
-#ifdef PIOS_INCLUDE_GPS_UBX_PARSER
+#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
 void AuxMagSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 {
     load_mag_settings();
 }
-#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
+
 void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev)
 {
     uint8_t ubxAutoConfig;
@@ -537,7 +544,6 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev)
     ubx_autoconfig_set(newconfig);
 }
 #endif /* if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) */
-#endif /* ifdef PIOS_INCLUDE_GPS_UBX_PARSER */
 /**
  * @}
  * @}

flight/modules/GPS/NMEA.c

@@ -40,14 +40,13 @@
 #include "GPS.h"
 
 // #define ENABLE_DEBUG_MSG						///< define to enable debug-messages
-#define DEBUG_PORT PIOS_COM_TELEM_RF ///< defines which serial port is ued for debug-messages
+#define DEBUG_PORT PIOS_COM_TELEM_RF ///< defines which serial port is used for debug-messages
-
 
 // Debugging
 #ifdef ENABLE_DEBUG_MSG
 // #define DEBUG_MSG_IN			///< define to display the incoming NMEA messages
 // #define DEBUG_PARAMS			///< define to display the incoming NMEA messages split into its parameters
-// #define DEBUG_MGSID_IN		///< define to display the the names of the incoming NMEA messages
+// #define DEBUG_MSGID_IN		///< define to display the names of the incoming NMEA messages
 // #define NMEA_DEBUG_PKT		///< define to enable debug of all NMEA messages
 // #define NMEA_DEBUG_GGA		///< define to enable debug of GGA messages
 // #define NMEA_DEBUG_VTG		///< define to enable debug of VTG messages
@@ -68,40 +67,40 @@ struct nmea_parser {
     bool (*handler)(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
 };
 
-static bool nmeaProcessGPGGA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
+static bool nmeaProcessGxGGA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
-static bool nmeaProcessGPRMC(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
+static bool nmeaProcessGxRMC(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
-static bool nmeaProcessGPVTG(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
+static bool nmeaProcessGxVTG(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
-static bool nmeaProcessGPGSA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
+static bool nmeaProcessGxGSA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
 #if !defined(PIOS_GPS_MINIMAL)
-static bool nmeaProcessGPZDA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
+static bool nmeaProcessGxZDA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
-static bool nmeaProcessGPGSV(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
+static bool nmeaProcessGxGSV(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
 #endif // PIOS_GPS_MINIMAL
 
 static const struct nmea_parser nmea_parsers[] = {
     {
-        .prefix  = "GPGGA",
+        .prefix  = "GGA",
-        .handler = nmeaProcessGPGGA,
+        .handler = nmeaProcessGxGGA,
     },
     {
-        .prefix  = "GPVTG",
+        .prefix  = "VTG",
-        .handler = nmeaProcessGPVTG,
+        .handler = nmeaProcessGxVTG,
     },
     {
-        .prefix  = "GPGSA",
+        .prefix  = "GSA",
-        .handler = nmeaProcessGPGSA,
+        .handler = nmeaProcessGxGSA,
     },
     {
-        .prefix  = "GPRMC",
+        .prefix  = "RMC",
-        .handler = nmeaProcessGPRMC,
+        .handler = nmeaProcessGxRMC,
     },
 #if !defined(PIOS_GPS_MINIMAL)
     {
-        .prefix  = "GPZDA",
+        .prefix  = "ZDA",
-        .handler = nmeaProcessGPZDA,
+        .handler = nmeaProcessGxZDA,
     },
     {
-        .prefix  = "GPGSV",
+        .prefix  = "GSV",
-        .handler = nmeaProcessGPGSV,
+        .handler = nmeaProcessGxGSV,
     },
 #endif // PIOS_GPS_MINIMAL
 };
@@ -373,6 +372,8 @@ bool NMEA_update_position(char *nmea_sentence, GPSPositionSensorData *GpsData)
     // Sample NMEA message: "GPRMC,000131.736,V,,,,,0.00,0.00,060180,,,N*43"
 
     // The first parameter starts at the beginning of the message
+    // Skip first two character, allow GL, GN, GP...
+    p += 2;
     params[0] = p;
     nbParams  = 1;
     while (*p != 0) {
@@ -406,11 +407,13 @@ bool NMEA_update_position(char *nmea_sentence, GPSPositionSensorData *GpsData)
     parser = NMEA_find_parser_by_prefix(params[0]);
     if (!parser) {
         // No parser found
+                #ifdef DEBUG_MSGID_IN
         DEBUG_MSG(" NO PARSER (\"%s\")\n", params[0]);
+                #endif
         return false;
     }
 
-        #ifdef DEBUG_MGSID_IN
+        #ifdef DEBUG_MSGID_IN
     DEBUG_MSG("%s %d ", params[0]);
         #endif
     // Send the message to the parser and get it update the GpsData
@@ -422,7 +425,9 @@ bool NMEA_update_position(char *nmea_sentence, GPSPositionSensorData *GpsData)
 
     if (!parser->handler(GpsData, &gpsDataUpdated, params, nbParams)) {
         // Parse failed
+                #ifdef DEBUG_MSGID_IN
         DEBUG_MSG("PARSE FAILED (\"%s\")\n", params[0]);
+                #endif
         if (gpsDataUpdated && (GpsData->Status == GPSPOSITIONSENSOR_STATUS_NOFIX)) {
             GPSPositionSensorSet(GpsData);
         }
@@ -432,13 +437,13 @@ bool NMEA_update_position(char *nmea_sentence, GPSPositionSensorData *GpsData)
 
     // All is fine :)  Update object if data has changed
     if (gpsDataUpdated) {
-                #ifdef DEBUG_MGSID_IN
+                #ifdef DEBUG_MSGID_IN
         DEBUG_MSG("U");
                 #endif
         GPSPositionSensorSet(GpsData);
     }
 
-        #ifdef DEBUG_MGSID_IN
+        #ifdef DEBUG_MSGID_IN
     DEBUG_MSG("\n");
         #endif
     return true;
@@ -446,11 +451,11 @@ bool NMEA_update_position(char *nmea_sentence, GPSPositionSensorData *GpsData)
 
 
 /**
- * Parse an NMEA GPGGA sentence and update the given UAVObject
+ * Parse an NMEA GxGGA sentence and update the given UAVObject
  * \param[in] A pointer to a GPSPositionSensor UAVObject to be updated.
  * \param[in] An NMEA sentence with a valid checksum
  */
-static bool nmeaProcessGPGGA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
+static bool nmeaProcessGxGGA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
 {
     if (nbParam != 15) {
         return false;
@@ -499,11 +504,11 @@ static bool nmeaProcessGPGGA(GPSPositionSensorData *GpsData, bool *gpsDataUpdate
 }
 
 /**
- * Parse an NMEA GPRMC sentence and update the given UAVObject
+ * Parse an NMEA GxRMC sentence and update the given UAVObject
  * \param[in] A pointer to a GPSPositionSensor UAVObject to be updated.
  * \param[in] An NMEA sentence with a valid checksum
  */
-static bool nmeaProcessGPRMC(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
+static bool nmeaProcessGxRMC(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
 {
     if (nbParam != 13) {
         return false;
@@ -567,11 +572,11 @@ static bool nmeaProcessGPRMC(GPSPositionSensorData *GpsData, bool *gpsDataUpdate
 }
 
 /**
- * Parse an NMEA GPVTG sentence and update the given UAVObject
+ * Parse an NMEA GxVTG sentence and update the given UAVObject
  * \param[in] A pointer to a GPSPositionSensor UAVObject to be updated.
  * \param[in] An NMEA sentence with a valid checksum
  */
-static bool nmeaProcessGPVTG(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
+static bool nmeaProcessGxVTG(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
 {
     if (nbParam != 9 && nbParam != 10 /*GTOP GPS seems to gemnerate an extra parameter...*/) {
         return false;
@@ -592,11 +597,11 @@ static bool nmeaProcessGPVTG(GPSPositionSensorData *GpsData, bool *gpsDataUpdate
 
 #if !defined(PIOS_GPS_MINIMAL)
 /**
- * Parse an NMEA GPZDA sentence and update the @ref GPSTime object
+ * Parse an NMEA GxZDA sentence and update the @ref GPSTime object
  * \param[in] A pointer to a GPSPositionSensor UAVObject to be updated (unused).
  * \param[in] An NMEA sentence with a valid checksum
  */
-static bool nmeaProcessGPZDA(__attribute__((unused)) GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
+static bool nmeaProcessGxZDA(__attribute__((unused)) GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
 {
     if (nbParam != 7) {
         return false;
@@ -636,7 +641,7 @@ static uint8_t gsv_processed_mask;
 static uint16_t gsv_incomplete_error;
 static uint16_t gsv_duplicate_error;
 
-static bool nmeaProcessGPGSV(__attribute__((unused)) GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
+static bool nmeaProcessGxGSV(__attribute__((unused)) GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
 {
     if (nbParam < 4) {
         return false;
@@ -723,7 +728,7 @@ static bool nmeaProcessGPGSV(__attribute__((unused)) GPSPositionSensorData *GpsD
  * \param[in] A pointer to a GPSPositionSensor UAVObject to be updated.
  * \param[in] An NMEA sentence with a valid checksum
  */
-static bool nmeaProcessGPGSA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
+static bool nmeaProcessGxGSA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
 {
     if (nbParam != 18) {
         return false;

flight/modules/GPS/ubx_autoconfig.c

@@ -399,6 +399,7 @@ void ubx_autoconfig_set(ubx_autoconfig_settings_t config)
     if (config.autoconfigEnabled) {
         if (!status) {
             status = (status_t *)pios_malloc(sizeof(status_t));
+            PIOS_Assert(status);
             memset(status, 0, sizeof(status_t));
             status->currentStep = INIT_STEP_DISABLED;
         }
@@ -406,7 +407,7 @@ void ubx_autoconfig_set(ubx_autoconfig_settings_t config)
         status->currentStep     = INIT_STEP_START;
         enabled = true;
     } else {
-        if (!status) {
+        if (status) {
             status->currentStep = INIT_STEP_DISABLED;
         }
     }

flight/modules/ManualControl/manualcontrol.c

@@ -43,8 +43,8 @@
 #include <systemsettings.h>
 #include <stabilizationdesired.h>
 #include <callbackinfo.h>
-#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
 #include <stabilizationsettings.h>
+#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
 #include <vtolpathfollowersettings.h>
 #endif
 
@@ -160,9 +160,9 @@ int32_t ManualControlInitialize()
     ManualControlSettingsInitialize();
     FlightModeSettingsInitialize();
     SystemSettingsInitialize();
+    StabilizationSettingsInitialize();
 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
     VtolSelfTuningStatsInitialize();
-    StabilizationSettingsInitialize();
     VtolPathFollowerSettingsInitialize();
 #endif
     callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&manualControlTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_MANUALCONTROL, STACK_SIZE_BYTES);

flight/modules/OPLink/oplinkmod.c

@@ -158,7 +158,6 @@ static void systemTask(__attribute__((unused)) void *parameters)
             oplinkStatus.RxMissed = radio_stats.rx_missed;
             oplinkStatus.RxFailure     = radio_stats.rx_failure;
             oplinkStatus.TxDropped     = radio_stats.tx_dropped;
-            oplinkStatus.TxResent = radio_stats.tx_resent;
             oplinkStatus.TxFailure     = radio_stats.tx_failure;
             oplinkStatus.Resets      = radio_stats.resets;
             oplinkStatus.Timeouts    = radio_stats.timeouts;

flight/modules/Receiver/receiver.c

@@ -135,7 +135,7 @@ int32_t ReceiverInitialize()
 {
     /* Check the assumptions about uavobject enum's are correct */
     PIOS_STATIC_ASSERT(assumptions);
-
+    AccessoryDesiredInitialize();
     ManualControlCommandInitialize();
     ReceiverActivityInitialize();
     ManualControlSettingsInitialize();

flight/modules/Sensors/inc/sensors.h

@@ -2,16 +2,17 @@
  ******************************************************************************
  * @addtogroup OpenPilotModules OpenPilot Modules
  * @{
- * @addtogroup Sensors Sensors Module
+ * @addtogroup Sensors
+ * @brief Acquires sensor data
  * @{
  *
- * @file       attitude.h
+ * @file       sensors.c
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
- * @brief      Acquires sensor data and fuses it into attitude estimate for CC
+ * @brief      Module to handle fetch and preprocessing of sensor data
  *
  * @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

flight/modules/Sensors/sensors.c

@@ -4,12 +4,11 @@
  * @{
  * @addtogroup Sensors
  * @brief Acquires sensor data
- * Specifically updates the the @ref GyroSensor, @ref AccelSensor, and @ref MagSensor objects
  * @{
  *
  * @file       sensors.c
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
- * @brief      Module to handle all comms to the AHRS on a periodic basis.
+ * @brief      Module to handle fetch and preprocessing of sensor data
  *
  * @see        The GNU Public License (GPL) Version 3
  *
@@ -47,69 +46,118 @@
  */
 
 #include <openpilot.h>
-
+#include <pios_sensors.h>
 #include <homelocation.h>
+
 #include <magsensor.h>
 #include <accelsensor.h>
 #include <gyrosensor.h>
-#include <attitudestate.h>
+#include <barosensor.h>
+#include <flightstatus.h>
+
 #include <attitudesettings.h>
 #include <revocalibration.h>
 #include <accelgyrosettings.h>
-#include <flightstatus.h>
+#include <revosettings.h>
+
+#include <mathmisc.h>
 #include <taskinfo.h>
 #include <pios_math.h>
+#include <pios_constants.h>
 #include <CoordinateConversions.h>
-
 #include <pios_board_info.h>
+#include <string.h>
 
 // Private constants
-#define STACK_SIZE_BYTES    1000
+#define STACK_SIZE_BYTES         1000
-#define TASK_PRIORITY       (tskIDLE_PRIORITY + 3)
+#define TASK_PRIORITY            (tskIDLE_PRIORITY + 3)
 
-static const uint32_t sensor_period_ms = ((uint32_t)1000.0f / PIOS_SENSOR_RATE);
+#define MAX_SENSORS_PER_INSTANCE 2
+#ifdef PIOS_INCLUDE_WDG
+#define RELOAD_WDG()   PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS)
+#define REGISTER_WDG() PIOS_WDG_RegisterFlag(PIOS_WDG_SENSORS)
+#else
+#define RELOAD_WDG()
+#define REGISTER_WDG()
+#endif
+
+static const TickType_t sensor_period_ticks = ((uint32_t)1000.0f / PIOS_SENSOR_RATE) / portTICK_RATE_MS;
 
 // Interval in number of sample to recalculate temp bias
-#define TEMP_CALIB_INTERVAL 30
+#define TEMP_CALIB_INTERVAL      30
 
 // LPF
-#define TEMP_DT             (1.0f / PIOS_SENSOR_RATE)
+#define TEMP_DT_GYRO_ACCEL       (1.0f / PIOS_SENSOR_RATE)
-#define TEMP_LPF_FC         5.0f
+#define TEMP_LPF_FC_GYRO_ACCEL   5.0f
-static const float temp_alpha = TEMP_DT / (TEMP_DT + 1.0f / (2.0f * M_PI_F * TEMP_LPF_FC));
+static const float temp_alpha_gyro_accel = LPF_ALPHA(TEMP_DT_GYRO_ACCEL, TEMP_LPF_FC_GYRO_ACCEL);
 
-#define ZERO_ROT_ANGLE      0.00001f
+// Interval in number of sample to recalculate temp bias
+#define BARO_TEMP_CALIB_INTERVAL 10
+
+// LPF
+#define TEMP_DT_BARO             (1.0f / 120.0f)
+#define TEMP_LPF_FC_BARO         5.0f
+static const float temp_alpha_baro = TEMP_DT_BARO / (TEMP_DT_BARO + 1.0f / (2.0f * M_PI_F * TEMP_LPF_FC_BARO));
+
+
+#define ZERO_ROT_ANGLE           0.00001f
 // Private types
+typedef struct {
+    // used to accumulate all samples in a task iteration
+    Vector3i32 accum[2];
+    int32_t    temperature;
+    uint32_t   count;
+} sensor_fetch_context;
+
+#define MAX_SENSOR_DATA_SIZE (sizeof(PIOS_SENSORS_3Axis_SensorsWithTemp) + MAX_SENSORS_PER_INSTANCE * sizeof(Vector3i16))
+typedef union {
+    PIOS_SENSORS_3Axis_SensorsWithTemp sensorSample3Axis;
+    PIOS_SENSORS_1Axis_SensorsWithTemp sensorSample1Axis;
+} sensor_data;
+
 #define PIOS_INSTRUMENT_MODULE
 #include <pios_instrumentation_helper.h>
 
-PERF_DEFINE_COUNTER(counterGyroSamples);
+PERF_DEFINE_COUNTER(counterAccelSamples);
+PERF_DEFINE_COUNTER(counterAccelPeriod);
+PERF_DEFINE_COUNTER(counterMagPeriod);
+PERF_DEFINE_COUNTER(counterBaroPeriod);
 PERF_DEFINE_COUNTER(counterSensorPeriod);
-
+PERF_DEFINE_COUNTER(counterSensorResets);
-// Counters:
-// - 0x53000001 Sensor fetch rate(period)
-// - 0x53000002 number of gyro samples read for each loop
 
 // Private functions
 static void SensorsTask(void *parameters);
 static void settingsUpdatedCb(UAVObjEvent *objEv);
 
+static void accumulateSamples(sensor_fetch_context *sensor_context, sensor_data *sample);
+static void processSamples3d(sensor_fetch_context *sensor_context, const PIOS_SENSORS_Instance *sensor);
+static void processSamples1d(PIOS_SENSORS_1Axis_SensorsWithTemp *sample, const PIOS_SENSORS_Instance *sensor);
+
+static void clearContext(sensor_fetch_context *sensor_context);
+
+static void handleAccel(float *samples, float temperature);
+static void handleGyro(float *samples, float temperature);
+static void handleMag(float *samples, float temperature);
+static void handleBaro(float sample, float temperature);
+
+static void updateAccelTempBias(float temperature);
+static void updateGyroTempBias(float temperature);
+static void updateBaroTempBias(float temperature);
+
 // Private variables
+static sensor_data *source_data;
 static xTaskHandle sensorsTaskHandle;
 RevoCalibrationData cal;
 AccelGyroSettingsData agcal;
 
-#ifdef PIOS_INCLUDE_HMC5X83
-#include <pios_hmc5x83.h>
-extern pios_hmc5x83_dev_t onboard_mag;
-#endif
-
 // 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_transform[3][3] = {
     { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }
 };
-// temp coefficient to calculate gyro bias
+
+// Variables used to handle accel/gyro temperature bias
 static volatile bool gyro_temp_calibrated  = false;
 static volatile bool accel_temp_calibrated = false;
 
@@ -117,42 +165,45 @@ static float accel_temperature  = NAN;
 static float gyro_temperature   = NAN;
 static float accel_temp_bias[3] = { 0 };
 static float gyro_temp_bias[3] = { 0 };
-static uint8_t temp_calibration_count = 0;
+static uint8_t accel_temp_calibration_count = 0;
-
+static uint8_t gyro_temp_calibration_count  = 0;
 
 static float R[3][3] = {
     { 0 }
 };
+// Variables used to handle baro temperature bias
+static RevoSettingsBaroTempCorrectionPolynomialData baroCorrection;
+static RevoSettingsBaroTempCorrectionExtentData baroCorrectionExtent;
+static volatile bool baro_temp_correction_enabled;
+static float baro_temp_bias   = 0;
+static float baro_temperature = NAN;
+static uint8_t baro_temp_calibration_count = 0;
+
 static int8_t rotate = 0;
 
-/**
- * API for sensor fusion algorithms:
- * Configure(xQueueHandle gyro, xQueueHandle accel, xQueueHandle mag, xQueueHandle baro)
- *   Stores all the queues the algorithm will pull data from
- * FinalizeSensors() -- before saving the sensors modifies them based on internal state (gyro bias)
- * Update() -- queries queues and updates the attitude estiamte
- */
-
-
 /**
  * Initialise the module.  Called before the start function
  * \returns 0 on success or -1 if initialisation failed
  */
 int32_t SensorsInitialize(void)
 {
+    source_data = (sensor_data *)pios_malloc(MAX_SENSOR_DATA_SIZE);
     GyroSensorInitialize();
     AccelSensorInitialize();
     MagSensorInitialize();
+    BaroSensorInitialize();
     RevoCalibrationInitialize();
+    RevoSettingsInitialize();
     AttitudeSettingsInitialize();
     AccelGyroSettingsInitialize();
 
     rotate = 0;
 
+    RevoSettingsConnectCallback(&settingsUpdatedCb);
     RevoCalibrationConnectCallback(&settingsUpdatedCb);
     AttitudeSettingsConnectCallback(&settingsUpdatedCb);
-
     AccelGyroSettingsConnectCallback(&settingsUpdatedCb);
+
     return 0;
 }
 
@@ -165,10 +216,7 @@ int32_t SensorsStart(void)
     // Start main task
     xTaskCreate(SensorsTask, "Sensors", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &sensorsTaskHandle);
     PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_SENSORS, sensorsTaskHandle);
-#ifdef PIOS_INCLUDE_WDG
+    REGISTER_WDG();
-    PIOS_WDG_RegisterFlag(PIOS_WDG_SENSORS);
-#endif
-
     return 0;
 }
 
@@ -184,82 +232,55 @@ int32_t mag_test;
  * The sensor task.  This polls the gyros at 500 Hz and pumps that data to
  * stabilization and to the attitude loop
  *
- * This function has a lot of if/defs right now to allow these configurations:
- * 1. BMA180 accel and MPU6000 gyro
- * 2. MPU6000 gyro and accel
- * 3. BMA180 accel and L3GD20 gyro
  */
 
 uint32_t sensor_dt_us;
 static void SensorsTask(__attribute__((unused)) void *parameters)
 {
     portTickType lastSysTime;
-    uint32_t accel_samples = 0;
+    sensor_fetch_context sensor_context;
-    uint32_t gyro_samples  = 0;
+    bool error = false;
-    int32_t accel_accum[3] = { 0, 0, 0 };
+    const PIOS_SENSORS_Instance *sensors_list = PIOS_SENSORS_GetList();
-    int32_t gyro_accum[3]  = { 0, 0, 0 };
+    PIOS_SENSORS_Instance *sensor;
-    float gyro_scaling     = 0;
-    float accel_scaling    = 0;
-    static int32_t timeval;
 
     AlarmsClear(SYSTEMALARMS_ALARM_SENSORS);
+    settingsUpdatedCb(NULL);
 
-    UAVObjEvent ev;
+    // Performance counters
-    settingsUpdatedCb(&ev);
+    PERF_INIT_COUNTER(counterAccelSamples, 0x53000001);
+    PERF_INIT_COUNTER(counterAccelPeriod, 0x53000002);
+    PERF_INIT_COUNTER(counterMagPeriod, 0x53000003);
+    PERF_INIT_COUNTER(counterBaroPeriod, 0x53000004);
+    PERF_INIT_COUNTER(counterSensorPeriod, 0x53000005);
+    PERF_INIT_COUNTER(counterSensorResets, 0x53000006);
 
-    const struct pios_board_info *bdinfo = &pios_board_info_blob;
+    // Test sensors
-
+    bool sensors_test = true;
-    switch (bdinfo->board_rev) {
+    uint8_t count     = 0;
-    case 0x01:
+    LL_FOREACH((PIOS_SENSORS_Instance *)sensors_list, sensor) {
-#if defined(PIOS_INCLUDE_L3GD20)
+        sensors_test &= PIOS_SENSORS_Test(sensor);
-        gyro_test  = PIOS_L3GD20_Test();
+        count++;
-#endif
-#if defined(PIOS_INCLUDE_BMA180)
-        accel_test = PIOS_BMA180_Test();
-#endif
-        break;
-    case 0x02:
-#if defined(PIOS_INCLUDE_MPU6000)
-        gyro_test  = PIOS_MPU6000_Test();
-        accel_test = gyro_test;
-#endif
-        break;
-    default:
-        PIOS_DEBUG_Assert(0);
     }
 
-#if defined(PIOS_INCLUDE_HMC5X83)
+    PIOS_Assert(count);
-    mag_test = PIOS_HMC5x83_Test(onboard_mag);
+    RELOAD_WDG();
-#else
+    if (!sensors_test) {
-    mag_test = 0;
-#endif
-
-    if (accel_test < 0 || gyro_test < 0 || mag_test < 0) {
         AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL);
         while (1) {
-#ifdef PIOS_INCLUDE_WDG
-            PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
-#endif
             vTaskDelay(10);
         }
     }
-    PERF_INIT_COUNTER(counterGyroSamples, 0x53000001);
+
-    PERF_INIT_COUNTER(counterSensorPeriod, 0x53000002);
     // Main task loop
     lastSysTime = xTaskGetTickCount();
-    bool error = false;
+    uint32_t reset_counter = 0;
-    uint32_t mag_update_time = PIOS_DELAY_GetRaw();
+
     while (1) {
         // TODO: add timeouts to the sensor reads and set an error if the fail
-        sensor_dt_us = PIOS_DELAY_DiffuS(timeval);
-        timeval = PIOS_DELAY_GetRaw();
-
         if (error) {
-#ifdef PIOS_INCLUDE_WDG
+            RELOAD_WDG();
-            PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
-#endif
             lastSysTime = xTaskGetTickCount();
-            vTaskDelayUntil(&lastSysTime, sensor_period_ms / portTICK_RATE_MS);
+            vTaskDelayUntil(&lastSysTime, sensor_period_ticks);
             AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL);
             error = false;
         } else {
@@ -267,219 +288,250 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
         }
 
 
-        for (int i = 0; i < 3; i++) {
+        // reset the fetch context
-            accel_accum[i] = 0;
+        clearContext(&sensor_context);
-            gyro_accum[i]  = 0;
+        LL_FOREACH((PIOS_SENSORS_Instance *)sensors_list, sensor) {
-        }
+            // we will wait on the sensor that's marked as primary( that means the sensor with higher sample rate)
-        accel_samples = 0;
+            bool is_primary = (sensor->type & PIOS_SENSORS_TYPE_3AXIS_ACCEL);
-        gyro_samples  = 0;
 
-        AccelSensorData accelSensorData;
+            if (!sensor->driver->is_polled) {
-        GyroSensorData gyroSensorData;
+                const QueueHandle_t queue = PIOS_SENSORS_GetQueue(sensor);
-
+                while (xQueueReceive(queue,
-        switch (bdinfo->board_rev) {
+                                     (void *)source_data,
-        case 0x01: // L3GD20 + BMA180 board
+                                     (is_primary && !sensor_context.count) ? sensor_period_ticks : 0) == pdTRUE) {
-#if defined(PIOS_INCLUDE_BMA180)
+                    accumulateSamples(&sensor_context, source_data);
-            {
-                struct pios_bma180_data accel;
-
-                int32_t read_good;
-                int32_t count;
-
-                count = 0;
-                while ((read_good = PIOS_BMA180_ReadFifo(&accel)) != 0 && !error) {
-                    error = ((xTaskGetTickCount() - lastSysTime) > sensor_period_ms) ? true : error;
                 }
-                if (error) {
+                if (sensor_context.count) {
-                    // Unfortunately if the BMA180 ever misses getting read, then it will not
+                    processSamples3d(&sensor_context, sensor);
-                    // trigger more interrupts.  In this case we must force a read to kickstarts
+                    clearContext(&sensor_context);
-                    // it.
+                } else if (is_primary) {
-                    struct pios_bma180_data data;
+                    PIOS_SENSOR_Reset(sensor);
-                    PIOS_BMA180_ReadAccels(&data);
+                    reset_counter++;
-                    continue;
+                    PERF_TRACK_VALUE(counterSensorResets, reset_counter);
-                }
-                while (read_good == 0) {
-                    count++;
-
-                    accel_accum[1] += accel.x;
-                    accel_accum[0] += accel.y;
-                    accel_accum[2] -= accel.z;
-
-                    read_good = PIOS_BMA180_ReadFifo(&accel);
-                }
-                accel_samples = count;
-                accel_scaling = PIOS_BMA180_GetScale();
-
-                // Get temp from last reading
-                accelSensorData.temperature = 25.0f + ((float)accel.temperature - 2.0f) / 2.0f;
-            }
-#endif /* if defined(PIOS_INCLUDE_BMA180) */
-#if defined(PIOS_INCLUDE_L3GD20)
-            {
-                struct pios_l3gd20_data gyro;
-                gyro_samples = 0;
-                xQueueHandle gyro_queue = PIOS_L3GD20_GetQueue();
-
-                if (xQueueReceive(gyro_queue, (void *)&gyro, 4) == errQUEUE_EMPTY) {
                     error = true;
-                    continue;
                 }
-
+            } else {
-                gyro_samples   = 1;
+                if (PIOS_SENSORS_Poll(sensor)) {
-                gyro_accum[1] += gyro.gyro_x;
+                    PIOS_SENSOR_Fetch(sensor, (void *)source_data, MAX_SENSORS_PER_INSTANCE);
-                gyro_accum[0] += gyro.gyro_y;
+                    if (sensor->type & PIOS_SENSORS_TYPE_3D) {
-                gyro_accum[2] -= gyro.gyro_z;
+                        accumulateSamples(&sensor_context, source_data);
-
+                        processSamples3d(&sensor_context, sensor);
-                gyro_scaling   = PIOS_L3GD20_GetScale();
+                    } else {
-
+                        processSamples1d(&source_data->sensorSample1Axis, sensor);
-                // Get temp from last reading
+                    }
-                gyroSensorData.temperature = gyro.temperature;
+                    clearContext(&sensor_context);
-            }
-#endif /* if defined(PIOS_INCLUDE_L3GD20) */
-            break;
-        case 0x02: // MPU6000 board
-        case 0x03: // MPU6000 board
-#if defined(PIOS_INCLUDE_MPU6000)
-            {
-                struct pios_mpu6000_data mpu6000_data;
-                xQueueHandle queue = PIOS_MPU6000_GetQueue();
-
-                while (xQueueReceive(queue, (void *)&mpu6000_data, gyro_samples == 0 ? 10 : 0) != errQUEUE_EMPTY) {
-                    gyro_accum[0]  += mpu6000_data.gyro_x;
-                    gyro_accum[1]  += mpu6000_data.gyro_y;
-                    gyro_accum[2]  += mpu6000_data.gyro_z;
-
-                    accel_accum[0] += mpu6000_data.accel_x;
-                    accel_accum[1] += mpu6000_data.accel_y;
-                    accel_accum[2] += mpu6000_data.accel_z;
-
-                    gyro_samples++;
-                    accel_samples++;
                 }
-
-                PERF_MEASURE_PERIOD(counterSensorPeriod);
-                PERF_TRACK_VALUE(counterGyroSamples, gyro_samples);
-
-                if (gyro_samples == 0) {
-                    PIOS_MPU6000_ReadGyros(&mpu6000_data);
-                    error = true;
-                    continue;
-                }
-
-                gyro_scaling  = PIOS_MPU6000_GetScale();
-                accel_scaling = PIOS_MPU6000_GetAccelScale();
-
-                gyroSensorData.temperature  = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f;
-                accelSensorData.temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f;
-            }
-#endif /* PIOS_INCLUDE_MPU6000 */
-            break;
-        default:
-            PIOS_DEBUG_Assert(0);
-        }
-
-        if (isnan(accel_temperature)) {
-            accel_temperature = accelSensorData.temperature;
-            gyro_temperature  = gyroSensorData.temperature;
-        }
-
-        accel_temperature = temp_alpha * (accelSensorData.temperature - accel_temperature) + accel_temperature;
-        gyro_temperature  = temp_alpha * (gyroSensorData.temperature - gyro_temperature) + gyro_temperature;
-
-        if ((accel_temp_calibrated || gyro_temp_calibrated) && !temp_calibration_count) {
-            temp_calibration_count = TEMP_CALIB_INTERVAL;
-            if (accel_temp_calibrated) {
-                float ctemp = boundf(accel_temperature, agcal.temp_calibrated_extent.max, agcal.temp_calibrated_extent.min);
-                accel_temp_bias[0] = agcal.accel_temp_coeff.X * ctemp;
-                accel_temp_bias[1] = agcal.accel_temp_coeff.Y * ctemp;
-                accel_temp_bias[2] = agcal.accel_temp_coeff.Z * ctemp;
-            }
-
-            if (gyro_temp_calibrated) {
-                float ctemp = boundf(gyro_temperature, agcal.temp_calibrated_extent.max, agcal.temp_calibrated_extent.min);
-
-                gyro_temp_bias[0] = (agcal.gyro_temp_coeff.X + agcal.gyro_temp_coeff.X2 * ctemp) * ctemp;
-                gyro_temp_bias[1] = (agcal.gyro_temp_coeff.Y + agcal.gyro_temp_coeff.Y2 * ctemp) * ctemp;
-                gyro_temp_bias[2] = (agcal.gyro_temp_coeff.Z + agcal.gyro_temp_coeff.Z2 * ctemp) * ctemp;
             }
         }
-        temp_calibration_count--;
+        PERF_MEASURE_PERIOD(counterSensorPeriod);
-        // Scale the accels
+        RELOAD_WDG();
-        float accels[3] = { (float)accel_accum[0] / accel_samples,
+        vTaskDelayUntil(&lastSysTime, sensor_period_ticks);
-                            (float)accel_accum[1] / accel_samples,
-                            (float)accel_accum[2] / accel_samples };
-
-
-        float accels_out[3] = { accels[0] * accel_scaling * agcal.accel_scale.X - agcal.accel_bias.X - accel_temp_bias[0],
-                                accels[1] * accel_scaling * agcal.accel_scale.Y - agcal.accel_bias.Y - accel_temp_bias[1],
-                                accels[2] * accel_scaling * agcal.accel_scale.Z - agcal.accel_bias.Z - accel_temp_bias[2] };
-
-
-        if (rotate) {
-            rot_mult(R, accels_out, accels);
-            accelSensorData.x = accels[0];
-            accelSensorData.y = accels[1];
-            accelSensorData.z = accels[2];
-        } else {
-            accelSensorData.x = accels_out[0];
-            accelSensorData.y = accels_out[1];
-            accelSensorData.z = accels_out[2];
-        }
-        AccelSensorSet(&accelSensorData);
-
-        // Scale the gyros
-        float gyros[3] = { (float)gyro_accum[0] / gyro_samples,
-                           (float)gyro_accum[1] / gyro_samples,
-                           (float)gyro_accum[2] / gyro_samples };
-
-        float gyros_out[3] = { gyros[0] * gyro_scaling * agcal.gyro_scale.X - agcal.gyro_bias.X - gyro_temp_bias[0],
-                               gyros[1] * gyro_scaling * agcal.gyro_scale.Y - agcal.gyro_bias.Y - gyro_temp_bias[1],
-                               gyros[2] * gyro_scaling * agcal.gyro_scale.Z - agcal.gyro_bias.Z - gyro_temp_bias[2] };
-
-        if (rotate) {
-            rot_mult(R, gyros_out, gyros);
-            gyroSensorData.x = gyros[0];
-            gyroSensorData.y = gyros[1];
-            gyroSensorData.z = gyros[2];
-        } else {
-            gyroSensorData.x = gyros_out[0];
-            gyroSensorData.y = gyros_out[1];
-            gyroSensorData.z = gyros_out[2];
-        }
-
-        GyroSensorSet(&gyroSensorData);
-
-        // Because most crafts wont get enough information from gravity to zero yaw gyro, we try
-        // and make it average zero (weakly)
-
-#if defined(PIOS_INCLUDE_HMC5X83)
-        MagSensorData mag;
-        if (PIOS_HMC5x83_NewDataAvailable(onboard_mag) || PIOS_DELAY_DiffuS(mag_update_time) > 150000) {
-            int16_t values[3];
-            PIOS_HMC5x83_ReadMag(onboard_mag, values);
-            float mags[3] = { (float)values[1] - mag_bias[0],
-                              (float)values[0] - mag_bias[1],
-                              -(float)values[2] - mag_bias[2] };
-
-            float mag_out[3];
-            rot_mult(mag_transform, mags, mag_out);
-
-            mag.x = mag_out[0];
-            mag.y = mag_out[1];
-            mag.z = mag_out[2];
-
-            MagSensorSet(&mag);
-            mag_update_time = PIOS_DELAY_GetRaw();
-        }
-#endif /* if defined(PIOS_INCLUDE_HMC5X83) */
-
-#ifdef PIOS_INCLUDE_WDG
-        PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
-#endif
-        vTaskDelayUntil(&lastSysTime, sensor_period_ms / portTICK_RATE_MS);
     }
 }
 
+static void clearContext(sensor_fetch_context *sensor_context)
+{
+    // clear the context once it has finished
+    for (uint32_t i = 0; i < MAX_SENSORS_PER_INSTANCE; i++) {
+        sensor_context->accum[i].x = 0;
+        sensor_context->accum[i].y = 0;
+        sensor_context->accum[i].z = 0;
+    }
+    sensor_context->temperature = 0;
+    sensor_context->count = 0;
+}
+
+static void accumulateSamples(sensor_fetch_context *sensor_context, sensor_data *sample)
+{
+    for (uint32_t i = 0; (i < MAX_SENSORS_PER_INSTANCE) && (i < sample->sensorSample3Axis.count); i++) {
+        sensor_context->accum[i].x += sample->sensorSample3Axis.sample[i].x;
+        sensor_context->accum[i].y += sample->sensorSample3Axis.sample[i].y;
+        sensor_context->accum[i].z += sample->sensorSample3Axis.sample[i].z;
+    }
+    sensor_context->temperature += sample->sensorSample3Axis.temperature;
+    sensor_context->count++;
+}
+
+static void processSamples3d(sensor_fetch_context *sensor_context, const PIOS_SENSORS_Instance *sensor)
+{
+    float samples[3];
+    float temperature;
+    float scales[MAX_SENSORS_PER_INSTANCE];
+
+    PIOS_SENSORS_GetScales(sensor, scales, MAX_SENSORS_PER_INSTANCE);
+    float inv_count = 1.0f / (float)sensor_context->count;
+    if ((sensor->type & PIOS_SENSORS_TYPE_3AXIS_ACCEL) ||
+        (sensor->type == PIOS_SENSORS_TYPE_3AXIS_MAG)) {
+        float t = inv_count * scales[0];
+        samples[0]  = ((float)sensor_context->accum[0].x * t);
+        samples[1]  = ((float)sensor_context->accum[0].y * t);
+        samples[2]  = ((float)sensor_context->accum[0].z * t);
+        temperature = (float)sensor_context->temperature * inv_count * 0.01f;
+        if (sensor->type == PIOS_SENSORS_TYPE_3AXIS_MAG) {
+            handleMag(samples, temperature);
+            PERF_MEASURE_PERIOD(counterMagPeriod);
+            return;
+        } else {
+            PERF_TRACK_VALUE(counterAccelSamples, sensor_context->count);
+            PERF_MEASURE_PERIOD(counterAccelPeriod);
+            handleAccel(samples, temperature);
+        }
+    }
+
+    if (sensor->type & PIOS_SENSORS_TYPE_3AXIS_GYRO) {
+        uint8_t index = 0;
+        if (sensor->type == PIOS_SENSORS_TYPE_3AXIS_GYRO_ACCEL) {
+            index = 1;
+        }
+        float t = inv_count * scales[index];
+        samples[0]  = ((float)sensor_context->accum[index].x * t);
+        samples[1]  = ((float)sensor_context->accum[index].y * t);
+        samples[2]  = ((float)sensor_context->accum[index].z * t);
+        temperature = (float)sensor_context->temperature * inv_count * 0.01f;
+        handleGyro(samples, temperature);
+        return;
+    }
+}
+
+static void processSamples1d(PIOS_SENSORS_1Axis_SensorsWithTemp *sample, const PIOS_SENSORS_Instance *sensor)
+{
+    switch (sensor->type) {
+    case PIOS_SENSORS_TYPE_1AXIS_BARO:
+        PERF_MEASURE_PERIOD(counterBaroPeriod);
+        handleBaro(sample->sample, sample->temperature);
+        return;
+
+    default:
+        PIOS_Assert(0);
+    }
+}
+
+static void handleAccel(float *samples, float temperature)
+{
+    AccelSensorData accelSensorData;
+
+    updateAccelTempBias(temperature);
+    float accels_out[3] = { samples[0] * agcal.accel_scale.X - agcal.accel_bias.X - accel_temp_bias[0],
+                            samples[1] * agcal.accel_scale.Y - agcal.accel_bias.Y - accel_temp_bias[1],
+                            samples[2] * agcal.accel_scale.Z - agcal.accel_bias.Z - accel_temp_bias[2] };
+
+    rot_mult(R, accels_out, samples);
+    accelSensorData.x = samples[0];
+    accelSensorData.y = samples[1];
+    accelSensorData.z = samples[2];
+    accelSensorData.temperature = temperature;
+    AccelSensorSet(&accelSensorData);
+}
+
+static void handleGyro(float *samples, float temperature)
+{
+    GyroSensorData gyroSensorData;
+
+    updateGyroTempBias(temperature);
+    float gyros_out[3] = { samples[0] * agcal.gyro_scale.X - agcal.gyro_bias.X - gyro_temp_bias[0],
+                           samples[1] * agcal.gyro_scale.Y - agcal.gyro_bias.Y - gyro_temp_bias[1],
+                           samples[2] * agcal.gyro_scale.Z - agcal.gyro_bias.Z - gyro_temp_bias[2] };
+
+    rot_mult(R, gyros_out, samples);
+    gyroSensorData.temperature = temperature;
+    gyroSensorData.x = samples[0];
+    gyroSensorData.y = samples[1];
+    gyroSensorData.z = samples[2];
+
+    GyroSensorSet(&gyroSensorData);
+}
+
+static void handleMag(float *samples, float temperature)
+{
+    MagSensorData mag;
+    float mags[3] = { (float)samples[1] - mag_bias[0],
+                      (float)samples[0] - mag_bias[1],
+                      (float)samples[2] - mag_bias[2] };
+
+    rot_mult(mag_transform, mags, samples);
+
+    mag.x = samples[0];
+    mag.y = samples[1];
+    mag.z = samples[2];
+    mag.temperature = temperature;
+
+    MagSensorSet(&mag);
+}
+
+static void handleBaro(float sample, float temperature)
+{
+    updateBaroTempBias(temperature);
+    sample -= baro_temp_bias;
+
+    float altitude = 44330.0f * (1.0f - powf((sample) / PIOS_CONST_MKS_STD_ATMOSPHERE_F, (1.0f / 5.255f)));
+
+    if (!isnan(altitude)) {
+        BaroSensorData data;
+        data.Altitude    = altitude;
+        data.Temperature = temperature;
+        data.Pressure    = sample;
+        // Update the BasoSensor UAVObject
+        BaroSensorSet(&data);
+    }
+}
+
+static void updateAccelTempBias(float temperature)
+{
+    if (isnan(accel_temperature)) {
+        accel_temperature = temperature;
+    }
+    accel_temperature = temp_alpha_gyro_accel * (temperature - accel_temperature) + accel_temperature;
+
+    if ((accel_temp_calibrated) && !accel_temp_calibration_count) {
+        accel_temp_calibration_count = TEMP_CALIB_INTERVAL;
+        if (accel_temp_calibrated) {
+            float ctemp = boundf(accel_temperature, agcal.temp_calibrated_extent.max, agcal.temp_calibrated_extent.min);
+            accel_temp_bias[0] = agcal.accel_temp_coeff.X * ctemp;
+            accel_temp_bias[1] = agcal.accel_temp_coeff.Y * ctemp;
+            accel_temp_bias[2] = agcal.accel_temp_coeff.Z * ctemp;
+        }
+    }
+    accel_temp_calibration_count--;
+}
+
+static void updateGyroTempBias(float temperature)
+{
+    if (isnan(gyro_temperature)) {
+        gyro_temperature = temperature;
+    }
+
+    gyro_temperature = temp_alpha_gyro_accel * (temperature - gyro_temperature) + gyro_temperature;
+
+    if (gyro_temp_calibrated && !gyro_temp_calibration_count) {
+        gyro_temp_calibration_count = TEMP_CALIB_INTERVAL;
+
+        if (gyro_temp_calibrated) {
+            float ctemp = boundf(gyro_temperature, agcal.temp_calibrated_extent.max, agcal.temp_calibrated_extent.min);
+            gyro_temp_bias[0] = (agcal.gyro_temp_coeff.X + agcal.gyro_temp_coeff.X2 * ctemp) * ctemp;
+            gyro_temp_bias[1] = (agcal.gyro_temp_coeff.Y + agcal.gyro_temp_coeff.Y2 * ctemp) * ctemp;
+            gyro_temp_bias[2] = (agcal.gyro_temp_coeff.Z + agcal.gyro_temp_coeff.Z2 * ctemp) * ctemp;
+        }
+    }
+    gyro_temp_calibration_count--;
+}
+
+static void updateBaroTempBias(float temperature)
+{
+    if (isnan(baro_temperature)) {
+        baro_temperature = temperature;
+    }
+
+    baro_temperature = temp_alpha_baro * (temperature - baro_temperature) + baro_temperature;
+
+    if (baro_temp_correction_enabled && !baro_temp_calibration_count) {
+        baro_temp_calibration_count = BARO_TEMP_CALIB_INTERVAL;
+        // 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 = boundf(baro_temperature, baroCorrectionExtent.max, baroCorrectionExtent.min);
+        baro_temp_bias = baroCorrection.a + ((baroCorrection.d * ctemp + baroCorrection.c) * ctemp + baroCorrection.b) * ctemp;
+    }
+    baro_temp_calibration_count--;
+}
 /**
  * Locally cache some variables from the AtttitudeSettings object
  */
@@ -533,6 +585,11 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
         Quaternion2R(rotationQuat, R);
     }
     matrix_mult_3x3f((float(*)[3])RevoCalibrationmag_transformToArray(cal.mag_transform), R, mag_transform);
+
+    RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection);
+    RevoSettingsBaroTempCorrectionExtentGet(&baroCorrectionExtent);
+    baro_temp_correction_enabled = !(baroCorrectionExtent.max - baroCorrectionExtent.min < 0.1f ||
+                                     (baroCorrection.a < 1e-9f && baroCorrection.b < 1e-9f && baroCorrection.c < 1e-9f && baroCorrection.d < 1e-9f));
 }
 /**
  * @}

flight/modules/System/systemmod.c

@@ -267,7 +267,6 @@ static void systemTask(__attribute__((unused)) void *parameters)
             oplinkStatus.RxMissed = radio_stats.rx_missed;
             oplinkStatus.RxFailure     = radio_stats.rx_failure;
             oplinkStatus.TxDropped     = radio_stats.tx_dropped;
-            oplinkStatus.TxResent = radio_stats.tx_resent;
             oplinkStatus.TxFailure     = radio_stats.tx_failure;
             oplinkStatus.Resets      = radio_stats.resets;
             oplinkStatus.Timeouts    = radio_stats.timeouts;

flight/pios/common/pios_adxl345.c

@@ -29,7 +29,7 @@
  */
 
 #include "pios.h"
-
+#include <pios_adxl345.h>
 #ifdef PIOS_INCLUDE_ADXL345
 
 enum pios_adxl345_dev_magic {

flight/pios/common/pios_bma180.c

@@ -30,7 +30,7 @@
  */
 
 #include "pios.h"
-
+#include <pios_bma180.h>
 #ifdef PIOS_INCLUDE_BMA180
 
 #include "fifo_buffer.h"

flight/pios/common/pios_bmp085.c

@@ -29,7 +29,7 @@
  */
 
 #include "pios.h"
-
+#include <pios_bmp085.h>
 #ifdef PIOS_INCLUDE_BMP085
 
 #ifndef PIOS_INCLUDE_EXTI

flight/pios/common/pios_hmc5x83.c

@@ -50,6 +50,22 @@ typedef struct {
 
 static int32_t PIOS_HMC5x83_Config(pios_hmc5x83_dev_data_t *dev);
 
+// sensor driver interface
+bool PIOS_HMC5x83_driver_Test(uintptr_t context);
+void PIOS_HMC5x83_driver_Reset(uintptr_t context);
+void PIOS_HMC5x83_driver_get_scale(float *scales, uint8_t size, uintptr_t context);
+void PIOS_HMC5x83_driver_fetch(void *, uint8_t size, uintptr_t context);
+bool PIOS_HMC5x83_driver_poll(uintptr_t context);
+
+const PIOS_SENSORS_Driver PIOS_HMC5x83_Driver = {
+    .test      = PIOS_HMC5x83_driver_Test,
+    .poll      = PIOS_HMC5x83_driver_poll,
+    .fetch     = PIOS_HMC5x83_driver_fetch,
+    .reset     = PIOS_HMC5x83_driver_Reset,
+    .get_queue = NULL,
+    .get_scale = PIOS_HMC5x83_driver_get_scale,
+    .is_polled = true,
+};
 /**
  * Allocate the device setting structure
  * @return pios_hmc5x83_dev_data_t pointer to newly created structure
@@ -99,6 +115,11 @@ pios_hmc5x83_dev_t PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_
     return (pios_hmc5x83_dev_t)dev;
 }
 
+void PIOS_HMC5x83_Register(pios_hmc5x83_dev_t handler)
+{
+    PIOS_SENSORS_Register(&PIOS_HMC5x83_Driver, PIOS_SENSORS_TYPE_3AXIS_MAG, handler);
+}
+
 /**
  * @brief Initialize the HMC5x83 magnetometer sensor
  * \return none
@@ -549,6 +570,37 @@ int32_t PIOS_HMC5x83_I2C_Write(pios_hmc5x83_dev_t handler, uint8_t address, uint
 }
 #endif /* PIOS_INCLUDE_I2C */
 
+/* PIOS sensor driver implementation */
+bool PIOS_HMC5x83_driver_Test(uintptr_t context)
+{
+    return !PIOS_HMC5x83_Test((pios_hmc5x83_dev_t)context);
+}
+
+void PIOS_HMC5x83_driver_Reset(__attribute__((unused)) uintptr_t context) {}
+
+void PIOS_HMC5x83_driver_get_scale(float *scales, uint8_t size, __attribute__((unused))  uintptr_t context)
+{
+    PIOS_Assert(size > 0);
+    scales[0] = 1;
+}
+
+void PIOS_HMC5x83_driver_fetch(void *data, uint8_t size, uintptr_t context)
+{
+    PIOS_Assert(size > 0);
+    int16_t mag[3];
+    PIOS_HMC5x83_ReadMag((pios_hmc5x83_dev_t)context, mag);
+    PIOS_SENSORS_3Axis_SensorsWithTemp *tmp = data;
+    tmp->count = 1;
+    tmp->sample[0].x = mag[0];
+    tmp->sample[0].y = mag[1];
+    tmp->sample[0].z = mag[2];
+    tmp->temperature = 0;
+}
+
+bool PIOS_HMC5x83_driver_poll(uintptr_t context)
+{
+    return PIOS_HMC5x83_NewDataAvailable((pios_hmc5x83_dev_t)context);
+}
 
 #endif /* PIOS_INCLUDE_HMC5x83 */
 

flight/pios/common/pios_instrumentation.c

@@ -52,8 +52,8 @@ pios_counter_t PIOS_Instrumentation_CreateCounter(uint32_t id)
     if (!counter_handle) {
         pios_perf_counter_t *newcounter = &pios_instrumentation_perf_counters[++pios_instrumentation_last_used_counter];
         newcounter->id  = id;
-        newcounter->max = INT32_MIN;
+        newcounter->max = INT32_MIN + 1;
-        newcounter->min = INT32_MAX;
+        newcounter->min = INT32_MAX - 1;
         counter_handle  = (pios_counter_t)newcounter;
     }
     return counter_handle;

flight/pios/common/pios_l3gd20.c

@@ -30,7 +30,7 @@
  */
 
 #include "pios.h"
-
+#include <pios_l3gd20.h>
 #ifdef PIOS_INCLUDE_L3GD20
 
 #include "fifo_buffer.h"

flight/pios/common/pios_mpu6000.c

@@ -1,6 +1,6 @@
 /**
  ******************************************************************************
- * @addtogroup PIOS PIOS Core hardware abstraction layer
+ * @addtogroup PIOS PIOS Core haoftware; you can rnedtt
  * @{
  * @addtogroup PIOS_MPU6000 MPU6000 Functions
  * @brief Deals with the hardware interface to the 3-axis gyro
@@ -13,8 +13,8 @@
  *
  ******************************************************************************
  */
-/*
+/*istribu
- * This program is free software; you can redistribute it and/or modify
+ * This program is free software; you can rnedtt ad/oe ir 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.
@@ -30,20 +30,40 @@
  */
 
 #include "pios.h"
-
+#include <pios_mpu6000.h>
 #ifdef PIOS_INCLUDE_MPU6000
-
+#include <stdint.h>
 #include <pios_constants.h>
+#include <pios_sensors.h>
+
 /* Global Variables */
 
 enum pios_mpu6000_dev_magic {
     PIOS_MPU6000_DEV_MAGIC = 0x9da9b3ed,
 };
 
+// sensor driver interface
+bool PIOS_MPU6000_driver_Test(uintptr_t context);
+void PIOS_MPU6000_driver_Reset(uintptr_t context);
+void PIOS_MPU6000_driver_get_scale(float *scales, uint8_t size, uintptr_t context);
+QueueHandle_t PIOS_MPU6000_driver_get_queue(uintptr_t context);
+
+const PIOS_SENSORS_Driver PIOS_MPU6000_Driver = {
+    .test      = PIOS_MPU6000_driver_Test,
+    .poll      = NULL,
+    .fetch     = NULL,
+    .reset     = PIOS_MPU6000_driver_Reset,
+    .get_queue = PIOS_MPU6000_driver_get_queue,
+    .get_scale = PIOS_MPU6000_driver_get_scale,
+    .is_polled = false,
+};
+//
+
+
 struct mpu6000_dev {
-    uint32_t     spi_id;
+    uint32_t spi_id;
-    uint32_t     slave_num;
+    uint32_t slave_num;
-    xQueueHandle queue;
+    QueueHandle_t queue;
     const struct pios_mpu6000_cfg *cfg;
     enum pios_mpu6000_range gyro_range;
     enum pios_mpu6000_accel_range accel_range;
@@ -51,26 +71,19 @@ struct mpu6000_dev {
     enum pios_mpu6000_dev_magic   magic;
 };
 
-#ifdef PIOS_MPU6000_ACCEL
 #define PIOS_MPU6000_SAMPLES_BYTES    14
 #define PIOS_MPU6000_SENSOR_FIRST_REG PIOS_MPU6000_ACCEL_X_OUT_MSB
-#else
-#define PIOS_MPU6000_SENSOR_FIRST_REG PIOS_MPU6000_TEMP_OUT_MSB
-#define PIOS_MPU6000_SAMPLES_BYTES    8
-#endif
 
 typedef union {
     uint8_t buffer[1 + PIOS_MPU6000_SAMPLES_BYTES];
     struct {
         uint8_t dummy;
-#ifdef PIOS_MPU6000_ACCEL
         uint8_t Accel_X_h;
         uint8_t Accel_X_l;
         uint8_t Accel_Y_h;
         uint8_t Accel_Y_l;
         uint8_t Accel_Z_h;
         uint8_t Accel_Z_l;
-#endif
         uint8_t Temperature_h;
         uint8_t Temperature_l;
         uint8_t Gyro_X_h;
@@ -88,7 +101,9 @@ typedef union {
 static struct mpu6000_dev *dev;
 volatile bool mpu6000_configured = false;
 static mpu6000_data_t mpu6000_data;
-
+static PIOS_SENSORS_3Axis_SensorsWithTemp *queue_data = 0;
+#define SENSOR_COUNT     2
+#define SENSOR_DATA_SIZE (sizeof(PIOS_SENSORS_3Axis_SensorsWithTemp) + sizeof(Vector3i16) * SENSOR_COUNT)
 // ! Private functions
 static struct mpu6000_dev *PIOS_MPU6000_alloc(const struct pios_mpu6000_cfg *cfg);
 static int32_t PIOS_MPU6000_Validate(struct mpu6000_dev *dev);
@@ -97,8 +112,14 @@ static int32_t PIOS_MPU6000_SetReg(uint8_t address, uint8_t buffer);
 static int32_t PIOS_MPU6000_GetReg(uint8_t address);
 static void PIOS_MPU6000_SetSpeed(const bool fast);
 static bool PIOS_MPU6000_HandleData();
-static bool PIOS_MPU6000_ReadFifo(bool *woken);
 static bool PIOS_MPU6000_ReadSensor(bool *woken);
+
+static int32_t PIOS_MPU6000_Test(void);
+
+void PIOS_MPU6000_Register()
+{
+    PIOS_SENSORS_Register(&PIOS_MPU6000_Driver, PIOS_SENSORS_TYPE_3AXIS_GYRO_ACCEL, 0);
+}
 /**
  * @brief Allocate a new device
  */
@@ -107,18 +128,16 @@ static struct mpu6000_dev *PIOS_MPU6000_alloc(const struct pios_mpu6000_cfg *cfg
     struct mpu6000_dev *mpu6000_dev;
 
     mpu6000_dev = (struct mpu6000_dev *)pios_malloc(sizeof(*mpu6000_dev));
-    if (!mpu6000_dev) {
+    PIOS_Assert(mpu6000_dev);
-        return NULL;
-    }
 
     mpu6000_dev->magic = PIOS_MPU6000_DEV_MAGIC;
 
-    mpu6000_dev->queue = xQueueCreate(cfg->max_downsample + 1, sizeof(struct pios_mpu6000_data));
+    mpu6000_dev->queue = xQueueCreate(cfg->max_downsample + 1, SENSOR_DATA_SIZE);
-    if (mpu6000_dev->queue == NULL) {
+    PIOS_Assert(mpu6000_dev->queue);
-        vPortFree(mpu6000_dev);
-        return NULL;
-    }
 
+    queue_data = (PIOS_SENSORS_3Axis_SensorsWithTemp *)pios_malloc(SENSOR_DATA_SIZE);
+    PIOS_Assert(queue_data);
+    queue_data->count = SENSOR_COUNT;
     return mpu6000_dev;
 }
 
@@ -211,15 +230,9 @@ static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const *cfg)
     }
 
     // FIFO storage
-#if defined(PIOS_MPU6000_ACCEL)
-    while (PIOS_MPU6000_SetReg(PIOS_MPU6000_FIFO_EN_REG, cfg->Fifo_store | PIOS_MPU6000_ACCEL_OUT) != 0) {
-        ;
-    }
-#else
     while (PIOS_MPU6000_SetReg(PIOS_MPU6000_FIFO_EN_REG, cfg->Fifo_store) != 0) {
         ;
     }
-#endif
     PIOS_MPU6000_ConfigureRanges(cfg->gyro_range, cfg->accel_range, cfg->filter);
     // Interrupt configuration
     while (PIOS_MPU6000_SetReg(PIOS_MPU6000_USER_CTRL_REG, cfg->User_ctl) != 0) {
@@ -279,14 +292,12 @@ int32_t PIOS_MPU6000_ConfigureRanges(
     }
 
     dev->gyro_range = gyroRange;
-#if defined(PIOS_MPU6000_ACCEL)
     // Set the accel range
     while (PIOS_MPU6000_SetReg(PIOS_MPU6000_ACCEL_CFG_REG, accelRange) != 0) {
         ;
     }
 
     dev->accel_range = accelRange;
-#endif
     return 0;
 }
 
@@ -414,11 +425,10 @@ static int32_t PIOS_MPU6000_SetReg(uint8_t reg, uint8_t data)
 }
 
 /**
- * @brief Read current X, Z, Y values (in that order)
+ * @brief Perform a dummy read in order to restart interrupt generation
- * \param[out] int16_t array of size 3 to store X, Z, and Y magnetometer readings
  * \returns 0 if succesful
  */
-int32_t PIOS_MPU6000_ReadGyros(struct pios_mpu6000_data *data)
+int32_t PIOS_MPU6000_DummyReadGyros()
 {
     // THIS FUNCTION IS DEPRECATED AND DOES NOT PERFORM A ROTATION
     uint8_t buf[7] = { PIOS_MPU6000_GYRO_X_OUT_MSB | 0x80, 0, 0, 0, 0, 0, 0 };
@@ -434,10 +444,6 @@ int32_t PIOS_MPU6000_ReadGyros(struct pios_mpu6000_data *data)
 
     PIOS_MPU6000_ReleaseBus();
 
-    data->gyro_x = rec[1] << 8 | rec[2];
-    data->gyro_y = rec[3] << 8 | rec[4];
-    data->gyro_z = rec[5] << 8 | rec[6];
-
     return 0;
 }
 
@@ -469,7 +475,7 @@ xQueueHandle PIOS_MPU6000_GetQueue()
 }
 
 
-float PIOS_MPU6000_GetScale()
+static float PIOS_MPU6000_GetScale()
 {
     switch (dev->gyro_range) {
     case PIOS_MPU6000_SCALE_250_DEG:
@@ -487,7 +493,7 @@ float PIOS_MPU6000_GetScale()
     return 0;
 }
 
-float PIOS_MPU6000_GetAccelScale()
+static float PIOS_MPU6000_GetAccelScale()
 {
     switch (dev->accel_range) {
     case PIOS_MPU6000_ACCEL_2G:
@@ -510,7 +516,7 @@ float PIOS_MPU6000_GetAccelScale()
  * \return 0 if test succeeded
  * \return non-zero value if test succeeded
  */
-int32_t PIOS_MPU6000_Test(void)
+static int32_t PIOS_MPU6000_Test(void)
 {
     /* Verify that ID matches (MPU6000 ID is 0x69) */
     int32_t mpu6000_id = PIOS_MPU6000_ReadID();
@@ -526,172 +532,76 @@ int32_t PIOS_MPU6000_Test(void)
     return 0;
 }
 
-/**
- * @brief Reads the contents of the MPU6000 Interrupt Status register from an ISR
- * @return The register value or -1 on failure to claim the bus
- */
-static int32_t PIOS_MPU6000_GetInterruptStatusRegISR(bool *woken)
-{
-    /* Interrupt Status register can be read at high SPI clock speed */
-    uint8_t data;
-
-    if (PIOS_MPU6000_ClaimBusISR(woken, false) != 0) {
-        return -1;
-    }
-    PIOS_SPI_TransferByte(dev->spi_id, (0x80 | PIOS_MPU6000_INT_STATUS_REG));
-    data = PIOS_SPI_TransferByte(dev->spi_id, 0);
-    PIOS_MPU6000_ReleaseBusISR(woken);
-    return data;
-}
-
-/**
- * @brief Resets the MPU6000 FIFO from an ISR
- * @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
- *                      task has is now eligible to run, else unchanged
- * @return  0 if operation was successful
- * @return -1 if unable to claim SPI bus
- * @return -2 if write to the device failed
- */
-static int32_t PIOS_MPU6000_ResetFifoISR(bool *woken)
-{
-    int32_t result = 0;
-
-    if (PIOS_MPU6000_ClaimBusISR(woken, false) != 0) {
-        return -1;
-    }
-    /* Reset FIFO. */
-    if (PIOS_SPI_TransferByte(dev->spi_id, 0x7f & PIOS_MPU6000_USER_CTRL_REG) != 0) {
-        result = -2;
-    } else if (PIOS_SPI_TransferByte(dev->spi_id, (dev->cfg->User_ctl | PIOS_MPU6000_USERCTL_FIFO_RST)) != 0) {
-        result = -2;
-    }
-    PIOS_MPU6000_ReleaseBusISR(woken);
-    return result;
-}
-
-/**
- * @brief Obtains the number of bytes in the FIFO. Call from ISR only.
- * @return the number of bytes in the FIFO
- * @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
- *                      task has is now eligible to run, else unchanged
- */
-static int32_t PIOS_MPU6000_FifoDepthISR(bool *woken)
-{
-    uint8_t mpu6000_send_buf[3] = { PIOS_MPU6000_FIFO_CNT_MSB | 0x80, 0, 0 };
-    uint8_t mpu6000_rec_buf[3];
-
-    if (PIOS_MPU6000_ClaimBusISR(woken, false) != 0) {
-        return -1;
-    }
-
-    if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) {
-        PIOS_MPU6000_ReleaseBusISR(woken);
-        return -1;
-    }
-
-    PIOS_MPU6000_ReleaseBusISR(woken);
-
-    return (mpu6000_rec_buf[1] << 8) | mpu6000_rec_buf[2];
-}
-
 /**
  * @brief EXTI IRQ Handler.  Read all the data from onboard buffer
- * @return a boolean to the EXTI IRQ Handler wrapper indicating if a
+ * @return a boleoan to the EXTI IRQ Handler wrapper indicating if a
  *         higher priority task is now eligible to run
  */
-uint32_t mpu6000_irq = 0;
-int32_t mpu6000_count;
-uint32_t mpu6000_fifo_backup    = 0;
-
-uint8_t mpu6000_last_read_count = 0;
-uint32_t mpu6000_fails = 0;
-
-uint32_t mpu6000_interval_us;
-uint32_t mpu6000_time_us;
-uint32_t mpu6000_transfer_size;
 
 bool PIOS_MPU6000_IRQHandler(void)
 {
     bool woken = false;
-    static uint32_t timeval;
-
-    mpu6000_interval_us = PIOS_DELAY_DiffuS(timeval);
-    timeval = PIOS_DELAY_GetRaw();
 
     if (!mpu6000_configured) {
         return false;
     }
 
     bool read_ok = false;
-    if (dev->cfg->User_ctl & PIOS_MPU6000_USERCTL_FIFO_EN) {
+    read_ok = PIOS_MPU6000_ReadSensor(&woken);
-        read_ok = PIOS_MPU6000_ReadFifo(&woken);
+
-    } else {
-        read_ok = PIOS_MPU6000_ReadSensor(&woken);
-    }
     if (read_ok) {
         bool woken2 = PIOS_MPU6000_HandleData();
         woken |= woken2;
     }
 
-    mpu6000_irq++;
-
-    mpu6000_time_us = PIOS_DELAY_DiffuS(timeval);
-
     return woken;
 }
 
 static bool PIOS_MPU6000_HandleData()
 {
+    if (!queue_data) {
+        return false;
+    }
+
     // Rotate the sensor to OP convention.  The datasheet defines X as towards the right
     // and Y as forward.  OP convention transposes this.  Also the Z is defined negatively
     // to our convention
 
-    static struct pios_mpu6000_data data;
-
     // Currently we only support rotations on top so switch X/Y accordingly
     switch (dev->cfg->orientation) {
     case PIOS_MPU6000_TOP_0DEG:
-#ifdef PIOS_MPU6000_ACCEL
+        queue_data->sample[0].y = GET_SENSOR_DATA(mpu6000_data, Accel_X); // chip X
-        data.accel_y = GET_SENSOR_DATA(mpu6000_data, Accel_X); // chip X
+        queue_data->sample[0].x = GET_SENSOR_DATA(mpu6000_data, Accel_Y); // chip Y
-        data.accel_x = GET_SENSOR_DATA(mpu6000_data, Accel_Y); // chip Y
+        queue_data->sample[1].y = GET_SENSOR_DATA(mpu6000_data, Gyro_X); // chip X
-#endif
+        queue_data->sample[1].x = GET_SENSOR_DATA(mpu6000_data, Gyro_Y); // chip Y
-        data.gyro_y  = GET_SENSOR_DATA(mpu6000_data, Gyro_X); // chip X
-        data.gyro_x  = GET_SENSOR_DATA(mpu6000_data, Gyro_Y); // chip Y
         break;
     case PIOS_MPU6000_TOP_90DEG:
         // -1 to bring it back to -32768 +32767 range
-#ifdef PIOS_MPU6000_ACCEL
+        queue_data->sample[0].y = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_Y)); // chip Y
-        data.accel_y = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_Y)); // chip Y
+        queue_data->sample[0].x = GET_SENSOR_DATA(mpu6000_data, Accel_X); // chip X
-        data.accel_x = GET_SENSOR_DATA(mpu6000_data, Accel_X); // chip X
+        queue_data->sample[1].y = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_Y)); // chip Y
-#endif
+        queue_data->sample[1].x = GET_SENSOR_DATA(mpu6000_data, Gyro_X); // chip X
-        data.gyro_y  = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_Y)); // chip Y
-        data.gyro_x  = GET_SENSOR_DATA(mpu6000_data, Gyro_X); // chip X
         break;
     case PIOS_MPU6000_TOP_180DEG:
-#ifdef PIOS_MPU6000_ACCEL
+        queue_data->sample[0].y = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_X)); // chip X
-        data.accel_y = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_X)); // chip X
+        queue_data->sample[0].x = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_Y)); // chip Y
-        data.accel_x = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_Y)); // chip Y
+        queue_data->sample[1].y = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_X)); // chip X
-#endif
+        queue_data->sample[1].x = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_Y)); // chip Y
-        data.gyro_y  = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_X)); // chip X
-        data.gyro_x  = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_Y)); // chip Y
         break;
     case PIOS_MPU6000_TOP_270DEG:
-#ifdef PIOS_MPU6000_ACCEL
+        queue_data->sample[0].y = GET_SENSOR_DATA(mpu6000_data, Accel_Y); // chip Y
-        data.accel_y = GET_SENSOR_DATA(mpu6000_data, Accel_Y); // chip Y
+        queue_data->sample[0].x = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_X)); // chip X
-        data.accel_x = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_X)); // chip X
+        queue_data->sample[1].y = GET_SENSOR_DATA(mpu6000_data, Gyro_Y); // chip Y
-#endif
+        queue_data->sample[1].x = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_X)); // chip X
-        data.gyro_y  = GET_SENSOR_DATA(mpu6000_data, Gyro_Y); // chip Y
-        data.gyro_x  = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_X)); // chip X
         break;
     }
-#ifdef PIOS_MPU6000_ACCEL
+    queue_data->sample[0].z = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_Z));
-    data.accel_z     = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_Z));
+    queue_data->sample[1].z = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_Z));
-#endif
+    const int16_t temp = GET_SENSOR_DATA(mpu6000_data, Temperature);
-    data.gyro_z      = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_Z));
+    queue_data->temperature = 3500 + ((float)(temp + 512)) * (1.0f / 3.4f);
-    data.temperature = GET_SENSOR_DATA(mpu6000_data, Temperature);
 
     BaseType_t higherPriorityTaskWoken;
-    xQueueSendToBackFromISR(dev->queue, (void *)&data, &higherPriorityTaskWoken);
+    xQueueSendToBackFromISR(dev->queue, (void *)queue_data, &higherPriorityTaskWoken);
     return higherPriorityTaskWoken == pdTRUE;
 }
 
@@ -704,72 +614,33 @@ static bool PIOS_MPU6000_ReadSensor(bool *woken)
     }
     if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_data.buffer[0], sizeof(mpu6000_data_t), NULL) < 0) {
         PIOS_MPU6000_ReleaseBusISR(woken);
-        mpu6000_fails++;
         return false;
     }
     PIOS_MPU6000_ReleaseBusISR(woken);
     return true;
 }
 
-static bool PIOS_MPU6000_ReadFifo(bool *woken)
+// Sensor driver implementation
+bool PIOS_MPU6000_driver_Test(__attribute__((unused)) uintptr_t context)
 {
-    /* Temporary fix for OP-1049. Expected to be superceded for next major release
+    return !PIOS_MPU6000_Test();
-     * by code changes for OP-1039.
+}
-     * Read interrupt status register to check for FIFO overflow. Must be the
-     * first read after interrupt, in case the device is configured so that
-     * any read clears in the status register (PIOS_MPU6000_INT_CLR_ANYRD set in
-     * interrupt config register) */
-    int32_t result;
 
-    if ((result = PIOS_MPU6000_GetInterruptStatusRegISR(woken)) < 0) {
+void PIOS_MPU6000_driver_Reset(__attribute__((unused)) uintptr_t context)
-        return false;
+{
-    }
+    PIOS_MPU6000_DummyReadGyros();
-    if (result & PIOS_MPU6000_INT_STATUS_FIFO_OVERFLOW) {
+}
-        /* The FIFO has overflowed, so reset it,
-         * to enable sample sync to be recovered.
-         * If the reset fails, we are in trouble, but
-         * we keep trying on subsequent interrupts. */
-        PIOS_MPU6000_ResetFifoISR(woken);
-        /* Return and wait for the next new sample. */
-        return false;
-    }
 
-    /* Usual case - FIFO has not overflowed. */
+void PIOS_MPU6000_driver_get_scale(float *scales, uint8_t size, __attribute__((unused)) uintptr_t contet)
-    mpu6000_count = PIOS_MPU6000_FifoDepthISR(woken);
+{
-    if (mpu6000_count < PIOS_MPU6000_SAMPLES_BYTES) {
+    PIOS_Assert(size >= 2);
-        return false;
+    scales[0] = PIOS_MPU6000_GetAccelScale();
-    }
+    scales[1] = PIOS_MPU6000_GetScale();
+}
 
-    if (PIOS_MPU6000_ClaimBusISR(woken, true) != 0) {
+QueueHandle_t PIOS_MPU6000_driver_get_queue(__attribute__((unused)) uintptr_t context)
-        return false;
+{
-    }
+    return dev->queue;
-
-    const uint8_t mpu6000_send_buf[1 + PIOS_MPU6000_SAMPLES_BYTES] = { PIOS_MPU6000_FIFO_REG | 0x80 };
-
-    if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_data.buffer[0], sizeof(mpu6000_data_t), NULL) < 0) {
-        PIOS_MPU6000_ReleaseBusISR(woken);
-        mpu6000_fails++;
-        return false;
-    }
-
-    PIOS_MPU6000_ReleaseBusISR(woken);
-
-    // In the case where extras samples backed up grabbed an extra
-    if (mpu6000_count >= PIOS_MPU6000_SAMPLES_BYTES * 2) {
-        mpu6000_fifo_backup++;
-        if (PIOS_MPU6000_ClaimBusISR(woken, true) != 0) {
-            return false;
-        }
-
-        if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_data.buffer[0], sizeof(mpu6000_data_t), NULL) < 0) {
-            PIOS_MPU6000_ReleaseBusISR(woken);
-            mpu6000_fails++;
-            return false;
-        }
-
-        PIOS_MPU6000_ReleaseBusISR(woken);
-    }
-    return true;
 }
 #endif /* PIOS_INCLUDE_MPU6000 */
 

flight/pios/common/pios_ms5611.c

@@ -29,17 +29,40 @@
  */
 
 #include "pios.h"
-
 #ifdef PIOS_INCLUDE_MS5611
-
+#include <pios_ms5611.h>
 #define POW2(x) (1 << x)
 
 // TODO: Clean this up.  Getting around old constant.
-#define PIOS_MS5611_OVERSAMPLING oversampling
+#define PIOS_MS5611_OVERSAMPLING   oversampling
 
+// Option to change the interleave between Temp and Pressure conversions
+// Undef for normal operation
+#define PIOS_MS5611_SLOW_TEMP_RATE 20
+#ifndef PIOS_MS5611_SLOW_TEMP_RATE
+#define PIOS_MS5611_SLOW_TEMP_RATE 1
+#endif
+
+/* Local Types */
+typedef struct {
+    uint16_t C[6];
+} MS5611CalibDataTypeDef;
+
+typedef enum {
+    MS5611_CONVERSION_TYPE_None = 0,
+    MS5611_CONVERSION_TYPE_PressureConv,
+    MS5611_CONVERSION_TYPE_TemperatureConv
+} ConversionTypeTypeDef;
+
+typedef enum {
+    MS5611_FSM_INIT = 0,
+    MS5611_FSM_TEMPERATURE,
+    MS5611_FSM_PRESSURE,
+    MS5611_FSM_CALCULATE,
+} MS5611_FSM_State;
 
 /* Glocal Variables */
-ConversionTypeTypeDef CurrentRead;
+ConversionTypeTypeDef CurrentRead = MS5611_CONVERSION_TYPE_None;
 
 /* Local Variables */
 MS5611CalibDataTypeDef CalibData;
@@ -50,8 +73,14 @@ static uint32_t RawPressure;
 static int64_t Pressure;
 static int64_t Temperature;
 static int32_t lastConversionStart;
+
+static uint32_t conversionDelayMs;
+static uint32_t conversionDelayUs;
+
 static int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len);
 static int32_t PIOS_MS5611_WriteCommand(uint8_t command);
+static uint32_t PIOS_MS5611_GetDelay();
+static uint32_t PIOS_MS5611_GetDelayUs();
 
 // Second order temperature compensation. Temperature offset
 static int64_t compensation_t2;
@@ -60,17 +89,36 @@ static int64_t compensation_t2;
 static uint32_t oversampling;
 static const struct pios_ms5611_cfg *dev_cfg;
 static int32_t i2c_id;
+static PIOS_SENSORS_1Axis_SensorsWithTemp results;
 
+// sensor driver interface
+bool PIOS_MS5611_driver_Test(uintptr_t context);
+void PIOS_MS5611_driver_Reset(uintptr_t context);
+void PIOS_MS5611_driver_get_scale(float *scales, uint8_t size, uintptr_t context);
+void PIOS_MS5611_driver_fetch(void *, uint8_t size, uintptr_t context);
+bool PIOS_MS5611_driver_poll(uintptr_t context);
 
+const PIOS_SENSORS_Driver PIOS_MS5611_Driver = {
+    .test      = PIOS_MS5611_driver_Test,
+    .poll      = PIOS_MS5611_driver_poll,
+    .fetch     = PIOS_MS5611_driver_fetch,
+    .reset     = PIOS_MS5611_driver_Reset,
+    .get_queue = NULL,
+    .get_scale = PIOS_MS5611_driver_get_scale,
+    .is_polled = true,
+};
 /**
  * Initialise the MS5611 sensor
  */
 int32_t ms5611_read_flag;
 void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device)
 {
-    i2c_id  = i2c_device;
+    i2c_id = i2c_device;
 
     oversampling = cfg->oversampling;
+    conversionDelayMs = PIOS_MS5611_GetDelay();
+    conversionDelayUs = PIOS_MS5611_GetDelayUs();
+
     dev_cfg = cfg; // Store cfg before enabling interrupt
 
     PIOS_MS5611_WriteCommand(MS5611_RESET);
@@ -80,10 +128,10 @@ void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device)
 
     // 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);
+        while (PIOS_MS5611_Read(MS5611_CALIB_ADDR + i * 2, data, 2)) {}
+        ;
         CalibData.C[i] = (data[0] << 8) | data[1];
     }
 }
@@ -96,11 +144,11 @@ void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device)
 int32_t PIOS_MS5611_StartADC(ConversionTypeTypeDef Type)
 {
     /* Start the conversion */
-    if (Type == TemperatureConv) {
+    if (Type == MS5611_CONVERSION_TYPE_TemperatureConv) {
         while (PIOS_MS5611_WriteCommand(MS5611_TEMP_ADDR + oversampling) != 0) {
             continue;
         }
-    } else if (Type == PressureConv) {
+    } else if (Type == MS5611_CONVERSION_TYPE_PressureConv) {
         while (PIOS_MS5611_WriteCommand(MS5611_PRES_ADDR + oversampling) != 0) {
             continue;
         }
@@ -114,7 +162,7 @@ int32_t PIOS_MS5611_StartADC(ConversionTypeTypeDef Type)
 /**
  * @brief Return the delay for the current osr
  */
-int32_t PIOS_MS5611_GetDelay()
+static uint32_t PIOS_MS5611_GetDelay()
 {
     switch (oversampling) {
     case MS5611_OSR_256:
@@ -141,7 +189,7 @@ int32_t PIOS_MS5611_GetDelay()
 /**
  * @brief Return the delay for the current osr in uS
  */
-uint32_t PIOS_MS5611_GetDelayUs()
+static uint32_t PIOS_MS5611_GetDelayUs()
 {
     switch (oversampling) {
     case MS5611_OSR_256:
@@ -167,7 +215,7 @@ uint32_t PIOS_MS5611_GetDelayUs()
 
 /**
  * Read the ADC conversion value (once ADC conversion has completed)
- * \return 0 if successfully read the ADC, -1 if failed
+ * \return 0 if successfully read the ADC, -1 if conversion time has not elapsed, -2 if failure occurred
  */
 int32_t PIOS_MS5611_ReadADC(void)
 {
@@ -177,16 +225,19 @@ int32_t PIOS_MS5611_ReadADC(void)
     Data[1] = 0;
     Data[2] = 0;
 
-    while (PIOS_MS5611_GetDelayUs() > PIOS_DELAY_DiffuS(lastConversionStart)) {
+    if (CurrentRead == MS5611_CONVERSION_TYPE_None) {
-        vTaskDelay(0);
+        return -2;
+    }
+    if (conversionDelayUs > PIOS_DELAY_DiffuS(lastConversionStart)) {
+        return -1;
     }
     static int64_t deltaTemp;
 
     /* Read and store the 16bit result */
-    if (CurrentRead == TemperatureConv) {
+    if (CurrentRead == MS5611_CONVERSION_TYPE_TemperatureConv) {
         /* Read the temperature conversion */
         if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) {
-            return -1;
+            return -2;
         }
 
         RawTemperature = (Data[0] << 16) | (Data[1] << 8) | Data[2];
@@ -205,7 +256,7 @@ int32_t PIOS_MS5611_ReadADC(void)
 
         /* Read the pressure conversion */
         if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) {
-            return -1;
+            return -2;
         }
         // check if temperature is less than 20°C
         if (Temperature < 2000) {
@@ -248,18 +299,18 @@ int32_t PIOS_MS5611_ReadADC(void)
 /**
  * Return the most recently computed temperature in kPa
  */
-float PIOS_MS5611_GetTemperature(void)
+static float PIOS_MS5611_GetTemperature(void)
 {
     // Apply the second order low and very low temperature compensation offset
     return ((float)(Temperature - compensation_t2)) / 100.0f;
 }
 
 /**
- * Return the most recently computed pressure in kPa
+ * Return the most recently computed pressure in Pa
  */
-float PIOS_MS5611_GetPressure(void)
+static float PIOS_MS5611_GetPressure(void)
 {
-    return ((float)Pressure) / 1000.0f;
+    return (float)Pressure;
 }
 
 /**
@@ -270,7 +321,7 @@ float PIOS_MS5611_GetPressure(void)
  * \return 0 if operation was successful
  * \return -1 if error during I2C transfer
  */
-int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len)
+static int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len)
 {
     const struct pios_i2c_txn txn_list[] = {
         {
@@ -300,7 +351,7 @@ int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len)
  * \return 0 if operation was successful
  * \return -1 if error during I2C transfer
  */
-int32_t PIOS_MS5611_WriteCommand(uint8_t command)
+static int32_t PIOS_MS5611_WriteCommand(uint8_t command)
 {
     const struct pios_i2c_txn txn_list[] = {
         {
@@ -326,16 +377,16 @@ int32_t PIOS_MS5611_Test()
     int32_t cur_value = 0;
 
     cur_value = Temperature;
-    PIOS_MS5611_StartADC(TemperatureConv);
+    PIOS_MS5611_StartADC(MS5611_CONVERSION_TYPE_TemperatureConv);
-    PIOS_DELAY_WaitmS(5);
+    PIOS_DELAY_WaitmS(10);
     PIOS_MS5611_ReadADC();
     if (cur_value == Temperature) {
         return -1;
     }
 
     cur_value = Pressure;
-    PIOS_MS5611_StartADC(PressureConv);
+    PIOS_MS5611_StartADC(MS5611_CONVERSION_TYPE_PressureConv);
-    PIOS_DELAY_WaitmS(26);
+    PIOS_DELAY_WaitmS(10);
     PIOS_MS5611_ReadADC();
     if (cur_value == Pressure) {
         return -1;
@@ -344,6 +395,80 @@ int32_t PIOS_MS5611_Test()
     return 0;
 }
 
+
+/* PIOS sensor driver implementation */
+void PIOS_MS5611_Register()
+{
+    PIOS_SENSORS_Register(&PIOS_MS5611_Driver, PIOS_SENSORS_TYPE_1AXIS_BARO, 0);
+}
+
+bool PIOS_MS5611_driver_Test(__attribute__((unused)) uintptr_t context)
+{
+    return true; // !PIOS_MS5611_Test();
+}
+
+void PIOS_MS5611_driver_Reset(__attribute__((unused)) uintptr_t context) {}
+
+void PIOS_MS5611_driver_get_scale(float *scales, uint8_t size, __attribute__((unused))  uintptr_t context)
+{
+    PIOS_Assert(size > 0);
+    scales[0] = 1;
+}
+
+void PIOS_MS5611_driver_fetch(void *data, __attribute__((unused)) uint8_t size, __attribute__((unused)) uintptr_t context)
+{
+    PIOS_Assert(data);
+    memcpy(data, (void *)&results, sizeof(PIOS_SENSORS_1Axis_SensorsWithTemp));
+}
+
+bool PIOS_MS5611_driver_poll(__attribute__((unused)) uintptr_t context)
+{
+    static uint8_t temp_press_interleave_count = 1;
+    static MS5611_FSM_State next_state = MS5611_FSM_INIT;
+
+    int32_t conversionResult = PIOS_MS5611_ReadADC();
+
+    if (__builtin_expect(conversionResult == -1, 1)) {
+        return false; // wait for conversion to complete
+    } else if (__builtin_expect(conversionResult == -2, 0)) {
+        temp_press_interleave_count = 1;
+        next_state = MS5611_FSM_INIT;
+    }
+    switch (next_state) {
+    case MS5611_FSM_INIT:
+    case MS5611_FSM_TEMPERATURE:
+        PIOS_MS5611_StartADC(MS5611_CONVERSION_TYPE_TemperatureConv);
+        next_state = MS5611_FSM_PRESSURE;
+        return false;
+
+    case MS5611_FSM_PRESSURE:
+        PIOS_MS5611_StartADC(MS5611_CONVERSION_TYPE_PressureConv);
+        next_state = MS5611_FSM_CALCULATE;
+        return false;
+
+    case MS5611_FSM_CALCULATE:
+        temp_press_interleave_count--;
+        if (!temp_press_interleave_count) {
+            temp_press_interleave_count = PIOS_MS5611_SLOW_TEMP_RATE;
+            PIOS_MS5611_StartADC(MS5611_CONVERSION_TYPE_TemperatureConv);
+            next_state = MS5611_FSM_PRESSURE;
+        } else {
+            PIOS_MS5611_StartADC(MS5611_CONVERSION_TYPE_PressureConv);
+            next_state = MS5611_FSM_CALCULATE;
+        }
+
+        results.temperature = PIOS_MS5611_GetTemperature();
+        results.sample = PIOS_MS5611_GetPressure();
+        return true;
+
+    default:
+        // it should not be there
+        PIOS_Assert(0);
+    }
+    return false;
+}
+
+
 #endif /* PIOS_INCLUDE_MS5611 */
 
 /**

flight/pios/common/pios_rfm22b.c

@@ -47,8 +47,8 @@
 // 6-byte (32-bit) preamble .. alternating 0's & 1's
 // 4-byte (32-bit) sync
 // 1-byte packet length (number of data bytes to follow)
-// 1 byte valid bitmask
+// 1 byte PPM values LSB (bit 0)
-// 8 PPM values (0-255)
+// 8 bytes PPM values MSBs (bit 8:1)
 // 1 byte CRC
 //
 // *****************************************************************
@@ -61,6 +61,7 @@
 #include <pios_rfm22b_priv.h>
 #include <pios_ppm_out.h>
 #include <ecc.h>
+#include <sha1.h>
 
 /* Local Defines */
 #define STACK_SIZE_BYTES                 200
@@ -73,9 +74,16 @@
 #define RFM22B_LINK_QUALITY_THRESHOLD    20
 #define RFM22B_DEFAULT_MIN_CHANNEL       0
 #define RFM22B_DEFAULT_MAX_CHANNEL       250
-#define RFM22B_DEFAULT_CHANNEL_SET       24
 #define RFM22B_PPM_ONLY_DATARATE         RFM22_datarate_9600
 
+// PPM encoding limits
+#define RFM22B_PPM_MIN                   1
+#define RFM22B_PPM_MAX                   511
+#define RFM22B_PPM_INVALID               0
+#define RFM22B_PPM_SCALE                 2
+#define RFM22B_PPM_MIN_US                990
+#define RFM22B_PPM_MAX_US                (RFM22B_PPM_MIN_US + (RFM22B_PPM_MAX - RFM22B_PPM_MIN) * RFM22B_PPM_SCALE)
+
 // The maximum amount of time without activity before initiating a reset.
 #define PIOS_RFM22B_SUPERVISOR_TIMEOUT   150  // ms
 
@@ -116,6 +124,9 @@
 #define USB_LED_OFF
 #endif
 
+#define CONNECTED_TIMEOUT (250 / portTICK_RATE_MS) /* ms */
+#define MAX_CHANNELS      32
+
 /* Local type definitions */
 
 struct pios_rfm22b_transition {
@@ -155,21 +166,6 @@ static const uint8_t OUT_FF[64] = {
     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, 14
-};
-
 /* Local function forwared declarations */
 static void pios_rfm22_task(void *parameters);
 static bool pios_rfm22_readStatus(struct pios_rfm22b_dev *rfm22b_dev);
@@ -207,6 +203,10 @@ static void rfm22_clearLEDs();
 // Utility functions.
 static uint32_t pios_rfm22_time_difference_ms(portTickType start_time, portTickType end_time);
 static struct pios_rfm22b_dev *pios_rfm22_alloc(void);
+static void rfm22_hmac_sha1(const uint8_t *data, size_t len, uint8_t key[SHA1_DIGEST_LENGTH],
+                            uint8_t digest[SHA1_DIGEST_LENGTH]);
+static bool rfm22_gen_channels(uint32_t coordid, enum rfm22b_datarate datarate, uint8_t min,
+                               uint8_t max, uint8_t channels[MAX_CHANNELS], uint8_t *clen);
 
 // SPI read/write functions
 static void rfm22_assertCs(struct pios_rfm22b_dev *rfm22b_dev);
@@ -331,6 +331,18 @@ static const uint32_t data_rate[] = {
     256000, // 256 kbps, 433 MHz, 150 khz freq dev
 };
 
+static const uint8_t channel_spacing[] = {
+    1, /* 9.6kbps */
+    2, /* 19.2kps */
+    2, /* 32kps */
+    2, /* 57.6kps */
+    2, /* 64kps */
+    3, /* 100kps */
+    4, /* 128kps */
+    4, /* 192kps */
+    4 /* 256kps */
+};
+
 static const uint8_t reg_1C[] = { 0x01, 0x05, 0x06, 0x95, 0x95, 0x81, 0x88, 0x8B, 0x8D }; // rfm22_if_filter_bandwidth
 
 static const uint8_t reg_1D[] = { 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40 }; // rfm22_afc_loop_gearshift_override
@@ -358,7 +370,7 @@ static const uint8_t reg_72[] = { 0x30, 0x48, 0x48, 0x48, 0x48, 0x60, 0x90, 0xCD
 
 static const uint8_t packet_time[] = { 80, 40, 25, 15, 13, 10, 8, 6, 5 };
 static const uint8_t packet_time_ppm[] = { 26, 25, 25, 15, 13, 10, 8, 6, 5 };
-static const uint8_t num_channels[] = { 4, 4, 4, 6, 8, 8, 10, 12, 16 };
+static const uint8_t num_channels[] = { 32, 32, 32, 32, 32, 32, 32, 32, 32 };
 
 static struct pios_rfm22b_dev *g_rfm22b_dev = NULL;
 
@@ -412,7 +424,6 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu
     rfm22b_dev->stats.rx_error     = 0;
     rfm22b_dev->stats.rx_missed    = 0;
     rfm22b_dev->stats.tx_dropped   = 0;
-    rfm22b_dev->stats.tx_resent    = 0;
     rfm22b_dev->stats.resets       = 0;
     rfm22b_dev->stats.timeouts     = 0;
     rfm22b_dev->stats.link_quality = 0;
@@ -423,7 +434,7 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu
 
     // Initialize the channels.
     PIOS_RFM22B_SetChannelConfig(*rfm22b_id, RFM22B_DEFAULT_RX_DATARATE, RFM22B_DEFAULT_MIN_CHANNEL,
-                                 RFM22B_DEFAULT_MAX_CHANNEL, RFM22B_DEFAULT_CHANNEL_SET, false, false, false, false);
+                                 RFM22B_DEFAULT_MAX_CHANNEL, false, false, false, false);
 
     // Create the event queue
     rfm22b_dev->eventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(enum pios_radio_event));
@@ -566,12 +577,12 @@ void PIOS_RFM22B_SetTxPower(uint32_t rfm22b_id, enum rfm22b_tx_power tx_pwr)
  * @param[in] datarate  The desired datarate.
  * @param[in] min_chan  The minimum channel.
  * @param[in] max_chan  The maximum channel.
- * @param[in] chan_set  The "seed" for selecting a channel sequence.
  * @param[in] coordinator Is this modem an coordinator.
  * @param[in] ppm_mode Should this modem send/receive ppm packets?
  * @param[in] oneway Only the coordinator can send packets if true.
+ * @param[in] ppm_only Should this modem run in ppm only mode?
  */
-void PIOS_RFM22B_SetChannelConfig(uint32_t rfm22b_id, enum rfm22b_datarate datarate, uint8_t min_chan, uint8_t max_chan, uint8_t chan_set, bool coordinator, bool oneway, bool ppm_mode, bool ppm_only)
+void PIOS_RFM22B_SetChannelConfig(uint32_t rfm22b_id, enum rfm22b_datarate datarate, uint8_t min_chan, uint8_t max_chan, bool coordinator, bool oneway, bool ppm_mode, bool ppm_only)
 {
     struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
 
@@ -596,15 +607,11 @@ void PIOS_RFM22B_SetChannelConfig(uint32_t rfm22b_id, enum rfm22b_datarate datar
     }
     rfm22b_dev->packet_time = (ppm_mode ? packet_time_ppm[datarate] : packet_time[datarate]);
 
-    // Find the first N channels that meet the min/max criteria out of the random channel list.
     uint8_t num_found = 0;
-    for (uint16_t i = 0; (i < RFM22B_NUM_CHANNELS) && (num_found < num_channels[datarate]); ++i) {
+    rfm22_gen_channels(rfm22_destinationID(rfm22b_dev), datarate, min_chan, max_chan,
-        uint8_t idx  = (i + chan_set) % RFM22B_NUM_CHANNELS;
+                       rfm22b_dev->channels, &num_found);
-        uint8_t chan = channel_list[idx];
+
-        if ((chan >= min_chan) && (chan <= max_chan)) {
+    rfm22b_dev->num_channels = num_found;
-            rfm22b_dev->channels[num_found++] = chan;
-        }
-    }
 
     // Calculate the maximum packet length from the datarate.
     float bytes_per_period = (float)data_rate[datarate] * (float)(rfm22b_dev->packet_time - 2) / 9000;
@@ -1354,7 +1361,7 @@ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
     rfm22b_dev->packet_start_ticks = 0;
     rfm22b_dev->tx_complete_ticks  = 0;
     rfm22b_dev->rfm22b_state       = RFM22B_STATE_INITIALIZING;
-    rfm22b_dev->on_sync_channel    = false;
+    rfm22b_dev->last_contact       = 0;
 
     // software reset the RF chip .. following procedure according to Si4x3x Errata (rev. B)
     rfm22_write_claim(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_swres);
@@ -1725,7 +1732,7 @@ static bool pios_rfm22_readStatus(struct pios_rfm22b_dev *rfm22b_dev)
  */
 static void rfm22_rxFailure(struct pios_rfm22b_dev *rfm22b_dev)
 {
-    rfm22b_dev->stats.rx_failure++;
+    rfm22b_add_rx_status(rfm22b_dev, RADIO_FAILURE_RX_PACKET);
     rfm22b_dev->rx_buffer_wr = 0;
     rfm22b_dev->packet_start_ticks = 0;
     rfm22b_dev->rfm22b_state = RFM22B_STATE_TRANSITION;
@@ -1767,18 +1774,31 @@ static enum pios_radio_event radio_txStart(struct pios_rfm22b_dev *radio_dev)
             return RADIO_EVENT_RX_MODE;
         }
 
-        // The first byte is a bitmask of valid channels.
+        // The first byte stores the LSB of each channel
         p[0] = 0;
 
         // Read the PPM input.
         for (uint8_t i = 0; i < RFM22B_PPM_NUM_CHANNELS; ++i) {
             int32_t val = radio_dev->ppm[i];
+
+            // Clamp and translate value, or transmit as reserved "invalid" constant
             if ((val == PIOS_RCVR_INVALID) || (val == PIOS_RCVR_TIMEOUT)) {
-                p[i + 1] = 0;
+                val = RFM22B_PPM_INVALID;
+            } else if (val > RFM22B_PPM_MAX_US) {
+                val = RFM22B_PPM_MAX;
+            } else if (val < RFM22B_PPM_MIN_US) {
+                val = RFM22B_PPM_MIN;
             } else {
-                p[0]    |= 1 << i;
+                val = (val - RFM22B_PPM_MIN_US) / RFM22B_PPM_SCALE + RFM22B_PPM_MIN;
-                p[i + 1] = (val < 1000) ? 0 : ((val >= 1900) ? 255 : (uint8_t)(256 * (val - 1000) / 900));
             }
+
+            // Store LSB
+            if (val & 1) {
+                p[0] |= (1 << i);
+            }
+
+            // Store upper 8 bits in array
+            p[i + 1] = val >> 1;
         }
 
         // The last byte is a CRC.
@@ -1798,8 +1818,8 @@ static enum pios_radio_event radio_txStart(struct pios_rfm22b_dev *radio_dev)
         len += (radio_dev->tx_out_cb)(radio_dev->tx_out_context, p + len, max_data_len - len, NULL, &need_yield);
     }
 
-    // Always send a packet on the sync channel if this modem is a coordinator.
+    // Always send a packet if this modem is a coordinator.
-    if ((len == 0) && ((radio_dev->channel_index != 0) || !rfm22_isCoordinator(radio_dev))) {
+    if ((len == 0) && !rfm22_isCoordinator(radio_dev)) {
         return RADIO_EVENT_RX_MODE;
     }
 
@@ -1921,10 +1941,11 @@ static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_d
 
         if (good_packet || corrected_packet) {
             for (uint8_t i = 0; i < RFM22B_PPM_NUM_CHANNELS; ++i) {
+                // Calculate 9-bit value taking the LSB from byte 0
+                uint32_t val = (p[i + 1] << 1) + ((p[0] >> i) & 1);
                 // Is this a valid channel?
-                if (p[0] & (1 << i)) {
+                if (val != RFM22B_PPM_INVALID) {
-                    uint32_t val = p[i + 1];
+                    radio_dev->ppm[i] = (uint16_t)(RFM22B_PPM_MIN_US + (val - RFM22B_PPM_MIN) * RFM22B_PPM_SCALE);
-                    radio_dev->ppm[i] = (uint16_t)(1000 + val * 900 / 256);
                 } else {
                     radio_dev->ppm[i] = PIOS_RCVR_INVALID;
                 }
@@ -1959,12 +1980,16 @@ static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_d
             (radio_dev->rx_in_cb)(radio_dev->rx_in_context, p, data_len, NULL, &rx_need_yield);
         }
 
-        // We only synchronize the clock on packets from our coordinator on the sync channel.
+        /*
-        if (!rfm22_isCoordinator(radio_dev) && (radio_dev->rx_destination_id == rfm22_destinationID(radio_dev)) && (radio_dev->channel_index == 0)) {
+         * If the packet is valid and destined for us we synchronize the clock.
+         */
+        if (!rfm22_isCoordinator(radio_dev) &&
+            radio_dev->rx_destination_id == rfm22_destinationID(radio_dev)) {
             rfm22_synchronizeClock(radio_dev);
             radio_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTED;
-            radio_dev->on_sync_channel  = false;
         }
+
+        radio_dev->last_contact = xTaskGetTickCount();
     } else {
         ret_event = RADIO_EVENT_RX_COMPLETE;
     }
@@ -2069,7 +2094,7 @@ static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev)
     rfm22b_dev->stats.rx_good      = 0;
     rfm22b_dev->stats.rx_corrected = 0;
     rfm22b_dev->stats.rx_error     = 0;
-    rfm22b_dev->stats.tx_resent    = 0;
+    rfm22b_dev->stats.rx_failure   = 0;
     for (uint8_t i = 0; i < RFM22B_RX_PACKET_STATS_LEN; ++i) {
         uint32_t val = rfm22b_dev->rx_packet_stats[i];
         for (uint8_t j = 0; j < 16; ++j) {
@@ -2083,8 +2108,8 @@ static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev)
             case RADIO_ERROR_RX_PACKET:
                 rfm22b_dev->stats.rx_error++;
                 break;
-            case RADIO_RESENT_TX_PACKET:
+            case RADIO_FAILURE_RX_PACKET:
-                rfm22b_dev->stats.tx_resent++;
+                rfm22b_dev->stats.rx_failure++;
                 break;
             }
         }
@@ -2094,7 +2119,7 @@ static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev)
     // Note: This assumes that the number of packets sampled for the stats is 64.
     // Using this equation, error and resent packets are counted as -2, and corrected packets are counted as -1.
     // The range is 0 (all error or resent packets) to 128 (all good packets).
-    rfm22b_dev->stats.link_quality = 64 + rfm22b_dev->stats.rx_good - rfm22b_dev->stats.rx_error - rfm22b_dev->stats.tx_resent;
+    rfm22b_dev->stats.link_quality = 64 + rfm22b_dev->stats.rx_good - rfm22b_dev->stats.rx_error - rfm22b_dev->stats.rx_failure;
 }
 
 /**
@@ -2160,14 +2185,14 @@ static void rfm22_synchronizeClock(struct pios_rfm22b_dev *rfm22b_dev)
     portTickType start_time = rfm22b_dev->packet_start_ticks;
 
     // This packet was transmitted on channel 0, calculate the time delta that will force us to transmit on channel 0 at the time this packet started.
-    uint8_t num_chan    = num_channels[rfm22b_dev->datarate];
+    uint16_t frequency_hop_cycle_time = rfm22b_dev->packet_time * rfm22b_dev->num_channels;
-    uint16_t frequency_hop_cycle_time = rfm22b_dev->packet_time * num_chan;
     uint16_t time_delta = start_time % frequency_hop_cycle_time;
 
     // Calculate the adjustment for the preamble
     uint8_t offset = (uint8_t)ceil(35000.0F / data_rate[rfm22b_dev->datarate]);
 
-    rfm22b_dev->time_delta = frequency_hop_cycle_time - time_delta + offset;
+    rfm22b_dev->time_delta = frequency_hop_cycle_time - time_delta + offset +
+                             rfm22b_dev->packet_time * rfm22b_dev->channel_index;
 }
 
 /**
@@ -2222,15 +2247,13 @@ static bool rfm22_timeToSend(struct pios_rfm22b_dev *rfm22b_dev)
 static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t index)
 {
     // Make sure we don't index outside of the range.
-    uint8_t num_chan = num_channels[rfm22b_dev->datarate];
+    uint8_t idx = index % rfm22b_dev->num_channels;
-    uint8_t idx = index % num_chan;
 
     // Are we switching to a new channel?
     if (idx != rfm22b_dev->channel_index) {
-        // If the on_sync_channel flag is set, it means that we were on the sync channel, but no packet was received, so transition to a non-connected state.
+        if (!rfm22_isCoordinator(rfm22b_dev) &&
-        if (!rfm22_isCoordinator(rfm22b_dev) && (rfm22b_dev->channel_index == 0) && rfm22b_dev->on_sync_channel) {
+            pios_rfm22_time_difference_ms(rfm22b_dev->last_contact, xTaskGetTickCount()) >=
-            rfm22b_dev->on_sync_channel = false;
+            CONNECTED_TIMEOUT) {
-
             // Set the link state to disconnected.
             if (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) {
                 rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_DISCONNECTED;
@@ -2240,11 +2263,8 @@ static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t ind
                 }
             }
 
-            // Stay on the sync channel.
+            // Stay on first channel.
             idx = 0;
-        } else if (idx == 0) {
-            // If we're switching to the sync channel, set a flag that can be used to detect if a packet was received.
-            rfm22b_dev->on_sync_channel = true;
         }
 
         rfm22b_dev->channel_index = idx;
@@ -2263,8 +2283,7 @@ static uint8_t rfm22_calcChannelFromClock(struct pios_rfm22b_dev *rfm22b_dev)
     portTickType time = rfm22_coordinatorTime(rfm22b_dev, xTaskGetTickCount());
     // Divide time into 8ms blocks.  Coordinator sends in first 2 ms, and remote send in 5th and 6th ms.
     // Channel changes occur in the last 2 ms.
-    uint8_t num_chan  = num_channels[rfm22b_dev->datarate];
+    uint8_t n = (time / rfm22b_dev->packet_time) % rfm22b_dev->num_channels;
-    uint8_t n = (time / rfm22b_dev->packet_time) % num_chan;
 
     return rfm22_calcChannel(rfm22b_dev, n);
 }
@@ -2553,6 +2572,86 @@ static uint8_t rfm22_read(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr)
     return in[1];
 }
 
+
+static void rfm22_hmac_sha1(const uint8_t *data, size_t len,
+                            uint8_t key[SHA1_DIGEST_LENGTH],
+                            uint8_t digest[SHA1_DIGEST_LENGTH])
+{
+    uint8_t ipad[64] = { 0 };
+    uint8_t opad[64] = { 0 };
+    static SHA1_CTX *ctx;
+
+    ctx = pios_malloc(sizeof(SHA1_CTX));
+
+    memcpy(ipad, key, SHA1_DIGEST_LENGTH);
+    memcpy(opad, key, SHA1_DIGEST_LENGTH);
+
+    for (int i = 0; i < 64; i++) {
+        ipad[i] ^= 0x36;
+        opad[i] ^= 0x5c;
+    }
+
+    SHA1Init(ctx);
+    SHA1Update(ctx, ipad, sizeof(ipad));
+    SHA1Update(ctx, data, len);
+    SHA1Final(digest, ctx);
+
+    SHA1Init(ctx);
+    SHA1Update(ctx, opad, sizeof(opad));
+    SHA1Update(ctx, digest, SHA1_DIGEST_LENGTH);
+    SHA1Final(digest, ctx);
+
+    pios_free(ctx);
+}
+
+static bool rfm22_gen_channels(uint32_t coordid, enum rfm22b_datarate rate, uint8_t min,
+                               uint8_t max, uint8_t channels[MAX_CHANNELS], uint8_t *clen)
+{
+    uint32_t data = 0;
+    uint8_t cpos  = 0;
+    uint8_t chan_range = (max / channel_spacing[rate] - min / channel_spacing[rate]) + 1;
+    uint8_t key[SHA1_DIGEST_LENGTH] = { 0 };
+    uint8_t digest[SHA1_DIGEST_LENGTH];
+    uint8_t *all_channels;
+
+    all_channels = pios_malloc(RFM22B_NUM_CHANNELS);
+
+    memcpy(key, &coordid, sizeof(coordid));
+
+    for (int i = 0; i < chan_range; i++) {
+        all_channels[i] = min / channel_spacing[rate] + i;
+    }
+
+    int j = SHA1_DIGEST_LENGTH;
+    for (int i = 0; i < chan_range && i < MAX_CHANNELS; i++) {
+        uint8_t rnd;
+        uint8_t r;
+        uint8_t tmp;
+
+        if (j == SHA1_DIGEST_LENGTH) {
+            rfm22_hmac_sha1((uint8_t *)&data, sizeof(data), key, digest);
+            j = 0;
+            data++;
+        }
+        rnd = digest[j];
+        j++;
+        r   = rnd % (chan_range - i) + i;
+        tmp = all_channels[i];
+        all_channels[i] = all_channels[r];
+        all_channels[r] = tmp;
+    }
+
+    for (int i = 0; i < chan_range && cpos < MAX_CHANNELS; i++, cpos++) {
+        channels[cpos] = all_channels[i] * channel_spacing[rate];
+    }
+
+    *clen = cpos & 0xfe;
+
+    pios_free(all_channels);
+
+    return *clen > 0;
+}
+
 #endif /* PIOS_INCLUDE_RFM22B */
 
 /**

flight/pios/common/pios_sensors.c

@@ -0,0 +1,65 @@
+/**
+ ******************************************************************************
+ *
+ * @file       pios_sensors.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ * @brief      PiOS sensors handling
+ *             --
+ * @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_mem.h>
+#include <pios_sensors.h>
+#include <string.h>
+
+// private variables
+
+static PIOS_SENSORS_Instance *sensor_list = 0;
+
+PIOS_SENSORS_Instance *PIOS_SENSORS_Register(const PIOS_SENSORS_Driver *driver, PIOS_SENSORS_TYPE type, uintptr_t context)
+{
+    PIOS_SENSORS_Instance *instance = (PIOS_SENSORS_Instance *)pios_malloc(sizeof(PIOS_SENSORS_Instance));
+
+    instance->driver  = driver;
+    instance->type    = type;
+    instance->context = context;
+    instance->next    = NULL;
+    LL_APPEND(sensor_list, instance);
+    return instance;
+}
+
+PIOS_SENSORS_Instance *PIOS_SENSORS_GetList()
+{
+    return sensor_list;
+}
+
+PIOS_SENSORS_Instance *PIOS_SENSORS_GetInstanceByType(const PIOS_SENSORS_Instance *previous_instance, PIOS_SENSORS_TYPE type)
+{
+    if (!previous_instance) {
+        previous_instance = sensor_list;
+    }
+    PIOS_SENSORS_Instance *sensor;
+
+    LL_FOREACH((PIOS_SENSORS_Instance *)previous_instance, sensor) {
+        if (sensor->type && type) {
+            return sensor;
+        }
+    }
+    return NULL;
+}

flight/pios/inc/pios_hmc5x83.h

@@ -31,6 +31,7 @@
 #ifndef PIOS_HMC5x83_H
 #define PIOS_HMC5x83_H
 #include <stdint.h>
+#include <pios_sensors.h>
 /* HMC5x83 Addresses */
 #define PIOS_HMC5x83_I2C_ADDR           0x1E
 #define PIOS_HMC5x83_I2C_READ_ADDR      0x3D
@@ -123,12 +124,16 @@ struct pios_hmc5x83_cfg {
 
 /* Public Functions */
 extern pios_hmc5x83_dev_t PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_t port_id, uint8_t device_num);
+extern void PIOS_HMC5x83_Register(pios_hmc5x83_dev_t handler);
+
 extern bool PIOS_HMC5x83_NewDataAvailable(pios_hmc5x83_dev_t handler);
 extern int32_t PIOS_HMC5x83_ReadMag(pios_hmc5x83_dev_t handler, int16_t out[3]);
 extern uint8_t PIOS_HMC5x83_ReadID(pios_hmc5x83_dev_t handler, uint8_t out[4]);
 extern int32_t PIOS_HMC5x83_Test(pios_hmc5x83_dev_t handler);
 extern bool PIOS_HMC5x83_IRQHandler(pios_hmc5x83_dev_t handler);
 
+extern const PIOS_SENSORS_Driver PIOS_HMC5x83_Driver;
+
 #endif /* PIOS_HMC5x83_H */
 
 /**

flight/pios/inc/pios_instrumentation.h

@@ -53,9 +53,11 @@ inline void PIOS_Instrumentation_updateCounter(pios_counter_t counter_handle, in
     vPortEnterCritical();
     pios_perf_counter_t *counter = (pios_perf_counter_t *)counter_handle;
     counter->value = newValue;
+    counter->max--;
     if (counter->value > counter->max) {
         counter->max = counter->value;
     }
+    counter->min++;
     if (counter->value < counter->min) {
         counter->min = counter->value;
     }
@@ -88,9 +90,11 @@ inline void PIOS_Instrumentation_TimeEnd(pios_counter_t counter_handle)
     pios_perf_counter_t *counter = (pios_perf_counter_t *)counter_handle;
 
     counter->value = PIOS_DELAY_DiffuS(counter->lastUpdateTS);
+    counter->max--;
     if (counter->value > counter->max) {
         counter->max = counter->value;
     }
+    counter->min++;
     if (counter->value < counter->min) {
         counter->min = counter->value;
     }
@@ -110,9 +114,11 @@ inline void PIOS_Instrumentation_TrackPeriod(pios_counter_t counter_handle)
         vPortEnterCritical();
         uint32_t period = PIOS_DELAY_DiffuS(counter->lastUpdateTS);
         counter->value = (counter->value * 15 + period) / 16;
+        counter->max--;
         if ((int32_t)period > counter->max) {
             counter->max = period;
         }
+        counter->min++;
         if ((int32_t)period < counter->min) {
             counter->min = period;
         }

flight/pios/inc/pios_math.h

@@ -72,6 +72,9 @@
 #define RAD2DEG_D(rad)          ((rad) * (180.0d / M_PI_D))
 #define DEG2RAD_D(deg)          ((deg) * (M_PI_D / 180.0d))
 
+// helper macros for LPFs
+#define LPF_ALPHA(dt, fc)       (dt / (dt + 1.0f / (2.0f * M_PI_F * fc)))
+
 // Useful math macros
 #define MAX(a, b)               ((a) > (b) ? (a) : (b))
 #define MIN(a, b)               ((a) < (b) ? (a) : (b))

flight/pios/inc/pios_mem.h

@@ -26,6 +26,7 @@
  */
 #ifndef PIOS_MEM_H
 #define PIOS_MEM_H
+#include <strings.h>
 
 void *pios_fastheapmalloc(size_t size);
 

flight/pios/inc/pios_mpu6000.h

@@ -31,6 +31,7 @@
 
 #ifndef PIOS_MPU6000_H
 #define PIOS_MPU6000_H
+#include <pios_sensors.h>
 
 /* MPU6000 Addresses */
 #define PIOS_MPU6000_SMPLRT_DIV_REG           0X19
@@ -131,18 +132,6 @@ enum pios_mpu6000_orientation { // clockwise rotation from board forward
     PIOS_MPU6000_TOP_270DEG = 0x03
 };
 
-struct pios_mpu6000_data {
-    int16_t gyro_x;
-    int16_t gyro_y;
-    int16_t gyro_z;
-#if defined(PIOS_MPU6000_ACCEL)
-    int16_t accel_x;
-    int16_t accel_y;
-    int16_t accel_z;
-#endif /* PIOS_MPU6000_ACCEL */
-    int16_t temperature;
-};
-
 struct pios_mpu6000_cfg {
     const struct pios_exti_cfg *exti_cfg; /* Pointer to the EXTI configuration */
 
@@ -167,14 +156,11 @@ struct pios_mpu6000_cfg {
 /* Public Functions */
 extern int32_t PIOS_MPU6000_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_mpu6000_cfg *new_cfg);
 extern int32_t PIOS_MPU6000_ConfigureRanges(enum pios_mpu6000_range gyroRange, enum pios_mpu6000_accel_range accelRange, enum pios_mpu6000_filter filterSetting);
-extern xQueueHandle PIOS_MPU6000_GetQueue();
-extern int32_t PIOS_MPU6000_ReadGyros(struct pios_mpu6000_data *buffer);
 extern int32_t PIOS_MPU6000_ReadID();
-extern int32_t PIOS_MPU6000_Test();
+extern void PIOS_MPU6000_Register();
-extern float PIOS_MPU6000_GetScale();
-extern float PIOS_MPU6000_GetAccelScale();
 extern bool PIOS_MPU6000_IRQHandler(void);
 
+extern const PIOS_SENSORS_Driver PIOS_MPU6000_Driver;
 #endif /* PIOS_MPU6000_H */
 
 /**

flight/pios/inc/pios_ms5611.h

@@ -30,7 +30,7 @@
 
 #ifndef PIOS_MS5611_H
 #define PIOS_MS5611_H
-
+#include <pios_sensors.h>
 /* BMP085 Addresses */
 #define MS5611_I2C_ADDR   0x77
 #define MS5611_RESET      0x1E
@@ -40,17 +40,6 @@
 #define MS5611_PRES_ADDR  0x40
 #define MS5611_TEMP_ADDR  0x50
 #define MS5611_ADC_MSB    0xF6
-#define MS5611_P0         101.3250f
-
-/* Local Types */
-typedef struct {
-    uint16_t C[6];
-} MS5611CalibDataTypeDef;
-
-typedef enum {
-    PressureConv,
-    TemperatureConv
-} ConversionTypeTypeDef;
 
 struct pios_ms5611_cfg {
     uint32_t oversampling;
@@ -66,12 +55,8 @@ enum pios_ms5611_osr {
 
 /* Public Functions */
 extern void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device);
-extern int32_t PIOS_MS5611_StartADC(ConversionTypeTypeDef Type);
+extern const PIOS_SENSORS_Driver PIOS_MS5611_Driver;
-extern int32_t PIOS_MS5611_ReadADC(void);
+void PIOS_MS5611_Register();
-extern float PIOS_MS5611_GetTemperature(void);
-extern float PIOS_MS5611_GetPressure(void);
-extern int32_t PIOS_MS5611_Test();
-extern int32_t PIOS_MS5611_GetDelay();
 
 #endif /* PIOS_MS5611_H */
 

flight/pios/inc/pios_pwm.h

@@ -30,4 +30,6 @@
 #ifndef PIOS_PWM_H
 #define PIOS_PWM_H
 
+#define PIOS_PWM_VALID_RANGE_MAX 2250
+#define PIOS_PWM_VALID_RANGE_MIN 750
 #endif /* PIOS_PWM_H */

flight/pios/inc/pios_rfm22b.h

@@ -90,7 +90,6 @@ struct rfm22b_stats {
     uint8_t  rx_missed;
     uint8_t  rx_failure;
     uint8_t  tx_dropped;
-    uint8_t  tx_resent;
     uint8_t  tx_failure;
     uint8_t  resets;
     uint8_t  timeouts;
@@ -104,7 +103,7 @@ struct rfm22b_stats {
 extern int32_t PIOS_RFM22B_Init(uint32_t *rfb22b_id, uint32_t spi_id, uint32_t slave_num, const struct pios_rfm22b_cfg *cfg);
 extern void PIOS_RFM22B_Reinit(uint32_t rfb22b_id);
 extern void PIOS_RFM22B_SetTxPower(uint32_t rfm22b_id, enum rfm22b_tx_power tx_pwr);
-extern void PIOS_RFM22B_SetChannelConfig(uint32_t rfm22b_id, enum rfm22b_datarate datarate, uint8_t min_chan, uint8_t max_chan, uint8_t chan_set, bool coordinator, bool oneway, bool ppm_mode, bool ppm_only);
+extern void PIOS_RFM22B_SetChannelConfig(uint32_t rfm22b_id, enum rfm22b_datarate datarate, uint8_t min_chan, uint8_t max_chan, bool coordinator, bool oneway, bool ppm_mode, bool ppm_only);
 extern void PIOS_RFM22B_SetCoordinatorID(uint32_t rfm22b_id, uint32_t coord_id);
 extern uint32_t PIOS_RFM22B_DeviceID(uint32_t rfb22b_id);
 extern void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats);

flight/pios/inc/pios_rfm22b_priv.h

@@ -40,7 +40,7 @@
 // ************************************
 
 #define RFM22B_MAX_PACKET_LEN                     64
-#define RFM22B_NUM_CHANNELS                       250
+#define RFM22B_NUM_CHANNELS                       251
 
 // ************************************
 
@@ -578,7 +578,7 @@ enum pios_rfm22b_rx_packet_status {
     RADIO_GOOD_RX_PACKET      = 0x00,
     RADIO_CORRECTED_RX_PACKET = 0x01,
     RADIO_ERROR_RX_PACKET     = 0x2,
-    RADIO_RESENT_TX_PACKET    = 0x3
+    RADIO_FAILURE_RX_PACKET   = 0x3
 };
 
 typedef struct {
@@ -780,7 +780,7 @@ struct pios_rfm22b_dev {
     portTickType packet_start_ticks;
     portTickType tx_complete_ticks;
     portTickType time_delta;
-    bool         on_sync_channel;
+    portTickType last_contact;
 };
 
 

flight/pios/inc/pios_sensors.h

@@ -0,0 +1,190 @@
+/**
+ ******************************************************************************
+ *
+ * @file       pios_sensors.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ * @brief      PiOS sensors handling
+ *             --
+ * @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_SENSORS_H
+#define PIOS_SENSORS_H
+#include <pios.h>
+#include <utlist.h>
+#include <stdint.h>
+#include <vectors.h>
+// needed for debug APIs.
+
+typedef bool (*PIOS_SENSORS_test_function)(uintptr_t context);
+typedef void (*PIOS_SENSORS_reset_function)(uintptr_t context);
+typedef bool (*PIOS_SENSORS_poll_function)(uintptr_t context);
+typedef void (*PIOS_SENSORS_fetch_function)(void *samples, uint8_t size, uintptr_t context);
+/**
+ * return an array with current scale for the instance.
+ * Instances with multiples sensors returns several value in the same
+ * order as they appear in PIOS_SENSORS_TYPE enums.
+ */
+typedef void (*PIOS_SENSORS_get_scale_function)(float *, uint8_t size, uintptr_t context);
+typedef QueueHandle_t (*PIOS_SENSORS_get_queue_function)(uintptr_t context);
+
+typedef struct PIOS_SENSORS_Driver {
+    PIOS_SENSORS_test_function      test; // called at startup to test the sensor
+    PIOS_SENSORS_poll_function      poll; // called to check whether data are available for polled sensors
+    PIOS_SENSORS_fetch_function     fetch; // called to fetch data for polled sensors
+    PIOS_SENSORS_reset_function     reset; // reset sensor. for example if data are not received in the allotted time
+    PIOS_SENSORS_get_queue_function get_queue; // get the queue reference
+    PIOS_SENSORS_get_scale_function get_scale; // return scales for the sensors
+    bool is_polled;
+} PIOS_SENSORS_Driver;
+
+typedef enum PIOS_SENSORS_TYPE {
+    PIOS_SENSORS_TYPE_3AXIS_ACCEL = 0x01,
+    PIOS_SENSORS_TYPE_3AXIS_GYRO  = 0x02,
+    PIOS_SENSORS_TYPE_3AXIS_GYRO_ACCEL = 0x03,
+    PIOS_SENSORS_TYPE_3AXIS_MAG   = 0x04,
+    PIOS_SENSORS_TYPE_1AXIS_BARO  = 0x08,
+} PIOS_SENSORS_TYPE;
+
+#define PIOS_SENSORS_TYPE_1D (PIOS_SENSORS_TYPE_1AXIS_BARO)
+#define PIOS_SENSORS_TYPE_3D (PIOS_SENSORS_TYPE_3AXIS_ACCEL | PIOS_SENSORS_TYPE_3AXIS_GYRO | PIOS_SENSORS_TYPE_3AXIS_MAG)
+
+typedef struct PIOS_SENSORS_Instance {
+    const PIOS_SENSORS_Driver    *driver;
+    uintptr_t context;
+    struct PIOS_SENSORS_Instance *next;
+    uint8_t type;
+} PIOS_SENSORS_Instance;
+
+/**
+ * A 3d Accel sample with temperature
+ */
+typedef struct PIOS_SENSORS_3Axis_SensorsWithTemp {
+    uint16_t   count; // number of sensor instances
+    int16_t    temperature;  // Degrees Celsius * 100
+    Vector3i16 sample[];
+} PIOS_SENSORS_3Axis_SensorsWithTemp;
+
+typedef struct PIOS_SENSORS_1Axis_SensorsWithTemp {
+    float temperature; // Degrees Celsius
+    float sample; // sample
+} PIOS_SENSORS_1Axis_SensorsWithTemp;
+
+/**
+ * Register a new sensor instance with sensor subsystem
+ * @param driver sensor driver
+ * @param type sensor type @ref PIOS_SENSORS_TYPE
+ * @param context context to be passed to sensor driver
+ * @return the new sensor instance
+ */
+
+PIOS_SENSORS_Instance *PIOS_SENSORS_Register(const PIOS_SENSORS_Driver *driver, PIOS_SENSORS_TYPE type, uintptr_t context);
+/**
+ * return the list of registered sensors.
+ * @return the first sensor instance in the list.
+ */
+PIOS_SENSORS_Instance *PIOS_SENSORS_GetList();
+
+/**
+ * Perform sensor test and return true if passed
+ * @param sensor instance to test
+ * @return true if test passes
+ */
+static inline bool PIOS_SENSORS_Test(const PIOS_SENSORS_Instance *sensor)
+{
+    PIOS_Assert(sensor);
+
+    if (!sensor->driver->test) {
+        return true;
+    } else {
+        return sensor->driver->test(sensor->context);
+    }
+}
+
+/**
+ * Poll sensor for new values
+ * @param sensor instance to poll
+ * @return true if sensor has samples available
+ */
+static inline bool PIOS_SENSORS_Poll(const PIOS_SENSORS_Instance *sensor)
+{
+    PIOS_Assert(sensor);
+
+    if (!sensor->driver->poll) {
+        return true;
+    } else {
+        return sensor->driver->poll(sensor->context);
+    }
+}
+/**
+ *
+ * @param sensor
+ * @param samples
+ * @param size
+ */
+static inline void PIOS_SENSOR_Fetch(const PIOS_SENSORS_Instance *sensor, void *samples, uint8_t size)
+{
+    PIOS_Assert(sensor);
+    sensor->driver->fetch(samples, size, sensor->context);
+}
+
+static inline void PIOS_SENSOR_Reset(const PIOS_SENSORS_Instance *sensor)
+{
+    PIOS_Assert(sensor);
+    sensor->driver->reset(sensor->context);
+}
+
+/**
+ * retrieve the sensor queue
+ * @param sensor
+ * @return sensor queue or null if not supported
+ */
+static inline QueueHandle_t PIOS_SENSORS_GetQueue(const PIOS_SENSORS_Instance *sensor)
+{
+    PIOS_Assert(sensor);
+    if (!sensor->driver->get_queue) {
+        return NULL;
+    }
+    return sensor->driver->get_queue(sensor->context);
+}
+/**
+ * Get the sensor scales.
+ * @param sensor sensor instance
+ * @param scales float array that will contains scales
+ * @param size number of floats within the array
+ */
+static inline void PIOS_SENSORS_GetScales(const PIOS_SENSORS_Instance *sensor, float *scales, uint8_t size)
+{
+    PIOS_Assert(sensor);
+    sensor->driver->get_scale(scales, size, sensor->context);
+}
+/**
+ * return head of sensor list
+ * @return head of sensor list
+ */
+PIOS_SENSORS_Instance *PIOS_SENSORS_GetList();
+
+/**
+ * Return the first occurrence of specified sensor type
+ * @param previous_instance last instance found or 0
+ * @param type type of sensor to find
+ * @return the first occurence found or NULL
+ */
+PIOS_SENSORS_Instance *PIOS_SENSORS_GetInstanceByType(const PIOS_SENSORS_Instance *previous_instance, PIOS_SENSORS_TYPE type);
+
+#endif /* PIOS_SENSORS_H */

flight/pios/inc/pios_servo.h

@@ -30,9 +30,17 @@
 #ifndef PIOS_SERVO_H
 #define PIOS_SERVO_H
 
+/* Global types */
+enum pios_servo_bank_mode {
+    PIOS_SERVO_BANK_MODE_PWM = 0,
+    PIOS_SERVO_BANK_MODE_SINGLE_PULSE = 1
+};
 /* Public Functions */
-extern void PIOS_Servo_SetHz(const uint16_t *update_rates, uint8_t banks);
+extern void PIOS_Servo_SetHz(const uint16_t *speeds, const uint32_t *clock, uint8_t banks);
 extern void PIOS_Servo_Set(uint8_t Servo, uint16_t Position);
+extern void PIOS_Servo_Update();
+extern void PIOS_Servo_SetBankMode(uint8_t bank, uint8_t mode);
+extern uint8_t PIOS_Servo_GetPinBank(uint8_t pin);
 
 #endif /* PIOS_SERVO_H */
 

flight/pios/pios.h

@@ -34,6 +34,10 @@
 #ifndef PIOS_H
 #define PIOS_H
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 #include <pios_helpers.h>
 #include <pios_math.h>
 #include <pios_constants.h>
@@ -182,49 +186,6 @@
 #endif
 #endif
 
-/* PIOS sensor interfaces */
-#ifdef PIOS_INCLUDE_ADXL345
-/* ADXL345 3-Axis Accelerometer */
-#include <pios_adxl345.h>
-#endif
-
-#ifdef PIOS_INCLUDE_BMA180
-/* BMA180 3-Axis Accelerometer */
-#include <pios_bma180.h>
-#endif
-
-#ifdef PIOS_INCLUDE_L3GD20
-/* L3GD20 3-Axis Gyro */
-#include <pios_l3gd20.h>
-#endif
-
-#ifdef PIOS_INCLUDE_MPU6000
-/* MPU6000 3-Axis Gyro/Accelerometer */
-/* #define PIOS_MPU6000_ACCEL */
-#include <pios_mpu6000.h>
-#endif
-
-#ifdef PIOS_INCLUDE_HMC5843
-/* HMC5843 3-Axis Digital Compass */
-#include <pios_hmc5843.h>
-#endif
-
-#ifdef PIOS_INCLUDE_HMC5X83
-/* HMC5883/HMC5983 3-Axis Digital Compass */
-/* #define PIOS_HMC5x83_HAS_GPIOS */
-#include <pios_hmc5x83.h>
-#endif
-
-#ifdef PIOS_INCLUDE_BMP085
-/* BMP085 Barometric Pressure Sensor */
-#include <pios_bmp085.h>
-#endif
-
-#ifdef PIOS_INCLUDE_MS5611
-/* MS5611 Barometric Pressure Sensor */
-#include <pios_ms5611.h>
-#endif
-
 #ifdef PIOS_INCLUDE_MPXV
 /* MPXV5004, MPXV7002 based Airspeed Sensor */
 #include <pios_mpxv.h>
@@ -360,4 +321,10 @@
 /* #define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 995998 */
 
 #endif /* USE_SIM_POSIX */
+
+
+#ifdef __cplusplus
+} // closing brace for extern "C"
+#endif
+
 #endif /* PIOS_H */

flight/pios/posix/pios_servo.c

@@ -43,13 +43,13 @@ static volatile uint16_t ServoPosition[PIOS_SERVO_NUM_TIMERS];
  */
 void PIOS_Servo_Init(void)
 {}
-
 /**
  * Set the servo update rate (Max 500Hz)
- * \param[in] onetofour Rate for outputs 1 to 4 (Hz)
+ * \param[in] array of rates in Hz
- * \param[in] fivetoeight Rate for outputs 5 to 8 (Hz)
+ * \param[in] array of timer clocks in Hz
+ * \param[in] maximum number of banks
  */
-void PIOS_Servo_SetHz(const uint16_t *banks, uint8_t num_banks)
+void PIOS_Servo_SetHz(const uint16_t *speeds, const uint32_t *clock, uint8_t banks)
 {}
 
 /**

flight/pios/stm32f10x/pios_pwm.c

@@ -209,7 +209,9 @@ static void PIOS_PWM_tim_overflow_cb(__attribute__((unused)) uint32_t tim_id, ui
         /* Channel out of range */
         return;
     }
-
+    if (!pwm_dev->CaptureState[channel]) {
+        return;
+    }
     pwm_dev->us_since_update[channel] += count;
     if (pwm_dev->us_since_update[channel] >= PWM_SUPERVISOR_TIMEOUT) {
         pwm_dev->CaptureState[channel]    = 0;
@@ -256,16 +258,19 @@ static void PIOS_PWM_tim_edge_cb(__attribute__((unused)) uint32_t tim_id, uint32
         TIM_ICInitStructure.TIM_Channel    = chan->timer_chan;
         TIM_ICInit(chan->timer, &TIM_ICInitStructure);
     } else {
+        uint32_t value = 0;
         /* Capture computation */
-        if (pwm_dev->FallValue[chan_idx] > pwm_dev->RiseValue[chan_idx]) {
+        value = pwm_dev->us_since_update[chan_idx] + (pwm_dev->FallValue[chan_idx] - pwm_dev->RiseValue[chan_idx]);
-            pwm_dev->CaptureValue[chan_idx] = (pwm_dev->FallValue[chan_idx] - pwm_dev->RiseValue[chan_idx]);
+
-        } else {
+        // From time to time glitches happens. Most of the observed case are related to missing or spurious overflows
-            pwm_dev->CaptureValue[chan_idx] = ((chan->timer->ARR - pwm_dev->RiseValue[chan_idx]) + pwm_dev->FallValue[chan_idx]);
+        // happening when they are triggered very close each other.
+        // The following workaround prevents them to cause troubles by filtering unacceptable values
+        if (PIOS_PWM_VALID_RANGE_MAX > value && PIOS_PWM_VALID_RANGE_MIN < value) {
+            pwm_dev->CaptureValue[chan_idx] = value;
         }
-
         /* Switch states */
-        pwm_dev->CaptureState[chan_idx] = 0;
+        pwm_dev->CaptureState[chan_idx]    = 0;
-
+        pwm_dev->us_since_update[chan_idx] = 0;
         /* Increase supervisor counter */
         pwm_dev->CapCounter[chan_idx]++;
 

flight/pios/stm32f10x/pios_servo.c

@@ -39,6 +39,19 @@
 
 static const struct pios_servo_cfg *servo_cfg;
 
+// determine if the related timer will work in synchronous (or OneShot/OneShot125) One Pulse mode.
+static uint8_t pios_servo_bank_mode[PIOS_SERVO_BANKS] = { 0 };
+// used to skip updates when pulse length is higher than update cycle
+static uint16_t pios_servo_bank_next_update[PIOS_SERVO_BANKS] = { 0 };
+static uint16_t pios_servo_bank_max_pulse[PIOS_SERVO_BANKS] = { 0 };
+// timer associated to each bank
+static TIM_TypeDef *pios_servo_bank_timer[PIOS_SERVO_BANKS] = { 0 };
+
+// index of bank used for each pin
+static uint8_t *pios_servo_pin_bank;
+
+#define PIOS_SERVO_TIMER_CLOCK 1000000
+#define PIOS_SERVO_SAFE_MARGIN 50
 /**
  * Initialise Servos
  */
@@ -52,10 +65,42 @@ int32_t PIOS_Servo_Init(const struct pios_servo_cfg *cfg)
 
     /* Store away the requested configuration */
     servo_cfg = cfg;
+    pios_servo_pin_bank = pios_malloc(sizeof(uint8_t) * cfg->num_channels);
 
+    uint8_t bank = 0;
     /* Configure the channels to be in output compare mode */
     for (uint8_t i = 0; i < cfg->num_channels; i++) {
-        const struct pios_tim_channel *chan = &cfg->channels[i];
+        const struct pios_tim_channel *chan = &servo_cfg->channels[i];
+        bool new = true;
+        /* See if any previous channels use that same timer */
+        for (uint8_t j = 0; (j < i) && new; j++) {
+            new &= chan->timer != servo_cfg->channels[j].timer;
+        }
+
+        if (new) {
+            PIOS_Assert(bank < PIOS_SERVO_BANKS);
+            for (uint8_t j = i; j < servo_cfg->num_channels; j++) {
+                if (servo_cfg->channels[j].timer == chan->timer) {
+                    pios_servo_pin_bank[j] = bank;
+                }
+            }
+            pios_servo_bank_timer[bank] = chan->timer;
+
+            PIOS_Assert(bank < PIOS_SERVO_BANKS);
+
+            for (uint8_t j = i; j < servo_cfg->num_channels; j++) {
+                if (servo_cfg->channels[j].timer == chan->timer) {
+                    pios_servo_pin_bank[j] = bank;
+                }
+            }
+            pios_servo_bank_timer[bank] = chan->timer;
+
+            TIM_ARRPreloadConfig(chan->timer, ENABLE);
+            TIM_CtrlPWMOutputs(chan->timer, ENABLE);
+            TIM_Cmd(chan->timer, ENABLE);
+
+            bank++;
+        }
 
         /* Set up for output compare function */
         switch (chan->timer_chan) {
@@ -76,10 +121,6 @@ int32_t PIOS_Servo_Init(const struct pios_servo_cfg *cfg)
             TIM_OC4PreloadConfig(chan->timer, TIM_OCPreload_Enable);
             break;
         }
-
-        TIM_ARRPreloadConfig(chan->timer, ENABLE);
-        TIM_CtrlPWMOutputs(chan->timer, ENABLE);
-        TIM_Cmd(chan->timer, ENABLE);
     }
 
     return 0;
@@ -88,10 +129,12 @@ int32_t PIOS_Servo_Init(const struct pios_servo_cfg *cfg)
 /**
  * Set the servo update rate (Max 500Hz)
  * \param[in] array of rates in Hz
+ * \param[in] array of timer clocks in Hz
  * \param[in] maximum number of banks
  */
-void PIOS_Servo_SetHz(const uint16_t *speeds, uint8_t banks)
+void PIOS_Servo_SetHz(const uint16_t *speeds, const uint32_t *clock, uint8_t banks)
 {
+    PIOS_Assert(banks <= PIOS_SERVO_BANKS);
     if (!servo_cfg) {
         return;
     }
@@ -99,23 +142,18 @@ void PIOS_Servo_SetHz(const uint16_t *speeds, uint8_t banks)
     TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = servo_cfg->tim_base_init;
     TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
     TIM_TimeBaseStructure.TIM_CounterMode   = TIM_CounterMode_Up;
-    TIM_TimeBaseStructure.TIM_Prescaler     = (PIOS_MASTER_CLOCK / 1000000) - 1;
 
-    uint8_t set = 0;
+    for (uint8_t i = 0; i < banks && i < PIOS_SERVO_BANKS; i++) {
+        const TIM_TypeDef *timer = pios_servo_bank_timer[i];
+        if (timer) {
+            uint32_t new_clock = PIOS_SERVO_TIMER_CLOCK;
+            if (clock[i]) {
+                new_clock = clock[i];
+            }
+            TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_MASTER_CLOCK / new_clock) - 1;
+            TIM_TimeBaseStructure.TIM_Period    = ((new_clock / speeds[i]) - 1);
 
-    for (uint8_t i = 0; (i < servo_cfg->num_channels) && (set < banks); i++) {
+            TIM_TimeBaseInit((TIM_TypeDef *)timer, &TIM_TimeBaseStructure);
-        bool new = true;
-        const struct pios_tim_channel *chan = &servo_cfg->channels[i];
-
-        /* See if any previous channels use that same timer */
-        for (uint8_t j = 0; (j < i) && new; j++) {
-            new &= chan->timer != servo_cfg->channels[j].timer;
-        }
-
-        if (new) {
-            TIM_TimeBaseStructure.TIM_Period = ((1000000 / speeds[set]) - 1);
-            TIM_TimeBaseInit(chan->timer, &TIM_TimeBaseStructure);
-            set++;
         }
     }
 }
@@ -132,22 +170,114 @@ void PIOS_Servo_Set(uint8_t servo, uint16_t position)
         return;
     }
 
+
     /* Update the position */
     const struct pios_tim_channel *chan = &servo_cfg->channels[servo];
+    uint16_t val    = position;
+    uint16_t margin = chan->timer->ARR / 50; // Leave 2% of period as margin to prevent overlaps
+    if (val > (chan->timer->ARR - margin)) {
+        val = chan->timer->ARR - margin;
+    }
+
+    uint8_t bank = pios_servo_pin_bank[servo];
+    if (pios_servo_bank_max_pulse[bank] < val) {
+        pios_servo_bank_max_pulse[bank] = val;
+    }
     switch (chan->timer_chan) {
     case TIM_Channel_1:
-        TIM_SetCompare1(chan->timer, position);
+        TIM_SetCompare1(chan->timer, val);
         break;
     case TIM_Channel_2:
-        TIM_SetCompare2(chan->timer, position);
+        TIM_SetCompare2(chan->timer, val);
         break;
     case TIM_Channel_3:
-        TIM_SetCompare3(chan->timer, position);
+        TIM_SetCompare3(chan->timer, val);
         break;
     case TIM_Channel_4:
-        TIM_SetCompare4(chan->timer, position);
+        TIM_SetCompare4(chan->timer, val);
         break;
     }
 }
 
+void PIOS_Servo_Update()
+{
+    for (uint8_t i = 0; (i < PIOS_SERVO_BANKS); i++) {
+        const TIM_TypeDef *timer = pios_servo_bank_timer[i];
+        if (timer && pios_servo_bank_mode[i] == PIOS_SERVO_BANK_MODE_SINGLE_PULSE) {
+            // a pulse to be generated is longer than cycle period. skip this update.
+            if (TIM_GetCounter((TIM_TypeDef *)timer) > (uint32_t)(pios_servo_bank_next_update[i] + PIOS_SERVO_SAFE_MARGIN)) {
+                TIM_GenerateEvent((TIM_TypeDef *)timer, TIM_EventSource_Update);
+                pios_servo_bank_next_update[i] = pios_servo_bank_max_pulse[i];
+            }
+        }
+        pios_servo_bank_max_pulse[i] = 0;
+    }
+    for (uint8_t i = 0; (i < servo_cfg->num_channels); i++) {
+        uint8_t bank = pios_servo_pin_bank[i];
+        uint8_t mode = pios_servo_bank_mode[bank];
+        if (mode == PIOS_SERVO_BANK_MODE_SINGLE_PULSE) {
+            /* Update the position */
+            const struct pios_tim_channel *chan = &servo_cfg->channels[i];
+
+            switch (chan->timer_chan) {
+            case TIM_Channel_1:
+                TIM_SetCompare1(chan->timer, 0);
+                break;
+            case TIM_Channel_2:
+                TIM_SetCompare2(chan->timer, 0);
+                break;
+            case TIM_Channel_3:
+                TIM_SetCompare3(chan->timer, 0);
+                break;
+            case TIM_Channel_4:
+                TIM_SetCompare4(chan->timer, 0);
+                break;
+            }
+        }
+    }
+}
+
+void PIOS_Servo_SetBankMode(uint8_t bank, uint8_t mode)
+{
+    PIOS_Assert(bank < PIOS_SERVO_BANKS);
+    pios_servo_bank_mode[bank] = mode;
+
+    if (pios_servo_bank_timer[bank]) {
+        for (uint8_t i = 0; (i < servo_cfg->num_channels); i++) {
+            if (pios_servo_pin_bank[i] == bank) {
+                const struct pios_tim_channel *chan = &servo_cfg->channels[i];
+                /* Set up for output compare function */
+                switch (chan->timer_chan) {
+                case TIM_Channel_1:
+                    TIM_OC1PolarityConfig(chan->timer, TIM_OCPolarity_High);
+                    break;
+                case TIM_Channel_2:
+                    TIM_OC2PolarityConfig(chan->timer, TIM_OCPolarity_High);
+                    break;
+                case TIM_Channel_3:
+                    TIM_OC3PolarityConfig(chan->timer, TIM_OCPolarity_High);
+                    break;
+                case TIM_Channel_4:
+                    TIM_OC4PolarityConfig(chan->timer, TIM_OCPolarity_High);
+                    break;
+                }
+            }
+        }
+
+        // Setup the timer accordingly
+        TIM_SelectOnePulseMode(pios_servo_bank_timer[bank], TIM_OPMode_Repetitive);
+        TIM_Cmd(pios_servo_bank_timer[bank], ENABLE);
+    }
+}
+
+
+uint8_t PIOS_Servo_GetPinBank(uint8_t pin)
+{
+    if (pin < servo_cfg->num_channels) {
+        return pios_servo_pin_bank[pin];
+    } else {
+        return 0;
+    }
+}
+
 #endif /* PIOS_INCLUDE_SERVO */

flight/pios/stm32f10x/pios_tim.c

@@ -159,6 +159,16 @@ out_fail:
 
 static void PIOS_TIM_generic_irq_handler(TIM_TypeDef *timer)
 {
+    /* Check for an overflow event on this timer */
+    bool overflow_event     = 0;
+    uint16_t overflow_count = false;
+
+    if (TIM_GetITStatus(timer, TIM_IT_Update) == SET) {
+        TIM_ClearITPendingBit(timer, TIM_IT_Update);
+        overflow_count = timer->ARR;
+        overflow_event = true;
+    }
+
     /* Iterate over all registered clients of the TIM layer to find channels on this timer */
     for (uint8_t i = 0; i < pios_tim_num_devs; i++) {
         const struct pios_tim_dev *tim_dev = &pios_tim_devs[i];
@@ -168,18 +178,6 @@ static void PIOS_TIM_generic_irq_handler(TIM_TypeDef *timer)
             continue;
         }
 
-        /* Check for an overflow event on this timer */
-        bool overflow_event;
-        uint16_t overflow_count;
-        if (TIM_GetITStatus(timer, TIM_IT_Update) == SET) {
-            TIM_ClearITPendingBit(timer, TIM_IT_Update);
-            overflow_count = timer->ARR;
-            overflow_event = true;
-        } else {
-            overflow_count = 0;
-            overflow_event = false;
-        }
-
         for (uint8_t j = 0; j < tim_dev->num_channels; j++) {
             const struct pios_tim_channel *chan = &tim_dev->channels[j];
 
@@ -251,11 +249,11 @@ static void PIOS_TIM_generic_irq_handler(TIM_TypeDef *timer)
                  * get the order wrong, our pulse width calculations could be off by up
                  * to ARR ticks.  That could be bad.
                  *
-                 * Heuristic: If the edge_count is < 16 ticks above zero then we assume the
+                 * Heuristic: If the edge_count is < 32 ticks above zero then we assume the
                  *            edge happened just after the overflow.
                  */
 
-                if (edge_count < 16) {
+                if (edge_count < 32) {
                     /* Call the overflow callback first */
                     if (tim_dev->callbacks->overflow) {
                         (*tim_dev->callbacks->overflow)((uint32_t)tim_dev,
@@ -300,67 +298,6 @@ static void PIOS_TIM_generic_irq_handler(TIM_TypeDef *timer)
         }
     }
 }
-#if 0
-uint16_t val = 0;
-for (uint8_t i = 0; i < pios_pwm_cfg.num_channels; i++) {
-    struct pios_pwm_channel channel = pios_pwm_cfg.channels[i];
-    if ((channel.timer == timer) && (TIM_GetITStatus(channel.timer, channel.ccr) == SET)) {
-        TIM_ClearITPendingBit(channel.timer, channel.ccr);
-
-        switch (channel.channel) {
-        case TIM_Channel_1:
-            val = TIM_GetCapture1(channel.timer);
-            break;
-        case TIM_Channel_2:
-            val = TIM_GetCapture2(channel.timer);
-            break;
-        case TIM_Channel_3:
-            val = TIM_GetCapture3(channel.timer);
-            break;
-        case TIM_Channel_4:
-            val = TIM_GetCapture4(channel.timer);
-            break;
-        }
-
-        if (CaptureState[i] == 0) {
-            RiseValue[i] = val;
-        } else {
-            FallValue[i] = val;
-        }
-
-        // flip state machine and capture value here
-        /* Simple rise or fall state machine */
-        TIM_ICInitTypeDef TIM_ICInitStructure = pios_pwm_cfg.tim_ic_init;
-        if (CaptureState[i] == 0) {
-            /* Switch states */
-            CaptureState[i] = 1;
-
-            /* Switch polarity of input capture */
-            TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling;
-            TIM_ICInitStructure.TIM_Channel    = channel.channel;
-            TIM_ICInit(channel.timer, &TIM_ICInitStructure);
-        } else {
-            /* Capture computation */
-            if (FallValue[i] > RiseValue[i]) {
-                CaptureValue[i] = (FallValue[i] - RiseValue[i]);
-            } else {
-                CaptureValue[i] = ((channel.timer->ARR - RiseValue[i]) + FallValue[i]);
-            }
-
-            /* Switch states */
-            CaptureState[i] = 0;
-
-            /* Increase supervisor counter */
-            CapCounter[i]++;
-
-            /* Switch polarity of input capture */
-            TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
-            TIM_ICInitStructure.TIM_Channel    = channel.channel;
-            TIM_ICInit(channel.timer, &TIM_ICInitStructure);
-        }
-    }
-}
-#endif /* if 0 */
 
 /* Bind Interrupt Handlers
  *

flight/pios/stm32f4xx/pios_servo.c

@@ -39,6 +39,19 @@
 
 static const struct pios_servo_cfg *servo_cfg;
 
+// determine if the related timer will work in synchronous (or OneShot/OneShot125) One Pulse mode.
+static uint8_t pios_servo_bank_mode[PIOS_SERVO_BANKS] = { 0 };
+// used to skip updates when pulse length is higher than update cycle
+static uint16_t pios_servo_bank_next_update[PIOS_SERVO_BANKS] = { 0 };
+static uint16_t pios_servo_bank_max_pulse[PIOS_SERVO_BANKS] = { 0 };
+// timer associated to each bank
+static TIM_TypeDef *pios_servo_bank_timer[PIOS_SERVO_BANKS] = { 0 };
+
+// index of bank used for each pin
+static uint8_t *pios_servo_pin_bank;
+
+#define PIOS_SERVO_TIMER_CLOCK 1000000
+#define PIOS_SERVO_SAFE_MARGIN 50
 /**
  * Initialise Servos
  */
@@ -52,46 +65,137 @@ int32_t PIOS_Servo_Init(const struct pios_servo_cfg *cfg)
 
     /* Store away the requested configuration */
     servo_cfg = cfg;
+    pios_servo_pin_bank = pios_malloc(sizeof(uint8_t) * cfg->num_channels);
 
-    /* Configure the channels to be in output compare mode */
+    uint8_t bank = 0;
-    for (uint8_t i = 0; i < cfg->num_channels; i++) {
+    for (uint8_t i = 0; (i < servo_cfg->num_channels); i++) {
-        const struct pios_tim_channel *chan = &cfg->channels[i];
+        const struct pios_tim_channel *chan = &servo_cfg->channels[i];
+        bool new = true;
+        /* See if any previous channels use that same timer */
+        for (uint8_t j = 0; (j < i) && new; j++) {
+            new &= chan->timer != servo_cfg->channels[j].timer;
+        }
+
+        if (new) {
+            PIOS_Assert(bank < PIOS_SERVO_BANKS);
+            for (uint8_t j = i; j < servo_cfg->num_channels; j++) {
+                if (servo_cfg->channels[j].timer == chan->timer) {
+                    pios_servo_pin_bank[j] = bank;
+                }
+            }
+            pios_servo_bank_timer[bank] = chan->timer;
+
+            TIM_ARRPreloadConfig(chan->timer, ENABLE);
+            TIM_CtrlPWMOutputs(chan->timer, ENABLE);
+            TIM_Cmd(chan->timer, DISABLE);
+
+            bank++;
+        }
 
         /* Set up for output compare function */
         switch (chan->timer_chan) {
         case TIM_Channel_1:
-            TIM_OC1Init(chan->timer, &cfg->tim_oc_init);
+            TIM_OC1Init(chan->timer, &servo_cfg->tim_oc_init);
             TIM_OC1PreloadConfig(chan->timer, TIM_OCPreload_Enable);
             break;
         case TIM_Channel_2:
-            TIM_OC2Init(chan->timer, &cfg->tim_oc_init);
+            TIM_OC2Init(chan->timer, &servo_cfg->tim_oc_init);
             TIM_OC2PreloadConfig(chan->timer, TIM_OCPreload_Enable);
             break;
         case TIM_Channel_3:
-            TIM_OC3Init(chan->timer, &cfg->tim_oc_init);
+            TIM_OC3Init(chan->timer, &servo_cfg->tim_oc_init);
             TIM_OC3PreloadConfig(chan->timer, TIM_OCPreload_Enable);
             break;
         case TIM_Channel_4:
-            TIM_OC4Init(chan->timer, &cfg->tim_oc_init);
+            TIM_OC4Init(chan->timer, &servo_cfg->tim_oc_init);
             TIM_OC4PreloadConfig(chan->timer, TIM_OCPreload_Enable);
             break;
         }
-
-        TIM_ARRPreloadConfig(chan->timer, ENABLE);
-        TIM_CtrlPWMOutputs(chan->timer, ENABLE);
-        TIM_Cmd(chan->timer, ENABLE);
     }
 
     return 0;
 }
 
+void PIOS_Servo_SetBankMode(uint8_t bank, uint8_t mode)
+{
+    PIOS_Assert(bank < PIOS_SERVO_BANKS);
+    pios_servo_bank_mode[bank] = mode;
+
+    if (pios_servo_bank_timer[bank]) {
+        for (uint8_t i = 0; (i < servo_cfg->num_channels); i++) {
+            if (pios_servo_pin_bank[i] == bank) {
+                const struct pios_tim_channel *chan = &servo_cfg->channels[i];
+                /* Set up for output compare function */
+                switch (chan->timer_chan) {
+                case TIM_Channel_1:
+                    TIM_OC1PolarityConfig(chan->timer, TIM_OCPolarity_High);
+                    break;
+                case TIM_Channel_2:
+                    TIM_OC2PolarityConfig(chan->timer, TIM_OCPolarity_High);
+                    break;
+                case TIM_Channel_3:
+                    TIM_OC3PolarityConfig(chan->timer, TIM_OCPolarity_High);
+                    break;
+                case TIM_Channel_4:
+                    TIM_OC4PolarityConfig(chan->timer, TIM_OCPolarity_High);
+                    break;
+                }
+            }
+        }
+
+        // Setup the timer accordingly
+        TIM_SelectOnePulseMode(pios_servo_bank_timer[bank], TIM_OPMode_Repetitive);
+        TIM_Cmd(pios_servo_bank_timer[bank], ENABLE);
+    }
+}
+
+
+void PIOS_Servo_Update()
+{
+    for (uint8_t i = 0; (i < PIOS_SERVO_BANKS); i++) {
+        const TIM_TypeDef *timer = pios_servo_bank_timer[i];
+        if (timer && pios_servo_bank_mode[i] == PIOS_SERVO_BANK_MODE_SINGLE_PULSE) {
+            // a pulse to be generated is longer than cycle period. skip this update.
+            if (TIM_GetCounter((TIM_TypeDef *)timer) > (uint32_t)(pios_servo_bank_next_update[i] + PIOS_SERVO_SAFE_MARGIN)) {
+                TIM_GenerateEvent((TIM_TypeDef *)timer, TIM_EventSource_Update);
+                pios_servo_bank_next_update[i] = pios_servo_bank_max_pulse[i];
+            }
+        }
+        pios_servo_bank_max_pulse[i] = 0;
+    }
+    for (uint8_t i = 0; (i < servo_cfg->num_channels); i++) {
+        uint8_t bank = pios_servo_pin_bank[i];
+        uint8_t mode = pios_servo_bank_mode[bank];
+        if (mode == PIOS_SERVO_BANK_MODE_SINGLE_PULSE) {
+            /* Update the position */
+            const struct pios_tim_channel *chan = &servo_cfg->channels[i];
+
+            switch (chan->timer_chan) {
+            case TIM_Channel_1:
+                TIM_SetCompare1(chan->timer, 0);
+                break;
+            case TIM_Channel_2:
+                TIM_SetCompare2(chan->timer, 0);
+                break;
+            case TIM_Channel_3:
+                TIM_SetCompare3(chan->timer, 0);
+                break;
+            case TIM_Channel_4:
+                TIM_SetCompare4(chan->timer, 0);
+                break;
+            }
+        }
+    }
+}
 /**
  * Set the servo update rate (Max 500Hz)
  * \param[in] array of rates in Hz
+ * \param[in] array of timer clocks in Hz
  * \param[in] maximum number of banks
  */
-void PIOS_Servo_SetHz(const uint16_t *speeds, uint8_t banks)
+void PIOS_Servo_SetHz(const uint16_t *speeds, const uint32_t *clock, uint8_t banks)
 {
+    PIOS_Assert(banks <= PIOS_SERVO_BANKS);
     if (!servo_cfg) {
         return;
     }
@@ -99,30 +203,22 @@ void PIOS_Servo_SetHz(const uint16_t *speeds, uint8_t banks)
     TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = servo_cfg->tim_base_init;
     TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
     TIM_TimeBaseStructure.TIM_CounterMode   = TIM_CounterMode_Up;
-    //
 
-    uint8_t set = 0;
+    for (uint8_t i = 0; i < banks && i < PIOS_SERVO_BANKS; i++) {
-
+        const TIM_TypeDef *timer = pios_servo_bank_timer[i];
-    for (uint8_t i = 0; (i < servo_cfg->num_channels) && (set < banks); i++) {
+        if (timer) {
-        bool new = true;
+            uint32_t new_clock = PIOS_SERVO_TIMER_CLOCK;
-        const struct pios_tim_channel *chan = &servo_cfg->channels[i];
+            if (clock[i]) {
-
+                new_clock = clock[i];
-        /* See if any previous channels use that same timer */
-        for (uint8_t j = 0; (j < i) && new; j++) {
-            new &= chan->timer != servo_cfg->channels[j].timer;
-        }
-
-        if (new) {
-            // Choose the correct prescaler value for the APB the timer is attached
-            if (chan->timer == TIM1 || chan->timer == TIM8 || chan->timer == TIM9 || chan->timer == TIM10 || chan->timer == TIM11) {
-                TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / 1000000) - 1;
-            } else {
-                TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_PERIPHERAL_APB1_CLOCK / 1000000) - 1;
             }
-
+            // Choose the correct prescaler value for the APB the timer is attached
-            TIM_TimeBaseStructure.TIM_Period = ((1000000 / speeds[set]) - 1);
+            if (timer == TIM1 || timer == TIM8 || timer == TIM9 || timer == TIM10 || timer == TIM11) {
-            TIM_TimeBaseInit(chan->timer, &TIM_TimeBaseStructure);
+                TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / new_clock) - 1;
-            set++;
+            } else {
+                TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_PERIPHERAL_APB1_CLOCK / new_clock) - 1;
+            }
+            TIM_TimeBaseStructure.TIM_Period = ((new_clock / speeds[i]) - 1);
+            TIM_TimeBaseInit((TIM_TypeDef *)timer, &TIM_TimeBaseStructure);
         }
     }
 }
@@ -139,22 +235,42 @@ void PIOS_Servo_Set(uint8_t servo, uint16_t position)
         return;
     }
 
+
     /* Update the position */
     const struct pios_tim_channel *chan = &servo_cfg->channels[servo];
+    uint16_t val    = position;
+    uint16_t margin = chan->timer->ARR / 50; // Leave 2% of period as margin to prevent overlaps
+    if (val > (chan->timer->ARR - margin)) {
+        val = chan->timer->ARR - margin;
+    }
+
+    uint8_t bank = pios_servo_pin_bank[servo];
+    if (pios_servo_bank_max_pulse[bank] < val) {
+        pios_servo_bank_max_pulse[bank] = val;
+    }
     switch (chan->timer_chan) {
     case TIM_Channel_1:
-        TIM_SetCompare1(chan->timer, position);
+        TIM_SetCompare1(chan->timer, val);
         break;
     case TIM_Channel_2:
-        TIM_SetCompare2(chan->timer, position);
+        TIM_SetCompare2(chan->timer, val);
         break;
     case TIM_Channel_3:
-        TIM_SetCompare3(chan->timer, position);
+        TIM_SetCompare3(chan->timer, val);
         break;
     case TIM_Channel_4:
-        TIM_SetCompare4(chan->timer, position);
+        TIM_SetCompare4(chan->timer, val);
         break;
     }
 }
 
+uint8_t PIOS_Servo_GetPinBank(uint8_t pin)
+{
+    if (pin < servo_cfg->num_channels) {
+        return pios_servo_pin_bank[pin];
+    } else {
+        return 0;
+    }
+}
+
 #endif /* PIOS_INCLUDE_SERVO */

flight/pios/stm32f4xx/startup.c

@@ -29,6 +29,8 @@
 #include <string.h>
 #include <stm32f4xx.h>
 
+extern int __libc_init_array(void);
+
 /* prototype for main() that tells us not to worry about it possibly returning */
 extern int              main(void) __attribute__((noreturn));
 
@@ -91,6 +93,10 @@ void _main(void)
     /* leave a little space at the top in case memset() isn't a leaf with no locals */
     memset(&irq_stack, 0xa5, sizeof(irq_stack) - 64);
 
+#ifdef PIOS_ENABLE_CXX
+    __libc_init_array();
+#endif
+
     /* call main */
     (void)main();
 }

flight/targets/boards/coptercontrol/board_hw_defs.c

@@ -1247,6 +1247,18 @@ const struct pios_ppm_cfg pios_ppm_cfg = {
     .num_channels = 1,
 };
 
+const struct pios_ppm_cfg pios_ppm_pin8_cfg = {
+    .tim_ic_init         = {
+        .TIM_ICPolarity  = TIM_ICPolarity_Rising,
+        .TIM_ICSelection = TIM_ICSelection_DirectTI,
+        .TIM_ICPrescaler = TIM_ICPSC_DIV1,
+        .TIM_ICFilter    = 0x0,
+    },
+    /* Use only the first channel for ppm */
+    .channels     = &pios_tim_rcvrport_all_channels[5],
+    .num_channels = 1,
+};
+
 #endif /* PIOS_INCLUDE_PPM */
 
 #if defined(PIOS_INCLUDE_PPM_FLEXI)

flight/targets/boards/coptercontrol/firmware/Makefile

@@ -30,6 +30,12 @@ USE_DSP_LIB ?= NO
 # Set to YES to build a FW version that will erase data flash memory
 ERASE_FLASH ?= NO
 
+# Set to yes to include Gcsreceiver module
+GCSRECEIVER ?= NO
+
+# Enable Diag tasks ?
+DIAG_TASKS ?= NO
+
 # List of mandatory modules to include
 MODULES += Attitude
 MODULES += Stabilization
@@ -103,7 +109,6 @@ ifndef TESTAPP
     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
@@ -116,15 +121,23 @@ ifndef TESTAPP
     SRC += $(OPUAVSYNTHDIR)/gpsvelocitysensor.c
     SRC += $(OPUAVSYNTHDIR)/gpssettings.c
     SRC += $(OPUAVSYNTHDIR)/hwsettings.c
-    SRC += $(OPUAVSYNTHDIR)/gcsreceiver.c
     SRC += $(OPUAVSYNTHDIR)/receiveractivity.c
-    SRC += $(OPUAVSYNTHDIR)/taskinfo.c
-    SRC += $(OPUAVSYNTHDIR)/callbackinfo.c
     SRC += $(OPUAVSYNTHDIR)/mixerstatus.c
     SRC += $(OPUAVSYNTHDIR)/ratedesired.c
     SRC += $(OPUAVSYNTHDIR)/txpidsettings.c
     SRC += $(OPUAVSYNTHDIR)/mpu6000settings.c
-    SRC += $(OPUAVSYNTHDIR)/perfcounter.c
+    # Command line option for Gcsreceiver module
+    ifeq ($(GCSRECEIVER), YES)
+        SRC += $(OPUAVSYNTHDIR)/gcsreceiver.c
+    endif
+    # Enable Diag tasks and UAVOs needed
+    ifeq ($(DIAG_TASKS), YES)
+        CDEFS += -DDIAG_TASKS
+        SRC += $(OPUAVSYNTHDIR)/taskinfo.c
+        SRC += $(OPUAVSYNTHDIR)/callbackinfo.c
+        SRC += $(OPUAVSYNTHDIR)/perfcounter.c
+        SRC += $(OPUAVSYNTHDIR)/i2cstats.c
+    endif
 else
     ## Test Code
     SRC += $(OPTESTS)/test_common.c

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

@@ -97,10 +97,10 @@
 /* PIOS receiver drivers */
 #define PIOS_INCLUDE_PWM
 #define PIOS_INCLUDE_PPM
-#define PIOS_INCLUDE_PPM_FLEXI
+/* #define PIOS_INCLUDE_PPM_FLEXI */
 #define PIOS_INCLUDE_DSM
 #define PIOS_INCLUDE_SBUS
-#define PIOS_INCLUDE_GCSRCVR
+/* #define PIOS_INCLUDE_GCSRCVR */
 /* #define PIOS_INCLUDE_OPLINKRCVR */
 
 /* PIOS abstract receiver interface */
@@ -142,7 +142,7 @@
 /* #define PIOS_TELEM_PRIORITY_QUEUE */
 #define PIOS_INCLUDE_GPS
 #define PIOS_GPS_MINIMAL
-#define PIOS_INCLUDE_GPS_NMEA_PARSER
+/* #define PIOS_INCLUDE_GPS_NMEA_PARSER */
 #define PIOS_INCLUDE_GPS_UBX_PARSER
 /* #define PIOS_GPS_SETS_HOMELOCATION */
 
@@ -162,7 +162,7 @@
 
 /* Task stack sizes */
 #define PIOS_ACTUATOR_STACK_SIZE        820
-#define PIOS_MANUAL_STACK_SIZE          635
+#define PIOS_MANUAL_STACK_SIZE          735
 #define PIOS_RECEIVER_STACK_SIZE        620
 #define PIOS_STABILIZATION_STACK_SIZE   400
 

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

@@ -32,10 +32,16 @@
 #include <manualcontrolsettings.h>
 #include <gcsreceiver.h>
 #include <taskinfo.h>
+#include <sanitycheck.h>
+#include <actuatorsettings.h>
 
 #ifdef PIOS_INCLUDE_INSTRUMENTATION
 #include <pios_instrumentation.h>
 #endif
+#if defined(PIOS_INCLUDE_ADXL345)
+#include <pios_adxl345.h>
+#endif
+
 /*
  * Pull in the board-specific static HW definitions.
  * Including .c files is a bit ugly but this allows all of
@@ -52,6 +58,9 @@
  */
 uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE];
 
+static SystemAlarmsExtendedAlarmStatusOptions CopterControlConfigHook();
+static void ActuatorSettingsUpdatedCb(UAVObjEvent *ev);
+
 #define PIOS_COM_TELEM_RF_RX_BUF_LEN     32
 #define PIOS_COM_TELEM_RF_TX_BUF_LEN     12
 
@@ -232,8 +241,10 @@ void PIOS_Board_Init(void)
     HwSettingsInitialize();
 
 #ifndef ERASE_FLASH
+#ifdef PIOS_INCLUDE_WDG
     /* Initialize watchdog as early as possible to catch faults during init */
     PIOS_WDG_Init();
+#endif
 #endif
 
     /* Initialize the alarms library */
@@ -712,8 +723,8 @@ void PIOS_Board_Init(void)
     uint8_t hwsettings_rcvrport;
     HwSettingsCC_RcvrPortGet(&hwsettings_rcvrport);
 
-    switch (hwsettings_rcvrport) {
+    switch ((HwSettingsCC_RcvrPortOptions)hwsettings_rcvrport) {
-    case HWSETTINGS_CC_RCVRPORT_DISABLED:
+    case HWSETTINGS_CC_RCVRPORT_DISABLEDONESHOT:
 #if defined(PIOS_INCLUDE_HCSR04)
         {
             uint32_t pios_hcsr04_id;
@@ -721,7 +732,7 @@ void PIOS_Board_Init(void)
         }
 #endif
         break;
-    case HWSETTINGS_CC_RCVRPORT_PWM:
+    case HWSETTINGS_CC_RCVRPORT_PWMNOONESHOT:
 #if defined(PIOS_INCLUDE_PWM)
         {
             uint32_t pios_pwm_id;
@@ -735,12 +746,17 @@ void PIOS_Board_Init(void)
         }
 #endif /* PIOS_INCLUDE_PWM */
         break;
-    case HWSETTINGS_CC_RCVRPORT_PPM:
+    case HWSETTINGS_CC_RCVRPORT_PPMNOONESHOT:
-    case HWSETTINGS_CC_RCVRPORT_PPMOUTPUTS:
+    case HWSETTINGS_CC_RCVRPORT_PPMOUTPUTSNOONESHOT:
+    case HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT:
 #if defined(PIOS_INCLUDE_PPM)
         {
             uint32_t pios_ppm_id;
-            PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg);
+            if (hwsettings_rcvrport == HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT) {
+                PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_pin8_cfg);
+            } else {
+                PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg);
+            }
 
             uint32_t pios_ppm_rcvr_id;
             if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) {
@@ -750,7 +766,7 @@ void PIOS_Board_Init(void)
         }
 #endif /* PIOS_INCLUDE_PPM */
         break;
-    case HWSETTINGS_CC_RCVRPORT_PPMPWM:
+    case HWSETTINGS_CC_RCVRPORT_PPMPWMNOONESHOT:
         /* This is a combination of PPM and PWM inputs */
 #if defined(PIOS_INCLUDE_PPM)
         {
@@ -777,6 +793,8 @@ void PIOS_Board_Init(void)
         }
 #endif /* PIOS_INCLUDE_PWM */
         break;
+    case HWSETTINGS_CC_RCVRPORT_OUTPUTSONESHOT:
+        break;
     }
 
 #if defined(PIOS_INCLUDE_GCSRCVR)
@@ -794,15 +812,16 @@ void PIOS_Board_Init(void)
     GPIO_PinRemapConfig(GPIO_Remap_SWJ_NoJTRST, ENABLE);
 
 #ifndef PIOS_ENABLE_DEBUG_PINS
-    switch (hwsettings_rcvrport) {
+    switch ((HwSettingsCC_RcvrPortOptions)hwsettings_rcvrport) {
-    case HWSETTINGS_CC_RCVRPORT_DISABLED:
+    case HWSETTINGS_CC_RCVRPORT_DISABLEDONESHOT:
-    case HWSETTINGS_CC_RCVRPORT_PWM:
+    case HWSETTINGS_CC_RCVRPORT_PWMNOONESHOT:
-    case HWSETTINGS_CC_RCVRPORT_PPM:
+    case HWSETTINGS_CC_RCVRPORT_PPMNOONESHOT:
-    case HWSETTINGS_CC_RCVRPORT_PPMPWM:
+    case HWSETTINGS_CC_RCVRPORT_PPMPWMNOONESHOT:
+    case HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT:
         PIOS_Servo_Init(&pios_servo_cfg);
         break;
-    case HWSETTINGS_CC_RCVRPORT_PPMOUTPUTS:
+    case HWSETTINGS_CC_RCVRPORT_PPMOUTPUTSNOONESHOT:
-    case HWSETTINGS_CC_RCVRPORT_OUTPUTS:
+    case HWSETTINGS_CC_RCVRPORT_OUTPUTSONESHOT:
         PIOS_Servo_Init(&pios_servo_rcvr_cfg);
         break;
     }
@@ -831,7 +850,7 @@ void PIOS_Board_Init(void)
         }
         PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg);
         PIOS_MPU6000_CONFIG_Configure();
-        init_test = PIOS_MPU6000_Test();
+        init_test = !PIOS_MPU6000_Driver.test(0);
 #endif /* PIOS_INCLUDE_MPU6000 */
 
         break;
@@ -841,6 +860,59 @@ void PIOS_Board_Init(void)
 
     /* Make sure we have at least one telemetry link configured or else fail initialization */
     PIOS_Assert(pios_com_telem_rf_id || pios_com_telem_usb_id);
+
+    // Attach the board config check hook
+    SANITYCHECK_AttachHook(&CopterControlConfigHook);
+    // trigger a config check if actuatorsettings are updated
+    ActuatorSettingsInitialize();
+    ActuatorSettingsConnectCallback(ActuatorSettingsUpdatedCb);
+}
+
+SystemAlarmsExtendedAlarmStatusOptions CopterControlConfigHook()
+{
+    // inhibit usage of oneshot for non supported RECEIVER port modes
+    uint8_t recmode;
+
+    HwSettingsCC_RcvrPortGet(&recmode);
+    uint8_t flexiMode;
+    uint8_t modes[ACTUATORSETTINGS_BANKMODE_NUMELEM];
+    ActuatorSettingsBankModeGet(modes);
+    HwSettingsCC_FlexiPortGet(&flexiMode);
+
+    switch ((HwSettingsCC_RcvrPortOptions)recmode) {
+    // Those modes allows oneshot usage
+    case HWSETTINGS_CC_RCVRPORT_DISABLEDONESHOT:
+    case HWSETTINGS_CC_RCVRPORT_OUTPUTSONESHOT:
+    case HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT:
+        if ((recmode == HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT ||
+             flexiMode == HWSETTINGS_CC_FLEXIPORT_PPM) &&
+            (modes[3] == ACTUATORSETTINGS_BANKMODE_PWMSYNC ||
+             modes[3] == ACTUATORSETTINGS_BANKMODE_ONESHOT125)) {
+            return SYSTEMALARMS_EXTENDEDALARMSTATUS_UNSUPPORTEDCONFIG_ONESHOT;
+        } else {
+            return SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE;
+        }
+
+    // inhibit oneshot for the following modes
+    case HWSETTINGS_CC_RCVRPORT_PPMNOONESHOT:
+    case HWSETTINGS_CC_RCVRPORT_PPMOUTPUTSNOONESHOT:
+    case HWSETTINGS_CC_RCVRPORT_PPMPWMNOONESHOT:
+    case HWSETTINGS_CC_RCVRPORT_PWMNOONESHOT:
+        for (uint8_t i = 0; i < ACTUATORSETTINGS_BANKMODE_NUMELEM; i++) {
+            if (modes[i] == ACTUATORSETTINGS_BANKMODE_PWMSYNC ||
+                modes[i] == ACTUATORSETTINGS_BANKMODE_ONESHOT125) {
+                return SYSTEMALARMS_EXTENDEDALARMSTATUS_UNSUPPORTEDCONFIG_ONESHOT;;
+            }
+
+            return SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE;
+        }
+    }
+    return SYSTEMALARMS_EXTENDEDALARMSTATUS_UNSUPPORTEDCONFIG_ONESHOT;;
+}
+// trigger a configuration check if ActuatorSettings are changed.
+void ActuatorSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+{
+    configuration_check();
 }
 
 /**

flight/targets/boards/coptercontrol/pios_board.h

@@ -251,6 +251,7 @@ extern uint32_t pios_com_hkosd_id;
 // -------------------------
 #define PIOS_SERVO_UPDATE_HZ         50
 #define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */
+#define PIOS_SERVO_BANKS             6
 
 // --------------------------
 // Timer controller settings

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

@@ -747,11 +747,11 @@ void PIOS_Board_Init(void)
         }
 
         /* Set the radio configuration parameters. */
-        PIOS_RFM22B_SetChannelConfig(pios_rfm22b_id, datarate, oplinkSettings.MinChannel, oplinkSettings.MaxChannel, oplinkSettings.ChannelSet, is_coordinator, is_oneway, ppm_mode, ppm_only);
+        PIOS_RFM22B_SetChannelConfig(pios_rfm22b_id, datarate, oplinkSettings.MinChannel, oplinkSettings.MaxChannel, is_coordinator, is_oneway, ppm_mode, ppm_only);
         PIOS_RFM22B_SetCoordinatorID(pios_rfm22b_id, oplinkSettings.CoordID);
 
         /* Set the PPM callback if we should be receiving PPM. */
-        if (ppm_mode) {
+        if (ppm_mode || (ppm_only && !is_coordinator)) {
             PIOS_RFM22B_SetPPMCallback(pios_rfm22b_id, PIOS_Board_PPM_callback);
         }
 

flight/targets/boards/discoveryf4bare/pios_board.h

@@ -248,6 +248,7 @@ extern uint32_t pios_packet_handler;
 // -------------------------
 #define PIOS_SERVO_UPDATE_HZ         50
 #define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */
+#define PIOS_SERVO_BANKS             6
 
 // --------------------------
 // Timer controller settings

flight/targets/boards/gpsplatinum/board_hw_defs.c

@@ -205,8 +205,9 @@ void PIOS_SPI_mag_flash_irq_handler(void)
 #endif /* PIOS_INCLUDE_FLASH */
 
 #if defined(PIOS_INCLUDE_HMC5X83)
-pios_hmc5x83_dev_t onboard_mag;
 #include "pios_hmc5x83.h"
+pios_hmc5x83_dev_t onboard_mag;
+
 #ifdef PIOS_HMC5X83_HAS_GPIOS
 bool pios_board_mag_handler()
 {

flight/targets/boards/oplinkmini/firmware/Makefile

@@ -48,6 +48,9 @@ ifndef TESTAPP
     SRC += $(OPUAVOBJ)/uavobjectpersistence.c
     SRC += $(OPUAVOBJ)/eventdispatcher.c
 
+    ## Misc library functions
+    SRC += $(FLIGHTLIB)/sha1.c
+
     ## UAVObjects
     SRC += $(OPUAVSYNTHDIR)/oplinkstatus.c
     SRC += $(OPUAVSYNTHDIR)/oplinksettings.c

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

@@ -438,11 +438,11 @@ void PIOS_Board_Init(void)
         }
 
         // Set the radio configuration parameters.
-        PIOS_RFM22B_SetChannelConfig(pios_rfm22b_id, datarate, oplinkSettings.MinChannel, oplinkSettings.MaxChannel, oplinkSettings.ChannelSet, is_coordinator, is_oneway, ppm_mode, ppm_only);
         PIOS_RFM22B_SetCoordinatorID(pios_rfm22b_id, oplinkSettings.CoordID);
+        PIOS_RFM22B_SetChannelConfig(pios_rfm22b_id, datarate, oplinkSettings.MinChannel, oplinkSettings.MaxChannel, is_coordinator, is_oneway, ppm_mode, ppm_only);
 
         /* Set the PPM callback if we should be receiving PPM. */
-        if (ppm_mode) {
+        if (ppm_mode || (ppm_only && !is_coordinator)) {
             PIOS_RFM22B_SetPPMCallback(pios_rfm22b_id, PIOS_Board_PPM_callback);
         }
 

flight/targets/boards/oplinkmini/pios_board.h

@@ -260,6 +260,7 @@ extern uint32_t pios_ppm_out_id;
 // -------------------------
 #define PIOS_SERVO_UPDATE_HZ         50
 #define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */
+#define PIOS_SERVO_BANKS             6
 
 // --------------------------
 // Timer controller settings

flight/targets/boards/osd/firmware/Makefile

@@ -31,7 +31,7 @@ MODULES += Osd/osdgen
 MODULES += Osd/osdinput
 MODULES += Osd/WavPlayer
 MODULES += GPS
-MODULES += Extensions/MagBaro
+#MODULES += Extensions/MagBaro
 MODULES += FirmwareIAP
 MODULES += Telemetry
 

flight/targets/boards/osd/pios_board.h

@@ -247,6 +247,8 @@ extern uint32_t pios_com_telem_usb_id;
 // --------------------------
 #define PIOS_TIM_MAX_DEVS     6
 
+#define PIOS_SERVO_BANKS      6
+
 // -------------------------
 // USB
 // -------------------------

flight/targets/boards/revolution/firmware/Makefile

@@ -24,16 +24,16 @@ endif
 include ../board-info.mk
 include $(ROOT_DIR)/make/firmware-defs.mk
 
+# REVO C++ support 
+USE_CXX = YES
+
 # ARM DSP library
 USE_DSP_LIB ?= NO
 
 # List of mandatory modules to include
 MODULES += Sensors
-#MODULES += Attitude/revolution
+MODULES += StateEstimation
-MODULES += StateEstimation # use instead of Attitude
-MODULES += Altitude/revolution
 MODULES += Airspeed
-#MODULES += AltitudeHold # now integrated in Stabilization
 MODULES += Stabilization
 MODULES += ManualControl
 MODULES += Receiver
@@ -70,7 +70,7 @@ ifndef TESTAPP
     ## Application Core
     SRC += ../pios_usb_board_data.c
     SRC += $(OPMODULEDIR)/System/systemmod.c
-    SRC += $(OPSYSTEM)/revolution.c
+    CPPSRC += $(OPSYSTEM)/revolution.cpp
     SRC += $(OPSYSTEM)/pios_board.c
     SRC += $(FLIGHTLIB)/alarms.c
     SRC += $(FLIGHTLIB)/instrumentation.c
@@ -91,6 +91,8 @@ ifndef TESTAPP
     SRC += $(FLIGHTLIB)/insgps13state.c
     SRC += $(FLIGHTLIB)/auxmagsupport.c
     SRC += $(FLIGHTLIB)/lednotification.c    
+    SRC += $(FLIGHTLIB)/sha1.c
+    CPPSRC += $(FLIGHTLIB)/mini_cpp.cpp
 
     ## UAVObjects
     include ./UAVObjects.inc

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

@@ -206,7 +206,7 @@ static const struct pios_mpu6000_cfg pios_mpu6000_cfg = {
     .orientation    = PIOS_MPU6000_TOP_180DEG,
     .fast_prescaler = PIOS_SPI_PRESCALER_4,
     .std_prescaler  = PIOS_SPI_PRESCALER_64,
-    .max_downsample = 16,
+    .max_downsample = 20,
 };
 #endif /* PIOS_INCLUDE_MPU6000 */
 
@@ -770,11 +770,11 @@ void PIOS_Board_Init(void)
         }
 
         /* Set the radio configuration parameters. */
-        PIOS_RFM22B_SetChannelConfig(pios_rfm22b_id, datarate, oplinkSettings.MinChannel, oplinkSettings.MaxChannel, oplinkSettings.ChannelSet, is_coordinator, is_oneway, ppm_mode, ppm_only);
         PIOS_RFM22B_SetCoordinatorID(pios_rfm22b_id, oplinkSettings.CoordID);
+        PIOS_RFM22B_SetChannelConfig(pios_rfm22b_id, datarate, oplinkSettings.MinChannel, oplinkSettings.MaxChannel, is_coordinator, is_oneway, ppm_mode, ppm_only);
 
         /* Set the PPM callback if we should be receiving PPM. */
-        if (ppm_mode) {
+        if (ppm_mode || (ppm_only && !is_coordinator)) {
             PIOS_RFM22B_SetPPMCallback(pios_rfm22b_id, PIOS_Board_PPM_callback);
         }
 
@@ -922,20 +922,23 @@ void PIOS_Board_Init(void)
     PIOS_ADC_Init(&pios_adc_cfg);
 #endif
 
+#if defined(PIOS_INCLUDE_MPU6000)
+    PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg);
+    PIOS_MPU6000_CONFIG_Configure();
+    PIOS_MPU6000_Register();
+#endif
+
 #if defined(PIOS_INCLUDE_HMC5X83)
     onboard_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_mag_pressure_adapter_id, 0);
+    PIOS_HMC5x83_Register(onboard_mag);
 #endif
 
 #if defined(PIOS_INCLUDE_MS5611)
     PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_mag_pressure_adapter_id);
+    PIOS_MS5611_Register();
 #endif
 
-#if defined(PIOS_INCLUDE_MPU6000)
+    #ifdef PIOS_INCLUDE_WS2811
-    PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg);
-    PIOS_MPU6000_CONFIG_Configure();
-#endif
-
-#ifdef PIOS_INCLUDE_WS2811
 #include <pios_ws2811.h>
     HwSettingsWS2811LED_OutOptions ws2811_pin_settings;
     HwSettingsWS2811LED_OutGet(&ws2811_pin_settings);

flight/targets/boards/revolution/firmware/revolution.cpp

@@ -31,7 +31,7 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 
-
+extern "C" {
 #include "inc/openpilot.h"
 #include <uavobjectsinit.h>
 
@@ -74,6 +74,7 @@ static void initTask(void *parameters);
 
 /* Prototype of generated InitModules() function */
 extern void InitModules(void);
+}
 
 /**
  * OpenPilot Main function:

flight/targets/boards/revolution/pios_board.h

@@ -268,7 +268,7 @@ extern uint32_t pios_packet_handler;
 // -------------------------
 #define PIOS_SERVO_UPDATE_HZ         50
 #define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */
-
+#define PIOS_SERVO_BANKS             6
 // --------------------------
 // Timer controller settings
 // --------------------------

flight/targets/boards/revoproto/firmware/Makefile

@@ -29,11 +29,8 @@ USE_DSP_LIB ?= NO
 
 # List of mandatory modules to include
 MODULES += Sensors
-#MODULES += Attitude/revolution
+MODULES += StateEstimation
-MODULES += StateEstimation # use instead of Attitude
-MODULES += Altitude/revolution
 MODULES += Airspeed
-#MODULES += AltitudeHold # now integrated in Stabilization
 MODULES += Stabilization
 MODULES += ManualControl
 MODULES += Receiver

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

@@ -108,8 +108,6 @@
 #define PIOS_INCLUDE_IAP
 #define PIOS_INCLUDE_SERVO
 /* #define PIOS_INCLUDE_I2C_ESC */
-#define PIOS_INCLUDE_OVERO
-#define PIOS_OVERO_SPI
 /* #define PIOS_INCLUDE_SDCARD */
 /* #define LOG_FILENAME "startup.log" */
 #define PIOS_INCLUDE_FLASH

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

@@ -82,13 +82,14 @@ void PIOS_ADC_DMC_irq_handler(void)
 #endif /* if defined(PIOS_INCLUDE_ADC) */
 
 #if defined(PIOS_INCLUDE_HMC5X83)
+#include "pios_hmc5x83.h"
+
 pios_hmc5x83_dev_t onboard_mag = 0;
 
 bool pios_board_internal_mag_handler()
 {
     return PIOS_HMC5x83_IRQHandler(onboard_mag);
 }
-#include "pios_hmc5x83.h"
 static const struct pios_exti_cfg pios_exti_hmc5x83_cfg __exti_config = {
     .vector = pios_board_internal_mag_handler,
     .line   = EXTI_Line5,
@@ -893,19 +894,23 @@ void PIOS_Board_Init(void)
 
 #if defined(PIOS_INCLUDE_HMC5X83)
     onboard_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_mag_adapter_id, 0);
+    PIOS_HMC5x83_Register(onboard_mag);
 #endif
 
 #if defined(PIOS_INCLUDE_MS5611)
     PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_pressure_adapter_id);
+    PIOS_MS5611_Register();
 #endif
 
     switch (bdinfo->board_rev) {
     case 0x01:
 #if defined(PIOS_INCLUDE_L3GD20)
+        PIOS_Assert(0); // L3gd20 has not been ported to Sensor framework!!!
         PIOS_L3GD20_Init(pios_spi_gyro_id, 0, &pios_l3gd20_cfg);
         PIOS_Assert(PIOS_L3GD20_Test() == 0);
 #endif
 #if defined(PIOS_INCLUDE_BMA180)
+        PIOS_Assert(0); // BMA180 has not been ported to Sensor framework!!!
         PIOS_BMA180_Init(pios_spi_accel_id, 0, &pios_bma180_cfg);
         PIOS_Assert(PIOS_BMA180_Test() == 0);
 #endif
@@ -914,6 +919,7 @@ void PIOS_Board_Init(void)
 #if defined(PIOS_INCLUDE_MPU6000)
         PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg);
         PIOS_MPU6000_CONFIG_Configure();
+        PIOS_MPU6000_Register();
 #endif
         break;
     default:

flight/targets/boards/revoproto/pios_board.h

@@ -225,6 +225,7 @@ extern uint32_t pios_com_hkosd_id;
 // -------------------------
 #define PIOS_SERVO_UPDATE_HZ         50
 #define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */
+#define PIOS_SERVO_BANKS             6
 
 // --------------------------
 // Timer controller settings

ground/openpilotgcs/openpilotgcs.pri

@@ -66,6 +66,8 @@ equals(TEST, 1) {
 win32:!isEmpty(QMAKE_SH):QMAKE_COPY_DIR = cp -r -f
 
 GCS_SOURCE_TREE = $$PWD
+ROOT_DIR = $$GCS_SOURCE_TREE/../..
+
 isEmpty(GCS_BUILD_TREE) {
     sub_dir = $$_PRO_FILE_PWD_
     sub_dir ~= s,^$$re_escape($$PWD),,
@@ -80,7 +82,7 @@ isEmpty(TOOLS_DIR) {
     # check for custom enviroment variable,
     TOOLS_DIR = $$(OPENPILOT_TOOLS_DIR)
     # fallback to default location.
-    isEmpty(TOOLS_DIR):TOOLS_DIR = $$clean_path($$GCS_SOURCE_TREE/../../tools)
+    isEmpty(TOOLS_DIR):TOOLS_DIR = $$clean_path($$ROOT_DIR/tools)
 }
 
 GCS_APP_PATH = $$GCS_BUILD_TREE/bin

ground/openpilotgcs/share/openpilotgcs/cloudconfig/multirotor/BlackOutB330-Multirotor-QuadrocopterX-e9980c55302.optmpl

@@ -0,0 +1,2185 @@
+{
+    "battery": "2200 4s 60c",
+    "comment": "Bank 1 Acro + \nBank 2 Attitude mode\npids set for Blheli 13.xx and oneshot enabled",
+    "controller": "CC3D",
+    "esc": "Afro 30 Blheli 13.01",
+    "motor": "Blackout 2208 2000kv",
+    "name": "BlackOut B330 ",
+    "nick": "Failsafe",
+    "objects": [
+        {
+            "fields": [
+                {
+                    "name": "VbarSensitivity",
+                    "type": "float32",
+                    "unit": "frac",
+                    "values": [
+                        {
+                            "name": "Roll",
+                            "value": 0.5
+                        },
+                        {
+                            "name": "Pitch",
+                            "value": 0.5
+                        },
+                        {
+                            "name": "Yaw",
+                            "value": 0.5
+                        }
+                    ]
+                },
+                {
+                    "name": "VbarRollPI",
+                    "type": "float32",
+                    "unit": "1/(deg/s)",
+                    "values": [
+                        {
+                            "name": "Kp",
+                            "value": 0.004999999888241291
+                        },
+                        {
+                            "name": "Ki",
+                            "value": 0.0020000000949949026
+                        }
+                    ]
+                },
+                {
+                    "name": "VbarPitchPI",
+                    "type": "float32",
+                    "unit": "1/(deg/s)",
+                    "values": [
+                        {
+                            "name": "Kp",
+                            "value": 0.004999999888241291
+                        },
+                        {
+                            "name": "Ki",
+                            "value": 0.0020000000949949026
+                        }
+                    ]
+                },
+                {
+                    "name": "VbarYawPI",
+                    "type": "float32",
+                    "unit": "1/(deg/s)",
+                    "values": [
+                        {
+                            "name": "Kp",
+                            "value": 0.004999999888241291
+                        },
+                        {
+                            "name": "Ki",
+                            "value": 0.0020000000949949026
+                        }
+                    ]
+                },
+                {
+                    "name": "VbarTau",
+                    "type": "float32",
+                    "unit": "sec",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 0.5
+                        }
+                    ]
+                },
+                {
+                    "name": "GyroTau",
+                    "type": "float32",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 0.004999999888241291
+                        }
+                    ]
+                },
+                {
+                    "name": "DerivativeGamma",
+                    "type": "float32",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 1
+                        }
+                    ]
+                },
+                {
+                    "name": "AxisLockKp",
+                    "type": "float32",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 2.5
+                        }
+                    ]
+                },
+                {
+                    "name": "WeakLevelingKp",
+                    "type": "float32",
+                    "unit": "(deg/s)/deg",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 0.10000000149011612
+                        }
+                    ]
+                },
+                {
+                    "name": "CruiseControlMaxPowerFactor",
+                    "type": "float32",
+                    "unit": "x",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 3
+                        }
+                    ]
+                },
+                {
+                    "name": "CruiseControlPowerTrim",
+                    "type": "float32",
+                    "unit": "%",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 100
+                        }
+                    ]
+                },
+                {
+                    "name": "CruiseControlPowerDelayComp",
+                    "type": "float32",
+                    "unit": "sec",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 0.25
+                        }
+                    ]
+                },
+                {
+                    "name": "ScaleToAirspeed",
+                    "type": "float32",
+                    "unit": "m/s",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 0
+                        }
+                    ]
+                },
+                {
+                    "name": "ScaleToAirspeedLimits",
+                    "type": "float32",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "Min",
+                            "value": 0.05000000074505806
+                        },
+                        {
+                            "name": "Max",
+                            "value": 3
+                        }
+                    ]
+                },
+                {
+                    "name": "FlightModeMap",
+                    "type": "enum",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": "Bank1"
+                        },
+                        {
+                            "name": "1",
+                            "value": "Bank2"
+                        },
+                        {
+                            "name": "2",
+                            "value": "Bank1"
+                        },
+                        {
+                            "name": "3",
+                            "value": "Bank1"
+                        },
+                        {
+                            "name": "4",
+                            "value": "Bank1"
+                        },
+                        {
+                            "name": "5",
+                            "value": "Bank1"
+                        }
+                    ]
+                },
+                {
+                    "name": "VbarGyroSuppress",
+                    "type": "int8",
+                    "unit": "%",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 30
+                        }
+                    ]
+                },
+                {
+                    "name": "VbarPiroComp",
+                    "type": "enum",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": "FALSE"
+                        }
+                    ]
+                },
+                {
+                    "name": "VbarMaxAngle",
+                    "type": "uint8",
+                    "unit": "deg",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 10
+                        }
+                    ]
+                },
+                {
+                    "name": "DerivativeCutoff",
+                    "type": "uint8",
+                    "unit": "Hz",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 20
+                        }
+                    ]
+                },
+                {
+                    "name": "MaxAxisLock",
+                    "type": "uint8",
+                    "unit": "deg",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 30
+                        }
+                    ]
+                },
+                {
+                    "name": "MaxAxisLockRate",
+                    "type": "uint8",
+                    "unit": "deg/s",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 2
+                        }
+                    ]
+                },
+                {
+                    "name": "MaxWeakLevelingRate",
+                    "type": "uint8",
+                    "unit": "deg/s",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 5
+                        }
+                    ]
+                },
+                {
+                    "name": "RattitudeModeTransition",
+                    "type": "uint8",
+                    "unit": "%",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 80
+                        }
+                    ]
+                },
+                {
+                    "name": "CruiseControlMinThrust",
+                    "type": "int8",
+                    "unit": "%",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 5
+                        }
+                    ]
+                },
+                {
+                    "name": "CruiseControlMaxThrust",
+                    "type": "uint8",
+                    "unit": "%",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 90
+                        }
+                    ]
+                },
+                {
+                    "name": "CruiseControlMaxAngle",
+                    "type": "uint8",
+                    "unit": "deg",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 105
+                        }
+                    ]
+                },
+                {
+                    "name": "CruiseControlFlightModeSwitchPosEnable",
+                    "type": "enum",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": "FALSE"
+                        },
+                        {
+                            "name": "1",
+                            "value": "FALSE"
+                        },
+                        {
+                            "name": "2",
+                            "value": "FALSE"
+                        },
+                        {
+                            "name": "3",
+                            "value": "FALSE"
+                        },
+                        {
+                            "name": "4",
+                            "value": "FALSE"
+                        },
+                        {
+                            "name": "5",
+                            "value": "FALSE"
+                        }
+                    ]
+                },
+                {
+                    "name": "CruiseControlInvertedThrustReversing",
+                    "type": "enum",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": "Unreversed"
+                        }
+                    ]
+                },
+                {
+                    "name": "CruiseControlInvertedPowerOutput",
+                    "type": "enum",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": "Zero"
+                        }
+                    ]
+                },
+                {
+                    "name": "LowThrottleZeroIntegral",
+                    "type": "enum",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": "TRUE"
+                        }
+                    ]
+                },
+                {
+                    "name": "FlightModeAssistMap",
+                    "type": "enum",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": "None"
+                        },
+                        {
+                            "name": "1",
+                            "value": "None"
+                        },
+                        {
+                            "name": "2",
+                            "value": "None"
+                        },
+                        {
+                            "name": "3",
+                            "value": "None"
+                        },
+                        {
+                            "name": "4",
+                            "value": "None"
+                        },
+                        {
+                            "name": "5",
+                            "value": "None"
+                        }
+                    ]
+                }
+            ],
+            "id": "73603180",
+            "instance": 0,
+            "name": "StabilizationSettings",
+            "setting": true
+        },
+        {
+            "fields": [
+                {
+                    "name": "ManualRate",
+                    "type": "float32",
+                    "unit": "degrees/sec",
+                    "values": [
+                        {
+                            "name": "Roll",
+                            "value": 200
+                        },
+                        {
+                            "name": "Pitch",
+                            "value": 200
+                        },
+                        {
+                            "name": "Yaw",
+                            "value": 220
+                        }
+                    ]
+                },
+                {
+                    "name": "MaximumRate",
+                    "type": "float32",
+                    "unit": "degrees/sec",
+                    "values": [
+                        {
+                            "name": "Roll",
+                            "value": 500
+                        },
+                        {
+                            "name": "Pitch",
+                            "value": 500
+                        },
+                        {
+                            "name": "Yaw",
+                            "value": 300
+                        }
+                    ]
+                },
+                {
+                    "name": "RollRatePID",
+                    "type": "float32",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "Kp",
+                            "value": 0.002899999963119626
+                        },
+                        {
+                            "name": "Ki",
+                            "value": 0.0065000001341104507
+                        },
+                        {
+                            "name": "Kd",
+                            "value": 3.4000000596279278e-05
+                        },
+                        {
+                            "name": "ILimit",
+                            "value": 0.30000001192092896
+                        }
+                    ]
+                },
+                {
+                    "name": "PitchRatePID",
+                    "type": "float32",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "Kp",
+                            "value": 0.004120000172406435
+                        },
+                        {
+                            "name": "Ki",
+                            "value": 0.0082999998703598976
+                        },
+                        {
+                            "name": "Kd",
+                            "value": 4.1999999666586518e-05
+                        },
+                        {
+                            "name": "ILimit",
+                            "value": 0.30000001192092896
+                        }
+                    ]
+                },
+                {
+                    "name": "YawRatePID",
+                    "type": "float32",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "Kp",
+                            "value": 0.0086000002920627594
+                        },
+                        {
+                            "name": "Ki",
+                            "value": 0.014299999922513962
+                        },
+                        {
+                            "name": "Kd",
+                            "value": 4.9999998736893758e-05
+                        },
+                        {
+                            "name": "ILimit",
+                            "value": 0.30000001192092896
+                        }
+                    ]
+                },
+                {
+                    "name": "RollPI",
+                    "type": "float32",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "Kp",
+                            "value": 2.9000000953674316
+                        },
+                        {
+                            "name": "Ki",
+                            "value": 0
+                        },
+                        {
+                            "name": "ILimit",
+                            "value": 50
+                        }
+                    ]
+                },
+                {
+                    "name": "PitchPI",
+                    "type": "float32",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "Kp",
+                            "value": 3.2999999523162842
+                        },
+                        {
+                            "name": "Ki",
+                            "value": 0
+                        },
+                        {
+                            "name": "ILimit",
+                            "value": 50
+                        }
+                    ]
+                },
+                {
+                    "name": "YawPI",
+                    "type": "float32",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "Kp",
+                            "value": 2.5
+                        },
+                        {
+                            "name": "Ki",
+                            "value": 0
+                        },
+                        {
+                            "name": "ILimit",
+                            "value": 50
+                        }
+                    ]
+                },
+                {
+                    "name": "AcroInsanityFactor",
+                    "type": "float32",
+                    "unit": "percent",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 0.4699999988079071
+                        }
+                    ]
+                },
+                {
+                    "name": "ThrustPIDScaleCurve",
+                    "type": "float32",
+                    "unit": "percent",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 0.0042716437019407749
+                        },
+                        {
+                            "name": "25",
+                            "value": -0.021436728537082672
+                        },
+                        {
+                            "name": "50",
+                            "value": -0.051431018859148026
+                        },
+                        {
+                            "name": "75",
+                            "value": -0.15000000596046448
+                        },
+                        {
+                            "name": "100",
+                            "value": -0.34714511036872864
+                        }
+                    ]
+                },
+                {
+                    "name": "RollMax",
+                    "type": "uint8",
+                    "unit": "degrees",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 55
+                        }
+                    ]
+                },
+                {
+                    "name": "PitchMax",
+                    "type": "uint8",
+                    "unit": "degrees",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 55
+                        }
+                    ]
+                },
+                {
+                    "name": "YawMax",
+                    "type": "uint8",
+                    "unit": "degrees",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 35
+                        }
+                    ]
+                },
+                {
+                    "name": "StickExpo",
+                    "type": "int8",
+                    "unit": "percent",
+                    "values": [
+                        {
+                            "name": "Roll",
+                            "value": 12
+                        },
+                        {
+                            "name": "Pitch",
+                            "value": 12
+                        },
+                        {
+                            "name": "Yaw",
+                            "value": -5
+                        }
+                    ]
+                },
+                {
+                    "name": "EnablePiroComp",
+                    "type": "enum",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": "FALSE"
+                        }
+                    ]
+                },
+                {
+                    "name": "EnableThrustPIDScaling",
+                    "type": "enum",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": "TRUE"
+                        }
+                    ]
+                },
+                {
+                    "name": "ThrustPIDScaleSource",
+                    "type": "enum",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": "ActuatorDesiredThrust"
+                        }
+                    ]
+                },
+                {
+                    "name": "ThrustPIDScaleTarget",
+                    "type": "enum",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": "PD"
+                        }
+                    ]
+                },
+                {
+                    "name": "ThrustPIDScaleAxes",
+                    "type": "enum",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": "Roll Pitch"
+                        }
+                    ]
+                }
+            ],
+            "id": "E8EBBD48",
+            "instance": 0,
+            "name": "StabilizationSettingsBank1",
+            "setting": true
+        },
+        {
+            "fields": [
+                {
+                    "name": "ManualRate",
+                    "type": "float32",
+                    "unit": "degrees/sec",
+                    "values": [
+                        {
+                            "name": "Roll",
+                            "value": 200
+                        },
+                        {
+                            "name": "Pitch",
+                            "value": 200
+                        },
+                        {
+                            "name": "Yaw",
+                            "value": 220
+                        }
+                    ]
+                },
+                {
+                    "name": "MaximumRate",
+                    "type": "float32",
+                    "unit": "degrees/sec",
+                    "values": [
+                        {
+                            "name": "Roll",
+                            "value": 500
+                        },
+                        {
+                            "name": "Pitch",
+                            "value": 500
+                        },
+                        {
+                            "name": "Yaw",
+                            "value": 300
+                        }
+                    ]
+                },
+                {
+                    "name": "RollRatePID",
+                    "type": "float32",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "Kp",
+                            "value": 0.00279999990016222
+                        },
+                        {
+                            "name": "Ki",
+                            "value": 0.0054999999701976776
+                        },
+                        {
+                            "name": "Kd",
+                            "value": 2.9000000722589903e-05
+                        },
+                        {
+                            "name": "ILimit",
+                            "value": 0.30000001192092896
+                        }
+                    ]
+                },
+                {
+                    "name": "PitchRatePID",
+                    "type": "float32",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "Kp",
+                            "value": 0.0036200000904500484
+                        },
+                        {
+                            "name": "Ki",
+                            "value": 0.0086000002920627594
+                        },
+                        {
+                            "name": "Kd",
+                            "value": 3.600000127335079e-05
+                        },
+                        {
+                            "name": "ILimit",
+                            "value": 0.30000001192092896
+                        }
+                    ]
+                },
+                {
+                    "name": "YawRatePID",
+                    "type": "float32",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "Kp",
+                            "value": 0.0086000002920627594
+                        },
+                        {
+                            "name": "Ki",
+                            "value": 0.014299999922513962
+                        },
+                        {
+                            "name": "Kd",
+                            "value": 4.9999998736893758e-05
+                        },
+                        {
+                            "name": "ILimit",
+                            "value": 0.30000001192092896
+                        }
+                    ]
+                },
+                {
+                    "name": "RollPI",
+                    "type": "float32",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "Kp",
+                            "value": 3
+                        },
+                        {
+                            "name": "Ki",
+                            "value": 0
+                        },
+                        {
+                            "name": "ILimit",
+                            "value": 50
+                        }
+                    ]
+                },
+                {
+                    "name": "PitchPI",
+                    "type": "float32",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "Kp",
+                            "value": 3.4000000953674316
+                        },
+                        {
+                            "name": "Ki",
+                            "value": 0
+                        },
+                        {
+                            "name": "ILimit",
+                            "value": 50
+                        }
+                    ]
+                },
+                {
+                    "name": "YawPI",
+                    "type": "float32",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "Kp",
+                            "value": 2.5
+                        },
+                        {
+                            "name": "Ki",
+                            "value": 0
+                        },
+                        {
+                            "name": "ILimit",
+                            "value": 50
+                        }
+                    ]
+                },
+                {
+                    "name": "AcroInsanityFactor",
+                    "type": "float32",
+                    "unit": "percent",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 0.40000000596046448
+                        }
+                    ]
+                },
+                {
+                    "name": "ThrustPIDScaleCurve",
+                    "type": "float32",
+                    "unit": "percent",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 0.012843480333685875
+                        },
+                        {
+                            "name": "25",
+                            "value": -7.1359904723067302e-06
+                        },
+                        {
+                            "name": "50",
+                            "value": -0.12000571191310883
+                        },
+                        {
+                            "name": "75",
+                            "value": -0.18857327103614807
+                        },
+                        {
+                            "name": "100",
+                            "value": -0.25714081525802612
+                        }
+                    ]
+                },
+                {
+                    "name": "RollMax",
+                    "type": "uint8",
+                    "unit": "degrees",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 55
+                        }
+                    ]
+                },
+                {
+                    "name": "PitchMax",
+                    "type": "uint8",
+                    "unit": "degrees",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 55
+                        }
+                    ]
+                },
+                {
+                    "name": "YawMax",
+                    "type": "uint8",
+                    "unit": "degrees",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 35
+                        }
+                    ]
+                },
+                {
+                    "name": "StickExpo",
+                    "type": "int8",
+                    "unit": "percent",
+                    "values": [
+                        {
+                            "name": "Roll",
+                            "value": 15
+                        },
+                        {
+                            "name": "Pitch",
+                            "value": 15
+                        },
+                        {
+                            "name": "Yaw",
+                            "value": -5
+                        }
+                    ]
+                },
+                {
+                    "name": "EnablePiroComp",
+                    "type": "enum",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": "FALSE"
+                        }
+                    ]
+                },
+                {
+                    "name": "EnableThrustPIDScaling",
+                    "type": "enum",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": "TRUE"
+                        }
+                    ]
+                },
+                {
+                    "name": "ThrustPIDScaleSource",
+                    "type": "enum",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": "ActuatorDesiredThrust"
+                        }
+                    ]
+                },
+                {
+                    "name": "ThrustPIDScaleTarget",
+                    "type": "enum",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": "PD"
+                        }
+                    ]
+                },
+                {
+                    "name": "ThrustPIDScaleAxes",
+                    "type": "enum",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": "Roll Pitch"
+                        }
+                    ]
+                }
+            ],
+            "id": "70E9539A",
+            "instance": 0,
+            "name": "StabilizationSettingsBank2",
+            "setting": true
+        },
+        {
+            "fields": [
+                {
+                    "name": "ManualRate",
+                    "type": "float32",
+                    "unit": "degrees/sec",
+                    "values": [
+                        {
+                            "name": "Roll",
+                            "value": 220
+                        },
+                        {
+                            "name": "Pitch",
+                            "value": 220
+                        },
+                        {
+                            "name": "Yaw",
+                            "value": 220
+                        }
+                    ]
+                },
+                {
+                    "name": "MaximumRate",
+                    "type": "float32",
+                    "unit": "degrees/sec",
+                    "values": [
+                        {
+                            "name": "Roll",
+                            "value": 300
+                        },
+                        {
+                            "name": "Pitch",
+                            "value": 300
+                        },
+                        {
+                            "name": "Yaw",
+                            "value": 300
+                        }
+                    ]
+                },
+                {
+                    "name": "RollRatePID",
+                    "type": "float32",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "Kp",
+                            "value": 0.0024999999441206455
+                        },
+                        {
+                            "name": "Ki",
+                            "value": 0.0040000001899898052
+                        },
+                        {
+                            "name": "Kd",
+                            "value": 1.9999999494757503e-05
+                        },
+                        {
+                            "name": "ILimit",
+                            "value": 0.30000001192092896
+                        }
+                    ]
+                },
+                {
+                    "name": "PitchRatePID",
+                    "type": "float32",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "Kp",
+                            "value": 0.0024999999441206455
+                        },
+                        {
+                            "name": "Ki",
+                            "value": 0.0040000001899898052
+                        },
+                        {
+                            "name": "Kd",
+                            "value": 1.9999999494757503e-05
+                        },
+                        {
+                            "name": "ILimit",
+                            "value": 0.30000001192092896
+                        }
+                    ]
+                },
+                {
+                    "name": "YawRatePID",
+                    "type": "float32",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "Kp",
+                            "value": 0.0062000001780688763
+                        },
+                        {
+                            "name": "Ki",
+                            "value": 0.0099999997764825821
+                        },
+                        {
+                            "name": "Kd",
+                            "value": 4.9999998736893758e-05
+                        },
+                        {
+                            "name": "ILimit",
+                            "value": 0.30000001192092896
+                        }
+                    ]
+                },
+                {
+                    "name": "RollPI",
+                    "type": "float32",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "Kp",
+                            "value": 2.5
+                        },
+                        {
+                            "name": "Ki",
+                            "value": 0
+                        },
+                        {
+                            "name": "ILimit",
+                            "value": 50
+                        }
+                    ]
+                },
+                {
+                    "name": "PitchPI",
+                    "type": "float32",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "Kp",
+                            "value": 2.5
+                        },
+                        {
+                            "name": "Ki",
+                            "value": 0
+                        },
+                        {
+                            "name": "ILimit",
+                            "value": 50
+                        }
+                    ]
+                },
+                {
+                    "name": "YawPI",
+                    "type": "float32",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "Kp",
+                            "value": 2.5
+                        },
+                        {
+                            "name": "Ki",
+                            "value": 0
+                        },
+                        {
+                            "name": "ILimit",
+                            "value": 50
+                        }
+                    ]
+                },
+                {
+                    "name": "AcroInsanityFactor",
+                    "type": "float32",
+                    "unit": "percent",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 0.5
+                        }
+                    ]
+                },
+                {
+                    "name": "ThrustPIDScaleCurve",
+                    "type": "float32",
+                    "unit": "percent",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 0.30000001192092896
+                        },
+                        {
+                            "name": "25",
+                            "value": 0.15000000596046448
+                        },
+                        {
+                            "name": "50",
+                            "value": 0
+                        },
+                        {
+                            "name": "75",
+                            "value": -0.15000000596046448
+                        },
+                        {
+                            "name": "100",
+                            "value": -0.30000001192092896
+                        }
+                    ]
+                },
+                {
+                    "name": "RollMax",
+                    "type": "uint8",
+                    "unit": "degrees",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 55
+                        }
+                    ]
+                },
+                {
+                    "name": "PitchMax",
+                    "type": "uint8",
+                    "unit": "degrees",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 55
+                        }
+                    ]
+                },
+                {
+                    "name": "YawMax",
+                    "type": "uint8",
+                    "unit": "degrees",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 35
+                        }
+                    ]
+                },
+                {
+                    "name": "StickExpo",
+                    "type": "int8",
+                    "unit": "percent",
+                    "values": [
+                        {
+                            "name": "Roll",
+                            "value": 0
+                        },
+                        {
+                            "name": "Pitch",
+                            "value": 0
+                        },
+                        {
+                            "name": "Yaw",
+                            "value": 0
+                        }
+                    ]
+                },
+                {
+                    "name": "EnablePiroComp",
+                    "type": "enum",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": "TRUE"
+                        }
+                    ]
+                },
+                {
+                    "name": "EnableThrustPIDScaling",
+                    "type": "enum",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": "FALSE"
+                        }
+                    ]
+                },
+                {
+                    "name": "ThrustPIDScaleSource",
+                    "type": "enum",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": "ActuatorDesiredThrust"
+                        }
+                    ]
+                },
+                {
+                    "name": "ThrustPIDScaleTarget",
+                    "type": "enum",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": "PID"
+                        }
+                    ]
+                },
+                {
+                    "name": "ThrustPIDScaleAxes",
+                    "type": "enum",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": "Roll Pitch"
+                        }
+                    ]
+                }
+            ],
+            "id": "C02DAA6A",
+            "instance": 0,
+            "name": "StabilizationSettingsBank3",
+            "setting": true
+        },
+        {
+            "fields": [
+                {
+                    "name": "MaxAccel",
+                    "type": "float32",
+                    "unit": "units/sec",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 1000
+                        }
+                    ]
+                },
+                {
+                    "name": "FeedForward",
+                    "type": "float32",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 0
+                        }
+                    ]
+                },
+                {
+                    "name": "AccelTime",
+                    "type": "float32",
+                    "unit": "ms",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 0
+                        }
+                    ]
+                },
+                {
+                    "name": "DecelTime",
+                    "type": "float32",
+                    "unit": "ms",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 0
+                        }
+                    ]
+                },
+                {
+                    "name": "ThrottleCurve1",
+                    "type": "float32",
+                    "unit": "percent",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 0
+                        },
+                        {
+                            "name": "25",
+                            "value": 0.22499999403953552
+                        },
+                        {
+                            "name": "50",
+                            "value": 0.44999998807907104
+                        },
+                        {
+                            "name": "75",
+                            "value": 0.67499995231628418
+                        },
+                        {
+                            "name": "100",
+                            "value": 0.89999997615814209
+                        }
+                    ]
+                },
+                {
+                    "name": "ThrottleCurve2",
+                    "type": "float32",
+                    "unit": "percent",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 0
+                        },
+                        {
+                            "name": "25",
+                            "value": 0.22499999403953552
+                        },
+                        {
+                            "name": "50",
+                            "value": 0.44999998807907104
+                        },
+                        {
+                            "name": "75",
+                            "value": 0.67499995231628418
+                        },
+                        {
+                            "name": "100",
+                            "value": 0.89999997615814209
+                        }
+                    ]
+                },
+                {
+                    "name": "MixerValueRoll",
+                    "type": "int8",
+                    "unit": "percent",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 50
+                        }
+                    ]
+                },
+                {
+                    "name": "MixerValuePitch",
+                    "type": "int8",
+                    "unit": "percent",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 50
+                        }
+                    ]
+                },
+                {
+                    "name": "MixerValueYaw",
+                    "type": "int8",
+                    "unit": "percent",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": 50
+                        }
+                    ]
+                },
+                {
+                    "name": "Curve2Source",
+                    "type": "enum",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": "Throttle"
+                        }
+                    ]
+                },
+                {
+                    "name": "Mixer1Type",
+                    "type": "enum",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": "Motor"
+                        }
+                    ]
+                },
+                {
+                    "name": "Mixer1Vector",
+                    "type": "int8",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "ThrottleCurve1",
+                            "value": 127
+                        },
+                        {
+                            "name": "ThrottleCurve2",
+                            "value": 0
+                        },
+                        {
+                            "name": "Roll",
+                            "value": 64
+                        },
+                        {
+                            "name": "Pitch",
+                            "value": 64
+                        },
+                        {
+                            "name": "Yaw",
+                            "value": -64
+                        }
+                    ]
+                },
+                {
+                    "name": "Mixer2Type",
+                    "type": "enum",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": "Motor"
+                        }
+                    ]
+                },
+                {
+                    "name": "Mixer2Vector",
+                    "type": "int8",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "ThrottleCurve1",
+                            "value": 127
+                        },
+                        {
+                            "name": "ThrottleCurve2",
+                            "value": 0
+                        },
+                        {
+                            "name": "Roll",
+                            "value": -64
+                        },
+                        {
+                            "name": "Pitch",
+                            "value": 64
+                        },
+                        {
+                            "name": "Yaw",
+                            "value": 64
+                        }
+                    ]
+                },
+                {
+                    "name": "Mixer3Type",
+                    "type": "enum",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": "Motor"
+                        }
+                    ]
+                },
+                {
+                    "name": "Mixer3Vector",
+                    "type": "int8",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "ThrottleCurve1",
+                            "value": 127
+                        },
+                        {
+                            "name": "ThrottleCurve2",
+                            "value": 0
+                        },
+                        {
+                            "name": "Roll",
+                            "value": -64
+                        },
+                        {
+                            "name": "Pitch",
+                            "value": -64
+                        },
+                        {
+                            "name": "Yaw",
+                            "value": -64
+                        }
+                    ]
+                },
+                {
+                    "name": "Mixer4Type",
+                    "type": "enum",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": "Motor"
+                        }
+                    ]
+                },
+                {
+                    "name": "Mixer4Vector",
+                    "type": "int8",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "ThrottleCurve1",
+                            "value": 127
+                        },
+                        {
+                            "name": "ThrottleCurve2",
+                            "value": 0
+                        },
+                        {
+                            "name": "Roll",
+                            "value": 64
+                        },
+                        {
+                            "name": "Pitch",
+                            "value": -64
+                        },
+                        {
+                            "name": "Yaw",
+                            "value": 64
+                        }
+                    ]
+                },
+                {
+                    "name": "Mixer5Type",
+                    "type": "enum",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": "Disabled"
+                        }
+                    ]
+                },
+                {
+                    "name": "Mixer5Vector",
+                    "type": "int8",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "ThrottleCurve1",
+                            "value": 0
+                        },
+                        {
+                            "name": "ThrottleCurve2",
+                            "value": 0
+                        },
+                        {
+                            "name": "Roll",
+                            "value": 0
+                        },
+                        {
+                            "name": "Pitch",
+                            "value": 0
+                        },
+                        {
+                            "name": "Yaw",
+                            "value": 0
+                        }
+                    ]
+                },
+                {
+                    "name": "Mixer6Type",
+                    "type": "enum",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": "Disabled"
+                        }
+                    ]
+                },
+                {
+                    "name": "Mixer6Vector",
+                    "type": "int8",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "ThrottleCurve1",
+                            "value": 0
+                        },
+                        {
+                            "name": "ThrottleCurve2",
+                            "value": 0
+                        },
+                        {
+                            "name": "Roll",
+                            "value": 0
+                        },
+                        {
+                            "name": "Pitch",
+                            "value": 0
+                        },
+                        {
+                            "name": "Yaw",
+                            "value": 0
+                        }
+                    ]
+                },
+                {
+                    "name": "Mixer7Type",
+                    "type": "enum",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": "Disabled"
+                        }
+                    ]
+                },
+                {
+                    "name": "Mixer7Vector",
+                    "type": "int8",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "ThrottleCurve1",
+                            "value": 0
+                        },
+                        {
+                            "name": "ThrottleCurve2",
+                            "value": 0
+                        },
+                        {
+                            "name": "Roll",
+                            "value": 0
+                        },
+                        {
+                            "name": "Pitch",
+                            "value": 0
+                        },
+                        {
+                            "name": "Yaw",
+                            "value": 0
+                        }
+                    ]
+                },
+                {
+                    "name": "Mixer8Type",
+                    "type": "enum",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": "Disabled"
+                        }
+                    ]
+                },
+                {
+                    "name": "Mixer8Vector",
+                    "type": "int8",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "ThrottleCurve1",
+                            "value": 0
+                        },
+                        {
+                            "name": "ThrottleCurve2",
+                            "value": 0
+                        },
+                        {
+                            "name": "Roll",
+                            "value": 0
+                        },
+                        {
+                            "name": "Pitch",
+                            "value": 0
+                        },
+                        {
+                            "name": "Yaw",
+                            "value": 0
+                        }
+                    ]
+                },
+                {
+                    "name": "Mixer9Type",
+                    "type": "enum",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": "Disabled"
+                        }
+                    ]
+                },
+                {
+                    "name": "Mixer9Vector",
+                    "type": "int8",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "ThrottleCurve1",
+                            "value": 0
+                        },
+                        {
+                            "name": "ThrottleCurve2",
+                            "value": 0
+                        },
+                        {
+                            "name": "Roll",
+                            "value": 0
+                        },
+                        {
+                            "name": "Pitch",
+                            "value": 0
+                        },
+                        {
+                            "name": "Yaw",
+                            "value": 0
+                        }
+                    ]
+                },
+                {
+                    "name": "Mixer10Type",
+                    "type": "enum",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": "Disabled"
+                        }
+                    ]
+                },
+                {
+                    "name": "Mixer10Vector",
+                    "type": "int8",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "ThrottleCurve1",
+                            "value": 0
+                        },
+                        {
+                            "name": "ThrottleCurve2",
+                            "value": 0
+                        },
+                        {
+                            "name": "Roll",
+                            "value": 0
+                        },
+                        {
+                            "name": "Pitch",
+                            "value": 0
+                        },
+                        {
+                            "name": "Yaw",
+                            "value": 0
+                        }
+                    ]
+                },
+                {
+                    "name": "Mixer11Type",
+                    "type": "enum",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": "Disabled"
+                        }
+                    ]
+                },
+                {
+                    "name": "Mixer11Vector",
+                    "type": "int8",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "ThrottleCurve1",
+                            "value": 0
+                        },
+                        {
+                            "name": "ThrottleCurve2",
+                            "value": 0
+                        },
+                        {
+                            "name": "Roll",
+                            "value": 0
+                        },
+                        {
+                            "name": "Pitch",
+                            "value": 0
+                        },
+                        {
+                            "name": "Yaw",
+                            "value": 0
+                        }
+                    ]
+                },
+                {
+                    "name": "Mixer12Type",
+                    "type": "enum",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "0",
+                            "value": "Disabled"
+                        }
+                    ]
+                },
+                {
+                    "name": "Mixer12Vector",
+                    "type": "int8",
+                    "unit": "",
+                    "values": [
+                        {
+                            "name": "ThrottleCurve1",
+                            "value": 0
+                        },
+                        {
+                            "name": "ThrottleCurve2",
+                            "value": 0
+                        },
+                        {
+                            "name": "Roll",
+                            "value": 0
+                        },
+                        {
+                            "name": "Pitch",
+                            "value": 0
+                        },
+                        {
+                            "name": "Yaw",
+                            "value": 0
+                        }
+                    ]
+                }
+            ],
+            "id": "7BF2CFA8",
+            "instance": 0,
+            "name": "MixerSettings",
+            "setting": true
+        },
+        {
+            "fields": [
+                {
+                    "name": "P",
+                    "type": "float32",
+                    "unit": "1^2",
+                    "values": [
+                        {
+                            "name": "PositionNorth",
+                            "value": 10
+                        },
+                        {
+                            "name": "PositionEast",
+                            "value": 10
+                        },
+                        {
+                            "name": "PositionDown",
+                            "value": 10
+                        },
+                        {
+                            "name": "VelocityNorth",
+                            "value": 1
+                        },
+                        {
+                            "name": "VelocityEast",
+                            "value": 1
+                        },
+                        {
+                            "name": "VelocityDown",
+                            "value": 1
+                        },
+                        {
+                            "name": "AttitudeQ1",
+                            "value": 0.0070000002160668373
+                        },
+                        {
+                            "name": "AttitudeQ2",
+                            "value": 0.0070000002160668373
+                        },
+                        {
+                            "name": "AttitudeQ3",
+                            "value": 0.0070000002160668373
+                        },
+                        {
+                            "name": "AttitudeQ4",
+                            "value": 0.0070000002160668373
+                        },
+                        {
+                            "name": "GyroDriftX",
+                            "value": 9.9999999747524271e-07
+                        },
+                        {
+                            "name": "GyroDriftY",
+                            "value": 9.9999999747524271e-07
+                        },
+                        {
+                            "name": "GyroDriftZ",
+                            "value": 9.9999999747524271e-07
+                        }
+                    ]
+                },
+                {
+                    "name": "Q",
+                    "type": "float32",
+                    "unit": "1^2",
+                    "values": [
+                        {
+                            "name": "GyroX",
+                            "value": 0.0099999997764825821
+                        },
+                        {
+                            "name": "GyroY",
+                            "value": 0.0099999997764825821
+                        },
+                        {
+                            "name": "GyroZ",
+                            "value": 0.0099999997764825821
+                        },
+                        {
+                            "name": "AccelX",
+                            "value": 0.0099999997764825821
+                        },
+                        {
+                            "name": "AccelY",
+                            "value": 0.0099999997764825821
+                        },
+                        {
+                            "name": "AccelZ",
+                            "value": 0.0099999997764825821
+                        },
+                        {
+                            "name": "GyroDriftX",
+                            "value": 9.9999999747524271e-07
+                        },
+                        {
+                            "name": "GyroDriftY",
+                            "value": 9.9999999747524271e-07
+                        },
+                        {
+                            "name": "GyroDriftZ",
+                            "value": 9.9999999747524271e-07
+                        }
+                    ]
+                },
+                {
+                    "name": "R",
+                    "type": "float32",
+                    "unit": "1^2",
+                    "values": [
+                        {
+                            "name": "GPSPosNorth",
+                            "value": 1
+                        },
+                        {
+                            "name": "GPSPosEast",
+                            "value": 1
+                        },
+                        {
+                            "name": "GPSPosDown",
+                            "value": 1000000
+                        },
+                        {
+                            "name": "GPSVelNorth",
+                            "value": 0.0010000000474974513
+                        },
+                        {
+                            "name": "GPSVelEast",
+                            "value": 0.0010000000474974513
+                        },
+                        {
+                            "name": "GPSVelDown",
+                            "value": 0.0010000000474974513
+                        },
+                        {
+                            "name": "MagX",
+                            "value": 10
+                        },
+                        {
+                            "name": "MagY",
+                            "value": 10
+                        },
+                        {
+                            "name": "MagZ",
+                            "value": 10
+                        },
+                        {
+                            "name": "BaroZ",
+                            "value": 0.0099999997764825821
+                        }
+                    ]
+                },
+                {
+                    "name": "FakeR",
+                    "type": "float32",
+                    "unit": "1^2",
+                    "values": [
+                        {
+                            "name": "FakeGPSPosIndoor",
+                            "value": 10
+                        },
+                        {
+                            "name": "FakeGPSVelIndoor",
+                            "value": 1
+                        },
+                        {
+                            "name": "FakeGPSVelAirspeed",
+                            "value": 1000
+                        }
+                    ]
+                }
+            ],
+            "id": "5E91213C",
+            "instance": 0,
+            "name": "EKFConfiguration",
+            "setting": true
+        }
+    ],
+    "owner": "Jim ",
+    "propeller": "HQ DD 6 X4.5",
+    "servo": "",
+    "size": "330",
+    "subtype": 2,
+    "type": 1,
+    "uuid": "{5bf30e57-f44b-427d-bfd6-9e9980c55302}",
+    "weight": "1003"
+}