Merge remote-tracking branch 'origin/revo-fixes' into amorale/revo-merge

Conflicts:
	flight/Modules/ManualControl/manualcontrol.c
	make/scripts/version-info.py
	package/Makefile.linux

.gitignore

@@ -26,6 +26,9 @@ flight/Project/OpenPilotOSX/build
 # /flight/PipBee/
 /flight/PipBee/Build
 
+# /flight/Project/OpenPilotOSX/
+/flight/Project/OpenPilotOSX/build/
+
 # /ground/
 /ground/Build
 

Makefile

@@ -74,7 +74,7 @@ help:
 	@echo
 	@echo "   [Tool Installers]"
 	@echo "     qt_sdk_install       - Install the QT v4.7.3 tools"
-	@echo "     arm_sdk_install      - Install the Code Sourcery ARM gcc toolchain"
+	@echo "     arm_sdk_install      - Install the GNU ARM gcc toolchain"
 	@echo "     openocd_install      - Install the OpenOCD JTAG daemon"
 	@echo "     stm32flash_install   - Install the stm32flash tool for unbricking boards"
 	@echo "     dfuutil_install      - Install the dfu-util tool for unbricking F4-based boards"
@@ -120,6 +120,8 @@ help:
 	@echo "                            supported boards are ($(BL_BOARDS))"
 	@echo
 	@echo "   [Simulation]"
+	@echo "     sim_osx              - Build OpenPilot simulation firmware for OSX"
+	@echo "     sim_osx_clean        - Delete all build output for the osx simulation"
 	@echo "     sim_win32            - Build OpenPilot simulation firmware for"
 	@echo "                            Windows using mingw and msys"
 	@echo "     sim_win32_clean      - Delete all build output for the win32 simulation"
@@ -201,10 +203,10 @@ qt_sdk_clean:
 	$(V1) [ ! -d "$(QT_SDK_DIR)" ] || $(RM) -rf $(QT_SDK_DIR)
 
 # Set up ARM (STM32) SDK
-ARM_SDK_DIR := $(TOOLS_DIR)/arm-2011.03
+ARM_SDK_DIR := $(TOOLS_DIR)/gcc-arm-none-eabi-4_6-2012q1
 
 .PHONY: arm_sdk_install
-arm_sdk_install: ARM_SDK_URL  := https://sourcery.mentor.com/sgpp/lite/arm/portal/package8736/public/arm-none-eabi/arm-2011.03-42-arm-none-eabi-i686-pc-linux-gnu.tar.bz2
+arm_sdk_install: ARM_SDK_URL  := https://launchpad.net/gcc-arm-embedded/4.6/4.6-2012-q1-update/+download/gcc-arm-none-eabi-4_6-2012q1-20120316.tar.bz2
 arm_sdk_install: ARM_SDK_FILE := $(notdir $(ARM_SDK_URL))
 # order-only prereq on directory existance:
 arm_sdk_install: | $(DL_DIR) $(TOOLS_DIR)
@@ -861,43 +863,47 @@ 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 simposix
+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
 # all other platforms.
 ifneq ($(UNAME), Linux)
 ALL_BOARDS  := $(filter-out simposix, $(ALL_BOARDS))
+ALL_BOARDS_BU  := $(filter-out simposix, $(ALL_BOARDS_BU))
 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
 
-# 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
-
-# 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 '
 
 # Start out assuming that we'll build fw, bl and bu for all boards
 FW_BOARDS  := $(ALL_BOARDS)
 BL_BOARDS  := $(ALL_BOARDS)
-BU_BOARDS  := $(ALL_BOARDS)
+BU_BOARDS  := $(ALL_BOARDS_BU)
 EF_BOARDS  := $(ALL_BOARDS)
 
-# FIXME: The INS build doesn't have a bootloader or bootloader
-#        updater yet so we need to filter them out to prevent errors.
-BL_BOARDS  := $(filter-out ins, $(BL_BOARDS))
-BU_BOARDS  := $(filter-out ins, $(BU_BOARDS))
+# FIXME: The BU image doesn't work for F4 boards so we need to
+#        filter them out to prevent errors.
+BU_BOARDS  := $(filter-out revolution osd, $(BU_BOARDS))
+
+# SimPosix doesn't have a BL, BU or EF target so we need to
+# filter them out to prevent errors on the all_flight target.
+BL_BOARDS  := $(filter-out simposix, $(BL_BOARDS))
+BU_BOARDS  := $(filter-out simposix, $(BU_BOARDS))
+EF_BOARDS  := $(filter-out simposix, $(EF_BOARDS))
 
 # SimPosix doesn't have a BL, BU or EF target so we need to
 # filter them out to prevent errors on the all_flight target.
@@ -954,6 +960,13 @@ sim_win32_%: uavobjects_flight
 	$(V1) $(MAKE) --no-print-directory \
 		-C $(ROOT_DIR)/flight/OpenPilot --file=$(ROOT_DIR)/flight/OpenPilot/Makefile.win32 $*
 
