Merge branch 'next' into dwillis/OP-792

Conflicts:
	shared/uavobjectdefinition/manualcontrolsettings.xml

Merge next

.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)
@@ -443,7 +445,7 @@ dfuutil_clean:
 # see http://developer.android.com/sdk/ for latest versions
 ANDROID_SDK_DIR := $(TOOLS_DIR)/android-sdk-linux
 .PHONY: android_sdk_install
-android_sdk_install: ANDROID_SDK_URL  := http://dl.google.com/android/android-sdk_r20.0.3-linux.tgz
+android_sdk_install: ANDROID_SDK_URL  := http://dl.google.com/android/android-sdk_r21.0.1-linux.tgz
 android_sdk_install: ANDROID_SDK_FILE := $(notdir $(ANDROID_SDK_URL))
 # order-only prereq on directory existance:
 android_sdk_install: | $(DL_DIR) $(TOOLS_DIR)
@@ -464,7 +466,7 @@ android_sdk_clean:
 .PHONY: android_sdk_update
 android_sdk_update:
 	$(V0) @echo " UPDATE       $(ANDROID_SDK_DIR)"
-	$(ANDROID_SDK_DIR)/tools/android update sdk --no-ui -t platform-tools,android-16,addon-google_apis-google-16
+	$(ANDROID_SDK_DIR)/tools/android update sdk --no-ui -t platform-tools,android-14,addon-google_apis-google-14
 
 ##############################
 #
@@ -577,6 +579,7 @@ uavobjects_clean:
 #
 ################################
 
+ANDROIDGCS_BUILD_CONF ?= debug
 
 # Build the output directory for the Android GCS build
 ANDROIDGCS_OUT_DIR := $(BUILD_DIR)/androidgcs
@@ -599,12 +602,12 @@ endif
 androidgcs: uavo-collections_java
 	$(V0) @echo " ANDROID   $(call toprel, $(ANDROIDGCS_OUT_DIR))"
 	$(V1) mkdir -p $(ANDROIDGCS_OUT_DIR)
-	$(V1) $(ANDROID) $(ANDROID_SILENT) update project --target 'Google Inc.:Google APIs:16' --name androidgcs --path ./androidgcs
+	$(V1) $(ANDROID) $(ANDROID_SILENT) update project --target 'Google Inc.:Google APIs:14' --name androidgcs --path ./androidgcs
 	$(V1) ant -f ./androidgcs/build.xml \
 		$(ANT_QUIET) \
 		-Dout.dir="../$(call toprel, $(ANDROIDGCS_OUT_DIR)/bin)" \
 		-Dgen.absolute.dir="$(ANDROIDGCS_OUT_DIR)/gen" \
-		debug
+		$(ANDROIDGCS_BUILD_CONF)
 
 .PHONY: androidgcs_clean
 androidgcs_clean:
@@ -616,7 +619,7 @@ androidgcs_clean:
 #
 # Find the git hashes of each commit that changes uavobjects with:
 #   git log --format=%h -- shared/uavobjectdefinition/ | head -n 2
-UAVO_GIT_VERSIONS := 684620d 43f85d9
+UAVO_GIT_VERSIONS := 5e14f53
 
 # All versions includes a pseudo collection called "working" which represents
 # the UAVOs in the source tree
@@ -861,43 +864,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 +961,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
@@ -967,3 +981,12 @@ package:
 .PHONY: package_resources
 package_resources:
 	$(V1) cd package && $(MAKE) --no-print-directory opfw_resource
+
+.PHONY: build-info
+build-info:
+	$(V1) mkdir -p $(BUILD_DIR)
+	$(V1) python $(ROOT_DIR)/make/scripts/version-info.py \
+		--path=$(ROOT_DIR) \
+		--uavodir=$(ROOT_DIR)/shared/uavobjectdefinition \
+		--template="make/templates/$@.txt" \
+		--outfile="$(BUILD_DIR)/$@.txt"

androidgcs/AndroidManifest.xml

@@ -1,50 +1,83 @@
 <?xml version="1.0" encoding="utf-8"?>
 <manifest xmlns:android="http://schemas.android.com/apk/res/android"
-	package="org.openpilot.androidgcs" android:versionCode="1"
-	android:versionName="1.0">
-	<uses-sdk android:minSdkVersion="14" />
+    package="org.openpilot.androidgcs"
+    android:versionCode="1"
+    android:versionName="1.0" >
+
+    <uses-sdk android:minSdkVersion="14" android:targetSdkVersion="17"/>
 
     <uses-permission android:name="android.permission.INTERNET" />
-
     <uses-permission android:name="android.permission.BLUETOOTH" />
     <uses-permission android:name="android.permission.BLUETOOTH_ADMIN" />
     <uses-permission android:name="android.permission.ACCESS_FINE_LOCATION" />
-	<uses-permission android:name="android.permission.WRITE_EXTERNAL_STORAGE"/>  
+    <uses-permission android:name="android.permission.WRITE_EXTERNAL_STORAGE" />
+
     <uses-feature android:name="android.hardware.usb.host" />
 
-	<application android:icon="@drawable/ic_logo" android:label="@string/app_name" android:theme="@android:style/Theme.Holo">
+    <application
+        android:icon="@drawable/ic_logo"
+        android:label="@string/app_name"
+        android:theme="@android:style/Theme.Holo" 
+        android:allowBackup="false">
+
         <!-- for map overlay -->
         <uses-library android:name="com.google.android.maps" />
 
         <!-- Object browser - main activity at the moment -->
-		<activity android:name="HomePage" android:label="@string/app_name">
-		    
+        <activity
+            android:name="HomePage"
+            android:label="@string/app_name" >
             <intent-filter>
                 <action android:name="android.intent.action.MAIN" />
+
                 <category android:name="android.intent.category.LAUNCHER" />
             </intent-filter>
 
-<!--  			<intent-filter> -->
-<!--                 <action android:name="android.hardware.usb.action.USB_DEVICE_ATTACHED" /> -->
-<!--             </intent-filter> -->
+            <!-- <intent-filter> -->
+            <!-- <action android:name="android.hardware.usb.action.USB_DEVICE_ATTACHED" /> -->
+            <!-- </intent-filter> -->
 
-<!--             <meta-data android:name="android.hardware.usb.action.USB_DEVICE_ATTACHED" -->
-<!--                 android:resource="@xml/device_filter" /> -->
+
+            <!-- <meta-data android:name="android.hardware.usb.action.USB_DEVICE_ATTACHED" -->
+            <!-- android:resource="@xml/device_filter" /> -->
         </activity>
