Merge remote-tracking branch 'op-revo/james/revo' into revo-next

Conflicts:
	Makefile
	flight/Modules/Attitude/revolution/attitude.c
	flight/Modules/Battery/battery.c
	flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c
	flight/Modules/GPS/GPS.c
	flight/Modules/ManualControl/inc/manualcontrol.h
	flight/Modules/ManualControl/manualcontrol.c
	flight/Modules/OveroSync/overosync.c
	flight/Modules/PathPlanner/inc/pathplanner.h
	flight/Modules/PathPlanner/pathplanner.c
	flight/Modules/Sensors/sensors.c
	flight/Modules/VtolPathFollower/vtolpathfollower.c
	flight/PiOS/Boards/STM32F4xx_Revolution.h
	flight/PiOS/Boards/pios_board.h
	flight/PiOS/STM32F4xx/library.mk
	flight/PiOS/inc/pios_hmc5883.h
	flight/PiOS/inc/pios_l3gd20.h
	flight/PiOS/inc/pios_rfm22b_priv.h
	flight/Revolution/Makefile
	flight/Revolution/Makefile.osx
	flight/Revolution/System/inc/pios_config.h
	flight/Revolution/UAVObjects.inc
	ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp
	ground/openpilotgcs/src/libs/utils/homelocationutil.cpp
	ground/openpilotgcs/src/libs/utils/homelocationutil.h
	ground/openpilotgcs/src/plugins/config/configrevowidget.cpp
	ground/openpilotgcs/src/plugins/hitlnew/il2simulator.cpp
	ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp
	ground/openpilotgcs/src/plugins/plugins.pro
	ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
	package/Makefile
	shared/uavobjectdefinition/fixedwingpathfollowersettings.xml
	shared/uavobjectdefinition/fixedwingpathfollowerstatus.xml
	shared/uavobjectdefinition/flightstatus.xml
	shared/uavobjectdefinition/hwsettings.xml
	shared/uavobjectdefinition/manualcontrolsettings.xml
	shared/uavobjectdefinition/pathdesired.xml
	shared/uavobjectdefinition/vtolpathfollowersettings.xml
	shared/uavobjectdefinition/waypoint.xml

Makefile

@@ -863,7 +863,7 @@ all_$(1)_clean: $$(addsuffix _clean, $$(filter bu_$(1), $$(BU_TARGETS)))
 all_$(1)_clean: $$(addsuffix _clean, $$(filter ef_$(1), $$(EF_TARGETS)))
 endef
 
-ALL_BOARDS := coptercontrol pipxtreme revolution simposix osd
+ALL_BOARDS := coptercontrol pipxtreme revolution revomini simposix osd
 ALL_BOARDS_BU := coptercontrol pipxtreme simposix
 
 # SimPosix only builds on Linux so drop it from the list for
@@ -873,30 +873,19 @@ ALL_BOARDS  := $(filter-out simposix, $(ALL_BOARDS))
 ALL_BOARDS_BU  := $(filter-out simposix, $(ALL_BOARDS_BU))
 endif
 
-# SimPosix only builds on Linux so drop it from the list for
-# all other platforms.
-ifneq ($(UNAME), Linux)
-ALL_BOARDS  := $(filter-out simposix, $(ALL_BOARDS))
-endif
-
 # Friendly names of each board (used to find source tree)
 coptercontrol_friendly := CopterControl
 pipxtreme_friendly     := PipXtreme
 revolution_friendly    := Revolution
+revomini_friendly      := RevoMini
 simposix_friendly      := SimPosix
 osd_friendly           := OSD
 
-# Short hames of each board (used to display board name in parallel builds)
-coptercontrol_short    := 'cc  '
-pipxtreme_short        := 'pipx'
-revolution_short       := 'revo'
-simposix_short         := 'posx'
-osd_short              := 'osd '
-
-# Short hames of each board (used to display board name in parallel builds)
+# Short names of each board (used to display board name in parallel builds)
 coptercontrol_short    := 'cc  '
 pipxtreme_short        := 'pipx'
 revolution_short       := 'revo'
+revomini_short         := 'rm  '
 simposix_short         := 'posx'
 osd_short              := 'osd '
 

androidgcs/AndroidManifest.xml

@@ -43,7 +43,17 @@
 			android:theme="@android:style/Theme.Dialog" />
 		<activity android:name="Logger" android:label="Logger"
 		    android:theme="@android:style/Theme.Dialog" />
+		       		
+		       		
+		       		
+		<activity
+		    android:name="FragmentTester"
+		    android:label="FragmentTester" />
 
+        <activity
+            android:name="OsgViewer"
+            android:label="3DView" />
+		
 		<receiver android:name="TelemetryWidget">
 			<intent-filter>
 				<action android:name="android.appwidget.action.APPWIDGET_UPDATE" />

androidgcs/jni/Android.mk

@@ -0,0 +1,70 @@
+LOCAL_PATH := $(call my-dir)
+
+include $(CLEAR_VARS)
+
+LOCAL_MODULE    := osgNativeLib
+### Main Install dir
+OSG_ANDROID_DIR	        := ../../osg_android_git/osg-install
+LIBDIR 			:= $(OSG_ANDROID_DIR)/obj/local/armeabi
+
+ifeq ($(TARGET_ARCH_ABI),armeabi-v7a)
+	LOCAL_ARM_NEON 	:= true
+	LIBDIR          := $(OSG_ANDROID_DIR)/obj/local/armeabi-v7a
+endif
+
+### Add all source file names to be included in lib separated by a whitespace
+
+LOCAL_C_INCLUDES:= $(OSG_ANDROID_DIR)/include
+LOCAL_CFLAGS    := -Werror -fno-short-enums
+LOCAL_CPPFLAGS  := -DOSG_LIBRARY_STATIC 
+
+LOCAL_LDLIBS    := -llog -lGLESv2 -ldl -lz -lgnustl_static -ljpeg -lpng
+LOCAL_SRC_FILES := osgNativeLib.cpp OsgMainApp.cpp OsgAndroidNotifyHandler.cpp
+LOCAL_LDFLAGS   := -L $(LIBDIR) \
+-losgdb_dds \
+-losgdb_openflight \
+-losgdb_tga \
+-losgdb_rgb \
+-losgdb_osgterrain \
+-losgdb_osg \
+-losgdb_ive \
+-losgdb_3ds \
+-losgdb_jpeg \
+-losgdb_png \
+-losgdb_deprecated_osgviewer \
+-losgdb_deprecated_osgvolume \
+-losgdb_deprecated_osgtext \
+-losgdb_deprecated_osgterrain \
+-losgdb_deprecated_osgsim \
+-losgdb_deprecated_osgshadow \
+-losgdb_deprecated_osgparticle \
+-losgdb_deprecated_osgfx \
+-losgdb_deprecated_osganimation \
+-losgdb_deprecated_osg \
+-losgdb_serializers_osgvolume \
+-losgdb_serializers_osgtext \
+-losgdb_serializers_osgterrain \
+-losgdb_serializers_osgsim \
+-losgdb_serializers_osgshadow \
+-losgdb_serializers_osgparticle \
+-losgdb_serializers_osgmanipulator \
+-losgdb_serializers_osgfx \
+-losgdb_serializers_osganimation \
+-losgdb_serializers_osg \
+-losgViewer \
+-losgVolume \
+-losgTerrain \
+-losgText \
+-losgShadow \
+-losgSim \
+-losgParticle \
+-losgManipulator \
+-losgGA \
+-losgFX \
+-losgDB \
+-losgAnimation \
+-losgUtil \
+-losg \
+-lOpenThreads
+
+include $(BUILD_SHARED_LIBRARY)

androidgcs/jni/Application.mk

@@ -0,0 +1,11 @@
+#ANDROID APPLICATION MAKEFILE
+APP_BUILD_SCRIPT := $(call my-dir)/Android.mk
+#APP_PROJECT_PATH := $(call my-dir)
+
+APP_OPTIM := release
+
+APP_PLATFORM 	:= android-7
+APP_STL 		:= gnustl_static
+APP_CPPFLAGS 	:= -fexceptions -frtti
+APP_ABI 		:= armeabi armeabi-v7a
+APP_MODULES     := osgNativeLib

androidgcs/jni/OsgAndroidNotifyHandler.cpp

@@ -0,0 +1,38 @@
+/*
+ * OsgAndroidNotifyHandler.cpp
+ *
+ *  Created on: 31/05/2011
+ *      Author: Jorge Izquierdo Ciges
+ */
+
+#include "OsgAndroidNotifyHandler.hpp"
+#include <iostream>
+
+void OsgAndroidNotifyHandler::setTag(std::string tag){
+    _tag = tag;
+}
+void OsgAndroidNotifyHandler::notify(osg::NotifySeverity severity, const char *message){
+
+    switch ( severity ) {
+        case osg::DEBUG_FP:
+            __android_log_write(ANDROID_LOG_VERBOSE,_tag.c_str(),message);
+          break;
+        case osg::DEBUG_INFO:
+            __android_log_write(ANDROID_LOG_DEBUG,_tag.c_str(),message);
+          break;
+        case osg::NOTICE:
+        case osg::INFO:
+            __android_log_write(ANDROID_LOG_INFO,_tag.c_str(),message);
+          break;
+        case osg::WARN:
+            __android_log_write(ANDROID_LOG_WARN,_tag.c_str(),message);
+          break;
+        case osg::FATAL:
+        case osg::ALWAYS:
+            __android_log_write(ANDROID_LOG_ERROR,_tag.c_str(),message);
+          break;
+        default:
+            __android_log_write(ANDROID_LOG_DEBUG,_tag.c_str(),message);
+          break;
+    }
+}

androidgcs/jni/OsgAndroidNotifyHandler.hpp

@@ -0,0 +1,26 @@
+/*
+ * OsgAndroidNotifyHandler.hpp
+ *
+ *  Created on: 31/05/2011
+ *      Author: Jorge Izquierdo Ciges
+ */
+
+#ifndef OSGANDROIDNOTIFYHANDLER_HPP_
+#define OSGANDROIDNOTIFYHANDLER_HPP_
+
+#include <android/log.h>
+
+#include <osg/Notify>
+
+#include <string>
+
+class OSG_EXPORT OsgAndroidNotifyHandler : public osg::NotifyHandler
+{
+private:
+    std::string _tag;
+public:
+    void setTag(std::string tag);
+    void notify(osg::NotifySeverity severity, const char *message);
+};
+
+#endif /* OSGANDROIDNOTIFYHANDLER_HPP_ */

androidgcs/jni/OsgMainApp.cpp

@@ -0,0 +1,247 @@
+#include "OsgMainApp.hpp"
+#include <osg/Quat>
+
+OsgMainApp::OsgMainApp(){
+
+    _lodScale = 1.0f;
+    _prevFrame = 0;
+
+    _initialized = false;
+    _clean_scene = false;
+
+}
+OsgMainApp::~OsgMainApp(){
+
+}
+void OsgMainApp::loadModels(){
+    if(_vModelsToLoad.size()==0) return;
+
+    osg::notify(osg::ALWAYS)<<"There are "<<_vModelsToLoad.size()<<" models to load"<<std::endl;
+
+    Model newModel;
+    for(unsigned int i=0; i<_vModelsToLoad.size(); i++){
+        newModel = _vModelsToLoad[i];
+        osg::notify(osg::ALWAYS)<<"Loading: "<<newModel.filename<<std::endl;
+
+        osg::ref_ptr<osg::Node> loadedModel = osgDB::readNodeFile(newModel.filename);
+        if (loadedModel == 0) {
+            osg::notify(osg::ALWAYS)<<"Model not loaded"<<std::endl;
+        } else {
+            osg::notify(osg::ALWAYS)<<"Model loaded"<<std::endl;
+            _vModels.push_back(newModel);
+
+            loadedModel->setName(newModel.name);
+
+            osg::Shader * vshader = new osg::Shader(osg::Shader::VERTEX, gVertexShader );
+            osg::Shader * fshader = new osg::Shader(osg::Shader::FRAGMENT, gFragmentShader );
+
+            osg::Program * prog = new osg::Program;
+            prog->addShader ( vshader );
+            prog->addShader ( fshader );
+
+            loadedModel->getOrCreateStateSet()->setAttribute ( prog );
+
+            // Woohoo leaky code.  This no longer works for multiple models
+            uavAttitudeAndScale = new osg::MatrixTransform();
+
+            // Set the rotation to normal
+            float q[4] = {1.0, 0, 0, 0};
+            setQuat(q);
+
+            osg::MatrixTransform *rotateModelNED = new osg::MatrixTransform();
+            rotateModelNED->setMatrix(osg::Matrixd::rotate(M_PI, osg::Vec3d(0,0,1)));
+            rotateModelNED->addChild( loadedModel );
+
+            uavAttitudeAndScale->addChild( rotateModelNED );
+
+            _root->addChild(uavAttitudeAndScale);
+        }
+    }
+
+    osgViewer::Viewer::Windows windows;
+    _viewer->getWindows(windows);
+    for(osgViewer::Viewer::Windows::iterator itr = windows.begin();itr != windows.end();++itr)
+    {
+      (*itr)->getState()->setUseModelViewAndProjectionUniforms(true);
+      (*itr)->getState()->setUseVertexAttributeAliasing(true);
+    }
+
+    _viewer->setSceneData(NULL);
+    _viewer->setSceneData(_root.get());
+    _manipulator->getNode();
+    _viewer->home();
+
+    _viewer->getDatabasePager()->clear();
+    _viewer->getDatabasePager()->registerPagedLODs(_root.get());
+    _viewer->getDatabasePager()->setUpThreads(3, 1);
+    _viewer->getDatabasePager()->setTargetMaximumNumberOfPageLOD(2);
+    _viewer->getDatabasePager()->setUnrefImageDataAfterApplyPolicy(true, true);
+
+    _vModelsToLoad.clear();
+
+}
+void OsgMainApp::deleteModels(){
+    if(_vModelsToDelete.size()==0) return;
+
+    osg::notify(osg::ALWAYS)<<"There are "<<_vModelsToDelete.size()<<" models to delete"<<std::endl;
+
+    Model modelToDelete;
+    for(unsigned int i=0; i<_vModelsToDelete.size(); i++){
+        modelToDelete = _vModelsToDelete[i];
+        osg::notify(osg::ALWAYS)<<"Deleting: "<<modelToDelete.name<<std::endl;
+
+        for(unsigned int j=_root->getNumChildren(); j>0; j--){
+            osg::ref_ptr<osg::Node> children = _root->getChild(j-1);
+            if(children->getName() == modelToDelete.name){
+                _root->removeChild(children);
+            }
+        }
+
+    }
+
+    _vModelsToDelete.clear();
+    osg::notify(osg::ALWAYS)<<"finished"<<std::endl;
+}
+//Initialization function
+void OsgMainApp::initOsgWindow(int x,int y,int width,int height){
+
+    __android_log_write(ANDROID_LOG_ERROR, "OSGANDROID",
+            "Initializing geometry");
+
+    //Pending
+    _notifyHandler = new OsgAndroidNotifyHandler();
+    _notifyHandler->setTag("Osg Viewer");
+    osg::setNotifyHandler(_notifyHandler);
+    osg::setNotifyLevel(osg::DEBUG_INFO);
+
+    _viewer = new osgViewer::Viewer();
+    _viewer->setUpViewerAsEmbeddedInWindow(x, y, width, height);
+    _viewer->setThreadingModel(osgViewer::ViewerBase::SingleThreaded);
+
+    _root = new osg::Group();
+
+    _viewer->realize();
+
+    _viewer->addEventHandler(new osgViewer::StatsHandler);
+    _viewer->addEventHandler(new osgGA::StateSetManipulator(_viewer->getCamera()->getOrCreateStateSet()));
+    _viewer->addEventHandler(new osgViewer::ThreadingHandler);
+    _viewer->addEventHandler(new osgViewer::LODScaleHandler);
+
+    _manipulator = new osgGA::KeySwitchMatrixManipulator;
+
+    _manipulator->addMatrixManipulator( '1', "Trackball", new osgGA::TrackballManipulator() );
+    _manipulator->addMatrixManipulator( '2', "Flight", new osgGA::FlightManipulator() );
+    _manipulator->addMatrixManipulator( '3', "Drive", new osgGA::DriveManipulator() );
+    _manipulator->addMatrixManipulator( '4', "Terrain", new osgGA::TerrainManipulator() );
+    _manipulator->addMatrixManipulator( '5', "Orbit", new osgGA::OrbitManipulator() );
+    _manipulator->addMatrixManipulator( '6', "FirstPerson", new osgGA::FirstPersonManipulator() );
+    _manipulator->addMatrixManipulator( '7', "Spherical", new osgGA::SphericalManipulator() );
+
+    _viewer->setCameraManipulator( _manipulator.get() );
+
+    _viewer->getViewerStats()->collectStats("scene", true);
+
+    _initialized = true;
+
+}
+//Draw
+void OsgMainApp::draw(){
+    //Every load o remove has to be done before any drawing
+    loadModels();
+    deleteModels();
+
+    _viewer->frame();
+}
+//Events
+void OsgMainApp::mouseButtonPressEvent(float x,float y,int button){
+    _viewer->getEventQueue()->mouseButtonPress(x, y, button);
+}
+void OsgMainApp::mouseButtonReleaseEvent(float x,float y,int button){
+    _viewer->getEventQueue()->mouseButtonRelease(x, y, button);
+}
+void OsgMainApp::mouseMoveEvent(float x,float y){
+    _viewer->getEventQueue()->mouseMotion(x, y);
+}
+void OsgMainApp::keyboardDown(int key){
+    _viewer->getEventQueue()->keyPress(key);
+}
+void OsgMainApp::keyboardUp(int key){
+    _viewer->getEventQueue()->keyRelease(key);
+}
+//Loading and unloading
+void OsgMainApp::loadObject(std::string filePath){
+    Model newModel;
+    newModel.filename = filePath;
+    newModel.name = filePath;
+
+    int num = 0;
+    for(unsigned int i=0;i<_vModels.size();i++){
+        if(_vModels[i].name==newModel.name)
+            return;
+    }
+
+    _vModelsToLoad.push_back(newModel);
+
+}
+void OsgMainApp::loadObject(std::string name,std::string filePath){
+
+    Model newModel;
+    newModel.filename = filePath;
+    newModel.name = name;
+
+    for(unsigned int i=0;i<_vModels.size();i++){
+        if(_vModels[i].name==newModel.name){
+            osg::notify(osg::ALWAYS)<<"Name already used"<<std::endl;
+            return;
+        }
+    }
+
+    _vModelsToLoad.push_back(newModel);
+}
+void OsgMainApp::unLoadObject(int number){
+    if(_vModels.size() <= number){
+        osg::notify(osg::FATAL)<<"Index number error"<<std::endl;
+        return;
+    }
+
+    Model modelToDelete = _vModels[number];
+    _vModels.erase(_vModels.begin()+number);
+    _vModelsToDelete.push_back(modelToDelete);
+}
+void OsgMainApp::clearScene(){
+    _vModelsToDelete = _vModels;
+    _vModels.clear();
+}
+//Other Functions
+int OsgMainApp::getNumberObjects(){
+    return _vModels.size();
+}
+std::string OsgMainApp::getObjectName(int number){
+    return _vModels[number].name;
+}
+void OsgMainApp::setClearColor(osg::Vec4f color){
+    osg::notify(osg::ALWAYS)<<"Setting Clear Color"<<std::endl;
+    _viewer->getCamera()->setClearColor(color);
+}
+osg::Vec4f OsgMainApp::getClearColor(){
+    osg::notify(osg::ALWAYS)<<"Getting Clear Color"<<std::endl;
+    return _viewer->getCamera()->getClearColor();
+}
+
+void OsgMainApp::setQuat(float *q){
+    osg::Quat quat(q[1], q[2], q[3], q[0]);
+
+    // Have to rotate the axes from OP NED frame to OSG frame (X east, Y north, Z down)
+    double angle;
+    osg::Vec3d axis;
+    quat.getRotate(angle,axis);
+    quat.makeRotate(angle, osg::Vec3d(axis[1],axis[0],-axis[2]));
+    osg::Matrixd rot = osg::Matrixd::rotate(quat);
+
+    if (uavAttitudeAndScale) {
+    	osg::notify(osg::ALWAYS)<<"Setting attitude"<<std::endl;
+    	uavAttitudeAndScale->setMatrix(rot);
+    } else {
+    	osg::notify(osg::ALWAYS)<<"Pointer missing"<<std::endl;
+    }
+}

androidgcs/jni/OsgMainApp.hpp

@@ -0,0 +1,191 @@
+/*
+ * OsgMainApp.hpp
+ *
+ *  Created on: 29/05/2011
+ *      Author: Jorge Izquierdo Ciges
+ */
+
+#ifndef OSGMAINAPP_HPP_
+#define OSGMAINAPP_HPP_
+
+//Android log
+#include <android/log.h>
+#include <iostream>
+#include <cstdlib>
+#include <math.h>
+
+//Standard libraries
+#include <string>
+
+//osg
+#include <osg/GL>
+#include <osg/GLExtensions>
+#include <osg/Depth>
+#include <osg/Program>
+#include <osg/Shader>
+#include <osg/MatrixTransform>
+#include <osg/Node>
+#include <osg/Notify>
+//osgText
+#include <osgText/Text>
+//osgDB
+#include <osgDB/DatabasePager>
+#include <osgDB/Registry>
+#include <osgDB/ReadFile>
+#include <osgDB/WriteFile>
+//osg_viewer
+#include <osgViewer/Viewer>
+#include <osgViewer/Renderer>
+#include <osgViewer/ViewerEventHandlers>
+//osgGA
+#include <osgGA/GUIEventAdapter>
+#include <osgGA/MultiTouchTrackballManipulator>
+#include <osgGA/TrackballManipulator>
+#include <osgGA/FlightManipulator>
+#include <osgGA/DriveManipulator>
+#include <osgGA/KeySwitchMatrixManipulator>
+#include <osgGA/StateSetManipulator>
+#include <osgGA/AnimationPathManipulator>
+#include <osgGA/TerrainManipulator>
+#include <osgGA/SphericalManipulator>
+//Self headers
+#include "OsgAndroidNotifyHandler.hpp"
+
+//Static plugins Macro
+USE_OSGPLUGIN(ive)
+USE_OSGPLUGIN(osg)
+USE_OSGPLUGIN(osg2)
+USE_OSGPLUGIN(terrain)
+USE_OSGPLUGIN(rgb)
+USE_OSGPLUGIN(OpenFlight)
+USE_OSGPLUGIN(dds)
+USE_OSGPLUGIN(3ds)
+USE_OSGPLUGIN(jpeg)
+USE_OSGPLUGIN(png)
+
+//Static DOTOSG
+USE_DOTOSGWRAPPER_LIBRARY(osg)
+USE_DOTOSGWRAPPER_LIBRARY(osgFX)
+USE_DOTOSGWRAPPER_LIBRARY(osgParticle)
+USE_DOTOSGWRAPPER_LIBRARY(osgTerrain)
+USE_DOTOSGWRAPPER_LIBRARY(osgText)
+USE_DOTOSGWRAPPER_LIBRARY(osgViewer)
+USE_DOTOSGWRAPPER_LIBRARY(osgVolume)
+//Static serializer
+USE_SERIALIZER_WRAPPER_LIBRARY(osg)
+USE_SERIALIZER_WRAPPER_LIBRARY(osgAnimation)
+USE_SERIALIZER_WRAPPER_LIBRARY(osgFX)
+USE_SERIALIZER_WRAPPER_LIBRARY(osgManipulator)
+USE_SERIALIZER_WRAPPER_LIBRARY(osgParticle)
+USE_SERIALIZER_WRAPPER_LIBRARY(osgTerrain)
+USE_SERIALIZER_WRAPPER_LIBRARY(osgText)
+USE_SERIALIZER_WRAPPER_LIBRARY(osgVolume)
+
+#define  LOG_TAG    "osgNativeLib"
+#define  LOGI(...)  __android_log_print(ANDROID_LOG_INFO,LOG_TAG,__VA_ARGS__)
+#define  LOGE(...)  __android_log_print(ANDROID_LOG_ERROR,LOG_TAG,__VA_ARGS__)
+
+struct Model{
+    std::string filename;
+    std::string name;
+};
+
+static const char gVertexShader[] =
+    "varying vec4 color;                                                    \n"
+    "const vec3 lightPos      =vec3(0.0, 0.0, 10.0);                        \n"
+    "const vec4 cessnaColor   =vec4(0.8, 0.8, 0.8, 1.0);                    \n"
+    "const vec4 lightAmbient  =vec4(0.1, 0.1, 0.1, 1.0);                    \n"
+    "const vec4 lightDiffuse  =vec4(0.4, 0.4, 0.4, 1.0);                    \n"
+    "const vec4 lightSpecular =vec4(0.8, 0.8, 0.8, 1.0);                    \n"
+    "void DirectionalLight(in vec3 normal,                                  \n"
+    "                      in vec3 ecPos,                                   \n"
+    "                      inout vec4 ambient,                              \n"
+    "                      inout vec4 diffuse,                              \n"
+    "                      inout vec4 specular)                             \n"
+    "{                                                                      \n"
+    "     float nDotVP;                                                     \n"
+    "     vec3 L = normalize(gl_ModelViewMatrix*vec4(lightPos, 0.0)).xyz;   \n"
+    "     nDotVP = max(0.0, dot(normal, L));                                \n"
+    "                                                                       \n"
+    "     if (nDotVP > 0.0) {                                               \n"
+    "       vec3 E = normalize(-ecPos);                                     \n"
+    "       vec3 R = normalize(reflect( L, normal ));                       \n"
+    "       specular = pow(max(dot(R, E), 0.0), 16.0) * lightSpecular;      \n"
+    "     }                                                                 \n"
+    "     ambient  = lightAmbient;                                          \n"
+    "     diffuse  = lightDiffuse * nDotVP;                                 \n"
+    "}                                                                      \n"
+    "void main() {                                                          \n"
+    "    vec4 ambiCol = vec4(0.0);                                          \n"
+    "    vec4 diffCol = vec4(0.0);                                          \n"
+    "    vec4 specCol = vec4(0.0);                                          \n"
+    "    gl_Position   = gl_ModelViewProjectionMatrix * gl_Vertex;          \n"
+    "    vec3 normal   = normalize(gl_NormalMatrix * gl_Normal);            \n"
+    "    vec4 ecPos    = gl_ModelViewMatrix * gl_Vertex;                    \n"
+    "    DirectionalLight(normal, ecPos.xyz, ambiCol, diffCol, specCol);    \n"
+    "    color = cessnaColor * (ambiCol + diffCol + specCol);               \n"
+    "}                                                                      \n";
+
+static const char gFragmentShader[] =
+    "precision mediump float;                  \n"
+    "varying mediump vec4 color;               \n"
+    "void main() {                             \n"
+    "  gl_FragColor = color;                   \n"
+    "}                                         \n";
+
+
+class OsgMainApp{
+private:
+    osg::ref_ptr<osgViewer::Viewer> _viewer;
+    osg::ref_ptr<osg::Group> _root;
+    osg::ref_ptr<osg::StateSet> _state;
+    osg::ref_ptr<osgGA::KeySwitchMatrixManipulator> _manipulator;
+
+    float _lodScale;
+    unsigned int _prevFrame;
+
+    bool _initialized;
+    bool _clean_scene;
+
+    OsgAndroidNotifyHandler *_notifyHandler;
+
+    std::vector<Model> _vModels;
+    std::vector<Model> _vModelsToLoad;
+    std::vector<Model> _vModelsToDelete;
+
+    void loadModels();
+    void deleteModels();
+
+public:
+    OsgMainApp();
+    ~OsgMainApp();
+
+    //Initialization function
+    void initOsgWindow(int x,int y,int width,int height);
+    //Draw
+    void draw();
+    //Events
+    void mouseButtonPressEvent(float x,float y,int button);
+    void mouseButtonReleaseEvent(float x,float y,int button);
+    void mouseMoveEvent(float x,float y);
+    void keyboardDown(int key);
+    void keyboardUp(int key);
+    //Loading and unloading
+    void loadObject(std::string filePath);
+    void loadObject(std::string name,std::string filePath);
+    void unLoadObject(int number);
+    void clearScene();
+    //Other functions
+    int  getNumberObjects();
+    std::string getObjectName(int nunmber);
+
+    void setClearColor(osg::Vec4f color);
+    osg::Vec4f getClearColor();
+
+    //Manipulating the view
+    void setQuat(float *q);
+    osg::MatrixTransform *uavAttitudeAndScale;
+};
+
+
+#endif /* OSGMAINAPP_HPP_ */

androidgcs/jni/osgNativeLib.cpp

@@ -0,0 +1,119 @@
+#include <string.h>
+#include <jni.h>
+#include <android/log.h>
+
+#include <iostream>
+
+#include "OsgMainApp.hpp"
+
+OsgMainApp mainApp;
+
+extern "C" {
+    JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_init(JNIEnv * env, jobject obj, jint width, jint height);
+    JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_step(JNIEnv * env, jobject obj);
+    JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_clearContents(JNIEnv * env, jobject obj);
+    JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_mouseButtonPressEvent(JNIEnv * env, jobject obj, jfloat x, jfloat y, jint button);
+    JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_mouseButtonReleaseEvent(JNIEnv * env, jobject obj, jfloat x, jfloat y, jint button);
+    JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_mouseMoveEvent(JNIEnv * env, jobject obj, jfloat x, jfloat y);
+    JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_keyboardDown(JNIEnv * env, jobject obj, jint key);
+    JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_keyboardUp(JNIEnv * env, jobject obj, jint key);
+    JNIEXPORT jintArray JNICALL Java_org_openpilot_osg_osgNativeLib_getClearColor(JNIEnv * env, jobject obj);
+    JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_setClearColor(JNIEnv * env, jobject obj, jint red, jint green, jint blue);
+    JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_loadObject(JNIEnv * env, jobject obj, jstring address);
+    JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_unLoadObject(JNIEnv * env, jobject obj, jint number);
+    JNIEXPORT jobjectArray JNICALL Java_org_openpilot_osg_osgNativeLib_getObjectNames(JNIEnv * env, jobject obj);
+    JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_setQuat(JNIEnv * env, jobject obj, jfloat q1, jfloat q2, jfloat q3, jfloat q4);
+};
+
+JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_init(JNIEnv * env, jobject obj, jint width, jint height){
+    mainApp.initOsgWindow(0,0,width,height);
+}
+JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_step(JNIEnv * env, jobject obj){
+    mainApp.draw();
+}
+JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_clearContents(JNIEnv * env, jobject obj){
+    mainApp.clearScene();
+}
+JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_mouseButtonPressEvent(JNIEnv * env, jobject obj, jfloat x, jfloat y, jint button){
+    mainApp.mouseButtonPressEvent(x,y,button);
+}
+JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_mouseButtonReleaseEvent(JNIEnv * env, jobject obj, jfloat x, jfloat y, jint button){
+    mainApp.mouseButtonReleaseEvent(x,y,button);
+}
+JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_mouseMoveEvent(JNIEnv * env, jobject obj, jfloat x, jfloat y){
+    mainApp.mouseMoveEvent(x,y);
+}
+JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_keyboardDown(JNIEnv * env, jobject obj, jint key){
+    mainApp.keyboardDown(key);
+}
+JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_keyboardUp(JNIEnv * env, jobject obj, jint key){
+    mainApp.keyboardUp(key);
+}
+JNIEXPORT jintArray JNICALL Java_org_openpilot_osg_osgNativeLib_getClearColor(JNIEnv * env, jobject obj){
+
+    jintArray color;
+    color = env->NewIntArray(3);
+    if (color == NULL) {
+        return NULL;
+    }
+    osg::Vec4 vTemp1 = mainApp.getClearColor();
+
+    jint vTemp2[3];
+
+    vTemp2[0] = (int) (vTemp1.r() * 255);
+    vTemp2[1] = (int) (vTemp1.g() * 255);
+    vTemp2[2] = (int) (vTemp1.b() * 255);
+
+    std::cout<<vTemp2[0]<<" "<<vTemp2[1]<<" "<<vTemp2[2]<<" "<<std::endl;
+
+    env->SetIntArrayRegion(color, 0, 3, vTemp2);
+
+    return color;
+}
+JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_setClearColor(JNIEnv * env, jobject obj, jint red, jint green, jint blue){
+    osg::Vec4 tVec((float) red / 255.0f, (float) green / 255.0f, (float) blue / 255.0f, 0.0f);
+    mainApp.setClearColor(tVec);
+}
+JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_loadObject(JNIEnv * env, jobject obj, jstring address){
+    //Import Strings from JNI
+    const char *nativeAddress = env->GetStringUTFChars(address, JNI_FALSE);
+
+    mainApp.loadObject(std::string(nativeAddress));
+
+    //Release Strings to JNI
+    env->ReleaseStringUTFChars(address, nativeAddress);
+}
+JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_loadObject(JNIEnv * env, jobject obj, jstring address, jstring name){
+    //Import Strings from JNI
+    const char *nativeAddress = env->GetStringUTFChars(address, JNI_FALSE);
+    const char *nativeName = env->GetStringUTFChars(name, JNI_FALSE);
+
+    mainApp.loadObject(std::string(nativeName),std::string(nativeAddress));
+
+    //Release Strings to JNI
+    env->ReleaseStringUTFChars(address, nativeAddress);
+    env->ReleaseStringUTFChars(address, nativeName);
+}
+JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_unLoadObject(JNIEnv * env, jobject obj, jint number){
+
+    mainApp.unLoadObject(number);
+
+}
+JNIEXPORT jobjectArray JNICALL Java_org_openpilot_osg_osgNativeLib_getObjectNames(JNIEnv * env, jobject obj){
+
+    jobjectArray fileNames;
+    unsigned int numModels = mainApp.getNumberObjects();
+    fileNames = (jobjectArray)env->NewObjectArray(numModels,env->FindClass("java/lang/String"),env->NewStringUTF(""));
+
+    for(unsigned int i=0;i < numModels;i++){
+        std::string name = mainApp.getObjectName(i);
+        env->SetObjectArrayElement(fileNames,i,env->NewStringUTF(name.c_str()));
+    }
+
+    return fileNames;
+}
+
+JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_setQuat(JNIEnv * env, jobject obj, jfloat q1, jfloat q2, jfloat q3, jfloat q4){
+	float q[4] = {q1, q2, q3, q4};
+	mainApp.setQuat(q);
+}

androidgcs/res/layout-w600dp/gcs_home.xml

@@ -46,7 +46,6 @@
         android:layout_width="132dp"
         android:layout_height="wrap_content"
         android:layout_column="4"
-        android:layout_columnSpan="2"
         android:layout_gravity="right|bottom"
         android:layout_row="2"
         android:layout_rowSpan="2"
@@ -54,19 +53,6 @@
         android:drawableTop="@drawable/ic_tuning"
         android:text="@string/tuning" />
 
-    <Button
-        android:id="@+id/launch_alarms"
-        android:layout_width="132dp"
-        android:layout_height="wrap_content"
-        android:layout_column="1"
-        android:layout_columnSpan="2"
-        android:layout_gravity="left|bottom"
-        android:layout_row="3"
-        android:layout_rowSpan="3"
-        android:background="@android:color/transparent"
-        android:drawableTop="@drawable/ic_alarms"
-        android:text="@string/alarms" />
-
     <Button
         android:id="@+id/launch_logger"
         android:layout_width="132dp"
@@ -80,16 +66,47 @@
         android:text="@string/logger_name" />
 
     <Button
-        android:id="@+id/launch_pfd"
+        android:id="@+id/launch_alarms"
         android:layout_width="132dp"
         android:layout_height="wrap_content"
         android:layout_column="0"
         android:layout_gravity="left"
         android:layout_row="5"
         android:background="@android:color/transparent"
+        android:drawableTop="@drawable/ic_alarms"
+        android:text="@string/alarms" />
+
+    <Button
+        android:id="@+id/launch_pfd"
+        android:layout_width="132dp"
+        android:layout_height="wrap_content"
+        android:layout_column="2"
+        android:layout_gravity="left"
+        android:layout_row="5"
+        android:background="@android:color/transparent"
         android:drawableTop="@drawable/ic_pfd"
         android:text="@string/pfd_name" />
 
+    <Button
+	     android:id="@+id/launch_osgViewer"
+	     android:layout_width="132dp"
+	     android:layout_height="wrap_content"
+	     android:layout_column="0"
+	     android:layout_gravity="right"
+	     android:layout_row="9"
+	     android:background="@android:color/transparent"
+	     android:drawableTop="@drawable/ic_alarms"
+	     android:text="@string/_3dview" />
+    
+    <Button
+        android:id="@+id/launch_tester"
+        android:layout_width="132dp"
+        android:layout_height="wrap_content"
+        android:layout_column="2"
+        android:layout_row="9"
+        android:background="@android:color/transparent"
+        android:drawableTop="@drawable/ic_alarms"
+        android:text="@string/tester" />
 
     <Space
         android:layout_width="1dp"
@@ -103,4 +120,19 @@
         android:layout_column="0"
         android:layout_row="4" />
 
+    <Space
+        android:layout_width="1dp"
+        android:layout_height="47dp"
+        android:layout_column="0"
+        android:layout_row="8" />
+    
+    <Space
+        android:layout_height="15dp"
+        android:layout_column="1" />
+
+    <Space
+        android:layout_width="100dp"
+        android:layout_height="2dp"
+        android:layout_row="6" />
+
 </GridLayout>

androidgcs/res/layout/gcs_home.xml

@@ -78,6 +78,29 @@
         android:text="@string/alarms" />
 
     <Button
+        android:id="@+id/launch_tester"
+        android:layout_width="132dp"
+        android:layout_height="wrap_content"
+        android:layout_row="8"
+        android:layout_column="0"
+        android:layout_rowSpan="2"
+        android:background="@android:color/transparent"
+        android:drawableTop="@drawable/ic_alarms"
+        android:text="@string/tester" />    
+
+	 <Button
+	     android:id="@+id/launch_osgViewer"
+	     android:layout_width="132dp"
+	     android:layout_height="wrap_content"
+	     android:layout_column="1"
+	     android:layout_gravity="right"
+	     android:layout_row="8"
+	     android:layout_rowSpan="2"
+	     android:background="@android:color/transparent"
+	     android:drawableTop="@drawable/ic_alarms"
+	     android:text="@string/_3dview" />
+        
+	<Button
         android:id="@+id/launch_tuning"
         android:layout_width="132dp"
         android:layout_height="wrap_content"
@@ -100,4 +123,4 @@
         android:layout_height="69dp"
         android:layout_row="1" />
 
-</GridLayout>
+</GridLayout>

androidgcs/res/layout/map_positioner.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0" encoding="utf-8"?>
+<LinearLayout xmlns:android="http://schemas.android.com/apk/res/android"
+    android:layout_width="match_parent"
+    android:layout_height="match_parent"
+    android:orientation="vertical" >
+
+    <SeekBar
+        android:id="@+id/altitude_slider"
+        android:layout_width="match_parent"
+        android:layout_height="wrap_content"
+        android:max="50" />
+    
+    <FrameLayout
+        android:id="@+id/map_view"
+        android:layout_width="match_parent"
+        android:layout_height="match_parent"
+        android:background="@drawable/map_positioner_background" >
+    </FrameLayout>
+
+</LinearLayout>

androidgcs/res/layout/system_alarms.xml

@@ -16,11 +16,4 @@
         android:layout_width="wrap_content"
         android:layout_height="match_parent" />
     
-    <TextView
-        xmlns:android="http://schemas.android.com/apk/res/android"
-        android:id="@+id/system_alarms_status"
-        android:layout_width="fill_parent"
-        android:layout_height="fill_parent"
-        android:layout_gravity="center" />
-
-</LinearLayout>
+</LinearLayout>

androidgcs/res/layout/ui_layout_gles.xml

@@ -0,0 +1,44 @@
+<?xml version="1.0" encoding="utf-8"?>
+<LinearLayout
+  xmlns:android="http://schemas.android.com/apk/res/android"
+  android:orientation="vertical"
+  android:layout_width="fill_parent"
+  android:layout_height="fill_parent">
+  <FrameLayout
+  	android:layout_width="fill_parent"
+  	android:layout_height="fill_parent"
+  	android:layout_weight="1">
+  	<org.openpilot.osg.EGLview android:id="@+id/surfaceGLES"
+  		android:visibility="visible"
+  		android:layout_width="fill_parent"
+  		android:layout_height="fill_parent"
+  	/>
+  </FrameLayout>
+  <RelativeLayout android:id="@+id/uiNavigation"
+  	android:gravity="center"
+	android:layout_width="fill_parent"
+	android:layout_height="wrap_content">
+	
+	<LinearLayout
+		android:layout_centerInParent="true"
+		android:orientation="horizontal"
+		android:layout_width="wrap_content"
+		android:layout_height="wrap_content"
+		android:gravity="center">
+		<Button android:id="@+id/uiButtonLight"
+					android:layout_width="wrap_content"
+					android:layout_height="wrap_content"
+					android:text="light"/>
+		<Button android:id="@+id/uiButtonCenter"
+					android:layout_width="wrap_content"
+					android:layout_height="wrap_content"
+					android:text="center"/>
+		<Button android:id="@+id/uiButtonChangeNavigation"
+					android:layout_width="wrap_content"
+					android:layout_height="wrap_content"
+					android:text="navigation"/>
+	</LinearLayout>
+	
+	</RelativeLayout>
+  	
+</LinearLayout>

androidgcs/res/values/strings.xml

@@ -32,6 +32,8 @@
 	<string name="alarms">Alarms</string>
 	<string name="txrate">TxRate: </string>
 	<string name="rxrate">RxRate: </string>
+	<string name="tester">Tester</string>
+	<string name="_3dview">3DView</string>
 	<string name="tuning">Tuning</string>
 	<string name="apply">Apply</string>
 	<string name="save">Save</string>

androidgcs/src/org/openpilot/androidgcs/BluetoothUAVTalk.java

@@ -0,0 +1,154 @@
+package org.openpilot.androidgcs;
+
+import java.io.IOException;
+import java.util.Set;
+import java.util.UUID;
+
+import org.openpilot.uavtalk.UAVObjectManager;
+import org.openpilot.uavtalk.UAVTalk;
+
+import android.annotation.TargetApi;
+import android.app.Activity;
+import android.bluetooth.BluetoothAdapter;
+import android.bluetooth.BluetoothDevice;
+import android.bluetooth.BluetoothSocket;
+import android.content.BroadcastReceiver;
+import android.content.Context;
+import android.content.Intent;
+import android.content.SharedPreferences;
+import android.preference.PreferenceManager;
+import android.util.Log;
+
+@TargetApi(10) public class BluetoothUAVTalk {
+	private final String TAG = "BluetoothUAVTalk";
+	public static int LOGLEVEL = 2;
+	public static boolean WARN = LOGLEVEL > 1;
+	public static boolean DEBUG = LOGLEVEL > 0;
+	
+	// Temporarily define fixed device name
+	private String device_name = "RN42-222D";
+	private final static UUID MY_UUID = UUID.fromString("00001101-0000-1000-8000-00805F9B34FB");
+	
+	private BluetoothAdapter mBluetoothAdapter;
+	private BluetoothSocket socket;
+	private BluetoothDevice device;
+	private UAVTalk uavTalk;
+	private boolean connected; 
+	
+	public BluetoothUAVTalk(Context caller) {
+		
+		SharedPreferences prefs = PreferenceManager.getDefaultSharedPreferences(caller);
+		device_name = prefs.getString("bluetooth_mac","");
+
+        if (DEBUG) Log.d(TAG, "Trying to open UAVTalk with " + device_name);
+
+        connected = false;
+        device = null;
+        
+        mBluetoothAdapter = BluetoothAdapter.getDefaultAdapter();
+        if (mBluetoothAdapter == null) {
+            // Device does not support Bluetooth
+        	Log.e(TAG, "Device does not support Bluetooth");
+        	return;
+        }
+        
+        if (!mBluetoothAdapter.isEnabled()) {
+        	// Enable bluetooth if it isn't already
+            Intent enableBtIntent = new Intent(BluetoothAdapter.ACTION_REQUEST_ENABLE);
+            caller.sendOrderedBroadcast(enableBtIntent, "android.permission.BLUETOOTH_ADMIN", new BroadcastReceiver() {
+				@Override
+				public void onReceive(Context context, Intent intent) {
+					Log.e(TAG,"Received " + context + intent);		
+					//TODO: some logic here to see if it worked
+					queryDevices();
+				}            	
+            }, null, Activity.RESULT_OK, null, null);
+        } else {
+        	queryDevices();
+        }        		
+    }
+	
+	public boolean connect(UAVObjectManager objMngr) {
+		if( getConnected() ) 
+			return true;
+		if( !getFoundDevice() )
+			return false;		
+		if( !openTelemetryBluetooth(objMngr) )
+			return false;
+		return true;		
+	}
+
+	public boolean getConnected() {
+		return connected;
+	}
+    
+	public boolean getFoundDevice() {
+		return (device != null);
+	}
+	
+	public UAVTalk getUavtalk() {
+		return uavTalk;
+	}
+	
+    private void queryDevices() {
+    	Log.d(TAG, "Searching for devices");
+		Set<BluetoothDevice> pairedDevices = mBluetoothAdapter.getBondedDevices();
+		// If there are paired devices
+		if (pairedDevices.size() > 0) {
+		    // Loop through paired devices
+		    for (BluetoothDevice device : pairedDevices) {
+		        // Add the name and address to an array adapter to show in a ListView
+		        //mArrayAdapter.add(device.getName() + "\n" + device.getAddress());
+		    	Log.d(TAG, "Paired device: " + device.getAddress() + " compared to " + device_name);
+		    	if(device.getAddress().compareTo(device_name) == 0) {
+		    		Log.d(TAG, "Found device: " + device.getName());
+		    		this.device = device;
+		    		return;
+		    	}
+		    }
+		}
+    	
+    }
+
+	private boolean openTelemetryBluetooth(UAVObjectManager objMngr) {
+		Log.d(TAG, "Opening connection to " + device.getName());
+		socket = null;
+		connected = false;
+		try {
+			socket = device.createInsecureRfcommSocketToServiceRecord(MY_UUID);
+		} catch (IOException e) {
+			Log.e(TAG,"Unable to create Rfcomm socket");
+			return false;
+			//e.printStackTrace();			
+		}
+		
+		mBluetoothAdapter.cancelDiscovery();
+		
+		try {
+			socket.connect();
+		}
+		catch (IOException e) {
+			Log.e(TAG,"Unable to connect to requested device", e);
+            try {
+                socket.close();
+            } catch (IOException e2) {
+                Log.e(TAG, "unable to close() socket during connection failure", e2);
+            }
+			return false;
+		}
+
+		connected = true;
+		
+		try {
+			uavTalk = new UAVTalk(socket.getInputStream(), socket.getOutputStream(), objMngr);
+		} catch (IOException e) {
+			Log.e(TAG,"Error starting UAVTalk");
+			// TODO Auto-generated catch block
+			//e.printStackTrace();
+			return false;
+		}
+				
+		return true;
+	}
+
+}

androidgcs/src/org/openpilot/androidgcs/FragmentTester.java

@@ -0,0 +1,57 @@
+/**
+ ******************************************************************************
+ * @file       FragmentTester.java
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @brief      An activity that displays the SystemAlarmsFragment.
+ * @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
+ */
+package org.openpilot.androidgcs;
+
+import org.openpilot.androidgcs.fragments.MapPositioner;
+
+import android.app.FragmentTransaction;
+import android.os.Bundle;
+import android.view.View;
+import android.widget.AbsListView;
+import android.widget.LinearLayout;
+
+public class FragmentTester extends ObjectManagerActivity {
+	@Override
+	public void onCreate(Bundle savedInstanceState) {
+		super.onCreate(savedInstanceState);
+		setContentView(createUI());
+	}
+
+	private View createUI() {
+		LinearLayout layout = new LinearLayout(this);
+
+		layout.setOrientation(LinearLayout.HORIZONTAL);
+		layout.setLayoutParams(new LinearLayout.LayoutParams(
+				AbsListView.LayoutParams.MATCH_PARENT,
+				AbsListView.LayoutParams.MATCH_PARENT));
+		layout.setId(0x101);
+		{
+			FragmentTransaction fragmentTransaction = getFragmentManager()
+					.beginTransaction();
+			fragmentTransaction.add(0x101, new MapPositioner());
+			fragmentTransaction.commit();
+		}
+		return layout;
+	}
+}
+

androidgcs/src/org/openpilot/androidgcs/HomePage.java

@@ -84,6 +84,22 @@ public class HomePage extends ObjectManagerActivity {
 			}
 		});
 
+		Button tester = (Button) findViewById(R.id.launch_tester);
+		tester.setOnClickListener(new OnClickListener() {
+			@Override
+			public void onClick(View arg0) {
+				startActivity(new Intent(HomePage.this, FragmentTester.class));
+			}
+		});
+
+		Button osgViewer = (Button) findViewById(R.id.launch_osgViewer);
+		osgViewer.setOnClickListener(new OnClickListener() {
+			@Override
+			public void onClick(View arg0) {
+				startActivity(new Intent(HomePage.this, OsgViewer.class));
+			}
+		});
+
 		Button tuning = (Button) findViewById(R.id.launch_tuning);
 		tuning.setOnClickListener(new OnClickListener() {
 			@Override

androidgcs/src/org/openpilot/androidgcs/ObjectManagerActivity.java

@@ -82,7 +82,7 @@ public abstract class ObjectManagerActivity extends Activity {
 	private HashMap<Observer, UAVObject> listeners;
 	/** Called when the activity is first created. */
 	@Override
-	public void onCreate(Bundle savedInstanceState) {
+	protected void onCreate(Bundle savedInstanceState) {
 		super.onCreate(savedInstanceState);
 	}
 
@@ -254,6 +254,7 @@ public abstract class ObjectManagerActivity extends Activity {
 		// Bind to the telemetry service (which will start it)
 		Intent intent = new Intent(getApplicationContext(),
 				org.openpilot.androidgcs.telemetry.OPTelemetryService.class);
+		startService(intent);
 		if (DEBUG)
 			Log.d(TAG, "Attempting to bind: " + intent);
 		bindService(intent, mConnection, Context.BIND_AUTO_CREATE);

androidgcs/src/org/openpilot/androidgcs/OsgViewer.java

@@ -0,0 +1,323 @@
+package org.openpilot.androidgcs;
+
+import org.openpilot.osg.ColorPickerDialog;
+import org.openpilot.osg.EGLview;
+import org.openpilot.osg.osgNativeLib;
+import org.openpilot.uavtalk.UAVObject;
+
+import android.app.AlertDialog;
+import android.graphics.Color;
+import android.graphics.PointF;
+import android.os.Bundle;
+import android.os.Environment;
+import android.util.FloatMath;
+import android.util.Log;
+import android.view.KeyEvent;
+import android.view.MotionEvent;
+import android.view.View;
+import android.view.View.OnClickListener;
+import android.widget.Button;
+import android.widget.ImageButton;
+import android.widget.Toast;
+
+public class OsgViewer extends ObjectManagerActivity implements View.OnTouchListener, View.OnKeyListener, ColorPickerDialog.OnColorChangeListener {
+	private static final String TAG = OsgViewer.class.getSimpleName();
+	private static final int LOGLEVEL = 2;
+//	private static boolean WARN = LOGLEVEL > 1;
+	private static final boolean DEBUG = LOGLEVEL > 0;
+
+
+	enum moveTypes { NONE , DRAG, MDRAG, ZOOM ,ACTUALIZE}
+	enum navType { PRINCIPAL , SECONDARY }
+	enum lightType { ON , OFF }
+
+	moveTypes mode=moveTypes.NONE;
+	navType navMode = navType.PRINCIPAL;
+	lightType lightMode = lightType.ON;
+
+	PointF oneFingerOrigin = new PointF(0,0);
+	long timeOneFinger=0;
+	PointF twoFingerOrigin = new PointF(0,0);
+	long timeTwoFinger=0;
+	float distanceOrigin;
+
+	int backgroundColor;
+
+	//Ui elements
+    EGLview mView;
+    Button uiCenterViewButton;
+    Button uiNavigationChangeButton;
+    ImageButton uiNavigationLeft;
+    ImageButton uiNavigationRight;
+    Button uiLightChangeButton;
+
+    //Toasts
+    Toast msgUiNavPrincipal;
+    Toast msgUiNavSecondary;
+    Toast msgUiLightOn;
+    Toast msgUiLightOff;
+
+    //Dialogs
+    AlertDialog removeLayerDialog;
+    AlertDialog loadLayerAddress;
+
+    //Main Android Activity life cycle
+    @Override protected void onCreate(Bundle icicle) {
+        super.onCreate(icicle);
+        setContentView(R.layout.ui_layout_gles);
+        //Obtain every Ui element
+	        mView= (EGLview) findViewById(R.id.surfaceGLES);
+		        mView.setOnTouchListener(this);
+		        mView.setOnKeyListener(this);
+
+	        uiCenterViewButton = (Button) findViewById(R.id.uiButtonCenter);
+	        	uiCenterViewButton.setOnClickListener(uiListenerCenterView);
+	        uiNavigationChangeButton = (Button) findViewById(R.id.uiButtonChangeNavigation);
+	        	uiNavigationChangeButton.setOnClickListener(uiListenerChangeNavigation);
+	        uiLightChangeButton = (Button) findViewById(R.id.uiButtonLight);
+	        	uiLightChangeButton.setOnClickListener(uiListenerChangeLight);
+
+       	String address = Environment.getExternalStorageDirectory().getPath() + "/Models/J14-QT_X.3DS"; //quad.osg";
+       	Log.d(TAG, "Address: " + address);
+       	osgNativeLib.loadObject(address);
+    }
+    @Override protected void onPause() {
+        super.onPause();
+        mView.onPause();
+    }
+    @Override protected void onResume() {
+        super.onResume();
+        mView.onResume();
+    }
+
+    //Main view event processing
+    @Override
+	public boolean onKey(View v, int keyCode, KeyEvent event) {
+
+		return true;
+	}
+    @Override
+    public boolean onKeyDown(int keyCode, KeyEvent event){
+    	//DO NOTHING this will render useless every menu key except Home
+    	int keyChar= event.getUnicodeChar();
+    	osgNativeLib.keyboardDown(keyChar);
+    	return true;
+    }
+    @Override
+    public boolean onKeyUp(int keyCode, KeyEvent event){
+    	switch (keyCode){
+    	case KeyEvent.KEYCODE_BACK:
+    		super.onDestroy();
+    		this.finish();
+    		break;
+    	case KeyEvent.KEYCODE_SEARCH:
+    		break;
+    	case KeyEvent.KEYCODE_MENU:
+    		this.openOptionsMenu();
+    		break;
+    	default:
+    		int keyChar= event.getUnicodeChar();
+    		osgNativeLib.keyboardUp(keyChar);
+    	}
+
+    	return true;
+    }
+    @Override
+    public boolean onTouch(View v, MotionEvent event) {
+
+    	//dumpEvent(event);
+
+    	int n_points = event.getPointerCount();
+    	int action = event.getAction() & MotionEvent.ACTION_MASK;
+
+    	switch(n_points){
+    	case 1:
+    		switch(action){
+    		case MotionEvent.ACTION_DOWN:
+	    		mode = moveTypes.DRAG;
+
+	    		osgNativeLib.mouseMoveEvent(event.getX(0), event.getY(0));
+	    		if(navMode==navType.PRINCIPAL)
+	    			osgNativeLib.mouseButtonPressEvent(event.getX(0), event.getY(0), 2);
+	    		else
+	    			osgNativeLib.mouseButtonPressEvent(event.getX(0), event.getY(0), 1);
+
+	    		oneFingerOrigin.x=event.getX(0);
+	    		oneFingerOrigin.y=event.getY(0);
+    			break;
+    		case MotionEvent.ACTION_CANCEL:
+    			switch(mode){
+    			case DRAG:
+    				osgNativeLib.mouseMoveEvent(event.getX(0), event.getY(0));
+    				if(navMode==navType.PRINCIPAL)
+    					osgNativeLib.mouseButtonReleaseEvent(event.getX(0), event.getY(0), 2);
+    				else
+    					osgNativeLib.mouseButtonReleaseEvent(event.getX(0), event.getY(0), 1);
+    				break;
+    			default :
+    				Log.e(TAG,"There has been an anomaly in touch input 1point/action");
+    			}
+    			mode = moveTypes.NONE;
+    			break;
+    		case MotionEvent.ACTION_MOVE:
+
+    			osgNativeLib.mouseMoveEvent(event.getX(0), event.getY(0));
+
+    			oneFingerOrigin.x=event.getX(0);
+	    		oneFingerOrigin.y=event.getY(0);
+
+    			break;
+    		case MotionEvent.ACTION_UP:
+    			switch(mode){
+    			case DRAG:
+    				if(navMode==navType.PRINCIPAL)
+    					osgNativeLib.mouseButtonReleaseEvent(event.getX(0), event.getY(0), 2);
+    				else
+    					osgNativeLib.mouseButtonReleaseEvent(event.getX(0), event.getY(0), 1);
+    				break;
+    			default :
+    				Log.e(TAG,"There has been an anomaly in touch input 1 point/action");
+    			}
+    			mode = moveTypes.NONE;
+    			break;
+    		default :
+    			Log.e(TAG,"1 point Action not captured");
+    		}
+    		break;
+    	case 2:
+    		switch (action){
+    		case MotionEvent.ACTION_POINTER_DOWN:
+    			//Free previous Action
+    			switch(mode){
+    			case DRAG:
+    				if(navMode==navType.PRINCIPAL)
+    					osgNativeLib.mouseButtonReleaseEvent(event.getX(0), event.getY(0), 2);
+    				else
+    					osgNativeLib.mouseButtonReleaseEvent(event.getX(0), event.getY(0), 1);
+    				break;
+				default:
+					break;
+    			}
+    			mode = moveTypes.ZOOM;
+    			distanceOrigin = sqrDistance(event);
+    			twoFingerOrigin.x=event.getX(1);
+    			twoFingerOrigin.y=event.getY(1);
+    			oneFingerOrigin.x=event.getX(0);
+	    		oneFingerOrigin.y=event.getY(0);
+
+    			osgNativeLib.mouseMoveEvent(oneFingerOrigin.x,oneFingerOrigin.y);
+    			osgNativeLib.mouseButtonPressEvent(oneFingerOrigin.x,oneFingerOrigin.y, 3);
+    			osgNativeLib.mouseMoveEvent(oneFingerOrigin.x,oneFingerOrigin.y);
+
+    		case MotionEvent.ACTION_MOVE:
+    			float distance = sqrDistance(event);
+    			float result = distance-distanceOrigin;
+    			distanceOrigin=distance;
+
+    			if(result>1||result<-1){
+    	    		oneFingerOrigin.y=oneFingerOrigin.y+result;
+    				osgNativeLib.mouseMoveEvent(oneFingerOrigin.x,oneFingerOrigin.y);
+    			}
+
+    			break;
+    		case MotionEvent.ACTION_POINTER_UP:
+    			mode =moveTypes.NONE;
+    			osgNativeLib.mouseButtonReleaseEvent(oneFingerOrigin.x,oneFingerOrigin.y, 3);
+    			break;
+    		case MotionEvent.ACTION_UP:
+    			mode =moveTypes.NONE;
+    			osgNativeLib.mouseButtonReleaseEvent(oneFingerOrigin.x,oneFingerOrigin.y, 3);
+    			break;
+    		default :
+    			Log.e(TAG,"2 point Action not captured");
+    		}
+    		break;
+    	}
+
+		return true;
+	}
+
+    //Ui Listeners
+    OnClickListener uiListenerCenterView = new OnClickListener() {
+        @Override
+		public void onClick(View v) {
+        	//Log.d(TAG, "Center View");
+        	osgNativeLib.keyboardDown(32);
+        	osgNativeLib.keyboardUp(32);
+        }
+    };
+    OnClickListener uiListenerChangeNavigation = new OnClickListener() {
+        @Override
+		public void onClick(View v) {
+        	//Log.d(TAG, "Change Navigation");
+        	if(navMode==navType.PRINCIPAL){
+        		msgUiNavSecondary.show();
+        		navMode=navType.SECONDARY;
+        	}
+        	else{
+        		msgUiNavPrincipal.show();
+        		navMode=navType.PRINCIPAL;
+        	}
+        }
+    };
+    OnClickListener uiListenerChangeLight = new OnClickListener() {
+        @Override
+		public void onClick(View v) {
+        	//Log.d(TAG, "Change Light");
+        	if(lightMode==lightType.ON){
+        		msgUiLightOff.show();
+        		lightMode=lightType.OFF;
+        		osgNativeLib.keyboardDown(108);
+            	osgNativeLib.keyboardUp(108);
+        	}
+        	else{
+        		msgUiLightOn.show();
+        		lightMode=lightType.ON;
+        		osgNativeLib.keyboardDown(108);
+            	osgNativeLib.keyboardUp(108);
+        	}
+        }
+    };
+
+    //Menu
+
+    @Override
+	public void colorChange(int color) {
+		// TODO Auto-generated method stub
+		// Do nothing
+    	int red = Color.red(color);
+    	int green = Color.green(color);
+    	int blue = Color.blue(color);
+    	//Log.d(TAG,"BACK color "+red+" "+green+" "+blue+" ");
+    	osgNativeLib.setClearColor(red,green,blue);
+	}
+
+    private float sqrDistance(MotionEvent event) {
+        float x = event.getX(0) - event.getX(1);
+        float y = event.getY(0) - event.getY(1);
+        return (float)(FloatMath.sqrt(x * x + y * y));
+     }
+
+	// The below methods should all be called by the parent activity at the appropriate times
+	@Override
+	public void onOPConnected() {
+		super.onOPConnected();
+
+		registerObjectUpdates(objMngr.getObject("AttitudeActual"));
+	}
+
+	@Override
+	protected void objectUpdated(UAVObject obj) {
+		float q[] = new float[4];
+
+		q[0] = (float) obj.getField("q1").getDouble();
+		q[1] = (float) obj.getField("q2").getDouble();
+		q[2] = (float) obj.getField("q3").getDouble();
+		q[3] = (float) obj.getField("q4").getDouble();
+		if (DEBUG) Log.d(TAG, "Attitude: " + q[0] + " " + q[1] + " " + q[2] + " " + q[3]);
+
+		osgNativeLib.setQuat(q[0], q[1], q[2], q[3]);
+	}
+
+
+}

androidgcs/src/org/openpilot/androidgcs/TcpUAVTalk.java

@@ -0,0 +1,114 @@
+package org.openpilot.androidgcs;
+
+import java.io.IOException;
+import java.net.InetAddress;
+import java.net.Socket;
+import java.net.UnknownHostException;
+import org.openpilot.uavtalk.UAVObjectManager;
+import org.openpilot.uavtalk.UAVTalk;
+
+import android.content.Context;
+import android.content.SharedPreferences;
+import android.preference.PreferenceManager;
+import android.util.Log;
+
+public class TcpUAVTalk {
+	private final String TAG = "TcpUAVTalk";
+	public static int LOGLEVEL = 2;
+	public static boolean WARN = LOGLEVEL > 1;
+	public static boolean DEBUG = LOGLEVEL > 0;
+	
+	// Temporarily define fixed device name
+	private String ip_address = "1";
+	private int port = 9001;
+	
+	private UAVTalk uavTalk;
+	private boolean connected; 
+	private Socket socket;
+	
+	/**
+	 * Construct a TcpUAVTalk object attached to the OPTelemetryService.  Gets the 
+	 * connection settings from the preferences.
+	 */
+	public TcpUAVTalk(Context caller) {
+		SharedPreferences prefs = PreferenceManager.getDefaultSharedPreferences(caller);
+		ip_address = prefs.getString("ip_address","127.0.0.1");
+		try {
+			port = Integer.decode(prefs.getString("port", ""));
+		} catch (NumberFormatException e) {
+			//TODO: Handle this exception
+		}
+
+		if (DEBUG) Log.d(TAG, "Trying to open UAVTalk with " + ip_address);
+
+        connected = false;         
+    }
+	
+	/**
+	 * Connect a TCP object to an object manager.  Returns true if already
+	 * connected, otherwise returns true if managed a successful socket.
+	 */
+	public boolean connect(UAVObjectManager objMngr) {
+		if( getConnected() ) 
+			return true;
+		if( !openTelemetryTcp(objMngr) )
+			return false;
+		return true;		
+	}
+	
+	public void disconnect() {
+		try {
+			socket.close();
+		} catch (IOException e) {
+			// TODO Auto-generated catch block
+			e.printStackTrace();
+		}
+		socket = null;
+	}
+
+	public boolean getConnected() {
+		return connected;
+	}
+    	
+	public UAVTalk getUavtalk() {
+		return uavTalk;
+	}
+	
+	/**
+	 * Opens a TCP socket to the address determined on construction.  If successful
+	 * creates a UAVTalk stream connection this socket to the passed in object manager
+	 */
+	private boolean openTelemetryTcp(UAVObjectManager objMngr) {
+		Log.d(TAG, "Opening connection to " + ip_address + " at address " + port);
+		
+		InetAddress serverAddr = null;
+		try {
+			serverAddr = InetAddress.getByName(ip_address);
+		} catch (UnknownHostException e1) {
+			// TODO Auto-generated catch block
+			e1.printStackTrace();
+			return false;
+		}
+
+		socket = null;
+		try {
+			socket = new Socket(serverAddr,port);
+		} catch (IOException e1) {
+			return false;
+		}
+
+		connected = true;
+		
+		try {
+			uavTalk = new UAVTalk(socket.getInputStream(), socket.getOutputStream(), objMngr);
+		} catch (IOException e) {
+			Log.e(TAG,"Error starting UAVTalk");
+			// TODO Auto-generated catch block
+			//e.printStackTrace();
+			return false;
+		}
+				
+		return true;
+	}
+
+}

androidgcs/src/org/openpilot/androidgcs/fragments/MapPositioner.java

@@ -0,0 +1,229 @@
+/**
+ ******************************************************************************
+ * @file       MapPositioner.java
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @brief      A fragmenet to move the UAV around by dragging
+ * @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
+ */
+ package org.openpilot.androidgcs.fragments;
+
+import org.openpilot.androidgcs.R;
+import org.openpilot.uavtalk.UAVObject;
+import org.openpilot.uavtalk.UAVObjectManager;
+
+import android.content.Context;
+import android.graphics.Canvas;
+import android.graphics.Color;
+import android.graphics.Paint;
+import android.graphics.drawable.Drawable;
+import android.os.Bundle;
+import android.util.Log;
+import android.view.LayoutInflater;
+import android.view.MotionEvent;
+import android.view.View;
+import android.view.ViewGroup;
+import android.widget.FrameLayout;
+import android.widget.SeekBar;
+
+public class MapPositioner extends ObjectManagerFragment {
+
+	private static final String TAG = ObjectManagerFragment.class
+			.getSimpleName();
+	private static final int LOGLEVEL = 0;
+	private static final boolean DEBUG = LOGLEVEL > 0;
+
+	private final float MAX_DISTANCE_M = 50.0f;
+
+	float px_to_m(View v, float px) {
+		final float MAX_DISTANCE_PX = Math.max(v.getMeasuredHeight(),
+				v.getMeasuredWidth());
+		float scale = MAX_DISTANCE_M / MAX_DISTANCE_PX;
+		return px * scale;
+	}
+
+	float m_to_px(View v, float m) {
+		final float MAX_DISTANCE_PX = Math.max(v.getMeasuredHeight(),
+				v.getMeasuredWidth());
+		float scale = MAX_DISTANCE_M / MAX_DISTANCE_PX;
+		return m / scale;
+	}
+
+	@Override
+	public View onCreateView(LayoutInflater inflater, ViewGroup container,
+			Bundle savedInstanceState) {
+
+		super.onCreate(savedInstanceState);
+		return inflater.inflate(R.layout.map_positioner, container, false);
+	}
+
+	private float pos_x = 0, pos_y = 0;
+	private float desired_x = 0, desired_y = 0, desired_z = 0;
+	//private final float poi_x = 1, poi_y = 1;
+	private UAVOverlay uav = null;
+
+	public class UAVOverlay extends View {
+
+		Paint paint = null;
+
+		public UAVOverlay(Context context) {
+			super(context);
+			paint = new Paint();
+			paint.setColor(Color.WHITE);
+			paint.setTextSize(20);
+		}
+
+		private void draw(Canvas canvas, int resource, float x, float y) {
+			Drawable icon = getContext().getResources().getDrawable(resource);
+			final int offset_x = canvas.getWidth() / 2;
+			final int offset_y = canvas.getHeight() / 2;
+			final int SIZE = 50 / 2;
+			icon.setBounds((int) x - SIZE + offset_x, (int) y - SIZE + offset_y,
+					(int) x + SIZE + offset_x, (int) y + SIZE + offset_y);
+			icon.draw(canvas);
+
+		}
+
+		@Override
+		/**
+		 * Draw the UAV, the position desired, and the camera POI
+		 */
+		protected void onDraw(Canvas canvas) {
+			draw(canvas, R.drawable.map_positioner_uav, (int) m_to_px(this, pos_x), (int) m_to_px(this, pos_y));
+			draw(canvas, R.drawable.map_positioner_waypoint, (int) m_to_px(this, desired_x), (int) -m_to_px(this, desired_y));
+			//draw(canvas, R.drawable.map_positioner_poi, (int) m_to_px(this, poi_x), (int) -m_to_px(this, poi_y));
+
+			// Annotate the desired position
+			canvas.drawText("(" + String.format("%.1f",desired_x) + "," + String.format("%.1f",desired_y) + "," + String.format("%.1f",desired_z) + ")",
+					m_to_px(this, desired_x) + canvas.getWidth() / 2 + 25,
+					-m_to_px(this, desired_y) + canvas.getHeight() / 2, paint);
+		}
+
+	};
+
+	public void connectTouch() {
+		if (DEBUG)
+			Log.d(TAG, "Connected touch listener()");
+		View v = getActivity().findViewById(R.id.map_view);
+		if (v != null)
+			v.setOnTouchListener(new View.OnTouchListener() {
+				@Override
+				public boolean onTouch(View v, MotionEvent event) {
+					switch (event.getActionMasked()) {
+					case MotionEvent.ACTION_DOWN:
+					case MotionEvent.ACTION_MOVE:
+						UAVObject desired = objMngr.getObject("PathDesired");
+						if (desired != null) {
+							if (DEBUG) Log.d(TAG, "Updating path desired");
+							desired.getField("End").setDouble(px_to_m(v, (int) event.getX() - v.getMeasuredWidth() / 2), 1);
+							desired.getField("End").setDouble(px_to_m(v, -(event.getY() - v.getMeasuredHeight() / 2)), 0);
+							desired.updated();
+							if (uav != null)
+								uav.invalidate();
+						}
+						break;
+					}
+					return true;
+				}
+			});
+		else
+			if (DEBUG)
+				Log.d(TAG, "No view");
+
+		FrameLayout f = (FrameLayout) getActivity().findViewById(R.id.map_view);
+		uav = new UAVOverlay(getActivity().getBaseContext());
+		f.addView(uav);
+	}
+
+	@Override
+	public void onStart() {
+		super.onStart();
+		if (DEBUG)
+			Log.d(TAG, "onStart()");
+		connectTouch();
+
+		SeekBar altitudeBar = (SeekBar) getActivity().findViewById(R.id.altitude_slider);
+		if(altitudeBar != null)
+			altitudeBar.setOnSeekBarChangeListener(new SeekBar.OnSeekBarChangeListener() {
+
+				@Override
+				public void onProgressChanged(SeekBar seekBar, int progress,
+						boolean fromUser) {
+					UAVObject desired = objMngr.getObject("PathDesired");
+					if (desired != null) {
+						if (DEBUG) Log.d(TAG, "Updating path desired");
+						desired.getField("End").setDouble(-progress, 2);
+						desired.updated();
+						if (uav != null)
+							uav.invalidate();
+					}
+
+				}
+
+				@Override
+				public void onStartTrackingTouch(SeekBar seekBar) {
+					// TODO Auto-generated method stub
+
+				}
+
+				@Override
+				public void onStopTrackingTouch(SeekBar seekBar) {
+					// TODO Auto-generated method stub
+
+				}
+
+			});
+	}
+
+	@Override
+	public void onOPConnected(UAVObjectManager objMngr) {
+		super.onOPConnected(objMngr);
+		if (DEBUG)
+			Log.d(TAG, "On connected");
+
+		UAVObject obj = objMngr.getObject("PositionActual");
+		if (obj != null)
+			registerObjectUpdates(obj);
+		objectUpdated(obj);
+
+		obj = objMngr.getObject("PathDesired");
+		if (obj != null)
+			registerObjectUpdates(obj);
+		objectUpdated(obj);
+	}
+
+	/**
+	 * Called whenever any objects subscribed to via registerObjects. Store the
+	 * local copy of the information.
+	 */
+	@Override
+	public void objectUpdated(UAVObject obj) {
+		if (obj.getName().compareTo("PositionActual") == 0) {
+			pos_x = (int) obj.getField("East").getDouble();
+			pos_y = (int) obj.getField("North").getDouble();
+			uav.invalidate();
+		}
+		if (obj.getName().compareTo("PathDesired") == 0) {
+			desired_x = (float) obj.getField("End").getDouble(1);
+			desired_y = (float) obj.getField("End").getDouble(0);
+			desired_z = (float) obj.getField("End").getDouble(2);
+			uav.invalidate();
+		}
+	}
+
+
+}

androidgcs/src/org/openpilot/androidgcs/fragments/SystemAlarmsFragment.java

@@ -30,7 +30,6 @@ import org.openpilot.uavtalk.UAVObject;
 import org.openpilot.uavtalk.UAVObjectField;
 import org.openpilot.uavtalk.UAVObjectManager;
 
-import android.app.Activity;
 import android.os.Bundle;
 import android.util.Log;
 import android.view.LayoutInflater;
@@ -73,16 +72,7 @@ public class SystemAlarmsFragment extends ObjectManagerFragment {
 		if (DEBUG)
 			Log.d(TAG, "Updated");
 		if (obj.getName().compareTo("SystemAlarms") == 0) {
-			Activity activity = getActivity();
-			if (activity == null) {
-				// TODO: Need to unregister all the callbacks
-				return;
-			}
 			TextView alarms = (TextView) getActivity().findViewById(R.id.system_alarms_fragment_field);
-			if (alarms == null) {
-				// TODO: Need to figure out how to unregister all the callbacks
-				return;
-			}
 			UAVObjectField a = obj.getField("Alarm");
 			List<String> names = a.getElementNames();
 			String contents = new String();

androidgcs/src/org/openpilot/androidgcs/telemetry/BluetoothUAVTalk.java

@@ -143,7 +143,7 @@ public class BluetoothUAVTalk extends TelemetryTask {
 		socket = null;
 
 		try {
-			socket = device.createInsecureRfcommSocketToServiceRecord(MY_UUID);
+			socket = device.createRfcommSocketToServiceRecord(MY_UUID);
 		} catch (IOException e) {
 			if (ERROR) Log.e(TAG,"Unable to create Rfcomm socket");
 			return false;

androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java

@@ -123,7 +123,10 @@ public class HidUAVTalk extends TelemetryTask {
 		while(deviceIterator.hasNext()){
 			UsbDevice dev = deviceIterator.next();
 			if (DEBUG) Log.d(TAG, "Testing device: " + dev);
-			usbManager.requestPermission(dev, permissionIntent);
+			if( ValidateFoundDevice(dev) ) {
+				usbManager.requestPermission(dev, permissionIntent);
+				break;
+			}
 		}
 
 		if (DEBUG) Log.d(TAG, "Registered the deviceAttachedFilter");
@@ -254,6 +257,12 @@ public class HidUAVTalk extends TelemetryTask {
 		UsbEndpoint ep1 = null;
 		UsbEndpoint ep2 = null;
 
+		if (connectDevice.getInterfaceCount() < 2) {
+			if (ERROR) Log.e(TAG, "Interface count for USB device incorrect");
+			telemService.toastMessage("Failed to connect");
+			return false;
+		}
+
 		// Using the same interface for reading and writing
 		usbInterface = connectDevice.getInterface(0x2);
 		if (usbInterface.getEndpointCount() == 2)

androidgcs/src/org/openpilot/osg/ColorPickerDialog.java

@@ -0,0 +1,251 @@
+package org.openpilot.osg;
+
+import android.app.Dialog;
+import android.content.Context;
+import android.graphics.Canvas;
+import android.graphics.Color;
+import android.graphics.LinearGradient;
+import android.graphics.Paint;
+import android.graphics.Shader;
+import android.os.Bundle;
+import android.view.MotionEvent;
+import android.view.View;
+
+public class ColorPickerDialog extends Dialog{
+
+	public interface OnColorChangeListener{
+		void colorChange(int color);
+	}
+
+	private final OnColorChangeListener tListener;
+	private final int tInitialColor;
+
+	private static class ColorPickerView extends View{
+
+		private final Paint 			tPaint;
+		private float 			tCurrentHue = 0;
+		private int 			tCurrentX = 0;
+		private int 			tCurrentY = 0;
+		private int 			tCurrentColor;
+		private final int[] 	tHueGradientColors = new int [258];
+		private final int []			tGradientColors = new int[65536]; //256X256 colors
+		private final OnColorChangeListener tListener;
+		private boolean			tQSelected = false;
+
+		public ColorPickerView(Context context, OnColorChangeListener listener, int color) {
+			super(context);
+			// TODO Auto-generated constructor stub
+			tListener = listener;
+
+			//Get Hue from tCurrentColor and update the Gradient of Color
+			float[] newHSV = new float[3];
+			Color.colorToHSV(color, newHSV);
+			tCurrentHue = newHSV[0];
+			updateGradientColors();
+			tCurrentColor = color;
+
+			//Initialize of colors in Hue slider
+
+			int index = 0;
+			for(float i=0; i<256; i += 256/42 , index++){
+				tHueGradientColors[index] = Color.rgb(255, 0, (int)i);
+			}
+			for(float i=0; i<256; i += 256/42 , index++){
+				tHueGradientColors[index] = Color.rgb(255-(int)i, 0, 255);
+			}
+			for(float i=0; i<256; i += 256/42 , index++){
+				tHueGradientColors[index] = Color.rgb(0, (int) i, 255);
+			}
+			for(float i=0; i<256; i += 256/42 , index++){
+				tHueGradientColors[index] = Color.rgb(0, 255, 255-(int)i);
+			}
+			for(float i=0; i<256; i += 256/42 , index++){
+				tHueGradientColors[index] = Color.rgb((int)i, 255, 0);
+			}
+			for(float i=0; i<256; i += 256/42 , index++){
+				tHueGradientColors[index] = Color.rgb(255, 255-(int)i, 0);
+			}
+
+			// Paint initialized
+			tPaint = new Paint(Paint.ANTI_ALIAS_FLAG);
+			tPaint.setTextAlign(Paint.Align.CENTER);
+			tPaint.setTextSize(12);
+		}
+
+		// Get the Color from Hue Bar
+		private int getCurrentGradientColor(){
+
+			int currentHue = 255 - (int)(tCurrentHue*255/360);
+			int index = 0;
+			for (float i=0; i<256; i += 256/42, index++){
+				if (index == currentHue) return Color.rgb(255, 0, (int) i );
+			}
+			for (float i=0; i<256; i += 256/42, index++){
+				if (index == currentHue) return Color.rgb(255-(int)i, 0, 255 );
+			}
+			for (float i=0; i<256; i += 256/42, index++){
+				if (index == currentHue) return Color.rgb(0, (int) i, 255 );
+			}
+			for (float i=0; i<256; i += 256/42, index++){
+				if (index == currentHue) return Color.rgb(0, 255, 255-(int) i );
+			}
+			for (float i=0; i<256; i += 256/42, index++){
+				if (index == currentHue) return Color.rgb((int) i, 255, 0 );
+			}
+			for (float i=0; i<256; i += 256/42, index++){
+				if (index == currentHue) return Color.rgb(255, 255-(int) i, 0);
+			}
+			return Color.RED;
+		}
+
+		private void updateGradientColors(){
+
+			int actualColor = getCurrentGradientColor();
+			int index = 0;
+			int[] colColors = new int[256];
+			for(int y=0; y<256; y++){
+				for(int x=0; x<256; x++ , index++){
+					if(y==0){
+						tGradientColors[index] = Color.rgb(255-(255-Color.red(actualColor))*x/255, 255-(255-Color.green(actualColor))*x/255, 255-(255-Color.blue(actualColor))*x/255);
+						colColors[x] = tGradientColors[index];
+					}
+					else{
+						tGradientColors[index] = Color.rgb((255-y)*Color.red(colColors[x])/255, (255-y)*Color.green(colColors[x])/255, (255-y)*Color.blue(colColors[x])/255);
+					}
+				}
+			}
+		}
+
+		@Override
+		protected void onDraw(Canvas canvas){
+			int translatedHue = 255 - (int)(tCurrentHue*255/360);
+			//Display HUE with bar lines
+
+			for(int x=0; x<256; x++){
+				//We display the color or a big white bar
+				if(translatedHue != x){
+					tPaint.setColor(tHueGradientColors[x]);
+					tPaint.setStrokeWidth(1);
+				}
+				else{
+					tPaint.setColor(Color.WHITE);
+					tPaint.setStrokeWidth(3);
+				}
+				canvas.drawLine(x+10, 0, x+10, 40, tPaint);
+			}
+
+			// Display Gradient Box
+			for(int x=0; x<256;x++){
+				int[] colors = new int[2];
+				colors[0] = tGradientColors[x];
+				colors[1] = Color.BLACK;
+				Shader shader = new LinearGradient(0,50,0,306,colors,null, Shader.TileMode.REPEAT);
+				tPaint.setShader(shader);
+				canvas.drawLine(x+10, 50, x+10, 306, tPaint);
+			}
+			tPaint.setShader(null);
+
+			//Display the circle around the currently selected color in the main field
+			if(tCurrentX !=0 && tCurrentY != 0){
+				tPaint.setStyle(Paint.Style.STROKE);
+				tPaint.setColor(Color.BLACK);
+				canvas.drawCircle(tCurrentX, tCurrentY, 10, tPaint);
+			}
+
+			//Draw a button
+
+			tPaint.setStyle(Paint.Style.FILL);
+			if(tQSelected){
+				tPaint.setColor(Color.WHITE);
+				canvas.drawCircle(138, 336, 30, tPaint);
+			}
+			tPaint.setColor(tCurrentColor);
+			canvas.drawCircle(138, 336, 20, tPaint);
+
+		}
+
+		@Override
+		protected void onMeasure(int width,int height){
+			setMeasuredDimension(276, 366);
+		}
+
+		@Override
+		public boolean onTouchEvent(MotionEvent event){
+			if(event.getAction() != MotionEvent.ACTION_DOWN && event.getAction() != MotionEvent.ACTION_MOVE && event.getAction() != MotionEvent.ACTION_UP) return true;
+			float x = event.getX();
+			float y = event.getY();
+
+			tQSelected=false;
+
+			// if in Hue Bar
+			if(x >10 && x <266 && y>0 && y<40){
+				//Update gradient
+				tCurrentHue = (255-x)*360/255;
+				updateGradientColors();
+
+				//Update current Selected Color
+				int nX = tCurrentX-10;
+				int nY = tCurrentY-60;
+				int index = 256 * (nY-1)+nX;
+
+				if(index>0 && index < tGradientColors.length)
+					tCurrentColor = tGradientColors[256*(nY-1)+nX];
+
+				invalidate(); //By invalidating we are forcing a redraw;
+			}
+
+			// If Main gradient
+
+			if ( x >10 && x< 266 && y>50 && y <306){
+				tCurrentX = (int) x;
+				tCurrentY = (int) y;
+				int nX = tCurrentX - 10;
+				int nY = tCurrentY - 60;
+				int index = 256*(nY-1)+nX;
+				if (index >0 && index < tGradientColors.length){
+					tCurrentColor = tGradientColors[index];
+
+					invalidate(); //By invalidating we are forcing a redraw;
+				}
+			}
+			if( x>118 && x<158 && y > 316 && y <356){
+				if(event.getAction() == MotionEvent.ACTION_DOWN || event.getAction() == MotionEvent.ACTION_MOVE){
+					tQSelected=true;
+				}
+				if(event.getAction() == MotionEvent.ACTION_UP){
+					tQSelected=false;
+					tListener.colorChange(tCurrentColor);
+				}
+				invalidate();
+			}
+
+
+
+			return true;
+		}
+
+
+	}
+
+	public ColorPickerDialog(Context context, OnColorChangeListener listener, int initialColor){
+		super(context);
+
+		tListener = listener;
+		tInitialColor = initialColor;
+	}
+
+	@Override
+	protected void onCreate( Bundle savedInstanceState){
+		super.onCreate(savedInstanceState);
+		OnColorChangeListener l = new OnColorChangeListener(){
+			@Override
+			public void colorChange(int color){
+				tListener.colorChange(color);
+				dismiss();
+			}
+		};
+
+		setContentView(new ColorPickerView(getContext(),l,tInitialColor));
+	}
+
+}

androidgcs/src/org/openpilot/osg/EGLview.java

@@ -0,0 +1,333 @@
+
+package org.openpilot.osg;
+/*
+ * Copyright (C) 2008 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+import javax.microedition.khronos.egl.EGL10;
+import javax.microedition.khronos.egl.EGLConfig;
+import javax.microedition.khronos.egl.EGLContext;
+import javax.microedition.khronos.egl.EGLDisplay;
+import javax.microedition.khronos.opengles.GL10;
+
+import android.content.Context;
+import android.graphics.PixelFormat;
+import android.opengl.GLSurfaceView;
+import android.util.AttributeSet;
+import android.util.Log;
+/**
+ * A simple GLSurfaceView sub-class that demonstrate how to perform
+ * OpenGL ES 1.0 rendering into a GL Surface. Note the following important
+ * details:
+ *
+ * - The class must use a custom context factory to enable 1.0 rendering.
+ *   See ContextFactory class definition below.
+ *
+ * - The class must use a custom EGLConfigChooser to be able to select
+ *   an EGLConfig that supports 1.0. This is done by providing a config
+ *   specification to eglChooseConfig() that has the attribute
+ *   EGL10.ELG_RENDERABLE_TYPE containing the EGL_OPENGL_ES_BIT flag
+ *   set. See ConfigChooser class definition below.
+ *
+ * - The class must select the surface's format, then choose an EGLConfig
+ *   that matches it exactly (with regards to red/green/blue/alpha channels
+ *   bit depths). Failure to do so would result in an EGL_BAD_MATCH error.
+ */
+public class EGLview extends GLSurfaceView {
+	private static String TAG = "EGLview";
+	private static final boolean DEBUG = false;
+
+	public EGLview(Context context) {
+	    super(context);
+	    init(false, 16, 8);
+	}
+	public EGLview(Context context, AttributeSet attrs) {
+	    super(context,attrs);
+	    init(false, 16, 8);
+	}
+
+
+	public EGLview(Context context, boolean translucent, int depth, int stencil) {
+	    super(context);
+	    init(translucent, depth, stencil);
+	}
+
+	private void init(boolean translucent, int depth, int stencil) {
+
+	    /* By default, GLSurfaceView() creates a RGB_565 opaque surface.
+	     * If we want a translucent one, we should change the surface's
+	     * format here, using PixelFormat.TRANSLUCENT for GL Surfaces
+	     * is interpreted as any 32-bit surface with alpha by SurfaceFlinger.
+	     */
+	    if (translucent) {
+	        this.getHolder().setFormat(PixelFormat.TRANSLUCENT);
+	    }
+
+	    /* Setup the context factory for 2.0 rendering.
+	     * See ContextFactory class definition below
+	     */
+	    setEGLContextFactory(new ContextFactory());
+
+	    /* We need to choose an EGLConfig that matches the format of
+	     * our surface exactly. This is going to be done in our
+	     * custom config chooser. See ConfigChooser class definition
+	     * below.
+	     */
+	    setEGLConfigChooser( translucent ?
+	                         new ConfigChooser(8, 8, 8, 8, depth, stencil) :
+	                         new ConfigChooser(5, 6, 5, 0, depth, stencil) );
+
+	    /* Set the renderer responsible for frame rendering */
+	    setRenderer(new Renderer());
+	}
+
+	private static class ContextFactory implements GLSurfaceView.EGLContextFactory {
+	    private static int EGL_CONTEXT_CLIENT_VERSION = 0x3098;
+	    @Override
+		public EGLContext createContext(EGL10 egl, EGLDisplay display, EGLConfig eglConfig) {
+	        Log.w(TAG, "creating OpenGL ES 2.0 context");
+	        checkEglError("Before eglCreateContext", egl);
+	        int[] attrib_list = {EGL_CONTEXT_CLIENT_VERSION, 2, EGL10.EGL_NONE };
+	        EGLContext context = egl.eglCreateContext(display, eglConfig, EGL10.EGL_NO_CONTEXT, attrib_list);
+	        checkEglError("After eglCreateContext", egl);
+	        return context;
+	    }
+
+	    @Override
+		public void destroyContext(EGL10 egl, EGLDisplay display, EGLContext context) {
+	        egl.eglDestroyContext(display, context);
+	    }
+	}
+
+	private static void checkEglError(String prompt, EGL10 egl) {
+	    int error;
+	    while ((error = egl.eglGetError()) != EGL10.EGL_SUCCESS) {
+	        Log.e(TAG, String.format("%s: EGL error: 0x%x", prompt, error));
+	    }
+	}
+
+	private static class ConfigChooser implements GLSurfaceView.EGLConfigChooser {
+
+	    public ConfigChooser(int r, int g, int b, int a, int depth, int stencil) {
+	        mRedSize = r;
+	        mGreenSize = g;
+	        mBlueSize = b;
+	        mAlphaSize = a;
+	        mDepthSize = depth;
+	        mStencilSize = stencil;
+	    }
+
+	    /* This EGL config specification is used to specify 1.x rendering.
+	     * We use a minimum size of 4 bits for red/green/blue, but will
+	     * perform actual matching in chooseConfig() below.
+	     */
+	    private static int EGL_OPENGL_ES_BIT = 4;
+	    private static int[] s_configAttribs2 =
+	    {
+	        EGL10.EGL_RED_SIZE, 4,
+	        EGL10.EGL_GREEN_SIZE, 4,
+	        EGL10.EGL_BLUE_SIZE, 4,
+	        EGL10.EGL_RENDERABLE_TYPE, EGL_OPENGL_ES_BIT,
+	        EGL10.EGL_NONE
+	    };
+
+	    @Override
+		public EGLConfig chooseConfig(EGL10 egl, EGLDisplay display) {
+
+	        /* Get the number of minimally matching EGL configurations
+	         */
+	        int[] num_config = new int[1];
+	        egl.eglChooseConfig(display, s_configAttribs2, null, 0, num_config);
+
+	        int numConfigs = num_config[0];
+
+	        if (numConfigs <= 0) {
+	            throw new IllegalArgumentException("No configs match configSpec");
+	        }
+
+	        /* Allocate then read the array of minimally matching EGL configs
+	         */
+	        EGLConfig[] configs = new EGLConfig[numConfigs];
+	        egl.eglChooseConfig(display, s_configAttribs2, configs, numConfigs, num_config);
+
+	        if (DEBUG) {
+	             printConfigs(egl, display, configs);
+	        }
+	        /* Now return the "best" one
+	         */
+	        return chooseConfig(egl, display, configs);
+	    }
+
+	    public EGLConfig chooseConfig(EGL10 egl, EGLDisplay display,
+	            EGLConfig[] configs) {
+	        for(EGLConfig config : configs) {
+	            int d = findConfigAttrib(egl, display, config,
+	                    EGL10.EGL_DEPTH_SIZE, 0);
+	            int s = findConfigAttrib(egl, display, config,
+	                    EGL10.EGL_STENCIL_SIZE, 0);
+
+	            // We need at least mDepthSize and mStencilSize bits
+	            if (d < mDepthSize || s < mStencilSize)
+	                continue;
+
+	            // We want an *exact* match for red/green/blue/alpha
+	            int r = findConfigAttrib(egl, display, config,
+	                    EGL10.EGL_RED_SIZE, 0);
+	            int g = findConfigAttrib(egl, display, config,
+	                        EGL10.EGL_GREEN_SIZE, 0);
+	            int b = findConfigAttrib(egl, display, config,
+	                        EGL10.EGL_BLUE_SIZE, 0);
+	            int a = findConfigAttrib(egl, display, config,
+	                    EGL10.EGL_ALPHA_SIZE, 0);
+
+	            if (r == mRedSize && g == mGreenSize && b == mBlueSize && a == mAlphaSize)
+	                return config;
+	        }
+	        return null;
+	    }
+
+	    private int findConfigAttrib(EGL10 egl, EGLDisplay display,
+	            EGLConfig config, int attribute, int defaultValue) {
+
+	        if (egl.eglGetConfigAttrib(display, config, attribute, mValue)) {
+	            return mValue[0];
+	        }
+	        return defaultValue;
+	    }
+
+	    private void printConfigs(EGL10 egl, EGLDisplay display,
+	        EGLConfig[] configs) {
+	        int numConfigs = configs.length;
+	        Log.w(TAG, String.format("%d configurations", numConfigs));
+	        for (int i = 0; i < numConfigs; i++) {
+	            Log.w(TAG, String.format("Configuration %d:\n", i));
+	            printConfig(egl, display, configs[i]);
+	        }
+	    }
+
+	    private void printConfig(EGL10 egl, EGLDisplay display,
+	            EGLConfig config) {
+	        int[] attributes = {
+	                EGL10.EGL_BUFFER_SIZE,
+	                EGL10.EGL_ALPHA_SIZE,
+	                EGL10.EGL_BLUE_SIZE,
+	                EGL10.EGL_GREEN_SIZE,
+	                EGL10.EGL_RED_SIZE,
+	                EGL10.EGL_DEPTH_SIZE,
+	                EGL10.EGL_STENCIL_SIZE,
+	                EGL10.EGL_CONFIG_CAVEAT,
+	                EGL10.EGL_CONFIG_ID,
+	                EGL10.EGL_LEVEL,
+	                EGL10.EGL_MAX_PBUFFER_HEIGHT,
+	                EGL10.EGL_MAX_PBUFFER_PIXELS,
+	                EGL10.EGL_MAX_PBUFFER_WIDTH,
+	                EGL10.EGL_NATIVE_RENDERABLE,
+	                EGL10.EGL_NATIVE_VISUAL_ID,
+	                EGL10.EGL_NATIVE_VISUAL_TYPE,
+	                0x3030, // EGL10.EGL_PRESERVED_RESOURCES,
+	                EGL10.EGL_SAMPLES,
+	                EGL10.EGL_SAMPLE_BUFFERS,
+	                EGL10.EGL_SURFACE_TYPE,
+	                EGL10.EGL_TRANSPARENT_TYPE,
+	                EGL10.EGL_TRANSPARENT_RED_VALUE,
+	                EGL10.EGL_TRANSPARENT_GREEN_VALUE,
+	                EGL10.EGL_TRANSPARENT_BLUE_VALUE,
+	                0x3039, // EGL10.EGL_BIND_TO_TEXTURE_RGB,
+	                0x303A, // EGL10.EGL_BIND_TO_TEXTURE_RGBA,
+	                0x303B, // EGL10.EGL_MIN_SWAP_INTERVAL,
+	                0x303C, // EGL10.EGL_MAX_SWAP_INTERVAL,
+	                EGL10.EGL_LUMINANCE_SIZE,
+	                EGL10.EGL_ALPHA_MASK_SIZE,
+	                EGL10.EGL_COLOR_BUFFER_TYPE,
+	                EGL10.EGL_RENDERABLE_TYPE,
+	                0x3042 // EGL10.EGL_CONFORMANT
+	        };
+	        String[] names = {
+	                "EGL_BUFFER_SIZE",
+	                "EGL_ALPHA_SIZE",
+	                "EGL_BLUE_SIZE",
+	                "EGL_GREEN_SIZE",
+	                "EGL_RED_SIZE",
+	                "EGL_DEPTH_SIZE",
+	                "EGL_STENCIL_SIZE",
+	                "EGL_CONFIG_CAVEAT",
+	                "EGL_CONFIG_ID",
+	                "EGL_LEVEL",
+	                "EGL_MAX_PBUFFER_HEIGHT",
+	                "EGL_MAX_PBUFFER_PIXELS",
+	                "EGL_MAX_PBUFFER_WIDTH",
+	                "EGL_NATIVE_RENDERABLE",
+	                "EGL_NATIVE_VISUAL_ID",
+	                "EGL_NATIVE_VISUAL_TYPE",
+	                "EGL_PRESERVED_RESOURCES",
+	                "EGL_SAMPLES",
+	                "EGL_SAMPLE_BUFFERS",
+	                "EGL_SURFACE_TYPE",
+	                "EGL_TRANSPARENT_TYPE",
+	                "EGL_TRANSPARENT_RED_VALUE",
+	                "EGL_TRANSPARENT_GREEN_VALUE",
+	                "EGL_TRANSPARENT_BLUE_VALUE",
+	                "EGL_BIND_TO_TEXTURE_RGB",
+	                "EGL_BIND_TO_TEXTURE_RGBA",
+	                "EGL_MIN_SWAP_INTERVAL",
+	                "EGL_MAX_SWAP_INTERVAL",
+	                "EGL_LUMINANCE_SIZE",
+	                "EGL_ALPHA_MASK_SIZE",
+	                "EGL_COLOR_BUFFER_TYPE",
+	                "EGL_RENDERABLE_TYPE",
+	                "EGL_CONFORMANT"
+	        };
+	        int[] value = new int[1];
+	        for (int i = 0; i < attributes.length; i++) {
+	            int attribute = attributes[i];
+	            String name = names[i];
+	            if ( egl.eglGetConfigAttrib(display, config, attribute, value)) {
+	                Log.w(TAG, String.format("  %s: %d\n", name, value[0]));
+	            } else {
+	                // Log.w(TAG, String.format("  %s: failed\n", name));
+	                while (egl.eglGetError() != EGL10.EGL_SUCCESS);
+	            }
+	        }
+	    }
+
+	    // Subclasses can adjust these values:
+	    protected int mRedSize;
+	    protected int mGreenSize;
+	    protected int mBlueSize;
+	    protected int mAlphaSize;
+	    protected int mDepthSize;
+	    protected int mStencilSize;
+	    private final int[] mValue = new int[1];
+	}
+
+	private static class Renderer implements GLSurfaceView.Renderer {
+	    @Override
+		public void onDrawFrame(GL10 gl) {
+	        osgNativeLib.step();
+	    }
+
+	    @Override
+		public void onSurfaceChanged(GL10 gl, int width, int height) {
+	        osgNativeLib.init(width, height);
+	    }
+
+	    @Override
+		public void onSurfaceCreated(GL10 gl, EGLConfig config) {
+	        // Do nothing
+	        	gl.glEnable(GL10.GL_DEPTH_TEST);
+	        }
+	    }
+
+}
+

androidgcs/src/org/openpilot/osg/osgNativeLib.java

@@ -0,0 +1,29 @@
+package org.openpilot.osg;
+
+public class osgNativeLib {
+
+	static {
+        System.loadLibrary("osgNativeLib");
+    }
+
+   /**
+    * @param width the current view width
+    * @param height the current view height
+    */
+    public static native void 		init(int width, int height);
+    public static native void 		step();
+    public static native void		clearContents();
+    public static native void 		mouseButtonPressEvent(float x,float y, int button);
+    public static native void 		mouseButtonReleaseEvent(float x,float y, int button);
+    public static native void 		mouseMoveEvent(float x,float y);
+    public static native void 		keyboardDown(int key);
+    public static native void 		keyboardUp(int key);
+    public static native void 		setClearColor(int red,int green, int blue);
+    public static native int[] 		getClearColor();
+    public static native void		loadObject(String address);
+    public static native void		loadObject(String address,String name);
+    public static native void		unLoadObject(int number);
+    public static native String[]	getObjectNames();
+    public static native void		setQuat(float q1, float q2, float q3, float q4);
+
+}

androidgcs/src/org/openpilot/uavtalk/UAVTalk.java

@@ -705,7 +705,7 @@ public class UAVTalk {
 		// because otherwise if a transaction timeout occurs at the same time we can get a
 		// deadlock:
 		// 1. processInputStream -> updateObjReq (locks uavtalk) -> tranactionCompleted (locks transInfo)
-		// 2. transactionTimeout (locks transInfo) -> sendObjectRequest -> É -> setupTransaction (locks uavtalk)
+		// 2. transactionTimeout (locks transInfo) -> sendObjectRequest -> ? -> setupTransaction (locks uavtalk)
 		synchronized(this) {
 			if(respObj != null && respType == TYPE_OBJ_REQ && respObj.getObjID() == obj.getObjID() &&
 					((respObj.getInstID() == obj.getInstID() || !respAllInstances))) {

flight/Bootloaders/RevoMini/Makefile

@@ -0,0 +1,383 @@
+ #####
+ # Project: OpenPilot INS_BOOTLOADER
+ #    
+ #  
+ # Makefile for OpenPilot INS project
+ #
+ # The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2009.
+ #    
+ # 
+ # 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
+ #####
+
+WHEREAMI := $(dir $(lastword $(MAKEFILE_LIST)))
+TOP      := $(realpath $(WHEREAMI)/../../../)
+include $(TOP)/make/firmware-defs.mk
+include $(TOP)/make/boards/$(BOARD_NAME)/board-info.mk
+
+# Target file name (without extension).
+TARGET := bl_$(BOARD_NAME)
+
+# Directory for output files (lst, obj, dep, elf, sym, map, hex, bin etc.)
+OUTDIR := $(TOP)/build/$(TARGET)
+
+# Set developer code and compile options
+# Set to YES for debugging
+DEBUG ?= NO
+
+# Set to YES when using Code Sourcery toolchain
+CODE_SOURCERY ?= NO
+
+ifeq ($(CODE_SOURCERY), YES)
+REMOVE_CMD = cs-rm
+else
+REMOVE_CMD = rm
+endif
+
+# Paths
+REVO_BL = $(WHEREAMI)
+REVO_BLINC = $(REVO_BL)/inc
+PIOS = ../../PiOS
+PIOSINC = $(PIOS)/inc
+FLIGHTLIB = ../../Libraries
+FLIGHTLIBINC = ../../Libraries/inc
+PIOSSTM32F4XX = $(PIOS)/STM32F4xx
+PIOSCOMMON = $(PIOS)/Common
+PIOSBOARDS = $(PIOS)/Boards
+PIOSCOMMONLIB = $(PIOSCOMMON)/Libraries
+APPLIBDIR = $(PIOSSTM32F4XX)/Libraries
+STMLIBDIR = $(APPLIBDIR)
+STMSPDDIR = $(STMLIBDIR)/STM32F4xx_StdPeriph_Driver
+STMSPDSRCDIR = $(STMSPDDIR)/src
+STMSPDINCDIR = $(STMSPDDIR)/inc
+HWDEFSINC = ../../board_hw_defs/$(BOARD_NAME)
+
+# List C source files here. (C dependencies are automatically generated.)
+# use file-extension c for "c-only"-files
+
+## BOOTLOADER:
+SRC += main.c
+SRC += pios_board.c
+SRC += pios_usb_board_data.c
+SRC += op_dfu.c
+
+## PIOS Hardware (STM32F4xx)
+include $(PIOS)/STM32F4xx/library.mk
+
+# PIOS Hardware (Common)
+SRC += $(PIOSCOMMON)/pios_board_info.c
+SRC += $(PIOSCOMMON)/pios_com_msg.c
+SRC += $(PIOSCOMMON)/printf-stdarg.c
+SRC += $(PIOSCOMMON)/pios_usb_desc_hid_only.c
+SRC += $(PIOSCOMMON)/pios_usb_util.c
+
+# List C source files here which must be compiled in ARM-Mode (no -mthumb).
+# use file-extension c for "c-only"-files
+## just for testing, timer.c could be compiled in thumb-mode too
+SRCARM = 
+
+# List C++ source files here.
+# use file-extension .cpp for C++-files (not .C)
+CPPSRC = 
+
+# List C++ source files here which must be compiled in ARM-Mode.
+# use file-extension .cpp for C++-files (not .C)
+#CPPSRCARM = $(TARGET).cpp
+CPPSRCARM = 
+
+# List any extra directories to look for include files here.
+#    Each directory must be seperated by a space.
+EXTRAINCDIRS  += $(PIOS)
+EXTRAINCDIRS  += $(PIOSINC)
+EXTRAINCDIRS  += $(FLIGHTLIBINC)
+EXTRAINCDIRS  += $(PIOSSTM34FXX)
+EXTRAINCDIRS  += $(PIOSCOMMON)
+EXTRAINCDIRS  += $(PIOSBOARDS)
+EXTRAINCDIRS  += $(STMSPDINCDIR)
+EXTRAINCDIRS  += $(CMSISDIR)
+EXTRAINCDIRS  += $(REVO_BLINC)
+EXTRAINCDIRS  += $(HWDEFSINC)
+
+# List any extra directories to look for library files here.
+# Also add directories where the linker should search for
+# includes from linker-script to the list
+#     Each directory must be seperated by a space.
+EXTRA_LIBDIRS = 
+
+# Extra Libraries
+#    Each library-name must be seperated by a space.
+#    i.e. to link with libxyz.a, libabc.a and libefsl.a: 
+#    EXTRA_LIBS = xyz abc efsl
+# for newlib-lpc (file: libnewlibc-lpc.a):
+#    EXTRA_LIBS = newlib-lpc
+EXTRA_LIBS =
+
+# Path to Linker-Scripts
+LINKERSCRIPTPATH = $(PIOSSTM32FXX)
+
+# Optimization level, can be [0, 1, 2, 3, s]. 
+# 0 = turn off optimization. s = optimize for size.
+# (Note: 3 is not always the best optimization level. See avr-libc FAQ.)
+
+ifeq ($(DEBUG),YES)
+OPT = 0
+else
+OPT = s
+endif
+
+# Output format. (can be ihex or binary or both)
+#  binary to create a load-image in raw-binary format i.e. for SAM-BA, 
+#  ihex to create a load-image in Intel hex format
+#LOADFORMAT = ihex
+#LOADFORMAT = binary
+LOADFORMAT = both
+
+# Debugging format.
+DEBUGF = dwarf-2
+
+# Place project-specific -D (define) and/or 
+# -U options for C here.
+CDEFS = -DSTM32F4XX
+CDEFS += -DSYSCLK_FREQ=$(SYSCLK_FREQ)
+CDEFS += -DHSE_VALUE=$(OSCILLATOR_FREQ)
+CDEFS += -DUSE_STDPERIPH_DRIVER
+CDEFS += -DUSE_$(BOARD)
+
+# Provide (only) the bootloader with board-specific defines
+BLONLY_CDEFS += -DBOARD_TYPE=$(BOARD_TYPE)
+BLONLY_CDEFS += -DBOARD_REVISION=$(BOARD_REVISION)
+BLONLY_CDEFS += -DHW_TYPE=$(HW_TYPE)
+BLONLY_CDEFS += -DBOOTLOADER_VERSION=$(BOOTLOADER_VERSION)
+BLONLY_CDEFS += -DFW_BANK_BASE=$(FW_BANK_BASE)
+BLONLY_CDEFS += -DFW_BANK_SIZE=$(FW_BANK_SIZE)
+BLONLY_CDEFS += -DFW_DESC_SIZE=$(FW_DESC_SIZE)
+
+# Place project-specific -D and/or -U options for 
+# Assembler with preprocessor here.
+#ADEFS = -DUSE_IRQ_ASM_WRAPPER
+ADEFS = -D__ASSEMBLY__
+
+# Compiler flag to set the C Standard level.
+# c89   - "ANSI" C
+# gnu89 - c89 plus GCC extensions
+# c99   - ISO C99 standard (not yet fully implemented)
+# gnu99 - c99 plus GCC extensions
+CSTANDARD = -std=gnu99
+
+#-----
+
+# Compiler flags.
+
+#  -g*:          generate debugging information
+#  -O*:          optimization level
+#  -f...:        tuning, see GCC manual and avr-libc documentation
+#  -Wall...:     warning level
+#  -Wa,...:      tell GCC to pass this to the assembler.
+#    -adhlns...: create assembler listing
+#
+# Flags for C and C++ (arm-elf-gcc/arm-elf-g++)
+
+ifeq ($(DEBUG),YES)
+CFLAGS = 
+endif
+
+# This is not the best place for these.  Really should abstract out
+# to the board file or something
+CFLAGS += -DSTM32F4XX
+CFLAGS += -DMEM_SIZE=1024000000
+
+CFLAGS += -g$(DEBUGF)
+CFLAGS += -O$(OPT)
+ifeq ($(DEBUG),NO)
+CFLAGS += -fdata-sections -ffunction-sections
+endif
+CFLAGS += -mcpu=$(MCU)
+CFLAGS += $(CDEFS)
+CFLAGS += $(BLONLY_CDEFS)
+CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I.
+
+CFLAGS += -mapcs-frame 
+CFLAGS += -fomit-frame-pointer
+ifeq ($(CODE_SOURCERY), YES)
+CFLAGS += -fpromote-loop-indices
+endif
+
+CFLAGS += -Wall
+#CFLAGS += -Werror
+CFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
+# Compiler flags to generate dependency files:
+CFLAGS += -MD -MP -MF $(OUTDIR)/dep/$(@F).d
+
+# flags only for C
+#CONLYFLAGS += -Wnested-externs 
+CONLYFLAGS += $(CSTANDARD)
+
+# Assembler flags.
+#  -Wa,...:    tell GCC to pass this to the assembler.
+#  -ahlns:     create listing
+ASFLAGS  = -mcpu=$(MCU) -I. -x assembler-with-cpp
+ASFLAGS += $(ADEFS)
+ASFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
+ASFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS))
+
+MATH_LIB = -lm
+
+# Linker flags.
+#  -Wl,...:     tell GCC to pass this to linker.
+#    -Map:      create map file
+#    --cref:    add cross reference to  map file
+LDFLAGS = -nostartfiles -Wl,-Map=$(OUTDIR)/$(TARGET).map,--cref,--gc-sections
+ifeq ($(DEBUG),NO)
+LDFLAGS += -Wl,-static
+endif
+LDFLAGS += $(patsubst %,-L%,$(EXTRA_LIBDIRS))
+LDFLAGS += -lc
+LDFLAGS += $(patsubst %,-l%,$(EXTRA_LIBS))
+LDFLAGS += $(MATH_LIB)
+LDFLAGS += -lc -lgcc 
+
+#Linker scripts                                                                                                                                                                                                              
+LDFLAGS += $(addprefix -T,$(LINKER_SCRIPTS_BL))
+
+# Define programs and commands.
+REMOVE  = $(REMOVE_CMD) -f
+
+# List of all source files.
+ALLSRC     = $(ASRCARM) $(ASRC) $(SRCARM) $(SRC) $(CPPSRCARM) $(CPPSRC)
+# List of all source files without directory and file-extension.
+ALLSRCBASE = $(notdir $(basename $(ALLSRC)))
+
+# Define all object files.
+ALLOBJ     = $(addprefix $(OUTDIR)/, $(addsuffix .o, $(ALLSRCBASE)))
+
+# Define all listing files (used for make clean).
+LSTFILES   = $(addprefix $(OUTDIR)/, $(addsuffix .lst, $(ALLSRCBASE)))
+# Define all depedency-files (used for make clean).
+DEPFILES   = $(addprefix $(OUTDIR)/dep/, $(addsuffix .o.d, $(ALLSRCBASE)))
+
+# Default target.
+all: build
+
+ifeq ($(LOADFORMAT),ihex)
+build: elf hex sym
+else 
+ifeq ($(LOADFORMAT),binary)
+build: elf bin sym
+else 
+ifeq ($(LOADFORMAT),both)
+build: elf hex bin sym
+else 
+$(error "$(MSG_FORMATERROR) $(FORMAT)")
+endif
+endif
+endif
+
+# Link: create ELF output file from object files.
+$(eval $(call LINK_TEMPLATE, $(OUTDIR)/$(TARGET).elf, $(ALLOBJ)))
+
+# Assemble: create object files from assembler source files.
+$(foreach src, $(ASRC), $(eval $(call ASSEMBLE_TEMPLATE, $(src)))) 
+
+# Assemble: create object files from assembler source files. ARM-only
+$(foreach src, $(ASRCARM), $(eval $(call ASSEMBLE_ARM_TEMPLATE, $(src)))) 
+
+# Compile: create object files from C source files.
+$(foreach src, $(SRC), $(eval $(call COMPILE_C_TEMPLATE, $(src)))) 
+
+# Compile: create object files from C source files. ARM-only
+$(foreach src, $(SRCARM), $(eval $(call COMPILE_C_ARM_TEMPLATE, $(src)))) 
+
+# Compile: create object files from C++ source files.
+$(foreach src, $(CPPSRC), $(eval $(call COMPILE_CPP_TEMPLATE, $(src)))) 
+
+# Compile: create object files from C++ source files. ARM-only
+$(foreach src, $(CPPSRCARM), $(eval $(call COMPILE_CPP_ARM_TEMPLATE, $(src)))) 
+
+# Compile: create assembler files from C source files. ARM/Thumb
+$(eval $(call PARTIAL_COMPILE_TEMPLATE, SRC))
+
+# Compile: create assembler files from C source files. ARM only
+$(eval $(call PARTIAL_COMPILE_ARM_TEMPLATE, SRCARM))
+
+$(OUTDIR)/$(TARGET).bin.o: $(OUTDIR)/$(TARGET).bin
+
+$(eval $(call OPFW_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BOARD_TYPE),$(BOARD_REVISION)))
+
+# Add jtag targets (program and wipe)
+$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BL_BANK_BASE),$(BL_BANK_SIZE),$(OPENOCD_JTAG_CONFIG),$(OPENOCD_CONFIG)))
+
+.PHONY: elf lss sym hex bin bino
+elf: $(OUTDIR)/$(TARGET).elf
+lss: $(OUTDIR)/$(TARGET).lss 
+sym: $(OUTDIR)/$(TARGET).sym
+hex: $(OUTDIR)/$(TARGET).hex
+bin: $(OUTDIR)/$(TARGET).bin
+bino: $(OUTDIR)/$(TARGET).bin.o
+
+# Display sizes of sections.
+$(eval $(call SIZE_TEMPLATE, $(OUTDIR)/$(TARGET).elf))
+
+# Generate Doxygen documents
+docs:
+	doxygen  $(DOXYGENDIR)/doxygen.cfg
+
+# Install: install binary file with prefix/suffix into install directory
+install: $(OUTDIR)/$(TARGET).bin
+ifneq ($(INSTALL_DIR),)
+	@echo $(MSG_INSTALLING) $(call toprel, $<)
+	$(V1) mkdir -p $(INSTALL_DIR)
+	$(V1) $(INSTALL) $< $(INSTALL_DIR)/$(INSTALL_PFX)$(TARGET)$(INSTALL_SFX).bin
+else
+	$(error INSTALL_DIR must be specified for $@)
+endif
+
+# Target: clean project.
+clean: clean_list
+
+clean_list :
+	@echo $(MSG_CLEANING)
+	$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).map
+	$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).elf
+	$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).hex
+	$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).bin
+	$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).sym
+	$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).lss
+	$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).bin.o
+	$(V1) $(REMOVE) $(ALLOBJ)
+	$(V1) $(REMOVE) $(LSTFILES)
+	$(V1) $(REMOVE) $(DEPFILES)
+	$(V1) $(REMOVE) $(SRC:.c=.s)
+	$(V1) $(REMOVE) $(SRCARM:.c=.s)
+	$(V1) $(REMOVE) $(CPPSRC:.cpp=.s)
+	$(V1) $(REMOVE) $(CPPSRCARM:.cpp=.s)
+
+
+# Create output files directory
+# all known MS Windows OS define the ComSpec environment variable
+ifdef ComSpec
+$(shell md $(subst /,\\,$(OUTDIR)) 2>NUL)
+else
+$(shell mkdir -p $(OUTDIR) 2>/dev/null)
+endif
+
+# Include the dependency files.
+ifdef ComSpec
+-include $(shell md $(subst /,\\,$(OUTDIR))\dep 2>NUL) $(wildcard $(OUTDIR)/dep/*)
+else
+-include $(shell mkdir -p $(OUTDIR) 2>/dev/null) $(shell mkdir $(OUTDIR)/dep 2>/dev/null) $(wildcard $(OUTDIR)/dep/*)
+endif
+
+# Listing of phony targets.
+.PHONY : all build clean clean_list install

flight/Bootloaders/RevoMini/inc/common.h

@@ -0,0 +1,115 @@
+/**
+ ******************************************************************************
+ * @addtogroup CopterControlBL CopterControl BootLoader
+ * @brief These files contain the code to the CopterControl Bootloader.
+ *
+ * @{
+ * @file       common.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      This file contains various common defines for the BootLoader
+ * @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 COMMON_H_
+#define COMMON_H_
+
+//#include "board.h"
+
+typedef enum {
+	start, keepgoing,
+} DownloadAction;
+
+/**************************************************/
+/* OP_DFU states                       */
+/**************************************************/
+
+typedef enum {
+	DFUidle, //0
+	uploading, //1
+	wrong_packet_received, //2
+	too_many_packets, //3
+	too_few_packets, //4
+	Last_operation_Success, //5
+	downloading, //6
+	BLidle, //7
+	Last_operation_failed, //8
+	uploadingStarting, //9
+	outsideDevCapabilities, //10
+	CRC_Fail,//11
+	failed_jump,
+//12
+} DFUStates;
+/**************************************************/
+/* OP_DFU commands                       */
+/**************************************************/
+typedef enum {
+	Reserved, //0
+	Req_Capabilities, //1
+	Rep_Capabilities, //2
+	EnterDFU, //3
+	JumpFW, //4
+	Reset, //5
+	Abort_Operation, //6
+	Upload, //7
+	Op_END, //8
+	Download_Req, //9
+	Download, //10
+	Status_Request, //11
+	Status_Rep
+//12
+} DFUCommands;
+
+typedef enum {
+	High_Density, Medium_Density
+} DeviceType;
+/**************************************************/
+/* OP_DFU transfer types                       */
+/**************************************************/
+typedef enum {
+	FW, //0
+	Descript
+//2
+} DFUTransfer;
+/**************************************************/
+/* OP_DFU transfer port                           */
+/**************************************************/
+typedef enum {
+	Usb, //0
+	Serial
+//2
+} DFUPort;
+/**************************************************/
+/* OP_DFU programable programable HW types        */
+/**************************************************/
+typedef enum {
+	Self_flash, //0
+	Remote_flash_via_spi
+//1
+} DFUProgType;
+/**************************************************/
+/* OP_DFU programable sources			          */
+/**************************************************/
+#define USB	0
+#define SPI 1
+
+#define DownloadDelay					100000
+
+#define MAX_DEL_RETRYS					3
+#define MAX_WRI_RETRYS					3
+
+#endif /* COMMON_H_ */

flight/Bootloaders/RevoMini/inc/op_dfu.h

@@ -0,0 +1,60 @@
+/**
+ ******************************************************************************
+ *
+ * @file       op_dfu.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief
+ * @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
+ */
+
+/* Define to prevent recursive inclusion -------------------------------------*/
+#ifndef __OP_DFU_H
+#define __OP_DFU_H
+#include "common.h"
+/* Includes ------------------------------------------------------------------*/
+/* Exported types ------------------------------------------------------------*/
+typedef struct {
+	uint8_t programmingType;
+	uint8_t readWriteFlags;
+	uint32_t startOfUserCode;
+	uint32_t sizeOfCode;
+	uint8_t sizeOfDescription;
+	uint8_t BL_Version;
+	uint16_t devID;
+	DeviceType devType;
+	uint32_t FW_Crc;
+} Device;
+
+/* Exported constants --------------------------------------------------------*/
+/* Exported macro ------------------------------------------------------------*/
+/* Exported define -----------------------------------------------------------*/
+#define COMMAND	0
+#define COUNT	1
+#define DATA	5
+
+/* Exported functions ------------------------------------------------------- */
+void processComand(uint8_t *Receive_Buffer);
+uint32_t baseOfAdressType(uint8_t type);
+uint8_t isBiggerThanAvailable(uint8_t type, uint32_t size);
+void OPDfuIni(uint8_t discover);
+void DataDownload(DownloadAction);
+bool flash_read(uint8_t * buffer, uint32_t adr, DFUProgType type);
+#endif /* __OP_DFU_H */
+
+/******************* (C) COPYRIGHT 2010 STMicroelectronics *****END OF FILE****/

flight/Bootloaders/RevoMini/inc/pios_config.h

@@ -0,0 +1,43 @@
+/**
+ ******************************************************************************
+ *
+ * @file       pios_config.h  
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      PiOS configuration header. 
+ *                 - Central compile time config for the project.
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/* 
+ * 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_CONFIG_H
+#define PIOS_CONFIG_H
+
+/* Enable/Disable PiOS Modules */
+#define PIOS_INCLUDE_DELAY
+#define PIOS_INCLUDE_IRQ
+#define PIOS_INCLUDE_LED
+#define PIOS_INCLUDE_SPI
+#define PIOS_INCLUDE_SYS
+#define PIOS_INCLUDE_IAP
+#define PIOS_INCLUDE_USB
+#define PIOS_INCLUDE_USB_HID
+#define PIOS_INCLUDE_COM_MSG
+#define PIOS_INCLUDE_BL_HELPER
+#define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT
+
+#endif /* PIOS_CONFIG_H */

flight/Bootloaders/RevoMini/inc/pios_usb_board_data.h

@@ -0,0 +1,52 @@
+/**
+ ******************************************************************************
+ * @addtogroup PIOS PIOS Core hardware abstraction layer
+ * @{
+ * @addtogroup PIOS_USB_BOARD Board specific USB definitions
+ * @brief Board specific USB definitions
+ * @{
+ *
+ * @file       pios_usb_board_data.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Board specific USB definitions
+ * @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_USB_BOARD_DATA_H
+#define PIOS_USB_BOARD_DATA_H
+
+#define PIOS_USB_BOARD_HID_DATA_LENGTH 64
+
+#define PIOS_USB_BOARD_EP_NUM 2
+
+#include "pios_usb_defs.h" 	/* struct usb_* */
+
+#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_REVOLUTION
+#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_REVOLUTION, USB_OP_BOARD_MODE_BL)
+#define PIOS_USB_BOARD_SN_SUFFIX "+BL"
+
+/*
+ * The bootloader uses a simplified report structure
+ *   BL: <REPORT_ID><DATA>...<DATA>
+ *   FW: <REPORT_ID><LENGTH><DATA>...<DATA>
+ * This define changes the behaviour in pios_usb_hid.c
+ */
+#define PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE
+
+#endif	/* PIOS_USB_BOARD_DATA_H */

flight/Bootloaders/RevoMini/main.c

@@ -0,0 +1,230 @@
+/**
+ ******************************************************************************
+ * @addtogroup RevolutionBL Revolution BootLoader
+ * @brief These files contain the code to the Revolution Bootloader.
+ *
+ * @{
+ * @file       main.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      This is the file with the main function of the Revolution BootLoader
+ * @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
+ */
+/* Bootloader Includes */
+#include <pios.h>
+#include <pios_board_info.h>
+#include <stdbool.h>
+#include "op_dfu.h"
+#include "pios_iap.h"
+#include "fifo_buffer.h"
+#include "pios_com_msg.h"
+#include "pios_usbhook.h"	/* PIOS_USBHOOK_* */
+/* Prototype of PIOS_Board_Init() function */
+extern void PIOS_Board_Init(void);
+extern void FLASH_Download();
+void check_bor();
+#define BSL_HOLD_STATE ((PIOS_USB_DETECT_GPIO_PORT->IDR & PIOS_USB_DETECT_GPIO_PIN) ? 0 : 1)
+
+/* Private typedef -----------------------------------------------------------*/
+typedef void (*pFunction)(void);
+/* Private define ------------------------------------------------------------*/
+/* Private macro -------------------------------------------------------------*/
+/* Private variables ---------------------------------------------------------*/
+pFunction Jump_To_Application;
+uint32_t JumpAddress;
+
+/// LEDs PWM
+uint32_t period1 = 5000; // 5 mS
+uint32_t sweep_steps1 = 100; // * 5 mS -> 500 mS
+uint32_t period2 = 5000; // 5 mS
+uint32_t sweep_steps2 = 100; // * 5 mS -> 500 mS
+
+
+////////////////////////////////////////
+uint8_t tempcount = 0;
+
+/* Extern variables ----------------------------------------------------------*/
+DFUStates DeviceState;
+int16_t status = 0;
+bool JumpToApp = false;
+bool GO_dfu = false;
+bool USB_connected = false;
+bool User_DFU_request = false;
+static uint8_t mReceive_Buffer[63];
+/* Private function prototypes -----------------------------------------------*/
+uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count);
+uint8_t processRX();
+void jump_to_app();
+
+int main() {
+	PIOS_SYS_Init();	
+	PIOS_Board_Init();
+	PIOS_IAP_Init();
+
+	// Make sure the brown out reset value for this chip
+	// is 2.7 volts
+	check_bor();
+
+	USB_connected = PIOS_USB_CheckAvailable(0);
+
+	if (PIOS_IAP_CheckRequest() == true) {
+		PIOS_DELAY_WaitmS(1000);
+		User_DFU_request = true;
+		PIOS_IAP_ClearRequest();
+	}
+
+	GO_dfu = (USB_connected == true) || (User_DFU_request == true);
+
+	if (GO_dfu == true) {
+		if (User_DFU_request == true)
+			DeviceState = DFUidle;
+		else
+			DeviceState = BLidle;
+	} else
+		JumpToApp = true;
+
+	uint32_t stopwatch = 0;
+	uint32_t prev_ticks = PIOS_DELAY_GetuS();
+	while (true) {
+		/* Update the stopwatch */
+		uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks);
+		prev_ticks += elapsed_ticks;
+		stopwatch += elapsed_ticks;
+
+		if (JumpToApp == true)
+			jump_to_app();
+
+		switch (DeviceState) {
+		case Last_operation_Success:
+		case uploadingStarting:
+		case DFUidle:
+			period1 = 5000;
+			sweep_steps1 = 100;
+			PIOS_LED_Off(PIOS_LED_HEARTBEAT);
+			period2 = 0;
+			break;
+		case uploading:
+			period1 = 5000;
+			sweep_steps1 = 100;
+			period2 = 2500;
+			sweep_steps2 = 50;
+			break;
+		case downloading:
+			period1 = 2500;
+			sweep_steps1 = 50;
+			PIOS_LED_Off(PIOS_LED_HEARTBEAT);
+			period2 = 0;
+			break;
+		case BLidle:
+			period1 = 0;
+			PIOS_LED_On(PIOS_LED_HEARTBEAT);
+			period2 = 0;
+			break;
+		default://error
+			period1 = 5000;
+			sweep_steps1 = 100;
+			period2 = 5000;
+			sweep_steps2 = 100;
+		}
+
+		if (period1 != 0) {
+			if (LedPWM(period1, sweep_steps1, stopwatch))
+				PIOS_LED_On(PIOS_LED_HEARTBEAT);
+			else
+				PIOS_LED_Off(PIOS_LED_HEARTBEAT);
+		} else
+			PIOS_LED_On(PIOS_LED_HEARTBEAT);
+
+		if (period2 != 0) {
+			if (LedPWM(period2, sweep_steps2, stopwatch))
+				PIOS_LED_On(PIOS_LED_HEARTBEAT);
+			else
+				PIOS_LED_Off(PIOS_LED_HEARTBEAT);
+		} else
+			PIOS_LED_Off(PIOS_LED_HEARTBEAT);
+
+		if (stopwatch > 50 * 1000 * 1000)
+			stopwatch = 0;
+		if ((stopwatch > 6 * 1000 * 1000) && (DeviceState
+				== BLidle))
+			JumpToApp = true;
+
+		processRX();
+		DataDownload(start);
+	}
+}
+
+void jump_to_app() {
+	const struct pios_board_info * bdinfo = &pios_board_info_blob;
+
+	PIOS_LED_On(PIOS_LED_HEARTBEAT);
+	if (((*(__IO uint32_t*) bdinfo->fw_base) & 0x2FFE0000) == 0x20000000) { /* Jump to user application */
+		FLASH_Lock();
+		RCC_APB2PeriphResetCmd(0xffffffff, ENABLE);
+		RCC_APB1PeriphResetCmd(0xffffffff, ENABLE);
+		RCC_APB2PeriphResetCmd(0xffffffff, DISABLE);
+		RCC_APB1PeriphResetCmd(0xffffffff, DISABLE);
+
+		PIOS_USBHOOK_Deactivate();
+
+		JumpAddress = *(__IO uint32_t*) (bdinfo->fw_base + 4);
+		Jump_To_Application = (pFunction) JumpAddress;
+		/* Initialize user application's Stack Pointer */
+		__set_MSP(*(__IO uint32_t*) bdinfo->fw_base);
+		Jump_To_Application();
+	} else {
+		DeviceState = failed_jump;
+		return;
+	}
+}
+uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) {
+	uint32_t curr_step = (count / pwm_period) % pwm_sweep_steps; /* 0 - pwm_sweep_steps */
+	uint32_t pwm_duty = pwm_period * curr_step / pwm_sweep_steps; /* fraction of pwm_period */
+
+	uint32_t curr_sweep = (count / (pwm_period * pwm_sweep_steps)); /* ticks once per full sweep */
+	if (curr_sweep & 1) {
+		pwm_duty = pwm_period - pwm_duty; /* reverse direction in odd sweeps */
+	}
+	return ((count % pwm_period) > pwm_duty) ? 1 : 0;
+}
+
+uint8_t processRX() {
+	if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) {
+		processComand(mReceive_Buffer);
+	}
+	return true;
+}
+
+/**
+ * Check the brown out reset threshold is 2.7 volts and if not
+ * resets it.  This solves an issue that can prevent boards
+ * powering up with some BEC
+ */
+void check_bor()
+{
+	uint8_t bor = FLASH_OB_GetBOR();
+	if(bor != OB_BOR_LEVEL3) {
+		FLASH_OB_Unlock();
+		FLASH_OB_BORConfig(OB_BOR_LEVEL3);
+		FLASH_OB_Launch();
+		while(FLASH_WaitForLastOperation() == FLASH_BUSY);
+		FLASH_OB_Lock();
+		while(FLASH_WaitForLastOperation() == FLASH_BUSY);
+	}
+}
+

flight/Bootloaders/RevoMini/op_dfu.c

@@ -0,0 +1,468 @@
+/**
+ ******************************************************************************
+ * @addtogroup CopterControlBL CopterControl BootLoader
+ * @brief These files contain the code to the CopterControl Bootloader.
+ *
+ * @{
+ * @file       op_dfu.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      This file contains the DFU commands handling code
+ * @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
+ */
+
+/* Includes ------------------------------------------------------------------*/
+#include "pios.h"
+#include <stdbool.h>
+#include "op_dfu.h"
+#include "pios_bl_helper.h"
+#include "pios_com_msg.h"
+#include <pios_board_info.h>
+//programmable devices
+Device devicesTable[10];
+uint8_t numberOfDevices = 0;
+
+DFUProgType currentProgrammingDestination; //flash, flash_trough spi
+uint8_t currentDeviceCanRead;
+uint8_t currentDeviceCanWrite;
+Device currentDevice;
+
+uint8_t Buffer[64];
+uint8_t echoBuffer[64];
+uint8_t SendBuffer[64];
+uint8_t Command = 0;
+uint8_t EchoReqFlag = 0;
+uint8_t EchoAnsFlag = 0;
+uint8_t StartFlag = 0;
+uint32_t Aditionals = 0;
+uint32_t SizeOfTransfer = 0;
+uint32_t Expected_CRC = 0;
+uint8_t SizeOfLastPacket = 0;
+uint32_t Next_Packet = 0;
+uint8_t TransferType;
+uint32_t Count = 0;
+uint32_t Data;
+uint8_t Data0;
+uint8_t Data1;
+uint8_t Data2;
+uint8_t Data3;
+uint8_t offset = 0;
+uint32_t aux;
+//Download vars
+uint32_t downSizeOfLastPacket = 0;
+uint32_t downPacketTotal = 0;
+uint32_t downPacketCurrent = 0;
+DFUTransfer downType = 0;
+/* Extern variables ----------------------------------------------------------*/
+extern DFUStates DeviceState;
+extern uint8_t JumpToApp;
+/* Private function prototypes -----------------------------------------------*/
+/* Private functions ---------------------------------------------------------*/
+void sendData(uint8_t * buf, uint16_t size);
+uint32_t CalcFirmCRC(void);
+
+void DataDownload(DownloadAction action) {
+	if ((DeviceState == downloading)) {
+
+		uint8_t packetSize;
+		uint32_t offset;
+		uint32_t partoffset;
+		SendBuffer[0] = 0x01;
+		SendBuffer[1] = Download;
+		SendBuffer[2] = downPacketCurrent >> 24;
+		SendBuffer[3] = downPacketCurrent >> 16;
+		SendBuffer[4] = downPacketCurrent >> 8;
+		SendBuffer[5] = downPacketCurrent;
+		if (downPacketCurrent == downPacketTotal - 1) {
+			packetSize = downSizeOfLastPacket;
+		} else {
+			packetSize = 14;
+		}
+		for (uint8_t x = 0; x < packetSize; ++x) {
+			partoffset = (downPacketCurrent * 14 * 4) + (x * 4);
+			offset = baseOfAdressType(downType) + partoffset;
+			if (!flash_read(SendBuffer + (6 + x * 4), offset,
+					currentProgrammingDestination)) {
+				DeviceState = Last_operation_failed;
+			}
+		}
+		downPacketCurrent = downPacketCurrent + 1;
+		if (downPacketCurrent > downPacketTotal - 1) {
+			DeviceState = Last_operation_Success;
+			Aditionals = (uint32_t) Download;
+		}
+		sendData(SendBuffer + 1, 63);
+	}
+}
+void processComand(uint8_t *xReceive_Buffer) {
+
+	Command = xReceive_Buffer[COMMAND];
+#ifdef DEBUG_SSP
+	char str[63]= {0};
+	sprintf(str,"Received COMMAND:%d|",Command);
+	PIOS_COM_SendString(PIOS_COM_TELEM_USB,str);
+#endif
+	EchoReqFlag = (Command >> 7);
+	EchoAnsFlag = (Command >> 6) & 0x01;
+	StartFlag = (Command >> 5) & 0x01;
+	Count = xReceive_Buffer[COUNT] << 24;
+	Count += xReceive_Buffer[COUNT + 1] << 16;
+	Count += xReceive_Buffer[COUNT + 2] << 8;
+	Count += xReceive_Buffer[COUNT + 3];
+
+	Data = xReceive_Buffer[DATA] << 24;
+	Data += xReceive_Buffer[DATA + 1] << 16;
+	Data += xReceive_Buffer[DATA + 2] << 8;
+	Data += xReceive_Buffer[DATA + 3];
+	Data0 = xReceive_Buffer[DATA];
+	Data1 = xReceive_Buffer[DATA + 1];
+	Data2 = xReceive_Buffer[DATA + 2];
+	Data3 = xReceive_Buffer[DATA + 3];
+	Command = Command & 0b00011111;
+
+	if (EchoReqFlag == 1) {
+		memcpy(echoBuffer, xReceive_Buffer, 64);
+	}
+	switch (Command) {
+	case EnterDFU:
+		if (((DeviceState == BLidle) && (Data0 < numberOfDevices))
+				|| (DeviceState == DFUidle)) {
+			if (Data0 > 0)
+				OPDfuIni(true);
+			DeviceState = DFUidle;
+			currentProgrammingDestination = devicesTable[Data0].programmingType;
+			currentDeviceCanRead = devicesTable[Data0].readWriteFlags & 0x01;
+			currentDeviceCanWrite = devicesTable[Data0].readWriteFlags >> 1
+					& 0x01;
+			currentDevice = devicesTable[Data0];
+			uint8_t result = 0;
+			switch (currentProgrammingDestination) {
+			case Self_flash:
+				result = PIOS_BL_HELPER_FLASH_Ini();
+				break;
+			case Remote_flash_via_spi:
+				result = true;
+				break;
+			default:
+				DeviceState = Last_operation_failed;
+				Aditionals = (uint16_t) Command;
+			}
+			if (result != 1) {
+				DeviceState = Last_operation_failed;
+				Aditionals = (uint32_t) Command;
+			}
+		}
+		break;
+	case Upload:
+		if ((DeviceState == DFUidle) || (DeviceState == uploading)) {
+			if ((StartFlag == 1) && (Next_Packet == 0)) {
+				TransferType = Data0;
+				SizeOfTransfer = Count;
+				Next_Packet = 1;
+				Expected_CRC = Data2 << 24;
+				Expected_CRC += Data3 << 16;
+				Expected_CRC += xReceive_Buffer[DATA + 4] << 8;
+				Expected_CRC += xReceive_Buffer[DATA + 5];
+				SizeOfLastPacket = Data1;
+
+				if (isBiggerThanAvailable(TransferType, (SizeOfTransfer - 1)
+						* 14 * 4 + SizeOfLastPacket * 4) == true) {
+					DeviceState = outsideDevCapabilities;
+					Aditionals = (uint32_t) Command;
+				} else {
+					uint8_t result = 1;
+					if (TransferType == FW) {
+						switch (currentProgrammingDestination) {
+						case Self_flash:
+							result = PIOS_BL_HELPER_FLASH_Start();
+							break;
+						case Remote_flash_via_spi:
+							result = false;
+							break;
+						default:
+							break;
+						}
+					}
+					if (result != 1) {
+						DeviceState = Last_operation_failed;
+						Aditionals = (uint32_t) Command;
+					} else {
+
+						DeviceState = uploading;
+					}
+				}
+			} else if ((StartFlag != 1) && (Next_Packet != 0)) {
+				if (Count > SizeOfTransfer) {
+					DeviceState = too_many_packets;
+					Aditionals = Count;
+				} else if (Count == Next_Packet - 1) {
+					uint8_t numberOfWords = 14;
+					if (Count == SizeOfTransfer - 1)//is this the last packet?
+					{
+						numberOfWords = SizeOfLastPacket;
+					}
+					uint8_t result = 0;
+					switch (currentProgrammingDestination) {
+					case Self_flash:
+						for (uint8_t x = 0; x < numberOfWords; ++x) {
+							offset = 4 * x;
+							Data = xReceive_Buffer[DATA + offset] << 24;
+							Data += xReceive_Buffer[DATA + 1 + offset] << 16;
+							Data += xReceive_Buffer[DATA + 2 + offset] << 8;
+							Data += xReceive_Buffer[DATA + 3 + offset];
+							aux = baseOfAdressType(TransferType) + (uint32_t)(
+									Count * 14 * 4 + x * 4);
+							result = 0;
+							for (int retry = 0; retry < MAX_WRI_RETRYS; ++retry) {
+								if (result == 0) {
+									result = (FLASH_ProgramWord(aux, Data)
+											== FLASH_COMPLETE) ? 1 : 0;
+								}
+							}
+						}
+						break;
+					case Remote_flash_via_spi:
+						result = false; // No support for this for the PipX
+						break;
+					default:
+						result = 0;
+						break;
+					}
+					if (result != 1) {
+						DeviceState = Last_operation_failed;
+						Aditionals = (uint32_t) Command;
+					}
+
+					++Next_Packet;
+				} else {
+					DeviceState = wrong_packet_received;
+					Aditionals = Count;
+				}
+			} else {
+				DeviceState = Last_operation_failed;
+				Aditionals = (uint32_t) Command;
+			}
+		}
+		break;
+	case Req_Capabilities:
+		OPDfuIni(true);
+		Buffer[0] = 0x01;
+		Buffer[1] = Rep_Capabilities;
+		if (Data0 == 0) {
+			Buffer[2] = 0;
+			Buffer[3] = 0;
+			Buffer[4] = 0;
+			Buffer[5] = 0;
+			Buffer[6] = 0;
+			Buffer[7] = numberOfDevices;
+			uint16_t WRFlags = 0;
+			for (int x = 0; x < numberOfDevices; ++x) {
+				WRFlags = ((devicesTable[x].readWriteFlags << (x * 2))
+						| WRFlags);
+			}
+			Buffer[8] = WRFlags >> 8;
+			Buffer[9] = WRFlags;
+		} else {
+			Buffer[2] = devicesTable[Data0 - 1].sizeOfCode >> 24;
+			Buffer[3] = devicesTable[Data0 - 1].sizeOfCode >> 16;
+			Buffer[4] = devicesTable[Data0 - 1].sizeOfCode >> 8;
+			Buffer[5] = devicesTable[Data0 - 1].sizeOfCode;
+			Buffer[6] = Data0;
+			Buffer[7] = devicesTable[Data0 - 1].BL_Version;
+			Buffer[8] = devicesTable[Data0 - 1].sizeOfDescription;
+			Buffer[9] = devicesTable[Data0 - 1].devID;
+			Buffer[10] = devicesTable[Data0 - 1].FW_Crc >> 24;
+			Buffer[11] = devicesTable[Data0 - 1].FW_Crc >> 16;
+			Buffer[12] = devicesTable[Data0 - 1].FW_Crc >> 8;
+			Buffer[13] = devicesTable[Data0 - 1].FW_Crc;
+			Buffer[14] = devicesTable[Data0 - 1].devID >> 8;
+			Buffer[15] = devicesTable[Data0 - 1].devID;
+		}
+		sendData(Buffer + 1, 63);
+		break;
+	case JumpFW:
+		if (Data == 0x5AFE) {
+			/* Force board into safe mode */
+			PIOS_IAP_WriteBootCount(0xFFFF);
+		}
+		FLASH_Lock();
+		JumpToApp = 1;
+		break;
+	case Reset:
+		PIOS_SYS_Reset();
+		break;
+	case Abort_Operation:
+		Next_Packet = 0;
+		DeviceState = DFUidle;
+		break;
+
+	case Op_END:
+		if (DeviceState == uploading) {
+			if (Next_Packet - 1 == SizeOfTransfer) {
+				Next_Packet = 0;
+				if ((TransferType != FW) || (Expected_CRC == CalcFirmCRC())) {
+					DeviceState = Last_operation_Success;
+				} else {
+					DeviceState = CRC_Fail;
+				}
+			}
+			if (Next_Packet - 1 < SizeOfTransfer) {
+				Next_Packet = 0;
+				DeviceState = too_few_packets;
+			}
+		}
+		break;
+	case Download_Req:
+#ifdef DEBUG_SSP
+		sprintf(str,"COMMAND:DOWNLOAD_REQ 1 Status=%d|",DeviceState);
+		PIOS_COM_SendString(PIOS_COM_TELEM_USB,str);
+#endif
+		if (DeviceState == DFUidle) {
+#ifdef DEBUG_SSP
+			PIOS_COM_SendString(PIOS_COM_TELEM_USB,"COMMAND:DOWNLOAD_REQ 1|");
+#endif
+			downType = Data0;
+			downPacketTotal = Count;
+			downSizeOfLastPacket = Data1;
+			if (isBiggerThanAvailable(downType, (downPacketTotal - 1) * 14 * 4
+					+ downSizeOfLastPacket * 4) == 1) {
+				DeviceState = outsideDevCapabilities;
+				Aditionals = (uint32_t) Command;
+
+			} else {
+				downPacketCurrent = 0;
+				DeviceState = downloading;
+			}
+		} else {
+			DeviceState = Last_operation_failed;
+			Aditionals = (uint32_t) Command;
+		}
+		break;
+
+	case Status_Request:
+		Buffer[0] = 0x01;
+		Buffer[1] = Status_Rep;
+		if (DeviceState == wrong_packet_received) {
+			Buffer[2] = Aditionals >> 24;
+			Buffer[3] = Aditionals >> 16;
+			Buffer[4] = Aditionals >> 8;
+			Buffer[5] = Aditionals;
+		} else {
+			Buffer[2] = 0;
+			Buffer[3] = ((uint16_t) Aditionals) >> 8;
+			Buffer[4] = ((uint16_t) Aditionals);
+			Buffer[5] = 0;
+		}
+		Buffer[6] = DeviceState;
+		Buffer[7] = 0;
+		Buffer[8] = 0;
+		Buffer[9] = 0;
+		sendData(Buffer + 1, 63);
+		if (DeviceState == Last_operation_Success) {
+			DeviceState = DFUidle;
+		}
+		break;
+	case Status_Rep:
+
+		break;
+
+	}
+	if (EchoReqFlag == 1) {
+		echoBuffer[0] = echoBuffer[0] | (1 << 6);
+		sendData(echoBuffer, 63);
+	}
+	return;
+}
+void OPDfuIni(uint8_t discover) {
+	const struct pios_board_info * bdinfo = &pios_board_info_blob;
+	Device dev;
+
+	dev.programmingType = Self_flash;
+	dev.readWriteFlags = (BOARD_READABLE | (BOARD_WRITABLE << 1));
+	dev.startOfUserCode = bdinfo->fw_base;
+	dev.sizeOfCode = bdinfo->fw_size;
+	dev.sizeOfDescription = bdinfo->desc_size;
+	dev.BL_Version = bdinfo->bl_rev;
+	dev.FW_Crc = CalcFirmCRC();
+	dev.devID = (bdinfo->board_type << 8) | (bdinfo->board_rev);
+	dev.devType = bdinfo->hw_type;
+	numberOfDevices = 1;
+	devicesTable[0] = dev;
+	if (discover) {
+		//TODO check other devices trough spi or whatever
+	}
+}
+uint32_t baseOfAdressType(DFUTransfer type) {
+	switch (type) {
+	case FW:
+		return currentDevice.startOfUserCode;
+		break;
+	case Descript:
+		return currentDevice.startOfUserCode + currentDevice.sizeOfCode;
+		break;
+	default:
+
+		return 0;
+	}
+}
+uint8_t isBiggerThanAvailable(DFUTransfer type, uint32_t size) {
+	switch (type) {
+	case FW:
+		return (size > currentDevice.sizeOfCode) ? 1 : 0;
+		break;
+	case Descript:
+		return (size > currentDevice.sizeOfDescription) ? 1 : 0;
+		break;
+	default:
+		return true;
+	}
+}
+
+uint32_t CalcFirmCRC() {
+	switch (currentProgrammingDestination) {
+	case Self_flash:
+		return PIOS_BL_HELPER_CRC_Memory_Calc();
+		break;
+	case Remote_flash_via_spi:
+		return 0;
+		break;
+	default:
+		return 0;
+		break;
+	}
+
+}
+void sendData(uint8_t * buf, uint16_t size) {
+	PIOS_COM_MSG_Send(PIOS_COM_TELEM_USB, buf, size);
+}
+
+bool flash_read(uint8_t * buffer, uint32_t adr, DFUProgType type) {
+	switch (type) {
+	case Remote_flash_via_spi:
+		return false; // We should not get this for the PipX
+		break;
+	case Self_flash:
+		for (uint8_t x = 0; x < 4; ++x) {
+			buffer[x] = *PIOS_BL_HELPER_FLASH_If_Read(adr + x);
+		}
+		return true;
+		break;
+	default:
+		return false;
+	}
+}

flight/Bootloaders/RevoMini/pios_board.c

@@ -0,0 +1,82 @@
+/**
+ ******************************************************************************
+ *
+ * @file       pios_board.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Defines board specific static initializers for hardware for the AHRS board.
+ * @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
+ */
+
+/* Pull in the board-specific static HW definitions.
+ * Including .c files is a bit ugly but this allows all of
+ * the HW definitions to be const and static to limit their
+ * scope.  
+ *
+ * NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE
+ */
+#include "board_hw_defs.c"
+
+#include <pios_board_info.h>
+#include <pios.h>
+
+uint32_t pios_com_telem_usb_id;
+
+static bool board_init_complete = false;
+void PIOS_Board_Init() {
+	if (board_init_complete) {
+		return;
+	}
+
+	/* Delay system */
+	PIOS_DELAY_Init();
+	
+	const struct pios_board_info * bdinfo = &pios_board_info_blob;
+	
+#if defined(PIOS_INCLUDE_LED)
+	const struct pios_led_cfg * led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev);
+	PIOS_Assert(led_cfg);
+	PIOS_LED_Init(led_cfg);
+#endif	/* PIOS_INCLUDE_LED */
+
+#if defined(PIOS_INCLUDE_USB)
+	/* Initialize board specific USB data */
+	PIOS_USB_BOARD_DATA_Init();
+
+	/* Activate the HID-only USB configuration */
+	PIOS_USB_DESC_HID_ONLY_Init();
+
+	uint32_t pios_usb_id;
+	PIOS_USB_Init(&pios_usb_id, PIOS_BOARD_HW_DEFS_GetUsbCfg(bdinfo->board_rev));
+
+#if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_COM_MSG)
+	uint32_t pios_usb_hid_id;
+	if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) {
+		PIOS_Assert(0);
+	}
+	if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) {
+		PIOS_Assert(0);
+	}
+#endif	/* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */
+
+	PIOS_USBHOOK_Activate();
+
+#endif	/* PIOS_INCLUDE_USB */
+
+	board_init_complete = true;
+}

flight/Bootloaders/RevoMini/pios_usb_board_data.c

@@ -0,0 +1,98 @@
+/**
+ ******************************************************************************
+ * @addtogroup PIOS PIOS Core hardware abstraction layer
+ * @{
+ * @addtogroup PIOS_USB_BOARD Board specific USB definitions
+ * @brief Board specific USB definitions
+ * @{
+ *
+ * @file       pios_usb_board_data.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Board specific USB definitions
+ * @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_usb_board_data.h" /* struct usb_*, USB_* */
+#include "pios_sys.h"		 /* PIOS_SYS_SerialNumberGet */
+#include "pios_usbhook.h"	 /* PIOS_USBHOOK_* */
+#include "pios_usb_util.h"	 /* PIOS_USB_UTIL_AsciiToUtf8 */
+
+static const uint8_t usb_product_id[22] = {
+	sizeof(usb_product_id),
+	USB_DESC_TYPE_STRING,
+	'R', 0,
+	'e', 0,
+	'v', 0,
+	'o', 0,
+	'l', 0,
+	'u', 0,
+	't', 0,
+	'i', 0,
+	'o', 0,
+	'n', 0,
+};
+
+static uint8_t usb_serial_number[2 + PIOS_SYS_SERIAL_NUM_ASCII_LEN*2 + (sizeof(PIOS_USB_BOARD_SN_SUFFIX)-1)*2] = {
+	sizeof(usb_serial_number),
+	USB_DESC_TYPE_STRING,
+};
+
+static const struct usb_string_langid usb_lang_id = {
+	.bLength = sizeof(usb_lang_id),
+	.bDescriptorType = USB_DESC_TYPE_STRING,
+	.bLangID = htousbs(USB_LANGID_ENGLISH_US),
+};
+
+static const uint8_t usb_vendor_id[28] = {
+	sizeof(usb_vendor_id),
+	USB_DESC_TYPE_STRING,
+	'o', 0,
+	'p', 0,
+	'e', 0,
+	'n', 0,
+	'p', 0,
+	'i', 0,
+	'l', 0,
+	'o', 0,
+	't', 0,
+	'.', 0,
+	'o', 0,
+	'r', 0,
+	'g', 0
+};
+
+int32_t PIOS_USB_BOARD_DATA_Init(void)
+{
+	/* Load device serial number into serial number string */
+	uint8_t sn[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1];
+	PIOS_SYS_SerialNumberGet((char *)sn);
+
+	/* Concatenate the device serial number and the appropriate suffix ("+BL" or "+FW") into the USB serial number */
+	uint8_t * utf8 = &(usb_serial_number[2]);
+	utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, sn, PIOS_SYS_SERIAL_NUM_ASCII_LEN);
+	utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, (uint8_t *)PIOS_USB_BOARD_SN_SUFFIX, sizeof(PIOS_USB_BOARD_SN_SUFFIX)-1);
+
+	PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id));
+	PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number));
+
+	PIOS_USBHOOK_RegisterString(USB_STRING_DESC_LANG, (uint8_t *)&usb_lang_id, sizeof(usb_lang_id));
+	PIOS_USBHOOK_RegisterString(USB_STRING_DESC_VENDOR, (uint8_t *)&usb_vendor_id, sizeof(usb_vendor_id));
+
+	return 0;
+}

flight/Bootloaders/Revolution/main.c

@@ -37,6 +37,7 @@
 /* Prototype of PIOS_Board_Init() function */
 extern void PIOS_Board_Init(void);
 extern void FLASH_Download();
+void check_bor();
 #define BSL_HOLD_STATE ((PIOS_USB_DETECT_GPIO_PORT->IDR & PIOS_USB_DETECT_GPIO_PIN) ? 0 : 1)
 
 /* Private typedef -----------------------------------------------------------*/
@@ -75,6 +76,10 @@ int main() {
 	PIOS_Board_Init();
 	PIOS_IAP_Init();
 
+	// Make sure the brown out reset value for this chip
+	// is 2.7 volts
+	check_bor();
+
 	USB_connected = PIOS_USB_CheckAvailable(0);
 
 	if (PIOS_IAP_CheckRequest() == true) {
@@ -86,7 +91,6 @@ int main() {
 	GO_dfu = (USB_connected == true) || (User_DFU_request == true);
 
 	if (GO_dfu == true) {
-		PIOS_Board_Init();
 		if (User_DFU_request == true)
 			DeviceState = DFUidle;
 		else
@@ -206,3 +210,21 @@ uint8_t processRX() {
 	return true;
 }
 
+/**
+ * Check the brown out reset threshold is 2.7 volts and if not
+ * resets it.  This solves an issue that can prevent boards
+ * powering up with some BEC
+ */
+void check_bor()
+{
+	uint8_t bor = FLASH_OB_GetBOR();
+	if(bor != OB_BOR_LEVEL3) {
+		FLASH_OB_Unlock();
+		FLASH_OB_BORConfig(OB_BOR_LEVEL3);
+		FLASH_OB_Launch();
+		while(FLASH_WaitForLastOperation() == FLASH_BUSY);
+		FLASH_OB_Lock();
+		while(FLASH_WaitForLastOperation() == FLASH_BUSY);
+	}
+}
+

flight/CopterControl/Makefile

@@ -139,9 +139,10 @@ STMUSBINCDIR = $(STMUSBDIR)/inc
 CMSISDIR  = $(STMLIBDIR)/CMSIS/Core/CM3
 DOSFSDIR  = $(APPLIBDIR)/dosfs
 MSDDIR  = $(APPLIBDIR)/msd
-RTOSDIR = $(APPLIBDIR)/FreeRTOS
+RTOSDIR = $(PIOSCOMMON)/Libraries/FreeRTOS
 RTOSSRCDIR = $(RTOSDIR)/Source
 RTOSINCDIR = $(RTOSSRCDIR)/include
+RTOSPORTDIR = $(APPLIBDIR)/FreeRTOS/Source
 DOXYGENDIR = ../Doc/Doxygen
 AHRSBOOTLOADER = ../Bootloaders/AHRS/
 AHRSBOOTLOADERINC = $(AHRSBOOTLOADER)/inc
@@ -320,8 +321,8 @@ SRC += $(RTOSSRCDIR)/queue.c
 SRC += $(RTOSSRCDIR)/tasks.c
 
 ## RTOS Portable
-SRC += $(RTOSSRCDIR)/portable/GCC/ARM_CM3/port.c
-SRC += $(RTOSSRCDIR)/portable/MemMang/heap_1.c
+SRC += $(RTOSPORTDIR)/portable/GCC/ARM_CM3/port.c
+SRC += $(RTOSPORTDIR)/portable/MemMang/heap_1.c
 
 ## Dosfs file system
 #SRC += $(DOSFSDIR)/dosfs.c
@@ -392,7 +393,7 @@ EXTRAINCDIRS  += $(DOSFSDIR)
 EXTRAINCDIRS  += $(MSDDIR)
 EXTRAINCDIRS  += $(RTOSINCDIR)
 EXTRAINCDIRS  += $(APPLIBDIR)
-EXTRAINCDIRS  += $(RTOSSRCDIR)/portable/GCC/ARM_CM3
+EXTRAINCDIRS  += $(RTOSPORTDIR)/portable/GCC/ARM_CM3
 EXTRAINCDIRS  += $(AHRSBOOTLOADERINC)
 EXTRAINCDIRS  += $(PYMITEINC)
 EXTRAINCDIRS  += $(HWDEFSINC)

flight/CopterControl/System/pios_board.c

@@ -119,7 +119,8 @@ static const struct pios_mpu6000_cfg pios_mpu6000_cfg = {
 	.Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK,
 	.accel_range = PIOS_MPU6000_ACCEL_8G,
 	.gyro_range = PIOS_MPU6000_SCALE_500_DEG,
-	.filter = PIOS_MPU6000_LOWPASS_256_HZ
+	.filter = PIOS_MPU6000_LOWPASS_256_HZ,
+	.orientation = PIOS_MPU6000_TOP_180DEG
 };
 #endif /* PIOS_INCLUDE_MPU6000 */
 

flight/Libraries/aes.c

@@ -1,7 +1,7 @@
 
 // http://gladman.plushost.co.uk/oldsite/AES/index.php
 
-//#include <stdio.h>
+#include <stdint.h>
 
 #include "aes.h"
 

flight/Libraries/inc/aes.h

@@ -2,8 +2,6 @@
 #ifndef _AES_H_
 #define _AES_H_
 
-#include "stm32f10x.h"
-
 #define N_ROW			4
 #define N_COL			4
 #define N_BLOCK			(N_ROW * N_COL)

flight/Libraries/inc/packet_handler.h

@@ -30,6 +30,9 @@
 #ifndef __PACKET_HANDLER_H__
 #define __PACKET_HANDLER_H__
 
+#include <uavobjectmanager.h>
+#include <gcsreceiver.h>
+
 // Public defines / macros
 #define PHPacketSize(p) ((uint8_t*)(p->data) + p->header.data_size - (uint8_t*)p)
 #define PHPacketSizeECC(p) ((uint8_t*)(p->data) + p->header.data_size  + RS_ECC_NPARITY - (uint8_t*)p)
@@ -57,8 +60,7 @@ typedef struct {
 	uint32_t source_id;
 	uint8_t type;
 	uint8_t data_size;
-	uint8_t tx_seq;
-	uint8_t rx_seq;
+	uint16_t seq_num;
 } PHPacketHeader;
 
 #define PH_MAX_DATA (PIOS_PH_MAX_PACKET - sizeof(PHPacketHeader) - RS_ECC_NPARITY)
@@ -71,7 +73,7 @@ typedef struct {
 #define PH_PPM_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data))
 typedef struct {
 	PHPacketHeader header;
-	uint16_t channels[PIOS_RCVR_MAX_CHANNELS];
+	uint16_t channels[GCSRECEIVER_CHANNEL_NUMELEM];
 	uint8_t ecc[RS_ECC_NPARITY];
 } PHPpmPacket, *PHPpmPacketHandle;
 
@@ -87,8 +89,10 @@ typedef struct {
 } PHStatusPacket, *PHStatusPacketHandle;
 
 typedef struct {
-	uint8_t winSize;
-	uint16_t maxConnections;
+	uint32_t default_destination_id;
+	uint32_t source_id;
+	uint16_t max_connections;
+	uint8_t win_size;
 } PacketHandlerConfig;
 
 typedef int32_t (*PHOutputStream)(PHPacketHandle packet);
@@ -110,8 +114,8 @@ void PHReleaseRXPacket(PHInstHandle h, PHPacketHandle p);
 PHPacketHandle PHGetTXPacket(PHInstHandle h);
 void PHReleaseTXPacket(PHInstHandle h, PHPacketHandle p);
 uint8_t PHTransmitPacket(PHInstHandle h, PHPacketHandle p);
-int32_t PHVerifyPacket(PHInstHandle h, PHPacketHandle p, uint16_t received_len);
-uint8_t PHReceivePacket(PHInstHandle h, PHPacketHandle p, bool rx_error);
+uint8_t PHTransmitData(PHInstHandle h, uint8_t *buf, uint16_t len);
+uint8_t PHReceivePacket(PHInstHandle h, PHPacketHandle p);
 
 #endif // __PACKET_HANDLER_H__
 

flight/Libraries/insgps13state.c

@@ -359,6 +359,9 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
 	Nav.q[1] = X[7];
 	Nav.q[2] = X[8];
 	Nav.q[3] = X[9];
+	Nav.gyro_bias[0] = X[10];
+	Nav.gyro_bias[1] = X[11];
+	Nav.gyro_bias[2] = X[12];
 }
 
 //  *************  CovariancePrediction *************

flight/Libraries/packet_handler.c

@@ -43,8 +43,6 @@ typedef struct {
 	PHPacket *rx_packets;
 	uint8_t rx_win_start;
 	uint8_t rx_win_end;
-	uint16_t tx_seq_id;
-	PHOutputStream stream;
 	xSemaphoreHandle lock;
 	PHOutputStream output_stream;
 	PHDataHandler data_handler;
@@ -53,8 +51,6 @@ typedef struct {
 } PHPacketData, *PHPacketDataHandle;
 
 // Private functions
-static uint8_t PHLSendAck(PHPacketDataHandle data, PHPacketHandle p);
-static uint8_t PHLSendNAck(PHPacketDataHandle data, PHPacketHandle p);
 static uint8_t PHLTransmitPacket(PHPacketDataHandle data, PHPacketHandle p);
 
 /**
@@ -72,16 +68,15 @@ PHInstHandle PHInitialize(PacketHandlerConfig *cfg)
 	if (!data)
 		return 0;
 	data->cfg = *cfg;
-	data->tx_seq_id = 0;
 
 	// Allocate the packet windows
-	data->tx_packets = pvPortMalloc(sizeof(PHPacket) * data->cfg.winSize);
-	data->rx_packets = pvPortMalloc(sizeof(PHPacket) * data->cfg.winSize);
+	data->tx_packets = pvPortMalloc(sizeof(PHPacket) * data->cfg.win_size);
+	data->rx_packets = pvPortMalloc(sizeof(PHPacket) * data->cfg.win_size);
 
 	// Initialize the windows
 	data->tx_win_start = data->tx_win_end = 0;
 	data->rx_win_start = data->rx_win_end = 0;
-	for (uint8_t i = 0; i < data->cfg.winSize; ++i)
+	for (uint8_t i = 0; i < data->cfg.win_size; ++i)
 	{
 		data->tx_packets[i].header.type = PACKET_TYPE_NONE;
 		data->rx_packets[i].header.type = PACKET_TYPE_NONE;
@@ -93,6 +88,12 @@ PHInstHandle PHInitialize(PacketHandlerConfig *cfg)
 	// Initialize the ECC library.
 	initialize_ecc();
 
+	// Initialize the handlers
+	data->output_stream = 0;
+	data->data_handler = 0;
+	data->status_handler = 0;
+	data->ppm_handler = 0;
+
 	// Return the structure.
 	return (PHInstHandle)data;
 }
@@ -172,7 +173,7 @@ PHPacketHandle PHGetTXPacket(PHInstHandle h)
 
 	// Find a free packet.
 	PHPacketHandle p = NULL;
-	for (uint8_t i = 0; i < data->cfg.winSize; ++i)
+	for (uint8_t i = 0; i < data->cfg.win_size; ++i)
 		if (data->tx_packets[i].header.type == PACKET_TYPE_NONE)
 		{
 			p = data->tx_packets + i;
@@ -205,7 +206,7 @@ void PHReleaseTXPacket(PHInstHandle h, PHPacketHandle p)
 	// If this packet is at the start of the window, increment the start index.
 	while ((data->tx_win_start != data->tx_win_end) &&
 	       (data->tx_packets[data->tx_win_start].header.type == PACKET_TYPE_NONE))
-		data->tx_win_start = (data->tx_win_start + 1) % data->cfg.winSize;
+		data->tx_win_start = (data->tx_win_start + 1) % data->cfg.win_size;
 
 	// Release lock
 	xSemaphoreGiveRecursive(data->lock);
@@ -226,7 +227,7 @@ PHPacketHandle PHGetRXPacket(PHInstHandle h)
 
 	// Find a free packet.
 	PHPacketHandle p = NULL;
-	for (uint8_t i = 0; i < data->cfg.winSize; ++i)
+	for (uint8_t i = 0; i < data->cfg.win_size; ++i)
 		if (data->rx_packets[i].header.type == PACKET_TYPE_NONE)
 		{
 			p = data->rx_packets + i;
@@ -259,7 +260,7 @@ void PHReleaseRXPacket(PHInstHandle h, PHPacketHandle p)
 	// If this packet is at the start of the window, increment the start index.
 	while ((data->rx_win_start != data->rx_win_end) &&
 				 (data->rx_packets[data->rx_win_start].header.type == PACKET_TYPE_NONE))
-		data->rx_win_start = (data->rx_win_start + 1) % data->cfg.winSize;
+		data->rx_win_start = (data->rx_win_start + 1) % data->cfg.win_size;
 
 	// Release lock
 	xSemaphoreGiveRecursive(data->lock);
@@ -280,51 +281,37 @@ uint8_t PHTransmitPacket(PHInstHandle h, PHPacketHandle p)
 	if (!PHLTransmitPacket(data, p))
 		return 0;
 
-	// If this packet doesn't require an ACK, remove it from the TX window.
-	switch (p->header.type) {
-	case PACKET_TYPE_READY:
-	case PACKET_TYPE_NOTREADY:
-	case PACKET_TYPE_DATA:
-	case PACKET_TYPE_PPM:
-		PHReleaseTXPacket(h, p);
-		break;
-	}
-
 	return 1;
 }
 
 /**
- * Verify that a buffer contains a valid packet.
+ * Transmit a packet of data.
  * \param[in] h The packet handler instance data pointer.
- * \param[in] p A pointer to the packet buffer.
- * \param[in] received_len The length of data received.
- * \return < 0 Failure
- * \return > 0 Number of bytes consumed.
+ * \param[in] p A pointer to the data buffer.
+ * \param[in] len The length of the data buffer.
+ * \return 1 Success
+ * \return 0 Failure
  */
-int32_t PHVerifyPacket(PHInstHandle h, PHPacketHandle p, uint16_t received_len)
+uint8_t PHTransmitData(PHInstHandle h, uint8_t *buf, uint16_t len)
 {
+	PHPacketDataHandle data = (PHPacketDataHandle)h;
 
-	// Verify the packet length.
-	// Note: The last two bytes should be the RSSI and AFC.
-	uint16_t len = PHPacketSizeECC(p);
-	if (received_len < (len + 2))
-	{
-		DEBUG_PRINTF(1, "Packet length error %d %d\n\r", received_len, len + 2);
-		return -1;
-	}
+	// Get a packet from the packet handler.
+	PHPacketHandle p = PHGetTXPacket(pios_packet_handler);
+	if (!p)
+		return 0;
 
-	// Attempt to correct any errors in the packet.
-	decode_data((unsigned char*)p, len);
+	// Initialize the packet.
+	p->header.destination_id = data->cfg.default_destination_id;
+	p->header.source_id = data->cfg.source_id;
+	p->header.type = PACKET_TYPE_DATA;
+	p->header.data_size = len;
 
-	// Check that there were no unfixed errors.
-	bool rx_error = check_syndrome() != 0;
-	if(rx_error)
-	{
-		DEBUG_PRINTF(1, "Error in packet\n\r");
-		return -2;
-	}
+	// Copy the data into the packet.
+	memcpy(p->data, buf, len);
 
-	return len + 2;
+	// Send the packet.
+	return PHLTransmitPacket(data, p);
 }
 
 /**
@@ -335,7 +322,7 @@ int32_t PHVerifyPacket(PHInstHandle h, PHPacketHandle p, uint16_t received_len)
  * \return 0 Failure
  * \return 1 Success
  */
-uint8_t PHReceivePacket(PHInstHandle h, PHPacketHandle p, bool rx_error)
+uint8_t PHReceivePacket(PHInstHandle h, PHPacketHandle p)
 {
 	PHPacketDataHandle data = (PHPacketDataHandle)h;
 	uint16_t len = PHPacketSizeECC(p);
@@ -348,81 +335,23 @@ uint8_t PHReceivePacket(PHInstHandle h, PHPacketHandle p, bool rx_error)
 
 	case PACKET_TYPE_STATUS:
 
-		if (!rx_error)
-
-			// Pass on the channels to the status handler.
-			if(data->status_handler)
-				data->status_handler((PHStatusPacketHandle)p, rssi, afc);
-
+		// Pass on the channels to the status handler.
+		if(data->status_handler)
+			data->status_handler((PHStatusPacketHandle)p, rssi, afc);
 		break;
 
-	case PACKET_TYPE_ACKED_DATA:
-
-		// Send the ACK / NACK
-		if (rx_error)
-		{
-			DEBUG_PRINTF(1, "Sending NACK\n\r");
-			PHLSendNAck(data, p);
-		}
-		else
-		{
-
-			PHLSendAck(data, p);
-
-			// Pass on the data.
-			if(data->data_handler)
-				data->data_handler(p->data, p->header.data_size, rssi, afc);
-		}
-
-		break;
-
-	case PACKET_TYPE_ACK:
-	{
-		// Find the packet ID in the TX buffer, and free it.
-		unsigned int i = 0;
-		for (unsigned int i = 0; i < data->cfg.winSize; ++i)
-			if (data->tx_packets[i].header.tx_seq == p->header.rx_seq)
-				PHReleaseTXPacket(h, data->tx_packets + i);
-#ifdef DEBUG_LEVEL
-		if (i == data->cfg.winSize)
-			DEBUG_PRINTF(1, "Error finding acked packet to release\n\r");
-#endif
-	}
-	break;
-
-	case PACKET_TYPE_NACK:
-	{
-		// Resend the packet.
-		unsigned int i = 0;
-		for ( ; i < data->cfg.winSize; ++i)
-			if (data->tx_packets[i].header.tx_seq == p->header.rx_seq)
-				PHLTransmitPacket(data, data->tx_packets + i);
-#ifdef DEBUG_LEVEL
-		if (i == data->cfg.winSize)
-			DEBUG_PRINTF(1, "Error finding acked packet to NACK\n\r");
-		DEBUG_PRINTF(1, "Resending after NACK\n\r");
-#endif
-	}
-	break;
-
 	case PACKET_TYPE_PPM:
 
-		if (!rx_error)
-
-			// Pass on the channels to the PPM handler.
-			if(data->ppm_handler)
-				data->ppm_handler(((PHPpmPacketHandle)p)->channels);
-
+		// Pass on the channels to the PPM handler.
+		if(data->ppm_handler)
+			data->ppm_handler(((PHPpmPacketHandle)p)->channels);
 		break;
 
 	case PACKET_TYPE_DATA:
 
-		if (!rx_error)
-
-			// Pass on the data to the data handler.
-			if(data->data_handler)
-				data->data_handler(p->data, p->header.data_size, rssi, afc);
-
+		// Pass on the data to the data handler.
+		if(data->data_handler)
+			data->data_handler(p->data, p->header.data_size, rssi, afc);
 		break;
 
 	default:
@@ -448,57 +377,9 @@ static uint8_t PHLTransmitPacket(PHPacketDataHandle data, PHPacketHandle p)
 	if(!data->output_stream)
 		return 0;
 
-	// Set the sequence ID to the current ID.
-	p->header.tx_seq = data->tx_seq_id++;
-
-	// Add the error correcting code.
-	encode_data((unsigned char*)p, PHPacketSize(p), (unsigned char*)p);
-
 	// Transmit the packet using the output stream.
 	if(data->output_stream(p) == -1)
 		return 0;
 
 	return 1;
 }
-
-/**
- * Send an ACK packet.
- * \param[in] data The packet handler instance data pointer.
- * \param[in] p A pointer to the packet buffer of the packet to be ACKed.
- * \return 1 Success
- * \return 0 Failure
- */
-static uint8_t PHLSendAck(PHPacketDataHandle data, PHPacketHandle p)
-{
-
-	// Create the ACK message
-	PHPacketHeader ack;
-	ack.destination_id = p->header.source_id;
-	ack.type = PACKET_TYPE_ACK;
-	ack.rx_seq = p->header.tx_seq;
-	ack.data_size = 0;
-
-	// Send the packet.
-	return PHLTransmitPacket(data, (PHPacketHandle)&ack);
-}
-
-/**
- * Send an NAck packet.
- * \param[in] data The packet handler instance data pointer.
- * \param[in] p A pointer to the packet buffer of the packet to be ACKed.
- * \return 1 Success
- * \return 0 Failure
- */
-static uint8_t PHLSendNAck(PHPacketDataHandle data, PHPacketHandle p)
-{
-
-	// Create the NAck message
-	PHPacketHeader ack;
-	ack.destination_id = p->header.source_id;
-	ack.type = PACKET_TYPE_NACK;
-	ack.rx_seq = p->header.tx_seq;
-	ack.data_size = 0;
-
-	// Set the packet.
-	return PHLTransmitPacket(data, (PHPacketHandle)&ack);
-}

flight/Libraries/rscode/rs.c

@@ -159,7 +159,7 @@ debug_check_syndrome (void)
 static void
 compute_genpoly (int nbytes, int genpoly[])
 {
-  int i, tp[256], tp1[256];
+  int i, tp[MAXDEG], tp1[MAXDEG];
 	
   /* multiply (x + a^n) for n = 1 to nbytes */
 

flight/Modules/Actuator/actuator.c

@@ -565,28 +565,82 @@ static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSet
 static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSettingsData * actuatorSettings)
 {
 	switch(actuatorSettings->ChannelType[mixer_channel]) {
-		case ACTUATORSETTINGS_CHANNELTYPE_PWMALARMBUZZER: {
+		case ACTUATORSETTINGS_CHANNELTYPE_PWMALARMBUZZER: 
+                case ACTUATORSETTINGS_CHANNELTYPE_ARMINGLED:
+                case ACTUATORSETTINGS_CHANNELTYPE_INFOLED:
+                {
 			// This is for buzzers that take a PWM input
 
 			static uint32_t currBuzzTune = 0;
 			static uint32_t currBuzzTuneState;
-			uint32_t bewBuzzTune;
 
-			// Decide what tune to play
-			if (AlarmsGet(SYSTEMALARMS_ALARM_BATTERY) > SYSTEMALARMS_ALARM_WARNING) {
-				bewBuzzTune = 0b11110110110000;	// pause, short, short, short, long
-			} else if (AlarmsGet(SYSTEMALARMS_ALARM_GPS) >= SYSTEMALARMS_ALARM_WARNING) {
-				bewBuzzTune = 0x80000000;			// pause, short
-			} else {
-				bewBuzzTune = 0;
-			}
+                        static uint32_t currArmingTune = 0;
+			static uint32_t currArmingTuneState;
 
-			// Do we need to change tune?
-			if (bewBuzzTune != currBuzzTune) {
-				currBuzzTune = bewBuzzTune;
-				currBuzzTuneState = currBuzzTune;
-			}
+                        static uint32_t currInfoTune = 0;
+			static uint32_t currInfoTuneState;
 
+                        uint32_t newTune = 0;
+                        if(actuatorSettings->ChannelType[mixer_channel] == ACTUATORSETTINGS_CHANNELTYPE_PWMALARMBUZZER)
+                        {
+                            
+                            // Decide what tune to play
+                            if (AlarmsGet(SYSTEMALARMS_ALARM_BATTERY) > SYSTEMALARMS_ALARM_WARNING) {
+                                    newTune = 0b11110110110000;	// pause, short, short, short, long
+                            } else if (AlarmsGet(SYSTEMALARMS_ALARM_GPS) >= SYSTEMALARMS_ALARM_WARNING) {
+                                    newTune = 0x80000000;			// pause, short
+                            } else {
+                                    newTune = 0;
+                            }
+
+                            // Do we need to change tune?
+                            if (newTune != currBuzzTune) {
+                                    currBuzzTune = newTune;
+                                    currBuzzTuneState = currBuzzTune;
+                            }
+                        }
+                        else // ACTUATORSETTINGS_CHANNELTYPE_ARMINGLED || ACTUATORSETTINGS_CHANNELTYPE_INFOLED
+                        {
+                            uint8_t arming;
+                            FlightStatusArmedGet(&arming);
+                            //base idle tune  
+                            newTune =  0x80000000;      // 0b1000...
+                            
+                            // Merge the error pattern for InfoLed
+                            if(actuatorSettings->ChannelType[mixer_channel] == ACTUATORSETTINGS_CHANNELTYPE_INFOLED)
+                            {
+                                if (AlarmsGet(SYSTEMALARMS_ALARM_BATTERY) > SYSTEMALARMS_ALARM_WARNING) 
+                                {
+                                    newTune |= 0b00000000001111111011111110000000;
+                                }
+                                else if(AlarmsGet(SYSTEMALARMS_ALARM_GPS) >= SYSTEMALARMS_ALARM_WARNING) 
+                                {             
+                                    newTune |= 0b00000000000000110110110000000000;
+                                }
+                            }
+                            // fast double blink pattern if armed 
+                            if (arming == FLIGHTSTATUS_ARMED_ARMED) 
+                               newTune |= 0xA0000000;   // 0b101000... 
+
+                            // Do we need to change tune?
+                            if(actuatorSettings->ChannelType[mixer_channel] == ACTUATORSETTINGS_CHANNELTYPE_ARMINGLED)
+                            {
+                                if (newTune != currArmingTune) {
+                                    currArmingTune = newTune;
+                                    // note: those are both updated so that Info and Arming are in sync if used simultaneously
+                                    currArmingTuneState = currArmingTune;
+                                    currInfoTuneState = currInfoTune;
+                                }
+                            }
+                            else
+                            {
+                                if (newTune != currInfoTune) {
+                                    currInfoTune = newTune;
+                                    currArmingTuneState = currArmingTune;
+                                    currInfoTuneState = currInfoTune;
+                                }
+                            }
+                        }
 
 			// Play tune
 			bool buzzOn = false;
@@ -595,17 +649,30 @@ static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSet
 			portTickType dT = 0;
 
 			// For now, only look at the battery alarm, because functions like AlarmsHasCritical() can block for some time; to be discussed
-			if (currBuzzTune) {
-				if(thisSysTime > lastSysTime)
-					dT = thisSysTime - lastSysTime;
-				buzzOn = (currBuzzTuneState&1);	// Buzz when the LS bit is 1
-				if (dT > 80) {
-					// Go to next bit in alarm_seq_state
-					currBuzzTuneState >>= 1;
-					if (currBuzzTuneState == 0)
-						currBuzzTuneState = currBuzzTune;	// All done, re-start the tune
-					lastSysTime = thisSysTime;
-				}
+			if (currBuzzTune||currArmingTune||currInfoTune) {
+                            if(thisSysTime > lastSysTime)
+                                dT = thisSysTime - lastSysTime;
+                            if(actuatorSettings->ChannelType[mixer_channel] == ACTUATORSETTINGS_CHANNELTYPE_PWMALARMBUZZER)
+                                buzzOn = (currBuzzTuneState&1);	// Buzz when the LS bit is 1
+                            else if(actuatorSettings->ChannelType[mixer_channel] == ACTUATORSETTINGS_CHANNELTYPE_ARMINGLED)
+                                buzzOn = (currArmingTuneState&1);	
+                            else if(actuatorSettings->ChannelType[mixer_channel] == ACTUATORSETTINGS_CHANNELTYPE_INFOLED)
+                                buzzOn = (currInfoTuneState&1);
+
+                            if (dT > 80) {
+                                    // Go to next bit in alarm_seq_state
+                                    currArmingTuneState >>=1;
+                                    currInfoTuneState >>= 1;
+                                    currBuzzTuneState >>= 1;
+
+                                    if (currBuzzTuneState == 0)
+                                            currBuzzTuneState = currBuzzTune;	// All done, re-start the tune
+                                    if (currArmingTuneState == 0)
+                                            currArmingTuneState = currArmingTune;
+                                    if (currInfoTuneState == 0)
+                                            currInfoTuneState = currInfoTune;	
+                                    lastSysTime = thisSysTime;
+                            }
 			}
 			PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel],
 							buzzOn?actuatorSettings->ChannelMax[mixer_channel]:actuatorSettings->ChannelMin[mixer_channel]);

flight/Modules/Altitude/revolution/altitude.c

@@ -100,6 +100,13 @@ static void altitudeTask(void *parameters)
 
 	// 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
+ 
+#ifdef PIOS_MS5611_SLOW_TEMP_RATE
+	uint8_t temp_press_interleave_count = 1;
+#endif
 	// Main task loop
 	while (1)
 	{
@@ -128,11 +135,20 @@ static void altitudeTask(void *parameters)
 		}
 #endif
 		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);

flight/Modules/Attitude/attitude.c

@@ -379,13 +379,13 @@ static int32_t updateSensorsCC3D(AccelsData * accelsData, GyrosData * gyrosData)
 	if (GyrosReadOnly() || AccelsReadOnly())
 		return 0;
 
-	gyros[0] = -mpu6000_data.gyro_y * PIOS_MPU6000_GetScale();
-	gyros[1] = -mpu6000_data.gyro_x * PIOS_MPU6000_GetScale();
-	gyros[2] = -mpu6000_data.gyro_z * PIOS_MPU6000_GetScale();
+	gyros[0] = mpu6000_data.gyro_x * PIOS_MPU6000_GetScale();
+	gyros[1] = mpu6000_data.gyro_y * PIOS_MPU6000_GetScale();
+	gyros[2] = mpu6000_data.gyro_z * PIOS_MPU6000_GetScale();
 	
-	accels[0] = -mpu6000_data.accel_y * PIOS_MPU6000_GetAccelScale();
-	accels[1] = -mpu6000_data.accel_x * PIOS_MPU6000_GetAccelScale();
-	accels[2] = -mpu6000_data.accel_z * PIOS_MPU6000_GetAccelScale();
+	accels[0] = mpu6000_data.accel_x * PIOS_MPU6000_GetAccelScale();
+	accels[1] = mpu6000_data.accel_y * PIOS_MPU6000_GetAccelScale();
+	accels[2] = mpu6000_data.accel_z * PIOS_MPU6000_GetAccelScale();
 
 	gyrosData->temperature = 35.0f + ((float) mpu6000_data.temperature + 512.0f) / 340.0f;
 	accelsData->temperature = 35.0f + ((float) mpu6000_data.temperature + 512.0f) / 340.0f;

flight/Modules/Attitude/revolution/attitude.c

@@ -99,6 +99,7 @@ static AttitudeSettingsData attitudeSettings;
 static HomeLocationData homeLocation;
 static RevoCalibrationData revoCalibration;
 static RevoSettingsData revoSettings;
+static bool gyroBiasSettingsUpdated = false;
 const uint32_t SENSOR_QUEUE_SIZE = 10;
 
 // Private functions
@@ -202,8 +203,7 @@ static void AttitudeTask(void *parameters)
 	AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
 
 	// Force settings update to make sure rotation loaded
-	settingsUpdatedCb(AttitudeSettingsHandle());
-	settingsUpdatedCb(RevoSettingsHandle());
+	settingsUpdatedCb(NULL);
 
 	// Wait for all the sensors be to read
 	vTaskDelay(100);
@@ -233,7 +233,7 @@ static void AttitudeTask(void *parameters)
 				ret_val = updateAttitudeINSGPS(first_run, false);
 				break;
 			default:
-				AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR);
+				AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_CRITICAL);
 				break;
 		}
 
@@ -262,8 +262,9 @@ static int32_t updateAttitudeComplementary(bool first_run)
 
 	// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
 	if ( xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE ||
-	     xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE ) // When one of these is updated so should the other
+	     xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE )
 	{
+		// When one of these is updated so should the other
 		// Do not set attitude timeout warnings in simulation mode
 		if (!AttitudeActualReadOnly()){
 			AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_WARNING);
@@ -360,28 +361,30 @@ static int32_t updateAttitudeComplementary(bool first_run)
 		float brot[3];
 		float Rbe[3][3];
 		MagnetometerData mag;
-		HomeLocationData home;
-
+		
 		Quaternion2R(q, Rbe);
 		MagnetometerGet(&mag);
-		HomeLocationGet(&home);
-		rot_mult(Rbe, home.Be, brot);
 
-		float mag_len = sqrtf(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z);
-		mag.x /= mag_len;
-		mag.y /= mag_len;
-		mag.z /= mag_len;
+		// If the mag is producing bad data don't use it (normally bad calibration)
+		if  (mag.x == mag.x && mag.y == mag.y && mag.z == mag.z) {
+			rot_mult(Rbe, homeLocation.Be, brot);
 
-		float bmag = sqrtf(brot[0] * brot[0] + brot[1] * brot[1] + brot[2] * brot[2]);
-		brot[0] /= bmag;
-		brot[1] /= bmag;
-		brot[2] /= bmag;
+			float mag_len = sqrtf(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z);
+			mag.x /= mag_len;
+			mag.y /= mag_len;
+			mag.z /= mag_len;
 
-		// Only compute if neither vector is null
-		if (bmag < 1 || mag_len < 1)
-			mag_err[0] = mag_err[1] = mag_err[2] = 0;
-		else
-			CrossProduct((const float *) &mag.x, (const float *) brot, mag_err);
+			float bmag = sqrtf(brot[0] * brot[0] + brot[1] * brot[1] + brot[2] * brot[2]);
+			brot[0] /= bmag;
+			brot[1] /= bmag;
+			brot[2] /= bmag;
+
+			// Only compute if neither vector is null
+			if (bmag < 1 || mag_len < 1)
+				mag_err[0] = mag_err[1] = mag_err[2] = 0;
+			else
+				CrossProduct((const float *) &mag.x, (const float *) brot, mag_err);
+		}
 	} else {
 		mag_err[0] = mag_err[1] = mag_err[2] = 0;
 	}
@@ -389,9 +392,9 @@ static int32_t updateAttitudeComplementary(bool first_run)
 	// Accumulate integral of error.  Scale here so that units are (deg/s) but Ki has units of s
 	GyrosBiasData gyrosBias;
 	GyrosBiasGet(&gyrosBias);
-	gyrosBias.x += accel_err[0] * attitudeSettings.AccelKi;
-	gyrosBias.y += accel_err[1] * attitudeSettings.AccelKi;
-	gyrosBias.z += mag_err[2] * magKi;
+	gyrosBias.x -= accel_err[0] * attitudeSettings.AccelKi;
+	gyrosBias.y -= accel_err[1] * attitudeSettings.AccelKi;
+	gyrosBias.z -= mag_err[2] * magKi;
 	GyrosBiasSet(&gyrosBias);
 
 	// Correct rates based on error, integral component dealt with in updateSensors
@@ -507,7 +510,6 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 	GPSPositionData gpsData;
 	GPSVelocityData gpsVelData;
 	GyrosBiasData gyrosBias;
-	HomeLocationData home;
 
 	static bool mag_updated = false;
 	static bool baro_updated;
@@ -545,11 +547,6 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 		gps_vel_updated = 0;
 	}
 
-	mag_updated |= (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && homeLocation.Set == HOMELOCATION_SET_TRUE;
-	baro_updated |= xQueueReceive(baroQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE;
-	gps_updated |= (xQueueReceive(gpsQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode;
-	gps_vel_updated |= (xQueueReceive(gpsVelQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode;
-
 	if (first_run) {
 		inited = false;
 		init_stage = 0;
@@ -563,7 +560,12 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 
 		return 0;
 	}
-	
+
+	mag_updated |= (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE);
+	baro_updated |= xQueueReceive(baroQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE;
+	gps_updated |= (xQueueReceive(gpsQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode;
+	gps_vel_updated |= (xQueueReceive(gpsVelQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode;
+
 	// Get most recent data
 	GyrosGet(&gyrosData);
 	AccelsGet(&accelsData);
@@ -571,7 +573,13 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 	BaroAltitudeGet(&baroData);
 	GPSPositionGet(&gpsData);
 	GPSVelocityGet(&gpsVelData);
-	HomeLocationGet(&home);
+	GyrosBiasGet(&gyrosBias);
+
+	// Discard mag if it has NAN (normally from bad calibration)
+	mag_updated &= (magData.x == magData.x && magData.y == magData.y && magData.z == magData.z);
+	// Don't require HomeLocation.Set to be true but at least require a mag configuration (allows easily
+	// switching between indoor and outdoor mode with Set = false)
+	mag_updated &= (homeLocation.Be[0] != 0 || homeLocation.Be[1] != 0 || homeLocation.Be[2]);
 
 	// Have a minimum requirement for gps usage
 	gps_updated &= (gpsData.Satellites >= 7) && (gpsData.PDOP <= 4.0f) && (homeLocation.Set == HOMELOCATION_SET_TRUE);
@@ -601,6 +609,10 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 			INSSetGyroVar(revoCalibration.gyro_var);
 			INSSetBaroVar(revoCalibration.baro_var);
 
+			// Initialize the gyro bias from the settings
+			float gyro_bias[3] = {gyrosBias.x * F_PI / 180.0f, gyrosBias.y * F_PI / 180.0f, gyrosBias.z * F_PI / 180.0f};
+			INSSetGyroBias(gyro_bias);
+
 			xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS);
 			MagnetometerGet(&magData);
 
@@ -630,7 +642,11 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 			INSSetGyroVar(revoCalibration.gyro_var);
 			INSSetBaroVar(revoCalibration.baro_var);
 
-			INSSetMagNorth(home.Be);
+			INSSetMagNorth(homeLocation.Be);
+
+			// Initialize the gyro bias from the settings
+			float gyro_bias[3] = {gyrosBias.x * F_PI / 180.0f, gyrosBias.y * F_PI / 180.0f, gyrosBias.z * F_PI / 180.0f};
+			INSSetGyroBias(gyro_bias);
 
 			GPSPositionData gpsPosition;
 			GPSPositionGet(&gpsPosition);
@@ -664,9 +680,9 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 			dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f;
 
 			GyrosBiasGet(&gyrosBias);
-			float gyros[3] = {(gyrosData.x - gyrosBias.x) * F_PI / 180.0f, 
-				(gyrosData.y - gyrosBias.y) * F_PI / 180.0f, 
-				(gyrosData.z - gyrosBias.z) * F_PI / 180.0f};
+			float gyros[3] = {(gyrosData.x + gyrosBias.x) * F_PI / 180.0f, 
+				(gyrosData.y + gyrosBias.y) * F_PI / 180.0f, 
+				(gyrosData.z + gyrosBias.z) * F_PI / 180.0f};
 			INSStatePrediction(gyros, &accelsData.x, dT);
 			
 			AttitudeActualData attitude;
@@ -700,12 +716,22 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 	else if(dT <= 0.001f)
 		dT = 0.001f;
 
+	// If the gyro bias setting was updated we should reset
+	// the state estimate of the EKF
+	if(gyroBiasSettingsUpdated) {
+		float gyro_bias[3] = {gyrosBias.x * F_PI / 180.0f, gyrosBias.y * F_PI / 180.0f, gyrosBias.z * F_PI / 180.0f};
+		INSSetGyroBias(gyro_bias);
+		gyroBiasSettingsUpdated = false;
+	}
+
 	// Because the sensor module remove the bias we need to add it
-	// back in here so that the INS algorithm can track it correctly 
-	GyrosBiasGet(&gyrosBias);
-	float gyros[3] = {(gyrosData.x - gyrosBias.x) * F_PI / 180.0f, 
-		(gyrosData.y - gyrosBias.y) * F_PI / 180.0f, 
-		(gyrosData.z - gyrosBias.z) * F_PI / 180.0f};
+	// back in here so that the INS algorithm can track it correctly
+	float gyros[3] = {gyrosData.x * F_PI / 180.0f, gyrosData.y * F_PI / 180.0f, gyrosData.z * F_PI / 180.0f};
+	if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) {
+		gyros[0] += gyrosBias.x * F_PI / 180.0f;
+		gyros[1] += gyrosBias.y * F_PI / 180.0f;
+		gyros[2] += gyrosBias.z * F_PI / 180.0f;
+	}
 
 	// Advance the state estimate
 	INSStatePrediction(gyros, &accelsData.x, dT);
@@ -720,12 +746,6 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 	Quaternion2RPY(&attitude.q1,&attitude.Roll);
 	AttitudeActualSet(&attitude);
 
-	// Copy the gyro bias into the UAVO
-	gyrosBias.x = -Nav.gyro_bias[0] * 180.0f / F_PI;
-	gyrosBias.y = -Nav.gyro_bias[1] * 180.0f / F_PI;
-	gyrosBias.z = -Nav.gyro_bias[2] * 180.0f / F_PI;
-	GyrosBiasSet(&gyrosBias);
-
 	// Advance the covariance estimate
 	INSCovariancePrediction(dT);
 
@@ -735,8 +755,8 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 	if(baro_updated)
 		sensors |= BARO_SENSOR;
 
-	INSSetMagNorth(home.Be);
-
+	INSSetMagNorth(homeLocation.Be);
+	
 	if (gps_updated && outdoor_mode)
 	{
 		INSSetPosVelVar(revoCalibration.gps_var[REVOCALIBRATION_GPS_VAR_POS], revoCalibration.gps_var[REVOCALIBRATION_GPS_VAR_VEL]);
@@ -765,6 +785,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 		NEDPositionSet(&nedPos);
 
 	} else if (!outdoor_mode) {
+		baroOffset = 0;
 		INSSetPosVelVar(1e2f, 1e2f);
 		vel[0] = vel[1] = vel[2] = 0;
 		NED[0] = NED[1] = 0;
@@ -801,10 +822,15 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 	velocityActual.East = Nav.Vel[1];
 	velocityActual.Down = Nav.Vel[2];
 	VelocityActualSet(&velocityActual);
-	
-	if(fabs(Nav.gyro_bias[0]) > 0.5f || fabs(Nav.gyro_bias[1]) > 0.5f || fabs(Nav.gyro_bias[2]) > 0.5f) {
-		float zeros[3] = {0.0f,0.0f,0.0f};
-		INSSetGyroBias(zeros);
+
+	if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE && !gyroBiasSettingsUpdated) {
+		// Copy the gyro bias into the UAVO except when it was updated
+		// from the settings during the calculation, then consume it
+		// next cycle
+		gyrosBias.x = Nav.gyro_bias[0] * 180.0f / F_PI;
+		gyrosBias.y = Nav.gyro_bias[1] * 180.0f / F_PI;
+		gyrosBias.z = Nav.gyro_bias[2] * 180.0f / F_PI;
+		GyrosBiasSet(&gyrosBias);
 	}
 
 	return 0;
@@ -834,35 +860,42 @@ static int32_t getNED(GPSPositionData * gpsPosition, float * NED)
 	return 0;
 }
 
-static void settingsUpdatedCb(UAVObjEvent * objEv) 
+static void settingsUpdatedCb(UAVObjEvent * ev) 
 {
-	float lat, alt;
+	if (ev == NULL || ev->obj == RevoCalibrationHandle()) {
+		RevoCalibrationGet(&revoCalibration);
+		
+		/* When the revo calibration is updated, update the GyroBias object */
+		GyrosBiasData gyrosBias;
+		GyrosBiasGet(&gyrosBias);
+		gyrosBias.x = revoCalibration.gyro_bias[REVOCALIBRATION_GYRO_BIAS_X];
+		gyrosBias.y = revoCalibration.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Y];
+		gyrosBias.z = revoCalibration.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Z];
+		GyrosBiasSet(&gyrosBias);
 
-	AttitudeSettingsGet(&attitudeSettings);
-	RevoCalibrationGet(&revoCalibration);
-	RevoSettingsGet(&revoSettings);
-	HomeLocationGet(&homeLocation);
+		gyroBiasSettingsUpdated = true;
 
-	GyrosBiasData gyrosBias;
-	GyrosBiasGet(&gyrosBias);
-	gyrosBias.x = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_X] / 100.0f;
-	gyrosBias.y = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Y] / 100.0f;
-	gyrosBias.z = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Z] / 100.0f;
-	GyrosBiasSet(&gyrosBias);
+		// In case INS currently running
+		INSSetMagVar(revoCalibration.mag_var);
+		INSSetAccelVar(revoCalibration.accel_var);
+		INSSetGyroVar(revoCalibration.gyro_var);
+		INSSetBaroVar(revoCalibration.baro_var);
+	}
+	if(ev == NULL || ev->obj == HomeLocationHandle()) {
+		HomeLocationGet(&homeLocation);
+		// Compute matrix to convert deltaLLA to NED
+		float lat, alt;
+		lat = homeLocation.Latitude / 10.0e6f * DEG2RAD;
+		alt = homeLocation.Altitude;
 
-	// Compute matrix to convert deltaLLA to NED
-	lat = homeLocation.Latitude / 10.0e6f * DEG2RAD;
-	alt = homeLocation.Altitude;
-
-	// In case INS currently running
-	INSSetMagVar(revoCalibration.mag_var);
-	INSSetAccelVar(revoCalibration.accel_var);
-	INSSetGyroVar(revoCalibration.gyro_var);
-	INSSetBaroVar(revoCalibration.baro_var);
-
-	T[0] = alt+6.378137E6f;
-	T[1] = cosf(lat)*(alt+6.378137E6f);
-	T[2] = -1.0f;
+		T[0] = alt+6.378137E6f;
+		T[1] = cosf(lat)*(alt+6.378137E6f);
+		T[2] = -1.0f;
+	}
+	if (ev == NULL || ev->obj == AttitudeSettingsHandle())
+		AttitudeSettingsGet(&attitudeSettings);
+	if (ev == NULL || ev->obj == RevoSettingsHandle())
+		RevoSettingsGet(&revoSettings);
 }
 /**
  * @}

flight/Modules/Battery/battery.c

@@ -55,7 +55,6 @@
 // Configuration
 //
 #define SAMPLE_PERIOD_MS		500
-
 // Private types
 
 // Private variables
@@ -82,7 +81,6 @@ int32_t BatteryInitialize(void)
 #ifdef MODULE_BATTERY_BUILTIN
 	batteryEnabled = true;
 #else
-	HwSettingsInitialize();
 	uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
 
 	HwSettingsOptionalModulesGet(optionalModules);
@@ -125,17 +123,17 @@ int32_t BatteryInitialize(void)
 }
 
 MODULE_INITCALL(BatteryInitialize, 0)
-
+#define HAS_SENSOR(x) batterySettings.SensorType[x]==FLIGHTBATTERYSETTINGS_SENSORTYPE_ENABLED 
 static void onTimer(UAVObjEvent* ev)
 {
 	static FlightBatteryStateData flightBatteryData;
-
 	FlightBatterySettingsData batterySettings;
-	static float dT = SAMPLE_PERIOD_MS / 1000.0f;
-	float energyRemaining;
 
 	FlightBatterySettingsGet(&batterySettings);
 
+	static float dT = SAMPLE_PERIOD_MS / 1000.0f;
+	float energyRemaining;
+
 	//calculate the battery parameters
 	if (voltageADCPin >=0) {
 		flightBatteryData.Voltage = ((float)PIOS_ADC_PinGet(voltageADCPin)) * batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_VOLTAGEFACTOR]; //in Volts
@@ -201,7 +199,7 @@ static void onTimer(UAVObjEvent* ev)
 		else 
 			AlarmsClear(SYSTEMALARMS_ALARM_BATTERY);
 	}
-
+	
 	FlightBatteryStateSet(&flightBatteryData);
 }
 

flight/Modules/FirmwareIAP/inc/firmwareiap.h

@@ -27,6 +27,7 @@
 #define FIRMWAREIAP_H
 
 int32_t FirmwareIAPInitialize();
+int32_t FirmwareIAPStart() {return 0;};
 
 #endif // FIRMWAREIAP_H
 

flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c

@@ -75,7 +75,7 @@
 #define F_PI 3.14159265358979323846f
 #define RAD2DEG (180.0f/F_PI)
 #define DEG2RAD (F_PI/180.0f)
-#define GEE 9.805f
+#define GEE 9.81f
 // Private types
 
 // Private variables

flight/Modules/GPS/GPS.c

@@ -66,7 +66,7 @@ static float GravityAccel(float latitude, float longitude, float altitude);
 
 #ifdef PIOS_GPS_SETS_HOMELOCATION
 // Unfortunately need a good size stack for the WMM calculation
-	#define STACK_SIZE_BYTES            784
+	#define STACK_SIZE_BYTES            850
 #else
 #if defined(PIOS_GPS_MINIMAL)
 	#define STACK_SIZE_BYTES            500
@@ -139,8 +139,9 @@ int32_t GPSInitialize(void)
 #endif
 
 #if defined(REVOLUTION)
-	// Revolution expects these objects to always be defined.  Not doing so will fail some
-	// queue connections in navigation
+	// These objects MUST be initialized for Revolution
+	// because the rest of the system expects to just
+	// attach to their queues
 	GPSPositionInitialize();
 	GPSVelocityInitialize();
 	GPSTimeInitialize();

flight/Modules/GPS/NMEA.c

@@ -493,11 +493,6 @@ static bool nmeaProcessGPGGA(GPSPositionData * GpsData, bool* gpsDataUpdated, ch
 		return false;
 	}
 
-
-	// check for invalid GPS fix
-	if (param[6][0] == '0') {
-		return false;
-	}
 	// get number of satellites used in GPS solution
 	GpsData->Satellites = atoi(param[7]);
 

flight/Modules/OveroSync/overosync.c

@@ -38,7 +38,7 @@
 
 // Private constants
 #define OVEROSYNC_PACKET_SIZE 1024
-#define MAX_QUEUE_SIZE   40
+#define MAX_QUEUE_SIZE   200
 #define STACK_SIZE_BYTES 512
 #define TASK_PRIORITY (tskIDLE_PRIORITY + 0)
 
@@ -55,86 +55,21 @@ static bool overoEnabled;
 // Private functions
 static void overoSyncTask(void *parameters);
 static int32_t packData(uint8_t * data, int32_t length);
-static int32_t transmitData();
-static void transmitDataDone(bool crc_ok, uint8_t crc_val);
 static void registerObject(UAVObjHandle obj);
 
 // External variables
-extern int32_t pios_spi_overo_id;
-
-struct dma_transaction {
-	uint8_t tx_buffer[OVEROSYNC_PACKET_SIZE] __attribute__ ((aligned(4)));
-	uint8_t rx_buffer[OVEROSYNC_PACKET_SIZE] __attribute__ ((aligned(4)));
-};
+extern uint32_t pios_com_overo_id;
+extern uint32_t pios_overo_id;
 
 struct overosync {
-	struct dma_transaction transactions[2];
-	uint32_t active_transaction_id;
-	uint32_t loading_transaction_id;
-	xSemaphoreHandle transaction_lock;
-	xSemaphoreHandle buffer_lock;
-	volatile bool transaction_done;
 	uint32_t sent_bytes;
-	uint32_t write_pointer;
 	uint32_t sent_objects;
 	uint32_t failed_objects;
 	uint32_t received_objects;
-	uint32_t framesync_error;
 };
 
 struct overosync *overosync;
 
-static bool PIOS_OVERO_IRQHandler();
-
-static const struct pios_exti_cfg pios_exti_overo_cfg __exti_config = {
-	.vector = PIOS_OVERO_IRQHandler,
-	.line = EXTI_Line15,
-	.pin = {
-		.gpio = GPIOA,
-		.init = {
-			.GPIO_Pin = GPIO_Pin_15,
-			.GPIO_Speed = GPIO_Speed_100MHz,
-			.GPIO_Mode = GPIO_Mode_IN,
-			.GPIO_OType = GPIO_OType_OD,
-			.GPIO_PuPd = GPIO_PuPd_NOPULL,
-		},
-	},
-	.irq = {
-		.init = {
-			.NVIC_IRQChannel = EXTI15_10_IRQn,
-			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
-			.NVIC_IRQChannelSubPriority = 0,
-			.NVIC_IRQChannelCmd = ENABLE,
-		},
-	},
-	.exti = {
-		.init = {
-			.EXTI_Line = EXTI_Line15, // matches above GPIO pin
-			.EXTI_Mode = EXTI_Mode_Interrupt,
-			.EXTI_Trigger = EXTI_Trigger_Rising,
-			.EXTI_LineCmd = ENABLE,
-		},
-	},
-};
-
-/**
- * On the rising edge of NSS schedule a new transaction.  This cannot be
- * done by the DMA complete because there is 150 us between that and the
- * Overo deasserting the CS line.  We don't want to spin that long in an
- * isr
- */
-bool PIOS_OVERO_IRQHandler()
-{
-	// transmitData must not block to get semaphore for when we get out of
-	// frame and transaction is still running here.  -1 indicates the transaction
-	// semaphore is blocked and we are still in a transaction, thus a framesync
-	// error occurred.  This shouldn't happen.  Race condition?
-	if(transmitData() == -1)
-		overosync->framesync_error++;
-
-	return false;
-}
-
 /**
  * Initialise the telemetry module
  * \return -1 if initialisation failed
@@ -142,8 +77,6 @@ bool PIOS_OVERO_IRQHandler()
  */
 int32_t OveroSyncInitialize(void)
 {
-	if(pios_spi_overo_id == 0)
-		return -1;
 
 #ifdef MODULE_OVERO_BUILTIN
 	overoEnabled = true;
@@ -155,6 +88,9 @@ int32_t OveroSyncInitialize(void)
 	
 	if (optionalModules[HWSETTINGS_OPTIONALMODULES_OVERO] == HWSETTINGS_OPTIONALMODULES_ENABLED) {
 		overoEnabled = true;
+	
+		// Create object queues
+		queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
 	} else {
 		overoEnabled = false;
 		return -1;
@@ -164,9 +100,6 @@ int32_t OveroSyncInitialize(void)
 	
 	OveroSyncStatsInitialize();
 
-	PIOS_EXTI_Init(&pios_exti_overo_cfg);
-	// Create object queues
-	queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
 
 	// Initialise UAVTalk
 	uavTalkCon = UAVTalkInitialize(&packData);
@@ -186,26 +119,11 @@ int32_t OveroSyncStart(void)
 		return -1;
 	}
 	
-	if(pios_spi_overo_id == 0)
-		return -1;
-
 	overosync = (struct overosync *) pvPortMalloc(sizeof(*overosync));
 	if(overosync == NULL)
 		return -1;
 
-	overosync->transaction_lock = xSemaphoreCreateMutex();
-	if(overosync->transaction_lock == NULL)
-		return -1;
-
-	overosync->buffer_lock = xSemaphoreCreateMutex();
-	if(overosync->buffer_lock == NULL)
-		return -1;
-
-	overosync->active_transaction_id = 0;
-	overosync->loading_transaction_id = 0;
-	overosync->write_pointer = 0;
 	overosync->sent_bytes = 0;
-	overosync->framesync_error = 0;
 
 	// Process all registered objects and connect queue for updates
 	UAVObjIterate(&registerObject);
@@ -249,65 +167,40 @@ static void overoSyncTask(void *parameters)
 	UAVObjEvent ev;
 
 	// Kick off SPI transfers (once one is completed another will automatically transmit)
-	overosync->transaction_done = true;
 	overosync->sent_objects = 0;
 	overosync->failed_objects = 0;
 	overosync->received_objects = 0;
 	
 	portTickType lastUpdateTime = xTaskGetTickCount();
 	portTickType updateTime;
-	
+
 	// Loop forever
 	while (1) {
 		// Wait for queue message
 		if (xQueueReceive(queue, &ev, portMAX_DELAY) == pdTRUE) {
-		
-			// Check it will fit before packetizing
-			if ((overosync->write_pointer + UAVObjGetNumBytes(ev.obj) + 12) >=
-				sizeof(overosync->transactions[overosync->loading_transaction_id].tx_buffer)) {
-				overosync->failed_objects ++;
-			} else {
-				// Process event.  This calls transmitData
-				UAVTalkSendObject(uavTalkCon, ev.obj, ev.instId, false, 0);
-			}
 
+			// Process event.  This calls transmitData
+			UAVTalkSendObjectTimestamped(uavTalkCon, ev.obj, ev.instId, false, 0);
+			
 			updateTime = xTaskGetTickCount();
 			if(((portTickType) (updateTime - lastUpdateTime)) > 1000) {
 				// Update stats.  This will trigger a local send event too
 				OveroSyncStatsData syncStats;
 				syncStats.Send = overosync->sent_bytes;
-				syncStats.Received = 0;
 				syncStats.Connected = syncStats.Send > 500 ? OVEROSYNCSTATS_CONNECTED_TRUE : OVEROSYNCSTATS_CONNECTED_FALSE;
 				syncStats.DroppedUpdates = overosync->failed_objects;
+				syncStats.Packets = PIOS_OVERO_GetPacketCount(pios_overo_id);
 				OveroSyncStatsSet(&syncStats);
 				overosync->failed_objects = 0;
 				overosync->sent_bytes = 0;
 				lastUpdateTime = updateTime;
 			}
+
+			// TODO: Check the receive buffer
 		}
 	}
 }
 
-static void transmitDataDone(bool crc_ok, uint8_t crc_val)
-{
-	uint8_t *rx_buffer;
-	static signed portBASE_TYPE xHigherPriorityTaskWoken;
-
-	rx_buffer = overosync->transactions[overosync->active_transaction_id].rx_buffer;
-
-	// Release the semaphore and start another transaction (which grabs semaphore again but then
-	// returns instantly).  Because this is called by the DMA ISR we need to be aware of context
-	// switches.
-	xSemaphoreGiveFromISR(overosync->transaction_lock, &xHigherPriorityTaskWoken);
-	portEND_SWITCHING_ISR(xHigherPriorityTaskWoken);
-
-	overosync->transaction_done = true;
-	
-	// Parse the data from overo
-	for (uint32_t i = 0; rx_buffer[0] != 0 && i < sizeof(rx_buffer) ; i++)
-		UAVTalkProcessInputStream(uavTalkCon, rx_buffer[i]);
-}
-
 /**
  * Transmit data buffer to the modem or USB port.
  * \param[in] data Data buffer to send
@@ -315,87 +208,18 @@ static void transmitDataDone(bool crc_ok, uint8_t crc_val)
  * \return -1 on failure
  * \return number of bytes transmitted on success
  */
-uint32_t too_long = 0;
 static int32_t packData(uint8_t * data, int32_t length)
 {
-	uint8_t *tx_buffer;
-	
-	portTickType tickTime = xTaskGetTickCount();
+	if( PIOS_COM_SendBufferNonBlocking(pios_com_overo_id, data, length) < 0)
+		goto fail;
 
-	// Get the lock for manipulating the buffer
-	xSemaphoreTake(overosync->buffer_lock, portMAX_DELAY);
-
-	// Check this packet will fit
-	if ((overosync->write_pointer + length + sizeof(tickTime)) >
-		sizeof(overosync->transactions[overosync->loading_transaction_id].tx_buffer)) {
-		overosync->failed_objects ++;
-		xSemaphoreGive(overosync->buffer_lock);
-		return -1;
-	}
-
-	// Get offset into buffer and copy contents
-	tx_buffer = overosync->transactions[overosync->loading_transaction_id].tx_buffer +
-		overosync->write_pointer;
-	memcpy(tx_buffer, &tickTime, sizeof(tickTime));
-	memcpy(tx_buffer + sizeof(tickTime),data,length);
-	overosync->write_pointer += length + sizeof(tickTime);
 	overosync->sent_bytes += length;
-	overosync->sent_objects++;
-
-	xSemaphoreGive(overosync->buffer_lock);
-	
-	// When the NSS line rises while we are packing data then a transaction doesn't start
-	// because that means we will be here very shortly afterwards (priority of task making that
-	// not always perfectly true) schedule the transaction here.
-	if (buffer_swap_failed && (PIOS_DELAY_DiffuS(buffer_swap_timeval) < 50)) {
-		buffer_swap_failed = false;
-		transmitData();
-	} else if (buffer_swap_failed) {
-		buffer_swap_failed = false;
-		too_long++;
-	}
 
 	return length;
-}
 
-static int32_t transmitData()
-{
-	uint8_t *tx_buffer, *rx_buffer;
-	static signed portBASE_TYPE xHigherPriorityTaskWoken;
-
-	// Get this lock first so we don't swap buffers and then fail
-	// to start
-	if (xSemaphoreTake(overosync->transaction_lock, 0) == pdFALSE)
-		return -1;
-
-	// Get lock to manipulate buffers
-	if(xSemaphoreTake(overosync->buffer_lock, 0) == pdFALSE) {
-		xSemaphoreGiveFromISR(overosync->transaction_lock, &xHigherPriorityTaskWoken);
-		portEND_SWITCHING_ISR(xHigherPriorityTaskWoken);
-		buffer_swap_failed = true;
-		buffer_swap_timeval = PIOS_DELAY_GetRaw();
-		return -2;
-	}
-
-	overosync->transaction_done = false;
-
-	// Swap buffers
-	overosync->active_transaction_id = overosync->loading_transaction_id;
-	overosync->loading_transaction_id = (overosync->loading_transaction_id + 1) % 
-		NELEMENTS(overosync->transactions);
-		
-	tx_buffer = overosync->transactions[overosync->active_transaction_id].tx_buffer;
-	rx_buffer = overosync->transactions[overosync->active_transaction_id].rx_buffer;
-	
-	// Prepare the new loading buffer
-	memset(overosync->transactions[overosync->loading_transaction_id].tx_buffer, 0xff, 
-		   sizeof(overosync->transactions[overosync->loading_transaction_id].tx_buffer));
-	overosync->write_pointer = 0;
-
-	xSemaphoreGiveFromISR(overosync->buffer_lock, &xHigherPriorityTaskWoken);
-	portEND_SWITCHING_ISR(xHigherPriorityTaskWoken);
-
-	return PIOS_SPI_TransferBlock(pios_spi_overo_id, (uint8_t *) tx_buffer, (uint8_t *) rx_buffer, sizeof(overosync->transactions[overosync->active_transaction_id].tx_buffer), &transmitDataDone) == 0 ? 0 : -3;
+fail:
+	overosync->failed_objects++;
+	return -1;
 }
 
 /**

flight/Modules/PathPlanner/pathplanner.c

@@ -44,7 +44,7 @@
 #include "waypointactive.h"
 
 // Private constants
-#define STACK_SIZE_BYTES 1024 // TODO profile and reduce!
+#define STACK_SIZE_BYTES 1024
 #define TASK_PRIORITY (tskIDLE_PRIORITY+1)
 #define MAX_QUEUE_SIZE 2
 #define F_PI 3.141526535897932f

flight/Modules/Radio/inc/radio.h

@@ -0,0 +1,39 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{ 
+ * @addtogroup Radio Input / Output Module
+ * @brief Read and Write packets from/to a radio device.
+ * @{ 
+ *
+ * @file       radio.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @brief      Include file of the radio module.
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#ifndef RADIOMODULE_H
+#define RADIOMODULE_H
+
+#endif // RADIOMODULE_H
+
+/**
+  * @}
+  * @}
+  */

flight/Modules/Radio/radio.c

@@ -0,0 +1,476 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{ 
+ * @addtogroup Radio Input / Output Module
+ * @brief Read and Write packets from/to a radio device.
+ * @{ 
+ *
+ * @file       radio.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @brief      Bridges selected Com Port to the COM VCP emulated serial port
+ * @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>
+#include <pios_board_info.h>
+#include <openpilot.h>
+#include <gcsreceiver.h>
+#include <hwsettings.h>
+#include <pipxsettings.h>
+#include <pipxstatus.h>
+#include <packet_handler.h>
+#include <pios_com_priv.h>
+#include <pios_rfm22b_priv.h>
+#include <radio.h>
+
+// ****************
+// Private constants
+
+#define STACK_SIZE_BYTES 200
+#define TASK_PRIORITY (tskIDLE_PRIORITY + 2)
+#define PACKET_QUEUE_SIZE PIOS_PH_WIN_SIZE
+#define MAX_PORT_DELAY 200
+#define STATS_UPDATE_PERIOD_MS 500
+#define RADIOSTATS_UPDATE_PERIOD_MS 250
+#define MAX_LOST_CONTACT_TIME 4
+#define PACKET_MAX_DELAY 50
+
+#ifndef LINK_LED_ON
+#define LINK_LED_ON
+#define LINK_LED_OFF
+#endif
+
+// ****************
+// Private types
+
+typedef struct {
+	uint32_t pairID;
+	uint16_t retries;
+	uint16_t errors;
+	uint16_t uavtalk_errors;
+	uint16_t resets;
+	uint16_t dropped;
+	int8_t rssi;
+	uint8_t lastContact;
+} PairStats;
+
+typedef struct {
+
+	// The task handles.
+	xTaskHandle radioReceiveTaskHandle;
+	xTaskHandle radioStatusTaskHandle;
+
+	// Queue handles.
+	xQueueHandle radioPacketQueue;
+
+	// Error statistics.
+	uint32_t radioTxErrors;
+	uint32_t radioRxErrors;
+	uint16_t txBytes;
+	uint16_t rxBytes;
+
+	// External error statistics
+	uint32_t droppedPackets;
+	uint32_t comTxRetries;
+	uint32_t UAVTalkErrors;
+
+	// The destination ID
+	uint32_t destination_id;
+
+	// Track other radios that are in range.
+	PairStats pairStats[PIPXSTATUS_PAIRIDS_NUMELEM];
+
+	// The RSSI of the last packet received.
+	int8_t RSSI;
+
+} RadioData;
+
+// ****************
+// Private functions
+
+static void radioReceiveTask(void *parameters);
+static void radioStatusTask(void *parameters);
+static void StatusHandler(PHStatusPacketHandle p, int8_t rssi, int8_t afc);
+static int32_t transmitPacket(PHPacketHandle packet);
+static void PPMHandler(uint16_t *channels);
+
+// ****************
+// Private variables
+
+static RadioData *data = 0;
+
+// ****************
+// Global variables
+uint32_t pios_rfm22b_id = 0;
+uint32_t pios_com_rfm22b_id = 0;
+uint32_t pios_packet_handler = 0;
+const struct pios_rfm22b_cfg *pios_rfm22b_cfg;
+
+// ***************
+// External functions
+extern const struct pios_rfm22b_cfg * PIOS_BOARD_HW_DEFS_GetRfm22Cfg (uint32_t board_revision);
+
+/**
+ * Start the module
+ * \return -1 if initialisation failed
+ * \return 0 on success
+ */
+static int32_t RadioStart(void)
+{
+	if (!data)
+		return -1;
+
+	// Start the tasks.
+	xTaskCreate(radioReceiveTask, (signed char *)"RadioReceive", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->radioReceiveTaskHandle));
+	xTaskCreate(radioStatusTask, (signed char *)"RadioStatus", STACK_SIZE_BYTES * 2, NULL, TASK_PRIORITY, &(data->radioStatusTaskHandle));
+
+	// Install the monitors
+	TaskMonitorAdd(TASKINFO_RUNNING_MODEMRX, data->radioReceiveTaskHandle);
+	TaskMonitorAdd(TASKINFO_RUNNING_MODEMSTAT, data->radioStatusTaskHandle);
+
+	// Register the watchdog timers.
+#ifdef PIOS_WDG_RADIORECEIVE
+	PIOS_WDG_RegisterFlag(PIOS_WDG_RADIORECEIVE);
+#endif /* PIOS_WDG_RADIORECEIVE */
+
+	return 0;
+}
+
+
+/**
+ * Initialise the module
+ * \return -1 if initialisation failed
+ * \return 0 on success
+ */
+static int32_t RadioInitialize(void)
+{
+
+	// See if this module is enabled.
+#ifndef RADIO_BUILTIN
+	HwSettingsInitialize();
+	uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
+	HwSettingsOptionalModulesGet(optionalModules);
+	if (optionalModules[HWSETTINGS_OPTIONALMODULES_RADIO] != HWSETTINGS_OPTIONALMODULES_ENABLED) {
+		pios_packet_handler = 0;
+		return -1;
+	}
+#endif
+
+	// Initalize out UAVOs
+	PipXSettingsInitialize();
+	PipXStatusInitialize();
+
+	PipXSettingsData pipxSettings;
+	PipXSettingsGet(&pipxSettings);
+
+	/* Retrieve hardware settings. */
+	const struct pios_board_info * bdinfo = &pios_board_info_blob;
+	pios_rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev);
+
+	/* Initalize the RFM22B radio COM device. */
+	if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, pios_rfm22b_cfg->slave_num, pios_rfm22b_cfg))
+		return -1;
+
+	// Set the maximum radio RF power.
+	switch (pipxSettings.MaxRFPower)
+	{
+	case PIPXSETTINGS_MAXRFPOWER_125:
+		PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_0);
+		break;
+	case PIPXSETTINGS_MAXRFPOWER_16:
+		PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_1);
+		break;
+	case PIPXSETTINGS_MAXRFPOWER_316:
+		PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_2);
+		break;
+	case PIPXSETTINGS_MAXRFPOWER_63:
+		PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_3);
+		break;
+	case PIPXSETTINGS_MAXRFPOWER_126:
+		PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_4);
+		break;
+	case PIPXSETTINGS_MAXRFPOWER_25:
+		PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_5);
+		break;
+	case PIPXSETTINGS_MAXRFPOWER_50:
+		PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_6);
+		break;
+	case PIPXSETTINGS_MAXRFPOWER_100:
+		PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_7);
+		break;
+	}
+
+	switch (pipxSettings.RFSpeed) {
+	case PIPXSETTINGS_RFSPEED_2400:
+		RFM22_SetDatarate(pios_rfm22b_id, RFM22_datarate_2000, true);
+		break;
+	case PIPXSETTINGS_RFSPEED_4800:
+		RFM22_SetDatarate(pios_rfm22b_id, RFM22_datarate_4000, true);
+		break;
+	case PIPXSETTINGS_RFSPEED_9600:
+		RFM22_SetDatarate(pios_rfm22b_id, RFM22_datarate_9600, true);
+		break;
+	case PIPXSETTINGS_RFSPEED_19200:
+		RFM22_SetDatarate(pios_rfm22b_id, RFM22_datarate_19200, true);
+		break;
+	case PIPXSETTINGS_RFSPEED_38400:
+		RFM22_SetDatarate(pios_rfm22b_id, RFM22_datarate_32000, true);
+		break;
+	case PIPXSETTINGS_RFSPEED_57600:
+		RFM22_SetDatarate(pios_rfm22b_id, RFM22_datarate_64000, true);
+		break;
+	case PIPXSETTINGS_RFSPEED_115200:
+		RFM22_SetDatarate(pios_rfm22b_id, RFM22_datarate_128000, true);
+		break;
+	}
+
+	// Set the radio destination ID.
+	PIOS_RFM22B_SetDestinationId(pios_rfm22b_id, pipxSettings.PairID);
+
+	// Initialize the packet handler
+	PacketHandlerConfig pios_ph_cfg = {
+		.default_destination_id = 0xffffffff, // Broadcast
+		.source_id = PIOS_RFM22B_DeviceID(pios_rfm22b_id),
+		.win_size = PIOS_PH_WIN_SIZE,
+		.max_connections = PIOS_PH_MAX_CONNECTIONS,
+	};
+	pios_packet_handler = PHInitialize(&pios_ph_cfg);
+
+	// allocate and initialize the static data storage only if module is enabled
+	data = (RadioData *)pvPortMalloc(sizeof(RadioData));
+	if (!data)
+		return -1;
+
+	// Initialize the statistics.
+	data->radioTxErrors = 0;
+	data->radioRxErrors = 0;
+	data->droppedPackets = 0;
+	data->comTxRetries = 0;
+	data->UAVTalkErrors = 0;
+	data->RSSI = -127;
+
+	// Initialize the detected device statistics.
+	for (uint8_t i = 0; i < PIPXSTATUS_PAIRIDS_NUMELEM; ++i)
+	{
+		data->pairStats[i].pairID = 0;
+		data->pairStats[i].rssi = -127;
+		data->pairStats[i].retries = 0;
+		data->pairStats[i].errors = 0;
+		data->pairStats[i].uavtalk_errors = 0;
+		data->pairStats[i].resets = 0;
+		data->pairStats[i].dropped = 0;
+		data->pairStats[i].lastContact = 0;
+	}
+	// The first slot is reserved for our current pairID
+	PipXSettingsPairIDGet(&(data->pairStats[0].pairID));
+	data->destination_id = data->pairStats[0].pairID ? data->pairStats[0].pairID : 0xffffffff;
+
+	// Register the callbacks with the packet handler
+	PHRegisterStatusHandler(pios_packet_handler, StatusHandler);
+	PHRegisterOutputStream(pios_packet_handler, transmitPacket);
+	PHRegisterPPMHandler(pios_packet_handler, PPMHandler);
+
+	return 0;
+}
+MODULE_INITCALL(RadioInitialize, RadioStart)
+
+/**
+ * The task that receives packets from the radio.
+ */
+static void radioReceiveTask(void *parameters)
+{
+	PHPacketHandle p = NULL;
+
+	/* Handle radio -> usart/usb direction */
+	while (1) {
+		uint32_t rx_bytes;
+
+#ifdef PIOS_WDG_RADIORECEIVE
+		// Update the watchdog timer.
+		PIOS_WDG_UpdateFlag(PIOS_WDG_RADIORECEIVE);
+#endif /* PIOS_INCLUDE_WDG */
+
+		// Receive data from the radio port
+		p = NULL;
+		rx_bytes = PIOS_RFM22B_Receive_Packet(pios_rfm22b_id, &p, MAX_PORT_DELAY);
+		if(rx_bytes == 0)
+			continue;
+		data->rxBytes += rx_bytes;
+		PHReceivePacket(pios_packet_handler, p);
+		p = NULL;
+	}
+}
+
+/**
+ * Transmit a packet to the radio port.
+ * \param[in] buf Data buffer to send
+ * \param[in] length Length of buffer
+ * \return -1 on failure
+ * \return number of bytes transmitted on success
+ */
+static int32_t transmitPacket(PHPacketHandle p)
+{
+	uint16_t len = PH_PACKET_SIZE(p);
+	data->txBytes += len;
+	if (!PIOS_RFM22B_Send_Packet(pios_rfm22b_id, p, PACKET_MAX_DELAY))
+		return -1;
+	return len;
+}
+
+/**
+ * Receive a status packet
+ * \param[in] status The status structure
+ */
+static void StatusHandler(PHStatusPacketHandle status, int8_t rssi, int8_t afc)
+{
+	uint32_t id = status->header.source_id;
+	bool found = false;
+	// Have we seen this device recently?
+	uint8_t id_idx = 0;
+	for ( ; id_idx < PIPXSTATUS_PAIRIDS_NUMELEM; ++id_idx)
+		if(data->pairStats[id_idx].pairID == id)
+		{
+			found = true;
+			break;
+		}
+
+	// If we have seen it, update the RSSI and reset the last contact couter
+	if(found)
+	{
+		data->pairStats[id_idx].rssi = rssi;
+		data->pairStats[id_idx].retries = status->retries;
+		data->pairStats[id_idx].errors = status->errors;
+		data->pairStats[id_idx].uavtalk_errors = status->uavtalk_errors;
+		data->pairStats[id_idx].resets = status->resets;
+		data->pairStats[id_idx].dropped = status->dropped;
+		data->pairStats[id_idx].lastContact = 0;
+	}
+
+	// If we haven't seen it, find a slot to put it in.
+	if (!found)
+	{
+		uint32_t pairID;
+		PipXSettingsPairIDGet(&pairID);
+
+		uint8_t min_idx = 0;
+		if(id != pairID)
+		{
+			int8_t min_rssi = data->pairStats[0].rssi;
+			for (id_idx = 1; id_idx < PIPXSTATUS_PAIRIDS_NUMELEM; ++id_idx)
+			{
+				if(data->pairStats[id_idx].rssi < min_rssi)
+				{
+					min_rssi = data->pairStats[id_idx].rssi;
+					min_idx = id_idx;
+				}
+			}
+		}
+		data->pairStats[min_idx].pairID = id;
+		data->pairStats[min_idx].rssi = rssi;
+		data->pairStats[min_idx].retries = status->retries;
+		data->pairStats[min_idx].errors = status->errors;
+		data->pairStats[min_idx].uavtalk_errors = status->uavtalk_errors;
+		data->pairStats[min_idx].resets = status->resets;
+		data->pairStats[min_idx].dropped = status->dropped;
+		data->pairStats[min_idx].lastContact = 0;
+	}
+}
+
+/**
+ * The stats update task.
+ */
+static void radioStatusTask(void *parameters)
+{
+	while (1) {
+		PipXStatusData pipxStatus;
+		uint32_t pairID;
+
+		// Get object data
+		PipXStatusGet(&pipxStatus);
+		PipXSettingsPairIDGet(&pairID);
+
+		// Update the status
+		pipxStatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id);
+		pipxStatus.Retries = data->comTxRetries;
+		pipxStatus.LinkQuality = PIOS_RFM22B_LinkQuality(pios_rfm22b_id);
+		pipxStatus.UAVTalkErrors = data->UAVTalkErrors;
+		pipxStatus.Dropped = data->droppedPackets;
+		pipxStatus.Resets = PIOS_RFM22B_Resets(pios_rfm22b_id);
+		pipxStatus.TXRate = (uint16_t)((float)(data->txBytes * 1000) / STATS_UPDATE_PERIOD_MS);
+		data->txBytes = 0;
+		pipxStatus.RXRate = (uint16_t)((float)(data->rxBytes * 1000) / STATS_UPDATE_PERIOD_MS);
+		data->rxBytes = 0;
+		pipxStatus.LinkState = PIPXSTATUS_LINKSTATE_DISCONNECTED;
+		pipxStatus.RSSI = PIOS_RFM22B_LinkQuality(pios_rfm22b_id);
+		LINK_LED_OFF;
+
+		// Update the potential pairing contacts
+		for (uint8_t i = 0; i < PIPXSTATUS_PAIRIDS_NUMELEM; ++i)
+		{
+			pipxStatus.PairIDs[i] = data->pairStats[i].pairID;
+			pipxStatus.PairSignalStrengths[i] = data->pairStats[i].rssi;
+			data->pairStats[i].lastContact++;
+			// Remove this device if it's stale.
+			if(data->pairStats[i].lastContact > MAX_LOST_CONTACT_TIME)
+			{
+				data->pairStats[i].pairID = 0;
+				data->pairStats[i].rssi = -127;
+				data->pairStats[i].retries = 0;
+				data->pairStats[i].errors = 0;
+				data->pairStats[i].uavtalk_errors = 0;
+				data->pairStats[i].resets = 0;
+				data->pairStats[i].dropped = 0;
+				data->pairStats[i].lastContact = 0;
+			}
+			// Add the paired devices statistics to ours.
+			if(pairID && (data->pairStats[i].pairID == pairID) && (data->pairStats[i].rssi > -127))
+			{
+				pipxStatus.Retries += data->pairStats[i].retries;
+				pipxStatus.UAVTalkErrors += data->pairStats[i].uavtalk_errors;
+				pipxStatus.Dropped += data->pairStats[i].dropped;
+				pipxStatus.Resets += data->pairStats[i].resets;
+				pipxStatus.Dropped += data->pairStats[i].dropped;
+				pipxStatus.LinkState = PIPXSTATUS_LINKSTATE_CONNECTED;
+				LINK_LED_ON;
+			}
+		}
+
+		// Update the object
+		PipXStatusSet(&pipxStatus);
+
+		vTaskDelay(STATS_UPDATE_PERIOD_MS / portTICK_RATE_MS);
+	}
+}
+
+/**
+ * Receive a ppm packet
+ * \param[in] channels The ppm channels
+ */
+static void PPMHandler(uint16_t *channels)
+{
+	GCSReceiverData rcvr;
+
+	// Copy the receiver channels into the GCSReceiver object.
+	for (uint8_t i = 0; i < GCSRECEIVER_CHANNEL_NUMELEM; ++i)
+		rcvr.Channel[i] = channels[i];
+
+	// Set the GCSReceiverData object.
+	GCSReceiverSet(&rcvr);
+}

flight/Modules/RadioComBridge/RadioComBridge.c

@@ -55,8 +55,8 @@
 #define BRIDGE_BUF_LEN 512
 #define MAX_RETRIES 2
 #define RETRY_TIMEOUT_MS 20
-#define STATS_UPDATE_PERIOD_MS 500
-#define RADIOSTATS_UPDATE_PERIOD_MS 250
+#define STATS_UPDATE_PERIOD_MS 1000
+#define RADIOSTATS_UPDATE_PERIOD_MS 500
 #define MAX_LOST_CONTACT_TIME 4
 #define PACKET_QUEUE_SIZE 10
 #define MAX_PORT_DELAY 200
@@ -68,25 +68,13 @@
 // ****************
 // Private types
 
-typedef struct {
-	uint32_t pairID;
-	uint16_t retries;
-	uint16_t errors;
-	uint16_t uavtalk_errors;
-	uint16_t resets;
-	uint16_t dropped;
-	int8_t rssi;
-	uint8_t lastContact;
-} PairStats;
-
 typedef struct {
 	uint32_t comPort;
 	UAVTalkConnection UAVTalkCon;
 	xQueueHandle sendQueue;
 	xQueueHandle recvQueue;
-	xQueueHandle gcsQueue;
 	uint16_t wdg;
-	bool checkHID;
+	bool isGCS;
 } UAVTalkComTaskParams;
 
 typedef struct {
@@ -94,10 +82,7 @@ typedef struct {
 	// The task handles.
 	xTaskHandle GCSUAVTalkRecvTaskHandle;
 	xTaskHandle UAVTalkRecvTaskHandle;
-	xTaskHandle radioReceiveTaskHandle;
-	xTaskHandle sendPacketTaskHandle;
 	xTaskHandle UAVTalkSendTaskHandle;
-	xTaskHandle radioStatusTaskHandle;
 	xTaskHandle transparentCommTaskHandle;
 	xTaskHandle ppmInputTaskHandle;
 
@@ -106,23 +91,14 @@ typedef struct {
 	UAVTalkConnection GCSUAVTalkCon;
 
 	// Queue handles.
-	xQueueHandle radioPacketQueue;
 	xQueueHandle gcsEventQueue;
 	xQueueHandle uavtalkEventQueue;
-	xQueueHandle ppmOutQueue;
 
 	// Error statistics.
 	uint32_t comTxErrors;
 	uint32_t comTxRetries;
-	uint32_t comRxErrors;
-	uint32_t radioTxErrors;
-	uint32_t radioTxRetries;
-	uint32_t radioRxErrors;
 	uint32_t UAVTalkErrors;
-	uint32_t packetErrors;
 	uint32_t droppedPackets;
-	uint16_t txBytes;
-	uint16_t rxBytes;
 
 	// The destination ID
 	uint32_t destination_id;
@@ -131,9 +107,6 @@ typedef struct {
 	portTickType send_timeout;
 	uint16_t min_packet_size;
 
-	// Track other radios that are in range.
-	PairStats pairStats[PIPXSTATUS_PAIRIDS_NUMELEM];
-
 	// The RSSI of the last packet received.
 	int8_t RSSI;
 
@@ -155,19 +128,13 @@ typedef struct {
 // Private functions
 
 static void UAVTalkRecvTask(void *parameters);
-static void radioReceiveTask(void *parameters);
-static void sendPacketTask(void *parameters);
 static void UAVTalkSendTask(void *parameters);
 static void transparentCommTask(void * parameters);
-static void radioStatusTask(void *parameters);
 static void ppmInputTask(void *parameters);
 static int32_t UAVTalkSendHandler(uint8_t * data, int32_t length);
 static int32_t GCSUAVTalkSendHandler(uint8_t * data, int32_t length);
-static int32_t transmitPacket(PHPacketHandle packet);
 static void receiveData(uint8_t *buf, uint8_t len, int8_t rssi, int8_t afc);
 static void transmitData(uint32_t outputPort, uint8_t *buf, uint8_t len, bool checkHid);
-static void StatusHandler(PHStatusPacketHandle p, int8_t rssi, int8_t afc);
-static void PPMHandler(uint16_t *channels);
 static BufferedReadHandle BufferedReadInit(uint32_t com_port, uint16_t buffer_length);
 static bool BufferedRead(BufferedReadHandle h, uint8_t *value, uint32_t timeout_ms);
 static void BufferedReadSetCom(BufferedReadHandle h, uint32_t com_port);
@@ -187,6 +154,11 @@ static RadioComBridgeData *data;
 static int32_t RadioComBridgeStart(void)
 {
 	if(data) {
+
+		// Register the callbacks with the packet handler
+		// This has to happen after the Radio module is initialized.
+		PHRegisterDataHandler(pios_packet_handler, receiveData);
+
 		// Start the primary tasks for receiving/sending UAVTalk packets from the GCS.
 		xTaskCreate(UAVTalkRecvTask, (signed char *)"GCSUAVTalkRecvTask", STACK_SIZE_BYTES, (void*)&(data->gcs_uavtalk_params), TASK_PRIORITY + 2, &(data->GCSUAVTalkRecvTaskHandle));
 		xTaskCreate(UAVTalkSendTask, (signed char *)"GCSUAVTalkSendTask", STACK_SIZE_BYTES, (void*)&(data->gcs_uavtalk_params), TASK_PRIORITY+ 2, &(data->UAVTalkSendTaskHandle));
@@ -202,19 +174,15 @@ static int32_t RadioComBridgeStart(void)
 		// Start the tasks
 		if(PIOS_COM_TRANS_COM)
 			xTaskCreate(transparentCommTask, (signed char *)"transparentComm", STACK_SIZE_BYTES, NULL, TASK_PRIORITY + 2, &(data->transparentCommTaskHandle));
-		xTaskCreate(radioReceiveTask, (signed char *)"RadioReceive", STACK_SIZE_BYTES, NULL, TASK_PRIORITY+ 2, &(data->radioReceiveTaskHandle));
-		xTaskCreate(sendPacketTask, (signed char *)"SendPacketTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY + 2, &(data->sendPacketTaskHandle));
-		xTaskCreate(radioStatusTask, (signed char *)"RadioStatus", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->radioStatusTaskHandle));
 		if(PIOS_PPM_RECEIVER)
 			xTaskCreate(ppmInputTask, (signed char *)"PPMInputTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY + 2, &(data->ppmInputTaskHandle));
+
 #ifdef PIOS_INCLUDE_WDG
 		PIOS_WDG_RegisterFlag(PIOS_WDG_COMGCS);
 		if(PIOS_COM_UAVTALK)
 			PIOS_WDG_RegisterFlag(PIOS_WDG_COMUAVTALK);
 		if(PIOS_COM_TRANS_COM)
 			PIOS_WDG_RegisterFlag(PIOS_WDG_TRANSCOMM);
-		PIOS_WDG_RegisterFlag(PIOS_WDG_RADIORECEIVE);
-		//PIOS_WDG_RegisterFlag(PIOS_WDG_SENDPACKET);
 		if(PIOS_PPM_RECEIVER)
 			PIOS_WDG_RegisterFlag(PIOS_WDG_PPMINPUT);
 #endif
@@ -247,76 +215,47 @@ static int32_t RadioComBridgeInitialize(void)
 	data->GCSUAVTalkCon = UAVTalkInitialize(&GCSUAVTalkSendHandler);
 	if (PIOS_COM_UAVTALK)
 		data->UAVTalkCon = UAVTalkInitialize(&UAVTalkSendHandler);
+	else
+		data->UAVTalkCon = 0;
 
 	// Initialize the queues.
-	data->ppmOutQueue = 0;
-	data->radioPacketQueue = xQueueCreate(PACKET_QUEUE_SIZE, sizeof(UAVObjEvent));
 	data->gcsEventQueue = xQueueCreate(PACKET_QUEUE_SIZE, sizeof(UAVObjEvent));
 	if (PIOS_COM_UAVTALK)
 		data->uavtalkEventQueue = xQueueCreate(PACKET_QUEUE_SIZE, sizeof(UAVObjEvent));
 	else
 	{
 		data->uavtalkEventQueue = 0;
-		data->ppmOutQueue = data->radioPacketQueue;
 	}
 
 	// Initialize the statistics.
-	data->radioTxErrors = 0;
-	data->radioTxRetries = 0;
-	data->radioRxErrors = 0;
 	data->comTxErrors = 0;
 	data->comTxRetries = 0;
-	data->comRxErrors = 0;
 	data->UAVTalkErrors = 0;
-	data->packetErrors = 0;
 	data->RSSI = -127;
 
-	// Register the callbacks with the packet handler
-	PHRegisterOutputStream(pios_packet_handler, transmitPacket);
-	PHRegisterDataHandler(pios_packet_handler, receiveData);
-	PHRegisterPPMHandler(pios_packet_handler, PPMHandler);
-	PHRegisterStatusHandler(pios_packet_handler, StatusHandler);
-
 	// Initialize the packet send timeout
 	data->send_timeout = 25; // ms
 	data->min_packet_size = 50;
 
-	// Initialize the detected device statistics.
-	for (uint8_t i = 0; i < PIPXSTATUS_PAIRIDS_NUMELEM; ++i)
-	{
-		data->pairStats[i].pairID = 0;
-		data->pairStats[i].rssi = -127;
-		data->pairStats[i].retries = 0;
-		data->pairStats[i].errors = 0;
-		data->pairStats[i].uavtalk_errors = 0;
-		data->pairStats[i].resets = 0;
-		data->pairStats[i].dropped = 0;
-		data->pairStats[i].lastContact = 0;
-	}
-	// The first slot is reserved for our current pairID
-	PipXSettingsPairIDGet(&(data->pairStats[0].pairID));
-
 	// Configure our UAVObjects for updates.
 	UAVObjConnectQueue(UAVObjGetByID(PIPXSTATUS_OBJID), data->gcsEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ);
 	UAVObjConnectQueue(UAVObjGetByID(GCSRECEIVER_OBJID), data->uavtalkEventQueue ? data->uavtalkEventQueue : data->gcsEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ);
 	UAVObjConnectQueue(UAVObjGetByID(OBJECTPERSISTENCE_OBJID), data->gcsEventQueue, EV_UPDATED | EV_UPDATED_MANUAL);
 
 	// Initialize the UAVTalk comm parameters.
+	data->gcs_uavtalk_params.isGCS = true;
 	data->gcs_uavtalk_params.UAVTalkCon = data->GCSUAVTalkCon;
-	data->gcs_uavtalk_params.sendQueue = data->radioPacketQueue;
+	data->gcs_uavtalk_params.sendQueue = data->uavtalkEventQueue;
 	data->gcs_uavtalk_params.recvQueue = data->gcsEventQueue;
 	data->gcs_uavtalk_params.wdg = PIOS_WDG_COMGCS;
-	data->gcs_uavtalk_params.checkHID = true;
 	data->gcs_uavtalk_params.comPort = PIOS_COM_GCS;
 	if (PIOS_COM_UAVTALK)
 	{
-		data->gcs_uavtalk_params.sendQueue = data->uavtalkEventQueue;
+		data->uavtalk_params.isGCS = false;
 		data->uavtalk_params.UAVTalkCon = data->UAVTalkCon;
-		data->uavtalk_params.sendQueue = data->radioPacketQueue;
+		data->uavtalk_params.sendQueue = data->gcsEventQueue;
 		data->uavtalk_params.recvQueue = data->uavtalkEventQueue;
-		data->uavtalk_params.gcsQueue = data->gcsEventQueue;
 		data->uavtalk_params.wdg = PIOS_WDG_COMUAVTALK;
-		data->uavtalk_params.checkHID = false;
 		data->uavtalk_params.comPort = PIOS_COM_UAVTALK;
 	}
 
@@ -336,6 +275,8 @@ static void UAVTalkRecvTask(void *parameters)
 	BufferedReadHandle f = BufferedReadInit(params->comPort, TEMP_BUFFER_SIZE);
 
 	while (1) {
+		bool HID_available = false;
+		xQueueHandle sendQueue = 0;
 
 #ifdef PIOS_INCLUDE_WDG
 		// Update the watchdog timer.
@@ -343,13 +284,16 @@ static void UAVTalkRecvTask(void *parameters)
 			PIOS_WDG_UpdateFlag(params->wdg);
 #endif /* PIOS_INCLUDE_WDG */
 
-		// Receive from USB HID if available, otherwise UAVTalk com if it's available.
 #if defined(PIOS_INCLUDE_USB)
-		// Determine input port (USB takes priority over telemetry port)
-		if (params->checkHID && PIOS_USB_CheckAvailable(0))
+		// Is USB plugged in?
+		if (PIOS_USB_CheckAvailable(0))
+			HID_available = true;
+#endif /* PIOS_INCLUDE_USB */
+
+		// Receive from USB HID if available, otherwise UAVTalk com if it's available.
+		if (params->isGCS && HID_available)
 			BufferedReadSetCom(f, PIOS_COM_USB_HID);
 		else
-#endif /* PIOS_INCLUDE_USB */
 		{
 			if (params->comPort)
 				BufferedReadSetCom(f, params->comPort);
@@ -360,9 +304,14 @@ static void UAVTalkRecvTask(void *parameters)
 			}
 		}
 
+		// Send packets to the UAVTalk port if this is a GCS connction and UAVTalk port is configured
+                // or to the GCS port if this is a UAVTalk connection and USB is plugged in.
+		if ((params->isGCS && data->UAVTalkCon) || (!params->isGCS && HID_available))
+			sendQueue = params->sendQueue;
+
 		// Read the next byte
 		uint8_t rx_byte;
-		if(!BufferedRead(f, &rx_byte, MAX_PORT_DELAY))
+		if (!BufferedRead(f, &rx_byte, MAX_PORT_DELAY))
 			continue;
 
 		// Get a TX packet from the packet handler if required.
@@ -386,7 +335,6 @@ static void UAVTalkRecvTask(void *parameters)
 
 			// Initialize the packet.
 			p->header.destination_id = data->destination_id;
-			p->header.source_id = PIOS_RFM22B_DeviceID(pios_rfm22b_id);
 			p->header.type = PACKET_TYPE_DATA;
 			p->data[0] = rx_byte;
 			p->header.data_size = 1;
@@ -403,13 +351,6 @@ static void UAVTalkRecvTask(void *parameters)
 
 		if (state == UAVTALK_STATE_COMPLETE)
 		{
-			xQueueHandle sendQueue = params->sendQueue;
-#if defined(PIOS_INCLUDE_USB)
-			if (params->gcsQueue)
-				if (PIOS_USB_CheckAvailable(0) && PIOS_COM_USB_HID)
-					sendQueue = params->gcsQueue;
-#endif /* PIOS_INCLUDE_USB */
-
 			// Is this a local UAVObject?
 			// We only generate GcsReceiver ojects, we don't consume them.
 			if ((iproc->obj != NULL) && (iproc->objId != GCSRECEIVER_OBJID))
@@ -486,7 +427,10 @@ static void UAVTalkRecvTask(void *parameters)
 					else
 					{
 						// Otherwise, queue the packet for transmission.
-						queueEvent(sendQueue, (void*)p, 0, EV_TRANSMIT_PACKET);
+						if (sendQueue)
+							queueEvent(sendQueue, (void*)p, 0, EV_TRANSMIT_PACKET);
+						else
+							PHTransmitPacket(PIOS_PACKET_HANDLER, p);
 					}
 				}
 				else
@@ -515,17 +459,14 @@ static void UAVTalkRecvTask(void *parameters)
 			else
 			{
 				// Queue the packet for transmission.
-				queueEvent(sendQueue, (void*)p, 0, EV_TRANSMIT_PACKET);
+				if (sendQueue)
+					queueEvent(sendQueue, (void*)p, 0, EV_TRANSMIT_PACKET);
+				else
+					PHTransmitPacket(PIOS_PACKET_HANDLER, p);
 			}
 			p = NULL;
 
 		} else if(state == UAVTALK_STATE_ERROR) {
-			xQueueHandle sendQueue = params->sendQueue;
-#if defined(PIOS_INCLUDE_USB)
-			if (params->gcsQueue)
-				if (PIOS_USB_CheckAvailable(0) && PIOS_COM_USB_HID)
-					sendQueue = params->gcsQueue;
-#endif /* PIOS_INCLUDE_USB */
 			data->UAVTalkErrors++;
 
 			// Send a NACK if required.
@@ -540,84 +481,16 @@ static void UAVTalkRecvTask(void *parameters)
 			else
 			{
 				// Transmit the packet anyway...
-				queueEvent(sendQueue, (void*)p, 0, EV_TRANSMIT_PACKET);
+				if (sendQueue)
+					queueEvent(sendQueue, (void*)p, 0, EV_TRANSMIT_PACKET);
+				else
+					PHTransmitPacket(PIOS_PACKET_HANDLER, p);
 			}
 			p = NULL;
 		}
 	}
 }
 
-/**
- * The radio to com bridge task.
- */
-static void radioReceiveTask(void *parameters)
-{
-	PHPacketHandle p = NULL;
-
-	/* Handle radio -> usart/usb direction */
-	while (1) {
-		uint32_t rx_bytes;
-
-#ifdef PIOS_INCLUDE_WDG
-		// Update the watchdog timer.
-		PIOS_WDG_UpdateFlag(PIOS_WDG_RADIORECEIVE);
-#endif /* PIOS_INCLUDE_WDG */
-
-		// Get a RX packet from the packet handler if required.
-		if (p == NULL)
-			p = PHGetRXPacket(pios_packet_handler);
-
-		if(p == NULL) {
-			DEBUG_PRINTF(2, "RX Packet Unavailable.!\n\r");
-			// Wait a bit for a packet to come available.
-			vTaskDelay(5);
-			continue;
-		}
-
-		// Receive data from the radio port
-		rx_bytes = PIOS_COM_ReceiveBuffer(PIOS_COM_RADIO, (uint8_t*)p, PIOS_PH_MAX_PACKET, MAX_PORT_DELAY);
-		if(rx_bytes == 0)
-			continue;
-		data->rxBytes += rx_bytes;
-
-		// Verify that the packet is valid and pass it on.
-		if(PHVerifyPacket(pios_packet_handler, p, rx_bytes) > 0) {
-			UAVObjEvent ev;
-			ev.obj = (UAVObjHandle)p;
-			ev.event = EV_PACKET_RECEIVED;
-			xQueueSend(data->gcsEventQueue, &ev, portMAX_DELAY);
-		} else
-		{
-			data->packetErrors++;
-			PHReceivePacket(pios_packet_handler, p, true);
-		}
-		p = NULL;
-	}
-}
-
-/**
- * Send packets to the radio.
- */
-static void sendPacketTask(void *parameters)
-{
-	UAVObjEvent ev;
-
-	// Loop forever
-	while (1) {
-#ifdef PIOS_INCLUDE_WDG
-		// Update the watchdog timer.
-		//PIOS_WDG_UpdateFlag(PIOS_WDG_SENDPACKET);
-#endif /* PIOS_INCLUDE_WDG */
-		// Wait for a packet on the queue.
-		if (xQueueReceive(data->radioPacketQueue, &ev, MAX_PORT_DELAY) == pdTRUE) {
-			PHPacketHandle p = (PHPacketHandle)ev.obj;
-			// Send the packet.
-			if(!PHTransmitPacket(pios_packet_handler, p))
-				PHReleaseRXPacket(pios_packet_handler, p);
-		}
-	}
-}
-
 /**
  * Send packets to the com port.
  */
@@ -675,13 +548,13 @@ static void UAVTalkSendTask(void *parameters)
 			else if(ev.event == EV_PACKET_RECEIVED)
 			{
 				// Receive the packet.
-				PHReceivePacket(pios_packet_handler, (PHPacketHandle)ev.obj, false);
+				PHReceivePacket(pios_packet_handler, (PHPacketHandle)ev.obj);
 			}
 			else if(ev.event == EV_TRANSMIT_PACKET)
 			{
 				// Transmit the packet.
 				PHPacketHandle p = (PHPacketHandle)ev.obj;
-				transmitData(params->comPort, p->data, p->header.data_size, params->checkHID);
+				UAVTalkSendBuf(params->UAVTalkCon, p->data, p->header.data_size);
 				PHReleaseTXPacket(pios_packet_handler, p);
 			}
 		}
@@ -721,7 +594,6 @@ static void transparentCommTask(void * parameters)
 
 			// Initialize the packet.
 			p->header.destination_id = data->destination_id;
-			p->header.source_id = PIOS_RFM22B_DeviceID(pios_rfm22b_id);
 			//p->header.type = PACKET_TYPE_ACKED_DATA;
 			p->header.type = PACKET_TYPE_DATA;
 			p->header.data_size = 0;
@@ -760,7 +632,7 @@ static void transparentCommTask(void * parameters)
 			if (send_packet)
 			{
 				// Queue the packet for transmission.
-				queueEvent(data->radioPacketQueue, (void*)p, 0, EV_TRANSMIT_PACKET);
+				PHTransmitPacket(PIOS_PACKET_HANDLER, p);
 
 				// Reset the timeout
 				timeout = MAX_PORT_DELAY;
@@ -771,97 +643,6 @@ static void transparentCommTask(void * parameters)
 	}
 }
 
-/**
- * The stats update task.
- */
-static void radioStatusTask(void *parameters)
-{
-	PHStatusPacket status_packet;
-
-	while (1) {
-		PipXStatusData pipxStatus;
-		uint32_t pairID;
-
-		// Get object data
-		PipXStatusGet(&pipxStatus);
-		PipXSettingsPairIDGet(&pairID);
-
-		// Update the status
-		pipxStatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id);
-		pipxStatus.Retries = data->comTxRetries;
-		pipxStatus.Errors = data->packetErrors;
-		pipxStatus.UAVTalkErrors = data->UAVTalkErrors;
-		pipxStatus.Dropped = data->droppedPackets;
-		pipxStatus.Resets = PIOS_RFM22B_Resets(pios_rfm22b_id);
-		pipxStatus.TXRate = (uint16_t)((float)(data->txBytes * 1000) / STATS_UPDATE_PERIOD_MS);
-		data->txBytes = 0;
-		pipxStatus.RXRate = (uint16_t)((float)(data->rxBytes * 1000) / STATS_UPDATE_PERIOD_MS);
-		data->rxBytes = 0;
-		pipxStatus.LinkState = PIPXSTATUS_LINKSTATE_DISCONNECTED;
-		pipxStatus.RSSI = data->RSSI;
-		LINK_LED_OFF;
-
-		// Update the potential pairing contacts
-		for (uint8_t i = 0; i < PIPXSTATUS_PAIRIDS_NUMELEM; ++i)
-		{
-			pipxStatus.PairIDs[i] = data->pairStats[i].pairID;
-			pipxStatus.PairSignalStrengths[i] = data->pairStats[i].rssi;
-			data->pairStats[i].lastContact++;
-			// Remove this device if it's stale.
-			if(data->pairStats[i].lastContact > MAX_LOST_CONTACT_TIME)
-			{
-				data->pairStats[i].pairID = 0;
-				data->pairStats[i].rssi = -127;
-				data->pairStats[i].retries = 0;
-				data->pairStats[i].errors = 0;
-				data->pairStats[i].uavtalk_errors = 0;
-				data->pairStats[i].resets = 0;
-				data->pairStats[i].dropped = 0;
-				data->pairStats[i].lastContact = 0;
-			}
-			// Add the paired devices statistics to ours.
-			if(pairID && (data->pairStats[i].pairID == pairID) && (data->pairStats[i].rssi > -127))
-			{
-				pipxStatus.Retries += data->pairStats[i].retries;
-				pipxStatus.Errors += data->pairStats[i].errors;
-				pipxStatus.UAVTalkErrors += data->pairStats[i].uavtalk_errors;
-				pipxStatus.Dropped += data->pairStats[i].dropped;
-				pipxStatus.Resets += data->pairStats[i].resets;
-				pipxStatus.Dropped += data->pairStats[i].dropped;
-				pipxStatus.LinkState = PIPXSTATUS_LINKSTATE_CONNECTED;
-				LINK_LED_ON;
-			}
-		}
-
-		// Update the object
-		PipXStatusSet(&pipxStatus);
-
-		// Broadcast the status.
-		{
-			static uint16_t cntr = 0;
-			if(cntr++ == RADIOSTATS_UPDATE_PERIOD_MS / STATS_UPDATE_PERIOD_MS)
-			{
-				// Queue the status message
-				status_packet.header.destination_id = 0xffffffff;
-				status_packet.header.type = PACKET_TYPE_STATUS;
-				status_packet.header.data_size = PH_STATUS_DATA_SIZE(&status_packet);
-				status_packet.header.source_id = pipxStatus.DeviceID;
-				status_packet.retries = data->comTxRetries;
-				status_packet.errors = data->packetErrors;
-				status_packet.uavtalk_errors = data->UAVTalkErrors;
-				status_packet.dropped = data->droppedPackets;
-				status_packet.resets = PIOS_RFM22B_Resets(pios_rfm22b_id);
-				PHPacketHandle sph = (PHPacketHandle)&status_packet;
-				queueEvent(data->radioPacketQueue, (void*)sph, 0, EV_TRANSMIT_PACKET);
-				cntr = 0;
-			}
-		}
-
-		// Delay until the next update period.
-		vTaskDelay(STATS_UPDATE_PERIOD_MS / portTICK_RATE_MS);
-	}
-}
-
 /**
  * The PPM input task.
  */
@@ -878,20 +659,38 @@ static void ppmInputTask(void *parameters)
 #endif /* PIOS_INCLUDE_WDG */
 
 		// Read the receiver.
+		bool valid_input_detected = false;
 		for (uint8_t i = 1; i <= PIOS_PPM_NUM_INPUTS; ++i)
-			ppm_packet.channels[i - 1] = PIOS_RCVR_Read(PIOS_PPM_RECEIVER, i);
-
-		// Send the PPM packet
-		if (data->ppmOutQueue)
 		{
-			ppm_packet.header.destination_id = data->destination_id;
-			ppm_packet.header.source_id = PIOS_RFM22B_DeviceID(pios_rfm22b_id);
-			ppm_packet.header.type = PACKET_TYPE_PPM;
-			ppm_packet.header.data_size = PH_PPM_DATA_SIZE(&ppm_packet);
-			queueEvent(data->ppmOutQueue, (void*)pph, 0, EV_TRANSMIT_PACKET);
+			ppm_packet.channels[i - 1] = PIOS_RCVR_Read(PIOS_PPM_RECEIVER, i);
+			if(ppm_packet.channels[i - 1] != PIOS_RCVR_TIMEOUT)
+				valid_input_detected = true;
+		}
+
+		// Send the PPM packet if it's valid
+		if (valid_input_detected)
+		{
+			// Set the GCSReceiver UAVO if we're connected to the FC.
+			if (data->UAVTalkCon)
+			{
+				GCSReceiverData rcvr;
+
+				// Copy the receiver channels into the GCSReceiver object.
+				for (uint8_t i = 0; i < GCSRECEIVER_CHANNEL_NUMELEM; ++i)
+					rcvr.Channel[i] = ppm_packet.channels[i];
+
+				// Set the GCSReceiverData object.
+				GCSReceiverSet(&rcvr);
+			}
+			else
+			{
+				// Otherwise, send a PPM packet over the radio link.
+				ppm_packet.header.destination_id = data->destination_id;
+				ppm_packet.header.type = PACKET_TYPE_PPM;
+				ppm_packet.header.data_size = PH_PPM_DATA_SIZE(&ppm_packet);
+				PHTransmitPacket(PIOS_PACKET_HANDLER, pph);
+			}
 		}
-		else
-			PPMHandler(ppm_packet.channels);
 
 		// Delay until the next update period.
 		vTaskDelay(PIOS_PPM_PACKET_UPDATE_PERIOD_MS / portTICK_RATE_MS);
@@ -910,7 +709,7 @@ static int32_t UAVTalkSend(UAVTalkComTaskParams *params, uint8_t *buf, int32_t l
 {
 	uint32_t outputPort = params->comPort;
 #if defined(PIOS_INCLUDE_USB)
-	if (params->checkHID)
+	if (params->isGCS)
 	{
 		// Determine output port (USB takes priority over telemetry port)
 		if (PIOS_USB_CheckAvailable(0) && PIOS_COM_USB_HID)
@@ -918,7 +717,7 @@ static int32_t UAVTalkSend(UAVTalkComTaskParams *params, uint8_t *buf, int32_t l
 	}
 #endif /* PIOS_INCLUDE_USB */
 	if(outputPort)
-		return PIOS_COM_SendBuffer(outputPort, buf, length);
+		return PIOS_COM_SendBufferNonBlocking(outputPort, buf, length);
 	else
 		return -1;
 }
@@ -947,19 +746,6 @@ static int32_t GCSUAVTalkSendHandler(uint8_t *buf, int32_t length)
 	return UAVTalkSend(&(data->gcs_uavtalk_params), buf, length);
 }
 
-/**
- * Transmit a packet to the radio port.
- * \param[in] buf Data buffer to send
- * \param[in] length Length of buffer
- * \return -1 on failure
- * \return number of bytes transmitted on success
- */
-static int32_t transmitPacket(PHPacketHandle p)
-{
-	data->txBytes += PH_PACKET_SIZE(p);
-	return PIOS_COM_SendBuffer(PIOS_COM_RADIO, (uint8_t*)p, PH_PACKET_SIZE(p));
-}
-
 /**
  * Receive a packet
  * \param[in] buf The received data buffer
@@ -993,85 +779,13 @@ static void receiveData(uint8_t *buf, uint8_t len, int8_t rssi, int8_t afc)
 	data->RSSI = rssi;
 
 	// Packet data should go to transparent com if it's configured,
-	// USB HID if it's connected, otherwise, UAVTalk com if it's configured.
-	uint32_t outputPort = PIOS_COM_TRANS_COM ? PIOS_COM_TRANS_COM : PIOS_COM_UAVTALK;
-	bool checkHid = (PIOS_COM_TRANS_COM == 0);
-	transmitData(outputPort, buf, len, checkHid);
-}
-
-/**
- * Receive a status packet
- * \param[in] status The status structure
- */
-static void StatusHandler(PHStatusPacketHandle status, int8_t rssi, int8_t afc)
-{
-	uint32_t id = status->header.source_id;
-	bool found = false;
-	// Have we seen this device recently?
-	uint8_t id_idx = 0;
-	for ( ; id_idx < PIPXSTATUS_PAIRIDS_NUMELEM; ++id_idx)
-		if(data->pairStats[id_idx].pairID == id)
-		{
-			found = true;
-			break;
-		}
-
-	// If we have seen it, update the RSSI and reset the last contact couter
-	if(found)
-	{
-		data->pairStats[id_idx].rssi = rssi;
-		data->pairStats[id_idx].retries = status->retries;
-		data->pairStats[id_idx].errors = status->errors;
-		data->pairStats[id_idx].uavtalk_errors = status->uavtalk_errors;
-		data->pairStats[id_idx].resets = status->resets;
-		data->pairStats[id_idx].dropped = status->dropped;
-		data->pairStats[id_idx].lastContact = 0;
-	}
-
-	// If we haven't seen it, find a slot to put it in.
-	if (!found)
-	{
-		uint32_t pairID;
-		PipXSettingsPairIDGet(&pairID);
-
-		uint8_t min_idx = 0;
-		if(id != pairID)
-		{
-			int8_t min_rssi = data->pairStats[0].rssi;
-			for (id_idx = 1; id_idx < PIPXSTATUS_PAIRIDS_NUMELEM; ++id_idx)
-			{
-				if(data->pairStats[id_idx].rssi < min_rssi)
-				{
-					min_rssi = data->pairStats[id_idx].rssi;
-					min_idx = id_idx;
-				}
-			}
-		}
-		data->pairStats[min_idx].pairID = id;
-		data->pairStats[min_idx].rssi = rssi;
-		data->pairStats[min_idx].retries = status->retries;
-		data->pairStats[min_idx].errors = status->errors;
-		data->pairStats[min_idx].uavtalk_errors = status->uavtalk_errors;
-		data->pairStats[min_idx].resets = status->resets;
-		data->pairStats[min_idx].dropped = status->dropped;
-		data->pairStats[min_idx].lastContact = 0;
-	}
-}
-
-/**
- * Receive a ppm packet
- * \param[in] channels The ppm channels
- */
-static void PPMHandler(uint16_t *channels)
-{
-	GCSReceiverData rcvr;
-
-	// Copy the receiver channels into the GCSReceiver object.
-	for (uint8_t i = 0; i < GCSRECEIVER_CHANNEL_NUMELEM; ++i)
-		rcvr.Channel[i] = channels[i];
-
-	// Set the GCSReceiverData object.
-	GCSReceiverSet(&rcvr);
+	// or just send it through the UAVTalk link.
+	if (PIOS_COM_TRANS_COM)
+		transmitData(PIOS_COM_TRANS_COM, buf, len, false);
+	else if (data->UAVTalkCon)
+		UAVTalkSendBuf(data->UAVTalkCon, buf, len);
+	else
+		UAVTalkSendBuf(data->GCSUAVTalkCon, buf, len);
 }
 
 static BufferedReadHandle BufferedReadInit(uint32_t com_port, uint16_t buffer_length)

flight/Modules/Sensors/sensors.c

@@ -47,8 +47,9 @@
  */
 
 #include "pios.h"
-#include "attitude.h"
+#include "homelocation.h"
 #include "magnetometer.h"
+#include "magbias.h"
 #include "accels.h"
 #include "gyros.h"
 #include "gyrosbias.h"
@@ -56,14 +57,12 @@
 #include "attitudesettings.h"
 #include "revocalibration.h"
 #include "flightstatus.h"
-#include "gpsposition.h"
-#include "baroaltitude.h"
 #include "CoordinateConversions.h"
 
 #include <pios_board_info.h>
 
 // Private constants
-#define STACK_SIZE_BYTES 700
+#define STACK_SIZE_BYTES 1000
 #define TASK_PRIORITY (tskIDLE_PRIORITY+3)
 #define SENSOR_PERIOD 2
 
@@ -71,15 +70,15 @@
 #define PI_MOD(x) (fmodf(x + F_PI, F_PI * 2) - F_PI)
 // Private types
 
-// Private variables
-static xTaskHandle sensorsTaskHandle;
-static bool gps_updated = false;
-static bool baro_updated = false;
 
 // Private functions
 static void SensorsTask(void *parameters);
 static void settingsUpdatedCb(UAVObjEvent * objEv);
-static void sensorsUpdatedCb(UAVObjEvent * objEv);
+static void magOffsetEstimation(MagnetometerData *mag);
+
+// Private variables
+static xTaskHandle sensorsTaskHandle;
+RevoCalibrationData cal;
 
 // These values are initialized by settings but can be updated by the attitude algorithm
 static bool bias_correct_gyro = true;
@@ -88,8 +87,6 @@ static float mag_bias[3] = {0,0,0};
 static float mag_scale[3] = {0,0,0};
 static float accel_bias[3] = {0,0,0};
 static float accel_scale[3] = {0,0,0};
-static float gyro_bias[3] = {0,0,0};
-static float gyro_scale[3] = {0,0,0};
 
 static float R[3][3] = {{0}};
 static int8_t rotate = 0;
@@ -113,6 +110,7 @@ int32_t SensorsInitialize(void)
 	GyrosBiasInitialize();
 	AccelsInitialize();
 	MagnetometerInitialize();
+	MagBiasInitialize();
 	RevoCalibrationInitialize();
 	AttitudeSettingsInitialize();
 
@@ -208,12 +206,6 @@ static void SensorsTask(void *parameters)
 		}
 	}
 	
-	// If debugging connect callback
-	if(pios_com_aux_id != 0) {
-		BaroAltitudeConnectCallback(&sensorsUpdatedCb);
-		GPSPositionConnectCallback(&sensorsUpdatedCb);
-	}
-	
 	// Main task loop
 	lastSysTime = xTaskGetTickCount();
 	bool error = false;
@@ -233,8 +225,6 @@ static void SensorsTask(void *parameters)
 			AlarmsClear(SYSTEMALARMS_ALARM_SENSORS);
 		}
 
-		int32_t read_good;
-		int32_t count;
 
 		for (int i = 0; i < 3; i++) {
 			accel_accum[i] = 0;
@@ -252,6 +242,9 @@ static void SensorsTask(void *parameters)
 			{
 				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) ? true : error;
@@ -266,9 +259,9 @@ static void SensorsTask(void *parameters)
 				while(read_good == 0) {	
 					count++;
 					
-					accel_accum[0] += accel.x;
-					accel_accum[1] += accel.y;
-					accel_accum[2] += accel.z;
+					accel_accum[1] += accel.x;
+					accel_accum[0] += accel.y;
+					accel_accum[2] -= accel.z;
 					
 					read_good = PIOS_BMA180_ReadFifo(&accel);
 				}
@@ -291,9 +284,9 @@ static void SensorsTask(void *parameters)
 				}
 				
 				gyro_samples = 1;
-				gyro_accum[0] += gyro.gyro_x;
-				gyro_accum[1] += gyro.gyro_y;
-				gyro_accum[2] += gyro.gyro_z;
+				gyro_accum[1] += gyro.gyro_x;
+				gyro_accum[0] += gyro.gyro_y;
+				gyro_accum[2] -= gyro.gyro_z;
 				
 				gyro_scaling = PIOS_L3GD20_GetScale();
 
@@ -303,6 +296,7 @@ static void SensorsTask(void *parameters)
 #endif
 				break;
 			case 0x02:  // MPU6000 board
+			case 0x03:  // MPU6000 board
 #if defined(PIOS_INCLUDE_MPU6000)
 			{
 				struct pios_mpu6000_data mpu6000_data;
@@ -341,9 +335,9 @@ static void SensorsTask(void *parameters)
 		}
 
 		// Scale the accels
-		float accels[3] = {(float) accel_accum[1] / accel_samples, 
-		                   (float) accel_accum[0] / accel_samples,
-		                  -(float) accel_accum[2] / accel_samples};
+		float accels[3] = {(float) accel_accum[0] / accel_samples, 
+		                   (float) accel_accum[1] / accel_samples,
+		                   (float) accel_accum[2] / accel_samples};
 		float accels_out[3] = {accels[0] * accel_scaling * accel_scale[0] - accel_bias[0],
 		                       accels[1] * accel_scaling * accel_scale[1] - accel_bias[1],
 		                       accels[2] * accel_scaling * accel_scale[2] - accel_bias[2]};
@@ -360,12 +354,12 @@ static void SensorsTask(void *parameters)
 		AccelsSet(&accelsData);
 
 		// Scale the gyros
-		float gyros[3] = {(float) gyro_accum[1] / gyro_samples,
-		                  (float) gyro_accum[0] / gyro_samples,
-		                 -(float) gyro_accum[2] / gyro_samples};
-		float gyros_out[3] = {gyros[0] * gyro_scaling * gyro_scale[0] - gyro_bias[0],
-		                      gyros[1] * gyro_scaling * gyro_scale[1] - gyro_bias[1],
-		                      gyros[2] * gyro_scaling * gyro_scale[2] - gyro_bias[2]};
+		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,
+		                      gyros[1] * gyro_scaling,
+		                      gyros[2] * gyro_scaling};
 		if (rotate) {
 			rot_mult(R, gyros_out, gyros);
 			gyrosData.x = gyros[0];
@@ -378,12 +372,12 @@ static void SensorsTask(void *parameters)
 		}
 		
 		if (bias_correct_gyro) {
-			// Apply bias correction to the gyros
+			// Apply bias correction to the gyros from the state estimator
 			GyrosBiasData gyrosBias;
 			GyrosBiasGet(&gyrosBias);
-			gyrosData.x += gyrosBias.x;
-			gyrosData.y += gyrosBias.y;
-			gyrosData.z += gyrosBias.z;
+			gyrosData.x -= gyrosBias.x;
+			gyrosData.y -= gyrosBias.y;
+			gyrosData.z -= gyrosBias.z;
 		}
 		GyrosSet(&gyrosData);
 		
@@ -409,6 +403,11 @@ static void SensorsTask(void *parameters)
 				mag.y = mags[1];
 				mag.z = mags[2];
 			}
+			
+			// Correct for mag bias and update if the rate is non zero
+			if(cal.MagBiasNullingRate > 0)
+				magOffsetEstimation(&mag);
+
 			MagnetometerSet(&mag);
 			mag_update_time = PIOS_DELAY_GetRaw();
 		}
@@ -421,21 +420,108 @@ static void SensorsTask(void *parameters)
 }
 
 /**
- * Indicate that these sensors have been updated
+ * Perform an update of the @ref MagBias based on
+ * Magnetometer Offset Cancellation: Theory and Implementation, 
+ * revisited William Premerlani, October 14, 2011
  */
-static void sensorsUpdatedCb(UAVObjEvent * objEv)
+static void magOffsetEstimation(MagnetometerData *mag)
 {
-	if(objEv->obj == GPSPositionHandle())
-		gps_updated = true;
-	if(objEv->obj == BaroAltitudeHandle())
-		baro_updated = true;
+#if 0
+	// Constants, to possibly go into a UAVO
+	static const float MIN_NORM_DIFFERENCE = 50;
+
+	static float B2[3] = {0, 0, 0};
+
+	MagBiasData magBias;
+	MagBiasGet(&magBias);
+
+	// Remove the current estimate of the bias
+	mag->x -= magBias.x;
+	mag->y -= magBias.y;
+	mag->z -= magBias.z;
+
+	// First call
+	if (B2[0] == 0 && B2[1] == 0 && B2[2] == 0) {
+		B2[0] = mag->x;
+		B2[1] = mag->y;
+		B2[2] = mag->z;
+		return;
+	}
+
+	float B1[3] = {mag->x, mag->y, mag->z};
+	float norm_diff = sqrtf(powf(B2[0] - B1[0],2) + powf(B2[1] - B1[1],2) + powf(B2[2] - B1[2],2));
+	if (norm_diff > MIN_NORM_DIFFERENCE) {
+		float norm_b1 = sqrtf(B1[0]*B1[0] + B1[1]*B1[1] + B1[2]*B1[2]);
+		float norm_b2 = sqrtf(B2[0]*B2[0] + B2[1]*B2[1] + B2[2]*B2[2]);
+		float scale = cal.MagBiasNullingRate * (norm_b2 - norm_b1) / norm_diff;
+		float b_error[3] = {(B2[0] - B1[0]) * scale, (B2[1] - B1[1]) * scale, (B2[2] - B1[2]) * scale};
+
+		magBias.x += b_error[0];
+		magBias.y += b_error[1];
+		magBias.z += b_error[2];
+
+		MagBiasSet(&magBias);
+
+		// Store this value to compare against next update
+		B2[0] = B1[0]; B2[1] = B1[1]; B2[2] = B1[2];
+	}
+#else
+	MagBiasData magBias;
+	MagBiasGet(&magBias);
+	
+	// Remove the current estimate of the bias
+	mag->x -= magBias.x;
+	mag->y -= magBias.y;
+	mag->z -= magBias.z;
+	
+	HomeLocationData homeLocation;
+	HomeLocationGet(&homeLocation);
+	
+	AttitudeActualData attitude;
+	AttitudeActualGet(&attitude);
+	
+	const float Rxy = sqrtf(homeLocation.Be[0]*homeLocation.Be[0] + homeLocation.Be[1]*homeLocation.Be[1]);
+	const float Rz = homeLocation.Be[2];
+	
+	const float rate = cal.MagBiasNullingRate;
+	float R[3][3];
+	float B_e[3];
+	float xy[2];
+	float delta[3];
+	
+	// Get the rotation matrix
+	Quaternion2R(&attitude.q1, R);
+	
+	// Rotate the mag into the NED frame
+	B_e[0] = R[0][0] * mag->x + R[1][0] * mag->y + R[2][0] * mag->z;
+	B_e[1] = R[0][1] * mag->x + R[1][1] * mag->y + R[2][1] * mag->z;
+	B_e[2] = R[0][2] * mag->x + R[1][2] * mag->y + R[2][2] * mag->z;
+	
+	float cy = cosf(attitude.Yaw * M_PI / 180.0f);
+	float sy = sinf(attitude.Yaw * M_PI / 180.0f);
+	
+	xy[0] =  cy * B_e[0] + sy * B_e[1];
+	xy[1] = -sy * B_e[0] + cy * B_e[1];
+	
+	float xy_norm = sqrtf(xy[0]*xy[0] + xy[1]*xy[1]);
+	
+	delta[0] = -rate * (xy[0] / xy_norm * Rxy - xy[0]);
+	delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]);
+	delta[2] = -rate * (Rz - B_e[2]);
+	
+	if (delta[0] == delta[0] && delta[1] == delta[1] && delta[2] == delta[2]) {		
+		magBias.x += delta[0];
+		magBias.y += delta[1];
+		magBias.z += delta[2];
+		MagBiasSet(&magBias);
+	}
+#endif
 }
 
 /**
  * Locally cache some variables from the AtttitudeSettings object
  */
 static void settingsUpdatedCb(UAVObjEvent * objEv) {
-	RevoCalibrationData cal;
 	RevoCalibrationGet(&cal);
 	
 	mag_bias[0] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_X];
@@ -450,15 +536,21 @@ static void settingsUpdatedCb(UAVObjEvent * objEv) {
 	accel_scale[0] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_X];
 	accel_scale[1] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Y];
 	accel_scale[2] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Z];
-	gyro_bias[0] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_X];
-	gyro_bias[1] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Y];
-	gyro_bias[2] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Z];
-	gyro_scale[0] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_X];
-	gyro_scale[1] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_Y];
-	gyro_scale[2] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_Z];
+	// Do not store gyros_bias here as that comes from the state estimator and should be
+	// used from GyroBias directly
+	
+	// Zero out any adaptive tracking
+	MagBiasData magBias;
+	MagBiasGet(&magBias);
+	magBias.x = 0;
+	magBias.y = 0;
+	magBias.z = 0;
+	MagBiasSet(&magBias);
+	
 
 	AttitudeSettingsData attitudeSettings;
 	AttitudeSettingsGet(&attitudeSettings);
+	bias_correct_gyro = (cal.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE);
 
 	// Indicates not to expend cycles on rotation
 	if(attitudeSettings.BoardRotation[0] == 0 && attitudeSettings.BoardRotation[1] == 0 &&

flight/Modules/Sensors/simulated/sensors.c

@@ -54,6 +54,7 @@
 #include "attitudeactual.h"
 #include "attitudesimulated.h"
 #include "attitudesettings.h"
+#include "baroairspeed.h"
 #include "baroaltitude.h"
 #include "gyros.h"
 #include "gyrosbias.h"
@@ -62,8 +63,10 @@
 #include "gpsvelocity.h"
 #include "homelocation.h"
 #include "magnetometer.h"
+#include "magbias.h"
 #include "ratedesired.h"
 #include "revocalibration.h"
+#include "systemsettings.h"
 
 #include "CoordinateConversions.h"
 
@@ -84,13 +87,17 @@ static void SensorsTask(void *parameters);
 static void simulateConstant();
 static void simulateModelAgnostic();
 static void simulateModelQuadcopter();
+static void simulateModelAirplane();
+
+static void magOffsetEstimation(MagnetometerData *mag);
 
 static float accel_bias[3];
 
 static float rand_gauss();
 
-enum sensor_sim_type {CONSTANT, MODEL_AGNOSTIC, MODEL_QUADCOPTER} sensor_sim_type;
+enum sensor_sim_type {CONSTANT, MODEL_AGNOSTIC, MODEL_QUADCOPTER, MODEL_AIRPLANE} sensor_sim_type;
 
+#define GRAV 9.81
 /**
  * Initialise the module.  Called before the start function
  * \returns 0 on success or -1 if initialisation failed
@@ -105,11 +112,13 @@ int32_t SensorsInitialize(void)
 	AccelsInitialize();
 	AttitudeSimulatedInitialize();
 	BaroAltitudeInitialize();
+	BaroAirspeedInitialize();
 	GyrosInitialize();
 	GyrosBiasInitialize();
 	GPSPositionInitialize();
 	GPSVelocityInitialize();
 	MagnetometerInitialize();
+	MagBiasInitialize();
 	RevoCalibrationInitialize();
 
 	return 0;
@@ -152,13 +161,32 @@ static void SensorsTask(void *parameters)
 //	homeLocation.Set = HOMELOCATION_SET_TRUE;
 //	HomeLocationSet(&homeLocation);
 
-	sensor_sim_type = MODEL_QUADCOPTER;
 
 	// Main task loop
 	lastSysTime = xTaskGetTickCount();
 	uint32_t last_time = PIOS_DELAY_GetRaw();
 	while (1) {
 		PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
+
+		SystemSettingsData systemSettings;
+		SystemSettingsGet(&systemSettings);
+
+		switch(systemSettings.AirframeType) {
+			case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING:
+			case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON:
+			case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGVTAIL:
+				sensor_sim_type = MODEL_AIRPLANE;
+				break;
+			case SYSTEMSETTINGS_AIRFRAMETYPE_QUADX:
+			case SYSTEMSETTINGS_AIRFRAMETYPE_QUADP:
+			case SYSTEMSETTINGS_AIRFRAMETYPE_VTOL:
+			case SYSTEMSETTINGS_AIRFRAMETYPE_HEXA:
+			case SYSTEMSETTINGS_AIRFRAMETYPE_OCTO:
+				sensor_sim_type = MODEL_QUADCOPTER;
+				break;
+			default:
+				sensor_sim_type = MODEL_AGNOSTIC;
+		}
 		
 		static int i;
 		i++;
@@ -180,6 +208,8 @@ static void SensorsTask(void *parameters)
 			case MODEL_QUADCOPTER:
 				simulateModelQuadcopter();
 				break;
+			case MODEL_AIRPLANE:
+				simulateModelAirplane();
 		}
 
 		vTaskDelay(2 / portTICK_RATE_MS);
@@ -192,7 +222,7 @@ static void simulateConstant()
 	AccelsData accelsData; // Skip get as we set all the fields
 	accelsData.x = 0;
 	accelsData.y = 0;
-	accelsData.z = -9.81;
+	accelsData.z = -GRAV;
 	accelsData.temperature = 0;
 	AccelsSet(&accelsData);
 
@@ -246,9 +276,9 @@ static void simulateModelAgnostic()
 	Quaternion2R(q,Rbe);
 
 	AccelsData accelsData; // Skip get as we set all the fields
-	accelsData.x = -9.81 * Rbe[0][2];
-	accelsData.y = -9.81 * Rbe[1][2];
-	accelsData.z = -9.81 * Rbe[2][2];
+	accelsData.x = -GRAV * Rbe[0][2];
+	accelsData.y = -GRAV * Rbe[1][2];
+	accelsData.z = -GRAV * Rbe[2][2];
 	accelsData.temperature = 30;
 	AccelsSet(&accelsData);
 
@@ -303,7 +333,7 @@ static void simulateModelQuadcopter()
 	float Rbe[3][3];
 	
 	const float ACTUATOR_ALPHA = 0.8;
-	const float MAX_THRUST = 9.81 * 2;
+	const float MAX_THRUST = GRAV * 2;
 	const float K_FRICTION = 1;
 	const float GPS_PERIOD = 0.1;
 	const float MAG_PERIOD = 1.0 / 75.0;
@@ -391,12 +421,12 @@ static void simulateModelQuadcopter()
 	ned_accel[0] = -thrust * Rbe[2][0];
 	ned_accel[1] = -thrust * Rbe[2][1];
 	// Gravity causes acceleration of 9.81 in the down direction
-	ned_accel[2] = -thrust * Rbe[2][2] + 9.81;
+	ned_accel[2] = -thrust * Rbe[2][2] + GRAV;
 	
 	// Apply acceleration based on velocity
 	ned_accel[0] -= K_FRICTION * (vel[0] - wind[0]);
-	ned_accel[1] -= K_FRICTION * (vel[1] - wind[0]);
-	ned_accel[2] -= K_FRICTION * (vel[2] - wind[0]);
+	ned_accel[1] -= K_FRICTION * (vel[1] - wind[1]);
+	ned_accel[2] -= K_FRICTION * (vel[2] - wind[2]);
 
 	// Predict the velocity forward in time
 	vel[0] = vel[0] + ned_accel[0] * dT;
@@ -497,6 +527,10 @@ static void simulateModelQuadcopter()
 		mag.x = homeLocation.Be[0] * Rbe[0][0] + homeLocation.Be[1] * Rbe[0][1] + homeLocation.Be[2] * Rbe[0][2];
 		mag.y = homeLocation.Be[0] * Rbe[1][0] + homeLocation.Be[1] * Rbe[1][1] + homeLocation.Be[2] * Rbe[1][2];
 		mag.z = homeLocation.Be[0] * Rbe[2][0] + homeLocation.Be[1] * Rbe[2][1] + homeLocation.Be[2] * Rbe[2][2];
+
+		// Run the offset compensation algorithm from the firmware
+		magOffsetEstimation(&mag);
+
 		MagnetometerSet(&mag);
 		last_mag_time = PIOS_DELAY_GetRaw();
 	}
@@ -517,6 +551,285 @@ static void simulateModelQuadcopter()
 	AttitudeSimulatedSet(&attitudeSimulated);
 }
 
+/**
+ * This method performs a simple simulation of a quadcopter
+ * 
+ * It takes in the ActuatorDesired command to rotate the aircraft and performs
+ * a simple kinetic model where the throttle increases the energy and drag decreases
+ * it.  Changing altitude moves energy from kinetic to potential.
+ *
+ * 1. Update attitude based on ActuatorDesired
+ * 2. Update position based on velocity
+ */
+static void simulateModelAirplane()
+{
+	static double pos[3] = {0,0,0};
+	static double vel[3] = {0,0,0};
+	static double ned_accel[3] = {0,0,0};
+	static float q[4] = {1,0,0,0};
+	static float rpy[3] = {0,0,0}; // Low pass filtered actuator
+	static float baro_offset = 0.0f;
+	float Rbe[3][3];
+	
+	const float LIFT_SPEED = 8; // (m/s) where achieve lift for zero pitch
+	const float ACTUATOR_ALPHA = 0.8;
+	const float MAX_THRUST = 9.81 * 2;
+	const float K_FRICTION = 0.2;
+	const float GPS_PERIOD = 0.1;
+	const float MAG_PERIOD = 1.0 / 75.0;
+	const float BARO_PERIOD = 1.0 / 20.0;
+	const float ROLL_HEADING_COUPLING = 0.1; // (deg/s) heading change per deg of roll
+	const float PITCH_THRUST_COUPLING = 0.2; // (m/s^2) of forward acceleration per deg of pitch
+	
+	static uint32_t last_time;
+	
+	float dT = (PIOS_DELAY_DiffuS(last_time) / 1e6);
+	if(dT < 1e-3)
+		dT = 2e-3;
+	last_time = PIOS_DELAY_GetRaw();
+	
+	FlightStatusData flightStatus;
+	FlightStatusGet(&flightStatus);
+	ActuatorDesiredData actuatorDesired;
+	ActuatorDesiredGet(&actuatorDesired);
+	
+	float thrust = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) ? actuatorDesired.Throttle * MAX_THRUST : 0;
+	if (thrust < 0)
+		thrust = 0;
+	
+	if (thrust != thrust)
+		thrust = 0;
+	
+	//	float control_scaling = thrust * thrustToDegs;
+	//	// In rad/s
+	//	rpy[0] = control_scaling * actuatorDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA;
+	//	rpy[1] = control_scaling * actuatorDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA;
+	//	rpy[2] = control_scaling * actuatorDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA;
+	//	
+	//	GyrosData gyrosData; // Skip get as we set all the fields
+	//	gyrosData.x = rpy[0] * 180 / M_PI + rand_gauss();
+	//	gyrosData.y = rpy[1] * 180 / M_PI + rand_gauss();
+	//	gyrosData.z = rpy[2] * 180 / M_PI + rand_gauss();
+	
+	/**** 1. Update attitude ****/
+	RateDesiredData rateDesired;
+	RateDesiredGet(&rateDesired);
+	
+	// Need to get roll angle for easy cross coupling
+	AttitudeActualData attitudeActual;
+	AttitudeActualGet(&attitudeActual);
+	double roll = attitudeActual.Roll;
+	double pitch = attitudeActual.Pitch;
+
+	rpy[0] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA;
+	rpy[1] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA;
+	rpy[2] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA;
+	rpy[2] += roll * ROLL_HEADING_COUPLING;
+	
+
+	GyrosData gyrosData; // Skip get as we set all the fields
+	gyrosData.x = rpy[0] + rand_gauss();
+	gyrosData.y = rpy[1] + rand_gauss();
+	gyrosData.z = rpy[2] + rand_gauss();
+	GyrosSet(&gyrosData);
+	
+	// Predict the attitude forward in time
+	float qdot[4];
+	qdot[0] = (-q[1] * rpy[0] - q[2] * rpy[1] - q[3] * rpy[2]) * dT * M_PI / 180 / 2;
+	qdot[1] = (q[0] * rpy[0] - q[3] * rpy[1] + q[2] * rpy[2]) * dT * M_PI / 180 / 2;
+	qdot[2] = (q[3] * rpy[0] + q[0] * rpy[1] - q[1] * rpy[2]) * dT * M_PI / 180 / 2;
+	qdot[3] = (-q[2] * rpy[0] + q[1] * rpy[1] + q[0] * rpy[2]) * dT * M_PI / 180 / 2;
+	
+	// Take a time step
+	q[0] = q[0] + qdot[0];
+	q[1] = q[1] + qdot[1];
+	q[2] = q[2] + qdot[2];
+	q[3] = q[3] + qdot[3];
+	
+	float qmag = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
+	q[0] = q[0] / qmag;
+	q[1] = q[1] / qmag;
+	q[2] = q[2] / qmag;
+	q[3] = q[3] / qmag;
+	
+	if(overideAttitude){
+		AttitudeActualData attitudeActual;
+		AttitudeActualGet(&attitudeActual);
+		attitudeActual.q1 = q[0];
+		attitudeActual.q2 = q[1];
+		attitudeActual.q3 = q[2];
+		attitudeActual.q4 = q[3];
+		AttitudeActualSet(&attitudeActual);
+	}
+	
+	/**** 2. Update position based on velocity ****/
+	static float wind[3] = {0,0,0};
+	wind[0] = wind[0] * 0.95 + rand_gauss() / 10.0;
+	wind[1] = wind[1] * 0.95 + rand_gauss() / 10.0;
+	wind[2] = wind[2] * 0.95 + rand_gauss() / 10.0;
+	wind[0] = 0;
+	wind[1] = 0;
+	wind[2] = 0;
+	
+	// Rbe takes a vector from body to earth.  If we take (1,0,0)^T through this and then dot with airspeed
+	// we get forward airspeed		
+	Quaternion2R(q,Rbe);
+
+	double airspeed[3] = {vel[0] - wind[0], vel[1] - wind[1], vel[2] - wind[2]};
+	double forwardAirspeed = Rbe[0][0] * airspeed[0] + Rbe[0][1] * airspeed[1] + Rbe[0][2] * airspeed[2];
+	double sidewaysAirspeed = Rbe[1][0] * airspeed[0] + Rbe[1][1] * airspeed[1] + Rbe[1][2] * airspeed[2];
+	double downwardAirspeed = Rbe[2][0] * airspeed[0] + Rbe[2][1] * airspeed[1] + Rbe[2][2] * airspeed[2];
+	
+	/* Compute aerodynamic forces in body referenced frame.  Later use more sophisticated equations  */
+	/* TODO: This should become more accurate.  Use the force equations to calculate lift from the   */
+	/* various surfaces based on AoA and airspeed.  From that compute torques and forces.  For later */
+	double forces[3]; // X, Y, Z
+	forces[0] = thrust - pitch * PITCH_THRUST_COUPLING - forwardAirspeed * K_FRICTION;         // Friction is applied in all directions in NED
+	forces[1] = 0 - sidewaysAirspeed * K_FRICTION * 100;      // No side slip
+	forces[2] = GRAV * (forwardAirspeed - LIFT_SPEED) + downwardAirspeed * K_FRICTION * 100;    // Stupidly simple, always have gravity lift when straight and level
+	
+	// Negate force[2] as NED defines down as possitive, aircraft convention is Z up is positive (?)
+	ned_accel[0] = forces[0] * Rbe[0][0] + forces[1] * Rbe[1][0] - forces[2] * Rbe[2][0];
+	ned_accel[1] = forces[0] * Rbe[0][1] + forces[1] * Rbe[1][1] - forces[2] * Rbe[2][1];
+	ned_accel[2] = forces[0] * Rbe[0][2] + forces[1] * Rbe[1][2] - forces[2] * Rbe[2][2];
+	// Gravity causes acceleration of 9.81 in the down direction
+	ned_accel[2] += 9.81;
+
+	// Apply acceleration based on velocity
+	ned_accel[0] -= K_FRICTION * (vel[0] - wind[0]);
+	ned_accel[1] -= K_FRICTION * (vel[1] - wind[1]);
+	ned_accel[2] -= K_FRICTION * (vel[2] - wind[2]);
+	
+	// Predict the velocity forward in time
+	vel[0] = vel[0] + ned_accel[0] * dT;
+	vel[1] = vel[1] + ned_accel[1] * dT;
+	vel[2] = vel[2] + ned_accel[2] * dT;
+	
+	// Predict the position forward in time
+	pos[0] = pos[0] + vel[0] * dT;
+	pos[1] = pos[1] + vel[1] * dT;
+	pos[2] = pos[2] + vel[2] * dT;
+	
+	// Simulate hitting ground
+	if(pos[2] > 0) {
+		pos[2] = 0;
+		vel[2] = 0;
+		ned_accel[2] = 0;
+	}
+	
+	// Sensor feels gravity (when not acceleration in ned frame e.g. ned_accel[2] = 0)
+	ned_accel[2] -= GRAV;
+	
+	// Transform the accels back in to body frame
+	AccelsData accelsData; // Skip get as we set all the fields
+	accelsData.x = ned_accel[0] * Rbe[0][0] + ned_accel[1] * Rbe[0][1] + ned_accel[2] * Rbe[0][2] + accel_bias[0];
+	accelsData.y = ned_accel[0] * Rbe[1][0] + ned_accel[1] * Rbe[1][1] + ned_accel[2] * Rbe[1][2] + accel_bias[1];
+	accelsData.z = ned_accel[0] * Rbe[2][0] + ned_accel[1] * Rbe[2][1] + ned_accel[2] * Rbe[2][2] + accel_bias[2];
+	accelsData.temperature = 30;
+	AccelsSet(&accelsData);
+	
+	if(baro_offset == 0) {
+		// Hacky initialization
+		baro_offset = 50;// * rand_gauss();
+	} else {
+		// Very small drift process
+		baro_offset += rand_gauss() / 100;
+	}
+	// Update baro periodically	
+	static uint32_t last_baro_time = 0;
+	if(PIOS_DELAY_DiffuS(last_baro_time) / 1.0e6 > BARO_PERIOD) {
+		BaroAltitudeData baroAltitude;
+		BaroAltitudeGet(&baroAltitude);
+		baroAltitude.Altitude = -pos[2] + baro_offset;
+		BaroAltitudeSet(&baroAltitude);
+		last_baro_time = PIOS_DELAY_GetRaw();
+	}
+	
+	// Update baro airpseed periodically	
+	static uint32_t last_airspeed_time = 0;
+	if(PIOS_DELAY_DiffuS(last_airspeed_time) / 1.0e6 > BARO_PERIOD) {
+		BaroAirspeedData baroAirspeed;
+		baroAirspeed.Connected = BAROAIRSPEED_CONNECTED_TRUE;
+		baroAirspeed.ZeroPoint = 0;
+		baroAirspeed.Airspeed = forwardAirspeed;
+		BaroAirspeedSet(&baroAirspeed);
+		last_airspeed_time = PIOS_DELAY_GetRaw();
+	}
+	
+	HomeLocationData homeLocation;
+	HomeLocationGet(&homeLocation);
+	
+	static float gps_vel_drift[3] = {0,0,0};
+	gps_vel_drift[0] = gps_vel_drift[0] * 0.65 + rand_gauss() / 5.0;
+	gps_vel_drift[1] = gps_vel_drift[1] * 0.65 + rand_gauss() / 5.0;
+	gps_vel_drift[2] = gps_vel_drift[2] * 0.65 + rand_gauss() / 5.0;
+	
+	// Update GPS periodically	
+	static uint32_t last_gps_time = 0;
+	if(PIOS_DELAY_DiffuS(last_gps_time) / 1.0e6 > GPS_PERIOD) {
+		// Use double precision here as simulating what GPS produces
+		double T[3];
+		T[0] = homeLocation.Altitude+6.378137E6f * M_PI / 180.0;
+		T[1] = cos(homeLocation.Latitude / 10e6 * M_PI / 180.0f)*(homeLocation.Altitude+6.378137E6) * M_PI / 180.0;
+		T[2] = -1.0;
+		
+		static float gps_drift[3] = {0,0,0};
+		gps_drift[0] = gps_drift[0] * 0.95 + rand_gauss() / 10.0;
+		gps_drift[1] = gps_drift[1] * 0.95 + rand_gauss() / 10.0;
+		gps_drift[2] = gps_drift[2] * 0.95 + rand_gauss() / 10.0;
+		
+		GPSPositionData gpsPosition;
+		GPSPositionGet(&gpsPosition);
+		gpsPosition.Latitude = homeLocation.Latitude + ((pos[0] + gps_drift[0]) / T[0] * 10.0e6);
+		gpsPosition.Longitude = homeLocation.Longitude + ((pos[1] + gps_drift[1])/ T[1] * 10.0e6);
+		gpsPosition.Altitude = homeLocation.Altitude + ((pos[2] + gps_drift[2]) / T[2]);
+		gpsPosition.Groundspeed = sqrt(pow(vel[0] + gps_vel_drift[0],2) + pow(vel[1] + gps_vel_drift[1],2));
+		gpsPosition.Heading = 180 / M_PI * atan2(vel[1] + gps_vel_drift[1],vel[0] + gps_vel_drift[0]);
+		gpsPosition.Satellites = 7;
+		gpsPosition.PDOP = 1;
+		GPSPositionSet(&gpsPosition);
+		last_gps_time = PIOS_DELAY_GetRaw();
+	}
+	
+	// Update GPS Velocity measurements
+	static uint32_t last_gps_vel_time = 1000; // Delay by a millisecond
+	if(PIOS_DELAY_DiffuS(last_gps_vel_time) / 1.0e6 > GPS_PERIOD) {
+		GPSVelocityData gpsVelocity;
+		GPSVelocityGet(&gpsVelocity);
+		gpsVelocity.North = vel[0] + gps_vel_drift[0];
+		gpsVelocity.East = vel[1] + gps_vel_drift[1];
+		gpsVelocity.Down = vel[2] + gps_vel_drift[2];
+		GPSVelocitySet(&gpsVelocity);
+		last_gps_vel_time = PIOS_DELAY_GetRaw();
+	}
+	
+	// Update mag periodically
+	static uint32_t last_mag_time = 0;
+	if(PIOS_DELAY_DiffuS(last_mag_time) / 1.0e6 > MAG_PERIOD) {
+		MagnetometerData mag;
+		mag.x = 100+homeLocation.Be[0] * Rbe[0][0] + homeLocation.Be[1] * Rbe[0][1] + homeLocation.Be[2] * Rbe[0][2];
+		mag.y = 100+homeLocation.Be[0] * Rbe[1][0] + homeLocation.Be[1] * Rbe[1][1] + homeLocation.Be[2] * Rbe[1][2];
+		mag.z = 100+homeLocation.Be[0] * Rbe[2][0] + homeLocation.Be[1] * Rbe[2][1] + homeLocation.Be[2] * Rbe[2][2];
+		magOffsetEstimation(&mag);
+		MagnetometerSet(&mag);
+		last_mag_time = PIOS_DELAY_GetRaw();
+	}
+	
+	AttitudeSimulatedData attitudeSimulated;
+	AttitudeSimulatedGet(&attitudeSimulated);
+	attitudeSimulated.q1 = q[0];
+	attitudeSimulated.q2 = q[1];
+	attitudeSimulated.q3 = q[2];
+	attitudeSimulated.q4 = q[3];
+	Quaternion2RPY(q,&attitudeSimulated.Roll);
+	attitudeSimulated.Position[0] = pos[0];
+	attitudeSimulated.Position[1] = pos[1];
+	attitudeSimulated.Position[2] = pos[2];
+	attitudeSimulated.Velocity[0] = vel[0];
+	attitudeSimulated.Velocity[1] = vel[1];
+	attitudeSimulated.Velocity[2] = vel[2];
+	AttitudeSimulatedSet(&attitudeSimulated);
+}
 
 static float rand_gauss (void) {
 	float v1,v2,s;
@@ -534,6 +847,107 @@ static float rand_gauss (void) {
 		return (v1*sqrt(-2.0 * log(s) / s));
 }
 
+/**
+ * Perform an update of the @ref MagBias based on
+ * Magnetometer Offset Cancellation: Theory and Implementation, 
+ * revisited William Premerlani, October 14, 2011
+ */
+static void magOffsetEstimation(MagnetometerData *mag)
+{
+#if 0
+	RevoCalibrationData cal;
+	RevoCalibrationGet(&cal);
+
+	// Constants, to possibly go into a UAVO
+	static const float MIN_NORM_DIFFERENCE = 50;
+
+	static float B2[3] = {0, 0, 0};
+
+	MagBiasData magBias;
+	MagBiasGet(&magBias);
+
+	// Remove the current estimate of the bias
+	mag->x -= magBias.x;
+	mag->y -= magBias.y;
+	mag->z -= magBias.z;
+
+	// First call
+	if (B2[0] == 0 && B2[1] == 0 && B2[2] == 0) {
+		B2[0] = mag->x;
+		B2[1] = mag->y;
+		B2[2] = mag->z;
+		return;
+	}
+
+	float B1[3] = {mag->x, mag->y, mag->z};
+	float norm_diff = sqrtf(powf(B2[0] - B1[0],2) + powf(B2[1] - B1[1],2) + powf(B2[2] - B1[2],2));
+	if (norm_diff > MIN_NORM_DIFFERENCE) {
+		float norm_b1 = sqrtf(B1[0]*B1[0] + B1[1]*B1[1] + B1[2]*B1[2]);
+		float norm_b2 = sqrtf(B2[0]*B2[0] + B2[1]*B2[1] + B2[2]*B2[2]);
+		float scale = cal.MagBiasNullingRate * (norm_b2 - norm_b1) / norm_diff;
+		float b_error[3] = {(B2[0] - B1[0]) * scale, (B2[1] - B1[1]) * scale, (B2[2] - B1[2]) * scale};
+
+		magBias.x += b_error[0];
+		magBias.y += b_error[1];
+		magBias.z += b_error[2];
+
+		MagBiasSet(&magBias);
+
+		// Store this value to compare against next update
+		B2[0] = B1[0]; B2[1] = B1[1]; B2[2] = B1[2];
+	}
+#else
+	HomeLocationData homeLocation;
+	HomeLocationGet(&homeLocation);
+	
+	AttitudeActualData attitude;
+	AttitudeActualGet(&attitude);
+	
+	MagBiasData magBias;
+	MagBiasGet(&magBias);
+	
+	// Remove the current estimate of the bias
+	mag->x -= magBias.x;
+	mag->y -= magBias.y;
+	mag->z -= magBias.z;
+	
+	const float Rxy = sqrtf(homeLocation.Be[0]*homeLocation.Be[0] + homeLocation.Be[1]*homeLocation.Be[1]);
+	const float Rz = homeLocation.Be[2];
+	
+	const float rate = 0.01;
+	float R[3][3];
+	float B_e[3];
+	float xy[2];
+	float delta[3];
+	
+	// Get the rotation matrix
+	Quaternion2R(&attitude.q1, R);
+	
+	// Rotate the mag into the NED frame
+	B_e[0] = R[0][0] * mag->x + R[1][0] * mag->y + R[2][0] * mag->z;
+	B_e[1] = R[0][1] * mag->x + R[1][1] * mag->y + R[2][1] * mag->z;
+	B_e[2] = R[0][2] * mag->x + R[1][2] * mag->y + R[2][2] * mag->z;
+	
+	float cy = cosf(attitude.Yaw * M_PI / 180.0f);
+	float sy = sinf(attitude.Yaw * M_PI / 180.0f);
+
+	xy[0] =  cy * B_e[0] + sy * B_e[1];
+	xy[1] = -sy * B_e[0] + cy * B_e[1];
+	
+	float xy_norm = sqrtf(xy[0]*xy[0] + xy[1]*xy[1]);
+	
+	delta[0] = -rate * (xy[0] / xy_norm * Rxy - xy[0]);
+	delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]);
+	delta[2] = -rate * (Rz - B_e[2]);
+	
+	magBias.x += delta[0];
+	magBias.y += delta[1];
+	magBias.z += delta[2];
+	MagBiasSet(&magBias);
+#endif
+
+}
+
 /**
   * @}
   * @}

flight/Modules/System/systemmod.c

@@ -230,8 +230,19 @@ static void objectUpdatedCb(UAVObjEvent * ev)
 		ObjectPersistenceGet(&objper);
 
 		int retval = 1;
-		// Execute action
-		if (objper.Operation == OBJECTPERSISTENCE_OPERATION_LOAD) {
+		FlightStatusData flightStatus;
+		FlightStatusGet(&flightStatus);
+
+		// When this is called because of this method don't do anything
+		if (objper.Operation == OBJECTPERSISTENCE_OPERATION_ERROR ||
+			objper.Operation == OBJECTPERSISTENCE_OPERATION_COMPLETED) {
+			return;
+		}
+
+		// Execute action if disarmed
+		if(flightStatus.Armed != FLIGHTSTATUS_ARMED_DISARMED) {
+			retval = -1;
+		} else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_LOAD) {
 			if (objper.Selection == OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT) {
 				// Get selected object
 				obj = UAVObjGetByID(objper.ObjectID);

flight/Modules/Telemetry/telemetry.c

@@ -35,6 +35,10 @@
 #include "flighttelemetrystats.h"
 #include "gcstelemetrystats.h"
 #include "hwsettings.h"
+#if defined(PIOS_PACKET_HANDLER)
+#include "pipxstatus.h"
+#include "packet_handler.h"
+#endif
 
 // Private constants
 #define MAX_QUEUE_SIZE   TELEM_QUEUE_SIZE
@@ -80,6 +84,10 @@ static void processObjEvent(UAVObjEvent * ev);
 static void updateTelemetryStats();
 static void gcsTelemetryStatsUpdated();
 static void updateSettings();
+static uint32_t getComPort();
+#ifdef PIOS_PACKET_HANDLER
+static void receivePacketData(uint8_t *buf, uint8_t len, int8_t rssi, int8_t afc);
+#endif
 
 /**
  * Initialise the telemetry module
@@ -93,6 +101,13 @@ int32_t TelemetryStart(void)
     
 	// Listen to objects of interest
 	GCSTelemetryStatsConnectQueue(priorityQueue);
+
+	// Register to receive data from the radio packet handler.
+	// This must be after the radio module is initialized.
+#ifdef PIOS_PACKET_HANDLER
+	if (PIOS_PACKET_HANDLER)
+		PHRegisterDataHandler(PIOS_PACKET_HANDLER, receivePacketData);
+#endif
     
 	// Start telemetry tasks
 	xTaskCreate(telemetryTxTask, (signed char *)"TelTx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_TX, &telemetryTxTaskHandle);
@@ -247,6 +262,11 @@ static void processObjEvent(UAVObjEvent * ev)
 			retries = 0;
 			success = -1;
 			if (ev->event == EV_UPDATED || ev->event == EV_UPDATED_MANUAL || ((ev->event == EV_UPDATED_PERIODIC) && (updateMode != UPDATEMODE_THROTTLED))) {
+#ifdef PIOS_PACKET_HANDLER
+				// Don't send PipXStatus objects over the radio link.
+				if (PIOS_PACKET_HANDLER && (ev->obj == PipXStatusHandle()) && (getComPort() == 0))
+					return;
+#endif
 				// Send update to GCS (with retries)
 				while (retries < MAX_RETRIES && success == -1) {
 					success = UAVTalkSendObject(uavTalkCon, ev->obj, ev->instId, UAVObjGetTelemetryAcked(&metadata), REQ_TIMEOUT_MS);	// call blocks until ack is received or timeout
@@ -322,19 +342,10 @@ static void telemetryTxPriTask(void *parameters)
  */
 static void telemetryRxTask(void *parameters)
 {
-	uint32_t inputPort;
 
 	// Task loop
 	while (1) {
-#if defined(PIOS_INCLUDE_USB)
-		// Determine input port (USB takes priority over telemetry port)
-		if (PIOS_USB_CheckAvailable(0) && PIOS_COM_TELEM_USB) {
-			inputPort = PIOS_COM_TELEM_USB;
-		} else
-#endif /* PIOS_INCLUDE_USB */
-		{
-			inputPort = telemetryPort;
-		}
+		uint32_t inputPort = getComPort();
 
 		if (inputPort) {
 			// Block until data are available
@@ -362,23 +373,17 @@ static void telemetryRxTask(void *parameters)
  */
 static int32_t transmitData(uint8_t * data, int32_t length)
 {
-	uint32_t outputPort;
-
-	// Determine input port (USB takes priority over telemetry port)
-#if defined(PIOS_INCLUDE_USB)
-	if (PIOS_USB_CheckAvailable(0) && PIOS_COM_TELEM_USB) {
-		outputPort = PIOS_COM_TELEM_USB;
-	} else
-#endif /* PIOS_INCLUDE_USB */
-	{
-		outputPort = telemetryPort;
-	}
+	uint32_t outputPort = getComPort();
 
 	if (outputPort) {
 		return PIOS_COM_SendBuffer(outputPort, data, length);
-	} else {
-		return -1;
 	}
+#ifdef PIOS_PACKET_HANDLER
+	if (PIOS_PACKET_HANDLER)
+		if (PHTransmitData(PIOS_PACKET_HANDLER, data, length))
+			return length;
+#endif
+	return -1;
 }
 
 /**
@@ -564,6 +569,31 @@ static void updateSettings()
 	}
 }
 
+/**
+ * Determine input/output com port (USB takes priority over telemetry port)
+ */
+static uint32_t getComPort() {
+#if defined(PIOS_INCLUDE_USB)
+	if (PIOS_USB_CheckAvailable(0) && PIOS_COM_TELEM_USB)
+		return PIOS_COM_TELEM_USB;
+	else
+#endif /* PIOS_INCLUDE_USB */
+		return telemetryPort;
+}
+
+#ifdef PIOS_PACKET_HANDLER
+/**
+ * Receive a packet
+ * \param[in] buf The received data buffer
+ * \param[in] length Length of buffer
+ */
+static void receivePacketData(uint8_t *buf, uint8_t len, int8_t rssi, int8_t afc)
+{
+	for (uint8_t i = 0; i < len; ++i)
+		UAVTalkProcessInputStream(uavTalkCon, buf[i]);
+}
+#endif
+
 /**
   * @}
   * @}

flight/Modules/VtolPathFollower/vtolpathfollower.c

@@ -3,9 +3,9 @@
  *
  * @file       vtolpathfollower.c
  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
- * @brief      This module compared @ref PositionActuatl to @ref ActiveWaypoint 
- * and sets @ref AttitudeDesired.  It only does this when the FlightMode field
- * of @ref ManualControlCommand is Auto.
+ * @brief      This module compared @ref PositionActual to @ref PathDesired 
+ * and sets @ref Stabilization.  It only does this when the FlightMode field
+ * of @ref FlightStatus is PathPlanner or RTH.
  *
  * @see        The GNU Public License (GPL) Version 3
  *
@@ -27,12 +27,15 @@
  */
 
 /**
- * Input object: ActiveWaypoint
+ * Input object: FlightStatus
+ * Input object: PathDesired
  * Input object: PositionActual
- * Input object: ManualControlCommand
- * Output object: AttitudeDesired
+ * Output object: StabilizationDesired
  *
- * This module will periodically update the value of the AttitudeDesired object.
+ * This module will periodically update the value of the @ref StabilizationDesired object based on 
+ * @ref PathDesired and @PositionActual when the Flight Mode selected in @FlightStatus is supported
+ * by this module.  Otherwise another module (e.g. @ref ManualControlCommand) is expected to be
+ * writing to @ref StabilizationDesired.
  *
  * The module executes in its own thread in this example.
  *
@@ -48,8 +51,8 @@
 
 #include "vtolpathfollower.h"
 #include "accels.h"
-#include "hwsettings.h"
 #include "attitudeactual.h"
+#include "hwsettings.h"
 #include "pathdesired.h"        // object that will be updated by the module
 #include "positionactual.h"
 #include "manualcontrol.h"
@@ -76,7 +79,6 @@
 // Private types
 
 // Private variables
-static bool followerEnabled = false;
 static xTaskHandle pathfollowerTaskHandle;
 static PathDesiredData pathDesired;
 static VtolPathFollowerSettingsData vtolpathfollowerSettings;
@@ -90,6 +92,7 @@ static void updateEndpointVelocity();
 static void updateFixedAttitude(float* attitude);
 static void updateVtolDesiredAttitude();
 static float bound(float val, float min, float max);
+static bool vtolpathfollower_enabled;
 
 /**
  * Initialise the module, called on startup
@@ -97,9 +100,9 @@ static float bound(float val, float min, float max);
  */
 int32_t VtolPathFollowerStart()
 {
-	if (followerEnabled) {
+	if (vtolpathfollower_enabled) {
 		// Start main task
-		xTaskCreate(vtolPathFollowerTask, (signed char *)"PathFollower", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &pathfollowerTaskHandle);
+		xTaskCreate(vtolPathFollowerTask, (signed char *)"VtolPathFollower", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &pathfollowerTaskHandle);
 		TaskMonitorAdd(TASKINFO_RUNNING_PATHFOLLOWER, pathfollowerTaskHandle);
 	}
 
@@ -112,18 +115,19 @@ int32_t VtolPathFollowerStart()
  */
 int32_t VtolPathFollowerInitialize()
 {
-	HwSettingsInitialize();
 	uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
+	
 	HwSettingsOptionalModulesGet(optionalModules);
+	
 	if (optionalModules[HWSETTINGS_OPTIONALMODULES_VTOLPATHFOLLOWER] == HWSETTINGS_OPTIONALMODULES_ENABLED) {
-		followerEnabled = true;
 		VtolPathFollowerSettingsInitialize();
 		NedAccelInitialize();
 		PathDesiredInitialize();
 		PathStatusInitialize();
 		VelocityDesiredInitialize();
+		vtolpathfollower_enabled = true;
 	} else {
-		followerEnabled = false;
+		vtolpathfollower_enabled = false;
 	}
 	
 	return 0;
@@ -251,6 +255,9 @@ static void vtolPathFollowerTask(void *parameters)
 
 				break;
 		}
+
+		AlarmsClear(SYSTEMALARMS_ALARM_GUIDANCE);
+
 	}
 }
 

flight/PiOS.osx/inc/pios_bl_helper.h

@@ -0,0 +1,46 @@
+/**
+ ******************************************************************************
+ * @addtogroup PIOS PIOS Core hardware abstraction layer
+ * @{
+ * @addtogroup PIOS_BOOTLOADER Functions
+ * @brief HAL code to interface to the OpenPilot AHRS module
+ * @{
+ *
+ * @file       pios_bl_helper.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Bootloader Helper Functions
+ * @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_BL_HELPER_H_
+#define PIOS_BL_HELPER_H_
+
+extern uint8_t *PIOS_BL_HELPER_FLASH_If_Read(uint32_t SectorAddress);
+
+extern uint8_t PIOS_BL_HELPER_FLASH_Ini();
+
+extern uint32_t PIOS_BL_HELPER_CRC_Memory_Calc();
+
+extern void PIOS_BL_HELPER_FLASH_Read_Description(uint8_t * array, uint8_t size);
+
+extern uint8_t PIOS_BL_HELPER_FLASH_Start();
+
+extern void PIOS_BL_HELPER_CRC_Ini();
+
+#endif /* PIOS_BL_HELPER_H_ */

flight/PiOS.osx/inc/pios_board.h

@@ -0,0 +1,5 @@
+#ifndef PIOS_BOARD_H_
+#define PIOS_BOARD_H_
+
+
+#endif /* PIOS_BOARD_H_ */

flight/PiOS.osx/inc/pios_board_info.h

@@ -0,0 +1,24 @@
+#ifndef PIOS_BOARD_INFO_H
+#define PIOS_BOARD_INFO_H
+
+#include <stdint.h>		/* uint* */
+
+#define PIOS_BOARD_INFO_BLOB_MAGIC 0xBDBDBDBD
+
+struct pios_board_info {
+  uint32_t magic;
+  uint8_t  board_type;
+  uint8_t  board_rev;
+  uint8_t  bl_rev;
+  uint8_t  hw_type;
+  uint32_t fw_base;
+  uint32_t fw_size;
+  uint32_t desc_base;
+  uint32_t desc_size;
+  uint32_t ee_base;
+  uint32_t ee_size;
+} __attribute__((packed));
+
+extern const struct pios_board_info pios_board_info_blob;
+
+#endif /* PIOS_BOARD_INFO_H */

flight/PiOS.osx/inc/pios_gcsrcvr_priv.h

@@ -0,0 +1,47 @@
+/**
+ ******************************************************************************
+ * @addtogroup PIOS PIOS Core hardware abstraction layer
+ * @{
+ * @addtogroup   PIOS_GCSRCVR GCS Receiver Functions
+ * @brief PIOS interface to read from GCS receiver port
+ * @{
+ *
+ * @file       pios_gcsrcvr_priv.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      GCS receiver private functions
+ * @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_GCSRCVR_PRIV_H
+#define PIOS_GCSRCVR_PRIV_H
+
+#include <pios.h>
+
+#include "gcsreceiver.h"
+
+extern const struct pios_rcvr_driver pios_gcsrcvr_rcvr_driver;
+
+extern int32_t PIOS_GCSRCVR_Init(uint32_t *gcsrcvr_id);
+
+#endif /* PIOS_GCSRCVR_PRIV_H */
+
+/**
+ * @}
+ * @}
+ */

flight/PiOS.osx/inc/pios_iap.h

@@ -0,0 +1,45 @@
+/*!
+ * 	@File iap.h
+ *	@Brief	Header file for the In-Application-Programming Module
+ *
+ *  Created on: Sep 6, 2010
+ *      Author: joe
+ */
+
+#ifndef PIOS_IAP_H_
+#define PIOS_IAP_H_
+
+
+/****************************************************************************************
+ *  Header files
+ ****************************************************************************************/
+
+/*****************************************************************************************
+ *	Public Definitions/Macros
+ ****************************************************************************************/
+#if defined(STM32F4XX)
+#define MAGIC_REG_1     RTC_BKP_DR1
+#define MAGIC_REG_2     RTC_BKP_DR2
+#define IAP_BOOTCOUNT   RTC_BKP_DR3
+#else
+#define MAGIC_REG_1     BKP_DR1
+#define MAGIC_REG_2     BKP_DR2
+#define IAP_BOOTCOUNT   BKP_DR3
+#endif
+
+/****************************************************************************************
+ *  Public Functions
+ ****************************************************************************************/
+void		PIOS_IAP_Init(void);
+uint32_t	PIOS_IAP_CheckRequest( void );
+void		PIOS_IAP_SetRequest1(void);
+void		PIOS_IAP_SetRequest2(void);
+void		PIOS_IAP_ClearRequest(void);
+uint16_t	PIOS_IAP_ReadBootCount(void);
+void		PIOS_IAP_WriteBootCount(uint16_t);
+
+/****************************************************************************************
+ *  Public Data
+ ****************************************************************************************/
+
+#endif /* PIOS_IAP_H_ */

flight/PiOS.osx/inc/pios_servo.h

@@ -32,7 +32,7 @@
 
 /* Public Functions */
 extern void PIOS_Servo_Init(void);
-extern void PIOS_Servo_SetHz(uint16_t * speeds, uint8_t num_banks);
+extern void PIOS_Servo_SetHz(const uint16_t * speeds, uint8_t num_banks);
 extern void PIOS_Servo_Set(uint8_t Servo, uint16_t Position);
 
 #endif /* PIOS_SERVO_H */

flight/PiOS.osx/inc/pios_sys.h

@@ -1,35 +1,50 @@
-/**
- ******************************************************************************
- *
- * @file       pios_sys.h  
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * 	       Parts by Thorsten Klose (tk@midibox.org)
- * @brief      System and hardware Init functions header.
- * @see        The GNU Public License (GPL) Version 3
- *
- *****************************************************************************/
-/* 
- * This program is free software; you can redistribute it and/or modify 
- * it under the terms of the GNU General Public License as published by 
- * the Free Software Foundation; either version 3 of the License, or 
- * (at your option) any later version.
- * 
- * This program is distributed in the hope that it will be useful, but 
- * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 
- * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 
- * for more details.
- * 
- * You should have received a copy of the GNU General Public License along 
- * with this program; if not, write to the Free Software Foundation, Inc., 
- * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- */
-
-#ifndef PIOS_SYS_H
-#define PIOS_SYS_H
-
-/* Public Functions */
-extern void PIOS_SYS_Init(void);
-extern int32_t PIOS_SYS_Reset(void);
-extern int32_t PIOS_SYS_SerialNumberGet(char *str);
-
-#endif /* PIOS_SYS_H */
+/**
+ ******************************************************************************
+ * @addtogroup PIOS PIOS Core hardware abstraction layer
+ * @{
+ * @addtogroup PIOS_SYS System Functions
+ * @brief PIOS System Initialization code
+ * @{
+ *
+ * @file       pios_sys.h  
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * 	       Parts by Thorsten Klose (tk@midibox.org)
+ * @brief      System and hardware Init functions header.
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/* 
+ * This program is free software; you can redistribute it and/or modify 
+ * it under the terms of the GNU General Public License as published by 
+ * the Free Software Foundation; either version 3 of the License, or 
+ * (at your option) any later version.
+ * 
+ * This program is distributed in the hope that it will be useful, but 
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 
+ * for more details.
+ * 
+ * You should have received a copy of the GNU General Public License along 
+ * with this program; if not, write to the Free Software Foundation, Inc., 
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#ifndef PIOS_SYS_H
+#define PIOS_SYS_H
+
+#define PIOS_SYS_SERIAL_NUM_BINARY_LEN 12
+#define PIOS_SYS_SERIAL_NUM_ASCII_LEN (PIOS_SYS_SERIAL_NUM_BINARY_LEN * 2)
+
+/* Public Functions */
+extern void PIOS_SYS_Init(void);
+extern int32_t PIOS_SYS_Reset(void);
+extern uint32_t PIOS_SYS_getCPUFlashSize(void);
+extern int32_t PIOS_SYS_SerialNumberGetBinary(uint8_t array[PIOS_SYS_SERIAL_NUM_BINARY_LEN]);
+extern int32_t PIOS_SYS_SerialNumberGet(char str[PIOS_SYS_SERIAL_NUM_ASCII_LEN+1]);
+
+#endif /* PIOS_SYS_H */
+
+/**
+  * @}
+  * @}
+  */

flight/PiOS.osx/osx/pios_bl_helper.c

@@ -0,0 +1,53 @@
+/**
+ ******************************************************************************
+ * @addtogroup PIOS PIOS Core hardware abstraction layer
+ * @{
+ * @addtogroup PIOS_BOOTLOADER Functions
+ * @brief HAL code to interface to the OpenPilot AHRS module
+ * @{
+ *
+ * @file       pios_bl_helper.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @brief      Bootloader Helper Functions
+ * @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
+ */
+
+/* Project Includes */
+#include "pios.h"
+#if defined(PIOS_INCLUDE_BL_HELPER)
+#include <pios_board_info.h>
+
+uint32_t PIOS_BL_HELPER_CRC_Memory_Calc()
+{
+	return 0;
+}
+
+extern const struct fw_version_info fw_version_blob;
+void PIOS_BL_HELPER_FLASH_Read_Description(uint8_t * array, uint8_t size)
+{
+	uint8_t * desc = (uint8_t *) &fw_version_blob;
+	for (uint32_t i = 0;  i < size; i++) {
+		array[i] = desc[i];
+	}
+}
+
+void PIOS_BL_HELPER_CRC_Ini()
+{
+}
+#endif

flight/PiOS.osx/osx/pios_board_info.c

@@ -0,0 +1,20 @@
+#include <pios.h>
+#include <pios_board.h>
+
+#include "pios_board_info.h"
+
+const struct pios_board_info pios_board_info_blob = {
+  .magic      = PIOS_BOARD_INFO_BLOB_MAGIC,
+  .board_type = BOARD_TYPE,
+  .board_rev  = BOARD_REVISION,
+  .bl_rev     = BOOTLOADER_VERSION,
+  .hw_type    = HW_TYPE,
+  .fw_base    = FW_BANK_BASE,
+  .fw_size    = FW_BANK_SIZE - FW_DESC_SIZE,
+  .desc_base  = FW_BANK_BASE + FW_BANK_SIZE - FW_DESC_SIZE,
+  .desc_size  = FW_DESC_SIZE,
+#ifdef EE_BANK_BASE
+  .ee_base    = EE_BANK_BASE,
+  .ee_size    = EE_BANK_SIZE,
+#endif
+};

flight/PiOS.osx/osx/pios_gcsrcvr.c

@@ -0,0 +1,139 @@
+/**
+ ******************************************************************************
+ * @addtogroup PIOS PIOS Core hardware abstraction layer
+ * @{
+ * @addtogroup   PIOS_GCSRCVR GCS Receiver Input Functions
+ * @brief		Code to read the channels within the GCS Receiver UAVObject
+ * @{
+ *
+ * @file       pios_gcsrcvr.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      GCS Input functions (STM32 dependent)
+ * @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
+ */
+
+/* Project Includes */
+#include "pios.h"
+
+#if defined(PIOS_INCLUDE_GCSRCVR)
+#include "openpilot.h"
+#include "pios_gcsrcvr_priv.h"
+
+static GCSReceiverData gcsreceiverdata;
+
+/* Provide a RCVR driver */
+static int32_t PIOS_GCSRCVR_Get(uint32_t rcvr_id, uint8_t channel);
+
+const struct pios_rcvr_driver pios_gcsrcvr_rcvr_driver = {
+	.read = PIOS_GCSRCVR_Get,
+};
+
+/* Local Variables */
+enum pios_gcsrcvr_dev_magic {
+	PIOS_GCSRCVR_DEV_MAGIC = 0xe9da5c56,
+};
+
+struct pios_gcsrcvr_dev {
+	enum pios_gcsrcvr_dev_magic magic;
+
+	uint8_t supv_timer;
+	bool Fresh;
+};
+
+static struct pios_gcsrcvr_dev *global_gcsrcvr_dev;
+
+#if defined(PIOS_INCLUDE_FREERTOS)
+static struct pios_gcsrcvr_dev *PIOS_gcsrcvr_alloc(void)
+{
+	struct pios_gcsrcvr_dev * gcsrcvr_dev;
+
+	gcsrcvr_dev = (struct pios_gcsrcvr_dev *)pvPortMalloc(sizeof(*gcsrcvr_dev));
+	if (!gcsrcvr_dev) return(NULL);
+
+	gcsrcvr_dev->magic = PIOS_GCSRCVR_DEV_MAGIC;
+	gcsrcvr_dev->Fresh = FALSE;
+	gcsrcvr_dev->supv_timer = 0;
+
+	/* The update callback cannot receive the device pointer, so set it in a global */
+	global_gcsrcvr_dev = gcsrcvr_dev;
+
+	return(gcsrcvr_dev);
+}
+#else
+static struct pios_gcsrcvr_dev pios_gcsrcvr_devs[PIOS_GCSRCVR_MAX_DEVS];
+static uint8_t pios_gcsrcvr_num_devs;
+static struct pios_gcsrcvr_dev *PIOS_gcsrcvr_alloc(void)
+{
+	struct pios_gcsrcvr_dev *gcsrcvr_dev;
+
+	if (pios_gcsrcvr_num_devs >= PIOS_GCSRCVR_MAX_DEVS) {
+		return (NULL);
+	}
+
+	gcsrcvr_dev = &pios_gcsrcvr_devs[pios_gcsrcvr_num_devs++];
+	gcsrcvr_dev->magic = PIOS_GCSRCVR_DEV_MAGIC;
+	gcsrcvr_dev->Fresh = FALSE;
+	gcsrcvr_dev->supv_timer = 0;
+
+	global_gcsrcvr_dev = gcsrcvr_dev;
+
+	return (gcsrcvr_dev);
+}
+#endif
+
+static void gcsreceiver_updated(UAVObjEvent * ev)
+{
+	struct pios_gcsrcvr_dev *gcsrcvr_dev = global_gcsrcvr_dev;
+	if (ev->obj == GCSReceiverHandle()) {
+		GCSReceiverGet(&gcsreceiverdata);
+		gcsrcvr_dev->Fresh = TRUE;
+	}
+}
+
+extern int32_t PIOS_GCSRCVR_Init(uint32_t *gcsrcvr_id)
+{
+	struct pios_gcsrcvr_dev *gcsrcvr_dev;
+
+	/* Allocate the device structure */
+	gcsrcvr_dev = (struct pios_gcsrcvr_dev *)PIOS_gcsrcvr_alloc();
+	if (!gcsrcvr_dev)
+		return -1;
+
+	/* Register uavobj callback */
+	GCSReceiverConnectCallback (gcsreceiver_updated);
+
+	return 0;
+}
+
+static int32_t PIOS_GCSRCVR_Get(uint32_t rcvr_id, uint8_t channel)
+{
+	if (channel >= GCSRECEIVER_CHANNEL_NUMELEM) {
+		/* channel is out of range */
+		return -1;
+	}
+
+	return (gcsreceiverdata.Channel[channel]);
+}
+
+#endif	/* PIOS_INCLUDE_GCSRCVR */
+
+/** 
+  * @}
+  * @}
+  */

flight/PiOS.osx/osx/pios_iap.c

@@ -0,0 +1,69 @@
+/*!
+ * 	@File iap.c
+ *	@Brief	
+ *
+ *  Created on: Sep 6, 2010
+ *      Author: joe
+ */
+
+
+/****************************************************************************************
+ *  Header files
+ ****************************************************************************************/
+#include <pios.h>
+
+/*!
+ * \brief	PIOS_IAP_Init - performs required initializations for iap module.
+ * \param   none.
+ * \return	none.
+ * \retval	none.
+ *
+ *	Created: Sep 8, 2010 10:10:48 PM by joe
+ */
+void PIOS_IAP_Init( void )
+{
+
+}
+
+/*!
+ * \brief     Determines if an In-Application-Programming request has been made.
+ * \param   *comm - Which communication stream to use for the IAP (USB, Telemetry, I2C, SPI, etc)
+ * \return    TRUE - if correct sequence found, along with 'comm' updated.
+ * 			FALSE - Note that 'comm' will have an invalid comm identifier.
+ * \retval
+ *
+ */
+uint32_t	PIOS_IAP_CheckRequest( void )
+{
+
+	return false;
+}
+
+
+
+/*!
+ * \brief   Sets the 1st word of the request sequence.
+ * \param   n/a
+ * \return  n/a
+ * \retval
+ */
+void	PIOS_IAP_SetRequest1(void)
+{
+}
+
+void	PIOS_IAP_SetRequest2(void)
+{
+}
+
+void	PIOS_IAP_ClearRequest(void)
+{
+}
+
+uint16_t PIOS_IAP_ReadBootCount(void)
+{
+	return 0;
+}
+
+void PIOS_IAP_WriteBootCount (uint16_t boot_count)
+{
+}

flight/PiOS.osx/osx/pios_rcvr.c

@@ -80,6 +80,15 @@ out_fail:
 
 int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel)
 {
+	// Publicly facing API uses channel 1 for first channel
+	if(channel == 0)
+		return PIOS_RCVR_INVALID;
+	else
+		channel--;
+
+	if (rcvr_id == 0)
+		return PIOS_RCVR_NODRIVER;
+
   struct pios_rcvr_dev * rcvr_dev = &pios_rcvr_devs[rcvr_id];
 
   if (!PIOS_RCVR_validate(rcvr_dev)) {

flight/PiOS.osx/osx/pios_servo.c

@@ -50,7 +50,7 @@ void PIOS_Servo_Init(void)
 * \param[in] onetofour Rate for outputs 1 to 4 (Hz)
 * \param[in] fivetoeight Rate for outputs 5 to 8 (Hz)
 */
-void PIOS_Servo_SetHz(uint16_t * banks, uint8_t num_banks)
+void PIOS_Servo_SetHz(const uint16_t * banks, uint8_t num_banks)
 {
 }
 

flight/PiOS.osx/osx/pios_sys.c

@@ -1,165 +1,111 @@
-/**
- ******************************************************************************
- *
- * @file       pios_sys.c  
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * 	        Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org)
- * @brief      Sets up basic system hardware, functions are called from Main.
- * @see        The GNU Public License (GPL) Version 3
- * @defgroup   PIOS_SYS System Functions
- * @{
- *
- *****************************************************************************/
-/* 
- * 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
- */
-
-
-/* Project Includes */
-#include "pios.h"
-
-#if defined(PIOS_INCLUDE_SYS)
-
-
-/* Private Function Prototypes */
-void NVIC_Configuration(void);
-void SysTick_Handler(void);
-
-/* Local Macros */
-#define MEM8(addr)  (*((volatile uint8_t  *)(addr)))
-
-/**
-* Initialises all system peripherals
-*/
-void PIOS_SYS_Init(void)
-{
-
-	/**
-	 * stub
-	 */
-	printf("PIOS_SYS_Init\n");
-
-	/* Initialise Basic NVIC */
-	NVIC_Configuration();
-
-#if defined(PIOS_INCLUDE_LED)
-	/* Initialise LEDs */
-	PIOS_LED_Init();
-#endif
-}
-
-/**
-* Shutdown PIOS and reset the microcontroller:<BR>
-* <UL>
-*   <LI>Disable all RTOS tasks
-*   <LI>Disable all interrupts
-*   <LI>Turn off all board LEDs
-*   <LI>Reset STM32
-* </UL>
-* \return < 0 if reset failed
-*/
-int32_t PIOS_SYS_Reset(void)
-{
-	/**
-	 * stub
-	 */
-	printf("PIOS_SYS_Reset\n");
-	/* Disable all RTOS tasks */
-#if defined(PIOS_INCLUDE_FREERTOS)
-	/* port specific FreeRTOS function to disable tasks (nested) */
-	portENTER_CRITICAL();
-#endif
-
-
-	while(1);
-
-	/* We will never reach this point */
-	return -1;
-}
-
-/**
-* Returns the serial number as a string
-* param[out] str pointer to a string which can store at least 32 digits + zero terminator!
-* (24 digits returned for STM32)
-* return < 0 if feature not supported
-*/
-int32_t PIOS_SYS_SerialNumberGet(char *str)
-{
-	int i;
-
-	/* Stored in the so called "electronic signature" */
-	for(i=0; i<24; ++i) {
-		//uint8_t b = MEM8(0x1ffff7e8 + (i/2));
-		//if( !(i & 1) )
-		//b >>= 4;
-		//b &= 0x0f;
-
-		//str[i] = ((b > 9) ? ('A'-10) : '0') + b;
-		str[i]='6';
-	}
-	str[i] = 0;
-
-	/* No error */
-	return 0;
-}
-
-/**
-* Configures Vector Table base location and SysTick
-*/
-void NVIC_Configuration(void)
-{
-	/**
-	 * stub
-	 */
-	printf("NVIC_Configuration\n");
-	/* Set the Vector Table base address as specified in .ld file */
-	//NVIC_SetVectorTable(PIOS_NVIC_VECTTAB_FLASH, 0x0);
-
-	/* 4 bits for Interrupt priorities so no sub priorities */
-	//NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4);
-
-	/* Configure HCLK clock as SysTick clock source. */
-	//SysTick_CLKSourceConfig(SysTick_CLKSource_HCLK);
-}
-
-#ifdef USE_FULL_ASSERT
-/**
-* Reports the name of the source file and the source line number
-*   where the assert_param error has occurred.
-* \param[in]  file pointer to the source file name
-* \param[in]  line assert_param error line source number
-* \retval None
-*/
-void assert_failed(uint8_t* file, uint32_t line)
-{
-	/* When serial debugging is implemented, use something like this. */
-	/* printf("Wrong parameters value: file %s on line %d\r\n", file, line); */
-	printf("Wrong parameters value: file %s on line %d\r\n", file, line);
-
-	/* Setup the LEDs to Alternate */
-	PIOS_LED_On(LED1);
-	PIOS_LED_Off(LED2);
-
-	/* Infinite loop */
-	while (1)
-	{
-		PIOS_LED_Toggle(LED1);
-		PIOS_LED_Toggle(LED2);
-		for(int i = 0; i < 1000000; i++);
-	}
-}
-#endif
-
-#endif
+/**
+ ******************************************************************************
+ * @addtogroup PIOS PIOS Core hardware abstraction layer
+ * @{
+ * @addtogroup PIOS_SYS System Functions
+ * @brief PIOS System Initialization code
+ * @{
+ *
+ * @file       pios_sys.c  
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * 	        Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org)
+ * @brief      Sets up basic STM32 system hardware, functions are called from Main.
+ * @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
+ */
+
+/* Project Includes */
+#include "pios.h"
+
+#if defined(PIOS_INCLUDE_SYS)
+
+
+/**
+* Initialises all system peripherals
+*/
+void PIOS_SYS_Init(void)
+{
+	
+}
+
+/**
+* Shutdown PIOS and reset the microcontroller:<BR>
+* <UL>
+*   <LI>Disable all RTOS tasks
+*   <LI>Disable all interrupts
+*   <LI>Turn off all board LEDs
+*   <LI>Reset STM32
+* </UL>
+* \return < 0 if reset failed
+*/
+int32_t PIOS_SYS_Reset(void)
+{
+	/* We will never reach this point */
+	return -1;
+}
+
+/**
+* Returns the CPU's flash size (in bytes)
+*/
+uint32_t PIOS_SYS_getCPUFlashSize(void)
+{
+	return 1024000;
+}
+
+/**
+* Returns the serial number as a string
+* param[out] uint8_t pointer to a string which can store at least 12 bytes
+* (12 bytes returned for STM32)
+* return < 0 if feature not supported
+*/
+int32_t PIOS_SYS_SerialNumberGetBinary(uint8_t *array)
+{
+	/* Stored in the so called "electronic signature" */
+	for (int i = 0; i < PIOS_SYS_SERIAL_NUM_BINARY_LEN; ++i) {
+		array[i] = 0xff;
+	}
+
+	/* No error */
+	return 0;
+}
+
+/**
+* Returns the serial number as a string
+* param[out] str pointer to a string which can store at least 32 digits + zero terminator!
+* (24 digits returned for STM32)
+* return < 0 if feature not supported
+*/
+int32_t PIOS_SYS_SerialNumberGet(char *str)
+{
+	/* Stored in the so called "electronic signature" */
+	int i;
+	for (i = 0; i < PIOS_SYS_SERIAL_NUM_ASCII_LEN; ++i) {
+		str[i] = 'F';
+	}
+	str[i] = '\0';
+
+	/* No error */
+	return 0;
+}
+
+#endif
+
+/**
+  * @}
+  * @}
+  */

flight/PiOS.osx/pios.h

@@ -72,6 +72,13 @@
 #include <pios_irq.h>
 #include <pios_sim.h>
 
+#if defined(PIOS_INCLUDE_IAP)
+#include <pios_iap.h>
+#endif
+#if defined(PIOS_INCLUDE_BL_HELPER)
+#include <pios_bl_helper.h>
+#endif
+
 #define NELEMENTS(x) (sizeof(x) / sizeof(*(x)))
 
 #endif /* PIOS_H */

flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h

@@ -71,13 +71,12 @@ TIM4  |  RC In 1  |  Servo 3  |  Servo 2  |  Servo 1
 //------------------------
 #define PIOS_WATCHDOG_TIMEOUT    500
 #define PIOS_WDG_REGISTER        BKP_DR4
-#define PIOS_WDG_COMUAVTALK      0x0001
-#define PIOS_WDG_RADIORECEIVE    0x0002
-#define PIOS_WDG_SENDPACKET      0x0004
+#define PIOS_WDG_COMGCS          0x0001
+#define PIOS_WDG_COMUAVTALK      0x0002
+#define PIOS_WDG_RADIORECEIVE    0x0004
 #define PIOS_WDG_SENDDATA        0x0008
-#define PIOS_WDG_TRANSCOMM       0x0010
-#define PIOS_WDG_PPMINPUT        0x0020
-#define PIOS_WDG_COMGCS          0x0040
+#define PIOS_WDG_TRANSCOMM       0x0008
+#define PIOS_WDG_PPMINPUT        0x0010
 
 //------------------------
 // TELEMETRY
@@ -138,8 +137,6 @@ extern uint32_t pios_i2c_flexi_adapter_id;
 // See also pios_board.c
 //------------------------
 #define PIOS_SPI_MAX_DEVS               1
-extern uint32_t pios_spi_port_id;
-#define PIOS_SPI_PORT                   (pios_spi_port_id)
 
 //-------------------------
 // PIOS_USART
@@ -270,30 +267,40 @@ extern uint32_t pios_ppm_rcvr_id;
 // RFM22
 //-------------------------
 
-extern uint32_t pios_rfm22b_id;
+#if defined(PIOS_INCLUDE_RFM22B)
+#define PIOS_COM_RFM22B_RF_RX_BUF_LEN 256
+#define PIOS_COM_RFM22B_RF_TX_BUF_LEN 256
+extern uint32_t pios_com_rfm22b_id;
+#define PIOS_COM_RADIO                  (pios_com_rfm22b_id)
+extern uint32_t pios_spi_rfm22b_id;
+#define PIOS_RFM22_SPI_PORT             (pios_spi_rfm22b_id)
 #define RFM22_EXT_INT_USE
-#define RFM22_PIOS_SPI	PIOS_SPI_PORT	// SPIx
+extern uint32_t pios_rfm22b_id;
+#endif /* PIOS_INCLUDE_RFM22B */
 
-#if defined(RFM22_EXT_INT_USE)
-#define PIOS_RFM22_EXTI_GPIO_PORT               GPIOA
-#define PIOS_RFM22_EXTI_GPIO_PIN                GPIO_Pin_2
-#define PIOS_RFM22_EXTI_PORT_SOURCE             GPIO_PortSourceGPIOA
-#define PIOS_RFM22_EXTI_PIN_SOURCE              GPIO_PinSource2
-#define PIOS_RFM22_EXTI_CLK                     RCC_APB2Periph_GPIOA
-#define PIOS_RFM22_EXTI_LINE                    EXTI_Line2
-#define PIOS_RFM22_EXTI_IRQn                    EXTI2_IRQn
-#define PIOS_RFM22_EXTI_PRIO                    PIOS_IRQ_PRIO_LOW
-#endif
+//-------------------------
+// Packet Handler
+//-------------------------
+#if defined(PIOS_INCLUDE_PACKET_HANDLER)
+extern uint32_t pios_packet_handler;
+#define PIOS_PACKET_HANDLER (pios_packet_handler)
+#define PIOS_PH_MAX_PACKET 255
+#define PIOS_PH_WIN_SIZE 3
+#define PIOS_PH_MAX_CONNECTIONS 1
+#define RS_ECC_NPARITY 4
+#endif /* PIOS_INCLUDE_PACKET_HANDLER */
 
 //-------------------------
 // Packet Handler
 //-------------------------
 
+#if defined(PIOS_INCLUDE_PACKET_HANDLER)
 uint32_t pios_packet_handler;
-#define PIOS_INCLUDE_PACKET_HANDLER
+#define PIOS_PACKET_HANDLER (pios_packet_handler)
 #define PIOS_PH_MAX_PACKET 255
 #define PIOS_PH_WIN_SIZE 3
 #define PIOS_PH_MAX_CONNECTIONS 1
+#endif /* PIOS_INCLUDE_PACKET_HANDLER */
 
 //-------------------------
 // Reed-Solomon ECC

flight/PiOS/Boards/STM32F4xx_RevoMini.h

@@ -0,0 +1,297 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotSystem OpenPilot System
+ * @{
+ * @addtogroup OpenPilotCore OpenPilot Core
+ * @{
+ * @file       pios_board.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @brief      Defines board hardware for the OpenPilot Version 1.1 hardware.
+ * @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 STM3210E_INS_H_
+
+#include <stdbool.h>
+
+#if defined(PIOS_INCLUDE_DEBUG_CONSOLE)
+#define DEBUG_LEVEL 0
+#define DEBUG_PRINTF(level, ...) {if(level <= DEBUG_LEVEL && pios_com_aux_id > 0) { PIOS_COM_SendFormattedStringNonBlocking(pios_com_aux_id, __VA_ARGS__); }}
+#else
+#define DEBUG_PRINTF(level, ...)
+#endif	/* PIOS_INCLUDE_DEBUG_CONSOLE */
+
+//------------------------
+// Timers and Channels Used
+//------------------------
+/*
+Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4
+------+-----------+-----------+-----------+----------
+TIM1  |           |           |           |
+TIM2  | --------------- PIOS_DELAY -----------------
+TIM3  |           |           |           |
+TIM4  |           |           |           |
+TIM5  |           |           |           |
+TIM6  |           |           |           |
+TIM7  |           |           |           |
+TIM8  |           |           |           |
+------+-----------+-----------+-----------+----------
+*/
+
+//------------------------
+// DMA Channels Used
+//------------------------
+/* Channel 1  -                                 */
+/* Channel 2  - SPI1 RX                         */
+/* Channel 3  - SPI1 TX                         */
+/* Channel 4  - SPI2 RX                         */
+/* Channel 5  - SPI2 TX                         */
+/* Channel 6  -                                 */
+/* Channel 7  -                                 */
+/* Channel 8  -                                 */
+/* Channel 9  -                                 */
+/* Channel 10 -                                 */
+/* Channel 11 -                                 */
+/* Channel 12 -                                 */
+
+//------------------------
+// BOOTLOADER_SETTINGS
+//------------------------
+#define BOARD_READABLE	true
+#define BOARD_WRITABLE	true
+#define MAX_DEL_RETRYS	3
+
+
+//------------------------
+// PIOS_LED
+//------------------------
+#define PIOS_LED_HEARTBEAT	0
+#define PIOS_LED_ALARM		1
+
+//------------------------
+// PIOS_SPI
+// See also pios_board.c
+//------------------------
+#define PIOS_SPI_MAX_DEVS        3
+
+//------------------------
+// PIOS_WDG
+//------------------------
+#define PIOS_WATCHDOG_TIMEOUT    250
+#define PIOS_WDG_REGISTER        RTC_BKP_DR4
+#define PIOS_WDG_ACTUATOR        0x0001
+#define PIOS_WDG_STABILIZATION   0x0002
+#define PIOS_WDG_ATTITUDE        0x0004
+#define PIOS_WDG_MANUAL          0x0008
+#define PIOS_WDG_SENSORS         0x0010
+
+//------------------------
+// PIOS_I2C
+// See also pios_board.c
+//------------------------
+#define PIOS_I2C_MAX_DEVS			3
+extern uint32_t pios_i2c_mag_pressure_adapter_id;
+#define PIOS_I2C_MAIN_ADAPTER			(pios_i2c_mag_pressure_adapter_id)
+extern uint32_t pios_i2c_flexiport_adapter_id;
+#define PIOS_I2C_FLEXI_ADAPTER			(pios_i2c_flexiport_adapter_id)
+#define PIOS_I2C_ETASV3_ADAPTER			(PIOS_I2C_FLEXI_ADAPTER)
+
+//-------------------------
+// PIOS_USART
+//
+// See also pios_board.c
+//-------------------------
+#define PIOS_USART_MAX_DEVS             5
+
+//-------------------------
+// PIOS_COM
+//
+// See also pios_board.c
+//-------------------------
+#define PIOS_COM_MAX_DEVS               4
+extern uint32_t pios_com_telem_rf_id;
+extern uint32_t pios_com_gps_id;
+extern uint32_t pios_com_telem_usb_id;
+extern uint32_t pios_com_bridge_id;
+extern uint32_t pios_com_vcp_id;
+#define PIOS_COM_GPS                    (pios_com_gps_id)
+#define PIOS_COM_TELEM_USB              (pios_com_telem_usb_id)
+#define PIOS_COM_TELEM_RF               (pios_com_telem_rf_id)
+#define PIOS_COM_BRIDGE                 (pios_com_bridge_id)
+#define PIOS_COM_VCP                    (pios_com_vcp_id)
+
+#if defined(PIOS_INCLUDE_DEBUG_CONSOLE)
+extern uint32_t pios_com_debug_id;
+#define PIOS_COM_DEBUG                  (pios_com_debug_id)
+#endif	/* PIOS_INCLUDE_DEBUG_CONSOLE */
+
+#if defined(PIOS_INCLUDE_RFM22B)
+#define PIOS_COM_RFM22B_RF_RX_BUF_LEN 512
+#define PIOS_COM_RFM22B_RF_TX_BUF_LEN 512
+extern uint32_t pios_com_rfm22b_id;
+#define PIOS_COM_RADIO                  (pios_com_rfm22b_id)
+extern uint32_t pios_spi_telem_flash_id;
+#define PIOS_RFM22_SPI_PORT             (pios_spi_telem_flash_id)
+#endif /* PIOS_INCLUDE_RFM22B */
+
+//-------------------------
+// Packet Handler
+//-------------------------
+#define RS_ECC_NPARITY 4
+#define PIOS_PH_MAX_PACKET 255
+#define PIOS_PH_WIN_SIZE 3
+#define PIOS_PH_MAX_CONNECTIONS 1
+extern uint32_t pios_packet_handler;
+#define PIOS_PACKET_HANDLER (pios_packet_handler)
+
+//------------------------
+// TELEMETRY 
+//------------------------
+#define TELEM_QUEUE_SIZE         80
+#define PIOS_TELEM_STACK_SIZE    624
+
+//-------------------------
+// System Settings
+// 
+// See also System_stm32f4xx.c
+//-------------------------
+//These macros are deprecated
+//please use PIOS_PERIPHERAL_APBx_CLOCK According to the table below
+//#define PIOS_MASTER_CLOCK                       
+//#define PIOS_PERIPHERAL_CLOCK                   
+//#define PIOS_PERIPHERAL_CLOCK							
+
+#define PIOS_SYSCLK										168000000
+//	Peripherals that belongs to APB1 are:
+//	DAC			|PWR				|CAN1,2
+//	I2C1,2,3		|UART4,5			|USART3,2
+//	I2S3Ext		|SPI3/I2S3		|SPI2/I2S2
+//	I2S2Ext		|IWDG				|WWDG
+//	RTC/BKP reg	
+// TIM2,3,4,5,6,7,12,13,14
+
+// Calculated as SYSCLK / APBPresc * (APBPre == 1 ? 1 : 2)   
+// Default APB1 Prescaler = 4 
+#define PIOS_PERIPHERAL_APB1_CLOCK					(PIOS_SYSCLK / 2)
+
+//	Peripherals belonging to APB2
+//	SDIO			|EXTI				|SYSCFG			|SPI1
+//	ADC1,2,3				
+//	USART1,6
+//	TIM1,8,9,10,11
+//
+// Default APB2 Prescaler = 2
+//
+#define PIOS_PERIPHERAL_APB2_CLOCK					PIOS_SYSCLK
+
+
+//-------------------------
+// Interrupt Priorities
+//-------------------------
+#define PIOS_IRQ_PRIO_LOW                       12              // lower than RTOS
+#define PIOS_IRQ_PRIO_MID                       8               // higher than RTOS
+#define PIOS_IRQ_PRIO_HIGH                      5               // for SPI, ADC, I2C etc...
+#define PIOS_IRQ_PRIO_HIGHEST                   4               // for USART etc...
+
+//------------------------
+// PIOS_RCVR
+// See also pios_board.c
+//------------------------
+#define PIOS_RCVR_MAX_DEVS           3
+#define PIOS_RCVR_MAX_CHANNELS       12
+#define PIOS_GCSRCVR_TIMEOUT_MS      100
+
+//-------------------------
+// Receiver PPM input
+//-------------------------
+#define PIOS_PPM_MAX_DEVS            1
+#define PIOS_PPM_NUM_INPUTS          12
+
+//-------------------------
+// Receiver PWM input
+//-------------------------
+#define PIOS_PWM_MAX_DEVS            1
+#define PIOS_PWM_NUM_INPUTS          8
+
+//-------------------------
+// Receiver SPEKTRUM input
+//-------------------------
+#define PIOS_SPEKTRUM_MAX_DEVS       2
+#define PIOS_SPEKTRUM_NUM_INPUTS     12
+
+//-------------------------
+// Receiver S.Bus input
+//-------------------------
+#define PIOS_SBUS_MAX_DEVS           1
+#define PIOS_SBUS_NUM_INPUTS         (16+2)
+
+//-------------------------
+// Receiver DSM input
+//-------------------------
+#define PIOS_DSM_MAX_DEVS			2
+#define PIOS_DSM_NUM_INPUTS			12
+
+//-------------------------
+// Servo outputs
+//-------------------------
+#define PIOS_SERVO_UPDATE_HZ                    50
+#define PIOS_SERVOS_INITIAL_POSITION            0 /* dont want to start motors, have no pulse till settings loaded */
+
+//--------------------------
+// Timer controller settings
+//--------------------------
+#define PIOS_TIM_MAX_DEVS			6
+
+//-------------------------
+// ADC
+// PIOS_ADC_PinGet(0) = Current sensor
+// PIOS_ADC_PinGet(1) = Voltage sensor
+// PIOS_ADC_PinGet(4) = VREF
+// PIOS_ADC_PinGet(5) = Temperature sensor
+//-------------------------
+#define PIOS_DMA_PIN_CONFIG                                                                 \
+{                                                                                           \
+	{GPIOC, GPIO_Pin_1,     ADC_Channel_11},                                                \
+	{GPIOC, GPIO_Pin_2,     ADC_Channel_12},                                                \
+	{NULL,  0,              ADC_Channel_Vrefint},           /* Voltage reference */         \
+	{NULL,  0,              ADC_Channel_TempSensor},        /* Temperature sensor */        \
+	{GPIOC, GPIO_Pin_2,     ADC_Channel_12}  \
+}
+
+/* we have to do all this to satisfy the PIOS_ADC_MAX_SAMPLES define in pios_adc.h */
+/* which is annoying because this then determines the rate at which we generate buffer turnover events */
+/* the objective here is to get enough buffer space to support 100Hz averaging rate */
+#define PIOS_ADC_NUM_CHANNELS           4
+#define PIOS_ADC_MAX_OVERSAMPLING       2
+#define PIOS_ADC_USE_ADC2               0
+#define PIOS_ADC_VOLTAGE_SCALE 3.30/4096.0
+
+//-------------------------
+// USB
+//-------------------------
+#define PIOS_USB_MAX_DEVS                       1
+#define PIOS_USB_ENABLED                        1 /* Should remove all references to this */
+#define PIOS_USB_HID_MAX_DEVS                   1
+
+#endif /* STM3210E_INS_H_ */
+/**
+ * @}
+ * @}
+ */

flight/PiOS/Boards/STM32F4xx_Revolution.h

@@ -189,6 +189,7 @@ extern uint32_t pios_com_vcp_id;
 //------------------------
 #define PIOS_RCVR_MAX_DEVS           3
 #define PIOS_RCVR_MAX_CHANNELS       12
+#define PIOS_GCSRCVR_TIMEOUT_MS      100
 
 //-------------------------
 // Receiver PPM input
@@ -242,8 +243,9 @@ extern uint32_t pios_com_vcp_id;
 {                                                                                                   \
 	{GPIOC, GPIO_Pin_0,     ADC_Channel_10},                                                        \
 	{GPIOC, GPIO_Pin_1,     ADC_Channel_11},                                                        \
-	{NULL,  0,              ADC_Channel_Vrefint},           /* Voltage reference */         \
-	{NULL,  0,              ADC_Channel_TempSensor}         /* Temperature sensor */        \
+	{NULL,  0,                      ADC_Channel_Vrefint},           /* Voltage reference */         \
+	{NULL,  0,                      ADC_Channel_TempSensor},         /* Temperature sensor */        \
+	{GPIOC, GPIO_Pin_2,     ADC_Channel_12}  \
 }
 
 /* we have to do all this to satisfy the PIOS_ADC_MAX_SAMPLES define in pios_adc.h */
@@ -252,6 +254,7 @@ extern uint32_t pios_com_vcp_id;
 #define PIOS_ADC_NUM_CHANNELS           4
 #define PIOS_ADC_MAX_OVERSAMPLING       2
 #define PIOS_ADC_USE_ADC2               0
+#define PIOS_ADC_VOLTAGE_SCALE 3.30/4096.0
 
 //-------------------------
 // USB

flight/PiOS/Boards/pios_board.h

@@ -15,6 +15,8 @@
 #include "STM32F4xx_Revolution.h"
 #elif USE_STM32F4xx_OSD
 #include "STM32F4xx_OSD.h"
+#elif USE_STM32F4xx_RM
+#include "STM32F4xx_RevoMini.h"
 #else
 #error Board definition has not been provided.
 #endif

flight/PiOS/Common/Libraries/FreeRTOS/Source/include/FreeRTOS.h

@@ -1,473 +1,522 @@
-/*
-    FreeRTOS V7.0.2 - Copyright (C) 2011 Real Time Engineers Ltd.
-	
-
-    ***************************************************************************
-     *                                                                       *
-     *    FreeRTOS tutorial books are available in pdf and paperback.        *
-     *    Complete, revised, and edited pdf reference manuals are also       *
-     *    available.                                                         *
-     *                                                                       *
-     *    Purchasing FreeRTOS documentation will not only help you, by       *
-     *    ensuring you get running as quickly as possible and with an        *
-     *    in-depth knowledge of how to use FreeRTOS, it will also help       *
-     *    the FreeRTOS project to continue with its mission of providing     *
-     *    professional grade, cross platform, de facto standard solutions    *
-     *    for microcontrollers - completely free of charge!                  *
-     *                                                                       *
-     *    >>> See http://www.FreeRTOS.org/Documentation for details. <<<     *
-     *                                                                       *
-     *    Thank you for using FreeRTOS, and thank you for your support!      *
-     *                                                                       *
-    ***************************************************************************
-
-
-    This file is part of the FreeRTOS distribution.
-
-    FreeRTOS is free software; you can redistribute it and/or modify it under
-    the terms of the GNU General Public License (version 2) as published by the
-    Free Software Foundation AND MODIFIED BY the FreeRTOS exception.
-    >>>NOTE<<< The modification to the GPL is included to allow you to
-    distribute a combined work that includes FreeRTOS without being obliged to
-    provide the source code for proprietary components outside of the FreeRTOS
-    kernel.  FreeRTOS 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 and the FreeRTOS license exception along with FreeRTOS; if not it
-    can be viewed here: http://www.freertos.org/a00114.html and also obtained
-    by writing to Richard Barry, contact details for whom are available on the
-    FreeRTOS WEB site.
-
-    1 tab == 4 spaces!
-
-    http://www.FreeRTOS.org - Documentation, latest information, license and
-    contact details.
-
-    http://www.SafeRTOS.com - A version that is certified for use in safety
-    critical systems.
-
-    http://www.OpenRTOS.com - Commercial support, development, porting,
-    licensing and training services.
-*/
-
-#ifndef INC_FREERTOS_H
-#define INC_FREERTOS_H
-
-
-/*
- * Include the generic headers required for the FreeRTOS port being used.
- */
-#include <stddef.h>
-
-/* Basic FreeRTOS definitions. */
-#include "projdefs.h"
-
-/* Application specific configuration options. */
-#include "FreeRTOSConfig.h"
-
-/* Definitions specific to the port being used. */
-#include "portable.h"
-
-
-/* Defines the prototype to which the application task hook function must
-conform. */
-typedef portBASE_TYPE (*pdTASK_HOOK_CODE)( void * );
-
-
-
-
-
-/*
- * Check all the required application specific macros have been defined.
- * These macros are application specific and (as downloaded) are defined
- * within FreeRTOSConfig.h.
- */
-
-#ifndef configUSE_PREEMPTION
-	#error Missing definition:  configUSE_PREEMPTION should be defined in FreeRTOSConfig.h as either 1 or 0.  See the Configuration section of the FreeRTOS API documentation for details.
-#endif
-
-#ifndef configUSE_IDLE_HOOK
-	#error Missing definition:  configUSE_IDLE_HOOK should be defined in FreeRTOSConfig.h as either 1 or 0.  See the Configuration section of the FreeRTOS API documentation for details.
-#endif
-
-#ifndef configUSE_TICK_HOOK
-	#error Missing definition:  configUSE_TICK_HOOK should be defined in FreeRTOSConfig.h as either 1 or 0.  See the Configuration section of the FreeRTOS API documentation for details.
-#endif
-
-#ifndef configUSE_CO_ROUTINES
-	#error  Missing definition:  configUSE_CO_ROUTINES should be defined in FreeRTOSConfig.h as either 1 or 0.  See the Configuration section of the FreeRTOS API documentation for details.
-#endif
-
-#ifndef INCLUDE_vTaskPrioritySet
-	#error Missing definition:  INCLUDE_vTaskPrioritySet should be defined in FreeRTOSConfig.h as either 1 or 0.  See the Configuration section of the FreeRTOS API documentation for details.
-#endif
-
-#ifndef INCLUDE_uxTaskPriorityGet
-	#error Missing definition:  INCLUDE_uxTaskPriorityGet should be defined in FreeRTOSConfig.h as either 1 or 0.  See the Configuration section of the FreeRTOS API documentation for details.
-#endif
-
-#ifndef INCLUDE_vTaskDelete		
-	#error Missing definition:  INCLUDE_vTaskDelete		 should be defined in FreeRTOSConfig.h as either 1 or 0.  See the Configuration section of the FreeRTOS API documentation for details.
-#endif
-
-#ifndef INCLUDE_vTaskSuspend	
-	#error Missing definition:  INCLUDE_vTaskSuspend	 should be defined in FreeRTOSConfig.h as either 1 or 0.  See the Configuration section of the FreeRTOS API documentation for details.
-#endif
-
-#ifndef INCLUDE_vTaskDelayUntil
-	#error Missing definition:  INCLUDE_vTaskDelayUntil should be defined in FreeRTOSConfig.h as either 1 or 0.  See the Configuration section of the FreeRTOS API documentation for details.
-#endif
-
-#ifndef INCLUDE_vTaskDelay
-	#error Missing definition:  INCLUDE_vTaskDelay should be defined in FreeRTOSConfig.h as either 1 or 0.  See the Configuration section of the FreeRTOS API documentation for details.
-#endif
-
-#ifndef configUSE_16_BIT_TICKS
-	#error Missing definition:  configUSE_16_BIT_TICKS should be defined in FreeRTOSConfig.h as either 1 or 0.  See the Configuration section of the FreeRTOS API documentation for details.
-#endif
-
-#ifndef INCLUDE_xTaskGetIdleTaskHandle
-	#define INCLUDE_xTaskGetIdleTaskHandle 0
-#endif
-
-#ifndef INCLUDE_xTimerGetTimerDaemonTaskHandle
-	#define INCLUDE_xTimerGetTimerDaemonTaskHandle 0
-#endif
-
-#ifndef INCLUDE_pcTaskGetTaskName
-	#define INCLUDE_pcTaskGetTaskName 0
-#endif
-
-#ifndef configUSE_APPLICATION_TASK_TAG
-	#define configUSE_APPLICATION_TASK_TAG 0
-#endif
-
-#ifndef INCLUDE_uxTaskGetStackHighWaterMark
-	#define INCLUDE_uxTaskGetStackHighWaterMark 0
-#endif
-
-#ifndef configUSE_RECURSIVE_MUTEXES
-	#define configUSE_RECURSIVE_MUTEXES 0
-#endif
-
-#ifndef configUSE_MUTEXES
-	#define configUSE_MUTEXES 0
-#endif
-
-#ifndef configUSE_TIMERS
-	#define configUSE_TIMERS 0
-#endif
-
-#ifndef configUSE_COUNTING_SEMAPHORES
-	#define configUSE_COUNTING_SEMAPHORES 0
-#endif
-
-#ifndef configUSE_ALTERNATIVE_API
-	#define configUSE_ALTERNATIVE_API 0
-#endif
-
-#ifndef portCRITICAL_NESTING_IN_TCB
-	#define portCRITICAL_NESTING_IN_TCB 0
-#endif
-
-#ifndef configMAX_TASK_NAME_LEN
-	#define configMAX_TASK_NAME_LEN 16
-#endif
-
-#ifndef configIDLE_SHOULD_YIELD
-	#define configIDLE_SHOULD_YIELD		1
-#endif
-
-#if configMAX_TASK_NAME_LEN < 1
-	#error configMAX_TASK_NAME_LEN must be set to a minimum of 1 in FreeRTOSConfig.h
-#endif
-
-#ifndef INCLUDE_xTaskResumeFromISR
-	#define INCLUDE_xTaskResumeFromISR 1
-#endif
-
-#ifndef configASSERT
-	#define configASSERT( x )
-#endif
-
-/* The timers module relies on xTaskGetSchedulerState(). */
-#if configUSE_TIMERS == 1
-
-	#ifndef configTIMER_TASK_PRIORITY
-		#error If configUSE_TIMERS is set to 1 then configTIMER_TASK_PRIORITY must also be defined.
-	#endif /* configTIMER_TASK_PRIORITY */
-
-	#ifndef configTIMER_QUEUE_LENGTH
-		#error If configUSE_TIMERS is set to 1 then configTIMER_QUEUE_LENGTH must also be defined.
-	#endif /* configTIMER_QUEUE_LENGTH */
-
-	#ifndef configTIMER_TASK_STACK_DEPTH
-		#error If configUSE_TIMERS is set to 1 then configTIMER_TASK_STACK_DEPTH must also be defined.
-	#endif /* configTIMER_TASK_STACK_DEPTH */
-
-#endif /* configUSE_TIMERS */
-
-#ifndef INCLUDE_xTaskGetSchedulerState
-	#define INCLUDE_xTaskGetSchedulerState 0
-#endif
-
-#ifndef INCLUDE_xTaskGetCurrentTaskHandle
-	#define INCLUDE_xTaskGetCurrentTaskHandle 0
-#endif
-
-
-#ifndef portSET_INTERRUPT_MASK_FROM_ISR
-	#define portSET_INTERRUPT_MASK_FROM_ISR() 0
-#endif
-
-#ifndef portCLEAR_INTERRUPT_MASK_FROM_ISR
-	#define portCLEAR_INTERRUPT_MASK_FROM_ISR( uxSavedStatusValue ) ( void ) uxSavedStatusValue
-#endif
-
-
-#ifndef configQUEUE_REGISTRY_SIZE
-	#define configQUEUE_REGISTRY_SIZE 0U
-#endif
-
-#if ( configQUEUE_REGISTRY_SIZE < 1 )
-	#define vQueueAddToRegistry( xQueue, pcName )
-	#define vQueueUnregisterQueue( xQueue )
-#endif
-
-#ifndef portPOINTER_SIZE_TYPE
-	#define portPOINTER_SIZE_TYPE unsigned long
-#endif
-
-/* Remove any unused trace macros. */
-#ifndef traceSTART
-	/* Used to perform any necessary initialisation - for example, open a file
-	into which trace is to be written. */
-	#define traceSTART()
-#endif
-
-#ifndef traceEND
-	/* Use to close a trace, for example close a file into which trace has been
-	written. */
-	#define traceEND()
-#endif
-
-#ifndef traceTASK_SWITCHED_IN
-	/* Called after a task has been selected to run.  pxCurrentTCB holds a pointer
-	to the task control block of the selected task. */
-	#define traceTASK_SWITCHED_IN()
-#endif
-
-#ifndef traceTASK_SWITCHED_OUT
-	/* Called before a task has been selected to run.  pxCurrentTCB holds a pointer
-	to the task control block of the task being switched out. */
-	#define traceTASK_SWITCHED_OUT()
-#endif
-
-#ifndef traceBLOCKING_ON_QUEUE_RECEIVE
-	/* Task is about to block because it cannot read from a
-	queue/mutex/semaphore.  pxQueue is a pointer to the queue/mutex/semaphore
-	upon which the read was attempted.  pxCurrentTCB points to the TCB of the
-	task that attempted the read. */
-	#define traceBLOCKING_ON_QUEUE_RECEIVE( pxQueue )
-#endif
-
-#ifndef traceBLOCKING_ON_QUEUE_SEND
-	/* Task is about to block because it cannot write to a
-	queue/mutex/semaphore.  pxQueue is a pointer to the queue/mutex/semaphore
-	upon which the write was attempted.  pxCurrentTCB points to the TCB of the
-	task that attempted the write. */
-	#define traceBLOCKING_ON_QUEUE_SEND( pxQueue )
-#endif
-
-#ifndef configCHECK_FOR_STACK_OVERFLOW
-	#define configCHECK_FOR_STACK_OVERFLOW 0
-#endif
-
-/* The following event macros are embedded in the kernel API calls. */
-
-#ifndef traceQUEUE_CREATE	
-	#define traceQUEUE_CREATE( pxNewQueue )
-#endif
-
-#ifndef traceQUEUE_CREATE_FAILED
-	#define traceQUEUE_CREATE_FAILED()
-#endif
-
-#ifndef traceCREATE_MUTEX
-	#define traceCREATE_MUTEX( pxNewQueue )
-#endif
-
-#ifndef traceCREATE_MUTEX_FAILED
-	#define traceCREATE_MUTEX_FAILED()
-#endif
-
-#ifndef traceGIVE_MUTEX_RECURSIVE
-	#define traceGIVE_MUTEX_RECURSIVE( pxMutex )
-#endif
-
-#ifndef traceGIVE_MUTEX_RECURSIVE_FAILED
-	#define traceGIVE_MUTEX_RECURSIVE_FAILED( pxMutex )
-#endif
-
-#ifndef traceTAKE_MUTEX_RECURSIVE
-	#define traceTAKE_MUTEX_RECURSIVE( pxMutex )
-#endif
-
-#ifndef traceTAKE_MUTEX_RECURSIVE_FAILED
-	#define traceTAKE_MUTEX_RECURSIVE_FAILED( pxMutex )
-#endif
-
-#ifndef traceCREATE_COUNTING_SEMAPHORE
-	#define traceCREATE_COUNTING_SEMAPHORE()
-#endif
-
-#ifndef traceCREATE_COUNTING_SEMAPHORE_FAILED
-	#define traceCREATE_COUNTING_SEMAPHORE_FAILED()
-#endif
-
-#ifndef traceQUEUE_SEND
-	#define traceQUEUE_SEND( pxQueue )
-#endif
-
-#ifndef traceQUEUE_SEND_FAILED
-	#define traceQUEUE_SEND_FAILED( pxQueue )
-#endif
-
-#ifndef traceQUEUE_RECEIVE
-	#define traceQUEUE_RECEIVE( pxQueue )
-#endif
-
-#ifndef traceQUEUE_PEEK
-	#define traceQUEUE_PEEK( pxQueue )
-#endif
-
-#ifndef traceQUEUE_RECEIVE_FAILED
-	#define traceQUEUE_RECEIVE_FAILED( pxQueue )
-#endif
-
-#ifndef traceQUEUE_SEND_FROM_ISR
-	#define traceQUEUE_SEND_FROM_ISR( pxQueue )
-#endif
-
-#ifndef traceQUEUE_SEND_FROM_ISR_FAILED
-	#define traceQUEUE_SEND_FROM_ISR_FAILED( pxQueue )
-#endif
-
-#ifndef traceQUEUE_RECEIVE_FROM_ISR
-	#define traceQUEUE_RECEIVE_FROM_ISR( pxQueue )
-#endif
-
-#ifndef traceQUEUE_RECEIVE_FROM_ISR_FAILED
-	#define traceQUEUE_RECEIVE_FROM_ISR_FAILED( pxQueue )
-#endif
-
-#ifndef traceQUEUE_DELETE
-	#define traceQUEUE_DELETE( pxQueue )
-#endif
-
-#ifndef traceTASK_CREATE
-	#define traceTASK_CREATE( pxNewTCB )
-#endif
-
-#ifndef traceTASK_CREATE_FAILED
-	#define traceTASK_CREATE_FAILED()
-#endif
-
-#ifndef traceTASK_DELETE
-	#define traceTASK_DELETE( pxTaskToDelete )
-#endif
-
-#ifndef traceTASK_DELAY_UNTIL
-	#define traceTASK_DELAY_UNTIL()
-#endif
-
-#ifndef traceTASK_DELAY
-	#define traceTASK_DELAY()
-#endif
-
-#ifndef traceTASK_PRIORITY_SET
-	#define traceTASK_PRIORITY_SET( pxTask, uxNewPriority )
-#endif
-
-#ifndef traceTASK_SUSPEND
-	#define traceTASK_SUSPEND( pxTaskToSuspend )
-#endif
-
-#ifndef traceTASK_RESUME
-	#define traceTASK_RESUME( pxTaskToResume )
-#endif
-
-#ifndef traceTASK_RESUME_FROM_ISR
-	#define traceTASK_RESUME_FROM_ISR( pxTaskToResume )
-#endif
-
-#ifndef traceTASK_INCREMENT_TICK
-	#define traceTASK_INCREMENT_TICK( xTickCount )
-#endif
-
-#ifndef traceTIMER_CREATE
-	#define traceTIMER_CREATE( pxNewTimer )
-#endif
-
-#ifndef traceTIMER_CREATE_FAILED
-	#define traceTIMER_CREATE_FAILED()
-#endif
-
-#ifndef traceTIMER_COMMAND_SEND
-	#define traceTIMER_COMMAND_SEND( xTimer, xMessageID, xMessageValueValue, xReturn )
-#endif
-
-#ifndef traceTIMER_EXPIRED
-	#define traceTIMER_EXPIRED( pxTimer )
-#endif
-
-#ifndef traceTIMER_COMMAND_RECEIVED
-	#define traceTIMER_COMMAND_RECEIVED( pxTimer, xMessageID, xMessageValue )
-#endif
-
-#ifndef configGENERATE_RUN_TIME_STATS
-	#define configGENERATE_RUN_TIME_STATS 0
-#endif
-
-#if ( configGENERATE_RUN_TIME_STATS == 1 )
-
-	#ifndef portCONFIGURE_TIMER_FOR_RUN_TIME_STATS
-		#error If configGENERATE_RUN_TIME_STATS is defined then portCONFIGURE_TIMER_FOR_RUN_TIME_STATS must also be defined.  portCONFIGURE_TIMER_FOR_RUN_TIME_STATS should call a port layer function to setup a peripheral timer/counter that can then be used as the run time counter time base.
-	#endif /* portCONFIGURE_TIMER_FOR_RUN_TIME_STATS */
-
-	#ifndef portGET_RUN_TIME_COUNTER_VALUE
-		#ifndef portALT_GET_RUN_TIME_COUNTER_VALUE
-			#error If configGENERATE_RUN_TIME_STATS is defined then either portGET_RUN_TIME_COUNTER_VALUE or portALT_GET_RUN_TIME_COUNTER_VALUE must also be defined.  See the examples provided and the FreeRTOS web site for more information.
-		#endif /* portALT_GET_RUN_TIME_COUNTER_VALUE */
-	#endif /* portGET_RUN_TIME_COUNTER_VALUE */
-
-#endif /* configGENERATE_RUN_TIME_STATS */
-
-#ifndef portCONFIGURE_TIMER_FOR_RUN_TIME_STATS
-	#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS()
-#endif
-
-#ifndef configUSE_MALLOC_FAILED_HOOK
-	#define configUSE_MALLOC_FAILED_HOOK 0
-#endif
-
-#ifndef portPRIVILEGE_BIT
-	#define portPRIVILEGE_BIT ( ( unsigned portBASE_TYPE ) 0x00 )
-#endif
-
-#ifndef portYIELD_WITHIN_API
-	#define portYIELD_WITHIN_API portYIELD
-#endif
-
-#ifndef pvPortMallocAligned
-	#define pvPortMallocAligned( x, puxStackBuffer ) ( ( ( puxStackBuffer ) == NULL ) ? ( pvPortMalloc( ( x ) ) ) : ( puxStackBuffer ) )
-#endif
-
-#ifndef vPortFreeAligned
-	#define vPortFreeAligned( pvBlockToFree ) vPortFree( pvBlockToFree )
-#endif
-
-#endif /* INC_FREERTOS_H */
-
+/*
+    FreeRTOS V7.2.0 - Copyright (C) 2012 Real Time Engineers Ltd.
+	
+
+    ***************************************************************************
+     *                                                                       *
+     *    FreeRTOS tutorial books are available in pdf and paperback.        *
+     *    Complete, revised, and edited pdf reference manuals are also       *
+     *    available.                                                         *
+     *                                                                       *
+     *    Purchasing FreeRTOS documentation will not only help you, by       *
+     *    ensuring you get running as quickly as possible and with an        *
+     *    in-depth knowledge of how to use FreeRTOS, it will also help       *
+     *    the FreeRTOS project to continue with its mission of providing     *
+     *    professional grade, cross platform, de facto standard solutions    *
+     *    for microcontrollers - completely free of charge!                  *
+     *                                                                       *
+     *    >>> See http://www.FreeRTOS.org/Documentation for details. <<<     *
+     *                                                                       *
+     *    Thank you for using FreeRTOS, and thank you for your support!      *
+     *                                                                       *
+    ***************************************************************************
+
+
+    This file is part of the FreeRTOS distribution.
+
+    FreeRTOS is free software; you can redistribute it and/or modify it under
+    the terms of the GNU General Public License (version 2) as published by the
+    Free Software Foundation AND MODIFIED BY the FreeRTOS exception.
+    >>>NOTE<<< The modification to the GPL is included to allow you to
+    distribute a combined work that includes FreeRTOS without being obliged to
+    provide the source code for proprietary components outside of the FreeRTOS
+    kernel.  FreeRTOS 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 and the FreeRTOS license exception along with FreeRTOS; if not it
+    can be viewed here: http://www.freertos.org/a00114.html and also obtained
+    by writing to Richard Barry, contact details for whom are available on the
+    FreeRTOS WEB site.
+
+    1 tab == 4 spaces!
+    
+    ***************************************************************************
+     *                                                                       *
+     *    Having a problem?  Start by reading the FAQ "My application does   *
+     *    not run, what could be wrong?                                      *
+     *                                                                       *
+     *    http://www.FreeRTOS.org/FAQHelp.html                               *
+     *                                                                       *
+    ***************************************************************************
+
+    
+    http://www.FreeRTOS.org - Documentation, training, latest information, 
+    license and contact details.
+    
+    http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products,
+    including FreeRTOS+Trace - an indispensable productivity tool.
+
+    Real Time Engineers ltd license FreeRTOS to High Integrity Systems, who sell 
+    the code with commercial support, indemnification, and middleware, under 
+    the OpenRTOS brand: http://www.OpenRTOS.com.  High Integrity Systems also
+    provide a safety engineered and independently SIL3 certified version under 
+    the SafeRTOS brand: http://www.SafeRTOS.com.
+*/
+
+#ifndef INC_FREERTOS_H
+#define INC_FREERTOS_H
+
+
+/*
+ * Include the generic headers required for the FreeRTOS port being used.
+ */
+#include <stddef.h>
+
+/* Basic FreeRTOS definitions. */
+#include "projdefs.h"
+
+/* Application specific configuration options. */
+#include "FreeRTOSConfig.h"
+
+/* Definitions specific to the port being used. */
+#include "portable.h"
+
+
+/* Defines the prototype to which the application task hook function must
+conform. */
+typedef portBASE_TYPE (*pdTASK_HOOK_CODE)( void * );
+
+
+
+
+
+/*
+ * Check all the required application specific macros have been defined.
+ * These macros are application specific and (as downloaded) are defined
+ * within FreeRTOSConfig.h.
+ */
+
+#ifndef configUSE_PREEMPTION
+	#error Missing definition:  configUSE_PREEMPTION should be defined in FreeRTOSConfig.h as either 1 or 0.  See the Configuration section of the FreeRTOS API documentation for details.
+#endif
+
+#ifndef configUSE_IDLE_HOOK
+	#error Missing definition:  configUSE_IDLE_HOOK should be defined in FreeRTOSConfig.h as either 1 or 0.  See the Configuration section of the FreeRTOS API documentation for details.
+#endif
+
+#ifndef configUSE_TICK_HOOK
+	#error Missing definition:  configUSE_TICK_HOOK should be defined in FreeRTOSConfig.h as either 1 or 0.  See the Configuration section of the FreeRTOS API documentation for details.
+#endif
+
+#ifndef configUSE_CO_ROUTINES
+	#error  Missing definition:  configUSE_CO_ROUTINES should be defined in FreeRTOSConfig.h as either 1 or 0.  See the Configuration section of the FreeRTOS API documentation for details.
+#endif
+
+#ifndef INCLUDE_vTaskPrioritySet
+	#error Missing definition:  INCLUDE_vTaskPrioritySet should be defined in FreeRTOSConfig.h as either 1 or 0.  See the Configuration section of the FreeRTOS API documentation for details.
+#endif
+
+#ifndef INCLUDE_uxTaskPriorityGet
+	#error Missing definition:  INCLUDE_uxTaskPriorityGet should be defined in FreeRTOSConfig.h as either 1 or 0.  See the Configuration section of the FreeRTOS API documentation for details.
+#endif
+
+#ifndef INCLUDE_vTaskDelete		
+	#error Missing definition:  INCLUDE_vTaskDelete		 should be defined in FreeRTOSConfig.h as either 1 or 0.  See the Configuration section of the FreeRTOS API documentation for details.
+#endif
+
+#ifndef INCLUDE_vTaskSuspend	
+	#error Missing definition:  INCLUDE_vTaskSuspend	 should be defined in FreeRTOSConfig.h as either 1 or 0.  See the Configuration section of the FreeRTOS API documentation for details.
+#endif
+
+#ifndef INCLUDE_vTaskDelayUntil
+	#error Missing definition:  INCLUDE_vTaskDelayUntil should be defined in FreeRTOSConfig.h as either 1 or 0.  See the Configuration section of the FreeRTOS API documentation for details.
+#endif
+
+#ifndef INCLUDE_vTaskDelay
+	#error Missing definition:  INCLUDE_vTaskDelay should be defined in FreeRTOSConfig.h as either 1 or 0.  See the Configuration section of the FreeRTOS API documentation for details.
+#endif
+
+#ifndef configUSE_16_BIT_TICKS
+	#error Missing definition:  configUSE_16_BIT_TICKS should be defined in FreeRTOSConfig.h as either 1 or 0.  See the Configuration section of the FreeRTOS API documentation for details.
+#endif
+
+#ifndef INCLUDE_xTaskGetIdleTaskHandle
+	#define INCLUDE_xTaskGetIdleTaskHandle 0
+#endif
+
+#ifndef INCLUDE_xTimerGetTimerDaemonTaskHandle
+	#define INCLUDE_xTimerGetTimerDaemonTaskHandle 0
+#endif
+
+#ifndef INCLUDE_xQueueGetMutexHolder
+	#define INCLUDE_xQueueGetMutexHolder 0
+#endif
+
+#ifndef INCLUDE_pcTaskGetTaskName
+	#define INCLUDE_pcTaskGetTaskName 0
+#endif
+
+#ifndef configUSE_APPLICATION_TASK_TAG
+	#define configUSE_APPLICATION_TASK_TAG 0
+#endif
+
+#ifndef INCLUDE_uxTaskGetStackHighWaterMark
+	#define INCLUDE_uxTaskGetStackHighWaterMark 0
+#endif
+
+#ifndef configUSE_RECURSIVE_MUTEXES
+	#define configUSE_RECURSIVE_MUTEXES 0
+#endif
+
+#ifndef configUSE_MUTEXES
+	#define configUSE_MUTEXES 0
+#endif
+
+#ifndef configUSE_TIMERS
+	#define configUSE_TIMERS 0
+#endif
+
+#ifndef configUSE_COUNTING_SEMAPHORES
+	#define configUSE_COUNTING_SEMAPHORES 0
+#endif
+
+#ifndef configUSE_ALTERNATIVE_API
+	#define configUSE_ALTERNATIVE_API 0
+#endif
+
+#ifndef portCRITICAL_NESTING_IN_TCB
+	#define portCRITICAL_NESTING_IN_TCB 0
+#endif
+
+#ifndef configMAX_TASK_NAME_LEN
+	#define configMAX_TASK_NAME_LEN 16
+#endif
+
+#ifndef configIDLE_SHOULD_YIELD
+	#define configIDLE_SHOULD_YIELD		1
+#endif
+
+#if configMAX_TASK_NAME_LEN < 1
+	#error configMAX_TASK_NAME_LEN must be set to a minimum of 1 in FreeRTOSConfig.h
+#endif
+
+#ifndef INCLUDE_xTaskResumeFromISR
+	#define INCLUDE_xTaskResumeFromISR 1
+#endif
+
+#ifndef configASSERT
+	#define configASSERT( x )
+#endif
+
+#ifndef portALIGNMENT_ASSERT_pxCurrentTCB
+	#define portALIGNMENT_ASSERT_pxCurrentTCB configASSERT
+#endif
+
+/* The timers module relies on xTaskGetSchedulerState(). */
+#if configUSE_TIMERS == 1
+
+	#ifndef configTIMER_TASK_PRIORITY
+		#error If configUSE_TIMERS is set to 1 then configTIMER_TASK_PRIORITY must also be defined.
+	#endif /* configTIMER_TASK_PRIORITY */
+
+	#ifndef configTIMER_QUEUE_LENGTH
+		#error If configUSE_TIMERS is set to 1 then configTIMER_QUEUE_LENGTH must also be defined.
+	#endif /* configTIMER_QUEUE_LENGTH */
+
+	#ifndef configTIMER_TASK_STACK_DEPTH
+		#error If configUSE_TIMERS is set to 1 then configTIMER_TASK_STACK_DEPTH must also be defined.
+	#endif /* configTIMER_TASK_STACK_DEPTH */
+
+#endif /* configUSE_TIMERS */
+
+#ifndef INCLUDE_xTaskGetSchedulerState
+	#define INCLUDE_xTaskGetSchedulerState 0
+#endif
+
+#ifndef INCLUDE_xTaskGetCurrentTaskHandle
+	#define INCLUDE_xTaskGetCurrentTaskHandle 0
+#endif
+
+
+#ifndef portSET_INTERRUPT_MASK_FROM_ISR
+	#define portSET_INTERRUPT_MASK_FROM_ISR() 0
+#endif
+
+#ifndef portCLEAR_INTERRUPT_MASK_FROM_ISR
+	#define portCLEAR_INTERRUPT_MASK_FROM_ISR( uxSavedStatusValue ) ( void ) uxSavedStatusValue
+#endif
+
+#ifndef portCLEAN_UP_TCB
+	#define portCLEAN_UP_TCB( pxTCB ) ( void ) pxTCB
+#endif
+
+#ifndef portSETUP_TCB
+	#define portSETUP_TCB( pxTCB ) ( void ) pxTCB
+#endif
+
+#ifndef configQUEUE_REGISTRY_SIZE
+	#define configQUEUE_REGISTRY_SIZE 0U
+#endif
+
+#if ( configQUEUE_REGISTRY_SIZE < 1 )
+	#define vQueueAddToRegistry( xQueue, pcName )
+	#define vQueueUnregisterQueue( xQueue )
+#endif
+
+#ifndef portPOINTER_SIZE_TYPE
+	#define portPOINTER_SIZE_TYPE unsigned long
+#endif
+
+/* Remove any unused trace macros. */
+#ifndef traceSTART
+	/* Used to perform any necessary initialisation - for example, open a file
+	into which trace is to be written. */
+	#define traceSTART()
+#endif
+
+#ifndef traceEND
+	/* Use to close a trace, for example close a file into which trace has been
+	written. */
+	#define traceEND()
+#endif
+
+#ifndef traceTASK_SWITCHED_IN
+	/* Called after a task has been selected to run.  pxCurrentTCB holds a pointer
+	to the task control block of the selected task. */
+	#define traceTASK_SWITCHED_IN()
+#endif
+
+#ifndef traceTASK_SWITCHED_OUT
+	/* Called before a task has been selected to run.  pxCurrentTCB holds a pointer
+	to the task control block of the task being switched out. */
+	#define traceTASK_SWITCHED_OUT()
+#endif
+
+#ifndef traceTASK_PRIORITY_INHERIT
+	/* Called when a task attempts to take a mutex that is already held by a
+	lower priority task.  pxTCBOfMutexHolder is a pointer to the TCB of the task
+	that holds the mutex.  uxInheritedPriority is the priority the mutex holder
+	will inherit (the priority of the task that is attempting to obtain the
+	muted. */
+	#define traceTASK_PRIORITY_INHERIT( pxTCBOfMutexHolder, uxInheritedPriority )
+#endif
+
+#ifndef traceTASK_PRIORITY_DISINHERIT
+	/* Called when a task releases a mutex, the holding of which had resulted in
+	the task inheriting the priority of a higher priority task.  
+	pxTCBOfMutexHolder is a pointer to the TCB of the task that is releasing the
+	mutex.  uxOriginalPriority is the task's configured (base) priority. */
+	#define traceTASK_PRIORITY_DISINHERIT( pxTCBOfMutexHolder, uxOriginalPriority )
+#endif
+
+#ifndef traceBLOCKING_ON_QUEUE_RECEIVE
+	/* Task is about to block because it cannot read from a
+	queue/mutex/semaphore.  pxQueue is a pointer to the queue/mutex/semaphore
+	upon which the read was attempted.  pxCurrentTCB points to the TCB of the
+	task that attempted the read. */
+	#define traceBLOCKING_ON_QUEUE_RECEIVE( pxQueue )
+#endif
+
+#ifndef traceBLOCKING_ON_QUEUE_SEND
+	/* Task is about to block because it cannot write to a
+	queue/mutex/semaphore.  pxQueue is a pointer to the queue/mutex/semaphore
+	upon which the write was attempted.  pxCurrentTCB points to the TCB of the
+	task that attempted the write. */
+	#define traceBLOCKING_ON_QUEUE_SEND( pxQueue )
+#endif
+
+#ifndef configCHECK_FOR_STACK_OVERFLOW
+	#define configCHECK_FOR_STACK_OVERFLOW 0
+#endif
+
+/* The following event macros are embedded in the kernel API calls. */
+
+#ifndef traceMOVED_TASK_TO_READY_STATE
+	#define traceMOVED_TASK_TO_READY_STATE( pxTCB )
+#endif
+
+#ifndef traceQUEUE_CREATE	
+	#define traceQUEUE_CREATE( pxNewQueue )
+#endif
+
+#ifndef traceQUEUE_CREATE_FAILED
+	#define traceQUEUE_CREATE_FAILED( ucQueueType )
+#endif
+
+#ifndef traceCREATE_MUTEX
+	#define traceCREATE_MUTEX( pxNewQueue )
+#endif
+
+#ifndef traceCREATE_MUTEX_FAILED
+	#define traceCREATE_MUTEX_FAILED()
+#endif
+
+#ifndef traceGIVE_MUTEX_RECURSIVE
+	#define traceGIVE_MUTEX_RECURSIVE( pxMutex )
+#endif
+
+#ifndef traceGIVE_MUTEX_RECURSIVE_FAILED
+	#define traceGIVE_MUTEX_RECURSIVE_FAILED( pxMutex )
+#endif
+
+#ifndef traceTAKE_MUTEX_RECURSIVE
+	#define traceTAKE_MUTEX_RECURSIVE( pxMutex )
+#endif
+
+#ifndef traceTAKE_MUTEX_RECURSIVE_FAILED
+	#define traceTAKE_MUTEX_RECURSIVE_FAILED( pxMutex )
+#endif
+
+#ifndef traceCREATE_COUNTING_SEMAPHORE
+	#define traceCREATE_COUNTING_SEMAPHORE()
+#endif
+
+#ifndef traceCREATE_COUNTING_SEMAPHORE_FAILED
+	#define traceCREATE_COUNTING_SEMAPHORE_FAILED()
+#endif
+
+#ifndef traceQUEUE_SEND
+	#define traceQUEUE_SEND( pxQueue )
+#endif
+
+#ifndef traceQUEUE_SEND_FAILED
+	#define traceQUEUE_SEND_FAILED( pxQueue )
+#endif
+
+#ifndef traceQUEUE_RECEIVE
+	#define traceQUEUE_RECEIVE( pxQueue )
+#endif
+
+#ifndef traceQUEUE_PEEK
+	#define traceQUEUE_PEEK( pxQueue )
+#endif
+
+#ifndef traceQUEUE_RECEIVE_FAILED
+	#define traceQUEUE_RECEIVE_FAILED( pxQueue )
+#endif
+
+#ifndef traceQUEUE_SEND_FROM_ISR
+	#define traceQUEUE_SEND_FROM_ISR( pxQueue )
+#endif
+
+#ifndef traceQUEUE_SEND_FROM_ISR_FAILED
+	#define traceQUEUE_SEND_FROM_ISR_FAILED( pxQueue )
+#endif
+
+#ifndef traceQUEUE_RECEIVE_FROM_ISR
+	#define traceQUEUE_RECEIVE_FROM_ISR( pxQueue )
+#endif
+
+#ifndef traceQUEUE_RECEIVE_FROM_ISR_FAILED
+	#define traceQUEUE_RECEIVE_FROM_ISR_FAILED( pxQueue )
+#endif
+
+#ifndef traceQUEUE_DELETE
+	#define traceQUEUE_DELETE( pxQueue )
+#endif
+
+#ifndef traceTASK_CREATE
+	#define traceTASK_CREATE( pxNewTCB )
+#endif
+
+#ifndef traceTASK_CREATE_FAILED
+	#define traceTASK_CREATE_FAILED()
+#endif
+
+#ifndef traceTASK_DELETE
+	#define traceTASK_DELETE( pxTaskToDelete )
+#endif
+
+#ifndef traceTASK_DELAY_UNTIL
+	#define traceTASK_DELAY_UNTIL()
+#endif
+
+#ifndef traceTASK_DELAY
+	#define traceTASK_DELAY()
+#endif
+
+#ifndef traceTASK_PRIORITY_SET
+	#define traceTASK_PRIORITY_SET( pxTask, uxNewPriority )
+#endif
+
+#ifndef traceTASK_SUSPEND
+	#define traceTASK_SUSPEND( pxTaskToSuspend )
+#endif
+
+#ifndef traceTASK_RESUME
+	#define traceTASK_RESUME( pxTaskToResume )
+#endif
+
+#ifndef traceTASK_RESUME_FROM_ISR
+	#define traceTASK_RESUME_FROM_ISR( pxTaskToResume )
+#endif
+
+#ifndef traceTASK_INCREMENT_TICK
+	#define traceTASK_INCREMENT_TICK( xTickCount )
+#endif
+
+#ifndef traceTIMER_CREATE
+	#define traceTIMER_CREATE( pxNewTimer )
+#endif
+
+#ifndef traceTIMER_CREATE_FAILED
+	#define traceTIMER_CREATE_FAILED()
+#endif
+
+#ifndef traceTIMER_COMMAND_SEND
+	#define traceTIMER_COMMAND_SEND( xTimer, xMessageID, xMessageValueValue, xReturn )
+#endif
+
+#ifndef traceTIMER_EXPIRED
+	#define traceTIMER_EXPIRED( pxTimer )
+#endif
+
+#ifndef traceTIMER_COMMAND_RECEIVED
+	#define traceTIMER_COMMAND_RECEIVED( pxTimer, xMessageID, xMessageValue )
+#endif
+
+#ifndef configGENERATE_RUN_TIME_STATS
+	#define configGENERATE_RUN_TIME_STATS 0
+#endif
+
+#if ( configGENERATE_RUN_TIME_STATS == 1 )
+
+	#ifndef portCONFIGURE_TIMER_FOR_RUN_TIME_STATS
+		#error If configGENERATE_RUN_TIME_STATS is defined then portCONFIGURE_TIMER_FOR_RUN_TIME_STATS must also be defined.  portCONFIGURE_TIMER_FOR_RUN_TIME_STATS should call a port layer function to setup a peripheral timer/counter that can then be used as the run time counter time base.
+	#endif /* portCONFIGURE_TIMER_FOR_RUN_TIME_STATS */
+
+	#ifndef portGET_RUN_TIME_COUNTER_VALUE
+		#ifndef portALT_GET_RUN_TIME_COUNTER_VALUE
+			#error If configGENERATE_RUN_TIME_STATS is defined then either portGET_RUN_TIME_COUNTER_VALUE or portALT_GET_RUN_TIME_COUNTER_VALUE must also be defined.  See the examples provided and the FreeRTOS web site for more information.
+		#endif /* portALT_GET_RUN_TIME_COUNTER_VALUE */
+	#endif /* portGET_RUN_TIME_COUNTER_VALUE */
+
+#endif /* configGENERATE_RUN_TIME_STATS */
+
+#ifndef portCONFIGURE_TIMER_FOR_RUN_TIME_STATS
+	#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS()
+#endif
+
+#ifndef configUSE_MALLOC_FAILED_HOOK
+	#define configUSE_MALLOC_FAILED_HOOK 0
+#endif
+
+#ifndef portPRIVILEGE_BIT
+	#define portPRIVILEGE_BIT ( ( unsigned portBASE_TYPE ) 0x00 )
+#endif
+
+#ifndef portYIELD_WITHIN_API
+	#define portYIELD_WITHIN_API portYIELD
+#endif
+
+#ifndef pvPortMallocAligned
+	#define pvPortMallocAligned( x, puxStackBuffer ) ( ( ( puxStackBuffer ) == NULL ) ? ( pvPortMalloc( ( x ) ) ) : ( puxStackBuffer ) )
+#endif
+
+#ifndef vPortFreeAligned
+	#define vPortFreeAligned( pvBlockToFree ) vPortFree( pvBlockToFree )
+#endif
+
+#endif /* INC_FREERTOS_H */
+

flight/PiOS/Common/Libraries/FreeRTOS/Source/include/StackMacros.h

@@ -1,168 +1,181 @@
-/*
-    FreeRTOS V7.0.2 - Copyright (C) 2011 Real Time Engineers Ltd.
-
-
-    ***************************************************************************
-     *                                                                       *
-     *    FreeRTOS tutorial books are available in pdf and paperback.        *
-     *    Complete, revised, and edited pdf reference manuals are also       *
-     *    available.                                                         *
-     *                                                                       *
-     *    Purchasing FreeRTOS documentation will not only help you, by       *
-     *    ensuring you get running as quickly as possible and with an        *
-     *    in-depth knowledge of how to use FreeRTOS, it will also help       *
-     *    the FreeRTOS project to continue with its mission of providing     *
-     *    professional grade, cross platform, de facto standard solutions    *
-     *    for microcontrollers - completely free of charge!                  *
-     *                                                                       *
-     *    >>> See http://www.FreeRTOS.org/Documentation for details. <<<     *
-     *                                                                       *
-     *    Thank you for using FreeRTOS, and thank you for your support!      *
-     *                                                                       *
-    ***************************************************************************
-
-
-    This file is part of the FreeRTOS distribution.
-
-    FreeRTOS is free software; you can redistribute it and/or modify it under
-    the terms of the GNU General Public License (version 2) as published by the
-    Free Software Foundation AND MODIFIED BY the FreeRTOS exception.
-    >>>NOTE<<< The modification to the GPL is included to allow you to
-    distribute a combined work that includes FreeRTOS without being obliged to
-    provide the source code for proprietary components outside of the FreeRTOS
-    kernel.  FreeRTOS 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 and the FreeRTOS license exception along with FreeRTOS; if not it
-    can be viewed here: http://www.freertos.org/a00114.html and also obtained
-    by writing to Richard Barry, contact details for whom are available on the
-    FreeRTOS WEB site.
-
-    1 tab == 4 spaces!
-
-    http://www.FreeRTOS.org - Documentation, latest information, license and
-    contact details.
-
-    http://www.SafeRTOS.com - A version that is certified for use in safety
-    critical systems.
-
-    http://www.OpenRTOS.com - Commercial support, development, porting,
-    licensing and training services.
-*/
-
-#ifndef STACK_MACROS_H
-#define STACK_MACROS_H
-
-/*
- * Call the stack overflow hook function if the stack of the task being swapped
- * out is currently overflowed, or looks like it might have overflowed in the
- * past.
- *
- * Setting configCHECK_FOR_STACK_OVERFLOW to 1 will cause the macro to check
- * the current stack state only - comparing the current top of stack value to
- * the stack limit.  Setting configCHECK_FOR_STACK_OVERFLOW to greater than 1
- * will also cause the last few stack bytes to be checked to ensure the value
- * to which the bytes were set when the task was created have not been
- * overwritten.  Note this second test does not guarantee that an overflowed
- * stack will always be recognised.
- */
-
-/*-----------------------------------------------------------*/
-
-#if( configCHECK_FOR_STACK_OVERFLOW == 0 )
-
-	/* FreeRTOSConfig.h is not set to check for stack overflows. */
-	#define taskFIRST_CHECK_FOR_STACK_OVERFLOW()
-	#define taskSECOND_CHECK_FOR_STACK_OVERFLOW()
-
-#endif /* configCHECK_FOR_STACK_OVERFLOW == 0 */
-/*-----------------------------------------------------------*/
-
-#if( configCHECK_FOR_STACK_OVERFLOW == 1 )
-
-	/* FreeRTOSConfig.h is only set to use the first method of
-	overflow checking. */
-	#define taskSECOND_CHECK_FOR_STACK_OVERFLOW()
-
-#endif
-/*-----------------------------------------------------------*/
-
-#if( ( configCHECK_FOR_STACK_OVERFLOW > 0 ) && ( portSTACK_GROWTH < 0 ) )
-
-	/* Only the current stack state is to be checked. */
-	#define taskFIRST_CHECK_FOR_STACK_OVERFLOW()														\
-	{																									\
-		/* Is the currently saved stack pointer within the stack limit? */								\
-		if( pxCurrentTCB->pxTopOfStack <= pxCurrentTCB->pxStack )										\
-		{																								\
-			vApplicationStackOverflowHook( ( xTaskHandle ) pxCurrentTCB, pxCurrentTCB->pcTaskName );	\
-		}																								\
-	}
-
-#endif /* configCHECK_FOR_STACK_OVERFLOW > 0 */
-/*-----------------------------------------------------------*/
-
-#if( ( configCHECK_FOR_STACK_OVERFLOW > 0 ) && ( portSTACK_GROWTH > 0 ) )
-
-	/* Only the current stack state is to be checked. */
-	#define taskFIRST_CHECK_FOR_STACK_OVERFLOW()														\
-	{																									\
-																										\
-		/* Is the currently saved stack pointer within the stack limit? */								\
-		if( pxCurrentTCB->pxTopOfStack >= pxCurrentTCB->pxEndOfStack )									\
-		{																								\
-			vApplicationStackOverflowHook( ( xTaskHandle ) pxCurrentTCB, pxCurrentTCB->pcTaskName );	\
-		}																								\
-	}
-
-#endif /* configCHECK_FOR_STACK_OVERFLOW == 1 */
-/*-----------------------------------------------------------*/
-
-#if( ( configCHECK_FOR_STACK_OVERFLOW > 1 ) && ( portSTACK_GROWTH < 0 ) )
-
-	#define taskSECOND_CHECK_FOR_STACK_OVERFLOW()																								\
-	{																																			\
-	static const unsigned char ucExpectedStackBytes[] = {	tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE,		\
-															tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE,		\
-															tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE,		\
-															tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE,		\
-															tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE };	\
-																																				\
-																																				\
-		/* Has the extremity of the task stack ever been written over? */																		\
-		if( memcmp( ( void * ) pxCurrentTCB->pxStack, ( void * ) ucExpectedStackBytes, sizeof( ucExpectedStackBytes ) ) != 0 )					\
-		{																																		\
-			vApplicationStackOverflowHook( ( xTaskHandle ) pxCurrentTCB, pxCurrentTCB->pcTaskName );											\
-		}																																		\
-	}
-
-#endif /* #if( configCHECK_FOR_STACK_OVERFLOW > 1 ) */
-/*-----------------------------------------------------------*/
-
-#if( ( configCHECK_FOR_STACK_OVERFLOW > 1 ) && ( portSTACK_GROWTH > 0 ) )
-
-	#define taskSECOND_CHECK_FOR_STACK_OVERFLOW()																								\
-	{																																			\
-	char *pcEndOfStack = ( char * ) pxCurrentTCB->pxEndOfStack;																					\
-	static const unsigned char ucExpectedStackBytes[] = {	tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE,		\
-															tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE,		\
-															tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE,		\
-															tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE,		\
-															tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE };	\
-																																				\
-																																				\
-		pcEndOfStack -= sizeof( ucExpectedStackBytes );																							\
-																																				\
-		/* Has the extremity of the task stack ever been written over? */																		\
-		if( memcmp( ( void * ) pcEndOfStack, ( void * ) ucExpectedStackBytes, sizeof( ucExpectedStackBytes ) ) != 0 )							\
-		{																																		\
-			vApplicationStackOverflowHook( ( xTaskHandle ) pxCurrentTCB, pxCurrentTCB->pcTaskName );											\
-		}																																		\
-	}
-
-#endif /* #if( configCHECK_FOR_STACK_OVERFLOW > 1 ) */
-/*-----------------------------------------------------------*/
-
-#endif /* STACK_MACROS_H */
-
+/*
+    FreeRTOS V7.2.0 - Copyright (C) 2012 Real Time Engineers Ltd.
+
+
+    ***************************************************************************
+     *                                                                       *
+     *    FreeRTOS tutorial books are available in pdf and paperback.        *
+     *    Complete, revised, and edited pdf reference manuals are also       *
+     *    available.                                                         *
+     *                                                                       *
+     *    Purchasing FreeRTOS documentation will not only help you, by       *
+     *    ensuring you get running as quickly as possible and with an        *
+     *    in-depth knowledge of how to use FreeRTOS, it will also help       *
+     *    the FreeRTOS project to continue with its mission of providing     *
+     *    professional grade, cross platform, de facto standard solutions    *
+     *    for microcontrollers - completely free of charge!                  *
+     *                                                                       *
+     *    >>> See http://www.FreeRTOS.org/Documentation for details. <<<     *
+     *                                                                       *
+     *    Thank you for using FreeRTOS, and thank you for your support!      *
+     *                                                                       *
+    ***************************************************************************
+
+
+    This file is part of the FreeRTOS distribution.
+
+    FreeRTOS is free software; you can redistribute it and/or modify it under
+    the terms of the GNU General Public License (version 2) as published by the
+    Free Software Foundation AND MODIFIED BY the FreeRTOS exception.
+    >>>NOTE<<< The modification to the GPL is included to allow you to
+    distribute a combined work that includes FreeRTOS without being obliged to
+    provide the source code for proprietary components outside of the FreeRTOS
+    kernel.  FreeRTOS 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 and the FreeRTOS license exception along with FreeRTOS; if not it
+    can be viewed here: http://www.freertos.org/a00114.html and also obtained
+    by writing to Richard Barry, contact details for whom are available on the
+    FreeRTOS WEB site.
+
+    1 tab == 4 spaces!
+    
+    ***************************************************************************
+     *                                                                       *
+     *    Having a problem?  Start by reading the FAQ "My application does   *
+     *    not run, what could be wrong?                                      *
+     *                                                                       *
+     *    http://www.FreeRTOS.org/FAQHelp.html                               *
+     *                                                                       *
+    ***************************************************************************
+
+    
+    http://www.FreeRTOS.org - Documentation, training, latest information, 
+    license and contact details.
+    
+    http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products,
+    including FreeRTOS+Trace - an indispensable productivity tool.
+
+    Real Time Engineers ltd license FreeRTOS to High Integrity Systems, who sell 
+    the code with commercial support, indemnification, and middleware, under 
+    the OpenRTOS brand: http://www.OpenRTOS.com.  High Integrity Systems also
+    provide a safety engineered and independently SIL3 certified version under 
+    the SafeRTOS brand: http://www.SafeRTOS.com.
+*/
+
+#ifndef STACK_MACROS_H
+#define STACK_MACROS_H
+
+/*
+ * Call the stack overflow hook function if the stack of the task being swapped
+ * out is currently overflowed, or looks like it might have overflowed in the
+ * past.
+ *
+ * Setting configCHECK_FOR_STACK_OVERFLOW to 1 will cause the macro to check
+ * the current stack state only - comparing the current top of stack value to
+ * the stack limit.  Setting configCHECK_FOR_STACK_OVERFLOW to greater than 1
+ * will also cause the last few stack bytes to be checked to ensure the value
+ * to which the bytes were set when the task was created have not been
+ * overwritten.  Note this second test does not guarantee that an overflowed
+ * stack will always be recognised.
+ */
+
+/*-----------------------------------------------------------*/
+
+#if( configCHECK_FOR_STACK_OVERFLOW == 0 )
+
+	/* FreeRTOSConfig.h is not set to check for stack overflows. */
+	#define taskFIRST_CHECK_FOR_STACK_OVERFLOW()
+	#define taskSECOND_CHECK_FOR_STACK_OVERFLOW()
+
+#endif /* configCHECK_FOR_STACK_OVERFLOW == 0 */
+/*-----------------------------------------------------------*/
+
+#if( configCHECK_FOR_STACK_OVERFLOW == 1 )
+
+	/* FreeRTOSConfig.h is only set to use the first method of
+	overflow checking. */
+	#define taskSECOND_CHECK_FOR_STACK_OVERFLOW()
+
+#endif
+/*-----------------------------------------------------------*/
+
+#if( ( configCHECK_FOR_STACK_OVERFLOW > 0 ) && ( portSTACK_GROWTH < 0 ) )
+
+	/* Only the current stack state is to be checked. */
+	#define taskFIRST_CHECK_FOR_STACK_OVERFLOW()														\
+	{																									\
+		/* Is the currently saved stack pointer within the stack limit? */								\
+		if( pxCurrentTCB->pxTopOfStack <= pxCurrentTCB->pxStack )										\
+		{																								\
+			vApplicationStackOverflowHook( ( xTaskHandle ) pxCurrentTCB, pxCurrentTCB->pcTaskName );	\
+		}																								\
+	}
+
+#endif /* configCHECK_FOR_STACK_OVERFLOW > 0 */
+/*-----------------------------------------------------------*/
+
+#if( ( configCHECK_FOR_STACK_OVERFLOW > 0 ) && ( portSTACK_GROWTH > 0 ) )
+
+	/* Only the current stack state is to be checked. */
+	#define taskFIRST_CHECK_FOR_STACK_OVERFLOW()														\
+	{																									\
+																										\
+		/* Is the currently saved stack pointer within the stack limit? */								\
+		if( pxCurrentTCB->pxTopOfStack >= pxCurrentTCB->pxEndOfStack )									\
+		{																								\
+			vApplicationStackOverflowHook( ( xTaskHandle ) pxCurrentTCB, pxCurrentTCB->pcTaskName );	\
+		}																								\
+	}
+
+#endif /* configCHECK_FOR_STACK_OVERFLOW == 1 */
+/*-----------------------------------------------------------*/
+
+#if( ( configCHECK_FOR_STACK_OVERFLOW > 1 ) && ( portSTACK_GROWTH < 0 ) )
+
+	#define taskSECOND_CHECK_FOR_STACK_OVERFLOW()																								\
+	{																																			\
+	static const unsigned char ucExpectedStackBytes[] = {	tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE,		\
+															tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE,		\
+															tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE,		\
+															tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE,		\
+															tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE };	\
+																																				\
+																																				\
+		/* Has the extremity of the task stack ever been written over? */																		\
+		if( memcmp( ( void * ) pxCurrentTCB->pxStack, ( void * ) ucExpectedStackBytes, sizeof( ucExpectedStackBytes ) ) != 0 )					\
+		{																																		\
+			vApplicationStackOverflowHook( ( xTaskHandle ) pxCurrentTCB, pxCurrentTCB->pcTaskName );											\
+		}																																		\
+	}
+
+#endif /* #if( configCHECK_FOR_STACK_OVERFLOW > 1 ) */
+/*-----------------------------------------------------------*/
+
+#if( ( configCHECK_FOR_STACK_OVERFLOW > 1 ) && ( portSTACK_GROWTH > 0 ) )
+
+	#define taskSECOND_CHECK_FOR_STACK_OVERFLOW()																								\
+	{																																			\
+	char *pcEndOfStack = ( char * ) pxCurrentTCB->pxEndOfStack;																					\
+	static const unsigned char ucExpectedStackBytes[] = {	tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE,		\
+															tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE,		\
+															tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE,		\
+															tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE,		\
+															tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE };	\
+																																				\
+																																				\
+		pcEndOfStack -= sizeof( ucExpectedStackBytes );																							\
+																																				\
+		/* Has the extremity of the task stack ever been written over? */																		\
+		if( memcmp( ( void * ) pcEndOfStack, ( void * ) ucExpectedStackBytes, sizeof( ucExpectedStackBytes ) ) != 0 )							\
+		{																																		\
+			vApplicationStackOverflowHook( ( xTaskHandle ) pxCurrentTCB, pxCurrentTCB->pcTaskName );											\
+		}																																		\
+	}
+
+#endif /* #if( configCHECK_FOR_STACK_OVERFLOW > 1 ) */
+/*-----------------------------------------------------------*/
+
+#endif /* STACK_MACROS_H */
+

flight/PiOS/Common/Libraries/FreeRTOS/Source/include/croutine.h

@@ -1,746 +1,759 @@
-/*
-    FreeRTOS V7.0.2 - Copyright (C) 2011 Real Time Engineers Ltd.
-
-
-    ***************************************************************************
-     *                                                                       *
-     *    FreeRTOS tutorial books are available in pdf and paperback.        *
-     *    Complete, revised, and edited pdf reference manuals are also       *
-     *    available.                                                         *
-     *                                                                       *
-     *    Purchasing FreeRTOS documentation will not only help you, by       *
-     *    ensuring you get running as quickly as possible and with an        *
-     *    in-depth knowledge of how to use FreeRTOS, it will also help       *
-     *    the FreeRTOS project to continue with its mission of providing     *
-     *    professional grade, cross platform, de facto standard solutions    *
-     *    for microcontrollers - completely free of charge!                  *
-     *                                                                       *
-     *    >>> See http://www.FreeRTOS.org/Documentation for details. <<<     *
-     *                                                                       *
-     *    Thank you for using FreeRTOS, and thank you for your support!      *
-     *                                                                       *
-    ***************************************************************************
-
-
-    This file is part of the FreeRTOS distribution.
-
-    FreeRTOS is free software; you can redistribute it and/or modify it under
-    the terms of the GNU General Public License (version 2) as published by the
-    Free Software Foundation AND MODIFIED BY the FreeRTOS exception.
-    >>>NOTE<<< The modification to the GPL is included to allow you to
-    distribute a combined work that includes FreeRTOS without being obliged to
-    provide the source code for proprietary components outside of the FreeRTOS
-    kernel.  FreeRTOS 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 and the FreeRTOS license exception along with FreeRTOS; if not it
-    can be viewed here: http://www.freertos.org/a00114.html and also obtained
-    by writing to Richard Barry, contact details for whom are available on the
-    FreeRTOS WEB site.
-
-    1 tab == 4 spaces!
-
-    http://www.FreeRTOS.org - Documentation, latest information, license and
-    contact details.
-
-    http://www.SafeRTOS.com - A version that is certified for use in safety
-    critical systems.
-
-    http://www.OpenRTOS.com - Commercial support, development, porting,
-    licensing and training services.
-*/
-
-#ifndef CO_ROUTINE_H
-#define CO_ROUTINE_H
-
-#ifndef INC_FREERTOS_H
-	#error "include FreeRTOS.h must appear in source files before include croutine.h"
-#endif
-
-#include "list.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/* Used to hide the implementation of the co-routine control block.  The
-control block structure however has to be included in the header due to
-the macro implementation of the co-routine functionality. */
-typedef void * xCoRoutineHandle;
-
-/* Defines the prototype to which co-routine functions must conform. */
-typedef void (*crCOROUTINE_CODE)( xCoRoutineHandle, unsigned portBASE_TYPE );
-
-typedef struct corCoRoutineControlBlock
-{
-	crCOROUTINE_CODE 		pxCoRoutineFunction;
-	xListItem				xGenericListItem;	/*< List item used to place the CRCB in ready and blocked queues. */
-	xListItem				xEventListItem;		/*< List item used to place the CRCB in event lists. */
-	unsigned portBASE_TYPE 	uxPriority;			/*< The priority of the co-routine in relation to other co-routines. */
-	unsigned portBASE_TYPE 	uxIndex;			/*< Used to distinguish between co-routines when multiple co-routines use the same co-routine function. */
-	unsigned short 		uxState;			/*< Used internally by the co-routine implementation. */
-} corCRCB; /* Co-routine control block.  Note must be identical in size down to uxPriority with tskTCB. */
-
-/**
- * croutine. h
- *<pre>
- portBASE_TYPE xCoRoutineCreate(
-                                 crCOROUTINE_CODE pxCoRoutineCode,
-                                 unsigned portBASE_TYPE uxPriority,
-                                 unsigned portBASE_TYPE uxIndex
-                               );</pre>
- *
- * Create a new co-routine and add it to the list of co-routines that are
- * ready to run.
- *
- * @param pxCoRoutineCode Pointer to the co-routine function.  Co-routine
- * functions require special syntax - see the co-routine section of the WEB
- * documentation for more information.
- *
- * @param uxPriority The priority with respect to other co-routines at which
- *  the co-routine will run.
- *
- * @param uxIndex Used to distinguish between different co-routines that
- * execute the same function.  See the example below and the co-routine section
- * of the WEB documentation for further information.
- *
- * @return pdPASS if the co-routine was successfully created and added to a ready
- * list, otherwise an error code defined with ProjDefs.h.
- *
- * Example usage:
-   <pre>
- // Co-routine to be created.
- void vFlashCoRoutine( xCoRoutineHandle xHandle, unsigned portBASE_TYPE uxIndex )
- {
- // Variables in co-routines must be declared static if they must maintain value across a blocking call.
- // This may not be necessary for const variables.
- static const char cLedToFlash[ 2 ] = { 5, 6 };
- static const portTickType uxFlashRates[ 2 ] = { 200, 400 };
-
-     // Must start every co-routine with a call to crSTART();
-     crSTART( xHandle );
-
-     for( ;; )
-     {
-         // This co-routine just delays for a fixed period, then toggles
-         // an LED.  Two co-routines are created using this function, so
-         // the uxIndex parameter is used to tell the co-routine which
-         // LED to flash and how long to delay.  This assumes xQueue has
-         // already been created.
-         vParTestToggleLED( cLedToFlash[ uxIndex ] );
-         crDELAY( xHandle, uxFlashRates[ uxIndex ] );
-     }
-
-     // Must end every co-routine with a call to crEND();
-     crEND();
- }
-
- // Function that creates two co-routines.
- void vOtherFunction( void )
- {
- unsigned char ucParameterToPass;
- xTaskHandle xHandle;
-		
-     // Create two co-routines at priority 0.  The first is given index 0
-     // so (from the code above) toggles LED 5 every 200 ticks.  The second
-     // is given index 1 so toggles LED 6 every 400 ticks.
-     for( uxIndex = 0; uxIndex < 2; uxIndex++ )
-     {
-         xCoRoutineCreate( vFlashCoRoutine, 0, uxIndex );
-     }
- }
-   </pre>
- * \defgroup xCoRoutineCreate xCoRoutineCreate
- * \ingroup Tasks
- */
-signed portBASE_TYPE xCoRoutineCreate( crCOROUTINE_CODE pxCoRoutineCode, unsigned portBASE_TYPE uxPriority, unsigned portBASE_TYPE uxIndex );
-
-
-/**
- * croutine. h
- *<pre>
- void vCoRoutineSchedule( void );</pre>
- *
- * Run a co-routine.
- *
- * vCoRoutineSchedule() executes the highest priority co-routine that is able
- * to run.  The co-routine will execute until it either blocks, yields or is
- * preempted by a task.  Co-routines execute cooperatively so one
- * co-routine cannot be preempted by another, but can be preempted by a task.
- *
- * If an application comprises of both tasks and co-routines then
- * vCoRoutineSchedule should be called from the idle task (in an idle task
- * hook).
- *
- * Example usage:
-   <pre>
- // This idle task hook will schedule a co-routine each time it is called.
- // The rest of the idle task will execute between co-routine calls.
- void vApplicationIdleHook( void )
- {
-	vCoRoutineSchedule();
- }
-
- // Alternatively, if you do not require any other part of the idle task to
- // execute, the idle task hook can call vCoRoutineScheduler() within an
- // infinite loop.
- void vApplicationIdleHook( void )
- {
-    for( ;; )
-    {
-        vCoRoutineSchedule();
-    }
- }
- </pre>
- * \defgroup vCoRoutineSchedule vCoRoutineSchedule
- * \ingroup Tasks
- */
-void vCoRoutineSchedule( void );
-
-/**
- * croutine. h
- * <pre>
- crSTART( xCoRoutineHandle xHandle );</pre>
- *
- * This macro MUST always be called at the start of a co-routine function.
- *
- * Example usage:
-   <pre>
- // Co-routine to be created.
- void vACoRoutine( xCoRoutineHandle xHandle, unsigned portBASE_TYPE uxIndex )
- {
- // Variables in co-routines must be declared static if they must maintain value across a blocking call.
- static long ulAVariable;
-
-     // Must start every co-routine with a call to crSTART();
-     crSTART( xHandle );
-
-     for( ;; )
-     {
-          // Co-routine functionality goes here.
-     }
-
-     // Must end every co-routine with a call to crEND();
-     crEND();
- }</pre>
- * \defgroup crSTART crSTART
- * \ingroup Tasks
- */
-#define crSTART( pxCRCB ) switch( ( ( corCRCB * )( pxCRCB ) )->uxState ) { case 0:
-
-/**
- * croutine. h
- * <pre>
- crEND();</pre>
- *
- * This macro MUST always be called at the end of a co-routine function.
- *
- * Example usage:
-   <pre>
- // Co-routine to be created.
- void vACoRoutine( xCoRoutineHandle xHandle, unsigned portBASE_TYPE uxIndex )
- {
- // Variables in co-routines must be declared static if they must maintain value across a blocking call.
- static long ulAVariable;
-
-     // Must start every co-routine with a call to crSTART();
-     crSTART( xHandle );
-
-     for( ;; )
-     {
-          // Co-routine functionality goes here.
-     }
-
-     // Must end every co-routine with a call to crEND();
-     crEND();
- }</pre>
- * \defgroup crSTART crSTART
- * \ingroup Tasks
- */
-#define crEND() }
-
-/*
- * These macros are intended for internal use by the co-routine implementation
- * only.  The macros should not be used directly by application writers.
- */
-#define crSET_STATE0( xHandle ) ( ( corCRCB * )( xHandle ) )->uxState = (__LINE__ * 2); return; case (__LINE__ * 2):
-#define crSET_STATE1( xHandle ) ( ( corCRCB * )( xHandle ) )->uxState = ((__LINE__ * 2)+1); return; case ((__LINE__ * 2)+1):
-
-/**
- * croutine. h
- *<pre>
- crDELAY( xCoRoutineHandle xHandle, portTickType xTicksToDelay );</pre>
- *
- * Delay a co-routine for a fixed period of time.
- *
- * crDELAY can only be called from the co-routine function itself - not
- * from within a function called by the co-routine function.  This is because
- * co-routines do not maintain their own stack.
- *
- * @param xHandle The handle of the co-routine to delay.  This is the xHandle
- * parameter of the co-routine function.
- *
- * @param xTickToDelay The number of ticks that the co-routine should delay
- * for.  The actual amount of time this equates to is defined by
- * configTICK_RATE_HZ (set in FreeRTOSConfig.h).  The constant portTICK_RATE_MS
- * can be used to convert ticks to milliseconds.
- *
- * Example usage:
-   <pre>
- // Co-routine to be created.
- void vACoRoutine( xCoRoutineHandle xHandle, unsigned portBASE_TYPE uxIndex )
- {
- // Variables in co-routines must be declared static if they must maintain value across a blocking call.
- // This may not be necessary for const variables.
- // We are to delay for 200ms.
- static const xTickType xDelayTime = 200 / portTICK_RATE_MS;
-
-     // Must start every co-routine with a call to crSTART();
-     crSTART( xHandle );
-
-     for( ;; )
-     {
-        // Delay for 200ms.
-        crDELAY( xHandle, xDelayTime );
-
-        // Do something here.
-     }
-
-     // Must end every co-routine with a call to crEND();
-     crEND();
- }</pre>
- * \defgroup crDELAY crDELAY
- * \ingroup Tasks
- */
-#define crDELAY( xHandle, xTicksToDelay )												\
-	if( ( xTicksToDelay ) > 0 )															\
-	{																					\
-		vCoRoutineAddToDelayedList( ( xTicksToDelay ), NULL );							\
-	}																					\
-	crSET_STATE0( ( xHandle ) );
-
-/**
- * <pre>
- crQUEUE_SEND(
-                  xCoRoutineHandle xHandle,
-                  xQueueHandle pxQueue,
-                  void *pvItemToQueue,
-                  portTickType xTicksToWait,
-                  portBASE_TYPE *pxResult
-             )</pre>
- *
- * The macro's crQUEUE_SEND() and crQUEUE_RECEIVE() are the co-routine
- * equivalent to the xQueueSend() and xQueueReceive() functions used by tasks.
- *
- * crQUEUE_SEND and crQUEUE_RECEIVE can only be used from a co-routine whereas
- * xQueueSend() and xQueueReceive() can only be used from tasks.
- *
- * crQUEUE_SEND can only be called from the co-routine function itself - not
- * from within a function called by the co-routine function.  This is because
- * co-routines do not maintain their own stack.
- *
- * See the co-routine section of the WEB documentation for information on
- * passing data between tasks and co-routines and between ISR's and
- * co-routines.
- *
- * @param xHandle The handle of the calling co-routine.  This is the xHandle
- * parameter of the co-routine function.
- *
- * @param pxQueue The handle of the queue on which the data will be posted.
- * The handle is obtained as the return value when the queue is created using
- * the xQueueCreate() API function.
- *
- * @param pvItemToQueue A pointer to the data being posted onto the queue.
- * The number of bytes of each queued item is specified when the queue is
- * created.  This number of bytes is copied from pvItemToQueue into the queue
- * itself.
- *
- * @param xTickToDelay The number of ticks that the co-routine should block
- * to wait for space to become available on the queue, should space not be
- * available immediately. The actual amount of time this equates to is defined
- * by configTICK_RATE_HZ (set in FreeRTOSConfig.h).  The constant
- * portTICK_RATE_MS can be used to convert ticks to milliseconds (see example
- * below).
- *
- * @param pxResult The variable pointed to by pxResult will be set to pdPASS if
- * data was successfully posted onto the queue, otherwise it will be set to an
- * error defined within ProjDefs.h.
- *
- * Example usage:
-   <pre>
- // Co-routine function that blocks for a fixed period then posts a number onto
- // a queue.
- static void prvCoRoutineFlashTask( xCoRoutineHandle xHandle, unsigned portBASE_TYPE uxIndex )
- {
- // Variables in co-routines must be declared static if they must maintain value across a blocking call.
- static portBASE_TYPE xNumberToPost = 0;
- static portBASE_TYPE xResult;
-
-    // Co-routines must begin with a call to crSTART().
-    crSTART( xHandle );
-
-    for( ;; )
-    {
-        // This assumes the queue has already been created.
-        crQUEUE_SEND( xHandle, xCoRoutineQueue, &xNumberToPost, NO_DELAY, &xResult );
-
-        if( xResult != pdPASS )
-        {
-            // The message was not posted!
-        }
-
-        // Increment the number to be posted onto the queue.
-        xNumberToPost++;
-
-        // Delay for 100 ticks.
-        crDELAY( xHandle, 100 );
-    }
-
-    // Co-routines must end with a call to crEND().
-    crEND();
- }</pre>
- * \defgroup crQUEUE_SEND crQUEUE_SEND
- * \ingroup Tasks
- */
-#define crQUEUE_SEND( xHandle, pxQueue, pvItemToQueue, xTicksToWait, pxResult )			\
-{																						\
-	*( pxResult ) = xQueueCRSend( ( pxQueue) , ( pvItemToQueue) , ( xTicksToWait ) );	\
-	if( *( pxResult ) == errQUEUE_BLOCKED )												\
-	{																					\
-		crSET_STATE0( ( xHandle ) );													\
-		*pxResult = xQueueCRSend( ( pxQueue ), ( pvItemToQueue ), 0 );					\
-	}																					\
-	if( *pxResult == errQUEUE_YIELD )													\
-	{																					\
-		crSET_STATE1( ( xHandle ) );													\
-		*pxResult = pdPASS;																\
-	}																					\
-}
-
-/**
- * croutine. h
- * <pre>
-  crQUEUE_RECEIVE(
-                     xCoRoutineHandle xHandle,
-                     xQueueHandle pxQueue,
-                     void *pvBuffer,
-                     portTickType xTicksToWait,
-                     portBASE_TYPE *pxResult
-                 )</pre>
- *
- * The macro's crQUEUE_SEND() and crQUEUE_RECEIVE() are the co-routine
- * equivalent to the xQueueSend() and xQueueReceive() functions used by tasks.
- *
- * crQUEUE_SEND and crQUEUE_RECEIVE can only be used from a co-routine whereas
- * xQueueSend() and xQueueReceive() can only be used from tasks.
- *
- * crQUEUE_RECEIVE can only be called from the co-routine function itself - not
- * from within a function called by the co-routine function.  This is because
- * co-routines do not maintain their own stack.
- *
- * See the co-routine section of the WEB documentation for information on
- * passing data between tasks and co-routines and between ISR's and
- * co-routines.
- *
- * @param xHandle The handle of the calling co-routine.  This is the xHandle
- * parameter of the co-routine function.
- *
- * @param pxQueue The handle of the queue from which the data will be received.
- * The handle is obtained as the return value when the queue is created using
- * the xQueueCreate() API function.
- *
- * @param pvBuffer The buffer into which the received item is to be copied.
- * The number of bytes of each queued item is specified when the queue is
- * created.  This number of bytes is copied into pvBuffer.
- *
- * @param xTickToDelay The number of ticks that the co-routine should block
- * to wait for data to become available from the queue, should data not be
- * available immediately. The actual amount of time this equates to is defined
- * by configTICK_RATE_HZ (set in FreeRTOSConfig.h).  The constant
- * portTICK_RATE_MS can be used to convert ticks to milliseconds (see the
- * crQUEUE_SEND example).
- *
- * @param pxResult The variable pointed to by pxResult will be set to pdPASS if
- * data was successfully retrieved from the queue, otherwise it will be set to
- * an error code as defined within ProjDefs.h.
- *
- * Example usage:
- <pre>
- // A co-routine receives the number of an LED to flash from a queue.  It
- // blocks on the queue until the number is received.
- static void prvCoRoutineFlashWorkTask( xCoRoutineHandle xHandle, unsigned portBASE_TYPE uxIndex )
- {
- // Variables in co-routines must be declared static if they must maintain value across a blocking call.
- static portBASE_TYPE xResult;
- static unsigned portBASE_TYPE uxLEDToFlash;
-
-    // All co-routines must start with a call to crSTART().
-    crSTART( xHandle );
-
-    for( ;; )
-    {
-        // Wait for data to become available on the queue.
-        crQUEUE_RECEIVE( xHandle, xCoRoutineQueue, &uxLEDToFlash, portMAX_DELAY, &xResult );
-
-        if( xResult == pdPASS )
-        {
-            // We received the LED to flash - flash it!
-            vParTestToggleLED( uxLEDToFlash );
-        }
-    }
-
-    crEND();
- }</pre>
- * \defgroup crQUEUE_RECEIVE crQUEUE_RECEIVE
- * \ingroup Tasks
- */
-#define crQUEUE_RECEIVE( xHandle, pxQueue, pvBuffer, xTicksToWait, pxResult )			\
-{																						\
-	*( pxResult ) = xQueueCRReceive( ( pxQueue) , ( pvBuffer ), ( xTicksToWait ) );		\
-	if( *( pxResult ) == errQUEUE_BLOCKED ) 											\
-	{																					\
-		crSET_STATE0( ( xHandle ) );													\
-		*( pxResult ) = xQueueCRReceive( ( pxQueue) , ( pvBuffer ), 0 );				\
-	}																					\
-	if( *( pxResult ) == errQUEUE_YIELD )												\
-	{																					\
-		crSET_STATE1( ( xHandle ) );													\
-		*( pxResult ) = pdPASS;															\
-	}																					\
-}
-
-/**
- * croutine. h
- * <pre>
-  crQUEUE_SEND_FROM_ISR(
-                            xQueueHandle pxQueue,
-                            void *pvItemToQueue,
-                            portBASE_TYPE xCoRoutinePreviouslyWoken
-                       )</pre>
- *
- * The macro's crQUEUE_SEND_FROM_ISR() and crQUEUE_RECEIVE_FROM_ISR() are the
- * co-routine equivalent to the xQueueSendFromISR() and xQueueReceiveFromISR()
- * functions used by tasks.
- *
- * crQUEUE_SEND_FROM_ISR() and crQUEUE_RECEIVE_FROM_ISR() can only be used to
- * pass data between a co-routine and and ISR, whereas xQueueSendFromISR() and
- * xQueueReceiveFromISR() can only be used to pass data between a task and and
- * ISR.
- *
- * crQUEUE_SEND_FROM_ISR can only be called from an ISR to send data to a queue
- * that is being used from within a co-routine.
- *
- * See the co-routine section of the WEB documentation for information on
- * passing data between tasks and co-routines and between ISR's and
- * co-routines.
- *
- * @param xQueue The handle to the queue on which the item is to be posted.
- *
- * @param pvItemToQueue A pointer to the item that is to be placed on the
- * queue.  The size of the items the queue will hold was defined when the
- * queue was created, so this many bytes will be copied from pvItemToQueue
- * into the queue storage area.
- *
- * @param xCoRoutinePreviouslyWoken This is included so an ISR can post onto
- * the same queue multiple times from a single interrupt.  The first call
- * should always pass in pdFALSE.  Subsequent calls should pass in
- * the value returned from the previous call.
- *
- * @return pdTRUE if a co-routine was woken by posting onto the queue.  This is
- * used by the ISR to determine if a context switch may be required following
- * the ISR.
- *
- * Example usage:
- <pre>
- // A co-routine that blocks on a queue waiting for characters to be received.
- static void vReceivingCoRoutine( xCoRoutineHandle xHandle, unsigned portBASE_TYPE uxIndex )
- {
- char cRxedChar;
- portBASE_TYPE xResult;
-
-     // All co-routines must start with a call to crSTART().
-     crSTART( xHandle );
-
-     for( ;; )
-     {
-         // Wait for data to become available on the queue.  This assumes the
-         // queue xCommsRxQueue has already been created!
-         crQUEUE_RECEIVE( xHandle, xCommsRxQueue, &uxLEDToFlash, portMAX_DELAY, &xResult );
-
-         // Was a character received?
-         if( xResult == pdPASS )
-         {
-             // Process the character here.
-         }
-     }
-
-     // All co-routines must end with a call to crEND().
-     crEND();
- }
-
- // An ISR that uses a queue to send characters received on a serial port to
- // a co-routine.
- void vUART_ISR( void )
- {
- char cRxedChar;
- portBASE_TYPE xCRWokenByPost = pdFALSE;
-
-     // We loop around reading characters until there are none left in the UART.
-     while( UART_RX_REG_NOT_EMPTY() )
-     {
-         // Obtain the character from the UART.
-         cRxedChar = UART_RX_REG;
-
-         // Post the character onto a queue.  xCRWokenByPost will be pdFALSE
-         // the first time around the loop.  If the post causes a co-routine
-         // to be woken (unblocked) then xCRWokenByPost will be set to pdTRUE.
-         // In this manner we can ensure that if more than one co-routine is
-         // blocked on the queue only one is woken by this ISR no matter how
-         // many characters are posted to the queue.
-         xCRWokenByPost = crQUEUE_SEND_FROM_ISR( xCommsRxQueue, &cRxedChar, xCRWokenByPost );
-     }
- }</pre>
- * \defgroup crQUEUE_SEND_FROM_ISR crQUEUE_SEND_FROM_ISR
- * \ingroup Tasks
- */
-#define crQUEUE_SEND_FROM_ISR( pxQueue, pvItemToQueue, xCoRoutinePreviouslyWoken ) xQueueCRSendFromISR( ( pxQueue ), ( pvItemToQueue ), ( xCoRoutinePreviouslyWoken ) )
-
-
-/**
- * croutine. h
- * <pre>
-  crQUEUE_SEND_FROM_ISR(
-                            xQueueHandle pxQueue,
-                            void *pvBuffer,
-                            portBASE_TYPE * pxCoRoutineWoken
-                       )</pre>
- *
- * The macro's crQUEUE_SEND_FROM_ISR() and crQUEUE_RECEIVE_FROM_ISR() are the
- * co-routine equivalent to the xQueueSendFromISR() and xQueueReceiveFromISR()
- * functions used by tasks.
- *
- * crQUEUE_SEND_FROM_ISR() and crQUEUE_RECEIVE_FROM_ISR() can only be used to
- * pass data between a co-routine and and ISR, whereas xQueueSendFromISR() and
- * xQueueReceiveFromISR() can only be used to pass data between a task and and
- * ISR.
- *
- * crQUEUE_RECEIVE_FROM_ISR can only be called from an ISR to receive data
- * from a queue that is being used from within a co-routine (a co-routine
- * posted to the queue).
- *
- * See the co-routine section of the WEB documentation for information on
- * passing data between tasks and co-routines and between ISR's and
- * co-routines.
- *
- * @param xQueue The handle to the queue on which the item is to be posted.
- *
- * @param pvBuffer A pointer to a buffer into which the received item will be
- * placed.  The size of the items the queue will hold was defined when the
- * queue was created, so this many bytes will be copied from the queue into
- * pvBuffer.
- *
- * @param pxCoRoutineWoken A co-routine may be blocked waiting for space to become
- * available on the queue.  If crQUEUE_RECEIVE_FROM_ISR causes such a
- * co-routine to unblock *pxCoRoutineWoken will get set to pdTRUE, otherwise
- * *pxCoRoutineWoken will remain unchanged.
- *
- * @return pdTRUE an item was successfully received from the queue, otherwise
- * pdFALSE.
- *
- * Example usage:
- <pre>
- // A co-routine that posts a character to a queue then blocks for a fixed
- // period.  The character is incremented each time.
- static void vSendingCoRoutine( xCoRoutineHandle xHandle, unsigned portBASE_TYPE uxIndex )
- {
- // cChar holds its value while this co-routine is blocked and must therefore
- // be declared static.
- static char cCharToTx = 'a';
- portBASE_TYPE xResult;
-
-     // All co-routines must start with a call to crSTART().
-     crSTART( xHandle );
-
-     for( ;; )
-     {
-         // Send the next character to the queue.
-         crQUEUE_SEND( xHandle, xCoRoutineQueue, &cCharToTx, NO_DELAY, &xResult );
-
-         if( xResult == pdPASS )
-         {
-             // The character was successfully posted to the queue.
-         }
-		 else
-		 {
-			// Could not post the character to the queue.
-		 }
-
-         // Enable the UART Tx interrupt to cause an interrupt in this
-		 // hypothetical UART.  The interrupt will obtain the character
-		 // from the queue and send it.
-		 ENABLE_RX_INTERRUPT();
-
-		 // Increment to the next character then block for a fixed period.
-		 // cCharToTx will maintain its value across the delay as it is
-		 // declared static.
-		 cCharToTx++;
-		 if( cCharToTx > 'x' )
-		 {
-			cCharToTx = 'a';
-		 }
-		 crDELAY( 100 );
-     }
-
-     // All co-routines must end with a call to crEND().
-     crEND();
- }
-
- // An ISR that uses a queue to receive characters to send on a UART.
- void vUART_ISR( void )
- {
- char cCharToTx;
- portBASE_TYPE xCRWokenByPost = pdFALSE;
-
-     while( UART_TX_REG_EMPTY() )
-     {
-         // Are there any characters in the queue waiting to be sent?
-		 // xCRWokenByPost will automatically be set to pdTRUE if a co-routine
-		 // is woken by the post - ensuring that only a single co-routine is
-		 // woken no matter how many times we go around this loop.
-         if( crQUEUE_RECEIVE_FROM_ISR( pxQueue, &cCharToTx, &xCRWokenByPost ) )
-		 {
-			 SEND_CHARACTER( cCharToTx );
-		 }
-     }
- }</pre>
- * \defgroup crQUEUE_RECEIVE_FROM_ISR crQUEUE_RECEIVE_FROM_ISR
- * \ingroup Tasks
- */
-#define crQUEUE_RECEIVE_FROM_ISR( pxQueue, pvBuffer, pxCoRoutineWoken ) xQueueCRReceiveFromISR( ( pxQueue ), ( pvBuffer ), ( pxCoRoutineWoken ) )
-
-/*
- * This function is intended for internal use by the co-routine macros only.
- * The macro nature of the co-routine implementation requires that the
- * prototype appears here.  The function should not be used by application
- * writers.
- *
- * Removes the current co-routine from its ready list and places it in the
- * appropriate delayed list.
- */
-void vCoRoutineAddToDelayedList( portTickType xTicksToDelay, xList *pxEventList );
-
-/*
- * This function is intended for internal use by the queue implementation only.
- * The function should not be used by application writers.
- *
- * Removes the highest priority co-routine from the event list and places it in
- * the pending ready list.
- */
-signed portBASE_TYPE xCoRoutineRemoveFromEventList( const xList *pxEventList );
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* CO_ROUTINE_H */
+/*
+    FreeRTOS V7.2.0 - Copyright (C) 2012 Real Time Engineers Ltd.
+
+
+    ***************************************************************************
+     *                                                                       *
+     *    FreeRTOS tutorial books are available in pdf and paperback.        *
+     *    Complete, revised, and edited pdf reference manuals are also       *
+     *    available.                                                         *
+     *                                                                       *
+     *    Purchasing FreeRTOS documentation will not only help you, by       *
+     *    ensuring you get running as quickly as possible and with an        *
+     *    in-depth knowledge of how to use FreeRTOS, it will also help       *
+     *    the FreeRTOS project to continue with its mission of providing     *
+     *    professional grade, cross platform, de facto standard solutions    *
+     *    for microcontrollers - completely free of charge!                  *
+     *                                                                       *
+     *    >>> See http://www.FreeRTOS.org/Documentation for details. <<<     *
+     *                                                                       *
+     *    Thank you for using FreeRTOS, and thank you for your support!      *
+     *                                                                       *
+    ***************************************************************************
+
+
+    This file is part of the FreeRTOS distribution.
+
+    FreeRTOS is free software; you can redistribute it and/or modify it under
+    the terms of the GNU General Public License (version 2) as published by the
+    Free Software Foundation AND MODIFIED BY the FreeRTOS exception.
+    >>>NOTE<<< The modification to the GPL is included to allow you to
+    distribute a combined work that includes FreeRTOS without being obliged to
+    provide the source code for proprietary components outside of the FreeRTOS
+    kernel.  FreeRTOS 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 and the FreeRTOS license exception along with FreeRTOS; if not it
+    can be viewed here: http://www.freertos.org/a00114.html and also obtained
+    by writing to Richard Barry, contact details for whom are available on the
+    FreeRTOS WEB site.
+
+    1 tab == 4 spaces!
+    
+    ***************************************************************************
+     *                                                                       *
+     *    Having a problem?  Start by reading the FAQ "My application does   *
+     *    not run, what could be wrong?                                      *
+     *                                                                       *
+     *    http://www.FreeRTOS.org/FAQHelp.html                               *
+     *                                                                       *
+    ***************************************************************************
+
+    
+    http://www.FreeRTOS.org - Documentation, training, latest information, 
+    license and contact details.
+    
+    http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products,
+    including FreeRTOS+Trace - an indispensable productivity tool.
+
+    Real Time Engineers ltd license FreeRTOS to High Integrity Systems, who sell 
+    the code with commercial support, indemnification, and middleware, under 
+    the OpenRTOS brand: http://www.OpenRTOS.com.  High Integrity Systems also
+    provide a safety engineered and independently SIL3 certified version under 
+    the SafeRTOS brand: http://www.SafeRTOS.com.
+*/
+
+#ifndef CO_ROUTINE_H
+#define CO_ROUTINE_H
+
+#ifndef INC_FREERTOS_H
+	#error "include FreeRTOS.h must appear in source files before include croutine.h"
+#endif
+
+#include "list.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Used to hide the implementation of the co-routine control block.  The
+control block structure however has to be included in the header due to
+the macro implementation of the co-routine functionality. */
+typedef void * xCoRoutineHandle;
+
+/* Defines the prototype to which co-routine functions must conform. */
+typedef void (*crCOROUTINE_CODE)( xCoRoutineHandle, unsigned portBASE_TYPE );
+
+typedef struct corCoRoutineControlBlock
+{
+	crCOROUTINE_CODE 		pxCoRoutineFunction;
+	xListItem				xGenericListItem;	/*< List item used to place the CRCB in ready and blocked queues. */
+	xListItem				xEventListItem;		/*< List item used to place the CRCB in event lists. */
+	unsigned portBASE_TYPE 	uxPriority;			/*< The priority of the co-routine in relation to other co-routines. */
+	unsigned portBASE_TYPE 	uxIndex;			/*< Used to distinguish between co-routines when multiple co-routines use the same co-routine function. */
+	unsigned short 		uxState;			/*< Used internally by the co-routine implementation. */
+} corCRCB; /* Co-routine control block.  Note must be identical in size down to uxPriority with tskTCB. */
+
+/**
+ * croutine. h
+ *<pre>
+ portBASE_TYPE xCoRoutineCreate(
+                                 crCOROUTINE_CODE pxCoRoutineCode,
+                                 unsigned portBASE_TYPE uxPriority,
+                                 unsigned portBASE_TYPE uxIndex
+                               );</pre>
+ *
+ * Create a new co-routine and add it to the list of co-routines that are
+ * ready to run.
+ *
+ * @param pxCoRoutineCode Pointer to the co-routine function.  Co-routine
+ * functions require special syntax - see the co-routine section of the WEB
+ * documentation for more information.
+ *
+ * @param uxPriority The priority with respect to other co-routines at which
+ *  the co-routine will run.
+ *
+ * @param uxIndex Used to distinguish between different co-routines that
+ * execute the same function.  See the example below and the co-routine section
+ * of the WEB documentation for further information.
+ *
+ * @return pdPASS if the co-routine was successfully created and added to a ready
+ * list, otherwise an error code defined with ProjDefs.h.
+ *
+ * Example usage:
+   <pre>
+ // Co-routine to be created.
+ void vFlashCoRoutine( xCoRoutineHandle xHandle, unsigned portBASE_TYPE uxIndex )
+ {
+ // Variables in co-routines must be declared static if they must maintain value across a blocking call.
+ // This may not be necessary for const variables.
+ static const char cLedToFlash[ 2 ] = { 5, 6 };
+ static const portTickType uxFlashRates[ 2 ] = { 200, 400 };
+
+     // Must start every co-routine with a call to crSTART();
+     crSTART( xHandle );
+
+     for( ;; )
+     {
+         // This co-routine just delays for a fixed period, then toggles
+         // an LED.  Two co-routines are created using this function, so
+         // the uxIndex parameter is used to tell the co-routine which
+         // LED to flash and how long to delay.  This assumes xQueue has
+         // already been created.
+         vParTestToggleLED( cLedToFlash[ uxIndex ] );
+         crDELAY( xHandle, uxFlashRates[ uxIndex ] );
+     }
+
+     // Must end every co-routine with a call to crEND();
+     crEND();
+ }
+
+ // Function that creates two co-routines.
+ void vOtherFunction( void )
+ {
+ unsigned char ucParameterToPass;
+ xTaskHandle xHandle;
+		
+     // Create two co-routines at priority 0.  The first is given index 0
+     // so (from the code above) toggles LED 5 every 200 ticks.  The second
+     // is given index 1 so toggles LED 6 every 400 ticks.
+     for( uxIndex = 0; uxIndex < 2; uxIndex++ )
+     {
+         xCoRoutineCreate( vFlashCoRoutine, 0, uxIndex );
+     }
+ }
+   </pre>
+ * \defgroup xCoRoutineCreate xCoRoutineCreate
+ * \ingroup Tasks
+ */
+signed portBASE_TYPE xCoRoutineCreate( crCOROUTINE_CODE pxCoRoutineCode, unsigned portBASE_TYPE uxPriority, unsigned portBASE_TYPE uxIndex );
+
+
+/**
+ * croutine. h
+ *<pre>
+ void vCoRoutineSchedule( void );</pre>
+ *
+ * Run a co-routine.
+ *
+ * vCoRoutineSchedule() executes the highest priority co-routine that is able
+ * to run.  The co-routine will execute until it either blocks, yields or is
+ * preempted by a task.  Co-routines execute cooperatively so one
+ * co-routine cannot be preempted by another, but can be preempted by a task.
+ *
+ * If an application comprises of both tasks and co-routines then
+ * vCoRoutineSchedule should be called from the idle task (in an idle task
+ * hook).
+ *
+ * Example usage:
+   <pre>
+ // This idle task hook will schedule a co-routine each time it is called.
+ // The rest of the idle task will execute between co-routine calls.
+ void vApplicationIdleHook( void )
+ {
+	vCoRoutineSchedule();
+ }
+
+ // Alternatively, if you do not require any other part of the idle task to
+ // execute, the idle task hook can call vCoRoutineScheduler() within an
+ // infinite loop.
+ void vApplicationIdleHook( void )
+ {
+    for( ;; )
+    {
+        vCoRoutineSchedule();
+    }
+ }
+ </pre>
+ * \defgroup vCoRoutineSchedule vCoRoutineSchedule
+ * \ingroup Tasks
+ */
+void vCoRoutineSchedule( void );
+
+/**
+ * croutine. h
+ * <pre>
+ crSTART( xCoRoutineHandle xHandle );</pre>
+ *
+ * This macro MUST always be called at the start of a co-routine function.
+ *
+ * Example usage:
+   <pre>
+ // Co-routine to be created.
+ void vACoRoutine( xCoRoutineHandle xHandle, unsigned portBASE_TYPE uxIndex )
+ {
+ // Variables in co-routines must be declared static if they must maintain value across a blocking call.
+ static long ulAVariable;
+
+     // Must start every co-routine with a call to crSTART();
+     crSTART( xHandle );
+
+     for( ;; )
+     {
+          // Co-routine functionality goes here.
+     }
+
+     // Must end every co-routine with a call to crEND();
+     crEND();
+ }</pre>
+ * \defgroup crSTART crSTART
+ * \ingroup Tasks
+ */
+#define crSTART( pxCRCB ) switch( ( ( corCRCB * )( pxCRCB ) )->uxState ) { case 0:
+
+/**
+ * croutine. h
+ * <pre>
+ crEND();</pre>
+ *
+ * This macro MUST always be called at the end of a co-routine function.
+ *
+ * Example usage:
+   <pre>
+ // Co-routine to be created.
+ void vACoRoutine( xCoRoutineHandle xHandle, unsigned portBASE_TYPE uxIndex )
+ {
+ // Variables in co-routines must be declared static if they must maintain value across a blocking call.
+ static long ulAVariable;
+
+     // Must start every co-routine with a call to crSTART();
+     crSTART( xHandle );
+
+     for( ;; )
+     {
+          // Co-routine functionality goes here.
+     }
+
+     // Must end every co-routine with a call to crEND();
+     crEND();
+ }</pre>
+ * \defgroup crSTART crSTART
+ * \ingroup Tasks
+ */
+#define crEND() }
+
+/*
+ * These macros are intended for internal use by the co-routine implementation
+ * only.  The macros should not be used directly by application writers.
+ */
+#define crSET_STATE0( xHandle ) ( ( corCRCB * )( xHandle ) )->uxState = (__LINE__ * 2); return; case (__LINE__ * 2):
+#define crSET_STATE1( xHandle ) ( ( corCRCB * )( xHandle ) )->uxState = ((__LINE__ * 2)+1); return; case ((__LINE__ * 2)+1):
+
+/**
+ * croutine. h
+ *<pre>
+ crDELAY( xCoRoutineHandle xHandle, portTickType xTicksToDelay );</pre>
+ *
+ * Delay a co-routine for a fixed period of time.
+ *
+ * crDELAY can only be called from the co-routine function itself - not
+ * from within a function called by the co-routine function.  This is because
+ * co-routines do not maintain their own stack.
+ *
+ * @param xHandle The handle of the co-routine to delay.  This is the xHandle
+ * parameter of the co-routine function.
+ *
+ * @param xTickToDelay The number of ticks that the co-routine should delay
+ * for.  The actual amount of time this equates to is defined by
+ * configTICK_RATE_HZ (set in FreeRTOSConfig.h).  The constant portTICK_RATE_MS
+ * can be used to convert ticks to milliseconds.
+ *
+ * Example usage:
+   <pre>
+ // Co-routine to be created.
+ void vACoRoutine( xCoRoutineHandle xHandle, unsigned portBASE_TYPE uxIndex )
+ {
+ // Variables in co-routines must be declared static if they must maintain value across a blocking call.
+ // This may not be necessary for const variables.
+ // We are to delay for 200ms.
+ static const xTickType xDelayTime = 200 / portTICK_RATE_MS;
+
+     // Must start every co-routine with a call to crSTART();
+     crSTART( xHandle );
+
+     for( ;; )
+     {
+        // Delay for 200ms.
+        crDELAY( xHandle, xDelayTime );
+
+        // Do something here.
+     }
+
+     // Must end every co-routine with a call to crEND();
+     crEND();
+ }</pre>
+ * \defgroup crDELAY crDELAY
+ * \ingroup Tasks
+ */
+#define crDELAY( xHandle, xTicksToDelay )												\
+	if( ( xTicksToDelay ) > 0 )															\
+	{																					\
+		vCoRoutineAddToDelayedList( ( xTicksToDelay ), NULL );							\
+	}																					\
+	crSET_STATE0( ( xHandle ) );
+
+/**
+ * <pre>
+ crQUEUE_SEND(
+                  xCoRoutineHandle xHandle,
+                  xQueueHandle pxQueue,
+                  void *pvItemToQueue,
+                  portTickType xTicksToWait,
+                  portBASE_TYPE *pxResult
+             )</pre>
+ *
+ * The macro's crQUEUE_SEND() and crQUEUE_RECEIVE() are the co-routine
+ * equivalent to the xQueueSend() and xQueueReceive() functions used by tasks.
+ *
+ * crQUEUE_SEND and crQUEUE_RECEIVE can only be used from a co-routine whereas
+ * xQueueSend() and xQueueReceive() can only be used from tasks.
+ *
+ * crQUEUE_SEND can only be called from the co-routine function itself - not
+ * from within a function called by the co-routine function.  This is because
+ * co-routines do not maintain their own stack.
+ *
+ * See the co-routine section of the WEB documentation for information on
+ * passing data between tasks and co-routines and between ISR's and
+ * co-routines.
+ *
+ * @param xHandle The handle of the calling co-routine.  This is the xHandle
+ * parameter of the co-routine function.
+ *
+ * @param pxQueue The handle of the queue on which the data will be posted.
+ * The handle is obtained as the return value when the queue is created using
+ * the xQueueCreate() API function.
+ *
+ * @param pvItemToQueue A pointer to the data being posted onto the queue.
+ * The number of bytes of each queued item is specified when the queue is
+ * created.  This number of bytes is copied from pvItemToQueue into the queue
+ * itself.
+ *
+ * @param xTickToDelay The number of ticks that the co-routine should block
+ * to wait for space to become available on the queue, should space not be
+ * available immediately. The actual amount of time this equates to is defined
+ * by configTICK_RATE_HZ (set in FreeRTOSConfig.h).  The constant
+ * portTICK_RATE_MS can be used to convert ticks to milliseconds (see example
+ * below).
+ *
+ * @param pxResult The variable pointed to by pxResult will be set to pdPASS if
+ * data was successfully posted onto the queue, otherwise it will be set to an
+ * error defined within ProjDefs.h.
+ *
+ * Example usage:
+   <pre>
+ // Co-routine function that blocks for a fixed period then posts a number onto
+ // a queue.
+ static void prvCoRoutineFlashTask( xCoRoutineHandle xHandle, unsigned portBASE_TYPE uxIndex )
+ {
+ // Variables in co-routines must be declared static if they must maintain value across a blocking call.
+ static portBASE_TYPE xNumberToPost = 0;
+ static portBASE_TYPE xResult;
+
+    // Co-routines must begin with a call to crSTART().
+    crSTART( xHandle );
+
+    for( ;; )
+    {
+        // This assumes the queue has already been created.
+        crQUEUE_SEND( xHandle, xCoRoutineQueue, &xNumberToPost, NO_DELAY, &xResult );
+
+        if( xResult != pdPASS )
+        {
+            // The message was not posted!
+        }
+
+        // Increment the number to be posted onto the queue.
+        xNumberToPost++;
+
+        // Delay for 100 ticks.
+        crDELAY( xHandle, 100 );
+    }
+
+    // Co-routines must end with a call to crEND().
+    crEND();
+ }</pre>
+ * \defgroup crQUEUE_SEND crQUEUE_SEND
+ * \ingroup Tasks
+ */
+#define crQUEUE_SEND( xHandle, pxQueue, pvItemToQueue, xTicksToWait, pxResult )			\
+{																						\
+	*( pxResult ) = xQueueCRSend( ( pxQueue) , ( pvItemToQueue) , ( xTicksToWait ) );	\
+	if( *( pxResult ) == errQUEUE_BLOCKED )												\
+	{																					\
+		crSET_STATE0( ( xHandle ) );													\
+		*pxResult = xQueueCRSend( ( pxQueue ), ( pvItemToQueue ), 0 );					\
+	}																					\
+	if( *pxResult == errQUEUE_YIELD )													\
+	{																					\
+		crSET_STATE1( ( xHandle ) );													\
+		*pxResult = pdPASS;																\
+	}																					\
+}
+
+/**
+ * croutine. h
+ * <pre>
+  crQUEUE_RECEIVE(
+                     xCoRoutineHandle xHandle,
+                     xQueueHandle pxQueue,
+                     void *pvBuffer,
+                     portTickType xTicksToWait,
+                     portBASE_TYPE *pxResult
+                 )</pre>
+ *
+ * The macro's crQUEUE_SEND() and crQUEUE_RECEIVE() are the co-routine
+ * equivalent to the xQueueSend() and xQueueReceive() functions used by tasks.
+ *
+ * crQUEUE_SEND and crQUEUE_RECEIVE can only be used from a co-routine whereas
+ * xQueueSend() and xQueueReceive() can only be used from tasks.
+ *
+ * crQUEUE_RECEIVE can only be called from the co-routine function itself - not
+ * from within a function called by the co-routine function.  This is because
+ * co-routines do not maintain their own stack.
+ *
+ * See the co-routine section of the WEB documentation for information on
+ * passing data between tasks and co-routines and between ISR's and
+ * co-routines.
+ *
+ * @param xHandle The handle of the calling co-routine.  This is the xHandle
+ * parameter of the co-routine function.
+ *
+ * @param pxQueue The handle of the queue from which the data will be received.
+ * The handle is obtained as the return value when the queue is created using
+ * the xQueueCreate() API function.
+ *
+ * @param pvBuffer The buffer into which the received item is to be copied.
+ * The number of bytes of each queued item is specified when the queue is
+ * created.  This number of bytes is copied into pvBuffer.
+ *
+ * @param xTickToDelay The number of ticks that the co-routine should block
+ * to wait for data to become available from the queue, should data not be
+ * available immediately. The actual amount of time this equates to is defined
+ * by configTICK_RATE_HZ (set in FreeRTOSConfig.h).  The constant
+ * portTICK_RATE_MS can be used to convert ticks to milliseconds (see the
+ * crQUEUE_SEND example).
+ *
+ * @param pxResult The variable pointed to by pxResult will be set to pdPASS if
+ * data was successfully retrieved from the queue, otherwise it will be set to
+ * an error code as defined within ProjDefs.h.
+ *
+ * Example usage:
+ <pre>
+ // A co-routine receives the number of an LED to flash from a queue.  It
+ // blocks on the queue until the number is received.
+ static void prvCoRoutineFlashWorkTask( xCoRoutineHandle xHandle, unsigned portBASE_TYPE uxIndex )
+ {
+ // Variables in co-routines must be declared static if they must maintain value across a blocking call.
+ static portBASE_TYPE xResult;
+ static unsigned portBASE_TYPE uxLEDToFlash;
+
+    // All co-routines must start with a call to crSTART().
+    crSTART( xHandle );
+
+    for( ;; )
+    {
+        // Wait for data to become available on the queue.
+        crQUEUE_RECEIVE( xHandle, xCoRoutineQueue, &uxLEDToFlash, portMAX_DELAY, &xResult );
+
+        if( xResult == pdPASS )
+        {
+            // We received the LED to flash - flash it!
+            vParTestToggleLED( uxLEDToFlash );
+        }
+    }
+
+    crEND();
+ }</pre>
+ * \defgroup crQUEUE_RECEIVE crQUEUE_RECEIVE
+ * \ingroup Tasks
+ */
+#define crQUEUE_RECEIVE( xHandle, pxQueue, pvBuffer, xTicksToWait, pxResult )			\
+{																						\
+	*( pxResult ) = xQueueCRReceive( ( pxQueue) , ( pvBuffer ), ( xTicksToWait ) );		\
+	if( *( pxResult ) == errQUEUE_BLOCKED ) 											\
+	{																					\
+		crSET_STATE0( ( xHandle ) );													\
+		*( pxResult ) = xQueueCRReceive( ( pxQueue) , ( pvBuffer ), 0 );				\
+	}																					\
+	if( *( pxResult ) == errQUEUE_YIELD )												\
+	{																					\
+		crSET_STATE1( ( xHandle ) );													\
+		*( pxResult ) = pdPASS;															\
+	}																					\
+}
+
+/**
+ * croutine. h
+ * <pre>
+  crQUEUE_SEND_FROM_ISR(
+                            xQueueHandle pxQueue,
+                            void *pvItemToQueue,
+                            portBASE_TYPE xCoRoutinePreviouslyWoken
+                       )</pre>
+ *
+ * The macro's crQUEUE_SEND_FROM_ISR() and crQUEUE_RECEIVE_FROM_ISR() are the
+ * co-routine equivalent to the xQueueSendFromISR() and xQueueReceiveFromISR()
+ * functions used by tasks.
+ *
+ * crQUEUE_SEND_FROM_ISR() and crQUEUE_RECEIVE_FROM_ISR() can only be used to
+ * pass data between a co-routine and and ISR, whereas xQueueSendFromISR() and
+ * xQueueReceiveFromISR() can only be used to pass data between a task and and
+ * ISR.
+ *
+ * crQUEUE_SEND_FROM_ISR can only be called from an ISR to send data to a queue
+ * that is being used from within a co-routine.
+ *
+ * See the co-routine section of the WEB documentation for information on
+ * passing data between tasks and co-routines and between ISR's and
+ * co-routines.
+ *
+ * @param xQueue The handle to the queue on which the item is to be posted.
+ *
+ * @param pvItemToQueue A pointer to the item that is to be placed on the
+ * queue.  The size of the items the queue will hold was defined when the
+ * queue was created, so this many bytes will be copied from pvItemToQueue
+ * into the queue storage area.
+ *
+ * @param xCoRoutinePreviouslyWoken This is included so an ISR can post onto
+ * the same queue multiple times from a single interrupt.  The first call
+ * should always pass in pdFALSE.  Subsequent calls should pass in
+ * the value returned from the previous call.
+ *
+ * @return pdTRUE if a co-routine was woken by posting onto the queue.  This is
+ * used by the ISR to determine if a context switch may be required following
+ * the ISR.
+ *
+ * Example usage:
+ <pre>
+ // A co-routine that blocks on a queue waiting for characters to be received.
+ static void vReceivingCoRoutine( xCoRoutineHandle xHandle, unsigned portBASE_TYPE uxIndex )
+ {
+ char cRxedChar;
+ portBASE_TYPE xResult;
+
+     // All co-routines must start with a call to crSTART().
+     crSTART( xHandle );
+
+     for( ;; )
+     {
+         // Wait for data to become available on the queue.  This assumes the
+         // queue xCommsRxQueue has already been created!
+         crQUEUE_RECEIVE( xHandle, xCommsRxQueue, &uxLEDToFlash, portMAX_DELAY, &xResult );
+
+         // Was a character received?
+         if( xResult == pdPASS )
+         {
+             // Process the character here.
+         }
+     }
+
+     // All co-routines must end with a call to crEND().
+     crEND();
+ }
+
+ // An ISR that uses a queue to send characters received on a serial port to
+ // a co-routine.
+ void vUART_ISR( void )
+ {
+ char cRxedChar;
+ portBASE_TYPE xCRWokenByPost = pdFALSE;
+
+     // We loop around reading characters until there are none left in the UART.
+     while( UART_RX_REG_NOT_EMPTY() )
+     {
+         // Obtain the character from the UART.
+         cRxedChar = UART_RX_REG;
+
+         // Post the character onto a queue.  xCRWokenByPost will be pdFALSE
+         // the first time around the loop.  If the post causes a co-routine
+         // to be woken (unblocked) then xCRWokenByPost will be set to pdTRUE.
+         // In this manner we can ensure that if more than one co-routine is
+         // blocked on the queue only one is woken by this ISR no matter how
+         // many characters are posted to the queue.
+         xCRWokenByPost = crQUEUE_SEND_FROM_ISR( xCommsRxQueue, &cRxedChar, xCRWokenByPost );
+     }
+ }</pre>
+ * \defgroup crQUEUE_SEND_FROM_ISR crQUEUE_SEND_FROM_ISR
+ * \ingroup Tasks
+ */
+#define crQUEUE_SEND_FROM_ISR( pxQueue, pvItemToQueue, xCoRoutinePreviouslyWoken ) xQueueCRSendFromISR( ( pxQueue ), ( pvItemToQueue ), ( xCoRoutinePreviouslyWoken ) )
+
+
+/**
+ * croutine. h
+ * <pre>
+  crQUEUE_SEND_FROM_ISR(
+                            xQueueHandle pxQueue,
+                            void *pvBuffer,
+                            portBASE_TYPE * pxCoRoutineWoken
+                       )</pre>
+ *
+ * The macro's crQUEUE_SEND_FROM_ISR() and crQUEUE_RECEIVE_FROM_ISR() are the
+ * co-routine equivalent to the xQueueSendFromISR() and xQueueReceiveFromISR()
+ * functions used by tasks.
+ *
+ * crQUEUE_SEND_FROM_ISR() and crQUEUE_RECEIVE_FROM_ISR() can only be used to
+ * pass data between a co-routine and and ISR, whereas xQueueSendFromISR() and
+ * xQueueReceiveFromISR() can only be used to pass data between a task and and
+ * ISR.
+ *
+ * crQUEUE_RECEIVE_FROM_ISR can only be called from an ISR to receive data
+ * from a queue that is being used from within a co-routine (a co-routine
+ * posted to the queue).
+ *
+ * See the co-routine section of the WEB documentation for information on
+ * passing data between tasks and co-routines and between ISR's and
+ * co-routines.
+ *
+ * @param xQueue The handle to the queue on which the item is to be posted.
+ *
+ * @param pvBuffer A pointer to a buffer into which the received item will be
+ * placed.  The size of the items the queue will hold was defined when the
+ * queue was created, so this many bytes will be copied from the queue into
+ * pvBuffer.
+ *
+ * @param pxCoRoutineWoken A co-routine may be blocked waiting for space to become
+ * available on the queue.  If crQUEUE_RECEIVE_FROM_ISR causes such a
+ * co-routine to unblock *pxCoRoutineWoken will get set to pdTRUE, otherwise
+ * *pxCoRoutineWoken will remain unchanged.
+ *
+ * @return pdTRUE an item was successfully received from the queue, otherwise
+ * pdFALSE.
+ *
+ * Example usage:
+ <pre>
+ // A co-routine that posts a character to a queue then blocks for a fixed
+ // period.  The character is incremented each time.
+ static void vSendingCoRoutine( xCoRoutineHandle xHandle, unsigned portBASE_TYPE uxIndex )
+ {
+ // cChar holds its value while this co-routine is blocked and must therefore
+ // be declared static.
+ static char cCharToTx = 'a';
+ portBASE_TYPE xResult;
+
+     // All co-routines must start with a call to crSTART().
+     crSTART( xHandle );
+
+     for( ;; )
+     {
+         // Send the next character to the queue.
+         crQUEUE_SEND( xHandle, xCoRoutineQueue, &cCharToTx, NO_DELAY, &xResult );
+
+         if( xResult == pdPASS )
+         {
+             // The character was successfully posted to the queue.
+         }
+		 else
+		 {
+			// Could not post the character to the queue.
+		 }
+
+         // Enable the UART Tx interrupt to cause an interrupt in this
+		 // hypothetical UART.  The interrupt will obtain the character
+		 // from the queue and send it.
+		 ENABLE_RX_INTERRUPT();
+
+		 // Increment to the next character then block for a fixed period.
+		 // cCharToTx will maintain its value across the delay as it is
+		 // declared static.
+		 cCharToTx++;
+		 if( cCharToTx > 'x' )
+		 {
+			cCharToTx = 'a';
+		 }
+		 crDELAY( 100 );
+     }
+
+     // All co-routines must end with a call to crEND().
+     crEND();
+ }
+
+ // An ISR that uses a queue to receive characters to send on a UART.
+ void vUART_ISR( void )
+ {
+ char cCharToTx;
+ portBASE_TYPE xCRWokenByPost = pdFALSE;
+
+     while( UART_TX_REG_EMPTY() )
+     {
+         // Are there any characters in the queue waiting to be sent?
+		 // xCRWokenByPost will automatically be set to pdTRUE if a co-routine
+		 // is woken by the post - ensuring that only a single co-routine is
+		 // woken no matter how many times we go around this loop.
+         if( crQUEUE_RECEIVE_FROM_ISR( pxQueue, &cCharToTx, &xCRWokenByPost ) )
+		 {
+			 SEND_CHARACTER( cCharToTx );
+		 }
+     }
+ }</pre>
+ * \defgroup crQUEUE_RECEIVE_FROM_ISR crQUEUE_RECEIVE_FROM_ISR
+ * \ingroup Tasks
+ */
+#define crQUEUE_RECEIVE_FROM_ISR( pxQueue, pvBuffer, pxCoRoutineWoken ) xQueueCRReceiveFromISR( ( pxQueue ), ( pvBuffer ), ( pxCoRoutineWoken ) )
+
+/*
+ * This function is intended for internal use by the co-routine macros only.
+ * The macro nature of the co-routine implementation requires that the
+ * prototype appears here.  The function should not be used by application
+ * writers.
+ *
+ * Removes the current co-routine from its ready list and places it in the
+ * appropriate delayed list.
+ */
+void vCoRoutineAddToDelayedList( portTickType xTicksToDelay, xList *pxEventList );
+
+/*
+ * This function is intended for internal use by the queue implementation only.
+ * The function should not be used by application writers.
+ *
+ * Removes the highest priority co-routine from the event list and places it in
+ * the pending ready list.
+ */
+signed portBASE_TYPE xCoRoutineRemoveFromEventList( const xList *pxEventList );
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CO_ROUTINE_H */