+.PHONY: sim_osx
+sim_osx: sim_osx_elf
+
+sim_osx_%: uavobjects_flight
+	$(V1) mkdir -p $(BUILD_DIR)/sim_osx
+	$(V1) $(MAKE) --no-print-directory \
+		-C $(ROOT_DIR)/flight/Revolution --file=$(ROOT_DIR)/flight/Revolution/Makefile.osx $*
 ##############################
 #
 # Packaging components

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/OSD/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
+OSD_BL = $(WHEREAMI)
+OSD_BLINC = $(OSD_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  += $(OSD_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/OSD/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/OSD/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/OSD/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/OSD/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_OSD
+#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_OSD, 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/OSD/main.c

@@ -0,0 +1,208 @@
+/**
+ ******************************************************************************
+ * @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();
+#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();
+
+	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) {
+		PIOS_Board_Init();
+		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;
+}
+

flight/Bootloaders/OSD/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/OSD/pios_board.c

@@ -0,0 +1,78 @@
+/**
+ ******************************************************************************
+ *
+ * @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();
+	
+#if defined(PIOS_INCLUDE_LED)
+	PIOS_LED_Init(&pios_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_usb_main_cfg);
+
+#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/OSD/pios_usb_board_data.c

@@ -0,0 +1,91 @@
+/**
+ ******************************************************************************
+ * @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[8] = {
+	sizeof(usb_product_id),
+	USB_DESC_TYPE_STRING,
+	'O', 0,
+	'S', 0,
+	'D', 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/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/Makefile

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

flight/Bootloaders/Revolution/inc/pios_config.h

@@ -1,43 +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 */
+/**
+ ******************************************************************************
+ *
+ * @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/Revolution/inc/pios_usb_board_data.h

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

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/Bootloaders/Revolution/pios_usb_board_data.c

@@ -31,6 +31,7 @@
 #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),
@@ -47,40 +48,15 @@ static const uint8_t usb_product_id[22] = {
 	'n', 0,
 };
 
-static uint8_t usb_serial_number[52] = {
+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,
-	0, 0,
-	0, 0,
-	0, 0,
-	0, 0,
-	0, 0,
-	0, 0,
-	0, 0,
-	0, 0,
-	0, 0,
-	0, 0,
-	0, 0,
-	0, 0,
-	0, 0,
-	0, 0,
-	0, 0,
-	0, 0,
-	0, 0,
-	0, 0,
-	0, 0,
-	0, 0,
-	0, 0,
-	0, 0,
-	0, 0,
-	0, 0,
-	0, 0
 };
 
 static const struct usb_string_langid usb_lang_id = {
 	.bLength = sizeof(usb_lang_id),
 	.bDescriptorType = USB_DESC_TYPE_STRING,
-	.bLangID = htousbs(USB_LANGID_ENGLISH_UK),
+	.bLangID = htousbs(USB_LANGID_ENGLISH_US),
 };
 
 static const uint8_t usb_vendor_id[28] = {
@@ -104,11 +80,13 @@ static const uint8_t usb_vendor_id[28] = {
 int32_t PIOS_USB_BOARD_DATA_Init(void)
 {
 	/* Load device serial number into serial number string */
-	uint8_t sn[25];
+	uint8_t sn[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1];
 	PIOS_SYS_SerialNumberGet((char *)sn);
-	for (uint8_t i = 0; sn[i] != '\0' && (2 * i) < usb_serial_number[0]; i++) {
-		usb_serial_number[2 + 2 * i] = sn[i];
-	}
+
+	/* 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));

flight/CopterControl/Makefile

@@ -144,9 +144,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
@@ -244,7 +245,6 @@ SRC += $(PIOSSTM32F10X)/pios_spi.c
 SRC += $(PIOSSTM32F10X)/pios_ppm.c
 SRC += $(PIOSSTM32F10X)/pios_pwm.c
 SRC += $(PIOSSTM32F10X)/pios_dsm.c
-SRC += $(PIOSSTM32F10X)/pios_sbus.c
 SRC += $(PIOSSTM32F10X)/pios_debug.c
 SRC += $(PIOSSTM32F10X)/pios_gpio.c
 SRC += $(PIOSSTM32F10X)/pios_exti.c
@@ -274,6 +274,7 @@ SRC += $(PIOSCOMMON)/pios_flash_jedec.c
 SRC += $(PIOSCOMMON)/pios_adxl345.c
 SRC += $(PIOSCOMMON)/pios_mpu6000.c
 SRC += $(PIOSCOMMON)/pios_com.c
+SRC += $(PIOSCOMMON)/pios_sbus.c
 #SRC += $(PIOSCOMMON)/pios_i2c_esc.c
 #SRC += $(PIOSCOMMON)/pios_bmp085.c
 SRC += $(PIOSCOMMON)/pios_rcvr.c
@@ -324,8 +325,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
@@ -396,7 +397,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

@@ -111,15 +111,16 @@ static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = {
 static const struct pios_mpu6000_cfg pios_mpu6000_cfg = {
 	.exti_cfg = &pios_exti_mpu6000_cfg,
 	.Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT,
-	// Clock at 8 khz, downsampled by 8 for 1khz
+	// Clock at 8 khz, downsampled by 16 for 500 Hz
 	.Smpl_rate_div = 15,
 	.interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD,
 	.interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY,
-	.User_ctl = PIOS_MPU6000_USERCTL_FIFO_EN,
+	.User_ctl = PIOS_MPU6000_USERCTL_FIFO_EN | PIOS_MPU6000_USERCTL_DIS_I2C,
 	.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/CoordinateConversions.c

@@ -43,12 +43,12 @@ void LLA2ECEF(float LLA[3], float ECEF[3])
 	float sinLat, sinLon, cosLat, cosLon;
 	float N;
 
-	sinLat = sin(DEG2RAD * LLA[0]);
-	sinLon = sin(DEG2RAD * LLA[1]);
-	cosLat = cos(DEG2RAD * LLA[0]);
-	cosLon = cos(DEG2RAD * LLA[1]);
+	sinLat = sinf(DEG2RAD * LLA[0]);
+	sinLon = sinf(DEG2RAD * LLA[1]);
+	cosLat = cosf(DEG2RAD * LLA[0]);
+	cosLon = cosf(DEG2RAD * LLA[1]);
 
-	N = a / sqrt(1.0 - e * e * sinLat * sinLat);	//prime vertical radius of curvature
+	N = a / sqrtf(1.0f - e * e * sinLat * sinLat);	//prime vertical radius of curvature
 
 	ECEF[0] = (N + LLA[2]) * cosLat * cosLon;
 	ECEF[1] = (N + LLA[2]) * cosLat * sinLon;
@@ -68,28 +68,28 @@ uint16_t ECEF2LLA(float ECEF[3], float LLA[3])
 	 **/
 
 	const float a = 6378137.0;	// Equatorial Radius
-	const float e = 8.1819190842622e-2;	// Eccentricity
+	const float e = 8.1819190842622e-2f;	// Eccentricity
 	float x = ECEF[0], y = ECEF[1], z = ECEF[2];
 	float Lat, N, NplusH, delta, esLat;
 	uint16_t iter;
 #define MAX_ITER 10		// should not take more than 5 for valid coordinates
 #define ACCURACY 1.0e-11	// used to be e-14, but we don't need sub micrometer exact calculations
 
-	LLA[1] = RAD2DEG * atan2(y, x);
+	LLA[1] = RAD2DEG * atan2f(y, x);
 	Lat = DEG2RAD * LLA[0];
-	esLat = e * sin(Lat);
-	N = a / sqrt(1 - esLat * esLat);
+	esLat = e * sinf(Lat);
+	N = a / sqrtf(1 - esLat * esLat);
 	NplusH = N + LLA[2];
 	delta = 1;
 	iter = 0;
 
 	while (((delta > ACCURACY) || (delta < -ACCURACY))
 	       && (iter < MAX_ITER)) {
-		delta = Lat - atan(z / (sqrt(x * x + y * y) * (1 - (N * e * e / NplusH))));
+		delta = Lat - atanf(z / (sqrtf(x * x + y * y) * (1 - (N * e * e / NplusH))));
 		Lat = Lat - delta;
-		esLat = e * sin(Lat);
-		N = a / sqrt(1 - esLat * esLat);
-		NplusH = sqrt(x * x + y * y) / cos(Lat);
+		esLat = e * sinf(Lat);
+		N = a / sqrtf(1 - esLat * esLat);
+		NplusH = sqrtf(x * x + y * y) / cosf(Lat);
 		iter += 1;
 	}
 
@@ -104,10 +104,10 @@ void RneFromLLA(float LLA[3], float Rne[3][3])
 {
 	float sinLat, sinLon, cosLat, cosLon;
 
-	sinLat = (float)sin(DEG2RAD * LLA[0]);
-	sinLon = (float)sin(DEG2RAD * LLA[1]);
-	cosLat = (float)cos(DEG2RAD * LLA[0]);
-	cosLon = (float)cos(DEG2RAD * LLA[1]);
+	sinLat = (float)sinf(DEG2RAD * LLA[0]);
+	sinLon = (float)sinf(DEG2RAD * LLA[1]);
+	cosLat = (float)cosf(DEG2RAD * LLA[0]);
+	cosLon = (float)cosf(DEG2RAD * LLA[1]);
 
 	Rne[0][0] = -sinLat * cosLon;
 	Rne[0][1] = -sinLat * sinLon;

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/insgps.h

@@ -39,6 +39,8 @@
   * @{
   */
 #define POS_SENSORS 0x007
+#define HORIZ_POS_SENSORS 0x003
+#define VER_POS_SENSORS 0x004
 #define HORIZ_SENSORS 0x018
 #define VERT_SENSORS  0x020
 #define MAG_SENSORS 0x1C0
@@ -64,6 +66,7 @@ void INSSetAccelVar(float accel_var[3]);
 void INSSetGyroVar(float gyro_var[3]);
 void INSSetMagNorth(float B[3]);
 void INSSetMagVar(float scaled_mag_var[3]);
+void INSSetBaroVar(float baro_var);
 void INSPosVelReset(float pos[3], float vel[3]);
 
 void MagCorrection(float mag_data[3]);

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/inc/paths.h

@@ -0,0 +1,39 @@
+/**
+ ******************************************************************************
+ *
+ * @file       paths.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @brief      Header for path manipulation library 
+ *
+ * @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 PATHS_H_
+#define PATHS_H_
+
+struct path_status {
+	float fractional_progress;
+	float error;
+	float correction_direction[2];
+	float path_direction[2];
+};
+
+void path_progress(float * start_point, float * end_point, float * cur_point, struct path_status * status, uint8_t mode);
+
+#endif

flight/Libraries/insgps13state.c

@@ -121,7 +121,7 @@ void INSGPSInit()		//pretty much just a place holder for now
 	R[3] = R[4] = 0.004f;   // High freq GPS horizontal velocity noise variance (m/s)^2
 	R[5] = 100.0f;          // High freq GPS vertical velocity noise variance (m/s)^2
 	R[6] = R[7] = R[8] = 0.005f;    // magnetometer unit vector noise variance
-	R[9] = .05f;                    // High freq altimeter noise variance (m^2)
+	R[9] = .25f;                    // High freq altimeter noise variance (m^2)
 }
 
 void INSResetP(float PDiag[NUMX])
@@ -183,7 +183,7 @@ void INSSetPosVelVar(float PosVar, float VelVar)
 	R[2] = PosVar;
 	R[3] = VelVar;
 	R[4] = VelVar;
-//    R[5] = PosVar;  // Don't change vertical velocity, not measured
+	R[5] = VelVar;
 }
 
 void INSSetGyroBias(float gyro_bias[3])
@@ -214,11 +214,17 @@ void INSSetMagVar(float scaled_mag_var[3])
 	R[8] = scaled_mag_var[2];
 }
 
+void INSSetBaroVar(float baro_var)
+{
+	R[9] = baro_var;
+}
+
 void INSSetMagNorth(float B[3])
 {
-	Be[0] = B[0];
-	Be[1] = B[1];
-	Be[2] = B[2];
+	float mag = sqrtf(B[0] * B[0] + B[1] * B[1] + B[2] * B[2]);
+	Be[0] = B[0] / mag;
+	Be[1] = B[1] / mag;
+	Be[2] = B[2] / mag;
 }
 
 void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT)
@@ -353,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 *************
@@ -1397,7 +1406,7 @@ void RungeKutta(float X[NUMX], float U[NUMU], float dT)
 {
 
 	float dT2 =
-	    dT / 2, K1[NUMX], K2[NUMX], K3[NUMX], K4[NUMX], Xlast[NUMX];
+	    dT / 2.0f, K1[NUMX], K2[NUMX], K3[NUMX], K4[NUMX], Xlast[NUMX];
 	uint8_t i;
 
 	for (i = 0; i < NUMX; i++)
@@ -1417,8 +1426,8 @@ void RungeKutta(float X[NUMX], float U[NUMU], float dT)
 	// Xnew  = X + dT*(k1+2*k2+2*k3+k4)/6
 	for (i = 0; i < NUMX; i++)
 		X[i] =
-		    Xlast[i] + dT * (K1[i] + 2 * K2[i] + 2 * K3[i] +
-				     K4[i]) / 6;
+		    Xlast[i] + dT * (K1[i] + 2.0f * K2[i] + 2.0f * K3[i] +
+				     K4[i]) / 6.0f;
 }
 
 //  *************  Model Specific Stuff  ***************************
@@ -1463,23 +1472,23 @@ void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX])
 
 	// Vdot = Reb*a
 	Xdot[3] =
-	    (q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * ax + 2 * (q1 * q2 -
+	    (q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * ax + 2.0f * (q1 * q2 -
 								q0 * q3) *
-	    ay + 2 * (q1 * q3 + q0 * q2) * az;
+	    ay + 2.0f * (q1 * q3 + q0 * q2) * az;
 	Xdot[4] =
-	    2 * (q1 * q2 + q0 * q3) * ax + (q0 * q0 - q1 * q1 + q2 * q2 -
+	    2.0f * (q1 * q2 + q0 * q3) * ax + (q0 * q0 - q1 * q1 + q2 * q2 -
 					    q3 * q3) * ay + 2 * (q2 * q3 -
 								 q0 * q1) *
 	    az;
 	Xdot[5] =
-	    2 * (q1 * q3 - q0 * q2) * ax + 2 * (q2 * q3 + q0 * q1) * ay +
-	    (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * az + 9.81;
+	    2.0f * (q1 * q3 - q0 * q2) * ax + 2 * (q2 * q3 + q0 * q1) * ay +
+	    (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * az + 9.81f;
 
 	// qdot = Q*w
-	Xdot[6] = (-q1 * wx - q2 * wy - q3 * wz) / 2;
-	Xdot[7] = (q0 * wx - q3 * wy + q2 * wz) / 2;
-	Xdot[8] = (q3 * wx + q0 * wy - q1 * wz) / 2;
-	Xdot[9] = (-q2 * wx + q1 * wy + q0 * wz) / 2;
+	Xdot[6] = (-q1 * wx - q2 * wy - q3 * wz) / 2.0f;
+	Xdot[7] = (q0 * wx - q3 * wy + q2 * wz) / 2.0f;
+	Xdot[8] = (q3 * wx + q0 * wy - q1 * wz) / 2.0f;
+	Xdot[9] = (-q2 * wx + q1 * wy + q0 * wz) / 2.0f;
 
 	// best guess is that bias stays constant
 	Xdot[10] = Xdot[11] = Xdot[12] = 0;
@@ -1503,21 +1512,21 @@ void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
 	q3 = X[9];
 
 	// Pdot = V
-	F[0][3] = F[1][4] = F[2][5] = 1;
+	F[0][3] = F[1][4] = F[2][5] = 1.0f;
 
 	// dVdot/dq
-	F[3][6] = 2 * (q0 * ax - q3 * ay + q2 * az);
-	F[3][7] = 2 * (q1 * ax + q2 * ay + q3 * az);
-	F[3][8] = 2 * (-q2 * ax + q1 * ay + q0 * az);
-	F[3][9] = 2 * (-q3 * ax - q0 * ay + q1 * az);
-	F[4][6] = 2 * (q3 * ax + q0 * ay - q1 * az);
-	F[4][7] = 2 * (q2 * ax - q1 * ay - q0 * az);
-	F[4][8] = 2 * (q1 * ax + q2 * ay + q3 * az);
-	F[4][9] = 2 * (q0 * ax - q3 * ay + q2 * az);
-	F[5][6] = 2 * (-q2 * ax + q1 * ay + q0 * az);
-	F[5][7] = 2 * (q3 * ax + q0 * ay - q1 * az);
-	F[5][8] = 2 * (-q0 * ax + q3 * ay - q2 * az);
-	F[5][9] = 2 * (q1 * ax + q2 * ay + q3 * az);
+	F[3][6] = 2.0f * (q0 * ax - q3 * ay + q2 * az);
+	F[3][7] = 2.0f * (q1 * ax + q2 * ay + q3 * az);
+	F[3][8] = 2.0f * (-q2 * ax + q1 * ay + q0 * az);
+	F[3][9] = 2.0f * (-q3 * ax - q0 * ay + q1 * az);
+	F[4][6] = 2.0f * (q3 * ax + q0 * ay - q1 * az);
+	F[4][7] = 2.0f * (q2 * ax - q1 * ay - q0 * az);
+	F[4][8] = 2.0f * (q1 * ax + q2 * ay + q3 * az);
+	F[4][9] = 2.0f * (q0 * ax - q3 * ay + q2 * az);
+	F[5][6] = 2.0f * (-q2 * ax + q1 * ay + q0 * az);
+	F[5][7] = 2.0f * (q3 * ax + q0 * ay - q1 * az);
+	F[5][8] = 2.0f * (-q0 * ax + q3 * ay - q2 * az);
+	F[5][9] = 2.0f * (q1 * ax + q2 * ay + q3 * az);
 
 	// dVdot/dabias & dVdot/dna  - NO BIAS STATES ON ACCELS - S0 REPEAT FOR G BELOW
 	// F[3][13]=G[3][3]=-q0*q0-q1*q1+q2*q2+q3*q3; F[3][14]=G[3][4]=2*(-q1*q2+q0*q3);         F[3][15]=G[3][5]=-2*(q1*q3+q0*q2);
@@ -1526,63 +1535,63 @@ void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
 
 	// dqdot/dq
 	F[6][6] = 0;
-	F[6][7] = -wx / 2;
-	F[6][8] = -wy / 2;
-	F[6][9] = -wz / 2;
-	F[7][6] = wx / 2;
+	F[6][7] = -wx / 2.0f;
+	F[6][8] = -wy / 2.0f;
+	F[6][9] = -wz / 2.0f;
+	F[7][6] = wx / 2.0f;
 	F[7][7] = 0;
-	F[7][8] = wz / 2;
-	F[7][9] = -wy / 2;
-	F[8][6] = wy / 2;
-	F[8][7] = -wz / 2;
+	F[7][8] = wz / 2.0f;
+	F[7][9] = -wy / 2.0f;
+	F[8][6] = wy / 2.0f;
+	F[8][7] = -wz / 2.0f;
 	F[8][8] = 0;
-	F[8][9] = wx / 2;
-	F[9][6] = wz / 2;
-	F[9][7] = wy / 2;
-	F[9][8] = -wx / 2;
+	F[8][9] = wx / 2.0f;
+	F[9][6] = wz / 2.0f;
+	F[9][7] = wy / 2.0f;
+	F[9][8] = -wx / 2.0f;
 	F[9][9] = 0;
 
 	// dqdot/dwbias
-	F[6][10] = q1 / 2;
-	F[6][11] = q2 / 2;
-	F[6][12] = q3 / 2;
-	F[7][10] = -q0 / 2;
-	F[7][11] = q3 / 2;
-	F[7][12] = -q2 / 2;
-	F[8][10] = -q3 / 2;
-	F[8][11] = -q0 / 2;
-	F[8][12] = q1 / 2;
-	F[9][10] = q2 / 2;
-	F[9][11] = -q1 / 2;
-	F[9][12] = -q0 / 2;
+	F[6][10] = q1 / 2.0f;
+	F[6][11] = q2 / 2.0f;
+	F[6][12] = q3 / 2.0f;
+	F[7][10] = -q0 / 2.0f;
+	F[7][11] = q3 / 2.0f;
+	F[7][12] = -q2 / 2.0f;
+	F[8][10] = -q3 / 2.0f;
+	F[8][11] = -q0 / 2.0f;
+	F[8][12] = q1 / 2.0f;
+	F[9][10] = q2 / 2.0f;
+	F[9][11] = -q1 / 2.0f;
+	F[9][12] = -q0 / 2.0f;
 
 	// dVdot/dna  - NO BIAS STATES ON ACCELS - S0 REPEAT FOR G HERE
 	G[3][3] = -q0 * q0 - q1 * q1 + q2 * q2 + q3 * q3;
-	G[3][4] = 2 * (-q1 * q2 + q0 * q3);
-	G[3][5] = -2 * (q1 * q3 + q0 * q2);
-	G[4][3] = -2 * (q1 * q2 + q0 * q3);
+	G[3][4] = 2.0f * (-q1 * q2 + q0 * q3);
+	G[3][5] = -2.0f * (q1 * q3 + q0 * q2);
+	G[4][3] = -2.0f * (q1 * q2 + q0 * q3);
 	G[4][4] = -q0 * q0 + q1 * q1 - q2 * q2 + q3 * q3;
-	G[4][5] = 2 * (-q2 * q3 + q0 * q1);
-	G[5][3] = 2 * (-q1 * q3 + q0 * q2);
-	G[5][4] = -2 * (q2 * q3 + q0 * q1);
+	G[4][5] = 2.0f * (-q2 * q3 + q0 * q1);
+	G[5][3] = 2.0f * (-q1 * q3 + q0 * q2);
+	G[5][4] = -2.0f * (q2 * q3 + q0 * q1);
 	G[5][5] = -q0 * q0 + q1 * q1 + q2 * q2 - q3 * q3;
 
 	// dqdot/dnw
-	G[6][0] = q1 / 2;
-	G[6][1] = q2 / 2;
-	G[6][2] = q3 / 2;
-	G[7][0] = -q0 / 2;
-	G[7][1] = q3 / 2;
-	G[7][2] = -q2 / 2;
-	G[8][0] = -q3 / 2;
-	G[8][1] = -q0 / 2;
-	G[8][2] = q1 / 2;
-	G[9][0] = q2 / 2;
-	G[9][1] = -q1 / 2;
-	G[9][2] = -q0 / 2;
+	G[6][0] = q1 / 2.0f;
+	G[6][1] = q2 / 2.0f;
+	G[6][2] = q3 / 2.0f;
+	G[7][0] = -q0 / 2.0f;
+	G[7][1] = q3 / 2.0f;
+	G[7][2] = -q2 / 2.0f;
+	G[8][0] = -q3 / 2.0f;
+	G[8][1] = -q0 / 2.0f;
+	G[8][2] = q1 / 2.0f;
+	G[9][0] = q2 / 2.0f;
+	G[9][1] = -q1 / 2.0f;
+	G[9][2] = -q0 / 2.0f;
 
 	// dwbias = random walk noise
-	G[10][6] = G[11][7] = G[12][8] = 1;
+	G[10][6] = G[11][7] = G[12][8] = 1.0f;
 	// dabias = random walk noise
 	// G[13][9]=G[14][10]=G[15][11]=1;  // NO BIAS STATES ON ACCELS
 }
@@ -1607,19 +1616,19 @@ void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV])
 	// Bb=Rbe*Be
 	Y[6] =
 	    (q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * Be[0] +
-	    2 * (q1 * q2 + q0 * q3) * Be[1] + 2 * (q1 * q3 -
+	    2.0f * (q1 * q2 + q0 * q3) * Be[1] + 2.0f * (q1 * q3 -
 						   q0 * q2) * Be[2];
 	Y[7] =
-	    2 * (q1 * q2 - q0 * q3) * Be[0] + (q0 * q0 - q1 * q1 +
+	    2.0f * (q1 * q2 - q0 * q3) * Be[0] + (q0 * q0 - q1 * q1 +
 					       q2 * q2 - q3 * q3) * Be[1] +
-	    2 * (q2 * q3 + q0 * q1) * Be[2];
+	    2.0f * (q2 * q3 + q0 * q1) * Be[2];
 	Y[8] =
-	    2 * (q1 * q3 + q0 * q2) * Be[0] + 2 * (q2 * q3 -
+	    2.0f * (q1 * q3 + q0 * q2) * Be[0] + 2.0f * (q2 * q3 -
 						   q0 * q1) * Be[1] +
 	    (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * Be[2];
 
 	// Alt = -Pz
-	Y[9] = -X[2];
+	Y[9] = -1.0f * X[2];
 }
 
 void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX])
@@ -1632,26 +1641,26 @@ void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX])
 	q3 = X[9];
 
 	// dP/dP=I;
-	H[0][0] = H[1][1] = H[2][2] = 1;
+	H[0][0] = H[1][1] = H[2][2] = 1.0f;
 	// dV/dV=I;
-	H[3][3] = H[4][4] = H[5][5] = 1;
+	H[3][3] = H[4][4] = H[5][5] = 1.0f;
 
 	// dBb/dq
-	H[6][6] = 2 * (q0 * Be[0] + q3 * Be[1] - q2 * Be[2]);
-	H[6][7] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
-	H[6][8] = 2 * (-q2 * Be[0] + q1 * Be[1] - q0 * Be[2]);
-	H[6][9] = 2 * (-q3 * Be[0] + q0 * Be[1] + q1 * Be[2]);
-	H[7][6] = 2 * (-q3 * Be[0] + q0 * Be[1] + q1 * Be[2]);
-	H[7][7] = 2 * (q2 * Be[0] - q1 * Be[1] + q0 * Be[2]);
-	H[7][8] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
-	H[7][9] = 2 * (-q0 * Be[0] - q3 * Be[1] + q2 * Be[2]);
-	H[8][6] = 2 * (q2 * Be[0] - q1 * Be[1] + q0 * Be[2]);
-	H[8][7] = 2 * (q3 * Be[0] - q0 * Be[1] - q1 * Be[2]);
-	H[8][8] = 2 * (q0 * Be[0] + q3 * Be[1] - q2 * Be[2]);
-	H[8][9] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
+	H[6][6] = 2.0f * (q0 * Be[0] + q3 * Be[1] - q2 * Be[2]);
+	H[6][7] = 2.0f * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
+	H[6][8] = 2.0f * (-q2 * Be[0] + q1 * Be[1] - q0 * Be[2]);
+	H[6][9] = 2.0f * (-q3 * Be[0] + q0 * Be[1] + q1 * Be[2]);
+	H[7][6] = 2.0f * (-q3 * Be[0] + q0 * Be[1] + q1 * Be[2]);
+	H[7][7] = 2.0f * (q2 * Be[0] - q1 * Be[1] + q0 * Be[2]);
+	H[7][8] = 2.0f * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
+	H[7][9] = 2.0f * (-q0 * Be[0] - q3 * Be[1] + q2 * Be[2]);
+	H[8][6] = 2.0f * (q2 * Be[0] - q1 * Be[1] + q0 * Be[2]);
+	H[8][7] = 2.0f * (q3 * Be[0] - q0 * Be[1] - q1 * Be[2]);
+	H[8][8] = 2.0f * (q0 * Be[0] + q3 * Be[1] - q2 * Be[2]);
+	H[8][9] = 2.0f * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
 
 	// dAlt/dPz = -1
-	H[9][2] = -1;
+	H[9][2] = -1.0f;
 }
 
 /**

flight/Libraries/insgps16state.c

@@ -76,37 +76,37 @@ uint16_t ins_get_num_states()
 
 void INSGPSInit()		//pretty much just a place holder for now
 {
-	Be[0] = 1;
+	Be[0] = 1.0f;
 	Be[1] = 0;
 	Be[2] = 0;		// local magnetic unit vector
 
 	for (int i = 0; i < NUMX; i++) {
 		for (int j = 0; j < NUMX; j++) {
-			P[i][j] = 0; // zero all terms
+			P[i][j] = 0.0f; // zero all terms
 		}
 	}
 	
-	P[0][0] = P[1][1] = P[2][2] = 25;	// initial position variance (m^2)
-	P[3][3] = P[4][4] = P[5][5] = 5;	// initial velocity variance (m/s)^2
-	P[6][6] = P[7][7] = P[8][8] = P[9][9] = 1e-5;	// initial quaternion variance
-	P[10][10] = P[11][11] = P[12][12] = 1e-5;	// initial gyro bias variance (rad/s)^2
+	P[0][0] = P[1][1] = P[2][2] = 25.0f;	// initial position variance (m^2)
+	P[3][3] = P[4][4] = P[5][5] = 5.0f;	// initial velocity variance (m/s)^2
+	P[6][6] = P[7][7] = P[8][8] = P[9][9] = 1e-5f;	// initial quaternion variance
+	P[10][10] = P[11][11] = P[12][12] = 1e-5f;	// initial gyro bias variance (rad/s)^2
 
-	X[0] = X[1] = X[2] = X[3] = X[4] = X[5] = 0;	// initial pos and vel (m)
-	X[6] = 1;
-	X[7] = X[8] = X[9] = 0;	// initial quaternion (level and North) (m/s)
-	X[10] = X[11] = X[12] = 0;	// initial gyro bias (rad/s)
+	X[0] = X[1] = X[2] = X[3] = X[4] = X[5] = 0.0f;	// initial pos and vel (m)
+	X[6] = 1.0f;
+	X[7] = X[8] = X[9] = 0.0f;	// initial quaternion (level and North) (m/s)
+	X[10] = X[11] = X[12] = 0.0f;	// initial gyro bias (rad/s)
 
-	Q[0] = Q[1] = Q[2] = 50e-8;	// gyro noise variance (rad/s)^2
-	Q[3] = Q[4] = Q[5] = 0.01;	// accelerometer noise variance (m/s^2)^2
-	Q[6] = Q[7] = Q[8] = 2e-9;	// gyro bias random walk variance (rad/s^2)^2
-	Q[9] = Q[10] = Q[11] = 2e-20;	// accel bias random walk variance (m/s^3)^2
+	Q[0] = Q[1] = Q[2] = 50e-8f;	// gyro noise variance (rad/s)^2
+	Q[3] = Q[4] = Q[5] = 0.01f;	// accelerometer noise variance (m/s^2)^2
+	Q[6] = Q[7] = Q[8] = 2e-9f;	// gyro bias random walk variance (rad/s^2)^2
+	Q[9] = Q[10] = Q[11] = 2e-20f;	// accel bias random walk variance (m/s^3)^2
 
-	R[0] = R[1] = 0.004;	// High freq GPS horizontal position noise variance (m^2)
-	R[2] = 0.036;		// High freq GPS vertical position noise variance (m^2)
-	R[3] = R[4] = 0.004;	// High freq GPS horizontal velocity noise variance (m/s)^2
-	R[5] = 100;		// High freq GPS vertical velocity noise variance (m/s)^2
-	R[6] = R[7] = R[8] = 0.005;	// magnetometer unit vector noise variance
-	R[9] = .05;		// High freq altimeter noise variance (m^2)
+	R[0] = R[1] = 0.004f;	// High freq GPS horizontal position noise variance (m^2)
+	R[2] = 0.036f;		// High freq GPS vertical position noise variance (m^2)
+	R[3] = R[4] = 0.004f;	// High freq GPS horizontal velocity noise variance (m/s)^2
+	R[5] = 100.0f;		// High freq GPS vertical velocity noise variance (m/s)^2
+	R[6] = R[7] = R[8] = 0.005f;	// magnetometer unit vector noise variance
+	R[9] = .05f;		// High freq altimeter noise variance (m^2)
 }
 
 void INSResetP(float PDiag[NUMX])
@@ -117,7 +117,7 @@ void INSResetP(float PDiag[NUMX])
 	for (i=0;i<NUMX;i++){
 		if (PDiag != 0){
 			for (j=0;j<NUMX;j++)
-				P[i][j]=P[j][i]=0;
+				P[i][j]=P[j][i]=0.0f;
 			P[i][i]=PDiag[i];
 		}
 	}
@@ -147,13 +147,13 @@ void INSPosVelReset(float pos[3], float vel[3])
 {
 	for (int i = 0; i < 6; i++) {
 		for(int j = i; j < NUMX; j++) {
-			P[i][j] = 0;  // zero the first 6 rows and columns
-			P[j][i] = 0; 
+			P[i][j] = 0.0f;  // zero the first 6 rows and columns
+			P[j][i] = 0.0f; 
 		}
 	}
 	
-	P[0][0] = P[1][1] = P[2][2] = 25;	// initial position variance (m^2)
-	P[3][3] = P[4][4] = P[5][5] = 5;	// initial velocity variance (m/s)^2
+	P[0][0] = P[1][1] = P[2][2] = 25.0f;	// initial position variance (m^2)
+	P[3][3] = P[4][4] = P[5][5] = 5.0f;	// initial velocity variance (m/s)^2
 	
 	X[0] = pos[0];
 	X[1] = pos[1];
@@ -170,7 +170,7 @@ void INSSetPosVelVar(float PosVar, float VelVar)
 	R[2] = PosVar;
 	R[3] = VelVar;
 	R[4] = VelVar;
-//    R[5] = PosVar;  // Don't change vertical velocity, not measured
+    R[5] = PosVar;  // Don't change vertical velocity, not measured
 }
 
 void INSSetGyroBias(float gyro_bias[3])
@@ -254,7 +254,7 @@ void INSCovariancePrediction(float dT)
 	CovariancePrediction(F, G, Q, dT, P);
 }
 
-float zeros[3] = { 0, 0, 0 };
+float zeros[3] = { 0.0f, 0.0f, 0.0f };
 
 void MagCorrection(float mag_data[3])
 {
@@ -573,7 +573,7 @@ void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
 		if (SensorsUsed & (0x01 << m)) {	// use this sensor for update
 
 			for (j = 0; j < NUMX; j++) {	// Find Hp = H*P
-				HP[j] = 0;
+				HP[j] = 0.0f;
 				for (k = 0; k < NUMX; k++)
 					HP[j] += H[m][k] * P[k][j];
 			}
@@ -609,7 +609,7 @@ void RungeKutta(float X[NUMX], float U[NUMU], float dT)
 {
 
 	float dT2 =
-	    dT / 2, K1[NUMX], K2[NUMX], K3[NUMX], K4[NUMX], Xlast[NUMX];
+	    dT / 2.0f, K1[NUMX], K2[NUMX], K3[NUMX], K4[NUMX], Xlast[NUMX];
 	uint8_t i;
 
 	for (i = 0; i < NUMX; i++)
@@ -629,8 +629,8 @@ void RungeKutta(float X[NUMX], float U[NUMU], float dT)
 	// Xnew  = X + dT*(k1+2*k2+2*k3+k4)/6
 	for (i = 0; i < NUMX; i++)
 		X[i] =
-		    Xlast[i] + dT * (K1[i] + 2 * K2[i] + 2 * K3[i] +
-				     K4[i]) / 6;
+		    Xlast[i] + dT * (K1[i] + 2.0f * K2[i] + 2.0f * K3[i] +
+				     K4[i]) / 6.0f;
 }
 
 //  *************  Model Specific Stuff  ***************************
@@ -674,23 +674,23 @@ void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX])
 
 	// Vdot = Reb*a
 	Xdot[3] =
-	    (q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * ax + 2 * (q1 * q2 -
+	    (q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * ax + 2.0f * (q1 * q2 -
 								q0 * q3) *
-	    ay + 2 * (q1 * q3 + q0 * q2) * az;
+	    ay + 2.0f * (q1 * q3 + q0 * q2) * az;
 	Xdot[4] =
-	    2 * (q1 * q2 + q0 * q3) * ax + (q0 * q0 - q1 * q1 + q2 * q2 -
-					    q3 * q3) * ay + 2 * (q2 * q3 -
+	    2.0f * (q1 * q2 + q0 * q3) * ax + (q0 * q0 - q1 * q1 + q2 * q2 -
+					    q3 * q3) * ay + 2.0f * (q2 * q3 -
 								 q0 * q1) *
 	    az;
 	Xdot[5] =
-	    2 * (q1 * q3 - q0 * q2) * ax + 2 * (q2 * q3 + q0 * q1) * ay +
-	    (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * az + 9.81;
+	    2.0f * (q1 * q3 - q0 * q2) * ax + 2.0f * (q2 * q3 + q0 * q1) * ay +
+	    (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * az + 9.81f;
 
 	// qdot = Q*w
-	Xdot[6] = (-q1 * wx - q2 * wy - q3 * wz) / 2;
-	Xdot[7] = (q0 * wx - q3 * wy + q2 * wz) / 2;
-	Xdot[8] = (q3 * wx + q0 * wy - q1 * wz) / 2;
-	Xdot[9] = (-q2 * wx + q1 * wy + q0 * wz) / 2;
+	Xdot[6] = (-q1 * wx - q2 * wy - q3 * wz) / 2.0f;
+	Xdot[7] = (q0 * wx - q3 * wy + q2 * wz) / 2.0f;
+	Xdot[8] = (q3 * wx + q0 * wy - q1 * wz) / 2.0f;
+	Xdot[9] = (-q2 * wx + q1 * wy + q0 * wz) / 2.0f;
 
 	// best guess is that bias stays constant
 	Xdot[10] = Xdot[11] = Xdot[12] = 0;
@@ -715,58 +715,58 @@ void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
 	q3 = X[9];
 
 	// Pdot = V
-	F[0][3] = F[1][4] = F[2][5] = 1;
+	F[0][3] = F[1][4] = F[2][5] = 1.0f;
 
 	// dVdot/dq
-	F[3][6] = 2 * (q0 * ax - q3 * ay + q2 * az);
-	F[3][7] = 2 * (q1 * ax + q2 * ay + q3 * az);
-	F[3][8] = 2 * (-q2 * ax + q1 * ay + q0 * az);
-	F[3][9] = 2 * (-q3 * ax - q0 * ay + q1 * az);
-	F[4][6] = 2 * (q3 * ax + q0 * ay - q1 * az);
-	F[4][7] = 2 * (q2 * ax - q1 * ay - q0 * az);
-	F[4][8] = 2 * (q1 * ax + q2 * ay + q3 * az);
-	F[4][9] = 2 * (q0 * ax - q3 * ay + q2 * az);
-	F[5][6] = 2 * (-q2 * ax + q1 * ay + q0 * az);
-	F[5][7] = 2 * (q3 * ax + q0 * ay - q1 * az);
-	F[5][8] = 2 * (-q0 * ax + q3 * ay - q2 * az);
-	F[5][9] = 2 * (q1 * ax + q2 * ay + q3 * az);
+	F[3][6] = 2.0f * (q0 * ax - q3 * ay + q2 * az);
+	F[3][7] = 2.0f * (q1 * ax + q2 * ay + q3 * az);
+	F[3][8] = 2.0f * (-q2 * ax + q1 * ay + q0 * az);
+	F[3][9] = 2.0f * (-q3 * ax - q0 * ay + q1 * az);
+	F[4][6] = 2.0f * (q3 * ax + q0 * ay - q1 * az);
+	F[4][7] = 2.0f * (q2 * ax - q1 * ay - q0 * az);
+	F[4][8] = 2.0f * (q1 * ax + q2 * ay + q3 * az);
+	F[4][9] = 2.0f * (q0 * ax - q3 * ay + q2 * az);
+	F[5][6] = 2.0f * (-q2 * ax + q1 * ay + q0 * az);
+	F[5][7] = 2.0f * (q3 * ax + q0 * ay - q1 * az);
+	F[5][8] = 2.0f * (-q0 * ax + q3 * ay - q2 * az);
+	F[5][9] = 2.0f * (q1 * ax + q2 * ay + q3 * az);
 
 	// dVdot/dabias & dVdot/dna
-	F[3][13]=G[3][3]=-q0*q0-q1*q1+q2*q2+q3*q3; F[3][14]=G[3][4]=2*(-q1*q2+q0*q3);         F[3][15]=G[3][5]=-2*(q1*q3+q0*q2);
-	F[4][13]=G[4][3]=-2*(q1*q2+q0*q3);         F[4][14]=G[4][4]=-q0*q0+q1*q1-q2*q2+q3*q3; F[4][15]=G[4][5]=2*(-q2*q3+q0*q1);
-	F[5][13]=G[5][3]=2*(-q1*q3+q0*q2);         F[5][14]=G[5][4]=-2*(q2*q3+q0*q1);         F[5][15]=G[5][5]=-q0*q0+q1*q1+q2*q2-q3*q3;
+	F[3][13]=G[3][3]=-q0*q0-q1*q1+q2*q2+q3*q3; F[3][14]=G[3][4]=2.0f*(-q1*q2+q0*q3);         F[3][15]=G[3][5]=-2.0f*(q1*q3+q0*q2);
+	F[4][13]=G[4][3]=-2.0f*(q1*q2+q0*q3);         F[4][14]=G[4][4]=-q0*q0+q1*q1-q2*q2+q3*q3; F[4][15]=G[4][5]=2.0f*(-q2*q3+q0*q1);
+	F[5][13]=G[5][3]=2.0f*(-q1*q3+q0*q2);         F[5][14]=G[5][4]=-2.0f*(q2*q3+q0*q1);         F[5][15]=G[5][5]=-q0*q0+q1*q1+q2*q2-q3*q3;
 
 	// dqdot/dq
 	F[6][6] = 0;
-	F[6][7] = -wx / 2;
-	F[6][8] = -wy / 2;
-	F[6][9] = -wz / 2;
-	F[7][6] = wx / 2;
+	F[6][7] = -wx / 2.0f;
+	F[6][8] = -wy / 2.0f;
+	F[6][9] = -wz / 2.0f;
+	F[7][6] = wx / 2.0f;
 	F[7][7] = 0;
-	F[7][8] = wz / 2;
-	F[7][9] = -wy / 2;
-	F[8][6] = wy / 2;
-	F[8][7] = -wz / 2;
+	F[7][8] = wz / 2.0f;
+	F[7][9] = -wy / 2.0f;
+	F[8][6] = wy / 2.0f;
+	F[8][7] = -wz / 2.0f;
 	F[8][8] = 0;
-	F[8][9] = wx / 2;
-	F[9][6] = wz / 2;
-	F[9][7] = wy / 2;
-	F[9][8] = -wx / 2;
+	F[8][9] = wx / 2.0f;
+	F[9][6] = wz / 2.0f;
+	F[9][7] = wy / 2.0f;
+	F[9][8] = -wx / 2.0f;
 	F[9][9] = 0;
 
 	// dqdot/dwbias
-	F[6][10] = q1 / 2;
-	F[6][11] = q2 / 2;
-	F[6][12] = q3 / 2;
-	F[7][10] = -q0 / 2;
-	F[7][11] = q3 / 2;
-	F[7][12] = -q2 / 2;
-	F[8][10] = -q3 / 2;
-	F[8][11] = -q0 / 2;
-	F[8][12] = q1 / 2;
-	F[9][10] = q2 / 2;
-	F[9][11] = -q1 / 2;
-	F[9][12] = -q0 / 2;
+	F[6][10] = q1 / 2.0f;
+	F[6][11] = q2 / 2.0f;
+	F[6][12] = q3 / 2.0f;
+	F[7][10] = -q0 / 2.0f;
+	F[7][11] = q3 / 2.0f;
+	F[7][12] = -q2 / 2.0f;
+	F[8][10] = -q3 / 2.0f;
+	F[8][11] = -q0 / 2.0f;
+	F[8][12] = q1 / 2.0f;
+	F[9][10] = q2 / 2.0f;
+	F[9][11] = -q1 / 2.0f;
+	F[9][12] = -q0 / 2.0f;
 
 	// dVdot/dna  - WITH BIAS STATES ON ACCELS - THIS DONE ABOVE
 	//G[3][3]=-q0*q0-q1*q1+q2*q2+q3*q3; G[3][4]=2*(-q1*q2+q0*q3);         G[3][5]=-2*(q1*q3+q0*q2);
@@ -774,23 +774,23 @@ void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
 	//G[5][3]=2*(-q1*q3+q0*q2);         G[5][4]=-2*(q2*q3+q0*q1);         G[5][5]=-q0*q0+q1*q1+q2*q2-q3*q3;
 
 	// dqdot/dnw
-	G[6][0] = q1 / 2;
-	G[6][1] = q2 / 2;
-	G[6][2] = q3 / 2;
-	G[7][0] = -q0 / 2;
-	G[7][1] = q3 / 2;
-	G[7][2] = -q2 / 2;
-	G[8][0] = -q3 / 2;
-	G[8][1] = -q0 / 2;
-	G[8][2] = q1 / 2;
-	G[9][0] = q2 / 2;
-	G[9][1] = -q1 / 2;
-	G[9][2] = -q0 / 2;
+	G[6][0] = q1 / 2.0f;
+	G[6][1] = q2 / 2.0f;
+	G[6][2] = q3 / 2.0f;
+	G[7][0] = -q0 / 2.0f;
+	G[7][1] = q3 / 2.0f;
+	G[7][2] = -q2 / 2.0f;
+	G[8][0] = -q3 / 2.0f;
+	G[8][1] = -q0 / 2.0f;
+	G[8][2] = q1 / 2.0f;
+	G[9][0] = q2 / 2.0f;
+	G[9][1] = -q1 / 2.0f;
+	G[9][2] = -q0 / 2.0f;
 
 	// dwbias = random walk noise
-	G[10][6] = G[11][7] = G[12][8] = 1;
+	G[10][6] = G[11][7] = G[12][8] = 1.0f;
 	// dabias = random walk noise
-	G[13][9] = G[14][10] = G[15][11] = 1;
+	G[13][9] = G[14][10] = G[15][11] = 1.0f;
 }
 
 void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV])
@@ -813,19 +813,19 @@ void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV])
 	// Bb=Rbe*Be
 	Y[6] =
 	    (q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * Be[0] +
-	    2 * (q1 * q2 + q0 * q3) * Be[1] + 2 * (q1 * q3 -
+	    2.0f * (q1 * q2 + q0 * q3) * Be[1] + 2.0f * (q1 * q3 -
 						   q0 * q2) * Be[2];
 	Y[7] =
-	    2 * (q1 * q2 - q0 * q3) * Be[0] + (q0 * q0 - q1 * q1 +
+	    2.0f * (q1 * q2 - q0 * q3) * Be[0] + (q0 * q0 - q1 * q1 +
 					       q2 * q2 - q3 * q3) * Be[1] +
-	    2 * (q2 * q3 + q0 * q1) * Be[2];
+	    2.0f * (q2 * q3 + q0 * q1) * Be[2];
 	Y[8] =
-	    2 * (q1 * q3 + q0 * q2) * Be[0] + 2 * (q2 * q3 -
+	    2.0f * (q1 * q3 + q0 * q2) * Be[0] + 2.0f * (q2 * q3 -
 						   q0 * q1) * Be[1] +
 	    (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * Be[2];
 
 	// Alt = -Pz
-	Y[9] = -X[2];
+	Y[9] = X[2] * -1.0f;
 }
 
 void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX])
@@ -838,26 +838,26 @@ void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX])
 	q3 = X[9];
 
 	// dP/dP=I;
-	H[0][0] = H[1][1] = H[2][2] = 1;
+	H[0][0] = H[1][1] = H[2][2] = 1.0f;
 	// dV/dV=I;
-	H[3][3] = H[4][4] = H[5][5] = 1;
+	H[3][3] = H[4][4] = H[5][5] = 1.0f;
 
 	// dBb/dq
-	H[6][6] = 2 * (q0 * Be[0] + q3 * Be[1] - q2 * Be[2]);
-	H[6][7] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
-	H[6][8] = 2 * (-q2 * Be[0] + q1 * Be[1] - q0 * Be[2]);
-	H[6][9] = 2 * (-q3 * Be[0] + q0 * Be[1] + q1 * Be[2]);
-	H[7][6] = 2 * (-q3 * Be[0] + q0 * Be[1] + q1 * Be[2]);
-	H[7][7] = 2 * (q2 * Be[0] - q1 * Be[1] + q0 * Be[2]);
-	H[7][8] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
-	H[7][9] = 2 * (-q0 * Be[0] - q3 * Be[1] + q2 * Be[2]);
-	H[8][6] = 2 * (q2 * Be[0] - q1 * Be[1] + q0 * Be[2]);
-	H[8][7] = 2 * (q3 * Be[0] - q0 * Be[1] - q1 * Be[2]);
-	H[8][8] = 2 * (q0 * Be[0] + q3 * Be[1] - q2 * Be[2]);
-	H[8][9] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
+	H[6][6] = 2.0f * (q0 * Be[0] + q3 * Be[1] - q2 * Be[2]);
+	H[6][7] = 2.0f * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
+	H[6][8] = 2.0f * (-q2 * Be[0] + q1 * Be[1] - q0 * Be[2]);
+	H[6][9] = 2.0f * (-q3 * Be[0] + q0 * Be[1] + q1 * Be[2]);
+	H[7][6] = 2.0f * (-q3 * Be[0] + q0 * Be[1] + q1 * Be[2]);
+	H[7][7] = 2.0f * (q2 * Be[0] - q1 * Be[1] + q0 * Be[2]);
+	H[7][8] = 2.0f * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
+	H[7][9] = 2.0f * (-q0 * Be[0] - q3 * Be[1] + q2 * Be[2]);
+	H[8][6] = 2.0f * (q2 * Be[0] - q1 * Be[1] + q0 * Be[2]);
+	H[8][7] = 2.0f * (q3 * Be[0] - q0 * Be[1] - q1 * Be[2]);
+	H[8][8] = 2.0f * (q0 * Be[0] + q3 * Be[1] - q2 * Be[2]);
+	H[8][9] = 2.0f * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
 
 	// dAlt/dPz = -1
-	H[9][2] = -1;
+	H[9][2] = -1.0f;
 }
 
 /**

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/paths.c

@@ -0,0 +1,226 @@
+/**
+ ******************************************************************************
+ *
+ * @file       paths.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @brief      Library path manipulation 
+ *
+ * @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 "paths.h"
+
+#include "uavobjectmanager.h" // <--.
+#include "pathdesired.h" //<-- needed only for correct ENUM macro usage with path modes (PATHDESIRED_MODE_xxx,
+// no direct UAVObject usage allowed in this file
+
+// private functions
+static void path_endpoint( float * start_point, float * end_point, float * cur_point, struct path_status * status);
+static void path_vector( float * start_point, float * end_point, float * cur_point, struct path_status * status);
+static void path_circle(float * start_point, float * end_point, float * cur_point, struct path_status * status, bool clockwise);
+
+/**
+ * @brief Compute progress along path and deviation from it
+ * @param[in] start_point Starting point
+ * @param[in] end_point Ending point
+ * @param[in] cur_point Current location
+ * @param[in] mode Path following mode
+ * @param[out] status Structure containing progress along path and deviation
+ */
+void path_progress(float * start_point, float * end_point, float * cur_point, struct path_status * status, uint8_t mode)
+{
+	switch(mode) {
+		case PATHDESIRED_MODE_FLYVECTOR:
+		case PATHDESIRED_MODE_DRIVEVECTOR:
+			return path_vector(start_point, end_point, cur_point, status);
+			break;
+		case PATHDESIRED_MODE_FLYCIRCLERIGHT:
+		case PATHDESIRED_MODE_DRIVECIRCLERIGHT:
+			return path_circle(start_point, end_point, cur_point, status, 1);
+			break;
+		case PATHDESIRED_MODE_FLYCIRCLELEFT:
+		case PATHDESIRED_MODE_DRIVECIRCLELEFT:
+			return path_circle(start_point, end_point, cur_point, status, 0);
+			break;
+		case PATHDESIRED_MODE_FLYENDPOINT:
+		case PATHDESIRED_MODE_DRIVEENDPOINT:
+		default:
+			// use the endpoint as default failsafe if called in unknown modes
+			return path_endpoint(start_point, end_point, cur_point, status);
+			break;
+	}
+}
+
+/**
+ * @brief Compute progress towards endpoint. Deviation equals distance
+ * @param[in] start_point Starting point
+ * @param[in] end_point Ending point
+ * @param[in] cur_point Current location
+ * @param[out] status Structure containing progress along path and deviation
+ */
+static void path_endpoint( float * start_point, float * end_point, float * cur_point, struct path_status * status)
+{
+	float path_north, path_east, diff_north, diff_east;
+	float dist_path, dist_diff;
+
+	// we do not correct in this mode
+	status->correction_direction[0] = status->correction_direction[1] = 0;
+
+	// Distance to go
+	path_north = end_point[0] - start_point[0];
+	path_east = end_point[1] - start_point[1];
+
+	// Current progress location relative to end
+	diff_north = end_point[0] - cur_point[0];
+	diff_east = end_point[1] - cur_point[1];
+
+	dist_diff = sqrtf( diff_north * diff_north + diff_east * diff_east );
+	dist_path = sqrtf( path_north * path_north + path_east * path_east );
+
+	if(dist_diff < 1e-6 ) {
+		status->fractional_progress = 1;
+		status->error = 0;
+		status->path_direction[0] = status->path_direction[1] = 0;
+		return;
+	}
+
+	status->fractional_progress = 1 - dist_diff / (1 + dist_path);
+	status->error = dist_diff;
+
+	// Compute direction to travel
+	status->path_direction[0] = diff_north / dist_diff;
+	status->path_direction[1] = diff_east / dist_diff;
+
+}
+
+/**
+ * @brief Compute progress along path and deviation from it
+ * @param[in] start_point Starting point
+ * @param[in] end_point Ending point
+ * @param[in] cur_point Current location
+ * @param[out] status Structure containing progress along path and deviation
+ */
+static void path_vector( float * start_point, float * end_point, float * cur_point, struct path_status * status)
+{
+	float path_north, path_east, diff_north, diff_east;
+	float dist_path;
+	float dot;
+	float normal[2];
+
+	// Distance to go
+	path_north = end_point[0] - start_point[0];
+	path_east = end_point[1] - start_point[1];
+
+	// Current progress location relative to start
+	diff_north = cur_point[0] - start_point[0];
+	diff_east = cur_point[1] - start_point[1];
+
+	dot = path_north * diff_north + path_east * diff_east;
+	dist_path = sqrtf( path_north * path_north + path_east * path_east );
+
+	if(dist_path < 1e-6) {
+		// if the path is too short, we cannot determine vector direction.
+		// Fly towards the endpoint to prevent flying away,
+		// but assume progress=1 either way.
+		path_endpoint( start_point, end_point, cur_point, status );
+		status->fractional_progress = 1;
+		return;
+	}
+
+	// Compute the normal to the path
+	normal[0] = -path_east / dist_path;
+	normal[1] = path_north / dist_path;
+
+	status->fractional_progress = dot / (dist_path * dist_path);
+	status->error = normal[0] * diff_north + normal[1] * diff_east;
+
+	// Compute direction to correct error
+	status->correction_direction[0] = (status->error > 0) ? -normal[0] : normal[0];
+	status->correction_direction[1] = (status->error > 0) ? -normal[1] : normal[1];
+	
+	// Now just want magnitude of error
+	status->error = fabs(status->error);
+
+	// Compute direction to travel
+	status->path_direction[0] = path_north / dist_path;
+	status->path_direction[1] = path_east / dist_path;
+
+}
+
+/**
+ * @brief Compute progress along circular path and deviation from it
+ * @param[in] start_point Starting point
+ * @param[in] end_point Center point
+ * @param[in] cur_point Current location
+ * @param[out] status Structure containing progress along path and deviation
+ */
+static void path_circle(float * start_point, float * end_point, float * cur_point, struct path_status * status, bool clockwise)
+{
+	float radius_north, radius_east, diff_north, diff_east;
+	float radius,cradius;
+	float normal[2];
+
+	// Radius
+	radius_north = end_point[0] - start_point[0];
+	radius_east = end_point[1] - start_point[1];
+
+	// Current location relative to center
+	diff_north = cur_point[0] - end_point[0];
+	diff_east = cur_point[1] - end_point[1];
+
+	radius = sqrtf( radius_north * radius_north + radius_east * radius_east );
+	cradius = sqrtf(  diff_north * diff_north   +   diff_east * diff_east );
+
+	if (cradius < 1e-6) {
+		// cradius is zero, just fly somewhere and make sure correction is still a normal
+		status->fractional_progress = 1;
+		status->error = radius;
+		status->correction_direction[0] = 0;
+		status->correction_direction[1] = 1;
+		status->path_direction[0] = 1;
+		status->path_direction[1] = 0;
+		return;
+	}
+
+	if (clockwise) {
+		// Compute the normal to the radius clockwise
+		normal[0] = -diff_east / cradius;
+		normal[1] = diff_north / cradius;
+	} else {
+		// Compute the normal to the radius counter clockwise
+		normal[0] = diff_east / cradius;
+		normal[1] = -diff_north / cradius;
+	}
+	
+	status->fractional_progress = (clockwise?1:-1) * atan2f( diff_north, diff_east) - atan2f( radius_north, radius_east);
+
+	// error is current radius minus wanted radius - positive if too close
+	status->error = radius - cradius;
+
+	// Compute direction to correct error
+	status->correction_direction[0] = (status->error>0?1:-1) * diff_north / cradius;
+	status->correction_direction[1] = (status->error>0?1:-1) * diff_east / cradius;
+
+	// Compute direction to travel
+	status->path_direction[0] = normal[0];
+	status->path_direction[1] = normal[1];
+
+	status->error = fabs(status->error);
+}

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/Libraries/taskmonitor.c

@@ -125,10 +125,10 @@ void TaskMonitorUpdateAll(void)
 			data.StackRemaining[n] = 10000;
 #else
 			data.StackRemaining[n] = uxTaskGetStackHighWaterMark(handles[n]) * 4;
+#endif
 #if ( configGENERATE_RUN_TIME_STATS == 1 )
 			/* Generate run time stats */
 			data.RunningTime[n] = uxTaskGetRunTime(handles[n]) / deltaTime;
-#endif
 #endif
 			
 		}

flight/Modules/Actuator/actuator.c

@@ -569,6 +569,95 @@ static void setFailsafe(const ActuatorSettingsData * actuatorSettings, const Mix
 	ActuatorCommandChannelSet(Channel);
 }
 
+/**
+ * determine buzzer or blink sequence
+ **/
+
+typedef enum {BUZZ_BUZZER=0,BUZZ_ARMING=1,BUZZ_INFO=2,BUZZ_MAX=3} buzzertype;
+
+static inline bool buzzerState(buzzertype type)
+{
+	// This is for buzzers that take a PWM input
+
+	static uint32_t tune[BUZZ_MAX]={0};
+	static uint32_t tunestate[BUZZ_MAX]={0};
+
+
+	uint32_t newTune = 0;
+	if(type==BUZZ_BUZZER)
+	{
+		// 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;
+		}
+
+	} else { // BUZZ_ARMING || BUZZ_INFO
+		uint8_t arming;
+		FlightStatusArmedGet(&arming);
+		//base idle tune  
+		newTune =  0x80000000;	  // 0b1000...
+		
+		// Merge the error pattern for InfoLed
+		if(type==BUZZ_INFO)
+		{
+			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 (newTune != tune[type]) {
+		tune[type] = newTune;
+		// resynchronize all tunes on change, so they stay in sync
+		for (int i=0;i<BUZZ_MAX;i++) {
+			tunestate[i] = tune[i];
+		}
+	}
+
+	// Play tune
+	bool buzzOn = false;
+	static portTickType lastSysTime = 0;
+	portTickType thisSysTime = xTaskGetTickCount();
+	portTickType dT = 0;
+
+	// For now, only look at the battery alarm, because functions like AlarmsHasCritical() can block for some time; to be discussed
+	if (tune[type]) {
+		if(thisSysTime > lastSysTime) {
+			dT = thisSysTime - lastSysTime;
+		} else {
+			lastSysTime = 0; // avoid the case where SysTimeMax-lastSysTime <80
+		}
+
+		buzzOn = (tunestate[type]&1);
+
+		if (dT > 80) {
+			// Go to next bit in alarm_seq_state
+			for (int i=0;i<BUZZ_MAX;i++) {
+				tunestate[i] >>=1;
+				if (tunestate[i]==0) // All done, re-start the tune
+					tunestate[i]=tune[i];
+			}
+			lastSysTime = thisSysTime;
+		}
+	}
+	return buzzOn;
+}
+
+
 #if defined(ARCH_POSIX) || defined(ARCH_WIN32)
 static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSettingsData * actuatorSettings)
 {
@@ -578,52 +667,18 @@ 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: {
-			// 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;
-			}
-
-			// Do we need to change tune?
-			if (bewBuzzTune != currBuzzTune) {
-				currBuzzTune = bewBuzzTune;
-				currBuzzTuneState = currBuzzTune;
-			}
-
-
-			// Play tune
-			bool buzzOn = false;
-			static portTickType lastSysTime = 0;
-			portTickType thisSysTime = xTaskGetTickCount();
-			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;
-				}
-			}
+		case ACTUATORSETTINGS_CHANNELTYPE_PWMALARMBUZZER: 
 			PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel],
-							buzzOn?actuatorSettings->ChannelMax[mixer_channel]:actuatorSettings->ChannelMin[mixer_channel]);
+			              buzzerState(BUZZ_BUZZER)?actuatorSettings->ChannelMax[mixer_channel]:actuatorSettings->ChannelMin[mixer_channel]);
+			return true;
+		case ACTUATORSETTINGS_CHANNELTYPE_ARMINGLED:
+			PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel],
+			              buzzerState(BUZZ_ARMING)?actuatorSettings->ChannelMax[mixer_channel]:actuatorSettings->ChannelMin[mixer_channel]);
+			return true;
+		case ACTUATORSETTINGS_CHANNELTYPE_INFOLED:
+			PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel],
+			              buzzerState(BUZZ_INFO)?actuatorSettings->ChannelMax[mixer_channel]:actuatorSettings->ChannelMin[mixer_channel]);
 			return true;