-
-		<activity android:name="ObjectBrowser" android:label="@string/object_browser_name" />
-		<activity android:name="PfdActivity" android:label="PFD" />
-		<activity android:name="Controller" android:label="@string/controller_name" />
-		<activity android:name="Preferences" android:label="@string/preference_title" />
-		<activity android:name="UAVLocation" android:label="@string/location_name" />
-		<activity android:name="SystemAlarmActivity" android:label="System Alarms" />
-		<activity android:name="TuningActivity" android:label="Tuning" />
-				<activity android:name="ObjectEditor" android:label="ObjectEditor"
+        <activity
+            android:name="ObjectBrowser"
+            android:label="@string/object_browser_name" />
+        <activity
+            android:name="PfdActivity"
+            android:label="PFD" />
+        <activity
+            android:name="Controller"
+            android:label="@string/controller_name" />
+        <activity
+            android:name="Preferences"
+            android:label="@string/preference_title" />
+        <activity
+            android:name="UAVLocation"
+            android:label="@string/location_name" />
+        <activity
+            android:name="SystemAlarmActivity"
+            android:label="System Alarms" />
+        <activity
+            android:name="TuningActivity"
+            android:label="Tuning" />
+        <activity
+            android:name="ObjectEditor"
+            android:label="ObjectEditor"
             android:theme="@android:style/Theme.Dialog" />
-		<activity android:name="Logger" android:label="Logger"
+        <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">
+        <receiver android:name="TelemetryWidget" >
             <intent-filter>
                 <action android:name="android.appwidget.action.APPWIDGET_UPDATE" />
             </intent-filter>
@@ -52,10 +85,14 @@
                 <action android:name="org.openpilot.intent.action.CONNECTED" />
                 <action android:name="org.openpilot.intent.action.DISCONNECTED" />
             </intent-filter>
-			<meta-data android:name="android.appwidget.provider"
+
+            <meta-data
+                android:name="android.appwidget.provider"
                 android:resource="@xml/telemetry_widget_info" />
         </receiver>
 
-		<service android:name="org.openpilot.androidgcs.telemetry.OPTelemetryService"></service>
+        <service android:name="org.openpilot.androidgcs.telemetry.OPTelemetryService" >
+        </service>
     </application>
+
 </manifest>

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

@@ -77,6 +77,29 @@
         android:drawableTop="@drawable/ic_alarms"
         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"

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>

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