-		}
 		case ACTUATORSETTINGS_CHANNELTYPE_PWM:
 			PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], value);
 			return true;
@@ -632,7 +687,6 @@ static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSet
 			return PIOS_SetMKSpeed(actuatorSettings->ChannelAddr[mixer_channel],value);
 		case ACTUATORSETTINGS_CHANNELTYPE_ASTEC4:
 			return PIOS_SetAstec4Speed(actuatorSettings->ChannelAddr[mixer_channel],value);
-			break;
 #endif
 		default:
 			return false;

flight/Modules/Airspeed/revolution/airspeed.c

@@ -0,0 +1,190 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{ 
+ * @addtogroup AirspeedModule Airspeed Module
+ * @brief Calculate airspeed from diverse sources and update @ref Airspeed "Airspeed UAV Object"
+ * @{ 
+ *
+ * @file       airspeed.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @brief      Airspeed module
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+/**
+ * Output object: AirspeedSensor
+ *
+ * This module will periodically update the value of the AirspeedSensor object.
+ *
+ */
+
+#include "openpilot.h"
+#include "hwsettings.h"
+#include "airspeedsettings.h"
+#include "airspeedsensor.h" // object that will be updated by the module
+#include "baro_airspeed_etasv3.h"
+#include "baro_airspeed_mpxv.h"
+
+// Private constants
+
+#define STACK_SIZE_BYTES 500
+
+
+#define TASK_PRIORITY (tskIDLE_PRIORITY+1)
+
+// Private types
+
+// Private variables
+static xTaskHandle taskHandle;
+static bool airspeedEnabled = false;
+static AirspeedSettingsData airspeedSettings;
+
+static int8_t airspeedADCPin=-1;
+
+
+// Private functions
+static void airspeedTask(void *parameters);
+static void AirspeedSettingsUpdatedCb(UAVObjEvent * ev);
+
+
+/**
+ * Initialise the module, called on startup
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t AirspeedStart()
+{	
+	
+	//Check if module is enabled or not
+	if (airspeedEnabled == false) {
+		return -1;
+	}
+		
+	// Start main task
+	xTaskCreate(airspeedTask, (signed char *)"Airspeed", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
+	TaskMonitorAdd(TASKINFO_RUNNING_AIRSPEED, taskHandle);
+	return 0;
+}
+
+/**
+ * Initialise the module, called on startup
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t AirspeedInitialize()
+{
+#ifdef MODULE_AIRSPEED_BUILTIN
+	airspeedEnabled = true;
+#else
+	
+	HwSettingsInitialize();
+	uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
+	HwSettingsOptionalModulesGet(optionalModules);
+	
+	
+	if (optionalModules[HWSETTINGS_OPTIONALMODULES_AIRSPEED] == HWSETTINGS_OPTIONALMODULES_ENABLED) {
+		airspeedEnabled = true;
+	} else {
+		airspeedEnabled = false;
+		return -1;
+	}
+#endif
+	
+	uint8_t adcRouting[HWSETTINGS_ADCROUTING_NUMELEM];	
+	HwSettingsADCRoutingGet(adcRouting);
+	
+	//Determine if the barometric airspeed sensor is routed to an ADC pin 
+	for (int i=0; i < HWSETTINGS_ADCROUTING_NUMELEM; i++) {
+		if (adcRouting[i] == HWSETTINGS_ADCROUTING_ANALOGAIRSPEED) {
+			airspeedADCPin = i;
+		}
+	}
+	
+	AirspeedSensorInitialize();
+	AirspeedSettingsInitialize();
+	
+	AirspeedSettingsConnectCallback(AirspeedSettingsUpdatedCb);	
+	
+	return 0;
+}
+MODULE_INITCALL(AirspeedInitialize, AirspeedStart)
+
+
+/**
+ * Module thread, should not return.
+ */
+static void airspeedTask(void *parameters)
+{
+	AirspeedSettingsUpdatedCb(AirspeedSettingsHandle());
+	
+	AirspeedSensorData airspeedData;
+	AirspeedSensorGet(&airspeedData);
+
+	AirspeedSettingsUpdatedCb(NULL);
+
+
+	airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
+	
+	// Main task loop
+	portTickType lastSysTime = xTaskGetTickCount();
+	while (1)
+	{
+		vTaskDelayUntil(&lastSysTime, airspeedSettings.SamplePeriod / portTICK_RATE_MS);
+		
+		// Update the airspeed object
+		AirspeedSensorGet(&airspeedData);
+
+		switch (airspeedSettings.AirspeedSensorType) {
+#if defined(PIOS_INCLUDE_MPXV)
+			case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002:
+			case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004:
+				//MPXV5004 and MPXV7002 sensors
+				baro_airspeedGetMPXV(&airspeedData, &airspeedSettings,airspeedADCPin);
+				break;
+#endif
+#if defined(PIOS_INCLUDE_ETASV3)
+			case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_EAGLETREEAIRSPEEDV3:
+				//Eagletree Airspeed v3
+				baro_airspeedGetETASV3(&airspeedData, &airspeedSettings);
+				break;
+#endif
+			default:
+				airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
+		}
+
+		//Set the UAVO
+		AirspeedSensorSet(&airspeedData);
+			
+	}
+}
+
+	
+
+
+static void AirspeedSettingsUpdatedCb(UAVObjEvent * ev)
+{
+	AirspeedSettingsGet(&airspeedSettings);
+	
+}
+	
+	
+/**
+ * @}
+ * @}
+ */

flight/Modules/Airspeed/revolution/baro_airspeed_etasv3.c

@@ -0,0 +1,101 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{ 
+ * @addtogroup AirspeedModule Airspeed Module
+ * @brief Communicate with airspeed sensors and return values
+ * @{ 
+ *
+ * @file       baro_airspeed.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Airspeed module, handles temperature and pressure readings from BMP085
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+/**
+ * Output object: BaroAirspeed
+ *
+ * This module will periodically update the value of the BaroAirspeed object.
+ *
+ */
+
+#include "openpilot.h"
+#include "hwsettings.h"
+#include "airspeedsettings.h"
+#include "airspeedsensor.h"	// object that will be updated by the module
+
+#if defined(PIOS_INCLUDE_ETASV3)
+
+#define CALIBRATION_IDLE_MS                     2000   //Time to wait before calibrating, in [ms]
+#define CALIBRATION_COUNT_MS                    2000   //Time to spend calibrating, in [ms]
+
+// Private types
+
+// Private variables
+
+// Private functions
+
+static uint16_t calibrationCount=0;
+static uint16_t calibrationCount2=0;
+static uint32_t calibrationSum = 0;
+
+
+void baro_airspeedGetETASV3(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings){
+
+	//Check to see if airspeed sensor is returning airspeedSensor
+	airspeedSensor->SensorValue = PIOS_ETASV3_ReadAirspeed();
+	if (airspeedSensor->SensorValue==-1) {
+		airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
+		airspeedSensor->CalibratedAirspeed = 0;
+		return;
+	}
+	
+	// only calibrate if no stored calibration is available
+	if (!airspeedSettings->ZeroPoint) {
+		//Calibrate sensor by averaging zero point value
+		if (calibrationCount <= CALIBRATION_IDLE_MS/airspeedSettings->SamplePeriod) {
+			calibrationCount++;
+			calibrationCount2++;
+			return;
+		} else if (calibrationCount <= (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/airspeedSettings->SamplePeriod) {
+			calibrationCount++;
+			calibrationCount2++;
+			calibrationSum +=  airspeedSensor->SensorValue;
+			if (calibrationCount > (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/airspeedSettings->SamplePeriod) {
+
+				airspeedSettings->ZeroPoint = (int16_t) (((float)calibrationSum) / calibrationCount2);
+				AirspeedSettingsZeroPointSet( &airspeedSettings->ZeroPoint );
+			}
+			return;
+		}
+	}
+	
+	//Compute airspeed
+	airspeedSensor->CalibratedAirspeed = airspeedSettings->Scale * sqrtf((float)abs(airspeedSensor->SensorValue - airspeedSettings->ZeroPoint));
+	airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
+}
+
+
+#endif
+
+/**
+ * @}
+ * @}
+ */

flight/Modules/Airspeed/revolution/baro_airspeed_mpxv.c

@@ -0,0 +1,118 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{ 
+ * @addtogroup AirspeedModule Airspeed Module
+ * @brief Communicate with airspeed sensors and return values
+ * @{ 
+ *
+ * @file       baro_airspeed.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Airspeed module, handles temperature and pressure readings from BMP085
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+/**
+ * Output object: BaroAirspeed
+ *
+ * This module will periodically update the value of the BaroAirspeed object.
+ *
+ */
+
+#include "openpilot.h"
+#include "hwsettings.h"
+#include "airspeedsettings.h"
+#include "airspeedsensor.h"	// object that will be updated by the module
+
+#if defined(PIOS_INCLUDE_MPXV)
+
+#define CALIBRATION_IDLE_MS                     2000   //Time to wait before calibrating, in [ms]
+#define CALIBRATION_COUNT_MS                    2000   //Time to spend calibrating, in [ms]
+#define ANALOG_BARO_AIRSPEED_TIME_CONSTANT_MS    100   //low pass filter
+
+// Private types
+
+// Private variables
+
+// Private functions
+
+static uint16_t calibrationCount=0;
+
+PIOS_MPXV_descriptor sensor = { .type=PIOS_MPXV_UNKNOWN };
+
+
+void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings, int8_t airspeedADCPin){
+
+	//Ensure that the ADC pin is properly configured
+	if(airspeedADCPin <0){
+		airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
+		
+		return;
+	}
+	if (sensor.type==PIOS_MPXV_UNKNOWN) {
+		switch (airspeedSettings->AirspeedSensorType) {
+			case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002:
+				sensor = PIOS_MPXV_7002_DESC(airspeedADCPin);
+				break;
+			case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004:
+				sensor = PIOS_MPXV_5004_DESC(airspeedADCPin);
+				break;
+			default:
+				airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
+				return;
+		}
+	}
+
+	airspeedSensor->SensorValue = PIOS_MPXV_Measure(&sensor);
+
+	if (!airspeedSettings->ZeroPoint) {
+		//Calibrate sensor by averaging zero point value
+		if (calibrationCount < CALIBRATION_IDLE_MS/airspeedSettings->SamplePeriod) { //First let sensor warm up and stabilize.
+			calibrationCount++;
+			return;
+		} else if (calibrationCount <= (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/airspeedSettings->SamplePeriod) { //Then compute the average.
+			calibrationCount++; /*DO NOT MOVE FROM BEFORE sensorCalibration=... LINE, OR ELSE WILL HAVE DIVIDE BY ZERO */
+
+			PIOS_MPXV_Calibrate(&sensor,airspeedSensor->SensorValue);
+
+			//Set settings UAVO. The airspeed UAVO is set elsewhere in the function.
+			if (calibrationCount > (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/airspeedSettings->SamplePeriod) {
+				airspeedSettings->ZeroPoint = sensor.zeroPoint;
+				AirspeedSettingsZeroPointSet(&airspeedSettings->ZeroPoint);
+			}
+			return;
+		}
+	}
+	sensor.zeroPoint = airspeedSettings->ZeroPoint;
+		
+	//Filter CAS
+	float alpha=airspeedSettings->SamplePeriod/(airspeedSettings->SamplePeriod + ANALOG_BARO_AIRSPEED_TIME_CONSTANT_MS); //Low pass filter.
+	
+	airspeedSensor->CalibratedAirspeed = PIOS_MPXV_CalcAirspeed(&sensor,airspeedSensor->SensorValue) * (alpha) + airspeedSensor->CalibratedAirspeed*(1.0f-alpha);
+	airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
+
+}
+
+#endif
+
+/**
+ * @}
+ * @}
+ */

flight/Modules/Airspeed/revolution/gps_airspeed.c

@@ -0,0 +1,154 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{ 
+ * @addtogroup AirspeedModule Airspeed Module
+ * @brief Use GPS data to estimate airspeed
+ * @{ 
+ *
+ * @file       gps_airspeed.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @brief      Airspeed module, handles temperature and pressure readings from BMP085
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+
+#include "openpilot.h"
+#include "gps_airspeed.h"
+#include "gpsvelocity.h"
+#include "attitudeactual.h"
+#include "CoordinateConversions.h"
+
+
+
+// Private constants
+#define GPS_AIRSPEED_BIAS_KP           0.1f   //Needs to be settable in a UAVO
+#define GPS_AIRSPEED_BIAS_KI           0.1f   //Needs to be settable in a UAVO
+#define SAMPLING_DELAY_MS_GPS          100    //Needs to be settable in a UAVO
+#define GPS_AIRSPEED_TIME_CONSTANT_MS  500.0f //Needs to be settable in a UAVO
+
+#define F_PI 3.141526535897932f
+#define DEG2RAD (F_PI/180.0f)
+
+// Private types
+struct GPSGlobals {
+	float RbeCol1_old[3];
+	float gpsVelOld_N;
+	float gpsVelOld_E;
+	float gpsVelOld_D;
+};
+
+// Private variables
+static struct GPSGlobals *gps;
+
+// Private functions
+
+/*
+ * Initialize function loads first data sets, and allocates memory for structure.
+ */
+void gps_airspeedInitialize()
+{
+	//This method saves memory in case we don't use the GPS module.
+	gps=(struct GPSGlobals *)pvPortMalloc(sizeof(struct GPSGlobals));
+	
+	//GPS airspeed calculation variables
+	GPSVelocityData gpsVelData;
+	GPSVelocityGet(&gpsVelData);
+	
+	gps->gpsVelOld_N=gpsVelData.North;
+	gps->gpsVelOld_E=gpsVelData.East;
+	gps->gpsVelOld_D=gpsVelData.Down;
+	
+	AttitudeActualData attData;
+	AttitudeActualGet(&attData);
+	
+	float Rbe[3][3];
+	float q[4] ={attData.q1, attData.q2, attData.q3, attData.q4};
+	
+	//Calculate rotation matrix
+	Quaternion2R(q, Rbe);
+	
+	gps->RbeCol1_old[0]=Rbe[0][0];
+	gps->RbeCol1_old[1]=Rbe[0][1];
+	gps->RbeCol1_old[2]=Rbe[0][2];
+}
+
+/*
+ * Calculate airspeed as a function of GPS groundspeed and vehicle attitude.
+ *  From "IMU Wind Estimation (Theory)", by William Premerlani.
+ *  The idea is that V_gps=V_air+V_wind. If we assume wind constant, => 
+ *  V_gps_2-V_gps_1 = (V_air_2+V_wind_2) -(V_air_1+V_wind_1) = V_air_2 - V_air_1.
+ *  If we assume airspeed constant, => V_gps_2-V_gps_1 = |V|*(f_2 - f1),
+ *  where "f" is the fuselage vector in earth coordinates.
+ *  We then solve for |V| = |V_gps_2-V_gps_1|/ |f_2 - f1|.
+ */
+void gps_airspeedGet(float *v_air_GPS)
+{	
+	float Rbe[3][3];
+	
+	{ //Scoping to save memory. We really just need Rbe.
+		AttitudeActualData attData;
+		AttitudeActualGet(&attData);
+		
+		float q[4] ={attData.q1, attData.q2, attData.q3, attData.q4};
+		
+		//Calculate rotation matrix
+		Quaternion2R(q, Rbe);
+	}
+	
+	//Calculate the cos(angle) between the two fuselage basis vectors
+	float cosDiff=(Rbe[0][0]*gps->RbeCol1_old[0]) + (Rbe[0][1]*gps->RbeCol1_old[1]) + (Rbe[0][2]*gps->RbeCol1_old[2]);
+	
+	//If there's more than a 5 degree difference between two fuselage measurements, then we have sufficient delta to continue.
+	if (fabs(cosDiff) < cos(5.0f*DEG2RAD)) {
+		GPSVelocityData gpsVelData;
+		GPSVelocityGet(&gpsVelData);
+		
+		//Calculate the norm^2 of the difference between the two GPS vectors
+		float normDiffGPS2 = powf(gpsVelData.North-gps->gpsVelOld_N,2.0f) + powf(gpsVelData.East-gps->gpsVelOld_E,2.0f) + powf(gpsVelData.Down-gps->gpsVelOld_D,2.0f);
+
+		//Calculate the norm^2 of the difference between the two fuselage vectors
+		float normDiffAttitude2=powf(Rbe[0][0]-gps->RbeCol1_old[0],2.0f) + powf(Rbe[0][1]-gps->RbeCol1_old[1],2.0f) + powf(Rbe[0][2]-gps->RbeCol1_old[2],2.0f);
+		
+		//Airspeed magnitude is the ratio between the two difference norms
+		*v_air_GPS = sqrtf(normDiffGPS2/normDiffAttitude2);
+		
+		//Save old variables for next pass
+		gps->gpsVelOld_N=gpsVelData.North;
+		gps->gpsVelOld_E=gpsVelData.East;
+		gps->gpsVelOld_D=gpsVelData.Down;
+		
+		gps->RbeCol1_old[0]=Rbe[0][0];
+		gps->RbeCol1_old[1]=Rbe[0][1];
+		gps->RbeCol1_old[2]=Rbe[0][2];
+	}
+	else {
+		
+	}
+
+	
+}
+
+
+
+/**
+ * @}
+ * @}
+ */

flight/Modules/Airspeed/revolution/inc/airspeed.h

@@ -0,0 +1,41 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{ 
+ * @addtogroup AirspeedModule Airspeed Module
+ * @brief Communicate with EagleTree Airspeed Sensor and update @ref BaroAirspeed "BaroAirspeed UAV Object"
+ * @{ 
+ *
+ * @file       airspeed.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Airspeed module, reads temperature and pressure from BMP085
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+#ifndef AIRSPEED_H
+#define AIRSPEED_H
+
+int32_t AirspeedInitialize();
+
+#endif // AIRSPEED_H
+
+/**
+ * @}
+ * @}
+ */

flight/Modules/Airspeed/revolution/inc/baro_airspeed_etasv3.h

@@ -0,0 +1,43 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{ 
+ * @addtogroup AirspeedModule Airspeed Module
+ * @brief Calculate airspeed as a function of the difference between sequential GPS velocity and attitude measurements
+ * @{ 
+ *
+ * @file       baro_airspeed_etasv3.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @brief      Airspeed module, reads temperature and pressure from BMP085
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+#ifndef BARO_AIRSPEED_ETASV3_H
+#define BARO_AIRSPEED_ETASV3_H
+#if defined(PIOS_INCLUDE_ETASV3)
+
+void baro_airspeedGetETASV3(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings);
+
+#endif
+#endif // BARO_AIRSPEED_ETASV3_H
+
+/**
+ * @}
+ * @}
+ */

flight/Modules/Airspeed/revolution/inc/baro_airspeed_mpxv.h

@@ -0,0 +1,43 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{ 
+ * @addtogroup AirspeedModule Airspeed Module
+ * @brief Calculate airspeed as a function of the difference between sequential GPS velocity and attitude measurements
+ * @{ 
+ *
+ * @file       baro_airspeed_mpxv.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @brief      Airspeed module, reads temperature and pressure from BMP085
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+#ifndef BARO_AIRSPEED_MPXV_H
+#define BARO_AIRSPEED_MPXV_H
+#if defined(PIOS_INCLUDE_MPXV)
+
+void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings, int8_t airspeedADCPin);
+
+#endif
+#endif // BARO_AIRSPEED_MPXV_H
+
+/**
+ * @}
+ * @}
+ */

flight/Modules/Airspeed/revolution/inc/gps_airspeed.h

@@ -0,0 +1,42 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{ 
+ * @addtogroup AirspeedModule Airspeed Module
+ * @brief Calculate airspeed as a function of the difference between sequential GPS velocity and attitude measurements
+ * @{ 
+ *
+ * @file       gps_airspeed.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @brief      Airspeed module, reads temperature and pressure from BMP085
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+#ifndef GPS_AIRSPEED_H
+#define GPS_AIRSPEED_H
+
+void gps_airspeedInitialize();
+void gps_airspeedGet(float *v_air_GPS);
+
+#endif // GPS_AIRSPEED_H
+
+/**
+ * @}
+ * @}
+ */

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/AltitudeHold/altitudehold.c

@@ -60,6 +60,7 @@
 #define MAX_QUEUE_SIZE 2
 #define STACK_SIZE_BYTES 1024
 #define TASK_PRIORITY (tskIDLE_PRIORITY+1)
+#define ACCEL_DOWNSAMPLE 4
 // Private types
 
 // Private variables
@@ -178,12 +179,11 @@ static void altitudeHoldTask(void *parameters)
 			float P[4][4], K[4][2], x[2];
 			float G[4] = {1.0e-15f, 1.0e-15f, 1.0e-3f, 1.0e-7};
 			static float V[4][4] = {{10.0f, 0, 0, 0}, {0, 100.0f, 0, 0}, {0, 0, 100.0f, 0}, {0, 0, 0, 1000.0f}};
-
+			static uint32_t accel_downsample_count = 0;
+			static float accels_accum[3] = {0,0,0};
 			float dT;
 			static float S[2] = {1.0f,10.0f};
 
-			thisTime = xTaskGetTickCount();
-
 			/* Somehow this always assigns to zero.  Compiler bug? Race condition? */
 			S[0] = altitudeHoldSettings.PressureNoise;
 			S[1] = altitudeHoldSettings.AccelNoise;
@@ -196,6 +196,23 @@ static void altitudeHoldTask(void *parameters)
 			BaroAltitudeData baro;
 			BaroAltitudeGet(&baro);
 
+			/* Downsample accels to stop this calculation consuming too much CPU */
+			accels_accum[0] += accels.x;
+			accels_accum[1] += accels.y;
+			accels_accum[2] += accels.z;
+			accel_downsample_count++;
+
+			if (accel_downsample_count < ACCEL_DOWNSAMPLE)
+				continue;
+
+			accel_downsample_count = 0;
+			accels.x = accels_accum[0] / ACCEL_DOWNSAMPLE;
+			accels.y = accels_accum[1] / ACCEL_DOWNSAMPLE;
+			accels.z = accels_accum[2] / ACCEL_DOWNSAMPLE;
+			accels_accum[0] = accels_accum[1] = accels_accum[2] = 0;
+
+			thisTime = xTaskGetTickCount();
+
 			if (init == WAITIING_INIT) {
 				z[0] = baro.Altitude;
 				z[1] = 0;

flight/Modules/Attitude/attitude.c

@@ -387,13 +387,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

@@ -49,28 +49,46 @@
  */
 
 #include "pios.h"
+
 #include "attitude.h"
-#include "magnetometer.h"
+
 #include "accels.h"
-#include "gyros.h"
-#include "gyrosbias.h"
+#include "airspeedsensor.h"
+#include "airspeedactual.h"
 #include "attitudeactual.h"
 #include "attitudesettings.h"
-#include "positionactual.h"
-#include "velocityactual.h"
-#include "gpsposition.h"
 #include "baroaltitude.h"
 #include "flightstatus.h"
+#include "gpsposition.h"
+#include "gpsvelocity.h"
+#include "gyros.h"
+#include "gyrosbias.h"
 #include "homelocation.h"
+#include "magnetometer.h"
+#include "positionactual.h"
+#include "revocalibration.h"
+#include "revosettings.h"
+#include "velocityactual.h"
 #include "CoordinateConversions.h"
 
 // Private constants
-#define STACK_SIZE_BYTES 5540
+#define STACK_SIZE_BYTES 2048
 #define TASK_PRIORITY (tskIDLE_PRIORITY+3)
 #define FAILSAFE_TIMEOUT_MS 10
 
 #define F_PI 3.14159265358979323846f
 #define PI_MOD(x) (fmodf(x + F_PI, F_PI * 2) - F_PI)
+
+// low pass filter configuration to calculate offset
+// of barometric altitude sensor
+// reasoning: updates at: 10 Hz, tau= 300 s settle time
+// exp(-(1/f) / tau ) ~=~ 0.9997
+#define BARO_OFFSET_LOWPASS_ALPHA 0.9997f 
+
+// simple IAS to TAS aproximation - 2% increase per 1000ft
+// since we do not have flowing air temperature information
+#define IAS2TAS(alt) (1.0f + (0.02f*(alt)/304.8f))
+
 // Private types
 
 // Private variables
@@ -79,26 +97,26 @@ static xTaskHandle attitudeTaskHandle;
 static xQueueHandle gyroQueue;
 static xQueueHandle accelQueue;
 static xQueueHandle magQueue;
+static xQueueHandle airspeedQueue;
 static xQueueHandle baroQueue;
 static xQueueHandle gpsQueue;
+static xQueueHandle gpsVelQueue;
+
+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
 static void AttitudeTask(void *parameters);
 
-static int32_t updateAttitudeComplimentary(bool first_run);
-static int32_t updateAttitudeINSGPS(bool first_run);
+static int32_t updateAttitudeComplementary(bool first_run);
+static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode);
 static void settingsUpdatedCb(UAVObjEvent * objEv);
 
-static float accelKi = 0;
-static float accelKp = 0;
-static float yawBiasRate = 0;
-static float gyroGain = 0.42;
-static int16_t accelbias[3];
-static float R[3][3];
-static int8_t rotate = 0;
-static bool zero_during_arming = false;
-
+static int32_t getNED(GPSPositionData * gpsPosition, float * NED);
 
 /**
  * API for sensor fusion algorithms:
@@ -116,9 +134,13 @@ static bool zero_during_arming = false;
 int32_t AttitudeInitialize(void)
 {
 	AttitudeActualInitialize();
+	AirspeedActualInitialize();
+	AirspeedSensorInitialize();
 	AttitudeSettingsInitialize();
 	PositionActualInitialize();
 	VelocityActualInitialize();
+	RevoSettingsInitialize();
+	RevoCalibrationInitialize();
 	
 	// Initialize this here while we aren't setting the homelocation in GPS
 	HomeLocationInitialize();
@@ -140,12 +162,11 @@ int32_t AttitudeInitialize(void)
 	gyrosBias.z = 0;
 	GyrosBiasSet(&gyrosBias);
 	
-	for(uint8_t i = 0; i < 3; i++)
-		for(uint8_t j = 0; j < 3; j++)
-			R[i][j] = 0;
-	
 	AttitudeSettingsConnectCallback(&settingsUpdatedCb);
-	
+	RevoSettingsConnectCallback(&settingsUpdatedCb);
+	RevoCalibrationConnectCallback(&settingsUpdatedCb);
+	HomeLocationConnectCallback(&settingsUpdatedCb);
+
 	return 0;
 }
 
@@ -159,9 +180,11 @@ int32_t AttitudeStart(void)
 	gyroQueue = xQueueCreate(1, sizeof(UAVObjEvent));
 	accelQueue = xQueueCreate(1, sizeof(UAVObjEvent));
 	magQueue = xQueueCreate(1, sizeof(UAVObjEvent));
+	airspeedQueue = xQueueCreate(1, sizeof(UAVObjEvent));
 	baroQueue = xQueueCreate(1, sizeof(UAVObjEvent));
 	gpsQueue = xQueueCreate(1, sizeof(UAVObjEvent));
-			
+	gpsVelQueue = xQueueCreate(1, sizeof(UAVObjEvent));
+
 	// Start main task
 	xTaskCreate(AttitudeTask, (signed char *)"Attitude", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &attitudeTaskHandle);
 	TaskMonitorAdd(TASKINFO_RUNNING_ATTITUDE, attitudeTaskHandle);
@@ -170,9 +193,11 @@ int32_t AttitudeStart(void)
 	GyrosConnectQueue(gyroQueue);
 	AccelsConnectQueue(accelQueue);
 	MagnetometerConnectQueue(magQueue);
+	AirspeedSensorConnectQueue(airspeedQueue);
 	BaroAltitudeConnectQueue(baroQueue);
 	GPSPositionConnectQueue(gpsQueue);
-	
+	GPSVelocityConnectQueue(gpsVelQueue);
+
 	return 0;
 }
 
@@ -183,28 +208,48 @@ MODULE_INITCALL(AttitudeInitialize, AttitudeStart)
  */
 static void AttitudeTask(void *parameters)
 {
+	bool first_run = true;
+	uint32_t last_algorithm;
 	AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
 
 	// Force settings update to make sure rotation loaded
-	settingsUpdatedCb(AttitudeSettingsHandle());
-	
-	bool first_run = true;
-	
+	settingsUpdatedCb(NULL);
+
 	// Wait for all the sensors be to read
 	vTaskDelay(100);
-	
+
+	// Invalidate previous algorithm to trigger a first run
+	last_algorithm = 0xfffffff;
+
 	// Main task loop
 	while (1) {
-	
+
+		int32_t ret_val = -1;
+
+		if (last_algorithm != revoSettings.FusionAlgorithm) {
+			last_algorithm = revoSettings.FusionAlgorithm;
+			first_run = true;
+		}
+
 		// This  function blocks on data queue
-		if(1) 
-			updateAttitudeComplimentary(first_run);
-		else
-			updateAttitudeINSGPS(first_run);
-			
-		if (first_run)
+		switch (revoSettings.FusionAlgorithm ) {
+			case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY:
+				ret_val = updateAttitudeComplementary(first_run);
+				break;
+			case REVOSETTINGS_FUSIONALGORITHM_INSOUTDOOR:
+				ret_val = updateAttitudeINSGPS(first_run, true);
+				break;
+			case REVOSETTINGS_FUSIONALGORITHM_INSINDOOR:
+				ret_val = updateAttitudeINSGPS(first_run, false);
+				break;
+			default:
+				AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_CRITICAL);
+				break;
+		}
+
+		if(ret_val == 0)
 			first_run = false;
-		
+
 		PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
 	}
 }
@@ -214,9 +259,9 @@ float qmag;
 float attitudeDt;
 float mag_err[3];
 float magKi = 0.000001f;
-float magKp = 0.0001f;
+float magKp = 0.01f;
 
-static int32_t updateAttitudeComplimentary(bool first_run)
+static int32_t updateAttitudeComplementary(bool first_run)
 {
 	UAVObjEvent ev;
 	GyrosData gyrosData;
@@ -226,60 +271,87 @@ static int32_t updateAttitudeComplimentary(bool first_run)
 	static uint8_t init = 0;
 
 	// 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 )
-	{
-		AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_WARNING);
-		return -1;
-	}
-	if ( xQueueReceive(accelQueue, &ev, 0) != pdTRUE )
+	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
-		AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_WARNING);
-		return -1;
+		// Do not set attitude timeout warnings in simulation mode
+		if (!AttitudeActualReadOnly()){
+			AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_WARNING);
+			return -1;
+		}
 	}
-	
+
+	AccelsGet(&accelsData);
+
 	// During initialization and 
 	FlightStatusData flightStatus;
 	FlightStatusGet(&flightStatus);
-	if(first_run)
+	if(first_run) {
+#if defined(PIOS_INCLUDE_HMC5883)
+		// To initialize we need a valid mag reading
+		if ( xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) != pdTRUE )
+			return -1;
+		MagnetometerData magData;
+		MagnetometerGet(&magData);
+#else
+		MagnetometerData magData;
+		magData.x = 100;
+		magData.y = 0;
+		magData.z = 0;
+#endif
+		AttitudeActualData attitudeActual;
+		AttitudeActualGet(&attitudeActual);
 		init = 0;
+		attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI;
+		attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI;
+		attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / F_PI;
+
+		RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
+		AttitudeActualSet(&attitudeActual);
+
+		timeval = PIOS_DELAY_GetRaw();
+
+		return 0;
+
+	}
 
 	if((init == 0 && xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) {
 		// For first 7 seconds use accels to get gyro bias
-		accelKp = 1;
-		accelKi = 0.9;
-		yawBiasRate = 0.23;
-	} else if (zero_during_arming && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
-		accelKp = 1;
-		accelKi = 0.9;
-		yawBiasRate = 0.23;
+		attitudeSettings.AccelKp = 1;
+		attitudeSettings.AccelKi = 0.9;
+		attitudeSettings.YawBiasRate = 0.23;
+		magKp = 1;
+	} else if ((attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
+		attitudeSettings.AccelKp = 1;
+		attitudeSettings.AccelKi = 0.9;
+		attitudeSettings.YawBiasRate = 0.23;
+		magKp = 1;
 		init = 0;
 	} else if (init == 0) {
 		// Reload settings (all the rates)
-		AttitudeSettingsAccelKiGet(&accelKi);
-		AttitudeSettingsAccelKpGet(&accelKp);
-		AttitudeSettingsYawBiasRateGet(&yawBiasRate);
+		AttitudeSettingsGet(&attitudeSettings);
+		magKp = 0.01f;
 		init = 1;
-	}	
-	
+	}
+
 	GyrosGet(&gyrosData);
-	AccelsGet(&accelsData);
 
 	// Compute the dT using the cpu clock
 	dT = PIOS_DELAY_DiffuS(timeval) / 1000000.0f;
 	timeval = PIOS_DELAY_GetRaw();
-	
+
 	float q[4];
-	
+
 	AttitudeActualData attitudeActual;
 	AttitudeActualGet(&attitudeActual);
 
 	float grot[3];
 	float accel_err[3];
-	
+
 	// Get the current attitude estimate
 	quat_copy(&attitudeActual.q1, q);
-	
+
 	// Rotate gravity to body frame and cross with accels
 	grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2]));
 	grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1]));
@@ -292,51 +364,53 @@ static int32_t updateAttitudeComplimentary(bool first_run)
 	accel_err[0] /= accel_mag;
 	accel_err[1] /= accel_mag;
 	accel_err[2] /= accel_mag;	
-	
+
 	if ( xQueueReceive(magQueue, &ev, 0) != pdTRUE )
 	{
 		// Rotate gravity to body frame and cross with accels
 		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;
-		
-		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);
+
+		// 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 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;
+
+			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;
 	}
-	
+
 	// 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] * accelKi;
-	gyrosBias.y += accel_err[1] * 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
-	gyrosData.x += accel_err[0] * accelKp / dT;
-	gyrosData.y += accel_err[1] * accelKp / dT;
-	gyrosData.z += accel_err[2] * accelKp / dT + mag_err[2] * magKp / dT;
+	gyrosData.x += accel_err[0] * attitudeSettings.AccelKp / dT;
+	gyrosData.y += accel_err[1] * attitudeSettings.AccelKp / dT;
+	gyrosData.z += accel_err[2] * attitudeSettings.AccelKp / dT + mag_err[2] * magKp / dT;
 
 	// Work out time derivative from INSAlgo writeup
 	// Also accounts for the fact that gyros are in deg/s
@@ -345,20 +419,20 @@ static int32_t updateAttitudeComplimentary(bool first_run)
 	qdot[1] = (q[0] * gyrosData.x - q[3] * gyrosData.y + q[2] * gyrosData.z) * dT * F_PI / 180 / 2;
 	qdot[2] = (q[3] * gyrosData.x + q[0] * gyrosData.y - q[1] * gyrosData.z) * dT * F_PI / 180 / 2;
 	qdot[3] = (-q[2] * gyrosData.x + q[1] * gyrosData.y + q[0] * gyrosData.z) * dT * F_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];
-	
+
 	if(q[0] < 0) {
 		q[0] = -q[0];
 		q[1] = -q[1];
 		q[2] = -q[2];
 		q[3] = -q[3];
 	}
-	
+
 	// Renomalize
 	qmag = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
 	q[0] = q[0] / qmag;
@@ -381,14 +455,56 @@ static int32_t updateAttitudeComplimentary(bool first_run)
 	Quaternion2RPY(&attitudeActual.q1,&attitudeActual.Roll);
 
 	AttitudeActualSet(&attitudeActual);
-	
+
 	// Flush these queues for avoid errors
-	if ( xQueueReceive(baroQueue, &ev, 0) != pdTRUE )
-	{
+	xQueueReceive(baroQueue, &ev, 0);
+	if ( xQueueReceive(gpsQueue, &ev, 0) == pdTRUE && homeLocation.Set == HOMELOCATION_SET_TRUE ) {
+		float NED[3];
+		// Transform the GPS position into NED coordinates
+		GPSPositionData gpsPosition;
+		GPSPositionGet(&gpsPosition);
+		getNED(&gpsPosition, NED);
+		
+		PositionActualData positionActual;
+		PositionActualGet(&positionActual);
+		positionActual.North = NED[0];
+		positionActual.East = NED[1];
+		positionActual.Down = NED[2];
+		PositionActualSet(&positionActual);
 	}
-	if ( xQueueReceive(gpsQueue, &ev, 0) != pdTRUE )
-	{
+
+	if ( xQueueReceive(gpsVelQueue, &ev, 0) == pdTRUE ) {
+		// Transform the GPS position into NED coordinates
+		GPSVelocityData gpsVelocity;
+		GPSVelocityGet(&gpsVelocity);
+
+		VelocityActualData velocityActual;
+		VelocityActualGet(&velocityActual);
+		velocityActual.North = gpsVelocity.North;
+		velocityActual.East = gpsVelocity.East;
+		velocityActual.Down = gpsVelocity.Down;
+		VelocityActualSet(&velocityActual);
 	}
+	
+	if ( xQueueReceive(airspeedQueue, &ev, 0) == pdTRUE ) {
+		// Calculate true airspeed from indicated airspeed
+		AirspeedSensorData airspeedSensor;
+		AirspeedSensorGet(&airspeedSensor);
+
+		AirspeedActualData airspeed;
+		AirspeedActualGet(&airspeed);
+
+		PositionActualData positionActual;
+		PositionActualGet(&positionActual);
+
+		if (airspeedSensor.SensorConnected==AIRSPEEDSENSOR_SENSORCONNECTED_TRUE) {
+			// we have airspeed available
+			airspeed.CalibratedAirspeed = airspeedSensor.CalibratedAirspeed;
+			airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS( homeLocation.Altitude - positionActual.Down );
+			AirspeedActualSet(&airspeed);
+		}
+	}
+
 
 	AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
 
@@ -398,112 +514,259 @@ static int32_t updateAttitudeComplimentary(bool first_run)
 #include "insgps.h"
 int32_t ins_failed = 0;
 extern struct NavStruct Nav;
-static int32_t updateAttitudeINSGPS(bool first_run)
+int32_t init_stage = 0;
+
+/**
+ * @brief Use the INSGPS fusion algorithm in either indoor or outdoor mode (use GPS)
+ * @params[in] first_run This is the first run so trigger reinitialization
+ * @params[in] outdoor_mode If true use the GPS for position, if false weakly pull to (0,0)
+ * @return 0 for success, -1 for failure
+ */
+static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 {
 	UAVObjEvent ev;
 	GyrosData gyrosData;
 	AccelsData accelsData;
 	MagnetometerData magData;
+	AirspeedSensorData airspeedData;
 	BaroAltitudeData baroData;
-	
-	static uint32_t ins_last_time = 0;
+	GPSPositionData gpsData;
+	GPSVelocityData gpsVelData;
+	GyrosBiasData gyrosBias;
 
+	static bool mag_updated = false;
+	static bool baro_updated;
+	static bool airspeed_updated;
+	static bool gps_updated;
+	static bool gps_vel_updated;
+
+	static float baroOffset = 0;
+
+	static uint32_t ins_last_time = 0;
 	static bool inited;
-	if (first_run)
-		inited = false;
-	
+
+	float NED[3] = {0.0f, 0.0f, 0.0f};
+	float vel[3] = {0.0f, 0.0f, 0.0f};
+	float zeros[3] = {0.0f, 0.0f, 0.0f};
+
+	// Perform the update
+	uint16_t sensors = 0;
+	float dT;
+
 	// Wait until the gyro and accel object is updated, if a timeout then go to failsafe
-	if ( (xQueueReceive(gyroQueue, &ev, 10 / portTICK_RATE_MS) != pdTRUE) ||
-		(xQueueReceive(accelQueue, &ev, 10 / portTICK_RATE_MS) != pdTRUE) )
+	if ( (xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE) ||
+	     (xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE) )
 	{
-		AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_WARNING);
-		return -1;
+		// Do not set attitude timeout warnings in simulation mode
+		if (!AttitudeActualReadOnly()){
+			AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_WARNING);
+			return -1;
+		}
 	}
-	
+
+	if (inited) {
+		mag_updated = 0;
+		baro_updated = 0;
+		airspeed_updated = 0;
+		gps_updated = 0;
+		gps_vel_updated = 0;
+	}
+
+	if (first_run) {
+		inited = false;
+		init_stage = 0;
+
+		mag_updated = 0;
+		baro_updated = 0;
+		airspeed_updated = 0;
+		gps_updated = 0;
+		gps_vel_updated = 0;
+
+		ins_last_time = PIOS_DELAY_GetRaw();
+
+		return 0;
+	}
+
+	mag_updated |= (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE);
+	baro_updated |= xQueueReceive(baroQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE;
+	airspeed_updated |= xQueueReceive(airspeedQueue, &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
-	// TODO: Acquire all data in a queue
 	GyrosGet(&gyrosData);
 	AccelsGet(&accelsData);
 	MagnetometerGet(&magData);
 	BaroAltitudeGet(&baroData);
-	
-	bool mag_updated;
-	bool baro_updated;
-	bool gps_updated;
-	
-	if (inited) {
-		mag_updated = 0;
-		baro_updated = 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;
-	
-	if (!inited && (!mag_updated || !baro_updated || !gps_updated)) {
+	AirspeedSensorGet(&airspeedData);
+	GPSPositionGet(&gpsData);
+	GPSVelocityGet(&gpsVelData);
+	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]);
+
+	// Discard airspeed if sensor not connected
+	airspeed_updated &= ( airspeedData.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE );
+
+	// Have a minimum requirement for gps usage
+	gps_updated &= (gpsData.Satellites >= 7) && (gpsData.PDOP <= 4.0f) && (homeLocation.Set == HOMELOCATION_SET_TRUE);
+
+	if (!inited)
+		AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR);
+	else if (outdoor_mode && gpsData.Satellites < 7)
+		AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR);
+	else
+		AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
+			
+	if (!inited && mag_updated && baro_updated && (gps_updated || !outdoor_mode)) {
 		// Don't initialize until all sensors are read
-		return -1;
-	} else if (!inited ) {
-		inited = true;
+		if (init_stage == 0 && !outdoor_mode) {
+			float Pdiag[16]={25.0f,25.0f,25.0f,5.0f,5.0f,5.0f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-4f,1e-4f,1e-4f};
+			float q[4];
+			float pos[3] = {0.0f, 0.0f, 0.0f};
 
-		float Rbe[3][3], q[4];
-		float ge[3]={0.0f,0.0f,-9.81f};
-		float zeros[3]={0.0f,0.0f,0.0f};
-		float Pdiag[16]={25.0f,25.0f,25.0f,5.0f,5.0f,5.0f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-4f,1e-4f,1e-4f};
-		float vel[3], NED[3];
-		
-		INSGPSInit();
-		
-		HomeLocationData home;
-		HomeLocationGet(&home);
-		
-		GPSPositionData gpsPosition;
-		GPSPositionGet(&gpsPosition);
-		
-		vel[0] = gpsPosition.Groundspeed * cosf(gpsPosition.Heading * F_PI / 180.0f);
-		vel[1] = gpsPosition.Groundspeed * sinf(gpsPosition.Heading * F_PI / 180.0f);
-		vel[2] = 0;
-		
-		// convert from cm back to meters
-		float LLA[3] = {(float) gpsPosition.Latitude / 1e7f, (float) gpsPosition.Longitude / 1e7f, (float) (gpsPosition.GeoidSeparation + gpsPosition.Altitude)};
-		// put in local NED frame
-		float ECEF[3] = {(float) (home.ECEF[0] / 100.0f), (float) (home.ECEF[1] / 100.0f), (float) (home.ECEF[2] / 100.0f)};
-		LLA2Base(LLA, ECEF, (float (*)[3]) home.RNE, NED);
+			// Initialize barometric offset to homelocation altitude
+			baroOffset = -baroData.Altitude;
+			pos[2] = -(baroData.Altitude + baroOffset);
+
+			// Reset the INS algorithm
+			INSGPSInit();
+			INSSetMagVar(revoCalibration.mag_var);
+			INSSetAccelVar(revoCalibration.accel_var);
+			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);
+
+			// Set initial attitude
+			AttitudeActualData attitudeActual;
+			attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI;
+			attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI;
+			attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / F_PI;
+			RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
+			AttitudeActualSet(&attitudeActual);
+
+			q[0] = attitudeActual.q1;
+			q[1] = attitudeActual.q2;
+			q[2] = attitudeActual.q3;
+			q[3] = attitudeActual.q4;
+			INSSetState(pos, zeros, q, zeros, zeros);
+			INSResetP(Pdiag);
+		} else if (init_stage == 0 && outdoor_mode) {
+			float Pdiag[16]={25.0f,25.0f,25.0f,5.0f,5.0f,5.0f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-4f,1e-4f,1e-4f};
+			float q[4];
+			float NED[3];
+
+			// Reset the INS algorithm
+			INSGPSInit();
+			INSSetMagVar(revoCalibration.mag_var);
+			INSSetAccelVar(revoCalibration.accel_var);
+			INSSetGyroVar(revoCalibration.gyro_var);
+			INSSetBaroVar(revoCalibration.baro_var);
+
+			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);
+
+			// Transform the GPS position into NED coordinates
+			getNED(&gpsPosition, NED);
+			
+			// Initialize barometric offset to cirrent GPS NED coordinate
+			baroOffset = -NED[2] - baroData.Altitude;
+
+			xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS);
+			MagnetometerGet(&magData);
+
+			// Set initial attitude
+			AttitudeActualData attitudeActual;
+			attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI;
+			attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI;
+			attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / F_PI;
+			RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
+			AttitudeActualSet(&attitudeActual);
+
+			q[0] = attitudeActual.q1;
+			q[1] = attitudeActual.q2;
+			q[2] = attitudeActual.q3;
+			q[3] = attitudeActual.q4;
+
+			INSSetState(NED, zeros, q, zeros, zeros);
+			INSResetP(Pdiag);
+		} else if (init_stage > 0) {
+			// Run prediction a bit before any corrections
+			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};
+			INSStatePrediction(gyros, &accelsData.x, dT);
+			
+			AttitudeActualData attitude;
+			AttitudeActualGet(&attitude);
+			attitude.q1 = Nav.q[0];
+			attitude.q2 = Nav.q[1];
+			attitude.q3 = Nav.q[2];
+			attitude.q4 = Nav.q[3];
+			Quaternion2RPY(&attitude.q1,&attitude.Roll);
+			AttitudeActualSet(&attitude);
+		}
+
+		init_stage++;
+		if(init_stage > 10)
+			inited = true;
 
-		RotFrom2Vectors(&accelsData.x, ge, &magData.x, home.Be, Rbe);
-		R2Quaternion(Rbe,q);
-		INSSetState(NED, vel, q, &gyrosData.x, zeros);
-		INSSetGyroBias(&gyrosData.x);
-		INSResetP(Pdiag);
-		
 		ins_last_time = PIOS_DELAY_GetRaw();	
+
 		return 0;
 	}
-	
-	// Perform the update	
-	uint16_t sensors = 0;
-	float dT;
-	
+
+	if (!inited)
+		return 0;
+
 	dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f;
 	ins_last_time = PIOS_DELAY_GetRaw();
-	
+
 	// This should only happen at start up or at mode switches
 	if(dT > 0.01f)
 		dT = 0.01f;
 	else if(dT <= 0.001f)
 		dT = 0.001f;
-	
-	
-	GyrosBiasData gyrosBias;
-	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};
-	
+	// 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
+	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);
-	
+
 	// Copy the attitude into the UAVO
 	AttitudeActualData attitude;
 	AttitudeActualGet(&attitude);
@@ -513,50 +776,81 @@ static int32_t updateAttitudeINSGPS(bool first_run)
 	attitude.q4 = Nav.q[3];
 	Quaternion2RPY(&attitude.q1,&attitude.Roll);
 	AttitudeActualSet(&attitude);
-	
-	// Copy the gyro bias into the UAVO
-	gyrosBias.x = Nav.gyro_bias[0];
-	gyrosBias.y = Nav.gyro_bias[1];
-	gyrosBias.z = Nav.gyro_bias[2];
-	GyrosBiasSet(&gyrosBias);
-	
+
 	// Advance the covariance estimate
 	INSCovariancePrediction(dT);
-	
+
 	if(mag_updated)
 		sensors |= MAG_SENSORS;
+
 	if(baro_updated)
 		sensors |= BARO_SENSOR;
 
-	float NED[3] = {0,0,0};
-	float vel[3] = {0,0,0};
+	INSSetMagNorth(homeLocation.Be);
 	
-	if(gps_updated)
+	if (gps_updated && outdoor_mode)
 	{
-		sensors = HORIZ_SENSORS | VERT_SENSORS;
-		GPSPositionData gpsPosition;
-		GPSPositionGet(&gpsPosition);
+		INSSetPosVelVar(revoCalibration.gps_var[REVOCALIBRATION_GPS_VAR_POS], revoCalibration.gps_var[REVOCALIBRATION_GPS_VAR_VEL]);
+		sensors |= POS_SENSORS;
+
+		if (0) { // Old code to take horizontal velocity from GPS Position update
+			sensors |= HORIZ_SENSORS;
+			vel[0] = gpsData.Groundspeed * cosf(gpsData.Heading * F_PI / 180.0f);
+			vel[1] = gpsData.Groundspeed * sinf(gpsData.Heading * F_PI / 180.0f);
+			vel[2] = 0;
+		}
+		// Transform the GPS position into NED coordinates
+		getNED(&gpsData, NED);
+
+		// Track barometric altitude offset with a low pass filter
+		baroOffset = BARO_OFFSET_LOWPASS_ALPHA * baroOffset +
+		    (1.0f - BARO_OFFSET_LOWPASS_ALPHA )
+		    * ( -NED[2] - baroData.Altitude );
+
+	} else if (!outdoor_mode) {
+		INSSetPosVelVar(1e6f, 1e5f);
+		vel[0] = vel[1] = vel[2] = 0;
+		NED[0] = NED[1] = 0;
+		NED[2] = -(baroData.Altitude + baroOffset);
+		sensors |= HORIZ_SENSORS | HORIZ_POS_SENSORS;
+		sensors |= POS_SENSORS |VERT_SENSORS;
+	}
+
+	if (gps_vel_updated && outdoor_mode) {
+		sensors |= HORIZ_SENSORS | VERT_SENSORS;
+		vel[0] = gpsVelData.North;
+		vel[1] = gpsVelData.East;
+		vel[2] = gpsVelData.Down;
+	}
+
+	if (airspeed_updated) {
+		// we have airspeed available
+		AirspeedActualData airspeed;
+		AirspeedActualGet(&airspeed);
+
+		airspeed.CalibratedAirspeed = airspeedData.CalibratedAirspeed;
+		airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS( homeLocation.Altitude - Nav.Pos[2] );
+		AirspeedActualSet(&airspeed);
 		
-		vel[0] = gpsPosition.Groundspeed * cosf(gpsPosition.Heading * F_PI / 180.0f);
-		vel[1] = gpsPosition.Groundspeed * sinf(gpsPosition.Heading * F_PI / 180.0f);
-		vel[2] = 0;
-
-		HomeLocationData home;
-		HomeLocationGet(&home);
-
-		// convert from cm back to meters
-		float LLA[3] = {(float) gpsPosition.Latitude / 1e7f, (float) gpsPosition.Longitude / 1e7f, (float) (gpsPosition.GeoidSeparation + gpsPosition.Altitude)};
-		// put in local NED frame
-		float ECEF[3] = {(float) (home.ECEF[0] / 100.0f), (float) (home.ECEF[1] / 100.0f), (float) (home.ECEF[2] / 100.0f)};
-		LLA2Base(LLA, ECEF, (float (*)[3]) home.RNE, NED);
+		if ( !gps_vel_updated && !gps_updated) {
+			// feed airspeed into EKF, treat wind as 1e2 variance
+			sensors |= HORIZ_SENSORS | VERT_SENSORS;
+			INSSetPosVelVar(1e6f, 1e2f);
+			// rotate airspeed vector into NED frame - airspeed is measured in X axis only
+			float R[3][3];
+			Quaternion2R(Nav.q,R);
+			float vtas[3] = {airspeed.TrueAirspeed,0,0};
+			rot_mult(R,vtas,vel);
+		}
 	}
 	
 	/*
 	 * TODO: Need to add a general sanity check for all the inputs to make sure their kosher
 	 * although probably should occur within INS itself
 	 */
-	INSCorrection(&magData.x, NED, vel, baroData.Altitude, sensors);
-	
+	if (sensors)
+		INSCorrection(&magData.x, NED, vel, ( baroData.Altitude + baroOffset ), sensors);
+
 	// Copy the position and velocity into the UAVO
 	PositionActualData positionActual;
 	PositionActualGet(&positionActual);
@@ -571,59 +865,82 @@ static int32_t updateAttitudeINSGPS(bool first_run)
 	velocityActual.East = Nav.Vel[1];
 	velocityActual.Down = Nav.Vel[2];
 	VelocityActualSet(&velocityActual);
-	
 
-	if(fabs(Nav.gyro_bias[0]) > 0.1f || fabs(Nav.gyro_bias[1]) > 0.1f || fabs(Nav.gyro_bias[2]) > 0.1f) {
-		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;
 }
 
-static void settingsUpdatedCb(UAVObjEvent * objEv) 
+/**
+ * @brief Convert the GPS LLA position into NED coordinates
+ * @note this method uses a taylor expansion around the home coordinates
+ * to convert to NED which allows it to be done with all floating
+ * calculations
+ * @param[in] Current GPS coordinates
+ * @param[out] NED frame coordinates
+ * @returns 0 for success, -1 for failure
+ */
+float T[3];
+const float DEG2RAD = 3.141592653589793f / 180.0f;
+static int32_t getNED(GPSPositionData * gpsPosition, float * NED)
 {
-	AttitudeSettingsData attitudeSettings;
-	AttitudeSettingsGet(&attitudeSettings);
+	float dL[3] = {(gpsPosition->Latitude - homeLocation.Latitude) / 10.0e6f * DEG2RAD,
+		(gpsPosition->Longitude - homeLocation.Longitude) / 10.0e6f * DEG2RAD,
+		(gpsPosition->Altitude + gpsPosition->GeoidSeparation - homeLocation.Altitude)};
 
+	NED[0] = T[0] * dL[0];
+	NED[1] = T[1] * dL[1];
+	NED[2] = T[2] * dL[2];
 
-	accelKp = attitudeSettings.AccelKp;
-	accelKi = attitudeSettings.AccelKi;
-	yawBiasRate = attitudeSettings.YawBiasRate;
-	gyroGain = attitudeSettings.GyroGain;
+	return 0;
+}
 
-	zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE;
+static void settingsUpdatedCb(UAVObjEvent * ev) 
+{
+	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);
 
-	accelbias[0] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_X];
-	accelbias[1] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Y];
-	accelbias[2] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Z];
+		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);
-	
-	// Indicates not to expend cycles on rotation
-	if(attitudeSettings.BoardRotation[0] == 0 && attitudeSettings.BoardRotation[1] == 0 &&
-	   attitudeSettings.BoardRotation[2] == 0) {
-		rotate = 0;
-
-		// Shouldn't be used but to be safe
-		float rotationQuat[4] = {1,0,0,0};
-		Quaternion2R(rotationQuat, R);
-	} else {
-		float rotationQuat[4];
-		const float rpy[3] = {attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_ROLL],
-			attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_PITCH],
-			attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_YAW]};
-		RPY2Quaternion(rpy, rotationQuat);
-		Quaternion2R(rotationQuat, R);
-		rotate = 1;
+		// 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;
+
+		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

@@ -49,24 +49,23 @@
 
 #include "flightbatterystate.h"
 #include "flightbatterysettings.h"
+#include "hwsettings.h"
 
 //
 // Configuration
 //
 #define SAMPLE_PERIOD_MS		500
-
-//#define ENABLE_DEBUG_MSG
-
-#ifdef ENABLE_DEBUG_MSG
-#define DEBUG_PORT			PIOS_COM_GPS
-#define DEBUG_MSG(format, ...) PIOS_COM_SendFormattedString(DEBUG_PORT, format, ## __VA_ARGS__)
-#else
-#define DEBUG_MSG(format, ...)
-#endif
-
 // Private types
 
 // Private variables
+static bool batteryEnabled = false;
+
+//THESE COULD BE BETTER AS SOME KIND OF UNION OR STRUCT, BY WHICH 4 BITS ARE USED FOR EACH 
+//PIN VARIABLE, ONE OF WHICH INDICATES SIGN, AND THE OTHER 3 BITS INDICATE POSITION. THIS WILL
+//WORK FOR QUITE SOMETIME, UNTIL MORE THAN 8 ADC ARE AVAILABLE. EVEN AT THIS POINT, THE STRUCTURE 
+//CAN SIMPLY BE MODIFIED TO SUPPORT 15 ADC PINS, BY USING ALL AVAILABLE BITS.
+static int8_t voltageADCPin=-1; //ADC pin for voltage
+static int8_t currentADCPin=-1; //ADC pin for current
 
 // Private functions
 static void onTimer(UAVObjEvent* ev);