@@ -65,6 +65,8 @@ public class HidUAVTalk extends TelemetryTask {
 	private boolean readPending = false;
 	private boolean writePending = false;
 	private IntentFilter deviceAttachedFilter;
+	private boolean usbReceiverRegistered = false;
+	private boolean usbPermissionReceiverRegistered = false;
 
 	public HidUAVTalk(OPTelemetryService service) {
 		super(service);
@@ -74,8 +76,14 @@ public class HidUAVTalk extends TelemetryTask {
 	public void disconnect() {
 
 		CleanUpAndClose();
+		if(usbReceiverRegistered){
 			telemService.unregisterReceiver(usbReceiver);
+			usbReceiverRegistered = false;
+		}
+		if(usbPermissionReceiverRegistered){
 			telemService.unregisterReceiver(usbPermissionReceiver);
+			usbPermissionReceiverRegistered = false;
+		}
 
 		super.disconnect();
 
@@ -110,11 +118,13 @@ public class HidUAVTalk extends TelemetryTask {
 		permissionIntent = PendingIntent.getBroadcast(telemService, 0, new Intent(ACTION_USB_PERMISSION), 0);
 		permissionFilter = new IntentFilter(ACTION_USB_PERMISSION);
 		telemService.registerReceiver(usbPermissionReceiver, permissionFilter);
+		usbPermissionReceiverRegistered = true;
 
 		deviceAttachedFilter = new IntentFilter();
 		deviceAttachedFilter.addAction(UsbManager.ACTION_USB_DEVICE_ATTACHED);
 		deviceAttachedFilter.addAction(UsbManager.ACTION_USB_DEVICE_DETACHED);
 		telemService.registerReceiver(usbReceiver, deviceAttachedFilter);
+		usbReceiverRegistered = true;
 
 		// Go through all the devices plugged in
 		HashMap<String, UsbDevice> deviceList = usbManager.getDeviceList();
@@ -123,7 +133,10 @@ public class HidUAVTalk extends TelemetryTask {
 		while(deviceIterator.hasNext()){
 			UsbDevice dev = deviceIterator.next();
 			if (DEBUG) Log.d(TAG, "Testing device: " + dev);
+			if( ValidateFoundDevice(dev) ) {
 				usbManager.requestPermission(dev, permissionIntent);
+				break;
+			}
 		}
 
 		if (DEBUG) Log.d(TAG, "Registered the deviceAttachedFilter");
@@ -254,6 +267,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/androidgcs/telemetry/OPTelemetryService.java

@@ -55,6 +55,8 @@ import android.widget.Toast;
 import dalvik.system.DexClassLoader;
 
 public class OPTelemetryService extends Service {
+	// Track lifecycle
+	private static int _startupRequests = 0;
 
 	// Logging settings
 	private final String TAG = OPTelemetryService.class.getSimpleName();
@@ -66,6 +68,7 @@ public class OPTelemetryService extends Service {
 	public final static String INTENT_CATEGORY_GCS        = "org.openpilot.intent.category.GCS";
 
 	// Intent actions
+	public final static String INTENT_ACTION_TELEMETRYTASK_STARTED = "org.openpilot.intent.action.TELEMETRYTASK_STARTED";
 	public final static String INTENT_ACTION_CONNECTED    = "org.openpilot.intent.action.CONNECTED";
 	public final static String INTENT_ACTION_DISCONNECTED = "org.openpilot.intent.action.DISCONNECTED";
 
@@ -139,6 +142,9 @@ public class OPTelemetryService extends Service {
 				throw new Error("Unsupported");
 			}
 			activeTelem.start();
+			Intent startedIntent = new Intent();
+			startedIntent.setAction(OPTelemetryService.INTENT_ACTION_TELEMETRYTASK_STARTED);
+			sendBroadcast(startedIntent,null);
 			break;
 		case MSG_DISCONNECT:
 			Toast.makeText(getApplicationContext(), "Disconnect requested", Toast.LENGTH_SHORT).show();
@@ -184,6 +190,9 @@ public class OPTelemetryService extends Service {
 	 * and based on the stored preference will send itself a connect signal if needed.
 	 */
 	public void startup() {
+		synchronized (this) {
+			_startupRequests++;
+		}
 		Toast.makeText(getApplicationContext(), "Telemetry service starting", Toast.LENGTH_SHORT).show();
 
 		thread = new HandlerThread("TelemetryServiceHandler", Process.THREAD_PRIORITY_BACKGROUND);
@@ -232,11 +241,20 @@ public class OPTelemetryService extends Service {
 			telemTask = null;
 
 			try {
+				// Race condition - if we shut the service down before the telemetry task
+				// thread has started, this will hang so we need to check thread is runnable.
+				if(activeTelem.getState() == Thread.State.RUNNABLE){
 					activeTelem.join();
+				}else{
+					Log.d(TAG, "onDestroy() shut down telemetry task before it has started");
+				}
 			} catch (InterruptedException e) {
 				e.printStackTrace();
 			}
 		}
+		synchronized (this) {
+			_startupRequests = 0;
+		}
 		Log.d(TAG, "onDestory() shut down telemetry task");
 		Toast.makeText(this, "Telemetry service done", Toast.LENGTH_SHORT).show();
 	}
@@ -247,6 +265,9 @@ public class OPTelemetryService extends Service {
 				return telemTask.getTelemTaskIface();
 			return null;
 		}
+		public TelemetryTask getTelemetryTask(int id){
+			return telemTask;
+		}
 		public void openConnection() {
 			Toast.makeText(getApplicationContext(), "Requested open connection", Toast.LENGTH_SHORT).show();
 			Message msg = mServiceHandler.obtainMessage();
@@ -410,4 +431,8 @@ public class OPTelemetryService extends Service {
 
 		return true;
 	}
+
+	public static int getNumStartupRequests() {
+		return _startupRequests;
+	}
 }

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

@@ -142,12 +142,16 @@ public abstract class TelemetryTask implements Runnable {
 		}
 
 		// Stop the master telemetry thread
+		// Check handler is not null: if we attempt to disconnect before
+		// the connect process has completed, handler may be null.
+		if(handler != null){
 			handler.post(new Runnable() {
 				@Override
 				public void run() {
 					Looper.myLooper().quit();
 				}
 			});
+		}
 
 		if (inputProcessThread != null) {
 			inputProcessThread.interrupt();

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))) {

androidgcstests/.classpath

@@ -0,0 +1,9 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<classpath>
+	<classpathentry kind="src" path="src"/>
+	<classpathentry kind="src" path="gen"/>
+	<classpathentry kind="con" path="com.android.ide.eclipse.adt.ANDROID_FRAMEWORK"/>
+	<classpathentry kind="con" path="com.android.ide.eclipse.adt.LIBRARIES"/>
+	<classpathentry combineaccessrules="false" kind="src" path="/androidgcs"/>
+	<classpathentry kind="output" path="bin/classes"/>
+</classpath>

androidgcstests/.project

@@ -0,0 +1,33 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<projectDescription>
+	<name>androidgcstests</name>
+	<comment></comment>
+	<projects>
+	</projects>
+	<buildSpec>
+		<buildCommand>
+			<name>com.android.ide.eclipse.adt.ResourceManagerBuilder</name>
+			<arguments>
+			</arguments>
+		</buildCommand>
+		<buildCommand>
+			<name>com.android.ide.eclipse.adt.PreCompilerBuilder</name>
+			<arguments>
+			</arguments>
+		</buildCommand>
+		<buildCommand>
+			<name>org.eclipse.jdt.core.javabuilder</name>
+			<arguments>
+			</arguments>
+		</buildCommand>
+		<buildCommand>
+			<name>com.android.ide.eclipse.adt.ApkBuilder</name>
+			<arguments>
+			</arguments>
+		</buildCommand>
+	</buildSpec>
+	<natures>
+		<nature>com.android.ide.eclipse.adt.AndroidNature</nature>
+		<nature>org.eclipse.jdt.core.javanature</nature>
+	</natures>
+</projectDescription>

androidgcstests/AndroidManifest.xml

@@ -0,0 +1,19 @@
+<?xml version="1.0" encoding="utf-8"?>
+<manifest xmlns:android="http://schemas.android.com/apk/res/android"
+    package="org.openpilot.androidgcs.test"
+    android:versionCode="1"
+    android:versionName="1.0" >
+
+    <uses-sdk android:minSdkVersion="14" />
+
+    <instrumentation
+        android:name="android.test.InstrumentationTestRunner"
+        android:targetPackage="org.openpilot.androidgcs" />
+
+    <application
+        android:icon="@drawable/ic_launcher"
+        android:label="@string/app_name" >
+        <uses-library android:name="android.test.runner" />
+    </application>
+
+</manifest>

androidgcstests/proguard-project.txt

@@ -0,0 +1,20 @@
+# To enable ProGuard in your project, edit project.properties
+# to define the proguard.config property as described in that file.
+#
+# Add project specific ProGuard rules here.
+# By default, the flags in this file are appended to flags specified
+# in ${sdk.dir}/tools/proguard/proguard-android.txt
+# You can edit the include path and order by changing the ProGuard
+# include property in project.properties.
+#
+# For more details, see
+#   http://developer.android.com/guide/developing/tools/proguard.html
+
+# Add any project specific keep options here:
+
+# If your project uses WebView with JS, uncomment the following
+# and specify the fully qualified class name to the JavaScript interface
+# class:
+#-keepclassmembers class fqcn.of.javascript.interface.for.webview {
+#   public *;
+#}

androidgcstests/project.properties

@@ -0,0 +1,14 @@
+# This file is automatically generated by Android Tools.
+# Do not modify this file -- YOUR CHANGES WILL BE ERASED!
+#
+# This file must be checked in Version Control Systems.
+#
+# To customize properties used by the Ant build system edit
+# "ant.properties", and override values to adapt the script to your
+# project structure.
+#
+# To enable ProGuard to shrink and obfuscate your code, uncomment this (available properties: sdk.dir, user.home):
+#proguard.config=${sdk.dir}/tools/proguard/proguard-android.txt:proguard-project.txt
+
+# Project target.
+target=android-14

androidgcstests/res/values/strings.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="utf-8"?>
+<resources>
+
+    <string name="app_name">AndroidGCSTestTest</string>
+
+</resources>

androidgcstests/src/org/openpilot/androidgcs/test/telemetryservice/OPTelemetryServiceTests.java

@@ -0,0 +1,207 @@
+/**
+ ******************************************************************************
+ * @file       OPTelemetryServiceTests.java
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
+ * @brief      Tests for the OPTelemetryService class
+ * @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.test.telemetryservice;
+
+import org.openpilot.androidgcs.telemetry.HidUAVTalk;
+import org.openpilot.androidgcs.telemetry.TcpUAVTalk;
+import org.openpilot.androidgcs.telemetry.BluetoothUAVTalk;
+import org.openpilot.androidgcs.telemetry.OPTelemetryService;
+import org.openpilot.androidgcs.telemetry.OPTelemetryService.LocalBinder;
+import android.content.BroadcastReceiver;
+import android.content.Context;
+import android.content.Intent;
+import android.content.IntentFilter;
+import android.content.SharedPreferences;
+import android.preference.PreferenceManager;
+import android.test.ServiceTestCase;
+import android.test.mock.MockApplication;
+import android.test.mock.MockContext;
+
+public class OPTelemetryServiceTests extends
+		ServiceTestCase<OPTelemetryService> {
+
+	private static final int PREF_BLUETOOTH_CONN = 2;
+	private static final int PREF_TCP_CONN = 3;
+	private static final int PREF_USB_CONN = 4;
+	private Object _syncTelemetryTaskStarted = new Object();
+	private Object _syncTelemetryConnected = new Object();
+	private BroadcastReceiver _connectBroadcastReceiver;
+
+	public class MockAndroidGCSApplication extends MockApplication {
+
+	}
+
+	class MockAndroidGCSContext extends MockContext {
+
+	}
+	
+	public OPTelemetryServiceTests() {
+		this(OPTelemetryService.class);
+	}
+	
+	public OPTelemetryServiceTests(Class<OPTelemetryService> serviceClass) {
+		super(serviceClass);
+		_connectBroadcastReceiver = new BroadcastReceiver(){
+
+			@Override
+			public void onReceive(Context context, Intent intent) {
+				if(intent.getAction().equals(OPTelemetryService.INTENT_ACTION_CONNECTED)) {
+					if(_syncTelemetryConnected != null){
+						synchronized (_syncTelemetryConnected) {
+							_syncTelemetryConnected.notify();
+						}
+					}
+				}else if(intent.getAction().equals(OPTelemetryService.INTENT_ACTION_TELEMETRYTASK_STARTED)) {
+					// Not looked into why _syncTelemetryTaskStarted could possibly be null
+					// here but sometimes it is...
+					if(_syncTelemetryTaskStarted != null){
+						synchronized (_syncTelemetryTaskStarted) {
+							_syncTelemetryTaskStarted.notify();
+						}
+					}
+				}
+			}
+			
+		};
+	}
+
+	@Override
+	protected void setUp() throws Exception {
+		super.setUp();
+		
+		IntentFilter filter = new IntentFilter();
+		filter.addCategory(OPTelemetryService.INTENT_CATEGORY_GCS);
+		filter.addAction(OPTelemetryService.INTENT_ACTION_CONNECTED);
+		filter.addAction(OPTelemetryService.INTENT_ACTION_TELEMETRYTASK_STARTED);
+		
+		getContext().registerReceiver(_connectBroadcastReceiver, filter);
+		
+//		setApplication(new MockAndroidGCSApplication());
+//		setContext(new MockAndroidGCSContext());
+	}
+
+	@Override
+	protected void tearDown() throws Exception {
+		super.tearDown();
+//		getContext().unregisterReceiver(_connectBroadcastReceiver);
+	}
+	
+	public void testLifecycleSingleStart(){
+		bindService(new Intent(getContext(),
+				org.openpilot.androidgcs.telemetry.OPTelemetryService.class));
+		
+		assertEquals(1, OPTelemetryService.getNumStartupRequests());
+		
+		shutdownService();
+		
+		assertEquals(0, OPTelemetryService.getNumStartupRequests());
+	}
+	
+	public void testLifecycleMultiStart(){
+		bindService(new Intent(getContext(),
+				org.openpilot.androidgcs.telemetry.OPTelemetryService.class));
+		bindService(new Intent(getContext(),
+				org.openpilot.androidgcs.telemetry.OPTelemetryService.class));
+		
+		assertEquals(1, OPTelemetryService.getNumStartupRequests());
+		
+		shutdownService();
+		
+		assertEquals(0, OPTelemetryService.getNumStartupRequests());
+	}
+
+	public void testStartBluetoothTelemetryTask(){
+		LocalBinder binder = startTelemetryTask(Integer.valueOf(PREF_BLUETOOTH_CONN));
+		
+		assertTrue("Started wrong telemetry service type", binder.getTelemetryTask(0) instanceof BluetoothUAVTalk);
+	}
+
+	public void testStartBluetoothConnection() throws InterruptedException{
+		LocalBinder binder = startTelemetryTask(Integer.valueOf(PREF_BLUETOOTH_CONN));
+		
+		synchronized (_syncTelemetryConnected) {
+			_syncTelemetryConnected.wait(2000);
+		}
+		
+		assertTrue("Failed to connect to telemetry service", binder.isConnected());
+		
+		assertTrue("Started wrong telemetry service type", binder.getTelemetryTask(0) instanceof BluetoothUAVTalk);
+	}
+	
+	public void testStartTCPTelemetryTask(){
+		LocalBinder binder = startTelemetryTask(Integer.valueOf(PREF_TCP_CONN));
+		
+		assertTrue("Started wrong telemetry service type", binder.getTelemetryTask(0) instanceof TcpUAVTalk);
+	}
+
+	public void testStartTCPConnection() throws InterruptedException{
+		LocalBinder binder = startTelemetryTask(Integer.valueOf(PREF_TCP_CONN));
+		
+		synchronized (_syncTelemetryConnected) {
+			_syncTelemetryConnected.wait(2000);
+		}
+		
+		assertTrue("Failed to connect to telemetry service", binder.isConnected());
+		
+		assertTrue("Started wrong telemetry service type", binder.getTelemetryTask(0) instanceof TcpUAVTalk);
+	}
+	
+	public void testStartUSBTelemetryTask(){
+		LocalBinder binder = startTelemetryTask(Integer.valueOf(PREF_USB_CONN));
+		
+		assertTrue("Started wrong telemetry service type", binder.getTelemetryTask(0) instanceof HidUAVTalk);		
+	}
+
+	public void testStartUSBConnection() throws InterruptedException{
+		LocalBinder binder = startTelemetryTask(Integer.valueOf(PREF_USB_CONN));
+		
+		synchronized (_syncTelemetryConnected) {
+			_syncTelemetryConnected.wait(2000);
+		}
+		
+		assertTrue("Failed to connect to telemetry service", binder.isConnected());
+		
+		assertTrue("Started wrong telemetry service type", binder.getTelemetryTask(0) instanceof HidUAVTalk);
+	}
+
+	private LocalBinder startTelemetryTask(Integer telemetryType) {
+		LocalBinder binder = (LocalBinder) bindService(new Intent(getContext(),
+				org.openpilot.androidgcs.telemetry.OPTelemetryService.class));
+		
+		SharedPreferences prefs = PreferenceManager.getDefaultSharedPreferences(getContext());
+		SharedPreferences.Editor editor = prefs.edit();
+		editor.putString("connection_type", telemetryType.toString());
+		editor.apply();
+		
+		binder.openConnection();
+		synchronized (_syncTelemetryTaskStarted) {
+			try {
+				_syncTelemetryTaskStarted.wait(2000);
+			} catch (InterruptedException e) {
+				e.printStackTrace();
+			}
+		}
+		return binder;
+	}
+}

flight/Bootloaders/BootloaderUpdater/Makefile

@@ -74,6 +74,7 @@ PIOSINC = $(PIOS)/inc
 PIOSSTM32F10X = $(PIOS)/STM32F10x
 PIOSCOMMON = $(PIOS)/Common
 PIOSBOARDS = $(PIOS)/Boards
+PIOSCOMMONLIB = $(PIOSCOMMON)/Libraries
 APPLIBDIR = $(PIOSSTM32F10X)/Libraries
 STMLIBDIR = $(APPLIBDIR)
 STMSPDDIR = $(STMLIBDIR)/STM32F10x_StdPeriph_Driver
@@ -100,6 +101,7 @@ SRC += $(OPSYSTEM)/pios_board.c
 
 
 ## PIOS Hardware (STM32F10x)
+include $(PIOS)/STM32F10x/library.mk
 SRC += $(PIOSSTM32F10X)/pios_sys.c
 SRC += $(PIOSSTM32F10X)/pios_led.c
 SRC += $(PIOSSTM32F10X)/pios_delay.c
@@ -146,7 +148,7 @@ ASRCARM =
 
 # List any extra directories to look for include files here.
 #    Each directory must be seperated by a space.
-EXTRAINCDIRS  =  $(OPSYSTEM)
+EXTRAINCDIRS  += $(OPSYSTEM)
 EXTRAINCDIRS  += $(OPSYSTEMINC)
 EXTRAINCDIRS  += $(OPUAVTALK)
 EXTRAINCDIRS  += $(OPUAVTALKINC)
@@ -174,7 +176,7 @@ EXTRAINCDIRS  += $(HWDEFSINC)
 # 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_LIBDIRS +=
 
 # Extra Libraries
 #    Each library-name must be seperated by a space.
@@ -182,7 +184,7 @@ EXTRA_LIBDIRS =
 #    EXTRA_LIBS = xyz abc efsl
 # for newlib-lpc (file: libnewlibc-lpc.a):
 #    EXTRA_LIBS = newlib-lpc
-EXTRA_LIBS =
+#EXTRA_LIBS +=
 
 # Path to Linker-Scripts
 LINKERSCRIPTPATH = $(PIOSSTM32F10X)
@@ -209,7 +211,7 @@ DEBUGF = dwarf-2
 
 # Place project-specific -D (define) and/or 
 # -U options for C here.
-CDEFS = -DSTM32F10X_$(MODEL)
+CDEFS += -DSTM32F10X_$(MODEL)
 CDEFS += -DUSE_STDPERIPH_DRIVER
 CDEFS += -DUSE_$(BOARD)
 ifeq ($(ENABLE_DEBUG_PINS), YES)

flight/Bootloaders/CopterControl/Makefile

@@ -71,6 +71,7 @@ PIOSINC = $(PIOS)/inc
 PIOSSTM32F10X = $(PIOS)/STM32F10x
 PIOSCOMMON = $(PIOS)/Common
 PIOSBOARDS = $(PIOS)/Boards
+PIOSCOMMONLIB = $(PIOSCOMMON)/Libraries
 APPLIBDIR = $(PIOSSTM32F10X)/Libraries
 STMLIBDIR = $(APPLIBDIR)
 STMSPDDIR = $(STMLIBDIR)/STM32F10x_StdPeriph_Driver
@@ -97,6 +98,7 @@ SRC += $(OPSYSTEM)/pios_board.c
 SRC += $(OPSYSTEM)/op_dfu.c
 
 ## PIOS Hardware (STM32F10x)
+include $(PIOS)/STM32F10x/library.mk
 SRC += $(PIOSSTM32F10X)/pios_sys.c
 SRC += $(PIOSSTM32F10X)/pios_led.c
 SRC += $(PIOSSTM32F10X)/pios_delay.c
@@ -180,7 +182,7 @@ ASRCARM =
 
 # List any extra directories to look for include files here.
 #    Each directory must be seperated by a space.
-EXTRAINCDIRS  =  $(OPSYSTEM)
+EXTRAINCDIRS  += $(OPSYSTEM)
 EXTRAINCDIRS  += $(OPSYSTEMINC)
 EXTRAINCDIRS  += $(OPUAVTALK)
 EXTRAINCDIRS  += $(OPUAVTALKINC)
@@ -206,7 +208,7 @@ EXTRAINCDIRS  += $(HWDEFSINC)
 # 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_LIBDIRS +=
 
 # Extra Libraries
 #    Each library-name must be seperated by a space.
@@ -214,7 +216,7 @@ EXTRA_LIBDIRS =
 #    EXTRA_LIBS = xyz abc efsl
 # for newlib-lpc (file: libnewlibc-lpc.a):
 #    EXTRA_LIBS = newlib-lpc
-EXTRA_LIBS =
+#EXTRA_LIBS +=
 
 # Path to Linker-Scripts
 LINKERSCRIPTPATH = $(PIOSSTM32F10X)
@@ -241,7 +243,7 @@ DEBUGF = dwarf-2
 
 # Place project-specific -D (define) and/or 
 # -U options for C here.
-CDEFS = -DSTM32F10X_$(MODEL)
+CDEFS += -DSTM32F10X_$(MODEL)
 CDEFS += -DUSE_STDPERIPH_DRIVER
 CDEFS += -DUSE_$(BOARD)
 ifeq ($(ENABLE_DEBUG_PINS), YES)

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/PipXtreme/Makefile

@@ -71,6 +71,7 @@ PIOSINC = $(PIOS)/inc
 PIOSSTM32F10X = $(PIOS)/STM32F10x
 PIOSCOMMON = $(PIOS)/Common
 PIOSBOARDS = $(PIOS)/Boards
+PIOSCOMMONLIB = $(PIOSCOMMON)/Libraries
 APPLIBDIR = $(PIOSSTM32F10X)/Libraries
 STMLIBDIR = $(APPLIBDIR)
 STMSPDDIR = $(STMLIBDIR)/STM32F10x_StdPeriph_Driver
@@ -97,6 +98,7 @@ SRC += $(OPSYSTEM)/pios_board.c
 SRC += $(OPSYSTEM)/op_dfu.c
 
 ## PIOS Hardware (STM32F10x)
+include $(PIOS)/STM32F10x/library.mk
 SRC += $(PIOSSTM32F10X)/pios_sys.c
 SRC += $(PIOSSTM32F10X)/pios_led.c
 SRC += $(PIOSSTM32F10X)/pios_delay.c
@@ -180,7 +182,7 @@ ASRCARM =
 
 # List any extra directories to look for include files here.
 #    Each directory must be seperated by a space.
-EXTRAINCDIRS  =  $(OPSYSTEM)
+EXTRAINCDIRS  += $(OPSYSTEM)
 EXTRAINCDIRS  += $(OPSYSTEMINC)
 EXTRAINCDIRS  += $(OPUAVTALK)
 EXTRAINCDIRS  += $(OPUAVTALKINC)
@@ -206,7 +208,7 @@ EXTRAINCDIRS  += $(HWDEFSINC)
 # 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_LIBDIRS +=
 
 # Extra Libraries
 #    Each library-name must be seperated by a space.
@@ -214,7 +216,7 @@ EXTRA_LIBDIRS =
 #    EXTRA_LIBS = xyz abc efsl
 # for newlib-lpc (file: libnewlibc-lpc.a):
 #    EXTRA_LIBS = newlib-lpc
-EXTRA_LIBS =
+#EXTRA_LIBS +=
 
 # Path to Linker-Scripts
 LINKERSCRIPTPATH = $(PIOSSTM32F10X)
@@ -241,7 +243,7 @@ DEBUGF = dwarf-2
 
 # Place project-specific -D (define) and/or 
 # -U options for C here.
-CDEFS = -DSTM32F10X_$(MODEL)
+CDEFS += -DSTM32F10X_$(MODEL)
 CDEFS += -DUSE_STDPERIPH_DRIVER
 CDEFS += -DUSE_$(BOARD)
 ifeq ($(ENABLE_DEBUG_PINS), YES)

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
@@ -113,7 +114,7 @@ EXTRAINCDIRS  += $(HWDEFSINC)
 # 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_LIBDIRS +=
 
 # Extra Libraries
 #    Each library-name must be seperated by a space.
@@ -121,7 +122,7 @@ EXTRA_LIBDIRS =
 #    EXTRA_LIBS = xyz abc efsl
 # for newlib-lpc (file: libnewlibc-lpc.a):
 #    EXTRA_LIBS = newlib-lpc
-EXTRA_LIBS =
+#EXTRA_LIBS +=
 
 # Path to Linker-Scripts
 LINKERSCRIPTPATH = $(PIOSSTM32FXX)
@@ -148,7 +149,7 @@ DEBUGF = dwarf-2
 
 # Place project-specific -D (define) and/or 
 # -U options for C here.
-CDEFS = -DSTM32F4XX
+CDEFS += -DSTM32F4XX
 CDEFS += -DSYSCLK_FREQ=$(SYSCLK_FREQ)
 CDEFS += -DHSE_VALUE=$(OSCILLATOR_FREQ)
 CDEFS += -DUSE_STDPERIPH_DRIVER

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

@@ -115,6 +115,9 @@ MODULES = Attitude Stabilization Actuator ManualControl FirmwareIAP
 # Telemetry must be last to grab the optional modules (why?)
 MODULES += Telemetry
 
+# Enable ARM DSP library
+USE_DSP_LIB = NO
+
 # Paths
 OPSYSTEM = ./System
 OPSYSTEMINC = $(OPSYSTEM)/inc
@@ -133,6 +136,7 @@ PIOSINC = $(PIOS)/inc
 PIOSSTM32F10X = $(PIOS)/STM32F10x
 PIOSCOMMON = $(PIOS)/Common
 PIOSBOARDS = $(PIOS)/Boards
+PIOSCOMMONLIB = $(PIOSCOMMON)/Libraries
 APPLIBDIR = $(PIOSSTM32F10X)/Libraries
 STMLIBDIR = $(APPLIBDIR)
 STMSPDDIR = $(STMLIBDIR)/STM32F10x_StdPeriph_Driver
@@ -144,9 +148,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
@@ -232,6 +237,7 @@ SRC += $(OPUAVSYNTHDIR)/airspeedactual.c
 endif
 
 ## PIOS Hardware (STM32F10x)
+include $(PIOS)/STM32F10x/library.mk
 SRC += $(PIOSSTM32F10X)/pios_sys.c
 SRC += $(PIOSSTM32F10X)/pios_led.c
 SRC += $(PIOSSTM32F10X)/pios_delay.c
@@ -244,7 +250,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 +279,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 +330,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
@@ -375,7 +381,7 @@ ASRCARM =
 
 # List any extra directories to look for include files here.
 #    Each directory must be seperated by a space.
-EXTRAINCDIRS  =  $(OPSYSTEM)
+EXTRAINCDIRS  += $(OPSYSTEM)
 EXTRAINCDIRS  += $(OPSYSTEMINC)
 EXTRAINCDIRS  += $(OPUAVTALK)
 EXTRAINCDIRS  += $(OPUAVTALKINC)
@@ -396,7 +402,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)
@@ -408,7 +414,7 @@ EXTRAINCDIRS += ${foreach MOD, ${OPTMODULES} ${MODULES}, ${OPMODULEDIR}/${MOD}/i
 # 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_LIBDIRS +=
 
 # Extra Libraries
 #    Each library-name must be seperated by a space.
@@ -416,7 +422,7 @@ EXTRA_LIBDIRS =
 #    EXTRA_LIBS = xyz abc efsl
 # for newlib-lpc (file: libnewlibc-lpc.a):
 #    EXTRA_LIBS = newlib-lpc
-EXTRA_LIBS =
+#EXTRA_LIBS +=
 
 # Path to Linker-Scripts
 LINKERSCRIPTPATH = $(PIOSSTM32F10X)
@@ -443,7 +449,7 @@ DEBUGF = dwarf-2
 
 # Place project-specific -D (define) and/or
 # -U options for C here.
-CDEFS = -DSTM32F10X_$(MODEL)
+CDEFS += -DSTM32F10X_$(MODEL)
 CDEFS += -DUSE_STDPERIPH_DRIVER
 CDEFS += -DUSE_$(BOARD)
 ifeq ($(ENABLE_DEBUG_PINS), YES)
@@ -627,7 +633,7 @@ endif
 #	@$(PYTHON) $(PYMITETOOLS)/pmImgCreator.py -f $(PYMITEPLAT)/pmfeatures.py -c -u -o $(OUTDIR)/pmlibusr_img.c --native-file=$(OUTDIR)/pmlibusr_nat.c $(FLIGHTPLANS)/test.py
 
 # Link: create ELF output file from object files.
-$(eval $(call LINK_TEMPLATE, $(OUTDIR)/$(TARGET).elf, $(ALLOBJ)))
+$(eval $(call LINK_TEMPLATE, $(OUTDIR)/$(TARGET).elf, $(ALLOBJ), $(ALLLIB)))
 
 # Assemble: create object files from assembler source files.
 $(foreach src, $(ASRC), $(eval $(call ASSEMBLE_TEMPLATE, $(src))))

flight/CopterControl/System/inc/pios_config.h

@@ -51,6 +51,7 @@
 #define PIOS_INCLUDE_DSM
 #define PIOS_INCLUDE_SBUS
 #define PIOS_INCLUDE_PPM
+#define PIOS_INCLUDE_PPM_FLEXI
 #define PIOS_INCLUDE_PWM
 #define PIOS_INCLUDE_GCSRCVR
 

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 */
 
@@ -612,6 +613,20 @@ void PIOS_Board_Init(void) {
 		}
 #endif	/* PIOS_INCLUDE_GPS */
 		break;
+	case HWSETTINGS_CC_FLEXIPORT_PPM:
+#if defined(PIOS_INCLUDE_PPM_FLEXI)
+		{
+			uint32_t pios_ppm_id;
+			PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_flexi_cfg);
+
+			uint32_t pios_ppm_rcvr_id;
+			if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) {
+				PIOS_Assert(0);
+			}
+			pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id;
+		}
+#endif	/* PIOS_INCLUDE_PPM_FLEXI */
+		break;
 	case HWSETTINGS_CC_FLEXIPORT_DSM2:
 	case HWSETTINGS_CC_FLEXIPORT_DSMX10BIT:
 	case HWSETTINGS_CC_FLEXIPORT_DSMX11BIT:

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,11 @@
 #ifndef __PACKET_HANDLER_H__
 #define __PACKET_HANDLER_H__
 
+#include <uavobjectmanager.h>
+#include <gcsreceiver.h>
+#include <oplinksettings.h>
+#include <pios_rfm22b_rcvr.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)
@@ -37,19 +42,14 @@
 // Public types
 typedef enum {
 	PACKET_TYPE_NONE = 0,
-	PACKET_TYPE_CONNECT,        // for requesting a connection
-	PACKET_TYPE_DISCONNECT,     // to tell the other modem they cannot connect to us
-	PACKET_TYPE_READY,          // tells the other modem we are ready to accept more data
-	PACKET_TYPE_NOTREADY,       // tells the other modem we're not ready to accept more data - we can also send user data in this packet type
 	PACKET_TYPE_STATUS,         // broadcasts status of this modem
-	PACKET_TYPE_DATARATE,       // for changing the RF data rate
-	PACKET_TYPE_PING,           // used to check link is still up
-	PACKET_TYPE_ADJUST_TX_PWR,  // used to ask the other modem to adjust it's tx power
+	PACKET_TYPE_CON_REQUEST,    // request a connection to another modem
 	PACKET_TYPE_DATA,           // data packet (packet contains user data)
-	PACKET_TYPE_ACKED_DATA,     // data packet that requies an ACK
+	PACKET_TYPE_DUPLICATE_DATA, // a duplicate data packet
 	PACKET_TYPE_PPM,            // PPM relay values
-	PACKET_TYPE_ACK,
-	PACKET_TYPE_NACK
+	PACKET_TYPE_ACK,            // Acknowlege the receipt of a packet
+	PACKET_TYPE_ACK_RTS,        // Acknowlege the receipt of a packet and indicate that the receiving side has data to send (ready to send)
+	PACKET_TYPE_NACK,           // Acknowlege the receipt of an uncorrectable packet
 } PHPacketType;
 
 typedef struct {
@@ -57,61 +57,49 @@ 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)
-#define PH_PACKET_SIZE(p) (p->data + p->header.data_size - (uint8_t*)p + RS_ECC_NPARITY)
+#define PH_PACKET_SIZE(p) ((p)->data + (p)->header.data_size - (uint8_t*)(p) + RS_ECC_NPARITY)
 typedef struct {
 	PHPacketHeader header;
 	uint8_t data[PH_MAX_DATA + RS_ECC_NPARITY];
 } PHPacket, *PHPacketHandle;
 
+#define PH_ACK_NACK_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data))
+typedef struct {
+	PHPacketHeader header;
+	uint8_t ecc[RS_ECC_NPARITY];
+} PHAckNackPacket, *PHAckNackPacketHandle;
+
 #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[PIOS_RFM22B_RCVR_MAX_CHANNELS];
 	uint8_t ecc[RS_ECC_NPARITY];
 } PHPpmPacket, *PHPpmPacketHandle;
 
 #define PH_STATUS_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data))
 typedef struct {
 	PHPacketHeader header;
-	uint16_t retries;
-	uint16_t errors;
-	uint16_t uavtalk_errors;
-	uint16_t dropped;
-	uint16_t resets;
+	uint8_t link_quality;
+	int8_t received_rssi;
 	uint8_t ecc[RS_ECC_NPARITY];
 } PHStatusPacket, *PHStatusPacketHandle;
 
+#define PH_CONNECTION_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data))
 typedef struct {
-	uint8_t winSize;
-	uint16_t maxConnections;
-} PacketHandlerConfig;
-
-typedef int32_t (*PHOutputStream)(PHPacketHandle packet);
-typedef void (*PHDataHandler)(uint8_t *data, uint8_t len, int8_t rssi, int8_t afc);
-typedef void (*PHStatusHandler)(PHStatusPacketHandle s, int8_t rssi, int8_t afc);
-typedef void (*PHPPMHandler)(uint16_t *channels);
-
-typedef uint32_t PHInstHandle;
-
-// Public functions
-PHInstHandle PHInitialize(PacketHandlerConfig *cfg);
-void PHRegisterOutputStream(PHInstHandle h, PHOutputStream f);
-void PHRegisterDataHandler(PHInstHandle h, PHDataHandler f);
-void PHRegisterStatusHandler(PHInstHandle h, PHStatusHandler f);
-void PHRegisterPPMHandler(PHInstHandle h, PHPPMHandler f);
-uint32_t PHConnect(PHInstHandle h, uint32_t dest_id);
-PHPacketHandle PHGetRXPacket(PHInstHandle h);
-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);
+	PHPacketHeader header;
+	uint8_t datarate;
+	uint32_t frequency_hz;
+	uint32_t min_frequency;
+	uint32_t max_frequency;
+	uint8_t max_tx_power;
+	OPLinkSettingsOutputConnectionOptions port;
+	OPLinkSettingsComSpeedOptions com_speed;
+	uint8_t ecc[RS_ECC_NPARITY];
+} PHConnectionPacket, *PHConnectionPacketHandle;
 
 #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

@@ -1,504 +0,0 @@
-/**
- ******************************************************************************
- * @addtogroup OpenPilotSystem OpenPilot System
- * @{
- * @addtogroup OpenPilotLibraries OpenPilot System Libraries
- * @{
- *
- * @file       packet_handler.c
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
- * @brief      A packet handler for handeling radio packet transmission.
- * @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 "packet_handler.h"
-#include "aes.h"
-#include "ecc.h"
-
-extern char *debug_msg;
-
-// Private types and constants
-typedef struct {
-	PacketHandlerConfig cfg;
-	PHPacket *tx_packets;
-	uint8_t tx_win_start;
-	uint8_t tx_win_end;
-	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;
-	PHStatusHandler status_handler;
-	PHPPMHandler ppm_handler;
-} 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);
-
-/**
- * Initialize the Packet Handler library
- * \param[in] txWinSize The transmission window size (number of tx packet buffers).
- * \param[in] streme A callback function for transmitting the packet.
- * \param[in] id The source ID of transmitter.
- * \return PHInstHandle The Pachet Handler instance data.
- * \return 0 Failure
- */
-PHInstHandle PHInitialize(PacketHandlerConfig *cfg)
-{
-	// Allocate the primary structure
-	PHPacketDataHandle data = pvPortMalloc(sizeof(PHPacketData));
-	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);
-
-	// 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)
-	{
-		data->tx_packets[i].header.type = PACKET_TYPE_NONE;
-		data->rx_packets[i].header.type = PACKET_TYPE_NONE;
-	}
-
-	// Create the lock
-	data->lock = xSemaphoreCreateRecursiveMutex();
-
-	// Initialize the ECC library.
-	initialize_ecc();
-
-	// Return the structure.
-	return (PHInstHandle)data;
-}
-
-/**
- * Register an output stream handler
- * \param[in] h The packet handler instance data pointer.
- * \param[in] f The output stream handler function
- */
-void PHRegisterOutputStream(PHInstHandle h, PHOutputStream f)
-{
-	PHPacketDataHandle data = (PHPacketDataHandle)h;
-
-	data->output_stream = f;
-}
-
-/**
- * Register a data handler
- * \param[in] h The packet handler instance data pointer.
- * \param[in] f The data handler function
- */
-void PHRegisterDataHandler(PHInstHandle h, PHDataHandler f)
-{
-	PHPacketDataHandle data = (PHPacketDataHandle)h;
-
-	data->data_handler = f;
-}
-
-/**
- * Register a PPM packet handler
- * \param[in] h The packet handler instance data pointer.
- * \param[in] f The PPM handler function
- */
-void PHRegisterStatusHandler(PHInstHandle h, PHStatusHandler f)
-{
-	PHPacketDataHandle data = (PHPacketDataHandle)h;
-
-	data->status_handler = f;
-}
-
-/**
- * Register a PPM packet handler
- * \param[in] h The packet handler instance data pointer.
- * \param[in] f The PPM handler function
- */
-void PHRegisterPPMHandler(PHInstHandle h, PHPPMHandler f)
-{
-	PHPacketDataHandle data = (PHPacketDataHandle)h;
-
-	data->ppm_handler = f;
-}
-
-/**
- * Get a packet out of the transmit buffer.
- * \param[in] h The packet handler instance data pointer.
- * \param[in] dest_id The destination ID of this connection
- * \return PHPacketHandle A pointer to the packet buffer.
- * \return 0 No packets buffers avaiable in the transmit window.
- */
-uint32_t PHConnect(PHInstHandle h, uint32_t dest_id)
-{
-	return 1;
-}
-
-/**
- * Get a packet out of the transmit buffer.
- * \param[in] h The packet handler instance data pointer.
- * \return PHPacketHandle A pointer to the packet buffer.
- * \return 0 No packets buffers avaiable in the transmit window.
- */
-PHPacketHandle PHGetTXPacket(PHInstHandle h)
-{
-	PHPacketDataHandle data = (PHPacketDataHandle)h;
-
-	// Lock
-	xSemaphoreTakeRecursive(data->lock, portMAX_DELAY);
-
-	// Find a free packet.
-	PHPacketHandle p = NULL;
-	for (uint8_t i = 0; i < data->cfg.winSize; ++i)
-		if (data->tx_packets[i].header.type == PACKET_TYPE_NONE)
-		{
-			p = data->tx_packets + i;
-			break;
-		}
-
-	// Release lock
-	xSemaphoreGiveRecursive(data->lock);
-
-	// Return a pointer to the packet at the end of the TX window.
-	return p;
-}
-
-/**
- * Release a packet from the transmit packet buffer window.
- * \param[in] h The packet handler instance data pointer.
- * \param[in] p A pointer to the packet buffer.
- * \return Nothing
- */
-void PHReleaseTXPacket(PHInstHandle h, PHPacketHandle p)
-{
-	PHPacketDataHandle data = (PHPacketDataHandle)h;
-
-	// Lock
-	xSemaphoreTakeRecursive(data->lock, portMAX_DELAY);
-
-	// Change the packet type so we know this packet is unused.
-	p->header.type = PACKET_TYPE_NONE;
-
-	// 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;
-
-	// Release lock
-	xSemaphoreGiveRecursive(data->lock);
-}
-
-/**
- * Get a packet out of the receive buffer.
- * \param[in] h The packet handler instance data pointer.
- * \return PHPacketHandle A pointer to the packet buffer.
- * \return 0 No packets buffers avaiable in the transmit window.
- */
-PHPacketHandle PHGetRXPacket(PHInstHandle h)
-{
-	PHPacketDataHandle data = (PHPacketDataHandle)h;
-
-	// Lock
-	xSemaphoreTakeRecursive(data->lock, portMAX_DELAY);
-
-	// Find a free packet.
-	PHPacketHandle p = NULL;
-	for (uint8_t i = 0; i < data->cfg.winSize; ++i)
-		if (data->rx_packets[i].header.type == PACKET_TYPE_NONE)
-		{
-			p = data->rx_packets + i;
-			break;
-		}
-
-	// Release lock
-	xSemaphoreGiveRecursive(data->lock);
-
-	// Return a pointer to the packet at the end of the TX window.
-	return p;
-}
-
-/**
- * Release a packet from the receive packet buffer window.
- * \param[in] h The packet handler instance data pointer.
- * \param[in] p A pointer to the packet buffer.
- * \return Nothing
- */
-void PHReleaseRXPacket(PHInstHandle h, PHPacketHandle p)
-{
-	PHPacketDataHandle data = (PHPacketDataHandle)h;
-
-	// Lock
-	xSemaphoreTakeRecursive(data->lock, portMAX_DELAY);
-
-	// Change the packet type so we know this packet is unused.
-	p->header.type = PACKET_TYPE_NONE;
-
-	// 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;
-
-	// Release lock
-	xSemaphoreGiveRecursive(data->lock);
-}
-
-/**
- * Transmit a packet from the transmit packet buffer window.
- * \param[in] h The packet handler instance data pointer.
- * \param[in] p A pointer to the packet buffer.
- * \return 1 Success
- * \return 0 Failure
- */
-uint8_t PHTransmitPacket(PHInstHandle h, PHPacketHandle p)
-{
-	PHPacketDataHandle data = (PHPacketDataHandle)h;
-
-	// Try to transmit the packet.
-	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.
- * \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.
- */
-int32_t PHVerifyPacket(PHInstHandle h, PHPacketHandle p, uint16_t received_len)
-{
-
-	// 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;
-	}
-
-	// Attempt to correct any errors in the packet.
-	decode_data((unsigned char*)p, 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;
-	}
-
-	return len + 2;
-}
-
-/**
- * Process a packet that has been received.
- * \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 1 Success
- */
-uint8_t PHReceivePacket(PHInstHandle h, PHPacketHandle p, bool rx_error)
-{
-	PHPacketDataHandle data = (PHPacketDataHandle)h;
-	uint16_t len = PHPacketSizeECC(p);
-
-	// Extract the RSSI and AFC.
-	int8_t rssi = *(((int8_t*)p) + len);
-	int8_t afc = *(((int8_t*)p) + len + 1);
-
-	switch (p->header.type) {
-
-	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);
-
-		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);
-
-		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);
-
-		break;
-
-	default:
-		break;
-	}
-
-	// Release the packet.
-	PHReleaseRXPacket(h, p);
-	
-	return 1;
-}
-
-/**
- * Transmit a packet from the transmit packet buffer window.
- * \param[in] data The packet handler instance data pointer.
- * \param[in] p A pointer to the packet buffer.
- * \return 1 Success
- * \return 0 Failure
- */
-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