@@ -75,95 +74,132 @@ static void onTimer(UAVObjEvent* ev);
  * Initialise the module, called on startup
  * \returns 0 on success or -1 if initialisation failed
  */
-MODULE_INITCALL(BatteryInitialize, 0)
-
 int32_t BatteryInitialize(void)
 {
-	BatteryStateInitialze();
-	BatterySettingsInitialize();
-	
-	static UAVObjEvent ev;
 
-	memset(&ev,0,sizeof(UAVObjEvent));
-	EventPeriodicCallbackCreate(&ev, onTimer, SAMPLE_PERIOD_MS / portTICK_RATE_MS);
+	
+#ifdef MODULE_BATTERY_BUILTIN
+	batteryEnabled = true;
+#else
+	uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
+
+	HwSettingsOptionalModulesGet(optionalModules);
+
+	if ((optionalModules[HWSETTINGS_OPTIONALMODULES_BATTERY] == HWSETTINGS_OPTIONALMODULES_ENABLED))
+		batteryEnabled = true;
+	else
+		batteryEnabled = false;
+#endif
+
+	uint8_t adcRouting[HWSETTINGS_ADCROUTING_NUMELEM];	
+	HwSettingsADCRoutingGet(adcRouting);
+	
+	//Determine if the battery sensors are routed to ADC pins 
+	for (int i=0; i < HWSETTINGS_ADCROUTING_NUMELEM; i++) {
+		if (adcRouting[i] == HWSETTINGS_ADCROUTING_BATTERYVOLTAGE) {
+			voltageADCPin = i;
+		}
+		if (adcRouting[i] == HWSETTINGS_ADCROUTING_BATTERYCURRENT) {
+			currentADCPin = i;
+		}
+	}
+	
+	//Don't enable module if no ADC pins are routed to the sensors
+	if (voltageADCPin <0 && currentADCPin <0)
+		batteryEnabled = false;
+
+	//Start module
+	if (batteryEnabled) {
+		FlightBatteryStateInitialize();
+		FlightBatterySettingsInitialize();
+	
+		static UAVObjEvent ev;
+
+		memset(&ev,0,sizeof(UAVObjEvent));
+		EventPeriodicCallbackCreate(&ev, onTimer, SAMPLE_PERIOD_MS / portTICK_RATE_MS);
+	}
 
 	return 0;
 }
 
+MODULE_INITCALL(BatteryInitialize, 0)
+#define HAS_SENSOR(x) batterySettings.SensorType[x]==FLIGHTBATTERYSETTINGS_SENSORTYPE_ENABLED 
 static void onTimer(UAVObjEvent* ev)
 {
-	static portTickType lastSysTime;
-	static bool firstRun = true;
-
 	static FlightBatteryStateData flightBatteryData;
-
-	if (firstRun) {
-		#ifdef ENABLE_DEBUG_MSG
-			PIOS_COM_ChangeBaud(DEBUG_PORT, 57600);
-		#endif
-		lastSysTime = xTaskGetTickCount();
-		//FlightBatteryStateGet(&flightBatteryData);
-
-		firstRun = false;
-	}
-
-
-	AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_ERROR);
-
-
-	portTickType thisSysTime;
 	FlightBatterySettingsData batterySettings;
-	static float dT = SAMPLE_PERIOD_MS / 1000;
-	float Bob;
-	float energyRemaining;
-
-
-	// Check how long since last update
-	thisSysTime = xTaskGetTickCount();
-	if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
-		dT = (float)(thisSysTime - lastSysTime) / (float)(portTICK_RATE_MS * 1000.0f);
-	//lastSysTime = thisSysTime;
 
 	FlightBatterySettingsGet(&batterySettings);
 
+	static float dT = SAMPLE_PERIOD_MS / 1000.0f;
+	float energyRemaining;
+
 	//calculate the battery parameters
-	flightBatteryData.Voltage = ((float)PIOS_ADC_PinGet(2)) * batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_VOLTAGEFACTOR]; //in Volts
-	flightBatteryData.Current = ((float)PIOS_ADC_PinGet(1)) * batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_CURRENTFACTOR]; //in Amps
-Bob =dT; // FIXME: something funky happens if I don't do this... Andrew
-	flightBatteryData.ConsumedEnergy += (flightBatteryData.Current * 1000.0 * dT / 3600.0) ;//in mAh
+	if (voltageADCPin >=0) {
+		flightBatteryData.Voltage = ((float)PIOS_ADC_PinGet(voltageADCPin)) * batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_VOLTAGEFACTOR]; //in Volts
+	}
+	else {
+		flightBatteryData.Voltage=1234; //Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC
+	}
 
-	if (flightBatteryData.Current > flightBatteryData.PeakCurrent)flightBatteryData.PeakCurrent = flightBatteryData.Current; //in Amps
-	flightBatteryData.AvgCurrent=(flightBatteryData.AvgCurrent*0.8)+(flightBatteryData.Current*0.2); //in Amps
+	if (currentADCPin >=0) {
+		flightBatteryData.Current = ((float)PIOS_ADC_PinGet(currentADCPin)) * batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_CURRENTFACTOR]; //in Amps
+		if (flightBatteryData.Current > flightBatteryData.PeakCurrent) 
+			flightBatteryData.PeakCurrent = flightBatteryData.Current; //in Amps
+	}
+	else { //If there's no current measurement, we still need to assign one. Make it negative, so it can never trigger an alarm
+		flightBatteryData.Current=-0.1234f; //Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC
+	}
+	
+	flightBatteryData.ConsumedEnergy += (flightBatteryData.Current * dT * 1000.0f / 3600.0f) ;//in mAh
+	
+	//Apply a 2 second rise time low-pass filter to average the current
+	float alpha = 1.0f-dT/(dT+2.0f);
+	flightBatteryData.AvgCurrent=alpha*flightBatteryData.AvgCurrent+(1-alpha)*flightBatteryData.Current; //in Amps
 
-	//sanity checks
-	if (flightBatteryData.AvgCurrent<0)flightBatteryData.AvgCurrent=0.0;
-	if (flightBatteryData.PeakCurrent<0)flightBatteryData.PeakCurrent=0.0;
-	if (flightBatteryData.ConsumedEnergy<0)flightBatteryData.ConsumedEnergy=0.0;
+	/*The motor could regenerate power. Or we could have solar cells. 
+	 In short, is there any likelihood of measuring negative current? If it's a bad current reading we want to check, then 
+	 it makes sense to saturate at max and min values, because a misreading could as easily be very large, as negative. The simple
+	 sign check doesn't catch this.*/
+//	//sanity checks 
+//	if (flightBatteryData.AvgCurrent<0) flightBatteryData.AvgCurrent=0.0f;
+//	if (flightBatteryData.PeakCurrent<0) flightBatteryData.PeakCurrent=0.0f;
+//	if (flightBatteryData.ConsumedEnergy<0) flightBatteryData.ConsumedEnergy=0.0f;
 
 	energyRemaining = batterySettings.Capacity - flightBatteryData.ConsumedEnergy; // in mAh
-	flightBatteryData.EstimatedFlightTime = ((energyRemaining / (flightBatteryData.AvgCurrent*1000.0))*3600.0);//in Sec
+	if (flightBatteryData.AvgCurrent > 0)
+		flightBatteryData.EstimatedFlightTime = (energyRemaining / (flightBatteryData.AvgCurrent*1000.0f))*3600.0f;//in Sec
+	else
+		flightBatteryData.EstimatedFlightTime = 9999;
 
 	//generate alarms where needed...
-	if ((flightBatteryData.Voltage<=0)&&(flightBatteryData.Current<=0))
-	{
+	if ((flightBatteryData.Voltage<=0) && (flightBatteryData.Current<=0))
+	{ 
+		//FIXME: There's no guarantee that a floating ADC will give 0. So this 
+		// check might fail, even when there's nothing attached.
 		AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_ERROR);
 		AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_ERROR);
 	}
 	else
 	{
-		if (flightBatteryData.EstimatedFlightTime < 30) AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_CRITICAL);
-		else if (flightBatteryData.EstimatedFlightTime < 60) AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_WARNING);
-		else AlarmsClear(SYSTEMALARMS_ALARM_FLIGHTTIME);
+		// FIXME: should make the timer alarms user configurable
+		if (flightBatteryData.EstimatedFlightTime < 30) 
+			AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_CRITICAL);
+		else if (flightBatteryData.EstimatedFlightTime < 120) 
+			AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_WARNING);
+		else 
+			AlarmsClear(SYSTEMALARMS_ALARM_FLIGHTTIME);
 
-		// FIXME: should make the battery voltage detection dependent on battery type.
+		// FIXME: should make the battery voltage detection dependent on battery type. 
+		/*Not so sure. Some users will want to run their batteries harder than others, so it should be the user's choice. [KDS]*/
 		if (flightBatteryData.Voltage < batterySettings.VoltageThresholds[FLIGHTBATTERYSETTINGS_VOLTAGETHRESHOLDS_ALARM])
 			AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_CRITICAL);
 		else if (flightBatteryData.Voltage < batterySettings.VoltageThresholds[FLIGHTBATTERYSETTINGS_VOLTAGETHRESHOLDS_WARNING])
 			AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_WARNING);
-		else AlarmsClear(SYSTEMALARMS_ALARM_BATTERY);
+		else 
+			AlarmsClear(SYSTEMALARMS_ALARM_BATTERY);
 	}
-	lastSysTime = thisSysTime;
-
+	
 	FlightBatteryStateSet(&flightBatteryData);
 }
 

flight/Modules/Extensions/MagBaro/inc/magbaro.h