@@ -206,7 +206,11 @@ static void actuatorTask(void* parameters)
 
 		// Check how long since last update
 		thisSysTime = xTaskGetTickCount();
-		if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
+		// reuse dt in case of wraparound
+		// todo:
+		//  if dT actually matters...
+		//  fix it to know max value and subtract for currently correct dT on wrap
+		if(thisSysTime > lastSysTime)
 			dT = (thisSysTime - lastSysTime) * (portTICK_RATE_MS * 0.001f);
 		lastSysTime = thisSysTime;
 
@@ -328,17 +332,17 @@ static void actuatorTask(void* parameters)
 					status[ct] = -1;
 			}
 
-			if( (mixers[ct].type >= MIXERSETTINGS_MIXER1TYPE_CAMERAROLL) &&
+			if( (mixers[ct].type >= MIXERSETTINGS_MIXER1TYPE_CAMERAROLLORSERVO1) &&
 			   (mixers[ct].type <= MIXERSETTINGS_MIXER1TYPE_CAMERAYAW))
 			{
 				CameraDesiredData cameraDesired;
 				if( CameraDesiredGet(&cameraDesired) == 0 ) {
 					switch(mixers[ct].type) {
-						case MIXERSETTINGS_MIXER1TYPE_CAMERAROLL:
-							status[ct] = cameraDesired.Roll;
+						case MIXERSETTINGS_MIXER1TYPE_CAMERAROLLORSERVO1:
+							status[ct] = cameraDesired.RollOrServo1;
 							break;
-						case MIXERSETTINGS_MIXER1TYPE_CAMERAPITCH:
-							status[ct] = cameraDesired.Pitch;
+						case MIXERSETTINGS_MIXER1TYPE_CAMERAPITCHORSERVO2:
+							status[ct] = cameraDesired.PitchOrServo2;
 							break;
 						case MIXERSETTINGS_MIXER1TYPE_CAMERAYAW:
 							status[ct] = cameraDesired.Yaw;
@@ -569,6 +573,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 +671,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 +691,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 {
+		
+	}
+
+	
+}
+
+
+
+/**
+ * @}
+ * @}
+ */