@@ -0,0 +1,41 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{ 
+ * @addtogroup AltitudeModule Altitude Module
+ * @brief Communicate with BMP085 and update @ref AltitudeActual "AltitudeActual UAV Object"
+ * @{ 
+ *
+ * @file       altitude.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Altitude module, reads temperature and pressure from BMP085
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+#ifndef MAGBARO_H
+#define MAGBARO_H
+
+int32_t MagBaroInitialize();
+
+#endif // ALTITUDE_H
+
+/**
+ * @}
+ * @}
+ */

flight/Modules/Extensions/MagBaro/magbaro.c

@@ -0,0 +1,218 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{ 
+ * @addtogroup AltitudeModule Altitude Module
+ * @brief Communicate with BMP085 and update @ref BaroAltitude "BaroAltitude UAV Object"
+ * @{ 
+ *
+ * @file       altitude.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Altitude module, handles temperature and pressure readings from BMP085
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+/**
+ * Output object: BaroAltitude
+ *
+ * This module will periodically update the value of the BaroAltitude object.
+ *
+ */
+
+#include "openpilot.h"
+#include "hwsettings.h"
+#include "magbaro.h"
+#include "baroaltitude.h"	// object that will be updated by the module
+#include "magnetometer.h"
+
+// Private constants
+#define STACK_SIZE_BYTES 620
+#define TASK_PRIORITY (tskIDLE_PRIORITY+1)
+#define UPDATE_PERIOD 50
+
+// Private types
+
+// Private variables
+static xTaskHandle taskHandle;
+
+// down sampling variables
+#define alt_ds_size    4
+static int32_t alt_ds_temp = 0;
+static int32_t alt_ds_pres = 0;
+static int alt_ds_count = 0;
+int32_t mag_test;
+static bool magbaroEnabled;
+static float mag_bias[3] = {0,0,0};
+static float mag_scale[3] = {1,1,1};
+
+        // Private functions
+static void magbaroTask(void *parameters);
+
+/**
+ * Initialise the module, called on startup
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t MagBaroStart()
+{
+
+	if (magbaroEnabled) {
+		// Start main task
+		xTaskCreate(magbaroTask, (signed char *)"MagBaro", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
+		//TaskMonitorAdd(TASKINFO_RUNNING_MAGBARO, taskHandle);
+		return 0;
+	}
+	return -1;
+}
+
+/**
+ * Initialise the module, called on startup
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t MagBaroInitialize()
+{
+#ifdef MODULE_MagBaro_BUILTIN
+	magbaroEnabled = 1;
+#else
+	HwSettingsInitialize();
+	uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
+	HwSettingsOptionalModulesGet(optionalModules);
+	if (optionalModules[HWSETTINGS_OPTIONALMODULES_MAGBARO] == HWSETTINGS_OPTIONALMODULES_ENABLED) {
+		magbaroEnabled = 1;
+	} else {
+		magbaroEnabled = 0;
+	}
+#endif
+
+	if(magbaroEnabled)
+	{
+		MagnetometerInitialize();
+		BaroAltitudeInitialize();
+
+		// init down-sampling data
+		alt_ds_temp = 0;
+		alt_ds_pres = 0;
+		alt_ds_count = 0;
+	}
+	return 0;
+}
+MODULE_INITCALL(MagBaroInitialize, MagBaroStart)
+/**
+ * Module thread, should not return.
+ */
+
+static const struct pios_hmc5883_cfg pios_hmc5883_cfg = {
+#ifdef PIOS_HMC5883_HAS_GPIOS
+	.exti_cfg = 0,
+#endif
+	.M_ODR = PIOS_HMC5883_ODR_15,
+	.Meas_Conf = PIOS_HMC5883_MEASCONF_NORMAL,
+	.Gain = PIOS_HMC5883_GAIN_1_9,
+	.Mode = PIOS_HMC5883_MODE_CONTINUOUS,
+
+};
+
+static void magbaroTask(void *parameters)
+{
+	BaroAltitudeData data;
+	portTickType lastSysTime;
+	
+#if defined(PIOS_INCLUDE_BMP085)
+	PIOS_BMP085_Init();
+#endif
+#if defined(PIOS_INCLUDE_HMC5883)
+	PIOS_HMC5883_Init(&pios_hmc5883_cfg);
+#endif
+
+#if defined(PIOS_INCLUDE_HMC5883)
+	//mag_test = PIOS_HMC5883_Test();
+#else
+	mag_test = 0;
+#endif
+	// Main task loop
+	lastSysTime = xTaskGetTickCount();
+	uint32_t mag_update_time = PIOS_DELAY_GetRaw();
+	while (1)
+	{
+#if defined(PIOS_INCLUDE_BMP085)
+		// Update the temperature data
+		PIOS_BMP085_StartADC(TemperatureConv);
+#ifdef PIOS_BMP085_HAS_GPIOS
+		xSemaphoreTake(PIOS_BMP085_EOC, portMAX_DELAY);
+#else
+		vTaskDelay(5 / portTICK_RATE_MS);
+#endif
+		PIOS_BMP085_ReadADC();
+		alt_ds_temp += PIOS_BMP085_GetTemperature();
+		
+		// Update the pressure data
+		PIOS_BMP085_StartADC(PressureConv);
+#ifdef PIOS_BMP085_HAS_GPIOS
+		xSemaphoreTake(PIOS_BMP085_EOC, portMAX_DELAY);
+#else
+		vTaskDelay(26 / portTICK_RATE_MS);
+#endif
+		PIOS_BMP085_ReadADC();
+		alt_ds_pres += PIOS_BMP085_GetPressure();
+		
+		if (++alt_ds_count >= alt_ds_size)
+		{
+			alt_ds_count = 0;
+
+			// Convert from 1/10ths of degC to degC
+			data.Temperature = alt_ds_temp / (10.0 * alt_ds_size);
+			alt_ds_temp = 0;
+
+			// Convert from Pa to kPa
+			data.Pressure = alt_ds_pres / (1000.0f * alt_ds_size);
+			alt_ds_pres = 0;
+
+			// Compute the current altitude (all pressures in kPa)
+			data.Altitude = 44330.0 * (1.0 - powf((data.Pressure / (BMP085_P0 / 1000.0)), (1.0 / 5.255)));
+
+			// Update the AltitudeActual UAVObject
+			BaroAltitudeSet(&data);
+		}
+#endif
+
+#if defined(PIOS_INCLUDE_HMC5883)
+		MagnetometerData mag;
+		if (PIOS_HMC5883_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 100000) {
+			int16_t values[3];
+			PIOS_HMC5883_ReadMag(values);
+			float mags[3] = {(float) values[1] * mag_scale[0] - mag_bias[0],
+			                (float) values[0] * mag_scale[1] - mag_bias[1],
+			                -(float) values[2] * mag_scale[2] - mag_bias[2]};
+			mag.x = mags[0];
+			mag.y = mags[1];
+			mag.z = mags[2];
+			MagnetometerSet(&mag);
+			mag_update_time = PIOS_DELAY_GetRaw();
+		}
+#endif
+
+		// Delay until it is time to read the next sample
+		vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD / portTICK_RATE_MS);
+	}
+}
+
+/**
+ * @}
+ * @}
+ */

flight/Modules/FirmwareIAP/firmwareiap.c

@@ -108,6 +108,11 @@ int32_t FirmwareIAPInitialize()
 	return 0;
 }
 
+int32_t FirmwareIAPStart()
+{
+	return 0;
+}
+
 /*!
  * \brief	FirmwareIAPCallback - callback function for firmware IAP requests
  * \param[in]  ev - pointer objevent

flight/Modules/FirmwareIAP/inc/firmwareiap.h

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

flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c

@@ -0,0 +1,667 @@
+/**
+ ******************************************************************************
+ *
+ * @file       fixedwingpathfollower.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      This module compared @ref PositionActuatl to @ref ActiveWaypoint 
+ * and sets @ref AttitudeDesired.  It only does this when the FlightMode field
+ * of @ref ManualControlCommand is Auto.
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+/**
+ * Input object: ActiveWaypoint
+ * Input object: PositionActual
+ * Input object: ManualControlCommand
+ * Output object: AttitudeDesired
+ *
+ * This module will periodically update the value of the AttitudeDesired object.
+ *
+ * The module executes in its own thread in this example.
+ *
+ * Modules have no API, all communication to other modules is done through UAVObjects.
+ * However modules may use the API exposed by shared libraries.
+ * See the OpenPilot wiki for more details.
+ * http://www.openpilot.org/OpenPilot_Application_Architecture
+ *
+ */
+
+#include "openpilot.h"
+#include "paths.h"
+
+#include "accels.h"
+#include "hwsettings.h"
+#include "attitudeactual.h"
+#include "pathdesired.h"	// object that will be updated by the module
+#include "positionactual.h"
+#include "manualcontrol.h"
+#include "flightstatus.h"
+#include "pathstatus.h"
+#include "airspeedactual.h"
+#include "gpsvelocity.h"
+#include "gpsposition.h"
+#include "fixedwingpathfollowersettings.h"
+#include "fixedwingpathfollowerstatus.h"
+#include "homelocation.h"
+#include "stabilizationdesired.h"
+#include "stabilizationsettings.h"
+#include "systemsettings.h"
+#include "velocitydesired.h"
+#include "velocityactual.h"
+#include "CoordinateConversions.h"
+
+// Private constants
+#define MAX_QUEUE_SIZE 4
+#define STACK_SIZE_BYTES 1548
+#define TASK_PRIORITY (tskIDLE_PRIORITY+2)
+#define F_PI 3.14159265358979323846f
+#define RAD2DEG (180.0f/F_PI)
+#define DEG2RAD (F_PI/180.0f)
+#define GEE 9.81f
+// Private types
+
+// Private variables
+static bool followerEnabled = false;
+static xTaskHandle pathfollowerTaskHandle;
+static PathDesiredData pathDesired;
+static PathStatusData pathStatus;
+static FixedWingPathFollowerSettingsData fixedwingpathfollowerSettings;
+
+// Private functions
+static void pathfollowerTask(void *parameters);
+static void SettingsUpdatedCb(UAVObjEvent * ev);
+static void updatePathVelocity();
+static uint8_t updateFixedDesiredAttitude();
+static void updateFixedAttitude();
+static void airspeedActualUpdatedCb(UAVObjEvent * ev);
+static float bound(float val, float min, float max);
+
+/**
+ * Initialise the module, called on startup
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t FixedWingPathFollowerStart()
+{
+	if (followerEnabled) {
+		// Start main task
+		xTaskCreate(pathfollowerTask, (signed char *)"PathFollower", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &pathfollowerTaskHandle);
+		TaskMonitorAdd(TASKINFO_RUNNING_PATHFOLLOWER, pathfollowerTaskHandle);
+	}
+
+	return 0;
+}
+
+/**
+ * Initialise the module, called on startup
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t FixedWingPathFollowerInitialize()
+{
+	HwSettingsInitialize();
+	uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
+	HwSettingsOptionalModulesGet(optionalModules);
+	if (optionalModules[HWSETTINGS_OPTIONALMODULES_FIXEDWINGPATHFOLLOWER] == HWSETTINGS_OPTIONALMODULES_ENABLED) {
+		followerEnabled = true;
+		FixedWingPathFollowerSettingsInitialize();
+		FixedWingPathFollowerStatusInitialize();
+		PathDesiredInitialize();
+		PathStatusInitialize();
+		VelocityDesiredInitialize();
+		AirspeedActualInitialize();
+	} else {
+		followerEnabled = false;
+	}
+	return 0;
+}
+MODULE_INITCALL(FixedWingPathFollowerInitialize, FixedWingPathFollowerStart)
+
+static float northVelIntegral = 0;
+static float eastVelIntegral = 0;
+static float downVelIntegral = 0;
+
+static float bearingIntegral = 0;
+static float speedIntegral = 0;
+static float powerIntegral = 0;
+static float airspeedErrorInt=0;
+
+// correct speed by measured airspeed
+static float indicatedAirspeedActualBias = 0;
+
+/**
+ * Module thread, should not return.
+ */
+static void pathfollowerTask(void *parameters)
+{
+	SystemSettingsData systemSettings;
+	FlightStatusData flightStatus;
+	
+	portTickType lastUpdateTime;
+	
+	AirspeedActualConnectCallback(airspeedActualUpdatedCb);
+	FixedWingPathFollowerSettingsConnectCallback(SettingsUpdatedCb);
+	PathDesiredConnectCallback(SettingsUpdatedCb);
+	
+	FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings);
+	PathDesiredGet(&pathDesired);
+	
+	// Main task loop
+	lastUpdateTime = xTaskGetTickCount();
+	while (1) {
+
+		// Conditions when this runs:
+		// 1. Must have FixedWing type airframe
+		// 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint  OR
+		//    FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path
+
+		SystemSettingsGet(&systemSettings);
+		if ( (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING) &&
+			(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON) &&
+			(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGVTAIL) )
+		{
+			AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING);
+			vTaskDelay(1000);
+			continue;
+		}
+
+		// Continue collecting data if not enough time
+		vTaskDelayUntil(&lastUpdateTime, fixedwingpathfollowerSettings.UpdatePeriod / portTICK_RATE_MS);
+
+		
+		FlightStatusGet(&flightStatus);
+		PathStatusGet(&pathStatus);
+	
+		uint8_t result;
+		// Check the combinations of flightmode and pathdesired mode
+		switch(flightStatus.FlightMode) {
+			case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
+			case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
+				if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
+					updatePathVelocity();
+					result = updateFixedDesiredAttitude();
+					if (result) {
+						AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
+					} else {
+						AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING);
+					}
+				} else {
+					AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR);
+				}
+				break;
+			case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
+				pathStatus.UID = pathDesired.UID;
+				pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
+				switch(pathDesired.Mode) {
+					case PATHDESIRED_MODE_FLYENDPOINT:
+					case PATHDESIRED_MODE_FLYVECTOR:
+					case PATHDESIRED_MODE_FLYCIRCLERIGHT:
+					case PATHDESIRED_MODE_FLYCIRCLELEFT:
+						updatePathVelocity();
+						result = updateFixedDesiredAttitude();
+						if (result) {
+							AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
+						} else {
+							pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
+							AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING);
+						}
+						break;
+					case PATHDESIRED_MODE_FIXEDATTITUDE:
+						updateFixedAttitude(pathDesired.ModeParameters);
+						AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
+						break;
+					case PATHDESIRED_MODE_DISARMALARM:
+						AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_CRITICAL);
+						break;
+					default:
+						pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
+						AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR);
+						break;
+				}
+				break;
+			default:
+				// Be cleaner and get rid of global variables
+				northVelIntegral = 0;
+				eastVelIntegral = 0;
+				downVelIntegral = 0;
+				bearingIntegral = 0;
+				speedIntegral = 0;
+				powerIntegral = 0;
+
+				break;
+		}
+		PathStatusSet(&pathStatus);
+	}
+}
+
+/**
+ * Compute desired velocity from the current position and path
+ *
+ * Takes in @ref PositionActual and compares it to @ref PathDesired 
+ * and computes @ref VelocityDesired
+ */
+static void updatePathVelocity()
+{
+	PositionActualData positionActual;
+	PositionActualGet(&positionActual);
+	VelocityActualData velocityActual;
+	VelocityActualGet(&velocityActual);
+
+	// look ahead fixedwingpathfollowerSettings.HeadingFeedForward seconds
+	float cur[3] = {positionActual.North + (velocityActual.North * fixedwingpathfollowerSettings.HeadingFeedForward),
+					positionActual.East + (velocityActual.East * fixedwingpathfollowerSettings.HeadingFeedForward),
+					positionActual.Down + (velocityActual.Down * fixedwingpathfollowerSettings.HeadingFeedForward)
+					};
+	struct path_status progress;
+
+	path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode);
+	
+	float groundspeed;
+	float altitudeSetpoint;
+	switch (pathDesired.Mode) {
+		case PATHDESIRED_MODE_FLYCIRCLERIGHT:
+		case PATHDESIRED_MODE_DRIVECIRCLERIGHT:
+		case PATHDESIRED_MODE_FLYCIRCLELEFT:
+		case PATHDESIRED_MODE_DRIVECIRCLELEFT:
+			groundspeed = pathDesired.EndingVelocity;
+			altitudeSetpoint = pathDesired.End[2];
+			break;
+		case PATHDESIRED_MODE_FLYENDPOINT:
+		case PATHDESIRED_MODE_DRIVEENDPOINT:
+		case PATHDESIRED_MODE_FLYVECTOR:
+		case PATHDESIRED_MODE_DRIVEVECTOR:
+		default:
+			groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) *
+				bound(progress.fractional_progress,0,1);
+			altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) *
+				bound(progress.fractional_progress,0,1);
+			break;
+	}
+	// make sure groundspeed is not zero
+	if (groundspeed<1e-2) groundspeed=1e-2;
+	
+	// calculate velocity - can be zero if waypoints are too close
+	VelocityDesiredData velocityDesired;
+	velocityDesired.North = progress.path_direction[0];
+	velocityDesired.East = progress.path_direction[1];
+	
+	float error_speed = progress.error * fixedwingpathfollowerSettings.HorizontalPosP;
+
+	// if a plane is crossing its desired flightpath facing the wrong way (away from flight direction)
+	// it would turn towards the flightpath to get on its desired course. This however would reverse the correction vector
+	// once it crosses the flightpath again, which would make it again turn towards the flightpath (but away from its desired heading)
+	// leading to an S-shape snake course the wrong way
+	// this only happens especially if HorizontalPosP is too high, as otherwise the angle between velocity desired and path_direction won't
+	// turn steep unless there is enough space complete the turn before crossing the flightpath
+	// in this case the plane effectively needs to be turned around
+	// indicators:
+	// difference between correction_direction and velocityactual >90 degrees and
+	// difference between path_direction and velocityactual >90 degrees  ( 4th sector, facing away from eerything )
+	// fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore)
+	float angle1=RAD2DEG * ( atan2f(progress.path_direction[1],progress.path_direction[0]) - atan2f(velocityActual.East,velocityActual.North));
+	float angle2=RAD2DEG * ( atan2f(progress.correction_direction[1],progress.correction_direction[0]) - atan2f(velocityActual.East,velocityActual.North));
+	if (angle1<-180.0f) angle1+=360.0f;
+	if (angle1>180.0f) angle1-=360.0f;
+	if (angle2<-180.0f) angle2+=360.0f;
+	if (angle2>180.0f) angle2-=360.0f;
+	if (fabs(angle1)>=90.0f && fabs(angle2)>=90.0f) {
+		error_speed=0;
+	}
+
+	// calculate correction - can also be zero if correction vector is 0 or no error present
+	velocityDesired.North += progress.correction_direction[0] * error_speed;
+	velocityDesired.East += progress.correction_direction[1] * error_speed;
+
+	//scale to correct length
+	float l=sqrtf(velocityDesired.North*velocityDesired.North + velocityDesired.East*velocityDesired.East);
+	velocityDesired.North *= groundspeed/l;
+	velocityDesired.East *= groundspeed/l;
+	
+	float downError = altitudeSetpoint - positionActual.Down;
+	velocityDesired.Down = downError * fixedwingpathfollowerSettings.VerticalPosP;
+
+	// update pathstatus
+	pathStatus.error = progress.error;
+	pathStatus.fractional_progress = progress.fractional_progress;
+
+	VelocityDesiredSet(&velocityDesired);
+}
+
+
+/**
+ * Compute desired attitude from a fixed preset
+ *
+ */
+static void updateFixedAttitude(float* attitude)
+{
+	StabilizationDesiredData stabDesired;
+	StabilizationDesiredGet(&stabDesired);
+	stabDesired.Roll     = attitude[0];
+	stabDesired.Pitch    = attitude[1];
+	stabDesired.Yaw      = attitude[2];
+	stabDesired.Throttle = attitude[3];
+	stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+	stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+	stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
+	StabilizationDesiredSet(&stabDesired);
+}
+
+/**
+ * Compute desired attitude from the desired velocity
+ *
+ * Takes in @ref NedActual which has the acceleration in the 
+ * NED frame as the feedback term and then compares the 
+ * @ref VelocityActual against the @ref VelocityDesired
+ */
+static uint8_t updateFixedDesiredAttitude()
+{
+
+	uint8_t result = 1;
+
+	float dT = fixedwingpathfollowerSettings.UpdatePeriod / 1000.0f; //Convert from [ms] to [s]
+
+	VelocityDesiredData velocityDesired;
+	VelocityActualData velocityActual;
+	StabilizationDesiredData stabDesired;
+	AttitudeActualData attitudeActual;
+	AccelsData accels;
+	FixedWingPathFollowerSettingsData fixedwingpathfollowerSettings;
+	StabilizationSettingsData stabSettings;
+	FixedWingPathFollowerStatusData fixedwingpathfollowerStatus;
+	AirspeedActualData airspeedActual;
+	
+	float groundspeedActual;
+	float groundspeedDesired;
+	float indicatedAirspeedActual;
+	float indicatedAirspeedDesired;
+	float airspeedError;
+	
+	float pitchCommand;
+
+	float descentspeedDesired;
+	float descentspeedError;
+	float powerCommand;
+
+	float bearingError;
+	float bearingCommand;
+
+	FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings);
+
+	FixedWingPathFollowerStatusGet(&fixedwingpathfollowerStatus);
+	
+	VelocityActualGet(&velocityActual);
+	StabilizationDesiredGet(&stabDesired);
+	VelocityDesiredGet(&velocityDesired);
+	AttitudeActualGet(&attitudeActual);
+	AccelsGet(&accels);
+	StabilizationSettingsGet(&stabSettings);
+	AirspeedActualGet(&airspeedActual);
+
+
+	/**
+	 * Compute speed error (required for throttle and pitch)
+	 */
+
+	// Current ground speed
+	groundspeedActual  = sqrtf( velocityActual.East*velocityActual.East + velocityActual.North*velocityActual.North );
+	// note that airspeedActualBias is ( calibratedAirspeed - groundSpeed ) at the time of measurement,
+	// but thanks to accelerometers,  groundspeed reacts faster to changes in direction
+	// than airspeed and gps sensors alone
+	indicatedAirspeedActual     = groundspeedActual + indicatedAirspeedActualBias;
+
+	// Desired ground speed
+	groundspeedDesired = sqrtf(velocityDesired.North*velocityDesired.North + velocityDesired.East*velocityDesired.East);
+	indicatedAirspeedDesired    = bound( groundspeedDesired + indicatedAirspeedActualBias,
+							fixedwingpathfollowerSettings.BestClimbRateSpeed,
+							fixedwingpathfollowerSettings.CruiseSpeed);
+
+	// Airspeed error
+	airspeedError = indicatedAirspeedDesired - indicatedAirspeedActual;
+
+	// Vertical speed error
+	descentspeedDesired = bound (
+						velocityDesired.Down,
+						-fixedwingpathfollowerSettings.VerticalVelMax,
+						fixedwingpathfollowerSettings.VerticalVelMax);
+	descentspeedError = descentspeedDesired - velocityActual.Down;
+
+	// Error condition: wind speed is higher than maximum allowed speed. We are forced backwards!
+	fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 0;
+	if (groundspeedDesired - indicatedAirspeedActualBias <= 0 ) {
+		fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 1;
+		result = 0;
+	}
+	// Error condition: plane too slow or too fast
+	fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 0;
+	fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 0;
+	if ( indicatedAirspeedActual >  fixedwingpathfollowerSettings.AirSpeedMax) {
+		fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_OVERSPEED] = 1;
+		result = 0;
+	}
+	if ( indicatedAirspeedActual >  fixedwingpathfollowerSettings.CruiseSpeed * 1.2f) {
+		fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 1;
+		result = 0;
+	}
+	if (indicatedAirspeedActual < fixedwingpathfollowerSettings.BestClimbRateSpeed * 0.8f && 1) { //The next three && 1 are placeholders for UAVOs representing LANDING and TAKEOFF
+		fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1;
+		result = 0;
+	}
+	if (indicatedAirspeedActual < fixedwingpathfollowerSettings.StallSpeedClean && 1 && 1) { //Where the && 1 represents the UAVO that will control whether the airplane is prepped for landing or not
+		fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_STALLSPEED] = 1;
+		result = 0;
+	}
+	if (indicatedAirspeedActual < fixedwingpathfollowerSettings.StallSpeedDirty && 1) {
+		fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_STALLSPEED] = 1;
+		result = 0;
+	}
+	
+	if (indicatedAirspeedActual<1e-6) {
+		// prevent division by zero, abort without controlling anything. This guidance mode is not suited for takeoff or touchdown, or handling stationary planes
+		// also we cannot handle planes flying backwards, lets just wait until the nose drops
+		fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1;
+		return 0;
+	}
+
+	/**
+	 * Compute desired throttle command
+	 */
+	// compute saturated integral error throttle response. Make integral leaky for better performance. Approximately 30s time constant.
+	if (fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI] >0) {
+		powerIntegral =	bound(powerIntegral + -descentspeedError * dT, 
+										-fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT]/fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI],
+										fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT]/fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI]
+										)*(1.0f-1.0f/(1.0f+30.0f/dT));
+	} else powerIntegral = 0;
+	
+	//Compute the cross feed from vertical speed to pitch, with saturation
+	float speedErrorToPowerCommandComponent = bound (
+		 (airspeedError/fixedwingpathfollowerSettings.BestClimbRateSpeed)* fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOPOWERCROSSFEED_KP] ,
+		 -fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOPOWERCROSSFEED_MAX],
+		 fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOPOWERCROSSFEED_MAX]
+		 );
+
+	// Compute final throttle response
+	powerCommand = -descentspeedError * fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KP] +
+			powerIntegral*	fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI] +
+			speedErrorToPowerCommandComponent;
+
+	//Output internal state to telemetry
+	fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_POWER] = descentspeedError;
+	fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_POWER] = powerIntegral;
+	fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_POWER] = powerCommand;
+
+	// set throttle
+	stabDesired.Throttle = bound(fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_NEUTRAL] + powerCommand,
+				fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN],
+				fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX]);
+
+	// Error condition: plane cannot hold altitude at current speed.
+	fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 0;
+	if (
+		powerCommand == fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX] // throttle at maximum
+		&& velocityActual.Down > 0 // we ARE going down
+		&& descentspeedDesired < 0 // we WANT to go up
+		&& airspeedError > 0 // we are too slow already
+		) 
+	{
+		fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 1;
+		result = 0;
+	}
+	// Error condition: plane keeps climbing despite minimum throttle (opposite of above)
+	fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 0;
+	if (
+		powerCommand == fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN] // throttle at minimum
+		&& velocityActual.Down < 0 // we ARE going up
+		&& descentspeedDesired > 0 // we WANT to go down
+		&& airspeedError < 0 // we are too fast already
+		) 
+	{
+		fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 1;
+		result = 0;
+	}
+
+
+	/**
+	 * Compute desired pitch command
+	 */
+		
+	if (fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI] > 0){
+		//Integrate with saturation
+		airspeedErrorInt=bound(airspeedErrorInt + airspeedError * dT, 
+				-fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_ILIMIT]/fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI],
+				fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_ILIMIT]/fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI]);
+	}				
+	
+	//Compute the cross feed from vertical speed to pitch, with saturation
+	float verticalSpeedToPitchCommandComponent=bound (-descentspeedError * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_KP],
+			 -fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX],
+			 fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX]
+			 );
+	
+	//Compute the pitch command as err*Kp + errInt*Ki + X_feed.
+	pitchCommand= -(airspeedError*fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KP] 
+						 + airspeedErrorInt*fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI]
+						 )	+ verticalSpeedToPitchCommandComponent;
+	
+	fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_SPEED] = airspeedError;
+	fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_SPEED] = airspeedErrorInt;
+	fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_SPEED] = pitchCommand;
+
+	stabDesired.Pitch = bound(fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_NEUTRAL] +
+		pitchCommand,
+		fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MIN],
+		fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX]);
+
+	// Error condition: high speed dive
+	fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 0;
+	if (
+		pitchCommand == fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX] // pitch demand is full up
+		&& velocityActual.Down > 0 // we ARE going down
+		&& descentspeedDesired < 0 // we WANT to go up
+		&& airspeedError < 0 // we are too fast already
+		) {
+		fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 1;
+		result = 0;
+	}
+
+
+	/**
+	 * Compute desired roll command
+	 */
+	if (groundspeedDesired> 1e-6) {
+		bearingError = RAD2DEG * (atan2f(velocityDesired.East,velocityDesired.North) - atan2f(velocityActual.East,velocityActual.North));
+	} else {
+		// if we are not supposed to move, run in a circle
+		bearingError = -90.0f;
+	}
+	
+	if (bearingError<-180.0f) bearingError+=360.0f;
+	if (bearingError>180.0f) bearingError-=360.0f;
+
+	bearingIntegral = bound(bearingIntegral + bearingError * dT * fixedwingpathfollowerSettings.BearingPI[FIXEDWINGPATHFOLLOWERSETTINGS_BEARINGPI_KI], 
+		-fixedwingpathfollowerSettings.BearingPI[FIXEDWINGPATHFOLLOWERSETTINGS_BEARINGPI_ILIMIT],
+		fixedwingpathfollowerSettings.BearingPI[FIXEDWINGPATHFOLLOWERSETTINGS_BEARINGPI_ILIMIT]);
+	bearingCommand = (bearingError * fixedwingpathfollowerSettings.BearingPI[FIXEDWINGPATHFOLLOWERSETTINGS_BEARINGPI_KP] +
+		bearingIntegral);
+
+	fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_BEARING] = bearingError;
+	fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_BEARING] = bearingIntegral;
+	fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_BEARING] = bearingCommand;
+	
+	stabDesired.Roll = bound( fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_NEUTRAL] +
+		bearingCommand,
+		fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_MIN],
+		fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_MAX] );
+
+	// TODO: find a check to determine loss of directional control. Likely needs some check of derivative
+
+
+	/**
+	 * Compute desired yaw command
+	 */
+	// TODO implement raw control mode for yaw and base on Accels.Y
+	stabDesired.Yaw = 0;
+
+
+	stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+	stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+	stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE;
+	
+	StabilizationDesiredSet(&stabDesired);
+
+	FixedWingPathFollowerStatusSet(&fixedwingpathfollowerStatus);
+
+	return result;
+}
+
+
+/**
+ * Bound input value between limits
+ */
+static float bound(float val, float min, float max)
+{
+	if (val < min) {
+		val = min;
+	} else if (val > max) {
+		val = max;
+	}
+	return val;
+}
+
+static void SettingsUpdatedCb(UAVObjEvent * ev)
+{
+	FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings);
+	PathDesiredGet(&pathDesired);
+}
+
+static void airspeedActualUpdatedCb(UAVObjEvent * ev)
+{
+
+	AirspeedActualData airspeedActual;
+	VelocityActualData velocityActual;
+
+	AirspeedActualGet(&airspeedActual);
+	VelocityActualGet(&velocityActual);
+	float groundspeed = sqrtf(velocityActual.East*velocityActual.East + velocityActual.North*velocityActual.North );
+
+	
+	indicatedAirspeedActualBias = airspeedActual.CalibratedAirspeed - groundspeed;
+	// note - we do fly by Indicated Airspeed (== calibrated airspeed)
+	// however since airspeed is updated less often than groundspeed, we use sudden changes to groundspeed to offset the airspeed by the same measurement.
+
+}

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
@@ -138,6 +138,18 @@ int32_t GPSInitialize(void)
 		gpsEnabled = false;
 #endif
 
+#if defined(REVOLUTION)
+	// These objects MUST be initialized for Revolution
+	// because the rest of the system expects to just
+	// attach to their queues
+	GPSPositionInitialize();
+	GPSVelocityInitialize();
+	GPSTimeInitialize();
+	GPSSatellitesInitialize();
+	HomeLocationInitialize();
+	updateSettings();
+
+#else
 	if (gpsPort && gpsEnabled) {
 		GPSPositionInitialize();
 		GPSVelocityInitialize();
@@ -150,6 +162,7 @@ int32_t GPSInitialize(void)
 #endif
 		updateSettings();
 	}
+#endif
 
 	if (gpsPort && gpsEnabled) {
 		SystemSettingsInitialize();

flight/Modules/Guidance/guidance.c

@@ -1,448 +0,0 @@
-/**
- ******************************************************************************
- *
- * @file       guidance.c
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      This module compared @ref PositionActuatl to @ref ActiveWaypoint 
- * and sets @ref AttitudeDesired.  It only does this when the FlightMode field
- * of @ref ManualControlCommand is Auto.
- *
- * @see        The GNU Public License (GPL) Version 3
- *
- *****************************************************************************/
-/*
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
- * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
- * for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- */
-
-/**
- * Input object: ActiveWaypoint
- * Input object: PositionActual
- * Input object: ManualControlCommand
- * Output object: AttitudeDesired
- *
- * This module will periodically update the value of the AttitudeDesired object.
- *
- * The module executes in its own thread in this example.
- *
- * Modules have no API, all communication to other modules is done through UAVObjects.
- * However modules may use the API exposed by shared libraries.
- * See the OpenPilot wiki for more details.
- * http://www.openpilot.org/OpenPilot_Application_Architecture
- *
- */
-
-#include "openpilot.h"
-#include "guidance.h"
-#include "guidancesettings.h"
-#include "attituderaw.h"
-#include "attitudeactual.h"
-#include "positiondesired.h"	// object that will be updated by the module
-#include "positionactual.h"
-#include "manualcontrol.h"
-#include "flightstatus.h"
-#include "nedaccel.h"
-#include "stabilizationdesired.h"
-#include "stabilizationsettings.h"
-#include "systemsettings.h"
-#include "velocitydesired.h"
-#include "velocityactual.h"
-#include "CoordinateConversions.h"
-
-// Private constants
-#define MAX_QUEUE_SIZE 1
-#define STACK_SIZE_BYTES 1024
-#define TASK_PRIORITY (tskIDLE_PRIORITY+2)
-// Private types
-
-// Private variables
-static xTaskHandle guidanceTaskHandle;
-static xQueueHandle queue;
-
-// Private functions
-static void guidanceTask(void *parameters);
-static float bound(float val, float min, float max);
-
-static void updateVtolDesiredVelocity();
-static void manualSetDesiredVelocity();
-static void updateVtolDesiredAttitude();
-
-/**
- * Initialise the module, called on startup
- * \returns 0 on success or -1 if initialisation failed
- */
-int32_t GuidanceStart()
-{
-	// Start main task
-	xTaskCreate(guidanceTask, (signed char *)"Guidance", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &guidanceTaskHandle);
-	TaskMonitorAdd(TASKINFO_RUNNING_GUIDANCE, guidanceTaskHandle);
-
-	return 0;
-}
-
-/**
- * Initialise the module, called on startup
- * \returns 0 on success or -1 if initialisation failed
- */
-int32_t GuidanceInitialize()
-{
-
-	GuidanceSettingsInitialize();
-	PositionDesiredInitialize();
-	NedAccelInitialize();
-	VelocityDesiredInitialize();
-
-	// Create object queue
-	queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
-	
-	// Listen for updates.
-	AttitudeRawConnectQueue(queue);
-	
-	return 0;
-}
-MODULE_INITCALL(GuidanceInitialize, GuidanceStart)
-
-static float northVelIntegral = 0;
-static float eastVelIntegral = 0;
-static float downVelIntegral = 0;
-
-static float northPosIntegral = 0;
-static float eastPosIntegral = 0;
-static float downPosIntegral = 0;
-
-static uint8_t positionHoldLast = 0;
-
-/**
- * Module thread, should not return.
- */
-static void guidanceTask(void *parameters)
-{
-	SystemSettingsData systemSettings;
-	GuidanceSettingsData guidanceSettings;
-	FlightStatusData flightStatus;
-
-	portTickType thisTime;
-	portTickType lastUpdateTime;
-	UAVObjEvent ev;
-	
-	float accel[3] = {0,0,0};
-	uint32_t accel_accum = 0;
-	
-	float q[4];
-	float Rbe[3][3];
-	float accel_ned[3];
-	
-	// Main task loop
-	lastUpdateTime = xTaskGetTickCount();
-	while (1) {
-		GuidanceSettingsGet(&guidanceSettings);
-
-		// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
-		if ( xQueueReceive(queue, &ev, guidanceSettings.UpdatePeriod / portTICK_RATE_MS) != pdTRUE )
-		{
-			AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING);
-		} else {
-			AlarmsClear(SYSTEMALARMS_ALARM_GUIDANCE);
-		}
-				
-		// Collect downsampled attitude data
-		AttitudeRawData attitudeRaw;
-		AttitudeRawGet(&attitudeRaw);		
-		accel[0] += attitudeRaw.accels[0];
-		accel[1] += attitudeRaw.accels[1];
-		accel[2] += attitudeRaw.accels[2];
-		accel_accum++;
-		
-		// Continue collecting data if not enough time
-		thisTime = xTaskGetTickCount();
-		if( (thisTime - lastUpdateTime) < (guidanceSettings.UpdatePeriod / portTICK_RATE_MS) )
-			continue;
-		
-		lastUpdateTime = xTaskGetTickCount();
-		accel[0] /= accel_accum;
-		accel[1] /= accel_accum;
-		accel[2] /= accel_accum;
-		
-		//rotate avg accels into earth frame and store it
-		AttitudeActualData attitudeActual;
-		AttitudeActualGet(&attitudeActual);
-		q[0]=attitudeActual.q1;
-		q[1]=attitudeActual.q2;
-		q[2]=attitudeActual.q3;
-		q[3]=attitudeActual.q4;
-		Quaternion2R(q, Rbe);
-		for (uint8_t i=0; i<3; i++){
-			accel_ned[i]=0;
-			for (uint8_t j=0; j<3; j++)
-				accel_ned[i] += Rbe[j][i]*accel[j];
-		}
-		accel_ned[2] += 9.81;
-		
-		NedAccelData accelData;
-		NedAccelGet(&accelData);
-		// Convert from m/s to cm/s
-		accelData.North = accel_ned[0];
-		accelData.East = accel_ned[1];
-		accelData.Down = accel_ned[2];
-		NedAccelSet(&accelData);
-		
-		
-		FlightStatusGet(&flightStatus);
-		SystemSettingsGet(&systemSettings);
-		GuidanceSettingsGet(&guidanceSettings);
-		
-		if ((PARSE_FLIGHT_MODE(flightStatus.FlightMode) == FLIGHTMODE_GUIDANCE) &&
-		    ((systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) ||
-		     (systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) ||
-		     (systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADX) ||
-		     (systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) ))
-		{
-			if(positionHoldLast == 0) {
-				/* When enter position hold mode save current position */
-				PositionDesiredData positionDesired;
-				PositionActualData positionActual;
-				PositionDesiredGet(&positionDesired);
-				PositionActualGet(&positionActual);
-				positionDesired.North = positionActual.North;
-				positionDesired.East = positionActual.East;
-				PositionDesiredSet(&positionDesired);
-				positionHoldLast = 1;
-			}
-			
-			if( flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ) 
-				updateVtolDesiredVelocity();
-			else
-				manualSetDesiredVelocity();			
-			updateVtolDesiredAttitude();
-			
-		} else {
-			// Be cleaner and get rid of global variables
-			northVelIntegral = 0;
-			eastVelIntegral = 0;
-			downVelIntegral = 0;
-			northPosIntegral = 0;
-			eastPosIntegral = 0;
-			downPosIntegral = 0;
-			positionHoldLast = 0;
-		}
-		
-		accel[0] = accel[1] = accel[2] = 0;
-		accel_accum = 0;
-	}
-}
-
-/**
- * Compute desired velocity from the current position
- *
- * Takes in @ref PositionActual and compares it to @ref PositionDesired 
- * and computes @ref VelocityDesired
- */
-void updateVtolDesiredVelocity()
-{
-	static portTickType lastSysTime;
-	portTickType thisSysTime = xTaskGetTickCount();;
-	float dT = 0;
-
-	GuidanceSettingsData guidanceSettings;
-	PositionActualData positionActual;
-	PositionDesiredData positionDesired;
-	VelocityDesiredData velocityDesired;
-	
-	GuidanceSettingsGet(&guidanceSettings);
-	PositionActualGet(&positionActual);
-	PositionDesiredGet(&positionDesired);
-	VelocityDesiredGet(&velocityDesired);
-	
-	float northError;
-	float eastError;
-	float downError;
-	float northCommand;
-	float eastCommand;
-	float downCommand;
-	
-
-	// Check how long since last update
-	if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
-		dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;		
-	lastSysTime = thisSysTime;
-	
-	// Note all distances in cm
-	// Compute desired north command
-	northError = positionDesired.North - positionActual.North;
-	northPosIntegral = bound(northPosIntegral + northError * dT * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KI], 
-			      -guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT],
-			      guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT]);
-	northCommand = (northError * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KP] +
-			northPosIntegral);
-	
-	eastError = positionDesired.East - positionActual.East;
-	eastPosIntegral = bound(eastPosIntegral + eastError * dT * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KI], 
-				 -guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT],
-				 guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT]);
-	eastCommand = (eastError * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KP] +
-		       eastPosIntegral);
-	
-	
-	velocityDesired.North = bound(northCommand,-guidanceSettings.HorizontalVelMax,guidanceSettings.HorizontalVelMax);
-	velocityDesired.East = bound(eastCommand,-guidanceSettings.HorizontalVelMax,guidanceSettings.HorizontalVelMax);
-
-	downError = positionDesired.Down - positionActual.Down;
-	downPosIntegral = bound(downPosIntegral + downError * dT * guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_KI], 
-				-guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_ILIMIT],
-				guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_ILIMIT]);
-	downCommand = (downError * guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_KP] + downPosIntegral);
-	velocityDesired.Down = bound(downCommand,
-				     -guidanceSettings.VerticalVelMax, 
-				     guidanceSettings.VerticalVelMax);
-	
-	VelocityDesiredSet(&velocityDesired);	
-}
-
-/**
- * Compute desired attitude from the desired velocity
- *
- * Takes in @ref NedActual which has the acceleration in the 
- * NED frame as the feedback term and then compares the 
- * @ref VelocityActual against the @ref VelocityDesired
- */
-static void updateVtolDesiredAttitude()
-{
-	static portTickType lastSysTime;
-	portTickType thisSysTime = xTaskGetTickCount();;
-	float dT = 0;
-
-	VelocityDesiredData velocityDesired;
-	VelocityActualData velocityActual;
-	StabilizationDesiredData stabDesired;
-	AttitudeActualData attitudeActual;
-	NedAccelData nedAccel;
-	GuidanceSettingsData guidanceSettings;
-	StabilizationSettingsData stabSettings;
-	SystemSettingsData systemSettings;
-
-	float northError;
-	float northCommand;
-	
-	float eastError;
-	float eastCommand;
-
-	float downError;
-	float downCommand;
-	
-	// Check how long since last update
-	if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
-		dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;		
-	lastSysTime = thisSysTime;
-	
-	SystemSettingsGet(&systemSettings);
-	GuidanceSettingsGet(&guidanceSettings);
-	
-	VelocityActualGet(&velocityActual);
-	VelocityDesiredGet(&velocityDesired);
-	StabilizationDesiredGet(&stabDesired);
-	VelocityDesiredGet(&velocityDesired);
-	AttitudeActualGet(&attitudeActual);
-	StabilizationSettingsGet(&stabSettings);
-	NedAccelGet(&nedAccel);
-	
-	// Testing code - refactor into manual control command
-	ManualControlCommandData manualControlData;
-	ManualControlCommandGet(&manualControlData);
-	stabDesired.Yaw = stabSettings.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_YAW] * manualControlData.Yaw;	
-	
-	// Compute desired north command
-	northError = velocityDesired.North - velocityActual.North;
-	northVelIntegral = bound(northVelIntegral + northError * dT * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KI], 
-			      -guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT],
-			      guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT]);
-	northCommand = (northError * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KP] +
-			northVelIntegral -
-			nedAccel.North * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KD]);
-	
-	// Compute desired east command
-	eastError = velocityDesired.East - velocityActual.East;
-	eastVelIntegral = bound(eastVelIntegral + eastError * dT * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KI], 
-			     -guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT],
-			     guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT]);
-	eastCommand = (eastError * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KP] + 
-		       eastVelIntegral - 
-		       nedAccel.East * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KD]);
-	
-	// Compute desired down command
-	downError = velocityDesired.Down - velocityActual.Down;
-	downVelIntegral = bound(downVelIntegral + downError * dT * guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KI], 
-			      -guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_ILIMIT],
-			      guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_ILIMIT]);	
-	downCommand = (downError * guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KP] +
-		       downVelIntegral -
-		       nedAccel.Down * guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KD]);
-	
-	stabDesired.Throttle = bound(downCommand, 0, 1);
-	
-	// Project the north and east command signals into the pitch and roll based on yaw.  For this to behave well the
-	// craft should move similarly for 5 deg roll versus 5 deg pitch
-	stabDesired.Pitch = bound(-northCommand * cosf(attitudeActual.Yaw * M_PI / 180) + 
-				      -eastCommand * sinf(attitudeActual.Yaw * M_PI / 180),
-				      -guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch);
-	stabDesired.Roll = bound(-northCommand * sinf(attitudeActual.Yaw * M_PI / 180) + 
-				     eastCommand * cosf(attitudeActual.Yaw * M_PI / 180),
-				     -guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch);
-	
-	if(guidanceSettings.ThrottleControl == GUIDANCESETTINGS_THROTTLECONTROL_FALSE) {
-		// For now override throttle with manual control.  Disable at your risk, quad goes to China.
-		ManualControlCommandData manualControl;
-		ManualControlCommandGet(&manualControl);
-		stabDesired.Throttle = manualControl.Throttle;
-	}
-	
-	stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
-	stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
-	stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
-	
-	StabilizationDesiredSet(&stabDesired);
-}
-
-/**
- * Set the desired velocity from the input sticks
- */
-static void manualSetDesiredVelocity() 
-{
-	ManualControlCommandData cmd;
-	VelocityDesiredData velocityDesired;
-	
-	ManualControlCommandGet(&cmd);
-	VelocityDesiredGet(&velocityDesired);
-
-	GuidanceSettingsData guidanceSettings;
-	GuidanceSettingsGet(&guidanceSettings);
-	
-	velocityDesired.North = -guidanceSettings.HorizontalVelMax * cmd.Pitch;
-	velocityDesired.East = guidanceSettings.HorizontalVelMax * cmd.Roll;
-	velocityDesired.Down = 0;
-	
-	VelocityDesiredSet(&velocityDesired);	
-}
-
-/**
- * Bound input value between limits
- */
-static float bound(float val, float min, float max)
-{
-	if (val < min) {
-		val = min;
-	} else if (val > max) {
-		val = max;
-	}
-	return val;
-}